myprotocol.c

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

/* myProtocol.c - Example custom protocol for vxWorks (Modbus over Ethernet) */

/* Copyright 1992-2002 Wind River Systems, Inc. 
   
THIS CODE IS FOR EXAMPLE PURPOSES ONLY, USE AT YOUR OWN RISK, NO WARRANTY 
IS ASSUMED OR IMPLIED   

*/

/*
modification history
--------------------
01b,26jul02,ean  little endian fixes
01a,15jul02,brk  created

*/

/*
DESCRIPTION
This protocol example implements the Modbus protocol over Ethernet. This is not the published 
open standard, which implements Modbus over TCP/IP, (Please see www.modbus.org ). 
In our example the TCP/IP layers are eliminated for the purpose illustrating the use of a custom 
protocol on vxWorks. Modbus was chosen because it is a very simple open standard, 
and the author is familiar with it. 

For those not familiar with Modbus it is an industrial master/slave protocol.
The slaves are the publishers of 1 bit and 16 bit numeric data, originally used for 
communication between devices in an RS232 or RS485 serial network. 

*/


#include "stdarg.h"
#include "vxWorks.h"
#include "net/mbuf.h"
#include "myProtocol.h"
#include "net/protosw.h"
#include "end.h"
#include "netinet/in.h"
#include "netinet/ip.h"
#include "netinet/if_ether.h"
#include "private/muxLibP.h"
#include "string.h"
#include "stdlib.h"
#include "bootLib.h"
#include "fioLib.h"

/* the current protocol database is at http://www.iana.org/assignments/ethernet-numbers */ 
/* It notes that any type below 0x600 can be considered 802.1, 
/* but as of this writting 700s are unassigned, so we pick somthing in this range */ 

#define MUX_PROTO_MODBUS  0x777 

extern BOOT_PARAMS     sysBootParams;		/* parameters from boot line         */

/* globals */
uchar_t inputs[(NUMBER_OF_INPUTS/sizeof(uchar_t))+1];
uchar_t coils[(NUMBER_OF_COILS/sizeof(uchar_t))+1];
ushort_t inputRegisters[NUMBER_OF_INPUT_REGISTERS];
ushort_t holdingRegisters[NUMBER_OF_HOLDING_REGISTERS];

int myProtocolDebug = 0;

/* locals */
static BOOL protoState;

/* forward declarations */
static void myProtoNptShutdown ( void * callbackId);
static int myProtoNptInt(  void * callbackId, long type, M_BLK_ID pMblk, void *	pSpareData);
static int myProtocolInt(  void * pCookie, long type, M_BLK_ID pMblk, 
				LL_HDR_INFO * pLinkHdrInfo, void * pMode);
static void modbusSendError( MODBUS_PROTO_CTRL * pProtoCtrl, uchar_t errornum);
LOCAL void myProtocolError(  END_OBJ * 	pEnd, END_ERR * pError, void * 	pSpare );
LOCAL void myProtocolShutdown(  void * 	pEnd, void * pError);
LOCAL void myProtocolTxRestart(  END_OBJ * 	pEnd, END_ERR * pError, void * 	pSpare );
LOCAL void myProtoNptError ( void * ipCallbackId, END_ERR* pError);
   
/******************************************************************************
*
* myProtocolInit - Initialize the Protocol
*
* This function allocates resources to intialize the protocol 
* This function does the actual attachment of the protocol to the MUX 
* notice MUX_PROTO_MODBUS is the ethernet type of the protocol
*
*/
void * myProtocolInit(
   char *		pDevice,	/* Device (ln, ie, etc.) that we */
    					/* wish to bind to. */
    int         	unit            /* unit number (0, 1, etc.) */
)
{
   MODBUS_PROTO_CTRL *       pProtoCtrl;
   END_OBJ * pEnd;
   static BOOL     myProtoInit = FALSE;

   /* allocate a protocol control structure */

   if (NULL== (pProtoCtrl=(MODBUS_PROTO_CTRL *) malloc(sizeof (MODBUS_PROTO_CTRL))))
	return (NULL); 	

  /* bind the protocol to the Mux device */

    /* if device to bind to the protocol is NPT use the NPT toolkit function
       to bind it, otherwise use the muxBind function */
	
   if (muxTkDrvCheck (pDevice) == TRUE)
	{
	if ((pProtoCtrl->pCookie = (void *)muxTkBind (pDevice, unit, myProtoNptInt,
					    myProtoNptShutdown,
                                            NULL,
                                            myProtoNptError, 
                                            MUX_PROTO_MODBUS,
					   "Modbus",
					   pProtoCtrl, NULL, NULL)) == NULL)
	    {
	    return (NULL);
	    }
	}
   else  /* END */
	{
	if ((pProtoCtrl->pCookie = (void *) muxBind (pDevice, unit, myProtocolInt, 
					   (FUNCPTR)myProtocolShutdown,
                                           NULL,
                                           myProtocolError, 
                                           MUX_PROTO_MODBUS,
					   "Modbus", 
                                           (void *)pProtoCtrl)) == NULL)
    
	    {
	    return (NULL);
	    }

	}

   /* get a working buffer of the MTU size */
  
   pEnd = PCOOKIE_TO_ENDOBJ(pProtoCtrl->pCookie);

   pProtoCtrl->mtuSize = pEnd->mib2Tbl.ifMtu;
   pProtoCtrl->busy = FALSE;


   if (NULL == (pProtoCtrl->pBuffer = memalign (4,pProtoCtrl->mtuSize)))
   {
	/* TODO: we should detach the proto */
        free(pProtoCtrl);
        return (NULL);
   }
   myProtoInit = TRUE;
   return ((void * )pProtoCtrl); 
   
}


/******************************************************************************
*
* myProtoNptInt - NPT driver interrupt handler
*
* This is really a MUX receive routine, even though it has a 'Int in the name.
* NPT drivers get handed the full frame so they can do things with it,
* but in any case we can use the address resolution function registered with
* mux.
*/

static int myProtoNptInt
    (
    void * 	callbackId, 	/* call back Id supplied during muxTkBind */
    long 	type, 		/* our network service type */
    M_BLK_ID 	pMblk,		/* the packet */
    void * 	pSpare	        /* any spare data; we ignore it */
    )
    {
    MODBUS_PROTO_CTRL * pProtoCtrl = (MODBUS_PROTO_CTRL *)callbackId;
    LL_HDR_INFO llHdrInfo;

    if(ERROR==muxPacketDataGet(pProtoCtrl->pCookie,pMblk,&llHdrInfo))
	return(ERROR);	
    else 
	return (myProtocolInt (pProtoCtrl->pCookie, type, pMblk,
                               &llHdrInfo,pProtoCtrl));
    }

/********************************************************************************
* myProtocolShutdown - dummy shutdown routine for muxTkBind 
*
* RETURNS: N/A
*
*/

static void myProtocolShutdown
    (
    void * 	pCookie, 	/* protocol/device binding from muxBind() */
    void * 	pSpare 		/* spare pointer from muxBind() */
    )
    {
    MODBUS_PROTO_CTRL * pProtoCtrl = (MODBUS_PROTO_CTRL *)pSpare;

    if (muxUnbind (pProtoCtrl->pCookie,MUX_PROTO_MODBUS, myProtocolInt) != OK)
	{
	logMsg ("Could not un-bind from the  MUX!", 0, 0, 0, 0, 0, 0);
	return;
	}

    free(pProtoCtrl->pBuffer);
    pProtoCtrl->pCookie = NULL;

    return;
    }
/********************************************************************************
* myProtoNptShutdown - dummy shutdown routine for muxTkBind 
*
* RETURNS: N/A
*
*
*/

static void myProtoNptShutdown
    (
    void * 	callbackId 	/* call back Id supplied during muxTkBind */
    )
    {
    MODBUS_PROTO_CTRL * pProtoCtrl = (MODBUS_PROTO_CTRL *)callbackId;

    if (muxUnbind (pProtoCtrl->pCookie,MUX_PROTO_MODBUS, myProtoNptInt) != OK)
	{
	logMsg ("Could not un-bind from the  MUX!", 0, 0, 0, 0, 0, 0);
	return;
	}

    free(pProtoCtrl->pBuffer);
    pProtoCtrl->pCookie = NULL;

    return;
    }

/******************************************************************************
*
* myProtocolInt - END driver protocol interrupt handler
*
* This is really a MUX receive routine, even though it has a 'Int in the name.
*  
*
* RETURNS: TRUE if the packet was for us and can be put back in the pool
*          FALSE otherwise, and the MUX will look for other registed protocols.
*
* 
*/

static int myProtocolInt
    (
    void * 		pCookie,     /* pointer Identifying the MUX */
    long 		type,        /* type of packet */
    M_BLK_ID 		pMblk,       /* pointer to pMblk of recieved data */
    LL_HDR_INFO * 	pLinkHdrInfo,  /* decoded link level information structure */
    void * pSpare    /* spare callback pointer, use it for whatever you want */     
    )
    {
 /*   struct mbuf * 	pMbuf; */
    int			size;
    MODBUS_PROTO_CTRL * pProtoCtrl = (MODBUS_PROTO_CTRL *)pSpare;
    

    /* input buffer already in use - drop this packet */

    if (pProtoCtrl->busy)
	{
        if (myProtocolDebug)
	    logMsg ("Input busy!\n", 1, 2, 3, 4, 5, 6);
	return (FALSE);
	}

    pProtoCtrl->busy = TRUE;

    /* Check the type before doing anything expensive. */
   	
    if (type != MUX_PROTO_MODBUS )  /* is not our type */
        {
        if (myProtocolDebug)
            logMsg ("Type != MUX_PROTO_MODBUS\n", 1, 2, 3, 4, 5, 6);
        goto myProtocolIntError;
        }

    /* make sure this is a packet header */ 	

    size = pLinkHdrInfo->dataOffset+1;

    if (!(pMblk->mBlkHdr.mFlags & M_PKTHDR) || (pMblk->mBlkPktHdr.len <
                                                size))
        {
        if (myProtocolDebug)
	    logMsg ("bad header!\n", 1, 2, 3, 4, 5, 6);
        goto myProtocolIntError; 
        }

    /* here is where you can throw out your packet if the address form is wrong */ 

    /* store the address of the sender, for our reply */	
    bcopy ((char *)pMblk->mBlkHdr.mData + pLinkHdrInfo->srcAddrOffset,
          (char *)pProtoCtrl->returnAddress, 
           pLinkHdrInfo->srcSize);

    /* condense the data into buffer */
    /* in the real world you probably want to transfer the data to a mblk in a private mblk pool */	

    if ((pProtoCtrl->bufferSize = netMblkOffsetToBufCopy (pMblk, pLinkHdrInfo->dataOffset,
                                        pProtoCtrl->pBuffer, pProtoCtrl->mtuSize, NULL)) == 0)
	{
        if (myProtocolDebug)
	    logMsg ("no buffer!\n", 1, 2, 3, 4, 5, 6);
        goto myProtocolIntError;
	}

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

    if(modbusReceive(pProtoCtrl))
	{
	  /* returning TRUE means we handled the packet 
		and we must free the the buffer passed to us from the driver */
	 netMblkClChainFree (pMblk);		
         return (TRUE); 
	}
    	
    myProtocolIntError:
   	 {
	/* returning false means we did not handle the packet and it can be passed 
          to other protocols, if any */         
        
         pProtoCtrl->busy = FALSE;
         return (FALSE); 
         }
}    

LOCAL void myProtoNptError
    (
    void * ipCallbackId,  /* Sent down in muxTkBind call. */
    END_ERR* pError       /* Error message */
    )
    {
    MODBUS_PROTO_CTRL * pDrvCtrl = (MODBUS_PROTO_CTRL *)ipCallbackId;
    END_OBJ * pEnd  = NULL;

    if (pDrvCtrl)
	 pEnd = PCOOKIE_TO_ENDOBJ (pDrvCtrl->pCookie);

    if (pDrvCtrl == NULL || pEnd == NULL)
        return;

    myProtocolError (pEnd, pError, pDrvCtrl);

    return;
    }


LOCAL void myProtocolError
    (
    END_OBJ * 	pEnd, 		/* END reporting the error */
    END_ERR * 	pError, 	/* the error message */
    void * 	pSpare 		/* spare pointer from muxBind() */
    )
    {
    int s;
    MODBUS_PROTO_CTRL * pDrvCtrl = (MODBUS_PROTO_CTRL *)pSpare;

    if (pEnd == NULL || pDrvCtrl == NULL)
    	return;

    switch (pError->errCode)
	{
        case END_ERR_INFO:
            if (myProtocolDebug && pError->pMesg != NULL)
                logMsg ("INFO: Device: %s Unit: %d Msg: %s\n",
                        (int)pEnd->devObject.name, pEnd->devObject.unit,
                        (int)pError->pMesg, 0, 0, 0);
                break;
        case END_ERR_WARN:
            if (myProtocolDebug && pError->pMesg != NULL)
                logMsg ("WARN: Device: %s Unit: %d Msg: %s\n",
                        (int)pEnd->devObject.name, pEnd->devObject.unit,
                        (int)pError->pMesg, 0, 0, 0);
                break;
        case END_ERR_RESET:
            if (myProtocolDebug && pError->pMesg != NULL)
                logMsg ("RESET: Device: %s Unit: %d Msg: %s\n",
                        (int)pEnd->devObject.name, pEnd->devObject.unit,
                        (int)pError->pMesg, 0, 0, 0);
            break;

        case END_ERR_UP:
            if (myProtocolDebug && pError->pMesg != NULL)
                logMsg ("UP: Device: %s Unit: %d Msg: %s\n",
                        (int)pEnd->devObject.name, pEnd->devObject.unit,
                        (int)pError->pMesg, 0, 0, 0);
            break;
        case END_ERR_DOWN:
            if (myProtocolDebug && pError->pMesg != NULL)
                logMsg ("DOWN: Device: %s Unit: %d Msg: %s\n",
                        (int)pEnd->devObject.name, pEnd->devObject.unit,
                        (int)pError->pMesg, 0, 0, 0);
            break;

        case END_ERR_FLAGS:
            if (myProtocolDebug && pError->pMesg != NULL)
                logMsg ("ipError:Msg from device %s Unit: %d Msg: %s\n",
                        (int)pEnd->devObject.name, pEnd->devObject.unit,
                        (int)pError->pMesg, 0, 0, 0);
            break;

/*     this very useful error message is only avalible in Tornado 2.2/ vxWorks 4.5 and
       later versions, it allows the driver to notify the protocol layer that it is out of buffers,
       this allows the protocol to free any non-critical buffers lent up the stack from the
	   driver's pool so that transmission can continue 
     
        case END_ERR_NO_BUF:
            if (myProtocolDebug && pError->pMesg != NULL)