make demo server interactive

src/main.c

@@ -12,83 +12,156 @@ ModbusException_t startOfAccess(ModbusSlave_t *ms, ModbusFunction_t fcx, uint8_t
 
 void endOfAccess(ModbusSlave_t *ms) { printf("End of access\n"); }
 
+typedef struct {
+    uint16_t num;
+    uint16_t value;
+} register_slot_t;
+
+typedef struct {
+    uint16_t num;
+    bool value;
+} coil_slot_t;
+
+//@formatter:off
+static register_slot_t holdings[] = {
+    {1, 10},
+    {2, 20},
+    {3, 30},
+    {4, 40},
+    {5, 50},
+    {7, 70},
+    {0xffff, 0}
+};
+
+static register_slot_t inputs[] = {
+    {1, 11},
+    {2, 21},
+    {3, 31},
+    {4, 41},
+    {5, 51},
+    {7, 71},
+    {0xffff, 0}
+};
+
+static coil_slot_t coils[] = {
+    {1, 1},
+    {2, 0},
+    {3, 1},
+    {4, 0},
+    {5, 1},
+    {7, 1},
+    {0xffff, 0}
+};
+
+static coil_slot_t discretes[] = {
+    {1, 0},
+    {2, 1},
+    {3, 1},
+    {4, 1},
+    {5, 1},
+    {7, 0},
+    {0xffff, 0}
+};
+//@formatter:on
+
+
 ModbusException_t readHolding(ModbusSlave_t *ms, uint16_t i, uint16_t *out) {
     printf("rh %d\n", i);
-    switch (i) {
+
-        case 107:
+    const register_slot_t *p = holdings;
-            *out = 0xAE41;
+    while (p->num != 0xffff) {
-            break;
+        if (p->num == i) {
-        case 108:
+            *out = p->value;
-            *out = 0x5652;
+            return 0;
-            break;
+        }
-        case 109:
+        p++;
-            *out = 0x4340;
-            break;
-        default:
-            return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
     }
-    return 0;
+
+    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
 }
+
 ModbusException_t readInput(ModbusSlave_t *ms, uint16_t i, uint16_t *out) {
     printf("ri %d\n", i);
-    switch (i) {
+
-        case 8:
+    const register_slot_t *p = inputs;
-            *out = 0xa;
+    while (p->num != 0xffff) {
-            break;
+        if (p->num == i) {
-        default:
+            *out = p->value;
-            return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
+            return 0;
+        }
+        p++;
     }
-    return 0;
+
+    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
 }
-ModbusException_t readCoil(ModbusSlave_t *ms, uint16_t i, bool *out) {
-    uint8_t store[] = {
-            0b11001101, 0b01101011, 0b10110010, 0b00001110, 0b00011011,
-    };
 
-    if (i >= 19 && i <= 55) {
+ModbusException_t readCoil(ModbusSlave_t *ms, uint16_t i, bool *out) {
-        int pos = i - 19;
+    const coil_slot_t *p = coils;
-        *out = 0 != (store[pos / 8] & (1 << (pos % 8)));
+    while (p->num != 0xffff) {
-    } else {
+        if (p->num == i) {
-        return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
+            *out = p->value;
+            return 0;
+        }
+        p++;
     }
-    return 0;
+
+    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
 }
-ModbusException_t readDiscrete(ModbusSlave_t *ms, uint16_t i, bool *out) {
-    uint8_t store[] = {
-            0b10101100,
-            0b11011011,
-            0b00110101,
-    };
 
-    if (i >= 196 && i <= 217) {
+ModbusException_t readDiscrete(ModbusSlave_t *ms, uint16_t i, bool *out) {
-        int pos = i - 196;
+    const coil_slot_t *p = discretes;
-        *out = 0 != (store[pos / 8] & (1 << (pos % 8)));
+    while (p->num != 0xffff) {
-    } else {
+        if (p->num == i) {
-        return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
+            *out = p->value;
+            return 0;
+        }
+        p++;
     }
-    return 0;
+
+    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
 }
 
 ModbusException_t writeCoil(ModbusSlave_t *pSlave, uint16_t i, bool b) {
     printf("Write coil %d <- %d\n", i, b);
-    return 0;
+
+    coil_slot_t *p = coils;
+    while (p->num != 0) {
+        if (p->num == i) {
+            p->value = b;
+            return 0;
+        }
+        p++;
+    }
+
+    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
 }
-ModbusException_t writeHolding(ModbusSlave_t *pSlave, uint16_t i, uint16_t i1) {
+
-    printf("Write reg %d <- %d\n", i, i1);
+ModbusException_t writeHolding(ModbusSlave_t *pSlave, uint16_t i, uint16_t val) {
-    return 0;
+    printf("Write reg %d <- %d\n", i, val);
+
+    register_slot_t *p = holdings;
+    while (p->num != 0) {
+        if (p->num == i) {
+            p->value = val;
+            return 0;
+        }
+        p++;
+    }
+
+    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
 }
 
 static ModbusSlave_t ms = {
-        .addr = 1,
+    .addr = 1,
-        .proto = MB_PROTO_TCP,
+    .proto = MB_PROTO_TCP,
-        .startOfAccess = startOfAccess,
+    .startOfAccess = startOfAccess,
-        .endOfAccess = endOfAccess,
+    .endOfAccess = endOfAccess,
-        .readHolding = readHolding,
+    .readHolding = readHolding,
-        .readInput = readInput,
+    .readInput = readInput,
-        .readCoil = readCoil,
+    .readCoil = readCoil,
-        .readDiscrete = readDiscrete,
+    .readDiscrete = readDiscrete,
-        .writeCoil = writeCoil,
+    .writeCoil = writeCoil,
-        .writeHolding = writeHolding,
+    .writeHolding = writeHolding,
 };
 
 int msg_received_fce(int fd, const simple_msg_t *msg) {
@@ -103,8 +176,8 @@ int msg_received_fce(int fd, const simple_msg_t *msg) {
         return 1; // kick the client
 
     simple_msg_t resp = {
-            .data = resp_buf,
+        .data = resp_buf,
-            .len = resp_size,
+        .len = resp_size,
     };
     simple_tcp_send(fd, &resp);