From 72429c08a3daf5aca02dcbdee70bc4fbed823830 Mon Sep 17 00:00:00 2001 From: Fuyao Date: Mon, 17 Nov 2025 11:01:51 +0800 Subject: [PATCH] =?UTF-8?q?=E8=87=AA=E5=AE=9A=E4=B9=89=E5=AE=9E=E6=97=B6?= =?UTF-8?q?=E6=95=B0=E6=8D=AE=E8=AF=BB=E5=8F=96&=E8=AF=BB=E6=A0=87?= =?UTF-8?q?=E5=AE=9A=E4=BF=A1=E6=81=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- client.go | 81 +++++++++++++++++++++------- rtu_transport.go | 138 +++++++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 196 insertions(+), 23 deletions(-) diff --git a/client.go b/client.go index 9b9184f..ff569f7 100644 --- a/client.go +++ b/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 { diff --git a/rtu_transport.go b/rtu_transport.go index 01d46cb..0f29a70 100644 --- a/rtu_transport.go +++ b/rtu_transport.go @@ -4,6 +4,7 @@ import ( "fmt" "io" "log" + "os" "time" ) @@ -192,6 +193,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) @@ -206,6 +208,73 @@ func (rt *rtuTransport) readRTUFrame() (res *pdu, err error) { return } + // 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 + } + + // extract data length from bytes 5-6 (byte count field, big endian) + dataLength = bytesToUint16(BIG_ENDIAN, rxbuf[5:7]) + bytesNeeded = int(dataLength) + + // we need to read 2 additional bytes of CRC after the payload + bytesNeeded += 2 + + // calculate total frame size: 3 (header) + 4 (start addr + byte count) + data + 2 (CRC) + totalFrameSize := 7 + bytesNeeded + + // for custom function code, we may need a larger buffer than maxRTUFrameLength + // allocate buffer dynamically if needed + if totalFrameSize > maxRTUFrameLength { + // save already read data + header := make([]byte, 7) + copy(header, rxbuf[0:7]) + // resize buffer to accommodate larger frame + rxbuf = make([]byte, totalFrameSize) + // copy back the header + copy(rxbuf[0:7], header) + } + + // read the data and CRC + byteCount, err = io.ReadFull(rt.link, rxbuf[7:7+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 : 7+bytesNeeded-2]) + + // compare CRC values + if !crc.isEqual(rxbuf[7+bytesNeeded-2], rxbuf[7+bytesNeeded-1]) { + err = ErrBadCRC + return + } + + 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], + } + + 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 +374,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 -- 2.49.1