Add Modbus master logic. From Darcy Gong

5 files changed, 1424 insertions, 0 deletions

diff --git a/apps/modbus/functions/Make.defs b/apps/modbus/functions/Make.defs
index ba4a243c1..d7888e8d7 100644
--- a/apps/modbus/functions/Make.defs
+++ b/apps/modbus/functions/Make.defs
@@ -35,6 +35,7 @@
 
 CSRCS += mbfunccoils.c mbfuncdiag.c mbfuncdisc.c mbfuncholding.c
 CSRCS += mbfuncinput.c mbfuncother.c mbutils.c
+CSRCS += mbfunccoils_m.c mbfuncdisc_m.c mbfuncholding_m.c mbfuncinput_m.c
 
 DEPPATH += --dep-path functions
 VPATH += :functions
diff --git a/apps/modbus/functions/mbfunccoils_m.c b/apps/modbus/functions/mbfunccoils_m.c
new file mode 100644
index 000000000..e34a940af
--- /dev/null
+++ b/apps/modbus/functions/mbfunccoils_m.c
@@ -0,0 +1,472 @@
+/****************************************************************************
+ * apps/modbus/functions/mbfunccoils_m.c
+ * 
+ * FreeModbus Libary: A portable Modbus implementation for Modbus ASCII/RTU.
+ * Copyright (C) 2013 Armink <armink.ztl@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "port.h"
+
+#include <apps/modbus/mb.h>
+#include <apps/modbus/mb_m.h>
+#include <apps/modbus/mbframe.h>
+#include <apps/modbus/mbproto.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define MB_PDU_REQ_READ_ADDR_OFF            (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_REQ_READ_COILCNT_OFF         (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_REQ_READ_SIZE                (4)
+#define MB_PDU_FUNC_READ_COILCNT_OFF        (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_FUNC_READ_VALUES_OFF         (MB_PDU_DATA_OFF + 1)
+#define MB_PDU_FUNC_READ_SIZE_MIN           (1)
+
+#define MB_PDU_REQ_WRITE_ADDR_OFF           (MB_PDU_DATA_OFF)
+#define MB_PDU_REQ_WRITE_VALUE_OFF          (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_REQ_WRITE_SIZE               (4)
+#define MB_PDU_FUNC_WRITE_ADDR_OFF          (MB_PDU_DATA_OFF)
+#define MB_PDU_FUNC_WRITE_VALUE_OFF         (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_FUNC_WRITE_SIZE              (4)
+
+#define MB_PDU_REQ_WRITE_MUL_ADDR_OFF       (MB_PDU_DATA_OFF)
+#define MB_PDU_REQ_WRITE_MUL_COILCNT_OFF    (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_REQ_WRITE_MUL_BYTECNT_OFF    (MB_PDU_DATA_OFF + 4)
+#define MB_PDU_REQ_WRITE_MUL_VALUES_OFF     (MB_PDU_DATA_OFF + 5)
+#define MB_PDU_REQ_WRITE_MUL_SIZE_MIN       (5)
+#define MB_PDU_REQ_WRITE_MUL_COILCNT_MAX    (0x07B0)
+#define MB_PDU_FUNC_WRITE_MUL_ADDR_OFF      (MB_PDU_DATA_OFF)
+#define MB_PDU_FUNC_WRITE_MUL_COILCNT_OFF   (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_FUNC_WRITE_MUL_SIZE          (5)
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+eMBException prveMBError2Exception(eMBErrorCode eErrorCode);
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+#if MB_MASTER_RTU_ENABLED > 0 || MB_MASTER_ASCII_ENABLED > 0
+
+/****************************************************************************
+ * Description:
+ *   This function will request read coil.
+ *
+ * Input Parameters:
+ *   ucSndAddr salve address
+ *   usCoilAddr coil start address
+ *   usNCoils coil total number
+ *   lTimeOut timeout (-1 will waiting forever)
+ *
+ * Returned Value:
+ *   error code
+ *
+ ****************************************************************************/
+
+#if MB_FUNC_READ_COILS_ENABLED > 0
+eMBMasterReqErrCode eMBMasterReqReadCoils(uint8_t ucSndAddr,
+                                          uint16_t usCoilAddr,
+                                          uint16_t usNCoils,
+                                          uint32_t lTimeOut)
+{
+  uint8_t *ucMBFrame;
+  eMBMasterReqErrCode eErrStatus = MB_MRE_NO_ERR;
+
+  if (ucSndAddr > MB_MASTER_TOTAL_SLAVE_NUM)
+    {
+      eErrStatus = MB_MRE_ILL_ARG;
+    }
+  else if (xMBMasterRunResTake(lTimeOut) == false)
+    {
+      eErrStatus = MB_MRE_MASTER_BUSY;
+    }
+  else
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      vMBMasterSetDestAddress(ucSndAddr);
+      ucMBFrame[MB_PDU_FUNC_OFF] = MB_FUNC_READ_COILS;
+      ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF] = usCoilAddr >> 8;
+      ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF + 1] = usCoilAddr;
+      ucMBFrame[MB_PDU_REQ_READ_COILCNT_OFF] = usNCoils >> 8;
+      ucMBFrame[MB_PDU_REQ_READ_COILCNT_OFF + 1] = usNCoils;
+      vMBMasterSetPDUSndLength(MB_PDU_SIZE_MIN + MB_PDU_REQ_READ_SIZE);
+      (void)xMBMasterPortEventPost(EV_MASTER_FRAME_SENT);
+      eErrStatus = eMBMasterWaitRequestFinish();
+    }
+
+  return eErrStatus;
+}
+
+eMBException eMBMasterFuncReadCoils(uint8_t *pucFrame, uint16_t *usLen)
+{
+  uint8_t *ucMBFrame;
+  uint16_t usRegAddress;
+  uint16_t usCoilCount;
+  uint8_t ucByteCount;
+
+  eMBException eStatus = MB_EX_NONE;
+  eMBErrorCode eRegStatus;
+
+  /* If this request is broadcast, and it's read mode. This request don't need
+   * execute.
+   */
+
+  if (xMBMasterRequestIsBroadcast())
+    {
+      eStatus = MB_EX_NONE;
+    }
+  else if (*usLen >= MB_PDU_SIZE_MIN + MB_PDU_FUNC_READ_SIZE_MIN)
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      usRegAddress = (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF] << 8);
+      usRegAddress |= (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF + 1]);
+      usRegAddress++;
+
+      usCoilCount = (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_COILCNT_OFF] << 8);
+      usCoilCount |= (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_COILCNT_OFF + 1]);
+
+      /* Test if the quantity of coils is a multiple of 8. If not last byte is
+       * only partially field with unused coils set to zero.
+       */
+
+      if ((usCoilCount & 0x0007) != 0)
+        {
+          ucByteCount = (uint8_t) (usCoilCount / 8 + 1);
+        }
+      else
+        {
+          ucByteCount = (uint8_t) (usCoilCount / 8);
+        }
+
+      /* Check if the number of registers to read is valid. If not return
+       * Modbus illegal data value exception.
+       */
+
+      if ((usCoilCount >= 1) &&
+          (ucByteCount == pucFrame[MB_PDU_FUNC_READ_COILCNT_OFF]))
+        {
+          /* Make callback to fill the buffer. */
+
+          eRegStatus =
+            eMBMasterRegCoilsCB(&pucFrame[MB_PDU_FUNC_READ_VALUES_OFF],
+                                usRegAddress, usCoilCount, MB_REG_READ);
+
+          /* If an error occurred convert it into a Modbus exception. */
+
+          if (eRegStatus != MB_ENOERR)
+            {
+              eStatus = prveMBError2Exception(eRegStatus);
+            }
+        }
+      else
+        {
+          eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+        }
+    }
+  else
+    {
+      /* Can't be a valid read coil register request because the length is
+       * incorrect.
+       */
+
+      eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+    }
+
+  return eStatus;
+}
+#endif
+
+/****************************************************************************
+ * Description:
+ *   This function will request write one coil.
+ *
+ * Input Parameters:
+ *   ucSndAddr salve address
+ *   usCoilAddr coil start address
+ *   usCoilData data to be written
+ *   lTimeOut timeout (-1 will waiting forever)
+ *
+ * Returned Value:
+ *   error code
+ *
+ * See eMBMasterReqWriteMultipleCoils
+ *
+ ****************************************************************************/
+
+#if MB_FUNC_WRITE_COIL_ENABLED > 0
+eMBMasterReqErrCode eMBMasterReqWriteCoil(uint8_t ucSndAddr,
+                                          uint16_t usCoilAddr,
+                                          uint16_t usCoilData,
+                                          uint32_t lTimeOut)
+{
+  uint8_t *ucMBFrame;
+  eMBMasterReqErrCode eErrStatus = MB_MRE_NO_ERR;
+
+  if (ucSndAddr > MB_MASTER_TOTAL_SLAVE_NUM)
+    {
+      eErrStatus = MB_MRE_ILL_ARG;
+    }
+  else if ((usCoilData != 0xFF00) && (usCoilData != 0x0000))
+    {
+      eErrStatus = MB_MRE_ILL_ARG;
+    }
+  else if (xMBMasterRunResTake(lTimeOut) == false)
+    {
+      eErrStatus = MB_MRE_MASTER_BUSY;
+    }
+  else
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      vMBMasterSetDestAddress(ucSndAddr);
+      ucMBFrame[MB_PDU_FUNC_OFF] = MB_FUNC_WRITE_SINGLE_COIL;
+      ucMBFrame[MB_PDU_REQ_WRITE_ADDR_OFF] = usCoilAddr >> 8;
+      ucMBFrame[MB_PDU_REQ_WRITE_ADDR_OFF + 1] = usCoilAddr;
+      ucMBFrame[MB_PDU_REQ_WRITE_VALUE_OFF] = usCoilData >> 8;
+      ucMBFrame[MB_PDU_REQ_WRITE_VALUE_OFF + 1] = usCoilData;
+      vMBMasterSetPDUSndLength(MB_PDU_SIZE_MIN + MB_PDU_REQ_WRITE_SIZE);
+      (void)xMBMasterPortEventPost(EV_MASTER_FRAME_SENT);
+      eErrStatus = eMBMasterWaitRequestFinish();
+    }
+
+  return eErrStatus;
+}
+
+eMBException eMBMasterFuncWriteCoil(uint8_t *pucFrame, uint16_t *usLen)
+{
+  uint16_t usRegAddress;
+  uint8_t ucBuf[2];
+
+  eMBException eStatus = MB_EX_NONE;
+  eMBErrorCode eRegStatus;
+
+  if (*usLen == (MB_PDU_FUNC_WRITE_SIZE + MB_PDU_SIZE_MIN))
+    {
+      usRegAddress = (uint16_t) (pucFrame[MB_PDU_FUNC_WRITE_ADDR_OFF] << 8);
+      usRegAddress |= (uint16_t) (pucFrame[MB_PDU_FUNC_WRITE_ADDR_OFF + 1]);
+      usRegAddress++;
+
+      if ((pucFrame[MB_PDU_FUNC_WRITE_VALUE_OFF + 1] == 0x00) &&
+          ((pucFrame[MB_PDU_FUNC_WRITE_VALUE_OFF] == 0xFF) ||
+           (pucFrame[MB_PDU_FUNC_WRITE_VALUE_OFF] == 0x00)))
+        {
+          ucBuf[1] = 0;
+          if (pucFrame[MB_PDU_FUNC_WRITE_VALUE_OFF] == 0xFF)
+            {
+              ucBuf[0] = 1;
+            }
+          else
+            {
+              ucBuf[0] = 0;
+            }
+
+          eRegStatus = eMBMasterRegCoilsCB(&ucBuf[0], usRegAddress, 1,
+                                           MB_REG_WRITE);
+
+          /* If an error occured convert it into a Modbus exception. */
+
+          if (eRegStatus != MB_ENOERR)
+            {
+              eStatus = prveMBError2Exception(eRegStatus);
+            }
+        }
+      else
+        {
+          eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+        }
+    }
+  else
+    {
+      /* Can't be a valid write coil register request because the length is
+       * incorrect.
+       */
+
+      eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+    }
+
+  return eStatus;
+}
+#endif
+
+/****************************************************************************
+ * Description:
+ *   This function will request write multiple coils.
+ *
+ * Input Parameters:
+ *   ucSndAddr salve address
+ *   usCoilAddr coil start address
+ *   usNCoils coil total number
+ *   usCoilData data to be written
+ *   lTimeOut timeout (-1 will waiting forever)
+ *
+ * Returned Value:
+ *   error code
+ *
+ * See eMBMasterReqWriteCoil
+ *
+ ****************************************************************************/
+
+#if MB_FUNC_WRITE_MULTIPLE_COILS_ENABLED > 0
+eMBMasterReqErrCode eMBMasterReqWriteMultipleCoils(uint8_t ucSndAddr,
+                                                   uint16_t usCoilAddr,
+                                                   uint16_t usNCoils,
+                                                   uint8_t *pucDataBuffer,
+                                                   uint32_t lTimeOut)
+{
+  uint8_t *ucMBFrame;
+  uint16_t usRegIndex = 0;
+  uint8_t ucByteCount;
+  eMBMasterReqErrCode eErrStatus = MB_MRE_NO_ERR;
+
+  if (ucSndAddr > MB_MASTER_TOTAL_SLAVE_NUM)
+    {
+      eErrStatus = MB_MRE_ILL_ARG;
+    }
+  else if (usNCoils > MB_PDU_REQ_WRITE_MUL_COILCNT_MAX)
+    {
+      eErrStatus = MB_MRE_ILL_ARG;
+    }
+  else if (xMBMasterRunResTake(lTimeOut) == false)
+    {
+      eErrStatus = MB_MRE_MASTER_BUSY;
+    }
+  else
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      vMBMasterSetDestAddress(ucSndAddr);
+      ucMBFrame[MB_PDU_FUNC_OFF] = MB_FUNC_WRITE_MULTIPLE_COILS;
+      ucMBFrame[MB_PDU_REQ_WRITE_MUL_ADDR_OFF] = usCoilAddr >> 8;
+      ucMBFrame[MB_PDU_REQ_WRITE_MUL_ADDR_OFF + 1] = usCoilAddr;
+      ucMBFrame[MB_PDU_REQ_WRITE_MUL_COILCNT_OFF] = usNCoils >> 8;
+      ucMBFrame[MB_PDU_REQ_WRITE_MUL_COILCNT_OFF + 1] = usNCoils;
+
+      if ((usNCoils & 0x0007) != 0)
+        {
+          ucByteCount = (uint8_t) (usNCoils / 8 + 1);
+        }
+      else
+        {
+          ucByteCount = (uint8_t) (usNCoils / 8);
+        }
+
+      ucMBFrame[MB_PDU_REQ_WRITE_MUL_BYTECNT_OFF] = ucByteCount;
+      ucMBFrame += MB_PDU_REQ_WRITE_MUL_VALUES_OFF;
+
+      while (ucByteCount > usRegIndex)
+        {
+          *ucMBFrame++ = pucDataBuffer[usRegIndex++];
+        }
+
+      vMBMasterSetPDUSndLength(MB_PDU_SIZE_MIN + MB_PDU_REQ_WRITE_MUL_SIZE_MIN +
+                               ucByteCount);
+      (void)xMBMasterPortEventPost(EV_MASTER_FRAME_SENT);
+      eErrStatus = eMBMasterWaitRequestFinish();
+    }
+
+  return eErrStatus;
+}
+
+eMBException eMBMasterFuncWriteMultipleCoils(uint8_t *pucFrame,
+                                             uint16_t *usLen)
+{
+  uint16_t usRegAddress;
+  uint16_t usCoilCnt;
+  uint8_t ucByteCount;
+  uint8_t ucByteCountVerify;
+  uint8_t *ucMBFrame;
+
+  eMBException eStatus = MB_EX_NONE;
+  eMBErrorCode eRegStatus;
+
+  /* If this request is broadcast, the *usLen is not need check. */
+
+  if ((*usLen == MB_PDU_FUNC_WRITE_MUL_SIZE) || xMBMasterRequestIsBroadcast())
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      usRegAddress = (uint16_t) (pucFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF] << 8);
+      usRegAddress |= (uint16_t) (pucFrame[MB_PDU_FUNC_WRITE_MUL_ADDR_OFF + 1]);
+      usRegAddress++;
+
+      usCoilCnt = (uint16_t) (pucFrame[MB_PDU_FUNC_WRITE_MUL_COILCNT_OFF] << 8);
+      usCoilCnt |= (uint16_t) (pucFrame[MB_PDU_FUNC_WRITE_MUL_COILCNT_OFF + 1]);
+
+      ucByteCount = ucMBFrame[MB_PDU_REQ_WRITE_MUL_BYTECNT_OFF];
+
+      /* Compute the number of expected bytes in the request. */
+
+      if ((usCoilCnt & 0x0007) != 0)
+        {
+          ucByteCountVerify = (uint8_t) (usCoilCnt / 8 + 1);
+        }
+      else
+        {
+          ucByteCountVerify = (uint8_t) (usCoilCnt / 8);
+        }
+
+      if ((usCoilCnt >= 1) && (ucByteCountVerify == ucByteCount))
+        {
+          eRegStatus =
+            eMBMasterRegCoilsCB(&ucMBFrame[MB_PDU_REQ_WRITE_MUL_VALUES_OFF],
+                                usRegAddress, usCoilCnt, MB_REG_WRITE);
+
+          /* If an error occurred convert it into a Modbus exception. */
+
+          if (eRegStatus != MB_ENOERR)
+            {
+              eStatus = prveMBError2Exception(eRegStatus);
+            }
+        }
+      else
+        {
+          eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+        }
+    }
+  else
+    {
+      /* Can't be a valid write coil register request because the length is
+       * incorrect.
+       */
+
+      eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+    }
+
+  return eStatus;
+}
+
+#endif
+#endif
diff --git a/apps/modbus/functions/mbfuncdisc_m.c b/apps/modbus/functions/mbfuncdisc_m.c
new file mode 100644
index 000000000..7618dd9a5
--- /dev/null
+++ b/apps/modbus/functions/mbfuncdisc_m.c
@@ -0,0 +1,199 @@
+/****************************************************************************
+ * apps/modbus/functions/mbfuncdisc_m.c
+ * 
+ * FreeModbus Libary: A portable Modbus implementation for Modbus ASCII/RTU.
+ * Copyright (C) 2013 Armink <armink.ztl@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "port.h"
+
+#include <apps/modbus/mb.h>
+#include <apps/modbus/mb_m.h>
+#include <apps/modbus/mbframe.h>
+#include <apps/modbus/mbproto.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define MB_PDU_REQ_READ_ADDR_OFF            (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_REQ_READ_DISCCNT_OFF         (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_REQ_READ_SIZE                (4)
+#define MB_PDU_FUNC_READ_DISCCNT_OFF        (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_FUNC_READ_VALUES_OFF         (MB_PDU_DATA_OFF + 1)
+#define MB_PDU_FUNC_READ_SIZE_MIN           (1)
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+eMBException prveMBError2Exception(eMBErrorCode eErrorCode);
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+#if MB_MASTER_RTU_ENABLED > 0 || MB_MASTER_ASCII_ENABLED > 0
+
+/****************************************************************************
+ * Description:
+ *   This function will request read discrete inputs.
+ *
+ * Input Parameters:
+ *   ucSndAddr salve address
+ *   usDiscreteAddr discrete start address
+ *   usNDiscreteIn discrete total number
+ *   lTimeOut timeout (-1 will waiting forever)
+ *
+ * Returned Value:
+ *   error code
+ *
+ ****************************************************************************/
+
+#if MB_FUNC_READ_DISCRETE_INPUTS_ENABLED > 0
+eMBMasterReqErrCode eMBMasterReqReadDiscreteInputs(uint8_t ucSndAddr,
+                                                   uint16_t usDiscreteAddr,
+                                                   uint16_t usNDiscreteIn,
+                                                   uint32_t lTimeOut)
+{
+  uint8_t *ucMBFrame;
+  eMBMasterReqErrCode eErrStatus = MB_MRE_NO_ERR;
+
+  if (ucSndAddr > MB_MASTER_TOTAL_SLAVE_NUM)
+    {
+      eErrStatus = MB_MRE_ILL_ARG;
+    }
+  else if (xMBMasterRunResTake(lTimeOut) == FALSE)
+    {
+      eErrStatus = MB_MRE_MASTER_BUSY;
+    }
+  else
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      vMBMasterSetDestAddress(ucSndAddr);
+      ucMBFrame[MB_PDU_FUNC_OFF] = MB_FUNC_READ_DISCRETE_INPUTS;
+      ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF] = usDiscreteAddr >> 8;
+      ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF + 1] = usDiscreteAddr;
+      ucMBFrame[MB_PDU_REQ_READ_DISCCNT_OFF] = usNDiscreteIn >> 8;
+      ucMBFrame[MB_PDU_REQ_READ_DISCCNT_OFF + 1] = usNDiscreteIn;
+      vMBMasterSetPDUSndLength(MB_PDU_SIZE_MIN + MB_PDU_REQ_READ_SIZE);
+      (void)xMBMasterPortEventPost(EV_MASTER_FRAME_SENT);
+      eErrStatus = eMBMasterWaitRequestFinish();
+    }
+
+  return eErrStatus;
+}
+
+eMBException eMBMasterFuncReadDiscreteInputs(uint8_t *pucFrame,
+                                             uint16_t *usLen)
+{
+  uint16_t usRegAddress;
+  uint16_t usDiscreteCnt;
+  uint8_t ucNBytes;
+  uint8_t *ucMBFrame;
+
+  eMBException eStatus = MB_EX_NONE;
+  eMBErrorCode eRegStatus;
+
+  /* If this request is broadcast, and it's read mode. This request don't need
+   * execute.
+   */
+
+  if (xMBMasterRequestIsBroadcast())
+    {
+      eStatus = MB_EX_NONE;
+    }
+  else if (*usLen >= MB_PDU_SIZE_MIN + MB_PDU_FUNC_READ_SIZE_MIN)
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      usRegAddress = (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF] << 8);
+      usRegAddress |= (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF + 1]);
+      usRegAddress++;
+
+      usDiscreteCnt = (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_DISCCNT_OFF] << 8);
+      usDiscreteCnt |= (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_DISCCNT_OFF + 1]);
+
+      /* Test if the quantity of coils is a multiple of 8. If not last byte is
+       * only partially field with unused coils set to zero.
+       */
+
+      if ((usDiscreteCnt & 0x0007) != 0)
+        {
+          ucNBytes = (uint8_t) (usDiscreteCnt / 8 + 1);
+        }
+      else
+        {
+          ucNBytes = (uint8_t) (usDiscreteCnt / 8);
+        }
+
+      /* Check if the number of registers to read is valid. If not return
+       * Modbus illegal data value exception.
+       */
+
+      if ((usDiscreteCnt >= 1) &&
+          ucNBytes == pucFrame[MB_PDU_FUNC_READ_DISCCNT_OFF])
+        {
+          /* Make callback to fill the buffer. */
+
+          eRegStatus =
+            eMBMasterRegDiscreteCB(&pucFrame[MB_PDU_FUNC_READ_VALUES_OFF],
+                                   usRegAddress, usDiscreteCnt);
+
+          /* If an error occurred convert it into a Modbus exception. */
+
+          if (eRegStatus != MB_ENOERR)
+            {
+              eStatus = prveMBError2Exception(eRegStatus);
+            }
+        }
+      else
+        {
+          eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+        }
+    }
+  else
+    {
+      /* Can't be a valid read coil register request because the length is
+       * incorrect.
+       */
+
+      eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+    }
+
+  return eStatus;
+}
+
+#endif
+#endif
diff --git a/apps/modbus/functions/mbfuncholding_m.c b/apps/modbus/functions/mbfuncholding_m.c
new file mode 100644
index 000000000..d5e802700
--- /dev/null
+++ b/apps/modbus/functions/mbfuncholding_m.c
@@ -0,0 +1,567 @@
+/****************************************************************************
+ * apps/modbus/functions/mbfuncholding_m.c
+ * 
+ * FreeModbus Libary: A portable Modbus implementation for Modbus ASCII/RTU.
+ * Copyright (C) 2013 Armink <armink.ztl@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "port.h"
+
+#include <apps/modbus/mb.h>
+#include <apps/modbus/mb_m.h>
+#include <apps/modbus/mbframe.h>
+#include <apps/modbus/mbproto.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define MB_PDU_REQ_READ_ADDR_OFF                (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_REQ_READ_REGCNT_OFF              (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_REQ_READ_SIZE                    (4)
+#define MB_PDU_FUNC_READ_REGCNT_MAX             (0x007D)
+#define MB_PDU_FUNC_READ_BYTECNT_OFF            (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_FUNC_READ_VALUES_OFF             (MB_PDU_DATA_OFF + 1)
+#define MB_PDU_FUNC_READ_SIZE_MIN               (1)
+
+#define MB_PDU_REQ_WRITE_ADDR_OFF               (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_REQ_WRITE_VALUE_OFF              (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_REQ_WRITE_SIZE                   (4)
+#define MB_PDU_FUNC_WRITE_ADDR_OFF              (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_FUNC_WRITE_VALUE_OFF             (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_FUNC_WRITE_SIZE                  (4)
+
+#define MB_PDU_REQ_WRITE_MUL_ADDR_OFF           (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_REQ_WRITE_MUL_REGCNT_OFF         (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_REQ_WRITE_MUL_BYTECNT_OFF        (MB_PDU_DATA_OFF + 4)
+#define MB_PDU_REQ_WRITE_MUL_VALUES_OFF         (MB_PDU_DATA_OFF + 5)
+#define MB_PDU_REQ_WRITE_MUL_SIZE_MIN           (5)
+#define MB_PDU_REQ_WRITE_MUL_REGCNT_MAX         (0x0078)
+#define MB_PDU_FUNC_WRITE_MUL_ADDR_OFF          (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_FUNC_WRITE_MUL_REGCNT_OFF        (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_FUNC_WRITE_MUL_SIZE              (4)
+
+#define MB_PDU_REQ_READWRITE_READ_ADDR_OFF      (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_REQ_READWRITE_READ_REGCNT_OFF    (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_REQ_READWRITE_WRITE_ADDR_OFF     (MB_PDU_DATA_OFF + 4)
+#define MB_PDU_REQ_READWRITE_WRITE_REGCNT_OFF   (MB_PDU_DATA_OFF + 6)
+#define MB_PDU_REQ_READWRITE_WRITE_BYTECNT_OFF  (MB_PDU_DATA_OFF + 8)
+#define MB_PDU_REQ_READWRITE_WRITE_VALUES_OFF   (MB_PDU_DATA_OFF + 9)
+#define MB_PDU_REQ_READWRITE_SIZE_MIN           (9)
+#define MB_PDU_FUNC_READWRITE_READ_BYTECNT_OFF  (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_FUNC_READWRITE_READ_VALUES_OFF   (MB_PDU_DATA_OFF + 1)
+#define MB_PDU_FUNC_READWRITE_SIZE_MIN          (1)
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+eMBException prveMBError2Exception(eMBErrorCode eErrorCode);
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+#if MB_MASTER_RTU_ENABLED > 0 || MB_MASTER_ASCII_ENABLED > 0
+
+/****************************************************************************
+ * Description:
+ *   This function will request write holding register.
+ *
+ * Input Parameters:
+ *   ucSndAddr salve address
+ *   usRegAddr register start address
+ *   usRegData register data to be written
+ *   lTimeOut timeout (-1 will waiting forever)
+ *
+ * Returned Value:
+ *   error code
+ *
+ ****************************************************************************/
+
+#if MB_FUNC_WRITE_HOLDING_ENABLED > 0
+eMBMasterReqErrCode eMBMasterReqWriteHoldingRegister(uint8_t ucSndAddr,
+                                                     uint16_t usRegAddr,
+                                                     uint16_t usRegData,
+                                                     uint32_t lTimeOut)
+{
+  uint8_t *ucMBFrame;
+  eMBMasterReqErrCode eErrStatus = MB_MRE_NO_ERR;
+
+  if (ucSndAddr > MB_MASTER_TOTAL_SLAVE_NUM)
+    {
+      eErrStatus = MB_MRE_ILL_ARG;
+    }
+  else if (xMBMasterRunResTake(lTimeOut) == false)
+    {
+      eErrStatus = MB_MRE_MASTER_BUSY;
+    }
+  else
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      vMBMasterSetDestAddress(ucSndAddr);
+      ucMBFrame[MB_PDU_FUNC_OFF] = MB_FUNC_WRITE_REGISTER;
+      ucMBFrame[MB_PDU_REQ_WRITE_ADDR_OFF] = usRegAddr >> 8;
+      ucMBFrame[MB_PDU_REQ_WRITE_ADDR_OFF + 1] = usRegAddr;
+      ucMBFrame[MB_PDU_REQ_WRITE_VALUE_OFF] = usRegData >> 8;
+      ucMBFrame[MB_PDU_REQ_WRITE_VALUE_OFF + 1] = usRegData;
+      vMBMasterSetPDUSndLength(MB_PDU_SIZE_MIN + MB_PDU_REQ_WRITE_SIZE);
+      (void)xMBMasterPortEventPost(EV_MASTER_FRAME_SENT);
+      eErrStatus = eMBMasterWaitRequestFinish();
+    }
+
+  return eErrStatus;
+}
+
+eMBException eMBMasterFuncWriteHoldingRegister(uint8_t *pucFrame,
+                                               uint16_t *usLen)
+{
+  uint16_t usRegAddress;
+  eMBException eStatus = MB_EX_NONE;
+  eMBErrorCode eRegStatus;
+
+  if (*usLen == (MB_PDU_SIZE_MIN + MB_PDU_FUNC_WRITE_SIZE))
+    {
+      usRegAddress = (uint16_t) (pucFrame[MB_PDU_FUNC_WRITE_ADDR_OFF] << 8);
+      usRegAddress |= (uint16_t) (pucFrame[MB_PDU_FUNC_WRITE_ADDR_OFF + 1]);
+      usRegAddress++;
+
+      /* Make callback to update the value. */
+
+      eRegStatus = eMBMasterRegHoldingCB(&pucFrame[MB_PDU_FUNC_WRITE_VALUE_OFF],
+                                         usRegAddress, 1, MB_REG_WRITE);
+
+      /* If an error occured convert it into a Modbus exception. */
+
+      if (eRegStatus != MB_ENOERR)
+        {
+          eStatus = prveMBError2Exception(eRegStatus);
+        }
+    }
+  else
+    {
+      /* Can't be a valid request because the length is incorrect. */
+
+      eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+    }
+  return eStatus;
+}
+#endif
+
+/****************************************************************************
+ * This function will request write multiple holding register.
+ *
+ * Input Parameters:
+ *   ucSndAddr salve address
+ *   usRegAddr register start address
+ *   usNRegs register total number
+ *   pusDataBuffer data to be written
+ *   lTimeOut timeout (-1 will waiting forever)
+ *
+ * Returned Value:
+ *   error code
+ *
+ ****************************************************************************/
+
+#if MB_FUNC_WRITE_MULTIPLE_HOLDING_ENABLED > 0
+eMBMasterReqErrCode
+  eMBMasterReqWriteMultipleHoldingRegister(uint8_t ucSndAddr,
+                                           uint16_t usRegAddr,
+                                           uint16_t usNRegs,
+                                           uint16_t *pusDataBuffer,
+                                           uint32_t lTimeOut)
+{
+  uint8_t *ucMBFrame;
+  uint16_t usRegIndex = 0;
+  eMBMasterReqErrCode eErrStatus = MB_MRE_NO_ERR;
+
+  if (ucSndAddr > MB_MASTER_TOTAL_SLAVE_NUM)
+    {
+      eErrStatus = MB_MRE_ILL_ARG;
+    }
+  else if (xMBMasterRunResTake(lTimeOut) == false)
+    {
+      eErrStatus = MB_MRE_MASTER_BUSY;
+    }
+  else
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      vMBMasterSetDestAddress(ucSndAddr);
+      ucMBFrame[MB_PDU_FUNC_OFF] = MB_FUNC_WRITE_MULTIPLE_REGISTERS;
+      ucMBFrame[MB_PDU_REQ_WRITE_MUL_ADDR_OFF] = usRegAddr >> 8;
+      ucMBFrame[MB_PDU_REQ_WRITE_MUL_ADDR_OFF + 1] = usRegAddr;
+      ucMBFrame[MB_PDU_REQ_WRITE_MUL_REGCNT_OFF] = usNRegs >> 8;
+      ucMBFrame[MB_PDU_REQ_WRITE_MUL_REGCNT_OFF + 1] = usNRegs;
+      ucMBFrame[MB_PDU_REQ_WRITE_MUL_BYTECNT_OFF] = usNRegs * 2;
+      ucMBFrame += MB_PDU_REQ_WRITE_MUL_VALUES_OFF;
+
+      while (usNRegs > usRegIndex)
+        {
+          *ucMBFrame++ = pusDataBuffer[usRegIndex] >> 8;
+          *ucMBFrame++ = pusDataBuffer[usRegIndex++];
+        }
+
+      vMBMasterSetPDUSndLength(MB_PDU_SIZE_MIN + MB_PDU_REQ_WRITE_MUL_SIZE_MIN +
+                               2 * usNRegs);
+      (void)xMBMasterPortEventPost(EV_MASTER_FRAME_SENT);
+      eErrStatus = eMBMasterWaitRequestFinish();
+    }
+
+  return eErrStatus;
+}
+
+eMBException eMBMasterFuncWriteMultipleHoldingRegister(uint8_t *pucFrame,
+                                                       uint16_t *usLen)
+{
+  uint8_t *ucMBFrame;
+  uint16_t usRegAddress;
+  uint16_t usRegCount;
+  uint8_t ucRegByteCount;
+
+  eMBException eStatus = MB_EX_NONE;
+  eMBErrorCode eRegStatus;
+
+  /* If this request is broadcast, the *usLen is not need check. */
+
+  if ((*usLen == MB_PDU_SIZE_MIN + MB_PDU_FUNC_WRITE_MUL_SIZE) ||
+      xMBMasterRequestIsBroadcast())
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      usRegAddress = (uint16_t) (ucMBFrame[MB_PDU_REQ_WRITE_MUL_ADDR_OFF] << 8);
+      usRegAddress |= (uint16_t) (ucMBFrame[MB_PDU_REQ_WRITE_MUL_ADDR_OFF + 1]);
+      usRegAddress++;
+
+      usRegCount = (uint16_t) (ucMBFrame[MB_PDU_REQ_WRITE_MUL_REGCNT_OFF] << 8);
+      usRegCount |= (uint16_t) (ucMBFrame[MB_PDU_REQ_WRITE_MUL_REGCNT_OFF + 1]);
+
+      ucRegByteCount = ucMBFrame[MB_PDU_REQ_WRITE_MUL_BYTECNT_OFF];
+
+      if (ucRegByteCount == 2 * usRegCount)
+        {
+          /* Make callback to update the register values. */
+
+          eRegStatus =
+            eMBMasterRegHoldingCB(&ucMBFrame[MB_PDU_REQ_WRITE_MUL_VALUES_OFF],
+                                  usRegAddress, usRegCount, MB_REG_WRITE);
+
+          /* If an error occured convert it into a Modbus exception. */
+
+          if (eRegStatus != MB_ENOERR)
+            {
+              eStatus = prveMBError2Exception(eRegStatus);
+            }
+        }
+      else
+        {
+          eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+        }
+    }
+  else
+    {
+      /* Can't be a valid request because the length is incorrect. */
+
+      eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+    }
+
+  return eStatus;
+}
+#endif
+
+/****************************************************************************
+ * This function will request read holding register.
+ *
+ * Input Parameters:
+ *   ucSndAddr salve address
+ *   usRegAddr register start address
+ *   usNRegs register total number
+ *   lTimeOut timeout (-1 will waiting forever)
+ *
+ * Returned Value:
+ *   error code
+ *
+ ****************************************************************************/
+
+#if MB_FUNC_READ_HOLDING_ENABLED > 0
+eMBMasterReqErrCode eMBMasterReqReadHoldingRegister(uint8_t ucSndAddr,
+                                                    uint16_t usRegAddr,
+                                                    uint16_t usNRegs,
+                                                    uint32_t lTimeOut)
+{
+  uint8_t *ucMBFrame;
+  eMBMasterReqErrCode eErrStatus = MB_MRE_NO_ERR;
+
+  if (ucSndAddr > MB_MASTER_TOTAL_SLAVE_NUM)
+    {
+      eErrStatus = MB_MRE_ILL_ARG;
+    }
+  else if (xMBMasterRunResTake(lTimeOut) == false)
+    {
+      eErrStatus = MB_MRE_MASTER_BUSY;
+    }
+  else
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      vMBMasterSetDestAddress(ucSndAddr);
+      ucMBFrame[MB_PDU_FUNC_OFF] = MB_FUNC_READ_HOLDING_REGISTER;
+      ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF] = usRegAddr >> 8;
+      ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF + 1] = usRegAddr;
+      ucMBFrame[MB_PDU_REQ_READ_REGCNT_OFF] = usNRegs >> 8;
+      ucMBFrame[MB_PDU_REQ_READ_REGCNT_OFF + 1] = usNRegs;
+      vMBMasterSetPDUSndLength(MB_PDU_SIZE_MIN + MB_PDU_REQ_READ_SIZE);
+      (void)xMBMasterPortEventPost(EV_MASTER_FRAME_SENT);
+      eErrStatus = eMBMasterWaitRequestFinish();
+    }
+
+  return eErrStatus;
+}
+
+eMBException eMBMasterFuncReadHoldingRegister(uint8_t *pucFrame,
+                                              uint16_t *usLen)
+{
+  uint8_t *ucMBFrame;
+  uint16_t usRegAddress;
+  uint16_t usRegCount;
+
+  eMBException eStatus = MB_EX_NONE;
+  eMBErrorCode eRegStatus;
+
+  /* If this request is broadcast, and it's read mode. This request don't need
+   * execute.
+   */
+
+  if (xMBMasterRequestIsBroadcast())
+    {
+      eStatus = MB_EX_NONE;
+    }
+  else if (*usLen >= MB_PDU_SIZE_MIN + MB_PDU_FUNC_READ_SIZE_MIN)
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      usRegAddress = (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF] << 8);
+      usRegAddress |= (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF + 1]);
+      usRegAddress++;
+
+      usRegCount = (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_REGCNT_OFF] << 8);
+      usRegCount |= (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_REGCNT_OFF + 1]);
+
+      /* Check if the number of registers to read is valid. If not return
+       * Modbus illegal data value exception.
+       */
+
+      if ((usRegCount >= 1) &&
+          (2 * usRegCount == pucFrame[MB_PDU_FUNC_READ_BYTECNT_OFF]))
+        {
+          /* Make callback to fill the buffer. */
+
+          eRegStatus =
+            eMBMasterRegHoldingCB(&pucFrame[MB_PDU_FUNC_READ_VALUES_OFF],
+                                  usRegAddress, usRegCount, MB_REG_READ);
+
+          /* If an error occurred convert it into a Modbus exception. */
+
+          if (eRegStatus != MB_ENOERR)
+            {
+              eStatus = prveMBError2Exception(eRegStatus);
+            }
+        }
+      else
+        {
+          eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+        }
+    }
+  else
+    {
+      /* Can't be a valid request because the length is incorrect. */
+
+      eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+    }
+  return eStatus;
+}
+
+#endif
+
+/****************************************************************************
+ * Description:
+ *   This function will request read and write holding register.
+ *
+ * Input Parameters:
+ *   ucSndAddr salve address
+ *   usReadRegAddr read register start address
+ *   usNReadRegs read register total number
+ *   pusDataBuffer data to be written
+ *   usWriteRegAddr write register start address
+ *   usNWriteRegs write register total number
+ *   lTimeOut timeout (-1 will waiting forever)
+ *
+ * Returned Value:
+ *   error code
+ *
+ ****************************************************************************/
+
+#if MB_FUNC_READWRITE_HOLDING_ENABLED > 0
+eMBMasterReqErrCode
+  eMBMasterReqReadWriteMultipleHoldingRegister(uint8_t ucSndAddr,
+                                               uint16_t usReadRegAddr,
+                                               uint16_t usNReadRegs,
+                                               uint16_t *pusDataBuffer,
+                                               uint16_t usWriteRegAddr,
+                                               uint16_t usNWriteRegs,
+                                               uint32_t lTimeOut)
+{
+  uint8_t *ucMBFrame;
+  uint16_t usRegIndex = 0;
+  eMBMasterReqErrCode eErrStatus = MB_MRE_NO_ERR;
+
+  if (ucSndAddr > MB_MASTER_TOTAL_SLAVE_NUM)
+    {
+      eErrStatus = MB_MRE_ILL_ARG;
+    }
+  else if (xMBMasterRunResTake(lTimeOut) == false)
+    {
+      eErrStatus = MB_MRE_MASTER_BUSY;
+    }
+  else
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      vMBMasterSetDestAddress(ucSndAddr);
+      ucMBFrame[MB_PDU_FUNC_OFF] = MB_FUNC_READWRITE_MULTIPLE_REGISTERS;
+      ucMBFrame[MB_PDU_REQ_READWRITE_READ_ADDR_OFF] = usReadRegAddr >> 8;
+      ucMBFrame[MB_PDU_REQ_READWRITE_READ_ADDR_OFF + 1] = usReadRegAddr;
+      ucMBFrame[MB_PDU_REQ_READWRITE_READ_REGCNT_OFF] = usNReadRegs >> 8;
+      ucMBFrame[MB_PDU_REQ_READWRITE_READ_REGCNT_OFF + 1] = usNReadRegs;
+      ucMBFrame[MB_PDU_REQ_READWRITE_WRITE_ADDR_OFF] = usWriteRegAddr >> 8;
+      ucMBFrame[MB_PDU_REQ_READWRITE_WRITE_ADDR_OFF + 1] = usWriteRegAddr;
+      ucMBFrame[MB_PDU_REQ_READWRITE_WRITE_REGCNT_OFF] = usNWriteRegs >> 8;
+      ucMBFrame[MB_PDU_REQ_READWRITE_WRITE_REGCNT_OFF + 1] = usNWriteRegs;
+      ucMBFrame[MB_PDU_REQ_READWRITE_WRITE_BYTECNT_OFF] = usNWriteRegs * 2;
+      ucMBFrame += MB_PDU_REQ_READWRITE_WRITE_VALUES_OFF;
+
+      while (usNWriteRegs > usRegIndex)
+        {
+          *ucMBFrame++ = pusDataBuffer[usRegIndex] >> 8;
+          *ucMBFrame++ = pusDataBuffer[usRegIndex++];
+        }
+
+      vMBMasterSetPDUSndLength(MB_PDU_SIZE_MIN + MB_PDU_REQ_READWRITE_SIZE_MIN +
+                               2 * usNWriteRegs);
+      (void)xMBMasterPortEventPost(EV_MASTER_FRAME_SENT);
+      eErrStatus = eMBMasterWaitRequestFinish();
+    }
+
+  return eErrStatus;
+}
+
+eMBException eMBMasterFuncReadWriteMultipleHoldingRegister(uint8_t *pucFrame,
+                                                           uint16_t *usLen)
+{
+  uint16_t usRegReadAddress;
+  uint16_t usRegReadCount;
+  uint16_t usRegWriteAddress;
+  uint16_t usRegWriteCount;
+  uint8_t *ucMBFrame;
+
+  eMBException eStatus = MB_EX_NONE;
+  eMBErrorCode eRegStatus;
+
+  /* If this request is broadcast, and it's read mode. This request don't need
+   * execute.
+   */
+
+  if (xMBMasterRequestIsBroadcast())
+    {
+      eStatus = MB_EX_NONE;
+    }
+  else if (*usLen >= MB_PDU_SIZE_MIN + MB_PDU_FUNC_READWRITE_SIZE_MIN)
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      usRegReadAddress =
+        (uint16_t) (ucMBFrame[MB_PDU_REQ_READWRITE_READ_ADDR_OFF] << 8U);
+      usRegReadAddress |=
+        (uint16_t) (ucMBFrame[MB_PDU_REQ_READWRITE_READ_ADDR_OFF + 1]);
+      usRegReadAddress++;
+
+      usRegReadCount =
+        (uint16_t) (ucMBFrame[MB_PDU_REQ_READWRITE_READ_REGCNT_OFF] << 8U);
+      usRegReadCount |=
+        (uint16_t) (ucMBFrame[MB_PDU_REQ_READWRITE_READ_REGCNT_OFF + 1]);
+
+      usRegWriteAddress =
+        (uint16_t) (ucMBFrame[MB_PDU_REQ_READWRITE_WRITE_ADDR_OFF] << 8U);
+      usRegWriteAddress |=
+        (uint16_t) (ucMBFrame[MB_PDU_REQ_READWRITE_WRITE_ADDR_OFF + 1]);
+      usRegWriteAddress++;
+
+      usRegWriteCount =
+        (uint16_t) (ucMBFrame[MB_PDU_REQ_READWRITE_WRITE_REGCNT_OFF] << 8U);
+      usRegWriteCount |=
+        (uint16_t) (ucMBFrame[MB_PDU_REQ_READWRITE_WRITE_REGCNT_OFF + 1]);
+
+      if ((2 * usRegReadCount) ==
+          pucFrame[MB_PDU_FUNC_READWRITE_READ_BYTECNT_OFF])
+        {
+          /* Make callback to update the register values. */
+
+          eRegStatus =
+            eMBMasterRegHoldingCB(&ucMBFrame
+                                  [MB_PDU_REQ_READWRITE_WRITE_VALUES_OFF],
+                                  usRegWriteAddress, usRegWriteCount,
+                                  MB_REG_WRITE);
+
+          if (eRegStatus == MB_ENOERR)
+            {
+              /* Make the read callback. */
+
+              eRegStatus =
+                eMBMasterRegHoldingCB(&pucFrame
+                                      [MB_PDU_FUNC_READWRITE_READ_VALUES_OFF],
+                                      usRegReadAddress, usRegReadCount,
+                                      MB_REG_READ);
+            }
+
+          if (eRegStatus != MB_ENOERR)
+            {
+              eStatus = prveMBError2Exception(eRegStatus);
+            }
+        }
+      else
+        {
+          eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+        }
+    }
+
+  return eStatus;
+}
+
+#endif
+#endif
diff --git a/apps/modbus/functions/mbfuncinput_m.c b/apps/modbus/functions/mbfuncinput_m.c
new file mode 100644
index 000000000..475cdca56
--- /dev/null
+++ b/apps/modbus/functions/mbfuncinput_m.c
@@ -0,0 +1,185 @@
+/****************************************************************************
+ * apps/modbus/functions/mbfuncinput_m.c
+ * 
+ * FreeModbus Libary: A portable Modbus implementation for Modbus ASCII/RTU.
+ * Copyright (C) 2013 Armink <armink.ztl@gmail.com>
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
+ * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
+ * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
+ * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "port.h"
+
+#include <apps/modbus/mb.h>
+#include <apps/modbus/mb_m.h>
+#include <apps/modbus/mbframe.h>
+#include <apps/modbus/mbproto.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define MB_PDU_REQ_READ_ADDR_OFF            (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_REQ_READ_REGCNT_OFF          (MB_PDU_DATA_OFF + 2)
+#define MB_PDU_REQ_READ_SIZE                (4)
+#define MB_PDU_FUNC_READ_BYTECNT_OFF        (MB_PDU_DATA_OFF + 0)
+#define MB_PDU_FUNC_READ_VALUES_OFF         (MB_PDU_DATA_OFF + 1)
+#define MB_PDU_FUNC_READ_SIZE_MIN           (1)
+
+#define MB_PDU_FUNC_READ_RSP_BYTECNT_OFF    (MB_PDU_DATA_OFF)
+
+/****************************************************************************
+ * Public Function Prototypes
+ ****************************************************************************/
+
+eMBException prveMBError2Exception(eMBErrorCode eErrorCode);
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+#if MB_MASTER_RTU_ENABLED > 0 || MB_MASTER_ASCII_ENABLED > 0
+
+/****************************************************************************
+ * Description:
+ *   This function will request read input register.
+ *
+ * Input Parameters:
+ *   ucSndAddr salve address
+ *   usRegAddr register start address
+ *   usNRegs register total number
+ *   lTimeOut timeout (-1 will waiting forever)
+ *
+ * Returned Value:
+ *   error code
+ *
+ ****************************************************************************/
+
+#if MB_FUNC_READ_INPUT_ENABLED > 0
+eMBMasterReqErrCode eMBMasterReqReadInputRegister(uint8_t ucSndAddr,
+                                                  uint16_t usRegAddr,
+                                                  uint16_t usNRegs,
+                                                  uint32_t lTimeOut)
+{
+  uint8_t *ucMBFrame;
+  eMBMasterReqErrCode eErrStatus = MB_MRE_NO_ERR;
+
+  if (ucSndAddr > MB_MASTER_TOTAL_SLAVE_NUM)
+    {
+      eErrStatus = MB_MRE_ILL_ARG;
+    }
+  else if (xMBMasterRunResTake(lTimeOut) == false)
+    {
+      eErrStatus = MB_MRE_MASTER_BUSY;
+    }
+  else
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      vMBMasterSetDestAddress(ucSndAddr);
+      ucMBFrame[MB_PDU_FUNC_OFF] = MB_FUNC_READ_INPUT_REGISTER;
+      ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF] = usRegAddr >> 8;
+      ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF + 1] = usRegAddr;
+      ucMBFrame[MB_PDU_REQ_READ_REGCNT_OFF] = usNRegs >> 8;
+      ucMBFrame[MB_PDU_REQ_READ_REGCNT_OFF + 1] = usNRegs;
+      vMBMasterSetPDUSndLength(MB_PDU_SIZE_MIN + MB_PDU_REQ_READ_SIZE);
+      (void)xMBMasterPortEventPost(EV_MASTER_FRAME_SENT);
+      eErrStatus = eMBMasterWaitRequestFinish();
+    }
+
+  return eErrStatus;
+}
+
+eMBException eMBMasterFuncReadInputRegister(uint8_t *pucFrame,
+                                            uint16_t *usLen)
+{
+  uint8_t *ucMBFrame;
+  uint16_t usRegAddress;
+  uint16_t usRegCount;
+
+  eMBException eStatus = MB_EX_NONE;
+  eMBErrorCode eRegStatus;
+
+  /* If this request is broadcast, and it's read mode. This request don't need
+   * execute.
+   */
+
+  if (xMBMasterRequestIsBroadcast())
+    {
+      eStatus = MB_EX_NONE;
+    }
+  else if (*usLen >= MB_PDU_SIZE_MIN + MB_PDU_FUNC_READ_SIZE_MIN)
+    {
+      vMBMasterGetPDUSndBuf(&ucMBFrame);
+      usRegAddress = (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF] << 8);
+      usRegAddress |= (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_ADDR_OFF + 1]);
+      usRegAddress++;
+
+      usRegCount = (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_REGCNT_OFF] << 8);
+      usRegCount |= (uint16_t) (ucMBFrame[MB_PDU_REQ_READ_REGCNT_OFF + 1]);
+
+      /* Check if the number of registers to read is valid. If not return
+       * Modbus illegal data value exception.
+       */
+
+      if ((usRegCount >= 1) &&
+          (2 * usRegCount == pucFrame[MB_PDU_FUNC_READ_BYTECNT_OFF]))
+        {
+          /* Make callback to fill the buffer. */
+
+          eRegStatus =
+            eMBMasterRegInputCB(&pucFrame[MB_PDU_FUNC_READ_VALUES_OFF],
+                                usRegAddress, usRegCount);
+
+          /* If an error occurred convert it into a Modbus exception. */
+
+          if (eRegStatus != MB_ENOERR)
+            {
+              eStatus = prveMBError2Exception(eRegStatus);
+            }
+        }
+      else
+        {
+          eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+        }
+    }
+  else
+    {
+      /* Can't be a valid request because the length is incorrect. */
+
+      eStatus = MB_EX_ILLEGAL_DATA_VALUE;
+    }
+
+  return eStatus;
+}
+
+#endif
+#endif