From 4283f1e5dd08ab0a010830e74394d411bf51739a Mon Sep 17 00:00:00 2001 From: NoahLan <6995syu@163.com> Date: Mon, 19 Jun 2023 16:52:00 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=B7=BB=E5=8A=A0modbus=E8=AF=B7?= =?UTF-8?q?=E6=B1=82hook=EF=BC=8C=E5=8C=85=E6=8B=ACread=E5=92=8Cwrite?= =?UTF-8?q?=E9=83=A8=E5=88=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- nmodbus/handler/fn_coils.go | 139 ++++++++++++++++++++ nmodbus/handler/fn_discreteinputs.go | 48 +++++++ nmodbus/handler/fn_holdingregisters.go | 110 ++++++++++++++++ nmodbus/handler/fn_inputregisters.go | 35 +++++ nmodbus/handler/functions.go | 169 ------------------------- nmodbus/handler/handler.go | 66 ++++++---- 6 files changed, 373 insertions(+), 194 deletions(-) create mode 100644 nmodbus/handler/fn_coils.go create mode 100644 nmodbus/handler/fn_discreteinputs.go create mode 100644 nmodbus/handler/fn_holdingregisters.go create mode 100644 nmodbus/handler/fn_inputregisters.go delete mode 100644 nmodbus/handler/functions.go diff --git a/nmodbus/handler/fn_coils.go b/nmodbus/handler/fn_coils.go new file mode 100644 index 0000000..a9896af --- /dev/null +++ b/nmodbus/handler/fn_coils.go @@ -0,0 +1,139 @@ +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" +) + +type ReadCoils struct { + fnCode uint8 +} + +func NewReadCoils(fnCode uint8) FunctionHandler { + return &ReadCoils{fnCode: fnCode} +} + +func (r *ReadCoils) FnCode() uint8 { + return r.fnCode +} + +// Function 读线圈寄存器（DO） 功能码0x01 +func (r *ReadCoils) Function(s *Handler, req protocol.Packet, resp protocol.Packet) *protocol.MError { + register, numRegs, endRegister := serialize.RegisterAddressAndNumber(req.GetBody()) + if endRegister > 65535 { + resp.SetBody([]byte{}) + return &protocol.IllegalDataAddress + } + // 一个寄存器2字节 + dataSize := numRegs / 8 + if (numRegs % 8) != 0 { + dataSize++ + } + data := make([]byte, 1+dataSize) + data[0] = byte(dataSize) + + dataMgr := s.DataManager(req.GetAddress()) + coils := dataMgr.ReadCoils(register, endRegister) + for i, value := range coils { + if value != 0 { + shift := uint(i) % 8 + data[1+i/8] |= byte(1 << shift) + } + } + + s.HandleLogic(r.fnCode, register, numRegs, coils) + + resp.SetBody(data) + return &protocol.Success +} + +////////////////////////////////////////////// write-single + +type WriteSingleCoil struct { + fnCode uint8 +} + +func NewWriteSingleCoil(fnCode uint8) FunctionHandler { + return &WriteSingleCoil{fnCode: fnCode} +} + +func (r *WriteSingleCoil) FnCode() uint8 { + return r.fnCode +} + +// Function 写单个线圈寄存器 功能码0x05 +func (r *WriteSingleCoil) Function(s *Handler, req protocol.Packet, resp protocol.Packet) *protocol.MError { + data := req.GetBody() + register, value := serialize.RegisterAddressAndValue(data) + // TODO Should we use 0 for off and 65,280 (0xFF00) for on? + if value != 0 { + value = 1 + } + dataMgr := s.DataManager(req.GetAddress()) + dataMgr.WriteSingleCoil(register, byte(value)) + + s.HandleLogic(r.fnCode, register, 1, value) + + resp.SetBody(data[0:4]) + + return &protocol.Success +} + +////////////////////////////////////////////// write-multiple + +type WriteMultipleCoils struct { + fnCode uint8 +} + +func NewWriteMultipleCoils(fnCode uint8) FunctionHandler { + return &WriteMultipleCoils{fnCode: fnCode} +} + +func (r *WriteMultipleCoils) FnCode() uint8 { + return r.fnCode +} + +// Function 写多个线圈寄存器 功能码 0x0F=15 +func (r *WriteMultipleCoils) Function(s *Handler, req protocol.Packet, resp protocol.Packet) *protocol.MError { + data := req.GetBody() + register, numRegs, endRegister := serialize.RegisterAddressAndNumber(data) + valueBytes := data[5:] + + if endRegister > 65536 { + resp.SetBody([]byte{}) + return &protocol.IllegalDataAddress + } + + // TODO This is not correct, bits and bytes do not always align + //if len(valueBytes)/2 != numRegs { + // return []byte{}, &IllegalDataAddress + //} + + idxList := make([]int, 0) + values := make([]byte, 0) + bitCount := 0 + for i, value := range valueBytes { + for bitPos := uint(0); bitPos < 8; bitPos++ { + idxList = append(idxList, register+(i*8)+int(bitPos)) + values = append(values, util.BitAtPosition(value, bitPos)) + + bitCount++ + if bitCount >= numRegs { + break + } + } + if bitCount >= numRegs { + break + } + } + + dataMgr := s.DataManager(req.GetAddress()) + dataMgr.WriteCoils(idxList, values) + + s.HandleLogic(r.fnCode, register, numRegs, values) + + resp.SetBody(data[0:4]) + + return &protocol.Success +} diff --git a/nmodbus/handler/fn_discreteinputs.go b/nmodbus/handler/fn_discreteinputs.go new file mode 100644 index 0000000..66dbd27 --- /dev/null +++ b/nmodbus/handler/fn_discreteinputs.go @@ -0,0 +1,48 @@ +package handler + +import ( + "git.noahlan.cn/noahlan/ntool-biz/nmodbus/protocol" + "git.noahlan.cn/noahlan/ntool-biz/nmodbus/serialize" +) + +type ReadDiscreteInputs struct { + fnCode uint8 +} + +func NewReadDiscreteInputs(fnCode uint8) FunctionHandler { + return &ReadDiscreteInputs{fnCode: fnCode} +} + +func (r *ReadDiscreteInputs) FnCode() uint8 { + return r.fnCode +} + +// Function 读离散输入 功能码 0x04 +func (r *ReadDiscreteInputs) Function(s *Handler, req protocol.Packet, resp protocol.Packet) *protocol.MError { + register, numRegs, endRegister := serialize.RegisterAddressAndNumber(req.GetBody()) + if endRegister > 65535 { + resp.SetBody([]byte{}) + return &protocol.IllegalDataAddress + } + dataSize := numRegs / 8 + if (numRegs % 8) != 0 { + dataSize++ + } + data := make([]byte, 1+dataSize) + data[0] = byte(dataSize) + + dataMgr := s.DataManager(req.GetAddress()) + discreteInputs := dataMgr.ReadDiscreteInputs(register, endRegister) + + for i, value := range discreteInputs { + if value != 0 { + shift := uint(i) % 8 + data[1+i/8] |= byte(1 << shift) + } + } + + s.HandleLogic(r.fnCode, register, numRegs, data) + + resp.SetBody(data) + return &protocol.Success +} diff --git a/nmodbus/handler/fn_holdingregisters.go b/nmodbus/handler/fn_holdingregisters.go new file mode 100644 index 0000000..390e163 --- /dev/null +++ b/nmodbus/handler/fn_holdingregisters.go @@ -0,0 +1,110 @@ +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" +) + +type ReadHoldingRegisters struct { + fnCode uint8 +} + +func NewReadHoldingRegisters(fnCode uint8) FunctionHandler { + return &ReadHoldingRegisters{fnCode: fnCode} +} + +func (r *ReadHoldingRegisters) FnCode() uint8 { + return r.fnCode +} + +// Function 读保持寄存器（AI） 功能码0x03 +func (r *ReadHoldingRegisters) Function(s *Handler, req protocol.Packet, resp protocol.Packet) *protocol.MError { + register, numRegs, endRegister := serialize.RegisterAddressAndNumber(req.GetBody()) + if endRegister > 65536 { + resp.SetBody([]byte{}) + return &protocol.IllegalDataAddress + } + dataMgr := s.DataManager(req.GetAddress()) + data := dataMgr.ReadHoldingRegisters(register, endRegister) + + resp.SetBody(append([]byte{byte(numRegs * 2)}, util.Uint16ToBytes(s.byteOrder, data)...)) + + s.HandleLogic(r.fnCode, register, numRegs, data) + + return &protocol.Success +} + +////////////////////////////////////////////////////// WriteHoldingRegister + +type WriteHoldingRegister struct { + fnCode uint8 +} + +func NewWriteHoldingRegister(fnCode uint8) FunctionHandler { + return &WriteHoldingRegister{fnCode: fnCode} +} + +func (r *WriteHoldingRegister) FnCode() uint8 { + return r.fnCode +} + +// Function 写单个保持寄存器 功能码0x06 +func (r *WriteHoldingRegister) Function(s *Handler, req protocol.Packet, resp protocol.Packet) *protocol.MError { + data := req.GetBody() + register, value := serialize.RegisterAddressAndValue(data) + + dataMgr := s.DataManager(req.GetAddress()) + dataMgr.WriteSingleHoldingRegister(register, value) + + resp.SetBody(data[0:4]) + + s.HandleLogic(r.fnCode, register, 1, value) + + return &protocol.Success +} + +////////////////////////////////////////////////////// WriteHoldingRegisters + +type WriteHoldingRegisters struct { + fnCode uint8 +} + +func NewWriteHoldingRegisters(fnCode uint8) FunctionHandler { + return &WriteHoldingRegisters{fnCode: fnCode} +} + +func (r *WriteHoldingRegisters) FnCode() uint8 { + return r.fnCode +} + +// Function 写入 多个保持寄存器 功能码 0x10=16 +func (r *WriteHoldingRegisters) Function(s *Handler, req protocol.Packet, resp protocol.Packet) *protocol.MError { + pkgData := req.GetBody() + register, numRegs, _ := serialize.RegisterAddressAndNumber(req.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) + dataMgr := s.DataManager(req.GetAddress()) + + valuesUpdated := dataMgr.WriteHoldingRegisters(values, register) + if valuesUpdated == numRegs { + err = &protocol.Success + data = pkgData[0:4] + } else { + err = &protocol.IllegalDataAddress + } + + resp.SetBody(data) + + s.HandleLogic(r.fnCode, register, numRegs, values) + + return err +} diff --git a/nmodbus/handler/fn_inputregisters.go b/nmodbus/handler/fn_inputregisters.go new file mode 100644 index 0000000..b1bcd9e --- /dev/null +++ b/nmodbus/handler/fn_inputregisters.go @@ -0,0 +1,35 @@ +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" +) + +type ReadInputRegisters struct { + fnCode uint8 +} + +func NewReadInputRegisters(fnCode uint8) FunctionHandler { + return &ReadInputRegisters{fnCode: fnCode} +} + +func (r *ReadInputRegisters) FnCode() uint8 { + return r.fnCode +} + +// Function 读输入寄存器（AO） 功能码0x04 +func (r *ReadInputRegisters) Function(s *Handler, req protocol.Packet, resp protocol.Packet) *protocol.MError { + register, numRegs, endRegister := serialize.RegisterAddressAndNumber(req.GetBody()) + if endRegister > 65536 { + resp.SetBody([]byte{}) + return &protocol.IllegalDataAddress + } + dataMgr := s.DataManager(req.GetAddress()) + data := dataMgr.ReadInputRegisters(register, endRegister) + resp.SetBody(append([]byte{byte(numRegs * 2)}, util.Uint16ToBytes(s.byteOrder, data)...)) + + s.HandleLogic(r.fnCode, register, numRegs, data) + + return &protocol.Success +} diff --git a/nmodbus/handler/functions.go b/nmodbus/handler/functions.go deleted file mode 100644 index 144b5d7..0000000 --- a/nmodbus/handler/functions.go +++ /dev/null @@ -1,169 +0,0 @@ -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) - - dataMgr := s.DataManager(packet.GetAddress()) - coils := dataMgr.ReadCoils(register, endRegister) - - for i, value := range coils { - 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) - - dataMgr := s.DataManager(packet.GetAddress()) - discreteInputs := dataMgr.ReadDiscreteInputs(register, endRegister) - - for i, value := range discreteInputs { - 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 - } - dataMgr := s.DataManager(packet.GetAddress()) - return append([]byte{byte(numRegs * 2)}, util.Uint16ToBytes(s.byteOrder, dataMgr.ReadHoldingRegisters(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 - } - dataMgr := s.DataManager(packet.GetAddress()) - return append([]byte{byte(numRegs * 2)}, util.Uint16ToBytes(s.byteOrder, dataMgr.ReadInputRegisters(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 - } - dataMgr := s.DataManager(packet.GetAddress()) - dataMgr.WriteSingleCoil(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) - - dataMgr := s.DataManager(packet.GetAddress()) - dataMgr.WriteSingleHoldingRegister(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 - //} - - idxList := make([]int, 0) - values := make([]byte, 0) - bitCount := 0 - for i, value := range valueBytes { - for bitPos := uint(0); bitPos < 8; bitPos++ { - idxList = append(idxList, register+(i*8)+int(bitPos)) - values = append(values, util.BitAtPosition(value, bitPos)) - - bitCount++ - if bitCount >= numRegs { - break - } - } - if bitCount >= numRegs { - break - } - } - - dataMgr := s.DataManager(packet.GetAddress()) - dataMgr.WriteCoils(idxList, values) - - 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) - dataMgr := s.DataManager(packet.GetAddress()) - - valuesUpdated := dataMgr.WriteHoldingRegisters(values, register) - if valuesUpdated == numRegs { - err = &protocol.Success - data = pkgData[0:4] - } else { - err = &protocol.IllegalDataAddress - } - - return data, err -} diff --git a/nmodbus/handler/handler.go b/nmodbus/handler/handler.go index cc2b0e5..f6e7ca8 100644 --- a/nmodbus/handler/handler.go +++ b/nmodbus/handler/handler.go @@ -16,38 +16,50 @@ type ( Handler struct { byteOrder binary.ByteOrder // 字节序 - functions map[uint8]FunctionHandler + logics map[uint8]LogicFunc + handlerMap map[uint8]FunctionHandler dataMap map[uint8]*DataMgr mu sync.RWMutex } - FunctionHandler func(handler *Handler, pkg protocol.Packet) ([]byte, *protocol.MError) + FunctionHandler interface { + Function(handler *Handler, req protocol.Packet, resp protocol.Packet) *protocol.MError + FnCode() uint8 + } + + LogicFunc func(startAddr, number int, val any) ) func NewHandler(byteOrder binary.ByteOrder) *Handler { p := &Handler{ byteOrder: byteOrder, - functions: make(map[uint8]FunctionHandler), - dataMap: make(map[uint8]*DataMgr), - mu: sync.RWMutex{}, + //functions: make(map[uint8]FunctionFunc), + logics: make(map[uint8]LogicFunc), + handlerMap: make(map[uint8]FunctionHandler), + dataMap: make(map[uint8]*DataMgr), + mu: sync.RWMutex{}, } // 添加功能码对应功能 - p.functions[0x01] = ReadCoils - p.functions[0x02] = ReadDiscreteInputs - p.functions[0x03] = ReadHoldingRegisters - p.functions[0x04] = ReadInputRegisters - p.functions[0x05] = WriteSingleCoil - p.functions[0x06] = WriteHoldingRegister - p.functions[0x0F] = WriteMultipleCoils - p.functions[0x10] = WriteHoldingRegisters + p.RegisterFunction(NewReadCoils(0x01)) + p.RegisterFunction(NewReadDiscreteInputs(0x02)) + p.RegisterFunction(NewReadHoldingRegisters(0x03)) + p.RegisterFunction(NewReadInputRegisters(0x04)) + p.RegisterFunction(NewWriteSingleCoil(0x05)) + p.RegisterFunction(NewWriteHoldingRegister(0x06)) + p.RegisterFunction(NewWriteMultipleCoils(0x0F)) + p.RegisterFunction(NewWriteHoldingRegisters(0x10)) return p } -func (p *Handler) RegisterFunction(fnCode uint8, fn FunctionHandler) { - p.functions[fnCode] = fn +func (p *Handler) RegisterFunction(fn FunctionHandler) { + p.handlerMap[fn.FnCode()] = fn +} + +func (p *Handler) RegisterLogic(fnCode uint8, fn LogicFunc) { + p.logics[fnCode] = fn } func (p *Handler) DataManager(address uint8) *DataMgr { @@ -63,22 +75,18 @@ func (p *Handler) DataManager(address uint8) *DataMgr { } func (p *Handler) Handle(conn *connection.Connection, pkg packet.IPacket) { - pp, ok := pkg.(protocol.Packet) + req, ok := pkg.(protocol.Packet) if !ok { nlog.Error(packet.ErrWrongPacketType) } - var ( - err *protocol.MError - data []byte - ) + var err *protocol.MError - resp := pp.Copy() + resp := req.Copy() - fnCode := pp.GetFunction() - fn, ok := p.functions[fnCode] + fnCode := req.GetFunction() + handler, ok := p.handlerMap[fnCode] if ok { - data, err = fn(p, pp) - resp.SetBody(data) + err = handler.Function(p, req, resp) } else { err = &protocol.IllegalFunction } @@ -88,3 +96,11 @@ func (p *Handler) Handle(conn *connection.Connection, pkg packet.IPacket) { } _ = conn.Send(resp.GetHeader(), resp.GetBody()) } + +func (p *Handler) HandleLogic(fnCode uint8, startAddr, number int, val any) { + go func() { + if fn, ok := p.logics[fnCode]; ok { + fn(startAddr, number, val) + } + }() +}