Modbus TCP - Multi Devices
This commit is contained in:
Death7z
13
main/CMakeLists.txt
Normal file
13
main/CMakeLists.txt
Normal file
@@ -0,0 +1,13 @@
|
||||
idf_component_register(
|
||||
SRCS
|
||||
"main.c"
|
||||
"modbus_memory.c"
|
||||
"modbus_points.c"
|
||||
"modbus_tcp.c"
|
||||
"config_store.c"
|
||||
"wifi_manager.c"
|
||||
INCLUDE_DIRS
|
||||
"."
|
||||
)
|
||||
|
||||
spiffs_create_partition_image(spiffs ../spiffs FLASH_IN_PROJECT)
|
||||
356
main/config_store.c
Normal file
356
main/config_store.c
Normal file
@@ -0,0 +1,356 @@
|
||||
#include "config_store.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <ctype.h>
|
||||
|
||||
static void trim_whitespace(char *s)
|
||||
{
|
||||
char *start;
|
||||
char *end;
|
||||
|
||||
if (s == NULL || *s == '\0')
|
||||
return;
|
||||
|
||||
start = s;
|
||||
while (*start != '\0' && isspace((unsigned char)*start))
|
||||
start++;
|
||||
|
||||
if (start != s)
|
||||
memmove(s, start, strlen(start) + 1U);
|
||||
|
||||
if (*s == '\0')
|
||||
return;
|
||||
|
||||
end = s + strlen(s) - 1;
|
||||
while (end >= s && isspace((unsigned char)*end))
|
||||
{
|
||||
*end = '\0';
|
||||
end--;
|
||||
}
|
||||
}
|
||||
|
||||
static bool str_ieq(const char *a, const char *b)
|
||||
{
|
||||
unsigned char ca, cb;
|
||||
|
||||
if (a == NULL || b == NULL)
|
||||
return false;
|
||||
|
||||
while (*a != '\0' && *b != '\0')
|
||||
{
|
||||
ca = (unsigned char)tolower((unsigned char)*a);
|
||||
cb = (unsigned char)tolower((unsigned char)*b);
|
||||
|
||||
if (ca != cb)
|
||||
return false;
|
||||
|
||||
a++;
|
||||
b++;
|
||||
}
|
||||
|
||||
return (*a == '\0' && *b == '\0');
|
||||
}
|
||||
|
||||
static bool parse_bool(const char *s, bool *out)
|
||||
{
|
||||
if (s == NULL || out == NULL)
|
||||
return false;
|
||||
|
||||
if (str_ieq(s, "true") || str_ieq(s, "yes") || strcmp(s, "1") == 0)
|
||||
{
|
||||
*out = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
if (str_ieq(s, "false") || str_ieq(s, "no") || strcmp(s, "0") == 0)
|
||||
{
|
||||
*out = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool parse_u32(const char *s, uint32_t *out)
|
||||
{
|
||||
char *endptr = NULL;
|
||||
unsigned long v;
|
||||
|
||||
if (s == NULL || out == NULL || *s == '\0')
|
||||
return false;
|
||||
|
||||
v = strtoul(s, &endptr, 10);
|
||||
if (endptr == NULL || *endptr != '\0')
|
||||
return false;
|
||||
|
||||
*out = (uint32_t)v;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void copy_str(char *dst, size_t dst_size, const char *src)
|
||||
{
|
||||
if (dst == NULL || dst_size == 0U)
|
||||
return;
|
||||
|
||||
if (src == NULL)
|
||||
src = "";
|
||||
|
||||
strncpy(dst, src, dst_size - 1U);
|
||||
dst[dst_size - 1U] = '\0';
|
||||
}
|
||||
|
||||
static int parse_device_section_index(const char *section)
|
||||
{
|
||||
unsigned long idx;
|
||||
char *endptr = NULL;
|
||||
|
||||
if (section == NULL)
|
||||
return -1;
|
||||
|
||||
if (strlen(section) < 8U)
|
||||
return -1;
|
||||
|
||||
if (tolower((unsigned char)section[0]) != 'd' ||
|
||||
tolower((unsigned char)section[1]) != 'e' ||
|
||||
tolower((unsigned char)section[2]) != 'v' ||
|
||||
tolower((unsigned char)section[3]) != 'i' ||
|
||||
tolower((unsigned char)section[4]) != 'c' ||
|
||||
tolower((unsigned char)section[5]) != 'e')
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (section[6] != ' ')
|
||||
return -1;
|
||||
|
||||
idx = strtoul(§ion[7], &endptr, 10);
|
||||
if (endptr == NULL || *endptr != '\0')
|
||||
return -1;
|
||||
|
||||
if (idx == 0UL || idx > MAX_VIRTUAL_DEVICES)
|
||||
return -1;
|
||||
|
||||
return (int)(idx - 1UL);
|
||||
}
|
||||
|
||||
static void set_device_defaults(virtual_device_settings_t *dev, uint8_t index)
|
||||
{
|
||||
char default_name[DEVICE_NAME_MAX_LEN];
|
||||
|
||||
if (dev == NULL)
|
||||
return;
|
||||
|
||||
memset(dev, 0, sizeof(*dev));
|
||||
|
||||
dev->enabled = false;
|
||||
dev->unit_id = (uint8_t)(index + 1U);
|
||||
|
||||
snprintf(default_name, sizeof(default_name), "Device_%u", (unsigned)(index + 1U));
|
||||
copy_str(dev->name, sizeof(dev->name), default_name);
|
||||
dev->csv[0] = '\0';
|
||||
}
|
||||
|
||||
void config_store_set_defaults(device_config_t *cfg)
|
||||
{
|
||||
size_t i;
|
||||
|
||||
if (cfg == NULL)
|
||||
return;
|
||||
|
||||
memset(cfg, 0, sizeof(*cfg));
|
||||
|
||||
copy_str(cfg->wifi.ssid, sizeof(cfg->wifi.ssid), "YOUR_WIFI_SSID");
|
||||
copy_str(cfg->wifi.password, sizeof(cfg->wifi.password), "YOUR_WIFI_PASSWORD");
|
||||
cfg->wifi.dhcp = true;
|
||||
copy_str(cfg->wifi.static_ip, sizeof(cfg->wifi.static_ip), "192.168.1.50");
|
||||
copy_str(cfg->wifi.gateway, sizeof(cfg->wifi.gateway), "192.168.1.1");
|
||||
copy_str(cfg->wifi.netmask, sizeof(cfg->wifi.netmask), "255.255.255.0");
|
||||
|
||||
cfg->modbus.port = 502;
|
||||
cfg->modbus.max_clients = 4;
|
||||
cfg->modbus.device_count = 1;
|
||||
|
||||
for (i = 0; i < MAX_VIRTUAL_DEVICES; i++)
|
||||
set_device_defaults(&cfg->devices[i], (uint8_t)i);
|
||||
}
|
||||
|
||||
bool config_store_load(const char *filename, device_config_t *cfg)
|
||||
{
|
||||
FILE *fp;
|
||||
char line[256];
|
||||
char section[32];
|
||||
|
||||
if (filename == NULL || cfg == NULL)
|
||||
return false;
|
||||
|
||||
config_store_set_defaults(cfg);
|
||||
memset(section, 0, sizeof(section));
|
||||
|
||||
fp = fopen(filename, "r");
|
||||
if (fp == NULL)
|
||||
return false;
|
||||
|
||||
while (fgets(line, sizeof(line), fp) != NULL)
|
||||
{
|
||||
char *eq;
|
||||
char *key;
|
||||
char *value;
|
||||
|
||||
line[strcspn(line, "\r\n")] = '\0';
|
||||
trim_whitespace(line);
|
||||
|
||||
if (line[0] == '\0')
|
||||
continue;
|
||||
|
||||
if (line[0] == ';' || line[0] == '#')
|
||||
continue;
|
||||
|
||||
if (line[0] == '[')
|
||||
{
|
||||
size_t len = strlen(line);
|
||||
|
||||
if (len >= 3U && line[len - 1U] == ']')
|
||||
{
|
||||
line[len - 1U] = '\0';
|
||||
copy_str(section, sizeof(section), &line[1]);
|
||||
trim_whitespace(section);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
eq = strchr(line, '=');
|
||||
if (eq == NULL)
|
||||
continue;
|
||||
|
||||
*eq = '\0';
|
||||
key = line;
|
||||
value = eq + 1;
|
||||
|
||||
trim_whitespace(key);
|
||||
trim_whitespace(value);
|
||||
|
||||
if (str_ieq(section, "wifi"))
|
||||
{
|
||||
if (str_ieq(key, "ssid"))
|
||||
copy_str(cfg->wifi.ssid, sizeof(cfg->wifi.ssid), value);
|
||||
else if (str_ieq(key, "password"))
|
||||
copy_str(cfg->wifi.password, sizeof(cfg->wifi.password), value);
|
||||
else if (str_ieq(key, "dhcp"))
|
||||
{
|
||||
bool b;
|
||||
if (parse_bool(value, &b))
|
||||
cfg->wifi.dhcp = b;
|
||||
}
|
||||
else if (str_ieq(key, "static_ip"))
|
||||
copy_str(cfg->wifi.static_ip, sizeof(cfg->wifi.static_ip), value);
|
||||
else if (str_ieq(key, "gateway"))
|
||||
copy_str(cfg->wifi.gateway, sizeof(cfg->wifi.gateway), value);
|
||||
else if (str_ieq(key, "netmask"))
|
||||
copy_str(cfg->wifi.netmask, sizeof(cfg->wifi.netmask), value);
|
||||
}
|
||||
else if (str_ieq(section, "modbus"))
|
||||
{
|
||||
uint32_t v;
|
||||
|
||||
if (str_ieq(key, "port"))
|
||||
{
|
||||
if (parse_u32(value, &v) && v <= 65535U)
|
||||
cfg->modbus.port = (uint16_t)v;
|
||||
}
|
||||
else if (str_ieq(key, "max_clients"))
|
||||
{
|
||||
if (parse_u32(value, &v) && v > 0U && v <= 16U)
|
||||
cfg->modbus.max_clients = (uint8_t)v;
|
||||
}
|
||||
else if (str_ieq(key, "device_count"))
|
||||
{
|
||||
if (parse_u32(value, &v) && v > 0U)
|
||||
{
|
||||
if (v > MAX_VIRTUAL_DEVICES)
|
||||
cfg->modbus.device_count = MAX_VIRTUAL_DEVICES;
|
||||
else
|
||||
cfg->modbus.device_count = (uint8_t)v;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int dev_index = parse_device_section_index(section);
|
||||
|
||||
if (dev_index >= 0)
|
||||
{
|
||||
virtual_device_settings_t *dev = &cfg->devices[dev_index];
|
||||
uint32_t v;
|
||||
|
||||
if (str_ieq(key, "enabled"))
|
||||
{
|
||||
bool b;
|
||||
if (parse_bool(value, &b))
|
||||
dev->enabled = b;
|
||||
}
|
||||
else if (str_ieq(key, "unit_id"))
|
||||
{
|
||||
if (parse_u32(value, &v) && v > 0U && v <= 255U)
|
||||
dev->unit_id = (uint8_t)v;
|
||||
}
|
||||
else if (str_ieq(key, "name"))
|
||||
{
|
||||
copy_str(dev->name, sizeof(dev->name), value);
|
||||
}
|
||||
else if (str_ieq(key, "csv"))
|
||||
{
|
||||
copy_str(dev->csv, sizeof(dev->csv), value);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fclose(fp);
|
||||
|
||||
if (cfg->wifi.ssid[0] == '\0')
|
||||
return false;
|
||||
|
||||
if (cfg->modbus.port == 0U)
|
||||
return false;
|
||||
|
||||
if (cfg->modbus.max_clients == 0U)
|
||||
return false;
|
||||
|
||||
if (cfg->modbus.device_count == 0U || cfg->modbus.device_count > MAX_VIRTUAL_DEVICES)
|
||||
return false;
|
||||
|
||||
for (uint8_t i = 0; i < cfg->modbus.device_count; i++)
|
||||
{
|
||||
uint8_t j;
|
||||
virtual_device_settings_t *dev = &cfg->devices[i];
|
||||
|
||||
if (!dev->enabled)
|
||||
continue;
|
||||
|
||||
if (dev->unit_id == 0U)
|
||||
return false;
|
||||
|
||||
if (dev->csv[0] == '\0')
|
||||
return false;
|
||||
|
||||
if (dev->name[0] == '\0')
|
||||
{
|
||||
char default_name[DEVICE_NAME_MAX_LEN];
|
||||
snprintf(default_name, sizeof(default_name), "Device_%u", (unsigned)(i + 1U));
|
||||
copy_str(dev->name, sizeof(dev->name), default_name);
|
||||
}
|
||||
|
||||
for (j = (uint8_t)(i + 1U); j < cfg->modbus.device_count; j++)
|
||||
{
|
||||
if (!cfg->devices[j].enabled)
|
||||
continue;
|
||||
|
||||
if (dev->unit_id == cfg->devices[j].unit_id)
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
52
main/config_store.h
Normal file
52
main/config_store.h
Normal file
@@ -0,0 +1,52 @@
|
||||
#ifndef CONFIG_STORE_H
|
||||
#define CONFIG_STORE_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#define DEVICE_NAME_MAX_LEN 32
|
||||
#define DEVICE_CSV_MAX_LEN 64
|
||||
#define WIFI_SSID_MAX_LEN 32
|
||||
#define WIFI_PASSWORD_MAX_LEN 64
|
||||
#define IPV4_STR_MAX_LEN 16
|
||||
#define MAX_VIRTUAL_DEVICES 2
|
||||
|
||||
typedef struct
|
||||
{
|
||||
char ssid[WIFI_SSID_MAX_LEN];
|
||||
char password[WIFI_PASSWORD_MAX_LEN];
|
||||
|
||||
bool dhcp;
|
||||
|
||||
char static_ip[IPV4_STR_MAX_LEN];
|
||||
char gateway[IPV4_STR_MAX_LEN];
|
||||
char netmask[IPV4_STR_MAX_LEN];
|
||||
} wifi_settings_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint16_t port;
|
||||
uint8_t max_clients;
|
||||
uint8_t device_count;
|
||||
} modbus_settings_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
bool enabled;
|
||||
uint8_t unit_id;
|
||||
char name[DEVICE_NAME_MAX_LEN];
|
||||
char csv[DEVICE_CSV_MAX_LEN];
|
||||
} virtual_device_settings_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
wifi_settings_t wifi;
|
||||
modbus_settings_t modbus;
|
||||
virtual_device_settings_t devices[MAX_VIRTUAL_DEVICES];
|
||||
} device_config_t;
|
||||
|
||||
void config_store_set_defaults(device_config_t *cfg);
|
||||
bool config_store_load(const char *filename, device_config_t *cfg);
|
||||
|
||||
#endif /* CONFIG_STORE_H */
|
||||
918
main/main.c
Normal file
918
main/main.c
Normal file
@@ -0,0 +1,918 @@
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <errno.h>
|
||||
#include <unistd.h>
|
||||
#include <stdlib.h>
|
||||
#include <sys/select.h>
|
||||
#include <dirent.h>
|
||||
|
||||
#include "freertos/FreeRTOS.h"
|
||||
#include "freertos/task.h"
|
||||
#include "config_store.h"
|
||||
#include "wifi_manager.h"
|
||||
|
||||
#include "esp_log.h"
|
||||
#include "esp_err.h"
|
||||
#include "nvs_flash.h"
|
||||
#include "esp_spiffs.h"
|
||||
|
||||
#include "lwip/sockets.h"
|
||||
#include "lwip/netdb.h"
|
||||
#include "lwip/inet.h"
|
||||
#include "lwip/ip_addr.h"
|
||||
#include "lwip/ip4_addr.h"
|
||||
#include "esp_netif_ip_addr.h"
|
||||
|
||||
#include "modbus_memory.h"
|
||||
#include "modbus_points.h"
|
||||
#include "modbus_tcp.h"
|
||||
|
||||
#define DEVICE_CONFIG_PATH "/spiffs/device_config.ini"
|
||||
#define TAG "MODBUS_MAIN"
|
||||
#define MODBUS_RX_BUF_SIZE MODBUS_TCP_MAX_ADU_LEN
|
||||
#define MODBUS_TX_BUF_SIZE MODBUS_TCP_MAX_ADU_LEN
|
||||
|
||||
static device_config_t g_device_cfg;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
int sock;
|
||||
struct sockaddr_in addr;
|
||||
} modbus_client_ctx_t;
|
||||
|
||||
static volatile int g_active_modbus_clients = 0;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint8_t unit_id;
|
||||
char name[DEVICE_NAME_MAX_LEN];
|
||||
char csv_path[128];
|
||||
|
||||
modbus_db_t db;
|
||||
modbus_memory_t mem;
|
||||
} modbus_device_t;
|
||||
|
||||
static modbus_device_t g_devices[MAX_VIRTUAL_DEVICES];
|
||||
static size_t g_device_count = 0;
|
||||
|
||||
/* ------------------------------------------------------------ */
|
||||
/* Helpers */
|
||||
/* ------------------------------------------------------------ */
|
||||
|
||||
static modbus_device_t *find_device_by_unit_id(uint8_t unit_id)
|
||||
{
|
||||
size_t i;
|
||||
|
||||
for (i = 0; i < g_device_count; i++)
|
||||
{
|
||||
if (g_devices[i].unit_id == unit_id)
|
||||
return &g_devices[i];
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static bool file_exists(const char *path)
|
||||
{
|
||||
FILE *fp;
|
||||
|
||||
if (path == NULL)
|
||||
return false;
|
||||
|
||||
fp = fopen(path, "r");
|
||||
if (fp == NULL)
|
||||
return false;
|
||||
|
||||
fclose(fp);
|
||||
return true;
|
||||
}
|
||||
|
||||
static void spiffs_list_files(void)
|
||||
{
|
||||
DIR *dir;
|
||||
struct dirent *entry;
|
||||
|
||||
dir = opendir("/spiffs");
|
||||
if (dir == NULL)
|
||||
{
|
||||
ESP_LOGE(TAG, "Failed to open /spiffs for listing");
|
||||
return;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "----- SPIFFS file list -----");
|
||||
|
||||
while ((entry = readdir(dir)) != NULL)
|
||||
{
|
||||
ESP_LOGI(TAG, "SPIFFS file: %s", entry->d_name);
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "----------------------------");
|
||||
|
||||
closedir(dir);
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------ */
|
||||
/* SPIFFS */
|
||||
/* ------------------------------------------------------------ */
|
||||
|
||||
static esp_err_t spiffs_init(void)
|
||||
{
|
||||
esp_vfs_spiffs_conf_t conf = {
|
||||
.base_path = "/spiffs",
|
||||
.partition_label = NULL,
|
||||
.max_files = 8,
|
||||
.format_if_mount_failed = true
|
||||
};
|
||||
|
||||
esp_err_t ret = esp_vfs_spiffs_register(&conf);
|
||||
if (ret != ESP_OK)
|
||||
{
|
||||
ESP_LOGE(TAG, "Failed to mount SPIFFS (%s)", esp_err_to_name(ret));
|
||||
return ret;
|
||||
}
|
||||
|
||||
{
|
||||
size_t total = 0;
|
||||
size_t used = 0;
|
||||
ret = esp_spiffs_info(NULL, &total, &used);
|
||||
if (ret == ESP_OK)
|
||||
{
|
||||
ESP_LOGI(TAG, "SPIFFS mounted: total=%u used=%u",
|
||||
(unsigned)total, (unsigned)used);
|
||||
}
|
||||
}
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------ */
|
||||
/* Modbus TCP server */
|
||||
/* ------------------------------------------------------------ */
|
||||
|
||||
static void modbus_client_task(void *arg)
|
||||
{
|
||||
modbus_client_ctx_t *ctx = (modbus_client_ctx_t *)arg;
|
||||
uint8_t rx_buf[MODBUS_RX_BUF_SIZE];
|
||||
uint8_t tx_buf[MODBUS_TX_BUF_SIZE];
|
||||
|
||||
if (ctx == NULL)
|
||||
{
|
||||
vTaskDelete(NULL);
|
||||
return;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "Client connected: %s:%d (active=%d)",
|
||||
inet_ntoa(ctx->addr.sin_addr),
|
||||
ntohs(ctx->addr.sin_port),
|
||||
g_active_modbus_clients);
|
||||
|
||||
while (1)
|
||||
{
|
||||
int len;
|
||||
size_t resp_len = 0;
|
||||
bool ok;
|
||||
modbus_device_t *dev;
|
||||
uint8_t unit_id = 0;
|
||||
|
||||
len = recv(ctx->sock, rx_buf, sizeof(rx_buf), 0);
|
||||
if (len < 0)
|
||||
{
|
||||
ESP_LOGW(TAG, "recv failed: errno=%d", errno);
|
||||
break;
|
||||
}
|
||||
else if (len == 0)
|
||||
{
|
||||
ESP_LOGI(TAG, "Client disconnected: %s:%d",
|
||||
inet_ntoa(ctx->addr.sin_addr),
|
||||
ntohs(ctx->addr.sin_port));
|
||||
break;
|
||||
}
|
||||
|
||||
if (len < 7)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
unit_id = rx_buf[6];
|
||||
dev = find_device_by_unit_id(unit_id);
|
||||
|
||||
if (dev == NULL)
|
||||
{
|
||||
ESP_LOGW(TAG, "Unknown Unit ID %u from %s:%d",
|
||||
(unsigned)unit_id,
|
||||
inet_ntoa(ctx->addr.sin_addr),
|
||||
ntohs(ctx->addr.sin_port));
|
||||
continue;
|
||||
}
|
||||
|
||||
ok = modbus_tcp_process_request(&dev->mem,
|
||||
rx_buf,
|
||||
(size_t)len,
|
||||
tx_buf,
|
||||
sizeof(tx_buf),
|
||||
&resp_len);
|
||||
|
||||
if (!ok)
|
||||
{
|
||||
if (len >= 8 && (rx_buf[6] == 1U || rx_buf[6] == 2U))
|
||||
{
|
||||
ESP_LOGW(TAG, "Unsupported Modbus request from %s:%d (unit_id=%u fc=0x%02X)",
|
||||
inet_ntoa(ctx->addr.sin_addr),
|
||||
ntohs(ctx->addr.sin_port),
|
||||
(unsigned)rx_buf[6],
|
||||
(unsigned)rx_buf[7]);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
if (resp_len > 0U)
|
||||
{
|
||||
int sent = send(ctx->sock, tx_buf, (int)resp_len, 0);
|
||||
if (sent < 0)
|
||||
{
|
||||
ESP_LOGW(TAG, "send failed: errno=%d", errno);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
shutdown(ctx->sock, 0);
|
||||
close(ctx->sock);
|
||||
|
||||
if (g_active_modbus_clients > 0)
|
||||
g_active_modbus_clients--;
|
||||
|
||||
free(ctx);
|
||||
vTaskDelete(NULL);
|
||||
}
|
||||
|
||||
static void modbus_server_task(void *arg)
|
||||
{
|
||||
int listen_sock = -1;
|
||||
struct sockaddr_in server_addr;
|
||||
int opt = 1;
|
||||
|
||||
(void)arg;
|
||||
|
||||
listen_sock = socket(AF_INET, SOCK_STREAM, IPPROTO_IP);
|
||||
if (listen_sock < 0)
|
||||
{
|
||||
ESP_LOGE(TAG, "Unable to create socket: errno=%d", errno);
|
||||
vTaskDelete(NULL);
|
||||
return;
|
||||
}
|
||||
|
||||
setsockopt(listen_sock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
|
||||
|
||||
memset(&server_addr, 0, sizeof(server_addr));
|
||||
server_addr.sin_family = AF_INET;
|
||||
server_addr.sin_port = htons(g_device_cfg.modbus.port);
|
||||
server_addr.sin_addr.s_addr = htonl(INADDR_ANY);
|
||||
|
||||
if (bind(listen_sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0)
|
||||
{
|
||||
ESP_LOGE(TAG, "Socket bind failed: errno=%d", errno);
|
||||
close(listen_sock);
|
||||
vTaskDelete(NULL);
|
||||
return;
|
||||
}
|
||||
|
||||
if (listen(listen_sock, g_device_cfg.modbus.max_clients) < 0)
|
||||
{
|
||||
ESP_LOGE(TAG, "Socket listen failed: errno=%d", errno);
|
||||
close(listen_sock);
|
||||
vTaskDelete(NULL);
|
||||
return;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "Modbus TCP server listening on port %u (max_clients=%u)",
|
||||
(unsigned)g_device_cfg.modbus.port,
|
||||
(unsigned)g_device_cfg.modbus.max_clients);
|
||||
|
||||
while (1)
|
||||
{
|
||||
struct sockaddr_in client_addr;
|
||||
socklen_t client_len = sizeof(client_addr);
|
||||
int client_sock = accept(listen_sock, (struct sockaddr *)&client_addr, &client_len);
|
||||
|
||||
if (client_sock < 0)
|
||||
{
|
||||
ESP_LOGW(TAG, "accept failed: errno=%d", errno);
|
||||
continue;
|
||||
}
|
||||
|
||||
if (g_active_modbus_clients >= (int)g_device_cfg.modbus.max_clients)
|
||||
{
|
||||
ESP_LOGW(TAG, "Rejecting client %s:%d (active=%d max=%u)",
|
||||
inet_ntoa(client_addr.sin_addr),
|
||||
ntohs(client_addr.sin_port),
|
||||
g_active_modbus_clients,
|
||||
(unsigned)g_device_cfg.modbus.max_clients);
|
||||
shutdown(client_sock, 0);
|
||||
close(client_sock);
|
||||
continue;
|
||||
}
|
||||
|
||||
{
|
||||
modbus_client_ctx_t *ctx = malloc(sizeof(modbus_client_ctx_t));
|
||||
if (ctx == NULL)
|
||||
{
|
||||
ESP_LOGE(TAG, "Out of memory allocating client context");
|
||||
shutdown(client_sock, 0);
|
||||
close(client_sock);
|
||||
continue;
|
||||
}
|
||||
|
||||
ctx->sock = client_sock;
|
||||
ctx->addr = client_addr;
|
||||
|
||||
g_active_modbus_clients++;
|
||||
|
||||
if (xTaskCreate(modbus_client_task,
|
||||
"modbus_client_task",
|
||||
6144,
|
||||
ctx,
|
||||
5,
|
||||
NULL) != pdPASS)
|
||||
{
|
||||
ESP_LOGE(TAG, "Failed to create client task");
|
||||
g_active_modbus_clients--;
|
||||
shutdown(client_sock, 0);
|
||||
close(client_sock);
|
||||
free(ctx);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------ */
|
||||
/* Override Sync Task */
|
||||
/* ------------------------------------------------------------ */
|
||||
|
||||
static void modbus_override_task(void *arg)
|
||||
{
|
||||
(void)arg;
|
||||
|
||||
while (1)
|
||||
{
|
||||
size_t d;
|
||||
|
||||
for (d = 0; d < g_device_count; d++)
|
||||
{
|
||||
modbus_device_t *dev = &g_devices[d];
|
||||
size_t i;
|
||||
|
||||
for (i = 0; i < dev->db.count; i++)
|
||||
{
|
||||
modbus_point_t *pt = &dev->db.points[i];
|
||||
|
||||
if (pt->is_helper)
|
||||
continue;
|
||||
|
||||
if (!pt->has_control_address)
|
||||
continue;
|
||||
|
||||
if (pt->data_type == MB_DATA_BOOL)
|
||||
{
|
||||
uint8_t value;
|
||||
|
||||
if (modbus_memory_read_bit(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->control_type),
|
||||
pt->control_offset,
|
||||
&value))
|
||||
{
|
||||
modbus_memory_write_bit(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->type),
|
||||
pt->offset,
|
||||
value);
|
||||
}
|
||||
}
|
||||
else if (pt->data_type == MB_DATA_UINT16)
|
||||
{
|
||||
uint16_t value;
|
||||
|
||||
if (modbus_memory_read_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->control_type),
|
||||
pt->control_offset,
|
||||
&value))
|
||||
{
|
||||
modbus_memory_write_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->type),
|
||||
pt->offset,
|
||||
value);
|
||||
}
|
||||
}
|
||||
else if (pt->data_type == MB_DATA_UINT32 ||
|
||||
pt->data_type == MB_DATA_FLOAT)
|
||||
{
|
||||
uint16_t w1, w2;
|
||||
|
||||
if (modbus_memory_read_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->control_type),
|
||||
pt->control_offset,
|
||||
&w1) &&
|
||||
modbus_memory_read_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->control_type),
|
||||
(uint16_t)(pt->control_offset + 1U),
|
||||
&w2))
|
||||
{
|
||||
modbus_memory_write_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->type),
|
||||
pt->offset,
|
||||
w1);
|
||||
|
||||
modbus_memory_write_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->type),
|
||||
(uint16_t)(pt->offset + 1U),
|
||||
w2);
|
||||
}
|
||||
}
|
||||
else if (pt->data_type == MB_DATA_DOUBLE)
|
||||
{
|
||||
uint16_t w1, w2, w3, w4;
|
||||
|
||||
if (modbus_memory_read_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->control_type),
|
||||
pt->control_offset,
|
||||
&w1) &&
|
||||
modbus_memory_read_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->control_type),
|
||||
(uint16_t)(pt->control_offset + 1U),
|
||||
&w2) &&
|
||||
modbus_memory_read_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->control_type),
|
||||
(uint16_t)(pt->control_offset + 2U),
|
||||
&w3) &&
|
||||
modbus_memory_read_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->control_type),
|
||||
(uint16_t)(pt->control_offset + 3U),
|
||||
&w4))
|
||||
{
|
||||
modbus_memory_write_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->type),
|
||||
pt->offset,
|
||||
w1);
|
||||
|
||||
modbus_memory_write_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->type),
|
||||
(uint16_t)(pt->offset + 1U),
|
||||
w2);
|
||||
|
||||
modbus_memory_write_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->type),
|
||||
(uint16_t)(pt->offset + 2U),
|
||||
w3);
|
||||
|
||||
modbus_memory_write_reg(&dev->mem,
|
||||
modbus_points_type_to_mem(pt->type),
|
||||
(uint16_t)(pt->offset + 3U),
|
||||
w4);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
vTaskDelay(pdMS_TO_TICKS(100));
|
||||
}
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------ */
|
||||
/* Helpers for app_main */
|
||||
/* ------------------------------------------------------------ */
|
||||
|
||||
static const char *data_type_to_str(modbus_data_type_t type)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case MB_DATA_BOOL:
|
||||
return "bool";
|
||||
case MB_DATA_UINT16:
|
||||
return "uint16";
|
||||
case MB_DATA_UINT32:
|
||||
return "uint32";
|
||||
case MB_DATA_FLOAT:
|
||||
return "float";
|
||||
case MB_DATA_DOUBLE:
|
||||
return "double";
|
||||
default:
|
||||
return "invalid";
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t data_type_internal_cost(modbus_data_type_t type)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case MB_DATA_BOOL:
|
||||
case MB_DATA_UINT16:
|
||||
return 1U;
|
||||
|
||||
case MB_DATA_UINT32:
|
||||
case MB_DATA_FLOAT:
|
||||
return 2U;
|
||||
|
||||
case MB_DATA_DOUBLE:
|
||||
return 4U;
|
||||
|
||||
default:
|
||||
return 0U;
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t pics_type_cost(pics_data_type_t type)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case PICS_DATA_BOOLEAN:
|
||||
return 1U;
|
||||
case PICS_DATA_INTEGER:
|
||||
return 2U;
|
||||
case PICS_DATA_FLOAT:
|
||||
return 4U;
|
||||
case PICS_DATA_NONE:
|
||||
default:
|
||||
return 0U;
|
||||
}
|
||||
}
|
||||
|
||||
static void print_device_type_mix(const modbus_device_t *dev)
|
||||
{
|
||||
size_t i;
|
||||
size_t bool_count = 0;
|
||||
size_t uint16_count = 0;
|
||||
size_t uint32_count = 0;
|
||||
size_t float_count = 0;
|
||||
size_t double_count = 0;
|
||||
|
||||
size_t pics_bool_count = 0;
|
||||
size_t pics_int_count = 0;
|
||||
size_t pics_float_count = 0;
|
||||
size_t pics_none_count = 0;
|
||||
|
||||
size_t internal_points_from_base_rows = 0;
|
||||
size_t pics_registers_from_base_rows = 0;
|
||||
size_t base_rows = 0;
|
||||
|
||||
if (dev == NULL)
|
||||
return;
|
||||
|
||||
for (i = 0; i < dev->db.count; i++)
|
||||
{
|
||||
const modbus_point_t *pt = &dev->db.points[i];
|
||||
|
||||
if (pt->is_helper)
|
||||
continue;
|
||||
|
||||
base_rows++;
|
||||
|
||||
switch (pt->data_type)
|
||||
{
|
||||
case MB_DATA_BOOL:
|
||||
bool_count++;
|
||||
break;
|
||||
case MB_DATA_UINT16:
|
||||
uint16_count++;
|
||||
break;
|
||||
case MB_DATA_UINT32:
|
||||
uint32_count++;
|
||||
break;
|
||||
case MB_DATA_FLOAT:
|
||||
float_count++;
|
||||
break;
|
||||
case MB_DATA_DOUBLE:
|
||||
double_count++;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
|
||||
switch (pt->pics_data_type)
|
||||
{
|
||||
case PICS_DATA_BOOLEAN:
|
||||
pics_bool_count++;
|
||||
break;
|
||||
case PICS_DATA_INTEGER:
|
||||
pics_int_count++;
|
||||
break;
|
||||
case PICS_DATA_FLOAT:
|
||||
pics_float_count++;
|
||||
break;
|
||||
case PICS_DATA_NONE:
|
||||
default:
|
||||
pics_none_count++;
|
||||
break;
|
||||
}
|
||||
|
||||
internal_points_from_base_rows += data_type_internal_cost(pt->data_type);
|
||||
pics_registers_from_base_rows += pics_type_cost(pt->pics_data_type);
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG,
|
||||
"Device %u (%s) type mix: base_rows=%u internal_points=%u pics_regs=%u",
|
||||
(unsigned)dev->unit_id,
|
||||
dev->name,
|
||||
(unsigned)base_rows,
|
||||
(unsigned)internal_points_from_base_rows,
|
||||
(unsigned)pics_registers_from_base_rows);
|
||||
|
||||
ESP_LOGI(TAG,
|
||||
" data_type counts: bool=%u uint16=%u uint32=%u float=%u double=%u",
|
||||
(unsigned)bool_count,
|
||||
(unsigned)uint16_count,
|
||||
(unsigned)uint32_count,
|
||||
(unsigned)float_count,
|
||||
(unsigned)double_count);
|
||||
|
||||
ESP_LOGI(TAG,
|
||||
" pics_data_type counts: none=%u Boolean=%u Integer=%u Float=%u",
|
||||
(unsigned)pics_none_count,
|
||||
(unsigned)pics_bool_count,
|
||||
(unsigned)pics_int_count,
|
||||
(unsigned)pics_float_count);
|
||||
}
|
||||
|
||||
/* ------------------------------------------------------------ */
|
||||
/* app_main */
|
||||
/* ------------------------------------------------------------ */
|
||||
|
||||
void app_main(void)
|
||||
{
|
||||
esp_err_t ret;
|
||||
size_t i;
|
||||
|
||||
ESP_LOGI(TAG, "Starting Modbus TCP device");
|
||||
|
||||
ret = nvs_flash_init();
|
||||
if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND)
|
||||
{
|
||||
ESP_ERROR_CHECK(nvs_flash_erase());
|
||||
ret = nvs_flash_init();
|
||||
}
|
||||
ESP_ERROR_CHECK(ret);
|
||||
|
||||
ESP_ERROR_CHECK(spiffs_init());
|
||||
spiffs_list_files();
|
||||
|
||||
if (!config_store_load(DEVICE_CONFIG_PATH, &g_device_cfg))
|
||||
{
|
||||
ESP_LOGE(TAG, "Failed to load device config: %s", DEVICE_CONFIG_PATH);
|
||||
return;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "Modbus Port: %u", (unsigned)g_device_cfg.modbus.port);
|
||||
ESP_LOGI(TAG, "Configured device_count = %u", (unsigned)g_device_cfg.modbus.device_count);
|
||||
|
||||
for (i = 0; i < g_device_cfg.modbus.device_count; i++)
|
||||
{
|
||||
ESP_LOGI(TAG,
|
||||
"CFG device[%u]: enabled=%u unit_id=%u name=%s csv=%s",
|
||||
(unsigned)i,
|
||||
g_device_cfg.devices[i].enabled ? 1 : 0,
|
||||
(unsigned)g_device_cfg.devices[i].unit_id,
|
||||
g_device_cfg.devices[i].name,
|
||||
g_device_cfg.devices[i].csv);
|
||||
}
|
||||
|
||||
if (g_device_cfg.modbus.device_count == 0U)
|
||||
{
|
||||
ESP_LOGE(TAG, "No Modbus devices configured");
|
||||
return;
|
||||
}
|
||||
|
||||
if (g_device_cfg.modbus.device_count > MAX_VIRTUAL_DEVICES)
|
||||
{
|
||||
ESP_LOGE(TAG, "Too many virtual devices (%u)", (unsigned)g_device_cfg.modbus.device_count);
|
||||
return;
|
||||
}
|
||||
|
||||
memset(g_devices, 0, sizeof(g_devices));
|
||||
g_device_count = 0U;
|
||||
|
||||
for (i = 0; i < g_device_cfg.modbus.device_count; i++)
|
||||
{
|
||||
modbus_device_t *dev;
|
||||
const virtual_device_settings_t *cfg_dev = &g_device_cfg.devices[i];
|
||||
|
||||
if (!cfg_dev->enabled)
|
||||
{
|
||||
ESP_LOGI(TAG, "Skipping disabled device[%u]", (unsigned)i);
|
||||
continue;
|
||||
}
|
||||
|
||||
dev = &g_devices[g_device_count];
|
||||
|
||||
dev->unit_id = cfg_dev->unit_id;
|
||||
|
||||
strncpy(dev->name, cfg_dev->name, sizeof(dev->name) - 1U);
|
||||
dev->name[sizeof(dev->name) - 1U] = '\0';
|
||||
|
||||
snprintf(dev->csv_path,
|
||||
sizeof(dev->csv_path),
|
||||
"/spiffs/%s",
|
||||
cfg_dev->csv);
|
||||
|
||||
ESP_LOGI(TAG,
|
||||
"Preparing device[%u]: unit_id=%u name=%s csv=%s",
|
||||
(unsigned)i,
|
||||
(unsigned)dev->unit_id,
|
||||
dev->name,
|
||||
dev->csv_path);
|
||||
|
||||
if (!file_exists(dev->csv_path))
|
||||
{
|
||||
ESP_LOGE(TAG, "CSV file not found for device %u: %s",
|
||||
(unsigned)dev->unit_id,
|
||||
dev->csv_path);
|
||||
continue;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "CSV file found for device %u: %s",
|
||||
(unsigned)dev->unit_id,
|
||||
dev->csv_path);
|
||||
|
||||
modbus_points_init(&dev->db);
|
||||
|
||||
if (!modbus_points_load(&dev->db, dev->csv_path))
|
||||
{
|
||||
ESP_LOGE(TAG, "Failed to load CSV for device %u: %s",
|
||||
(unsigned)dev->unit_id,
|
||||
dev->csv_path);
|
||||
continue;
|
||||
}
|
||||
|
||||
/* Safety check: prevent memory overflow */
|
||||
if (dev->db.count > MODBUS_MAX_POINTS)
|
||||
{
|
||||
ESP_LOGE(TAG,
|
||||
"Device %u exceeded MODBUS_MAX_POINTS (%u > %u)",
|
||||
(unsigned)dev->unit_id,
|
||||
(unsigned)dev->db.count,
|
||||
(unsigned)MODBUS_MAX_POINTS);
|
||||
|
||||
ESP_LOGE(TAG, "Startup aborted to prevent memory corruption.");
|
||||
|
||||
abort();
|
||||
}
|
||||
|
||||
if (!modbus_memory_init(&dev->mem, &dev->db))
|
||||
{
|
||||
ESP_LOGE(TAG, "Memory init failed for device %u",
|
||||
(unsigned)dev->unit_id);
|
||||
continue;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "Device %u (%s) loaded %u Modbus points from %s",
|
||||
(unsigned)dev->unit_id,
|
||||
dev->name,
|
||||
(unsigned)dev->db.count,
|
||||
dev->csv_path);
|
||||
|
||||
g_device_count++;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "----- Runtime Modbus Device Table -----");
|
||||
|
||||
for (i = 0; i < g_device_count; i++)
|
||||
{
|
||||
ESP_LOGI(TAG,
|
||||
"RUNTIME device[%u]: unit_id=%u name=%s csv=%s points=%u",
|
||||
(unsigned)i,
|
||||
(unsigned)g_devices[i].unit_id,
|
||||
g_devices[i].name,
|
||||
g_devices[i].csv_path,
|
||||
(unsigned)g_devices[i].db.count);
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "---------------------------------------");
|
||||
|
||||
ESP_LOGI(TAG, "----- Modbus Point Capacity -----");
|
||||
|
||||
{
|
||||
size_t total_points_used = 0;
|
||||
size_t total_points_capacity = g_device_count * MODBUS_MAX_POINTS;
|
||||
size_t total_points_remaining = 0;
|
||||
|
||||
size_t free_heap = heap_caps_get_free_size(MALLOC_CAP_8BIT);
|
||||
size_t largest_block = heap_caps_get_largest_free_block(MALLOC_CAP_8BIT);
|
||||
|
||||
size_t point_struct_size = sizeof(modbus_point_t);
|
||||
size_t heap_equivalent_points = 0;
|
||||
|
||||
if (point_struct_size > 0U)
|
||||
heap_equivalent_points = free_heap / point_struct_size;
|
||||
|
||||
for (i = 0; i < g_device_count; i++)
|
||||
{
|
||||
const modbus_device_t *dev = &g_devices[i];
|
||||
size_t remaining = 0;
|
||||
float pct = 0.0f;
|
||||
|
||||
if (dev->db.count < MODBUS_MAX_POINTS)
|
||||
remaining = MODBUS_MAX_POINTS - dev->db.count;
|
||||
|
||||
if (MODBUS_MAX_POINTS > 0U)
|
||||
pct = (100.0f * (float)dev->db.count) / (float)MODBUS_MAX_POINTS;
|
||||
|
||||
ESP_LOGI(TAG,
|
||||
"Device %u (%s): used=%u / max=%u, remaining=%u (%.1f%% used)",
|
||||
(unsigned)dev->unit_id,
|
||||
dev->name,
|
||||
(unsigned)dev->db.count,
|
||||
(unsigned)MODBUS_MAX_POINTS,
|
||||
(unsigned)remaining,
|
||||
pct);
|
||||
|
||||
total_points_used += dev->db.count;
|
||||
total_points_remaining += remaining;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG,
|
||||
"Configured capacity: used=%u / max=%u, remaining=%u",
|
||||
(unsigned)total_points_used,
|
||||
(unsigned)total_points_capacity,
|
||||
(unsigned)total_points_remaining);
|
||||
|
||||
ESP_LOGI(TAG,
|
||||
"Free heap: %u bytes, largest block: %u bytes, point size: %u bytes",
|
||||
(unsigned)free_heap,
|
||||
(unsigned)largest_block,
|
||||
(unsigned)point_struct_size);
|
||||
|
||||
ESP_LOGI(TAG,
|
||||
"Heap-equivalent points available (informational only): %u",
|
||||
(unsigned)heap_equivalent_points);
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "---------------------------------");
|
||||
|
||||
ESP_LOGI(TAG, "----- Point Cost Guide -----");
|
||||
ESP_LOGI(TAG, "Internal point usage (counts against MODBUS_MAX_POINTS):");
|
||||
ESP_LOGI(TAG, " data_type=bool -> 1 point");
|
||||
ESP_LOGI(TAG, " data_type=uint16 -> 1 point");
|
||||
ESP_LOGI(TAG, " data_type=uint32 -> 2 points");
|
||||
ESP_LOGI(TAG, " data_type=float -> 2 points");
|
||||
ESP_LOGI(TAG, " data_type=double -> 4 points");
|
||||
|
||||
ESP_LOGI(TAG, "PICS staging usage (for pics_address spacing only):");
|
||||
ESP_LOGI(TAG, " pics_data_type=Boolean -> 1 register");
|
||||
ESP_LOGI(TAG, " pics_data_type=Integer -> 2 registers");
|
||||
ESP_LOGI(TAG, " pics_data_type=Float -> 4 registers");
|
||||
ESP_LOGI(TAG, " pics_data_type=None -> 0 registers");
|
||||
|
||||
ESP_LOGI(TAG, "Planning examples:");
|
||||
ESP_LOGI(TAG, " 100 uint16 CSV rows -> about 100 internal points");
|
||||
ESP_LOGI(TAG, " 100 float CSV rows -> about 200 internal points");
|
||||
ESP_LOGI(TAG, " 100 double CSV rows -> about 400 internal points");
|
||||
ESP_LOGI(TAG, "--------------------------------");
|
||||
|
||||
ESP_LOGI(TAG, "----- Device Type Mix -----");
|
||||
|
||||
for (i = 0; i < g_device_count; i++)
|
||||
{
|
||||
print_device_type_mix(&g_devices[i]);
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG, "---------------------------");
|
||||
|
||||
ESP_LOGI(TAG, "----- Modbus Memory Usage -----");
|
||||
|
||||
size_t total_points = 0;
|
||||
|
||||
for (size_t d = 0; d < g_device_count; d++)
|
||||
{
|
||||
const modbus_device_t *dev = &g_devices[d];
|
||||
|
||||
ESP_LOGI(TAG,
|
||||
"Device %u (%s): %u / %u points used (%.1f%%)",
|
||||
(unsigned)dev->unit_id,
|
||||
dev->name,
|
||||
(unsigned)dev->db.count,
|
||||
(unsigned)MODBUS_MAX_POINTS,
|
||||
(100.0f * (float)dev->db.count) / (float)MODBUS_MAX_POINTS);
|
||||
|
||||
total_points += dev->db.count;
|
||||
}
|
||||
|
||||
ESP_LOGI(TAG,
|
||||
"TOTAL POINTS LOADED: %u",
|
||||
(unsigned)total_points);
|
||||
|
||||
ESP_LOGI(TAG, "--------------------------------");
|
||||
|
||||
if (g_device_count == 0U)
|
||||
{
|
||||
ESP_LOGE(TAG, "No valid Modbus devices loaded, not starting server");
|
||||
return;
|
||||
}
|
||||
|
||||
ESP_ERROR_CHECK(wifi_manager_init());
|
||||
ESP_ERROR_CHECK(wifi_manager_start_sta(&g_device_cfg.wifi));
|
||||
|
||||
if (!wifi_manager_wait_connected(pdMS_TO_TICKS(15000)))
|
||||
{
|
||||
ESP_LOGW(TAG, "Wi-Fi not connected within timeout, server will still start");
|
||||
}
|
||||
|
||||
xTaskCreate(modbus_server_task, "modbus_server_task", 8192, NULL, 5, NULL);
|
||||
xTaskCreate(modbus_override_task, "modbus_override_task", 4096, NULL, 5, NULL);
|
||||
}
|
||||
877
main/modbus_memory.c
Normal file
877
main/modbus_memory.c
Normal file
@@ -0,0 +1,877 @@
|
||||
#include "modbus_memory.h"
|
||||
#include "modbus_points.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
static modbus_point_type_t mem_to_point_type(modbus_mem_type_t type)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case MB_MEM_COIL:
|
||||
return MB_POINT_COIL;
|
||||
|
||||
case MB_MEM_DISCRETE_INPUT:
|
||||
return MB_POINT_DISCRETE_INPUT;
|
||||
|
||||
case MB_MEM_INPUT_REGISTER:
|
||||
return MB_POINT_INPUT_REGISTER;
|
||||
|
||||
case MB_MEM_HOLDING_REGISTER:
|
||||
return MB_POINT_HOLDING_REGISTER;
|
||||
|
||||
default:
|
||||
return MB_POINT_INVALID;
|
||||
}
|
||||
}
|
||||
|
||||
static modbus_point_t *find_point_rw(modbus_memory_t *mem,
|
||||
modbus_mem_type_t mem_type,
|
||||
uint16_t offset)
|
||||
{
|
||||
size_t i;
|
||||
modbus_point_type_t point_type;
|
||||
|
||||
if (mem == NULL || mem->db == NULL)
|
||||
return NULL;
|
||||
|
||||
point_type = mem_to_point_type(mem_type);
|
||||
if (point_type == MB_POINT_INVALID)
|
||||
return NULL;
|
||||
|
||||
for (i = 0; i < mem->db->count; i++)
|
||||
{
|
||||
if (mem->db->points[i].type == point_type &&
|
||||
mem->db->points[i].offset == offset)
|
||||
{
|
||||
return &mem->db->points[i];
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static const modbus_point_t *find_point_ro(modbus_memory_t *mem,
|
||||
modbus_mem_type_t mem_type,
|
||||
uint16_t offset)
|
||||
{
|
||||
size_t i;
|
||||
modbus_point_type_t point_type;
|
||||
|
||||
if (mem == NULL || mem->db == NULL)
|
||||
return NULL;
|
||||
|
||||
point_type = mem_to_point_type(mem_type);
|
||||
if (point_type == MB_POINT_INVALID)
|
||||
return NULL;
|
||||
|
||||
for (i = 0; i < mem->db->count; i++)
|
||||
{
|
||||
if (mem->db->points[i].type == point_type &&
|
||||
mem->db->points[i].offset == offset)
|
||||
{
|
||||
return &mem->db->points[i];
|
||||
}
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static modbus_point_t *find_pics_point_rw(modbus_memory_t *mem,
|
||||
modbus_mem_type_t mem_type,
|
||||
uint16_t offset)
|
||||
{
|
||||
modbus_point_type_t point_type;
|
||||
const modbus_point_t *pt;
|
||||
|
||||
if (mem == NULL || mem->db == NULL)
|
||||
return NULL;
|
||||
|
||||
point_type = mem_to_point_type(mem_type);
|
||||
if (point_type == MB_POINT_INVALID)
|
||||
return NULL;
|
||||
|
||||
pt = modbus_points_find_by_pics_range(mem->db, point_type, offset);
|
||||
return (modbus_point_t *)pt;
|
||||
}
|
||||
|
||||
static bool write_point_words(modbus_memory_t *mem,
|
||||
modbus_point_t *pt,
|
||||
const uint16_t *words,
|
||||
uint8_t span)
|
||||
{
|
||||
uint8_t i;
|
||||
modbus_mem_type_t mem_type;
|
||||
|
||||
if (mem == NULL || pt == NULL || words == NULL)
|
||||
return false;
|
||||
|
||||
if (modbus_points_is_bit_type(pt->type))
|
||||
{
|
||||
pt->bit_value = (words[0] != 0U) ? 1U : 0U;
|
||||
return true;
|
||||
}
|
||||
|
||||
mem_type = modbus_points_type_to_mem(pt->type);
|
||||
|
||||
for (i = 0; i < span; i++)
|
||||
{
|
||||
modbus_point_t *word_pt;
|
||||
|
||||
word_pt = find_point_rw(mem, mem_type, (uint16_t)(pt->offset + i));
|
||||
if (word_pt == NULL)
|
||||
return false;
|
||||
|
||||
word_pt->reg_value = words[i];
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static void sync_points_controlled_by(modbus_memory_t *mem,
|
||||
const modbus_point_t *control_pt)
|
||||
{
|
||||
size_t i;
|
||||
uint8_t j;
|
||||
modbus_mem_type_t control_mem_type;
|
||||
modbus_mem_type_t target_mem_type;
|
||||
|
||||
if (mem == NULL || mem->db == NULL || control_pt == NULL)
|
||||
return;
|
||||
|
||||
for (i = 0; i < mem->db->count; i++)
|
||||
{
|
||||
modbus_point_t *target_pt = &mem->db->points[i];
|
||||
|
||||
if (!target_pt->has_control_address)
|
||||
continue;
|
||||
|
||||
if (target_pt->control_type != control_pt->type)
|
||||
continue;
|
||||
|
||||
if (target_pt->control_offset != control_pt->offset)
|
||||
continue;
|
||||
|
||||
if (modbus_points_is_bit_type(target_pt->type))
|
||||
{
|
||||
target_pt->bit_value = control_pt->bit_value;
|
||||
continue;
|
||||
}
|
||||
|
||||
control_mem_type = modbus_points_type_to_mem(control_pt->type);
|
||||
target_mem_type = modbus_points_type_to_mem(target_pt->type);
|
||||
|
||||
for (j = 0; j < target_pt->reg_span; j++)
|
||||
{
|
||||
const modbus_point_t *src_pt;
|
||||
modbus_point_t *dst_pt;
|
||||
|
||||
src_pt = find_point_ro(mem, control_mem_type,
|
||||
(uint16_t)(control_pt->offset + j));
|
||||
dst_pt = find_point_rw(mem, target_mem_type,
|
||||
(uint16_t)(target_pt->offset + j));
|
||||
|
||||
if (src_pt == NULL || dst_pt == NULL)
|
||||
break;
|
||||
|
||||
dst_pt->reg_value = src_pt->reg_value;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static bool apply_pics_to_official_point(modbus_memory_t *mem, modbus_point_t *pt)
|
||||
{
|
||||
uint16_t out_words[4] = {0U, 0U, 0U, 0U};
|
||||
|
||||
if (mem == NULL || pt == NULL || !pt->has_pics_address)
|
||||
return false;
|
||||
|
||||
switch (pt->pics_data_type)
|
||||
{
|
||||
case PICS_DATA_BOOLEAN:
|
||||
{
|
||||
uint16_t v = (pt->pics_words[0] != 0U) ? 1U : 0U;
|
||||
|
||||
if (modbus_points_is_bit_type(pt->type))
|
||||
{
|
||||
pt->bit_value = (v != 0U) ? 1U : 0U;
|
||||
sync_points_controlled_by(mem, pt);
|
||||
return true;
|
||||
}
|
||||
|
||||
out_words[0] = v;
|
||||
if (!write_point_words(mem, pt, out_words, 1U))
|
||||
return false;
|
||||
|
||||
sync_points_controlled_by(mem, pt);
|
||||
return true;
|
||||
}
|
||||
|
||||
case PICS_DATA_INTEGER:
|
||||
{
|
||||
uint32_t raw_u32;
|
||||
int32_t pics_i32;
|
||||
|
||||
raw_u32 = ((uint32_t)pt->pics_words[1] << 16) |
|
||||
(uint32_t)pt->pics_words[0];
|
||||
pics_i32 = (int32_t)raw_u32;
|
||||
|
||||
switch (pt->data_type)
|
||||
{
|
||||
case MB_DATA_UINT16:
|
||||
{
|
||||
uint32_t v = (pics_i32 < 0) ? 0U : (uint32_t)pics_i32;
|
||||
if (v > 65535U)
|
||||
v = 65535U;
|
||||
|
||||
out_words[0] = (uint16_t)v;
|
||||
if (!write_point_words(mem, pt, out_words, 1U))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
|
||||
case MB_DATA_UINT32:
|
||||
{
|
||||
uint32_t v = (pics_i32 < 0) ? 0U : (uint32_t)pics_i32;
|
||||
|
||||
out_words[0] = (uint16_t)((v >> 16) & 0xFFFFU);
|
||||
out_words[1] = (uint16_t)(v & 0xFFFFU);
|
||||
|
||||
if (!write_point_words(mem, pt, out_words, 2U))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
|
||||
case MB_DATA_FLOAT:
|
||||
{
|
||||
float f = (float)pics_i32;
|
||||
uint32_t raw_f32;
|
||||
|
||||
memcpy(&raw_f32, &f, sizeof(raw_f32));
|
||||
|
||||
out_words[0] = (uint16_t)((raw_f32 >> 16) & 0xFFFFU);
|
||||
out_words[1] = (uint16_t)(raw_f32 & 0xFFFFU);
|
||||
|
||||
if (!write_point_words(mem, pt, out_words, 2U))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
|
||||
case MB_DATA_DOUBLE:
|
||||
{
|
||||
double d = (double)pics_i32;
|
||||
uint64_t raw_f64;
|
||||
|
||||
memcpy(&raw_f64, &d, sizeof(raw_f64));
|
||||
|
||||
out_words[0] = (uint16_t)((raw_f64 >> 48) & 0xFFFFU);
|
||||
out_words[1] = (uint16_t)((raw_f64 >> 32) & 0xFFFFU);
|
||||
out_words[2] = (uint16_t)((raw_f64 >> 16) & 0xFFFFU);
|
||||
out_words[3] = (uint16_t)(raw_f64 & 0xFFFFU);
|
||||
|
||||
if (!write_point_words(mem, pt, out_words, 4U))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
||||
sync_points_controlled_by(mem, pt);
|
||||
return true;
|
||||
}
|
||||
|
||||
case PICS_DATA_FLOAT:
|
||||
{
|
||||
uint64_t raw_f64;
|
||||
double pics_d;
|
||||
|
||||
raw_f64 = ((uint64_t)pt->pics_words[0] << 48) |
|
||||
((uint64_t)pt->pics_words[1] << 32) |
|
||||
((uint64_t)pt->pics_words[2] << 16) |
|
||||
(uint64_t)pt->pics_words[3];
|
||||
|
||||
memcpy(&pics_d, &raw_f64, sizeof(pics_d));
|
||||
|
||||
switch (pt->data_type)
|
||||
{
|
||||
case MB_DATA_UINT16:
|
||||
{
|
||||
double v = pics_d;
|
||||
if (v < 0.0)
|
||||
v = 0.0;
|
||||
if (v > 65535.0)
|
||||
v = 65535.0;
|
||||
|
||||
out_words[0] = (uint16_t)v;
|
||||
if (!write_point_words(mem, pt, out_words, 1U))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
|
||||
case MB_DATA_UINT32:
|
||||
{
|
||||
double v = pics_d;
|
||||
uint32_t u32;
|
||||
|
||||
if (v < 0.0)
|
||||
v = 0.0;
|
||||
if (v > 4294967295.0)
|
||||
v = 4294967295.0;
|
||||
|
||||
u32 = (uint32_t)v;
|
||||
|
||||
out_words[0] = (uint16_t)((u32 >> 16) & 0xFFFFU);
|
||||
out_words[1] = (uint16_t)(u32 & 0xFFFFU);
|
||||
|
||||
if (!write_point_words(mem, pt, out_words, 2U))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
|
||||
case MB_DATA_FLOAT:
|
||||
{
|
||||
float f = (float)pics_d;
|
||||
uint32_t raw_f32;
|
||||
|
||||
memcpy(&raw_f32, &f, sizeof(raw_f32));
|
||||
|
||||
out_words[0] = (uint16_t)((raw_f32 >> 16) & 0xFFFFU);
|
||||
out_words[1] = (uint16_t)(raw_f32 & 0xFFFFU);
|
||||
|
||||
if (!write_point_words(mem, pt, out_words, 2U))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
|
||||
case MB_DATA_DOUBLE:
|
||||
{
|
||||
uint64_t raw_out_f64;
|
||||
|
||||
memcpy(&raw_out_f64, &pics_d, sizeof(raw_out_f64));
|
||||
|
||||
out_words[0] = (uint16_t)((raw_out_f64 >> 48) & 0xFFFFU);
|
||||
out_words[1] = (uint16_t)((raw_out_f64 >> 32) & 0xFFFFU);
|
||||
out_words[2] = (uint16_t)((raw_out_f64 >> 16) & 0xFFFFU);
|
||||
out_words[3] = (uint16_t)(raw_out_f64 & 0xFFFFU);
|
||||
|
||||
if (!write_point_words(mem, pt, out_words, 4U))
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
||||
sync_points_controlled_by(mem, pt);
|
||||
return true;
|
||||
}
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
static bool write_pics_staging_word(modbus_memory_t *mem,
|
||||
modbus_mem_type_t mem_type,
|
||||
uint16_t offset,
|
||||
uint16_t value)
|
||||
{
|
||||
modbus_point_t *pt;
|
||||
uint16_t word_index;
|
||||
|
||||
pt = find_pics_point_rw(mem, mem_type, offset);
|
||||
if (pt == NULL)
|
||||
return false;
|
||||
|
||||
if (!pt->has_pics_address || pt->pics_reg_span == 0U)
|
||||
return false;
|
||||
|
||||
if (offset < pt->pics_offset)
|
||||
return false;
|
||||
|
||||
word_index = (uint16_t)(offset - pt->pics_offset);
|
||||
if (word_index >= pt->pics_reg_span || word_index >= 4U)
|
||||
return false;
|
||||
|
||||
pt->pics_words[word_index] = value;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_init(modbus_memory_t *mem, modbus_db_t *db)
|
||||
{
|
||||
if (mem == NULL || db == NULL)
|
||||
return false;
|
||||
|
||||
mem->db = db;
|
||||
modbus_memory_reset(mem);
|
||||
return true;
|
||||
}
|
||||
|
||||
void modbus_memory_deinit(modbus_memory_t *mem)
|
||||
{
|
||||
if (mem == NULL)
|
||||
return;
|
||||
|
||||
mem->db = NULL;
|
||||
}
|
||||
|
||||
void modbus_memory_reset(modbus_memory_t *mem)
|
||||
{
|
||||
size_t i;
|
||||
|
||||
if (mem == NULL || mem->db == NULL)
|
||||
return;
|
||||
|
||||
for (i = 0; i < mem->db->count; i++)
|
||||
{
|
||||
mem->db->points[i].bit_value = 0U;
|
||||
mem->db->points[i].reg_value = 0U;
|
||||
memset(mem->db->points[i].pics_words, 0, sizeof(mem->db->points[i].pics_words));
|
||||
}
|
||||
}
|
||||
|
||||
bool modbus_memory_read_coil(modbus_memory_t *mem, uint16_t offset, uint8_t *value)
|
||||
{
|
||||
const modbus_point_t *pt;
|
||||
|
||||
if (mem == NULL || value == NULL)
|
||||
return false;
|
||||
|
||||
pt = find_point_ro(mem, MB_MEM_COIL, offset);
|
||||
if (pt == NULL)
|
||||
return false;
|
||||
|
||||
*value = (pt->bit_value != 0U) ? 1U : 0U;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_write_coil(modbus_memory_t *mem, uint16_t offset, uint8_t value)
|
||||
{
|
||||
modbus_point_t *pt;
|
||||
|
||||
if (mem == NULL)
|
||||
return false;
|
||||
|
||||
pt = find_point_rw(mem, MB_MEM_COIL, offset);
|
||||
if (pt != NULL)
|
||||
{
|
||||
pt->bit_value = (value != 0U) ? 1U : 0U;
|
||||
sync_points_controlled_by(mem, pt);
|
||||
return true;
|
||||
}
|
||||
|
||||
pt = find_pics_point_rw(mem, MB_MEM_COIL, offset);
|
||||
if (pt != NULL)
|
||||
{
|
||||
uint16_t staged = (value != 0U) ? 1U : 0U;
|
||||
return write_pics_staging_word(mem, MB_MEM_COIL, offset, staged);
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool modbus_memory_read_discrete_input(modbus_memory_t *mem, uint16_t offset, uint8_t *value)
|
||||
{
|
||||
const modbus_point_t *pt;
|
||||
|
||||
if (mem == NULL || value == NULL)
|
||||
return false;
|
||||
|
||||
pt = find_point_ro(mem, MB_MEM_DISCRETE_INPUT, offset);
|
||||
if (pt == NULL)
|
||||
return false;
|
||||
|
||||
*value = (pt->bit_value != 0U) ? 1U : 0U;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_write_discrete_input(modbus_memory_t *mem, uint16_t offset, uint8_t value)
|
||||
{
|
||||
modbus_point_t *pt;
|
||||
|
||||
if (mem == NULL)
|
||||
return false;
|
||||
|
||||
pt = find_point_rw(mem, MB_MEM_DISCRETE_INPUT, offset);
|
||||
if (pt == NULL)
|
||||
return false;
|
||||
|
||||
pt->bit_value = (value != 0U) ? 1U : 0U;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_read_input_register(modbus_memory_t *mem, uint16_t offset, uint16_t *value)
|
||||
{
|
||||
const modbus_point_t *pt;
|
||||
|
||||
if (mem == NULL || value == NULL)
|
||||
return false;
|
||||
|
||||
pt = find_point_ro(mem, MB_MEM_INPUT_REGISTER, offset);
|
||||
if (pt == NULL)
|
||||
return false;
|
||||
|
||||
*value = pt->reg_value;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_write_input_register(modbus_memory_t *mem, uint16_t offset, uint16_t value)
|
||||
{
|
||||
modbus_point_t *pt;
|
||||
|
||||
if (mem == NULL)
|
||||
return false;
|
||||
|
||||
pt = find_point_rw(mem, MB_MEM_INPUT_REGISTER, offset);
|
||||
if (pt == NULL)
|
||||
return false;
|
||||
|
||||
pt->reg_value = value;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_read_holding_register(modbus_memory_t *mem, uint16_t offset, uint16_t *value)
|
||||
{
|
||||
const modbus_point_t *pt;
|
||||
|
||||
if (mem == NULL || value == NULL)
|
||||
return false;
|
||||
|
||||
pt = find_point_ro(mem, MB_MEM_HOLDING_REGISTER, offset);
|
||||
if (pt == NULL)
|
||||
return false;
|
||||
|
||||
*value = pt->reg_value;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_write_holding_register(modbus_memory_t *mem, uint16_t offset, uint16_t value)
|
||||
{
|
||||
modbus_point_t *pt;
|
||||
|
||||
if (mem == NULL)
|
||||
return false;
|
||||
|
||||
pt = find_point_rw(mem, MB_MEM_HOLDING_REGISTER, offset);
|
||||
if (pt != NULL)
|
||||
{
|
||||
pt->reg_value = value;
|
||||
sync_points_controlled_by(mem, pt);
|
||||
return true;
|
||||
}
|
||||
|
||||
if (write_pics_staging_word(mem, MB_MEM_HOLDING_REGISTER, offset, value))
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool modbus_memory_read_bit(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t offset,
|
||||
uint8_t *value)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case MB_MEM_COIL:
|
||||
return modbus_memory_read_coil(mem, offset, value);
|
||||
|
||||
case MB_MEM_DISCRETE_INPUT:
|
||||
return modbus_memory_read_discrete_input(mem, offset, value);
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool modbus_memory_write_bit(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t offset,
|
||||
uint8_t value)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case MB_MEM_COIL:
|
||||
return modbus_memory_write_coil(mem, offset, value);
|
||||
|
||||
case MB_MEM_DISCRETE_INPUT:
|
||||
return modbus_memory_write_discrete_input(mem, offset, value);
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool modbus_memory_read_reg(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t offset,
|
||||
uint16_t *value)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case MB_MEM_INPUT_REGISTER:
|
||||
return modbus_memory_read_input_register(mem, offset, value);
|
||||
|
||||
case MB_MEM_HOLDING_REGISTER:
|
||||
return modbus_memory_read_holding_register(mem, offset, value);
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool modbus_memory_write_reg(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t offset,
|
||||
uint16_t value)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case MB_MEM_INPUT_REGISTER:
|
||||
return modbus_memory_write_input_register(mem, offset, value);
|
||||
|
||||
case MB_MEM_HOLDING_REGISTER:
|
||||
return modbus_memory_write_holding_register(mem, offset, value);
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool modbus_memory_valid_bit_range(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity)
|
||||
{
|
||||
uint32_t end_offset;
|
||||
|
||||
(void)type;
|
||||
|
||||
if (mem == NULL || mem->db == NULL || quantity == 0U)
|
||||
return false;
|
||||
|
||||
end_offset = (uint32_t)start_offset + (uint32_t)quantity - 1U;
|
||||
if (end_offset > 0xFFFFU)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_valid_reg_range(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity)
|
||||
{
|
||||
uint32_t end_offset;
|
||||
|
||||
(void)type;
|
||||
|
||||
if (mem == NULL || mem->db == NULL || quantity == 0U)
|
||||
return false;
|
||||
|
||||
end_offset = (uint32_t)start_offset + (uint32_t)quantity - 1U;
|
||||
if (end_offset > 0xFFFFU)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_read_coils(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
uint8_t *dest)
|
||||
{
|
||||
uint16_t i;
|
||||
|
||||
if (dest == NULL || !modbus_memory_valid_bit_range(mem, MB_MEM_COIL, start_offset, quantity))
|
||||
return false;
|
||||
|
||||
for (i = 0; i < quantity; i++)
|
||||
{
|
||||
uint8_t value = 0U;
|
||||
|
||||
if (!modbus_memory_read_coil(mem, (uint16_t)(start_offset + i), &value))
|
||||
value = 0U;
|
||||
|
||||
dest[i] = value;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_write_coils(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
const uint8_t *src)
|
||||
{
|
||||
uint16_t i;
|
||||
|
||||
if (src == NULL || !modbus_memory_valid_bit_range(mem, MB_MEM_COIL, start_offset, quantity))
|
||||
return false;
|
||||
|
||||
for (i = 0; i < quantity; i++)
|
||||
{
|
||||
if (!modbus_memory_write_coil(mem, (uint16_t)(start_offset + i), src[i]))
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_read_discrete_inputs(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
uint8_t *dest)
|
||||
{
|
||||
uint16_t i;
|
||||
|
||||
if (dest == NULL || !modbus_memory_valid_bit_range(mem, MB_MEM_DISCRETE_INPUT, start_offset, quantity))
|
||||
return false;
|
||||
|
||||
for (i = 0; i < quantity; i++)
|
||||
{
|
||||
uint8_t value = 0U;
|
||||
|
||||
if (!modbus_memory_read_discrete_input(mem, (uint16_t)(start_offset + i), &value))
|
||||
value = 0U;
|
||||
|
||||
dest[i] = value;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_write_discrete_inputs(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
const uint8_t *src)
|
||||
{
|
||||
uint16_t i;
|
||||
|
||||
if (src == NULL || !modbus_memory_valid_bit_range(mem, MB_MEM_DISCRETE_INPUT, start_offset, quantity))
|
||||
return false;
|
||||
|
||||
for (i = 0; i < quantity; i++)
|
||||
{
|
||||
if (!modbus_memory_write_discrete_input(mem, (uint16_t)(start_offset + i), src[i]))
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_read_input_registers(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
uint16_t *dest)
|
||||
{
|
||||
uint16_t i;
|
||||
|
||||
if (dest == NULL || !modbus_memory_valid_reg_range(mem, MB_MEM_INPUT_REGISTER, start_offset, quantity))
|
||||
return false;
|
||||
|
||||
for (i = 0; i < quantity; i++)
|
||||
{
|
||||
uint16_t value = 0U;
|
||||
|
||||
if (!modbus_memory_read_input_register(mem, (uint16_t)(start_offset + i), &value))
|
||||
value = 0U;
|
||||
|
||||
dest[i] = value;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_write_input_registers(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
const uint16_t *src)
|
||||
{
|
||||
uint16_t i;
|
||||
|
||||
if (src == NULL || !modbus_memory_valid_reg_range(mem, MB_MEM_INPUT_REGISTER, start_offset, quantity))
|
||||
return false;
|
||||
|
||||
for (i = 0; i < quantity; i++)
|
||||
{
|
||||
if (!modbus_memory_write_input_register(mem, (uint16_t)(start_offset + i), src[i]))
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_read_holding_registers(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
uint16_t *dest)
|
||||
{
|
||||
uint16_t i;
|
||||
|
||||
if (dest == NULL || !modbus_memory_valid_reg_range(mem, MB_MEM_HOLDING_REGISTER, start_offset, quantity))
|
||||
return false;
|
||||
|
||||
for (i = 0; i < quantity; i++)
|
||||
{
|
||||
uint16_t value = 0U;
|
||||
|
||||
if (!modbus_memory_read_holding_register(mem, (uint16_t)(start_offset + i), &value))
|
||||
value = 0U;
|
||||
|
||||
dest[i] = value;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_memory_write_holding_registers(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
const uint16_t *src)
|
||||
{
|
||||
uint16_t i;
|
||||
|
||||
if (src == NULL || quantity == 0U || mem == NULL || mem->db == NULL)
|
||||
return false;
|
||||
|
||||
for (i = 0; i < quantity; i++)
|
||||
{
|
||||
uint16_t off = (uint16_t)(start_offset + i);
|
||||
|
||||
if (find_point_rw(mem, MB_MEM_HOLDING_REGISTER, off) == NULL)
|
||||
{
|
||||
if (modbus_points_find_by_pics_range(mem->db,
|
||||
MB_POINT_HOLDING_REGISTER,
|
||||
off) == NULL)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (i = 0; i < quantity; i++)
|
||||
{
|
||||
if (!modbus_memory_write_holding_register(mem,
|
||||
(uint16_t)(start_offset + i),
|
||||
src[i]))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
modbus_point_t *pics_pt;
|
||||
|
||||
pics_pt = find_pics_point_rw(mem, MB_MEM_HOLDING_REGISTER, start_offset);
|
||||
if (pics_pt != NULL)
|
||||
{
|
||||
if (!apply_pics_to_official_point(mem, pics_pt))
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
131
main/modbus_memory.h
Normal file
131
main/modbus_memory.h
Normal file
@@ -0,0 +1,131 @@
|
||||
#ifndef MODBUS_MEMORY_H
|
||||
#define MODBUS_MEMORY_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MB_MEM_COIL = 0,
|
||||
MB_MEM_DISCRETE_INPUT,
|
||||
MB_MEM_INPUT_REGISTER,
|
||||
MB_MEM_HOLDING_REGISTER
|
||||
} modbus_mem_type_t;
|
||||
|
||||
typedef struct modbus_db modbus_db_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
modbus_db_t *db;
|
||||
} modbus_memory_t;
|
||||
|
||||
/* ---------------------------------------------------- */
|
||||
/* Init / deinit */
|
||||
/* ---------------------------------------------------- */
|
||||
|
||||
bool modbus_memory_init(modbus_memory_t *mem, modbus_db_t *db);
|
||||
void modbus_memory_deinit(modbus_memory_t *mem);
|
||||
|
||||
/* Clear all runtime point values back to 0 */
|
||||
void modbus_memory_reset(modbus_memory_t *mem);
|
||||
|
||||
/* ---------------------------------------------------- */
|
||||
/* Single-point access helpers */
|
||||
/* ---------------------------------------------------- */
|
||||
|
||||
bool modbus_memory_read_coil(modbus_memory_t *mem, uint16_t offset, uint8_t *value);
|
||||
bool modbus_memory_write_coil(modbus_memory_t *mem, uint16_t offset, uint8_t value);
|
||||
|
||||
bool modbus_memory_read_discrete_input(modbus_memory_t *mem, uint16_t offset, uint8_t *value);
|
||||
bool modbus_memory_write_discrete_input(modbus_memory_t *mem, uint16_t offset, uint8_t value);
|
||||
|
||||
bool modbus_memory_read_input_register(modbus_memory_t *mem, uint16_t offset, uint16_t *value);
|
||||
bool modbus_memory_write_input_register(modbus_memory_t *mem, uint16_t offset, uint16_t value);
|
||||
|
||||
bool modbus_memory_read_holding_register(modbus_memory_t *mem, uint16_t offset, uint16_t *value);
|
||||
bool modbus_memory_write_holding_register(modbus_memory_t *mem, uint16_t offset, uint16_t value);
|
||||
|
||||
/* ---------------------------------------------------- */
|
||||
/* Generic access helpers */
|
||||
/* ---------------------------------------------------- */
|
||||
|
||||
bool modbus_memory_read_bit(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t offset,
|
||||
uint8_t *value);
|
||||
|
||||
bool modbus_memory_write_bit(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t offset,
|
||||
uint8_t value);
|
||||
|
||||
bool modbus_memory_read_reg(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t offset,
|
||||
uint16_t *value);
|
||||
|
||||
bool modbus_memory_write_reg(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t offset,
|
||||
uint16_t value);
|
||||
|
||||
/* ---------------------------------------------------- */
|
||||
/* Multi-point access helpers */
|
||||
/* ---------------------------------------------------- */
|
||||
|
||||
bool modbus_memory_read_coils(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
uint8_t *dest);
|
||||
|
||||
bool modbus_memory_write_coils(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
const uint8_t *src);
|
||||
|
||||
bool modbus_memory_read_discrete_inputs(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
uint8_t *dest);
|
||||
|
||||
bool modbus_memory_write_discrete_inputs(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
const uint8_t *src);
|
||||
|
||||
bool modbus_memory_read_input_registers(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
uint16_t *dest);
|
||||
|
||||
bool modbus_memory_write_input_registers(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
const uint16_t *src);
|
||||
|
||||
bool modbus_memory_read_holding_registers(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
uint16_t *dest);
|
||||
|
||||
bool modbus_memory_write_holding_registers(modbus_memory_t *mem,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity,
|
||||
const uint16_t *src);
|
||||
|
||||
/* ---------------------------------------------------- */
|
||||
/* Utility helpers */
|
||||
/* ---------------------------------------------------- */
|
||||
|
||||
bool modbus_memory_valid_bit_range(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity);
|
||||
|
||||
bool modbus_memory_valid_reg_range(modbus_memory_t *mem,
|
||||
modbus_mem_type_t type,
|
||||
uint16_t start_offset,
|
||||
uint16_t quantity);
|
||||
|
||||
#endif /* MODBUS_MEMORY_H */
|
||||
864
main/modbus_points.c
Normal file
864
main/modbus_points.c
Normal file
@@ -0,0 +1,864 @@
|
||||
#include "modbus_points.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include <ctype.h>
|
||||
|
||||
/* ---------- local helpers ---------- */
|
||||
|
||||
static void trim_whitespace(char *s)
|
||||
{
|
||||
char *start;
|
||||
char *end;
|
||||
|
||||
if (s == NULL || *s == '\0')
|
||||
return;
|
||||
|
||||
start = s;
|
||||
while (*start != '\0' && isspace((unsigned char)*start))
|
||||
start++;
|
||||
|
||||
if (start != s)
|
||||
memmove(s, start, strlen(start) + 1U);
|
||||
|
||||
if (*s == '\0')
|
||||
return;
|
||||
|
||||
end = s + strlen(s) - 1;
|
||||
while (end >= s && isspace((unsigned char)*end))
|
||||
{
|
||||
*end = '\0';
|
||||
end--;
|
||||
}
|
||||
}
|
||||
|
||||
static bool str_equals(const char *a, const char *b)
|
||||
{
|
||||
if (a == NULL || b == NULL)
|
||||
return false;
|
||||
|
||||
return strcmp(a, b) == 0;
|
||||
}
|
||||
|
||||
static modbus_point_type_t parse_point_type(const char *s)
|
||||
{
|
||||
if (str_equals(s, "Coil"))
|
||||
return MB_POINT_COIL;
|
||||
if (str_equals(s, "Discrete_Input"))
|
||||
return MB_POINT_DISCRETE_INPUT;
|
||||
if (str_equals(s, "Input_Register"))
|
||||
return MB_POINT_INPUT_REGISTER;
|
||||
if (str_equals(s, "Holding_Register"))
|
||||
return MB_POINT_HOLDING_REGISTER;
|
||||
|
||||
return MB_POINT_INVALID;
|
||||
}
|
||||
|
||||
static modbus_data_type_t parse_data_type(const char *s)
|
||||
{
|
||||
if (str_equals(s, "bool"))
|
||||
return MB_DATA_BOOL;
|
||||
if (str_equals(s, "uint16"))
|
||||
return MB_DATA_UINT16;
|
||||
if (str_equals(s, "uint32"))
|
||||
return MB_DATA_UINT32;
|
||||
if (str_equals(s, "float"))
|
||||
return MB_DATA_FLOAT;
|
||||
if (str_equals(s, "double"))
|
||||
return MB_DATA_DOUBLE;
|
||||
|
||||
return MB_DATA_INVALID;
|
||||
}
|
||||
|
||||
static pics_data_type_t parse_pics_data_type(const char *s)
|
||||
{
|
||||
if (str_equals(s, "Boolean"))
|
||||
return PICS_DATA_BOOLEAN;
|
||||
|
||||
if (str_equals(s, "Integer"))
|
||||
return PICS_DATA_INTEGER;
|
||||
|
||||
if (str_equals(s, "Float"))
|
||||
return PICS_DATA_FLOAT;
|
||||
|
||||
if (str_equals(s, "String"))
|
||||
return PICS_DATA_STRING;
|
||||
|
||||
if (s == NULL || *s == '\0')
|
||||
return PICS_DATA_NONE;
|
||||
|
||||
return PICS_DATA_INVALID;
|
||||
}
|
||||
|
||||
static bool parse_u32(const char *s, uint32_t *value)
|
||||
{
|
||||
char *endptr;
|
||||
unsigned long v;
|
||||
|
||||
if (s == NULL || value == NULL || *s == '\0')
|
||||
return false;
|
||||
|
||||
v = strtoul(s, &endptr, 10);
|
||||
if (*endptr != '\0')
|
||||
return false;
|
||||
|
||||
*value = (uint32_t)v;
|
||||
return true;
|
||||
}
|
||||
|
||||
static modbus_point_type_t infer_point_type_from_address(uint32_t raw_address)
|
||||
{
|
||||
uint32_t family_digit;
|
||||
|
||||
if (raw_address == 0U)
|
||||
return MB_POINT_INVALID;
|
||||
|
||||
if (raw_address < 10000U)
|
||||
return MB_POINT_COIL;
|
||||
|
||||
if (raw_address >= 100000U)
|
||||
family_digit = raw_address / 100000U;
|
||||
else
|
||||
family_digit = raw_address / 10000U;
|
||||
|
||||
switch (family_digit)
|
||||
{
|
||||
case 1U:
|
||||
return MB_POINT_DISCRETE_INPUT;
|
||||
case 3U:
|
||||
return MB_POINT_INPUT_REGISTER;
|
||||
case 4U:
|
||||
return MB_POINT_HOLDING_REGISTER;
|
||||
default:
|
||||
return MB_POINT_INVALID;
|
||||
}
|
||||
}
|
||||
|
||||
static bool normalize_address(modbus_point_type_t type, uint32_t raw_address, uint16_t *offset)
|
||||
{
|
||||
uint32_t index_1_based;
|
||||
uint32_t off;
|
||||
uint32_t family_digit = 0U;
|
||||
|
||||
if (offset == NULL || raw_address == 0U)
|
||||
return false;
|
||||
|
||||
if (raw_address >= 100000U)
|
||||
{
|
||||
family_digit = raw_address / 100000U;
|
||||
index_1_based = raw_address % 100000U;
|
||||
}
|
||||
else if (raw_address >= 10000U)
|
||||
{
|
||||
family_digit = raw_address / 10000U;
|
||||
index_1_based = raw_address % 10000U;
|
||||
}
|
||||
else
|
||||
{
|
||||
family_digit = 0U;
|
||||
index_1_based = raw_address;
|
||||
}
|
||||
|
||||
if (index_1_based == 0U)
|
||||
return false;
|
||||
|
||||
switch (type)
|
||||
{
|
||||
case MB_POINT_COIL:
|
||||
if (family_digit != 0U)
|
||||
return false;
|
||||
off = index_1_based - 1U;
|
||||
break;
|
||||
|
||||
case MB_POINT_DISCRETE_INPUT:
|
||||
if (family_digit != 1U)
|
||||
return false;
|
||||
off = index_1_based - 1U;
|
||||
break;
|
||||
|
||||
case MB_POINT_INPUT_REGISTER:
|
||||
if (family_digit != 3U)
|
||||
return false;
|
||||
off = index_1_based - 1U;
|
||||
break;
|
||||
|
||||
case MB_POINT_HOLDING_REGISTER:
|
||||
if (family_digit != 4U)
|
||||
return false;
|
||||
off = index_1_based - 1U;
|
||||
break;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
|
||||
if (off > 0xFFFFU)
|
||||
return false;
|
||||
|
||||
*offset = (uint16_t)off;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool validate_type_and_datatype(modbus_point_type_t type, modbus_data_type_t data_type)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case MB_POINT_COIL:
|
||||
case MB_POINT_DISCRETE_INPUT:
|
||||
return data_type == MB_DATA_BOOL;
|
||||
|
||||
case MB_POINT_INPUT_REGISTER:
|
||||
case MB_POINT_HOLDING_REGISTER:
|
||||
return data_type == MB_DATA_UINT16 ||
|
||||
data_type == MB_DATA_UINT32 ||
|
||||
data_type == MB_DATA_FLOAT ||
|
||||
data_type == MB_DATA_DOUBLE;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
static uint8_t data_type_reg_span(modbus_data_type_t data_type)
|
||||
{
|
||||
switch (data_type)
|
||||
{
|
||||
case MB_DATA_BOOL:
|
||||
case MB_DATA_UINT16:
|
||||
return 1U;
|
||||
|
||||
case MB_DATA_UINT32:
|
||||
case MB_DATA_FLOAT:
|
||||
return 2U;
|
||||
|
||||
case MB_DATA_DOUBLE:
|
||||
return 4U;
|
||||
|
||||
default:
|
||||
return 0U;
|
||||
}
|
||||
}
|
||||
|
||||
static bool db_has_duplicate_name(const modbus_db_t *db, const char *name)
|
||||
{
|
||||
size_t i;
|
||||
|
||||
if (db == NULL || name == NULL)
|
||||
return false;
|
||||
|
||||
for (i = 0; i < db->count; i++)
|
||||
{
|
||||
if (strcmp(db->points[i].name, name) == 0)
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool db_has_duplicate_type_offset(const modbus_db_t *db,
|
||||
modbus_point_type_t type,
|
||||
uint16_t offset)
|
||||
{
|
||||
size_t i;
|
||||
|
||||
if (db == NULL)
|
||||
return false;
|
||||
|
||||
for (i = 0; i < db->count; i++)
|
||||
{
|
||||
if (db->points[i].type == type && db->points[i].offset == offset)
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool ranges_overlap(uint16_t start_a, uint8_t span_a,
|
||||
uint16_t start_b, uint8_t span_b)
|
||||
{
|
||||
uint32_t end_a;
|
||||
uint32_t end_b;
|
||||
|
||||
if (span_a == 0U || span_b == 0U)
|
||||
return false;
|
||||
|
||||
end_a = (uint32_t)start_a + (uint32_t)span_a - 1U;
|
||||
end_b = (uint32_t)start_b + (uint32_t)span_b - 1U;
|
||||
|
||||
return !((end_a < start_b) || (end_b < start_a));
|
||||
}
|
||||
|
||||
const modbus_point_t *modbus_points_find_by_pics_offset(const modbus_db_t *db,
|
||||
modbus_point_type_t type,
|
||||
uint16_t offset)
|
||||
{
|
||||
size_t i;
|
||||
|
||||
if (db == NULL)
|
||||
return NULL;
|
||||
|
||||
for (i = 0; i < db->count; i++)
|
||||
{
|
||||
if (db->points[i].has_pics_address &&
|
||||
db->points[i].pics_type == type &&
|
||||
db->points[i].pics_offset == offset)
|
||||
return &db->points[i];
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
const modbus_point_t *modbus_points_find_by_pics_range(const modbus_db_t *db,
|
||||
modbus_point_type_t type,
|
||||
uint16_t offset)
|
||||
{
|
||||
size_t i;
|
||||
|
||||
if (db == NULL)
|
||||
return NULL;
|
||||
|
||||
for (i = 0; i < db->count; i++)
|
||||
{
|
||||
const modbus_point_t *pt = &db->points[i];
|
||||
uint32_t end_offset;
|
||||
|
||||
if (!pt->has_pics_address || pt->pics_reg_span == 0U)
|
||||
continue;
|
||||
|
||||
if (pt->pics_type != type)
|
||||
continue;
|
||||
|
||||
end_offset = (uint32_t)pt->pics_offset + (uint32_t)pt->pics_reg_span - 1U;
|
||||
|
||||
if ((uint32_t)offset >= (uint32_t)pt->pics_offset &&
|
||||
(uint32_t)offset <= end_offset)
|
||||
return pt;
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
static bool db_has_duplicate_pics_range(const modbus_db_t *db,
|
||||
modbus_point_type_t pics_type,
|
||||
uint16_t pics_offset,
|
||||
uint8_t pics_reg_span)
|
||||
{
|
||||
size_t i;
|
||||
|
||||
if (db == NULL || pics_reg_span == 0U)
|
||||
return false;
|
||||
|
||||
for (i = 0; i < db->count; i++)
|
||||
{
|
||||
const modbus_point_t *pt = &db->points[i];
|
||||
|
||||
if (!pt->has_pics_address || pt->pics_reg_span == 0U)
|
||||
continue;
|
||||
|
||||
if (pt->pics_type != pics_type)
|
||||
continue;
|
||||
|
||||
if (ranges_overlap(pt->pics_offset, pt->pics_reg_span,
|
||||
pics_offset, pics_reg_span))
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool validate_control_mapping(const modbus_point_t *pt)
|
||||
{
|
||||
if (pt == NULL)
|
||||
return false;
|
||||
|
||||
if (!pt->has_control_address)
|
||||
return true;
|
||||
|
||||
if (!modbus_points_is_writable_type(pt->control_type))
|
||||
return false;
|
||||
|
||||
switch (pt->type)
|
||||
{
|
||||
case MB_POINT_DISCRETE_INPUT:
|
||||
return pt->control_type == MB_POINT_COIL;
|
||||
|
||||
case MB_POINT_INPUT_REGISTER:
|
||||
return pt->control_type == MB_POINT_HOLDING_REGISTER;
|
||||
|
||||
case MB_POINT_COIL:
|
||||
case MB_POINT_HOLDING_REGISTER:
|
||||
return false;
|
||||
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
static bool append_helper_points(modbus_db_t *db, const modbus_point_t *base_pt)
|
||||
{
|
||||
uint8_t i;
|
||||
|
||||
if (db == NULL || base_pt == NULL)
|
||||
return false;
|
||||
|
||||
if (base_pt->reg_span <= 1U)
|
||||
return true;
|
||||
|
||||
for (i = 1U; i < base_pt->reg_span; i++)
|
||||
{
|
||||
modbus_point_t helper = *base_pt;
|
||||
|
||||
helper.raw_address = base_pt->raw_address + (uint32_t)i;
|
||||
helper.offset = (uint16_t)(base_pt->offset + i);
|
||||
helper.data_type = MB_DATA_UINT16;
|
||||
helper.reg_span = 1U;
|
||||
helper.is_helper = true;
|
||||
helper.has_control_address = false;
|
||||
helper.control_raw_address = 0U;
|
||||
helper.control_type = MB_POINT_INVALID;
|
||||
helper.control_offset = 0U;
|
||||
helper.has_pics_address = false;
|
||||
helper.pics_raw_address = 0U;
|
||||
helper.pics_type = MB_POINT_INVALID;
|
||||
helper.pics_offset = 0U;
|
||||
helper.pics_data_type = PICS_DATA_NONE;
|
||||
helper.pics_reg_span = 0U;
|
||||
memset(helper.pics_words, 0, sizeof(helper.pics_words));
|
||||
helper.bit_value = 0U;
|
||||
helper.reg_value = 0U;
|
||||
|
||||
snprintf(helper.name,
|
||||
sizeof(helper.name),
|
||||
"%.25s__w%u",
|
||||
base_pt->name,
|
||||
(unsigned)(i + 1U));
|
||||
|
||||
if (db->count >= MODBUS_MAX_POINTS)
|
||||
return false;
|
||||
|
||||
if (db_has_duplicate_name(db, helper.name))
|
||||
return false;
|
||||
|
||||
if (db_has_duplicate_type_offset(db, helper.type, helper.offset))
|
||||
return false;
|
||||
|
||||
db->points[db->count++] = helper;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool parse_csv_line(const char *line_in, modbus_point_t *pt)
|
||||
{
|
||||
char line[MODBUS_MAX_CSV_LINE_LEN];
|
||||
char *fields[8];
|
||||
size_t field_count = 0;
|
||||
char *p;
|
||||
uint32_t raw_address;
|
||||
|
||||
if (line_in == NULL || pt == NULL)
|
||||
return false;
|
||||
|
||||
strncpy(line, line_in, sizeof(line) - 1U);
|
||||
line[sizeof(line) - 1U] = '\0';
|
||||
|
||||
p = line;
|
||||
fields[field_count++] = p;
|
||||
|
||||
while (*p != '\0' && field_count < 8U)
|
||||
{
|
||||
if (*p == ',')
|
||||
{
|
||||
*p = '\0';
|
||||
fields[field_count++] = p + 1;
|
||||
}
|
||||
p++;
|
||||
}
|
||||
|
||||
if (field_count != 8U)
|
||||
return false;
|
||||
|
||||
for (size_t i = 0; i < field_count; i++)
|
||||
trim_whitespace(fields[i]);
|
||||
|
||||
memset(pt, 0, sizeof(*pt));
|
||||
pt->control_type = MB_POINT_INVALID;
|
||||
pt->bit_value = 0U;
|
||||
pt->reg_value = 0U;
|
||||
|
||||
pt->type = parse_point_type(fields[0]);
|
||||
if (pt->type == MB_POINT_INVALID)
|
||||
return false;
|
||||
|
||||
if (!parse_u32(fields[1], &raw_address))
|
||||
return false;
|
||||
pt->raw_address = raw_address;
|
||||
|
||||
if (!normalize_address(pt->type, pt->raw_address, &pt->offset))
|
||||
return false;
|
||||
|
||||
if (fields[2][0] == '\0')
|
||||
return false;
|
||||
|
||||
strncpy(pt->name, fields[2], sizeof(pt->name) - 1U);
|
||||
pt->name[sizeof(pt->name) - 1U] = '\0';
|
||||
|
||||
pt->data_type = parse_data_type(fields[3]);
|
||||
if (pt->data_type == MB_DATA_INVALID)
|
||||
return false;
|
||||
|
||||
if (!validate_type_and_datatype(pt->type, pt->data_type))
|
||||
return false;
|
||||
|
||||
pt->reg_span = data_type_reg_span(pt->data_type);
|
||||
pt->is_helper = false;
|
||||
|
||||
if (pt->reg_span == 0U)
|
||||
return false;
|
||||
|
||||
if (fields[4][0] != '\0')
|
||||
{
|
||||
uint32_t control_raw;
|
||||
|
||||
if (!parse_u32(fields[4], &control_raw))
|
||||
return false;
|
||||
|
||||
pt->has_control_address = true;
|
||||
pt->control_raw_address = control_raw;
|
||||
pt->control_type = infer_point_type_from_address(control_raw);
|
||||
|
||||
if (pt->control_type == MB_POINT_INVALID)
|
||||
return false;
|
||||
|
||||
if (!normalize_address(pt->control_type, pt->control_raw_address, &pt->control_offset))
|
||||
return false;
|
||||
|
||||
if (!validate_control_mapping(pt))
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
pt->has_control_address = false;
|
||||
pt->control_raw_address = 0U;
|
||||
pt->control_type = MB_POINT_INVALID;
|
||||
pt->control_offset = 0U;
|
||||
}
|
||||
|
||||
if (fields[5][0] != '\0')
|
||||
{
|
||||
uint32_t pics_raw;
|
||||
|
||||
if (!parse_u32(fields[5], &pics_raw))
|
||||
return false;
|
||||
|
||||
pt->has_pics_address = true;
|
||||
pt->pics_raw_address = pics_raw;
|
||||
pt->pics_type = infer_point_type_from_address(pics_raw);
|
||||
|
||||
if (pt->pics_type == MB_POINT_INVALID)
|
||||
return false;
|
||||
|
||||
if (!normalize_address(pt->pics_type, pt->pics_raw_address, &pt->pics_offset))
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
pt->has_pics_address = false;
|
||||
pt->pics_raw_address = 0U;
|
||||
pt->pics_type = MB_POINT_INVALID;
|
||||
pt->pics_offset = 0U;
|
||||
}
|
||||
|
||||
pt->pics_data_type = parse_pics_data_type(fields[6]);
|
||||
|
||||
if (pt->pics_data_type == PICS_DATA_INVALID)
|
||||
return false;
|
||||
|
||||
if (pt->pics_data_type == PICS_DATA_STRING)
|
||||
return false;
|
||||
|
||||
switch (pt->pics_data_type)
|
||||
{
|
||||
case PICS_DATA_BOOLEAN:
|
||||
pt->pics_reg_span = 1U;
|
||||
break;
|
||||
case PICS_DATA_INTEGER:
|
||||
pt->pics_reg_span = 2U;
|
||||
break;
|
||||
case PICS_DATA_FLOAT:
|
||||
pt->pics_reg_span = 4U;
|
||||
break;
|
||||
case PICS_DATA_NONE:
|
||||
pt->pics_reg_span = 0U;
|
||||
break;
|
||||
default:
|
||||
pt->pics_reg_span = 0U;
|
||||
break;
|
||||
}
|
||||
|
||||
if (pt->has_pics_address && pt->pics_data_type == PICS_DATA_NONE)
|
||||
return false;
|
||||
|
||||
if (!pt->has_pics_address && pt->pics_data_type != PICS_DATA_NONE)
|
||||
return false;
|
||||
|
||||
if (pt->has_pics_address)
|
||||
{
|
||||
switch (pt->pics_data_type)
|
||||
{
|
||||
case PICS_DATA_INTEGER:
|
||||
case PICS_DATA_FLOAT:
|
||||
if (pt->pics_type != MB_POINT_HOLDING_REGISTER)
|
||||
return false;
|
||||
break;
|
||||
|
||||
case PICS_DATA_BOOLEAN:
|
||||
if (pt->pics_type != MB_POINT_COIL &&
|
||||
pt->pics_type != MB_POINT_HOLDING_REGISTER)
|
||||
return false;
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
strncpy(pt->notes, fields[7], sizeof(pt->notes) - 1U);
|
||||
pt->notes[sizeof(pt->notes) - 1U] = '\0';
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
/* ---------- public API ---------- */
|
||||
|
||||
void modbus_points_init(modbus_db_t *db)
|
||||
{
|
||||
modbus_points_reset(db);
|
||||
}
|
||||
|
||||
void modbus_points_reset(modbus_db_t *db)
|
||||
{
|
||||
if (db == NULL)
|
||||
return;
|
||||
|
||||
memset(db, 0, sizeof(*db));
|
||||
}
|
||||
|
||||
bool modbus_points_load(modbus_db_t *db, const char *filename)
|
||||
{
|
||||
FILE *fp;
|
||||
char line[MODBUS_MAX_CSV_LINE_LEN];
|
||||
size_t line_num = 0;
|
||||
|
||||
if (db == NULL || filename == NULL)
|
||||
return false;
|
||||
|
||||
fp = fopen(filename, "r");
|
||||
if (fp == NULL)
|
||||
{
|
||||
printf("CSV ERROR: failed to open file: %s\n", filename);
|
||||
return false;
|
||||
}
|
||||
|
||||
modbus_points_reset(db);
|
||||
|
||||
while (fgets(line, sizeof(line), fp) != NULL)
|
||||
{
|
||||
modbus_point_t pt;
|
||||
char *newline;
|
||||
|
||||
line_num++;
|
||||
|
||||
newline = strchr(line, '\n');
|
||||
if (newline != NULL)
|
||||
*newline = '\0';
|
||||
|
||||
newline = strchr(line, '\r');
|
||||
if (newline != NULL)
|
||||
*newline = '\0';
|
||||
|
||||
trim_whitespace(line);
|
||||
|
||||
if (line[0] == '\0')
|
||||
continue;
|
||||
|
||||
if (line_num == 1U)
|
||||
continue;
|
||||
|
||||
{
|
||||
char temp_line[MODBUS_MAX_CSV_LINE_LEN];
|
||||
char *fields[8];
|
||||
size_t field_count = 0;
|
||||
char *p;
|
||||
bool all_fields_empty = true;
|
||||
size_t i;
|
||||
|
||||
strncpy(temp_line, line, sizeof(temp_line) - 1U);
|
||||
temp_line[sizeof(temp_line) - 1U] = '\0';
|
||||
|
||||
p = temp_line;
|
||||
fields[field_count++] = p;
|
||||
|
||||
while (*p != '\0' && field_count < 8U)
|
||||
{
|
||||
if (*p == ',')
|
||||
{
|
||||
*p = '\0';
|
||||
fields[field_count++] = p + 1;
|
||||
}
|
||||
p++;
|
||||
}
|
||||
|
||||
if (field_count == 8U)
|
||||
{
|
||||
for (i = 0; i < field_count; i++)
|
||||
{
|
||||
trim_whitespace(fields[i]);
|
||||
if (fields[i][0] != '\0')
|
||||
{
|
||||
all_fields_empty = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (all_fields_empty)
|
||||
continue;
|
||||
}
|
||||
}
|
||||
|
||||
if (db->count >= MODBUS_MAX_POINTS)
|
||||
{
|
||||
printf("CSV ERROR line %u: exceeded MODBUS_MAX_POINTS (%u)\n",
|
||||
(unsigned)line_num,
|
||||
(unsigned)MODBUS_MAX_POINTS);
|
||||
fclose(fp);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!parse_csv_line(line, &pt))
|
||||
{
|
||||
printf("CSV ERROR line %u: parse failed: %s\n",
|
||||
(unsigned)line_num,
|
||||
line);
|
||||
fclose(fp);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (db_has_duplicate_name(db, pt.name))
|
||||
{
|
||||
printf("CSV ERROR line %u: duplicate name '%s'\n",
|
||||
(unsigned)line_num,
|
||||
pt.name);
|
||||
fclose(fp);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (db_has_duplicate_type_offset(db, pt.type, pt.offset))
|
||||
{
|
||||
printf("CSV ERROR line %u: duplicate Modbus address for name '%s' raw=%lu offset=%u\n",
|
||||
(unsigned)line_num,
|
||||
pt.name,
|
||||
(unsigned long)pt.raw_address,
|
||||
(unsigned)pt.offset);
|
||||
fclose(fp);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (pt.has_pics_address &&
|
||||
db_has_duplicate_pics_range(db,
|
||||
pt.pics_type,
|
||||
pt.pics_offset,
|
||||
pt.pics_reg_span))
|
||||
{
|
||||
printf("CSV ERROR line %u: overlapping PICS range for name '%s' pics_raw=%lu pics_offset=%u span=%u\n",
|
||||
(unsigned)line_num,
|
||||
pt.name,
|
||||
(unsigned long)pt.pics_raw_address,
|
||||
(unsigned)pt.pics_offset,
|
||||
(unsigned)pt.pics_reg_span);
|
||||
fclose(fp);
|
||||
return false;
|
||||
}
|
||||
|
||||
db->points[db->count++] = pt;
|
||||
|
||||
if (!append_helper_points(db, &pt))
|
||||
{
|
||||
printf("CSV ERROR line %u: failed to append helper points for '%s'\n",
|
||||
(unsigned)line_num,
|
||||
pt.name);
|
||||
fclose(fp);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
fclose(fp);
|
||||
return true;
|
||||
}
|
||||
|
||||
const modbus_point_t *modbus_points_find_by_name(const modbus_db_t *db, const char *name)
|
||||
{
|
||||
size_t i;
|
||||
|
||||
if (db == NULL || name == NULL)
|
||||
return NULL;
|
||||
|
||||
for (i = 0; i < db->count; i++)
|
||||
{
|
||||
if (strcmp(db->points[i].name, name) == 0)
|
||||
return &db->points[i];
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
const modbus_point_t *modbus_points_find_by_type_offset(const modbus_db_t *db,
|
||||
modbus_point_type_t type,
|
||||
uint16_t offset)
|
||||
{
|
||||
size_t i;
|
||||
|
||||
if (db == NULL)
|
||||
return NULL;
|
||||
|
||||
for (i = 0; i < db->count; i++)
|
||||
{
|
||||
if (db->points[i].type == type && db->points[i].offset == offset)
|
||||
return &db->points[i];
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
modbus_mem_type_t modbus_points_type_to_mem(modbus_point_type_t type)
|
||||
{
|
||||
switch (type)
|
||||
{
|
||||
case MB_POINT_COIL:
|
||||
return MB_MEM_COIL;
|
||||
case MB_POINT_DISCRETE_INPUT:
|
||||
return MB_MEM_DISCRETE_INPUT;
|
||||
case MB_POINT_INPUT_REGISTER:
|
||||
return MB_MEM_INPUT_REGISTER;
|
||||
case MB_POINT_HOLDING_REGISTER:
|
||||
return MB_MEM_HOLDING_REGISTER;
|
||||
default:
|
||||
return MB_MEM_COIL;
|
||||
}
|
||||
}
|
||||
|
||||
bool modbus_points_is_bit_type(modbus_point_type_t type)
|
||||
{
|
||||
return (type == MB_POINT_COIL || type == MB_POINT_DISCRETE_INPUT);
|
||||
}
|
||||
|
||||
bool modbus_points_is_reg_type(modbus_point_type_t type)
|
||||
{
|
||||
return (type == MB_POINT_INPUT_REGISTER || type == MB_POINT_HOLDING_REGISTER);
|
||||
}
|
||||
|
||||
bool modbus_points_is_writable_type(modbus_point_type_t type)
|
||||
{
|
||||
return (type == MB_POINT_COIL || type == MB_POINT_HOLDING_REGISTER);
|
||||
}
|
||||
112
main/modbus_points.h
Normal file
112
main/modbus_points.h
Normal file
@@ -0,0 +1,112 @@
|
||||
#ifndef MODBUS_POINTS_H
|
||||
#define MODBUS_POINTS_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#include "modbus_memory.h"
|
||||
|
||||
#define MODBUS_MAX_POINTS 384
|
||||
#define MODBUS_MAX_NAME_LEN 32
|
||||
#define MODBUS_MAX_NOTES_LEN 64
|
||||
#define MODBUS_MAX_CSV_LINE_LEN 256
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MB_POINT_COIL = 0,
|
||||
MB_POINT_DISCRETE_INPUT,
|
||||
MB_POINT_INPUT_REGISTER,
|
||||
MB_POINT_HOLDING_REGISTER,
|
||||
MB_POINT_INVALID
|
||||
} modbus_point_type_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
MB_DATA_BOOL = 0,
|
||||
MB_DATA_UINT16,
|
||||
MB_DATA_UINT32,
|
||||
MB_DATA_FLOAT,
|
||||
MB_DATA_DOUBLE,
|
||||
MB_DATA_INVALID
|
||||
} modbus_data_type_t;
|
||||
|
||||
typedef enum
|
||||
{
|
||||
PICS_DATA_NONE = 0,
|
||||
PICS_DATA_BOOLEAN,
|
||||
PICS_DATA_INTEGER, /* PICS Integer = 32-bit */
|
||||
PICS_DATA_FLOAT, /* PICS Float = 64-bit double */
|
||||
PICS_DATA_STRING,
|
||||
PICS_DATA_INVALID
|
||||
} pics_data_type_t;
|
||||
|
||||
typedef struct modbus_point
|
||||
{
|
||||
modbus_point_type_t type;
|
||||
uint32_t raw_address;
|
||||
uint16_t offset;
|
||||
|
||||
char name[MODBUS_MAX_NAME_LEN];
|
||||
modbus_data_type_t data_type;
|
||||
|
||||
uint8_t reg_span;
|
||||
bool is_helper;
|
||||
|
||||
bool has_control_address;
|
||||
uint32_t control_raw_address;
|
||||
modbus_point_type_t control_type;
|
||||
uint16_t control_offset;
|
||||
|
||||
bool has_pics_address;
|
||||
uint32_t pics_raw_address;
|
||||
modbus_point_type_t pics_type;
|
||||
uint16_t pics_offset;
|
||||
pics_data_type_t pics_data_type;
|
||||
uint8_t pics_reg_span;
|
||||
|
||||
uint16_t pics_words[4];
|
||||
|
||||
char notes[MODBUS_MAX_NOTES_LEN];
|
||||
|
||||
uint8_t bit_value;
|
||||
uint16_t reg_value;
|
||||
} modbus_point_t;
|
||||
|
||||
typedef struct modbus_db
|
||||
{
|
||||
modbus_point_t points[MODBUS_MAX_POINTS];
|
||||
size_t count;
|
||||
} modbus_db_t;
|
||||
|
||||
/* Initialize / clear point database */
|
||||
void modbus_points_init(modbus_db_t *db);
|
||||
void modbus_points_reset(modbus_db_t *db);
|
||||
|
||||
/* Load points from CSV */
|
||||
bool modbus_points_load(modbus_db_t *db, const char *filename);
|
||||
|
||||
/* Lookup helpers */
|
||||
const modbus_point_t *modbus_points_find_by_name(const modbus_db_t *db,
|
||||
const char *name);
|
||||
|
||||
const modbus_point_t *modbus_points_find_by_type_offset(const modbus_db_t *db,
|
||||
modbus_point_type_t type,
|
||||
uint16_t offset);
|
||||
|
||||
/* Optional helpers for PICS staging lookup */
|
||||
const modbus_point_t *modbus_points_find_by_pics_offset(const modbus_db_t *db,
|
||||
modbus_point_type_t type,
|
||||
uint16_t offset);
|
||||
|
||||
const modbus_point_t *modbus_points_find_by_pics_range(const modbus_db_t *db,
|
||||
modbus_point_type_t type,
|
||||
uint16_t offset);
|
||||
|
||||
/* Helpers */
|
||||
modbus_mem_type_t modbus_points_type_to_mem(modbus_point_type_t type);
|
||||
bool modbus_points_is_bit_type(modbus_point_type_t type);
|
||||
bool modbus_points_is_reg_type(modbus_point_type_t type);
|
||||
bool modbus_points_is_writable_type(modbus_point_type_t type);
|
||||
|
||||
#endif /* MODBUS_POINTS_H */
|
||||
707
main/modbus_tcp.c
Normal file
707
main/modbus_tcp.c
Normal file
@@ -0,0 +1,707 @@
|
||||
#include "modbus_tcp.h"
|
||||
#include "modbus_memory.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
/* Modbus spec limits */
|
||||
#define MODBUS_MAX_READ_BITS 2000
|
||||
#define MODBUS_MAX_READ_REGS 125
|
||||
#define MODBUS_MAX_WRITE_REGS 123
|
||||
#define MODBUS_MAX_WRITE_COILS 1968
|
||||
|
||||
static uint16_t read_u16_be(const uint8_t *p)
|
||||
{
|
||||
return (uint16_t)(((uint16_t)p[0] << 8) | (uint16_t)p[1]);
|
||||
}
|
||||
|
||||
static void write_u16_be(uint8_t *p, uint16_t value)
|
||||
{
|
||||
p[0] = (uint8_t)((value >> 8) & 0xFFU);
|
||||
p[1] = (uint8_t)(value & 0xFFU);
|
||||
}
|
||||
|
||||
static bool parse_mbap(const uint8_t *req, size_t req_len, modbus_mbap_t *mbap)
|
||||
{
|
||||
if (req == NULL || mbap == NULL)
|
||||
return false;
|
||||
|
||||
if (req_len < MODBUS_TCP_MBAP_LEN)
|
||||
return false;
|
||||
|
||||
mbap->transaction_id = read_u16_be(&req[0]);
|
||||
mbap->protocol_id = read_u16_be(&req[2]);
|
||||
mbap->length = read_u16_be(&req[4]);
|
||||
mbap->unit_id = req[6];
|
||||
|
||||
if (mbap->protocol_id != 0U)
|
||||
return false;
|
||||
|
||||
/* length includes unit id + PDU bytes */
|
||||
if (mbap->length < 2U)
|
||||
return false;
|
||||
|
||||
if ((size_t)(6U + mbap->length) != req_len)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool build_mbap(uint8_t *resp,
|
||||
size_t resp_max_len,
|
||||
const modbus_mbap_t *req_mbap,
|
||||
uint16_t pdu_len,
|
||||
size_t *adu_len)
|
||||
{
|
||||
uint16_t length;
|
||||
|
||||
if (resp == NULL || req_mbap == NULL || adu_len == NULL)
|
||||
return false;
|
||||
|
||||
length = (uint16_t)(1U + pdu_len); /* unit_id + pdu */
|
||||
|
||||
if ((size_t)(MODBUS_TCP_MBAP_LEN + pdu_len) > resp_max_len)
|
||||
return false;
|
||||
|
||||
write_u16_be(&resp[0], req_mbap->transaction_id);
|
||||
write_u16_be(&resp[2], 0U);
|
||||
write_u16_be(&resp[4], length);
|
||||
resp[6] = req_mbap->unit_id;
|
||||
|
||||
*adu_len = (size_t)MODBUS_TCP_MBAP_LEN + (size_t)pdu_len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool build_exception_response(const modbus_mbap_t *mbap,
|
||||
uint8_t function_code,
|
||||
uint8_t exception_code,
|
||||
uint8_t *resp,
|
||||
size_t resp_max_len,
|
||||
size_t *resp_len)
|
||||
{
|
||||
size_t adu_len;
|
||||
|
||||
if (!build_mbap(resp, resp_max_len, mbap, 2U, &adu_len))
|
||||
return false;
|
||||
|
||||
resp[7] = (uint8_t)(function_code | 0x80U);
|
||||
resp[8] = exception_code;
|
||||
|
||||
*resp_len = adu_len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static uint8_t get_bit_packed_byte_count(uint16_t quantity)
|
||||
{
|
||||
return (uint8_t)((quantity + 7U) / 8U);
|
||||
}
|
||||
|
||||
static bool handle_read_bits(modbus_memory_t *mem,
|
||||
const modbus_mbap_t *mbap,
|
||||
uint8_t function_code,
|
||||
modbus_mem_type_t mem_type,
|
||||
const uint8_t *pdu,
|
||||
size_t pdu_len,
|
||||
uint8_t *resp,
|
||||
size_t resp_max_len,
|
||||
size_t *resp_len)
|
||||
{
|
||||
uint16_t start_addr;
|
||||
uint16_t quantity;
|
||||
uint8_t byte_count;
|
||||
uint8_t bits[MODBUS_MAX_READ_BITS];
|
||||
size_t adu_len;
|
||||
|
||||
if (pdu == NULL || pdu_len != 5U)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
start_addr = read_u16_be(&pdu[1]);
|
||||
quantity = read_u16_be(&pdu[3]);
|
||||
|
||||
if (quantity == 0U || quantity > MODBUS_MAX_READ_BITS)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (!modbus_memory_valid_bit_range(mem, mem_type, start_addr, quantity))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_ILLEGAL_DATA_ADDRESS,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (mem_type == MB_MEM_COIL)
|
||||
{
|
||||
if (!modbus_memory_read_coils(mem, start_addr, quantity, bits))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_SERVER_DEVICE_FAILURE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
}
|
||||
else if (mem_type == MB_MEM_DISCRETE_INPUT)
|
||||
{
|
||||
if (!modbus_memory_read_discrete_inputs(mem, start_addr, quantity, bits))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_SERVER_DEVICE_FAILURE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_ILLEGAL_FUNCTION,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
byte_count = get_bit_packed_byte_count(quantity);
|
||||
|
||||
if (!build_mbap(resp, resp_max_len, mbap, (uint16_t)(2U + byte_count), &adu_len))
|
||||
return false;
|
||||
|
||||
resp[7] = function_code;
|
||||
resp[8] = byte_count;
|
||||
memset(&resp[9], 0, byte_count);
|
||||
|
||||
for (uint16_t i = 0; i < quantity; i++)
|
||||
{
|
||||
if (bits[i] != 0U)
|
||||
resp[9 + (i / 8U)] |= (uint8_t)(1U << (i % 8U));
|
||||
}
|
||||
|
||||
*resp_len = adu_len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool handle_read_registers(modbus_memory_t *mem,
|
||||
const modbus_mbap_t *mbap,
|
||||
uint8_t function_code,
|
||||
modbus_mem_type_t mem_type,
|
||||
const uint8_t *pdu,
|
||||
size_t pdu_len,
|
||||
uint8_t *resp,
|
||||
size_t resp_max_len,
|
||||
size_t *resp_len)
|
||||
{
|
||||
uint16_t start_addr;
|
||||
uint16_t quantity;
|
||||
uint16_t regs[MODBUS_MAX_READ_REGS];
|
||||
uint8_t byte_count;
|
||||
size_t adu_len;
|
||||
|
||||
if (pdu == NULL || pdu_len != 5U)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
start_addr = read_u16_be(&pdu[1]);
|
||||
quantity = read_u16_be(&pdu[3]);
|
||||
|
||||
if (quantity == 0U || quantity > MODBUS_MAX_READ_REGS)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (!modbus_memory_valid_reg_range(mem, mem_type, start_addr, quantity))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_ILLEGAL_DATA_ADDRESS,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (mem_type == MB_MEM_HOLDING_REGISTER)
|
||||
{
|
||||
if (!modbus_memory_read_holding_registers(mem, start_addr, quantity, regs))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_SERVER_DEVICE_FAILURE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
}
|
||||
else if (mem_type == MB_MEM_INPUT_REGISTER)
|
||||
{
|
||||
if (!modbus_memory_read_input_registers(mem, start_addr, quantity, regs))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_SERVER_DEVICE_FAILURE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
function_code,
|
||||
MODBUS_EX_ILLEGAL_FUNCTION,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
byte_count = (uint8_t)(quantity * 2U);
|
||||
|
||||
if (!build_mbap(resp, resp_max_len, mbap, (uint16_t)(2U + byte_count), &adu_len))
|
||||
return false;
|
||||
|
||||
resp[7] = function_code;
|
||||
resp[8] = byte_count;
|
||||
|
||||
for (uint16_t i = 0; i < quantity; i++)
|
||||
write_u16_be(&resp[9 + ((size_t)i * 2U)], regs[i]);
|
||||
|
||||
*resp_len = adu_len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool handle_write_single_coil(modbus_memory_t *mem,
|
||||
const modbus_mbap_t *mbap,
|
||||
const uint8_t *pdu,
|
||||
size_t pdu_len,
|
||||
uint8_t *resp,
|
||||
size_t resp_max_len,
|
||||
size_t *resp_len)
|
||||
{
|
||||
uint16_t addr;
|
||||
uint16_t value;
|
||||
size_t adu_len;
|
||||
uint8_t bit_value;
|
||||
|
||||
if (pdu == NULL || pdu_len != 5U)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_SINGLE_COIL,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
addr = read_u16_be(&pdu[1]);
|
||||
value = read_u16_be(&pdu[3]);
|
||||
|
||||
if (value == 0xFF00U)
|
||||
bit_value = 1U;
|
||||
else if (value == 0x0000U)
|
||||
bit_value = 0U;
|
||||
else
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_SINGLE_COIL,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (!modbus_memory_write_coil(mem, addr, bit_value))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_SINGLE_COIL,
|
||||
MODBUS_EX_ILLEGAL_DATA_ADDRESS,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (!build_mbap(resp, resp_max_len, mbap, 5U, &adu_len))
|
||||
return false;
|
||||
|
||||
memcpy(&resp[7], pdu, 5U);
|
||||
|
||||
*resp_len = adu_len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool handle_write_single_register(modbus_memory_t *mem,
|
||||
const modbus_mbap_t *mbap,
|
||||
const uint8_t *pdu,
|
||||
size_t pdu_len,
|
||||
uint8_t *resp,
|
||||
size_t resp_max_len,
|
||||
size_t *resp_len)
|
||||
{
|
||||
uint16_t addr;
|
||||
uint16_t value;
|
||||
size_t adu_len;
|
||||
|
||||
if (pdu == NULL || pdu_len != 5U)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_SINGLE_REGISTER,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
addr = read_u16_be(&pdu[1]);
|
||||
value = read_u16_be(&pdu[3]);
|
||||
|
||||
if (!modbus_memory_write_holding_register(mem, addr, value))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_SINGLE_REGISTER,
|
||||
MODBUS_EX_ILLEGAL_DATA_ADDRESS,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (!build_mbap(resp, resp_max_len, mbap, 5U, &adu_len))
|
||||
return false;
|
||||
|
||||
memcpy(&resp[7], pdu, 5U);
|
||||
|
||||
*resp_len = adu_len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool handle_write_multiple_coils(modbus_memory_t *mem,
|
||||
const modbus_mbap_t *mbap,
|
||||
const uint8_t *pdu,
|
||||
size_t pdu_len,
|
||||
uint8_t *resp,
|
||||
size_t resp_max_len,
|
||||
size_t *resp_len)
|
||||
{
|
||||
uint16_t start_addr;
|
||||
uint16_t quantity;
|
||||
uint8_t byte_count;
|
||||
uint8_t bits[MODBUS_MAX_WRITE_COILS];
|
||||
size_t adu_len;
|
||||
size_t expected_pdu_len;
|
||||
|
||||
if (pdu == NULL || pdu_len < 6U)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_COILS,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
start_addr = read_u16_be(&pdu[1]);
|
||||
quantity = read_u16_be(&pdu[3]);
|
||||
byte_count = pdu[5];
|
||||
|
||||
if (quantity == 0U || quantity > MODBUS_MAX_WRITE_COILS)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_COILS,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (byte_count != get_bit_packed_byte_count(quantity))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_COILS,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
expected_pdu_len = (size_t)(6U + byte_count);
|
||||
if (pdu_len != expected_pdu_len)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_COILS,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (!modbus_memory_valid_bit_range(mem, MB_MEM_COIL, start_addr, quantity))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_COILS,
|
||||
MODBUS_EX_ILLEGAL_DATA_ADDRESS,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
for (uint16_t i = 0; i < quantity; i++)
|
||||
bits[i] = (uint8_t)((pdu[6 + (i / 8U)] >> (i % 8U)) & 0x01U);
|
||||
|
||||
if (!modbus_memory_write_coils(mem, start_addr, quantity, bits))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_COILS,
|
||||
MODBUS_EX_SERVER_DEVICE_FAILURE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (!build_mbap(resp, resp_max_len, mbap, 5U, &adu_len))
|
||||
return false;
|
||||
|
||||
resp[7] = MODBUS_FC_WRITE_MULTIPLE_COILS;
|
||||
write_u16_be(&resp[8], start_addr);
|
||||
write_u16_be(&resp[10], quantity);
|
||||
|
||||
*resp_len = adu_len;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool handle_write_multiple_registers(modbus_memory_t *mem,
|
||||
const modbus_mbap_t *mbap,
|
||||
const uint8_t *pdu,
|
||||
size_t pdu_len,
|
||||
uint8_t *resp,
|
||||
size_t resp_max_len,
|
||||
size_t *resp_len)
|
||||
{
|
||||
uint16_t start_addr;
|
||||
uint16_t quantity;
|
||||
uint8_t byte_count;
|
||||
uint16_t regs[MODBUS_MAX_WRITE_REGS];
|
||||
size_t adu_len;
|
||||
size_t expected_pdu_len;
|
||||
|
||||
if (pdu == NULL || pdu_len < 6U)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_REGISTERS,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
start_addr = read_u16_be(&pdu[1]);
|
||||
quantity = read_u16_be(&pdu[3]);
|
||||
byte_count = pdu[5];
|
||||
|
||||
if (quantity == 0U || quantity > MODBUS_MAX_WRITE_REGS)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_REGISTERS,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (byte_count != (uint8_t)(quantity * 2U))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_REGISTERS,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
expected_pdu_len = (size_t)(6U + byte_count);
|
||||
if (pdu_len != expected_pdu_len)
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_REGISTERS,
|
||||
MODBUS_EX_ILLEGAL_DATA_VALUE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (!modbus_memory_valid_reg_range(mem, MB_MEM_HOLDING_REGISTER, start_addr, quantity))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_REGISTERS,
|
||||
MODBUS_EX_ILLEGAL_DATA_ADDRESS,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
for (uint16_t i = 0; i < quantity; i++)
|
||||
regs[i] = read_u16_be(&pdu[6 + ((size_t)i * 2U)]);
|
||||
|
||||
if (!modbus_memory_write_holding_registers(mem, start_addr, quantity, regs))
|
||||
{
|
||||
return build_exception_response(mbap,
|
||||
MODBUS_FC_WRITE_MULTIPLE_REGISTERS,
|
||||
MODBUS_EX_SERVER_DEVICE_FAILURE,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
|
||||
if (!build_mbap(resp, resp_max_len, mbap, 5U, &adu_len))
|
||||
return false;
|
||||
|
||||
resp[7] = MODBUS_FC_WRITE_MULTIPLE_REGISTERS;
|
||||
write_u16_be(&resp[8], start_addr);
|
||||
write_u16_be(&resp[10], quantity);
|
||||
|
||||
*resp_len = adu_len;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool modbus_tcp_process_request(modbus_memory_t *mem,
|
||||
const uint8_t *req,
|
||||
size_t req_len,
|
||||
uint8_t *resp,
|
||||
size_t resp_max_len,
|
||||
size_t *resp_len)
|
||||
{
|
||||
modbus_mbap_t mbap;
|
||||
const uint8_t *pdu;
|
||||
size_t pdu_len;
|
||||
uint8_t function_code;
|
||||
|
||||
if (req == NULL || resp == NULL || resp_len == NULL || mem == NULL)
|
||||
return false;
|
||||
|
||||
*resp_len = 0U;
|
||||
|
||||
if (!parse_mbap(req, req_len, &mbap))
|
||||
return false;
|
||||
|
||||
pdu = &req[7];
|
||||
pdu_len = req_len - MODBUS_TCP_MBAP_LEN;
|
||||
|
||||
if (pdu_len < 1U)
|
||||
return false;
|
||||
|
||||
function_code = pdu[0];
|
||||
|
||||
switch (function_code)
|
||||
{
|
||||
case MODBUS_FC_READ_COILS:
|
||||
return handle_read_bits(mem,
|
||||
&mbap,
|
||||
function_code,
|
||||
MB_MEM_COIL,
|
||||
pdu,
|
||||
pdu_len,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
|
||||
case MODBUS_FC_READ_DISCRETE_INPUTS:
|
||||
return handle_read_bits(mem,
|
||||
&mbap,
|
||||
function_code,
|
||||
MB_MEM_DISCRETE_INPUT,
|
||||
pdu,
|
||||
pdu_len,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
|
||||
case MODBUS_FC_READ_HOLDING_REGISTERS:
|
||||
return handle_read_registers(mem,
|
||||
&mbap,
|
||||
function_code,
|
||||
MB_MEM_HOLDING_REGISTER,
|
||||
pdu,
|
||||
pdu_len,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
|
||||
case MODBUS_FC_READ_INPUT_REGISTERS:
|
||||
return handle_read_registers(mem,
|
||||
&mbap,
|
||||
function_code,
|
||||
MB_MEM_INPUT_REGISTER,
|
||||
pdu,
|
||||
pdu_len,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
|
||||
case MODBUS_FC_WRITE_SINGLE_COIL:
|
||||
return handle_write_single_coil(mem,
|
||||
&mbap,
|
||||
pdu,
|
||||
pdu_len,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
|
||||
case MODBUS_FC_WRITE_SINGLE_REGISTER:
|
||||
return handle_write_single_register(mem,
|
||||
&mbap,
|
||||
pdu,
|
||||
pdu_len,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
|
||||
case MODBUS_FC_WRITE_MULTIPLE_COILS:
|
||||
return handle_write_multiple_coils(mem,
|
||||
&mbap,
|
||||
pdu,
|
||||
pdu_len,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
|
||||
case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
|
||||
return handle_write_multiple_registers(mem,
|
||||
&mbap,
|
||||
pdu,
|
||||
pdu_len,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
|
||||
default:
|
||||
return build_exception_response(&mbap,
|
||||
function_code,
|
||||
MODBUS_EX_ILLEGAL_FUNCTION,
|
||||
resp,
|
||||
resp_max_len,
|
||||
resp_len);
|
||||
}
|
||||
}
|
||||
65
main/modbus_tcp.h
Normal file
65
main/modbus_tcp.h
Normal file
@@ -0,0 +1,65 @@
|
||||
#ifndef MODBUS_TCP_H
|
||||
#define MODBUS_TCP_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <stddef.h>
|
||||
|
||||
#include "modbus_memory.h"
|
||||
|
||||
#define MODBUS_TCP_PORT 502
|
||||
|
||||
/* MBAP header is always 7 bytes */
|
||||
#define MODBUS_TCP_MBAP_LEN 7
|
||||
|
||||
/* Maximum Modbus TCP ADU size:
|
||||
* 7-byte MBAP + up to 253-byte PDU = 260 bytes
|
||||
*/
|
||||
#define MODBUS_TCP_MAX_ADU_LEN 260
|
||||
#define MODBUS_TCP_MAX_PDU_LEN 253
|
||||
|
||||
/* Modbus exception codes */
|
||||
#define MODBUS_EX_ILLEGAL_FUNCTION 0x01
|
||||
#define MODBUS_EX_ILLEGAL_DATA_ADDRESS 0x02
|
||||
#define MODBUS_EX_ILLEGAL_DATA_VALUE 0x03
|
||||
#define MODBUS_EX_SERVER_DEVICE_FAILURE 0x04
|
||||
|
||||
/* Supported function codes */
|
||||
#define MODBUS_FC_READ_COILS 0x01
|
||||
#define MODBUS_FC_READ_DISCRETE_INPUTS 0x02
|
||||
#define MODBUS_FC_READ_HOLDING_REGISTERS 0x03
|
||||
#define MODBUS_FC_READ_INPUT_REGISTERS 0x04
|
||||
#define MODBUS_FC_WRITE_SINGLE_COIL 0x05
|
||||
#define MODBUS_FC_WRITE_SINGLE_REGISTER 0x06
|
||||
#define MODBUS_FC_WRITE_MULTIPLE_COILS 0x0F
|
||||
#define MODBUS_FC_WRITE_MULTIPLE_REGISTERS 0x10
|
||||
|
||||
typedef struct
|
||||
{
|
||||
uint16_t transaction_id;
|
||||
uint16_t protocol_id;
|
||||
uint16_t length;
|
||||
uint8_t unit_id;
|
||||
} modbus_mbap_t;
|
||||
|
||||
/*
|
||||
* Process one Modbus TCP request ADU and write the response ADU.
|
||||
*
|
||||
* mem = target virtual device memory
|
||||
* req = incoming request bytes
|
||||
* req_len = number of request bytes
|
||||
* resp = output response buffer
|
||||
* resp_max_len = size of resp buffer
|
||||
* resp_len = actual response length written
|
||||
*
|
||||
* Returns true if a response was built successfully.
|
||||
* Returns false on malformed packet / buffer issues.
|
||||
*/
|
||||
bool modbus_tcp_process_request(modbus_memory_t *mem,
|
||||
const uint8_t *req,
|
||||
size_t req_len,
|
||||
uint8_t *resp,
|
||||
size_t resp_max_len,
|
||||
size_t *resp_len);
|
||||
|
||||
#endif /* MODBUS_TCP_H */
|
||||
189
main/wifi_manager.c
Normal file
189
main/wifi_manager.c
Normal file
@@ -0,0 +1,189 @@
|
||||
#include "wifi_manager.h"
|
||||
|
||||
#include <string.h>
|
||||
|
||||
#include "esp_log.h"
|
||||
#include "esp_event.h"
|
||||
#include "esp_netif.h"
|
||||
#include "esp_wifi.h"
|
||||
|
||||
#include "lwip/inet.h"
|
||||
#include "lwip/ip4_addr.h"
|
||||
|
||||
#include "freertos/event_groups.h"
|
||||
|
||||
#define WIFI_CONNECTED_BIT BIT0
|
||||
|
||||
static const char *TAG = "WIFI_MANAGER";
|
||||
|
||||
static EventGroupHandle_t s_wifi_event_group = NULL;
|
||||
static esp_netif_t *s_sta_netif = NULL;
|
||||
static bool s_connected = false;
|
||||
|
||||
static void wifi_event_handler(void *arg,
|
||||
esp_event_base_t event_base,
|
||||
int32_t event_id,
|
||||
void *event_data)
|
||||
{
|
||||
(void)arg;
|
||||
|
||||
if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START)
|
||||
{
|
||||
ESP_LOGI(TAG, "Wi-Fi STA start");
|
||||
esp_wifi_connect();
|
||||
}
|
||||
else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED)
|
||||
{
|
||||
ESP_LOGW(TAG, "Wi-Fi disconnected, retrying");
|
||||
s_connected = false;
|
||||
if (s_wifi_event_group != NULL)
|
||||
xEventGroupClearBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
|
||||
esp_wifi_connect();
|
||||
}
|
||||
else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP)
|
||||
{
|
||||
ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data;
|
||||
|
||||
ESP_LOGI(TAG, "Got IP: " IPSTR, IP2STR(&event->ip_info.ip));
|
||||
s_connected = true;
|
||||
|
||||
if (s_wifi_event_group != NULL)
|
||||
xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
|
||||
}
|
||||
}
|
||||
|
||||
static esp_err_t wifi_manager_apply_static_ip(const wifi_settings_t *cfg)
|
||||
{
|
||||
esp_err_t err;
|
||||
esp_netif_ip_info_t ip_info;
|
||||
ip4_addr_t ip, gw, netmask;
|
||||
|
||||
if (cfg == NULL || s_sta_netif == NULL)
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
|
||||
if (!ip4addr_aton(cfg->static_ip, &ip) ||
|
||||
!ip4addr_aton(cfg->gateway, &gw) ||
|
||||
!ip4addr_aton(cfg->netmask, &netmask))
|
||||
{
|
||||
ESP_LOGE(TAG, "Invalid static IP configuration");
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
}
|
||||
|
||||
err = esp_netif_dhcpc_stop(s_sta_netif);
|
||||
if (err != ESP_OK && err != ESP_ERR_ESP_NETIF_DHCP_ALREADY_STOPPED)
|
||||
return err;
|
||||
|
||||
memset(&ip_info, 0, sizeof(ip_info));
|
||||
ip_info.ip.addr = ip.addr;
|
||||
ip_info.gw.addr = gw.addr;
|
||||
ip_info.netmask.addr = netmask.addr;
|
||||
|
||||
err = esp_netif_set_ip_info(s_sta_netif, &ip_info);
|
||||
if (err != ESP_OK)
|
||||
return err;
|
||||
|
||||
ESP_LOGI(TAG,
|
||||
"Static IP configured: ip=%s gw=%s netmask=%s",
|
||||
cfg->static_ip,
|
||||
cfg->gateway,
|
||||
cfg->netmask);
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t wifi_manager_init(void)
|
||||
{
|
||||
static bool initialized = false;
|
||||
wifi_init_config_t wifi_init_cfg = WIFI_INIT_CONFIG_DEFAULT();
|
||||
|
||||
if (initialized)
|
||||
return ESP_OK;
|
||||
|
||||
esp_err_t err;
|
||||
|
||||
err = esp_netif_init();
|
||||
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE)
|
||||
return err;
|
||||
|
||||
err = esp_event_loop_create_default();
|
||||
if (err != ESP_OK && err != ESP_ERR_INVALID_STATE)
|
||||
return err;
|
||||
|
||||
s_sta_netif = esp_netif_create_default_wifi_sta();
|
||||
if (s_sta_netif == NULL)
|
||||
return ESP_FAIL;
|
||||
|
||||
s_wifi_event_group = xEventGroupCreate();
|
||||
if (s_wifi_event_group == NULL)
|
||||
return ESP_ERR_NO_MEM;
|
||||
|
||||
ESP_ERROR_CHECK(esp_wifi_init(&wifi_init_cfg));
|
||||
|
||||
ESP_ERROR_CHECK(esp_event_handler_register(WIFI_EVENT,
|
||||
ESP_EVENT_ANY_ID,
|
||||
&wifi_event_handler,
|
||||
NULL));
|
||||
|
||||
ESP_ERROR_CHECK(esp_event_handler_register(IP_EVENT,
|
||||
IP_EVENT_STA_GOT_IP,
|
||||
&wifi_event_handler,
|
||||
NULL));
|
||||
|
||||
initialized = true;
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
esp_err_t wifi_manager_start_sta(const wifi_settings_t *cfg)
|
||||
{
|
||||
wifi_config_t wifi_cfg;
|
||||
|
||||
if (cfg == NULL)
|
||||
return ESP_ERR_INVALID_ARG;
|
||||
|
||||
memset(&wifi_cfg, 0, sizeof(wifi_cfg));
|
||||
strncpy((char *)wifi_cfg.sta.ssid, cfg->ssid, sizeof(wifi_cfg.sta.ssid) - 1U);
|
||||
strncpy((char *)wifi_cfg.sta.password, cfg->password, sizeof(wifi_cfg.sta.password) - 1U);
|
||||
|
||||
wifi_cfg.sta.threshold.authmode = WIFI_AUTH_WPA2_PSK;
|
||||
wifi_cfg.sta.pmf_cfg.capable = true;
|
||||
wifi_cfg.sta.pmf_cfg.required = false;
|
||||
|
||||
ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
|
||||
|
||||
if (!cfg->dhcp)
|
||||
{
|
||||
esp_err_t err = wifi_manager_apply_static_ip(cfg);
|
||||
if (err != ESP_OK)
|
||||
return err;
|
||||
}
|
||||
else
|
||||
{
|
||||
esp_netif_dhcpc_start(s_sta_netif);
|
||||
}
|
||||
|
||||
ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_cfg));
|
||||
ESP_ERROR_CHECK(esp_wifi_start());
|
||||
|
||||
return ESP_OK;
|
||||
}
|
||||
|
||||
bool wifi_manager_wait_connected(TickType_t timeout_ticks)
|
||||
{
|
||||
EventBits_t bits;
|
||||
|
||||
if (s_wifi_event_group == NULL)
|
||||
return false;
|
||||
|
||||
bits = xEventGroupWaitBits(s_wifi_event_group,
|
||||
WIFI_CONNECTED_BIT,
|
||||
pdFALSE,
|
||||
pdFALSE,
|
||||
timeout_ticks);
|
||||
|
||||
return ((bits & WIFI_CONNECTED_BIT) != 0U);
|
||||
}
|
||||
|
||||
bool wifi_manager_is_connected(void)
|
||||
{
|
||||
return s_connected;
|
||||
}
|
||||
16
main/wifi_manager.h
Normal file
16
main/wifi_manager.h
Normal file
@@ -0,0 +1,16 @@
|
||||
#ifndef WIFI_MANAGER_H
|
||||
#define WIFI_MANAGER_H
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "esp_err.h"
|
||||
#include "freertos/FreeRTOS.h"
|
||||
|
||||
#include "config_store.h"
|
||||
|
||||
esp_err_t wifi_manager_init(void);
|
||||
esp_err_t wifi_manager_start_sta(const wifi_settings_t *cfg);
|
||||
bool wifi_manager_wait_connected(TickType_t timeout_ticks);
|
||||
bool wifi_manager_is_connected(void);
|
||||
|
||||
#endif /* WIFI_MANAGER_H */
|
||||
Reference in New Issue
Block a user