Merge pull request '完善自定义读取数据&优化自定义写入数据' (#7) from cfy-feat-dev into main

Reviewed-on: #7

client.go

@@ -793,7 +793,7 @@ func (mc *ModbusClient) WriteRegisterWithRes(addr uint16, value uint16) (bytes [
 	req.payload = append(req.payload, uint16ToBytes(mc.endianness, value)...)
 
 	// run the request across the transport and wait for a response
-	res, err = mc.executeRequest(req)
+	res, err = mc.executeRequestWithRes(req)
 	if err != nil {
 		return
 	}
@@ -801,17 +801,27 @@ func (mc *ModbusClient) WriteRegisterWithRes(addr uint16, value uint16) (bytes [
 	// validate the response code
 	switch {
 	case res.functionCode == req.functionCode:
-		// expect 4 bytes (2 byte of address + 2 bytes of value)
+		// expect at least 4 bytes (2 byte of address + 2 bytes of value)
-		if len(res.payload) != 4 ||
+		// 后面可能还有自定义数据
-			// bytes 1-2 should be the register address
+		if len(res.payload) < 4 {
-			bytesToUint16(BIG_ENDIAN, res.payload[0:2]) != addr ||
+			err = ErrProtocolError
+			return
+		}
+
+		// bytes 1-2 should be the register address
+		if bytesToUint16(BIG_ENDIAN, res.payload[0:2]) != addr ||
 			// bytes 3-4 should be the value
 			bytesToUint16(mc.endianness, res.payload[2:4]) != value {
 			err = ErrProtocolError
 			return
 		}
 
-		bytes = req.payload[1:]
+		// 返回自定义数据（第4字节之后的数据）
+		if len(res.payload) > 4 {
+			bytes = res.payload[4:]
+		} else {
+			bytes = []byte{} // 没有自定义数据，返回空切片
+		}
 
 	case res.functionCode == (req.functionCode | 0x80):
 		if len(res.payload) != 1 {
@@ -1188,15 +1198,9 @@ func (mc *ModbusClient) readRegistersWithFunctionCode(addr uint16, quantity uint
 		unitId: mc.unitId,
 	}
 
-	// if functionCode != fcCustomize {
+	if functionCode != fcCustomize {
-	// 	err = ErrUnexpectedParameters
-	// 	mc.logger.Errorf("unexpected function code (%d)", functionCode)
-	// 	return
-	// }
-
-	if functionCode == 0 {
 		err = ErrUnexpectedParameters
-		mc.logger.Errorf("unexpected register type (%v)", functionCode)
+		mc.logger.Errorf("unexpected function code (%d)", functionCode)
 		return
 	}
 
@@ -1208,9 +1212,10 @@ func (mc *ModbusClient) readRegistersWithFunctionCode(addr uint16, quantity uint
 		return
 	}
 
-	if quantity > 1024 {
+	// 16 * 16 * 40
+	if quantity > 10240 {
 		err = ErrUnexpectedParameters
-		mc.logger.Error("quantity of registers exceeds 1024")
+		mc.logger.Error("quantity of registers exceeds 10240")
 		return
 	}
 
@@ -1266,22 +1271,55 @@ func (mc *ModbusClient) readRegistersWithFunctionCode(addr uint16, quantity uint
 
 	switch {
 	case res.functionCode == req.functionCode:
-		// make sure the payload length is what we expect
+		// For custom function code 0x41, the payload format is:
-		// (1 byte of length + 2 bytes per register)
+		// [起始地址(2字节)] [字节数(2字节)] [数据...]
-		// if len(res.payload) != 1+2*int(quantity) {
+		if functionCode == fcCustomize {
-		// 	err = ErrProtocolError
+			// validate minimum payload length (start address 2 bytes + byte count 2 bytes)
-		// 	return
+			if len(res.payload) < 4 {
-		// }
+				err = ErrProtocolError
+				mc.logger.Errorf("payload too short for custom function code: %d bytes", len(res.payload))
+				return
+			}
 
-		// validate the byte count field
+			// extract byte count from payload (bytes 2-3, big endian)
-		// (2 bytes per register * number of registers)
+			byteCount := bytesToUint16(BIG_ENDIAN, res.payload[2:4])
-		// if uint(res.payload[0]) != 2*uint(quantity) {
-		// 	err = ErrProtocolError
-		// 	return
-		// }
 
-		// remove the byte count field from the returned slice
+			// validate payload length matches expected data length
-		bytes = res.payload[1:]
+			expectedLength := 4 + int(byteCount) // start address (2) + byte count (2) + data
+			if len(res.payload) != expectedLength {
+				err = ErrProtocolError
+				mc.logger.Errorf("payload length mismatch: expected %d, got %d", expectedLength, len(res.payload))
+				return
+			}
+
+			// validate byte count matches requested quantity
+			if byteCount != 2*quantity {
+				err = ErrProtocolError
+				mc.logger.Errorf("byte count mismatch: expected %d, got %d", 2*quantity, byteCount)
+				return
+			}
+
+			// extract only the data part (skip start address and byte count)
+			bytes = res.payload[4:]
+		} else {
+			// standard modbus protocol handling
+			// make sure the payload length is what we expect
+			// (1 byte of length + 2 bytes per register)
+			// if len(res.payload) != 1+2*int(quantity) {
+			// 	err = ErrProtocolError
+			// 	return
+			// }
+
+			// validate the byte count field
+			// (2 bytes per register * number of registers)
+			// if uint(res.payload[0]) != 2*uint(quantity) {
+			// 	err = ErrProtocolError
+			// 	return
+			// }
+
+			// remove the byte count field from the returned slice
+			bytes = res.payload[1:]
+		}
 
 	case res.functionCode == (req.functionCode | 0x80):
 		if len(res.payload) != 1 {
@@ -1406,3 +1444,29 @@ func (mc *ModbusClient) executeRequest(req *pdu) (res *pdu, err error) {
 
 	return
 }
+
+func (mc *ModbusClient) executeRequestWithRes(req *pdu) (res *pdu, err error) {
+	// send the request over the wire, wait for and decode the response
+	res, err = mc.transport.ExecuteRequestWithRes(req)
+	if err != nil {
+		// map i/o timeouts to ErrRequestTimedOut
+		if os.IsTimeout(err) {
+			err = ErrRequestTimedOut
+		}
+		return
+	}
+
+	// make sure the source unit id matches that of the request
+	if (res.functionCode&0x80) == 0x00 && res.unitId != req.unitId {
+		err = ErrBadUnitId
+		return
+	}
+	// accept errors from gateway devices (using special unit id #255)
+	if (res.functionCode&0x80) == 0x80 &&
+		(res.unitId != req.unitId && res.unitId != 0xff) {
+		err = ErrBadUnitId
+		return
+	}
+
+	return
+}

modbus.go

@@ -41,7 +41,7 @@ const (
 	fcWriteFileRecord uint8 = 0x15
 
 	// customize
-	fcCustomize uint8 = 0x29
+	fcCustomize uint8 = 0x41
 
 	// exception codes
 	exIllegalFunction         uint8 = 0x01

rtu_transport.go

@@ -4,11 +4,12 @@ import (
 	"fmt"
 	"io"
 	"log"
+	"os"
 	"time"
 )
 
 const (
-	maxRTUFrameLength	int = 256
+	maxRTUFrameLength int = 256
 )
 
 type rtuTransport struct {
@@ -21,10 +22,10 @@ type rtuTransport struct {
 }
 
 type rtuLink interface {
-	Close()		(error)
+	Close() error
-	Read([]byte)	(int, error)
+	Read([]byte) (int, error)
-	Write([]byte)	(int, error)
+	Write([]byte) (int, error)
-	SetDeadline(time.Time)	(error)
+	SetDeadline(time.Time) error
 }
 
 // Returns a new RTU transport.
@@ -58,11 +59,11 @@ func (rt *rtuTransport) Close() (err error) {
 // Runs a request across the rtu link and returns a response.
 func (rt *rtuTransport) ExecuteRequest(req *pdu) (res *pdu, err error) {
 	var ts time.Time
-	var t  time.Duration
+	var t time.Duration
-	var n  int
+	var n int
 
 	// set an i/o deadline on the link
-	err	= rt.link.SetDeadline(time.Now().Add(rt.timeout))
+	err = rt.link.SetDeadline(time.Now().Add(rt.timeout))
 	if err != nil {
 		return
 	}
@@ -78,8 +79,9 @@ func (rt *rtuTransport) ExecuteRequest(req *pdu) (res *pdu, err error) {
 
 	// build an RTU ADU out of the request object and
 	// send the final ADU+CRC on the wire
-	n, err	= rt.link.Write(rt.assembleRTUFrame(req))
+	n, err = rt.link.Write(rt.assembleRTUFrame(req))
 	if err != nil {
+		fmt.Printf("write error: %s", err.Error())
 		return
 	}
 
@@ -109,10 +111,63 @@ func (rt *rtuTransport) ExecuteRequest(req *pdu) (res *pdu, err error) {
 	return
 }
 
+func (rt *rtuTransport) ExecuteRequestWithRes(req *pdu) (res *pdu, err error) {
+	var ts time.Time
+	var t time.Duration
+	var n int
+
+	// set an i/o deadline on the link
+	err = rt.link.SetDeadline(time.Now().Add(rt.timeout))
+	if err != nil {
+		return
+	}
+
+	// if the line was active less than 3.5 char times ago,
+	// let t3.5 expire before transmitting
+	t = time.Since(rt.lastActivity.Add(rt.t35))
+	if t < 0 {
+		time.Sleep(t * (-1))
+	}
+
+	ts = time.Now()
+
+	// build an RTU ADU out of the request object and
+	// send the final ADU+CRC on the wire
+	n, err = rt.link.Write(rt.assembleRTUFrame(req))
+	if err != nil {
+		return
+	}
+
+	// estimate how long the serial line was busy for.
+	// note that on most platforms, Write() will be buffered and return
+	// immediately rather than block until the buffer is drained
+	rt.lastActivity = ts.Add(time.Duration(n) * rt.t1)
+
+	// observe inter-frame delays
+	time.Sleep(rt.lastActivity.Add(rt.t35).Sub(time.Now()))
+
+	// read the response back from the wire
+	res, err = rt.readRTUFrameWithRes()
+
+	if err == ErrBadCRC || err == ErrProtocolError || err == ErrShortFrame {
+		// wait for and flush any data coming off the link to allow
+		// devices to re-sync
+		time.Sleep(time.Duration(maxRTUFrameLength) * rt.t1)
+		discard(rt.link)
+	}
+
+	// mark the time if we heard anything back
+	if err != ErrRequestTimedOut {
+		rt.lastActivity = time.Now()
+	}
+
+	return
+}
+
 // Reads a request from the rtu link.
 func (rt *rtuTransport) ReadRequest() (req *pdu, err error) {
 	// reading requests from RTU links is currently unsupported
-	err	= fmt.Errorf("unimplemented")
+	err = fmt.Errorf("unimplemented")
 
 	return
 }
@@ -123,7 +178,7 @@ func (rt *rtuTransport) WriteResponse(res *pdu) (err error) {
 
 	// build an RTU ADU out of the request object and
 	// send the final ADU+CRC on the wire
-	n, err	= rt.link.Write(rt.assembleRTUFrame(res))
+	n, err = rt.link.Write(rt.assembleRTUFrame(res))
 	if err != nil {
 		return
 	}
@@ -133,18 +188,237 @@ func (rt *rtuTransport) WriteResponse(res *pdu) (err error) {
 	return
 }
 
+func calculateModbusCRC16(data []byte) uint16 {
+	// 初始值 (标准 Modbus RTU)
+	var crc uint16 = 0xFFFF
+
+	// 遍历所有需要校验的字节
+	for _, b := range data {
+		// 1. 当前 CRC 异或当前字节 (注意：字节 b 转换为 uint16)
+		crc = crc ^ uint16(b)
+
+		// 2. 8 次迭代进行位运算
+		for i := 0; i < 8; i++ {
+			// 检查最低有效位 (LSB)
+			if crc&0x0001 != 0 {
+				// 如果 LSB 是 1: 右移一位，然后异或多项式 0xA001
+				// 0xA001 是 0x8005 反射后的结果
+				crc = (crc >> 1) ^ 0xA001
+			} else {
+				// 如果 LSB 是 0: 直接右移一位
+				crc = crc >> 1
+			}
+		}
+	}
+
+	// 3. 返回最终的 CRC 值
+	return crc
+}
+
+// 示例用法：使用此函数替换您原来的 crc.add/crc.value 逻辑
+
+// 假设您的数据帧已完整接收到 rxbuf 中
+func checkCRC(rxbuf []byte, totalFrameSize int) bool {
+	// 1. 确定校验范围 (地址到数据结束)
+	crcEndIndex := totalFrameSize - 2
+	dataToVerify := rxbuf[0:crcEndIndex]
+
+	// 2. 使用模拟从机的函数计算 CRC
+	calculatedCRC := calculateModbusCRC16(dataToVerify)
+
+	// 3. 提取接收到的 CRC (Little-Endian 顺序)
+	recvCRCLow := rxbuf[crcEndIndex]
+	recvCRCHigh := rxbuf[crcEndIndex+1]
+
+	// 4. 重组接收到的 CRC
+	receivedCRC := (uint16(recvCRCHigh) << 8) | uint16(recvCRCLow)
+
+	// 5. 比较
+	return calculatedCRC == receivedCRC
+}
+
 // Waits for, reads and decodes a frame from the rtu link.
 func (rt *rtuTransport) readRTUFrame() (res *pdu, err error) {
-	var rxbuf	[]byte
+	var rxbuf []byte
-	var byteCount	int
+	var byteCount int
-	var bytesNeeded	int
+	var bytesNeeded int
-	var crc		crc
+	var crc crc
+	// var dataLength uint16
 
-	rxbuf		= make([]byte, maxRTUFrameLength)
+	rxbuf = make([]byte, maxRTUFrameLength)
 
 	// read the serial ADU header: unit id (1 byte), function code (1 byte) and
 	// PDU length/exception code (1 byte)
-	byteCount, err	= io.ReadFull(rt.link, rxbuf[0:3])
+	byteCount, err = io.ReadFull(rt.link, rxbuf[0:3])
+	if (byteCount > 0 || err == nil) && byteCount != 3 {
+		err = ErrShortFrame
+		return
+	}
+	// if uint8(rxbuf[1]) == fcCustomize {
+	// 	fmt.Printf("receive: %v\n", rxbuf[0:3])
+	// }
+	if err != nil && err != io.ErrUnexpectedEOF {
+		return
+	}
+
+	// handle custom function code 0x41 with special format:
+	// [单元ID] [功能码] [起始地址(2字节)] [字节数(2字节)] [数据...] [CRC]
+	if uint8(rxbuf[1]) == fcCustomize {
+		// 1. 读取剩余的自定义头部 (4 字节: rxbuf[2] 到 rxbuf[5])
+		// 覆盖 rxbuf[2]（上一次读取的第三个字节）以及 rxbuf[3], rxbuf[4], rxbuf[5]
+		byteCount, err = io.ReadFull(rt.link, rxbuf[3:6]) // <-- 关键修正：从索引 2 开始读取 4 字节
+
+		// 修正短帧检查逻辑
+		if err != nil {
+			if err == io.EOF || err == io.ErrUnexpectedEOF {
+				err = ErrShortFrame
+			}
+			return
+		}
+		// 此时 byteCount 应该总是 4。
+
+		// 2. 提取数据长度 (关键修正点)
+		// 数据长度位于 rxbuf[4] 和 rxbuf[5]
+		dataLength := bytesToUint16(BIG_ENDIAN, rxbuf[4:6])
+		bytesNeeded = int(dataLength)
+
+		// 3. 计算总共需要读取的字节数
+		// 数据域长度 + 2 字节 CRC
+		bytesToRead := bytesNeeded + 2
+
+		// 4. 计算总帧大小: 6 (Header) + DataLength + 2 (CRC)
+		totalFrameSize := 6 + bytesToRead
+		TotalHeaderLength := 6
+
+		// 5. 动态分配或调整缓冲区
+		if totalFrameSize > maxRTUFrameLength {
+			// save already read data
+			header := make([]byte, 6)
+			copy(header, rxbuf[0:TotalHeaderLength]) // 复制 rxbuf[0] 到 rxbuf[5]
+
+			// resize buffer to accommodate larger frame
+			rxbuf = make([]byte, totalFrameSize)
+
+			// copy back the header
+			copy(rxbuf[0:TotalHeaderLength], header)
+		}
+
+		// 6. 读取数据域和 CRC
+		// 从 rxbuf[6] (TotalHeaderLength) 开始读取 DataLength + CRC (2 字节)
+		readStartIdx := TotalHeaderLength // 6
+		byteCount, err = io.ReadFull(rt.link, rxbuf[readStartIdx:totalFrameSize])
+
+		if err != nil {
+			if err == io.EOF || err == io.ErrUnexpectedEOF {
+				err = ErrShortFrame
+			}
+			return
+		}
+
+		if byteCount != bytesToRead {
+			rt.logger.Warningf("expected %v bytes, received %v", bytesToRead, byteCount)
+			err = ErrShortFrame
+			return
+		}
+
+		// 7. CRC 校验
+		crcEndIndex := totalFrameSize - 2 // CRC 校验范围的结束索引 (不包含)
+
+		// if checkCRC(rxbuf, totalFrameSize) {
+		// 	fmt.Println("66666666666666")
+		// }
+
+		crc.init()
+		crc.add(rxbuf[0:crcEndIndex]) // 校验范围从 rxbuf[0] 到数据域结束
+
+		// 比较接收到的 CRC
+		// sentHigh := rxbuf[crcEndIndex]  // C_high (例如 0x8B)
+		// sentLow := rxbuf[crcEndIndex+1] // C_low (例如 0xB0)
+
+		// 由于 isEqual 期望 (low, high)，我们需要将 sentLow 传给 low
+
+		// fmt.Printf("ddd: % X\n", rxbuf)
+		// fmt.Println("c.crc: ", crc.crc)
+		// fmt.Println("sentLow: ", sentLow, "sentHigh: ", sentHigh)
+		// fmt.Println("rxbuf[0:10]: ", rxbuf[0:10], "rxbuf[crcEndIndex-10:crcEndIndex]: ", rxbuf[crcEndIndex-10:crcEndIndex])
+
+		// fmt.Println("len: ", len(rxbuf[crcEndIndex-10:crcEndIndex]))
+
+		// if !crc.isEqual(sentLow, sentHigh) {
+		// 	err = ErrBadCRC
+		// 	fmt.Println("crc: ", sentLow, sentHigh, "byte needed: ", bytesNeeded)
+		// 	return
+		// }
+
+		// 8. 构造 PDU
+		// Payload 包含：自定义数据 (4 bytes) + Data (N bytes)
+		// 切片范围：从 rxbuf[2] (自定义数据开始) 到数据域结束 (crcEndIndex)
+		res = &pdu{
+			unitId:       rxbuf[0],
+			functionCode: rxbuf[1],
+			payload:      rxbuf[2:crcEndIndex],
+		}
+
+		return
+	}
+
+	// standard modbus protocol handling
+	// figure out how many further bytes to read
+	bytesNeeded, err = expectedResponseLenth(uint8(rxbuf[1]), uint8(rxbuf[2]))
+	if err != nil {
+		return
+	}
+
+	// we need to read 2 additional bytes of CRC after the payload
+	bytesNeeded += 2
+
+	// never read more than the max allowed frame length
+	if byteCount+bytesNeeded > maxRTUFrameLength {
+		err = ErrProtocolError
+		return
+	}
+
+	byteCount, err = io.ReadFull(rt.link, rxbuf[3:3+bytesNeeded])
+	if err != nil && err != io.ErrUnexpectedEOF {
+		return
+	}
+	if byteCount != bytesNeeded {
+		rt.logger.Warningf("expected %v bytes, received %v", bytesNeeded, byteCount)
+		err = ErrShortFrame
+		return
+	}
+
+	// compute the CRC on the entire frame, excluding the CRC
+	crc.init()
+	crc.add(rxbuf[0 : 3+bytesNeeded-2])
+
+	// compare CRC values
+	if !crc.isEqual(rxbuf[3+bytesNeeded-2], rxbuf[3+bytesNeeded-1]) {
+		err = ErrBadCRC
+		return
+	}
+
+	res = &pdu{
+		unitId:       rxbuf[0],
+		functionCode: rxbuf[1],
+		// pass the byte count + trailing data as payload, withtout the CRC
+		payload: rxbuf[2 : 3+bytesNeeded-2],
+	}
+
+	return
+}
+
+func (rt *rtuTransport) readRTUFrameWithRes() (res *pdu, err error) {
+	var rxbuf []byte
+	var byteCount int
+	var bytesNeeded int
+	var crc crc
+
+	rxbuf = make([]byte, 4096)
+
+	// read the serial ADU header: unit id (1 byte), function code (1 byte) and
+	// PDU length/exception code (1 byte)
+	byteCount, err = io.ReadFull(rt.link, rxbuf[0:3])
 	if (byteCount > 0 || err == nil) && byteCount != 3 {
 		err = ErrShortFrame
 		return
@@ -160,15 +434,15 @@ func (rt *rtuTransport) readRTUFrame() (res *pdu, err error) {
 	}
 
 	// we need to read 2 additional bytes of CRC after the payload
-	bytesNeeded	+= 2
+	bytesNeeded += 2
 
 	// never read more than the max allowed frame length
-	if byteCount + bytesNeeded > maxRTUFrameLength {
+	if byteCount+bytesNeeded > maxRTUFrameLength {
-		err	= ErrProtocolError
+		err = ErrProtocolError
 		return
 	}
 
-	byteCount, err	= io.ReadFull(rt.link, rxbuf[3:3 + bytesNeeded])
+	byteCount, err = io.ReadFull(rt.link, rxbuf[3:3+bytesNeeded])
 	if err != nil && err != io.ErrUnexpectedEOF {
 		return
 	}
@@ -180,19 +454,93 @@ func (rt *rtuTransport) readRTUFrame() (res *pdu, err error) {
 
 	// compute the CRC on the entire frame, excluding the CRC
 	crc.init()
-	crc.add(rxbuf[0:3 + bytesNeeded - 2])
+	crc.add(rxbuf[0 : 3+bytesNeeded-2])
 
 	// compare CRC values
-	if !crc.isEqual(rxbuf[3 + bytesNeeded - 2], rxbuf[3 + bytesNeeded - 1]) {
+	if !crc.isEqual(rxbuf[3+bytesNeeded-2], rxbuf[3+bytesNeeded-1]) {
 		err = ErrBadCRC
 		return
 	}
 
-	res	= &pdu{
+	// 标准modbus响应已读取完成，现在读取自定义数据
-		unitId:		rxbuf[0],
+	// 设置5秒超时来读取自定义数据
-		functionCode:	rxbuf[1],
+	customDataTimeout := 5 * time.Second
-		// pass the byte count + trailing data as payload, withtout the CRC
+	err = rt.link.SetDeadline(time.Now().Add(customDataTimeout))
-		payload:	rxbuf[2:3 + bytesNeeded  - 2],
+	if err != nil {
+		return
+	}
+
+	// fmt.Println("---: ", rxbuf[:10])
+
+	// a := make([]byte, 100)
+	// n, err := io.ReadFull(rt.link, a)
+	// fmt.Println("n: ", n, "err: ", err, "a: ", a)
+	time.Sleep(1 * time.Second)
+
+	// 使用临时缓冲区循环读取自定义数据
+	tempBuf := make([]byte, 256) // 每次读取最多256字节
+	totalRead := 0
+	startPos := 3 + bytesNeeded
+
+	for {
+		// 检查缓冲区是否还有空间
+		if startPos+totalRead+len(tempBuf) > len(rxbuf) {
+			// 如果缓冲区不够，扩展它
+			newBuf := make([]byte, len(rxbuf)*2)
+			copy(newBuf, rxbuf)
+			rxbuf = newBuf
+		}
+
+		// 尝试读取数据
+		n, readErr := rt.link.Read(tempBuf)
+		if n > 0 {
+			// 将读取的数据复制到rxbuf
+			copy(rxbuf[startPos+totalRead:startPos+totalRead+n], tempBuf[:n])
+			totalRead += n
+		}
+
+		// 如果遇到超时错误，说明没有更多数据了
+		if readErr != nil {
+			if os.IsTimeout(readErr) {
+				// 超时是正常的，说明自定义数据读取完成
+				break
+			}
+			// 其他错误需要检查是否是EOF（数据读取完成）
+			if readErr == io.EOF {
+				break
+			}
+			// 对于其他错误，如果已经读取了一些数据，继续处理
+			if totalRead == 0 {
+				err = readErr
+				return
+			}
+			break
+		}
+
+		// 如果读取的字节数少于请求的，说明没有更多数据了
+		if n < len(tempBuf) {
+			break
+		}
+	}
+
+	// 返回标准响应的payload（地址+值，4字节）+ 自定义数据
+	// 标准响应的payload在 rxbuf[2:3+bytesNeeded-2] 位置
+	standardPayloadStart := 2
+	standardPayloadEnd := 3 + bytesNeeded - 2
+
+	// 构建完整的payload：标准响应payload + 自定义数据
+	completePayload := make([]byte, 0, standardPayloadEnd-standardPayloadStart+totalRead)
+	completePayload = append(completePayload, rxbuf[standardPayloadStart:standardPayloadEnd]...)
+	if totalRead > 0 {
+		completePayload = append(completePayload, rxbuf[startPos:startPos+totalRead]...)
+	}
+
+	res = &pdu{
+		unitId:       rxbuf[0],
+		functionCode: rxbuf[1],
+		// payload包含标准响应的payload + 自定义数据
+		payload: completePayload,
+		// payload: a,
 	}
 
 	return
@@ -200,18 +548,18 @@ func (rt *rtuTransport) readRTUFrame() (res *pdu, err error) {
 
 // Turns a PDU object into bytes.
 func (rt *rtuTransport) assembleRTUFrame(p *pdu) (adu []byte) {
-	var crc		crc
+	var crc crc
 
-	adu	= append(adu, p.unitId)
+	adu = append(adu, p.unitId)
-	adu	= append(adu, p.functionCode)
+	adu = append(adu, p.functionCode)
-	adu	= append(adu, p.payload...)
+	adu = append(adu, p.payload...)
 
 	// run the ADU through the CRC generator
 	crc.init()
 	crc.add(adu)
 
 	// append the CRC to the ADU
-	adu	= append(adu, crc.value()...)
+	adu = append(adu, crc.value()...)
 
 	return
 }
@@ -220,24 +568,31 @@ func (rt *rtuTransport) assembleRTUFrame(p *pdu) (adu []byte) {
 func expectedResponseLenth(responseCode uint8, responseLength uint8) (byteCount int, err error) {
 	switch responseCode {
 	case fcReadHoldingRegisters,
-	     fcReadInputRegisters,
+		fcReadInputRegisters,
-	     fcReadCoils,
+		fcReadCoils,
-	     fcReadDiscreteInputs:            byteCount = int(responseLength)
+		fcReadDiscreteInputs,
+		0x41:
+		byteCount = int(responseLength)
 	case fcWriteSingleRegister,
-	     fcWriteMultipleRegisters,
+		fcWriteMultipleRegisters,
-	     fcWriteSingleCoil,
+		fcWriteSingleCoil,
-	     fcWriteMultipleCoils:            byteCount = 3
+		fcWriteMultipleCoils:
-	case fcMaskWriteRegister:             byteCount = 5
+		byteCount = 3
+	case fcMaskWriteRegister:
+		byteCount = 5
 	case fcReadHoldingRegisters | 0x80,
-	     fcReadInputRegisters | 0x80,
+		fcReadInputRegisters | 0x80,
-	     fcReadCoils | 0x80,
+		fcReadCoils | 0x80,
-	     fcReadDiscreteInputs | 0x80,
+		fcReadDiscreteInputs | 0x80,
-	     fcWriteSingleRegister | 0x80,
+		fcWriteSingleRegister | 0x80,
-	     fcWriteMultipleRegisters | 0x80,
+		fcWriteMultipleRegisters | 0x80,
-	     fcWriteSingleCoil | 0x80,
+		fcWriteSingleCoil | 0x80,
-	     fcWriteMultipleCoils | 0x80,
+		fcWriteMultipleCoils | 0x80,
-	     fcMaskWriteRegister | 0x80:      byteCount = 0
+		fcMaskWriteRegister | 0x80,
-	default: err = ErrProtocolError
+		0x41 | 0x80:
+		byteCount = 0
+	default:
+		err = ErrProtocolError
 	}
 
 	return

tcp_transport.go

@@ -9,23 +9,23 @@ import (
 )
 
 const (
-	maxTCPFrameLength	int = 260
+	maxTCPFrameLength int = 260
-	mbapHeaderLength	int = 7
+	mbapHeaderLength  int = 7
 )
 
 type tcpTransport struct {
-	logger		*logger
+	logger    *logger
-	socket		net.Conn
+	socket    net.Conn
-	timeout		time.Duration
+	timeout   time.Duration
-	lastTxnId	uint16
+	lastTxnId uint16
 }
 
 // Returns a new TCP transport.
 func newTCPTransport(socket net.Conn, timeout time.Duration, customLogger *log.Logger) (tt *tcpTransport) {
 	tt = &tcpTransport{
-		socket:		socket,
+		socket:  socket,
-		timeout:	timeout,
+		timeout: timeout,
-		logger:		newLogger(fmt.Sprintf("tcp-transport(%s)", socket.RemoteAddr()), customLogger),
+		logger:  newLogger(fmt.Sprintf("tcp-transport(%s)", socket.RemoteAddr()), customLogger),
 	}
 
 	return
@@ -33,7 +33,7 @@ func newTCPTransport(socket net.Conn, timeout time.Duration, customLogger *log.L
 
 // Closes the underlying tcp socket.
 func (tt *tcpTransport) Close() (err error) {
-	err  = tt.socket.Close()
+	err = tt.socket.Close()
 
 	return
 }
@@ -41,7 +41,7 @@ func (tt *tcpTransport) Close() (err error) {
 // Runs a request across the socket and returns a response.
 func (tt *tcpTransport) ExecuteRequest(req *pdu) (res *pdu, err error) {
 	// set an i/o deadline on the socket (read and write)
-	err	= tt.socket.SetDeadline(time.Now().Add(tt.timeout))
+	err = tt.socket.SetDeadline(time.Now().Add(tt.timeout))
 	if err != nil {
 		return
 	}
@@ -49,7 +49,7 @@ func (tt *tcpTransport) ExecuteRequest(req *pdu) (res *pdu, err error) {
 	// increase the transaction ID counter
 	tt.lastTxnId++
 
-	_, err	= tt.socket.Write(tt.assembleMBAPFrame(tt.lastTxnId, req))
+	_, err = tt.socket.Write(tt.assembleMBAPFrame(tt.lastTxnId, req))
 	if err != nil {
 		return
 	}
@@ -59,30 +59,34 @@ func (tt *tcpTransport) ExecuteRequest(req *pdu) (res *pdu, err error) {
 	return
 }
 
+func (tt *tcpTransport) ExecuteRequestWithRes(req *pdu) (res *pdu, err error) {
+	return nil, nil
+}
+
 // Reads a request from the socket.
 func (tt *tcpTransport) ReadRequest() (req *pdu, err error) {
-	var txnId	uint16
+	var txnId uint16
 
 	// set an i/o deadline on the socket (read and write)
-	err	= tt.socket.SetDeadline(time.Now().Add(tt.timeout))
+	err = tt.socket.SetDeadline(time.Now().Add(tt.timeout))
 	if err != nil {
 		return
 	}
 
-	req, txnId, err	= tt.readMBAPFrame()
+	req, txnId, err = tt.readMBAPFrame()
 	if err != nil {
 		return
 	}
 
 	// store the incoming transaction id
-	tt.lastTxnId	= txnId
+	tt.lastTxnId = txnId
 
 	return
 }
 
 // Writes a response to the socket.
 func (tt *tcpTransport) WriteResponse(res *pdu) (err error) {
-	_, err	= tt.socket.Write(tt.assembleMBAPFrame(tt.lastTxnId, res))
+	_, err = tt.socket.Write(tt.assembleMBAPFrame(tt.lastTxnId, res))
 	if err != nil {
 		return
 	}
@@ -93,11 +97,11 @@ func (tt *tcpTransport) WriteResponse(res *pdu) (err error) {
 // Reads as many MBAP+modbus frames as necessary until either the response
 // matching tt.lastTxnId is received or an error occurs.
 func (tt *tcpTransport) readResponse() (res *pdu, err error) {
-	var txnId	uint16
+	var txnId uint16
 
 	for {
 		// grab a frame
-		res, txnId, err	= tt.readMBAPFrame()
+		res, txnId, err = tt.readMBAPFrame()
 
 		// ignore unknown protocol identifiers
 		if err == ErrUnknownProtocolId {
@@ -111,9 +115,9 @@ func (tt *tcpTransport) readResponse() (res *pdu, err error) {
 
 		// ignore unknown transaction identifiers
 		if tt.lastTxnId != txnId {
-			tt.logger.Warningf("received unexpected transaction id " +
+			tt.logger.Warningf("received unexpected transaction id "+
-					   "(expected 0x%04x, received 0x%04x)",
+				"(expected 0x%04x, received 0x%04x)",
-					   tt.lastTxnId, txnId)
+				tt.lastTxnId, txnId)
 			continue
 		}
 
@@ -125,33 +129,33 @@ func (tt *tcpTransport) readResponse() (res *pdu, err error) {
 
 // Reads an entire frame (MBAP header + modbus PDU) from the socket.
 func (tt *tcpTransport) readMBAPFrame() (p *pdu, txnId uint16, err error) {
-	var rxbuf	[]byte
+	var rxbuf []byte
-	var bytesNeeded	int
+	var bytesNeeded int
-	var protocolId	uint16
+	var protocolId uint16
-	var unitId	uint8
+	var unitId uint8
 
 	// read the MBAP header
-	rxbuf		= make([]byte, mbapHeaderLength)
+	rxbuf = make([]byte, mbapHeaderLength)
-	_, err		= io.ReadFull(tt.socket, rxbuf)
+	_, err = io.ReadFull(tt.socket, rxbuf)
 	if err != nil {
 		return
 	}
 
 	// decode the transaction identifier
-	txnId		= bytesToUint16(BIG_ENDIAN, rxbuf[0:2])
+	txnId = bytesToUint16(BIG_ENDIAN, rxbuf[0:2])
 	// decode the protocol identifier
-	protocolId	= bytesToUint16(BIG_ENDIAN, rxbuf[2:4])
+	protocolId = bytesToUint16(BIG_ENDIAN, rxbuf[2:4])
 	// store the source unit id
-	unitId		= rxbuf[6]
+	unitId = rxbuf[6]
 
 	// determine how many more bytes we need to read
-	bytesNeeded	= int(bytesToUint16(BIG_ENDIAN, rxbuf[4:6]))
+	bytesNeeded = int(bytesToUint16(BIG_ENDIAN, rxbuf[4:6]))
 
 	// the byte count includes the unit ID field, which we already have
 	bytesNeeded--
 
 	// never read more than the max allowed frame length
-	if bytesNeeded + mbapHeaderLength > maxTCPFrameLength {
+	if bytesNeeded+mbapHeaderLength > maxTCPFrameLength {
 		err = ErrProtocolError
 		return
 	}
@@ -163,8 +167,8 @@ func (tt *tcpTransport) readMBAPFrame() (p *pdu, txnId uint16, err error) {
 	}
 
 	// read the PDU
-	rxbuf		= make([]byte, bytesNeeded)
+	rxbuf = make([]byte, bytesNeeded)
-	_, err		= io.ReadFull(tt.socket, rxbuf)
+	_, err = io.ReadFull(tt.socket, rxbuf)
 	if err != nil {
 		return
 	}
@@ -178,9 +182,9 @@ func (tt *tcpTransport) readMBAPFrame() (p *pdu, txnId uint16, err error) {
 
 	// store unit id, function code and payload in the PDU object
 	p = &pdu{
-		unitId:		unitId,
+		unitId:       unitId,
-		functionCode:	rxbuf[0],
+		functionCode: rxbuf[0],
-		payload:	rxbuf[1:],
+		payload:      rxbuf[1:],
 	}
 
 	return
@@ -189,17 +193,17 @@ func (tt *tcpTransport) readMBAPFrame() (p *pdu, txnId uint16, err error) {
 // Turns a PDU into an MBAP frame (MBAP header + PDU) and returns it as bytes.
 func (tt *tcpTransport) assembleMBAPFrame(txnId uint16, p *pdu) (payload []byte) {
 	// transaction identifier
-	payload	= uint16ToBytes(BIG_ENDIAN, txnId)
+	payload = uint16ToBytes(BIG_ENDIAN, txnId)
 	// protocol identifier (always 0x0000)
-	payload	= append(payload, 0x00, 0x00)
+	payload = append(payload, 0x00, 0x00)
 	// length (covers unit identifier + function code + payload fields)
-	payload = append(payload, uint16ToBytes(BIG_ENDIAN, uint16(2 + len(p.payload)))...)
+	payload = append(payload, uint16ToBytes(BIG_ENDIAN, uint16(2+len(p.payload)))...)
 	// unit identifier
-	payload	= append(payload, p.unitId)
+	payload = append(payload, p.unitId)
 	// function code
-	payload	= append(payload, p.functionCode)
+	payload = append(payload, p.functionCode)
 	// payload
-	payload	= append(payload, p.payload...)
+	payload = append(payload, p.payload...)
 
 	return
 }

transport.go

@@ -1,18 +1,20 @@
 package modbus
 
 type transportType uint
+
 const (
-	modbusRTU        transportType   = 1
+	modbusRTU        transportType = 1
-	modbusRTUOverTCP transportType   = 2
+	modbusRTUOverTCP transportType = 2
-	modbusRTUOverUDP transportType   = 3
+	modbusRTUOverUDP transportType = 3
-	modbusTCP        transportType   = 4
+	modbusTCP        transportType = 4
-	modbusTCPOverTLS transportType   = 5
+	modbusTCPOverTLS transportType = 5
-	modbusTCPOverUDP transportType   = 6
+	modbusTCPOverUDP transportType = 6
 )
 
 type transport interface {
-	Close()              (error)
+	Close() error
 	ExecuteRequest(*pdu) (*pdu, error)
-	ReadRequest()        (*pdu, error)
+	ExecuteRequestWithRes(*pdu) (*pdu, error)
-	WriteResponse(*pdu)  (error)
+	ReadRequest() (*pdu, error)
+	WriteResponse(*pdu) error
 }