ntool-biz/nmodbus/handler/functions.go

package handler

import (
	"git.noahlan.cn/noahlan/ntool-biz/nmodbus/protocol"
	"git.noahlan.cn/noahlan/ntool-biz/nmodbus/serialize"
	"git.noahlan.cn/noahlan/ntool-biz/nmodbus/util"
)

// ReadCoils 读线圈寄存器（DO） 功能码0x01
func ReadCoils(s *Handler, packet protocol.Packet) ([]byte, *protocol.MError) {
	register, numRegs, endRegister := serialize.RegisterAddressAndNumber(packet.GetBody())
	if endRegister > 65535 {
		return []byte{}, &protocol.IllegalDataAddress
	}
	// 一个寄存器2字节
	dataSize := numRegs / 8
	if (numRegs % 8) != 0 {
		dataSize++
	}
	data := make([]byte, 1+dataSize)
	data[0] = byte(dataSize)
	for i, value := range s.Coils[register:endRegister] {
		if value != 0 {
			shift := uint(i) % 8
			data[1+i/8] |= byte(1 << shift)
		}
	}
	return data, &protocol.Success
}

// ReadDiscreteInputs 读离散输入寄存器(DI) 功能码0x02
func ReadDiscreteInputs(s *Handler, packet protocol.Packet) ([]byte, *protocol.MError) {
	register, numRegs, endRegister := serialize.RegisterAddressAndNumber(packet.GetBody())
	if endRegister > 65535 {
		return []byte{}, &protocol.IllegalDataAddress
	}
	dataSize := numRegs / 8
	if (numRegs % 8) != 0 {
		dataSize++
	}
	data := make([]byte, 1+dataSize)
	data[0] = byte(dataSize)
	for i, value := range s.DiscreteInputs[register:endRegister] {
		if value != 0 {
			shift := uint(i) % 8
			data[1+i/8] |= byte(1 << shift)
		}
	}
	return data, &protocol.Success
}

// ReadHoldingRegisters 读保持寄存器（AI） 功能码0x03
func ReadHoldingRegisters(s *Handler, packet protocol.Packet) ([]byte, *protocol.MError) {
	register, numRegs, endRegister := serialize.RegisterAddressAndNumber(packet.GetBody())
	if endRegister > 65536 {
		return []byte{}, &protocol.IllegalDataAddress
	}
	return append([]byte{byte(numRegs * 2)}, util.Uint16ToBytes(s.byteOrder, s.HoldingRegisters[register:endRegister])...), &protocol.Success
}

// ReadInputRegisters 读输入寄存器（AO） 功能码0x04
func ReadInputRegisters(s *Handler, packet protocol.Packet) ([]byte, *protocol.MError) {
	register, numRegs, endRegister := serialize.RegisterAddressAndNumber(packet.GetBody())
	if endRegister > 65536 {
		return []byte{}, &protocol.IllegalDataAddress
	}
	return append([]byte{byte(numRegs * 2)}, util.Uint16ToBytes(s.byteOrder, s.InputRegisters[register:endRegister])...), &protocol.Success
}

// WriteSingleCoil 写单个线圈寄存器 功能码0x05
func WriteSingleCoil(s *Handler, packet protocol.Packet) ([]byte, *protocol.MError) {
	data := packet.GetBody()
	register, value := serialize.RegisterAddressAndValue(data)
	// TODO Should we use 0 for off and 65,280 (0xFF00) for on?
	if value != 0 {
		value = 1
	}
	s.Coils[register] = byte(value)
	return data[0:4], &protocol.Success
}

// WriteHoldingRegister 写单个保持寄存器 功能码0x06
func WriteHoldingRegister(s *Handler, packet protocol.Packet) ([]byte, *protocol.MError) {
	data := packet.GetBody()
	register, value := serialize.RegisterAddressAndValue(data)
	s.HoldingRegisters[register] = value
	return data[0:4], &protocol.Success
}

// WriteMultipleCoils 写多个线圈寄存器 功能码 0x0F=15
func WriteMultipleCoils(s *Handler, packet protocol.Packet) ([]byte, *protocol.MError) {
	data := packet.GetBody()
	register, numRegs, endRegister := serialize.RegisterAddressAndNumber(data)
	valueBytes := data[5:]

	if endRegister > 65536 {
		return []byte{}, &protocol.IllegalDataAddress
	}

	// TODO This is not correct, bits and bytes do not always align
	//if len(valueBytes)/2 != numRegs {
	//	return []byte{}, &IllegalDataAddress
	//}

	bitCount := 0
	for i, value := range valueBytes {
		for bitPos := uint(0); bitPos < 8; bitPos++ {
			s.Coils[register+(i*8)+int(bitPos)] = util.BitAtPosition(value, bitPos)
			bitCount++
			if bitCount >= numRegs {
				break
			}
		}
		if bitCount >= numRegs {
			break
		}
	}

	return data[0:4], &protocol.Success
}

// WriteHoldingRegisters 写入 多个保持寄存器 功能码 0x10=16
func WriteHoldingRegisters(s *Handler, packet protocol.Packet) ([]byte, *protocol.MError) {
	pkgData := packet.GetBody()
	register, numRegs, _ := serialize.RegisterAddressAndNumber(packet.GetBody())
	valueBytes := pkgData[5:]
	var err *protocol.MError
	var data []byte

	if len(valueBytes)/2 != numRegs {
		err = &protocol.IllegalDataAddress
	}

	// Copy data to memory
	values := util.BytesToUint16(s.byteOrder, valueBytes)
	valuesUpdated := copy(s.HoldingRegisters[register:], values)
	if valuesUpdated == numRegs {
		err = &protocol.Success
		data = pkgData[0:4]
	} else {
		err = &protocol.IllegalDataAddress
	}

	return data, err
}