refactor: merge protocol cores as SETProtocol

2026-09-01 09:58:09 +03:00
parent 5504104cc5
commit 19becd7b8c
56 changed files with 1256 additions and 359 deletions
--- a/c/set-protocol/src/pcan_frame.c
+++ b/c/set-protocol/src/pcan_frame.c
@@ -0,0 +1,141 @@
+#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;
+}