diff --git a/dpsboot/dpsboot.c b/dpsboot/dpsboot.c index 2a8b7741..6b6e7794 100644 --- a/dpsboot/dpsboot.c +++ b/dpsboot/dpsboot.c @@ -79,8 +79,8 @@ static uint16_t fw_crc16; static upgrade_reason_t reason = reason_unknown; -static void handle_frame(uint8_t *frame, uint32_t length); -static void send_frame(uint8_t *frame, uint32_t length); +static void handle_frame(uint8_t *payload, uint32_t length); +static void send_frame(const frame_t *frame); /** * @brief Send ack to upgrade start and do some book keeping @@ -88,16 +88,17 @@ static void send_frame(uint8_t *frame, uint32_t length); */ static void send_start_response(void) { - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_response | cmd_upgrade_start); - PACK8(upgrade_continue); - PACK16(chunk_size); - PACK8(reason); - FINISH_FRAME(); + frame_t frame; + set_frame_header(&frame); + pack8(&frame, cmd_response | cmd_upgrade_start); + pack8(&frame, upgrade_continue); + pack16(&frame, chunk_size); + pack8(&frame, reason); + end_frame(&frame); uint32_t setting = 1; (void) past_write_unit(&past, past_upgrade_started, (void*) &setting, sizeof(setting)); cur_flash_address = (uint32_t) &_app_start; - send_frame(_buffer, _length); + send_frame(&frame); } /** @@ -173,12 +174,10 @@ static inline bool flash_write32(uint32_t address, uint32_t data) * @param length length of frame * @retval None */ -static void send_frame(uint8_t *frame, uint32_t length) +static void send_frame(const frame_t *frame) { - do { - usart_send_blocking(USART1, *frame); - frame++; - } while(--length); + for (uint32_t i = 0; i < frame->length; ++i) + usart_send_blocking(USART1, frame->buffer[i]); } /** @@ -187,24 +186,24 @@ static void send_frame(uint8_t *frame, uint32_t length) * @param length length of frame * @retval None */ -static void handle_frame(uint8_t *frame, uint32_t length) +static void handle_frame(uint8_t *payload, uint32_t length) { command_t cmd = cmd_response; - uint8_t *payload; upgrade_status_t status; - int32_t payload_len = uframe_extract_payload(frame, length); - payload = frame; // Why? Well, frame now points to the payload + frame_t frame; + int32_t payload_len = uframe_extract_payload(&frame, payload, length); + if (payload_len > 0) { - cmd = frame[0]; + cmd = frame.buffer[0]; switch(cmd) { case cmd_upgrade_start: { { - DECLARE_UNPACK(payload, length); - UNPACK8(cmd); - UNPACK16(chunk_size); + start_frame_unpacking(&frame); + unpack8(&frame, &cmd); + unpack16(&frame, &chunk_size); chunk_size = MIN(MAX_CHUNK_SIZE, chunk_size); - UNPACK16(fw_crc16); + unpack16(&frame, &fw_crc16); } send_start_response(); break; @@ -244,11 +243,12 @@ static void handle_frame(uint8_t *frame, uint32_t length) } } { - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_response | cmd_upgrade_data); - PACK8(status); - FINISH_FRAME(); - send_frame(_buffer, _length); + frame_t frame_resp; + set_frame_header(&frame_resp); + pack8(&frame_resp, cmd_response | cmd_upgrade_data); + pack8(&frame_resp, status); + end_frame(&frame_resp); + send_frame(&frame_resp); if (status == upgrade_success) { usart_wait_send_ready(USART1); /** make sure FIFO is empty */ (void) past_erase_unit(&past, past_upgrade_started); diff --git a/emu/dpsemul.c b/emu/dpsemul.c index 66e4cd17..f1d84949 100644 --- a/emu/dpsemul.c +++ b/emu/dpsemul.c @@ -37,6 +37,7 @@ #include "event.h" #include "tft.h" #include "dbg_printf.h" +#include "uframe.h" #define UDP_RX_BUF_LEN (512) #define DPS_PORT (5005) @@ -58,10 +59,10 @@ struct sockaddr_in comm_client_sock; * @param frame The frame * @param[in] length The length */ -void dps_emul_send_frame(uint8_t *frame, uint32_t length) +void dps_emul_send_frame(frame_t *frame) { int slen = sizeof(comm_client_sock); - if (sendto(comm_sock, frame, length, 0, (struct sockaddr*) &comm_client_sock, slen) == -1) { + if (sendto(comm_sock, frame->buffer, frame->length, 0, (struct sockaddr*) &comm_client_sock, slen) == -1) { printf("Error: sendto()\n"); } } diff --git a/emu/hw.c b/emu/hw.c index d583f0d3..32bc8360 100644 --- a/emu/hw.c +++ b/emu/hw.c @@ -81,3 +81,41 @@ bool hw_sel_button_pressed(void) { return false; } + +/** + * @brief Set TFT backlight value + * @retval None + */ +void hw_set_backlight(uint8_t brightness) +{ + (void) brightness; +} + +/** + * @brief Get TFT backlight value + * @retval Brightness percentage + */ +uint8_t hw_get_backlight(void) +{ + return 0; +} + +/** + * @brief Set the output voltage DAC value + * @param v_dac the value to set to + * @retval none + */ +void hw_set_voltage_dac(uint16_t v_dac) +{ + (void) v_dac; +} + +/** + * @brief Set the output current DAC value + * @param i_dac the value to set to + * @retval none + */ +void hw_set_current_dac(uint16_t i_dac) +{ + (void) i_dac; +} \ No newline at end of file diff --git a/emu/tft.c b/emu/tft.c index 93d01d14..9ba02bf2 100644 --- a/emu/tft.c +++ b/emu/tft.c @@ -107,7 +107,7 @@ void tft_blit(uint16_t *bits, uint32_t width, uint32_t height, uint32_t x, uint3 * @param highlight if true, the character will be inverted * @retval none */ -void tft_putch(uint8_t size, char ch, uint32_t x, uint32_t y, uint32_t w, uint32_t h, bool highlight) +uint8_t tft_putch(tft_font_size_t size, char ch, uint32_t x, uint32_t y, uint32_t w, uint32_t h, uint16_t color, bool invert) { if (x >= TFT_WIDTH || y >= TFT_HEIGHT) { printf("Error: character '%c' put outside of screen (%d, %d)\n", ch, x, y); @@ -116,7 +116,32 @@ void tft_putch(uint8_t size, char ch, uint32_t x, uint32_t y, uint32_t w, uint32 (void) size; (void) w; (void) h; - (void) highlight; + (void) color; + (void) invert; +} + +/** + * @brief Blit string on TFT, anchored to bottom-left + * @param size size of character + * @param str the string (must be a supported character) + * @param x x position (left-side of string) + * @param y y position (bottom of string) + * @param w width of bounding box + * @param h height of bounding box + * @param color color of the string + * @param invert if true, the string will be inverted + * @retval the width of the string drawn + */ +uint16_t tft_puts(tft_font_size_t size, const char *str, uint32_t x, uint32_t y, uint32_t w, uint32_t h, uint16_t color, bool invert) +{ + (void) size; + (void) str; + (void) x; + (void) y; + (void) w; + (void) h; + (void) color; + (void) invert; } /** @@ -137,6 +162,24 @@ void tft_fill_pattern(uint32_t x1, uint32_t y1, uint32_t x2, uint32_t y2, uint8_ (void) fill_size; } +/** + * @brief Draw a one pixel rectangle + * @param xpos x position + * @param ypos y position + * @param width width of frame + * @param height height of frame + * @param color color in bgr565 format + * @retval none + */ +void tft_rect(uint32_t xpos, uint32_t ypos, uint32_t width, uint32_t height, uint16_t color) +{ + (void) xpos; + (void) ypos; + (void) width; + (void) height; + (void) color; +} + /** * @brief Fill area with specified color * @param x y top left corner diff --git a/opendps/protocol.c b/opendps/protocol.c index 6b518ab8..ec3a6382 100644 --- a/opendps/protocol.c +++ b/opendps/protocol.c @@ -28,151 +28,123 @@ #include "protocol.h" #include "uframe.h" - -// Boiler plate code -#define COPY_FRAME_RETURN() \ - if (_length <= length) { \ - memcpy((void*) frame, (void*) _buffer, _length); \ - return _length; \ - } else { \ - return 0; \ - } - -uint32_t protocol_create_response(uint8_t *frame, uint32_t length, command_t cmd, uint8_t success) -{ - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_response | cmd); - PACK8(success); - FINISH_FRAME(); - COPY_FRAME_RETURN(); -} - -uint32_t protocol_create_ping(uint8_t *frame, uint32_t length) +void protocol_create_response(frame_t *frame, command_t cmd, uint8_t success) { - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_ping); - FINISH_FRAME(); - COPY_FRAME_RETURN(); + set_frame_header(frame); + pack8(frame, cmd_response | cmd); + pack8(frame, success); + end_frame(frame); } -uint32_t protocol_create_status(uint8_t *frame, uint32_t length) +void protocol_create_ping(frame_t *frame) { - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_query); - FINISH_FRAME(); - COPY_FRAME_RETURN(); + set_frame_header(frame); + pack8(frame, cmd_ping); + end_frame(frame); } -uint32_t protocol_create_wifi_status(uint8_t *frame, uint32_t length, wifi_status_t status) +void protocol_create_status(frame_t *frame) { - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_wifi_status); - PACK8(status); - FINISH_FRAME(); - COPY_FRAME_RETURN(); + set_frame_header(frame); + pack8(frame, cmd_query); + end_frame(frame); } -uint32_t protocol_create_lock(uint8_t *frame, uint32_t length, uint8_t locked) +void protocol_create_wifi_status(frame_t *frame, wifi_status_t status) { - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_lock); - PACK8(!!locked); - FINISH_FRAME(); - COPY_FRAME_RETURN(); + set_frame_header(frame); + pack8(frame, cmd_wifi_status); + pack8(frame, status); + end_frame(frame); } -uint32_t protocol_create_ocp(uint8_t *frame, uint32_t length, uint16_t i_cut) +void protocol_create_lock(frame_t *frame, uint8_t locked) { - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_ocp_event); - PACK16(i_cut); - FINISH_FRAME(); - COPY_FRAME_RETURN(); + set_frame_header(frame); + pack8(frame, cmd_lock); + pack8(frame, !!locked); + end_frame(frame); } -bool protocol_unpack_response(uint8_t *payload, uint32_t length, command_t *cmd, uint8_t *success) +void protocol_create_ocp(frame_t *frame, uint16_t i_cut) { - DECLARE_UNPACK(payload, length); - UNPACK8(*cmd); - UNPACK8(*success); - return _remain == 0; + set_frame_header(frame); + pack8(frame, cmd_ocp_event); + pack16(frame, i_cut); + end_frame(frame); } -#if 0 -bool protocol_unpack_power_enable(uint8_t *payload, uint32_t length, uint8_t *enable) +bool protocol_unpack_response(frame_t *frame, command_t *cmd, uint8_t *success) { - command_t cmd; - DECLARE_UNPACK(payload, length); - UNPACK8(cmd); - UNPACK8(*enable); - *enable = !!(*enable); - return _remain == 0 && cmd == cmd_power_enable; + start_frame_unpacking(frame); + unpack8(frame, cmd); + unpack8(frame, success); + return frame->length == 0; } -bool protocol_unpack_vout(uint8_t *payload, uint32_t length, uint16_t *vout_mv) -{ - command_t cmd; - DECLARE_UNPACK(payload, length); - UNPACK8(cmd); - UNPACK16(*vout_mv); - return _remain == 0 && cmd == cmd_set_vout; -} - -#endif - -bool protocol_unpack_query_response(uint8_t *payload, uint32_t length, uint16_t *v_in, uint16_t *v_out_setting, uint16_t *v_out, uint16_t *i_out, uint16_t *i_limit, uint8_t *power_enabled) +bool protocol_unpack_query_response(frame_t *frame, uint16_t *v_in, uint16_t *v_out_setting, uint16_t *v_out, uint16_t *i_out, uint16_t *i_limit, uint8_t *power_enabled) { command_t cmd; uint8_t status; - DECLARE_UNPACK(payload, length); - UNPACK8(cmd); - UNPACK8(status); - UNPACK16(*v_in); - UNPACK16(*v_out_setting); - UNPACK16(*v_out); - UNPACK16(*i_out); - UNPACK16(*i_limit); - UNPACK8(*power_enabled); + + start_frame_unpacking(frame); + unpack8(frame, &cmd); + unpack8(frame, &status); + unpack16(frame, v_in); + unpack16(frame, v_out_setting); + unpack16(frame, v_out); + unpack16(frame, i_out); + unpack16(frame, i_limit); + unpack8(frame, power_enabled); *power_enabled = !!(*power_enabled); (void) status; - return _remain == 0 && cmd == (cmd_response | cmd_query); + + return frame->length == 0 && cmd == (cmd_response | cmd_query); } -bool protocol_unpack_wifi_status(uint8_t *payload, uint32_t length, wifi_status_t *status) +bool protocol_unpack_wifi_status(frame_t *frame, wifi_status_t *status) { command_t cmd; - DECLARE_UNPACK(payload, length); - UNPACK8(cmd); - UNPACK8(*status); - return _remain == 0 && cmd == cmd_wifi_status; + + start_frame_unpacking(frame); + unpack8(frame, &cmd); + unpack8(frame, status); + + return frame->length == 0 && cmd == cmd_wifi_status; } -bool protocol_unpack_lock(uint8_t *payload, uint32_t length, uint8_t *locked) +bool protocol_unpack_lock(frame_t *frame, uint8_t *locked) { command_t cmd; - DECLARE_UNPACK(payload, length); - UNPACK8(cmd); - UNPACK8(*locked); + + start_frame_unpacking(frame); + unpack8(frame, &cmd); + unpack8(frame, locked); *locked = !!(*locked); - return _remain == 0 && cmd == cmd_lock; + + return frame->length == 0 && cmd == cmd_lock; } -bool protocol_unpack_upgrade_start(uint8_t *payload, uint32_t length, uint16_t *chunk_size, uint16_t *crc) +bool protocol_unpack_upgrade_start(frame_t *frame, uint16_t *chunk_size, uint16_t *crc) { command_t cmd; - DECLARE_UNPACK(payload, length); - UNPACK8(cmd); - UNPACK16(*chunk_size); - UNPACK16(*crc); - return _remain == 0 && cmd == cmd_upgrade_start; + + start_frame_unpacking(frame); + unpack8(frame, &cmd); + unpack16(frame, chunk_size); + unpack16(frame, crc); + + return frame->length == 0 && cmd == cmd_upgrade_start; } -bool protocol_unpack_ocp(uint8_t *payload, uint32_t length, uint16_t *i_cut) +bool protocol_unpack_ocp(frame_t *frame, uint16_t *i_cut) { command_t cmd; - DECLARE_UNPACK(payload, length); - UNPACK8(cmd); - UNPACK16(*i_cut); - return _remain == 0 && cmd == cmd_ocp_event; + + start_frame_unpacking(frame); + unpack8(frame, &cmd); + unpack16(frame, i_cut); + + return frame->length == 0 && cmd == cmd_ocp_event; } diff --git a/opendps/protocol.h b/opendps/protocol.h index b96519a5..d3f627d0 100644 --- a/opendps/protocol.h +++ b/opendps/protocol.h @@ -35,6 +35,7 @@ #include #include +#include "uframe.h" typedef enum { cmd_ping = 1, @@ -98,9 +99,6 @@ typedef enum { sp_illegal_value } set_parameter_status_t; - -#define MAX_FRAME_LENGTH (2*16) // Based on the cmd_status reponse frame (fully escaped) - #define INVALID_TEMPERATURE (0xffff) /* @@ -111,16 +109,16 @@ typedef enum { * the frame buffer is not sufficient, the return value will be zero and the * 'frame' buffer is left untouched. */ -uint32_t protocol_create_response(uint8_t *frame, uint32_t length, command_t cmd, uint8_t success); -uint32_t protocol_create_ping(uint8_t *frame, uint32_t length); -uint32_t protocol_create_power_enable(uint8_t *frame, uint32_t length, uint8_t enable); -uint32_t protocol_create_vout(uint8_t *frame, uint32_t length, uint16_t vout_mv); -uint32_t protocol_create_ilimit(uint8_t *frame, uint32_t length, uint16_t ilimit_ma); -uint32_t protocol_create_status(uint8_t *frame, uint32_t length); -uint32_t protocol_create_query_response(uint8_t *frame, uint32_t length, uint16_t v_in, uint16_t v_out_setting, uint16_t v_out, uint16_t i_out, uint16_t i_limit, uint8_t power_enabled); -uint32_t protocol_create_wifi_status(uint8_t *frame, uint32_t length, wifi_status_t status); -uint32_t protocol_create_lock(uint8_t *frame, uint32_t length, uint8_t locked); -uint32_t protocol_create_ocp(uint8_t *frame, uint32_t length, uint16_t i_cut); +void protocol_create_response(frame_t *frame, command_t cmd, uint8_t success); +void protocol_create_ping(frame_t *frame); +void protocol_create_power_enable(frame_t *frame, uint8_t enable); +void protocol_create_vout(frame_t *frame, uint16_t vout_mv); +void protocol_create_ilimit(frame_t *frame, uint16_t ilimit_ma); +void protocol_create_status(frame_t *frame); +void protocol_create_query_response(frame_t *frame, uint16_t v_in, uint16_t v_out_setting, uint16_t v_out, uint16_t i_out, uint16_t i_limit, uint8_t power_enabled); +void protocol_create_wifi_status(frame_t *frame, wifi_status_t status); +void protocol_create_lock(frame_t *frame, uint8_t locked); +void protocol_create_ocp(frame_t *frame, uint16_t i_cut); /* * Helpers for unpacking frames. @@ -129,15 +127,15 @@ uint32_t protocol_create_ocp(uint8_t *frame, uint32_t length, uint16_t i_cut); * true. If the command byte of the frame does not match the expectation or the * frame is too short to unpack the expected payload, false will be returned. */ -bool protocol_unpack_response(uint8_t *payload, uint32_t length, command_t *cmd, uint8_t *success); -bool protocol_unpack_power_enable(uint8_t *payload, uint32_t length, uint8_t *enable); -bool protocol_unpack_vout(uint8_t *payload, uint32_t length, uint16_t *vout_mv); -bool protocol_unpack_ilimit(uint8_t *payload, uint32_t length, uint16_t *ilimit_ma); -bool protocol_unpack_query_response(uint8_t *payload, uint32_t length, uint16_t *v_in, uint16_t *v_out_setting, uint16_t *v_out, uint16_t *i_out, uint16_t *i_limit, uint8_t *power_enabled); -bool protocol_unpack_wifi_status(uint8_t *payload, uint32_t length, wifi_status_t *status); -bool protocol_unpack_lock(uint8_t *payload, uint32_t length, uint8_t *locked); -bool protocol_unpack_ocp(uint8_t *payload, uint32_t length, uint16_t *i_cut); -bool protocol_unpack_upgrade_start(uint8_t *payload, uint32_t length, uint16_t *chunk_size, uint16_t *crc); +bool protocol_unpack_response(frame_t *frame, command_t *cmd, uint8_t *success); +bool protocol_unpack_power_enable(frame_t *frame, uint8_t *enable); +bool protocol_unpack_vout(frame_t *frame, uint16_t *vout_mv); +bool protocol_unpack_ilimit(frame_t *frame, uint16_t *ilimit_ma); +bool protocol_unpack_query_response(frame_t *frame, uint16_t *v_in, uint16_t *v_out_setting, uint16_t *v_out, uint16_t *i_out, uint16_t *i_limit, uint8_t *power_enabled); +bool protocol_unpack_wifi_status(frame_t *frame, wifi_status_t *status); +bool protocol_unpack_lock(frame_t *frame, uint8_t *locked); +bool protocol_unpack_ocp(frame_t *frame, uint16_t *i_cut); +bool protocol_unpack_upgrade_start(frame_t *frame, uint16_t *chunk_size, uint16_t *crc); /* diff --git a/opendps/protocol_handler.c b/opendps/protocol_handler.c index 2764f7b9..5ce3f08b 100644 --- a/opendps/protocol_handler.c +++ b/opendps/protocol_handler.c @@ -42,7 +42,7 @@ #include "opendps.h" #ifdef DPS_EMULATOR - extern void dps_emul_send_frame(uint8_t *frame, uint32_t length); + extern void dps_emul_send_frame(frame_t *frame); #endif // DPS_EMULATOR typedef enum { @@ -51,7 +51,7 @@ typedef enum { cmd_success_but_i_actually_sent_my_own_status_thank_you_very_much, } command_status_t; -static uint8_t frame_buffer[FRAME_OVERHEAD(MAX_FRAME_LENGTH)]; +static uint8_t frame_buffer[MAX_FRAME_LENGTH]; static uint32_t rx_idx = 0; static bool receiving_frame = false; @@ -61,17 +61,15 @@ static bool receiving_frame = false; * @param length length of frame * @retval None */ -static void send_frame(uint8_t *frame, uint32_t length) +static void send_frame(const frame_t *frame) { #ifdef DPS_EMULATOR - dps_emul_send_frame(frame, length); + dps_emul_send_frame(frame); #else // DPS_EMULATOR - do { - usart_send_blocking(USART1, *frame); - frame++; - } while(--length); + for (uint32_t i = 0; i < frame->length; ++i) + usart_send_blocking(USART1, frame->buffer[i]); #endif // DPS_EMULATOR - } +} /** * @brief Handle a query command @@ -96,35 +94,40 @@ static command_status_t handle_query(void) bool temp_shutdown; opendps_get_temperature(&temp1, &temp2, &temp_shutdown); // uint32_t len = protocol_create_query_response(frame_buffer, sizeof(frame_buffer), v_in, v_out_setting, v_out, i_out, i_limit, power_enabled); - DECLARE_FRAME(64); - PACK8(cmd_response | cmd_query); - PACK8(1); // Always success - PACK16(v_in); + + frame_t frame; + + set_frame_header(&frame); + pack8(&frame, cmd_response | cmd_query); + + + pack8(&frame, 1); // Always success + pack16(&frame, v_in); emu_printf("v_in = %d\n", v_in); - PACK16(v_out); + pack16(&frame, v_out); emu_printf("v_out = %d\n", v_out); - PACK16(i_out); + pack16(&frame, i_out); emu_printf("i_out = %d\n", i_out); - PACK8(output_enabled); + pack8(&frame, output_enabled); emu_printf("output_enabled = %d\n", output_enabled); - PACK16(temp1); - PACK16(temp2); - PACK8(temp_shutdown); - PACK_CSTR(curr_func); + pack16(&frame, temp1); + pack16(&frame, temp2); + pack8(&frame, temp_shutdown); + pack_cstr(&frame, curr_func); emu_printf("%s:\n", curr_func); for (uint32_t i=0; i < num_param; i++) { opendps_get_curr_function_param_value(params[i].name, value, sizeof(value)); emu_printf(" %s = %s\n" , params[i].name, value); - PACK_CSTR(params[i].name); - PACK_CSTR(value); + pack_cstr(&frame, params[i].name); + pack_cstr(&frame, value); } - FINISH_FRAME(); + end_frame(&frame); - send_frame(_buffer, _length); + send_frame(&frame); return cmd_success_but_i_actually_sent_my_own_status_thank_you_very_much; } -static command_status_t handle_set_function(uint8_t *payload, uint32_t payload_len) +static command_status_t handle_set_function(frame_t *frame) { emu_printf("%s\n", __FUNCTION__); uint32_t i = 0; @@ -134,13 +137,13 @@ static command_status_t handle_set_function(uint8_t *payload, uint32_t payload_l memset(func_name, 0, sizeof(func_name)); { command_t cmd; - DECLARE_UNPACK(payload, payload_len); - UNPACK8(cmd); + start_frame_unpacking(frame); + unpack8(frame, &cmd); (void) cmd; do { - UNPACK8(ch); + unpack8(frame, &ch); func_name[i++] = ch; - } while(i < sizeof(func_name) - 1 && ch && _remain); + } while(i < sizeof(func_name) - 1 && ch && frame->length); char *names[8]; uint32_t num_funcs = opendps_get_function_names(names, 8); @@ -159,11 +162,12 @@ static command_status_t handle_set_function(uint8_t *payload, uint32_t payload_l } { - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_response | cmd_set_function); - PACK8(success); // Always success - FINISH_FRAME(); - send_frame(_buffer, _length); + frame_t frame_resp; + set_frame_header(&frame_resp); + pack8(&frame_resp, cmd_response | cmd_set_function); + pack8(&frame_resp, success); // Always success + end_frame(&frame_resp); + send_frame(&frame_resp); } return cmd_success_but_i_actually_sent_my_own_status_thank_you_very_much; } @@ -174,19 +178,20 @@ static command_status_t handle_list_functions(void) char *names[OPENDPS_MAX_PARAMETERS]; uint32_t num_funcs = opendps_get_function_names(names, OPENDPS_MAX_PARAMETERS); emu_printf("Got %d functions\n" , num_funcs); - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_response | cmd_list_functions); - PACK8(1); // Always success + frame_t frame; + set_frame_header(&frame); + pack8(&frame, cmd_response | cmd_list_functions); + pack8(&frame, 1); // Always success for (uint32_t i=0; i < num_funcs; i++) { emu_printf(" %s\n" , names[i]); - PACK_CSTR(names[i]); + pack_cstr(&frame, names[i]); } - FINISH_FRAME(); - send_frame(_buffer, _length); + end_frame(&frame); + send_frame(&frame); return cmd_success_but_i_actually_sent_my_own_status_thank_you_very_much; } -static command_status_t handle_set_parameters(uint8_t *payload, uint32_t payload_len) +static command_status_t handle_set_parameters(frame_t *frame) { emu_printf("%s\n", __FUNCTION__); char *name = 0, *value = 0; @@ -194,39 +199,40 @@ static command_status_t handle_set_parameters(uint8_t *payload, uint32_t payload set_param_status_t stats[OPENDPS_MAX_PARAMETERS]; uint32_t status_index = 0; { - DECLARE_UNPACK(payload, payload_len); - UNPACK8(cmd); + start_frame_unpacking(frame); + unpack8(frame, &cmd); (void) cmd; do { /** Extract all occurences of =\0 ... */ name = value = 0; /** This is quite ugly, please don't look */ - name = (char*) &_buffer[_pos]; - _pos += strlen(name) + 1; - _remain -= strlen(name) + 1; - value = (char*) &_buffer[_pos]; - _pos += strlen(value) + 1; - _remain -= strlen(value) + 1; + name = (char*) &frame->buffer[frame->unpack_pos]; + frame->unpack_pos += strlen(name) + 1; + frame->length -= strlen(name) + 1; + value = (char*) &frame->buffer[frame->unpack_pos]; + frame->unpack_pos += strlen(value) + 1; + frame->length -= strlen(value) + 1; if (name && value) { stats[status_index++] = opendps_set_parameter(name, value); } - } while(_remain && status_index < OPENDPS_MAX_PARAMETERS); + } while(frame->length && status_index < OPENDPS_MAX_PARAMETERS); } { - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_response | cmd_set_parameters); - PACK8(1); // Always success + frame_t frame_resp; + set_frame_header(&frame_resp); + pack8(&frame_resp, cmd_response | cmd_set_parameters); + pack8(&frame_resp, 1); // Always success for (uint32_t i = 0; i < status_index; i++) { - PACK8(stats[i]); + pack8(&frame_resp, stats[i]); } - FINISH_FRAME(); - send_frame(_buffer, _length); + end_frame(&frame_resp); + send_frame(&frame_resp); } return cmd_success_but_i_actually_sent_my_own_status_thank_you_very_much; } -static command_status_t handle_set_calibration(uint8_t *payload, uint32_t payload_len) +static command_status_t handle_set_calibration(frame_t *frame) { emu_printf("%s\n", __FUNCTION__); char *name = 0; @@ -235,34 +241,35 @@ static command_status_t handle_set_calibration(uint8_t *payload, uint32_t payloa set_param_status_t stats[OPENDPS_MAX_PARAMETERS]; uint32_t status_index = 0; { - DECLARE_UNPACK(payload, payload_len); - UNPACK8(cmd); + start_frame_unpacking(frame); + unpack8(frame, &cmd); (void) cmd; do { /** Extract all occurences of = ... */ name = 0; /** This is quite ugly (again), please don't look */ - name = (char*) &_buffer[_pos]; - _pos += strlen(name) + sizeof(char); - _remain -= strlen(name) + sizeof(char); - value = (float*) &_buffer[_pos]; - _pos += sizeof(float); - _remain -= sizeof(float); + name = (char*) &frame->buffer[frame->unpack_pos]; + frame->unpack_pos += strlen(name) + sizeof(char); + frame->length -= strlen(name) + sizeof(char); + value = (float*) &frame->buffer[frame->unpack_pos]; + frame->unpack_pos += sizeof(float); + frame->length -= sizeof(float); if (name && value) { stats[status_index++] = opendps_set_calibration(name, value); } - } while(_remain && status_index < OPENDPS_MAX_PARAMETERS); + } while(frame->length && status_index < OPENDPS_MAX_PARAMETERS); } { - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_response | cmd_set_calibration); - PACK8(1); // Always success + frame_t frame_resp; + set_frame_header(&frame_resp); + pack8(&frame_resp, cmd_response | cmd_set_calibration); + pack8(&frame_resp, 1); // Always success for (uint32_t i = 0; i < status_index; i++) { - PACK8(stats[i]); + pack8(&frame_resp, stats[i]); } - FINISH_FRAME(); - send_frame(_buffer, _length); + end_frame(&frame_resp); + send_frame(&frame_resp); } return cmd_success_but_i_actually_sent_my_own_status_thank_you_very_much; } @@ -275,33 +282,34 @@ static command_status_t handle_list_parameters(void) const char* name = opendps_get_curr_function_name(); emu_printf("Got %d parameters for %s\n" , num_param, name); - DECLARE_FRAME(MAX_FRAME_LENGTH); - PACK8(cmd_response | cmd_list_parameters); - PACK8(1); // Always success + frame_t frame; + set_frame_header(&frame); + pack8(&frame, cmd_response | cmd_list_parameters); + pack8(&frame, 1); // Always success /** Pack name of current function */ - PACK_CSTR(name); + pack_cstr(&frame, name); for (uint32_t i=0; i < num_param; i++) { emu_printf(" %s %d %d\n", params[i].name, params[i].unit, params[i].prefix); - PACK_CSTR(params[i].name); - PACK8(params[i].unit); - PACK8(params[i].prefix); + pack_cstr(&frame, params[i].name); + pack8(&frame, params[i].unit); + pack8(&frame, params[i].prefix); } - FINISH_FRAME(); - send_frame(_buffer, _length); + end_frame(&frame); + send_frame(&frame); return cmd_success_but_i_actually_sent_my_own_status_thank_you_very_much; } -static command_status_t handle_enable_output(uint8_t *payload, uint32_t payload_len) +static command_status_t handle_enable_output(frame_t *frame) { emu_printf("%s\n", __FUNCTION__); uint8_t cmd; - bool enable; - DECLARE_UNPACK(payload, payload_len); - UNPACK8(cmd); + uint8_t enable_byte; + start_frame_unpacking(frame); + unpack8(frame, &cmd); (void) cmd; - UNPACK8(enable); - enable = !!enable; + unpack8(frame, &enable_byte); + bool enable = !!enable_byte; if (opendps_enable_output(enable)) { return cmd_success; } else { @@ -309,30 +317,30 @@ static command_status_t handle_enable_output(uint8_t *payload, uint32_t payload_ } } -static command_status_t handle_set_brightness(uint8_t *payload, uint32_t payload_len) +static command_status_t handle_set_brightness(frame_t *frame) { emu_printf("%s\n", __FUNCTION__); uint8_t cmd; uint8_t brightness_pct; - DECLARE_UNPACK(payload, payload_len); - UNPACK8(cmd); + start_frame_unpacking(frame); + unpack8(frame, &cmd); (void) cmd; - UNPACK8(brightness_pct); + unpack8(frame, &brightness_pct); hw_set_backlight(brightness_pct); return cmd_success; } -static command_status_t handle_temperature(uint8_t *payload, uint32_t payload_len) +static command_status_t handle_temperature(frame_t *frame) { emu_printf("%s\n", __FUNCTION__); command_t cmd; int16_t temp1, temp2; - DECLARE_UNPACK(payload, payload_len); - UNPACK8(cmd); + start_frame_unpacking(frame); + unpack8(frame, &cmd); (void) cmd; - UNPACK16(temp1); - UNPACK16(temp2); + unpack16(frame, (uint16_t*) &temp1); + unpack16(frame, (uint16_t*) &temp2); opendps_set_temperature(temp1, temp2); return cmd_success; } @@ -348,23 +356,24 @@ static command_status_t handle_version(void) boot_str_len = opendps_get_boot_git_hash(&boot_git_hash); app_str_len = opendps_get_app_git_hash(&app_git_hash); - DECLARE_FRAME(boot_str_len + app_str_len + 4); - PACK8(cmd_response | cmd_version); + frame_t frame; + set_frame_header(&frame); + pack8(&frame, cmd_response | cmd_version); if (boot_str_len > 0 && app_str_len > 0) { - PACK8(1); - PACK_CSTR(boot_git_hash); - PACK_CSTR(app_git_hash); + pack8(&frame, 1); + pack_cstr(&frame, boot_git_hash); + pack_cstr(&frame, app_git_hash); } else { - PACK8(0); - PACK8('\0'); PACK8('\0'); /** Pack two empty strings */ + pack8(&frame, 0); + pack8(&frame, '\0'); pack8(&frame, '\0'); /** Pack two empty strings */ } - FINISH_FRAME(); + end_frame(&frame); - send_frame(_buffer, _length); + send_frame(&frame); return cmd_success_but_i_actually_sent_my_own_status_thank_you_very_much; } @@ -377,26 +386,27 @@ static command_status_t handle_cal_report(void) uint16_t i_out_raw, v_in_raw, v_out_raw; hw_get_adc_values(&i_out_raw, &v_in_raw, &v_out_raw); - DECLARE_FRAME(64); - PACK8(cmd_response | cmd_cal_report); - PACK8(1); - PACK16(v_out_raw); - PACK16(v_in_raw); - PACK16(i_out_raw); - PACK16(DAC_DHR12R2); - PACK16(DAC_DHR12R1); - PACKFLOAT(a_adc_k_coef); - PACKFLOAT(a_adc_c_coef); - PACKFLOAT(a_dac_k_coef); - PACKFLOAT(a_dac_c_coef); - PACKFLOAT(v_adc_k_coef); - PACKFLOAT(v_adc_c_coef); - PACKFLOAT(v_dac_k_coef); - PACKFLOAT(v_dac_c_coef); - PACKFLOAT(vin_adc_k_coef); - PACKFLOAT(vin_adc_c_coef); - FINISH_FRAME(); - send_frame(_buffer, _length); + frame_t frame; + set_frame_header(&frame); + pack8(&frame, cmd_response | cmd_cal_report); + pack8(&frame, 1); + pack16(&frame, v_out_raw); + pack16(&frame, v_in_raw); + pack16(&frame, i_out_raw); + pack16(&frame, DAC_DHR12R2); + pack16(&frame, DAC_DHR12R1); + pack_float(&frame, a_adc_k_coef); + pack_float(&frame, a_adc_c_coef); + pack_float(&frame, a_dac_k_coef); + pack_float(&frame, a_dac_c_coef); + pack_float(&frame, v_adc_k_coef); + pack_float(&frame, v_adc_c_coef); + pack_float(&frame, v_dac_k_coef); + pack_float(&frame, v_dac_c_coef); + pack_float(&frame, vin_adc_k_coef); + pack_float(&frame, vin_adc_c_coef); + end_frame(&frame); + send_frame(&frame); return cmd_success_but_i_actually_sent_my_own_status_thank_you_very_much; } @@ -418,12 +428,12 @@ static command_status_t handle_clear_calibration(void) * @param payload_len length of payload * @retval command_status_t failed, success or "I sent my own frame" */ -static command_status_t handle_wifi_status(uint8_t *payload, uint32_t payload_len) +static command_status_t handle_wifi_status(frame_t *frame) { emu_printf("%s\n", __FUNCTION__); command_status_t success = cmd_failed; wifi_status_t status; - if (protocol_unpack_wifi_status(payload, payload_len, &status)) { + if (protocol_unpack_wifi_status(frame, &status)) { success = cmd_success; opendps_update_wifi_status(status); } @@ -436,12 +446,12 @@ static command_status_t handle_wifi_status(uint8_t *payload, uint32_t payload_le * @param payload_len length of payload * @retval command_status_t failed, success or "I sent my own frame" */ -static command_status_t handle_lock(uint8_t *payload, uint32_t payload_len) +static command_status_t handle_lock(frame_t *frame) { emu_printf("%s\n", __FUNCTION__); command_status_t success = cmd_failed; uint8_t status; - if (protocol_unpack_lock(payload, payload_len, &status)) { + if (protocol_unpack_lock(frame, &status)) { success = cmd_success; opendps_lock(status); } @@ -454,12 +464,12 @@ static command_status_t handle_lock(uint8_t *payload, uint32_t payload_len) * @param payload_len length of payload * @retval false in case of errors, if successful the device reboots */ -static command_status_t handle_upgrade_start(uint8_t *payload, uint32_t payload_len) +static command_status_t handle_upgrade_start(frame_t *frame) { emu_printf("%s\n", __FUNCTION__); command_status_t success = cmd_failed; uint16_t chunk_size, crc; - if (protocol_unpack_upgrade_start(payload, payload_len, &chunk_size, &crc)) { + if (protocol_unpack_upgrade_start(frame, &chunk_size, &crc)) { bootcom_put(0xfedebeda, (chunk_size << 16) | crc); opendps_upgrade_start(); } @@ -472,14 +482,14 @@ static command_status_t handle_upgrade_start(uint8_t *payload, uint32_t payload_ * @param payload_len length of payload * @retval false in case of errors, if successful the device reboots */ -static command_status_t handle_change_screen(uint8_t *payload, uint32_t payload_len) +static command_status_t handle_change_screen(frame_t *frame) { emu_printf("%s\n", __FUNCTION__); uint8_t cmd, screen_id; - DECLARE_UNPACK(payload, payload_len); - UNPACK8(cmd); + start_frame_unpacking(frame); + unpack8(frame, &cmd); (void) cmd; - UNPACK8(screen_id); + unpack8(frame, &screen_id); if (opendps_change_screen(screen_id)) { return cmd_success; } else { @@ -493,17 +503,19 @@ static command_status_t handle_change_screen(uint8_t *payload, uint32_t payload_ * @param length length of frame * @retval None */ -static void handle_frame(uint8_t *frame, uint32_t length) +static void handle_frame(uint8_t *data, uint32_t length) { command_status_t success = cmd_failed; command_t cmd = cmd_response; - uint8_t *payload; - int32_t payload_len = uframe_extract_payload(frame, length); - payload = frame; // Why? Well, frame now points to the payload + + frame_t frame; + + int32_t payload_len = uframe_extract_payload(&frame, data, length); + if (payload_len <= 0) { dbg_printf("Frame error %ld\n", payload_len); } else { - cmd = frame[0]; + cmd = frame.buffer[0]; switch(cmd) { case cmd_ping: success = 1; // Response will be sent below @@ -511,13 +523,13 @@ static void handle_frame(uint8_t *frame, uint32_t length) opendps_handle_ping(); break; case cmd_set_function: - success = handle_set_function(payload, payload_len); + success = handle_set_function(&frame); break; case cmd_list_functions: success = handle_list_functions(); break; case cmd_set_parameters: - success = handle_set_parameters(payload, payload_len); + success = handle_set_parameters(&frame); break; case cmd_list_parameters: success = handle_list_parameters(); @@ -526,19 +538,19 @@ static void handle_frame(uint8_t *frame, uint32_t length) success = handle_query(); break; case cmd_wifi_status: - success = handle_wifi_status(payload, payload_len); + success = handle_wifi_status(&frame); break; case cmd_lock: - success = handle_lock(payload, payload_len); + success = handle_lock(&frame); break; case cmd_upgrade_start: - success = handle_upgrade_start(payload, payload_len); + success = handle_upgrade_start(&frame); break; case cmd_enable_output: - success = handle_enable_output(payload, payload_len); + success = handle_enable_output(&frame); break; case cmd_temperature_report: - success = handle_temperature(payload, payload_len); + success = handle_temperature(&frame); break; case cmd_version: success = handle_version(); @@ -547,16 +559,16 @@ static void handle_frame(uint8_t *frame, uint32_t length) success = handle_cal_report(); break; case cmd_set_calibration: - success = handle_set_calibration(payload, payload_len); + success = handle_set_calibration(&frame); break; case cmd_clear_calibration: success = handle_clear_calibration(); break; case cmd_change_screen: - success = handle_change_screen(payload, payload_len); + success = handle_change_screen(&frame); break; case cmd_set_brightness: - success = handle_set_brightness(payload, payload_len); + success = handle_set_brightness(&frame); break; default: emu_printf("Got unknown command %d (0x%02x)\n", cmd, cmd); @@ -564,9 +576,10 @@ static void handle_frame(uint8_t *frame, uint32_t length) } } if (success != cmd_success_but_i_actually_sent_my_own_status_thank_you_very_much) { - length = protocol_create_response(frame_buffer, sizeof(frame_buffer), cmd, success); - if (length > 0 && cmd != cmd_response) { - send_frame(frame_buffer, length); + frame_t frame_resp; + protocol_create_response(&frame_resp, cmd, success); + if (frame_resp.length > 0 && cmd != cmd_response) { + send_frame(&frame_resp); } } } diff --git a/opendps/uframe.c b/opendps/uframe.c index f52dc085..01dbb2e6 100644 --- a/opendps/uframe.c +++ b/opendps/uframe.c @@ -24,9 +24,160 @@ #include #include +#include #include "uframe.h" #include "crc16.h" +void set_frame_header(frame_t *frame) +{ + frame->buffer[0] = _SOF; + frame->length = 1; + frame->crc = 0; + frame->unpack_pos = 0; +} + +void start_frame_unpacking(frame_t *frame) +{ + frame->unpack_pos = 0; +} + +/** Pack a byte with stuffing and crc updating */ +void pack8(frame_t *frame, uint8_t data) +{ + if (data == _SOF || data == _DLE || data == _EOF) { + if (frame->length + 2 >= MAX_FRAME_LENGTH) + goto overflow; + + frame->buffer[frame->length++] = _DLE; + frame->buffer[frame->length++] = data ^ _XOR; + } else { + if (frame->length + 1 >= MAX_FRAME_LENGTH) + goto overflow; + + frame->buffer[frame->length++] = data; + } + + frame->crc = crc16_add(frame->crc, data); + + return; + +overflow: + dbg_printf("uFrame overflow at %s:%d\n", __FILE__, __LINE__); +} + +/** Like pack8 but does not compute crc */ +void stuff8(frame_t *frame, uint8_t data) +{ + if (data == _SOF || data == _DLE || data == _EOF) { + if (frame->length + 2 >= MAX_FRAME_LENGTH) + goto overflow; + + frame->buffer[frame->length++] = _DLE; + frame->buffer[frame->length++] = data ^ _XOR; + } else { + if (frame->length >= MAX_FRAME_LENGTH) + goto overflow; + + frame->buffer[frame->length++] = data; + } + + return; + +overflow: + dbg_printf("uFrame overflow at %s:%d\n", __FILE__, __LINE__); +} + +void pack16(frame_t *frame, uint16_t data) +{ + pack8(frame, data >> 8); + pack8(frame, data & 0xFF); +} + +void pack32(frame_t *frame, uint32_t data) +{ + pack8(frame, data >> 24); + pack8(frame, (data >> 16) & 0xFF); + pack8(frame, (data >> 8) & 0xFF); + pack8(frame, data & 0xFF); +} + +void pack_float(frame_t *frame, float data) +{ + union { + float f; + uint32_t u32; + } overlay; + + overlay.f = data; + pack32(frame, overlay.u32); +} + +/** Pack a c string with null terminator */ +void pack_cstr(frame_t *frame, const char *data) +{ + while(*data) { + pack8(frame, *data); + data++; + } + pack8(frame, '\0'); +} + +/** Finish frame. Store crc and EOF */ +void end_frame(frame_t *frame) +{ + stuff8(frame, (frame->crc >> 8) & 0xff); + stuff8(frame, frame->crc & 0xff); + if (frame->length < MAX_FRAME_LENGTH) { + frame->buffer[frame->length++] = _EOF; + } +} + +uint32_t unpack8(frame_t *frame, uint8_t *data) +{ + if (frame->length >= 1) { + frame->length--; + *data = frame->buffer[frame->unpack_pos++]; + return 1; + } + + dbg_printf("uFrame underflow at %s:%d\n", __FILE__, __LINE__); + *data = 0; + return 0; +} + +uint32_t unpack16(frame_t *frame, uint16_t *data) +{ + uint32_t bytes_read; + uint8_t u8; + + bytes_read = unpack8(frame, &u8); + *data = ((uint16_t) u8) << 8; + + bytes_read += unpack8(frame, &u8); + *data |= ((uint16_t) u8); + + return bytes_read; +} + +uint32_t unpack32(frame_t *frame, uint32_t *data) +{ + uint32_t bytes_read; + uint8_t u8; + + bytes_read = unpack8(frame, &u8); + *data = ((uint32_t) u8) << 24; + + bytes_read += unpack8(frame, &u8); + *data |= ((uint32_t) u8) << 16; + + bytes_read += unpack8(frame, &u8); + *data |= ((uint32_t) u8) << 8; + + bytes_read += unpack8(frame, &u8); + *data |= ((uint32_t) u8); + + return bytes_read; +} /** * @brief Extract payload from frame following deframing, unescaping and @@ -38,17 +189,20 @@ * @param length length of frame * @retval length of payload or -E_* in case of errors (see uframe.h) */ -int32_t uframe_extract_payload(uint8_t *frame, uint32_t length) +int32_t uframe_extract_payload(frame_t *frame, uint8_t *data, uint32_t length) { int32_t status = -1; bool seen_dle = false; uint16_t frame_crc, calc_crc = 0; + + frame->length = 0; + do { if (length < 5) { // _SOF CRC16 _EOF is no usable frame status = -E_LEN; break; } - if (frame[0] != _SOF || frame[length-1] != _EOF) { + if (data[0] != _SOF || data[length-1] != _EOF) { status = -E_FRM; break; } @@ -58,18 +212,18 @@ int32_t uframe_extract_payload(uint8_t *frame, uint32_t length) for (uint32_t r = 1; r < length-1; r++) { if (seen_dle) { seen_dle = false; - frame[w++] = frame[r] ^_XOR; - } else if (frame[r] == _DLE) { + data[w++] = data[r] ^_XOR; + } else if (data[r] == _DLE) { seen_dle = true; } else { - frame[w++] = frame[r]; + data[w++] = data[r]; } } // Then calculate CRC - calc_crc = crc16((uint8_t*) frame, w - 2); + calc_crc = crc16((uint8_t*) data, w - 2); - frame_crc = (uint16_t) ((frame[w-2] << 8) | frame[w-1]); + frame_crc = (uint16_t) ((data[w-2] << 8) | data[w-1]); if (calc_crc == frame_crc) { status = (int32_t) w - 2; // omit crc from returned length } else { @@ -77,5 +231,11 @@ int32_t uframe_extract_payload(uint8_t *frame, uint32_t length) } } while(0); + if (status != -E_CRC) + { + memcpy(frame->buffer, data, status); + frame->length = status; + } + return status; } diff --git a/opendps/uframe.h b/opendps/uframe.h index dc8db88e..df199706 100644 --- a/opendps/uframe.h +++ b/opendps/uframe.h @@ -25,7 +25,6 @@ #ifndef __UFRAME_H__ #define __UFRAME_H__ -#include "crc16.h" #include "dbg_printf.h" #define _SOF 0x7e @@ -38,120 +37,29 @@ #define E_FRM 2 // Received data has no framing #define E_CRC 3 // CRC mismatch -/** Max size given a payload of 'size' bytes - * (SOF + every byte of payload escaped + 2x escaped crc bytes + EOF) -*/ #define FRAME_OVERHEAD(size) (1 + 2*(size) + 4 + 1) - - -/** Declare a frame of length 'length' for dumping data into */ -#define DECLARE_FRAME(length) \ - uint8_t _buffer[ FRAME_OVERHEAD(length) ]; \ - _buffer[0] = _SOF; \ - uint32_t _length = 1; \ - uint16_t _crc = 0; - -/** Pack a byte with stuffing and crc updating */ -#define PACK8(b) \ - if (_length < sizeof(_buffer)) { \ - _crc = crc16_add(_crc, b); \ - uint8_t _byte = (b); \ - if (_byte == _SOF || _byte == _DLE || _byte == _EOF) { \ - _buffer[_length++] = _DLE; \ - _buffer[_length++] = _byte ^ _XOR; \ - } else { \ - _buffer[_length++] = _byte; \ - } \ - } else { \ - dbg_printf("uFrame overflow at %s:%d\n", __FILE__, __LINE__); \ - } - -/** Pack a c string with null terminator */ -#define PACK_CSTR(s) \ -{ \ - uint8_t *_tmp = (uint8_t*) (s); \ - while(*_tmp) { \ - PACK8(*_tmp); \ - _tmp++; \ - } \ - PACK8(0); \ -} - -#define PACK16(h) \ - PACK8((h) >> 8); \ - PACK8((h) & 0xff); - -#define PACK32(w) \ - PACK8((w) >> 24); \ - PACK8(((w) >> 16) & 0xff); \ - PACK8(((w) >> 8) & 0xff); \ - PACK8((w) & 0xff); - -#define PACKFLOAT(w) \ - { \ - union { \ - float f; \ - uint32_t ui; \ - } overlay; \ - overlay.f = w; \ - PACK32(overlay.ui); \ - } - -/** Like PACK8 but does not compute crc */ -#define STUFF8(b) \ - if (_length+2 < sizeof(_buffer)) { \ - uint8_t _byte = (b); \ - if (_byte == _SOF || _byte == _DLE || _byte == _EOF) { \ - _buffer[_length++] = _DLE; \ - _buffer[_length++] = _byte ^ _XOR; \ - } else { \ - _buffer[_length++] = _byte; \ - } \ - } else { \ - dbg_printf("uFrame overflow at %s:%d\n", __FILE__, __LINE__); \ - } - -/** Finish frame. Store crc and EOF. You now have the frame in _buffer of length _length */ -#define FINISH_FRAME() \ - STUFF8((_crc >> 8) & 0xff); \ - STUFF8(_crc & 0xff); \ - if (_length < sizeof(_buffer)) { \ - _buffer[_length++] = _EOF; \ - } - -/** Declare an unpacker for the frame of given length */ -#define DECLARE_UNPACK(payload, length) \ - uint8_t *_buffer = (uint8_t*) payload; \ - uint32_t _pos = 0; \ - uint32_t _remain = length; - -#define UNPACK8(b) \ - if (_remain >= 1) { \ - (b) = _buffer[_pos++]; \ - _remain--; \ - } else { \ - (b) = 0; \ - } - -#define UNPACK16(h) \ - if (_remain >= 2) { \ - (h) = _buffer[_pos++] << 8; \ - (h) |= _buffer[_pos++]; \ - _remain -= 2; \ - } else { \ - (h) = 0; \ - } - -#define UNPACK32(h) \ - if (_remain >= 4) { \ - (h) = _buffer[_pos++] << 24; \ - (h) |= _buffer[_pos++] << 16; \ - (h) |= _buffer[_pos++] << 8; \ - (h) |= _buffer[_pos++]; \ - _remain -= 4; \ - } else { \ - (h) = 0; \ - } +#define MAX_FRAME_LENGTH (128) + +typedef struct +{ + uint8_t buffer[MAX_FRAME_LENGTH]; + uint32_t length; + uint16_t crc; + uint32_t unpack_pos; +} frame_t; + +void set_frame_header(frame_t *frame); +void end_frame(frame_t *frame); +void start_frame_unpacking(frame_t *frame); +void pack8(frame_t *frame, uint8_t data); +void stuff8(frame_t *frame, uint8_t data); +void pack16(frame_t *frame, uint16_t data); +void pack32(frame_t *frame, uint32_t data); +void pack_float(frame_t *frame, float data); +void pack_cstr(frame_t *frame, const char *data); +uint32_t unpack8(frame_t *frame, uint8_t *data); +uint32_t unpack16(frame_t *frame, uint16_t *data); +uint32_t unpack32(frame_t *frame, uint32_t *data); /** * @brief Extract payload from frame following deframing, unescaping and @@ -159,10 +67,11 @@ * @note The frame is processed 'in place', when exiting the payload will be * the first byte of 'frame'. If the frame contains two valid frames, * -E_CRC will be returned. - * @param frame the frame to process, payload on exit + * @param frame the returned processed frame + * @param data the raw data to be processed * @param length length of frame * @retval length of payload or -E_* in case of errors (see uframe.h) */ -int32_t uframe_extract_payload(uint8_t *frame, uint32_t length); +int32_t uframe_extract_payload(frame_t *frame, uint8_t *data, uint32_t length); #endif // __UFRAME_H__