diff --git a/examples/stm32/nanomodbus_stm32.c b/examples/stm32/nanomodbus_stm32.c index 8660b76..36069e6 100644 --- a/examples/stm32/nanomodbus_stm32.c +++ b/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]; -uint16_t reg_buf [REG_BUF_SIZE]; +static nmbs_server_t* servers; +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); - pf_conf.transport = NMBS_TRANSPORT_RTU; - pf_conf.read = read_serial; - pf_conf.write = write_serial; + nmbs_platform_conf_create(&conf); + conf.transport = NMBS_TRANSPORT_RTU; + conf.read = read_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, ®, 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++) { diff --git a/examples/stm32/nanomodbus_stm32.h b/examples/stm32/nanomodbus_stm32.h index 2a9e609..1f6fe77 100644 --- a/examples/stm32/nanomodbus_stm32.h +++ b/examples/stm32/nanomodbus_stm32.h @@ -19,14 +19,13 @@ extern "C" { // NanoModbus include #include "nanomodbus.h" -void nanomodbus_server_init(nmbs_t* nmbs); -void nanomodbus_server_add(nmbs_server_t* server); - -struct tNmbsServer{ +typedef 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;