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-2019 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 1.9.6
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 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  //Format Read Discrete Inputs request?
131  //Process Modbus PDU
132  error = modbusServerProcessReadDiscreteInputsReq(connection,
133  request, requestLen);
134  break;
135  //Read Holding Registers request?
137  //Process Modbus PDU
138  error = modbusServerProcessReadHoldingRegsReq(connection,
139  request, requestLen);
140  break;
141  //Read Input Registers request?
143  //Process Modbus PDU
144  error = modbusServerProcessReadInputRegsReq(connection,
145  request, requestLen);
146  break;
147  //Write Single Coil request?
149  //Process Modbus PDU
150  error = modbusServerProcessWriteSingleCoilReq(connection,
151  request, requestLen);
152  break;
153  //Write Single Register request?
155  //Process Modbus PDU
156  error = modbusServerProcessWriteSingleRegReq(connection,
157  request, requestLen);
158  break;
159  //Write Multiple Coils request?
161  //Process Modbus PDU
162  error = modbusServerProcessWriteMultipleCoilsReq(connection,
163  request, requestLen);
164  break;
165  //Write Multiple Registers request?
167  //Process Modbus PDU
168  error = modbusServerProcessWriteMultipleRegsReq(connection,
169  request, requestLen);
170  break;
171  //Mask Write Register request?
173  //Process Modbus PDU
174  error = modbusServerProcessMaskWriteRegReq(connection,
175  request, requestLen);
176  break;
177  //Read/Write Multiple Registers request?
179  //Process Modbus PDU
181  request, requestLen);
182  break;
183  //Illegal function code?
184  default:
185  //Report an error
187  break;
188  }
189  }
190 
191  //Any exception?
192  if(error == ERROR_INVALID_FUNCTION_CODE ||
193  error == ERROR_INVALID_ADDRESS ||
194  error == ERROR_INVALID_VALUE ||
195  error == ERROR_WRITE_FAILED ||
196  error == ERROR_READ_FAILED ||
197  error == ERROR_DEVICE_BUSY)
198  {
199  //Retrieve exception code
201 
202  //Send an exception response to the Modbus/TCP client
203  error = modbusServerFormatExceptionResp(connection, functionCode,
204  exceptionCode);
205  }
206 
207  //Return status code
208  return error;
209 }
210 
211 
212 /**
213  * @brief Process Read Coils request
214  * @param[in] connection Pointer to the client connection
215  * @param[in] request Pointer to the request PDU
216  * @param[in] length Length of the request PDU, in bytes
217  * @return Error code
218  **/
219 
221  const ModbusReadCoilsReq *request, size_t length)
222 {
223  error_t error;
224  uint16_t i;
225  uint16_t quantity;
226  uint16_t address;
227  bool_t state;
228  ModbusReadCoilsResp *response;
229 
230  //Initialize status code
231  error = NO_ERROR;
232 
233  //Malformed PDU?
234  if(length < sizeof(ModbusReadCoilsReq))
235  return ERROR_INVALID_LENGTH;
236 
237  //Get the address of the first coil
238  address = ntohs(request->startingAddr);
239  //Get the number of coils
240  quantity = ntohs(request->quantityOfCoils);
241 
242  //The number of discrete inputs must be in range 1 to 2000
243  if(quantity < 1 || quantity > 2000)
244  return ERROR_INVALID_VALUE;
245 
246  //Point to the Modbus response PDU
247  response = modbusServerGetResponsePdu(connection);
248 
249  //Format Read Coils response
250  response->functionCode = request->functionCode;
251  response->byteCount = (quantity + 7) / 8;
252 
253  //If the quantity of coils is not a multiple of eight, the remaining
254  //bits in the final data byte will be padded with zeros
255  if((quantity % 8) != 0)
256  response->coilStatus[response->byteCount - 1] = 0;
257 
258  //Lock access to Modbus table
259  modbusServerLock(connection);
260 
261  //Read the specified number of coils
262  for(i = 0; i < quantity && !error; i++)
263  {
264  //Retrieve the state of the current coil
265  error = modbusServerReadCoil(connection, address + i, &state);
266 
267  //Successful read operation?
268  if(!error)
269  {
270  //The coils in the response message are packed as one coil per bit
271  //of the data field
272  if(state)
273  MODBUS_SET_COIL(response->coilStatus, i);
274  else
275  MODBUS_RESET_COIL(response->coilStatus, i);
276  }
277  }
278 
279  //Unlock access to Modbus table
280  modbusServerUnlock(connection);
281 
282  //Check whether the read operation has failed
283  if(error)
284  return error;
285 
286  //Compute the length of the response PDU
287  length = sizeof(ModbusReadCoilsResp) + response->byteCount;
288 
289  //Debug message
290  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
291  //Dump the contents of the PDU for debugging purpose
292  modbusDumpResponsePdu(response, length);
293 
294  //Format MBAP header
295  return modbusServerFormatMbapHeader(connection, length);
296 }
297 
298 
299 /**
300  * @brief Process Read Discrete Inputs request
301  * @param[in] connection Pointer to the client connection
302  * @param[in] request Pointer to the request PDU
303  * @param[in] length Length of the request PDU, in bytes
304  * @return Error code
305  **/
306 
308  const ModbusReadDiscreteInputsReq *request, size_t length)
309 {
310  error_t error;
311  uint16_t i;
312  uint16_t address;
313  uint16_t quantity;
314  bool_t state;
316 
317  //Initialize status code
318  error = NO_ERROR;
319 
320  //Malformed PDU?
321  if(length < sizeof(ModbusReadDiscreteInputsReq))
322  return ERROR_INVALID_LENGTH;
323 
324  //Get the address of the first coil
325  address = ntohs(request->startingAddr);
326  //Get the number of coils
327  quantity = ntohs(request->quantityOfInputs);
328 
329  //The number of discrete inputs must be in range 1 to 2000
330  if(quantity < 1 || quantity > 2000)
331  return ERROR_INVALID_VALUE;
332 
333  //Point to the Modbus response PDU
334  response = modbusServerGetResponsePdu(connection);
335 
336  //Format Read Discrete Inputs response
337  response->functionCode = request->functionCode;
338  response->byteCount = (quantity + 7) / 8;
339 
340  //If the quantity of coils is not a multiple of eight, the remaining
341  //bits in the final data byte will be padded with zeros
342  if((quantity % 8) != 0)
343  response->inputStatus[response->byteCount - 1] = 0;
344 
345  //Lock access to Modbus table
346  modbusServerLock(connection);
347 
348  //Read the specified number of coils
349  for(i = 0; i < quantity && !error; i++)
350  {
351  //Retrieve the state of the current coil
352  error = modbusServerReadDiscreteInput(connection, address + i, &state);
353 
354  //Successful read operation?
355  if(!error)
356  {
357  //The coils in the response message are packed as one coil per bit
358  //of the data field
359  if(state)
360  MODBUS_SET_COIL(response->inputStatus, i);
361  else
362  MODBUS_RESET_COIL(response->inputStatus, i);
363  }
364  }
365 
366  //Unlock access to Modbus table
367  modbusServerUnlock(connection);
368 
369  //Check whether the read operation has failed
370  if(error)
371  return error;
372 
373  //Compute the length of the response PDU
374  length = sizeof(ModbusReadDiscreteInputsResp) + response->byteCount;
375 
376  //Debug message
377  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
378  //Dump the contents of the PDU for debugging purpose
379  modbusDumpResponsePdu(response, length);
380 
381  //Format MBAP header
382  return modbusServerFormatMbapHeader(connection, length);
383 }
384 
385 
386 /**
387  * @brief Process Read Holding Registers request
388  * @param[in] connection Pointer to the client connection
389  * @param[in] request Pointer to the request PDU
390  * @param[in] length Length of the request PDU, in bytes
391  * @return Error code
392  **/
393 
395  const ModbusReadHoldingRegsReq *request, size_t length)
396 {
397  error_t error;
398  uint16_t i;
399  uint16_t address;
400  uint16_t quantity;
401  uint16_t value;
402  ModbusReadHoldingRegsResp *response;
403 
404  //Initialize status code
405  error = NO_ERROR;
406 
407  //Malformed PDU?
408  if(length < sizeof(ModbusReadHoldingRegsReq))
409  return ERROR_INVALID_LENGTH;
410 
411  //Get the address of the first register
412  address = ntohs(request->startingAddr);
413  //Get the number of registers
414  quantity = ntohs(request->quantityOfRegs);
415 
416  //The number of registers must be in range 1 to 125
417  if(quantity < 1 || quantity > 125)
418  return ERROR_INVALID_VALUE;
419 
420  //Point to the Modbus response PDU
421  response = modbusServerGetResponsePdu(connection);
422 
423  //Format Read Holding Registers response
424  response->functionCode = request->functionCode;
425  response->byteCount = quantity * sizeof(uint16_t);
426 
427  //Lock access to Modbus table
428  modbusServerLock(connection);
429 
430  //Read the specified number of registers
431  for(i = 0; i < quantity && !error; i++)
432  {
433  //Retrieve the value of the current register
434  error = modbusServerReadHoldingReg(connection, address + i, &value);
435  //Convert the value to network byte order
436  response->regValue[i] = htons(value);
437  }
438 
439  //Unlock access to Modbus table
440  modbusServerUnlock(connection);
441 
442  //Check whether the read operation has failed
443  if(error)
444  return error;
445 
446  //Compute the length of the response PDU
447  length = sizeof(ModbusReadHoldingRegsResp) + response->byteCount;
448 
449  //Debug message
450  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
451  //Dump the contents of the PDU for debugging purpose
452  modbusDumpResponsePdu(response, length);
453 
454  //Format MBAP header
455  return modbusServerFormatMbapHeader(connection, length);
456 }
457 
458 
459 /**
460  * @brief Process Read Input Registers request
461  * @param[in] connection Pointer to the client connection
462  * @param[in] request Pointer to the request PDU
463  * @param[in] length Length of the request PDU, in bytes
464  * @return Error code
465  **/
466 
468  const ModbusReadInputRegsReq *request, size_t length)
469 {
470  error_t error;
471  uint16_t i;
472  uint16_t address;
473  uint16_t quantity;
474  uint16_t value;
475  ModbusReadInputRegsResp *response;
476 
477  //Initialize status code
478  error = NO_ERROR;
479 
480  //Malformed PDU?
481  if(length < sizeof(ModbusReadInputRegsReq))
482  return ERROR_INVALID_LENGTH;
483 
484  //Get the address of the first register
485  address = ntohs(request->startingAddr);
486  //Get the number of registers
487  quantity = ntohs(request->quantityOfRegs);
488 
489  //The number of registers must be in range 1 to 125
490  if(quantity < 1 || quantity > 125)
491  return ERROR_INVALID_VALUE;
492 
493  //Point to the Modbus response PDU
494  response = modbusServerGetResponsePdu(connection);
495 
496  //Format Read Input Registers response
497  response->functionCode = request->functionCode;
498  response->byteCount = quantity * sizeof(uint16_t);
499 
500  //Lock access to Modbus table
501  modbusServerLock(connection);
502 
503  //Read the specified number of registers
504  for(i = 0; i < quantity && !error; i++)
505  {
506  //Retrieve the value of the current register
507  error = modbusServerReadInputReg(connection, address + i, &value);
508  //Convert the value to network byte order
509  response->regValue[i] = htons(value);
510  }
511 
512  //Unlock access to Modbus table
513  modbusServerUnlock(connection);
514 
515  //Check whether the read operation has failed
516  if(error)
517  return error;
518 
519  //Compute the length of the response PDU
520  length = sizeof(ModbusReadInputRegsResp) + response->byteCount;
521 
522  //Debug message
523  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
524  //Dump the contents of the PDU for debugging purpose
525  modbusDumpResponsePdu(response, length);
526 
527  //Format MBAP header
528  return modbusServerFormatMbapHeader(connection, length);
529 }
530 
531 
532 /**
533  * @brief Process Write Single Coil request
534  * @param[in] connection Pointer to the client connection
535  * @param[in] request Pointer to the request PDU
536  * @param[in] length Length of the request PDU, in bytes
537  * @return Error code
538  **/
539 
541  const ModbusWriteSingleCoilReq *request, size_t length)
542 {
543  error_t error;
544  uint16_t address;
545  bool_t state;
546  ModbusWriteSingleCoilResp *response;
547 
548  //Malformed PDU?
549  if(length < sizeof(ModbusWriteSingleCoilReq))
550  return ERROR_INVALID_LENGTH;
551 
552  //Get the address of the coil to be forced
553  address = ntohs(request->outputAddr);
554 
555  //Check output value
556  if(ntohs(request->outputValue) == MODBUS_COIL_STATE_ON)
557  {
558  //A value of 0xFF00 requests the output to be ON
559  state = TRUE;
560  }
561  else if(ntohs(request->outputValue) == MODBUS_COIL_STATE_OFF)
562  {
563  //A value of 0x0000 requests the output to be OFF
564  state = FALSE;
565  }
566  else
567  {
568  //Report an error
569  return ERROR_INVALID_VALUE;
570  }
571 
572  //Lock access to Modbus table
573  modbusServerLock(connection);
574  //Force the coil to the desired ON/OFF state
575  error = modbusServerWriteCoil(connection, address, state, TRUE);
576  //Unlock access to Modbus table
577  modbusServerUnlock(connection);
578 
579  //Check whether the write operation has failed
580  if(error)
581  return error;
582 
583  //Point to the Modbus response PDU
584  response = modbusServerGetResponsePdu(connection);
585 
586  //The normal response is an echo of the request
587  response->functionCode = request->functionCode;
588  response->outputAddr = request->outputAddr;
589  response->outputValue = request->outputValue;
590 
591  //Compute the length of the response PDU
593 
594  //Debug message
595  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
596  //Dump the contents of the PDU for debugging purpose
597  modbusDumpResponsePdu(response, length);
598 
599  //Format MBAP header
600  return modbusServerFormatMbapHeader(connection, length);
601 }
602 
603 
604 /**
605  * @brief Process Write Single Register request
606  * @param[in] connection Pointer to the client connection
607  * @param[in] request Pointer to the request PDU
608  * @param[in] length Length of the request PDU, in bytes
609  * @return Error code
610  **/
611 
613  const ModbusWriteSingleRegReq *request, size_t length)
614 {
615  error_t error;
616  uint16_t address;
617  uint16_t value;
618  ModbusWriteSingleRegResp *response;
619 
620  //Malformed PDU?
621  if(length < sizeof(ModbusWriteSingleRegReq))
622  return ERROR_INVALID_LENGTH;
623 
624  //Get the address of the register
625  address = ntohs(request->regAddr);
626  //Get the value of the register
627  value = ntohs(request->regValue);
628 
629  //Lock access to Modbus table
630  modbusServerLock(connection);
631  //Write register value
632  error = modbusServerWriteReg(connection, address, value, TRUE);
633  //Unlock access to Modbus table
634  modbusServerUnlock(connection);
635 
636  //Check whether the write operation has failed
637  if(error)
638  return error;
639 
640  //Point to the Modbus response PDU
641  response = modbusServerGetResponsePdu(connection);
642 
643  //The normal response is an echo of the request
644  response->functionCode = request->functionCode;
645  response->regAddr = request->regAddr;
646  response->regValue = request->regValue;
647 
648  //Compute the length of the response PDU
650 
651  //Debug message
652  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
653  //Dump the contents of the PDU for debugging purpose
654  modbusDumpResponsePdu(response, length);
655 
656  //Format MBAP header
657  return modbusServerFormatMbapHeader(connection, length);
658 }
659 
660 
661 /**
662  * @brief Process Write Multiple Coils request
663  * @param[in] connection Pointer to the client connection
664  * @param[in] request Pointer to the request PDU
665  * @param[in] length Length of the request PDU, in bytes
666  * @return Error code
667  **/
668 
670  const ModbusWriteMultipleCoilsReq *request, size_t length)
671 {
672  error_t error;
673  uint16_t i;
674  uint16_t address;
675  uint16_t quantity;
677 
678  //Initialize status code
679  error = NO_ERROR;
680 
681  //Malformed PDU?
682  if(length < sizeof(ModbusWriteMultipleCoilsReq))
683  return ERROR_INVALID_LENGTH;
684 
685  //Compute the length of the data field
687 
688  //Malformed PDU?
689  if(length < request->byteCount)
690  return ERROR_INVALID_LENGTH;
691 
692  //Get the address of the first coil to be forced
693  address = ntohs(request->startingAddr);
694  //Get the number of coils
695  quantity = ntohs(request->quantityOfOutputs);
696 
697  //The number of discrete inputs must be in range 1 to 2000
698  if(quantity < 1 || quantity > 2000)
699  return ERROR_INVALID_VALUE;
700 
701  //Check byte count field
702  if(request->byteCount != ((quantity + 7) / 8))
703  return ERROR_INVALID_VALUE;
704 
705  //Lock access to Modbus table
706  modbusServerLock(connection);
707 
708  //Consistency check (first phase)
709  for(i = 0; i < quantity && !error; i++)
710  {
711  //Validate coil address
712  error = modbusServerWriteCoil(connection, address + i,
713  MODBUS_TEST_COIL(request->outputValue, i), FALSE);
714  }
715 
716  //Commit changes (second phase)
717  for(i = 0; i < quantity && !error; i++)
718  {
719  //Force the current coil to the desired ON/OFF state
720  error = modbusServerWriteCoil(connection, address + i,
721  MODBUS_TEST_COIL(request->outputValue, i), TRUE);
722  }
723 
724  //Unlock access to Modbus table
725  modbusServerUnlock(connection);
726 
727  //Check whether the write operation has failed
728  if(error)
729  return error;
730 
731  //Point to the Modbus response PDU
732  response = modbusServerGetResponsePdu(connection);
733 
734  //The normal response returns the starting address, and quantity of
735  //coils forced
736  response->functionCode = request->functionCode;
737  response->startingAddr = request->startingAddr;
738  response->quantityOfOutputs = request->quantityOfOutputs;
739 
740  //Compute the length of the response PDU
742 
743  //Debug message
744  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
745  //Dump the contents of the PDU for debugging purpose
746  modbusDumpResponsePdu(response, length);
747 
748  //Format MBAP header
749  return modbusServerFormatMbapHeader(connection, length);
750 }
751 
752 
753 /**
754  * @brief Process Write Multiple Registers request
755  * @param[in] connection Pointer to the client connection
756  * @param[in] request Pointer to the request PDU
757  * @param[in] length Length of the request PDU, in bytes
758  * @return Error code
759  **/
760 
762  const ModbusWriteMultipleRegsReq *request, size_t length)
763 {
764  error_t error;
765  uint16_t i;
766  uint16_t address;
767  uint16_t quantity;
768  ModbusWriteMultipleRegsResp *response;
769 
770  //Initialize status code
771  error = NO_ERROR;
772 
773  //Malformed PDU?
774  if(length < sizeof(ModbusWriteMultipleRegsReq))
775  return ERROR_INVALID_LENGTH;
776 
777  //Compute the length of the data field
779 
780  //Malformed PDU?
781  if(length < request->byteCount)
782  return ERROR_INVALID_LENGTH;
783 
784  //Get the address of the first register to be written
785  address = ntohs(request->startingAddr);
786  //Get the number of registers
787  quantity = ntohs(request->quantityOfRegs);
788 
789  //The number of registers must be in range 1 to 123
790  if(quantity < 1 || quantity > 123)
791  return ERROR_INVALID_VALUE;
792 
793  //Check byte count field
794  if(request->byteCount != (quantity * sizeof(uint16_t)))
795  return ERROR_INVALID_VALUE;
796 
797  //Lock access to Modbus table
798  modbusServerLock(connection);
799 
800  //Consistency check (first phase)
801  for(i = 0; i < quantity && !error; i++)
802  {
803  //Validate register address
804  error = modbusServerWriteReg(connection, address + i,
805  ntohs(request->regValue[i]), FALSE);
806  }
807 
808  //Commit changes (second phase)
809  for(i = 0; i < quantity && !error; i++)
810  {
811  //Write the value of the current register
812  error = modbusServerWriteReg(connection, address + i,
813  ntohs(request->regValue[i]), TRUE);
814  }
815 
816  //Unlock access to Modbus table
817  modbusServerUnlock(connection);
818 
819  //Check whether the write operation has failed
820  if(error)
821  return error;
822 
823  //Point to the Modbus response PDU
824  response = modbusServerGetResponsePdu(connection);
825 
826  //The normal response returns the starting address, and quantity of
827  //registers written
828  response->functionCode = request->functionCode;
829  response->startingAddr = request->startingAddr;
830  response->quantityOfRegs = request->quantityOfRegs;
831 
832  //Compute the length of the response PDU
834 
835  //Debug message
836  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
837  //Dump the contents of the PDU for debugging purpose
838  modbusDumpResponsePdu(response, length);
839 
840  //Format MBAP header
841  return modbusServerFormatMbapHeader(connection, length);
842 }
843 
844 
845 /**
846  * @brief Process Mask Write Register request
847  * @param[in] connection Pointer to the client connection
848  * @param[in] request Pointer to the request PDU
849  * @param[in] length Length of the request PDU, in bytes
850  * @return Error code
851  **/
852 
854  const ModbusMaskWriteRegReq *request, size_t length)
855 {
856  error_t error;
857  uint16_t address;
858  uint16_t andMask;
859  uint16_t orMask;
860  uint16_t value;
861  ModbusMaskWriteRegResp *response;
862 
863  //Malformed PDU?
864  if(length < sizeof(ModbusMaskWriteRegReq))
865  return ERROR_INVALID_LENGTH;
866 
867  //Get the address of the register
868  address = ntohs(request->referenceAddr);
869  //Get the value of the AND mask
870  andMask = ntohs(request->andMask);
871  //Get the value of the OR mask
872  orMask = ntohs(request->orMask);
873 
874  //Lock access to Modbus table
875  modbusServerLock(connection);
876 
877  //Retrieve the value of the register
878  error = modbusServerReadHoldingReg(connection, address, &value);
879 
880  //Check status code
881  if(!error)
882  {
883  //Apply AND mask and OR mask
884  value = (value & andMask) | (orMask & ~andMask);
885  //Write register value
886  error = modbusServerWriteReg(connection, address, value, TRUE);
887  }
888 
889  //Unlock access to Modbus table
890  modbusServerUnlock(connection);
891 
892  //Check whether the write operation has failed
893  if(error)
894  return error;
895 
896  //Point to the Modbus response PDU
897  response = modbusServerGetResponsePdu(connection);
898 
899  //The normal response is an echo of the request
900  response->functionCode = request->functionCode;
901  response->referenceAddr = request->referenceAddr;
902  response->andMask = request->andMask;
903  response->orMask = request->orMask;
904 
905  //Compute the length of the response PDU
906  length = sizeof(ModbusMaskWriteRegResp);
907 
908  //Debug message
909  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
910  //Dump the contents of the PDU for debugging purpose
911  modbusDumpResponsePdu(response, length);
912 
913  //Format MBAP header
914  return modbusServerFormatMbapHeader(connection, length);
915 }
916 
917 
918 /**
919  * @brief Process Read/Write Multiple Registers request
920  * @param[in] connection Pointer to the client connection
921  * @param[in] request Pointer to the request PDU
922  * @param[in] length Length of the request PDU, in bytes
923  * @return Error code
924  **/
925 
927  const ModbusReadWriteMultipleRegsReq *request, size_t length)
928 {
929  error_t error;
930  uint16_t i;
931  uint16_t readAddress;
932  uint16_t readQuantity;
933  uint16_t writeAddress;
934  uint16_t writeQuantity;
935  uint16_t value;
937 
938  //Initialize status code
939  error = NO_ERROR;
940 
941  //Malformed PDU?
943  return ERROR_INVALID_LENGTH;
944 
945  //Compute the length of the data field
947 
948  //Malformed PDU?
949  if(length < request->writeByteCount)
950  return ERROR_INVALID_LENGTH;
951 
952  //Get the address of the first register to be read
953  readAddress = ntohs(request->readStartingAddr);
954  //Get the number of registers to be read
955  readQuantity = ntohs(request->quantityToRead);
956  //Get the address of the first register to be written
957  writeAddress = ntohs(request->writeStartingAddr);
958  //Get the number of registers to be written
959  writeQuantity = ntohs(request->quantityToWrite);
960 
961  //The number of registers to be read must be in range 1 to 125
962  if(readQuantity < 1 || readQuantity > 125)
963  return ERROR_INVALID_VALUE;
964 
965  //The number of registers to be written must be in range 1 to 121
966  if(writeQuantity < 1 || writeQuantity > 121)
967  return ERROR_INVALID_VALUE;
968 
969  //Check byte count field
970  if(request->writeByteCount != (writeQuantity * sizeof(uint16_t)))
971  return ERROR_INVALID_VALUE;
972 
973  //Point to the Modbus response PDU
974  response = modbusServerGetResponsePdu(connection);
975 
976  //Format Read/Write Multiple Registers response
977  response->functionCode = request->functionCode;
978  response->readByteCount = readQuantity * sizeof(uint16_t);
979 
980  //Lock access to Modbus table
981  modbusServerLock(connection);
982 
983  //Read the specified number of registers
984  for(i = 0; i < readQuantity && !error; i++)
985  {
986  //Retrieve the value of the current register
987  error = modbusServerReadHoldingReg(connection, readAddress + i, &value);
988 
989  //Convert the value to network byte order
990  response->readRegValue[i] = htons(value);
991  }
992 
993  //Consistency check (first phase)
994  for(i = 0; i < writeQuantity && !error; i++)
995  {
996  //Validate register address
997  error = modbusServerWriteReg(connection, writeAddress + i,
998  ntohs(request->writeRegValue[i]), FALSE);
999  }
1000 
1001  //Commit changes (second phase)
1002  for(i = 0; i < writeQuantity && !error; i++)
1003  {
1004  //Write the value of the current register
1005  error = modbusServerWriteReg(connection, writeAddress + i,
1006  ntohs(request->writeRegValue[i]), TRUE);
1007  }
1008 
1009  //Unlock access to Modbus table
1010  modbusServerUnlock(connection);
1011 
1012  //Check whether the write operation has failed
1013  if(error)
1014  return error;
1015 
1016  //Compute the length of the response PDU
1017  length = sizeof(ModbusReadWriteMultipleRegsResp) + response->readByteCount;
1018 
1019  //Debug message
1020  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
1021  //Dump the contents of the PDU for debugging purpose
1022  modbusDumpResponsePdu(response, length);
1023 
1024  //Format MBAP header
1025  return modbusServerFormatMbapHeader(connection, length);
1026 }
1027 
1028 
1029 /**
1030  * @brief Format exception response
1031  * @param[in] connection Pointer to the client connection
1032  * @param[in] functionCode Function code of the request
1033  * @param[in] exceptionCode Exception code
1034  * @return Exception code
1035  **/
1036 
1039 {
1040  size_t length;
1041  ModbusExceptionResp *response;
1042 
1043  //Point to the Modbus response PDU
1044  response = modbusServerGetResponsePdu(connection);
1045 
1046  //Format Exception response
1047  response->functionCode = MODBUS_EXCEPTION_MASK | functionCode;
1048  response->exceptionCode = exceptionCode;
1049 
1050  //Compute the length of the response PDU
1051  length = sizeof(ModbusExceptionResp);
1052 
1053  //Debug message
1054  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
1055  //Dump the contents of the PDU for debugging purpose
1056  modbusDumpResponsePdu(response, length);
1057 
1058  //Format MBAP header
1059  return modbusServerFormatMbapHeader(connection, length);
1060 }
1061 
1062 #endif
#define htons(value)
Definition: cpu_endian.h:392
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.
uint8_t length
Definition: dtls_misc.h:149
__start_packed struct @228 ModbusReadWriteMultipleRegsResp
Read/Write Multiple Registers response PDU.
int bool_t
Definition: compiler_port.h:49
uint16_t byteCount
__start_packed struct @222 ModbusWriteMultipleCoilsResp
Write Multiple Coils response PDU.
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.
__start_packed struct @223 ModbusWriteMultipleRegsReq
Write Multiple Registers request PDU.
error_t modbusDumpResponsePdu(const void *pdu, size_t length)
Dump Modbus response PDU for debugging purpose.
Definition: modbus_debug.c:198
#define TRUE
Definition: os_port.h:50
error_t modbusServerWriteCoil(ModbusClientConnection *connection, uint16_t address, bool_t state, bool_t commit)
Set coil state.
__start_packed struct @229 ModbusExceptionResp
Exception 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)
Get discrete input state.
#define MODBUS_SET_COIL(a, n)
Definition: modbus_common.h:58
__start_packed struct @217 ModbusWriteSingleCoilReq
Write Single Coil request PDU.
error_t modbusServerReadInputReg(ModbusClientConnection *connection, uint16_t address, uint16_t *value)
Get input register value.
error_t modbusServerProcessReadHoldingRegsReq(ModbusClientConnection *connection, const ModbusReadHoldingRegsReq *request, size_t length)
Process Read Holding Registers request.
__start_packed struct @210 ModbusReadCoilsResp
Read Coils response PDU.
@ MODBUS_COIL_STATE_ON
__start_packed struct @224 ModbusWriteMultipleRegsResp
Write Multiple Registers response 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 @225 ModbusMaskWriteRegReq
Mask Write Register request PDU.
@ MODBUS_FUNCTION_READ_WRITE_MULTIPLE_REGS
Definition: modbus_common.h:92
error_t
Error codes.
Definition: error.h:42
__start_packed struct @214 ModbusReadHoldingRegsResp
Read Holding Registers response PDU.
ModbusExceptionCode modbusServerTranslateExceptionCode(error_t status)
Translate exception code.
Modbus/TCP server.
__start_packed struct @213 ModbusReadHoldingRegsReq
Read Holding Registers request PDU.
__start_packed struct @209 ModbusReadCoilsReq
Read Coils request PDU.
@ MODBUS_FUNCTION_WRITE_MULTIPLE_REGS
Definition: modbus_common.h:87
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
Helper functions for Modbus/TCP server.
__start_packed struct @219 ModbusWriteSingleRegReq
Write Single Register request PDU.
@ ERROR_INVALID_LENGTH
Definition: error.h:109
__start_packed struct @221 ModbusWriteMultipleCoilsReq
Write Multiple Coils request PDU.
@ MODBUS_FUNCTION_MASK_WRITE_REG
Definition: modbus_common.h:91
#define MODBUS_TEST_COIL(a, n)
Definition: modbus_common.h:62
#define MODBUS_RESET_COIL(a, n)
Definition: modbus_common.h:60
__start_packed struct @227 ModbusReadWriteMultipleRegsReq
Read/Write Multiple Registers request PDU.
#define TRACE_INFO(...)
Definition: debug.h:94
@ MODBUS_FUNCTION_WRITE_SINGLE_REG
Definition: modbus_common.h:81
@ MODBUS_FUNCTION_READ_INPUT_REGS
Definition: modbus_common.h:79
error_t modbusServerProcessRequest(ModbusClientConnection *connection)
Process Modbus request.
__start_packed struct @226 ModbusMaskWriteRegResp
Mask Write Register response PDU.
__start_packed struct @218 ModbusWriteSingleCoilResp
Write Single Coil response PDU.
#define ntohs(value)
Definition: cpu_endian.h:398
@ ERROR_DEVICE_BUSY
Definition: error.h:264
__start_packed struct @211 ModbusReadDiscreteInputsReq
Read Discrete Inputs request PDU.
error_t modbusServerProcessWriteMultipleCoilsReq(ModbusClientConnection *connection, const ModbusWriteMultipleCoilsReq *request, size_t length)
Process Write Multiple Coils request.
__start_packed struct @215 ModbusReadInputRegsReq
Read Holding Input request PDU.
void * modbusServerGetRequestPdu(ModbusClientConnection *connection, size_t *length)
Retrieve request PDU.
uint16_t orMask
@ ERROR_INVALID_VALUE
Definition: error.h:114
Data logging functions for debugging purpose (Modbus/TCP)
__start_packed struct @220 ModbusWriteSingleRegResp
Write Single Register response PDU.
error_t modbusServerWriteReg(ModbusClientConnection *connection, uint16_t address, uint16_t value, bool_t commit)
Set register value.
@ ERROR_READ_FAILED
Definition: error.h:220
error_t modbusServerProcessReadWriteMultipleRegsReq(ModbusClientConnection *connection, const ModbusReadWriteMultipleRegsReq *request, size_t length)
Process Read/Write Multiple Registers request.
@ ERROR_WRITE_FAILED
Definition: error.h:219
ModbusFunctionCode
Modbus functions codes.
Definition: modbus_common.h:74
error_t modbusServerReadHoldingReg(ModbusClientConnection *connection, uint16_t address, uint16_t *value)
Get holding register value.
__start_packed struct @212 ModbusReadDiscreteInputsResp
Read Discrete Inputs response 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)
Get coil state.
Modbus PDU processing.
Ipv6Addr address
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.
uint8_t value[]
Definition: dtls_misc.h:150
#define PRIuSIZE
Definition: compiler_port.h:78
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
__start_packed struct @216 ModbusReadInputRegsResp
Read Holding Input response PDU.
@ MODBUS_FUNCTION_READ_COILS
Definition: modbus_common.h:76
@ NO_ERROR
Success.
Definition: error.h:44
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:263