diff --git a/lib/lib_basic/TasmotaModbus-3.5.0/src/TasmotaModbus.cpp b/lib/lib_basic/TasmotaModbus-3.5.0/src/TasmotaModbus.cpp index f543725a5..95db697f5 100644 --- a/lib/lib_basic/TasmotaModbus-3.5.0/src/TasmotaModbus.cpp +++ b/lib/lib_basic/TasmotaModbus-3.5.0/src/TasmotaModbus.cpp @@ -19,8 +19,14 @@ Documentation about modbus protocol: https://www.modbustools.com/modbus.html */ +#include "Arduino.h" #include "TasmotaModbus.h" +extern void AddLog(uint32_t loglevel, PGM_P formatP, ...); +enum LoggingLevels {LOG_LEVEL_NONE, LOG_LEVEL_ERROR, LOG_LEVEL_INFO, LOG_LEVEL_DEBUG, LOG_LEVEL_DEBUG_MORE}; + +//#define TASMOTAMODBUSDEBUG + TasmotaModbus::TasmotaModbus(int receive_pin, int transmit_pin) : TasmotaSerial(receive_pin, transmit_pin, 1) { mb_address = 0; @@ -55,18 +61,21 @@ int TasmotaModbus::Begin(long speed, uint32_t config) return result; } -uint8_t TasmotaModbus::Send(uint8_t device_address, uint8_t function_code, uint16_t start_address, uint16_t register_count, uint16_t *registers) +uint8_t TasmotaModbus::Send(uint8_t device_address, uint8_t function_code, uint16_t start_address, uint16_t count, uint16_t *write_data) { uint8_t *frame; uint8_t framepointer = 0; + uint16_t byte_count = count * 2; // In register mode count is nr of registers (2 bytes) + if ((function_code == 1) || (function_code == 2) || (function_code == 15)) byte_count = ((count-1) / 8) + 1; // In bitmode count is nr of bits + if (function_code < 7) { frame = (uint8_t *)malloc(8); // Addres(1), Function(1), Start/Coil Address(2), Registercount or Data (2), CRC(2) } else { - frame = (uint8_t *)malloc(9 + (register_count * 2)); // Addres(1), Function(1), Start/Coil Address(2),Quantity of registers (2), Bytecount(1), Data(1..n), CRC(2) + frame = (uint8_t *)malloc(9 + byte_count); // Addres(1), Function(1), Start/Coil Address(2),Quantity of registers (2), Bytecount(1), Data(1..n), CRC(2) } mb_address = device_address; // Save address for receipt check @@ -77,43 +86,44 @@ uint8_t TasmotaModbus::Send(uint8_t device_address, uint8_t function_code, uint1 frame[framepointer++] = (uint8_t)(start_address); // LSB if (function_code < 5) { - frame[framepointer++] = (uint8_t)(register_count >> 8); // MSB - frame[framepointer++] = (uint8_t)(register_count); // LSB + frame[framepointer++] = (uint8_t)(count >> 8); // MSB + frame[framepointer++] = (uint8_t)(count); // LSB } else if ((function_code == 5) || (function_code == 6)) { - if (registers == NULL) + if (write_data == NULL) { free(frame); return 13; // Register data not specified } - if (register_count != 1) + if (count != 1) { free(frame); return 12; // Wrong register count } - frame[framepointer++] = (uint8_t)(registers[0] >> 8); // MSB - frame[framepointer++] = (uint8_t)(registers[0]); // LSB + frame[framepointer++] = (uint8_t)(write_data[0] >> 8); // MSB + frame[framepointer++] = (uint8_t)(write_data[0]); // LSB } else if ((function_code == 15) || (function_code == 16)) { - frame[framepointer++] = (uint8_t)(register_count >> 8); // MSB - frame[framepointer++] = (uint8_t)(register_count); // LSB - frame[framepointer++] = register_count * 2; - if (registers == NULL) + frame[framepointer++] = (uint8_t)(count >> 8); // MSB + frame[framepointer++] = (uint8_t)(count); // LSB + + frame[framepointer++] = byte_count; + + if (write_data == NULL) { free(frame); return 13; // Register data not specified } - if (register_count == 0) + if (count == 0) { free(frame); return 12; // Wrong register count } - for (int registerpointer = 0; registerpointer < register_count; registerpointer++) + for (uint16_t bytepointer = 0; bytepointer < byte_count; bytepointer++) { - frame[framepointer++] = (uint8_t)(registers[registerpointer] >> 8); // MSB - frame[framepointer++] = (uint8_t)(registers[registerpointer]); // LSB + frame[framepointer++] = (uint8_t)(write_data[bytepointer/2] >> (bytepointer % 2 ? 0 : 8)); // MSB, LSB, MSB .... } } else @@ -126,6 +136,18 @@ uint8_t TasmotaModbus::Send(uint8_t device_address, uint8_t function_code, uint1 frame[framepointer++] = (uint8_t)(crc); frame[framepointer++] = (uint8_t)(crc >> 8); +#ifdef TASMOTAMODBUSDEBUG + uint8_t *buf; + uint16_t bufsize=(framepointer + 1) * 3; + buf = (uint8_t *)malloc(bufsize); + memset(buf, 0, bufsize); + uint16_t i; + for (i = 0; i < framepointer;i++) + snprintf((char *)&buf[i*3], (bufsize-i*3), "%02X ",frame[i]); + AddLog(LOG_LEVEL_DEBUG_MORE, PSTR("MBS: Serial Send: %s"), buf); + free(buf); +#endif + flush(); write(frame, framepointer); free(frame); @@ -137,12 +159,13 @@ bool TasmotaModbus::ReceiveReady() return (available() > 4); } -uint8_t TasmotaModbus::ReceiveBuffer(uint8_t *buffer, uint8_t data_count) +uint8_t TasmotaModbus::ReceiveBuffer(uint8_t *buffer, uint8_t register_count, uint16_t byte_count) { mb_len = 0; uint32_t timeout = millis() + 10; uint8_t header_length = 3; - while ((mb_len < (data_count * 2) + header_length + 2) && (millis() < timeout)) { + if (byte_count == 0) byte_count = (register_count * 2); + while ((mb_len < byte_count + header_length + 2) && (millis() < timeout)) { if (available()) { uint8_t data = (uint8_t)read(); if (!mb_len) { // Skip leading data as provided by hardware serial @@ -152,20 +175,6 @@ uint8_t TasmotaModbus::ReceiveBuffer(uint8_t *buffer, uint8_t data_count) } else { buffer[mb_len++] = data; if (3 == mb_len) { - if (buffer[1] & 0x80) { // 01 84 02 f2 f1 - if (0 == buffer[2]) { - return 3; // 3 = Illegal Data Value, - } - return buffer[2]; // 1 = Illegal Function, - // 2 = Illegal Data Address, - // 3 = Illegal Data Value, - // 4 = Slave Error - // 5 = Acknowledge but not finished (no error) - // 6 = Slave Busy - // 8 = Memory Parity error - // 10 = Gateway Path Unavailable - // 11 = Gateway Target device failed to respond - } if ((buffer[1] == 5) || (buffer[1] == 6) || (buffer[1] == 15) || (buffer[1] == 16)) header_length = 4; // Addr, Func, StartAddr } } @@ -175,6 +184,36 @@ uint8_t TasmotaModbus::ReceiveBuffer(uint8_t *buffer, uint8_t data_count) } } +#ifdef TASMOTAMODBUSDEBUG +// RX Logging + uint8_t *buf; + uint16_t bufsize=(mb_len + 1) * 3; + buf = (uint8_t *)malloc(bufsize); + memset(buf, 0, bufsize); + uint16_t i; + for (i = 0; i < mb_len;i++) + snprintf((char *)&buf[i*3], (bufsize-i*3), "%02X ",buffer[i]); + AddLog(LOG_LEVEL_DEBUG_MORE, PSTR("MBS: Serial Received: %s"), buf); + free(buf); +#endif + + if (buffer[1] & 0x80) + { // 01 84 02 f2 f1 + if (0 == buffer[2]) + { + return 3; // 3 = Illegal Data Value, + } + return buffer[2]; // 1 = Illegal Function, + // 2 = Illegal Data Address, + // 3 = Illegal Data Value, + // 4 = Slave Error + // 5 = Acknowledge but not finished (no error) + // 6 = Slave Busy + // 8 = Memory Parity error + // 10 = Gateway Path Unavailable + // 11 = Gateway Target device failed to respond + } + if (mb_len < 6) { return 7; } // 7 = Not enough data /* diff --git a/lib/lib_basic/TasmotaModbus-3.5.0/src/TasmotaModbus.h b/lib/lib_basic/TasmotaModbus-3.5.0/src/TasmotaModbus.h index f0122b9b0..520bb166d 100644 --- a/lib/lib_basic/TasmotaModbus-3.5.0/src/TasmotaModbus.h +++ b/lib/lib_basic/TasmotaModbus-3.5.0/src/TasmotaModbus.h @@ -53,8 +53,12 @@ class TasmotaModbus : public TasmotaSerial { * 13 = Register data not specified * 14 = To many registers */ - uint8_t Send(uint8_t device_address, uint8_t function_code, uint16_t start_address, uint16_t register_count, uint16_t *registers = NULL); - uint8_t ReceiveBuffer(uint8_t *buffer, uint8_t register_count); + uint8_t Send(uint8_t device_address, uint8_t function_code, uint16_t start_address, uint16_t count, uint16_t *writeData = NULL); + // Count is nr of registers for function code 3,4,6 and 16, nr of coils/inputs for function code 1,2 and 15 and must be 1 for function code 5 and 6 + + uint8_t ReceiveBuffer(uint8_t *buffer, uint8_t register_count, uint16_t byte_count = 0); + // Bytecount is mandatory for functioncode 1 and 2 because they can return an odd number of bytes. + uint8_t Receive8BitRegister(uint8_t *value); uint8_t Receive16BitRegister(uint16_t *value); uint8_t Receive32BitRegister(float *value); diff --git a/tasmota/tasmota_xdrv_driver/xdrv_63_modbus_bridge.ino b/tasmota/tasmota_xdrv_driver/xdrv_63_modbus_bridge.ino index ac624d0fc..479ea560b 100644 --- a/tasmota/tasmota_xdrv_driver/xdrv_63_modbus_bridge.ino +++ b/tasmota/tasmota_xdrv_driver/xdrv_63_modbus_bridge.ino @@ -26,12 +26,23 @@ * When USE_MODBUS_BRIDGE_TCP is also defined, this bridge can also be used as an ModbusTCP * bridge. * - * Example Command: + * Example Commands: + * -- Read Coils -- + * ModbusSend {"deviceaddress": 1, "functioncode": 1, "startaddress": 1, "type":"bit", "count":2} + * * -- Read Input Register -- * ModbusSend {"deviceaddress": 1, "functioncode": 3, "startaddress": 1, "type":"uint16", "count":2} - * + * * -- Write multiple coils -- - * ModbusSend {"deviceaddress": 1, "functioncode": 15, "startaddress": 1, "type":"uint16", "count":4, "values":[1,2,3,4]} + * ModbusSend {"deviceaddress": 1, "functioncode": 15, "startaddress": 1, "type":"bit", "count":4, "values":[1,0,1,1]} + * + * Info for modbusBridgeTCPServer: + * https://ipc2u.com/articles/knowledge-base/detailed-description-of-the-modbus-tcp-protocol-with-command-examples/ + * + * Info for modbus serial communications: + * https://ozeki.hu/p_5879-mobdbus-function-code-4-read-input-registers.html + * https://www.modbustools.com/modbus.html + * https://ipc2u.com/articles/knowledge-base/modbus-rtu-made-simple-with-detailed-descriptions-and-examples/ \*********************************************************************************************/ #define XDRV_63 63 @@ -110,7 +121,7 @@ enum class ModbusBridgeFunctionCode { mb_undefined = 0, mb_readCoilStatus = 1, - mb_readContactStatus = 2, + mb_readInputStatus = 2, mb_readHoldingRegisters = 3, mb_readInputRegisters = 4, mb_writeSingleCoil = 5, @@ -150,6 +161,7 @@ struct ModbusBridge ModbusBridgeType type = ModbusBridgeType::mb_undefined; uint16_t dataCount = 0; + uint16_t byteCount = 0; uint16_t startAddress = 0; uint8_t deviceAddress = 0; uint8_t count = 0; @@ -158,6 +170,24 @@ struct ModbusBridge ModbusBridge modbusBridge; +/********************************************************************************************/ +// +// Helper functions for data conversion between little and big endian +// +uint16_t swap_endian16(uint16_t num) +{ + return (num>>8) | (num<<8); +} + +uint32_t swap_endian32(uint32_t num) +{ + return ((num>>24)&0xff) | // move byte 3 to byte 0 + ((num<<8)&0xff0000) | // move byte 1 to byte 2 + ((num>>8)&0xff00) | // move byte 2 to byte 1 + ((num<<24)&0xff000000); // byte 0 to byte 3 +} + + /********************************************************************************************/ // // Applies serial configuration to modbus serial port @@ -216,8 +246,67 @@ void ModbusBridgeHandle(void) if (data_ready) { uint8_t *buffer; - buffer = (uint8_t *)malloc(9 + (modbusBridge.dataCount * 2)); // Addres(1), Function(1), Length(1), Data(1..n), CRC(2) - uint32_t error = tasmotaModbus->ReceiveBuffer(buffer, modbusBridge.dataCount); + if (modbusBridge.byteCount == 0) modbusBridge.byteCount = modbusBridge.dataCount * 2; + buffer = (uint8_t *)malloc(9 + modbusBridge.byteCount); // Addres(1), Function(1), Length(1), Data(1..n), CRC(2) + memset(buffer, 0, 9 + modbusBridge.byteCount); + uint32_t error = tasmotaModbus->ReceiveBuffer(buffer, 0, modbusBridge.byteCount); + +#ifdef USE_MODBUS_BRIDGE_TCP + for (uint32_t i = 0; i < nitems(modbusBridgeTCP.client_tcp); i++) + { + WiFiClient &client = modbusBridgeTCP.client_tcp[i]; + if (client) + { + uint8_t header[8]; + uint8_t nrOfBytes = 8; + header[0] = modbusBridgeTCP.tcp_transaction_id >> 8; + header[1] = modbusBridgeTCP.tcp_transaction_id; + header[2] = 0; + header[3] = 0; + header[6] = buffer[0]; // Send slave address + header[7] = buffer[1]; // Send function code + if (error) + { + header[4] = 0; // Message Length Hi-Byte + header[5] = 3; // Message Length Low-Byte + header[7] = buffer[1] | 0x80; // Send function code + header[8] = error; + nrOfBytes += 1; + client.write(header, 9); + } + else if (buffer[1] <= 2) + { + header[4] = modbusBridge.byteCount >> 8; + header[5] = modbusBridge.byteCount + 3; + header[8] = modbusBridge.byteCount; + client.write(header, 9); + nrOfBytes += 1; + client.write(buffer + 3, modbusBridge.byteCount); // Don't send CRC + nrOfBytes += modbusBridge.byteCount; + } + else if (buffer[1] <= 4) + { + header[4] = modbusBridge.byteCount >> 8; + header[5] = modbusBridge.byteCount + 3; + header[8] = modbusBridge.byteCount; + client.write(header, 9); + nrOfBytes += 1; + client.write(buffer + 3, modbusBridge.byteCount); // Don't send CRC + nrOfBytes += modbusBridge.byteCount; + } + else + { + header[4] = 0; // Message Length Hi-Byte + header[5] = 6; // Message Length Low-Byte + client.write(header, 8); + client.write(buffer + 2, 4); // Don't send CRC + nrOfBytes += 4; + } + client.flush(); + AddLog(LOG_LEVEL_DEBUG, PSTR("MBS: MBRTCP from Modbus deviceAddress %d, writing %d bytes to client"), buffer[0], nrOfBytes); + } + } +#endif if (error) { @@ -226,33 +315,9 @@ void ModbusBridgeHandle(void) return; } -#ifdef USE_MODBUS_BRIDGE_TCP - for (uint32_t i = 0; i < nitems(modbusBridgeTCP.client_tcp); i++) - { - WiFiClient &client = modbusBridgeTCP.client_tcp[i]; - if (client) - { - uint8_t MBAP_Header[7]; - MBAP_Header[0] = modbusBridgeTCP.tcp_transaction_id >> 8; - MBAP_Header[1] = modbusBridgeTCP.tcp_transaction_id; - MBAP_Header[2] = 0; - MBAP_Header[3] = 0; - MBAP_Header[4] = ((modbusBridge.dataCount * 2) + 3) >> 8; - MBAP_Header[5] = (modbusBridge.dataCount * 2) + 3; - MBAP_Header[6] = buffer[0]; // Send slave address - client.write(MBAP_Header, 7); - client.write(buffer + 1, 1); // Send Functioncode - uint8_t bytecount[1]; - bytecount[0] = modbusBridge.dataCount * 2; - client.write(bytecount, 1); // Send length of rtu data - client.write(buffer + 3, (modbusBridge.dataCount * 2)); // Don't send CRC - client.flush(); - AddLog(LOG_LEVEL_DEBUG, PSTR("MBS: MBRTCP from Modbus deviceAddress %d, writing %d bytes to client"), buffer[0], (modbusBridge.dataCount * 2) + 9); - } - } -#endif - + modbusBridge.byteCount = 0; ModbusBridgeError errorcode = ModbusBridgeError::noerror; + if (modbusBridge.deviceAddress == 0) { #ifdef USE_MODBUS_BRIDGE_TCP @@ -278,17 +343,13 @@ void ModbusBridgeHandle(void) } else { - if ((modbusBridge.type == ModbusBridgeType::mb_int8 || modbusBridge.type == ModbusBridgeType::mb_uint8) - && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2])) + if ((modbusBridge.type == ModbusBridgeType::mb_int8 || modbusBridge.type == ModbusBridgeType::mb_uint8) && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2])) errorcode = ModbusBridgeError::wrongdataCount; - else if ((modbusBridge.type == ModbusBridgeType::mb_bit) - && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2])) + else if ((modbusBridge.type == ModbusBridgeType::mb_bit) && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2])) errorcode = ModbusBridgeError::wrongdataCount; - else if ((modbusBridge.type == ModbusBridgeType::mb_int16 || modbusBridge.type == ModbusBridgeType::mb_uint16) - && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2])) + else if ((modbusBridge.type == ModbusBridgeType::mb_int16 || modbusBridge.type == ModbusBridgeType::mb_uint16) && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2])) errorcode = ModbusBridgeError::wrongdataCount; - else if ((modbusBridge.type == ModbusBridgeType::mb_int32 || modbusBridge.type == ModbusBridgeType::mb_uint32 || modbusBridge.type == ModbusBridgeType::mb_float) - && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2])) + else if ((modbusBridge.type == ModbusBridgeType::mb_int32 || modbusBridge.type == ModbusBridgeType::mb_uint32 || modbusBridge.type == ModbusBridgeType::mb_float) && ((uint8_t)modbusBridge.dataCount * 2 != (uint8_t)buffer[2])) errorcode = ModbusBridgeError::wrongdataCount; } } @@ -354,11 +415,13 @@ void ModbusBridgeHandle(void) char svalue[MBR_MAX_VALUE_LENGTH + 1] = ""; if (modbusBridge.type == ModbusBridgeType::mb_float) { + // Convert next 4 bytes to float float value = 0; if (buffer[1] < 3) { + // In bit mode only convert returned bytes if (buffer[2] - (count * 4)) - ((uint8_t *)&value)[0] = buffer[dataOffset + (count * 4)]; // Get int values + ((uint8_t *)&value)[0] = buffer[dataOffset + (count * 4)]; // Get float values if ((buffer[2] - (count * 4)) >> 1) ((uint8_t *)&value)[1] = buffer[dataOffset + 1 + (count * 4)]; if ((buffer[2] - (count * 4) - 1) >> 1) @@ -377,7 +440,17 @@ void ModbusBridgeHandle(void) } else if (modbusBridge.type == ModbusBridgeType::mb_bit) { - uint8_t value = (uint8_t)(buffer[dataOffset + (count >> 3)]); + uint8_t bits_left = modbusBridge.count - ((count/8) * 8); + uint8_t value = 0; + if (bits_left < 8) + { + uint8_t bits_skip = 8 - bits_left; + value = (uint8_t)(buffer[dataOffset + ((count + bits_skip) >> 3)]); + } + else + { + value = (uint8_t)(buffer[dataOffset + (count >> 3)]); + } snprintf(svalue, MBR_MAX_VALUE_LENGTH, "%d", ((value >> (count & 7)) & 1)); } else @@ -389,17 +462,17 @@ void ModbusBridgeHandle(void) if (buffer[1] < 3) { if (buffer[2] - (count * 4)) - ((uint8_t *)&value)[0] = buffer[dataOffset + (count * 4)]; // Get int values - if ((buffer[2] - (count * 4)) >> 1) + ((uint8_t *)&value)[0] = buffer[dataOffset + (count * 4)]; // Get uint values + if (buffer[2] - ((count * 4) - 1)) ((uint8_t *)&value)[1] = buffer[dataOffset + 1 + (count * 4)]; - if ((buffer[2] - (count * 4) - 1) >> 1) + if (buffer[2] - ((count * 4) - 2)) ((uint8_t *)&value)[2] = buffer[dataOffset + 2 + (count * 4)]; - if ((buffer[2] - (count * 4)) >> 2) + if (buffer[2] - ((count * 4) - 3)) ((uint8_t *)&value)[3] = buffer[dataOffset + 3 + (count * 4)]; } else { - ((uint8_t *)&value)[3] = buffer[dataOffset + (count * 4)]; // Get int values + ((uint8_t *)&value)[3] = buffer[dataOffset + (count * 4)]; // Get uint values ((uint8_t *)&value)[2] = buffer[dataOffset + 1 + (count * 4)]; ((uint8_t *)&value)[1] = buffer[dataOffset + 2 + (count * 4)]; ((uint8_t *)&value)[0] = buffer[dataOffset + 3 + (count * 4)]; @@ -417,7 +490,7 @@ void ModbusBridgeHandle(void) { if (buffer[2] - (count * 2)) ((uint8_t *)&value)[0] = buffer[dataOffset + (count * 2)]; - if ((buffer[2] - (count * 2)) >> 1) + if (buffer[2] - ((count * 2) - 1)) ((uint8_t *)&value)[1] = buffer[dataOffset + 1 + (count * 2)]; } else @@ -439,12 +512,6 @@ void ModbusBridgeHandle(void) else snprintf(svalue, MBR_MAX_VALUE_LENGTH, "%u", value); } - /* - else if (modbusBridge.type == ModbusBridgeType::mb_bit) - { - uint8_t value = (uint8_t)(buffer[dataOffset + count]); - snprintf(svalue, MBR_MAX_VALUE_LENGTH, "%d%d%d%d%d%d%d%d", ((value >> 7) & 1), ((value >> 6) & 1), ((value >> 5) & 1), ((value >> 4) & 1), ((value >> 3) & 1), ((value >> 2) & 1), ((value >> 1) & 1), (value & 1)); - }*/ } ResponseAppend_P(PSTR("%s"), svalue); if (count < data_count - 1) @@ -576,18 +643,60 @@ void ModbusTCPHandle(void) busy = true; } } - if (buf_len == 12) + if (buf_len >= 12) { uint8_t mbdeviceaddress = (uint8_t)modbusBridgeTCP.tcp_buf[6]; uint8_t mbfunctioncode = (uint8_t)modbusBridgeTCP.tcp_buf[7]; uint16_t mbstartaddress = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[8]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[9])); - modbusBridge.dataCount = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[10]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[11])); + uint16_t *writeData = NULL; + uint16_t count = 0; + modbusBridgeTCP.tcp_transaction_id = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[0]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[1])); - AddLog(LOG_LEVEL_DEBUG_MORE, PSTR("MBS: MBRTCP to Modbus Transactionid:%d, deviceAddress:%d, functionCode:%d, startAddress:%d, Count:%d"), - modbusBridgeTCP.tcp_transaction_id, mbdeviceaddress, mbfunctioncode, mbstartaddress, modbusBridge.dataCount); + if (mbfunctioncode <= 2) + { + count = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[10]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[11])); + modbusBridge.byteCount = ((count - 1) >> 3) + 1; + modbusBridge.dataCount = ((count - 1) >> 4) + 1; + } + else if (mbfunctioncode <= 4) + { + count = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[10]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[11])); + modbusBridge.byteCount = count * 2; + modbusBridge.dataCount = count; + } + else + { + // For functioncode 15 & 16 ignore bytecount, tasmotaModbus does calculate this + uint8_t dataStartByte = mbfunctioncode <= 6 ? 10 : 13; + uint16_t byteCount = (buf_len - dataStartByte); + modbusBridge.byteCount = 2; + modbusBridge.dataCount = 1; - tasmotaModbus->Send(mbdeviceaddress, mbfunctioncode, mbstartaddress, modbusBridge.dataCount); + writeData = (uint16_t *)malloc((byteCount / 2)+1); + + if ((mbfunctioncode == 15) || (mbfunctioncode == 16)) count = (uint16_t)((((uint16_t)modbusBridgeTCP.tcp_buf[10]) << 8) | ((uint16_t)modbusBridgeTCP.tcp_buf[11])); + else count = 1; + + for (uint16_t dataPointer = 0; dataPointer < byteCount; dataPointer++) + { + if (dataPointer % 2 == 0) + { + writeData[dataPointer / 2] = (uint16_t)(((uint16_t)modbusBridgeTCP.tcp_buf[dataStartByte + dataPointer]) << 8); + } + else + { + writeData[dataPointer / 2] |= ((uint16_t)modbusBridgeTCP.tcp_buf[dataStartByte + dataPointer]); + } + } + } + + AddLog(LOG_LEVEL_DEBUG_MORE, PSTR("MBS: MBRTCP to Modbus TransactionId:%d, deviceAddress:%d, functionCode:%d, startAddress:%d, count:%d, recvCount:%d, recvBytes:%d"), + modbusBridgeTCP.tcp_transaction_id, mbdeviceaddress, mbfunctioncode, mbstartaddress, count, modbusBridge.dataCount, modbusBridge.byteCount); + + tasmotaModbus->Send(mbdeviceaddress, mbfunctioncode, mbstartaddress, count, writeData); + + free(writeData); } } yield(); // avoid WDT if heavy traffic @@ -603,6 +712,7 @@ void CmndModbusBridgeSend(void) { uint16_t *writeData = NULL; uint8_t writeDataSize = 0; + bool bitMode = false; ModbusBridgeError errorcode = ModbusBridgeError::noerror; JsonParser parser(XdrvMailbox.data); @@ -615,7 +725,9 @@ void CmndModbusBridgeSend(void) modbusBridge.startAddress = root.getULong(PSTR(D_JSON_MODBUS_START_ADDRESS), 0); const char *stype = root.getStr(PSTR(D_JSON_MODBUS_TYPE), "uint8"); - modbusBridge.count = root.getUInt(PSTR(D_JSON_MODBUS_COUNT), 1); + modbusBridge.count = root.getUInt(PSTR(D_JSON_MODBUS_COUNT), 1); // Number of bits or bytes to read / write + + if ((functionCode == 1) || (functionCode == 2) || (functionCode == 15)) bitMode = true; if (modbusBridge.deviceAddress == 0) errorcode = ModbusBridgeError::wrongdeviceaddress; @@ -634,7 +746,8 @@ void CmndModbusBridgeSend(void) if (strcmp(stype, "int8") == 0) { modbusBridge.type = ModbusBridgeType::mb_int8; - modbusBridge.dataCount = ((modbusBridge.count - 1)/ 2) + 1; + modbusBridge.dataCount = bitMode ? modbusBridge.count : ((modbusBridge.count - 1) / 2) + 1; + if (bitMode) modbusBridge.byteCount = (modbusBridge.count / 8) + 1; } else if (strcmp(stype, "int16") == 0) { @@ -644,12 +757,12 @@ void CmndModbusBridgeSend(void) else if (strcmp(stype, "int32") == 0) { modbusBridge.type = ModbusBridgeType::mb_int32; - modbusBridge.dataCount = 2 * modbusBridge.count; + modbusBridge.dataCount = bitMode ? modbusBridge.count : 2 * modbusBridge.count; } else if ((strcmp(stype, "uint8") == 0)) { modbusBridge.type = ModbusBridgeType::mb_uint8; - modbusBridge.dataCount = ((modbusBridge.count - 1)/ 2) + 1; + modbusBridge.dataCount = bitMode ? modbusBridge.count : ((modbusBridge.count - 1) / 2) + 1; } else if ((strcmp(stype, "uint16") == 0) || (strcmp(stype, "") == 0)) // Default is uint16 { @@ -659,37 +772,86 @@ void CmndModbusBridgeSend(void) else if (strcmp(stype, "uint32") == 0) { modbusBridge.type = ModbusBridgeType::mb_uint32; - modbusBridge.dataCount = 2 * modbusBridge.count; + modbusBridge.dataCount = bitMode ? modbusBridge.count : 2 * modbusBridge.count; } else if (strcmp(stype, "float") == 0) { modbusBridge.type = ModbusBridgeType::mb_float; - modbusBridge.dataCount = 2 * modbusBridge.count; + modbusBridge.dataCount = bitMode ? 2 * modbusBridge.count : modbusBridge.count; } else if (strcmp(stype, "raw") == 0) { modbusBridge.type = ModbusBridgeType::mb_raw; - modbusBridge.dataCount = modbusBridge.count; + modbusBridge.dataCount = bitMode ? modbusBridge.count : ((modbusBridge.count - 1) / 2) + 1; } else if (strcmp(stype, "hex") == 0) { modbusBridge.type = ModbusBridgeType::mb_hex; - modbusBridge.dataCount = modbusBridge.count; + modbusBridge.dataCount = bitMode ? modbusBridge.count : ((modbusBridge.count - 1) / 2) + 1; } else if (strcmp(stype, "bit") == 0) { modbusBridge.type = ModbusBridgeType::mb_bit; - modbusBridge.dataCount = ((modbusBridge.count - 1) / 16) + 1; + modbusBridge.dataCount = bitMode ? modbusBridge.count : ((modbusBridge.count - 1) / 16) + 1 ; } else errorcode = ModbusBridgeError::wrongtype; - if (modbusBridge.dataCount > MBR_MAX_REGISTERS) + // If functioncode is 15, the count is not the number of registers but the number + // of bit to write, so calculate the number data bytes to write. + if (modbusBridge.functionCode == ModbusBridgeFunctionCode::mb_writeMultipleCoils) + { + modbusBridge.dataCount = modbusBridge.count; + } + + // Prevent buffer overflow due to usage of to many registers + if ((!bitMode) && (modbusBridge.dataCount > MBR_MAX_REGISTERS)) errorcode = ModbusBridgeError::wrongcount; - // If write data is specified in JSON copy it into writeData array + if ((bitMode) && (modbusBridge.dataCount > MBR_MAX_REGISTERS * 8)) + errorcode = ModbusBridgeError::wrongcount; + + // Get Json data for writing JsonParserArray jsonDataArray = root[PSTR(D_JSON_MODBUS_VALUES)].getArray(); - if (jsonDataArray.isArray()) + writeDataSize = jsonDataArray.size(); + + // Check if number of supplied data items is valid + switch (modbusBridge.functionCode) + { + case ModbusBridgeFunctionCode::mb_writeMultipleCoils: + // In writeMultipleCoil mode the amount of given data bits is less or equal to the count + switch (modbusBridge.type) + { + case ModbusBridgeType::mb_bit: + if (modbusBridge.count > writeDataSize) errorcode = ModbusBridgeError::wrongcount; + break; + case ModbusBridgeType::mb_uint8: + case ModbusBridgeType::mb_int8: + case ModbusBridgeType::mb_raw: + case ModbusBridgeType::mb_hex: + if (modbusBridge.count > writeDataSize * 8) errorcode = ModbusBridgeError::wrongcount; + break; + case ModbusBridgeType::mb_uint16: + case ModbusBridgeType::mb_int16: + if (modbusBridge.count > writeDataSize * 16) errorcode = ModbusBridgeError::wrongcount; + break; + case ModbusBridgeType::mb_uint32: + case ModbusBridgeType::mb_int32: + if (modbusBridge.count > writeDataSize * 32) errorcode = ModbusBridgeError::wrongcount; + break; + } + break; + case ModbusBridgeFunctionCode::mb_writeSingleRegister: + case ModbusBridgeFunctionCode::mb_writeSingleCoil: + if (modbusBridge.count != 1) errorcode = ModbusBridgeError::wrongcount; + break; + case ModbusBridgeFunctionCode::mb_writeMultipleRegisters: + if (modbusBridge.count != writeDataSize) errorcode = ModbusBridgeError::wrongcount; + break; + } + + // If write data is specified in JSON copy it into writeData array + if ((errorcode == ModbusBridgeError::noerror) && (jsonDataArray.isArray())) { if (modbusBridge.dataCount > 40) { @@ -697,73 +859,85 @@ void CmndModbusBridgeSend(void) } else { - writeDataSize = jsonDataArray.size(); + writeData = (uint16_t *)malloc(modbusBridge.dataCount); - if (modbusBridge.count != writeDataSize) + for (uint8_t jsonDataArrayPointer = 0; jsonDataArrayPointer < writeDataSize; jsonDataArrayPointer++) { - errorcode = ModbusBridgeError::wrongcount; - } - else - { - writeData = (uint16_t *)malloc(writeDataSize * 2); - for (uint8_t jsonDataArrayPointer = 0; jsonDataArrayPointer < writeDataSize; jsonDataArrayPointer++) + if (errorcode != ModbusBridgeError::noerror) break; + switch (modbusBridge.type) { - switch (modbusBridge.type) + case ModbusBridgeType::mb_bit: + { + // Initialize current data/register to 0 + if (jsonDataArrayPointer % 16 == 0) { - case ModbusBridgeType::mb_bit: - { - // Set 2 following bytes to 0 - if (jsonDataArrayPointer % 16 == 0) - { - writeData[jsonDataArrayPointer/15] = 0; - } - // Swap low and high bytes according to modbus specification - uint16_t bitValue = (jsonDataArray[jsonDataArrayPointer].getUInt(0) == 1) ? 1 : 0; - uint8_t bitPointer = (jsonDataArrayPointer % 15) + 8; - if (bitPointer > 15) bitPointer -= 16; - - writeData[jsonDataArrayPointer/15] += bitValue << bitPointer; - } - break; - case ModbusBridgeType::mb_int8: - if (jsonDataArrayPointer % 2) writeData[jsonDataArrayPointer / 2] += (int8_t)jsonDataArray[jsonDataArrayPointer].getInt(0); - else writeData[jsonDataArrayPointer] = (int8_t)jsonDataArray[jsonDataArrayPointer / 2].getInt(0) << 8; - break; - case ModbusBridgeType::mb_uint8: - if (jsonDataArrayPointer % 2) writeData[jsonDataArrayPointer / 2] += (uint8_t)jsonDataArray[jsonDataArrayPointer].getInt(0); - else writeData[jsonDataArrayPointer / 2] = (uint8_t)jsonDataArray[jsonDataArrayPointer].getInt(0) << 8; - break; - case ModbusBridgeType::mb_int16: - writeData[jsonDataArrayPointer] = (int16_t)jsonDataArray[jsonDataArrayPointer].getInt(0); - break; - case ModbusBridgeType::mb_uint16: - writeData[jsonDataArrayPointer] = (uint16_t)jsonDataArray[jsonDataArrayPointer].getUInt(0); - break; - case ModbusBridgeType::mb_float: - // TODO - errorcode = ModbusBridgeError::wrongtype; - break; - case ModbusBridgeType::mb_int32: - writeData[jsonDataArrayPointer++] = (int16_t)(jsonDataArray[jsonDataArrayPointer].getInt(0) >> 16); - writeData[jsonDataArrayPointer] = (uint16_t)jsonDataArray[jsonDataArrayPointer].getInt(0); - break; - case ModbusBridgeType::mb_uint32: - writeData[jsonDataArrayPointer++] = (uint16_t)(jsonDataArray[jsonDataArrayPointer].getUInt(0) >> 16); - writeData[jsonDataArrayPointer] = (uint16_t)jsonDataArray[jsonDataArrayPointer].getUInt(0); - break; - case ModbusBridgeType::mb_raw: - writeData[jsonDataArrayPointer] = (uint8_t)jsonDataArray[jsonDataArrayPointer*2].getUInt(0) << 8 + (uint8_t)jsonDataArray[(jsonDataArrayPointer*2)+1].getUInt(0); - break; - case ModbusBridgeType::mb_hex: - writeData[jsonDataArrayPointer] = (uint8_t)jsonDataArray[jsonDataArrayPointer*2].getUInt(0) << 8 + (uint8_t)jsonDataArray[(jsonDataArrayPointer*2)+1].getUInt(0); - break; - default: - errorcode = ModbusBridgeError::wrongtype; - break; + writeData[jsonDataArrayPointer / 15] = 0; } + // Swap low and high bytes according to modbus specification + uint16_t bitValue = (jsonDataArray[jsonDataArrayPointer].getUInt(0) == 1) ? 1 : 0; + uint8_t bitPointer = (jsonDataArrayPointer % 16) + 8; + if (bitPointer > 15) bitPointer -= 16; + + writeData[jsonDataArrayPointer / 16] += bitValue << bitPointer; + } + break; + + case ModbusBridgeType::mb_int8: + if (jsonDataArrayPointer % 2) + writeData[jsonDataArrayPointer / 2] += (int8_t)jsonDataArray[jsonDataArrayPointer].getInt(0); + else + writeData[jsonDataArrayPointer / 2] = (int8_t)jsonDataArray[jsonDataArrayPointer / 2].getInt(0) << 8; + if (modbusBridge.dataCount != writeDataSize / 2) errorcode = ModbusBridgeError::wrongcount; + break; + + case ModbusBridgeType::mb_hex: + case ModbusBridgeType::mb_raw: + case ModbusBridgeType::mb_uint8: + if (jsonDataArrayPointer % 2) + writeData[jsonDataArrayPointer / 2] += (uint8_t)jsonDataArray[jsonDataArrayPointer].getUInt(0); + else + writeData[jsonDataArrayPointer / 2] = (uint8_t)jsonDataArray[jsonDataArrayPointer].getUInt(0) << 8; + if (modbusBridge.dataCount != writeDataSize / 2) errorcode = ModbusBridgeError::wrongcount; + break; + + case ModbusBridgeType::mb_int16: + writeData[jsonDataArrayPointer] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getInt(0)) + : (int16_t)jsonDataArray[jsonDataArrayPointer].getInt(0); + break; + + case ModbusBridgeType::mb_uint16: + writeData[jsonDataArrayPointer] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getUInt(0)) + : (int16_t)jsonDataArray[jsonDataArrayPointer].getUInt(0); + break; + + case ModbusBridgeType::mb_int32: + writeData[(jsonDataArrayPointer * 2)] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getInt(0)) + : (int16_t)(jsonDataArray[jsonDataArrayPointer].getInt(0) >> 16); + writeData[(jsonDataArrayPointer * 2) + 1] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getInt(0) >> 16) + : (uint16_t)(jsonDataArray[jsonDataArrayPointer].getInt(0)); + break; + + case ModbusBridgeType::mb_uint32: + writeData[(jsonDataArrayPointer * 2)] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getUInt(0)) + : (uint16_t)(jsonDataArray[jsonDataArrayPointer].getUInt(0) >> 16); + writeData[(jsonDataArrayPointer * 2) + 1] = bitMode ? swap_endian16(jsonDataArray[jsonDataArrayPointer].getUInt(0) >> 16) + : (uint16_t)(jsonDataArray[jsonDataArrayPointer].getUInt(0)); + break; + + case ModbusBridgeType::mb_float: + // TODO + default: + errorcode = ModbusBridgeError::wrongtype; + break; } } } + + // Adapt data according to modbus protocol + if (modbusBridge.functionCode == ModbusBridgeFunctionCode::mb_writeSingleCoil) + { + writeData[0] = writeData[0] ? 0xFF00 : 0x0000; // High Byte + } } // Handle errorcode and exit function when an error has occured @@ -774,23 +948,18 @@ void CmndModbusBridgeSend(void) return; } - // Adapt data according to modbus protocol - for (uint8_t writeDataPointer = 0; writeDataPointer < modbusBridge.dataCount; writeDataPointer++) - { - // For function code 5, on and off are 0xFF00 and 0x0000, not 1 and 0 - if (modbusBridge.functionCode == ModbusBridgeFunctionCode::mb_writeSingleCoil) - { - writeData[writeDataPointer] = writeData[writeDataPointer] ? 0xFF00 : 0x0000; - } - } - // If writing a single coil or single register, the register count is always 1. We also prevent writing data out of range if ((modbusBridge.functionCode == ModbusBridgeFunctionCode::mb_writeSingleCoil) || (modbusBridge.functionCode == ModbusBridgeFunctionCode::mb_writeSingleRegister)) modbusBridge.dataCount = 1; uint8_t error = tasmotaModbus->Send(modbusBridge.deviceAddress, (uint8_t)modbusBridge.functionCode, modbusBridge.startAddress, modbusBridge.dataCount, writeData); - if (error) AddLog(LOG_LEVEL_DEBUG, PSTR("MBS: MBR Driver send error %u"), error); free(writeData); + + if (error) + { + AddLog(LOG_LEVEL_DEBUG, PSTR("MBS: MBR Driver send error %u"), error); + return; + } ResponseCmndDone(); }