From b45e741dba87214e6fd7b09309afbe12a8727e1a Mon Sep 17 00:00:00 2001 From: "Matwey V. Kornilov" Date: Sun, 25 Jun 2017 16:04:06 +0300 Subject: [PATCH] Add initial implementation for decoder --- include/decoder.h | 40 +++++++++++++ src/cha.c | 100 ++++--------------------------- src/decoder.c | 120 +++++++++++++++++++++++++++++++++++++ test/decoder.c | 143 ++++++++++++++++++++++++++++++++++++++++++++ tools/sample/main.c | 4 +- 5 files changed, 318 insertions(+), 89 deletions(-) create mode 100644 include/decoder.h create mode 100644 src/decoder.c create mode 100644 test/decoder.c diff --git a/include/decoder.h b/include/decoder.h new file mode 100644 index 0000000..bd53d84 --- /dev/null +++ b/include/decoder.h @@ -0,0 +1,40 @@ +#ifndef _DECODER_H +#define _DECODER_H + +#include +#include + +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 diff --git a/src/cha.c b/src/cha.c index 0cebe86..d56777c 100644 --- a/src/cha.c +++ b/src/cha.c @@ -1,4 +1,5 @@ #include +#include #include #include @@ -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,17 +324,13 @@ 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) { @@ -412,7 +338,7 @@ int cha_loop(struct cha* cha, int cnt) { 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); diff --git a/src/decoder.c b/src/decoder.c new file mode 100644 index 0000000..5c2c37e --- /dev/null +++ b/src/decoder.c @@ -0,0 +1,120 @@ +#include + +#include + +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); +} diff --git a/test/decoder.c b/test/decoder.c new file mode 100644 index 0000000..e813903 --- /dev/null +++ b/test/decoder.c @@ -0,0 +1,143 @@ +#include +#include + +#include + +char buf[1024]; +struct packet_decoder pd; +struct frame_decoder fd; + +void packet_setup() { + ck_assert_int_eq(packet_decoder_init(&pd, buf, sizeof(buf)), 0); +} + +void packet_teardown() { + +} + +void frame_setup() { + ck_assert_int_eq(frame_decoder_init(&fd, buf, sizeof(buf)), 0); +} + +void frame_teardown() { + +} + +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); +} +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); +} +END_TEST + +START_TEST (test_packet_decoder3) { + char inp[] = {1,2,3}; + ck_assert_int_eq(packet_decoder_proc(&pd, inp, sizeof(inp)), -1); +} +END_TEST + +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); +} +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(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); +} +END_TEST + +START_TEST (test_frame_decoder1) { + char inp[] = { + 0xd0, 0x1f, 0xa0, 0x00, 0x00, 0x03, 0x00, 0xba, + 0x6e, 0x6e, 0x69, 0xd7, 0x60, 0xa0, 0x00, 0x00, + 0x01, 0x00, 0x22, 0x75, 0x6e, 0x5a, 0xa0, 0x00, + 0x00, 0x03, 0x00, 0xe2, 0xc1, 0x75, 0x69, 0xd7, + 0x60, 0xa0, 0x00, 0x00, 0x01, 0x00, 0x4a, 0xc8, + 0x75, 0x5a, 0xa0, 0x00, 0x00, 0x03, 0x00, 0x0a, + 0x15, 0x7d, 0x69, 0xd7, 0x60, 0xa0, 0x00, 0x00, + 0x01, 0x00, 0x72, 0x1b, 0x7d, 0x5a, 0xa0, 0x00, + 0x00, 0x03 + }; + + ck_assert_int_eq(frame_decoder_proc(&fd, inp, sizeof(inp)), sizeof(inp)); + ck_assert_int_eq(fd.state, NEED_FRAME_MAGIC); +} +END_TEST + +START_TEST (test_frame_decoder2) { + char inp[] = { + 0xd0, 0x1f, 0xa0, 0x00, 0x00, 0x03, 0x00, 0xba, + 0x6e, 0x6e, 0x69, 0xd7, 0x60, 0xa0, 0x00, 0x00, + 0x01, 0x00, 0x22, 0x75, 0x6e, 0x5a, 0xa0, 0x00, + 0x00, 0x03, 0x00, 0xe2, 0xc1, 0x75, 0x69, 0xd7, + 0x60, 0xa0, 0x00, 0x00, 0x01, 0x00, 0x4a, 0xc8, + 0x75, 0x5a, 0xa0, 0x00, 0x00, 0x03, 0x00, 0x0a, + 0x15, 0x7d, 0x69, 0xd7, 0x60, 0xa0, 0x00, 0x00, + 0x01, 0x00, 0x72, 0x1b, 0x7d, 0x5a, 0xa0, 0x00, + 0x00, 0x03, 0xd0, 0x03, 0x00, 0x0a, 0x15, 0x7d, + 0x69, 0xd7, 0x60, 0xa0 + }; + + ck_assert_int_eq(frame_decoder_proc(&fd, inp, sizeof(inp)), sizeof(inp)); + ck_assert_int_eq(fd.state, NEED_FRAME_MAGIC); +} +END_TEST + +Suite* range_suite(void) { + Suite *s; + TCase *tc_packet; + TCase *tc_frame; + + s = suite_create("decoder"); + + tc_packet = tcase_create("Packet"); + tcase_add_checked_fixture(tc_packet, packet_setup, packet_teardown); + tcase_add_test(tc_packet, test_packet_decoder1); + tcase_add_test(tc_packet, test_packet_decoder2); + tcase_add_test(tc_packet, test_packet_decoder3); + tcase_add_test(tc_packet, test_packet_decoder4); + tcase_add_test(tc_packet, test_packet_decoder5); + suite_add_tcase(s, tc_packet); + + tc_frame = tcase_create("Frame"); + tcase_add_checked_fixture(tc_frame, frame_setup, frame_teardown); + tcase_add_test(tc_frame, test_frame_decoder1); + tcase_add_test(tc_frame, test_frame_decoder2); + suite_add_tcase(s, tc_frame); + + return s; +} + +int main(void) { + int number_failed; + Suite *s; + SRunner *sr; + + s = range_suite(); + sr = srunner_create(s); + + srunner_run_all(sr, CK_NORMAL); + number_failed = srunner_ntests_failed(sr); + srunner_free(sr); + return (number_failed == 0) ? 0 : 1; +} + diff --git a/tools/sample/main.c b/tools/sample/main.c index 0afbb77..dd3ec07 100644 --- a/tools/sample/main.c +++ b/tools/sample/main.c @@ -78,13 +78,13 @@ int main(int argc, char** argv) { // self.regs.ucfg_wdata.wr(value) ret = cha_write_reg(&cha, 0x402, 0x4a); if (ret == -1) { - fprintf(stderr, "dev.ulpiregs.func_ctl.wr %s\n", cha_get_error_string(&cha)); + fprintf(stderr, "self.regs.ucfg_wdata.wr %s\n", cha_get_error_string(&cha)); return 1; } // self.regs.ucfg_wcmd.wr(UCFG_REG_GO | (addr & UCFG_REG_ADDRMASK)) ret = cha_write_reg(&cha, 0x403, 0x80 | (0x04 & 0x3F)); if (ret == -1) { - fprintf(stderr, cha_get_error_string(&cha)); + fprintf(stderr, "self.regs.ucfg_wcmd.wr %s\n", cha_get_error_string(&cha)); return 1; }