Skip to content

Commit

Permalink
Moved packet handler state to caller and updated unit test for packet…
Browse files Browse the repository at this point in the history 
… handler
  • Loading branch information
@vedderb
vedderb committed Jan 12, 2021
1 parent 25f4967 commit b412ac3
Show file tree
Hide file tree
Showing 10 changed files with 90 additions and 142 deletions.
16 changes: 8 additions & 8 deletions applications/app_uartcomm.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,6 @@

// Settings
#define BAUDRATE 115200
#define PACKET_HANDLER 1
#define PACKET_HANDLER_P 2

// Threads
static THD_FUNCTION(packet_process_thread, arg);
Expand All @@ -40,6 +38,7 @@ static volatile bool thread_is_running = false;
static volatile bool uart_is_running = false;
static mutex_t send_mutex;
static bool send_mutex_init_done = false;
static PACKET_STATE_t packet_state_external;

#ifdef HW_UART_P_DEV
static mutex_t send_mutex_p;
Expand All @@ -53,6 +52,7 @@ static void send_packet(unsigned char *data, unsigned int len);
#ifdef HW_UART_P_DEV
static void process_packet_p(unsigned char *data, unsigned int len);
static void send_packet_p(unsigned char *data, unsigned int len);
static PACKET_STATE_t packet_state_permanent;
#endif

static SerialConfig uart_cfg = {
Expand Down Expand Up @@ -102,7 +102,7 @@ static void send_packet_p(unsigned char *data, unsigned int len) {
#endif

void app_uartcomm_start(void) {
packet_init(send_packet, process_packet, PACKET_HANDLER);
packet_init(send_packet, process_packet, &packet_state_external);

if (!thread_is_running) {
chThdCreateStatic(packet_process_thread_wa, sizeof(packet_process_thread_wa),
Expand All @@ -123,7 +123,7 @@ void app_uartcomm_start(void) {

void app_uartcomm_start_permanent(void) {
#ifdef HW_UART_P_DEV
packet_init(send_packet_p, process_packet_p, PACKET_HANDLER_P);
packet_init(send_packet_p, process_packet_p, &packet_state_permanent);

if (!thread_is_running) {
chThdCreateStatic(packet_process_thread_wa, sizeof(packet_process_thread_wa),
Expand Down Expand Up @@ -166,7 +166,7 @@ void app_uartcomm_send_packet(unsigned char *data, unsigned int len) {
}

chMtxLock(&send_mutex);
packet_send_packet(data, len, PACKET_HANDLER);
packet_send_packet(data, len, &packet_state_external);
chMtxUnlock(&send_mutex);
}

Expand All @@ -178,7 +178,7 @@ void app_uartcomm_send_packet_p(unsigned char *data, unsigned int len) {
}

chMtxLock(&send_mutex_p);
packet_send_packet(data, len, PACKET_HANDLER_P);
packet_send_packet(data, len, &packet_state_permanent);
chMtxUnlock(&send_mutex_p);
#else
(void)data;
Expand Down Expand Up @@ -236,7 +236,7 @@ static THD_FUNCTION(packet_process_thread, arg) {
#ifdef HW_UART_P_DEV
from_p_uart = false;
#endif
packet_process_byte(res, PACKET_HANDLER);
packet_process_byte(res, &packet_state_external);
rx = true;
}
}
Expand All @@ -245,7 +245,7 @@ static THD_FUNCTION(packet_process_thread, arg) {
msg_t res = sdGetTimeout(&HW_UART_P_DEV, TIME_IMMEDIATE);
if (res != MSG_TIMEOUT) {
from_p_uart = true;
packet_process_byte(res, PACKET_HANDLER_P);
packet_process_byte(res, &packet_state_permanent);
rx = true;
}
#endif
Expand Down
10 changes: 4 additions & 6 deletions comm_usb.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,6 @@
#include "comm_usb_serial.h"
#include "commands.h"

// Settings
#define PACKET_HANDLER 0

// Private variables
#define SERIAL_RX_BUFFER_SIZE 2048
static uint8_t serial_rx_buffer[SERIAL_RX_BUFFER_SIZE];
Expand All @@ -38,6 +35,7 @@ static mutex_t send_mutex;
static thread_t *process_tp;
static volatile unsigned int write_timeout_cnt = 0;
static volatile bool was_timeout = false;
static PACKET_STATE_t packet_state;

// Private functions
static void process_packet(unsigned char *data, unsigned int len);
Expand Down Expand Up @@ -82,7 +80,7 @@ static THD_FUNCTION(serial_process_thread, arg) {
chEvtWaitAny((eventmask_t) 1);

while (serial_rx_read_pos != serial_rx_write_pos) {
packet_process_byte(serial_rx_buffer[serial_rx_read_pos++], PACKET_HANDLER);
packet_process_byte(serial_rx_buffer[serial_rx_read_pos++], &packet_state);

if (serial_rx_read_pos == SERIAL_RX_BUFFER_SIZE) {
serial_rx_read_pos = 0;
Expand Down Expand Up @@ -118,7 +116,7 @@ static void send_packet_raw(unsigned char *buffer, unsigned int len) {

void comm_usb_init(void) {
comm_usb_serial_init();
packet_init(send_packet_raw, process_packet, PACKET_HANDLER);
packet_init(send_packet_raw, process_packet, &packet_state);

chMtxObjectInit(&send_mutex);

Expand All @@ -129,7 +127,7 @@ void comm_usb_init(void) {

void comm_usb_send_packet(unsigned char *data, unsigned int len) {
chMtxLock(&send_mutex);
packet_send_packet(data, len, PACKET_HANDLER);
packet_send_packet(data, len, &packet_state);
chMtxUnlock(&send_mutex);
}

Expand Down
4 changes: 4 additions & 0 deletions crc.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,9 @@
*/

#include "crc.h"
#ifndef NO_STM32
#include "stm32f4xx.h"
#endif

// CRC Table
const unsigned short crc16_tab[] = { 0x0000, 0x1021, 0x2042, 0x3063, 0x4084,
Expand Down Expand Up @@ -60,6 +62,7 @@ unsigned short crc16(unsigned char *buf, unsigned int len) {
return cksum;
}

#ifndef NO_STM32
/**
* @brief Computes the 32-bit CRC of a given buffer of data word(32-bit) using
* Hardware Acceleration.
Expand All @@ -86,3 +89,4 @@ void crc32_reset(void) {
/* Reset CRC generator */
CRC->CR |= CRC_CR_RESET;
}
#endif
14 changes: 0 additions & 14 deletions main.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,6 @@

// Private variables
static THD_WORKING_AREA(periodic_thread_wa, 1024);
static THD_WORKING_AREA(timer_thread_wa, 128);
static THD_WORKING_AREA(flash_integrity_check_thread_wa, 256);

static THD_FUNCTION(flash_integrity_check_thread, arg) {
Expand Down Expand Up @@ -179,18 +178,6 @@ static THD_FUNCTION(periodic_thread, arg) {
}
}

static THD_FUNCTION(timer_thread, arg) {
(void)arg;

chRegSetThreadName("msec_timer");

for(;;) {
packet_timerfunc();
timeout_feed_WDT(THREAD_TIMER);
chThdSleepMilliseconds(1);
}
}

// When assertions enabled halve PWM frequency. The control loop ISR runs 40% slower
void assert_failed(uint8_t* file, uint32_t line) {
commands_printf("Wrong parameters value: file %s on line %d\r\n", file, line);
Expand Down Expand Up @@ -285,7 +272,6 @@ int main(void) {

// Threads
chThdCreateStatic(periodic_thread_wa, sizeof(periodic_thread_wa), NORMALPRIO, periodic_thread, NULL);
chThdCreateStatic(timer_thread_wa, sizeof(timer_thread_wa), NORMALPRIO, timer_thread, NULL);
chThdCreateStatic(flash_integrity_check_thread_wa, sizeof(flash_integrity_check_thread_wa), LOWPRIO, flash_integrity_check_thread, NULL);

#if WS2811_TEST
Expand Down
134 changes: 46 additions & 88 deletions packet.c
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se
Copyright 2016 - 2021 Benjamin Vedder benjamin@vedder.se
This file is part of the VESC firmware.
Expand All @@ -21,135 +21,93 @@
#include "packet.h"
#include "crc.h"

/**
* The latest update aims at achieving optimal re-synchronization in the
* case if lost data, at the cost of some performance.
*/

// Defines
#define BUFFER_LEN (PACKET_MAX_PL_LEN + 8)

// Private types
typedef struct {
volatile unsigned short rx_timeout;
void(*send_func)(unsigned char *data, unsigned int len);
void(*process_func)(unsigned char *data, unsigned int len);
unsigned int rx_read_ptr;
unsigned int rx_write_ptr;
int bytes_left;
unsigned char rx_buffer[BUFFER_LEN];
unsigned char tx_buffer[BUFFER_LEN];
} PACKET_STATE_t;

// Private variables
static PACKET_STATE_t m_handler_states[PACKET_HANDLERS];

// Private functions
static int try_decode_packet(unsigned char *buffer, unsigned int in_len,
void(*process_func)(unsigned char *data, unsigned int len), int *bytes_left);

void packet_init(void (*s_func)(unsigned char *data, unsigned int len),
void (*p_func)(unsigned char *data, unsigned int len), int handler_num) {
memset(&m_handler_states[handler_num], 0, sizeof(PACKET_STATE_t));
m_handler_states[handler_num].send_func = s_func;
m_handler_states[handler_num].process_func = p_func;
void (*p_func)(unsigned char *data, unsigned int len), PACKET_STATE_t *state) {
memset(state, 0, sizeof(PACKET_STATE_t));
state->send_func = s_func;
state->process_func = p_func;
}

void packet_reset(int handler_num) {
m_handler_states[handler_num].rx_read_ptr = 0;
m_handler_states[handler_num].rx_write_ptr = 0;
m_handler_states[handler_num].bytes_left = 0;
void packet_reset(PACKET_STATE_t *state) {
state->rx_read_ptr = 0;
state->rx_write_ptr = 0;
state->bytes_left = 0;
}

void packet_send_packet(unsigned char *data, unsigned int len, int handler_num) {
void packet_send_packet(unsigned char *data, unsigned int len, PACKET_STATE_t *state) {
if (len == 0 || len > PACKET_MAX_PL_LEN) {
return;
}

int b_ind = 0;
PACKET_STATE_t *handler = &m_handler_states[handler_num];

if (len <= 255) {
handler->tx_buffer[b_ind++] = 2;
handler->tx_buffer[b_ind++] = len;
state->tx_buffer[b_ind++] = 2;
state->tx_buffer[b_ind++] = len;
} else if (len <= 65535) {
handler->tx_buffer[b_ind++] = 3;
handler->tx_buffer[b_ind++] = len >> 8;
handler->tx_buffer[b_ind++] = len & 0xFF;
state->tx_buffer[b_ind++] = 3;
state->tx_buffer[b_ind++] = len >> 8;
state->tx_buffer[b_ind++] = len & 0xFF;
} else {
handler->tx_buffer[b_ind++] = 4;
handler->tx_buffer[b_ind++] = len >> 16;
handler->tx_buffer[b_ind++] = (len >> 8) & 0x0F;
handler->tx_buffer[b_ind++] = len & 0xFF;
state->tx_buffer[b_ind++] = 4;
state->tx_buffer[b_ind++] = len >> 16;
state->tx_buffer[b_ind++] = (len >> 8) & 0x0F;
state->tx_buffer[b_ind++] = len & 0xFF;
}

memcpy(handler->tx_buffer + b_ind, data, len);
memcpy(state->tx_buffer + b_ind, data, len);
b_ind += len;

unsigned short crc = crc16(data, len);
handler->tx_buffer[b_ind++] = (uint8_t)(crc >> 8);
handler->tx_buffer[b_ind++] = (uint8_t)(crc & 0xFF);
handler->tx_buffer[b_ind++] = 3;

if (handler->send_func) {
handler->send_func(handler->tx_buffer, b_ind);
}
}
state->tx_buffer[b_ind++] = (uint8_t)(crc >> 8);
state->tx_buffer[b_ind++] = (uint8_t)(crc & 0xFF);
state->tx_buffer[b_ind++] = 3;

/**
* Call this function every millisecond. This is not strictly necessary
* if the timeout is unimportant.
*/
void packet_timerfunc(void) {
for (int i = 0;i < PACKET_HANDLERS;i++) {
if (m_handler_states[i].rx_timeout) {
m_handler_states[i].rx_timeout--;
} else {
packet_reset(i);
}
if (state->send_func) {
state->send_func(state->tx_buffer, b_ind);
}
}

void packet_process_byte(uint8_t rx_data, int handler_num) {
PACKET_STATE_t *handler = &m_handler_states[handler_num];

handler->rx_timeout = PACKET_RX_TIMEOUT;

unsigned int data_len = handler->rx_write_ptr - handler->rx_read_ptr;
void packet_process_byte(uint8_t rx_data, PACKET_STATE_t *state) {
unsigned int data_len = state->rx_write_ptr - state->rx_read_ptr;

// Out of space (should not happen)
if (data_len >= BUFFER_LEN) {
handler->rx_write_ptr = 0;
handler->rx_read_ptr = 0;
handler->bytes_left = 0;
handler->rx_buffer[handler->rx_write_ptr++] = rx_data;
if (data_len >= PACKET_BUFFER_LEN) {
state->rx_write_ptr = 0;
state->rx_read_ptr = 0;
state->bytes_left = 0;
state->rx_buffer[state->rx_write_ptr++] = rx_data;
return;
}

// Everything has to be aligned, so shift buffer if we are out of space.
// (as opposed to using a circular buffer)
if (handler->rx_write_ptr >= BUFFER_LEN) {
memmove(handler->rx_buffer,
handler->rx_buffer + handler->rx_read_ptr,
if (state->rx_write_ptr >= PACKET_BUFFER_LEN) {
memmove(state->rx_buffer,
state->rx_buffer + state->rx_read_ptr,
data_len);

handler->rx_read_ptr = 0;
handler->rx_write_ptr = data_len;
state->rx_read_ptr = 0;
state->rx_write_ptr = data_len;
}

handler->rx_buffer[handler->rx_write_ptr++] = rx_data;
state->rx_buffer[state->rx_write_ptr++] = rx_data;
data_len++;

if (handler->bytes_left > 1) {
handler->bytes_left--;
if (state->bytes_left > 1) {
state->bytes_left--;
return;
}

// Try decoding the packet at various offsets until it succeeds, or
// until we run out of data.
for (;;) {
int res = try_decode_packet(handler->rx_buffer + handler->rx_read_ptr,
data_len, handler->process_func, &handler->bytes_left);
int res = try_decode_packet(state->rx_buffer + state->rx_read_ptr,
data_len, state->process_func, &state->bytes_left);

// More data is needed
if (res == -2) {
Expand All @@ -158,18 +116,18 @@ void packet_process_byte(uint8_t rx_data, int handler_num) {

if (res > 0) {
data_len -= res;
handler->rx_read_ptr += res;
state->rx_read_ptr += res;
} else if (res == -1) {
// Something went wrong. Move pointer forward and try again.
handler->rx_read_ptr++;
state->rx_read_ptr++;
data_len--;
}
}

// Nothing left, move pointers to avoid memmove
if (data_len == 0) {
handler->rx_read_ptr = 0;
handler->rx_write_ptr = 0;
state->rx_read_ptr = 0;
state->rx_write_ptr = 0;
}
}

Expand Down
Loading

0 comments on commit b412ac3

Please sign in to comment.