Improve packet decoding

include/decoder.h

@@ -4,10 +4,17 @@
 #include <memory.h>
 #include <stdint.h>
 
-typedef void (*packet_decoder_callback)(uint8_t*, size_t, void*);
+struct packet {
+	uint16_t flags;
+	uint16_t size;
+	uint32_t timestamp;
+	uint8_t  data[];
+};
+
+typedef void (*packet_decoder_callback)(struct packet*, void*);
 
 struct packet_decoder {
-	uint8_t* buf;
+	struct packet* packet;
 	size_t   buf_actual_length;
 	size_t   buf_length;
 	char*    error_str;
@@ -17,13 +24,18 @@ struct packet_decoder {
 
 	enum packet_decoder_state {
 		NEED_PACKET_MAGIC,
-		NEED_PACKET_LENGTH,
+		NEED_PACKET_FLAGS_LO,
+		NEED_PACKET_FLAGS_HI,
+		NEED_PACKET_LENGTH_LO,
+		NEED_PACKET_LENGTH_HI,
+		NEED_PACKET_TIMESTAMP_LO,
+		NEED_PACKET_TIMESTAMP_ME,
+		NEED_PACKET_TIMESTAMP_HI,
 		NEED_PACKET_DATA
 	} state;
-	size_t   required_length;
 };
 
-int packet_decoder_init(struct packet_decoder* pd, uint8_t* buf, size_t size, packet_decoder_callback callback, void* data);
+int packet_decoder_init(struct packet_decoder* pd, struct packet* p, size_t size, packet_decoder_callback callback, void* data);
 int packet_decoder_proc(struct packet_decoder* pd, uint8_t* buf, size_t size);
 
 struct frame_decoder {
@@ -39,7 +51,7 @@ struct frame_decoder {
 	size_t required_length;
 };
 
-int frame_decoder_init(struct frame_decoder* fd, uint8_t* buf, size_t size, packet_decoder_callback callback, void* data);
+int frame_decoder_init(struct frame_decoder* fd, struct packet* p, size_t size, packet_decoder_callback callback, void* data);
 int frame_decoder_proc(struct frame_decoder* fd, uint8_t* buf, size_t size);
 
 #endif // _DECODER_H

src/cha.c

@@ -317,12 +317,12 @@ int cha_stop_stream(struct cha* cha) {
 	return 0;
 }
 
-static void cha_loop_packet_callback(uint8_t* buf, size_t size, void* data) {
+static void cha_loop_packet_callback(struct packet* packet, void* data) {
 	struct cha_loop_packet_callback_state* state = (struct cha_loop_packet_callback_state*)data;
 
 	state->count++;
 
-	state->user_callback(buf, size, state->user_data);
+	state->user_callback(packet, state->user_data);
 }
 
 static void cha_loop_transfer_callback(struct libusb_transfer* transfer) {
@@ -361,15 +361,19 @@ int cha_loop(struct cha* cha, size_t count, packet_decoder_callback callback, vo
 	int ret;
 	struct libusb_transfer* usb_transfer;
 	unsigned char libusb_buf[cha->ftdi.max_packet_size];
-	unsigned char packet_buf[1024];
+
+	union {
+		struct packet packet;
+		uint8_t data[1024];
+	} p;
 
 	struct frame_decoder fd;
 	struct cha_loop_packet_callback_state state;
 	state.count = 0;
 	state.user_callback = callback;
 	state.user_data = data;
 
-	if (frame_decoder_init(&fd, packet_buf, sizeof(packet_buf), &cha_loop_packet_callback, &state) == -1) {
+	if (frame_decoder_init(&fd, &p.packet, sizeof(p), &cha_loop_packet_callback, &state) == -1) {
 		goto fail_frame_decode_init;
 	}
 

src/decoder.c

@@ -2,15 +2,14 @@
 
 #include <assert.h>
 
-int packet_decoder_init(struct packet_decoder* pd, uint8_t* buf, size_t size, packet_decoder_callback callback, void* data) {
-	pd->buf = buf;
+int packet_decoder_init(struct packet_decoder* pd, struct packet* p, size_t size, packet_decoder_callback callback, void* data) {
+	pd->packet = p;
 	pd->buf_actual_length = 0;
 	pd->buf_length = size;
 	pd->callback = callback;
 	pd->user_data = data;
 	pd->error_str = NULL;
 	pd->state = NEED_PACKET_MAGIC;
-	pd->required_length = 0;
 
 	return 0;
 }
@@ -24,41 +23,54 @@ int packet_decoder_proc(struct packet_decoder* pd, uint8_t* buf, size_t size) {
 				assert(pd->buf_actual_length == 0);
 				assert(pd->buf_length > 0);
 
-				if (buf[0] != 0xa0) {
+				if (*(buf++) != 0xa0) {
 					pd->error_str = "Wrong packet magic";
 					return -1;
 				}
 
-				pd->buf[pd->buf_actual_length++] = *(buf++);
-				pd->state = NEED_PACKET_LENGTH;
+				pd->state = NEED_PACKET_FLAGS_LO;
 			} 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;
+			case NEED_PACKET_FLAGS_LO: {
+				pd->packet->flags = *(buf++);
+				pd->state = NEED_PACKET_FLAGS_HI;
+			} break;
+			case NEED_PACKET_FLAGS_HI: {
+				pd->packet->flags |= *(buf++) << 8;
+				pd->state = NEED_PACKET_LENGTH_LO;
+			} break;
+			case NEED_PACKET_LENGTH_LO: {
+				pd->packet->size = *(buf++);
+				pd->state = NEED_PACKET_LENGTH_HI;
+			} break;
+			case NEED_PACKET_LENGTH_HI: {
+				pd->packet->size |= *(buf++) << 8;
+				pd->state = NEED_PACKET_TIMESTAMP_LO;
 
-				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;
-				}
+				// FIXME: check available buffer size
+			} break;
+			case NEED_PACKET_TIMESTAMP_LO: {
+				pd->packet->timestamp = *(buf++);
+				pd->state = NEED_PACKET_TIMESTAMP_ME;
+			} break;
+			case NEED_PACKET_TIMESTAMP_ME: {
+				pd->packet->timestamp |= *(buf++) << 8;
+				pd->state = NEED_PACKET_TIMESTAMP_HI;
+			} break;
+			case NEED_PACKET_TIMESTAMP_HI: {
+				pd->packet->timestamp |= *(buf++) << 16;
+				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);
+				const size_t required_length = pd->packet->size - pd->buf_actual_length;
+				const size_t copy = (required_length < (end - buf) ? required_length : end - buf);
 
-				memcpy(pd->buf + pd->buf_actual_length, buf, copy2);
-				pd->buf_actual_length += copy2;
+				memcpy(pd->packet->data + pd->buf_actual_length, buf, copy);
+				pd->buf_actual_length += copy;
 				buf += copy;
-				pd->required_length -= copy;
 
-				if (pd->required_length == 0) {
+				if (required_length == copy) {
 					/* Finalize packet here*/
-					pd->callback(pd->buf, pd->buf_actual_length, pd->user_data);
+					pd->callback(pd->packet, pd->user_data);
 
 					pd->buf_actual_length = 0;
 					pd->state = NEED_PACKET_MAGIC;
@@ -73,8 +85,8 @@ int packet_decoder_proc(struct packet_decoder* pd, uint8_t* buf, size_t size) {
 	return size - (end - buf);
 }
 
-int frame_decoder_init(struct frame_decoder* fd, uint8_t* buf, size_t size, packet_decoder_callback callback, void* data) {
-	if (packet_decoder_init(&fd->pd, buf, size, callback, data) != 0)
+int frame_decoder_init(struct frame_decoder* fd, struct packet* p, size_t size, packet_decoder_callback callback, void* data) {
+	if (packet_decoder_init(&fd->pd, p, size, callback, data) != 0)
 		return -1;
 
 	fd->error_str = NULL;

test/decoder.c

@@ -3,23 +3,26 @@
 
 #include <decoder.h>
 
-char buf[1024];
+union {
+	struct packet packet;
+	uint8_t data[1024];
+} p;
 struct packet_decoder pd;
 struct frame_decoder fd;
 
-void callback(uint8_t* buf, size_t size, void* data) {
+void callback(struct packet* packet, void* data) {
 }
 
 void packet_setup() {
-	ck_assert_int_eq(packet_decoder_init(&pd, buf, sizeof(buf), &callback, NULL), 0);
+	ck_assert_int_eq(packet_decoder_init(&pd, &p.packet, sizeof(p), &callback, NULL), 0);
 }
 
 void packet_teardown() {
 
 }
 
 void frame_setup() {
-	ck_assert_int_eq(frame_decoder_init(&fd, buf, sizeof(buf), &callback, NULL), 0);
+	ck_assert_int_eq(frame_decoder_init(&fd, &p.packet, sizeof(p), &callback, NULL), 0);
 }
 
 void frame_teardown() {
@@ -30,15 +33,19 @@ START_TEST (test_packet_decoder1) {
 	char inp[] = {0xa0,0,0,0x01,0,0xc4,0xcc,0x96,0x5a};
 	ck_assert_int_eq(packet_decoder_proc(&pd, inp, sizeof(inp)), sizeof(inp));
 	ck_assert_int_eq(pd.state, NEED_PACKET_MAGIC);
-	ck_assert_int_eq(memcmp(buf, inp, sizeof(inp)), 0);
+	ck_assert_int_eq(p.packet.size, 1);
+	ck_assert_int_eq(p.packet.timestamp, 0x96ccc4);
+	ck_assert_int_eq(memcmp(p.packet.data, inp+8, p.packet.size), 0);
 }
 END_TEST
 
 START_TEST (test_packet_decoder2) {
 	char inp[] = {0xa0,0,0,0x01,0,0xc4,0xcc,0x96,0x5a,0xa0};
 	ck_assert_int_eq(packet_decoder_proc(&pd, inp, sizeof(inp)), sizeof(inp)-1);
 	ck_assert_int_eq(pd.state, NEED_PACKET_MAGIC);
-	ck_assert_int_eq(memcmp(buf, inp, sizeof(inp)-1), 0);
+	ck_assert_int_eq(p.packet.size, 1);
+	ck_assert_int_eq(p.packet.timestamp, 0x96ccc4);
+	ck_assert_int_eq(memcmp(p.packet.data, inp+8, p.packet.size), 0);
 }
 END_TEST
 
@@ -52,19 +59,22 @@ START_TEST (test_packet_decoder4) {
 	char inp[] = {0xa0,0,0,0x03,0,0xac,0x6c,0xa5,0x69,0x83,0xe0};
 	ck_assert_int_eq(packet_decoder_proc(&pd, inp, sizeof(inp)), sizeof(inp));
 	ck_assert_int_eq(pd.state, NEED_PACKET_MAGIC);
-	ck_assert_int_eq(memcmp(buf, inp, sizeof(inp)), 0);
+	ck_assert_int_eq(p.packet.size, 3);
+	ck_assert_int_eq(p.packet.timestamp, 0xa56cac);
+	ck_assert_int_eq(memcmp(p.packet.data, inp+8, p.packet.size), 0);
 }
 END_TEST
 
 START_TEST (test_packet_decoder5) {
 	char inp1[] = {0xa0,0,0,0x03,0,0xac};
 	char inp2[] = {0x6c,0xa5,0x69,0x83,0xe0};
 	ck_assert_int_eq(packet_decoder_proc(&pd, inp1, sizeof(inp1)), sizeof(inp1));
-	ck_assert_int_eq(pd.state, NEED_PACKET_DATA);
+	ck_assert_int_eq(pd.state, NEED_PACKET_TIMESTAMP_ME);
+	ck_assert_int_eq(p.packet.size, 3);
 	ck_assert_int_eq(packet_decoder_proc(&pd, inp2, sizeof(inp2)), sizeof(inp2));
 	ck_assert_int_eq(pd.state, NEED_PACKET_MAGIC);
-	ck_assert_int_eq(memcmp(buf, inp1, sizeof(inp1)), 0);
-	ck_assert_int_eq(memcmp(buf+sizeof(inp1), inp2, sizeof(inp2)), 0);
+	ck_assert_int_eq(p.packet.timestamp, 0xa56cac);
+	ck_assert_int_eq(memcmp(p.packet.data, inp2+2, p.packet.size), 0);
 }
 END_TEST
 

tools/sample/main.c

@@ -12,10 +12,10 @@
 #define PORTB_DONE_BIT     (1 << 2)  // GPIOH2
 #define PORTB_INIT_BIT     (1 << 5)  // GPIOH5
 
-static void packet_handler(uint8_t* buf, size_t size, void* data) {
-	printf("Received %d :", size);
-	for (int i = 0; i < size; ++i)
-		printf(" %02x", buf[i]);
+static void packet_handler(struct packet* packet, void* data) {
+	printf("[%04x] Received %d bytes at %d:", packet->flags, packet->size, packet->timestamp);
+	for (int i = 0; i < packet->size; ++i)
+		printf(" %02x", packet->data[i]);
 	printf("\n");
 }