完善自定义数据读取

Reviewed-on: #5

client.go

@@ -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)
-		if len(res.payload) != 4 ||
-			// bytes 1-2 should be the register address
-			bytesToUint16(BIG_ENDIAN, res.payload[0:2]) != addr ||
+		// expect at least 4 bytes (2 byte of address + 2 bytes of value)
+		// 后面可能还有自定义数据
+		if len(res.payload) < 4 {
+			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 {
@@ -1261,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
-		// (1 byte of length + 2 bytes per register)
-		// if len(res.payload) != 1+2*int(quantity) {
-		// 	err = ErrProtocolError
-		// 	return
-		// }
+		// For custom function code 0x41, the payload format is:
+		// [起始地址(2字节)] [字节数(2字节)] [数据...]
+		if functionCode == fcCustomize {
+			// validate minimum payload length (start address 2 bytes + byte count 2 bytes)
+			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
-		// (2 bytes per register * number of registers)
-		// if uint(res.payload[0]) != 2*uint(quantity) {
-		// 	err = ErrProtocolError
-		// 	return
-		// }
+			// extract byte count from payload (bytes 2-3, big endian)
+			byteCount := bytesToUint16(BIG_ENDIAN, res.payload[2:4])
 
-		// remove the byte count field from the returned slice
-		bytes = res.payload[1:]
+			// validate payload length matches expected data length
+			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 {

rtu_transport.go

@@ -4,6 +4,7 @@ import (
 	"fmt"
 	"io"
 	"log"
+	"os"
 	"time"
 )
 
@@ -79,7 +80,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))
+	fmt.Printf("send: %X\n", rt.assembleRTUFrame(req))
 	if err != nil {
+		fmt.Printf("write error: %s", err.Error())
 		return
 	}
 
@@ -192,6 +195,7 @@ func (rt *rtuTransport) readRTUFrame() (res *pdu, err error) {
 	var byteCount int
 	var bytesNeeded int
 	var crc crc
+	// var dataLength uint16
 
 	rxbuf = make([]byte, maxRTUFrameLength)
 
@@ -202,10 +206,106 @@ func (rt *rtuTransport) readRTUFrame() (res *pdu, err error) {
 		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)
+		fmt.Println(bytesNeeded)
+
+		// 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 校验范围的结束索引 (不包含)
+		fmt.Println("crc end index", crcEndIndex)
+
+		crc.init()
+		crc.add(rxbuf[0:crcEndIndex]) // 校验范围从 rxbuf[0] 到数据域结束
+
+		// 比较接收到的 CRC
+		// recvCRCLow := rxbuf[crcEndIndex]
+		// recvCRCHigh := rxbuf[crcEndIndex+1]
+
+		// fmt.Println("len: ", len(rxbuf[6:crcEndIndex]))
+
+		// if !crc.isEqual(recvCRCLow, recvCRCHigh) {
+		// 	err = ErrBadCRC
+		// 	fmt.Println("crc: ", recvCRCLow, recvCRCHigh, "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 {
@@ -305,16 +405,77 @@ func (rt *rtuTransport) readRTUFrameWithRes() (res *pdu, err error) {
 		return
 	}
 
-	_, err = io.ReadFull(rt.link, rxbuf[3+bytesNeeded:])
-	if err != nil && err != io.ErrUnexpectedEOF {
+	// 标准modbus响应已读取完成，现在读取自定义数据
+	// 设置5秒超时来读取自定义数据
+	customDataTimeout := 5 * time.Second
+	err = rt.link.SetDeadline(time.Now().Add(customDataTimeout))
+	if err != nil {
 		return
 	}
 
+	// 使用临时缓冲区循环读取自定义数据
+	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],
-		// pass the byte count + trailing data as payload, withtout the CRC
-		payload: rxbuf[3+bytesNeeded:],
+		// payload包含标准响应的payload + 自定义数据
+		payload: completePayload,
 	}
 
 	return