portersky
a846b063f9
cel_crsf_frame_parse() parses ELRS USB format frames: [addr][length][type][payload...][crc] cel_crsf_stream_* provides incremental parsing from a byte stream: skips invalid sync bytes, discards bad CRC frames, buffers partial frames across feed calls.
82 lines
2.3 KiB
C
82 lines
2.3 KiB
C
#include "celrs/crsf_stream.h"
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
/* Valid CRSF address bytes (frame sync bytes) */
|
|
static uint8_t const s_valid_addrs[] = {
|
|
0x00, 0xC8, 0xC4, 0xEC, 0xEE, 0xEA, 0xEF
|
|
};
|
|
|
|
static int is_valid_addr(uint8_t addr) {
|
|
for (size_t i = 0; i < sizeof(s_valid_addrs); i++) {
|
|
if (s_valid_addrs[i] == addr) return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
struct cel_crsf_stream {
|
|
uint8_t buf[260]; /* max frame = 2 + 255 + 1 = 258 */
|
|
size_t buf_len;
|
|
};
|
|
|
|
cel_crsf_stream* cel_crsf_stream_create(void) {
|
|
cel_crsf_stream* s = calloc(1, sizeof(*s));
|
|
return s;
|
|
}
|
|
|
|
void cel_crsf_stream_destroy(cel_crsf_stream* stream) {
|
|
free(stream);
|
|
}
|
|
|
|
int cel_crsf_stream_feed(cel_crsf_stream* stream, uint8_t const* data,
|
|
size_t len, cel_crsf_frame* out,
|
|
size_t out_capacity) {
|
|
if (stream == NULL) return -1;
|
|
if (out == NULL) return -1;
|
|
if (data == NULL && len > 0) return -1;
|
|
if (data == NULL && len == 0) {
|
|
/* Drain remaining frames from buffer without new data */
|
|
len = 0;
|
|
}
|
|
|
|
/* Append new data to buffer */
|
|
if (len > 0) {
|
|
if (stream->buf_len + len > sizeof(stream->buf)) {
|
|
/* Buffer overflow — discard everything */
|
|
stream->buf_len = 0;
|
|
return -1;
|
|
}
|
|
memcpy(stream->buf + stream->buf_len, data, len);
|
|
stream->buf_len += len;
|
|
}
|
|
|
|
int count = 0;
|
|
while (stream->buf_len >= 4 && count < (int)out_capacity) {
|
|
/* Skip invalid sync bytes */
|
|
if (!is_valid_addr(stream->buf[0])) {
|
|
memmove(stream->buf, stream->buf + 1, stream->buf_len - 1);
|
|
stream->buf_len--;
|
|
continue;
|
|
}
|
|
|
|
uint8_t length = stream->buf[1];
|
|
size_t total = 2 + length;
|
|
if (stream->buf_len < total) break; /* need more data */
|
|
|
|
/* Try to parse frame */
|
|
cel_crsf_frame frame;
|
|
if (cel_crsf_frame_parse(&frame, stream->buf, total) == 0) {
|
|
out[count++] = frame;
|
|
}
|
|
/* Discard frame (valid or not) */
|
|
memmove(stream->buf, stream->buf + total, stream->buf_len - total);
|
|
stream->buf_len -= total;
|
|
}
|
|
|
|
return count;
|
|
}
|
|
|
|
void cel_crsf_stream_reset(cel_crsf_stream* stream) {
|
|
stream->buf_len = 0;
|
|
}
|