feat: implement frame parse and streaming reader
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.
This commit is contained in:
+26
-10
@@ -85,16 +85,32 @@ size_t cel_crsf_build_param_write_frame(uint8_t* dst, uint8_t index,
|
||||
|
||||
int cel_crsf_frame_parse(cel_crsf_frame* frame, uint8_t const* buf,
|
||||
size_t len) {
|
||||
/* TODO: parse ELRS-format frame [addr][length][type][payload...][crc].
|
||||
* Validate:
|
||||
* - buf has at least 4 bytes (addr + length + type + crc minimum)
|
||||
* - total frame = 2 (addr+length) + length bytes
|
||||
* - CRC over buf[2..2+length-1] matches buf[2+length-1]
|
||||
* Copy type and payload into frame struct. */
|
||||
(void)frame;
|
||||
(void)buf;
|
||||
(void)len;
|
||||
return -1;
|
||||
if (frame == NULL || buf == NULL) return -1;
|
||||
/* Minimum: addr(1) + length(1) + type(1) + crc(1) = 4 bytes */
|
||||
if (len < 4) return -1;
|
||||
|
||||
uint8_t addr = buf[0];
|
||||
uint8_t length = buf[1];
|
||||
size_t total = 2 + length;
|
||||
if (len < total) return -1;
|
||||
|
||||
uint8_t type = buf[2];
|
||||
size_t payload_len = length > 1 ? length - 2 : 0;
|
||||
uint8_t crc_recv = buf[total - 1];
|
||||
|
||||
/* CRC over type + payload */
|
||||
uint8_t crc_calc = cel_crsf_crc(buf + 2, 1 + payload_len);
|
||||
if (crc_calc != crc_recv) return -1;
|
||||
|
||||
frame->addr = addr;
|
||||
frame->length = length;
|
||||
frame->type = type;
|
||||
frame->payload_len = (uint8_t)payload_len;
|
||||
if (payload_len > 0) {
|
||||
memcpy(frame->payload, buf + 3, payload_len);
|
||||
}
|
||||
frame->crc = crc_recv;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* --------------------------------------------------------------------------- */
|
||||
|
||||
+48
-25
@@ -15,44 +15,67 @@ static int is_valid_addr(uint8_t addr) {
|
||||
}
|
||||
|
||||
struct cel_crsf_stream {
|
||||
uint8_t buf[260]; /* internal buffer (max frame = 2 + 255 + 1 = 258) */
|
||||
uint8_t buf[260]; /* max frame = 2 + 255 + 1 = 258 */
|
||||
size_t buf_len;
|
||||
};
|
||||
|
||||
cel_crsf_stream* cel_crsf_stream_create(void) {
|
||||
/* TODO: allocate and zero-initialize the stream struct. */
|
||||
return NULL;
|
||||
cel_crsf_stream* s = calloc(1, sizeof(*s));
|
||||
return s;
|
||||
}
|
||||
|
||||
void cel_crsf_stream_destroy(cel_crsf_stream* stream) {
|
||||
/* TODO: free the stream struct. */
|
||||
(void)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) {
|
||||
/* TODO: append data to internal buffer, scan for complete frames.
|
||||
* Algorithm:
|
||||
* 1. Copy incoming bytes into internal buffer.
|
||||
* 2. While buffer has >= 4 bytes:
|
||||
* a. If buf[0] is not a valid address byte, pop(0) and continue.
|
||||
* b. Read length = buf[1].
|
||||
* c. Total frame = 2 + length.
|
||||
* d. If buffer < total, break (need more data).
|
||||
* e. Parse frame from buf[0..total-1].
|
||||
* f. On success (CRC valid), copy into out[] and advance buffer.
|
||||
* g. On failure (CRC invalid), discard frame and continue.
|
||||
* 3. Return number of valid frames parsed, or -1 on error. */
|
||||
(void)stream;
|
||||
(void)data;
|
||||
(void)len;
|
||||
(void)out;
|
||||
(void)out_capacity;
|
||||
return 0;
|
||||
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) {
|
||||
/* TODO: zero buf_len to discard any partial frame. */
|
||||
(void)stream;
|
||||
stream->buf_len = 0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user