add modbus (not tested yet)

F1:F103/FX3U/modbusproto.c

@@ -0,0 +1,261 @@
+/*
+ * This file is part of the fx3u project.
+ * Copyright 2024 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "adc.h"
+#include "flash.h"
+#include "hardware.h"
+#include "modbusproto.h"
+#include "modbusrtu.h"
+#include "strfunc.h"
+#include "usart.h"
+
+// send error
+static void senderr(modbus_request *r, uint8_t exception){
+    modbus_response resp = {.ID = the_conf.modbusID, .Fcode = r->Fcode | MODBUS_RESPONSE_ERRMARK, .datalen = exception};
+    modbus_send_response(&resp);
+}
+
+// relays status
+TRUE_INLINE void readcoil(modbus_request *r){
+    // starting number shoul be 0
+    if(r->startreg){
+        senderr(r, ME_ILLEGAL_ADDRESS);
+        return;
+    }
+    // amount of bytes: should be 8-multiple
+    int amount = r->regno >> 3;
+    if(amount == 0 || (r->regno & 7) || amount > OUTMAXBYTES){
+        senderr(r, ME_ILLEGAL_VALUE);
+        return;
+    }
+    uint8_t bytes[OUTMAXBYTES] = {0};
+    int curidx = OUTMAXBYTES;
+    int vals = get_relay(OUTMAX+1);
+    for(int i = 0; i < amount; ++i){
+        bytes[--curidx] = vals & 0xff;
+        vals >>= 8;
+    }
+    modbus_response resp = {.Fcode = r->Fcode, .ID = the_conf.modbusID, .data = bytes+curidx, .datalen = amount};
+    modbus_send_response(&resp);
+}
+
+// input status
+TRUE_INLINE void readdiscr(modbus_request *r){
+    if(r->startreg){
+        senderr(r, ME_ILLEGAL_ADDRESS);
+        return;
+    }
+    // amount of bytes: should be 8-multiple
+    int amount = r->regno >> 3;
+    if(amount == 0 || (r->regno & 7) || amount > INMAXBYTES){
+        senderr(r, ME_ILLEGAL_VALUE);
+        return;
+    }
+    uint8_t bytes[INMAXBYTES] = {0};
+    int curidx = INMAXBYTES - 1;
+    int vals = get_esw(INMAX+1);
+    for(int i = 0; i < amount; ++i){
+        bytes[--curidx] = vals & 0xff;
+        vals >>= 8;
+    }
+    modbus_response resp = {.Fcode = r->Fcode, .ID = the_conf.modbusID, .data = bytes+curidx, .datalen = amount};
+    modbus_send_response(&resp);
+}
+
+// registers: read only by one!
+// !!! To simplify code I suppose that all registers read are 32-bit!
+TRUE_INLINE void readreg(modbus_request *r){
+    if(r->regno != 1){
+        senderr(r, ME_ILLEGAL_VALUE);
+        return;
+    }
+    uint32_t regval;
+    switch(r->startreg){
+        case MR_TIME:
+            regval = Tms;
+        break;
+        case MR_LED:
+            regval = LED(-1);
+        break;
+        case MR_INCHANNELS:
+            regval = inchannels();
+        break;
+        case MR_OUTCHANNELS:
+            regval = outchannels();
+        break;
+        default:
+            senderr(r, ME_ILLEGAL_ADDRESS);
+            return;
+    }
+    regval = __builtin_bswap32(regval);
+    modbus_response resp = {.Fcode = r->Fcode, .ID = the_conf.modbusID, .data = (uint8_t*)&regval, .datalen = 4};
+    modbus_send_response(&resp);
+}
+
+// read ADC values; startreg = N of starting sensor, regno = N values
+TRUE_INLINE void readadc(modbus_request *r){
+    if(r->startreg >= ADC_CHANNELS){
+        senderr(r, ME_ILLEGAL_ADDRESS);
+        return;
+    }
+    int nlast = r->regno + r->startreg;
+    if(r->regno == 0 || nlast > ADC_CHANNELS){
+        senderr(r, ME_ILLEGAL_VALUE);
+        return;
+    }
+    uint16_t vals[ADC_CHANNELS];
+    for(int i = r->startreg; i < nlast; ++i){
+        uint16_t v = getADCval(i);
+        vals[i] = __builtin_bswap16(v);
+    }
+    modbus_response resp = {.Fcode = r->Fcode, .ID = the_conf.modbusID, .data = (uint8_t*) vals, .datalen = r->regno * 2};
+    modbus_send_response(&resp);
+}
+
+TRUE_INLINE void writecoil(modbus_request *r){
+    if(r->startreg > OUTMAX || set_relay(r->startreg, r->regno) < 0){
+        senderr(r, ME_ILLEGAL_ADDRESS);
+        return;
+    }
+    modbus_send_request(r); // answer with same data
+}
+
+TRUE_INLINE void writereg(modbus_request *r){
+    switch(r->startreg){
+        case MR_LED:
+            LED(r->regno);
+        break;
+        case MR_RESET:
+            NVIC_SystemReset();
+        break;
+        default:
+            senderr(r, ME_ILLEGAL_ADDRESS);
+            return;
+    }
+    modbus_send_request(r);
+}
+
+// support ONLY write to ALL!
+// data - by bits, like in readcoil
+TRUE_INLINE void writecoils(modbus_request *r){
+    if(r->startreg){
+        senderr(r, ME_ILLEGAL_ADDRESS);
+        return;
+    }
+    int amount = r->regno;
+    if(amount == 0 || amount > OUTMAX || r->datalen > 4){
+        senderr(r, ME_ILLEGAL_VALUE);
+        return;
+    }
+    uint32_t v = 0;
+    for(int i = 0; i < amount; ++i){
+        v |= r->data[i];
+        v <<= 8;
+    }
+    if(set_relay(OUTMAX+1, v) < 0){
+        senderr(r, ME_NACK);
+        return;
+    }
+    r->datalen = 0;
+    modbus_send_request(r);
+}
+
+// amount of registers should be equal 1, data should have 4 bytes
+TRUE_INLINE void writeregs(modbus_request *r){
+    if(r->datalen != 4 || r->regno != 1){
+        senderr(r, ME_ILLEGAL_VALUE);
+        return;
+    }
+    uint32_t val = __builtin_bswap32(*(uint32_t*)r->data);
+    switch(r->startreg){
+        case MR_TIME:
+            Tms = val;
+        break;
+        default:
+            senderr(r, ME_ILLEGAL_ADDRESS);
+            return;
+    }
+    r->datalen = 0;
+    modbus_send_request(r);
+}
+
+// get request as slave and send answer about some registers
+// DO NOT ANSWER for broadcasting requests!
+// Understand only requests with codes <= 6
+void parse_modbus_request(modbus_request *r){
+    if(!r) return;
+    switch(r->Fcode){
+        case MC_READ_COIL:
+            readcoil(r);
+        break;
+        case MC_READ_DISCRETE:
+            readdiscr(r);
+        break;
+        case MC_READ_HOLDING_REG:
+            readreg(r);
+        break;
+        case MC_READ_INPUT_REG:
+            readadc(r);
+        break;
+        case MC_WRITE_COIL:
+            writecoil(r);
+        break;
+        case MC_WRITE_REG:
+            writereg(r);
+        break;
+        case MC_WRITE_MUL_COILS:
+            writecoils(r);
+        break;
+        case MC_WRITE_MUL_REGS: // write uint32_t as 2 registers
+            writeregs(r);
+        break;
+        default:
+            senderr(r, ME_ILLEGAL_FUNCION);
+    }
+}
+
+// get responce as master and show it on terminal
+void parse_modbus_response(modbus_response *r){
+    if(!r) return;
+    if(r->Fcode & MODBUS_RESPONSE_ERRMARK){ // error - three bytes
+        usart_send("MODBUS ERR (ID=");
+        printuhex(r->ID);
+        usart_send(", Fcode=");
+        printuhex(r->Fcode & ~MODBUS_RESPONSE_ERRMARK);
+        usart_send(") > ");
+        printuhex(r->datalen);
+        newline();
+        return;
+    }
+    // regular answer
+    usart_send("MODBUS RESP (ID=");
+    printuhex(r->ID);
+    usart_send(", Fcode=");
+    printuhex(r->Fcode & ~MODBUS_RESPONSE_ERRMARK);
+    usart_send(", Datalen=");
+    printu(r->datalen);
+    usart_send(") > ");
+    if(r->datalen){
+        for(int i = 0; i < r->datalen; ++i){
+            //uint16_t nxt = r->data[i] >> 8 | r->data[i] << 8;
+            printuhex(r->data[i]);
+            usart_putchar(' ');
+        }
+    }
+    newline();
+}