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