myprotocol.c

THIS CODE IS FOR EXAMPLE PURPOSES ONLY, USE AT YOUR OWN RISK, NO WARRANTY IS ASSUMED OR IMPLIED

                logMsg ("NO_BUF: Device: %s Unit: %d Msg: %s\n",
                        (int)pEnd->devObject.name, pEnd->devObject.unit,
                        (int)pError->pMesg, 0, 0, 0);
            break; */
        default:
            if (myProtocolDebug && pError->pMesg != NULL)
                logMsg ("UNKOWN ERROR: Device: %s Unit: %d Msg: %s\n",
                        (int)pEnd->devObject.name, pEnd->devObject.unit,
                        (int)pError->pMesg, 0, 0, 0);
		break;
        }
    }        

/******************************************************************************
*
* myProtocolTx - transmit data
*
* Allocates memory from the drivers pool and then copies the data into the  
* allocated mBlk Tuple and sends it.
*
* RETURNS: OK or ERROR
*
*/

int myProtocolTx(    
	void * pCookie,
	char * data,
	int dataSize,
	char * dstMacAddr
    )
    {
  
    int sendStatus = 0;
    END_OBJ *	pEnd;
    M_BLK_ID    pOutBlk;
    
    if (myProtocolDebug)
	logMsg ("entering send\n", 1, 2, 3, 4, 5, 6);

    /* get a cluster from the drivers pool */

    pEnd = PCOOKIE_TO_ENDOBJ(pCookie);

    if ((pOutBlk = netTupleGet (pEnd->pNetPool, dataSize+SIZEOF_ETHERHEADER, M_DONTWAIT, MT_DATA,
                              FALSE)) == NULL)
	{
        if (myProtocolDebug)
                logMsg ("Could not send, driver pool empty!\n", 1, 2, 3, 4, 5, 6);

  	return (ERROR);
	}

    /* Make sure we know that the outgoing buffer is clean/empty */
    pOutBlk->mBlkHdr.mLen  = 0;
    pOutBlk->mBlkHdr.mData = pOutBlk->pClBlk->clNode.pClBuf;

    /* increment the data pointer so that muxAddressForm can prepend header 
       without chaining blocks */
    pOutBlk->mBlkHdr.mData +=	SIZEOF_ETHERHEADER;

    /* copy our data into the allocated mBlk */
    bcopy(data, pOutBlk->mBlkHdr.mData, dataSize);
    
    pOutBlk->mBlkPktHdr.len = pOutBlk->mBlkHdr.mLen = dataSize;

    if (myProtocolDebug)
        logMsg ("Data copied !\n", 1, 2, 3, 4, 5, 6);

    /* the muxTkSend will prepend the correct ethernet header using the MAC address
        for non-Ethernet connections a different address resolution function is associated 
		with the interface and all that is necessary is for the protocol to pass the physical address
	    in the correct format */ 

    if (muxTkSend(pCookie, pOutBlk, dstMacAddr,
		       htons(MUX_PROTO_MODBUS), NULL) == OK)
            {
            if (myProtocolDebug)
                logMsg ("Sent regular packet!\n", 1, 2, 3, 4, 5, 6);
            }
        else
            {
            if (myProtocolDebug)
                logMsg ("Could not send regular packet!\n", 1, 2, 3, 4, 5, 6);
	    return (ERROR);	
            }
        
    
    return (OK);
    }

/******************************************************************************
*
*  modbusReceive - validate and process the protocol address layer
*
*  In this case there is no equivalent of an IP address for this layer,
*  so this function is very simple
*
*  RETURNS: TRUE or FALSE
*
*/


int modbusReceive 
	(
	MODBUS_PROTO_CTRL * pProtoCtrl
	)
{
	/* Modbus addressing is implimenation defined, so for this implimentation
           we add one char for the type of message, request or reply,
           that way any node can be both a master (or client) and slave 
           ( or server ) */

	MODBUS_HEADER * pHdr = (MODBUS_HEADER * )pProtoCtrl->pBuffer;
        
	switch(pHdr->type)
		{
		case MODBUS_REQUEST: 
			return(modbusRxRequest(pProtoCtrl));


		case MODBUS_REPLY:
			return(modbusRxReply(pProtoCtrl));

		default:
			pProtoCtrl->busy = FALSE;
			/* unknown type ? */
  		        if (myProtocolDebug)
				logMsg ("Unknown modbus type!", 0, 0, 0, 0, 0, 0);
			return(FALSE);

		}
}

void printIt ( char * buffer){ printf( buffer); free(buffer); }


/******************************************************************************
*
* modbusRxReply - process a RX data as a MODBUS master 
*
* RETURNS: TRUE if the packet was for us and we processed it
*          FALSE otherwise, and the MUX will look for other registed protocols.
*
* RETURNS: OK or ERROR
*
*/

int modbusRxReply 
	(
	MODBUS_PROTO_CTRL * pProtoCtrl
	)
{
	MODBUS_HEADER * pHdr = (MODBUS_HEADER * )pProtoCtrl->pBuffer;
 	
	char	* outputBuf = malloc(256);
	ushort_t address;
	int len,count;
	char    slaveName[30]={""};
        ushort_t * data;                   /* data (as ushort array) */ 
        ushort_t temp;
      
	address=ntohs(pHdr->address);     
   
        sprintf(slaveName,"%02x:%02x:%02x:%02x:%02x:%02x",
                (int)pProtoCtrl->returnAddress[0],
                (int)pProtoCtrl->returnAddress[1],
                (int)pProtoCtrl->returnAddress[2],
                (int)pProtoCtrl->returnAddress[3],
                (int)pProtoCtrl->returnAddress[4],
                (int)pProtoCtrl->returnAddress[5]);	

	switch(pHdr->function)
		{
		case  MODBUS_Read_Coils:
		       len = sprintf(outputBuf,"Read Coils %4hu@%s",address,slaveName );	
		       for( count=0; count< pHdr->u.rBytes; count++) 
		       		len += sprintf(&outputBuf[len],"%2x ",(int)pProtoCtrl->pBuffer[count+3] );	
	 	        len = sprintf(outputBuf[len],"\n" );	
 			break;  
		case  MODBUS_Read_Discrete_Inputs:
		       len = sprintf(outputBuf,"Read Inputs %4hu@%s",address,slaveName );	
		       for( count=0; count< pHdr->u.rBytes; count++) 
		       		len += sprintf(outputBuf[len],"%2x ",(int)pProtoCtrl->pBuffer[count+3] );	
	 	        len = sprintf(outputBuf[len],"\n" );	
 			break;  
		case  MODBUS_Read_Holding_Registers:
		       len = sprintf(outputBuf,"Read Holding Registers @%s ",slaveName );	
		       data= (ushort_t *)(&pProtoCtrl->pBuffer[3]);
		       for( count=0; count< (pProtoCtrl->pBuffer[2]/2); count++) 
				{
				temp=ntohs(data[count]);
		       		len += sprintf(&outputBuf[len],"%4hu ",temp );	
				}
	 	        len = sprintf(outputBuf[len],"\n" );	
 			break;  
		case  MODBUS_Read_Input_Registers:
		       len = sprintf(outputBuf,"Read Input Registers @%s ",slaveName );	
		       data= (ushort_t *)(&pProtoCtrl->pBuffer[3]);
		       for( count=0; count< (pProtoCtrl->pBuffer[2]/2); count++) 
				{
				temp=ntohs(data[count]);
		       		len += sprintf(&outputBuf[len],"%4hu ",temp );	
				}
		        len = sprintf(outputBuf[len],"\n" );	
 			break;  
		case  MODBUS_Write_Single_Coil:
		       len = sprintf(outputBuf,"Write Single Coil %4hu@%s OK\n",address,slaveName );	
 			break;  
		case  MODBUS_Write_Single_Register:
		       len = sprintf(outputBuf,"Write Single Register %4hu@%s OK\n",address,slaveName );	
 			break;  
		case  MODBUS_Write_Multiple_Coils:
		       len = sprintf(outputBuf,"Write Multiple Coils %4hu@%s OK\n",address,slaveName );	
 			break;  
		case  MODBUS_Write_Multiple_registers:
		       len = sprintf(outputBuf,"Write Multiple Register %4hu@%s OK\n",address,slaveName );	
 			break;  
		case  MODBUS_Read_File_Record:
		       len = sprintf(outputBuf,"Read File Record %4hu@%s OK\n",address,slaveName );	
 			break;  
		case  MODBUS_Mask_Write_Register:
		       len = sprintf(outputBuf,"Mask Write Register %4hu@%s OK\n",address,slaveName );	
 			break;  
		case  MODBUS_Read_Write_Multiple_registers:
		       len = sprintf(outputBuf,"Read Write Multiple registers %4hu@%s OK\n",address,slaveName );	
 			break;  
		case  MODBUS_Read_Device_Identification:
		       len = sprintf(outputBuf,"Read Device Identification %4hu@%s OK\n",address,slaveName );	
 			break;  

		case  MODBUS_ERROR|MODBUS_Read_Coils:
		       len = sprintf(outputBuf,"Write Multiple Coils @%s ERROR",slaveName );	
 			goto errorType;  
		case  MODBUS_ERROR|MODBUS_Read_Discrete_Inputs:
		       len = sprintf(outputBuf,"Write Multiple Coils @%s ERROR",slaveName );	
 			goto errorType;  
		case  MODBUS_ERROR|MODBUS_Read_Holding_Registers:
		       len = sprintf(outputBuf,"Read Holding Registers @%s ERROR",slaveName );	
 			goto errorType;  
		case  MODBUS_ERROR|MODBUS_Read_Input_Registers:
		       len = sprintf(outputBuf,"Read Input Registers @%s ERROR",slaveName );	
 			goto errorType;  
		case  MODBUS_ERROR|MODBUS_Write_Single_Coil:
		       len = sprintf(outputBuf,"Read Input Registers @%s ERROR",slaveName );	
 			goto errorType;  
		case  MODBUS_ERROR|MODBUS_Write_Single_Register:
		       len = sprintf(outputBuf,"Write Single Register @%s ERROR",slaveName );	
 			goto errorType;  
		case  MODBUS_ERROR|MODBUS_Write_Multiple_Coils:
		       len = sprintf(outputBuf,"Write Multiple Coils @%s ERROR",slaveName );	
 			goto errorType;  
		case  MODBUS_ERROR|MODBUS_Write_Multiple_registers:
		       len = sprintf(outputBuf,"Write Multiple Register @%s ERROR",slaveName );	
 			goto errorType;  
		case  MODBUS_ERROR|MODBUS_Read_File_Record:
		       len = sprintf(outputBuf,"Read_File_Record @%s ERROR",slaveName );	
 			goto errorType;  
		case  MODBUS_ERROR|MODBUS_Mask_Write_Register:
		       len = sprintf(outputBuf,"Mask_Write_Register @%s ERROR",slaveName );	
 			goto errorType;  
		case  MODBUS_ERROR|MODBUS_Read_Write_Multiple_registers:
		       len = sprintf(outputBuf,"Read_Write_Multiple_registers @%s ERROR",slaveName );	
 			goto errorType;  
		case  MODBUS_ERROR|MODBUS_Read_Device_Identification:
		       len = sprintf(outputBuf,"Read_Device_Identification @%s ERROR",slaveName );	
 			goto errorType;  
		}
        pProtoCtrl->busy = FALSE;
	excJobAdd( printIt, outputBuf,0,0,0,0);
 	return (TRUE);

errorType:
	switch(pProtoCtrl->pBuffer[2])
		{
		case 0x1:	
			sprintf(&outputBuf[len],":Unsupported Function\n" );	
			break;
		case 0x2:	
			sprintf(&outputBuf[len],":Illegal Data Request\n" );	
			break;
		case 0x3:	
			sprintf(&outputBuf[len],":Illegal Data Value\n" );	
			break;
		case 0x4:	
			sprintf(&outputBuf[len],":Slave Device Failure\n" );	
			break;
		case 0x5:	
			sprintf(&outputBuf[len],":Acknowledge, request processing\n" );	
			break;
		case 0x6:	
			sprintf(&outputBuf[len],":Slave Busy\n" );	
			break;
		case 0x8:	
			sprintf(&outputBuf[len],":Parity Error\n" );	
			break;
		case 0xA:
			sprintf(&outputBuf[len],":Gateway Error\n" );	
			break;
		case 0xB:
			sprintf(&outputBuf[len],":Device Not Responding\n" );	
			break;
		default:
			sprintf(&outputBuf[len],":Unknown\n" );	
			break;
                }
        pProtoCtrl->busy = FALSE;
	excJobAdd( printIt, outputBuf,0,0,0,0);
 	return (TRUE);

}

/******************************************************************************
*
* modbusRxRequest - process a RX data as a MODBUS slave 
*
* This follows the MODBUS standard fairly strictly, some of the error returns
* are a bit sloppy and not all functions are supported. The same buffer is 
* used to compose the reply message as the request came in on. Modbus registers are 
* always 16bit in BIG_ENDIAN order, this is why htons() is used. 
*
* RETURNS: TRUE if the packet was for us and has been processed.
*          FALSE otherwise, and the MUX will look for other registed protocols.
*
*
*/

int modbusRxRequest 
	(
	MODBUS_PROTO_CTRL * pProtoCtrl
	)
{
	MODBUS_HEADER * pHdr = (MODBUS_HEADER * )pProtoCtrl->pBuffer;
	int shift,count;
	uchar_t mask;
	ushort_t address;
	ushort_t size;
	ushort_t bytes;
        ushort_t * data;                   /* data (as ushort array) */ 


	if(pProtoCtrl->bufferSize > 257)
		{
		pProtoCtrl->busy = FALSE;
		return(ERROR); 
		}

	pHdr->type= MODBUS_REPLY;

	switch(pHdr->function)
		{
		case  MODBUS_Read_Coils:
		    	if (myProtocolDebug)
				logMsg ("Modbus Request MODBUS_Read_Coils\n", 1, 2, 3, 4, 5, 6);
			address=htons(pHdr->address);
			size=htons(pHdr->u.size);
			bytes = (size/8);
			shift = (size%8);                      
			if(shift) bytes++;
			
                        if (((address+size) > NUMBER_OF_COILS)
                                 || (pProtoCtrl->bufferSize < 6) 
                                 || (bytes > 256) )
				{
			        modbusSendError(pProtoCtrl,0x02);
				break;
				}

			/* compose reply in buffer */
   			pHdr->u.rBytes = (uchar_t)bytes;
                        bcopy( &coils[address/8],&(pProtoCtrl->pBuffer[3]),bytes );

			/* align if necessary */
   			if (shift)                     
				for (count=0;count<bytes;count++)
				{ 
					pProtoCtrl->pBuffer[3+count]<<=shift;	
					if (count!=(bytes-1))
						pProtoCtrl->pBuffer[3+count] &= pProtoCtrl->pBuffer[4+count]>>shift;
				}
			/* transmit reply */
			pProtoCtrl->bufferSize = 3+ bytes;
			break;

		case  MODBUS_Read_Discrete_Inputs:
		    	if (myProtocolDebug)
				logMsg ("Modbus Request MODBUS_Read_Discrete_Inputs\n", 1, 2, 3, 4, 5, 6);
			address=ntohs(pHdr->address);
			size=ntohs(pHdr->u.size);
			bytes = (size/8);
			shift = (size%8);                      
			if(shift) bytes++;
			
                        if (((address+size) > NUMBER_OF_INPUTS)
                                 || (pProtoCtrl->bufferSize < 6) 
                                 || (bytes > 256) ) 
				{
			        modbusSendError(pProtoCtrl,0x02);
				break;
				}

			/* compose reply in buffer */
			pHdr->type= MODBUS_REPLY;
   			pHdr->u.rBytes = (uchar_t)bytes;
			
                        bcopy( inputs,&(pProtoCtrl->pBuffer[3]),bytes );
   			if (shift)                     
				for (count=0;count<bytes;count++)
				{ 
					pProtoCtrl->pBuffer[3+count]<<=shift;	
					if (count!=(bytes-1))
						pProtoCtrl->pBuffer[3+count] &= pProtoCtrl->pBuffer[4+count]>>shift;
				}

			/* transmit reply */
			pProtoCtrl->bufferSize = 3+ bytes;
			break;