mirror of https://github.com/rusefi/bldc.git
289 lines
7.3 KiB
C
289 lines
7.3 KiB
C
/*
|
|
Copyright 2016 - 2019 Benjamin Vedder benjamin@vedder.se
|
|
|
|
This file is part of the VESC firmware.
|
|
|
|
The VESC firmware is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
The VESC firmware is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <string.h>
|
|
#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 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_send_packet(unsigned char *data, unsigned int len, int handler_num) {
|
|
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;
|
|
} else if (len <= 65535) {
|
|
handler->tx_buffer[b_ind++] = 3;
|
|
handler->tx_buffer[b_ind++] = len >> 8;
|
|
handler->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;
|
|
}
|
|
|
|
memcpy(handler->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);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* 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);
|
|
}
|
|
}
|
|
}
|
|
|
|
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;
|
|
|
|
// 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;
|
|
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,
|
|
data_len);
|
|
|
|
handler->rx_read_ptr = 0;
|
|
handler->rx_write_ptr = data_len;
|
|
}
|
|
|
|
handler->rx_buffer[handler->rx_write_ptr++] = rx_data;
|
|
data_len++;
|
|
|
|
if (handler->bytes_left > 1) {
|
|
handler->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);
|
|
|
|
// More data is needed
|
|
if (res == -2) {
|
|
break;
|
|
}
|
|
|
|
if (res > 0) {
|
|
data_len -= res;
|
|
handler->rx_read_ptr += res;
|
|
} else if (res == -1) {
|
|
// Something went wrong. Move pointer forward and try again.
|
|
handler->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;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Try if it is possible to decode a packet from a buffer.
|
|
*
|
|
* @param buffer
|
|
* The buffer to try from
|
|
*
|
|
* @param in_len
|
|
* The length of the buffer
|
|
*
|
|
* @param process_func
|
|
* Call this function with the decoded packet on success. Set to null
|
|
* to disable.
|
|
*
|
|
* @param bytes_left
|
|
* This many additional bytes are required to tell more about the packet.
|
|
*
|
|
* @return
|
|
* >0: Success, number of bytes decoded from buffer (not payload length)
|
|
* -1: Invalid structure
|
|
* -2: OK so far, but not enough data
|
|
*/
|
|
static int try_decode_packet(unsigned char *buffer, unsigned int in_len,
|
|
void(*process_func)(unsigned char *data, unsigned int len), int *bytes_left) {
|
|
*bytes_left = 0;
|
|
|
|
if (in_len == 0) {
|
|
*bytes_left = 1;
|
|
return -2;
|
|
}
|
|
|
|
bool is_len_8b = buffer[0] == 2;
|
|
unsigned int data_start = buffer[0];
|
|
|
|
#if PACKET_MAX_PL_LEN > 255
|
|
bool is_len_16b = buffer[0] == 3;
|
|
#else
|
|
#define is_len_16b false
|
|
#endif
|
|
|
|
#if PACKET_MAX_PL_LEN > 65535
|
|
bool is_len_24b = buffer[0] == 4;
|
|
#else
|
|
#define is_len_24b false
|
|
#endif
|
|
|
|
// No valid start byte
|
|
if (!is_len_8b && !is_len_16b && !is_len_24b) {
|
|
return -1;
|
|
}
|
|
|
|
// Not enough data to determine length
|
|
if (in_len < data_start) {
|
|
*bytes_left = data_start - in_len;
|
|
return -2;
|
|
}
|
|
|
|
unsigned int len = 0;
|
|
|
|
if (is_len_8b) {
|
|
len = (unsigned int)buffer[1];
|
|
|
|
// No support for zero length packets
|
|
if (len < 1) {
|
|
return -1;
|
|
}
|
|
} else if (is_len_16b) {
|
|
len = (unsigned int)buffer[1] << 8 | (unsigned int)buffer[2];
|
|
|
|
// A shorter packet should use less length bytes
|
|
if (len < 255) {
|
|
return -1;
|
|
}
|
|
} else if (is_len_24b) {
|
|
len = (unsigned int)buffer[1] << 16 |
|
|
(unsigned int)buffer[2] << 8 |
|
|
(unsigned int)buffer[3];
|
|
|
|
// A shorter packet should use less length bytes
|
|
if (len < 65535) {
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
// Too long packet
|
|
if (len > PACKET_MAX_PL_LEN) {
|
|
return -1;
|
|
}
|
|
|
|
// Need more data to determine rest of packet
|
|
if (in_len < (len + data_start + 3)) {
|
|
*bytes_left = (len + data_start + 3) - in_len;
|
|
return -2;
|
|
}
|
|
|
|
// Invalid stop byte
|
|
if (buffer[data_start + len + 2] != 3) {
|
|
return -1;
|
|
}
|
|
|
|
unsigned short crc_calc = crc16(buffer + data_start, len);
|
|
unsigned short crc_rx = (unsigned short)buffer[data_start + len] << 8
|
|
| (unsigned short)buffer[data_start + len + 1];
|
|
|
|
if (crc_calc == crc_rx) {
|
|
if (process_func) {
|
|
process_func(buffer + data_start, len);
|
|
}
|
|
|
|
return len + data_start + 3;
|
|
} else {
|
|
return -1;
|
|
}
|
|
}
|