diff --git a/nanomodbus.c b/nanomodbus.c index 1eaebcb..f5916ff 100644 --- a/nanomodbus.c +++ b/nanomodbus.c @@ -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"; diff --git a/nanomodbus.h b/nanomodbus.h index f63b39d..4db484f 100644 --- a/nanomodbus.h +++ b/nanomodbus.h @@ -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. diff --git a/tests/nanomodbus_tests.c b/tests/nanomodbus_tests.c index 303e740..461c979 100644 --- a/tests/nanomodbus_tests.c +++ b/tests/nanomodbus_tests.c @@ -1,6 +1,8 @@ #include "nanomodbus_tests.h" + #include #include +#include #include 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; }