implement modbus tcp slave at port 502, reg1=1042 as device ID

main/CMakeLists.txt

@@ -5,6 +5,7 @@ idf_component_register(SRCS
     sntp_cli.c
     utils.c
     wifi_conn.c
+    mbiface.c
     console/console_ioimpl.c
     console/console_server.c
     console/register_cmds.c

main/console/console_server.c

@@ -72,7 +72,7 @@ static void telnetsrv_handle(TcpdClient_t client, char *buf, int nbytes)
 /**
  * Socket read handler
  */
-esp_err_t read_fn(Tcpd_t serv, TcpdClient_t client, int sockfd)
+static esp_err_t read_fn(Tcpd_t serv, TcpdClient_t client, int sockfd)
 {
     char buf[64];
     int nbytes = read(sockfd, buf, sizeof(buf));

main/fanctl_main.c

@@ -19,6 +19,7 @@
 
 #include "console/register_cmds.h"
 #include "irblast.h"
+#include "mbiface.h"
 
 static const char *TAG = "main";
 
@@ -55,6 +56,8 @@ void app_main(void) {
     console_setup_uart_stdio();
     ESP_ERROR_CHECK(console_start_stdio(NULL, NULL));
 
+    mbiface_setup();
+
     telnetsrv_start(CONSOLE_TELNET_PORT);
     ESP_LOGI(TAG, "Startup finished, free heap = %u, cmds %"PRIu32, esp_get_free_heap_size(), console_count_commands());
 }

main/mbiface.c

@@ -0,0 +1,88 @@
+#include <esp_log.h>
+#include "mbiface.h"
+#include "socket_server.h"
+#include "tasks.h"
+#include "modbus.h"
+
+static const char * TAG = "mb";
+
+Tcpd_t g_mbifc_server = NULL;
+ModbusSlave_t g_modbus;
+static volatile uint16_t register2 = 0;
+
+/**
+ * Socket read handler
+ */
+static esp_err_t read_fn(Tcpd_t serv, TcpdClient_t client, int sockfd)
+{
+    uint8_t buf[1024];
+    uint8_t buf2[1024];
+    int nbytes = read(sockfd, buf, sizeof(buf));
+    if (nbytes <= 0) return ESP_FAIL;
+
+    size_t resp_len = 0;
+    ModbusError_t e = mb_handleRequest(&g_modbus, buf, nbytes, buf2, 1024, &resp_len);
+    if (e == 0) {
+        tcpd_send(client, buf2, (ssize_t) resp_len);
+    } else {
+        ESP_LOGE(TAG, "Error %d, closing socket", e);
+        tcpd_kick(client);
+    }
+
+    return ESP_OK;
+}
+
+ModbusException_t startOfAccess(ModbusSlave_t *pSlave, ModbusFunction_t function, uint8_t i) {
+    return 0;
+}
+
+void endOfAccess(ModbusSlave_t *ms) {
+    //
+}
+
+ModbusException_t rh(ModbusSlave_t *pSlave, uint16_t ref, uint16_t *pValue) {
+    ESP_LOGD(TAG, "Read holding %d", ref);
+
+    if (ref == 1) {
+        *pValue = 1042; // device ID
+        return 0;
+    }
+    if (ref == 2) {
+        *pValue = register2;
+        return 0;
+    }
+
+    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
+}
+
+ModbusException_t wh(ModbusSlave_t *pSlave, uint16_t ref, uint16_t value) {
+    ESP_LOGD(TAG, "Write holding %d := %02x", ref, value);
+
+    if (ref == 2) {
+        register2 = value;
+        return 0;
+    }
+
+    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
+}
+
+void mbiface_setup()
+{
+    ESP_LOGI(TAG, "initing MB iface");
+    g_modbus.proto = MB_PROTO_TCP;
+    g_modbus.addr = 1;
+    g_modbus.startOfAccess = startOfAccess;
+    g_modbus.endOfAccess = endOfAccess;
+    g_modbus.readHolding = rh;
+    g_modbus.writeHolding = wh;
+
+    tcpd_config_t server_config = TCPD_INIT_DEFAULT();
+    server_config.max_clients = 1;
+    server_config.task_prio = MBIFC_TASK_PRIO;
+    server_config.task_stack = MBIFC_TASK_STACK;
+    server_config.task_name = "MBIFC";
+    server_config.port = 502;
+    server_config.read_fn = read_fn;
+
+    ESP_ERROR_CHECK(tcpd_init(&server_config, &g_mbifc_server));
+}

main/mbiface.h

@@ -0,0 +1,16 @@
+/**
+ * Modbus interface
+ */
+
+#ifndef mbiface_H
+#define mbiface_H
+
+#include "socket_server.h"
+#include "modbus.h"
+
+extern Tcpd_t g_mbifc_server;
+extern ModbusSlave_t g_modbus;
+
+void mbiface_setup();
+
+#endif //mbiface_H

main/tasks.h

@@ -15,4 +15,7 @@
 #define TELNET_TASK_STACK  4096
 #define TELNET_TASK_PRIO   PRIO_LOW
 
+#define MBIFC_TASK_STACK  4096
+#define MBIFC_TASK_PRIO   PRIO_LOW
+
 #endif //CSPEMU_PRIORITIES_H