ESP8266 part of the f105-motor-demo project (see f105-motor-demo_stm32)
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
f105-motor-demo_esp/sbmp/sbmp_frame.c

443 lines
9.0 KiB

#include "esp8266.h"
#include <stdarg.h>
#include "sbmp_config.h"
#include "sbmp_logging.h"
#include "sbmp_checksum.h"
#include "sbmp_frame.h"
// protos
static void call_frame_rx_callback(SBMP_FrmInst *frm);
/** Allocate the state struct & init all fields */
SBMP_FrmInst FLASH_FN *sbmp_frm_init(
SBMP_FrmInst *frm,
uint8_t *buffer,
uint16_t buffer_size,
void (*rx_handler)(uint8_t *, uint16_t, void *),
void (*tx_func)(uint8_t))
{
#if SBMP_MALLOC
if (frm == NULL) {
// caller wants us to allocate it
frm = malloc(sizeof(SBMP_FrmInst));
}
if (buffer == NULL) {
// caller wants us to allocate it
buffer = malloc(buffer_size);
}
#else
if (frm == NULL || buffer == NULL) {
return NULL; // malloc not enabled, fail
}
#endif
frm->rx_handler = rx_handler;
frm->rx_buffer = buffer;
frm->rx_buffer_cap = buffer_size;
frm->user_token = NULL; // NULL if not set
frm->tx_func = tx_func;
frm->rx_enabled = false;
frm->tx_enabled = false;
sbmp_frm_reset(frm);
return frm;
}
/** Reset the internal state */
void FLASH_FN sbmp_frm_reset(SBMP_FrmInst *frm)
{
sbmp_frm_reset_rx(frm);
sbmp_frm_reset_tx(frm);
}
/** Enable or disable Rx */
void FLASH_FN sbmp_frm_enable_rx(SBMP_FrmInst *frm, bool enable)
{
frm->rx_enabled = enable;
}
/** Enable or disable Tx */
void FLASH_FN sbmp_frm_enable_tx(SBMP_FrmInst *frm, bool enable)
{
frm->tx_enabled = enable;
}
/** Enable or disable both Rx and Tx */
void FLASH_FN sbmp_frm_enable(SBMP_FrmInst *frm, bool enable)
{
sbmp_frm_enable_rx(frm, enable);
sbmp_frm_enable_tx(frm, enable);
}
/** Set user token */
void FLASH_FN sbmp_frm_set_user_token(SBMP_FrmInst *frm, void *token)
{
frm->user_token = token;
}
/** Reset the receiver state */
void FLASH_FN sbmp_frm_reset_rx(SBMP_FrmInst *frm)
{
frm->rx_buffer_i = 0;
frm->rx_length = 0;
frm->mb_buf = 0;
frm->mb_cnt = 0;
frm->rx_hdr_xor = 0;
frm->rx_cksum_scratch = 0;
frm->rx_cksum_type = SBMP_CKSUM_NONE;
frm->rx_status = FRM_STATE_IDLE;
// printf("---- RX RESET STATE ----\n");
}
/** Reset the transmitter state */
void FLASH_FN sbmp_frm_reset_tx(SBMP_FrmInst *frm)
{
frm->tx_status = FRM_STATE_IDLE;
frm->tx_remain = 0;
frm->tx_cksum_scratch = 0;
frm->tx_cksum_type = SBMP_CKSUM_NONE;
// printf("---- TX RESET STATE ----\n");
}
/** Update a header XOR */
static inline FLASH_FN
void hdrxor_update(SBMP_FrmInst *frm, uint8_t rxbyte)
{
frm->rx_hdr_xor ^= rxbyte;
}
/** Check header xor against received value */
static inline FLASH_FN
bool hdrxor_verify(SBMP_FrmInst *frm, uint8_t rx_xor)
{
return frm->rx_hdr_xor == rx_xor;
}
/** Append a byte to the rx buffer */
static inline FLASH_FN
void append_rx_byte(SBMP_FrmInst *frm, uint8_t b)
{
frm->rx_buffer[frm->rx_buffer_i++] = b;
}
/** Set n-th byte (0 = LSM) to a value */
static inline
void FLASH_FN set_byte(uint32_t *acc, uint8_t pos, uint8_t byte)
{
*acc |= (uint32_t)(byte << (pos * 8));
}
/**
* Call the message handler with the payload.
*
*/
static void FLASH_FN call_frame_rx_callback(SBMP_FrmInst *frm)
{
if (frm->rx_handler == NULL) {
sbmp_error("frame_handler is null!");
return;
}
frm->rx_status = FRM_STATE_WAIT_HANDLER;
frm->rx_handler(frm->rx_buffer, frm->rx_length, frm->user_token);
}
/**
* @brief Receive a byte
*
* SOF 8 | CKSUM_TYPE 8 | LEN 16 | PAYLOAD | CKSUM 0/4
*
* @param state
* @param rxbyte
* @return status
*/
SBMP_RxStatus FLASH_FN sbmp_frm_receive(SBMP_FrmInst *frm, uint8_t rxbyte)
{
if (! frm->rx_enabled) {
return SBMP_RX_DISABLED;
}
SBMP_RxStatus retval = SBMP_RX_OK;
switch (frm->rx_status) {
case FRM_STATE_WAIT_HANDLER:
retval = SBMP_RX_BUSY;
break;
case FRM_STATE_IDLE:
// can be first byte of a packet
if (rxbyte == 0x01) { // start byte
hdrxor_update(frm, rxbyte);
frm->rx_status = FRM_STATE_CKSUM_TYPE;
} else {
// bad char
retval = SBMP_RX_INVALID;
}
break;
case FRM_STATE_CKSUM_TYPE:
frm->rx_cksum_type = rxbyte; // checksum type received
hdrxor_update(frm, rxbyte);
// next will be length
frm->rx_status = FRM_STATE_LENGTH;
// clear MB for 2-byte length
frm->mb_buf = 0;
frm->mb_cnt = 0;
break;
case FRM_STATE_LENGTH:
// append to the multi-byte buffer
set_byte(&frm->mb_buf, frm->mb_cnt++, rxbyte);
hdrxor_update(frm, rxbyte);
// if last of the MB field
if (frm->mb_cnt == 2) {
// next will be the payload
uint16_t len = (uint16_t) frm->mb_buf;
frm->rx_length = len;
if (len == 0) {
sbmp_error("Rx packet with no payload!");
sbmp_frm_reset_rx(frm); // abort
break;
}
frm->rx_status = FRM_STATE_HDRXOR;
}
break;
case FRM_STATE_HDRXOR:
if (! hdrxor_verify(frm, rxbyte)) {
sbmp_error("Header XOR mismatch!");
sbmp_frm_reset_rx(frm); // abort
break;
}
// Check if not too long
if (frm->rx_length > frm->rx_buffer_cap) {
sbmp_error("Rx packet too long - %d!", (uint16_t)frm->rx_length);
// discard the rest + checksum
frm->rx_status = FRM_STATE_DISCARD;
frm->rx_length += chksum_length(frm->rx_cksum_type);
break;
}
frm->rx_status = FRM_STATE_PAYLOAD;
cksum_begin(frm->rx_cksum_type, &frm->rx_cksum_scratch);
break;
case FRM_STATE_DISCARD:
frm->rx_buffer_i++;
if (frm->rx_buffer_i == frm->rx_length) {
// done
sbmp_frm_reset_rx(frm); // go IDLE
}
break;
case FRM_STATE_PAYLOAD:
append_rx_byte(frm, rxbyte);
cksum_update(frm->rx_cksum_type, &frm->rx_cksum_scratch, rxbyte);
if (frm->rx_buffer_i == frm->rx_length) {
// payload rx complete
if (frm->rx_cksum_type != SBMP_CKSUM_NONE) {
// receive the checksum
frm->rx_status = FRM_STATE_CKSUM;
// clear MB for the 4-byte length
frm->mb_buf = 0;
frm->mb_cnt = 0;
} else {
// no checksum
// fire the callback
call_frame_rx_callback(frm);
// clear
sbmp_frm_reset_rx(frm);
}
}
break;
case FRM_STATE_CKSUM:
// append to the multi-byte buffer
set_byte(&frm->mb_buf, frm->mb_cnt++, rxbyte);
// if last of the MB field
if (frm->mb_cnt == chksum_length(frm->rx_cksum_type)) {
if (cksum_verify(frm->rx_cksum_type, &frm->rx_cksum_scratch, frm->mb_buf)) {
call_frame_rx_callback(frm);
} else {
sbmp_error("Rx checksum mismatch!");
}
// clear, enter IDLE
sbmp_frm_reset_rx(frm);
}
break;
}
return retval;
}
/** Send a frame header */
bool FLASH_FN sbmp_frm_start(SBMP_FrmInst *frm, SBMP_CksumType cksum_type, uint16_t length)
{
if (! frm->tx_enabled) {
sbmp_error("Can't tx, not enabled.");
return false;
}
if (frm->tx_status != FRM_STATE_IDLE) {
sbmp_error("Can't tx, busy.");
return false;
}
if (frm->tx_func == NULL) {
sbmp_error("Can't tx, no tx func!");
return false;
}
if (cksum_type == SBMP_CKSUM_CRC32 && !SBMP_HAS_CRC32) {
sbmp_error("CRC32 disabled, using XOR for Tx.");
cksum_type = SBMP_CKSUM_XOR;
}
sbmp_frm_reset_tx(frm);
frm->tx_cksum_type = cksum_type;
frm->tx_remain = length;
frm->tx_status = FRM_STATE_PAYLOAD;
// Send the header
uint16_t len = (uint16_t) length;
uint8_t hdr[4] = {
0x01,
cksum_type,
len & 0xFF,
(len >> 8) & 0xFF
};
uint8_t hdr_xor = 0;
for (int i = 0; i < 4; i++) {
hdr_xor ^= hdr[i];
frm->tx_func(hdr[i]);
}
frm->tx_func(hdr_xor);
cksum_begin(frm->tx_cksum_type, &frm->tx_cksum_scratch);
return true;
}
/** End frame and enter idle mode */
static void FLASH_FN end_frame(SBMP_FrmInst *frm)
{
if (!frm->tx_enabled) {
sbmp_error("Can't tx, not enabled.");
return;
}
cksum_end(frm->tx_cksum_type, &frm->tx_cksum_scratch);
uint32_t cksum = frm->tx_cksum_scratch;
switch (frm->tx_cksum_type) {
case SBMP_CKSUM_NONE:
break; // do nothing
case SBMP_CKSUM_XOR:
// 1-byte checksum
frm->tx_func(cksum & 0xFF);
break;
case SBMP_CKSUM_CRC32:
frm->tx_func(cksum & 0xFF);
frm->tx_func((cksum >> 8) & 0xFF);
frm->tx_func((cksum >> 16) & 0xFF);
frm->tx_func((cksum >> 24) & 0xFF);
}
frm->tx_status = FRM_STATE_IDLE; // tx done
}
/** Send a byte in the currently open frame */
bool FLASH_FN sbmp_frm_send_byte(SBMP_FrmInst *frm, uint8_t byte)
{
if (!frm->tx_enabled) {
sbmp_error("Can't tx, not enabled.");
return false;
}
if (frm->tx_remain == 0 || frm->tx_status != FRM_STATE_PAYLOAD) {
return false;
}
frm->tx_func(byte);
cksum_update(frm->tx_cksum_type, &frm->tx_cksum_scratch, byte);
frm->tx_remain--;
if (frm->tx_remain == 0) {
end_frame(frm); // checksum & go idle
}
return true;
}
/** Send a buffer in the currently open frame */
uint16_t FLASH_FN sbmp_frm_send_buffer(SBMP_FrmInst *frm, const uint8_t *buffer, uint16_t length)
{
if (! frm->tx_enabled) {
sbmp_error("Can't tx, not enabled.");
return false;
}
if (frm->tx_status != FRM_STATE_PAYLOAD) {
return false; // invalid call
}
if (length == 0) {
end_frame(frm); // checksum & go idle
return 0;
}
if (frm->tx_remain == 0) {
return false; // write past EOF (this shouldn't happen)
}
uint16_t remain = length;
while (frm->tx_status == FRM_STATE_PAYLOAD && remain-- > 0) {
if (! sbmp_frm_send_byte(frm, *buffer++)) {
remain++; // "push back"
break;
}
}
return (length - remain);
}