modbusclient.cpp

Convert Interface on a embedded System

 *		[out] lpRequest REQUEST struct to receive the results of parsing
 *
 *	Returns zero if no parsing error occured; otherwise, 
 *	returns the Modbus exception code for the error.
 */
BYTE CModbusClient::ParseRequest( LPBYTE buffer, WORD wRecv, LPREQUEST lpRequest )
{
	BYTE nDataBytes;

	WORD n = LEN_HEADER; // start parsing after the MBAP header

    // First byte is the Modbus 'Function Code' for the request
    lpRequest->bFnCode = buffer[n++];

    // Parse the specific Modbus function code
	switch( lpRequest->bFnCode )
    {
		case MODBUS_READ_REGS:
		{
			// Next word is the address of the first register to read
			lpRequest->wAddr = MAKEWORD(buffer[n+1], buffer[n]);
			n += 2;

			// Next word is the quantity of registers to read
			lpRequest->wQty = MAKEWORD(buffer[n+1], buffer[n]);
			n += 2;

			// Check for valid quantity of registers to read
			if( lpRequest->wQty < 1 || lpRequest->wQty > MAX_READ_REGS )
				return ILLEGAL_VALUE;

			break;
		}

		case MODBUS_WRITE_REGS:
		{
			// Next word is the address of the first register to write
			lpRequest->wAddr = MAKEWORD(buffer[n+1], buffer[n]); 
			n += 2;

			// Next word is the quantity of registers to write
			lpRequest->wQty = MAKEWORD(buffer[n+1], buffer[n]);
			n += 2;

			// Next byte is the number of data bytes in the write request
			nDataBytes = buffer[n++];

			// Check that the number of data bytes matches quantity of regs to write
			if( nDataBytes != lpRequest->wQty * 2 )
				return ILLEGAL_VALUE;

			// Check for valid quantity of regs to write
			if( lpRequest->wQty < 1 || lpRequest->wQty > MAX_WRITE_REGS )
				return ILLEGAL_VALUE;

			// Copy the values from the buffer (must swap bytes for Intel).
			_swab( (char *) &buffer[n], (char *) lpRequest->awData, nDataBytes );
			n += nDataBytes;
			break;
		}

		default:
			return ILLEGAL_FUNCTION;
    }

	// Check for having parsed past the end of the data bytes actually received
	if( n > wRecv )
	{
		return ILLEGAL_VALUE;
	}

	return 0;
}


/*
 *	ProcessRequest() function
 *
 *	Processes a client request by transferring values to/from 
 *	the 'Registers' data area:
 *	  For 'read requests', data values are transferred from the 
 *	  'Registers' data area to the 'awData' field of the REQUEST struct.
 *
 *	  For 'write requests', the data values provided in the 'awData' 
 *	  field are transferred to the 'Registers' data area.
 *
 *	Parameters:
 *		[in/out] lpRequest  REQUEST struct containing the request info and data
 *							
 *	Returns zero if no processing error occured; otherwise, 
 *	returns the Modbus exception code for the error.
 */
BYTE CModbusClient::ProcessRequest( LPREQUEST lpRequest )
{
	BYTE nRet = 0;

    switch( lpRequest->bFnCode )
    {
		case MODBUS_READ_REGS:  // Get 'Register' data values per read request
			if( !GetRegisters( lpRequest->wAddr, lpRequest->wQty, lpRequest->awData ) )
				nRet = ILLEGAL_ADDRESS;
			break;

        case MODBUS_WRITE_REGS:  // Set 'Register' data values per write request
            if( !SetRegisters( lpRequest->wAddr, lpRequest->wQty, lpRequest->awData ) )
				nRet = ILLEGAL_ADDRESS;
			break;
    }

	return nRet;
}


/*
 *	MakePositiveResponse() function
 *
 *	Creates a positive response to send to the client.
 *
 *	Parameters:
 *		[in]     lpRequest  REQUEST struct containing the request info and data
 *		[in/out] buffer     The buffer for composing response to send to client
 *
 *	Note that 'buffer' must contain the client's request when this function is 
 *	called, and it will contain the response for client when this function returns.
 *							
 *	Returns the length (in bytes) of the response.
 */
WORD CModbusClient::MakePositiveResponse( LPREQUEST lpRequest, LPBYTE buffer )
{
	BYTE bQtyData;
	
	// Start response after the MBAP header
	WORD n = LEN_HEADER;

	// Echo request function code back to client
	buffer[n++] = lpRequest->bFnCode;

	switch( lpRequest->bFnCode )
    {
        case MODBUS_READ_REGS:
        {
			// Next byte is the length of the data read (in bytes)
			bQtyData = (BYTE) (lpRequest->wQty * 2);
            buffer[n++] = bQtyData;

            // Now copy the data values to the buffer (must swap bytes for Intel)
			_swab( (char *) lpRequest->awData, (char *) &buffer[n], bQtyData );
			n += bQtyData;
            break;
        }

        case MODBUS_WRITE_REGS:
        {
			// Simply echo the next two words back to client
			n += 4;
            break;
        }
    }

    return n;
}


/*
 *	MakeErrorResponse() function
 *
 *	Creates a Modbus exception response to send to the client.
 *
 *	Parameters:
 *		[in]     bError  The error code for sending in the response
 *		[in]     bFnCode The Modbus function code of the request
 *		[in/out] buffer  The buffer for composing response to send to client
 *
 *	Note that 'buffer' must contain the client's request when this function is 
 *	called, and will it contain the response for client when this function returns.
 *							
 *	Returns the length (in bytes) of the response.
 */
WORD CModbusClient::MakeErrorResponse( BYTE bError, BYTE bFnCode, LPBYTE buffer )
{
	// Start response after the MBAP header
	WORD n = LEN_HEADER;

	// Echo request function code with high-order bit set
    buffer[n++] = (BYTE) (bFnCode | 0x80);

	// Next byte is the Modbus error code
    buffer[n++] = bError;
    return n;
}


/*
 *	SendResponse() function
 *
 *	Sends a Modbus/TCP response to the client.  Calling thread will be blocked
 *	until the complete response is sent or error.
 *
 *	Parameters:
 *		[in] socket        The socket to be used for sending response
 *		[in] buffer        The buffer containing the response
 *		[in] nBytesToSend  The length (in bytes) of the response
 *
 *	Returns TRUE if the send was successful; otherwise, returns FALSE
 */
BOOL CModbusClient::SendResponse( SOCKET socket, LPBYTE buffer, WORD nBytesToSend )
{
	int nRet;

	// Set response's 'Command Length' (third word of the MBAP header)
    WORD wCmdLen = nBytesToSend - (LEN_HEADER - LEN_DESTID);
    buffer[4] = HIBYTE(wCmdLen); // High-order byte of 'Command Length'
    buffer[5] = LOBYTE(wCmdLen); // Low-order byte of 'Command Length'

	// Send response
	nRet = send( socket, (char*) buffer, nBytesToSend, 0 );
	if( nRet == SOCKET_ERROR )
	{
	    CLogger::GetInstance()->Log(LOG_ERROR, _T("send()"), WSAGetLastError() );
	    return FALSE;
	}

	if( nRet != nBytesToSend )
	{
		// Should never get here
		CLogger::GetInstance()->Log(LOG_ERROR, _T("ERROR: Failed to send complete response to client."));
	    return FALSE;
	}

	return TRUE;
}

BOOL CModbusClient::SetRegisters( int iStart, int nQty, LPWORD lpData )
{
	// validate addresses before transferring any values
	BOOL fOK = (iStart >= 0) && (iStart + nQty <= N_REGISTERS);
	if( fOK )
	{
		EnterCriticalSection( &csRegs );
		memcpy( &awRegs[iStart], lpData, nQty * 2 );
	    LeaveCriticalSection( &csRegs );
	}

	return fOK;
}


/*
 *	GetRegisters() function
 *
 *	Retrieves the values of a range of 'Registers'.
 *
 *	Parameters:
 *		[in]  iStart	The address of first 'Register' to retrieve
 *		[in]  nQty		The number of 'Registers' to retrieve
 *		[out] lpData	Pointer to array of words to receive the values
 *
 *	Returns TRUE if able to retrieve the range of values; otherwise,
 *	returns FALSE (because a specifed address does not exist).
 */
BOOL CModbusClient::GetRegisters( int iStart, int nQty, LPWORD lpData )
{
	// validate addresses before transferring any values
	BOOL fOK = (iStart >= 0) && (iStart + nQty <= N_REGISTERS);
	if( fOK )
	{
		EnterCriticalSection( &csRegs );
		memcpy( lpData, &awRegs[iStart], nQty * 2 );
	    LeaveCriticalSection( &csRegs );
	}
	
	return fOK;
}