Improve server object to be multi-server spec available, add handler implementations

examples/stm32/nanomodbus_stm32.c

@@ -17,27 +17,27 @@ static ringBuf rb;
 static void ringbuf_init(ringBuf* rb, void (*overflow_callback)(struct tRingBuf* rq));
 static void ringbuf_overflow_error(ringBuf* rb);
 
-uint8_t  coil_buf[COIL_BUF_SIZE];
+static nmbs_server_t* servers;
-uint16_t reg_buf [REG_BUF_SIZE];
+static uint8_t        server_num;
 
 static nmbs_error server_read_coils(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, uint8_t unit_id, void* arg);
 static nmbs_error server_read_holding_registers(uint16_t address, uint16_t quantity, uint16_t* registers_out, uint8_t unit_id, void* arg);
 static nmbs_error server_write_single_coil(uint16_t address, bool value, uint8_t unit_id, void* arg);
 static nmbs_error server_write_multiple_coils(uint16_t address, uint16_t quantity, const nmbs_bitfield coils, uint8_t unit_id, void* arg);
 static nmbs_error server_write_single_register(uint16_t address, uint16_t value, uint8_t unit_id, void* arg);
-static nmbs_error server_write_multiple_resisters(uint16_t address, uint16_t quantity, const uint16_t* registers, uint8_t unit_id, void* arg);
+static nmbs_error server_write_multiple_registers(uint16_t address, uint16_t quantity, const uint16_t* registers, uint8_t unit_id, void* arg);
 
-void nanomodbus_server_init(nmbs_t* nmbs)
+nmbs_error nanomodbus_server_init(nmbs_t* nmbs, nmbs_server_t* _servers, uint8_t _server_num)
 {
     ringbuf_init(&rb, ringbuf_overflow_error);
 
-    nmbs_platform_conf pf_conf;
+    nmbs_platform_conf conf;
     nmbs_callbacks     cb;
 
-    nmbs_platform_conf_create(&pf_conf);
+    nmbs_platform_conf_create(&conf);
-    pf_conf.transport = NMBS_TRANSPORT_RTU; 
+    conf.transport = NMBS_TRANSPORT_RTU; 
-    pf_conf.read = read_serial;   
+    conf.read = read_serial;   
-    pf_conf.write = write_serial;
+    conf.write = write_serial;
 
     nmbs_callbacks_create(&cb);
     cb.read_coils                = server_read_coils;
@@ -45,42 +45,110 @@ void nanomodbus_server_init(nmbs_t* nmbs)
     cb.write_single_coil         = server_write_single_coil;
     cb.write_multiple_coils      = server_write_multiple_coils;
     cb.write_single_register     = server_write_single_register;
-    cb.write_multiple_registers  = server_write_multiple_resisters;
+    cb.write_multiple_registers  = server_write_multiple_registers;
+
+    servers = _servers;
+    server_num = _server_num;
+
+    for(size_t i = 0; i < server_num; i++)
+    {
+        nmbs_error status = nmbs_server_create(nmbs, servers[i].id, &conf, &cb);
+        if(status != NMBS_ERROR_NONE)
+        {
+            return status;
+        }
+    }
+    return NMBS_ERROR_NONE;
 }
 
-void nanomodbus_server_add(nmbs_server_t* server)
+static nmbs_server_t* get_server(uint8_t id)
 {
-
+    for(size_t i = 0; i < server_num; i++)
+    {
+        if(servers[i].id == id)
+        {
+            return &servers[i];
+        }
+    }
+    return (nmbs_server_t*)(NULL);
 }
 
 static nmbs_error server_read_coils(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, uint8_t unit_id, void* arg)
 {
-
+    nmbs_server_t* server = get_server(unit_id);
+    
+    for(size_t i = 0; i < quantity; i++)
+    {
+        if((address>>3) > COIL_BUF_SIZE)
+        {
+            return NMBS_ERROR_INVALID_REQUEST;
+        }
+        nmbs_bitfield_write(coils_out, address, nmbs_bitfield_read(server->coils, address));
+        address++;
+    }
+    return NMBS_ERROR_NONE;
 }
 
 static nmbs_error server_read_holding_registers(uint16_t address, uint16_t quantity, uint16_t* registers_out, uint8_t unit_id, void* arg)
 {
-
+    nmbs_server_t* server = get_server(unit_id);
+    
+    for(size_t i = 0; i < quantity; i++)
+    {
+        if(address > REG_BUF_SIZE)
+        {
+            return NMBS_ERROR_INVALID_REQUEST;
+        }
+        registers_out[i] = server->regs[address++];
+    }
+    return NMBS_ERROR_NONE;
 }
 
 static nmbs_error server_write_single_coil(uint16_t address, bool value, uint8_t unit_id, void* arg)
 {
-
+    uint8_t coil = 0;
+    if(value)
+    {
+        coil |= 0x01;
+    }
+    server_write_multiple_coils(address, 1, &coil, unit_id, arg);
 }
 
 static nmbs_error server_write_multiple_coils(uint16_t address, uint16_t quantity, const nmbs_bitfield coils, uint8_t unit_id, void* arg)
 {
-
+    nmbs_server_t* server = get_server(unit_id);
-
+    
+    for(size_t i = 0; i < quantity; i++)
+    {
+        if((address>>3) > COIL_BUF_SIZE)
+        {
+            return NMBS_ERROR_INVALID_REQUEST;
+        }
+        nmbs_bitfield_write(server->coils, address, nmbs_bitfield_read(coils, i));
+        address++;
+    }
+    return NMBS_ERROR_NONE;
 }
+
 static nmbs_error server_write_single_register(uint16_t address, uint16_t value, uint8_t unit_id, void* arg)
 {
-
+    uint16_t reg = value;
+    server_write_multiple_registers(address, 1, &reg, unit_id, arg);
 }
 
 static nmbs_error server_write_multiple_registers(uint16_t address, uint16_t quantity, const uint16_t* registers, uint8_t unit_id, void* arg)
 {
-
+    nmbs_server_t* server = get_server(unit_id);
+    
+    for(size_t i = 0; i < quantity; i++)
+    {
+        if(address > REG_BUF_SIZE)
+        {
+            return NMBS_ERROR_INVALID_REQUEST;
+        }
+        server->regs[address++] = registers[i];
+    }
+    return NMBS_ERROR_NONE;
 }
 
 // Function to initialize the ring buffer
@@ -97,11 +165,6 @@ static bool ringbuf_is_empty(ringBuf* rb) {
     return (!rb->full && (rb->head == rb->tail));
 }
 
-// Function to check if the ring buffer is full
-static bool ringbuf_is_full(ringBuf* rb) {
-    return rb->full;
-}
-
 // Function to write multiple bytes to the ring buffer
 static void ringbuf_put(ringBuf* rb, const uint8_t* data, uint16_t length) {
     for (uint16_t i = 0; i < length; i++) {

examples/stm32/nanomodbus_stm32.h

@@ -19,14 +19,13 @@ extern "C" {
 // NanoModbus include
 #include "nanomodbus.h"
 
-void nanomodbus_server_init(nmbs_t* nmbs);
+typedef struct tNmbsServer{
-void nanomodbus_server_add(nmbs_server_t* server);
-
-struct tNmbsServer{
     uint8_t  id;
     uint8_t  coils[COIL_BUF_SIZE];
     uint16_t regs[REG_BUF_SIZE];
-}typedef nmbs_server_t;
+}nmbs_server_t;
+
+nmbs_error nanomodbus_server_init(nmbs_t* nmbs, nmbs_server_t* servers, uint8_t server_num);
 
 extern UART_HandleTypeDef NANOMB_UART;