e467b61785
git-svn-id: svn://svn.code.sf.net/p/nuttx/code/trunk@4964 42af7a65-404d-4744-a932-0658087f49c3
124 lines
4.4 KiB
C
124 lines
4.4 KiB
C
/*
|
|
* FreeRTOS Modbus Libary: A Modbus serial implementation for FreeRTOS
|
|
* Copyright (C) 2006 Christian Walter <wolti@sil.at>
|
|
*
|
|
* This library is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU Lesser General Public
|
|
* License as published by the Free Software Foundation; either
|
|
* version 2.1 of the License, or (at your option) any later version.
|
|
*
|
|
* This library is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
|
* Lesser General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Lesser General Public
|
|
* License along with this library; if not, write to the Free Software
|
|
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
|
*/
|
|
|
|
/* ----------------------- System includes ----------------------------------*/
|
|
#include <nuttx/config.h>
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
/* ----------------------- Platform includes --------------------------------*/
|
|
#include "port.h"
|
|
|
|
/* ----------------------- Modbus includes ----------------------------------*/
|
|
#include <apps/modbus/mb.h>
|
|
#include <apps/modbus/mbframe.h>
|
|
#include <apps/modbus/mbproto.h>
|
|
|
|
/* ----------------------- Defines ------------------------------------------*/
|
|
#define MB_PDU_FUNC_READ_ADDR_OFF ( MB_PDU_DATA_OFF )
|
|
#define MB_PDU_FUNC_READ_DISCCNT_OFF ( MB_PDU_DATA_OFF + 2 )
|
|
#define MB_PDU_FUNC_READ_SIZE ( 4 )
|
|
#define MB_PDU_FUNC_READ_DISCCNT_MAX ( 0x07D0 )
|
|
|
|
/* ----------------------- Static functions ---------------------------------*/
|
|
eMBException prveMBError2Exception( eMBErrorCode eErrorCode );
|
|
|
|
/* ----------------------- Start implementation -----------------------------*/
|
|
|
|
#ifdef CONFIG_MB_FUNC_READ_COILS_ENABLED
|
|
|
|
eMBException
|
|
eMBFuncReadDiscreteInputs( uint8_t * pucFrame, uint16_t * usLen )
|
|
{
|
|
uint16_t usRegAddress;
|
|
uint16_t usDiscreteCnt;
|
|
uint8_t ucNBytes;
|
|
uint8_t *pucFrameCur;
|
|
|
|
eMBException eStatus = MB_EX_NONE;
|
|
eMBErrorCode eRegStatus;
|
|
|
|
if( *usLen == ( MB_PDU_FUNC_READ_SIZE + MB_PDU_SIZE_MIN ) )
|
|
{
|
|
usRegAddress = ( uint16_t )( pucFrame[MB_PDU_FUNC_READ_ADDR_OFF] << 8 );
|
|
usRegAddress |= ( uint16_t )( pucFrame[MB_PDU_FUNC_READ_ADDR_OFF + 1] );
|
|
usRegAddress++;
|
|
|
|
usDiscreteCnt = ( uint16_t )( pucFrame[MB_PDU_FUNC_READ_DISCCNT_OFF] << 8 );
|
|
usDiscreteCnt |= ( uint16_t )( pucFrame[MB_PDU_FUNC_READ_DISCCNT_OFF + 1] );
|
|
|
|
/* Check if the number of registers to read is valid. If not
|
|
* return Modbus illegal data value exception.
|
|
*/
|
|
if( ( usDiscreteCnt >= 1 ) &&
|
|
( usDiscreteCnt < MB_PDU_FUNC_READ_DISCCNT_MAX ) )
|
|
{
|
|
/* Set the current PDU data pointer to the beginning. */
|
|
pucFrameCur = &pucFrame[MB_PDU_FUNC_OFF];
|
|
*usLen = MB_PDU_FUNC_OFF;
|
|
|
|
/* First byte contains the function code. */
|
|
*pucFrameCur++ = MB_FUNC_READ_DISCRETE_INPUTS;
|
|
*usLen += 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 );
|
|
}
|
|
*pucFrameCur++ = ucNBytes;
|
|
*usLen += 1;
|
|
|
|
eRegStatus =
|
|
eMBRegDiscreteCB( pucFrameCur, usRegAddress, usDiscreteCnt );
|
|
|
|
/* If an error occured convert it into a Modbus exception. */
|
|
if( eRegStatus != MB_ENOERR )
|
|
{
|
|
eStatus = prveMBError2Exception( eRegStatus );
|
|
}
|
|
else
|
|
{
|
|
/* The response contains the function code, the starting address
|
|
* and the quantity of registers. We reuse the old values in the
|
|
* buffer because they are still valid. */
|
|
*usLen += ucNBytes;;
|
|
}
|
|
}
|
|
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
|