From e4b2eaadeb3bd19413e7e80f5d3ef7e57cf6f239 Mon Sep 17 00:00:00 2001
From: fengzero <fengzero@fengzero.com>
Date: Mon, 18 Jul 2022 09:51:56 +0000
Subject: [PATCH] modbus: write coil using 0x05

---
 plugins/modbus/modbus_stack.c | 15 +++++++++------
 simulator/modbus_s.c          |  9 +++++++++
 2 files changed, 18 insertions(+), 6 deletions(-)

diff --git a/plugins/modbus/modbus_stack.c b/plugins/modbus/modbus_stack.c
index 1d569daf..123fe0dc 100644
--- a/plugins/modbus/modbus_stack.c
+++ b/plugins/modbus/modbus_stack.c
@@ -85,6 +85,7 @@ int modbus_stack_recv(modbus_stack_t *stack, neu_protocol_unpack_buf_t *buf)
 
         break;
     }
+    case MODBUS_WRITE_S_COIL:
     case MODBUS_WRITE_M_HOLD_REG:
     case MODBUS_WRITE_M_COIL: {
         struct modbus_address address = { 0 };
@@ -98,7 +99,6 @@ int modbus_stack_recv(modbus_stack_t *stack, neu_protocol_unpack_buf_t *buf)
         stack->write_resp(stack->ctx, write_req, 0);
         break;
     }
-    case MODBUS_WRITE_S_COIL:
     case MODBUS_WRITE_S_HOLD_REG:
     default:
         break;
@@ -157,15 +157,18 @@ int modbus_stack_write(modbus_stack_t *stack, void *req, uint8_t slave_id,
 
     neu_protocol_pack_buf_init(&pbuf, buf, sizeof(buf));
 
-    modbus_data_wrap(&pbuf, n_byte, bytes);
-
-    modbus_address_wrap(&pbuf, start_address, n_reg);
-
     switch (area) {
     case MODBUS_AREA_COIL:
-        modbus_code_wrap(&pbuf, slave_id, MODBUS_WRITE_M_COIL);
+        if (*bytes > 0) {
+            modbus_address_wrap(&pbuf, start_address, 0xff00);
+        } else {
+            modbus_address_wrap(&pbuf, start_address, 0);
+        }
+        modbus_code_wrap(&pbuf, slave_id, MODBUS_WRITE_S_COIL);
         break;
     case MODBUS_AREA_HOLD_REGISTER:
+        modbus_data_wrap(&pbuf, n_byte, bytes);
+        modbus_address_wrap(&pbuf, start_address, n_reg);
         modbus_code_wrap(&pbuf, slave_id, MODBUS_WRITE_M_HOLD_REG);
         break;
     default:
diff --git a/simulator/modbus_s.c b/simulator/modbus_s.c
index 9aeba88f..53380796 100644
--- a/simulator/modbus_s.c
+++ b/simulator/modbus_s.c
@@ -125,6 +125,7 @@ ssize_t modbus_s_rtu_req(uint8_t *req, uint16_t req_len, uint8_t *res,
 
         crc = (uint16_t *) (res_value + len);
         break;
+    case MODBUS_WRITE_S_COIL:
     case MODBUS_WRITE_M_COIL:
     case MODBUS_WRITE_M_HOLD_REG:
         modbus_write(&tcp_registers[code->slave_id - 1], code->function,
@@ -200,6 +201,7 @@ ssize_t modbus_s_tcp_req(uint8_t *req, uint16_t req_len, uint8_t *res,
         res_data->n_byte = len;
         res_header->len += sizeof(struct modbus_data) + len;
         break;
+    case MODBUS_WRITE_S_COIL:
     case MODBUS_WRITE_M_COIL:
     case MODBUS_WRITE_M_HOLD_REG:
         nlog_info(
@@ -290,6 +292,13 @@ static void modbus_write(struct modbus_register *reg, uint8_t function,
 {
     pthread_mutex_lock(&reg->mutex);
     switch (function) {
+    case MODBUS_WRITE_S_COIL:
+        if (address->n_reg > 0) {
+            reg->coil[ntohs(address->start_address)].value = 1;
+        } else {
+            reg->coil[ntohs(address->start_address)].value = 0;
+        }
+        break;
     case MODBUS_WRITE_M_COIL:
         for (int i = 0; i < ntohs(address->n_reg); i++) {
             uint8_t x = (value[ntohs(address->n_reg) / 8] >> (i % 8)) & 0x1;