Add embedded storage drivers and extend firmware metadata

2026-09-27 01:44:59 +03:00
parent 795a1279b1
commit 2ad29e7ffd
52 changed files with 5801 additions and 3 deletions
--- a/c/modbus/src/modbus_data.c
+++ b/c/modbus/src/modbus_data.c
@@ -0,0 +1,57 @@
+#include "../include/modbus_data.h"
+
+uint8_t mb_data_validate(uint8_t function, uint16_t quantity, uint16_t bytes, size_t capacity)
+{
+    size_t needed;
+    switch (function) {
+    case 1: case 15:
+        if (!quantity || quantity > (function == 1 ? 2000U : 1968U)) return 3;
+        needed = ((size_t)quantity+15U)/16U;
+        if (function == 15 && bytes != (quantity+7U)/8U) return 3;
+        break;
+    case 3: case 4: case 16:
+        if (!quantity || quantity > (function == 16 ? 123U : 125U)) return 3;
+        needed = quantity;
+        if (function == 16 && bytes != quantity*2U) return 3;
+        break;
+    case 5: return (quantity == 0 || quantity == 0xff00U) ? 0 : 3;
+    case 6: return 0;
+    default: return 1;
+    }
+    return needed <= capacity ? 0 : 3;
+}
+
+uint8_t mb_data_transfer(uint8_t function, uint16_t quantity, uint16_t bytes,
+    uint16_t *bank, uint16_t offset, uint16_t *data, size_t capacity, uint16_t *response_bytes)
+{
+    uint16_t i;
+    uint8_t status = mb_data_validate(function, quantity, bytes, capacity);
+    if (status) return status;
+    if (bank == NULL || data == NULL || response_bytes == NULL || offset > 15U) return 3;
+    if (function == 1) {
+        for (i = 0; i < (quantity+15U)/16U; ++i) data[i] = 0;
+    }
+    if (function == 1 || function == 15) {
+        for (i = 0; i < quantity; ++i) {
+            uint16_t position = (uint16_t)(i+offset);
+            uint16_t mask = (uint16_t)(1U << (position%16U));
+            uint16_t wire_mask = (uint16_t)(1U << ((i%16U)^8U));
+            if (function == 1) {
+                if (bank[position/16U] & mask) data[i/16U] |= wire_mask;
+            } else if (data[i/16U] & wire_mask) bank[position/16U] |= mask;
+            else bank[position/16U] &= (uint16_t)~mask;
+        }
+        if (function == 1) *response_bytes = (uint16_t)((quantity+7U)/8U);
+    } else if (function == 5) {
+        if (quantity) *bank |= (uint16_t)(1U << offset);
+        else *bank &= (uint16_t)~(1U << offset);
+    } else if (function == 6) *bank = quantity;
+    else {
+        for (i = 0; i < quantity; ++i) {
+            if (function == 16) bank[i] = data[i];
+            else data[i] = bank[i];
+        }
+        if (function != 16) *response_bytes = (uint16_t)(quantity*2U);
+    }
+    return 0;
+}