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  * Copyright (C) 2010-2018 Oryx Embedded SARL. All rights reserved.
8  *
9  * This file is part of CycloneTCP Open.
10  *
11  * This program is free software; you can redistribute it and/or
12  * modify it under the terms of the GNU General Public License
13  * as published by the Free Software Foundation; either version 2
14  * of the License, or (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU General Public License for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program; if not, write to the Free Software Foundation,
23  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
24  *
25  * @author Oryx Embedded SARL (www.oryx-embedded.com)
26  * @version 1.9.0
27  **/
28 
29 //Switch to the appropriate trace level
30 #define TRACE_LEVEL MODBUS_TRACE_LEVEL
31 
32 //Dependencies
33 #include "modbus/modbus_server.h"
36 #include "modbus/modbus_debug.h"
37 #include "debug.h"
38 
39 //Check TCP/IP stack configuration
40 #if (MODBUS_SERVER_SUPPORT == ENABLED)
41 
42 
43 /**
44  * @brief Process Modbus request
45  * @param[in] connection Pointer to the client connection
46  * @return Error code
47  **/
48 
50 {
51  error_t error;
52  size_t requestLen;
53  size_t responseLen;
54  void *request;
55  void *response;
58  ModbusServerContext *context;
59 
60  //Point to the Modbus server context
61  context = connection->context;
62  //Point to the Modbus request PDU
63  request = modbusServerGetRequestPdu(connection, &requestLen);
64 
65  //Malformed request?
66  if(requestLen == 0)
67  return ERROR_INVALID_LENGTH;
68 
69  //Debug message
70  TRACE_INFO("Modbus Server: Request PDU received (%" PRIuSIZE " bytes)...\r\n",
71  requestLen);
72 
73  //Dump the contents of the PDU for debugging purpose
74  modbusDumpRequestPdu(request, requestLen);
75 
76  //Any registered callback?
77  if(context->settings.processPduCallback != NULL)
78  {
79  //Point to the Modbus response PDU
80  response = modbusServerGetResponsePdu(connection);
81  //Initialize response length
82  responseLen = 0;
83 
84  //Process request PDU
85  error = context->settings.processPduCallback(request, requestLen,
86  response, &responseLen);
87 
88  //Check status code
89  if(!error)
90  {
91  //Valid response PDU?
92  if(responseLen > 0)
93  {
94  //Debug message
95  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n",
96  responseLen);
97 
98  //Dump the contents of the PDU for debugging purpose
99  modbusDumpResponsePdu(response, responseLen);
100 
101  //Format MBAP header
102  error = modbusServerFormatMbapHeader(connection, responseLen);
103  }
104  }
105  }
106  else
107  {
108  //Keep processing
110  }
111 
112  //Unknown function code?
113  if(error == ERROR_INVALID_FUNCTION_CODE)
114  {
115  //Retrieve function code
116  functionCode = (ModbusFunctionCode) *((uint8_t *) request);
117 
118  //Check function code
119  switch(functionCode)
120  {
121  //Read Coils request?
123  //Process Modbus PDU
124  error = modbusServerProcessReadCoilsReq(connection,
125  request, requestLen);
126  break;
127  //Format Read Discrete Inputs request?
129  //Process Modbus PDU
130  error = modbusServerProcessReadDiscreteInputsReq(connection,
131  request, requestLen);
132  break;
133  //Read Holding Registers request?
135  //Process Modbus PDU
136  error = modbusServerProcessReadHoldingRegsReq(connection,
137  request, requestLen);
138  break;
139  //Read Input Registers request?
141  //Process Modbus PDU
142  error = modbusServerProcessReadInputRegsReq(connection,
143  request, requestLen);
144  break;
145  //Write Single Coil request?
147  //Process Modbus PDU
148  error = modbusServerProcessWriteSingleCoilReq(connection,
149  request, requestLen);
150  break;
151  //Write Single Register request?
153  //Process Modbus PDU
154  error = modbusServerProcessWriteSingleRegReq(connection,
155  request, requestLen);
156  break;
157  //Write Multiple Coils request?
159  //Process Modbus PDU
160  error = modbusServerProcessWriteMultipleCoilsReq(connection,
161  request, requestLen);
162  break;
163  //Write Multiple Registers request?
165  //Process Modbus PDU
166  error = modbusServerProcessWriteMultipleRegsReq(connection,
167  request, requestLen);
168  break;
169  //Mask Write Register request?
171  //Process Modbus PDU
172  error = modbusServerProcessMaskWriteRegReq(connection,
173  request, requestLen);
174  break;
175  //Read/Write Multiple Registers request?
177  //Process Modbus PDU
179  request, requestLen);
180  break;
181  //Illegal function code?
182  default:
183  //Report an error
185  break;
186  }
187  }
188 
189  //Any exception?
190  if(error == ERROR_INVALID_FUNCTION_CODE ||
191  error == ERROR_INVALID_ADDRESS ||
192  error == ERROR_INVALID_VALUE ||
193  error == ERROR_WRITE_FAILED ||
194  error == ERROR_READ_FAILED ||
195  error == ERROR_DEVICE_BUSY)
196  {
197  //Retrieve exception code
199 
200  //Send an exception response to the Modbus/TCP client
201  error = modbusServerFormatExceptionResp(connection, functionCode,
202  exceptionCode);
203  }
204 
205  //Return status code
206  return error;
207 }
208 
209 
210 /**
211  * @brief Process Read Coils request
212  * @param[in] connection Pointer to the client connection
213  * @param[in] request Pointer to the request PDU
214  * @param[in] length Length of the request PDU, in bytes
215  * @return Error code
216  **/
217 
219  const ModbusReadCoilsReq *request, size_t length)
220 {
221  error_t error;
222  uint16_t i;
223  uint16_t quantity;
224  uint16_t address;
225  bool_t state;
226  ModbusReadCoilsResp *response;
227 
228  //Initialize status code
229  error = NO_ERROR;
230 
231  //Malformed PDU?
232  if(length < sizeof(ModbusReadCoilsReq))
233  return ERROR_INVALID_LENGTH;
234 
235  //Get the address of the first coil
236  address = ntohs(request->startingAddr);
237  //Get the number of coils
238  quantity = ntohs(request->quantityOfCoils);
239 
240  //The number of discrete inputs must be in range 1 to 2000
241  if(quantity < 1 || quantity > 2000)
242  return ERROR_INVALID_VALUE;
243 
244  //Point to the Modbus response PDU
245  response = modbusServerGetResponsePdu(connection);
246 
247  //Format Read Coils response
248  response->functionCode = request->functionCode;
249  response->byteCount = (quantity + 7) / 8;
250 
251  //If the quantity of coils is not a multiple of eight, the remaining
252  //bits in the final data byte will be padded with zeros
253  if((quantity % 8) != 0)
254  response->coilStatus[response->byteCount - 1] = 0;
255 
256  //Lock access to Modbus table
257  modbusServerLock(connection->context);
258 
259  //Read the specified number of coils
260  for(i = 0; i < quantity && !error; i++)
261  {
262  //Retrieve the state of the current coil
263  error = modbusServerReadCoil(connection->context, address + i,
264  &state);
265 
266  //Successful read operation?
267  if(!error)
268  {
269  //The coils in the response message are packed as one coil per bit
270  //of the data field
271  if(state)
272  MODBUS_SET_COIL(response->coilStatus, i);
273  else
274  MODBUS_RESET_COIL(response->coilStatus, i);
275  }
276  }
277 
278  //Unlock access to Modbus table
279  modbusServerUnlock(connection->context);
280 
281  //Check whether the read operation has failed
282  if(error)
283  return error;
284 
285  //Compute the length of the response PDU
286  length = sizeof(ModbusReadCoilsResp) + response->byteCount;
287 
288  //Debug message
289  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
290  //Dump the contents of the PDU for debugging purpose
291  modbusDumpResponsePdu(response, length);
292 
293  //Format MBAP header
294  return modbusServerFormatMbapHeader(connection, length);
295 }
296 
297 
298 /**
299  * @brief Process Read Discrete Inputs request
300  * @param[in] connection Pointer to the client connection
301  * @param[in] request Pointer to the request PDU
302  * @param[in] length Length of the request PDU, in bytes
303  * @return Error code
304  **/
305 
307  const ModbusReadDiscreteInputsReq *request, size_t length)
308 {
309  error_t error;
310  uint16_t i;
311  uint16_t address;
312  uint16_t quantity;
313  bool_t state;
315 
316  //Initialize status code
317  error = NO_ERROR;
318 
319  //Malformed PDU?
320  if(length < sizeof(ModbusReadDiscreteInputsReq))
321  return ERROR_INVALID_LENGTH;
322 
323  //Get the address of the first coil
324  address = ntohs(request->startingAddr);
325  //Get the number of coils
326  quantity = ntohs(request->quantityOfInputs);
327 
328  //The number of discrete inputs must be in range 1 to 2000
329  if(quantity < 1 || quantity > 2000)
330  return ERROR_INVALID_VALUE;
331 
332  //Point to the Modbus response PDU
333  response = modbusServerGetResponsePdu(connection);
334 
335  //Format Read Discrete Inputs response
336  response->functionCode = request->functionCode;
337  response->byteCount = (quantity + 7) / 8;
338 
339  //If the quantity of coils is not a multiple of eight, the remaining
340  //bits in the final data byte will be padded with zeros
341  if((quantity % 8) != 0)
342  response->inputStatus[response->byteCount - 1] = 0;
343 
344  //Lock access to Modbus table
345  modbusServerLock(connection->context);
346 
347  //Read the specified number of coils
348  for(i = 0; i < quantity && !error; i++)
349  {
350  //Retrieve the state of the current coil
351  error = modbusServerReadCoil(connection->context, address + i,
352  &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->context);
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->context);
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 = modbusServerReadReg(connection->context, 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->context);
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->context);
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 = modbusServerReadReg(connection->context, 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->context);
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->context);
574  //Force the coil to the desired ON/OFF state
575  error = modbusServerWriteCoil(connection->context, address, state, TRUE);
576  //Unlock access to Modbus table
577  modbusServerUnlock(connection->context);
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->context);
631  //Write register value
632  error = modbusServerWriteReg(connection->context, address, value, TRUE);
633  //Unlock access to Modbus table
634  modbusServerUnlock(connection->context);
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->context);
707 
708  //Consistency check (first phase)
709  for(i = 0; i < quantity && !error; i++)
710  {
711  //Validate coil address
712  error = modbusServerWriteCoil(connection->context, 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->context, address + i,
721  MODBUS_TEST_COIL(request->outputValue, i), TRUE);
722  }
723 
724  //Unlock access to Modbus table
725  modbusServerUnlock(connection->context);
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->context);
799 
800  //Consistency check (first phase)
801  for(i = 0; i < quantity && !error; i++)
802  {
803  //Validate register address
804  error = modbusServerWriteReg(connection->context, 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->context, address + i,
813  ntohs(request->regValue[i]), TRUE);
814  }
815 
816  //Unlock access to Modbus table
817  modbusServerUnlock(connection->context);
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->context);
876 
877  //Retrieve the value of the register
878  error = modbusServerReadReg(connection->context, 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->context, address, value, TRUE);
887  }
888 
889  //Unlock access to Modbus table
890  modbusServerUnlock(connection->context);
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->context);
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 = modbusServerReadReg(connection->context, readAddress + i,
988  &value);
989 
990  //Convert the value to network byte order
991  response->readRegValue[i] = htons(value);
992  }
993 
994  //Consistency check (first phase)
995  for(i = 0; i < writeQuantity && !error; i++)
996  {
997  //Validate register address
998  error = modbusServerWriteReg(connection->context, writeAddress + i,
999  ntohs(request->writeRegValue[i]), FALSE);
1000  }
1001 
1002  //Commit changes (second phase)
1003  for(i = 0; i < writeQuantity && !error; i++)
1004  {
1005  //Write the value of the current register
1006  error = modbusServerWriteReg(connection->context, writeAddress + i,
1007  ntohs(request->writeRegValue[i]), TRUE);
1008  }
1009 
1010  //Unlock access to Modbus table
1011  modbusServerUnlock(connection->context);
1012 
1013  //Check whether the write operation has failed
1014  if(error)
1015  return error;
1016 
1017  //Compute the length of the response PDU
1018  length = sizeof(ModbusReadWriteMultipleRegsResp) + response->readByteCount;
1019 
1020  //Debug message
1021  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
1022  //Dump the contents of the PDU for debugging purpose
1023  modbusDumpResponsePdu(response, length);
1024 
1025  //Format MBAP header
1026  return modbusServerFormatMbapHeader(connection, length);
1027 }
1028 
1029 
1030 /**
1031  * @brief Format exception response
1032  * @param[in] connection Pointer to the client connection
1033  * @param[in] functionCode Function code of the request
1034  * @param[in] exceptionCode Exception code
1035  * @return Exception code
1036  **/
1037 
1040 {
1041  size_t length;
1042  ModbusExceptionResp *response;
1043 
1044  //Point to the Modbus response PDU
1045  response = modbusServerGetResponsePdu(connection);
1046 
1047  //Format Exception response
1048  response->functionCode = MODBUS_EXCEPTION_MASK | functionCode;
1049  response->exceptionCode = exceptionCode;
1050 
1051  //Compute the length of the response PDU
1052  length = sizeof(ModbusExceptionResp);
1053 
1054  //Debug message
1055  TRACE_INFO("Modbus Server: Sending Response PDU (%" PRIuSIZE " bytes)...\r\n", length);
1056  //Dump the contents of the PDU for debugging purpose
1057  modbusDumpResponsePdu(response, length);
1058 
1059  //Format MBAP header
1060  return modbusServerFormatMbapHeader(connection, length);
1061 }
1062 
1063 #endif
__start_packed struct @214 ModbusReadDiscreteInputsReq
Read Discrete Inputs request PDU.
Modbus/TCP server.
__start_packed struct @228 ModbusMaskWriteRegReq
Mask Write Register request PDU.
__start_packed struct @221 ModbusWriteSingleCoilResp
Write Single Coil response PDU.
__start_packed struct @219 ModbusReadInputRegsResp
Read Holding Input response PDU.
error_t modbusServerProcessMaskWriteRegReq(ModbusClientConnection *connection, const ModbusMaskWriteRegReq *request, size_t length)
Process Mask Write Register request.
error_t modbusDumpResponsePdu(const void *pdu, size_t length)
Dump Modbus response PDU for debugging purpose.
Definition: modbus_debug.c:196
Debugging facilities.
__start_packed struct @218 ModbusReadInputRegsReq
Read Holding Input request PDU.
error_t modbusServerProcessReadDiscreteInputsReq(ModbusClientConnection *connection, const ModbusReadDiscreteInputsReq *request, size_t length)
Process Read Discrete Inputs request.
__start_packed struct @231 ModbusReadWriteMultipleRegsResp
Read/Write Multiple Registers response PDU.
void * modbusServerGetRequestPdu(ModbusClientConnection *connection, size_t *length)
Retrieve request PDU.
uint16_t orMask
ModbusServerContext * context
Modbus/TCP server context.
error_t modbusServerProcessReadCoilsReq(ModbusClientConnection *connection, const ModbusReadCoilsReq *request, size_t length)
Process Read Coils request.
error_t modbusServerReadCoil(ModbusServerContext *context, uint16_t address, bool_t *state)
Get coil state.
void modbusServerLock(ModbusServerContext *context)
Lock Modbus table.
Modbus PDU processing.
#define htons(value)
Definition: cpu_endian.h:390
uint8_t writeByteCount
__start_packed struct @227 ModbusWriteMultipleRegsResp
Write Multiple Registers response PDU.
void * modbusServerGetResponsePdu(ModbusClientConnection *connection)
Retrieve response PDU.
__start_packed struct @217 ModbusReadHoldingRegsResp
Read Holding Registers response PDU.
__start_packed struct @230 ModbusReadWriteMultipleRegsReq
Read/Write Multiple Registers request PDU.
error_t modbusServerProcessReadInputRegsReq(ModbusClientConnection *connection, const ModbusReadInputRegsReq *request, size_t length)
Process Read Input Registers request.
#define MODBUS_SET_COIL(a, n)
Definition: modbus_common.h:53
#define TRUE
Definition: os_port.h:48
error_t modbusServerWriteReg(ModbusServerContext *context, uint16_t address, uint16_t value, bool_t commit)
Set register value.
error_t modbusServerProcessReadHoldingRegsReq(ModbusClientConnection *connection, const ModbusReadHoldingRegsReq *request, size_t length)
Process Read Holding Registers request.
#define ntohs(value)
Definition: cpu_endian.h:396
ModbusExceptionCode
Modbus exception codes.
Definition: modbus_common.h:97
__start_packed struct @215 ModbusReadDiscreteInputsResp
Read Discrete Inputs response PDU.
void modbusServerUnlock(ModbusServerContext *context)
Unlock Modbus table.
__start_packed struct @220 ModbusWriteSingleCoilReq
Write Single Coil request PDU.
ModbusExceptionCode modbusServerTranslateExceptionCode(error_t status)
Translate exception code.
error_t modbusServerFormatMbapHeader(ModbusClientConnection *connection, size_t length)
Format response MBAP header.
Modbus/TCP client connection.
__start_packed struct @229 ModbusMaskWriteRegResp
Mask Write Register response PDU.
Helper functions for Modbus/TCP server.
error_t modbusServerProcessRequest(ModbusClientConnection *connection)
Process Modbus request.
__start_packed struct @216 ModbusReadHoldingRegsReq
Read Holding Registers request PDU.
#define MODBUS_TEST_COIL(a, n)
Definition: modbus_common.h:57
error_t modbusServerProcessReadWriteMultipleRegsReq(ModbusClientConnection *connection, const ModbusReadWriteMultipleRegsReq *request, size_t length)
Process Read/Write Multiple Registers request.
Data logging functions for debugging purpose (Modbus/TCP)
uint8_t functionCode
ModbusFunctionCode
Modbus functions codes.
Definition: modbus_common.h:69
__start_packed struct @232 ModbusExceptionResp
Exception response PDU.
#define TRACE_INFO(...)
Definition: debug.h:86
__start_packed struct @223 ModbusWriteSingleRegResp
Write Single Register response PDU.
Success.
Definition: error.h:42
__start_packed struct @222 ModbusWriteSingleRegReq
Write Single Register request PDU.
Ipv6Addr address
#define ModbusServerContext
Definition: modbus_server.h:78
error_t
Error codes.
Definition: error.h:40
#define PRIuSIZE
Definition: compiler_port.h:72
__start_packed struct @225 ModbusWriteMultipleCoilsResp
Write Multiple Coils response PDU.
__start_packed struct @224 ModbusWriteMultipleCoilsReq
Write Multiple Coils request PDU.
uint8_t value[]
Definition: dtls_misc.h:141
error_t modbusServerWriteCoil(ModbusServerContext *context, uint16_t address, bool_t state, bool_t commit)
Set coil state.
uint8_t exceptionCode
__start_packed struct @226 ModbusWriteMultipleRegsReq
Write Multiple Registers request PDU.
uint16_t andMask
error_t modbusServerFormatExceptionResp(ModbusClientConnection *connection, ModbusFunctionCode functionCode, ModbusExceptionCode exceptionCode)
Format exception response.
error_t modbusServerProcessWriteMultipleCoilsReq(ModbusClientConnection *connection, const ModbusWriteMultipleCoilsReq *request, size_t length)
Process Write Multiple Coils request.
uint16_t byteCount
__start_packed struct @213 ModbusReadCoilsResp
Read Coils response PDU.
error_t modbusServerProcessWriteMultipleRegsReq(ModbusClientConnection *connection, const ModbusWriteMultipleRegsReq *request, size_t length)
Process Write Multiple Registers request.
uint8_t length
Definition: dtls_misc.h:140
error_t modbusServerProcessWriteSingleCoilReq(ModbusClientConnection *connection, const ModbusWriteSingleCoilReq *request, size_t length)
Process Write Single Coil request.
error_t modbusServerReadReg(ModbusServerContext *context, uint16_t address, uint16_t *value)
Get register value.
#define FALSE
Definition: os_port.h:44
int bool_t
Definition: compiler_port.h:47
#define MODBUS_RESET_COIL(a, n)
Definition: modbus_common.h:55
#define MODBUS_EXCEPTION_MASK
Definition: modbus_common.h:50
error_t modbusServerProcessWriteSingleRegReq(ModbusClientConnection *connection, const ModbusWriteSingleRegReq *request, size_t length)
Process Write Single Register request.
__start_packed struct @212 ModbusReadCoilsReq
Read Coils request PDU.
error_t modbusDumpRequestPdu(const void *pdu, size_t length)
Dump Modbus request PDU for debugging purpose.
Definition: modbus_debug.c:97