142 lines
3.8 KiB
C
142 lines
3.8 KiB
C
#include "pcan_frame.h"
|
|
#include "pcan_crc.h"
|
|
|
|
size_t pcan_frame_encode(const pcan_frame_t *frame, uint8_t *out, size_t out_size)
|
|
{
|
|
if ((frame == NULL) || (out == NULL) || (out_size < PCAN_FRAME_MAX)) {
|
|
return 0U;
|
|
}
|
|
|
|
uint8_t dlc = (frame->dlc > PCAN_DATA_MAX) ? (uint8_t)PCAN_DATA_MAX : frame->dlc;
|
|
size_t n = 0U;
|
|
|
|
out[n++] = PCAN_SOF0;
|
|
out[n++] = PCAN_SOF1;
|
|
|
|
size_t body = n;
|
|
out[n++] = (uint8_t)(PCAN_LEN_MIN + dlc);
|
|
out[n++] = frame->seq;
|
|
out[n++] = frame->flags;
|
|
out[n++] = (uint8_t)(frame->id & 0xFFU);
|
|
out[n++] = (uint8_t)((frame->id >> 8) & 0xFFU);
|
|
out[n++] = (uint8_t)((frame->id >> 16) & 0xFFU);
|
|
out[n++] = (uint8_t)((frame->id >> 24) & 0x1FU);
|
|
for (uint8_t i = 0U; i < dlc; i++) {
|
|
out[n++] = frame->data[i];
|
|
}
|
|
|
|
uint16_t crc = pcan_crc16(&out[body], n - body);
|
|
out[n++] = (uint8_t)(crc & 0xFFU);
|
|
out[n++] = (uint8_t)(crc >> 8);
|
|
return n;
|
|
}
|
|
|
|
void pcan_parser_init(pcan_parser_t *p)
|
|
{
|
|
p->state = PCAN_ST_SOF0;
|
|
p->len = 0U;
|
|
p->idx = 0U;
|
|
p->crc_rx = 0U;
|
|
p->stats.frames = 0U;
|
|
p->stats.crc_errors = 0U;
|
|
p->stats.bad_len = 0U;
|
|
p->stats.stray_bytes = 0U;
|
|
}
|
|
|
|
bool pcan_parser_push(pcan_parser_t *p, uint8_t byte, pcan_frame_t *out)
|
|
{
|
|
switch (p->state) {
|
|
case PCAN_ST_SOF0:
|
|
if (byte == PCAN_SOF0) {
|
|
p->state = PCAN_ST_SOF1;
|
|
} else {
|
|
p->stats.stray_bytes++;
|
|
}
|
|
break;
|
|
|
|
case PCAN_ST_SOF1:
|
|
if (byte == PCAN_SOF1) {
|
|
p->state = PCAN_ST_LEN;
|
|
} else if (byte == PCAN_SOF0) {
|
|
/* 0xAA 0xAA 0x55 - тоже валидное начало, остаёмся здесь. */
|
|
p->stats.stray_bytes++;
|
|
} else {
|
|
p->stats.stray_bytes += 2U;
|
|
p->state = PCAN_ST_SOF0;
|
|
}
|
|
break;
|
|
|
|
case PCAN_ST_LEN:
|
|
if ((byte < PCAN_LEN_MIN) || (byte > PCAN_LEN_MAX)) {
|
|
p->stats.bad_len++;
|
|
/* Байт мог оказаться началом настоящего кадра. */
|
|
p->state = (byte == PCAN_SOF0) ? PCAN_ST_SOF1 : PCAN_ST_SOF0;
|
|
break;
|
|
}
|
|
p->len = byte;
|
|
p->body[0] = byte;
|
|
p->idx = 1U;
|
|
p->state = PCAN_ST_BODY;
|
|
break;
|
|
|
|
case PCAN_ST_BODY:
|
|
p->body[p->idx++] = byte;
|
|
if (p->idx >= (uint8_t)(p->len + 1U)) {
|
|
p->state = PCAN_ST_CRC_L;
|
|
}
|
|
break;
|
|
|
|
case PCAN_ST_CRC_L:
|
|
p->crc_rx = byte;
|
|
p->state = PCAN_ST_CRC_H;
|
|
break;
|
|
|
|
case PCAN_ST_CRC_H: {
|
|
p->crc_rx |= (uint16_t)((uint16_t)byte << 8);
|
|
p->state = PCAN_ST_SOF0;
|
|
if (p->crc_rx != pcan_crc16(p->body, (size_t)p->len + 1U)) {
|
|
p->stats.crc_errors++;
|
|
return false;
|
|
}
|
|
out->seq = p->body[1];
|
|
out->flags = p->body[2];
|
|
out->id = ((uint32_t)p->body[3])
|
|
| ((uint32_t)p->body[4] << 8)
|
|
| ((uint32_t)p->body[5] << 16)
|
|
| ((uint32_t)p->body[6] << 24);
|
|
out->id &= 0x1FFFFFFFUL;
|
|
out->dlc = (uint8_t)(p->len - PCAN_LEN_MIN);
|
|
for (uint8_t i = 0U; i < out->dlc; i++) {
|
|
out->data[i] = p->body[7U + i];
|
|
}
|
|
for (uint8_t i = out->dlc; i < PCAN_DATA_MAX; i++) {
|
|
out->data[i] = 0U;
|
|
}
|
|
p->stats.frames++;
|
|
return true;
|
|
}
|
|
|
|
default:
|
|
p->state = PCAN_ST_SOF0;
|
|
break;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
size_t pcan_parser_feed(pcan_parser_t *p, const uint8_t *data, size_t len,
|
|
pcan_frame_cb_t cb, void *user)
|
|
{
|
|
pcan_frame_t frame;
|
|
size_t count = 0U;
|
|
|
|
for (size_t i = 0U; i < len; i++) {
|
|
if (pcan_parser_push(p, data[i], &frame)) {
|
|
count++;
|
|
if (cb != NULL) {
|
|
cb(&frame, user);
|
|
}
|
|
}
|
|
}
|
|
return count;
|
|
}
|