diff --git a/plugins/modbus/modbus.c b/plugins/modbus/modbus.c index 3aceb8eb..fbf16bb6 100644 --- a/plugins/modbus/modbus.c +++ b/plugins/modbus/modbus.c @@ -107,16 +107,24 @@ int modbus_code_unwrap(neu_protocol_unpack_buf_t *buf, } void modbus_address_wrap(neu_protocol_pack_buf_t *buf, uint16_t start, - uint16_t n_register) + uint16_t n_register, enum modbus_action action) { - assert(neu_protocol_pack_buf_unused_size(buf) >= - sizeof(struct modbus_address)); - struct modbus_address *address = - (struct modbus_address *) neu_protocol_pack_buf( - buf, sizeof(struct modbus_address)); + if (action == MODBUS_ACTION_HOLD_REG_WRITE && n_register == 1) { + assert(neu_protocol_pack_buf_unused_size(buf) >= sizeof(start)); + uint16_t *address = + (uint16_t *) neu_protocol_pack_buf(buf, sizeof(start)); - address->start_address = htons(start); - address->n_reg = htons(n_register); + *address = htons(start); + } else { + assert(neu_protocol_pack_buf_unused_size(buf) >= + sizeof(struct modbus_address)); + struct modbus_address *address = + (struct modbus_address *) neu_protocol_pack_buf( + buf, sizeof(struct modbus_address)); + + address->start_address = htons(start); + address->n_reg = htons(n_register); + } } int modbus_address_unwrap(neu_protocol_unpack_buf_t *buf, @@ -138,7 +146,7 @@ int modbus_address_unwrap(neu_protocol_unpack_buf_t *buf, } void modbus_data_wrap(neu_protocol_pack_buf_t *buf, uint8_t n_byte, - uint8_t *bytes) + uint8_t *bytes, enum modbus_action action) { assert(neu_protocol_pack_buf_unused_size(buf) >= sizeof(struct modbus_data) + n_byte); @@ -146,10 +154,13 @@ void modbus_data_wrap(neu_protocol_pack_buf_t *buf, uint8_t n_byte, memcpy(data, bytes, n_byte); - struct modbus_data *mdata = (struct modbus_data *) neu_protocol_pack_buf( - buf, sizeof(struct modbus_data)); + if (action == MODBUS_ACTION_DEFAULT || n_byte > 2) { + struct modbus_data *mdata = + (struct modbus_data *) neu_protocol_pack_buf( + buf, sizeof(struct modbus_data)); - mdata->n_byte = n_byte; + mdata->n_byte = n_byte; + } } int modbus_data_unwrap(neu_protocol_unpack_buf_t *buf, diff --git a/plugins/modbus/modbus.h b/plugins/modbus/modbus.h index ee1e871c..176519f7 100644 --- a/plugins/modbus/modbus.h +++ b/plugins/modbus/modbus.h @@ -34,6 +34,11 @@ /* big-endian order */ +typedef enum modbus_action { + MODBUS_ACTION_DEFAULT = 0, + MODBUS_ACTION_HOLD_REG_WRITE = 1 +} modbus_action_e; + typedef enum modbus_function { MODBUS_READ_COIL = 0x1, MODBUS_READ_INPUT = 0x02, @@ -78,7 +83,7 @@ struct modbus_address { } __attribute__((packed)); void modbus_address_wrap(neu_protocol_pack_buf_t *buf, uint16_t start, - uint16_t n_register); + uint16_t n_register, enum modbus_action m_action); int modbus_address_unwrap(neu_protocol_unpack_buf_t *buf, struct modbus_address * out_address); @@ -88,7 +93,7 @@ struct modbus_data { } __attribute__((packed)); void modbus_data_wrap(neu_protocol_pack_buf_t *buf, uint8_t n_byte, - uint8_t *bytes); + uint8_t *bytes, enum modbus_action action); int modbus_data_unwrap(neu_protocol_unpack_buf_t *buf, struct modbus_data * out_data); diff --git a/plugins/modbus/modbus_stack.c b/plugins/modbus/modbus_stack.c index c796d45a..33cda864 100644 --- a/plugins/modbus/modbus_stack.c +++ b/plugins/modbus/modbus_stack.c @@ -138,16 +138,17 @@ int modbus_stack_read(modbus_stack_t *stack, uint8_t slave_id, enum modbus_area area, uint16_t start_address, uint16_t n_reg, uint16_t *response_size) { - static __thread uint8_t buf[16] = { 0 }; - static __thread neu_protocol_pack_buf_t pbuf = { 0 }; - int ret = 0; + static __thread uint8_t buf[16] = { 0 }; + static __thread neu_protocol_pack_buf_t pbuf = { 0 }; + int ret = 0; + modbus_action_e m_action = MODBUS_ACTION_DEFAULT; neu_protocol_pack_buf_init(&pbuf, buf, sizeof(buf)); if (stack->protocol == MODBUS_PROTOCOL_RTU) { modbus_crc_wrap(&pbuf); } - modbus_address_wrap(&pbuf, start_address, n_reg); + modbus_address_wrap(&pbuf, start_address, n_reg, m_action); switch (area) { case MODBUS_AREA_COIL: @@ -196,7 +197,8 @@ int modbus_stack_write(modbus_stack_t *stack, void *req, uint8_t slave_id, uint16_t n_reg, uint8_t *bytes, uint8_t n_byte, uint16_t *response_size) { - static __thread neu_protocol_pack_buf_t pbuf = { 0 }; + static __thread neu_protocol_pack_buf_t pbuf = { 0 }; + modbus_action_e m_action = MODBUS_ACTION_DEFAULT; memset(stack->buf, 0, stack->buf_size); neu_protocol_pack_buf_init(&pbuf, stack->buf, stack->buf_size); @@ -208,16 +210,19 @@ int modbus_stack_write(modbus_stack_t *stack, void *req, uint8_t slave_id, switch (area) { case MODBUS_AREA_COIL: if (*bytes > 0) { - modbus_address_wrap(&pbuf, start_address, 0xff00); + modbus_address_wrap(&pbuf, start_address, 0xff00, m_action); } else { - modbus_address_wrap(&pbuf, start_address, 0); + modbus_address_wrap(&pbuf, start_address, 0, m_action); } 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); + m_action = MODBUS_ACTION_HOLD_REG_WRITE; + modbus_data_wrap(&pbuf, n_byte, bytes, m_action); + modbus_address_wrap(&pbuf, start_address, n_reg, m_action); + modbus_code_wrap(&pbuf, slave_id, + n_reg > 1 ? MODBUS_WRITE_M_HOLD_REG + : MODBUS_WRITE_S_HOLD_REG); break; default: stack->write_resp(stack->ctx, req, NEU_ERR_PLUGIN_TAG_NOT_ALLOW_WRITE); diff --git a/simulator/modbus_s.c b/simulator/modbus_s.c index 8d54c051..d6fb9c15 100644 --- a/simulator/modbus_s.c +++ b/simulator/modbus_s.c @@ -134,6 +134,7 @@ ssize_t modbus_s_rtu_req(uint8_t *req, uint16_t req_len, uint8_t *res, break; case MODBUS_WRITE_S_COIL: case MODBUS_WRITE_M_COIL: + case MODBUS_WRITE_S_HOLD_REG: case MODBUS_WRITE_M_HOLD_REG: modbus_write(&tcp_registers[code->slave_id - 1], code->function, address, data, value); @@ -214,12 +215,14 @@ ssize_t modbus_s_tcp_req(uint8_t *req, uint16_t req_len, uint8_t *res, break; case MODBUS_WRITE_S_COIL: case MODBUS_WRITE_M_COIL: + case MODBUS_WRITE_S_HOLD_REG: case MODBUS_WRITE_M_HOLD_REG: nlog_info( "write register: slave: %d, function: %d, address: %d, n: %d, " "nbyte: %d\n", code->slave_id, code->function, ntohs(address->start_address), - ntohs(address->n_reg), data->n_byte); + code->function != MODBUS_WRITE_S_HOLD_REG ? ntohs(address->n_reg) : 1, + code->function != MODBUS_WRITE_S_HOLD_REG ? data->n_byte : 2); modbus_write(&tcp_registers[code->slave_id - 1], code->function, address, data, value); *res_address = *address; @@ -322,6 +325,14 @@ static void modbus_write(struct modbus_register *reg, uint8_t function, } } break; + case MODBUS_WRITE_S_HOLD_REG: + reg->hold_register[ntohs(address->start_address)].value = 0; + + reg->hold_register[ntohs(address->start_address)].value |= + *(uint8_t *) &((struct modbus_data *) &address->n_reg)[0] << 8; + reg->hold_register[ntohs(address->start_address)].value |= + *(uint8_t *) &((struct modbus_data *) &address->n_reg)[1]; + break; case MODBUS_WRITE_M_HOLD_REG: for (int i = 0; i < data->n_byte; i++) { reg->hold_register[ntohs(address->start_address) + i / 2].value = 0;