package uk.co.wingpath.modbus; import java.io.*; import java.util.*; import uk.co.wingpath.util.*; /** * This class implements the {@link ModbusTransactionHandler} interface by * interrogating/updating a {@link ModbusModel}. */ public class ModbusSlave implements ModbusTransactionHandler { private final ModbusModel model; private ModbusFilePacker filePacker; private final ModbusCounters counters; private final ModbusForwarder nullForwarder; private ModbusForwarder forwarder; private int slaveId; private final boolean isMonitor; private int maxPdu = Modbus.MAX_IMP_PDU_SIZE; private boolean checkCountLimits = false; private boolean strictChecking = false; private boolean allowLongMessages = false; private DeviceId deviceId = null; private byte [] cmd17Data; /** * Constructs a {@code ModbusSlave} using the supplied {@link ModbusModel}, * slave ID, and {@link ModbusForwarder}. * @param model the Modbus model. * @param slaveId the slave ID. * @param isMonitor whether the slave is being used for monitoring. * If {@code true}, writes are allowed to registers that * are not flagged as writable, and special commands that write to * input registers and discrete inputs are supported. */ public ModbusSlave (ModbusModel model, int slaveId, boolean isMonitor) { this.model = model; this.slaveId = slaveId; this.isMonitor = isMonitor; filePacker = null; counters = new ModbusCounters (); try { String str = "Wingpath"; cmd17Data = str.getBytes ("UTF-8"); } catch (UnsupportedEncodingException e) { cmd17Data = new byte [0]; } nullForwarder = new ModbusForwarder () { @Override public void requires (int address, int nvalues, long requestTime, ModbusWaiter waiter) { waiter.inform (null); } @Override public void updated (int address, int nvalues, ModbusWaiter waiter) { waiter.inform (null); } }; forwarder = nullForwarder; } /** * Constructs a {@code ModbusSlave} using the supplied {@link ModbusModel} * and slave ID. * @param model the Modbus model. * @param slaveId the slave ID. */ public ModbusSlave (ModbusModel model, int slaveId) { this (model, slaveId, false); } /** * Sets the forwarder to be used to keep any cached values in sync with * the device. * @param forwarder the forwarder. May be null to disable forwarding. */ public void setForwarder (ModbusForwarder forwarder) { if (forwarder == null) forwarder = nullForwarder; this.forwarder = forwarder; } /** * Sets the Modbus file packer to be used for commands 20 & 21. * @param filePacker the Modbus file packer. */ public void setModbusFilePacker (ModbusFilePacker filePacker) { this.filePacker = filePacker; } /** * Gets the diagnostic counters. * @return the diagnostic counters. */ public ModbusCounters getCounters () { return counters; } /** * Sets the device identification. * Default value is {@code null}; * @param deviceId device identification. */ public void setDeviceId (DeviceId deviceId) { this.deviceId = deviceId; } /** * Sets the data for command 17 - Report Slave ID. * Default value is "Wingpath" in UTF-8. * @param data data for command 17. */ public void setCmd17Data (Numeric.Value [] data) { cmd17Data = Numeric.toByteArray (data); } /** * Sets the data for command 17 - Report Slave ID. * Default value is "Wingpath" in UTF-8. * @param str data for command 17. */ public void setCmd17Data (String str) { try { cmd17Data = str.getBytes ("UTF-8"); } catch (UnsupportedEncodingException e) { } } /** * Returns the Modbus model. * @return the model. */ public ModbusModel getModel () { return model; } /** Sets the slave ID. * @param slaveId the new slave ID. */ public void setSlaveId (int slaveId) { this.slaveId = slaveId; } /** * Gets the slave ID. * @return the slave ID. */ public int getSlaveId () { return slaveId; } /** * Sets the maximum PDU size. * Default value is 253 bytes. * @param maxPdu maximum PDU size in bytes. */ public void setMaxPdu (int maxPdu) { this.maxPdu = maxPdu; } /** * Sets the "check count limit" state. * Default value is {@code false}. * @param checkCountLimits value for "check count limits" state. */ public void setCheckCountLimits (boolean checkCountLimits) { this.checkCountLimits = checkCountLimits; } /** * Sets the "strict checking" state. * Default value is {@code false}. * If set to {@code true}, various checks will be done to ensure that * received messages strictly conform to the Modbus standard. * @param strictChecking value for "strict checking" state. */ public void setStrictChecking (boolean strictChecking) { this.strictChecking = strictChecking; } /** * Sets the "allow long messages" state. * Default value is {@code false}. * If set to {@code true}, messages with more than 255 bytes of data * will be allowed: * The byte count in received messages will be ignored, and the byte * count in sent messages will be set to 0 if more than 255 bytes of data * are being sent. * @param allowLongMessages value for "allow long messages" state. */ public void setAllowLongMessages (boolean allowLongMessages) { this.allowLongMessages = allowLongMessages; } /** * Checks whether the supplied {@code ModbusMessage} contains the * correct slave ID for this slave. * @param request the Modbus request. * @throws ModbusException if the slave ID is incorrect. */ private void checkSlaveId (ModbusMessage request) throws ModbusException { int id = request.getSlaveId (); if (id != slaveId && slaveId != 0) { throw new ModbusException (Modbus.ERROR_PATH_UNAVAILABLE, "S001", "Wrong slave ID: " + id + " instead of " + slaveId); } } /** * Checks the request size, and gets the number of data bytes in the * request. * If long messages are allowed, the supplied byte count is ignored and * the number of data bytes is calculated as the request length minus * the supplied overhead. * If long messages are not allowed, the supplied byte count is returned. * If strict checking is enabled, the request length is checked to be * exactly the supplied overhead plus the supplied byte count. * If strict checking is disabled and long messages are not allowed, * the request length is checked to be at least the supplied overhead plus * the supplied byte count. * @param request the request message. * @param overhead the number of bytes used in the request for fields * other than data. * @param byteCount the data byte count extracted from the message, or 0 if * the request does not contain a byte count. * @return the number of data bytes in the request. */ private int checkSize (ModbusMessage request, int overhead, int byteCount) throws ModbusException { if (allowLongMessages) { request.checkMinSize (overhead); return request.size () - overhead; } if (strictChecking) request.checkSize (overhead + byteCount); else request.checkMinSize (overhead + byteCount); return byteCount; } private void setException (ModbusTransaction trans, ModbusException e) { counters.incCommExceptionCount (); counters.addEvent (Modbus.COMM_EVENT_SND_EXCEPTION); trans.setException (e); } private void setResponse (ModbusTransaction trans, byte [] response, Tracer.Tracing tracing) { counters.addEvent (Modbus.COMM_EVENT_SND_OK); trans.setResponse (response, tracing); } private void functionNotSupported (ModbusTransaction trans, int func) { setException (trans, new ModbusException (Modbus.ERROR_ILLEGAL_FUNCTION, "S003", "Function " + func + " not supported")); } /** * Handles a Modbus transaction. *

The action appropriate to the request function code is taken. * If it is a read request, the response is constructed using values * from the Modbus model. * If it is a write request, the values are extracted from the request * and stored in the Modbus model. * If it is a diagnostic request, it is handled by this method. * @param trans the transaction whose request is to be handled. */ public void handleTransaction (ModbusTransaction trans) throws InterruptedException { trans.setActive (); ModbusMessage request = trans.getRequest (); int id = request.getSlaveId (); if (id != 0) { try { checkSlaveId (request); } catch (ModbusException e) { setException (trans, e); return; } } counters.incMessageCount (); if (id == 0) counters.incNoResponseCount (); int func = request.getFunctionCode (); switch (func) { case Modbus.FUNC_READ_HOLDING_REGISTERS: handleReadHoldingRegisters (trans); break; case Modbus.FUNC_READ_INPUT_REGISTERS: handleReadInputRegisters (trans); break; case Modbus.FUNC_WRITE_MULTIPLE_REGISTERS: handleWriteMultipleRegisters (trans); break; case Modbus.FUNC_WRITE_SINGLE_REGISTER: handleWriteSingleRegister (trans); break; case Modbus.FUNC_MASK_WRITE_REGISTER: handleMaskWriteRegister (trans); break; case Modbus.FUNC_READ_COILS: handleReadCoils (trans); break; case Modbus.FUNC_READ_DISCRETE_INPUTS: handleReadDiscreteInputs (trans); break; case Modbus.FUNC_WRITE_MULTIPLE_COILS: handleWriteCoils (trans); break; case Modbus.FUNC_WRITE_SINGLE_COIL: handleWriteCoil (trans); break; case Modbus.FUNC_READ_WRITE_MULTIPLE_REGISTERS: handleReadWriteRegisters (trans); break; case Modbus.FUNC_READ_EXCEPTION_STATUS: handleReadExceptionStatus (trans); break; case Modbus.FUNC_DIAGNOSTICS: handleDiagnostics (trans); break; case Modbus.FUNC_GET_COMM_EVENT_COUNTER: handleGetCommEventCounter (trans); counters.decCommEventCount (); break; case Modbus.FUNC_GET_COMM_EVENT_LOG: handleGetCommEventLog (trans); break; case Modbus.FUNC_REPORT_SLAVE_ID: handleReportSlaveID (trans); break; case Modbus.FUNC_ENCAPSULATED_INTERFACE_TRANSPORT: handleEncapsulatedInterfaceTransport (trans); break; case Modbus.FUNC_READ_FILE_RECORD: handleReadFileRecord (trans); break; case Modbus.FUNC_WRITE_FILE_RECORD: handleWriteFileRecord (trans); break; case Modbus.FUNC_READ_FIFO_QUEUE: handleReadFifoQueue (trans); break; default: if (isMonitor) { switch (func) { case Modbus.FUNC_MONITOR_INPUT_REGISTERS: handleMonitorInputRegisters (trans); break; case Modbus.FUNC_MONITOR_DISCRETE_INPUTS: handleMonitorDiscreteInputs (trans); break; case Modbus.FUNC_MONITOR_HOLDING_REGISTERS: handleMonitorHoldingRegisters (trans); break; case Modbus.FUNC_MONITOR_COILS: handleMonitorCoils (trans); break; case Modbus.FUNC_MONITOR_FILE_RECORDS: handleMonitorFileRecord (trans); break; default: functionNotSupported (trans, func); return; } } else { functionNotSupported (trans, func); return; } break; } counters.incCommEventCount (); // If no response handler has been provided, wait for the // transaction to finish. if (!trans.hasResponseHandler ()) trans.waitUntilFinished (); } private void handleReadHoldingRegisters (final ModbusTransaction trans) throws InterruptedException { try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); checkSize (request, 4, 0); int address = request.getInt (0); final int count = request.getInt (2); if (strictChecking && count == 0) Modbus.dataError ("S008", "Invalid count (0)"); final AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap (); final int addr = model.fromHoldingRegister (address); final int nvalues = model.getNumValues (addr, count, space); forwarder.requires (addr, nvalues, System.nanoTime (), new ModbusWaiter () { public void inform (ModbusException e) { if (e != null) { setException (trans, e); return; } try { Tracer.Tracing tracing = new Tracer.Tracing (); byte [] data = model.pack (addr, nvalues, space, tracing, slaveId, false); if (!allowLongMessages && data.length > 255) { Modbus.dataError ("S009", "Invalid count (" + count + ") - response byte count (" + data.length + ") would not fit in a byte"); } if (data.length + 2 > maxPdu) { Modbus.dataError ("S010", "Invalid count (" + count + ") - response would exceed max PDU size (" + maxPdu + " bytes)"); } if (checkCountLimits && data.length > 125 * 2) { Modbus.dataError ("S011", "Invalid count (" + count + ") - response would exceed count limit" + " (125 * 2 bytes)"); } MessageBuilder body = new MessageBuilder (); body.addByte (data.length > 255 ? 0 : data.length); body.addData (data); setResponse (trans, body.getData (), tracing); } catch (ModbusException ex) { setException (trans, ex); } } }); } catch (ModbusException e) { setException (trans, e); } } private void handleReadInputRegisters (final ModbusTransaction trans) throws InterruptedException { try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); checkSize (request, 4, 0); int address = request.getInt (0); final int count = request.getInt (2); if (strictChecking && count == 0) Modbus.dataError ("S008", "Invalid count (0)"); final AddressMap.Map space = model.getAddressMap ().getInputRegisterMap (); final int addr = model.fromInputRegister (address); final int nvalues = model.getNumValues (addr, count, space); forwarder.requires (addr, nvalues, System.nanoTime (), new ModbusWaiter () { public void inform (ModbusException e) { if (e != null) { setException (trans, e); return; } try { Tracer.Tracing tracing = new Tracer.Tracing (); byte [] data = model.pack (addr, nvalues, space, tracing, slaveId, false); if (!allowLongMessages && data.length > 255) { Modbus.dataError ("S009", "Invalid count (" + count + ") - response byte count (" + data.length + ") would not fit in a byte"); } if (data.length + 2 > maxPdu) { Modbus.dataError ("S010", "Invalid count (" + count + ") - response would exceed max PDU size (" + maxPdu + " bytes)"); } if (checkCountLimits && data.length > 125 * 2) { Modbus.dataError ("S011", "Invalid count (" + count + ") - response would exceed count limit" + " (125 * 2 bytes)"); } MessageBuilder body = new MessageBuilder (); body.addByte (data.length > 255 ? 0 : data.length); body.addData (data); setResponse (trans, body.getData (), tracing); } catch (ModbusException ex) { setException (trans, ex); } } }); } catch (ModbusException e) { setException (trans, e); } } private void handleWriteMultipleRegisters (final ModbusTransaction trans) throws InterruptedException { try { ModbusMessage request = trans.getRequest (); request.checkMinSize (5); final int address = request.getInt (0); final int count = request.getInt (2); if (strictChecking && count == 0) Modbus.dataError ("S008", "Invalid count (0)"); int size = checkSize (request, 5, request.getByte (4)); byte [] data = request.getData (5, size); AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap (); int addr = model.fromHoldingRegister (address); int nvalues = model.getNumValues (addr, count, space); if (strictChecking && model.getTotalBytes (addr, nvalues, space) != size) { Modbus.dataError ("S012", "Count (" + count + ") does not match number of data bytes (" + size + ")"); } if (checkCountLimits && size > 123 * 2) { Modbus.dataError ("S013", "Number of data bytes (" + size + ") exceeds count limit (123 * 2 bytes)"); } if (!isMonitor) model.checkWritable (addr, nvalues, space); model.unpack (data, addr, nvalues, space, strictChecking, slaveId, true, request.getTracer ()); forwarder.updated (addr, nvalues, new ModbusWaiter () { public void inform (ModbusException e) { if (e != null) { setException (trans, e); return; } MessageBuilder body = new MessageBuilder (); body.addInt (address); body.addInt (count); setResponse (trans, body.getData (), null); } }); } catch (ModbusException e) { setException (trans, e); } } private void handleWriteSingleRegister (final ModbusTransaction trans) throws InterruptedException { try { ModbusMessage request = trans.getRequest (); request.checkMinSize (3); final int address = request.getInt (0); final byte [] data = request.getData (2, request.size () - 2); AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap (); int addr = model.fromHoldingRegister (address); if (!isMonitor) model.checkWritable (addr, 1, space); model.unpack (data, addr, 1, space, strictChecking, slaveId, true, request.getTracer ()); forwarder.updated (addr, 1, new ModbusWaiter () { public void inform (ModbusException e) { if (e != null) { setException (trans, e); return; } MessageBuilder body = new MessageBuilder (); body.addInt (address); body.addData (data); setResponse (trans, body.getData (), null); } }); } catch (ModbusException e) { setException (trans, e); } } private void handleMaskWriteRegister (ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); request.checkMinSize (4); final int address = request.getInt (0); AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap (); int addr = model.fromHoldingRegister (address); if (!isMonitor) model.checkWritable (addr, 1, space); if (model.isFloat (addr)) { Modbus.addressError ("S005", "Can't mask float value (register " + addr + ")"); } int size = model.getValueSize (addr); if (size == 0) { Modbus.addressError ("S006", "Can't mask discrete value (register " + addr + ")"); } checkSize (request, 2 + size * 2, 0); byte [] data = request.getData (2, size * 2); long andMask = model.unpackLongValue (size, data, 0, 0); long orMask = model.unpackLongValue (size, data, size, 0); model.setValue (addr, (model.getLongValue (addr) & andMask) | (orMask & ~andMask)); MessageBuilder body = new MessageBuilder (); body.addInt (address); body.addData (data); setResponse (trans, body.getData (), null); } catch (ModbusException e) { setException (trans, e); } } private void handleReadCoils (final ModbusTransaction trans) throws InterruptedException { try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); checkSize (request, 4, 0); int address = request.getInt (0); final int nbits = request.getInt (2); if (strictChecking && nbits == 0) Modbus.dataError ("S008", "Invalid count (0)"); final int nbytes = (nbits + 7) / 8; if (!allowLongMessages && nbytes > 255) { Modbus.dataError ("S009", "Invalid count (" + nbits + ") - response byte count (" + nbytes + ") would not fit in a byte"); } if (nbytes + 2 > maxPdu) { Modbus.dataError ("S010", "Invalid count (" + nbits + ") - response would exceed max PDU size (" + maxPdu + " bytes)"); } if (checkCountLimits && nbits > 2000) { Modbus.dataError ("S014", "Count (" + nbits + ") exceeds count limit (2000)"); } final AddressMap.Map space = model.getAddressMap ().getCoilMap (); space.checkInRange (address, nbits); int valueSize = model.getCoilValueSize (); ModelAddress ma = model.fromCoil (address); final int addr = ma.getAddress (); final int shift = ma.getShift (); int multiplier = valueSize == 0 ? 1 : valueSize * 8; final int nvalues = (nbits + shift + multiplier - 1) / multiplier; forwarder.requires (addr, nvalues, System.nanoTime (), new ModbusWaiter () { public void inform (ModbusException e) { if (e != null) { setException (trans, e); return; } try { Tracer.Tracing tracing = new Tracer.Tracing (); byte [] shdata = model.pack (addr, nvalues, space, tracing, slaveId, false); if (model.bitReverseCoils ()) Bytes.reverseBits (shdata); if (model.byteSwapCoils ()) Bytes.byteSwap (shdata); byte [] data = new byte [nbytes]; shiftRight (data, shdata, shift); byte [] mask = new byte [nbytes]; setMask (mask, nbits); Bytes.and (data, mask); MessageBuilder body = new MessageBuilder (); body.addByte (nbytes > 255 ? 0 : nbytes); body.addData (data); setResponse (trans, body.getData (), tracing); } catch (ModbusException ex) { setException (trans, ex); } } }); } catch (ModbusException e) { setException (trans, e); } } private void handleReadDiscreteInputs (final ModbusTransaction trans) throws InterruptedException { try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); checkSize (request, 4, 0); int address = request.getInt (0); final int nbits = request.getInt (2); if (strictChecking && nbits == 0) Modbus.dataError ("S008", "Invalid count (0)"); final int nbytes = (nbits + 7) / 8; if (!allowLongMessages && nbytes > 255) { Modbus.dataError ("S009", "Invalid count (" + nbits + ") - response byte count (" + nbytes + ") would not fit in a byte"); } if (nbytes + 2 > maxPdu) { Modbus.dataError ("S010", "Invalid count (" + nbits + ") - response would exceed max PDU size (" + maxPdu + " bytes)"); } if (checkCountLimits && nbits > 2000) { Modbus.dataError ("S014", "Count (" + nbits + ") exceeds count limit (2000)"); } final AddressMap.Map space = model.getAddressMap ().getDiscreteInputMap (); space.checkInRange (address, nbits); int valueSize = model.getDiscreteInputValueSize (); ModelAddress ma = model.fromDiscreteInput (address); final int addr = ma.getAddress (); final int shift = ma.getShift (); int multiplier = valueSize == 0 ? 1 : valueSize * 8; final int nvalues = (nbits + shift + multiplier - 1) / multiplier; forwarder.requires (addr, nvalues, System.nanoTime (), new ModbusWaiter () { public void inform (ModbusException e) { if (e != null) { setException (trans, e); return; } try { Tracer.Tracing tracing = new Tracer.Tracing (); byte [] shdata = model.pack (addr, nvalues, space, tracing, slaveId, false); if (model.bitReverseDiscreteInputs ()) Bytes.reverseBits (shdata); if (model.byteSwapDiscreteInputs ()) Bytes.byteSwap (shdata); byte [] data = new byte [nbytes]; shiftRight (data, shdata, shift); byte [] mask = new byte [nbytes]; setMask (mask, nbits); Bytes.and (data, mask); MessageBuilder body = new MessageBuilder (); body.addByte (nbytes > 255 ? 0 : nbytes); body.addData (data); setResponse (trans, body.getData (), tracing); } catch (ModbusException ex) { setException (trans, ex); } } }); } catch (ModbusException e) { setException (trans, e); } } private void handleWriteCoils (final ModbusTransaction trans) throws InterruptedException { try { ModbusMessage request = trans.getRequest (); request.checkMinSize (5); final int address = request.getInt (0); final int nbits = request.getInt (2); if (strictChecking && nbits == 0) Modbus.dataError ("S008", "Invalid count (0)"); final int nbytes = checkSize (request, 5, (nbits + 7) / 8); if (strictChecking && nbytes != request.getByte (4)) { Modbus.dataError ("S015", "Number of coils (" + nbits + ") does not match byte count (" + nbytes + ")"); } if (checkCountLimits && nbits > 1968) { Modbus.dataError ("S014", "Count (" + nbits + ") exceeds count limit (1968)"); } byte [] data = request.getData (5, nbytes); final AddressMap.Map space = model.getAddressMap ().getCoilMap (); space.checkInRange (address, nbits); int valueSize = model.getCoilValueSize (); if (valueSize != 0 && forwarder != nullForwarder) { throw new IllegalStateException ( "Can only forward write-coils for 1-bit registers"); } ModelAddress ma = model.fromCoil (address); final int addr = ma.getAddress (); int shift = ma.getShift (); int multiplier = valueSize == 0 ? 1 : valueSize * 8; final int nvalues = (nbits + shift + multiplier - 1) / multiplier; if (!isMonitor) model.checkWritable (addr, nvalues, space); final int size = valueSize == 0 ? (nvalues + 7) / 8 : nvalues * valueSize; final byte [] newData = new byte [size]; shiftLeft (newData, data, shift); byte [] mask = new byte [nbytes]; setMask (mask, nbits); final byte [] shmask = new byte [size]; shiftLeft (shmask, mask, shift); if (model.bitReverseCoils ()) { Bytes.reverseBits (newData); Bytes.reverseBits (shmask); } if (model.byteSwapCoils ()) { Bytes.byteSwap (newData); Bytes.byteSwap (shmask); } byte [] oldData = model.pack (addr, nvalues, space, null, slaveId, true); assert oldData.length == size; Bytes.andNot (oldData, shmask); Bytes.or (newData, oldData); model.unpack (newData, addr, nvalues, space, strictChecking, slaveId, true, request.getTracer ()); forwarder.updated (addr, nvalues, new ModbusWaiter () { public void inform (ModbusException e) { if (e != null) { setException (trans, e); return; } MessageBuilder body = new MessageBuilder (); body.addInt (address); body.addInt (nbits); setResponse (trans, body.getData (), null); } }); } catch (ModbusException e) { setException (trans, e); } } private void handleWriteCoil (final ModbusTransaction trans) throws InterruptedException { try { ModbusMessage request = trans.getRequest (); checkSize (request, 4, 0); final int address = request.getInt (0); final int value = request.getInt (2); if (strictChecking && value != 0 && value != 0xff00) Modbus.dataError ("S016", "Value should be 0000 or FF00"); final boolean on = value != 0; final AddressMap.Map space = model.getAddressMap ().getCoilMap (); space.checkInRange (address, 1); int valueSize = model.getCoilValueSize (); if (valueSize != 0 && forwarder != nullForwarder) { throw new IllegalStateException ( "Can only forward write-coil for 1-bit register"); } ModelAddress ma = model.fromCoil (address); final int addr = ma.getAddress (); int shift = ma.getShift (); if (!isMonitor) model.checkWritable (addr, 1, space); byte [] mask = new byte [1]; mask [0] = 1; final int size = valueSize == 0 ? 1 : valueSize; final byte [] shmask = new byte [size]; shiftLeft (shmask, mask, shift); if (model.bitReverseCoils ()) Bytes.reverseBits (shmask); if (model.byteSwapCoils ()) Bytes.byteSwap (shmask); byte [] data = model.pack (addr, 1, space, null, slaveId, true); assert data.length == size; if (on) Bytes.or (data, shmask); else Bytes.andNot (data, shmask); model.unpack (data, addr, 1, space, strictChecking, slaveId, true, request.getTracer ()); forwarder.updated (addr, 1, new ModbusWaiter () { public void inform (ModbusException e) { if (e != null) { setException (trans, e); return; } MessageBuilder body = new MessageBuilder (); body.addInt (address); body.addInt (value); setResponse (trans, body.getData (), null); } }); } catch (ModbusException e) { setException (trans, e); } } private void handleReadWriteRegisters (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); request.checkMinSize (9); int readAddress = request.getInt (0); int readCount = request.getInt (2); if (strictChecking && readCount == 0) Modbus.dataError ("S017", "Invalid read count (0)"); int writeAddress = request.getInt (4); int writeCount = request.getInt (6); if (strictChecking && writeCount == 0) Modbus.dataError ("S018", "Invalid write count (0)"); int size = checkSize (request, 9, request.getByte (8)); byte [] writeData = request.getData (9, size); // Check the read for errors. AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap (); int readAddr = model.fromHoldingRegister (readAddress); int len = model.getTotalBytes (readAddr, model.getNumValues (readAddr, readCount, space), space); if (!allowLongMessages && len > 255) { Modbus.dataError ("S019", "Invalid read count (" + readCount + ") - response byte count (" + len + ") would not fit in a byte"); } if (len + 2 > maxPdu) { Modbus.dataError ("S020", "Invalid read count (" + readCount + ") - response would exceed max PDU size (" + maxPdu + " bytes)"); } if (checkCountLimits && len > 125 * 2) { Modbus.dataError ("S021", "Invalid read count (" + readCount + ") - response would exceed count limit (125 * 2 bytes)"); } // Do the write (latest spec says it must be done before read). int writeAddr = model.fromHoldingRegister (writeAddress); int nvalues = model.getNumValues (writeAddr, writeCount, space); if (strictChecking && model.getTotalBytes (writeAddr, nvalues, space) != size) { Modbus.dataError ("S022", "Write count (" + writeCount + ") does not match number of data bytes (" + size + ")"); } if (checkCountLimits && size > 121 * 2) { Modbus.dataError ("S023", "Number of bytes to write (" + size + ") exceeds count limit (121 * 2 bytes)"); } if (!isMonitor) model.checkWritable (writeAddr, nvalues, space); model.unpack (writeData, writeAddr, nvalues, space, strictChecking, slaveId, true, request.getTracer ()); // Do the read, unless this is a monitor-mode slave, in which case // fabricate the read data. It wouldn't do any harm to do the read // in monitor mode, but it isn't needed and it would show // in the register logging. Tracer.Tracing tracing = new Tracer.Tracing (); byte [] readData = isMonitor ? new byte [0] : model.pack (readAddr, model.getNumValues (readAddr, readCount, space), space, tracing, slaveId, false); // Construct the response. MessageBuilder body = new MessageBuilder (); body.addByte (readData.length > 255 ? 0 : readData.length); body.addData (readData); setResponse (trans, body.getData (), tracing); } catch (ModbusException e) { setException (trans, e); } } private void handleReadExceptionStatus (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); checkSize (request, 0, 0); int addr = model.getExceptionStatusRegister (); if (addr < 0) Modbus.addressError ("S025", "No exception status register"); AddressMap.Map space = model.getAddressMap ().getAddressSpace (addr); if (space == null) Modbus.addressError ("S025", "No exception status register"); MessageBuilder body = new MessageBuilder (); int size; try { size = model.getValueSize (addr); } catch (ModbusException e) { Modbus.addressError ("S025", "No exception status register"); throw new AssertionError ("Unreachable"); } int nvalues = size == 0 ? 8 : 1; Tracer.Tracing tracing = new Tracer.Tracing (); byte [] data = model.pack (addr, nvalues, space, tracing, slaveId, false); if (size == 0 && model.getBitReverse ()) Bytes.reverseBits (data); body.addByte (data [data.length - 1]); setResponse (trans, body.getData (), tracing); } catch (ModbusException e) { setException (trans, e); } } private void handleGetCommEventCounter (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); checkSize (request, 0, 0); MessageBuilder body = new MessageBuilder (); body.addInt (0); // status word. body.addInt (counters.getCommEventCount ()); setResponse (trans, body.getData (), null); } catch (ModbusException e) { setException (trans, e); } } private void handleGetCommEventLog (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); checkSize (request, 0, 0); MessageBuilder body = new MessageBuilder (); byte [] events = counters.getCommEventLog (); body.addByte (6 + events.length); body.addInt (0); // status word. body.addInt (counters.getCommEventCount ()); body.addInt (counters.getBusMessageCount ()); body.addData (events); setResponse (trans, body.getData (), null); } catch (ModbusException e) { setException (trans, e); } } private void handleReportSlaveID (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); checkSize (request, 0, 0); MessageBuilder body = new MessageBuilder (); int len = cmd17Data.length; if (len > maxPdu - 2) len = maxPdu - 2; body.addByte (len); body.addData (cmd17Data, 0, len); setResponse (trans, body.getData (), null); } catch (ModbusException e) { setException (trans, e); } } private void handleDiagnostics (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); request.checkMinSize (2); int subfunc = request.getInt (0); if (subfunc == Modbus.DIAG_RETURN_QUERY_DATA) { setResponse (trans, request.getData (), null); return; } checkSize (request, 4, 0); int requestData = request.getInt (2); int replyData = requestData; Tracer.Tracing tracing = new Tracer.Tracing (); switch (subfunc) { case Modbus.DIAG_RETURN_BUS_MESSAGE_COUNT: replyData = counters.getBusMessageCount (); break; case Modbus.DIAG_RETURN_BUS_EXCEPTION_COUNT: replyData = counters.getCommExceptionCount (); break; case Modbus.DIAG_RETURN_SLAVE_MESSAGE_COUNT: replyData = counters.getMessageCount (); break; case Modbus.DIAG_RETURN_SLAVE_NO_RESPONSE_COUNT: replyData = counters.getNoResponseCount (); break; case Modbus.DIAG_RETURN_BUS_COMM_ERROR_COUNT: replyData = counters.getCommErrorCount (); break; case Modbus.DIAG_RETURN_SLAVE_NAK_COUNT: replyData = 0; break; case Modbus.DIAG_RETURN_SLAVE_BUSY_COUNT: replyData = 0; break; case Modbus.DIAG_RETURN_BUS_CHARACTER_OVERRUN_COUNT: case Modbus.DIAG_RETURN_OVERRUN_ERROR_COUNT: replyData = counters.getOverrunCount (); break; case Modbus.DIAG_CLEAR_OVERRUN_COUNTER: counters.clearOverrunCount (); break; case Modbus.DIAG_RESTART_COMM_OPTION: counters.reset (); if (requestData != 0) counters.clearCommEventLog (); counters.addEvent (Modbus.COMM_EVENT_RESTART); // Build the response here (rather than using the common code // after the switch statement) to avoid adding a "send" event // to the Comm Event Log. MessageBuilder body = new MessageBuilder (); body.addInt (subfunc); body.addInt (replyData); trans.setResponse (body.getData (), null); return; case Modbus.DIAG_CLEAR_COUNTERS: { counters.reset (); int addr = model.getDiagnosticRegister (); if (addr >= 0) { model.setValue (addr, 0); } } break; case Modbus.DIAG_RETURN_DIAGNOSTIC_REGISTER: { int addr = model.getDiagnosticRegister (); if (addr < 0) Modbus.addressError ("S026", "No diagnostic register"); AddressMap.Map space = model.getAddressMap ().getAddressSpace (addr); if (space == null) Modbus.addressError ("S026", "No diagnostic register"); int size; try { size = model.getValueSize (addr); } catch (ModbusException e) { Modbus.addressError ("S026", "No diagnostic register"); throw new AssertionError ("Unreachable"); } int nvalues = size == 0 ? 16 : size == 1 ? 2 : 1; byte [] data = model.pack (addr, nvalues, space, tracing, slaveId, false); if (size == 0 && model.getBitReverse ()) Bytes.reverseBits (data); if (size <= 1 && model.getByteSwap ()) Bytes.byteSwap (data); replyData = Bytes.toShort (data, data.length - 2); } break; default: throw new ModbusException (Modbus.ERROR_ILLEGAL_FUNCTION, "S004", "Unknown sub-function (" + subfunc + ")"); } MessageBuilder body = new MessageBuilder (); body.addInt (subfunc); body.addInt (replyData); setResponse (trans, body.getData (), tracing); } catch (ModbusException e) { setException (trans, e); } } private void handleEncapsulatedInterfaceTransport (final ModbusTransaction trans) throws InterruptedException { try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); request.checkMinSize (1); switch (request.getByte (0)) { case Modbus.MEI_READ_DEVICE_IDENTIFICATION: handleReadDeviceIdentification (trans); break; default: throw new ModbusException (Modbus.ERROR_ILLEGAL_FUNCTION, "S003", "Function " + Modbus.FUNC_ENCAPSULATED_INTERFACE_TRANSPORT + "/" + request.getByte (0) + " not supported"); } } catch (ModbusException e) { setException (trans, e); } } private void handleReadDeviceIdentification (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); if (deviceId == null) { throw new ModbusException (Modbus.ERROR_ILLEGAL_FUNCTION, "S003", "Function " + Modbus.FUNC_ENCAPSULATED_INTERFACE_TRANSPORT + "/" + Modbus.MEI_READ_DEVICE_IDENTIFICATION + " not supported"); } checkSize (request, 3, 0); int code = request.getByte (1); int objectId = request.getByte (2); if (objectId != 0) { if (deviceId.get (objectId) == null && code != 0x04) objectId = 0; } // Get list of object IDs to send. List ids; switch (code) { case 0x01: ids = deviceId.getBasicIds (objectId); break; case 0x02: ids = deviceId.getRegularIds (objectId); break; case 0x03: ids = deviceId.getExtendedIds (objectId); break; case 0x04: if (deviceId.get (objectId) == null) { Modbus.addressError ("S007", "Invalid object ID: " + String.format ("%02x", objectId)); } ids = new ArrayList (); ids.add (objectId); break; default: Modbus.dataError ("S024", "Invalid Read Device ID code: " + String.format ("%02x", code)); throw new AssertionError ("Unreachable"); } // See how many of the objects in the list will fit in the response // message. int size = 7; int nextId = 0; int count = 0; for (int id : ids) { byte [] data = deviceId.getBytes (id); int len = data.length > maxPdu - 9 ? maxPdu - 9 : data.length; if (size + 2 + len > maxPdu) { nextId = id; break; } count++; size += 2 + len; } // Construct the response message. MessageBuilder body = new MessageBuilder (); body.addByte (Modbus.MEI_READ_DEVICE_IDENTIFICATION); body.addByte (code); body.addByte (deviceId.computeConformityLevel ()); body.addByte (nextId == 0 ? 0 : 0xff); // More follows? body.addByte (nextId); // Next object Id body.addByte (count); // Number of objects int n = 0; for (int id : ids) { if (n >= count) break; body.addByte (id); byte [] data = deviceId.getBytes (id); int len = data.length > maxPdu - 9 ? maxPdu - 9 : data.length; body.addByte (len); body.addData (data, 0, len); n++; } setResponse (trans, body.getData (), null); } catch (ModbusException e) { setException (trans, e); } } private void handleReadFileRecord (final ModbusTransaction trans) throws InterruptedException { if (filePacker == null) { functionNotSupported (trans, Modbus.FUNC_READ_FILE_RECORD); return; } if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); request.checkMinSize (strictChecking ? 7 : 1); int size = checkSize (request, 1, request.getByte (0)); trans.getReporter ().debug ("size " + size); Tracer.Tracing tracing = new Tracer.Tracing (); MessageBuilder body = new MessageBuilder (); body.addByte (0); for (int offset = 1 ; offset < size ; ) { trans.getReporter ().debug ("offset " + offset); if (offset + 7 > size + 1) { if (strictChecking) { Modbus.dataError ("M005", "Excess data in message: " + (size + 1) + " bytes when expecting " + offset); } break; } int refType = request.getByte (offset); if (refType != 6) { Modbus.dataError ("S028", "Incorrect reference type: " + refType + " instead of 6"); } int fileNum = request.getInt (offset + 1); if (fileNum < 1) { Modbus.dataError ("S029", "Invalid file number: " + fileNum); } int address = request.getInt (offset + 3); if (strictChecking && address > 9999) { Modbus.dataError ("S030", "Invalid register address: " + address); } int count = request.getInt (offset + 5); if (strictChecking && count == 0) Modbus.dataError ("S008", "Invalid count (0)"); int nvalues = filePacker.getNumValues (fileNum, address, count); byte [] data = filePacker.pack (fileNum, address, nvalues, tracing, slaveId, false); body.addByte (data.length > 254 ? 0 : data.length + 1); body.addByte (6); body.addData (data); offset += 7; } byte [] data = body.getData (); data [0] = (byte) (data.length > 256 ? 0 : data.length - 1); if (!allowLongMessages && data.length > 256) { Modbus.dataError ("S009", "Invalid request - " + "response byte count (" + (data.length - 1) + ") would not fit in a byte"); } if (data.length + 1 > maxPdu) { Modbus.dataError ("S010", "Invalid request - " + "response would exceed max PDU size (" + maxPdu + " bytes)"); } setResponse (trans, data, tracing); } catch (ModbusException e) { setException (trans, e); } } private void handleWriteFileRecord (final ModbusTransaction trans) throws InterruptedException { if (filePacker == null) { functionNotSupported (trans, Modbus.FUNC_WRITE_FILE_RECORD); return; } if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); request.checkMinSize (strictChecking ? 10 : 1); int size = checkSize (request, 1, request.getByte (0)); // Check the request before writing anything. for (int offset = 1 ; offset <= size ; ) { int sizeLeft = (size + 1) - (offset + 7); if (sizeLeft < 0) { if (strictChecking) Modbus.dataError ("S027", "Excess data in message"); break; } int refType = request.getByte (offset); if (refType != 6) { Modbus.dataError ("S028", "Incorrect reference type: " + refType + " instead of 6"); } int fileNum = request.getInt (offset + 1); if (fileNum < 1) { Modbus.dataError ("S029", "Invalid file number: " + fileNum); } int address = request.getInt (offset + 3); if (strictChecking && address > 9999) { Modbus.dataError ("S030", "Invalid register address: " + address); } int count = request.getInt (offset + 5); if (strictChecking && count == 0) Modbus.dataError ("S008", "Invalid count (0)"); int nvalues = filePacker.getNumValues (fileNum, address, count); int nbytes = filePacker.getTotalBytes (fileNum, address, nvalues); if (nbytes > sizeLeft) { Modbus.dataError ("S031", "Insufficient data for file " + fileNum + " register " + address + " length " + count + ": " + sizeLeft + " bytes"); } if (!isMonitor) filePacker.checkWritable (fileNum, address, nvalues); offset += 7 + nbytes; } // Go through the request again, executing the writes. for (int offset = 1 ; offset <= size ; ) { int refType = request.getByte (offset); int fileNum = request.getInt (offset + 1); int address = request.getInt (offset + 3); int count = request.getInt (offset + 5); int nvalues = filePacker.getNumValues (fileNum, address, count); int nbytes = filePacker.getTotalBytes (fileNum, address, nvalues); byte [] data = request.getData (offset + 7, nbytes); filePacker.unpack (data, fileNum, address, nvalues, strictChecking, slaveId, true, request.getTracer ()); offset += 7 + nbytes; } setResponse (trans, request.getData (), null); } catch (ModbusException e) { setException (trans, e); } } private void handleReadFifoQueue (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); checkSlaveId (request); checkSize (request, 2, 0); final int address = request.getInt (0); final AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap (); int addr = model.fromHoldingRegister (address); long nvalues = model.getLongValue (addr); if (nvalues > 31) Modbus.dataError ("S032", "Queue count exceeds 31: " + nvalues); Tracer.Tracing tracing = new Tracer.Tracing (); byte [] data = model.pack (addr, (int) nvalues + 1, space, tracing, slaveId, false); if (data.length + 2 > maxPdu) { Modbus.dataError ("S010", "Invalid queue size (" + nvalues + ") - response would exceed max PDU size (" + maxPdu + " bytes)"); } MessageBuilder body = new MessageBuilder (); body.addInt (data.length); body.addData (data); setResponse (trans, body.getData (), tracing); } catch (ModbusException e) { setException (trans, e); } } private void handleMonitorHoldingRegisters (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); request.checkMinSize (5); int address = request.getInt (0); int count = request.getInt (2); if (strictChecking && count == 0) Modbus.dataError ("S008", "Invalid count (0)"); int size = checkSize (request, 5, request.getByte (4)); byte [] data = request.getData (5, size); AddressMap.Map space = model.getAddressMap ().getHoldingRegisterMap (); int addr = model.fromHoldingRegister (address); int nvalues = model.getNumValues (addr, count, space); if (strictChecking && model.getTotalBytes (addr, nvalues, space) != size) { Modbus.dataError ("S012", "Count (" + count + ") does not match number of data bytes (" + size + ")"); } if (checkCountLimits && size > 123 * 2) { Modbus.dataError ("S013", "Number of data bytes (" + size + ") exceeds count limit (123 * 2 bytes)"); } if (!isMonitor) model.checkWritable (addr, nvalues, space); model.unpack (data, addr, nvalues, space, strictChecking, slaveId, false, request.getTracer ()); MessageBuilder body = new MessageBuilder (); body.addInt (address); body.addInt (count); setResponse (trans, body.getData (), null); } catch (ModbusException e) { setException (trans, e); } } private void handleMonitorInputRegisters (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); request.checkMinSize (5); int address = request.getInt (0); int count = request.getInt (2); if (strictChecking && count == 0) Modbus.dataError ("S008", "Invalid count (0)"); int size = checkSize (request, 5, request.getByte (4)); byte [] data = request.getData (5, size); AddressMap.Map space = model.getAddressMap ().getInputRegisterMap (); int addr = model.fromInputRegister (address); int nvalues = model.getNumValues (addr, count, space); if (strictChecking && model.getTotalBytes (addr, nvalues, space) != size) { Modbus.dataError ("S012", "Count (" + count + ") does not match number of data bytes (" + size + ")"); } if (checkCountLimits && size > 123 * 2) { Modbus.dataError ("S013", "Number of data bytes (" + size + ") exceeds count limit (123 * 2 bytes)"); } model.unpack (data, addr, nvalues, space, strictChecking, slaveId, false, request.getTracer ()); MessageBuilder body = new MessageBuilder (); body.addInt (address); body.addInt (count); setResponse (trans, body.getData (), null); } catch (ModbusException e) { setException (trans, e); } } private void handleMonitorCoils (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); request.checkMinSize (5); int address = request.getInt (0); int nbits = request.getInt (2); if (strictChecking && nbits == 0) Modbus.dataError ("S008", "Invalid count (0)"); int nbytes = checkSize (request, 5, (nbits + 7) / 8); if (strictChecking && nbytes != request.getByte (4)) { Modbus.dataError ("S015", "Number of coils (" + nbits + ") does not match byte count (" + nbytes + ")"); } if (checkCountLimits && nbits > 1968) { Modbus.dataError ("S014", "Count (" + nbits + ") exceeds count limit (1968)"); } byte [] data = request.getData (5, nbytes); AddressMap.Map space = model.getAddressMap ().getCoilMap (); space.checkInRange (address, nbits); int valueSize = model.getCoilValueSize (); ModelAddress ma = model.fromCoil (address); int addr = ma.getAddress (); int shift = ma.getShift (); int multiplier = valueSize == 0 ? 1 : valueSize * 8; int nvalues = (nbits + shift + multiplier - 1) / multiplier; if (!isMonitor) model.checkWritable (addr, nvalues, space); int size = valueSize == 0 ? (nvalues + 7) / 8 : nvalues * valueSize; byte [] newData = new byte [size]; shiftLeft (newData, data, shift); byte [] mask = new byte [nbytes]; setMask (mask, nbits); byte [] shmask = new byte [size]; shiftLeft (shmask, mask, shift); if (model.bitReverseCoils ()) { Bytes.reverseBits (newData); Bytes.reverseBits (shmask); } if (model.byteSwapCoils ()) { Bytes.byteSwap (newData); Bytes.byteSwap (shmask); } byte [] oldData = model.pack (addr, nvalues, space, null, slaveId, false); assert oldData.length == size; Bytes.andNot (oldData, shmask); Bytes.or (newData, oldData); model.unpack (newData, addr, nvalues, space, strictChecking, slaveId, false, request.getTracer ()); MessageBuilder body = new MessageBuilder (); body.addInt (address); body.addInt (nbits); setResponse (trans, body.getData (), null); } catch (ModbusException e) { setException (trans, e); } } private void handleMonitorDiscreteInputs (final ModbusTransaction trans) throws InterruptedException { if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); request.checkMinSize (5); int address = request.getInt (0); int nbits = request.getInt (2); if (strictChecking && nbits == 0) Modbus.dataError ("S008", "Invalid count (0)"); int nbytes = checkSize (request, 5, (nbits + 7) / 8); if (strictChecking && nbytes != request.getByte (4)) { Modbus.dataError ("S015", "Number of discrete inputs (" + nbits + ") does not match byte count (" + nbytes + ")"); } if (checkCountLimits && nbits > 1968) { Modbus.dataError ("S014", "Count (" + nbits + ") exceeds count limit (1968)"); } byte [] data = request.getData (5, nbytes); AddressMap.Map space = model.getAddressMap ().getDiscreteInputMap (); space.checkInRange (address, nbits); int valueSize = model.getDiscreteInputValueSize (); ModelAddress ma = model.fromDiscreteInput (address); int addr = ma.getAddress (); int shift = ma.getShift (); int multiplier = valueSize == 0 ? 1 : valueSize * 8; int nvalues = (nbits + shift + multiplier - 1) / multiplier; int size = valueSize == 0 ? (nvalues + 7) / 8 : nvalues * valueSize; byte [] newData = new byte [size]; shiftLeft (newData, data, shift); byte [] mask = new byte [nbytes]; setMask (mask, nbits); byte [] shmask = new byte [size]; shiftLeft (shmask, mask, shift); if (model.bitReverseCoils ()) { Bytes.reverseBits (newData); Bytes.reverseBits (shmask); } if (model.byteSwapCoils ()) { Bytes.byteSwap (newData); Bytes.byteSwap (shmask); } byte [] oldData = model.pack (addr, nvalues, space, null, slaveId, false); assert oldData.length == size; Bytes.andNot (oldData, shmask); Bytes.or (newData, oldData); model.unpack (newData, addr, nvalues, space, strictChecking, slaveId, false, request.getTracer ()); MessageBuilder body = new MessageBuilder (); body.addInt (address); body.addInt (nbits); setResponse (trans, body.getData (), null); } catch (ModbusException e) { setException (trans, e); } } private void handleMonitorFileRecord (final ModbusTransaction trans) throws InterruptedException { if (filePacker == null) { functionNotSupported (trans, Modbus.FUNC_WRITE_FILE_RECORD); return; } if (forwarder != nullForwarder) throw new IllegalStateException ("Command cannot be forwarded"); try { ModbusMessage request = trans.getRequest (); request.checkMinSize (strictChecking ? 10 : 1); int size = checkSize (request, 1, request.getByte (0)); // Check the request before writing anything. for (int offset = 1 ; offset <= size ; ) { int sizeLeft = (size + 1) - (offset + 7); if (sizeLeft < 0) { if (strictChecking) Modbus.dataError ("S027", "Excess data in message"); break; } int refType = request.getByte (offset); if (refType != 6) { Modbus.dataError ("S028", "Incorrect reference type: " + refType + " instead of 6"); } int fileNum = request.getInt (offset + 1); if (fileNum < 1) { Modbus.dataError ("S029", "Invalid file number: " + fileNum); } int address = request.getInt (offset + 3); if (strictChecking && address > 9999) { Modbus.dataError ("S030", "Invalid register address: " + address); } int count = request.getInt (offset + 5); if (strictChecking && count == 0) Modbus.dataError ("S008", "Invalid count (0)"); int nvalues = filePacker.getNumValues (fileNum, address, count); int nbytes = filePacker.getTotalBytes (fileNum, address, nvalues); if (nbytes > sizeLeft) { Modbus.dataError ("S031", "Insufficient data for file " + fileNum + " register " + address + " length " + count); } if (!isMonitor) filePacker.checkWritable (fileNum, address, nvalues); offset += 7 + nbytes; } // Go through the request again, executing the writes. for (int offset = 1 ; offset <= size ; ) { int refType = request.getByte (offset); int fileNum = request.getInt (offset + 1); int address = request.getInt (offset + 3); int count = request.getInt (offset + 5); int nvalues = filePacker.getNumValues (fileNum, address, count); int nbytes = filePacker.getTotalBytes (fileNum, address, nvalues); byte [] data = request.getData (offset + 7, nbytes); filePacker.unpack (data, fileNum, address, nvalues, strictChecking, slaveId, false, request.getTracer ()); offset += 7 + nbytes; } setResponse (trans, request.getData (), null); } catch (ModbusException e) { setException (trans, e); } } /** * Sets the first {@code len} bits of a byte array to 1, and the remaining * bits to 0. *

The "first" bit in each byte is the least-significant (rightmost) bit * (as required for Modbus coils and discrete-inputs) - this looks * confusing if you print the array using {@link Bytes#toHexString}. * @param mask the byte array. * @param len how many bits to set to 1. */ private static void setMask (byte [] mask, int len) { int byteLen = len / 8; int bitLen = len % 8; for (int i = 0 ; i < mask.length ; i++) { if (i < byteLen) mask [i] = (byte) 0xff; else if (i == byteLen) mask [i] = (byte) (0xff >> (8 - bitLen)); else mask [i] = 0; } } /** * Shifts the contents of a byte array left by a specified number of bits. *

The "first" bit in each byte is the least-significant (rightmost) bit * (as required for Modbus coils and discrete-inputs) - this looks * confusing if you print the result using {@link Bytes#toHexString}. * @param dest where to store the result. * @param src the byte array to be shifted. * @param shift how many bits to shift by. */ private static void shiftLeft (byte [] dest, byte [] src, int shift) { if (dest.length == 0) return; if (shift < 0) { shiftRight (dest, src, -shift); return; } int byteShift = shift / 8; int bitShift = shift % 8; byte last = 0; for (int i = 0 ; i < dest.length ; i++) { int j = i - byteShift; byte b = (j >= 0 && j < src.length) ? src [j] : 0; dest [i] = (byte) (((last & 0xff) >> (8 - bitShift)) | (b << bitShift)); last = b; } } /** * Shifts the contents of a byte array right by a specified number of bits. *

The "first" bit in each byte is the least-significant (rightmost) bit * (as required for Modbus coils and discrete-inputs) - this looks * confusing if you print the result using {@link Bytes#toHexString}. * @param dest where to store the result. * @param src the byte array to be shifted. * @param shift how many bits to shift by. */ private static void shiftRight (byte [] dest, byte [] src, int shift) { if (dest.length == 0) return; if (shift < 0) { shiftLeft (dest, src, -shift); return; } int byteShift = shift / 8; int bitShift = shift % 8; byte b = src [byteShift]; for (int i = 0 ; i < dest.length ; i++) { int j = i + byteShift + 1; byte next = (j >= 0 && j < src.length) ? src [j] : 0; dest [i] = (byte) (((b & 0xff) >> bitShift) | (next << (8 - bitShift))); b = next; } } }