feat(set-protocol): добавь порт F407 для DevBoard_V1

c/set-protocol/src/pcan_modbus_server.c

@@ -0,0 +1,144 @@
+#include "pcan_modbus_server.h"
+
+#include <stddef.h>
+
+#include "pcan_id.h"
+
+static bool is_bank(uint8_t value)
+{
+    return (value >= (uint8_t)PCAN_MODBUS_COIL) &&
+           (value <= (uint8_t)PCAN_MODBUS_INPUT);
+}
+
+bool pcan_modbus_server_init(pcan_modbus_server_t *server,
+                             uint8_t device_type, uint8_t device_id,
+                             pcan_modbus_read_fn read,
+                             pcan_modbus_write_fn write, void *user)
+{
+    if ((server == NULL) || (read == NULL) || (device_type > 7U) ||
+        (device_id > 15U)) {
+        return false;
+    }
+    server->device_type = device_type;
+    server->device_id = device_id;
+    server->read = read;
+    server->write = write;
+    server->user = user;
+    server->requests = 0U;
+    server->responses = 0U;
+    server->rejected = 0U;
+    return true;
+}
+
+static pcan_modbus_result_t reject(pcan_modbus_server_t *server)
+{
+    server->rejected++;
+    return PCAN_MODBUS_HANDLED_NO_RESPONSE;
+}
+
+pcan_modbus_result_t pcan_modbus_server_handle(pcan_modbus_server_t *server,
+                                                const pcan_frame_t *request,
+                                                pcan_frame_t *response)
+{
+    pcan_id_t id;
+    uint16_t address;
+    uint8_t count;
+    pcan_modbus_bank_t bank;
+
+    if ((server == NULL) || (request == NULL) || (response == NULL) ||
+        ((request->flags & PCAN_FLAG_IDE) == 0U)) {
+        return PCAN_MODBUS_NOT_FOR_US;
+    }
+    pcan_id_unpack(request->id, &id);
+    if ((id.route != PCAN_ROUTE_FROM_PM) ||
+        (id.device_type != server->device_type) ||
+        (id.device_id != server->device_id) || !is_bank(id.msg_type)) {
+        return PCAN_MODBUS_NOT_FOR_US;
+    }
+
+    server->requests++;
+    address = (uint16_t)(id.msg_body >> 4U);
+    count = (uint8_t)(id.msg_body & 0x0FU);
+    bank = (pcan_modbus_bank_t)id.msg_type;
+    if (count == 0U) {
+        return reject(server);
+    }
+
+    *response = *request;
+    id.route = PCAN_ROUTE_FROM_DEVICE;
+    response->id = pcan_id_pack(&id);
+    response->flags = PCAN_FLAG_IDE;
+    response->seq = 0U;
+
+    if (request->dlc == 0U) {
+        uint16_t packed = 0U;
+        if ((bank == PCAN_MODBUS_COIL) || (bank == PCAN_MODBUS_DISCRETE)) {
+            if (count > 16U) {
+                return reject(server);
+            }
+            for (uint8_t index = 0U; index < count; ++index) {
+                uint16_t value = 0U;
+                if (!server->read(server->user, bank,
+                                  (uint16_t)(address + index), &value)) {
+                    return reject(server);
+                }
+                if (value != 0U) {
+                    packed |= (uint16_t)(1U << index);
+                }
+            }
+            response->dlc = 2U;
+            response->data[0] = (uint8_t)packed;
+            response->data[1] = (uint8_t)(packed >> 8U);
+        } else {
+            if (count > 4U) {
+                return reject(server);
+            }
+            response->dlc = (uint8_t)(count * 2U);
+            for (uint8_t index = 0U; index < count; ++index) {
+                uint16_t value = 0U;
+                if (!server->read(server->user, bank,
+                                  (uint16_t)(address + index), &value)) {
+                    return reject(server);
+                }
+                response->data[index * 2U] = (uint8_t)value;
+                response->data[(index * 2U) + 1U] = (uint8_t)(value >> 8U);
+            }
+        }
+    } else {
+        if ((bank == PCAN_MODBUS_INPUT) || (bank == PCAN_MODBUS_DISCRETE) ||
+            (server->write == NULL)) {
+            return reject(server);
+        }
+        if (bank == PCAN_MODBUS_COIL) {
+            uint16_t bits;
+            if ((count > 16U) || (request->dlc != 2U)) {
+                return reject(server);
+            }
+            bits = (uint16_t)((uint16_t)request->data[0] |
+                              ((uint16_t)request->data[1] << 8U));
+            for (uint8_t index = 0U; index < count; ++index) {
+                if (!server->write(server->user, bank,
+                                   (uint16_t)(address + index),
+                                   (uint16_t)((bits >> index) & 1U))) {
+                    return reject(server);
+                }
+            }
+        } else {
+            if ((count > 4U) || (request->dlc != (uint8_t)(count * 2U))) {
+                return reject(server);
+            }
+            for (uint8_t index = 0U; index < count; ++index) {
+                uint16_t value = (uint16_t)(request->data[index * 2U] |
+                    ((uint16_t)request->data[(index * 2U) + 1U] << 8U));
+                if (!server->write(server->user, bank,
+                                   (uint16_t)(address + index), value)) {
+                    return reject(server);
+                }
+            }
+        }
+        response->dlc = request->dlc;
+    }
+
+    server->responses++;
+    return PCAN_MODBUS_RESPONSE;
+}