judsonupchurch/nanoMODBUS
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

WIP FC 43 / 14 Read Device Identification

Browse Source
This commit is contained in:
Valerio De Benedetto 2024-02-10 02:47:31 +01:00
parent 0234a563c1
commit dccaa4f432
3 changed files with 844 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

581
nanomodbus.c
View File

@ -75,6 +75,17 @@ static void put_2(nmbs_t* nmbs, uint16_t data) {
}
static void set_1(nmbs_t* nmbs, uint8_t data, uint8_t index) {
nmbs->msg.buf[index] = data;
}
static void set_2(nmbs_t* nmbs, uint16_t data, uint8_t index) {
nmbs->msg.buf[index] = (uint8_t) ((data >> 8) & 0xFFU);
nmbs->msg.buf[index + 1] = (uint8_t) data;
}
static uint8_t* get_n(nmbs_t* nmbs, uint16_t n) {
uint8_t* msg_buf_ptr = nmbs->msg.buf + nmbs->msg.buf_idx;
nmbs->msg.buf_idx += n;
@ -82,6 +93,12 @@ static uint8_t* get_n(nmbs_t* nmbs, uint16_t n) {
}
static void put_n(nmbs_t* nmbs, const uint8_t* data, uint8_t size) {
memcpy(&nmbs->msg.buf[nmbs->msg.buf_idx], data, size);
nmbs->msg.buf_idx += size;
}
static uint16_t* get_regs(nmbs_t* nmbs, uint16_t n) {
uint16_t* msg_buf_ptr = (uint16_t*) (nmbs->msg.buf + nmbs->msg.buf_idx);
nmbs->msg.buf_idx += n * 2;
@ -130,6 +147,9 @@ static void msg_state_req(nmbs_t* nmbs, uint8_t fc) {
else
nmbs->current_tid++;
// Flush the remaining data on the line before sending the request
nmbs->platform.read(nmbs->msg.buf, sizeof(nmbs->msg.buf), 0, nmbs->platform.arg);
msg_state_reset(nmbs);
nmbs->msg.unit_id = nmbs->dest_address_rtu;
nmbs->msg.fc = fc;
@ -338,6 +358,14 @@ static void put_msg_header(nmbs_t* nmbs, uint16_t data_length) {
}
static void set_msg_header_size(nmbs_t* nmbs, uint16_t data_length) {
if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) {
data_length += 2;
set_2(nmbs, data_length, 4);
}
}
static nmbs_error send_msg(nmbs_t* nmbs) {
NMBS_DEBUG_PRINT("\n");
@ -352,6 +380,17 @@ static nmbs_error send_msg(nmbs_t* nmbs) {
}
#ifndef NMBS_CLIENT_DISABLED
static nmbs_error send_req(nmbs_t* nmbs) {
if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) {
// Flush the remaining data on the line before sending the request
// nmbs->platform.read(nmbs->msg.buf, sizeof(nmbs->msg.buf), 0, nmbs->platform.arg);
}
return send_msg(nmbs);
}
#endif
#ifndef NMBS_SERVER_DISABLED
static nmbs_error recv_req_header(nmbs_t* nmbs, bool* first_byte_received) {
nmbs_error err = recv_msg_header(nmbs, first_byte_received);
@ -731,6 +770,85 @@ nmbs_error recv_write_file_record_res(nmbs_t* nmbs, uint16_t file_number, uint16
return NMBS_ERROR_NONE;
}
nmbs_error recv_read_device_identification_res(nmbs_t* nmbs, uint8_t buffers_count, char** buffers_out,
uint8_t buffers_length, const uint8_t* order, uint8_t* ids_out,
uint8_t* next_object_id_out, uint8_t* objects_count_out) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
err = recv(nmbs, 6);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t mei_type = get_1(nmbs);
if (mei_type != 0x0E)
return NMBS_ERROR_INVALID_RESPONSE;
uint8_t read_device_id_code = get_1(nmbs);
if (read_device_id_code < 1 || read_device_id_code > 4)
return NMBS_ERROR_INVALID_RESPONSE;
uint8_t conformity_level = get_1(nmbs);
if (conformity_level < 1 || (conformity_level > 3 && conformity_level < 0x81) || conformity_level > 0x83)
return NMBS_ERROR_INVALID_RESPONSE;
uint8_t more_follows = get_1(nmbs);
if (more_follows != 0 && more_follows != 0xFF)
return NMBS_ERROR_INVALID_RESPONSE;
uint8_t next_object_id = get_1(nmbs);
uint8_t objects_count = get_1(nmbs);
if (objects_count_out)
*objects_count_out = objects_count;
if (buffers_count == 0) {
buffers_out = NULL;
}
else if (objects_count > buffers_count)
return NMBS_ERROR_INVALID_ARGUMENT;
if (more_follows == 0)
next_object_id = 0x7F; // This value is reserved in the spec, we use it to signal stream is finished
if (next_object_id_out)
*next_object_id_out = next_object_id;
uint8_t res_size_left = 253 - 7;
for (int i = 0; i < objects_count; i++) {
err = recv(nmbs, 2);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t object_id = get_1(nmbs);
uint8_t object_length = get_1(nmbs);
res_size_left -= 2;
if (object_length > res_size_left)
return NMBS_ERROR_INVALID_RESPONSE;
err = recv(nmbs, object_length);
if (err != NMBS_ERROR_NONE)
return err;
const char* str = (const char*) get_n(nmbs, object_length);
if (ids_out)
ids_out[i] = object_id;
uint8_t buf_index = i;
if (order)
buf_index = order[object_id];
if (buffers_out) {
strncpy(buffers_out[buf_index], str, buffers_length);
buffers_out[buf_index][object_length] = 0;
}
}
return recv_msg_footer(nmbs);
}
#ifndef NMBS_SERVER_DISABLED
#if !defined(NMBS_SERVER_READ_COILS_DISABLED) || !defined(NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED)
@ -831,6 +949,7 @@ static nmbs_error handle_read_registers(nmbs_t* nmbs,
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
// TODO check all these read request broadcast use cases
if (!nmbs->msg.broadcast) {
uint8_t regs_bytes = quantity * 2;
put_res_header(nmbs, 1 + regs_bytes);
@ -1472,6 +1591,170 @@ static nmbs_error handle_read_write_registers(nmbs_t* nmbs) {
}
#endif
#ifndef NMBS_SERVER_READ_DEVICE_IDENTIFICATION_DISABLED
static nmbs_error handle_read_device_identification(nmbs_t* nmbs) {
nmbs_error err = recv(nmbs, 3);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t mei_type = get_1(nmbs);
uint8_t read_device_id_code = get_1(nmbs);
uint8_t object_id = get_1(nmbs);
NMBS_DEBUG_PRINT("c %d\to %d", read_device_id_code, object_id);
err = recv_msg_footer(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.ignored) {
if (!nmbs->callbacks.read_device_identification_map || !nmbs->callbacks.read_device_identification)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
if (mei_type != 0x0E)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION);
if (read_device_id_code < 1 || read_device_id_code > 4)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
if (object_id > 6 && object_id < 0x80)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
if (!nmbs->msg.broadcast) {
char str[NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH];
nmbs_bitfield_256 map;
nmbs_bitfield_reset(map);
err = nmbs->callbacks.read_device_identification_map(map);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
put_res_header(nmbs, 0); // Length will be set later
put_1(nmbs, 0x0E);
put_1(nmbs, read_device_id_code);
put_1(nmbs, 0x83);
if (read_device_id_code == 4) {
if (!nmbs_bitfield_read(map, object_id))
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
put_1(nmbs, 0); // More follows
put_1(nmbs, 0); // Next Object Id
put_1(nmbs, 1); // Number of objects
str[0] = 0;
err = nmbs->callbacks.read_device_identification(object_id, str);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
size_t str_len = strlen(str);
put_1(nmbs, object_id); // Object id
put_1(nmbs, str_len); // Object length
put_n(nmbs, (uint8_t*) str, str_len);
set_msg_header_size(nmbs, 6 + 2 + str_len);
return send_msg(nmbs);
}
uint8_t more_follows_idx = nmbs->msg.buf_idx;
put_1(nmbs, 0);
uint8_t next_object_id_idx = nmbs->msg.buf_idx;
put_1(nmbs, 0);
uint8_t number_of_objects_idx = nmbs->msg.buf_idx;
put_1(nmbs, 0);
int16_t res_size_left = 253 - 7;
uint8_t last_id = 0;
uint8_t msg_size = 6;
uint8_t res_more_follows = 0;
uint8_t res_next_object_id = 0;
uint8_t res_number_of_objects = 0;
switch (read_device_id_code) {
case 1:
if (object_id > 0x02)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
last_id = 0x02;
break;
case 2:
if (object_id < 0x03 || object_id > 0x07)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
last_id = 0x07;
break;
case 3:
if (object_id < 0x80)
return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
last_id = 0xFF;
break;
default:
// Unreachable
break;
}
for (uint16_t id = object_id; id <= last_id; id++) {
if (!nmbs_bitfield_read(map, id)) {
if (id < 0x03)
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
continue;
}
str[0] = 0;
err = nmbs->callbacks.read_device_identification((uint8_t) id, str);
if (err != NMBS_ERROR_NONE) {
if (nmbs_error_is_exception(err))
return send_exception_msg(nmbs, err);
return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
}
int16_t str_len = (int16_t) strlen(str);
res_size_left = res_size_left - 2 - str_len;
if (res_size_left < 0) {
res_more_follows = 0xFF;
res_next_object_id = id;
break;
}
put_1(nmbs, (uint8_t) id); // Object id
put_1(nmbs, str_len); // Object length
put_n(nmbs, (uint8_t*) str, str_len);
msg_size += (2 + str_len);
res_number_of_objects++;
}
set_1(nmbs, res_more_follows, more_follows_idx);
set_1(nmbs, res_next_object_id, next_object_id_idx);
set_1(nmbs, res_number_of_objects, number_of_objects_idx);
set_msg_header_size(nmbs, msg_size);
return send_msg(nmbs);
}
}
else {
return recv_read_device_identification_res(nmbs, 0, NULL, 0, NULL, NULL, NULL, NULL);
}
return NMBS_ERROR_NONE;
}
#endif
static nmbs_error handle_req_fc(nmbs_t* nmbs) {
NMBS_DEBUG_PRINT("fc %d\t", nmbs->msg.fc);
@ -1543,6 +1826,12 @@ static nmbs_error handle_req_fc(nmbs_t* nmbs) {
err = handle_read_write_registers(nmbs);
break;
#endif
#ifndef NMBS_SERVER_READ_DEVICE_IDENTIFICATION_DISABLED
case 43:
err = handle_read_device_identification(nmbs);
break;
#endif
default:
err = NMBS_EXCEPTION_ILLEGAL_FUNCTION;
}
@ -1630,7 +1919,7 @@ static nmbs_error read_discrete(nmbs_t* nmbs, uint8_t fc, uint16_t address, uint
NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity);
nmbs_error err = send_msg(nmbs);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
@ -1662,7 +1951,7 @@ static nmbs_error read_registers(nmbs_t* nmbs, uint8_t fc, uint16_t address, uin
NMBS_DEBUG_PRINT("a %d\tq %d ", address, quantity);
nmbs_error err = send_msg(nmbs);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
@ -1691,7 +1980,7 @@ nmbs_error nmbs_write_single_coil(nmbs_t* nmbs, uint16_t address, bool value) {
NMBS_DEBUG_PRINT("a %d\tvalue %d ", address, value_req);
nmbs_error err = send_msg(nmbs);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
@ -1711,7 +2000,7 @@ nmbs_error nmbs_write_single_register(nmbs_t* nmbs, uint16_t address, uint16_t v
NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value);
nmbs_error err = send_msg(nmbs);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
@ -1745,7 +2034,7 @@ nmbs_error nmbs_write_multiple_coils(nmbs_t* nmbs, uint16_t address, uint16_t qu
NMBS_DEBUG_PRINT("%d ", coils[i]);
}
nmbs_error err = send_msg(nmbs);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
@ -1779,7 +2068,7 @@ nmbs_error nmbs_write_multiple_registers(nmbs_t* nmbs, uint16_t address, uint16_
NMBS_DEBUG_PRINT("%d ", registers[i]);
}
nmbs_error err = send_msg(nmbs);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
@ -1812,14 +2101,11 @@ nmbs_error nmbs_read_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t re
put_2(nmbs, count);
NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fread ", file_number, record_number, count);
nmbs_error err = send_msg(nmbs);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
if (!nmbs->msg.broadcast)
return recv_read_file_record_res(nmbs, registers, count);
return NMBS_ERROR_NONE;
return recv_read_file_record_res(nmbs, registers, count);
}
@ -1847,7 +2133,7 @@ nmbs_error nmbs_write_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t r
put_regs(nmbs, registers, count);
NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fwrite ", file_number, record_number, count);
nmbs_error err = send_msg(nmbs);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
@ -1857,6 +2143,7 @@ nmbs_error nmbs_write_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t r
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_read_write_registers(nmbs_t* nmbs, uint16_t read_address, uint16_t read_quantity,
uint16_t* registers_out, uint16_t write_address, uint16_t write_quantity,
const uint16_t* registers) {
@ -1892,7 +2179,7 @@ nmbs_error nmbs_read_write_registers(nmbs_t* nmbs, uint16_t read_address, uint16
NMBS_DEBUG_PRINT("%d ", registers[i]);
}
nmbs_error err = send_msg(nmbs);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
@ -1903,6 +2190,258 @@ nmbs_error nmbs_read_write_registers(nmbs_t* nmbs, uint16_t read_address, uint16
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_read_device_identification_basic(nmbs_t* nmbs, char* vendor_name, char* product_code,
char* major_minor_revision, uint8_t buffers_length) {
const uint8_t order[3] = {0, 1, 2};
char* buffers[3] = {vendor_name, product_code, major_minor_revision};
uint8_t total_received = 0;
uint8_t next_object_id = 0x00;
while (next_object_id != 0x7F) {
msg_state_req(nmbs, 43);
put_msg_header(nmbs, 3);
put_1(nmbs, 0x0E);
put_1(nmbs, 1);
put_1(nmbs, next_object_id);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t objects_received = 0;
err = recv_read_device_identification_res(nmbs, 3, buffers, buffers_length, order, NULL, &next_object_id,
&objects_received);
if (err != NMBS_ERROR_NONE)
return err;
total_received += objects_received;
if (total_received > 3)
return NMBS_ERROR_INVALID_RESPONSE;
if (objects_received == 0)
return NMBS_ERROR_INVALID_RESPONSE;
}
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_read_device_identification_regular(nmbs_t* nmbs, char* vendor_url, char* product_name, char* model_name,
char* user_application_name, uint8_t buffers_length) {
const uint8_t order[7] = {0, 0, 0, 0, 1, 2, 3};
char* buffers[4] = {vendor_url, product_name, model_name, user_application_name};
uint8_t total_received = 0;
uint8_t next_object_id = 0x03;
while (next_object_id != 0x7F) {
msg_state_req(nmbs, 43);
put_req_header(nmbs, 3);
put_1(nmbs, 0x0E);
put_1(nmbs, 2);
put_1(nmbs, next_object_id);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t objects_received = 0;
err = recv_read_device_identification_res(nmbs, 4, buffers, buffers_length, order, NULL, &next_object_id,
&objects_received);
if (err != NMBS_ERROR_NONE)
return err;
total_received += objects_received;
if (total_received > 4)
return NMBS_ERROR_INVALID_RESPONSE;
if (objects_received == 0)
return NMBS_ERROR_INVALID_RESPONSE;
}
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_read_device_identification_extended(nmbs_t* nmbs, uint8_t object_id_start, uint8_t* ids, char** buffers,
uint8_t ids_length, uint8_t buffer_length,
uint8_t* objects_count_out) {
// if (object_id_start < 0x80)
// return NMBS_ERROR_INVALID_ARGUMENT;
//
// msg_state_req(nmbs, 43);
// put_msg_header(nmbs, 3);
// put_1(nmbs, 0x0E);
// put_1(nmbs, 3);
// put_1(nmbs, object_id_start);
//
// nmbs_error err = send_req(nmbs);
// if (err != NMBS_ERROR_NONE)
// return err;
//
// err = recv_read_device_identification_res(nmbs, buffers_count, buffers, buffer_length, NULL, next_object_id_out,
// objects_count_out);
// if (err != NMBS_ERROR_NONE)
// return err;
//
// if (next_object_id_out && *next_object_id_out == 0x7F) {
// *next_object_id_out = object_id_start;
// }
//
// return NMBS_ERROR_NONE;
if (object_id_start < 0x80)
return NMBS_ERROR_INVALID_ARGUMENT;
uint8_t total_received = 0;
uint8_t next_object_id = object_id_start;
while (next_object_id != 0x7F) {
msg_state_req(nmbs, 43);
put_req_header(nmbs, 3);
put_1(nmbs, 0x0E);
put_1(nmbs, 3);
put_1(nmbs, next_object_id);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
uint8_t objects_received = 0;
err = recv_read_device_identification_res(nmbs, ids_length - total_received, &buffers[total_received],
buffer_length, NULL, &ids[total_received], &next_object_id,
&objects_received);
if (err != NMBS_ERROR_NONE)
return err;
total_received += objects_received;
}
*objects_count_out = total_received;
return NMBS_ERROR_NONE;
}
nmbs_error nmbs_read_device_identification(nmbs_t* nmbs, uint8_t object_id, char* buffer, uint8_t buffer_length) {
if (object_id > 0x06 && object_id < 0x80)
return NMBS_ERROR_INVALID_ARGUMENT;
msg_state_req(nmbs, 43);
put_req_header(nmbs, 3);
put_1(nmbs, 0x0E);
put_1(nmbs, 4);
put_1(nmbs, object_id);
nmbs_error err = send_req(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
char* buf[1] = {buffer};
return recv_read_device_identification_res(nmbs, 1, buf, buffer_length, NULL, NULL, NULL, NULL);
}
//nmbs_error nmbs_read_device_identification(nmbs_t* nmbs, const nmbs_object_id* object_ids, uint8_t object_ids_len,
// char** buffers_out, uint8_t buffer_size) {
// if (object_ids_len == 0 || object_ids_len > 7) {
// return NMBS_ERROR_INVALID_ARGUMENT;
// }
//
// if (buffers_out == NULL) {
// return NMBS_ERROR_INVALID_ARGUMENT;
// }
//
// uint8_t cont_start = 0;
// uint8_t cont_length = 0;
// uint8_t last_id = 0;
// uint8_t indices[7] = {0};
//
// for (int i = 0; i < object_ids_len; i++) {
// if (object_ids[i] > 0x06)
// return NMBS_ERROR_INVALID_ARGUMENT;
//
// indices[object_ids[i]] = i;
// bool send = false;
//
// if (cont_length == 0) {
// if (object_ids[i] == 0x00 || object_ids[i] == 0x03) {
// cont_start = i;
// cont_length = 1;
// }
// }
// else if (object_ids[i] - last_id == 1) {
// cont_length++;
// }
// else {
// cont_length = 0;
// }
//
// if ((object_ids[cont_start] == 0x00 && cont_length == 3) ||
// (object_ids[cont_start] == 0x03 && cont_length == 5)) {
// send = true;
// }
//
// if (object_ids_len - i > 3 && object_ids[i] == 0x00 && object_ids[i + 1] == 0x01 && object_ids[i + 2] == 0x02) {
//
// }
//
// if (send || i == object_ids_len - 1) {
// uint8_t next_id = 0;
// uint8_t objects_received = 0;
//
// msg_state_req(nmbs, 43);
// put_msg_header(nmbs, 3);
// put_1(nmbs, 0x0E);
//
// if (cont_length > 1) {
// // Stream access
// while (cont_length > 0) {
// if (object_ids[cont_start] < 0x03)
// put_1(nmbs, 1);
// if (object_ids[cont_start] >= 0x03)
// put_1(nmbs, 2);
//
// put_1(nmbs, next_id);
//
// nmbs_error err = send_req(nmbs);
// if (err != NMBS_ERROR_NONE)
// return err;
//
// err = recv_read_device_identification_res(nmbs, cont_length, buffers_out + cont_start, buffer_size,
// indices + cont_start, &next_id, &objects_received);
// if (err != NMBS_ERROR_NONE)
// return err;
//
// cont_start += objects_received;
// cont_length -= objects_received;
//
// for (int j = 0; j < objects_received; j++) {}
// }
// }
// else {
// // Individual access
// put_1(nmbs, 4);
// put_1(nmbs, object_ids[i]);
//
// nmbs_error err = send_req(nmbs);
// if (err != NMBS_ERROR_NONE)
// return err;
//
// err = recv_read_device_identification_res(nmbs, 1, buffers_out + cont_start, buffer_size,
// indices + cont_start, &next_id, &objects_received);
// if (err != NMBS_ERROR_NONE)
// return err;
//
// cont_length = 0;
// }
// }
//
// last_id = object_ids[i];
// }
//
// return NMBS_ERROR_NONE;
//}
nmbs_error nmbs_send_raw_pdu(nmbs_t* nmbs, uint8_t fc, const uint8_t* data, uint16_t data_len) {
msg_state_req(nmbs, fc);
put_msg_header(nmbs, data_len);
@ -1913,11 +2452,11 @@ nmbs_error nmbs_send_raw_pdu(nmbs_t* nmbs, uint8_t fc, const uint8_t* data, uint
NMBS_DEBUG_PRINT("%d ", data[i]);
}
return send_msg(nmbs);
return send_req(nmbs);
}
nmbs_error nmbs_receive_raw_pdu_response(nmbs_t* nmbs, uint8_t* data_out, uint16_t data_out_len) {
nmbs_error nmbs_receive_raw_pdu_response(nmbs_t* nmbs, uint8_t* data_out, uint8_t data_out_len) {
nmbs_error err = recv_res_header(nmbs);
if (err != NMBS_ERROR_NONE)
return err;
@ -1926,8 +2465,13 @@ nmbs_error nmbs_receive_raw_pdu_response(nmbs_t* nmbs, uint8_t* data_out, uint16
if (err != NMBS_ERROR_NONE)
return err;
for (uint16_t i = 0; i < data_out_len; i++) {
data_out[i] = get_1(nmbs);
if (data_out) {
for (uint16_t i = 0; i < data_out_len; i++)
data_out[i] = get_1(nmbs);
}
else {
for (uint16_t i = 0; i < data_out_len; i++)
get_1(nmbs);
}
err = recv_msg_footer(nmbs);
@ -1942,6 +2486,9 @@ nmbs_error nmbs_receive_raw_pdu_response(nmbs_t* nmbs, uint8_t* data_out, uint16
#ifndef NMBS_STRERROR_DISABLED
const char* nmbs_strerror(nmbs_error error) {
switch (error) {
case NMBS_ERROR_INVALID_REQUEST:
return "invalid request received";
case NMBS_ERROR_INVALID_UNIT_ID:
return "invalid unit ID received";

101
nanomodbus.h
View File

@ -71,6 +71,19 @@ typedef enum nmbs_error {
NMBS_EXCEPTION_SERVER_DEVICE_FAILURE = 4, /**< Modbus exception 4 */
} nmbs_error;
/**
* 43 / 14 (0x2B / 0x0E) Read Device Identification Object Id definitions
*/
typedef enum nmbs_object_id {
NMBS_OBJECT_ID_VENDOR_NAME = 0,
NMBS_OBJECT_ID_PRODUCT_CODE = 1,
NMBS_OBJECT_ID_MAJOR_MINOR_REVISION = 2,
NMBS_OBJECT_ID_VENDOR_URL = 3,
NMBS_OBJECT_ID_PRODUCT_NAME = 4,
NMBS_OBJECT_ID_MODEL_NAME = 5,
NMBS_OBJECT_ID_USER_APPLICATION_NAME = 6,
} nmbs_object_id;
/**
* Return whether the nmbs_error is a modbus exception
* @e nmbs_error to check
@ -83,11 +96,26 @@ typedef enum nmbs_error {
*/
typedef uint8_t nmbs_bitfield[250];
/**
* Bitfield consisting of 256 values
*/
typedef uint8_t nmbs_bitfield_256[32];
/**
* Read a bit from the nmbs_bitfield bf at position b
*/
#define nmbs_bitfield_read(bf, b) ((bool) ((bf)[(b) / 8] & (0x1 << ((b) % 8))))
/**
* Set a bit of the nmbs_bitfield bf at position b
*/
#define nmbs_bitfield_set(bf, b) (((bf)[(b) / 8]) = (((bf)[(b) / 8]) | (0x1 << ((b) % 8))))
/**
* Reset a bit of the nmbs_bitfield bf at position b
*/
#define nmbs_bitfield_unset(bf, b) (((bf)[(b) / 8]) = (((bf)[(b) / 8]) & ~(0x1 << ((b) % 8))))
/**
* Write value v to the nmbs_bitfield bf at position b
*/
@ -97,8 +125,7 @@ typedef uint8_t nmbs_bitfield[250];
/**
* Reset (zero) the whole bitfield
*/
#define nmbs_bitfield_reset(bf) memset(bf, 0, sizeof(nmbs_bitfield))
#define nmbs_bitfield_reset(bf) memset(bf, 0, sizeof(bf))
/**
* Modbus transport type.
@ -194,6 +221,12 @@ typedef struct nmbs_callbacks {
nmbs_error (*write_file_record)(uint16_t file_number, uint16_t record_number, const uint16_t* registers,
uint16_t count, uint8_t unit_id, void* arg);
#endif
#ifndef NMBS_SERVER_READ_DEVICE_IDENTIFICATION_DISABLED
#define NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH 128
nmbs_error (*read_device_identification)(uint8_t object_id, char buffer[NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH]);
nmbs_error (*read_device_identification_map)(nmbs_bitfield_256 map);
#endif
#endif
void* arg; // User data, will be passed to functions above
@ -201,7 +234,8 @@ typedef struct nmbs_callbacks {
/**
* nanoMODBUS client/server instance type. All struct members are to be considered private, it is not advisable to read/write them directly.
* nanoMODBUS client/server instance type. All struct members are to be considered private,
* it is not advisable to read/write them directly.
*/
typedef struct nmbs_t {
struct {
@ -413,6 +447,61 @@ nmbs_error nmbs_read_write_registers(nmbs_t* nmbs, uint16_t read_address, uint16
uint16_t* registers_out, uint16_t write_address, uint16_t write_quantity,
const uint16_t* registers);
/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to read all Basic Object Id values (Read Device ID code 1)
* @param nmbs pointer to the nmbs_t instance
* @param object_id requested Object Id
* @param vendor_name char array where the read VendorName value will be stored
* @param product_code char array where the read ProductCode value will be stored
* @param major_minor_revision char array where the read MajorMinorRevision value will be stored
* @param buffer_length length of every char array
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_device_identification_basic(nmbs_t* nmbs, char* vendor_name, char* product_code,
char* major_minor_revision, uint8_t buffers_length);
/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to read all Regular Object Id values (Read Device ID code 2)
* @param nmbs pointer to the nmbs_t instance
* @param object_id requested Object Id
* @param vendor_url char array where the read VendorUrl value will be stored
* @param product_name char array where the read ProductName value will be stored
* @param model_name char array where the read ModelName value will be stored
* @param user_application_name char array where the read UserApplicationName value will be stored
*
* @param buffer_length length of every char array
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_device_identification_regular(nmbs_t* nmbs, char* vendor_url, char* product_name, char* model_name,
char* user_application_name, uint8_t buffers_length);
/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to read all Extended Object Id values (Read Device ID code 3)
* @param nmbs pointer to the nmbs_t instance
* @param object_id requested Object Id
* @param ids array where the read Object Ids will be stored
* @param buffers array of char arrays where the read values will be stored
* @param ids_length length of the ids array and buffers array
* @param buffer_length length of each char array
* @param objects_count_out retrieved Object Ids count
*
* @return NMBS_ERROR_NONE if successful, NMBS_INVALID_ARGUMENT if buffers_count is less than retrieved Object Ids count,
* other errors otherwise.
*/
nmbs_error nmbs_read_device_identification_extended(nmbs_t* nmbs, uint8_t object_id_start, uint8_t* ids, char** buffers,
uint8_t ids_length, uint8_t buffer_length,
uint8_t* objects_count_out);
/** Send a FC 43 / 14 (0x2B / 0x0E) Read Device Identification to retrieve a single Object Id value (Read Device ID code 4)
* @param nmbs pointer to the nmbs_t instance
* @param object_id requested Object Id
* @param buffer char array where the resulting value will be stored
* @param buffer_length length of the char array
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_read_device_identification(nmbs_t* nmbs, uint8_t object_id, char* buffer, uint8_t buffer_length);
/** Send a raw Modbus PDU.
* CRC on RTU will be calculated and sent by this function.
* @param nmbs pointer to the nmbs_t instance
@ -426,12 +515,12 @@ nmbs_error nmbs_send_raw_pdu(nmbs_t* nmbs, uint8_t fc, const uint8_t* data, uint
/** Receive a raw response Modbus PDU.
* @param nmbs pointer to the nmbs_t instance
* @param data_out response data. It's up to the caller to convert this data to host byte order.
* @param data_out_len length of the data_out parameter
* @param data_out response data. It's up to the caller to convert this data to host byte order. Can be NULL.
* @param data_out_len number of bytes to receive
*
* @return NMBS_ERROR_NONE if successful, other errors otherwise.
*/
nmbs_error nmbs_receive_raw_pdu_response(nmbs_t* nmbs, uint8_t* data_out, uint16_t data_out_len);
nmbs_error nmbs_receive_raw_pdu_response(nmbs_t* nmbs, uint8_t* data_out, uint8_t data_out_len);
#endif
/** Calculate the Modbus CRC of some data.

188
tests/nanomodbus_tests.c
View File

@ -1,6 +1,8 @@
#include "nanomodbus_tests.h"
#include <netinet/in.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
static int64_t callbacks_user_data = -64;
@ -994,7 +996,7 @@ void test_fc21(nmbs_transport transport) {
should("immediately return NMBS_ERROR_INVALID_ARGUMENT when calling with record_number > 9999");
expect(nmbs_write_file_record(&CLIENT, 1, 10000, registers, 1) == NMBS_ERROR_INVALID_ARGUMENT);
should("return NMBS_ERROR_INVALID_ARGUMENT hen calling with count > 123");
should("immediately return NMBS_ERROR_INVALID_ARGUMENT when calling with count > 123");
expect(nmbs_write_file_record(&CLIENT, 1, 0, registers, 123) == NMBS_ERROR_INVALID_ARGUMENT);
should("return NMBS_EXCEPTION_SERVER_DEVICE_FAILURE when server handler returns any non-exception error");
@ -1020,8 +1022,6 @@ void test_fc21(nmbs_transport transport) {
}
void test_fc23(nmbs_transport transport) {
const uint8_t fc = 23;
uint8_t raw_res[260];
uint16_t registers[125];
uint16_t registers_write[125];
nmbs_callbacks callbacks_empty = {0};
@ -1071,6 +1071,185 @@ void test_fc23(nmbs_transport transport) {
stop_client_and_server();
}
nmbs_error read_device_identification_map(nmbs_bitfield_256 map) {
nmbs_bitfield_set(map, NMBS_OBJECT_ID_VENDOR_NAME);
nmbs_bitfield_set(map, NMBS_OBJECT_ID_PRODUCT_CODE);
nmbs_bitfield_set(map, NMBS_OBJECT_ID_MAJOR_MINOR_REVISION);
nmbs_bitfield_set(map, NMBS_OBJECT_ID_VENDOR_URL);
nmbs_bitfield_set(map, NMBS_OBJECT_ID_PRODUCT_NAME);
nmbs_bitfield_set(map, NMBS_OBJECT_ID_MODEL_NAME);
nmbs_bitfield_set(map, NMBS_OBJECT_ID_USER_APPLICATION_NAME);
nmbs_bitfield_set(map, 0x80);
nmbs_bitfield_set(map, 0x91);
nmbs_bitfield_set(map, 0xA2);
nmbs_bitfield_set(map, 0xB3);
return NMBS_ERROR_NONE;
}
nmbs_error read_device_identification_map_incomplete(nmbs_bitfield_256 map) {
nmbs_bitfield_set(map, NMBS_OBJECT_ID_VENDOR_NAME);
nmbs_bitfield_set(map, NMBS_OBJECT_ID_MAJOR_MINOR_REVISION);
return NMBS_ERROR_NONE;
}
nmbs_error read_device_identification(uint8_t object_id, char buffer[NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH]) {
switch (object_id) {
case NMBS_OBJECT_ID_VENDOR_NAME:
strcpy(buffer, "VendorName");
break;
case NMBS_OBJECT_ID_PRODUCT_CODE:
strcpy(buffer, "ProductCode");
break;
case NMBS_OBJECT_ID_MAJOR_MINOR_REVISION:
strcpy(buffer, "MajorMinorRevision");
break;
case NMBS_OBJECT_ID_VENDOR_URL:
strncpy(buffer,
"VendorUrl90byteslongextendedobjectthatcombinedwithotheronesisdefinitelygonnaexceedthepdusiz"
"e0123456",
NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH);
break;
case NMBS_OBJECT_ID_PRODUCT_NAME:
strncpy(buffer,
"ProductName90byteslongextendedobjectthatcombinedwithotheronesisdefinitelygonnaexceedthepdus"
"ize0123456",
NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH);
break;
case NMBS_OBJECT_ID_MODEL_NAME:
strncpy(buffer,
"ModelName90byteslongextendedobjectthatcombinedwithotheronesisdefinitelygonnaexceedthepdusiz"
"e0123456",
NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH);
break;
case NMBS_OBJECT_ID_USER_APPLICATION_NAME:
strcpy(buffer, "UserApplicationName");
break;
case 0x80:
case 0x91:
case 0xA2:
case 0xB3:
strncpy(buffer,
"90byteslongextendedobjectthatcombinedwithotheronesisdefinitelygonnaexceedthepdusize0123456",
NMBS_DEVICE_IDENTIFICATION_STRING_LENGTH);
break;
default:
return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS;
}
return NMBS_ERROR_NONE;
}
void test_fc43_14(nmbs_transport transport) {
const uint8_t fc = 43;
const uint8_t mei = 14;
const uint8_t buf_size = 128;
char mem[7 * buf_size];
char* buffers[7];
for (int i = 0; i < 7; i++) {
buffers[i] = &mem[i * buf_size];
}
nmbs_callbacks callbacks_empty = {0};
start_client_and_server(transport, &callbacks_empty);
should("return NMBS_EXCEPTION_ILLEGAL_FUNCTION when callback is not registered server-side");
expect(nmbs_read_device_identification(&CLIENT, 0x00, buffers[0], buf_size) == NMBS_EXCEPTION_ILLEGAL_FUNCTION);
stop_client_and_server();
start_client_and_server(transport,
&(nmbs_callbacks){.read_device_identification = read_device_identification,
.read_device_identification_map = read_device_identification_map});
nmbs_set_callbacks_arg(&SERVER, (void*) &callbacks_user_data);
should("immediately return NMBS_ERROR_INVALID_ARGUMENT when calling with an invalid Object Id");
expect(nmbs_read_device_identification(&CLIENT, 0x07, buffers[0], buf_size) == NMBS_ERROR_INVALID_ARGUMENT);
stop_client_and_server();
start_client_and_server(
transport, &(nmbs_callbacks){.read_device_identification = read_device_identification,
.read_device_identification_map = read_device_identification_map_incomplete});
nmbs_set_callbacks_arg(&SERVER, (void*) &callbacks_user_data);
should("return NMBS_ERROR_SERVER_DEVICE_FAILURE when not exposing Basic object IDs");
expect(nmbs_read_device_identification_basic(&CLIENT, buffers[0], buffers[1], buffers[2], buf_size) ==
NMBS_EXCEPTION_SERVER_DEVICE_FAILURE);
stop_client_and_server();
start_client_and_server(transport,
&(nmbs_callbacks){.read_device_identification = read_device_identification,
.read_device_identification_map = read_device_identification_map});
nmbs_set_callbacks_arg(&SERVER, (void*) &callbacks_user_data);
should("return NMBS_EXCEPTION_ILLEGAL_FUNCTION with wrong MEI type");
nmbs_send_raw_pdu(&CLIENT, fc, (uint8_t[]){69, 1, 0}, 3);
expect(nmbs_receive_raw_pdu_response(&CLIENT, NULL, 2) == NMBS_EXCEPTION_ILLEGAL_FUNCTION);
should("return NMBS_EXCEPTION_ILLEGAL_DATA_VALUE with wrong Read Device ID code");
nmbs_send_raw_pdu(&CLIENT, fc, (uint8_t[]){mei, 0, 0}, 3);
expect(nmbs_receive_raw_pdu_response(&CLIENT, NULL, 2) == NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
should("return NMBS_EXCEPTION_ILLEGAL_DATA_VALUE with wrong Read Device ID code");
nmbs_send_raw_pdu(&CLIENT, fc, (uint8_t[]){mei, 5, 0}, 3);
expect(nmbs_receive_raw_pdu_response(&CLIENT, NULL, 2) == NMBS_EXCEPTION_ILLEGAL_DATA_VALUE);
should("return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS with reserved Object ID");
nmbs_send_raw_pdu(&CLIENT, fc, (uint8_t[]){mei, 1, 0x07}, 3);
expect(nmbs_receive_raw_pdu_response(&CLIENT, NULL, 2) == NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
should("return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS with reserved Object ID");
nmbs_send_raw_pdu(&CLIENT, fc, (uint8_t[]){mei, 4, 0x07}, 3);
expect(nmbs_receive_raw_pdu_response(&CLIENT, NULL, 2) == NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
should("return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS with out of range Object ID");
nmbs_send_raw_pdu(&CLIENT, fc, (uint8_t[]){mei, 1, 0x03}, 3);
expect(nmbs_receive_raw_pdu_response(&CLIENT, NULL, 2) == NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
should("return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS with out of range Object ID");
nmbs_send_raw_pdu(&CLIENT, fc, (uint8_t[]){mei, 2, 0x01}, 3);
expect(nmbs_receive_raw_pdu_response(&CLIENT, NULL, 2) == NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
should("return NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS with out of range Object ID");
nmbs_send_raw_pdu(&CLIENT, fc, (uint8_t[]){mei, 3, 0x02}, 3);
expect(nmbs_receive_raw_pdu_response(&CLIENT, NULL, 2) == NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS);
should("read basic object ids with no error");
check(nmbs_read_device_identification_basic(&CLIENT, buffers[0], buffers[1], buffers[2], buf_size));
expect(strcmp(buffers[0], "VendorName") == 0);
expect(strcmp(buffers[1], "ProductCode") == 0);
expect(strcmp(buffers[2], "MajorMinorRevision") == 0);
should("read regular object ids with no error");
check(nmbs_read_device_identification_regular(&CLIENT, buffers[0], buffers[1], buffers[2], buffers[3], buf_size));
expect(strcmp(buffers[0], "VendorUrl90byteslongextendedobjectthatcombinedwithotheronesisdefinitelygonnaexceedthepdu"
"size0123456") == 0);
expect(strcmp(buffers[1], "ProductName90byteslongextendedobjectthatcombinedwithotheronesisdefinitelygonnaexceedthep"
"dusize0123456") == 0);
expect(strcmp(buffers[2], "ModelName90byteslongextendedobjectthatcombinedwithotheronesisdefinitelygonnaexceedthepdu"
"size0123456") == 0);
expect(strcmp(buffers[3], "UserApplicationName") == 0);
should("immediately return NMBS_ERROR_INVALID_ARGUMENT when buffers_count is smaller that retrieved object ids");
uint8_t object_id = 0x80;
uint8_t objects_count = 0;
uint8_t ids[7];
expect(nmbs_read_device_identification_extended(&CLIENT, object_id, ids, buffers, 1, buf_size, &objects_count) ==
NMBS_ERROR_INVALID_ARGUMENT);
should("read extended object ids with no error");
check(nmbs_read_device_identification_extended(&CLIENT, object_id, ids, buffers, 7, buf_size, &objects_count));
expect(strcmp(buffers[0],
"90byteslongextendedobjectthatcombinedwithotheronesisdefinitelygonnaexceedthepdusize0123456") == 0);
expect(strcmp(buffers[1],
"90byteslongextendedobjectthatcombinedwithotheronesisdefinitelygonnaexceedthepdusize0123456") == 0);
expect(strcmp(buffers[2],
"90byteslongextendedobjectthatcombinedwithotheronesisdefinitelygonnaexceedthepdusize0123456") == 0);
expect(strcmp(buffers[3],
"90byteslongextendedobjectthatcombinedwithotheronesisdefinitelygonnaexceedthepdusize0123456") == 0);
stop_client_and_server();
}
nmbs_transport transports[2] = {NMBS_TRANSPORT_RTU, NMBS_TRANSPORT_TCP};
const char* transports_str[2] = {"RTU", "TCP"};
@ -1085,6 +1264,8 @@ int main(int argc, char* argv[]) {
UNUSED_PARAM(argc);
UNUSED_PARAM(argv);
for_transports(test_fc43_14, "send and receive FC 43 / 14 (0x2B / 0x0E) Read Device Identification");
for_transports(test_server_create, "create a modbus server");
for_transports(test_server_receive_base, "receive no messages without failing");
@ -1110,5 +1291,6 @@ int main(int argc, char* argv[]) {
for_transports(test_fc21, "send and receive FC 21 (0x15) Write File Record");
for_transports(test_fc23, "send and receive FC 23 (0x17) Read/Write Multiple Registers");
return 0;
}