Merge branch 'main' into cfy-feat-fix

Reviewed-on: #9

rtu_transport.go

@@ -81,6 +81,7 @@ func (rt *rtuTransport) ExecuteRequest(req *pdu) (res *pdu, err error) {
 	// send the final ADU+CRC on the wire
 	n, err = rt.link.Write(rt.assembleRTUFrame(req))
 	if err != nil {
+		fmt.Printf("write error: %s", err.Error())
 		return
 	}
 
@@ -187,13 +188,62 @@ 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 byteCount int
 	var bytesNeeded int
 	var crc crc
-	var dataLength uint16
+	// var dataLength uint16
 
 	rxbuf = make([]byte, maxRTUFrameLength)
 
@@ -204,6 +254,9 @@ 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
 	}
@@ -211,64 +264,99 @@ func (rt *rtuTransport) readRTUFrame() (res *pdu, err error) {
 	// handle custom function code 0x41 with special format:
 	// [单元ID] [功能码] [起始地址(2字节)] [字节数(2字节)] [数据...] [CRC]
 	if uint8(rxbuf[1]) == fcCustomize {
-		// read the start address (2 bytes) and byte count (2 bytes)
-		byteCount, err = io.ReadFull(rt.link, rxbuf[3:7])
-		if (byteCount > 0 || err == nil) && byteCount != 4 {
-			err = ErrShortFrame
-			return
-		}
-		if err != nil && err != io.ErrUnexpectedEOF {
-			return
-		}
+		// 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 字节
 
-		// extract data length from bytes 5-6 (byte count field, big endian)
-		dataLength = bytesToUint16(BIG_ENDIAN, rxbuf[5:7])
+		// 修正短帧检查逻辑
+		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)
 
-		// we need to read 2 additional bytes of CRC after the payload
-		bytesNeeded += 2
+		// 3. 计算总共需要读取的字节数
+		// 数据域长度 + 2 字节 CRC
+		bytesToRead := bytesNeeded + 2
 
-		// calculate total frame size: 3 (header) + 4 (start addr + byte count) + data + 2 (CRC)
-		totalFrameSize := 7 + bytesNeeded
+		// 4. 计算总帧大小: 6 (Header) + DataLength + 2 (CRC)
+		totalFrameSize := 6 + bytesToRead
+		TotalHeaderLength := 6
 
-		// for custom function code, we may need a larger buffer than maxRTUFrameLength
-		// allocate buffer dynamically if needed
+		// 5. 动态分配或调整缓冲区
 		if totalFrameSize > maxRTUFrameLength {
 			// save already read data
-			header := make([]byte, 7)
-			copy(header, rxbuf[0:7])
+			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:7], header)
+			copy(rxbuf[0:TotalHeaderLength], header)
 		}
 
-		// read the data and CRC
-		byteCount, err = io.ReadFull(rt.link, rxbuf[7:7+bytesNeeded])
-		if err != nil && err != io.ErrUnexpectedEOF {
+		// 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 != bytesNeeded {
-			rt.logger.Warningf("expected %v bytes, received %v", bytesNeeded, byteCount)
+
+		if byteCount != bytesToRead {
+			rt.logger.Warningf("expected %v bytes, received %v", bytesToRead, byteCount)
 			err = ErrShortFrame
 			return
 		}
 
-		// compute the CRC on the entire frame, excluding the CRC
-		crc.init()
-		crc.add(rxbuf[0 : 7+bytesNeeded-2])
+		// 7. CRC 校验
+		crcEndIndex := totalFrameSize - 2 // CRC 校验范围的结束索引 (不包含)
 
-		// compare CRC values
-		if !crc.isEqual(rxbuf[7+bytesNeeded-2], rxbuf[7+bytesNeeded-1]) {
+		// 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(sentHigh, sentLow) {
 			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],
-			// pass the start address (2 bytes) + byte count (2 bytes) + data as payload, without the CRC
-			payload: rxbuf[2 : 7+bytesNeeded-2],
+			payload:      rxbuf[2:crcEndIndex],
 		}
 
 		return
@@ -377,7 +465,8 @@ func (rt *rtuTransport) readRTUFrameWithRes() (res *pdu, err error) {
 	// 标准modbus响应已读取完成，现在读取自定义数据
 	// 设置5秒超时来读取自定义数据
 	customDataTimeout := 5 * time.Second
-	err = rt.link.SetDeadline(time.Now().Add(customDataTimeout))
+	deadline := time.Now().Add(customDataTimeout)
+	err = rt.link.SetDeadline(deadline)
 	if err != nil {
 		return
 	}
@@ -386,8 +475,19 @@ func (rt *rtuTransport) readRTUFrameWithRes() (res *pdu, err error) {
 	tempBuf := make([]byte, 256) // 每次读取最多256字节
 	totalRead := 0
 	startPos := 3 + bytesNeeded
+	var lastErr error // 记录最后一次非超时错误
 
 	for {
+		// 检查是否已经超时
+		if time.Now().After(deadline) {
+			// 超时时间到，退出循环
+			// 如果有之前记录的错误，使用它；否则err保持为nil（超时是正常的）
+			if lastErr != nil {
+				err = lastErr
+			}
+			break
+		}
+
 		// 检查缓冲区是否还有空间
 		if startPos+totalRead+len(tempBuf) > len(rxbuf) {
 			// 如果缓冲区不够，扩展它
@@ -404,31 +504,23 @@ func (rt *rtuTransport) readRTUFrameWithRes() (res *pdu, err error) {
 			totalRead += n
 		}
 
-		// 如果遇到超时错误，说明没有更多数据了
+		// 如果遇到超时错误，说明超时时间到了，退出循环
 		if readErr != nil {
 			if os.IsTimeout(readErr) {
-				// 超时是正常的，说明自定义数据读取完成
+				// 超时时间到，退出循环
+				// 如果有之前记录的错误，使用它；否则err保持为nil（超时是正常的）
+				if lastErr != nil {
+					err = lastErr
+				}
 				break
 			}
-			// 其他错误需要检查是否是EOF（数据读取完成）
-			if readErr == io.EOF {
-				break
-			}
-			// 对于其他错误，如果已经读取了一些数据，继续处理
-			if totalRead == 0 {
-				err = readErr
-				return
-			}
-			break
+			// 记录非超时错误，但继续等待直到超时
+			lastErr = readErr
+			// 继续循环，等待超时
 		}
 
-		// 如果读取的字节数少于请求的，说明没有更多数据了
-		if n < len(tempBuf) {
-			break
-		}
 	}
 
-	// 返回标准响应的payload（地址+值，4字节）+ 自定义数据
 	// 标准响应的payload在 rxbuf[2:3+bytesNeeded-2] 位置
 	standardPayloadStart := 2
 	standardPayloadEnd := 3 + bytesNeeded - 2
@@ -443,8 +535,7 @@ func (rt *rtuTransport) readRTUFrameWithRes() (res *pdu, err error) {
 	res = &pdu{
 		unitId:       rxbuf[0],
 		functionCode: rxbuf[1],
-		// payload包含标准响应的payload + 自定义数据
-		payload: completePayload,
+		payload:      completePayload,
 	}
 
 	return