Handling foreign RTU messages
This commit is contained in:
Valerio De Benedetto
parent
2d1ea540ca
commit
f8f8fd00b1
@ -16,6 +16,9 @@ target_compile_definitions(server_disabled PUBLIC NMBS_SERVER_DISABLED)
|
||||
add_executable(client_disabled nanomodbus.c tests/client_disabled.c)
|
||||
target_compile_definitions(client_disabled PUBLIC NMBS_CLIENT_DISABLED)
|
||||
|
||||
add_executable(multi_server_rtu nanomodbus.c tests/multi_server_rtu.c)
|
||||
target_compile_definitions(multi_server_rtu PUBLIC NMBS_DEBUG)
|
||||
|
||||
add_executable(client-tcp nanomodbus.c examples/linux/client-tcp.c)
|
||||
add_executable(server-tcp nanomodbus.c examples/linux/server-tcp.c)
|
||||
|
||||
|
||||
396
nanomodbus.c
396
nanomodbus.c
@ -25,10 +25,10 @@
|
||||
*/
|
||||
|
||||
#include "nanomodbus.h"
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
|
||||
|
||||
#ifdef NMBS_DEBUG
|
||||
#include <stdio.h>
|
||||
#define NMBS_DEBUG_PRINT(...) printf(__VA_ARGS__)
|
||||
@ -81,7 +81,7 @@ static void msg_state_reset(nmbs_t* nmbs) {
|
||||
nmbs->msg.fc = 0;
|
||||
nmbs->msg.transaction_id = 0;
|
||||
nmbs->msg.broadcast = false;
|
||||
nmbs->msg.ignored = 0;
|
||||
nmbs->msg.ignored = false;
|
||||
}
|
||||
|
||||
|
||||
@ -336,7 +336,7 @@ static nmbs_error recv_req_header(nmbs_t* nmbs, bool* first_byte_received) {
|
||||
|
||||
static void put_res_header(nmbs_t* nmbs, uint16_t data_length) {
|
||||
put_msg_header(nmbs, data_length);
|
||||
NMBS_DEBUG_PRINT("NMBS res -> fc %d\t", nmbs->msg.fc);
|
||||
NMBS_DEBUG_PRINT("%d NMBS res -> address_rtu %d\tfc %d\t", nmbs->address_rtu, nmbs->address_rtu, nmbs->msg.fc);
|
||||
}
|
||||
|
||||
|
||||
@ -345,7 +345,7 @@ static nmbs_error send_exception_msg(nmbs_t* nmbs, uint8_t exception) {
|
||||
put_msg_header(nmbs, 1);
|
||||
put_1(nmbs, exception);
|
||||
|
||||
NMBS_DEBUG_PRINT("NMBS res -> exception %d\n", exception);
|
||||
NMBS_DEBUG_PRINT("%d NMBS res -> address_rtu %d\texception %d", nmbs->address_rtu, nmbs->address_rtu, exception);
|
||||
|
||||
return send_msg(nmbs);
|
||||
}
|
||||
@ -368,7 +368,7 @@ static nmbs_error recv_res_header(nmbs_t* nmbs) {
|
||||
return NMBS_ERROR_INVALID_TCP_MBAP;
|
||||
}
|
||||
|
||||
if (nmbs->msg.unit_id != req_unit_id)
|
||||
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU && nmbs->msg.unit_id != req_unit_id)
|
||||
return NMBS_ERROR_INVALID_UNIT_ID;
|
||||
|
||||
if (nmbs->msg.fc != req_fc) {
|
||||
@ -385,14 +385,15 @@ static nmbs_error recv_res_header(nmbs_t* nmbs) {
|
||||
if (exception < 1 || exception > 4)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
NMBS_DEBUG_PRINT("exception %d\n", exception);
|
||||
NMBS_DEBUG_PRINT("%d NMBS res <- address_rtu %d\texception %d\n", nmbs->address_rtu, nmbs->msg.unit_id,
|
||||
exception);
|
||||
return exception;
|
||||
}
|
||||
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
}
|
||||
|
||||
NMBS_DEBUG_PRINT("NMBS res <- fc %d\t", nmbs->msg.fc);
|
||||
NMBS_DEBUG_PRINT("%d NMBS res <- address_rtu %d\tfc %d\t", nmbs->address_rtu, nmbs->msg.unit_id, nmbs->msg.fc);
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
@ -400,11 +401,198 @@ static nmbs_error recv_res_header(nmbs_t* nmbs) {
|
||||
|
||||
static void put_req_header(nmbs_t* nmbs, uint16_t data_length) {
|
||||
put_msg_header(nmbs, data_length);
|
||||
NMBS_DEBUG_PRINT("NMBS req -> fc %d\t", nmbs->msg.fc);
|
||||
#ifdef NMBS_DEBUG
|
||||
printf("%d ", nmbs->address_rtu);
|
||||
printf("NMBS req -> ");
|
||||
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) {
|
||||
if (nmbs->msg.broadcast)
|
||||
printf("broadcast\t");
|
||||
else
|
||||
printf("address_rtu %d\t", nmbs->dest_address_rtu);
|
||||
}
|
||||
|
||||
printf("fc %d\t", nmbs->msg.fc);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
static nmbs_error recv_read_discrete_res(nmbs_t* nmbs, nmbs_bitfield values) {
|
||||
nmbs_error err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 1);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint8_t coils_bytes = get_1(nmbs);
|
||||
NMBS_DEBUG_PRINT("b %d\t", coils_bytes);
|
||||
|
||||
err = recv(nmbs, coils_bytes);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
NMBS_DEBUG_PRINT("coils ");
|
||||
for (int i = 0; i < coils_bytes; i++) {
|
||||
uint8_t coil = get_1(nmbs);
|
||||
if (values)
|
||||
values[i] = coil;
|
||||
NMBS_DEBUG_PRINT("%d ", coil);
|
||||
}
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
static nmbs_error recv_read_registers_res(nmbs_t* nmbs, uint16_t quantity, uint16_t* registers) {
|
||||
nmbs_error err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 1);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint8_t registers_bytes = get_1(nmbs);
|
||||
NMBS_DEBUG_PRINT("b %d\t", registers_bytes);
|
||||
|
||||
err = recv(nmbs, registers_bytes);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
NMBS_DEBUG_PRINT("regs ");
|
||||
for (int i = 0; i < registers_bytes / 2; i++) {
|
||||
uint16_t reg = get_2(nmbs);
|
||||
if (registers)
|
||||
registers[i] = reg;
|
||||
NMBS_DEBUG_PRINT("%d ", reg);
|
||||
}
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (registers_bytes != quantity * 2)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
nmbs_error recv_write_single_coil_res(nmbs_t* nmbs, uint16_t address, bool value_req) {
|
||||
nmbs_error err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 4);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res = get_2(nmbs);
|
||||
uint16_t value_res = get_2(nmbs);
|
||||
|
||||
NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value_res);
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (address_res != address)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
if (value_res != value_req)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
nmbs_error recv_write_single_register_res(nmbs_t* nmbs, uint16_t address, uint16_t value_req) {
|
||||
nmbs_error err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 4);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res = get_2(nmbs);
|
||||
uint16_t value_res = get_2(nmbs);
|
||||
NMBS_DEBUG_PRINT("a %d\tvalue %d ", address, value_res);
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (address_res != address)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
if (value_res != value_req)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
nmbs_error recv_write_multiple_coils_res(nmbs_t* nmbs, uint16_t address, uint16_t quantity) {
|
||||
nmbs_error err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 4);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res = get_2(nmbs);
|
||||
uint16_t quantity_res = get_2(nmbs);
|
||||
NMBS_DEBUG_PRINT("a %d\tq %d", address_res, quantity_res);
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (address_res != address)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
if (quantity_res != quantity)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
nmbs_error recv_write_multiple_registers_res(nmbs_t* nmbs, uint16_t address, uint16_t quantity) {
|
||||
nmbs_error err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 4);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res = get_2(nmbs);
|
||||
uint16_t quantity_res = get_2(nmbs);
|
||||
NMBS_DEBUG_PRINT("a %d\tq %d", address_res, quantity_res);
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (address_res != address)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
if (quantity_res != quantity)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
|
||||
|
||||
#ifndef NMBS_SERVER_DISABLED
|
||||
#if !defined(NMBS_SERVER_READ_COILS_DISABLED) || !defined(NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED)
|
||||
static nmbs_error handle_read_discrete(nmbs_t* nmbs, nmbs_error (*callback)(uint16_t, uint16_t, nmbs_bitfield, void*)) {
|
||||
@ -461,6 +649,9 @@ static nmbs_error handle_read_discrete(nmbs_t* nmbs, nmbs_error (*callback)(uint
|
||||
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
|
||||
}
|
||||
}
|
||||
else {
|
||||
return recv_read_discrete_res(nmbs, NULL);
|
||||
}
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
@ -522,6 +713,9 @@ static nmbs_error handle_read_registers(nmbs_t* nmbs, nmbs_error (*callback)(uin
|
||||
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
|
||||
}
|
||||
}
|
||||
else {
|
||||
return recv_read_registers_res(nmbs, quantity, NULL);
|
||||
}
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
@ -600,6 +794,9 @@ static nmbs_error handle_write_single_coil(nmbs_t* nmbs) {
|
||||
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
|
||||
}
|
||||
}
|
||||
else {
|
||||
return recv_write_single_coil_res(nmbs, address, value);
|
||||
}
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
@ -647,6 +844,9 @@ static nmbs_error handle_write_single_register(nmbs_t* nmbs) {
|
||||
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
|
||||
}
|
||||
}
|
||||
else {
|
||||
return recv_write_single_register_res(nmbs, address, value);
|
||||
}
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
@ -717,6 +917,9 @@ static nmbs_error handle_write_multiple_coils(nmbs_t* nmbs) {
|
||||
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
|
||||
}
|
||||
}
|
||||
else {
|
||||
return recv_write_multiple_coils_res(nmbs, address, quantity);
|
||||
}
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
@ -787,6 +990,9 @@ static nmbs_error handle_write_multiple_registers(nmbs_t* nmbs) {
|
||||
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
|
||||
}
|
||||
}
|
||||
else {
|
||||
return recv_write_multiple_registers_res(nmbs, address, quantity);
|
||||
}
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
@ -883,23 +1089,28 @@ nmbs_error nmbs_server_poll(nmbs_t* nmbs) {
|
||||
}
|
||||
|
||||
#ifdef NMBS_DEBUG
|
||||
printf("%d ", nmbs->address_rtu);
|
||||
printf("NMBS req <- ");
|
||||
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) {
|
||||
if (nmbs->msg.broadcast)
|
||||
printf("broadcast\t");
|
||||
|
||||
printf("client_id %d\t", nmbs->msg.unit_id);
|
||||
else
|
||||
printf("address_rtu %d\t", nmbs->msg.unit_id);
|
||||
}
|
||||
#endif
|
||||
|
||||
err = handle_req_fc(nmbs);
|
||||
if (err != NMBS_ERROR_NONE) {
|
||||
if (!nmbs_error_is_exception(err))
|
||||
return err;
|
||||
if (err != NMBS_ERROR_NONE && !nmbs_error_is_exception(err)) {
|
||||
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU && err != NMBS_ERROR_TIMEOUT && nmbs->msg.ignored) {
|
||||
// Flush the remaining data on the line
|
||||
nmbs->platform.read(nmbs->msg.buf, sizeof(nmbs->msg.buf), 0, nmbs->platform.arg);
|
||||
}
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
@ -928,32 +1139,7 @@ static nmbs_error read_discrete(nmbs_t* nmbs, uint8_t fc, uint16_t address, uint
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 1);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint8_t coils_bytes = get_1(nmbs);
|
||||
NMBS_DEBUG_PRINT("b %d\t", coils_bytes);
|
||||
|
||||
err = recv(nmbs, coils_bytes);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
NMBS_DEBUG_PRINT("coils ");
|
||||
for (int i = 0; i < coils_bytes; i++) {
|
||||
values[i] = get_1(nmbs);
|
||||
NMBS_DEBUG_PRINT("%d", values[i]);
|
||||
}
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
return recv_read_discrete_res(nmbs, values);
|
||||
}
|
||||
|
||||
|
||||
@ -966,7 +1152,6 @@ nmbs_error nmbs_read_discrete_inputs(nmbs_t* nmbs, uint16_t address, uint16_t qu
|
||||
return read_discrete(nmbs, 2, address, quantity, inputs_out);
|
||||
}
|
||||
|
||||
|
||||
static nmbs_error read_registers(nmbs_t* nmbs, uint8_t fc, uint16_t address, uint16_t quantity, uint16_t* registers) {
|
||||
if (quantity < 1 || quantity > 125)
|
||||
return NMBS_ERROR_INVALID_ARGUMENT;
|
||||
@ -986,35 +1171,7 @@ static nmbs_error read_registers(nmbs_t* nmbs, uint8_t fc, uint16_t address, uin
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 1);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint8_t registers_bytes = get_1(nmbs);
|
||||
NMBS_DEBUG_PRINT("b %d\t", registers_bytes);
|
||||
|
||||
err = recv(nmbs, registers_bytes);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
NMBS_DEBUG_PRINT("regs ");
|
||||
for (int i = 0; i < registers_bytes / 2; i++) {
|
||||
registers[i] = get_2(nmbs);
|
||||
NMBS_DEBUG_PRINT("%d", registers[i]);
|
||||
}
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (registers_bytes != quantity * 2)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
return recv_read_registers_res(nmbs, quantity, registers);
|
||||
}
|
||||
|
||||
|
||||
@ -1043,30 +1200,8 @@ nmbs_error nmbs_write_single_coil(nmbs_t* nmbs, uint16_t address, bool value) {
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (!nmbs->msg.broadcast) {
|
||||
err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 4);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res = get_2(nmbs);
|
||||
uint16_t value_res = get_2(nmbs);
|
||||
|
||||
NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value_res);
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (address_res != address)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
if (value_res != value_req)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
}
|
||||
if (!nmbs->msg.broadcast)
|
||||
return recv_write_single_coil_res(nmbs, address, value_req);
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
@ -1085,29 +1220,8 @@ nmbs_error nmbs_write_single_register(nmbs_t* nmbs, uint16_t address, uint16_t v
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (!nmbs->msg.broadcast) {
|
||||
err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 4);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res = get_2(nmbs);
|
||||
uint16_t value_res = get_2(nmbs);
|
||||
NMBS_DEBUG_PRINT("a %d\tvalue %d ", address, value_res);
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (address_res != address)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
if (value_res != value)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
}
|
||||
if (!nmbs->msg.broadcast)
|
||||
return recv_write_single_register_res(nmbs, address, value);
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
@ -1140,29 +1254,8 @@ nmbs_error nmbs_write_multiple_coils(nmbs_t* nmbs, uint16_t address, uint16_t qu
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (!nmbs->msg.broadcast) {
|
||||
err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 4);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res = get_2(nmbs);
|
||||
uint16_t quantity_res = get_2(nmbs);
|
||||
NMBS_DEBUG_PRINT("a %d\tq %d", address_res, quantity_res);
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (address_res != address)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
if (quantity_res != quantity)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
}
|
||||
if (!nmbs->msg.broadcast)
|
||||
return recv_write_multiple_coils_res(nmbs, address, quantity);
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
@ -1195,29 +1288,8 @@ nmbs_error nmbs_write_multiple_registers(nmbs_t* nmbs, uint16_t address, uint16_
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (!nmbs->msg.broadcast) {
|
||||
err = recv_res_header(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
err = recv(nmbs, 4);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
uint16_t address_res = get_2(nmbs);
|
||||
uint16_t quantity_res = get_2(nmbs);
|
||||
NMBS_DEBUG_PRINT("a %d\tq %d", address_res, quantity_res);
|
||||
|
||||
err = recv_msg_footer(nmbs);
|
||||
if (err != NMBS_ERROR_NONE)
|
||||
return err;
|
||||
|
||||
if (address_res != address)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
|
||||
if (quantity_res != quantity)
|
||||
return NMBS_ERROR_INVALID_RESPONSE;
|
||||
}
|
||||
if (!nmbs->msg.broadcast)
|
||||
return recv_write_single_register_res(nmbs, address, quantity);
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
|
||||
198
tests/multi_server_rtu.c
Normal file
198
tests/multi_server_rtu.c
Normal file
@ -0,0 +1,198 @@
|
||||
#include <pthread.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "nanomodbus.h"
|
||||
|
||||
#define WIRE_SIZE 1024
|
||||
#define ITERATIONS 10
|
||||
|
||||
uint32_t run = 1;
|
||||
|
||||
pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
|
||||
|
||||
nmbs_t server1 = {0};
|
||||
nmbs_t server2 = {0};
|
||||
|
||||
uint32_t index_w = 0;
|
||||
uint32_t indices_r[3] = {0};
|
||||
|
||||
uint8_t wire[WIRE_SIZE];
|
||||
|
||||
|
||||
int32_t read_wire(uint8_t* buf, uint16_t count, int32_t timeout_ms, void* arg) {
|
||||
while (1) {
|
||||
pthread_mutex_lock(&mutex);
|
||||
|
||||
uint32_t index = (uint32_t) arg;
|
||||
uint32_t read = 0;
|
||||
for (int i = 0; i < count; i++) {
|
||||
if (indices_r[index] == index_w)
|
||||
break;
|
||||
|
||||
indices_r[index] = (indices_r[index] + 1) % WIRE_SIZE;
|
||||
buf[i] = wire[indices_r[index]];
|
||||
read++;
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&mutex);
|
||||
|
||||
if (read != 0)
|
||||
return read;
|
||||
|
||||
if (timeout_ms != 0)
|
||||
usleep(timeout_ms * 1000);
|
||||
else
|
||||
return 0;
|
||||
|
||||
timeout_ms = 0;
|
||||
}
|
||||
}
|
||||
|
||||
int32_t write_wire(const uint8_t* buf, uint16_t count, int32_t timeout_ms, void* arg) {
|
||||
pthread_mutex_lock(&mutex);
|
||||
|
||||
uint32_t index = (uint32_t) arg;
|
||||
uint32_t written = 0;
|
||||
for (int i = 0; i < count; i++) {
|
||||
index_w = (index_w + 1) % WIRE_SIZE;
|
||||
indices_r[index] = index_w;
|
||||
wire[index_w] = buf[i];
|
||||
written++;
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&mutex);
|
||||
|
||||
return written;
|
||||
}
|
||||
|
||||
nmbs_error read_coils(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, void* arg) {
|
||||
for (int i = 0; i < quantity; i++)
|
||||
nmbs_bitfield_write(coils_out, address + i, 1);
|
||||
|
||||
return NMBS_ERROR_NONE;
|
||||
}
|
||||
|
||||
void* poll_server1(void* arg) {
|
||||
while (run) {
|
||||
nmbs_server_poll(&server1);
|
||||
}
|
||||
}
|
||||
|
||||
void* poll_server2(void* arg) {
|
||||
while (run) {
|
||||
nmbs_server_poll(&server2);
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
nmbs_platform_conf c_conf;
|
||||
c_conf.arg = wire;
|
||||
c_conf.transport = NMBS_TRANSPORT_RTU;
|
||||
c_conf.read = read_wire;
|
||||
c_conf.write = write_wire;
|
||||
c_conf.arg = 0;
|
||||
|
||||
nmbs_platform_conf s1_conf = c_conf;
|
||||
s1_conf.arg = (void*) 1;
|
||||
|
||||
nmbs_platform_conf s2_conf = c_conf;
|
||||
s2_conf.arg = (void*) 2;
|
||||
|
||||
nmbs_t client = {0};
|
||||
nmbs_error err = nmbs_client_create(&client, &c_conf);
|
||||
if (err != NMBS_ERROR_NONE) {
|
||||
fprintf(stderr, "Error creating modbus client\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
nmbs_set_read_timeout(&client, 5000);
|
||||
nmbs_set_byte_timeout(&client, 100);
|
||||
|
||||
nmbs_callbacks callbacks = {0};
|
||||
callbacks.read_coils = read_coils;
|
||||
|
||||
err = nmbs_server_create(&server1, 33, &s1_conf, &callbacks);
|
||||
if (err != NMBS_ERROR_NONE) {
|
||||
fprintf(stderr, "Error creating modbus server 1\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
nmbs_set_read_timeout(&server1, 100);
|
||||
nmbs_set_byte_timeout(&server1, 100);
|
||||
|
||||
err = nmbs_server_create(&server2, 99, &s2_conf, &callbacks);
|
||||
if (err != NMBS_ERROR_NONE) {
|
||||
fprintf(stderr, "Error creating modbus server 2\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
nmbs_set_read_timeout(&server2, 100);
|
||||
nmbs_set_byte_timeout(&server2, 100);
|
||||
|
||||
pthread_t thread1, thread2;
|
||||
int ret = pthread_create(&thread1, NULL, poll_server1, NULL);
|
||||
if (ret != 0) {
|
||||
fprintf(stderr, "Error creating thread 1\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
ret = pthread_create(&thread2, NULL, poll_server2, NULL);
|
||||
if (ret != 0) {
|
||||
fprintf(stderr, "Error creating thread 2\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
sleep(1);
|
||||
|
||||
nmbs_bitfield coils;
|
||||
for (uint32_t c = 0; c < 10; c++) {
|
||||
nmbs_bitfield_write(coils, c, rand() % 1);
|
||||
}
|
||||
|
||||
nmbs_set_destination_rtu_address(&client, 33);
|
||||
|
||||
err = nmbs_write_multiple_coils(&client, 0, 10, coils);
|
||||
if (err != NMBS_ERROR_NONE) {
|
||||
fprintf(stderr, "Error writing coils to %d %s\n", 33, nmbs_strerror(err));
|
||||
}
|
||||
|
||||
nmbs_bitfield coils_read;
|
||||
err = nmbs_read_coils(&client, 0, 10, coils_read);
|
||||
if (err != NMBS_ERROR_NONE) {
|
||||
fprintf(stderr, "Error reading coils from %d %s\n", 33, nmbs_strerror(err));
|
||||
}
|
||||
|
||||
if (memcmp(coils, coils, sizeof(nmbs_bitfield)) != 0) {
|
||||
fprintf(stderr, "Coils mismatch from %d\n", 33);
|
||||
}
|
||||
|
||||
for (uint32_t c = 0; c < 10; c++) {
|
||||
nmbs_bitfield_write(coils, c, rand() % 1);
|
||||
}
|
||||
|
||||
nmbs_set_destination_rtu_address(&client, 99);
|
||||
|
||||
err = nmbs_write_multiple_coils(&client, 0, 10, coils);
|
||||
if (err != NMBS_ERROR_NONE) {
|
||||
fprintf(stderr, "Error writing coils to %d %s\n", 99, nmbs_strerror(err));
|
||||
}
|
||||
|
||||
err = nmbs_read_coils(&client, 0, 10, coils_read);
|
||||
if (err != NMBS_ERROR_NONE) {
|
||||
fprintf(stderr, "Error reading coils from %d %s\n", 99, nmbs_strerror(err));
|
||||
}
|
||||
|
||||
if (memcmp(coils, coils, sizeof(nmbs_bitfield)) != 0) {
|
||||
fprintf(stderr, "Coils mismatch from %d\n", 99);
|
||||
}
|
||||
|
||||
sleep(5);
|
||||
|
||||
run = 0;
|
||||
pthread_join(thread1, NULL);
|
||||
pthread_join(thread2, NULL);
|
||||
|
||||
return 0;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user