From 2dd35d3c74df75129fd88362e1292e9ff02b1291 Mon Sep 17 00:00:00 2001 From: mypx Date: Mon, 27 Oct 2025 15:41:28 +0800 Subject: [PATCH] rm cached --- etk/src/algorithm/trend | 1 - etk/src/algorithm/trend/src/et_trend.c | 236 +++ etk/src/algorithm/trend/src/et_trend.h | 74 + nanoMODBUS | 1 - nanoMODBUS/nanomodbus.c | 2461 ++++++++++++++++++++++++ nanoMODBUS/nanomodbus.h | 556 ++++++ 6 files changed, 3327 insertions(+), 2 deletions(-) delete mode 160000 etk/src/algorithm/trend create mode 100644 etk/src/algorithm/trend/src/et_trend.c create mode 100644 etk/src/algorithm/trend/src/et_trend.h delete mode 160000 nanoMODBUS create mode 100644 nanoMODBUS/nanomodbus.c create mode 100644 nanoMODBUS/nanomodbus.h diff --git a/etk/src/algorithm/trend b/etk/src/algorithm/trend deleted file mode 160000 index d50ec7a..0000000 --- a/etk/src/algorithm/trend +++ /dev/null @@ -1 +0,0 @@ -Subproject commit d50ec7a85ffa5c42ec23b9a69b7df3b1a75fcb7f diff --git a/etk/src/algorithm/trend/src/et_trend.c b/etk/src/algorithm/trend/src/et_trend.c new file mode 100644 index 0000000..05c2b28 --- /dev/null +++ b/etk/src/algorithm/trend/src/et_trend.c @@ -0,0 +1,236 @@ +#include "et_trend.h" +#include "et_log.h" +#include +#include + +/** + * @brief Find the index of minimum value in array + * + * @param data Input data array + * @param len Length of the array + * @return uint8_t Index of the minimum value + */ +static uint8_t find_min_index(const float *data, uint8_t len) +{ + uint8_t min_idx = 0; + for (uint8_t i = 1; i < len; i++) + { + if (data[i] < data[min_idx]) + { + min_idx = i; + } + } + return min_idx; +} + +/** + * @brief Find the index of maximum value in array + * + * @param data Input data array + * @param len Length of the array + * @return uint8_t Index of the maximum value + */ +static uint8_t find_max_index(const float *data, uint8_t len) +{ + uint8_t max_idx = 0; + for (uint8_t i = 1; i < len; i++) + { + if (data[i] > data[max_idx]) + { + max_idx = i; + } + } + return max_idx; +} + +ET_TrendResult et_trend_detect(const et_trend_t *trend, float *out_avg_diff) +{ + // Input parameter validation + if (!trend || !trend->data || trend->length < trend->config.window_size) + { + ET_ERR("Invalid input: trend=%p, data=%p, window_size=%d, length=%d", trend, trend ? trend->data : NULL, + trend ? trend->config.window_size : 0, trend ? trend->length : 0); + return TREND_INVALID; + } + + // Position window data (original time series, maintain order) + const float *input = trend->data + (trend->length - trend->config.window_size); + uint8_t window_size = trend->config.window_size; + + // Record index of outlier to exclude + uint8_t exclude_idx = 0xFF; // Initial invalid index + uint8_t valid_count = window_size; + + switch (trend->config.outlier_mode) + { + case OUTLIER_MIN: + { + exclude_idx = find_min_index(input, window_size); + valid_count--; + break; + } + case OUTLIER_MAX: + { + exclude_idx = find_max_index(input, window_size); + valid_count--; + break; + } + case OUTLIER_BOTH: + { + // First exclude minimum, then exclude maximum from remaining data + uint8_t min_idx = find_min_index(input, window_size); + // Construct temporary array excluding minimum + float temp[ET_TREND_WINDOW_MAX]; + uint8_t temp_len = 0; + for (uint8_t i = 0; i < window_size; i++) + { + if (i != min_idx) + { + temp[temp_len++] = input[i]; + } + } + // Find index of maximum in temporary array and map back to original index + uint8_t max_temp_idx = find_max_index(temp, temp_len); + // Map back to original index (skip min_idx) + exclude_idx = 0; + for (uint8_t i = 0; i < window_size; i++) + { + if (i == min_idx) + continue; + if (exclude_idx == max_temp_idx) + { + exclude_idx = i; + break; + } + exclude_idx++; + } + valid_count -= 2; + break; + } + default: // OUTLIER_NONE + break; + } + + // Insufficient valid data after filtering + if (valid_count < 2) + { + ET_DBG("Insufficient valid data after outlier exclusion: %d", valid_count); + return TREND_NO_VALID_DATA; + } + + // Collect valid data after filtering (maintain original order) + float valid_data[ET_TREND_WINDOW_MAX]; + uint8_t valid_idx = 0; + for (uint8_t i = 0; i < window_size; i++) + { + if (i != exclude_idx) + { // Skip outlier + valid_data[valid_idx++] = input[i]; + ET_DBG("valid_data[%d]=%.3f", valid_idx - 1, valid_data[valid_idx - 1]); + } + } + + // Calculate average values of first and second halves (based on original time order) + float avg_start = 0.0f, avg_end = 0.0f; + float diff = 0.0f; + uint8_t half = valid_count / 2; + + // First half (early data) + for (uint8_t i = 0; i < half; i++) + { + avg_start += valid_data[i]; + } + avg_start /= half; + + // Second half (later data) + uint8_t second_half_count = valid_count - half; + for (uint8_t i = half; i < valid_count; i++) + { + avg_end += valid_data[i]; + } + avg_end /= second_half_count; + + diff = avg_end - avg_start; + + // Fallback logic for OUTLIER_MIN mode (only when filtered data cannot show trend) + if (trend->config.outlier_mode == OUTLIER_MIN && diff > -trend->config.threshold) + { + ET_DBG("OUTLIER_MIN adjustment: diff=%.3f too small, using original input.", diff); + // Re-calculate using original data + uint8_t original_half = window_size / 2; + avg_start = 0.0f; + for (uint8_t i = 0; i < original_half; i++) + { + avg_start += input[i]; + } + avg_start /= original_half; + + avg_end = 0.0f; + uint8_t original_second_half = window_size - original_half; + for (uint8_t i = original_half; i < window_size; i++) + { + avg_end += input[i]; + } + avg_end /= original_second_half; + + diff = avg_end - avg_start; + } + + // Output difference value + if (out_avg_diff) + { + *out_avg_diff = diff; + } + + ET_DBG("avg_start=%.3f, avg_end=%.3f, diff=%.3f, threshold=%.3f", avg_start, avg_end, diff, + trend->config.threshold); + + // 增加数据窗口信息输出 + ET_DBG("[TREND] Window data (%d samples):", window_size); + for (uint8_t i = 0; i < window_size; i++) + { + ET_DBG("[%d]: %.3f", i, input[i]); + } + + // 输出过滤后的有效数据 + ET_DBG("[TREND] Valid data after filtering (%d samples):", valid_count); + for (uint8_t i = 0; i < valid_count; i++) + { + ET_DBG("[%d]: %.3f", i, valid_data[i]); + } + + ET_INFO("[TREND] Result: %d (avg_start=%.3f, avg_end=%.3f, diff=%.3f)", + (diff > trend->config.threshold) ? TREND_INCREASING + : (diff < -trend->config.threshold) ? TREND_DECREASING + : TREND_STABLE, + avg_start, avg_end, diff); + + // Determine trend + if (diff > trend->config.threshold) + { + return TREND_INCREASING; + } + else if (diff < -trend->config.threshold) + { + return TREND_DECREASING; + } + else + { + return TREND_STABLE; + } +} + +int et_trend_init(et_trend_t *trend, et_trend_config_t *cfg) +{ + if (!trend || !cfg || cfg->window_size < ET_TREND_WINDOW_MIN || cfg->window_size > ET_TREND_WINDOW_MAX) + { + ET_ERR("Invalid input: cfg=%p, window_size=%d", cfg, cfg ? cfg->window_size : 0); + return -1; + } + // Initialize configuration + trend->config.window_size = cfg->window_size; + trend->config.threshold = cfg->threshold; + trend->config.outlier_mode = cfg->outlier_mode; + + return 0; +} diff --git a/etk/src/algorithm/trend/src/et_trend.h b/etk/src/algorithm/trend/src/et_trend.h new file mode 100644 index 0000000..e45e405 --- /dev/null +++ b/etk/src/algorithm/trend/src/et_trend.h @@ -0,0 +1,74 @@ +/* + * @Author: mypx + * @Date: 2025-08-07 13:14:49 + * @LastEditTime: 2025-08-14 11:15:00 + * @LastEditors: mypx mypx_coder@163.com + * @Description: + */ +#ifndef __ET_TREND_H__ +#define __ET_TREND_H__ +#include "etk_utils.h" + +#define ET_TREND_WINDOW_MIN 3 +#define ET_TREND_WINDOW_MAX 64 + +typedef enum +{ + TREND_INVALID = -1, // Invalid input parameters + TREND_STABLE = 0, // Stable trend + TREND_INCREASING, // Increasing trend + TREND_DECREASING, // Decreasing trend + TREND_NO_VALID_DATA // Insufficient valid data (all outliers) +} ET_TrendResult; + +typedef enum +{ + OUTLIER_NONE = 0, // No outlier filtering + OUTLIER_MAX, // Filter only maximum values + OUTLIER_MIN, // Filter only minimum values + OUTLIER_BOTH // Filter both maximum and minimum values +} ET_OutlierMode; + +typedef struct +{ + uint8_t window_size; // Window size for trend determination + float threshold; // Trend determination threshold + ET_OutlierMode outlier_mode; // Outlier processing mode +}et_trend_config_t; + +typedef struct +{ + const float *data; // Input data array + uint16_t length; // Total data length + et_trend_config_t config; // Trend configuration parameters +} et_trend_t; + +#ifdef __cplusplus +extern "C" +{ +#endif + +/** + * @brief Detect trend in time series data + * + * This function analyzes the trend of input data by comparing the average values + * of the first and second halves of the data within a specified window. + * It can optionally filter out outliers before performing the trend analysis. + * + * @param cfg Pointer to the trend detection configuration structure + * @param out_avg_diff Pointer to store the average difference (can be NULL) + * @return ET_TrendResult Trend detection result + * - TREND_INVALID: Invalid input parameters + * - TREND_STABLE: Stable trend (difference within threshold) + * - TREND_INCREASING: Increasing trend (difference > threshold) + * - TREND_DECREASING: Decreasing trend (difference < -threshold) + * - TREND_NO_VALID_DATA: Insufficient valid data after outlier filtering + */ +ET_TrendResult et_trend_detect(const et_trend_t *cfg, float *out_avg_diff); + +int et_trend_init(et_trend_t *trend, et_trend_config_t *cfg); + +#ifdef __cplusplus +} +#endif +#endif \ No newline at end of file diff --git a/nanoMODBUS b/nanoMODBUS deleted file mode 160000 index 7df6f11..0000000 --- a/nanoMODBUS +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 7df6f11fb0e07dc5d5101f43f1a0c372147f137c diff --git a/nanoMODBUS/nanomodbus.c b/nanoMODBUS/nanomodbus.c new file mode 100644 index 0000000..d00f212 --- /dev/null +++ b/nanoMODBUS/nanomodbus.c @@ -0,0 +1,2461 @@ +/* + nanoMODBUS - A compact MODBUS RTU/TCP C library for microcontrollers + + MIT License + + Copyright (c) 2024 Valerio De Benedetto (@debevv) + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ + +#include "nanomodbus.h" + +#include +#include +#include + +#define NMBS_UNUSED_PARAM(x) ((x) = (x)) + +#ifdef NMBS_DEBUG +#include +#define NMBS_DEBUG_PRINT(...) printf(__VA_ARGS__) +#else +#define NMBS_DEBUG_PRINT(...) (void) (0) +#endif + + +static uint8_t get_1(nmbs_t* nmbs) { + uint8_t result = nmbs->msg.buf[nmbs->msg.buf_idx]; + nmbs->msg.buf_idx++; + return result; +} + + +static void put_1(nmbs_t* nmbs, uint8_t data) { + nmbs->msg.buf[nmbs->msg.buf_idx] = data; + nmbs->msg.buf_idx++; +} + + +static void discard_1(nmbs_t* nmbs) { + nmbs->msg.buf_idx++; +} + + +#ifndef NMBS_SERVER_DISABLED +#if !defined(NMBS_SERVER_READ_FILE_RECORD_DISABLED) || !defined(NMBS_SERVER_WRITE_FILE_RECORD_DISABLED) +static void discard_n(nmbs_t* nmbs, uint16_t n) { + nmbs->msg.buf_idx += n; +} +#endif +#endif + + +static uint16_t get_2(nmbs_t* nmbs) { + const uint16_t result = + ((uint16_t) nmbs->msg.buf[nmbs->msg.buf_idx]) << 8 | (uint16_t) nmbs->msg.buf[nmbs->msg.buf_idx + 1]; + nmbs->msg.buf_idx += 2; + return result; +} + + +static void put_2(nmbs_t* nmbs, uint16_t data) { + nmbs->msg.buf[nmbs->msg.buf_idx] = (uint8_t) ((data >> 8) & 0xFFU); + nmbs->msg.buf[nmbs->msg.buf_idx + 1] = (uint8_t) data; + nmbs->msg.buf_idx += 2; +} + + +#ifndef NMBS_SERVER_DISABLED +#if !defined(NMBS_SERVER_READ_DEVICE_IDENTIFICATION_DISABLED) +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; +} +#endif +#endif + + +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; + return msg_buf_ptr; +} + + +#ifndef NMBS_SERVER_DISABLED +#if !defined(NMBS_SERVER_READ_DEVICE_IDENTIFICATION_DISABLED) +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; +} +#endif + + +#if !defined(NMBS_SERVER_WRITE_FILE_RECORD_DISABLED) +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; + while (n--) { + msg_buf_ptr[n] = (msg_buf_ptr[n] << 8) | ((msg_buf_ptr[n] >> 8) & 0xFF); + } + return msg_buf_ptr; +} +#endif +#endif + + +#ifndef NMBS_CLIENT_DISABLED +static void put_regs(nmbs_t* nmbs, const uint16_t* data, uint16_t n) { + uint16_t* msg_buf_ptr = (uint16_t*) (nmbs->msg.buf + nmbs->msg.buf_idx); + nmbs->msg.buf_idx += n * 2; + while (n--) { + msg_buf_ptr[n] = (data[n] << 8) | ((data[n] >> 8) & 0xFF); + } +} +#endif + + +static void swap_regs(uint16_t* data, uint16_t n) { + while (n--) { + data[n] = (data[n] << 8) | ((data[n] >> 8) & 0xFF); + } +} + + +static nmbs_error recv(nmbs_t* nmbs, uint16_t count) { + if (nmbs->msg.complete) { + return NMBS_ERROR_NONE; + } + + int32_t ret = + nmbs->platform.read(nmbs->msg.buf + nmbs->msg.buf_idx, count, nmbs->byte_timeout_ms, nmbs->platform.arg); + + if (ret == count) + return NMBS_ERROR_NONE; + + if (ret < count) { + if (ret < 0) + return NMBS_ERROR_TRANSPORT; + + return NMBS_ERROR_TIMEOUT; + } + + return NMBS_ERROR_TRANSPORT; +} + + +static nmbs_error send(const nmbs_t* nmbs, uint16_t count) { + int32_t ret = nmbs->platform.write(nmbs->msg.buf, count, nmbs->byte_timeout_ms, nmbs->platform.arg); + + if (ret == count) + return NMBS_ERROR_NONE; + + if (ret < count) { + if (ret < 0) + return NMBS_ERROR_TRANSPORT; + + return NMBS_ERROR_TIMEOUT; + } + + return NMBS_ERROR_TRANSPORT; +} + + +static void flush(nmbs_t* nmbs) { + nmbs->platform.read(nmbs->msg.buf, sizeof(nmbs->msg.buf), 0, nmbs->platform.arg); +} + + +static void msg_buf_reset(nmbs_t* nmbs) { + nmbs->msg.buf_idx = 0; +} + + +static void msg_state_reset(nmbs_t* nmbs) { + msg_buf_reset(nmbs); + nmbs->msg.unit_id = 0; + nmbs->msg.fc = 0; + nmbs->msg.transaction_id = 0; + nmbs->msg.broadcast = false; + nmbs->msg.ignored = false; + nmbs->msg.complete = false; +} + + +#ifndef NMBS_CLIENT_DISABLED +static void msg_state_req(nmbs_t* nmbs, uint8_t fc) { + if (nmbs->current_tid == UINT16_MAX) + nmbs->current_tid = 1; + else + nmbs->current_tid++; + + // Flush the remaining data on the line before sending the request + flush(nmbs); + + msg_state_reset(nmbs); + nmbs->msg.unit_id = nmbs->dest_address_rtu; + nmbs->msg.fc = fc; + nmbs->msg.transaction_id = nmbs->current_tid; + if (nmbs->msg.unit_id == 0 && nmbs->platform.transport == NMBS_TRANSPORT_RTU) + nmbs->msg.broadcast = true; +} +#endif + + +nmbs_error nmbs_create(nmbs_t* nmbs, const nmbs_platform_conf* platform_conf) { + if (!nmbs) + return NMBS_ERROR_INVALID_ARGUMENT; + + memset(nmbs, 0, sizeof(nmbs_t)); + + nmbs->byte_timeout_ms = -1; + nmbs->read_timeout_ms = -1; + + if (!platform_conf || platform_conf->initialized != 0xFFFFDEBE) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (platform_conf->transport != NMBS_TRANSPORT_RTU && platform_conf->transport != NMBS_TRANSPORT_TCP) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (!platform_conf->read || !platform_conf->write) + return NMBS_ERROR_INVALID_ARGUMENT; + + nmbs->platform = *platform_conf; + + return NMBS_ERROR_NONE; +} + + +void nmbs_set_read_timeout(nmbs_t* nmbs, int32_t timeout_ms) { + nmbs->read_timeout_ms = timeout_ms; +} + + +void nmbs_set_byte_timeout(nmbs_t* nmbs, int32_t timeout_ms) { + nmbs->byte_timeout_ms = timeout_ms; +} + + +void nmbs_platform_conf_create(nmbs_platform_conf* platform_conf) { + memset(platform_conf, 0, sizeof(nmbs_platform_conf)); + platform_conf->crc_calc = nmbs_crc_calc; + // Workaround for older user code not calling nmbs_platform_conf_create() + platform_conf->initialized = 0xFFFFDEBE; +} + + +void nmbs_set_destination_rtu_address(nmbs_t* nmbs, uint8_t address) { + nmbs->dest_address_rtu = address; +} + + +void nmbs_set_platform_arg(nmbs_t* nmbs, void* arg) { + nmbs->platform.arg = arg; +} + + +uint16_t nmbs_crc_calc(const uint8_t* data, uint32_t length, void* arg) { + NMBS_UNUSED_PARAM(arg); + uint16_t crc = 0xFFFF; + for (uint32_t i = 0; i < length; i++) { + crc ^= (uint16_t) data[i]; + for (int j = 8; j != 0; j--) { + if ((crc & 0x0001) != 0) { + crc >>= 1; + crc ^= 0xA001; + } + else + crc >>= 1; + } + } + + return (uint16_t) (crc << 8) | (uint16_t) (crc >> 8); +} + +static nmbs_error recv_msg_footer(nmbs_t* nmbs) { + NMBS_DEBUG_PRINT("\n"); + + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + uint16_t crc = nmbs->platform.crc_calc(nmbs->msg.buf, nmbs->msg.buf_idx, nmbs->platform.arg); + + nmbs_error err = recv(nmbs, 2); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t recv_crc = get_2(nmbs); + + if (recv_crc != crc) + return NMBS_ERROR_CRC; + } + + return NMBS_ERROR_NONE; +} + + +static nmbs_error recv_msg_header(nmbs_t* nmbs, bool* first_byte_received) { + // We wait for the read timeout here, just for the first message byte + int32_t old_byte_timeout = nmbs->byte_timeout_ms; + nmbs->byte_timeout_ms = nmbs->read_timeout_ms; + + msg_state_reset(nmbs); + + *first_byte_received = false; + + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + nmbs_error err = recv(nmbs, 1); + + nmbs->byte_timeout_ms = old_byte_timeout; + + if (err != NMBS_ERROR_NONE) + return err; + + *first_byte_received = true; + + nmbs->msg.unit_id = get_1(nmbs); + + err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + nmbs->msg.fc = get_1(nmbs); + } + else if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) { + nmbs_error err = recv(nmbs, 1); + + nmbs->byte_timeout_ms = old_byte_timeout; + + if (err != NMBS_ERROR_NONE) + return err; + + *first_byte_received = true; + + // Advance buf_idx + discard_1(nmbs); + + err = recv(nmbs, 7); + if (err != NMBS_ERROR_NONE) + return err; + + // Starting over + msg_buf_reset(nmbs); + + nmbs->msg.transaction_id = get_2(nmbs); + uint16_t protocol_id = get_2(nmbs); + uint16_t length = get_2(nmbs); + nmbs->msg.unit_id = get_1(nmbs); + nmbs->msg.fc = get_1(nmbs); + + if (length < 2 || length > 255) + return NMBS_ERROR_INVALID_TCP_MBAP; + + // Receive the rest of the message + err = recv(nmbs, length - 2); + if (err != NMBS_ERROR_NONE) + return err; + + if (protocol_id != 0) + return NMBS_ERROR_INVALID_TCP_MBAP; + + nmbs->msg.complete = true; + } + + return NMBS_ERROR_NONE; +} + + +static void put_msg_header(nmbs_t* nmbs, uint16_t data_length) { + msg_buf_reset(nmbs); + + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + put_1(nmbs, nmbs->msg.unit_id); + } + else if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) { + put_2(nmbs, nmbs->msg.transaction_id); + put_2(nmbs, 0); + put_2(nmbs, (uint16_t) (1 + 1 + data_length)); + put_1(nmbs, nmbs->msg.unit_id); + } + + put_1(nmbs, nmbs->msg.fc); +} + + +#ifndef NMBS_SERVER_DISABLED +#if !defined(NMBS_SERVER_READ_DEVICE_IDENTIFICATION_DISABLED) +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); + } +} +#endif +#endif + + +static nmbs_error send_msg(nmbs_t* nmbs) { + NMBS_DEBUG_PRINT("\n"); + + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + uint16_t crc = nmbs->platform.crc_calc(nmbs->msg.buf, nmbs->msg.buf_idx, nmbs->platform.arg); + put_2(nmbs, crc); + } + + nmbs_error err = send(nmbs, nmbs->msg.buf_idx); + + return err; +} + + +#ifndef NMBS_SERVER_DISABLED +static nmbs_error recv_req_header(nmbs_t* nmbs, bool* first_byte_received) { + const nmbs_error err = recv_msg_header(nmbs, first_byte_received); + if (err != NMBS_ERROR_NONE) + return err; + + if (nmbs->platform.transport == NMBS_TRANSPORT_RTU) { + // Check if request is for us + if (nmbs->msg.unit_id == NMBS_BROADCAST_ADDRESS) + nmbs->msg.broadcast = true; + else if (nmbs->msg.unit_id != nmbs->address_rtu) + nmbs->msg.ignored = true; + else + nmbs->msg.ignored = false; + } + + return NMBS_ERROR_NONE; +} + + +static void put_res_header(nmbs_t* nmbs, uint16_t data_length) { + put_msg_header(nmbs, data_length); + NMBS_DEBUG_PRINT("%d NMBS res -> address_rtu %d\tfc %d\t", nmbs->address_rtu, nmbs->address_rtu, nmbs->msg.fc); +} + + +static nmbs_error send_exception_msg(nmbs_t* nmbs, uint8_t exception) { + nmbs->msg.fc += 0x80; + put_msg_header(nmbs, 1); + put_1(nmbs, exception); + + NMBS_DEBUG_PRINT("%d NMBS res -> address_rtu %d\texception %d", nmbs->address_rtu, nmbs->address_rtu, exception); + + return send_msg(nmbs); +} +#endif + + +static nmbs_error recv_res_header(nmbs_t* nmbs) { + uint16_t req_transaction_id = nmbs->msg.transaction_id; + uint8_t req_unit_id = nmbs->msg.unit_id; + uint8_t req_fc = nmbs->msg.fc; + + bool first_byte_received = false; + nmbs_error err = recv_msg_header(nmbs, &first_byte_received); + if (err != NMBS_ERROR_NONE) + return err; + + if (nmbs->platform.transport == NMBS_TRANSPORT_TCP) { + if (nmbs->msg.transaction_id != req_transaction_id) + return NMBS_ERROR_INVALID_TCP_MBAP; + } + + 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) { + if (nmbs->msg.fc - 0x80 == req_fc) { + err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t exception = get_1(nmbs); + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (exception < 1 || exception > 4) + return NMBS_ERROR_INVALID_RESPONSE; + + NMBS_DEBUG_PRINT("%d NMBS res <- address_rtu %d\texception %d\n", nmbs->address_rtu, nmbs->msg.unit_id, + exception); + return (nmbs_error) exception; + } + + return NMBS_ERROR_INVALID_RESPONSE; + } + + 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; +} + + +#ifndef NMBS_CLIENT_DISABLED +static void put_req_header(nmbs_t* nmbs, uint16_t data_length) { + put_msg_header(nmbs, data_length); +#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 + + +#if !defined(NMBS_CLIENT_DISABLED) || \ + (!defined(NMBS_SERVER_DISABLED) && \ + (!defined(NMBS_SERVER_READ_COILS_DISABLED) || !defined(NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED))) +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); + + if (coils_bytes > NMBS_BITFIELD_BYTES_MAX) { + return NMBS_ERROR_INVALID_RESPONSE; + } + + 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; +} +#endif + + +#if !defined(NMBS_CLIENT_DISABLED) || \ + (!defined(NMBS_SERVER_DISABLED) && (!defined(NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED) || \ + !defined(NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED))) +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); + + if (registers_bytes > 250) + return NMBS_ERROR_INVALID_RESPONSE; + + 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; +} +#endif + + +nmbs_error recv_write_single_coil_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_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; +} + + +nmbs_error recv_read_file_record_res(nmbs_t* nmbs, uint16_t* registers, uint16_t count) { + 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 response_size = get_1(nmbs); + if (response_size > 250) { + return NMBS_ERROR_INVALID_RESPONSE; + } + + err = recv(nmbs, response_size); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t subreq_data_size = get_1(nmbs) - 1; + uint8_t subreq_reference_type = get_1(nmbs); + uint16_t* subreq_record_data = (uint16_t*) get_n(nmbs, subreq_data_size); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (registers) { + if (subreq_reference_type != 6) + return NMBS_ERROR_INVALID_RESPONSE; + + if (count != (subreq_data_size / 2)) + return NMBS_ERROR_INVALID_RESPONSE; + + swap_regs(subreq_record_data, subreq_data_size / 2); + memcpy(registers, subreq_record_data, subreq_data_size); + } + + return NMBS_ERROR_NONE; +} + + +nmbs_error recv_write_file_record_res(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, + const uint16_t* registers, uint16_t count) { + 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 response_size = get_1(nmbs); + if (response_size > 251) + return NMBS_ERROR_INVALID_RESPONSE; + + err = recv(nmbs, response_size); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t subreq_reference_type = get_1(nmbs); + uint16_t subreq_file_number = get_2(nmbs); + uint16_t subreq_record_number = get_2(nmbs); + uint16_t subreq_record_length = get_2(nmbs); + NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fwrite ", subreq_file_number, subreq_record_number, subreq_record_length); + + uint16_t subreq_data_size = subreq_record_length * 2; + uint16_t* subreq_record_data = (uint16_t*) get_n(nmbs, subreq_data_size); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (registers) { + if (subreq_reference_type != 6) + return NMBS_ERROR_INVALID_RESPONSE; + + if (subreq_file_number != file_number) + return NMBS_ERROR_INVALID_RESPONSE; + + if (subreq_record_number != record_number) + return NMBS_ERROR_INVALID_RESPONSE; + + if (subreq_record_length != count) + return NMBS_ERROR_INVALID_RESPONSE; + + swap_regs(subreq_record_data, subreq_record_length); + if (memcmp(registers, subreq_record_data, subreq_data_size) != 0) + return NMBS_ERROR_INVALID_RESPONSE; + } + + 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) +static nmbs_error handle_read_discrete(nmbs_t* nmbs, + nmbs_error (*callback)(uint16_t, uint16_t, nmbs_bitfield, uint8_t, void*)) { + nmbs_error err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t quantity = get_2(nmbs); + + NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (quantity < 1 || quantity > 2000) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (callback) { + nmbs_bitfield bitfield = {0}; + err = callback(address, quantity, bitfield, nmbs->msg.unit_id, nmbs->callbacks.arg); + 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); + } + + if (!nmbs->msg.broadcast) { + uint8_t discrete_bytes = (quantity + 7) / 8; + put_res_header(nmbs, 1 + discrete_bytes); + + put_1(nmbs, discrete_bytes); + + NMBS_DEBUG_PRINT("b %d\t", discrete_bytes); + + NMBS_DEBUG_PRINT("coils "); + for (int i = 0; i < discrete_bytes; i++) { + put_1(nmbs, bitfield[i]); + NMBS_DEBUG_PRINT("%d ", bitfield[i]); + } + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_read_discrete_res(nmbs, NULL); + } + + return NMBS_ERROR_NONE; +} +#endif + + +#if !defined(NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED) || !defined(NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED) +static nmbs_error handle_read_registers(nmbs_t* nmbs, + nmbs_error (*callback)(uint16_t, uint16_t, uint16_t*, uint8_t, void*)) { + nmbs_error err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t quantity = get_2(nmbs); + + NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (quantity < 1 || quantity > 125) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (callback) { + uint16_t regs[125] = {0}; + err = callback(address, quantity, regs, nmbs->msg.unit_id, nmbs->callbacks.arg); + 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); + } + + // TODO check all these read request broadcast use cases + if (!nmbs->msg.broadcast) { + const uint8_t regs_bytes = quantity * 2; + put_res_header(nmbs, 1 + regs_bytes); + + put_1(nmbs, regs_bytes); + + NMBS_DEBUG_PRINT("b %d\t", regs_bytes); + + NMBS_DEBUG_PRINT("regs "); + for (int i = 0; i < quantity; i++) { + put_2(nmbs, regs[i]); + NMBS_DEBUG_PRINT("%d ", regs[i]); + } + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_read_registers_res(nmbs, quantity, NULL); + } + + return NMBS_ERROR_NONE; +} +#endif + + +#ifndef NMBS_SERVER_READ_COILS_DISABLED +static nmbs_error handle_read_coils(nmbs_t* nmbs) { + return handle_read_discrete(nmbs, nmbs->callbacks.read_coils); +} +#endif + + +#ifndef NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED +static nmbs_error handle_read_discrete_inputs(nmbs_t* nmbs) { + return handle_read_discrete(nmbs, nmbs->callbacks.read_discrete_inputs); +} +#endif + + +#ifndef NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED +static nmbs_error handle_read_holding_registers(nmbs_t* nmbs) { + return handle_read_registers(nmbs, nmbs->callbacks.read_holding_registers); +} +#endif + + +#ifndef NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED +static nmbs_error handle_read_input_registers(nmbs_t* nmbs) { + return handle_read_registers(nmbs, nmbs->callbacks.read_input_registers); +} +#endif + + +#ifndef NMBS_SERVER_WRITE_SINGLE_COIL_DISABLED +static nmbs_error handle_write_single_coil(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t value = get_2(nmbs); + + NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (nmbs->callbacks.write_single_coil) { + if (value != 0 && value != 0xFF00) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + err = nmbs->callbacks.write_single_coil(address, value == 0 ? false : true, nmbs->msg.unit_id, + nmbs->callbacks.arg); + 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); + } + + if (!nmbs->msg.broadcast) { + put_res_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, value); + NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value); + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_write_single_coil_res(nmbs, address, value); + } + + return NMBS_ERROR_NONE; +} +#endif + + +#ifndef NMBS_SERVER_WRITE_SINGLE_REGISTER_DISABLED +static nmbs_error handle_write_single_register(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 4); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t value = get_2(nmbs); + + NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (nmbs->callbacks.write_single_register) { + err = nmbs->callbacks.write_single_register(address, value, nmbs->msg.unit_id, nmbs->callbacks.arg); + 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); + } + + if (!nmbs->msg.broadcast) { + put_res_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, value); + NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value); + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_write_single_register_res(nmbs, address, value); + } + + return NMBS_ERROR_NONE; +} +#endif + + +#ifndef NMBS_SERVER_WRITE_MULTIPLE_COILS_DISABLED +static nmbs_error handle_write_multiple_coils(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 5); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t quantity = get_2(nmbs); + uint8_t coils_bytes = get_1(nmbs); + + NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\tcoils ", address, quantity, coils_bytes); + + if (coils_bytes > 246) + return NMBS_ERROR_INVALID_REQUEST; + + err = recv(nmbs, coils_bytes); + if (err != NMBS_ERROR_NONE) + return err; + + nmbs_bitfield coils = {0}; + for (int i = 0; i < coils_bytes; i++) { + coils[i] = get_1(nmbs); + NMBS_DEBUG_PRINT("%d ", coils[i]); + } + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (quantity < 1 || quantity > 0x07B0) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (coils_bytes == 0) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((quantity + 7) / 8 != coils_bytes) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (nmbs->callbacks.write_multiple_coils) { + err = nmbs->callbacks.write_multiple_coils(address, quantity, coils, nmbs->msg.unit_id, + nmbs->callbacks.arg); + 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); + } + + if (!nmbs->msg.broadcast) { + put_res_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, quantity); + NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity); + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_write_multiple_coils_res(nmbs, address, quantity); + } + + return NMBS_ERROR_NONE; +} +#endif + + +#ifndef NMBS_SERVER_WRITE_MULTIPLE_REGISTERS_DISABLED +static nmbs_error handle_write_multiple_registers(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 5); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t address = get_2(nmbs); + uint16_t quantity = get_2(nmbs); + uint8_t registers_bytes = get_1(nmbs); + + NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\tregs ", address, quantity, registers_bytes); + + err = recv(nmbs, registers_bytes); + if (err != NMBS_ERROR_NONE) + return err; + + if (registers_bytes > 246) + return NMBS_ERROR_INVALID_REQUEST; + + uint16_t registers[0x007B]; + 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 (!nmbs->msg.ignored) { + if (quantity < 1 || quantity > 0x007B) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (registers_bytes == 0) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (registers_bytes != quantity * 2) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (nmbs->callbacks.write_multiple_registers) { + err = nmbs->callbacks.write_multiple_registers(address, quantity, registers, nmbs->msg.unit_id, + nmbs->callbacks.arg); + 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); + } + + if (!nmbs->msg.broadcast) { + put_res_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, quantity); + NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity); + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + else { + return recv_write_multiple_registers_res(nmbs, address, quantity); + } + + return NMBS_ERROR_NONE; +} +#endif + +#ifndef NMBS_SERVER_READ_FILE_RECORD_DISABLED +static nmbs_error handle_read_file_record(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t request_size = get_1(nmbs); + if (request_size > 245) + return NMBS_ERROR_INVALID_REQUEST; + + err = recv(nmbs, request_size); + if (err != NMBS_ERROR_NONE) + return err; + + const uint8_t subreq_header_size = 7; + const uint8_t subreq_count = request_size / subreq_header_size; + + struct { + uint8_t reference_type; + uint16_t file_number; + uint16_t record_number; + uint16_t record_length; + } +#if defined(__STDC_NO_VLA__) || defined(_MSC_VER) + subreq[35]; // 245 / subreq_header_size +#else + subreq[subreq_count]; +#endif + + uint8_t response_data_size = 0; + + for (uint8_t i = 0; i < subreq_count; i++) { + subreq[i].reference_type = get_1(nmbs); + subreq[i].file_number = get_2(nmbs); + subreq[i].record_number = get_2(nmbs); + subreq[i].record_length = get_2(nmbs); + + response_data_size += 2 + subreq[i].record_length * 2; + } + + discard_n(nmbs, request_size % subreq_header_size); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + if (request_size % subreq_header_size) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (request_size < 0x07 || request_size > 0xF5) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + for (uint8_t i = 0; i < subreq_count; i++) { + if (subreq[i].reference_type != 0x06) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq[i].file_number == 0x0000) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq[i].record_number > 0x270F) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq[i].record_length > 124) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fread ", subreq[i].file_number, subreq[i].record_number, + subreq[i].record_length); + } + + put_res_header(nmbs, 1 + response_data_size); + put_1(nmbs, response_data_size); + + if (nmbs->callbacks.read_file_record) { + for (uint8_t i = 0; i < subreq_count; i++) { + uint16_t subreq_data_size = subreq[i].record_length * 2; + put_1(nmbs, subreq_data_size + 1); + put_1(nmbs, 0x06); // add Reference Type const + uint16_t* subreq_data = (uint16_t*) get_n(nmbs, subreq_data_size); + + err = nmbs->callbacks.read_file_record(subreq[i].file_number, subreq[i].record_number, subreq_data, + subreq[i].record_length, nmbs->msg.unit_id, nmbs->callbacks.arg); + 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); + } + + swap_regs(subreq_data, subreq[i].record_length); + } + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + + if (!nmbs->msg.broadcast) { + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return recv_read_file_record_res(nmbs, NULL, 0); + } + + return NMBS_ERROR_NONE; +} +#endif + +#ifndef NMBS_SERVER_WRITE_FILE_RECORD_DISABLED +static nmbs_error handle_write_file_record(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 1); + if (err != NMBS_ERROR_NONE) + return err; + + uint8_t request_size = get_1(nmbs); + if (request_size > 251) { + return NMBS_ERROR_INVALID_REQUEST; + } + + err = recv(nmbs, request_size); + if (err != NMBS_ERROR_NONE) + return err; + + // We can save msg.buf index and use it later for context recovery. + uint16_t msg_buf_idx = nmbs->msg.buf_idx; + discard_n(nmbs, request_size); + + err = recv_msg_footer(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.ignored) { + const uint8_t subreq_header_size = 7; + uint16_t size = request_size; + nmbs->msg.buf_idx = msg_buf_idx; // restore context + + if (request_size < 7) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + do { + uint8_t subreq_reference_type = get_1(nmbs); + uint16_t subreq_file_number_c = get_2(nmbs); + uint16_t subreq_record_number_c = get_2(nmbs); + uint16_t subreq_record_length_c = get_2(nmbs); + discard_n(nmbs, subreq_record_length_c * 2); + + if (subreq_reference_type != 0x06) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq_file_number_c == 0x0000) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq_record_number_c > 0x270F) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (subreq_record_length_c > 122) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + NMBS_DEBUG_PRINT("a %d\tr %d\tl %d\t fwrite ", subreq_file_number_c, subreq_record_number_c, + subreq_record_length_c); + size -= (subreq_header_size + subreq_record_length_c * 2); + } while (size >= subreq_header_size); + + if (size) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + // checks completed + + size = request_size; + nmbs->msg.buf_idx = msg_buf_idx; // restore context + + do { + discard_1(nmbs); + uint16_t subreq_file_number = get_2(nmbs); + uint16_t subreq_record_number = get_2(nmbs); + uint16_t subreq_record_length = get_2(nmbs); + uint16_t* subreq_data = get_regs(nmbs, subreq_record_length); + + if (nmbs->callbacks.write_file_record) { + err = nmbs->callbacks.write_file_record(subreq_file_number, subreq_record_number, subreq_data, + subreq_record_length, nmbs->msg.unit_id, nmbs->callbacks.arg); + 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); + } + + swap_regs(subreq_data, subreq_record_length); // restore swapping + } + else { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + + size -= (subreq_header_size + subreq_record_length * 2); + } while (size >= subreq_header_size); + + if (!nmbs->msg.broadcast) { + // The normal response to 'Write File' is an echo of the request. + // We can restore buffer index and response msg. + nmbs->msg.buf_idx = msg_buf_idx; + discard_n(nmbs, request_size); + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return recv_write_file_record_res(nmbs, 0, 0, NULL, 0); + } + + return NMBS_ERROR_NONE; +} +#endif + +#ifndef NMBS_SERVER_READ_WRITE_REGISTERS_DISABLED +static nmbs_error handle_read_write_registers(nmbs_t* nmbs) { + nmbs_error err = recv(nmbs, 9); + if (err != NMBS_ERROR_NONE) + return err; + + uint16_t read_address = get_2(nmbs); + uint16_t read_quantity = get_2(nmbs); + uint16_t write_address = get_2(nmbs); + uint16_t write_quantity = get_2(nmbs); + + uint8_t byte_count_write = get_1(nmbs); + + NMBS_DEBUG_PRINT("ra %d\trq %d\t wa %d\t wq %d\t b %d\tregs ", read_address, read_quantity, write_address, + write_quantity, byte_count_write); + + if (byte_count_write > 242) + return NMBS_ERROR_INVALID_REQUEST; + + err = recv(nmbs, byte_count_write); + if (err != NMBS_ERROR_NONE) + return err; + +#if defined(__STDC_NO_VLA__) || defined(_MSC_VER) + uint16_t registers[0x007B]; +#else + uint16_t registers[byte_count_write / 2]; +#endif + for (int i = 0; i < byte_count_write / 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 (!nmbs->msg.ignored) { + if (read_quantity < 1 || read_quantity > 0x007D) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (write_quantity < 1 || write_quantity > 0x007B) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if (byte_count_write != write_quantity * 2) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if ((uint32_t) read_address + (uint32_t) read_quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if ((uint32_t) write_address + (uint32_t) write_quantity > ((uint32_t) 0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if (!nmbs->callbacks.write_multiple_registers || !nmbs->callbacks.read_holding_registers) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + + err = nmbs->callbacks.write_multiple_registers(write_address, write_quantity, registers, nmbs->msg.unit_id, + nmbs->callbacks.arg); + 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); + } + + if (!nmbs->msg.broadcast) { +#if defined(__STDC_NO_VLA__) || defined(_MSC_VER) + uint16_t regs[125]; +#else + uint16_t regs[read_quantity]; +#endif + err = nmbs->callbacks.read_holding_registers(read_address, read_quantity, regs, nmbs->msg.unit_id, + nmbs->callbacks.arg); + 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); + } + + uint8_t regs_bytes = read_quantity * 2; + put_res_header(nmbs, 1 + regs_bytes); + + put_1(nmbs, regs_bytes); + + NMBS_DEBUG_PRINT("b %d\t", regs_bytes); + + NMBS_DEBUG_PRINT("regs "); + for (int i = 0; i < read_quantity; i++) { + put_2(nmbs, regs[i]); + NMBS_DEBUG_PRINT("%d ", regs[i]); + } + + err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + } + } + else { + return recv_write_multiple_registers_res(nmbs, write_address, write_quantity); + } + + return NMBS_ERROR_NONE; +} +#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 = (int16_t) (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); + + nmbs_error err = NMBS_ERROR_NONE; + switch (nmbs->msg.fc) { +#ifndef NMBS_SERVER_READ_COILS_DISABLED + case 1: + err = handle_read_coils(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED + case 2: + err = handle_read_discrete_inputs(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED + case 3: + err = handle_read_holding_registers(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED + case 4: + err = handle_read_input_registers(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_WRITE_SINGLE_COIL_DISABLED + case 5: + err = handle_write_single_coil(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_WRITE_SINGLE_REGISTER_DISABLED + case 6: + err = handle_write_single_register(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_WRITE_MULTIPLE_COILS_DISABLED + case 15: + err = handle_write_multiple_coils(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_WRITE_MULTIPLE_REGISTERS_DISABLED + case 16: + err = handle_write_multiple_registers(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_READ_FILE_RECORD_DISABLED + case 20: + err = handle_read_file_record(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_WRITE_FILE_RECORD_DISABLED + case 21: + err = handle_write_file_record(nmbs); + break; +#endif + +#ifndef NMBS_SERVER_READ_WRITE_REGISTERS_DISABLED + case 23: + 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: + flush(nmbs); + if (!nmbs->msg.ignored && !nmbs->msg.broadcast) + err = send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + + return err; +} + + +void nmbs_callbacks_create(nmbs_callbacks* callbacks) { + memset(callbacks, 0, sizeof(nmbs_callbacks)); + callbacks->initialized = 0xFFFFDEBE; +} + + +nmbs_error nmbs_server_create(nmbs_t* nmbs, uint8_t address_rtu, const nmbs_platform_conf* platform_conf, + const nmbs_callbacks* callbacks) { + if (platform_conf->transport == NMBS_TRANSPORT_RTU && address_rtu == 0) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (!callbacks || callbacks->initialized != 0xFFFFDEBE) + return NMBS_ERROR_INVALID_ARGUMENT; + + nmbs_error ret = nmbs_create(nmbs, platform_conf); + if (ret != NMBS_ERROR_NONE) + return ret; + + nmbs->address_rtu = address_rtu; + nmbs->callbacks = *callbacks; + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_server_poll(nmbs_t* nmbs) { + msg_state_reset(nmbs); + + bool first_byte_received = false; + nmbs_error err = recv_req_header(nmbs, &first_byte_received); + if (err != NMBS_ERROR_NONE) { + if (!first_byte_received && err == NMBS_ERROR_TIMEOUT) + return NMBS_ERROR_NONE; + + return err; + } + +#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->msg.unit_id); + } +#endif + + err = handle_req_fc(nmbs); + if (err != NMBS_ERROR_NONE) { + if (err != NMBS_ERROR_TIMEOUT) + flush(nmbs); + + return err; + } + + return NMBS_ERROR_NONE; +} + +void nmbs_set_callbacks_arg(nmbs_t* nmbs, void* arg) { + nmbs->callbacks.arg = arg; +} +#endif + + +#ifndef NMBS_CLIENT_DISABLED +nmbs_error nmbs_client_create(nmbs_t* nmbs, const nmbs_platform_conf* platform_conf) { + return nmbs_create(nmbs, platform_conf); +} + + +static nmbs_error read_discrete(nmbs_t* nmbs, uint8_t fc, uint16_t address, uint16_t quantity, nmbs_bitfield values) { + if (quantity < 1 || quantity > NMBS_BITFIELD_MAX) + return NMBS_ERROR_INVALID_ARGUMENT; + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + msg_state_req(nmbs, fc); + put_req_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, quantity); + + NMBS_DEBUG_PRINT("a %d\tq %d", address, quantity); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + return recv_read_discrete_res(nmbs, values); +} + + +nmbs_error nmbs_read_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield coils_out) { + return read_discrete(nmbs, 1, address, quantity, coils_out); +} + + +nmbs_error nmbs_read_discrete_inputs(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield inputs_out) { + 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; + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + msg_state_req(nmbs, fc); + put_req_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, quantity); + + NMBS_DEBUG_PRINT("a %d\tq %d ", address, quantity); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + return recv_read_registers_res(nmbs, quantity, registers); +} + + +nmbs_error nmbs_read_holding_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out) { + return read_registers(nmbs, 3, address, quantity, registers_out); +} + + +nmbs_error nmbs_read_input_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out) { + return read_registers(nmbs, 4, address, quantity, registers_out); +} + + +nmbs_error nmbs_write_single_coil(nmbs_t* nmbs, uint16_t address, bool value) { + msg_state_req(nmbs, 5); + put_req_header(nmbs, 4); + + uint16_t value_req = value ? 0xFF00 : 0; + + put_2(nmbs, address); + put_2(nmbs, value_req); + + NMBS_DEBUG_PRINT("a %d\tvalue %d ", address, value_req); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) + return recv_write_single_coil_res(nmbs, address, value_req); + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_write_single_register(nmbs_t* nmbs, uint16_t address, uint16_t value) { + msg_state_req(nmbs, 6); + put_req_header(nmbs, 4); + + put_2(nmbs, address); + put_2(nmbs, value); + + NMBS_DEBUG_PRINT("a %d\tvalue %d", address, value); + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) + return recv_write_single_register_res(nmbs, address, value); + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_write_multiple_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const nmbs_bitfield coils) { + if (quantity < 1 || quantity > 0x07B0) + return NMBS_ERROR_INVALID_ARGUMENT; + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + uint8_t coils_bytes = (quantity + 7) / 8; + + msg_state_req(nmbs, 15); + put_req_header(nmbs, 5 + coils_bytes); + + put_2(nmbs, address); + put_2(nmbs, quantity); + put_1(nmbs, coils_bytes); + NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\t", address, quantity, coils_bytes); + + NMBS_DEBUG_PRINT("coils "); + for (int i = 0; i < coils_bytes; i++) { + put_1(nmbs, coils[i]); + NMBS_DEBUG_PRINT("%d ", coils[i]); + } + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) + return recv_write_multiple_coils_res(nmbs, address, quantity); + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_write_multiple_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const uint16_t* registers) { + if (quantity < 1 || quantity > 0x007B) + return NMBS_ERROR_INVALID_ARGUMENT; + + if ((uint32_t) address + (uint32_t) quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + uint8_t registers_bytes = quantity * 2; + + msg_state_req(nmbs, 16); + put_req_header(nmbs, 5 + registers_bytes); + + put_2(nmbs, address); + put_2(nmbs, quantity); + put_1(nmbs, registers_bytes); + NMBS_DEBUG_PRINT("a %d\tq %d\tb %d\t", address, quantity, registers_bytes); + + NMBS_DEBUG_PRINT("regs "); + for (int i = 0; i < quantity; i++) { + put_2(nmbs, registers[i]); + NMBS_DEBUG_PRINT("%d ", registers[i]); + } + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) + return recv_write_multiple_registers_res(nmbs, address, quantity); + + return NMBS_ERROR_NONE; +} + + +nmbs_error nmbs_read_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, uint16_t* registers, + uint16_t count) { + if (file_number == 0x0000) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (record_number > 0x270F) + return NMBS_ERROR_INVALID_ARGUMENT; + + // In expected response: max PDU length = 253, assuming a single file request, (253 - 1 - 1 - 1 - 1) / 2 = 124 + if (count > 124) + return NMBS_ERROR_INVALID_ARGUMENT; + + msg_state_req(nmbs, 20); + put_req_header(nmbs, 8); + + put_1(nmbs, 7); // add Byte Count + put_1(nmbs, 6); // add Reference Type const + put_2(nmbs, file_number); + put_2(nmbs, record_number); + 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); + if (err != NMBS_ERROR_NONE) + return err; + + return recv_read_file_record_res(nmbs, registers, count); +} + + +nmbs_error nmbs_write_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, const uint16_t* registers, + uint16_t count) { + if (file_number == 0x0000) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (record_number > 0x270F) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (count > 122) + return NMBS_ERROR_INVALID_ARGUMENT; + + uint16_t data_size = count * 2; + + msg_state_req(nmbs, 21); + put_req_header(nmbs, 8 + data_size); + + put_1(nmbs, 7 + data_size); // add Byte Count + put_1(nmbs, 6); // add Reference Type const + put_2(nmbs, file_number); + put_2(nmbs, record_number); + put_2(nmbs, count); + 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); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) + return recv_write_file_record_res(nmbs, file_number, record_number, registers, count); + + 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) { + if (read_quantity < 1 || read_quantity > 0x007D) + return NMBS_ERROR_INVALID_ARGUMENT; + + if ((uint32_t) read_address + (uint32_t) read_quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + if (write_quantity < 1 || write_quantity > 0x0079) + return NMBS_ERROR_INVALID_ARGUMENT; + + if ((uint32_t) write_address + (uint32_t) write_quantity > ((uint32_t) 0xFFFF) + 1) + return NMBS_ERROR_INVALID_ARGUMENT; + + uint8_t registers_bytes = write_quantity * 2; + + msg_state_req(nmbs, 23); + put_req_header(nmbs, 9 + registers_bytes); + + put_2(nmbs, read_address); + put_2(nmbs, read_quantity); + put_2(nmbs, write_address); + put_2(nmbs, write_quantity); + put_1(nmbs, registers_bytes); + + NMBS_DEBUG_PRINT("read a %d\tq %d ", read_address, read_quantity); + NMBS_DEBUG_PRINT("write a %d\tq %d\tb %d\t", write_address, write_quantity, registers_bytes); + + NMBS_DEBUG_PRINT("regs "); + for (int i = 0; i < write_quantity; i++) { + put_2(nmbs, registers[i]); + NMBS_DEBUG_PRINT("%d ", registers[i]); + } + + nmbs_error err = send_msg(nmbs); + if (err != NMBS_ERROR_NONE) + return err; + + if (!nmbs->msg.broadcast) { + return recv_read_registers_res(nmbs, read_quantity, registers_out); + } + + 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_msg(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_msg(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; + + 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_msg(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_msg(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_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); + + NMBS_DEBUG_PRINT("raw "); + for (uint16_t i = 0; i < data_len; i++) { + put_1(nmbs, data[i]); + NMBS_DEBUG_PRINT("%d ", data[i]); + } + + return send_msg(nmbs); +} + + +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; + + err = recv(nmbs, data_out_len); + if (err != NMBS_ERROR_NONE) + return err; + + 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); + if (err != NMBS_ERROR_NONE) + return err; + + return NMBS_ERROR_NONE; +} +#endif + + +#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"; + + case NMBS_ERROR_INVALID_TCP_MBAP: + return "invalid TCP MBAP received"; + + case NMBS_ERROR_CRC: + return "invalid CRC received"; + + case NMBS_ERROR_TRANSPORT: + return "transport error"; + + case NMBS_ERROR_TIMEOUT: + return "timeout"; + + case NMBS_ERROR_INVALID_RESPONSE: + return "invalid response received"; + + case NMBS_ERROR_INVALID_ARGUMENT: + return "invalid argument provided"; + + case NMBS_ERROR_NONE: + return "no error"; + + case NMBS_EXCEPTION_ILLEGAL_FUNCTION: + return "modbus exception 1: illegal function"; + + case NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS: + return "modbus exception 2: illegal data address"; + + case NMBS_EXCEPTION_ILLEGAL_DATA_VALUE: + return "modbus exception 3: data value"; + + case NMBS_EXCEPTION_SERVER_DEVICE_FAILURE: + return "modbus exception 4: server device failure"; + + default: + return "unknown error"; + } +} +#endif diff --git a/nanoMODBUS/nanomodbus.h b/nanoMODBUS/nanomodbus.h new file mode 100644 index 0000000..5d3d356 --- /dev/null +++ b/nanoMODBUS/nanomodbus.h @@ -0,0 +1,556 @@ +/* + nanoMODBUS - A compact MODBUS RTU/TCP C library for microcontrollers + + MIT License + + Copyright (c) 2024 Valerio De Benedetto (@debevv) + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in all + copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + SOFTWARE. +*/ + + +/** @file */ + +/*! \mainpage nanoMODBUS - A compact MODBUS RTU/TCP C library for microcontrollers + * nanoMODBUS is a small C library that implements the Modbus protocol. It is especially useful in resource-constrained + * system like microcontrollers. + * + * GtiHub: https://github.com/debevv/nanoMODBUS + * + * API reference: \link nanomodbus.h \endlink + * + */ + +#ifndef NANOMODBUS_H +#define NANOMODBUS_H + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * nanoMODBUS errors. + * Values <= 0 are library errors, > 0 are modbus exceptions. + */ +typedef enum nmbs_error { + // Library errors + NMBS_ERROR_INVALID_REQUEST = -8, /**< Received invalid request from client */ + NMBS_ERROR_INVALID_UNIT_ID = -7, /**< Received invalid unit ID in response from server */ + NMBS_ERROR_INVALID_TCP_MBAP = -6, /**< Received invalid TCP MBAP */ + NMBS_ERROR_CRC = -5, /**< Received invalid CRC */ + NMBS_ERROR_TRANSPORT = -4, /**< Transport error */ + NMBS_ERROR_TIMEOUT = -3, /**< Read/write timeout occurred */ + NMBS_ERROR_INVALID_RESPONSE = -2, /**< Received invalid response from server */ + NMBS_ERROR_INVALID_ARGUMENT = -1, /**< Invalid argument provided */ + NMBS_ERROR_NONE = 0, /**< No error */ + + // Modbus exceptions + NMBS_EXCEPTION_ILLEGAL_FUNCTION = 1, /**< Modbus exception 1 */ + NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS = 2, /**< Modbus exception 2 */ + NMBS_EXCEPTION_ILLEGAL_DATA_VALUE = 3, /**< Modbus exception 3 */ + NMBS_EXCEPTION_SERVER_DEVICE_FAILURE = 4, /**< Modbus exception 4 */ +} nmbs_error; + + +/** + * Return whether the nmbs_error is a modbus exception + * @e nmbs_error to check + */ +#define nmbs_error_is_exception(e) ((e) > 0 && (e) < 5) + +#ifndef NMBS_BITFIELD_MAX +#define NMBS_BITFIELD_MAX 2000 +#endif + +/* check coil count divisible by 8 */ +#if ((NMBS_BITFIELD_MAX & 7) > 0) +#error "NMBS_BITFIELD_MAX must be divisible by 8" +#endif + +#define NMBS_BITFIELD_BYTES_MAX (NMBS_BITFIELD_MAX / 8) + +/** + * Bitfield consisting of 2000 coils/discrete inputs + */ +typedef uint8_t nmbs_bitfield[NMBS_BITFIELD_BYTES_MAX]; + +/** + * 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) >> 3] & (0x1 << ((b) & (8 - 1))))) + +/** + * Set a bit of the nmbs_bitfield bf at position b + */ +#define nmbs_bitfield_set(bf, b) (((bf)[(b) >> 3]) = (((bf)[(b) >> 3]) | (0x1 << ((b) & (8 - 1))))) + +/** + * Reset a bit of the nmbs_bitfield bf at position b + */ +#define nmbs_bitfield_unset(bf, b) (((bf)[(b) >> 3]) = (((bf)[(b) >> 3]) & ~(0x1 << ((b) & (8 - 1))))) + +/** + * Write value v to the nmbs_bitfield bf at position b + */ +#define nmbs_bitfield_write(bf, b, v) ((bf)[(b) >> 3] = ((bf)[(b) >> 3] & ~(1 << ((b) & 7))) | ((v) << ((b) & 7))) +/** + * Reset (zero) the whole bitfield + */ +#define nmbs_bitfield_reset(bf) memset(bf, 0, sizeof(bf)) + +/** + * Modbus transport type. + */ +typedef enum nmbs_transport { + NMBS_TRANSPORT_RTU = 1, + NMBS_TRANSPORT_TCP = 2, +} nmbs_transport; + + +/** + * nanoMODBUS platform configuration struct. + * Passed to nmbs_server_create() and nmbs_client_create(). + * + * read() and write() are the platform-specific methods that read/write data to/from a serial port or a TCP connection. + * + * Both methods should block until either: + * - `count` bytes of data are read/written + * - the byte timeout, with `byte_timeout_ms >= 0`, expires + * + * A value `< 0` for `byte_timeout_ms` means infinite timeout. + * With a value `== 0` for `byte_timeout_ms`, the method should read/write once in a non-blocking fashion and return immediately. + * + * + * Their return value should be the number of bytes actually read/written, or `< 0` in case of error. + * A return value between `0` and `count - 1` will be treated as if a timeout occurred on the transport side. All other + * values will be treated as transport errors. + * + * Additionally, an optional crc_calc() function can be defined to override the default nanoMODBUS CRC calculation function. + * + * These methods accept a pointer to arbitrary user-data, which is the arg member of this struct. + * After the creation of an instance it can be changed with nmbs_set_platform_arg(). + */ +typedef struct nmbs_platform_conf { + nmbs_transport transport; /*!< Transport type */ + int32_t (*read)(uint8_t* buf, uint16_t count, int32_t byte_timeout_ms, + void* arg); /*!< Bytes read transport function pointer */ + int32_t (*write)(const uint8_t* buf, uint16_t count, int32_t byte_timeout_ms, + void* arg); /*!< Bytes write transport function pointer */ + uint16_t (*crc_calc)(const uint8_t* data, uint32_t length, + void* arg); /*!< CRC calculation function pointer. Optional */ + void* arg; /*!< User data, will be passed to functions above */ + uint32_t initialized; /*!< Reserved, workaround for older user code not calling nmbs_platform_conf_create() */ +} nmbs_platform_conf; + + +/** + * Modbus server request callbacks. Passed to nmbs_server_create(). + * + * These methods accept a pointer to arbitrary user data, which is the arg member of the nmbs_platform_conf that was passed + * to nmbs_server_create together with this struct. + * + * `unit_id` is the RTU unit ID of the request sender. It is always 0 on TCP. + */ +typedef struct nmbs_callbacks { +#ifndef NMBS_SERVER_DISABLED +#ifndef NMBS_SERVER_READ_COILS_DISABLED + nmbs_error (*read_coils)(uint16_t address, uint16_t quantity, nmbs_bitfield coils_out, uint8_t unit_id, void* arg); +#endif + +#ifndef NMBS_SERVER_READ_DISCRETE_INPUTS_DISABLED + nmbs_error (*read_discrete_inputs)(uint16_t address, uint16_t quantity, nmbs_bitfield inputs_out, uint8_t unit_id, + void* arg); +#endif + +#if !defined(NMBS_SERVER_READ_HOLDING_REGISTERS_DISABLED) || !defined(NMBS_SERVER_READ_WRITE_REGISTERS_DISABLED) + nmbs_error (*read_holding_registers)(uint16_t address, uint16_t quantity, uint16_t* registers_out, uint8_t unit_id, + void* arg); +#endif + +#ifndef NMBS_SERVER_READ_INPUT_REGISTERS_DISABLED + nmbs_error (*read_input_registers)(uint16_t address, uint16_t quantity, uint16_t* registers_out, uint8_t unit_id, + void* arg); +#endif + +#ifndef NMBS_SERVER_WRITE_SINGLE_COIL_DISABLED + nmbs_error (*write_single_coil)(uint16_t address, bool value, uint8_t unit_id, void* arg); +#endif + +#ifndef NMBS_SERVER_WRITE_SINGLE_REGISTER_DISABLED + nmbs_error (*write_single_register)(uint16_t address, uint16_t value, uint8_t unit_id, void* arg); +#endif + +#ifndef NMBS_SERVER_WRITE_MULTIPLE_COILS_DISABLED + nmbs_error (*write_multiple_coils)(uint16_t address, uint16_t quantity, const nmbs_bitfield coils, uint8_t unit_id, + void* arg); +#endif + +#if !defined(NMBS_SERVER_WRITE_MULTIPLE_REGISTERS_DISABLED) || !defined(NMBS_SERVER_READ_WRITE_REGISTERS_DISABLED) + nmbs_error (*write_multiple_registers)(uint16_t address, uint16_t quantity, const uint16_t* registers, + uint8_t unit_id, void* arg); +#endif + +#ifndef NMBS_SERVER_READ_FILE_RECORD_DISABLED + nmbs_error (*read_file_record)(uint16_t file_number, uint16_t record_number, uint16_t* registers, uint16_t count, + uint8_t unit_id, void* arg); +#endif + +#ifndef NMBS_SERVER_WRITE_FILE_RECORD_DISABLED + 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 + uint32_t initialized; // Reserved, workaround for older user code not calling nmbs_callbacks_create() +} nmbs_callbacks; + + +/** + * 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 { + uint8_t buf[260]; + uint16_t buf_idx; + + uint8_t unit_id; + uint8_t fc; + uint16_t transaction_id; + bool broadcast; + bool ignored; + bool complete; + } msg; + + nmbs_callbacks callbacks; + + int32_t byte_timeout_ms; + int32_t read_timeout_ms; + + nmbs_platform_conf platform; + + uint8_t address_rtu; + uint8_t dest_address_rtu; + uint16_t current_tid; +} nmbs_t; + +/** + * Modbus broadcast address. Can be passed to nmbs_set_destination_rtu_address(). + */ +static const uint8_t NMBS_BROADCAST_ADDRESS = 0; + +/** Set the request/response timeout. + * If the target instance is a server, sets the timeout of the nmbs_server_poll() function. + * If the target instance is a client, sets the response timeout after sending a request. In case of timeout, + * the called method will return NMBS_ERROR_TIMEOUT. + * @param nmbs pointer to the nmbs_t instance + * @param timeout_ms timeout in milliseconds. If < 0, the timeout is disabled. + */ +void nmbs_set_read_timeout(nmbs_t* nmbs, int32_t timeout_ms); + +/** Set the timeout between the reception/transmission of two consecutive bytes. + * @param nmbs pointer to the nmbs_t instance + * @param timeout_ms timeout in milliseconds. If < 0, the timeout is disabled. + */ +void nmbs_set_byte_timeout(nmbs_t* nmbs, int32_t timeout_ms); + +/** Create a new nmbs_platform_conf struct. + * @param platform_conf pointer to the nmbs_platform_conf instance + */ +void nmbs_platform_conf_create(nmbs_platform_conf* platform_conf); + +/** Set the pointer to user data argument passed to platform functions. + * @param nmbs pointer to the nmbs_t instance + * @param arg user data argument + */ +void nmbs_set_platform_arg(nmbs_t* nmbs, void* arg); + +#ifndef NMBS_SERVER_DISABLED +/** Create a new nmbs_callbacks struct. + * @param callbacks pointer to the nmbs_callbacks instance + */ +void nmbs_callbacks_create(nmbs_callbacks* callbacks); + +/** Create a new Modbus server. + * @param nmbs pointer to the nmbs_t instance where the client will be created. + * @param address_rtu RTU address of this server. Can be 0 if transport is not RTU. + * @param platform_conf nmbs_platform_conf struct with platform configuration. It may be discarded after calling this method. + * @param callbacks nmbs_callbacks struct with server request callbacks. It may be discarded after calling this method. + * + * @return NMBS_ERROR_NONE if successful, NMBS_ERROR_INVALID_ARGUMENT otherwise. + */ +nmbs_error nmbs_server_create(nmbs_t* nmbs, uint8_t address_rtu, const nmbs_platform_conf* platform_conf, + const nmbs_callbacks* callbacks); + +/** Handle incoming requests to the server. + * This function should be called in a loop in order to serve any incoming request. Its maximum duration, in case of no + * received request, is the value set with nmbs_set_read_timeout() (unless set to < 0). + * @param nmbs pointer to the nmbs_t instance + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_server_poll(nmbs_t* nmbs); + +/** Set the pointer to user data argument passed to server request callbacks. + * @param nmbs pointer to the nmbs_t instance + * @param arg user data argument + */ +void nmbs_set_callbacks_arg(nmbs_t* nmbs, void* arg); +#endif + +#ifndef NMBS_CLIENT_DISABLED +/** Create a new Modbus client. + * @param nmbs pointer to the nmbs_t instance where the client will be created. + * @param platform_conf nmbs_platform_conf struct with platform configuration. It may be discarded after calling this method. + * +* @return NMBS_ERROR_NONE if successful, NMBS_ERROR_INVALID_ARGUMENT otherwise. + */ +nmbs_error nmbs_client_create(nmbs_t* nmbs, const nmbs_platform_conf* platform_conf); + +/** Set the recipient server address of the next request on RTU transport. + * @param nmbs pointer to the nmbs_t instance + * @param address server address + */ +void nmbs_set_destination_rtu_address(nmbs_t* nmbs, uint8_t address); + +/** Send a FC 01 (0x01) Read Coils request + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of coils + * @param coils_out nmbs_bitfield where the coils will be stored + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield coils_out); + +/** Send a FC 02 (0x02) Read Discrete Inputs request + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of inputs + * @param inputs_out nmbs_bitfield where the discrete inputs will be stored + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_discrete_inputs(nmbs_t* nmbs, uint16_t address, uint16_t quantity, nmbs_bitfield inputs_out); + +/** Send a FC 03 (0x03) Read Holding Registers request + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of registers + * @param registers_out array where the registers will be stored + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_holding_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out); + +/** Send a FC 04 (0x04) Read Input Registers request + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of registers + * @param registers_out array where the registers will be stored + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_input_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, uint16_t* registers_out); + +/** Send a FC 05 (0x05) Write Single Coil request + * @param nmbs pointer to the nmbs_t instance + * @param address coil address + * @param value coil value + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_write_single_coil(nmbs_t* nmbs, uint16_t address, bool value); + +/** Send a FC 06 (0x06) Write Single Register request + * @param nmbs pointer to the nmbs_t instance + * @param address register address + * @param value register value + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_write_single_register(nmbs_t* nmbs, uint16_t address, uint16_t value); + +/** Send a FC 15 (0x0F) Write Multiple Coils + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of coils + * @param coils bitfield of coils values + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_write_multiple_coils(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const nmbs_bitfield coils); + +/** Send a FC 16 (0x10) Write Multiple Registers + * @param nmbs pointer to the nmbs_t instance + * @param address starting address + * @param quantity quantity of registers + * @param registers array of registers values + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_write_multiple_registers(nmbs_t* nmbs, uint16_t address, uint16_t quantity, const uint16_t* registers); + +/** Send a FC 20 (0x14) Read File Record + * @param nmbs pointer to the nmbs_t instance + * @param file_number file number (1 to 65535) + * @param record_number record number from file (0000 to 9999) + * @param registers array of registers to read + * @param count count of registers + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_read_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, uint16_t* registers, + uint16_t count); + +/** Send a FC 21 (0x15) Write File Record + * @param nmbs pointer to the nmbs_t instance + * @param file_number file number (1 to 65535) + * @param record_number record number from file (0000 to 9999) + * @param registers array of registers to write + * @param count count of registers + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_write_file_record(nmbs_t* nmbs, uint16_t file_number, uint16_t record_number, const uint16_t* registers, + uint16_t count); + +/** Send a FC 23 (0x17) Read Write Multiple registers + * @param nmbs pointer to the nmbs_t instance + * @param read_address starting read address + * @param read_quantity quantity of registers to read + * @param registers_out array where the read registers will be stored + * @param write_address starting write address + * @param write_quantity quantity of registers to write + * @param registers array of registers values to write + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +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); + +/** 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 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 buffers_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 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 buffers_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_start Object Id to start reading from + * @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 + * @param fc request function code + * @param data request data. It's up to the caller to convert this data to network byte order + * @param data_len length of the data parameter + * + * @return NMBS_ERROR_NONE if successful, other errors otherwise. + */ +nmbs_error nmbs_send_raw_pdu(nmbs_t* nmbs, uint8_t fc, const uint8_t* data, uint16_t data_len); + +/** 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. 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, uint8_t data_out_len); +#endif + +/** Calculate the Modbus CRC of some data. + * @param data Data + * @param length Length of the data + */ +uint16_t nmbs_crc_calc(const uint8_t* data, uint32_t length, void* arg); + +#ifndef NMBS_STRERROR_DISABLED +/** Convert a nmbs_error to string + * @param error error to be converted + * + * @return string representation of the error + */ +const char* nmbs_strerror(nmbs_error error); +#endif + +#ifdef __cplusplus +} // extern "C" +#endif + +#endif //NANOMODBUS_H