Add initial implementation for decoder

include/decoder.h

@@ -0,0 +1,40 @@
+#ifndef _DECODER_H
+#define _DECODER_H
+
+#include <memory.h>
+#include <stdint.h>
+
+struct packet_decoder {
+	uint8_t* buf;
+	size_t   buf_actual_length;
+	size_t   buf_length;
+	char*    error_str;
+
+	enum packet_decoder_state {
+		NEED_PACKET_MAGIC,
+		NEED_PACKET_LENGTH,
+		NEED_PACKET_DATA
+	} state;
+	size_t   required_length;
+};
+
+int packet_decoder_init(struct packet_decoder* pd, uint8_t* buf, size_t size);
+int packet_decoder_proc(struct packet_decoder* pd, uint8_t* buf, size_t size);
+
+struct frame_decoder {
+	struct packet_decoder pd;
+
+	char* error_str;
+
+	enum frame_decoder_state {
+		NEED_FRAME_MAGIC,
+		NEED_FRAME_LENGTH,
+		NEED_FRAME_DATA
+	} state;
+	size_t required_length;
+};
+
+int frame_decoder_init(struct frame_decoder* fd, uint8_t* buf, size_t size);
+int frame_decoder_proc(struct frame_decoder* fd, uint8_t* buf, size_t size);
+
+#endif // _DECODER_H

src/cha.c

@@ -1,4 +1,5 @@
 #include <cha.h>
+#include <decoder.h>
 
 #include <assert.h>
 #include <string.h>
@@ -304,85 +305,14 @@ int cha_stop_stream(struct cha* cha) {
 	return 0;
 }
 
-struct cha_loop_state {
-	uint8_t* buf;
-	size_t   buf_length;
-	size_t   buf_actual_length;
-
-	enum state {
-		NEED_SDRAM_MARKER,
-		NEED_SDRAM_LENGTH,
-		NEED_SDRAM_DATA
-	} state;
-
-	size_t   required_pkg;
-	size_t   required_sdram;
-	enum pkg_state {
-		NEED_PKG_LENGTH,
-		NEED_PKG_DATA
-	} pkg_state;
-};
-
 static void cha_loop_transfer_callback(struct libusb_transfer* transfer) {
-	struct cha_loop_state* state = (struct cha_loop_state*)transfer->user_data;
-	uint8_t* inp = transfer->buffer;
-	size_t   inp_length = transfer->actual_length;
+	struct frame_decoder* fd = (struct frame_decoder*)transfer->user_data;
 
 	libusb_submit_transfer(transfer);
 
-	if (inp_length > 2) {
-		inp += 2;
-		inp_length -= 2;
-
-		while (inp_length) {
-			switch (state->state) {
-				case NEED_SDRAM_MARKER: {
-					assert(inp[0] == 0xd0);
-
-					state->state = NEED_SDRAM_LENGTH;
-
-					inp += 1;
-					inp_length -= 1;
-				} break;
-				case NEED_SDRAM_LENGTH: {
-					state->state = NEED_SDRAM_DATA;
-					state->required_sdram = ((size_t)(inp[0])+1)*2;
-
-					inp += 1;
-					inp_length -= 1;
-				} break;
-				case NEED_SDRAM_DATA: {
-					if (state->pkg_state == NEED_PKG_LENGTH) {
-						if (state->buf_actual_length > 4) {
-							state->pkg_state == NEED_PKG_DATA;
-							state->required_pkg = (state->buf[4] << 8) | state->buf[3] + 8 - state->buf_actual_length;
-						} else {
-							state->required_pkg = 5;
-						}
-					}
-
-					size_t len = (state->required_sdram < state->required_pkg ? state->required_sdram : state->required_pkg);
-					len = (len < inp_length ? len : inp_length);
-
-					memcpy(state->buf + state->buf_actual_length, inp, len);
-
-					inp += len;
-					inp_length -= len;
-					state->buf_actual_length += len;
-					state->required_sdram -= len;
-					state->required_pkg -= len;
-
-					if (state->pkg_state == NEED_PKG_DATA && state->required_pkg == 0) {
-						state->buf_actual_length = 0;
-						state->pkg_state = NEED_PKG_LENGTH;
-					}
-
-					if (state->required_sdram == 0) {
-						state->state = NEED_SDRAM_MARKER;
-					}
-				} break;
-			}
-		}
+	if (transfer->actual_length > 2) {
+		/* Skip FTDI header */
+		frame_decoder_proc(fd, transfer->buffer + 2, transfer->actual_length - 2);
 	}
 }
 
@@ -394,25 +324,21 @@ int cha_loop(struct cha* cha, int cnt) {
 	*/
 	int ret;
 	struct libusb_transfer* usb_transfer;
-	// FIXME:
-	unsigned char buf[1024+cha->ftdi.max_packet_size];
-	struct cha_loop_state state;
-
-	state.buf = buf;
-	state.buf_length = 1024;
-	state.buf_actual_length = 0;
-	state.state = NEED_SDRAM_MARKER;
-	state.required_pkg = 0;
-	state.required_sdram = 0;
-	state.pkg_state = NEED_PKG_LENGTH;
+	unsigned char libusb_buf[cha->ftdi.max_packet_size];
+	unsigned char packet_buf[1024];
+
+	struct frame_decoder fd;
+
+	if (frame_decoder_init(&fd, packet_buf, sizeof(packet_buf)) == -1)
+		return -1;
 
 	usb_transfer = libusb_alloc_transfer(0);
 	if (!usb_transfer) {
 		cha->error_str = "Can not allocate libusb_transfer";
 		goto fail_libusb_alloc_transfer;
 	}
 
-	libusb_fill_bulk_transfer(usb_transfer, cha->ftdi.usb_dev, cha->ftdi.out_ep, buf + 1024, cha->ftdi.max_packet_size, &cha_loop_transfer_callback, &state, 100);
+	libusb_fill_bulk_transfer(usb_transfer, cha->ftdi.usb_dev, cha->ftdi.out_ep, libusb_buf, cha->ftdi.max_packet_size, &cha_loop_transfer_callback, &fd, 100);
 
 	if ((ret = libusb_submit_transfer(usb_transfer)) < 0) {
 		cha->error_str = libusb_error_name(ret);

src/decoder.c

@@ -0,0 +1,120 @@
+#include <decoder.h>
+
+#include <assert.h>
+
+int packet_decoder_init(struct packet_decoder* pd, uint8_t* buf, size_t size) {
+	pd->buf = buf;
+	pd->buf_actual_length = 0;
+	pd->buf_length = size;
+	pd->error_str = NULL;
+	pd->state = NEED_PACKET_MAGIC;
+	pd->required_length = 0;
+
+	return 0;
+}
+
+int packet_decoder_proc(struct packet_decoder* pd, uint8_t* buf, size_t size) {
+	const uint8_t* end = buf + size;
+
+	while (buf != end) {
+		switch (pd->state) {
+			case NEED_PACKET_MAGIC: {
+				assert(pd->buf_actual_length == 0);
+				assert(pd->buf_length > 0);
+
+				if (buf[0] != 0xa0) {
+					pd->error_str = "Wrong packet magic";
+					return -1;
+				}
+
+				pd->buf[pd->buf_actual_length++] = *(buf++);
+				pd->state = NEED_PACKET_LENGTH;
+			} break;
+			case NEED_PACKET_LENGTH: {
+				assert(pd->buf_actual_length >= 1);
+				assert(pd->buf_actual_length < 5);
+				assert(pd->buf_length > 5);
+
+				for (; pd->buf_actual_length < 5 && buf != end; ++buf)
+					pd->buf[pd->buf_actual_length++] = *buf;
+
+				if (pd->buf_actual_length == 5) {
+					pd->required_length = ((((size_t)(pd->buf[4])) << 8) | pd->buf[3]) + 8 - pd->buf_actual_length;
+					pd->state = NEED_PACKET_DATA;
+				}
+			} break;
+			case NEED_PACKET_DATA: {
+				const size_t copy = (pd->required_length < (end - buf) ? pd->required_length : end - buf);
+				const size_t copy2 = (copy + pd->buf_actual_length > pd->buf_length ? pd->buf_length - pd->buf_actual_length : copy);
+
+				assert(pd->buf_actual_length >= 5);
+
+				memcpy(pd->buf + pd->buf_actual_length, buf, copy2);
+				pd->buf_actual_length += copy2;
+				buf += copy;
+				pd->required_length -= copy;
+
+				if (pd->required_length == 0) {
+					/* Finalize packet here*/
+
+					pd->buf_actual_length = 0;
+					pd->state = NEED_PACKET_MAGIC;
+
+					goto end;
+				}
+			} break;
+		}
+	}
+
+end:
+	return size - (end - buf);
+}
+
+int frame_decoder_init(struct frame_decoder* fd, uint8_t* buf, size_t size) {
+	if (packet_decoder_init(&fd->pd, buf, size) != 0)
+		return -1;
+
+	fd->error_str = NULL;
+	fd->state = NEED_FRAME_MAGIC;
+	fd->required_length = 0;
+
+	return 0;
+}
+
+int frame_decoder_proc(struct frame_decoder* fd, uint8_t* buf, size_t size) {
+	const uint8_t* end = buf + size;
+
+	while (buf != end) {
+		switch (fd->state) {
+			case NEED_FRAME_MAGIC: {
+				if (*buf++ != 0xd0) {
+					fd->error_str = "Wrong frame magic";
+					return -1;
+				}
+
+				fd->state = NEED_FRAME_LENGTH;
+			} break;
+			case NEED_FRAME_LENGTH: {
+				fd->required_length = ((size_t)(*buf++)+1)*2;
+				fd->state = NEED_FRAME_DATA;
+			} break;
+			case NEED_FRAME_DATA: {
+				const size_t size = (fd->required_length < end - buf ? fd->required_length : end - buf);
+				int ret = 0;
+
+				ret = packet_decoder_proc(&fd->pd, buf, size);
+				if (ret == -1)
+					return -1;
+
+				buf += ret;
+				fd->required_length -= ret;
+
+				if (fd->required_length == 0) {
+					fd->state = NEED_FRAME_MAGIC;
+				}
+			} break;
+		}
+	}
+
+	return size - (end - buf);
+}