smart_streetlights/edge/ipole/modbusrtu.go

package main

import (
	"context"
	"encoding/binary"
	"errors"
	"fmt"
	"time"

	"github.com/thinkgos/timing/v3"

	"lc/common/mqtt"
	"lc/common/protocol"
	"lc/common/util"
	"lc/edge/ipole/modbus"
)

var ModbusRtuProtocol = "ModbusRTU"
var ErrInvalidFunccode = errors.New("invalid function code")

const (
	rtuExceptionSize         = 5
	DefaultReadyQueuesLength = 256
)

type Request struct {
	Rtuinfo   *protocol.DevInfo //RTU信息
	CID       uint8             //cid->采集命令，下发给设备的命令
	FuncCode  byte              // 功能码
	Address   uint16            // 请求数据用实际地址
	Quantity  uint16            // 请求数量
	ScanRate  time.Duration     // 扫描速率scan rate
	TxCnt     uint64            // 发送计数
	ErrCnt    uint64            // 发送错误计数
	tmHandler func()
}

type ModbusRtu struct {
	reqList       []*Request
	devInfo       *protocol.DevInfo
	model         *protocol.IotModel
	ready         chan *Request
	ctx           context.Context
	cancel        context.CancelFunc
	chanDevInfo   chan *protocol.DevInfo //设备管理更新
	chanModelInfo chan *ModelInfo        //设备管理更新
}

func NewModbusRtu(info *protocol.DevInfo) Device {
	ctx, cancel := context.WithCancel(context.Background())
	rtu := &ModbusRtu{
		devInfo:       info,
		ready:         make(chan *Request, DefaultReadyQueuesLength),
		ctx:           ctx,
		cancel:        cancel,
		chanDevInfo:   make(chan *protocol.DevInfo),
		chanModelInfo: make(chan *ModelInfo),
	}

	iot, err := loadModel(info.TID)
	if err != nil {
		return nil
	}
	if iot.Protocol == ModbusRtuProtocol {
		rtu.model = iot
	}

	mapRtuUploadManager.Store(info.DevCode, NewRtuUploadManager(info))

	return rtu
}

func (o *ModbusRtu) Start() {
	GetMQTTMgr().Subscribe(GetTopic(o.GetDevType(), o.devInfo.DevCode, protocol.TP_MODBUS_CONTROL), mqtt.ExactlyOnce, o.HandleTpWControl, ToAll)
	go o.HandleData()
}

func (o *ModbusRtu) Stop() {
	//停止采集和处理
	o.cancel()
	//停止上传
	if value, ok := mapRtuUploadManager.Load(o.devInfo.DevCode); ok {
		uploadManager := value.(*RtuUploadManager)
		if uploadManager != nil {
			uploadManager.Stop()
		}
	}
}

func (o *ModbusRtu) UpdateInfo(devInfo protocol.DevInfo) {
	o.chanDevInfo <- &devInfo
}

func (o *ModbusRtu) GetDevInfo() *protocol.DevInfo {
	return o.devInfo
}

func (o *ModbusRtu) UpdateModel(tid uint16, flag int) {
	if tid > 0 {
		mi := ModelInfo{
			TID:  tid,
			Flag: flag,
		}
		o.chanModelInfo <- &mi
	}
}

func (o *ModbusRtu) UpdateModel2(mi *ModelInfo) {
	if o.devInfo.TID != mi.TID {
		return
	}
	if mi.Flag == 0 {
		return
	}
	iot, err := loadModel(mi.TID)
	if err != nil {
		return
	}
	if iot.Protocol == ModbusRtuProtocol { //合法的物模型
		o.model = iot
		o.clearRequest()
		o.updateRequest()
	}
}

func (o *ModbusRtu) GetDevType() string {
	if o.devInfo.DevType == 1 {
		return protocol.DT_CONCENTRATOR
	} else if o.devInfo.DevType == 2 {
		return protocol.DT_ENVIRONMENT
	} else if o.devInfo.DevType == 4 {
		return protocol.DT_LIQUID
	} else if o.devInfo.DevType == 5 {
		return protocol.DT_ROAD_COND
	}
	return "unknown"
}

// HandleData 数据处理协程
func (o *ModbusRtu) HandleData() {
	defer func() {
		recover()
		go o.HandleData()
	}()
	o.updateRequest()
	var req *Request
	for {
		select {
		case <-o.ctx.Done():
			return
		case info := <-o.chanDevInfo:
			o.devInfo = info
		case mi := <-o.chanModelInfo:
			o.UpdateModel2(mi) //物模型配置文件更新
		case req = <-o.ready: //查看是否有准备好的请求
			o.procRequest(req)
		default:
			time.Sleep(time.Millisecond * 100)
		}
	}
}

func (o *ModbusRtu) clearRequest() {
	for _, v := range o.reqList {
		v.ScanRate = 0 //置为0，则不再执行新请求
	}
	o.reqList = nil
}

func (o *ModbusRtu) updateRequest() {
	for k, v := range o.model.Packet {
		r := Request{CID: k, Rtuinfo: o.devInfo, FuncCode: v.Code, Address: v.Addr,
			Quantity: v.Quantity, ScanRate: time.Duration(v.Cycle) * time.Millisecond,
		}
		if err := o.AddGatherJob(&r); err == nil {
			o.reqList = append(o.reqList, &r)
		}
	}
}

// AddGatherJob 增加采集任务
func (o *ModbusRtu) AddGatherJob(r *Request) error {
	if err := o.ctx.Err(); err != nil {
		return err
	}
	if r.Rtuinfo.DevID < modbus.AddressMin || r.Rtuinfo.DevID > modbus.AddressMax {
		return fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			r.Rtuinfo.DevID, modbus.AddressMin, modbus.AddressMax)
	}
	if r.FuncCode == modbus.FuncCodeReadCoils || r.FuncCode == modbus.FuncCodeReadDiscreteInputs ||
		r.FuncCode == modbus.FuncCodeReadInputRegisters || r.FuncCode == modbus.FuncCodeReadHoldingRegisters {
		r.tmHandler = func() {
			select {
			case <-o.ctx.Done():
				return
			case o.ready <- r:
			default:
				timing.AddJobFunc(r.tmHandler, r.ScanRate*time.Millisecond)
			}
		}
		timing.AddJobFunc(r.tmHandler, r.ScanRate)
	} else {
		return ErrInvalidFunccode
	}
	return nil
}

func (o *ModbusRtu) ProcReadCoils(cid uint8, address, quality uint16, valBuf []byte) {

}
func (o *ModbusRtu) ReadDiscrete(cid uint8, address, quality uint16, valBuf []byte) {

}

func (o *ModbusRtu) ProcReadHoldingRegisters(cid uint8, address, quality uint16, valBuf []byte) {
	dataLen := len(valBuf)
	if o.model.Packet[cid].Resplen != uint(dataLen) {
		return
	}
	dataMap := make(map[uint16]float64)
	for _, v := range o.model.DataUp {
		if v.Cid != cid || v.Len == 0 {
			continue
		}
		if int(v.Start+v.Len) > dataLen { //索引超长，忽略该项目
			continue
		}
		var fVal float64
		strVal := valBuf[v.Start : v.Start+v.Len]
		if v.Len == 2 {
			var u16 uint16
			if v.Endian == 0 { //大端
				u16 = binary.BigEndian.Uint16(strVal)
			} else { //小端
				u16 = binary.LittleEndian.Uint16(strVal)
			}
			if v.Type == 0 { //处理符号
				fVal = float64(u16)
			} else {
				fVal = float64(int16(u16))
			}
		} else if v.Len == 4 {
			if v.Endian == 0 {
				fVal = float64(util.BEByteToFloat32(strVal))
			} else {
				fVal = float64(util.LEByteToFloat32(strVal))
			}
		} else if v.Len == 1 {
			if v.Type == 0 { //处理符号
				fVal = float64(strVal[0])
			} else {
				fVal = float64(int8(strVal[0]))
			}
		}
		if v.Ratio > 0 {
			fVal = fVal / float64(v.Ratio)
		}
		fVal = fVal - float64(v.Base)

		if v.Type == 0 || v.Type == 1 { //整数
			dataMap[v.SID] = Precision(fVal, 0, true)
		} else if v.Type == 2 || v.Type == 3 { //浮点数
			dataMap[v.SID] = Precision(fVal, 3, false)
		}
	}
	if value, ok := mapRtuUploadManager.Load(o.devInfo.DevCode); ok {
		uploadManager := value.(*RtuUploadManager)
		if uploadManager != nil {
			uploadManager.AddData(dataMap)
		}
	}
}

func (o *ModbusRtu) ProcReadInputRegisters(cid uint8, address, quality uint16, valBuf []byte) {
	dataLen := len(valBuf)
	if o.model.Packet[cid].Resplen != uint(dataLen) {
		return
	}
	dataMap := make(map[uint16]float64)
	for _, v := range o.model.DataUp {
		if v.Cid != cid || v.Len == 0 {
			continue
		}
		if int(v.Start+v.Len) > dataLen { //索引超长，忽略该项目
			continue
		}
		var fVal float64
		strVal := valBuf[v.Start : v.Start+v.Len]
		if v.Len == 2 {
			var u16 uint16
			if v.Endian == 0 { //大端
				u16 = binary.BigEndian.Uint16(strVal)
			} else { //小端
				u16 = binary.LittleEndian.Uint16(strVal)
			}
			if v.Type == 0 { //处理符号
				fVal = float64(u16)
			} else {
				fVal = float64(int16(u16))
			}
		} else if v.Len == 4 {
			if v.Endian == 0 {
				fVal = float64(util.BEByteToFloat32(strVal))
			} else {
				fVal = float64(util.LEByteToFloat32(strVal))
			}
		} else if v.Len == 1 {
			if v.Type == 0 { //处理符号
				fVal = float64(strVal[0])
			} else {
				fVal = float64(int8(strVal[0]))
			}
		}
		if v.Ratio > 0 {
			fVal = fVal / float64(v.Ratio)
		}
		fVal = fVal - float64(v.Base)

		if v.Type == 0 || v.Type == 1 { //整数
			dataMap[v.SID] = Precision(fVal, 0, true)
		} else if v.Type == 2 || v.Type == 3 { //浮点数
			dataMap[v.SID] = Precision(fVal, 3, false)
		}
	}

	if value, ok := mapRtuUploadManager.Load(o.devInfo.DevCode); ok {
		uploadManager := value.(*RtuUploadManager)
		if uploadManager != nil {
			uploadManager.AddData(dataMap)
		}
	}
}

func (o *ModbusRtu) ProcResult(err error, req *Request) {
	if err == nil {
		if req.ErrCnt > 0 {
			req.ErrCnt = 0
		}
	} else {
		//连续采集超过5次都错误，则报告设备离线
		if req.ErrCnt == 5 {
			//离线状态报告
			var obj protocol.Pack_UploadData
			if str, err := obj.EnCode(req.Rtuinfo.DevCode, appConfig.GID, GetNextUint64(), err, req.Rtuinfo.TID, nil); err == nil {
				topic := GetTopic(o.GetDevType(), req.Rtuinfo.DevCode, protocol.TP_MODBUS_DATA)
				GetMQTTMgr().Publish(topic, str, 0, ToAll)
			}
		}
	}
}

func (o *ModbusRtu) procRequest(req *Request) {
	var err error
	var result []byte
	defer func() {
		recover()
	}()
	req.TxCnt++
	switch req.FuncCode {
	// A bit of access read
	case modbus.FuncCodeReadCoils:
		result, err = o.ReadCoils(req.Rtuinfo.DevID, req.Address, req.Quantity)
		if err == nil {
			o.ProcReadCoils(req.CID, req.Address, req.Quantity, result)
		}
	case modbus.FuncCodeReadDiscreteInputs:
		result, err = o.ReadDiscreteInputs(req.Rtuinfo.DevID, req.Address, req.Quantity)
		if err == nil {
			o.ReadDiscrete(req.CID, req.Address, req.Quantity, result)
		}
	// 16-bit access read
	case modbus.FuncCodeReadHoldingRegisters: //03
		result, err = o.ReadHoldingRegistersBytes(req.Rtuinfo.DevID, req.Address, req.Quantity)
		if err == nil {
			o.ProcReadHoldingRegisters(req.CID, req.Address, req.Quantity, result)
		}

	case modbus.FuncCodeReadInputRegisters:
		result, err = o.ReadInputRegistersBytes(req.Rtuinfo.DevID, req.Address, req.Quantity)
		if err == nil {
			o.ProcReadInputRegisters(req.CID, req.Address, req.Quantity, result)
		}
	}

	if err != nil {
		req.ErrCnt++
	}

	o.ProcResult(err, req)

	if req.ScanRate > 0 {
		timing.AddJobFunc(req.tmHandler, req.ScanRate)
	}
}

func (o *ModbusRtu) HandleTpWControl(m mqtt.Message) {
	var obj protocol.Pack_ControlData
	var ret protocol.Pack_Ack
	var err error
	if err = obj.DeCode(m.PayloadString()); err == nil {
		if v, ok := o.model.DataDown[obj.Data.Sid]; ok {
			var data []byte
			if v.Vallen == 1 { //1个字节
				data = make([]byte, 1, 1)
				data = append(data, byte(obj.Data.Val))
			} else if v.Vallen == 2 { //2个字节
				data = make([]byte, 2, 2)
				binary.BigEndian.PutUint16(data, uint16(obj.Data.Val))
			} else if v.Vallen == 4 {
				data = make([]byte, 4, 4)
				binary.BigEndian.PutUint32(data, uint32(obj.Data.Val))
			}
			err = o.WriteData(o.devInfo.DevID, v.Code, v.Addr, v.Quantity, data)
		} else {
			err = errors.New(fmt.Sprintf("物模型[TID=%d]未配置该SID[sid=%d]，请确保模型文件存在", obj.Data.Tid, obj.Data.Sid))
		}
	}
	if str, err := ret.EnCode(o.devInfo.DevCode, appConfig.GID, obj.Seq, err); err == nil {
		GetMQTTMgr().Publish(GetTopic(o.GetDevType(), o.devInfo.DevCode, protocol.TP_MODBUS_CONTROL_ACK), str, 0, ToAll)
	}
}

func (o *ModbusRtu) WriteData(slaveID, funcCode byte, address, quantity uint16, value []byte) error {
	var err error
	defer func() {
		recover()
	}()
	switch funcCode {
	case modbus.FuncCodeWriteSingleCoil: //5
		var isOn = false
		if len(value) != 2 {
			return errors.New("数据长度不对")
		}
		if binary.BigEndian.Uint16(value) > 0 {
			isOn = true
		}
		err = o.WriteSingleCoil(slaveID, address, isOn)
	case modbus.FuncCodeWriteMultipleCoils: //15
		err = o.WriteMultipleCoils(slaveID, address, quantity, value)
	case modbus.FuncCodeWriteSingleRegister: //6
		err = o.WriteSingleRegister(slaveID, address, binary.BigEndian.Uint16(value))
	case modbus.FuncCodeWriteMultipleRegisters: //16
		err = o.WriteMultipleRegistersBytes(slaveID, address, quantity, value)
	default:
		err = errors.New("不支持的功能码")
	}
	return err
}

func (o *ModbusRtu) SendRecvData(aduRequest []byte) (aduResponse []byte, err error) {
	serial := GetSerialMgr().GetSerialPort(o.devInfo.Code)
	if serial == nil {
		return nil, ErrClosedConnection
	}
	return serial.SendRecvData(aduRequest, FlagModbusRtu, o.devInfo.WaitTime)
}

// Send request to the remote server, it implements on SendRawFrame
func (o *ModbusRtu) Send(slaveID byte, request modbus.ProtocolDataUnit) (modbus.ProtocolDataUnit, error) {
	var response modbus.ProtocolDataUnit
	aduRequest, err := modbus.EncodeRTUFrame(slaveID, request)
	if err != nil {
		return response, err
	}
	aduResponse, err := o.SendRecvData(aduRequest)

	if err != nil {
		return response, err
	}
	rspSlaveID, pdu, err := modbus.DecodeRTUFrame(aduResponse)
	if err != nil {
		return response, err
	}
	response = modbus.ProtocolDataUnit{FuncCode: pdu[0], Data: pdu[1:]}
	return response, modbus.Verify(slaveID, rspSlaveID, request, response)
}

// SendPdu send pdu request to the remote server
func (o *ModbusRtu) SendPdu(slaveID byte, pduRequest []byte) ([]byte, error) {
	if len(pduRequest) < modbus.PduMinSize || len(pduRequest) > modbus.PduMaxSize {
		return nil, fmt.Errorf("modbus: pdu size '%v' must not be between '%v' and '%v'", len(pduRequest), modbus.PduMinSize, modbus.PduMaxSize)
	}
	request := modbus.ProtocolDataUnit{FuncCode: pduRequest[0], Data: pduRequest[1:]}
	requestAdu, err := modbus.EncodeRTUFrame(slaveID, request)
	if err != nil {
		return nil, err
	}

	aduResponse, err := o.SendRecvData(requestAdu)
	if err != nil {
		return nil, err
	}
	rspSlaveID, pdu, err := modbus.DecodeRTUFrame(aduResponse)
	if err != nil {
		return nil, err
	}
	//  PDU pass slaveID & crc
	return pdu, modbus.Verify(slaveID, rspSlaveID, request, modbus.ProtocolDataUnit{FuncCode: pdu[0], Data: pdu[1:]})
}

// ReadCoils Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x01)
//	Starting address      : 2 bytes
//	Quantity of coils     : 2 bytes
//
// Response:
//
//	Function code         : 1 byte (0x01)
//	Byte count            : 1 byte
//	Coil status           : N* bytes (=N or N+1)
//	return coils status
func (o *ModbusRtu) ReadCoils(slaveID byte, address, quantity uint16) ([]byte, error) {
	if slaveID < modbus.AddressMin || slaveID > modbus.AddressMax {
		return nil, fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			slaveID, modbus.AddressMin, modbus.AddressMax)
	}
	if quantity < modbus.ReadBitsQuantityMin || quantity > modbus.ReadBitsQuantityMax {
		return nil, fmt.Errorf("modbus: quantity '%v' must be between '%v' and '%v'",
			quantity, modbus.ReadBitsQuantityMin, modbus.ReadBitsQuantityMax)
	}
	response, err := o.Send(slaveID, modbus.ProtocolDataUnit{FuncCode: modbus.FuncCodeReadCoils, Data: uint162Bytes(address, quantity)})
	switch {
	case err != nil:
		return nil, err
	case len(response.Data)-1 != int(response.Data[0]):
		return nil, fmt.Errorf("modbus: response byte size '%v' does not match count '%v'",
			len(response.Data)-1, int(response.Data[0]))
	case uint16(response.Data[0]) != (quantity+7)/8:
		return nil, fmt.Errorf("modbus: response byte size '%v' does not match quantity to bytes '%v'",
			response.Data[0], (quantity+7)/8)
	}
	return response.Data[1:], nil
}

// ReadDiscreteInputs Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x02)
//	Starting address      : 2 bytes
//	Quantity of inputs    : 2 bytes
//
// Response:
//
//	Function code         : 1 byte (0x02)
//	Byte count            : 1 byte
//	Input status          : N* bytes (=N or N+1)
//	return result data
func (o *ModbusRtu) ReadDiscreteInputs(slaveID byte, address, quantity uint16) ([]byte, error) {
	if slaveID < modbus.AddressMin || slaveID > modbus.AddressMax {
		return nil, fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			slaveID, modbus.AddressMin, modbus.AddressMax)
	}
	if quantity < modbus.ReadBitsQuantityMin || quantity > modbus.ReadBitsQuantityMax {
		return nil, fmt.Errorf("modbus: quantity '%v' must be between '%v' and '%v'",
			quantity, modbus.ReadBitsQuantityMin, modbus.ReadBitsQuantityMax)
	}
	response, err := o.Send(slaveID, modbus.ProtocolDataUnit{
		FuncCode: modbus.FuncCodeReadDiscreteInputs,
		Data:     uint162Bytes(address, quantity),
	})

	switch {
	case err != nil:
		return nil, err
	case len(response.Data)-1 != int(response.Data[0]):
		return nil, fmt.Errorf("modbus: response byte size '%v' does not match count '%v'",
			len(response.Data)-1, response.Data[0])
	case uint16(response.Data[0]) != (quantity+7)/8:
		return nil, fmt.Errorf("modbus: response byte size '%v' does not match quantity to bytes '%v'",
			response.Data[0], (quantity+7)/8)
	}
	return response.Data[1:], nil
}

// WriteSingleCoil Request:
//
//	Slave Id              : 1 byte
//	Function code         : 1 byte (0x05)
//	Output address        : 2 bytes
//	Output value          : 2 bytes
//
// Response:
//
//	Function code         : 1 byte (0x05)
//	Output address        : 2 bytes
//	Output value          : 2 bytes
func (o *ModbusRtu) WriteSingleCoil(slaveID byte, address uint16, isOn bool) error {
	if slaveID > modbus.AddressMax {
		return fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			slaveID, modbus.AddressBroadCast, modbus.AddressMax)
	}
	var value uint16
	if isOn { // The requested ON/OFF state can only be 0xFF00 and 0x0000
		value = 0xFF00
	}
	response, err := o.Send(slaveID, modbus.ProtocolDataUnit{
		FuncCode: modbus.FuncCodeWriteSingleCoil,
		Data:     uint162Bytes(address, value),
	})

	switch {
	case err != nil:
		return err
	case len(response.Data) != 4:
		// Fixed response length
		return fmt.Errorf("modbus: response data size '%v' does not match expected '%v'",
			len(response.Data), 4)
	case binary.BigEndian.Uint16(response.Data) != address:
		// check address
		return fmt.Errorf("modbus: response address '%v' does not match request '%v'",
			binary.BigEndian.Uint16(response.Data), address)
	case binary.BigEndian.Uint16(response.Data[2:]) != value:
		// check value
		return fmt.Errorf("modbus: response value '%v' does not match request '%v'",
			binary.BigEndian.Uint16(response.Data[2:]), value)
	}
	return nil
}

// WriteMultipleCoils Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x0F)
//	Starting address      : 2 bytes
//	Quantity of outputs   : 2 bytes
//	Byte count            : 1 byte
//	Outputs value         : N* bytes
//
// Response:
//
//	Function code         : 1 byte (0x0F)
//	Starting address      : 2 bytes
//	Quantity of outputs   : 2 bytes
func (o *ModbusRtu) WriteMultipleCoils(slaveID byte, address, quantity uint16, value []byte) error {
	if slaveID > modbus.AddressMax {
		return fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			slaveID, modbus.AddressBroadCast, modbus.AddressMax)
	}
	if quantity < modbus.WriteBitsQuantityMin || quantity > modbus.WriteBitsQuantityMax {
		return fmt.Errorf("modbus: quantity '%v' must be between '%v' and '%v'",
			quantity, modbus.WriteBitsQuantityMin, modbus.WriteBitsQuantityMax)
	}

	if len(value)*8 < int(quantity) {
		return fmt.Errorf("modbus: value bits size '%v' does not greater or equal to quantity '%v'", len(value)*8, quantity)
	}

	response, err := o.Send(slaveID, modbus.ProtocolDataUnit{
		FuncCode: modbus.FuncCodeWriteMultipleCoils,
		Data:     pduDataBlockSuffix(value, address, quantity),
	})

	switch {
	case err != nil:
		return err
	case len(response.Data) != 4:
		// Fixed response length
		return fmt.Errorf("modbus: response data size '%v' does not match expected '%v'",
			len(response.Data), 4)
	case binary.BigEndian.Uint16(response.Data) != address:
		return fmt.Errorf("modbus: response address '%v' does not match request '%v'",
			binary.BigEndian.Uint16(response.Data), address)
	case binary.BigEndian.Uint16(response.Data[2:]) != quantity:
		return fmt.Errorf("modbus: response quantity '%v' does not match request '%v'",
			binary.BigEndian.Uint16(response.Data[2:]), quantity)
	}
	return nil
}

/*********************************16-bits**************************************/

// ReadInputRegistersBytes Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x04)
//	Starting address      : 2 bytes
//	Quantity of registers : 2 bytes
//
// Response:
//
//	Function code         : 1 byte (0x04)
//	Byte count            : 1 byte
//	Input registers       : Nx2 bytes
func (o *ModbusRtu) ReadInputRegistersBytes(slaveID byte, address, quantity uint16) ([]byte, error) {
	if slaveID < modbus.AddressMin || slaveID > modbus.AddressMax {
		return nil, fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			slaveID, modbus.AddressMin, modbus.AddressMax)
	}
	if quantity < modbus.ReadRegQuantityMin || quantity > modbus.ReadRegQuantityMax {
		return nil, fmt.Errorf("modbus: quantity '%v' must be between '%v' and '%v'",
			quantity, modbus.ReadRegQuantityMin, modbus.ReadRegQuantityMax)

	}
	response, err := o.Send(slaveID, modbus.ProtocolDataUnit{
		FuncCode: modbus.FuncCodeReadInputRegisters,
		Data:     uint162Bytes(address, quantity),
	})

	switch {
	case err != nil:
		return nil, err
	case len(response.Data)-1 != int(response.Data[0]):
		return nil, fmt.Errorf("modbus: response data size '%v' does not match count '%v'",
			len(response.Data)-1, response.Data[0])
	case uint16(response.Data[0]) != quantity*2:
		return nil, fmt.Errorf("modbus: response data size '%v' does not match quantity to bytes '%v'",
			response.Data[0], quantity*2)
	}

	return response.Data[1:], nil
}

// ReadInputRegisters Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x04)
//	Starting address      : 2 bytes
//	Quantity of registers : 2 bytes
//
// Response:
//
//	Function code         : 1 byte (0x04)
//	Byte count            : 1 byte
//	Input registers       : N 2-bytes
func (o *ModbusRtu) ReadInputRegisters(slaveID byte, address, quantity uint16) ([]uint16, error) {
	b, err := o.ReadInputRegistersBytes(slaveID, address, quantity)
	if err != nil {
		return nil, err
	}
	return bytes2Uint16(b), nil
}

// ReadHoldingRegistersBytes Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x03)
//	Starting address      : 2 bytes
//	Quantity of registers : 2 bytes
//
// Response:
//
//	Function code         : 1 byte (0x03)
//	Byte count            : 1 byte
//	Register value        : Nx2 bytes
func (o *ModbusRtu) ReadHoldingRegistersBytes(slaveID byte, address, quantity uint16) ([]byte, error) {
	if slaveID < modbus.AddressMin || slaveID > modbus.AddressMax {
		return nil, fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			slaveID, modbus.AddressMin, modbus.AddressMax)
	}
	if quantity < modbus.ReadRegQuantityMin || quantity > modbus.ReadRegQuantityMax {
		return nil, fmt.Errorf("modbus: quantity '%v' must be between '%v' and '%v'",
			quantity, modbus.ReadRegQuantityMin, modbus.ReadRegQuantityMax)
	}
	response, err := o.Send(slaveID, modbus.ProtocolDataUnit{
		FuncCode: modbus.FuncCodeReadHoldingRegisters,
		Data:     uint162Bytes(address, quantity),
	})

	switch {
	case err != nil:
		return nil, err
	case len(response.Data)-1 != int(response.Data[0]):
		return nil, fmt.Errorf("modbus: response data size '%v' does not match count '%v'",
			len(response.Data)-1, response.Data[0])
		//case uint16(response.Data[0]) != quantity*2:
		//	return nil, fmt.Errorf("modbus: response data size '%v' does not match quantity to bytes '%v'",
		//		response.Data[0], quantity*2)
	}
	return response.Data[1:], nil
}

// ReadHoldingRegisters Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x03)
//	Starting address      : 2 bytes
//	Quantity of registers : 2 bytes
//
// Response:
//
//	Function code         : 1 byte (0x03)
//	Byte count            : 1 byte
//	Register value        : N 2-bytes
func (o *ModbusRtu) ReadHoldingRegisters(slaveID byte, address, quantity uint16) ([]uint16, error) {
	b, err := o.ReadHoldingRegistersBytes(slaveID, address, quantity)
	if err != nil {
		return nil, err
	}
	return bytes2Uint16(b), nil
}

// WriteSingleRegister Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x06)
//	Register address      : 2 bytes
//	Register value        : 2 bytes
//
// Response:
//
//	Function code         : 1 byte (0x06)
//	Register address      : 2 bytes
//	Register value        : 2 bytes
func (o *ModbusRtu) WriteSingleRegister(slaveID byte, address, value uint16) error {
	if slaveID > modbus.AddressMax {
		return fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			slaveID, modbus.AddressBroadCast, modbus.AddressMax)
	}
	response, err := o.Send(slaveID, modbus.ProtocolDataUnit{
		FuncCode: modbus.FuncCodeWriteSingleRegister,
		Data:     uint162Bytes(address, value),
	})

	switch {
	case err != nil:
		return err
	case len(response.Data) != 4:
		// Fixed response length
		return fmt.Errorf("modbus: response data size '%v' does not match expected '%v'",
			len(response.Data), 4)
	case binary.BigEndian.Uint16(response.Data) != address:
		return fmt.Errorf("modbus: response address '%v' does not match request '%v'",
			binary.BigEndian.Uint16(response.Data), address)
	case binary.BigEndian.Uint16(response.Data[2:]) != value:
		return fmt.Errorf("modbus: response value '%v' does not match request '%v'",
			binary.BigEndian.Uint16(response.Data[2:]), value)
	}
	return nil
}

// WriteMultipleRegistersBytes Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x10)
//	Starting address      : 2 bytes
//	Quantity of outputs   : 2 bytes
//	Byte count            : 1 byte
//	Registers value       : N* bytes
//
// Response:
//
//	Function code         : 1 byte (0x10)
//	Starting address      : 2 bytes
//	Quantity of registers : 2 bytes
func (o *ModbusRtu) WriteMultipleRegistersBytes(slaveID byte, address, quantity uint16, value []byte) error {
	if slaveID > modbus.AddressMax {
		return fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			slaveID, modbus.AddressBroadCast, modbus.AddressMax)
	}
	if quantity < modbus.WriteRegQuantityMin || quantity > modbus.WriteRegQuantityMax {
		return fmt.Errorf("modbus: quantity '%v' must be between '%v' and '%v'",
			quantity, modbus.WriteRegQuantityMin, modbus.WriteRegQuantityMax)
	}

	if len(value) != int(quantity*2) {
		return fmt.Errorf("modbus: value length '%v' does not twice as quantity '%v'", len(value), quantity)
	}

	response, err := o.Send(slaveID, modbus.ProtocolDataUnit{
		FuncCode: modbus.FuncCodeWriteMultipleRegisters,
		Data:     pduDataBlockSuffix(value, address, quantity),
	})

	switch {
	case err != nil:
		return err
	case len(response.Data) != 4:
		// Fixed response length
		return fmt.Errorf("modbus: response data size '%v' does not match expected '%v'",
			len(response.Data), 4)
	case binary.BigEndian.Uint16(response.Data) != address:
		return fmt.Errorf("modbus: response address '%v' does not match request '%v'",
			binary.BigEndian.Uint16(response.Data), address)
	case binary.BigEndian.Uint16(response.Data[2:]) != quantity:
		return fmt.Errorf("modbus: response quantity '%v' does not match request '%v'",
			binary.BigEndian.Uint16(response.Data[2:]), quantity)
	}
	return nil
}

// WriteMultipleRegisters Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x10)
//	Starting address      : 2 bytes
//	Quantity of outputs   : 2 bytes
//	Byte count            : 1 byte
//	Registers value       : N* bytes
//
// Response:
//
//	Function code         : 1 byte (0x10)
//	Starting address      : 2 bytes
//	Quantity of registers : 2 bytes
func (o *ModbusRtu) WriteMultipleRegisters(slaveID byte, address, quantity uint16, value []uint16) error {
	return o.WriteMultipleRegistersBytes(slaveID, address, quantity, uint162Bytes(value...))
}

// MaskWriteRegister Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x16)
//	Reference address     : 2 bytes
//	AND-mask              : 2 bytes
//	OR-mask               : 2 bytes
//
// Response:
//
//	Function code         : 1 byte (0x16)
//	Reference address     : 2 bytes
//	AND-mask              : 2 bytes
//	OR-mask               : 2 bytes
func (o *ModbusRtu) MaskWriteRegister(slaveID byte, address, andMask, orMask uint16) error {
	if slaveID > modbus.AddressMax {
		return fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			slaveID, modbus.AddressBroadCast, modbus.AddressMax)
	}
	response, err := o.Send(slaveID, modbus.ProtocolDataUnit{
		FuncCode: modbus.FuncCodeMaskWriteRegister,
		Data:     uint162Bytes(address, andMask, orMask),
	})

	switch {
	case err != nil:
		return err
	case len(response.Data) != 6:
		// Fixed response length
		return fmt.Errorf("modbus: response data size '%v' does not match expected '%v'",
			len(response.Data), 6)
	case binary.BigEndian.Uint16(response.Data) != address:
		return fmt.Errorf("modbus: response address '%v' does not match request '%v'",
			binary.BigEndian.Uint16(response.Data), address)
	case binary.BigEndian.Uint16(response.Data[2:]) != andMask:
		return fmt.Errorf("modbus: response AND-mask '%v' does not match request '%v'",
			binary.BigEndian.Uint16(response.Data[2:]), andMask)
	case binary.BigEndian.Uint16(response.Data[4:]) != orMask:
		return fmt.Errorf("modbus: response OR-mask '%v' does not match request '%v'",
			binary.BigEndian.Uint16(response.Data[4:]), orMask)
	}
	return nil
}

// ReadWriteMultipleRegistersBytes Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x17)
//	Read starting address : 2 bytes
//	Quantity to read      : 2 bytes
//	Write starting address: 2 bytes
//	Quantity to write     : 2 bytes
//	Write byte count      : 1 byte
//	Write registers value : N* bytes
//
// Response:
//
//	Function code         : 1 byte (0x17)
//	Byte count            : 1 byte
//	Read registers value  : Nx2 bytes
func (o *ModbusRtu) ReadWriteMultipleRegistersBytes(slaveID byte, readAddress, readQuantity,
	writeAddress, writeQuantity uint16, value []byte) ([]byte, error) {
	if slaveID < modbus.AddressMin || slaveID > modbus.AddressMax {
		return nil, fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			slaveID, modbus.AddressMin, modbus.AddressMax)
	}
	if readQuantity < modbus.ReadWriteOnReadRegQuantityMin || readQuantity > modbus.ReadWriteOnReadRegQuantityMax {
		return nil, fmt.Errorf("modbus: quantity to read '%v' must be between '%v' and '%v'",
			readQuantity, modbus.ReadWriteOnReadRegQuantityMin, modbus.ReadWriteOnReadRegQuantityMax)
	}
	if writeQuantity < modbus.ReadWriteOnWriteRegQuantityMin || writeQuantity > modbus.ReadWriteOnWriteRegQuantityMax {
		return nil, fmt.Errorf("modbus: quantity to write '%v' must be between '%v' and '%v'",
			writeQuantity, modbus.ReadWriteOnWriteRegQuantityMin, modbus.ReadWriteOnWriteRegQuantityMax)
	}

	if len(value) != int(writeQuantity*2) {
		return nil, fmt.Errorf("modbus: value length '%v' does not twice as write quantity '%v'",
			len(value), writeQuantity)
	}

	response, err := o.Send(slaveID, modbus.ProtocolDataUnit{
		FuncCode: modbus.FuncCodeReadWriteMultipleRegisters,
		Data:     pduDataBlockSuffix(value, readAddress, readQuantity, writeAddress, writeQuantity),
	})
	if err != nil {
		return nil, err
	}
	if int(response.Data[0]) != (len(response.Data) - 1) {
		return nil, fmt.Errorf("modbus: response data size '%v' does not match count '%v'",
			len(response.Data)-1, response.Data[0])
	}
	return response.Data[1:], nil
}

// ReadWriteMultipleRegisters Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x17)
//	Read starting address quantity: 2 bytes
//	Quantity to read      : 2 bytes
//	Write starting address: 2 bytes
//	Quantity to write     : 2 bytes
//	Write byte count      : 1 byte
//	Write registers value : N* bytes
//
// Response:
//
//	Function code         : 1 byte (0x17)
//	Byte count            : 1 byte
//	Read registers value  : N 2-bytes
func (o *ModbusRtu) ReadWriteMultipleRegisters(slaveID byte, readAddress, readQuantity,
	writeAddress, writeQuantity uint16, value []byte) ([]uint16, error) {
	b, err := o.ReadWriteMultipleRegistersBytes(slaveID, readAddress, readQuantity,
		writeAddress, writeQuantity, value)
	if err != nil {
		return nil, err
	}
	return bytes2Uint16(b), nil
}

// ReadFIFOQueue Request:
//
//	Slave ID              : 1 byte
//	Function code         : 1 byte (0x18)
//	FIFO pointer address  : 2 bytes
//
// Response:
//
//	Function code         : 1 byte (0x18)
//	Byte count            : 2 bytes  only include follow
//	FIFO count            : 2 bytes (<=31)
//	FIFO value register   : Nx2 bytes
func (o *ModbusRtu) ReadFIFOQueue(slaveID byte, address uint16) ([]byte, error) {
	if slaveID < modbus.AddressMin || slaveID > modbus.AddressMax {
		return nil, fmt.Errorf("modbus: slaveID '%v' must be between '%v' and '%v'",
			slaveID, modbus.AddressMin, modbus.AddressMax)
	}
	response, err := o.Send(slaveID, modbus.ProtocolDataUnit{
		FuncCode: modbus.FuncCodeReadFIFOQueue,
		Data:     uint162Bytes(address),
	})
	switch {
	case err != nil:
		return nil, err
	case len(response.Data) < 4:
		return nil, fmt.Errorf("modbus: response data size '%v' is less than expected '%v'",
			len(response.Data), 4)
	case len(response.Data)-2 != int(binary.BigEndian.Uint16(response.Data)):
		return nil, fmt.Errorf("modbus: response data size '%v' does not match count '%v'",
			len(response.Data)-2, binary.BigEndian.Uint16(response.Data))
	case int(binary.BigEndian.Uint16(response.Data[2:])) > 31:
		return nil, fmt.Errorf("modbus: fifo count '%v' is greater than expected '%v'",
			binary.BigEndian.Uint16(response.Data[2:]), 31)
	}
	return response.Data[4:], nil
}

// uint162Bytes creates a sequence of uint16 data.
func uint162Bytes(value ...uint16) []byte {
	data := make([]byte, 2*len(value))
	for i, v := range value {
		binary.BigEndian.PutUint16(data[i*2:], v)
	}
	return data
}

// bytes2Uint16 bytes convert to uint16 for register
func bytes2Uint16(buf []byte) []uint16 {
	data := make([]uint16, 0, len(buf)/2)
	for i := 0; i < len(buf)/2; i++ {
		data = append(data, binary.BigEndian.Uint16(buf[i*2:]))
	}
	return data
}

// pduDataBlockSuffix creates a sequence of uint16 data and append the suffix plus its length.
func pduDataBlockSuffix(suffix []byte, value ...uint16) []byte {
	length := 2 * len(value)
	data := make([]byte, length+1+len(suffix))
	for i, v := range value {
		binary.BigEndian.PutUint16(data[i*2:], v)
	}
	data[length] = uint8(len(suffix))
	copy(data[length+1:], suffix)
	return data
}