modbus_server_pdu.c
Go to the documentation of this file.
1 /**
2  * @file modbus_server_pdu.c
3  * @brief Modbus PDU processing
4  *
5  * @section License
6  *
7  * SPDX-License-Identifier: GPL-2.0-or-later
8  *
9  * Copyright (C) 2010-2022 Oryx Embedded SARL. All rights reserved.
10  *
11  * This file is part of CycloneTCP Open.
12  *
13  * This program is free software; you can redistribute it and/or
14  * modify it under the terms of the GNU General Public License
15  * as published by the Free Software Foundation; either version 2
16  * of the License, or (at your option) any later version.
17  *
18  * This program is distributed in the hope that it will be useful,
19  * but WITHOUT ANY WARRANTY; without even the implied warranty of
20  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
21  * GNU General Public License for more details.
22  *
23  * You should have received a copy of the GNU General Public License
24  * along with this program; if not, write to the Free Software Foundation,
25  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
26  *
27  * @author Oryx Embedded SARL (www.oryx-embedded.com)
28  * @version 2.2.0
29  **/
30 
31 //Switch to the appropriate trace level
32 #define TRACE_LEVEL MODBUS_TRACE_LEVEL
33 
34 //Dependencies
35 #include "modbus/modbus_server.h"
38 #include "modbus/modbus_debug.h"
39 #include "debug.h"
40 
41 //Check TCP/IP stack configuration
42 #if (MODBUS_SERVER_SUPPORT == ENABLED)
43 
44 
45 /**
46  * @brief Process Modbus request
47  * @param[in] connection Pointer to the client connection
48  * @return Error code
49  **/
50 
52 {
53  error_t error;
54  size_t requestLen;
55  size_t responseLen;
56  void *request;
57  void *response;
60  ModbusServerContext *context;
61 
62  //Point to the Modbus/TCP server context
63  context = connection->context;
64  //Point to the Modbus request PDU
65  request = modbusServerGetRequestPdu(connection, &requestLen);
66 
67  //Malformed request?
68  if(requestLen == 0)
69  return ERROR_INVALID_LENGTH;
70 
71  //Debug message
72  TRACE_INFO("Modbus Server: Request PDU received (%" PRIuSIZE " bytes)...\r\n",
73  requestLen);
74 
75  //Dump the contents of the PDU for debugging purpose
76  modbusDumpRequestPdu(request, requestLen);
77 
78  //Retrieve function code
79  functionCode = (ModbusFunctionCode) ((uint8_t *) request)[0];
80 
81  //Any registered callback?
82  if(context->settings.processPduCallback != NULL)
83  {
84  //Point to the Modbus response PDU
85  response = modbusServerGetResponsePdu(connection);
86  //Initialize response length
87  responseLen = 0;
88 
89  //Process request PDU
90  error = context->settings.processPduCallback(request, requestLen,
91  response, &responseLen);
92 
93  //Check status code
94  if(!error)
95  {
96  //Valid response PDU?
97  if(responseLen > 0)
98  {
99  //Debug message
100  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n",
101  responseLen);
102 
103  //Dump the contents of the PDU for debugging purpose
104  modbusDumpResponsePdu(response, responseLen);
105 
106  //Format MBAP header
107  error = modbusServerFormatMbapHeader(connection, responseLen);
108  }
109  }
110  }
111  else
112  {
113  //Keep processing
115  }
116 
117  //Unknown function code?
118  if(error == ERROR_INVALID_FUNCTION_CODE)
119  {
120  //Check function code
121  switch(functionCode)
122  {
123  //Read Coils request?
125  //Process Modbus PDU
126  error = modbusServerProcessReadCoilsReq(connection,
127  request, requestLen);
128  break;
129 
130  //Format Read Discrete Inputs request?
132  //Process Modbus PDU
133  error = modbusServerProcessReadDiscreteInputsReq(connection,
134  request, requestLen);
135  break;
136 
137  //Read Holding Registers request?
139  //Process Modbus PDU
140  error = modbusServerProcessReadHoldingRegsReq(connection,
141  request, requestLen);
142  break;
143 
144  //Read Input Registers request?
146  //Process Modbus PDU
147  error = modbusServerProcessReadInputRegsReq(connection,
148  request, requestLen);
149  break;
150 
151  //Write Single Coil request?
153  //Process Modbus PDU
154  error = modbusServerProcessWriteSingleCoilReq(connection,
155  request, requestLen);
156  break;
157 
158  //Write Single Register request?
160  //Process Modbus PDU
161  error = modbusServerProcessWriteSingleRegReq(connection,
162  request, requestLen);
163  break;
164 
165  //Write Multiple Coils request?
167  //Process Modbus PDU
168  error = modbusServerProcessWriteMultipleCoilsReq(connection,
169  request, requestLen);
170  break;
171 
172  //Write Multiple Registers request?
174  //Process Modbus PDU
175  error = modbusServerProcessWriteMultipleRegsReq(connection,
176  request, requestLen);
177  break;
178 
179  //Mask Write Register request?
181  //Process Modbus PDU
182  error = modbusServerProcessMaskWriteRegReq(connection,
183  request, requestLen);
184  break;
185 
186  //Read/Write Multiple Registers request?
188  //Process Modbus PDU
190  request, requestLen);
191  break;
192 
193  //Illegal function code?
194  default:
195  //Report an error
197  break;
198  }
199  }
200 
201  //Any exception?
202  if(error == ERROR_INVALID_FUNCTION_CODE ||
203  error == ERROR_INVALID_ADDRESS ||
204  error == ERROR_INVALID_VALUE ||
205  error == ERROR_WRITE_FAILED ||
206  error == ERROR_READ_FAILED ||
207  error == ERROR_DEVICE_BUSY)
208  {
209  //Retrieve exception code
211 
212  //Send an exception response to the Modbus/TCP client
213  error = modbusServerFormatExceptionResp(connection, functionCode,
214  exceptionCode);
215  }
216 
217  //Return status code
218  return error;
219 }
220 
221 
222 /**
223  * @brief Process Read Coils request
224  *
225  * This function code is used to read from 1 to 2000 contiguous status of
226  * coils in a remote device. The request specifies the starting address and
227  * the number of coils
228  *
229  * @param[in] connection Pointer to the client connection
230  * @param[in] request Pointer to the request PDU
231  * @param[in] length Length of the request PDU, in bytes
232  * @return Error code
233  **/
234 
236  const ModbusReadCoilsReq *request, size_t length)
237 {
238  error_t error;
239  uint16_t i;
240  uint16_t quantity;
241  uint16_t address;
242  bool_t state;
243  ModbusReadCoilsResp *response;
244 
245  //Initialize status code
246  error = NO_ERROR;
247 
248  //Malformed PDU?
249  if(length < sizeof(ModbusReadCoilsReq))
250  return ERROR_INVALID_LENGTH;
251 
252  //Get the address of the first coil
253  address = ntohs(request->startingAddr);
254  //Get the number of coils
255  quantity = ntohs(request->quantityOfCoils);
256 
257  //The number of discrete inputs must be in range 1 to 2000
258  if(quantity < 1 || quantity > 2000)
259  return ERROR_INVALID_VALUE;
260 
261  //Point to the Modbus response PDU
262  response = modbusServerGetResponsePdu(connection);
263 
264  //Format Read Coils response
265  response->functionCode = request->functionCode;
266  response->byteCount = (quantity + 7) / 8;
267 
268  //If the quantity of coils is not a multiple of eight, the remaining
269  //bits in the final data byte will be padded with zeros
270  if((quantity % 8) != 0)
271  {
272  response->coilStatus[response->byteCount - 1] = 0;
273  }
274 
275  //Lock access to Modbus table
276  modbusServerLock(connection);
277 
278  //Read the specified number of coils
279  for(i = 0; i < quantity && !error; i++)
280  {
281  //Retrieve the state of the current coil
282  error = modbusServerReadCoil(connection, address + i, &state);
283 
284  //Successful read operation?
285  if(!error)
286  {
287  //The coils in the response message are packed as one coil per bit
288  //of the data field
289  if(state)
290  {
291  MODBUS_SET_COIL(response->coilStatus, i);
292  }
293  else
294  {
295  MODBUS_RESET_COIL(response->coilStatus, i);
296  }
297  }
298  }
299 
300  //Unlock access to Modbus table
301  modbusServerUnlock(connection);
302 
303  //Check whether the read operation has failed
304  if(error)
305  return error;
306 
307  //Compute the length of the response PDU
308  length = sizeof(ModbusReadCoilsResp) + response->byteCount;
309 
310  //Debug message
311  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
312  //Dump the contents of the PDU for debugging purpose
313  modbusDumpResponsePdu(response, length);
314 
315  //Format MBAP header
316  return modbusServerFormatMbapHeader(connection, length);
317 }
318 
319 
320 /**
321  * @brief Process Read Discrete Inputs request
322  *
323  * This function code is used to read from 1 to 2000 contiguous status of
324  * discrete inputs in a remote device. The request specifies the starting
325  * address and the number of inputs
326  *
327  * @param[in] connection Pointer to the client connection
328  * @param[in] request Pointer to the request PDU
329  * @param[in] length Length of the request PDU, in bytes
330  * @return Error code
331  **/
332 
334  const ModbusReadDiscreteInputsReq *request, size_t length)
335 {
336  error_t error;
337  uint16_t i;
338  uint16_t address;
339  uint16_t quantity;
340  bool_t state;
342 
343  //Initialize status code
344  error = NO_ERROR;
345 
346  //Malformed PDU?
347  if(length < sizeof(ModbusReadDiscreteInputsReq))
348  return ERROR_INVALID_LENGTH;
349 
350  //Get the address of the first coil
351  address = ntohs(request->startingAddr);
352  //Get the number of coils
353  quantity = ntohs(request->quantityOfInputs);
354 
355  //The number of discrete inputs must be in range 1 to 2000
356  if(quantity < 1 || quantity > 2000)
357  return ERROR_INVALID_VALUE;
358 
359  //Point to the Modbus response PDU
360  response = modbusServerGetResponsePdu(connection);
361 
362  //Format Read Discrete Inputs response
363  response->functionCode = request->functionCode;
364  response->byteCount = (quantity + 7) / 8;
365 
366  //If the quantity of coils is not a multiple of eight, the remaining
367  //bits in the final data byte will be padded with zeros
368  if((quantity % 8) != 0)
369  {
370  response->inputStatus[response->byteCount - 1] = 0;
371  }
372 
373  //Lock access to Modbus table
374  modbusServerLock(connection);
375 
376  //Read the specified number of coils
377  for(i = 0; i < quantity && !error; i++)
378  {
379  //Retrieve the state of the current coil
380  error = modbusServerReadDiscreteInput(connection, address + i, &state);
381 
382  //Successful read operation?
383  if(!error)
384  {
385  //The coils in the response message are packed as one coil per bit
386  //of the data field
387  if(state)
388  {
389  MODBUS_SET_COIL(response->inputStatus, i);
390  }
391  else
392  {
393  MODBUS_RESET_COIL(response->inputStatus, i);
394  }
395  }
396  }
397 
398  //Unlock access to Modbus table
399  modbusServerUnlock(connection);
400 
401  //Check whether the read operation has failed
402  if(error)
403  return error;
404 
405  //Compute the length of the response PDU
406  length = sizeof(ModbusReadDiscreteInputsResp) + response->byteCount;
407 
408  //Debug message
409  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
410  //Dump the contents of the PDU for debugging purpose
411  modbusDumpResponsePdu(response, length);
412 
413  //Format MBAP header
414  return modbusServerFormatMbapHeader(connection, length);
415 }
416 
417 
418 /**
419  * @brief Process Read Holding Registers request
420  *
421  * This function code is used to read the contents of a contiguous block of
422  * holding registers in a remote device. The request specifies the starting
423  * register address and the number of registers
424  *
425  * @param[in] connection Pointer to the client connection
426  * @param[in] request Pointer to the request PDU
427  * @param[in] length Length of the request PDU, in bytes
428  * @return Error code
429  **/
430 
432  const ModbusReadHoldingRegsReq *request, size_t length)
433 {
434  error_t error;
435  uint16_t i;
436  uint16_t address;
437  uint16_t quantity;
438  uint16_t value;
439  ModbusReadHoldingRegsResp *response;
440 
441  //Initialize status code
442  error = NO_ERROR;
443 
444  //Malformed PDU?
445  if(length < sizeof(ModbusReadHoldingRegsReq))
446  return ERROR_INVALID_LENGTH;
447 
448  //Get the address of the first register
449  address = ntohs(request->startingAddr);
450  //Get the number of registers
451  quantity = ntohs(request->quantityOfRegs);
452 
453  //The number of registers must be in range 1 to 125
454  if(quantity < 1 || quantity > 125)
455  return ERROR_INVALID_VALUE;
456 
457  //Point to the Modbus response PDU
458  response = modbusServerGetResponsePdu(connection);
459 
460  //Format Read Holding Registers response
461  response->functionCode = request->functionCode;
462  response->byteCount = quantity * sizeof(uint16_t);
463 
464  //Lock access to Modbus table
465  modbusServerLock(connection);
466 
467  //Read the specified number of registers
468  for(i = 0; i < quantity && !error; i++)
469  {
470  //Retrieve the value of the current register
471  error = modbusServerReadHoldingReg(connection, address + i, &value);
472  //Convert the value to network byte order
473  response->regValue[i] = htons(value);
474  }
475 
476  //Unlock access to Modbus table
477  modbusServerUnlock(connection);
478 
479  //Check whether the read operation has failed
480  if(error)
481  return error;
482 
483  //Compute the length of the response PDU
484  length = sizeof(ModbusReadHoldingRegsResp) + response->byteCount;
485 
486  //Debug message
487  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
488  //Dump the contents of the PDU for debugging purpose
489  modbusDumpResponsePdu(response, length);
490 
491  //Format MBAP header
492  return modbusServerFormatMbapHeader(connection, length);
493 }
494 
495 
496 /**
497  * @brief Process Read Input Registers request
498  *
499  * This function code is used to read from 1 to 125 contiguous input registers
500  * in a remote device. The request specifies the starting register address and
501  * the number of registers
502  *
503  * @param[in] connection Pointer to the client connection
504  * @param[in] request Pointer to the request PDU
505  * @param[in] length Length of the request PDU, in bytes
506  * @return Error code
507  **/
508 
510  const ModbusReadInputRegsReq *request, size_t length)
511 {
512  error_t error;
513  uint16_t i;
514  uint16_t address;
515  uint16_t quantity;
516  uint16_t value;
517  ModbusReadInputRegsResp *response;
518 
519  //Initialize status code
520  error = NO_ERROR;
521 
522  //Malformed PDU?
523  if(length < sizeof(ModbusReadInputRegsReq))
524  return ERROR_INVALID_LENGTH;
525 
526  //Get the address of the first register
527  address = ntohs(request->startingAddr);
528  //Get the number of registers
529  quantity = ntohs(request->quantityOfRegs);
530 
531  //The number of registers must be in range 1 to 125
532  if(quantity < 1 || quantity > 125)
533  return ERROR_INVALID_VALUE;
534 
535  //Point to the Modbus response PDU
536  response = modbusServerGetResponsePdu(connection);
537 
538  //Format Read Input Registers response
539  response->functionCode = request->functionCode;
540  response->byteCount = quantity * sizeof(uint16_t);
541 
542  //Lock access to Modbus table
543  modbusServerLock(connection);
544 
545  //Read the specified number of registers
546  for(i = 0; i < quantity && !error; i++)
547  {
548  //Retrieve the value of the current register
549  error = modbusServerReadInputReg(connection, address + i, &value);
550  //Convert the value to network byte order
551  response->regValue[i] = htons(value);
552  }
553 
554  //Unlock access to Modbus table
555  modbusServerUnlock(connection);
556 
557  //Check whether the read operation has failed
558  if(error)
559  return error;
560 
561  //Compute the length of the response PDU
562  length = sizeof(ModbusReadInputRegsResp) + response->byteCount;
563 
564  //Debug message
565  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
566  //Dump the contents of the PDU for debugging purpose
567  modbusDumpResponsePdu(response, length);
568 
569  //Format MBAP header
570  return modbusServerFormatMbapHeader(connection, length);
571 }
572 
573 
574 /**
575  * @brief Process Write Single Coil request
576  *
577  * This function code is used to write a single output to either ON or OFF in
578  * a remote device. The request specifies the address of the coil to be forced
579  * and the requested ON/OFF state
580  *
581  * @param[in] connection Pointer to the client connection
582  * @param[in] request Pointer to the request PDU
583  * @param[in] length Length of the request PDU, in bytes
584  * @return Error code
585  **/
586 
588  const ModbusWriteSingleCoilReq *request, size_t length)
589 {
590  error_t error;
591  uint16_t address;
592  bool_t state;
593  ModbusWriteSingleCoilResp *response;
594 
595  //Malformed PDU?
596  if(length < sizeof(ModbusWriteSingleCoilReq))
597  return ERROR_INVALID_LENGTH;
598 
599  //Get the address of the coil to be forced
600  address = ntohs(request->outputAddr);
601 
602  //Check output value
603  if(ntohs(request->outputValue) == MODBUS_COIL_STATE_ON)
604  {
605  //A value of 0xFF00 requests the output to be ON
606  state = TRUE;
607  }
608  else if(ntohs(request->outputValue) == MODBUS_COIL_STATE_OFF)
609  {
610  //A value of 0x0000 requests the output to be OFF
611  state = FALSE;
612  }
613  else
614  {
615  //Report an error
616  return ERROR_INVALID_VALUE;
617  }
618 
619  //Lock access to Modbus table
620  modbusServerLock(connection);
621  //Force the coil to the desired ON/OFF state
622  error = modbusServerWriteCoil(connection, address, state, TRUE);
623  //Unlock access to Modbus table
624  modbusServerUnlock(connection);
625 
626  //Check whether the write operation has failed
627  if(error)
628  return error;
629 
630  //Point to the Modbus response PDU
631  response = modbusServerGetResponsePdu(connection);
632 
633  //The normal response is an echo of the request
634  response->functionCode = request->functionCode;
635  response->outputAddr = request->outputAddr;
636  response->outputValue = request->outputValue;
637 
638  //Compute the length of the response PDU
640 
641  //Debug message
642  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
643  //Dump the contents of the PDU for debugging purpose
644  modbusDumpResponsePdu(response, length);
645 
646  //Format MBAP header
647  return modbusServerFormatMbapHeader(connection, length);
648 }
649 
650 
651 /**
652  * @brief Process Write Single Register request
653  *
654  * This function code is used to write a single holding register in a remote
655  * device. The request specifies the address of the register to be written and
656  * the register value
657  *
658  * @param[in] connection Pointer to the client connection
659  * @param[in] request Pointer to the request PDU
660  * @param[in] length Length of the request PDU, in bytes
661  * @return Error code
662  **/
663 
665  const ModbusWriteSingleRegReq *request, size_t length)
666 {
667  error_t error;
668  uint16_t address;
669  uint16_t value;
670  ModbusWriteSingleRegResp *response;
671 
672  //Malformed PDU?
673  if(length < sizeof(ModbusWriteSingleRegReq))
674  return ERROR_INVALID_LENGTH;
675 
676  //Get the address of the register
677  address = ntohs(request->regAddr);
678  //Get the value of the register
679  value = ntohs(request->regValue);
680 
681  //Lock access to Modbus table
682  modbusServerLock(connection);
683  //Write register value
684  error = modbusServerWriteReg(connection, address, value, TRUE);
685  //Unlock access to Modbus table
686  modbusServerUnlock(connection);
687 
688  //Check whether the write operation has failed
689  if(error)
690  return error;
691 
692  //Point to the Modbus response PDU
693  response = modbusServerGetResponsePdu(connection);
694 
695  //The normal response is an echo of the request
696  response->functionCode = request->functionCode;
697  response->regAddr = request->regAddr;
698  response->regValue = request->regValue;
699 
700  //Compute the length of the response PDU
702 
703  //Debug message
704  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
705  //Dump the contents of the PDU for debugging purpose
706  modbusDumpResponsePdu(response, length);
707 
708  //Format MBAP header
709  return modbusServerFormatMbapHeader(connection, length);
710 }
711 
712 
713 /**
714  * @brief Process Write Multiple Coils request
715  *
716  * This function code is used to force each coil in a sequence of coils to
717  * either ON or OFF in a remote device. The request specifies the starting
718  * address, the number of outputs and the requested ON/OFF states
719  *
720  * @param[in] connection Pointer to the client connection
721  * @param[in] request Pointer to the request PDU
722  * @param[in] length Length of the request PDU, in bytes
723  * @return Error code
724  **/
725 
727  const ModbusWriteMultipleCoilsReq *request, size_t length)
728 {
729  error_t error;
730  uint16_t i;
731  uint16_t address;
732  uint16_t quantity;
734 
735  //Initialize status code
736  error = NO_ERROR;
737 
738  //Malformed PDU?
739  if(length < sizeof(ModbusWriteMultipleCoilsReq))
740  return ERROR_INVALID_LENGTH;
741 
742  //Compute the length of the data field
744 
745  //Malformed PDU?
746  if(length < request->byteCount)
747  return ERROR_INVALID_LENGTH;
748 
749  //Get the address of the first coil to be forced
750  address = ntohs(request->startingAddr);
751  //Get the number of coils
752  quantity = ntohs(request->quantityOfOutputs);
753 
754  //The number of discrete inputs must be in range 1 to 2000
755  if(quantity < 1 || quantity > 2000)
756  return ERROR_INVALID_VALUE;
757 
758  //Check byte count field
759  if(request->byteCount != ((quantity + 7) / 8))
760  return ERROR_INVALID_VALUE;
761 
762  //Lock access to Modbus table
763  modbusServerLock(connection);
764 
765  //Consistency check (first phase)
766  for(i = 0; i < quantity && !error; i++)
767  {
768  //Validate coil address
769  error = modbusServerWriteCoil(connection, address + i,
770  MODBUS_TEST_COIL(request->outputValue, i), FALSE);
771  }
772 
773  //Commit changes (second phase)
774  for(i = 0; i < quantity && !error; i++)
775  {
776  //Force the current coil to the desired ON/OFF state
777  error = modbusServerWriteCoil(connection, address + i,
778  MODBUS_TEST_COIL(request->outputValue, i), TRUE);
779  }
780 
781  //Unlock access to Modbus table
782  modbusServerUnlock(connection);
783 
784  //Check whether the write operation has failed
785  if(error)
786  return error;
787 
788  //Point to the Modbus response PDU
789  response = modbusServerGetResponsePdu(connection);
790 
791  //The normal response returns the starting address, and quantity of
792  //coils forced
793  response->functionCode = request->functionCode;
794  response->startingAddr = request->startingAddr;
795  response->quantityOfOutputs = request->quantityOfOutputs;
796 
797  //Compute the length of the response PDU
799 
800  //Debug message
801  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
802  //Dump the contents of the PDU for debugging purpose
803  modbusDumpResponsePdu(response, length);
804 
805  //Format MBAP header
806  return modbusServerFormatMbapHeader(connection, length);
807 }
808 
809 
810 /**
811  * @brief Process Write Multiple Registers request
812  *
813  * This function code is used to write a block of contiguous registers (1 to
814  * 123 registers) in a remote device. The request specifies the starting
815  * address, the number of registers and the requested written values
816  *
817  * @param[in] connection Pointer to the client connection
818  * @param[in] request Pointer to the request PDU
819  * @param[in] length Length of the request PDU, in bytes
820  * @return Error code
821  **/
822 
824  const ModbusWriteMultipleRegsReq *request, size_t length)
825 {
826  error_t error;
827  uint16_t i;
828  uint16_t address;
829  uint16_t quantity;
830  ModbusWriteMultipleRegsResp *response;
831 
832  //Initialize status code
833  error = NO_ERROR;
834 
835  //Malformed PDU?
836  if(length < sizeof(ModbusWriteMultipleRegsReq))
837  return ERROR_INVALID_LENGTH;
838 
839  //Compute the length of the data field
841 
842  //Malformed PDU?
843  if(length < request->byteCount)
844  return ERROR_INVALID_LENGTH;
845 
846  //Get the address of the first register to be written
847  address = ntohs(request->startingAddr);
848  //Get the number of registers
849  quantity = ntohs(request->quantityOfRegs);
850 
851  //The number of registers must be in range 1 to 123
852  if(quantity < 1 || quantity > 123)
853  return ERROR_INVALID_VALUE;
854 
855  //Check byte count field
856  if(request->byteCount != (quantity * sizeof(uint16_t)))
857  return ERROR_INVALID_VALUE;
858 
859  //Lock access to Modbus table
860  modbusServerLock(connection);
861 
862  //Consistency check (first phase)
863  for(i = 0; i < quantity && !error; i++)
864  {
865  //Validate register address
866  error = modbusServerWriteReg(connection, address + i,
867  ntohs(request->regValue[i]), FALSE);
868  }
869 
870  //Commit changes (second phase)
871  for(i = 0; i < quantity && !error; i++)
872  {
873  //Write the value of the current register
874  error = modbusServerWriteReg(connection, address + i,
875  ntohs(request->regValue[i]), TRUE);
876  }
877 
878  //Unlock access to Modbus table
879  modbusServerUnlock(connection);
880 
881  //Check whether the write operation has failed
882  if(error)
883  return error;
884 
885  //Point to the Modbus response PDU
886  response = modbusServerGetResponsePdu(connection);
887 
888  //The normal response returns the starting address, and quantity of
889  //registers written
890  response->functionCode = request->functionCode;
891  response->startingAddr = request->startingAddr;
892  response->quantityOfRegs = request->quantityOfRegs;
893 
894  //Compute the length of the response PDU
896 
897  //Debug message
898  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
899  //Dump the contents of the PDU for debugging purpose
900  modbusDumpResponsePdu(response, length);
901 
902  //Format MBAP header
903  return modbusServerFormatMbapHeader(connection, length);
904 }
905 
906 
907 /**
908  * @brief Process Mask Write Register request
909  *
910  * This function code is used to modify the contents of a specified holding
911  * register using a combination of an AND mask, an OR mask, and the register's
912  * current contents. The function can be used to set or clear individual bits
913  * in the register
914  *
915  * @param[in] connection Pointer to the client connection
916  * @param[in] request Pointer to the request PDU
917  * @param[in] length Length of the request PDU, in bytes
918  * @return Error code
919  **/
920 
922  const ModbusMaskWriteRegReq *request, size_t length)
923 {
924  error_t error;
925  uint16_t address;
926  uint16_t andMask;
927  uint16_t orMask;
928  uint16_t value;
929  ModbusMaskWriteRegResp *response;
930 
931  //Malformed PDU?
932  if(length < sizeof(ModbusMaskWriteRegReq))
933  return ERROR_INVALID_LENGTH;
934 
935  //Get the address of the register
936  address = ntohs(request->referenceAddr);
937  //Get the value of the AND mask
938  andMask = ntohs(request->andMask);
939  //Get the value of the OR mask
940  orMask = ntohs(request->orMask);
941 
942  //Lock access to Modbus table
943  modbusServerLock(connection);
944 
945  //Retrieve the value of the register
946  error = modbusServerReadHoldingReg(connection, address, &value);
947 
948  //Check status code
949  if(!error)
950  {
951  //Apply AND mask and OR mask
952  value = (value & andMask) | (orMask & ~andMask);
953  //Write register value
954  error = modbusServerWriteReg(connection, address, value, TRUE);
955  }
956 
957  //Unlock access to Modbus table
958  modbusServerUnlock(connection);
959 
960  //Check whether the write operation has failed
961  if(error)
962  return error;
963 
964  //Point to the Modbus response PDU
965  response = modbusServerGetResponsePdu(connection);
966 
967  //The normal response is an echo of the request
968  response->functionCode = request->functionCode;
969  response->referenceAddr = request->referenceAddr;
970  response->andMask = request->andMask;
971  response->orMask = request->orMask;
972 
973  //Compute the length of the response PDU
974  length = sizeof(ModbusMaskWriteRegResp);
975 
976  //Debug message
977  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
978  //Dump the contents of the PDU for debugging purpose
979  modbusDumpResponsePdu(response, length);
980 
981  //Format MBAP header
982  return modbusServerFormatMbapHeader(connection, length);
983 }
984 
985 
986 /**
987  * @brief Process Read/Write Multiple Registers request
988  *
989  * This function code performs a combination of one read operation and one
990  * write operation in a single Modbus transaction. The write operation is
991  * performed before the read
992  *
993  * @param[in] connection Pointer to the client connection
994  * @param[in] request Pointer to the request PDU
995  * @param[in] length Length of the request PDU, in bytes
996  * @return Error code
997  **/
998 
1000  const ModbusReadWriteMultipleRegsReq *request, size_t length)
1001 {
1002  error_t error;
1003  uint16_t i;
1004  uint16_t readAddress;
1005  uint16_t readQuantity;
1006  uint16_t writeAddress;
1007  uint16_t writeQuantity;
1008  uint16_t value;
1010 
1011  //Initialize status code
1012  error = NO_ERROR;
1013 
1014  //Malformed PDU?
1016  return ERROR_INVALID_LENGTH;
1017 
1018  //Compute the length of the data field
1020 
1021  //Malformed PDU?
1022  if(length < request->writeByteCount)
1023  return ERROR_INVALID_LENGTH;
1024 
1025  //Get the address of the first register to be read
1026  readAddress = ntohs(request->readStartingAddr);
1027  //Get the number of registers to be read
1028  readQuantity = ntohs(request->quantityToRead);
1029  //Get the address of the first register to be written
1030  writeAddress = ntohs(request->writeStartingAddr);
1031  //Get the number of registers to be written
1032  writeQuantity = ntohs(request->quantityToWrite);
1033 
1034  //The number of registers to be read must be in range 1 to 125
1035  if(readQuantity < 1 || readQuantity > 125)
1036  return ERROR_INVALID_VALUE;
1037 
1038  //The number of registers to be written must be in range 1 to 121
1039  if(writeQuantity < 1 || writeQuantity > 121)
1040  return ERROR_INVALID_VALUE;
1041 
1042  //Check byte count field
1043  if(request->writeByteCount != (writeQuantity * sizeof(uint16_t)))
1044  return ERROR_INVALID_VALUE;
1045 
1046  //Point to the Modbus response PDU
1047  response = modbusServerGetResponsePdu(connection);
1048 
1049  //Format Read/Write Multiple Registers response
1050  response->functionCode = request->functionCode;
1051  response->readByteCount = readQuantity * sizeof(uint16_t);
1052 
1053  //Lock access to Modbus table
1054  modbusServerLock(connection);
1055 
1056  //Consistency check (first phase)
1057  for(i = 0; i < writeQuantity && !error; i++)
1058  {
1059  //Validate register address
1060  error = modbusServerWriteReg(connection, writeAddress + i,
1061  ntohs(request->writeRegValue[i]), FALSE);
1062  }
1063 
1064  //Commit changes (second phase)
1065  for(i = 0; i < writeQuantity && !error; i++)
1066  {
1067  //Write the value of the current register
1068  error = modbusServerWriteReg(connection, writeAddress + i,
1069  ntohs(request->writeRegValue[i]), TRUE);
1070  }
1071 
1072  //Read the specified number of registers
1073  for(i = 0; i < readQuantity && !error; i++)
1074  {
1075  //Retrieve the value of the current register
1076  error = modbusServerReadHoldingReg(connection, readAddress + i, &value);
1077 
1078  //Convert the value to network byte order
1079  response->readRegValue[i] = htons(value);
1080  }
1081 
1082  //Unlock access to Modbus table
1083  modbusServerUnlock(connection);
1084 
1085  //Check whether the write operation has failed
1086  if(error)
1087  return error;
1088 
1089  //Compute the length of the response PDU
1090  length = sizeof(ModbusReadWriteMultipleRegsResp) + response->readByteCount;
1091 
1092  //Debug message
1093  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
1094  //Dump the contents of the PDU for debugging purpose
1095  modbusDumpResponsePdu(response, length);
1096 
1097  //Format MBAP header
1098  return modbusServerFormatMbapHeader(connection, length);
1099 }
1100 
1101 
1102 /**
1103  * @brief Format exception response
1104  * @param[in] connection Pointer to the client connection
1105  * @param[in] functionCode Function code of the request
1106  * @param[in] exceptionCode Exception code
1107  * @return Exception code
1108  **/
1109 
1112 {
1113  size_t length;
1114  ModbusExceptionResp *response;
1115 
1116  //Point to the Modbus response PDU
1117  response = modbusServerGetResponsePdu(connection);
1118 
1119  //Format Exception response
1120  response->functionCode = MODBUS_EXCEPTION_MASK | functionCode;
1121  response->exceptionCode = exceptionCode;
1122 
1123  //Compute the length of the response PDU
1124  length = sizeof(ModbusExceptionResp);
1125 
1126  //Debug message
1127  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
1128  //Dump the contents of the PDU for debugging purpose
1129  modbusDumpResponsePdu(response, length);
1130 
1131  //Format MBAP header
1132  return modbusServerFormatMbapHeader(connection, length);
1133 }
1134 
1135 #endif
uint8_t length
Definition: coap_common.h:193
#define htons(value)
Definition: cpu_endian.h:413
error_t modbusDumpRequestPdu(const void *pdu, size_t length)
Dump Modbus request PDU for debugging purpose.
Definition: modbus_debug.c:99
error_t modbusServerProcessReadDiscreteInputsReq(ModbusClientConnection *connection, const ModbusReadDiscreteInputsReq *request, size_t length)
Process Read Discrete Inputs request.
int bool_t
Definition: compiler_port.h:48
uint16_t byteCount
error_t modbusServerProcessReadCoilsReq(ModbusClientConnection *connection, const ModbusReadCoilsReq *request, size_t length)
Process Read Coils request.
uint16_t andMask
void modbusServerLock(ModbusClientConnection *connection)
Lock Modbus table.
error_t modbusDumpResponsePdu(const void *pdu, size_t length)
Dump Modbus response PDU for debugging purpose.
Definition: modbus_debug.c:212
#define TRUE
Definition: os_port.h:50
__start_packed struct @14 ModbusWriteMultipleCoilsResp
Write Multiple Coils response PDU.
error_t modbusServerWriteCoil(ModbusClientConnection *connection, uint16_t address, bool_t state, bool_t commit)
Write a single coil.
__start_packed struct @20 ModbusReadWriteMultipleRegsResp
Read/Write Multiple Registers response PDU.
error_t modbusServerFormatMbapHeader(ModbusClientConnection *connection, size_t length)
Format response MBAP header.
void modbusServerUnlock(ModbusClientConnection *connection)
Unlock Modbus table.
error_t modbusServerReadDiscreteInput(ModbusClientConnection *connection, uint16_t address, bool_t *state)
Read a single discrete input.
#define MODBUS_SET_COIL(a, n)
Definition: modbus_common.h:58
error_t modbusServerReadInputReg(ModbusClientConnection *connection, uint16_t address, uint16_t *value)
Read a single input register.
__start_packed struct @18 ModbusMaskWriteRegResp
Mask Write Register response PDU.
__start_packed struct @8 ModbusReadInputRegsResp
Read Holding Input response PDU.
error_t modbusServerProcessReadHoldingRegsReq(ModbusClientConnection *connection, const ModbusReadHoldingRegsReq *request, size_t length)
Process Read Holding Registers request.
@ MODBUS_COIL_STATE_ON
__start_packed struct @11 ModbusWriteSingleRegReq
Write Single Register request PDU.
#define ModbusClientConnection
@ MODBUS_FUNCTION_READ_DISCRETE_INPUTS
Definition: modbus_common.h:77
uint8_t writeByteCount
#define FALSE
Definition: os_port.h:46
__start_packed struct @5 ModbusReadHoldingRegsReq
Read Holding Registers request PDU.
__start_packed struct @4 ModbusReadDiscreteInputsResp
Read Discrete Inputs response PDU.
@ MODBUS_FUNCTION_READ_WRITE_MULTIPLE_REGS
Definition: modbus_common.h:92
error_t
Error codes.
Definition: error.h:43
__start_packed struct @9 ModbusWriteSingleCoilReq
Write Single Coil request PDU.
__start_packed struct @12 ModbusWriteSingleRegResp
Write Single Register response PDU.
ModbusExceptionCode modbusServerTranslateExceptionCode(error_t status)
Translate exception code.
Modbus/TCP server.
@ MODBUS_FUNCTION_WRITE_MULTIPLE_REGS
Definition: modbus_common.h:87
uint8_t value[]
Definition: tcp.h:367
error_t modbusServerProcessReadInputRegsReq(ModbusClientConnection *connection, const ModbusReadInputRegsReq *request, size_t length)
Process Read Input Registers request.
@ ERROR_INVALID_ADDRESS
Definition: error.h:102
#define MODBUS_EXCEPTION_MASK
Definition: modbus_common.h:55
__start_packed struct @10 ModbusWriteSingleCoilResp
Write Single Coil response PDU.
Helper functions for Modbus/TCP server.
__start_packed struct @13 ModbusWriteMultipleCoilsReq
Write Multiple Coils request PDU.
@ ERROR_INVALID_LENGTH
Definition: error.h:110
@ MODBUS_FUNCTION_MASK_WRITE_REG
Definition: modbus_common.h:91
__start_packed struct @7 ModbusReadInputRegsReq
Read Holding Input request PDU.
__start_packed struct @16 ModbusWriteMultipleRegsResp
Write Multiple Registers response PDU.
#define MODBUS_TEST_COIL(a, n)
Definition: modbus_common.h:62
__start_packed struct @2 ModbusReadCoilsResp
Read Coils response PDU.
#define MODBUS_RESET_COIL(a, n)
Definition: modbus_common.h:60
#define TRACE_INFO(...)
Definition: debug.h:95
@ MODBUS_FUNCTION_WRITE_SINGLE_REG
Definition: modbus_common.h:81
@ MODBUS_FUNCTION_READ_INPUT_REGS
Definition: modbus_common.h:79
__start_packed struct @3 ModbusReadDiscreteInputsReq
Read Discrete Inputs request PDU.
__start_packed struct @19 ModbusReadWriteMultipleRegsReq
Read/Write Multiple Registers request PDU.
__start_packed struct @21 ModbusExceptionResp
Exception response PDU.
error_t modbusServerProcessRequest(ModbusClientConnection *connection)
Process Modbus request.
#define ntohs(value)
Definition: cpu_endian.h:421
@ ERROR_DEVICE_BUSY
Definition: error.h:268
error_t modbusServerProcessWriteMultipleCoilsReq(ModbusClientConnection *connection, const ModbusWriteMultipleCoilsReq *request, size_t length)
Process Write Multiple Coils request.
void * modbusServerGetRequestPdu(ModbusClientConnection *connection, size_t *length)
Retrieve request PDU.
uint16_t orMask
@ ERROR_INVALID_VALUE
Definition: error.h:115
__start_packed struct @15 ModbusWriteMultipleRegsReq
Write Multiple Registers request PDU.
Data logging functions for debugging purpose (Modbus/TCP)
error_t modbusServerWriteReg(ModbusClientConnection *connection, uint16_t address, uint16_t value, bool_t commit)
Write a single register.
@ ERROR_READ_FAILED
Definition: error.h:221
error_t modbusServerProcessReadWriteMultipleRegsReq(ModbusClientConnection *connection, const ModbusReadWriteMultipleRegsReq *request, size_t length)
Process Read/Write Multiple Registers request.
@ ERROR_WRITE_FAILED
Definition: error.h:220
ModbusFunctionCode
Modbus functions codes.
Definition: modbus_common.h:75
error_t modbusServerReadHoldingReg(ModbusClientConnection *connection, uint16_t address, uint16_t *value)
Read a single holding register.
__start_packed struct @17 ModbusMaskWriteRegReq
Mask Write Register request PDU.
error_t modbusServerProcessWriteSingleCoilReq(ModbusClientConnection *connection, const ModbusWriteSingleCoilReq *request, size_t length)
Process Write Single Coil request.
@ MODBUS_FUNCTION_READ_HOLDING_REGS
Definition: modbus_common.h:78
void * modbusServerGetResponsePdu(ModbusClientConnection *connection)
Retrieve response PDU.
error_t modbusServerReadCoil(ModbusClientConnection *connection, uint16_t address, bool_t *state)
Read a single coil.
Modbus PDU processing.
Ipv6Addr address
__start_packed struct @6 ModbusReadHoldingRegsResp
Read Holding Registers response PDU.
error_t modbusServerFormatExceptionResp(ModbusClientConnection *connection, ModbusFunctionCode functionCode, ModbusExceptionCode exceptionCode)
Format exception response.
@ MODBUS_FUNCTION_WRITE_SINGLE_COIL
Definition: modbus_common.h:80
error_t modbusServerProcessWriteMultipleRegsReq(ModbusClientConnection *connection, const ModbusWriteMultipleRegsReq *request, size_t length)
Process Write Multiple Registers request.
#define PRIuSIZE
uint8_t functionCode
ModbusExceptionCode
Modbus exception codes.
@ MODBUS_FUNCTION_WRITE_MULTIPLE_COILS
Definition: modbus_common.h:86
error_t modbusServerProcessWriteSingleRegReq(ModbusClientConnection *connection, const ModbusWriteSingleRegReq *request, size_t length)
Process Write Single Register request.
#define ModbusServerContext
uint8_t exceptionCode
@ MODBUS_FUNCTION_READ_COILS
Definition: modbus_common.h:76
@ NO_ERROR
Success.
Definition: error.h:44
__start_packed struct @1 ModbusReadCoilsReq
Read Coils request PDU.
Debugging facilities.
@ MODBUS_COIL_STATE_OFF
error_t modbusServerProcessMaskWriteRegReq(ModbusClientConnection *connection, const ModbusMaskWriteRegReq *request, size_t length)
Process Mask Write Register request.
@ ERROR_INVALID_FUNCTION_CODE
Definition: error.h:267