udp.c

vxworks下的UDP驱动程序

/* includes */
#include "vxWorks.h"
#include "sockLib.h"
#include "inetLib.h"
#include "stdioLib.h"
#include "strLib.h"
#include "ioLib.h"
#include "fioLib.h"
#include "Udp.h"

tagUdpCtrl      g_tUdpCtrl;
tagQueueUdpMsg  g_tQueueUdpMsg;

unsigned int g_uiLogUdp = 0;    // for print logmsg

//=============================================================================

void Udp_Initial( void )
{
    tagUdpConn      *ptConn;
    tagPQueueCtrl   ptQueueCtrl;
    tagPUdpMsgCtrl  ptUdpMsgCtrl;
    short           iLoop;

    g_tUdpCtrl.nConnNum     = 0;
    g_tUdpCtrl.nServerNum   = 0;
    g_tUdpCtrl.nGroupNum    = 0;

    // initial
    for( iLoop=0;iLoop<C_UDP_NUM_CONN;iLoop++ )
    {
        ptConn = &(g_tUdpCtrl.tConnArray[iLoop]);
        ptConn->iSvrNo   = -1;
        ptConn->iGroupNo = -1;
    }

    ptQueueCtrl = &g_tQueueUdpMsg.tQueueCtrl;
    ptQueueCtrl->ptSemID = semMCreate( (SEM_Q_PRIORITY | SEM_INVERSION_SAFE | SEM_DELETE_SAFE) );
    ptQueueCtrl->bFull        = FALSE;
    ptQueueCtrl->wSave        = 0;
    ptQueueCtrl->wQueueIDSave = 0;
    for( iLoop=0; iLoop<(CONST_NUM_HOST+2); iLoop++ )
    {
        ptQueueCtrl->wRead[iLoop]        = 0;
        ptQueueCtrl->wQueueIDRead[iLoop] = 0;
        ptQueueCtrl->wQueueIDAck[ iLoop] = 0;
    }
    for( iLoop=0; iLoop<CONST_NUM_RPTU; iLoop++ )
    {
        ptUdpMsgCtrl = &g_tQueueUdpMsg.tUdpMsgCtrl[iLoop];
        ptUdpMsgCtrl->wQueueID = 0;
        ptUdpMsgCtrl->wConnNo  = 0;
        ptUdpMsgCtrl->wGroupNo = -1;
        ptUdpMsgCtrl->wLength  = 0;
        ptUdpMsgCtrl->wHead    = 0;
        ptUdpMsgCtrl->wRear    = 0;
        ptUdpMsgCtrl->wCRC     = 0;
        ptUdpMsgCtrl->wTag     = 0;
    }

    return;
}

//==========================================================
// Save Udp Message to Common Buffer
BOOLEAN Udp_SaveMsg( short iGroupNo, short iConnNo, BYTE *pbyMsgBuf, WORD wMsgLen )
{
    tagWord_Byte2   m_WB2;
    tagPQueueCtrl   ptQueueCtrl;
    tagUdpMsgCtrl*  ptUdpMsgCtrl;
    WORD            wSave, wStartPos, wWritePos;
    BYTE            *pbyReportBuf;

    ptQueueCtrl  = &g_tQueueUdpMsg.tQueueCtrl;
    pbyReportBuf =  g_tQueueUdpMsg.byReportBuf;
    semTake( ptQueueCtrl->ptSemID, WAIT_FOREVER );              // 获取写权限

    wSave = ptQueueCtrl->wSave;
    if( wSave >=CONST_NUM_REPORT_CTRL_UP ) wSave = 0;           // !!! ERROR
    ptUdpMsgCtrl = &g_tQueueUdpMsg.tUdpMsgCtrl[wSave];
    wStartPos = ptUdpMsgCtrl->wHead;
    if( wStartPos >=CONST_VOL_REPORT_BUF_UP  ) wStartPos = 0;   // !!! ERROR
    if( TRUE==Fun_QueueBufSave(pbyReportBuf, pbyMsgBuf, wStartPos, wMsgLen, CONST_VOL_REPORT_BUF_UP) )
    {
        // update the control flag
        wWritePos = (wStartPos + wMsgLen - 1) % CONST_VOL_REPORT_BUF_UP;
        ptUdpMsgCtrl->wCRC    = Fun_CreateCrc16( pbyMsgBuf, wMsgLen );
        ptUdpMsgCtrl->wHead   = wStartPos;  // maybe have been changed
        ptUdpMsgCtrl->wRear   = wWritePos;
        ptUdpMsgCtrl->wGroupNo= iGroupNo;
        ptUdpMsgCtrl->wConnNo = iConnNo;
        ptUdpMsgCtrl->wLength = wMsgLen;

        ptUdpMsgCtrl->wQueueID   = ptQueueCtrl->wQueueIDSave;

        // make ready for the message coming next
        wSave++;
        wWritePos++;
        if( wSave >=CONST_NUM_REPORT_CTRL_UP ) wSave = 0;
        if( wWritePos >=CONST_VOL_REPORT_BUF_UP  ) wWritePos = 0;
        ptQueueCtrl->wQueueIDSave++;
        ptQueueCtrl->wSave = wSave;
        ptUdpMsgCtrl = &g_tQueueUdpMsg.tUdpMsgCtrl[wSave];
        ptUdpMsgCtrl->wHead  = wWritePos;
        ptUdpMsgCtrl->wRear  = wWritePos;
        ptUdpMsgCtrl->wLength  = 0;
        ptUdpMsgCtrl->wQueueID = ptQueueCtrl->wQueueIDSave;
    }

    semGive( ptQueueCtrl->ptSemID );                            // 释放写权限

    return TRUE;
}

/*********************************************************************
 * Start UdpServer, read from UDP socket                             */
int Udp_ServerStart( short iSvrNo )
{
    struct sockaddr_in  tServerAddr,tClientAddr;    /* server/client's socket address */
    char                inetAddr[INET_ADDR_LEN];    /* buffer for client's inet addr */
    BYTE                byMsgBuf[C_UDP_VOL_BUF];    /* Message from client */
    int                 sockAddrSize;               /* size of socket address structure */
    int                 sFd;                        /* socket file descriptor */
    int                 iMsgLen;                    // the length of mesage received from client
    WORD                wClientPort;
    short               iConnNo, iGroupNo;
    tagUdpServer* ptServer=&(g_tUdpCtrl.tSvrArray[iSvrNo]);

    if( iSvrNo >=g_tUdpCtrl.nServerNum )   return -1;

    /* set up the local address */
    sockAddrSize = sizeof (struct sockaddr_in);
    bzero ((char *) &tServerAddr, sockAddrSize);
    tServerAddr.sin_len          = (u_char) sockAddrSize;
    tServerAddr.sin_family       = AF_INET;
    tServerAddr.sin_port         = htons (ptServer->wPort);
    tServerAddr.sin_addr.s_addr  = ptServer->dwServerIP;

    /* create a UDP-based socket */
    if ((sFd = socket (AF_INET, SOCK_DGRAM, 0)) == ERROR)
    {
        if( g_uiLogUdp ) printf("udp socket Create Error\n");
        return -1;
    }

    /* bind socket to local address */
    if (bind (sFd, (tagPSockAddr) &tServerAddr, sockAddrSize) == ERROR)
    {
        if( g_uiLogUdp ) printf("Tcp Bind Error %x:%d\n",ptServer->dwServerIP,ptServer->wPort);
        close (sFd);
        return -1;
    }

    /* read data from a socket and satisfy requests */
    ptServer->iSockFD = sFd;
    for( ; ; )
    {
        iMsgLen = recvfrom( sFd, byMsgBuf, C_UDP_VOL_BUF, 0, (struct sockaddr *)&tClientAddr, &sockAddrSize );
        if( ERROR == iMsgLen )
        {
            if( g_uiLogUdp )
            {
                Dbg_RtcClock( );
                printf("udp server recv Error\n");
            }
            close (sFd);
            ptServer->iSockFD = ERROR;
            return -1;
        }

        /* convert inet address to dot notation */
        inet_ntoa_b( tClientAddr.sin_addr, inetAddr );
        wClientPort = ntohs( tClientAddr.sin_port );   // 网络端口号
        if( Udp_GetConnNo(iSvrNo,tClientAddr.sin_addr.s_addr,wClientPort,&iConnNo,&iGroupNo) )  // 查找属于哪一组
        {// save the msg to the buffer
            Udp_SaveMsg( iGroupNo, iConnNo, byMsgBuf, iMsgLen );
        }
     }
     ptServer->iSockFD = ERROR;
     return -1;
}


//=======================================
// 增加服务器,返回序号
short Udp_AddServer( BYTE *ServerIP, WORD wPort )
{
    tagUdpServer *ptServer, *ptSvrTmp;
    char    szTxt[20];
    WORD    nSvrNo;
    short   iLoop;

    if(g_tUdpCtrl.nServerNum >=C_UDP_NUM_SVR)
    {// 队列已满
        g_tUdpCtrl.nServerNum = C_UDP_NUM_SVR;
        return -1;
    }

    ptServer = &(g_tUdpCtrl.tSvrArray[g_tUdpCtrl.nServerNum]);
    sprintf( szTxt, "%d.%d.%d.%d", ServerIP[0], ServerIP[1], ServerIP[2], ServerIP[3] );
    ptServer->dwServerIP = inet_addr(szTxt);

    // 检查服务器是否已经在列表中
    for( iLoop=0;iLoop<g_tUdpCtrl.nServerNum;iLoop++ )
    {
        ptSvrTmp = &(g_tUdpCtrl.tSvrArray[iLoop]);
        if( (ptSvrTmp->wPort==wPort) && (ptServer->dwServerIP ==ptSvrTmp->dwServerIP) )
        {
            if( g_uiLogUdp ) printf( "Duplicate Server[%d]=%d.%d.%d.%d:%d!\n", iLoop,
                                ServerIP[0], ServerIP[1], ServerIP[2], ServerIP[3], wPort );
            return iLoop;
        }
    }

    if( g_uiLogUdp ) printf( "Server[%d]=%d.%d.%d.%d:%d Add!\n", g_tUdpCtrl.nServerNum,
                                ServerIP[0], ServerIP[1], ServerIP[2], ServerIP[3], wPort );
    ptServer->wPort   = wPort;
    ptServer->iSockFD = ERROR;

    nSvrNo = g_tUdpCtrl.nServerNum;
    g_tUdpCtrl.nServerNum++;

    // 启动服务器任务
    sprintf(szTxt,"t_udpsvr_%d", nSvrNo );
    taskSpawn(szTxt,99,0,30000, (FUNCPTR)Udp_ServerStart, nSvrNo,0,0,0,0, 0,0,0,0,0);

    return nSvrNo;
}

//=======================================
// 增加连接,返回连接序号,组号(-1表示由程序生成)可能被改写
short Udp_AddConnection( short iSvrNo, BYTE *ClientIP, WORD wClientPort, short *piGroupNo )
{
    tagUdpConn *ptConn,*ptConnTmp;
    char    szTxt[20];
    short   iLoop;

    if( iSvrNo >=g_tUdpCtrl.nServerNum )  return -1;    // 服务器序号越限

    if(g_tUdpCtrl.nConnNum >=C_UDP_NUM_CONN)
    {// 队列已满
        g_tUdpCtrl.nConnNum = C_UDP_NUM_CONN;
        return -1;
    }

    ptConn = &(g_tUdpCtrl.tConnArray[g_tUdpCtrl.nConnNum]);
    ptConn->iSvrNo = iSvrNo;
    sprintf( szTxt, "%d.%d.%d.%d", ClientIP[0], ClientIP[1], ClientIP[2], ClientIP[3] );
    ptConn->dwClientIP = inet_addr(szTxt);

    // 检查链接是否已经在列表中
    for( iLoop=0;iLoop<g_tUdpCtrl.nConnNum;iLoop++ )
    {
        ptConnTmp = &(g_tUdpCtrl.tConnArray[iLoop]);
        if( (ptConnTmp->iSvrNo==iSvrNo) && (ptConn->dwClientIP ==ptConnTmp->dwClientIP) )
        {
            if( g_uiLogUdp ) printf( "Duplicate Connect:%d.%d.%d.%d->%d\n",
                ClientIP[0],ClientIP[1],ClientIP[2],ClientIP[3],iSvrNo );

            if( (*piGroupNo) >=0 )
            {// 重置组号
                ptConnTmp->iGroupNo = (*piGroupNo);
            }
            return iLoop;
        }
    }

    ptConn->iSvrNo = iSvrNo;
    if( (*piGroupNo) <0 )
    {// 生成新的组号
        *piGroupNo = g_tUdpCtrl.nGroupNum;
        g_tUdpCtrl.nGroupNum++;
    }
    ptConn->iGroupNo = *piGroupNo;
    ptConn->wClientPort = wClientPort;

    if( g_uiLogUdp )
    {
        printf( "Add Connect:%d.%d.%d.%d->%d\n",
          ClientIP[0],ClientIP[1],ClientIP[2],ClientIP[3],iSvrNo );
    }

    g_tUdpCtrl.nConnNum++;
    return( g_tUdpCtrl.nConnNum - 1 );
}


//=======================================
// 获取连接号和组号
BOOLEAN Udp_GetConnNo( short iSvrNo, DWORD dwClientIP, WORD wPort, short *pwConnNo, short *pwGroupNo )
{
    tagUdpConn  *ptConn;
    short       iLoop;
    WORD        nConnNum;

    *pwConnNo = -1;
    *pwGroupNo= -1;

    // 搜索链接
    nConnNum = g_tUdpCtrl.nConnNum;
    for( iLoop=0;iLoop<nConnNum;iLoop++ )
    {
        ptConn = &(g_tUdpCtrl.tConnArray[iLoop]);
        if( (ptConn->iSvrNo==iSvrNo) && (ptConn->dwClientIP ==dwClientIP) )
        {
            if( ptConn->iGroupNo >=0 )
            {
                *pwConnNo  = ptConn->iGroupNo;
                *pwGroupNo = iLoop;
              //ptConn->wClientPort = wPort;    // 更新客户端端口号,目前暂不更新端口
                return TRUE;
            }
        }
    }

    return FALSE;
}

//=======================================
// 向指定的连接发送数据
int Upd_Sendto( short iConnNo,BYTE* pbyBuff,WORD wLen )
{
    tagUdpConn      *ptConn;
    tagUdpServer    *ptServer;
    struct sockaddr_in tClientAddr;
    int             sockAddrSize;       /* size of socket address structure */
    short           iSvrNo;

    if( iConnNo >=g_tUdpCtrl.nConnNum  ) return -1;  // 连接序号越限
    ptConn = &(g_tUdpCtrl.tConnArray[iConnNo]);
    iSvrNo = ptConn->iSvrNo;
    if( iSvrNo >=g_tUdpCtrl.nServerNum )    return -1;  // 服务器序号越限
    ptServer = &(g_tUdpCtrl.tSvrArray[iSvrNo]);

    /* set up the remote address */
    sockAddrSize = sizeof (struct sockaddr_in);
    bzero ((char *) &tClientAddr, sockAddrSize);
    tClientAddr.sin_len          = (u_char) sockAddrSize;
    tClientAddr.sin_family       = AF_INET;
    tClientAddr.sin_port         = htons (ptConn->wClientPort);
    tClientAddr.sin_addr.s_addr  = ptConn->dwClientIP;

    wLen = sendto(ptServer->iSockFD, pbyBuff, wLen, 0, (struct sockaddr *)&tClientAddr, sockAddrSize);
    return wLen;
}

//=======================================
// 向指定的组发送数据
void Upd_SendtoGroup( short iGroupNo,BYTE* pbyBuff,WORD wLen )
{
    tagUdpConn  *ptConn;
    short       iLoop;
    WORD        nConnNum;

    for( iLoop=0;iLoop<nConnNum;iLoop++ )
    {
        ptConn = &(g_tUdpCtrl.tConnArray[iLoop]);
        if( ptConn->iGroupNo==iGroupNo )
        {
            Upd_Sendto( iLoop,pbyBuff,wLen );
        }
    }
    return;
}

//======================================================
// Read Udp Message From Common Buffer
// return the Length of the Message
WORD Udp_ReadMsg( short iGroupNo, short *piConnNo, BYTE *pbyMsgBuf, WORD wBufLen, WORD *pwRead, WORD *pwQueueIDRead )
{
    tagPQueueCtrl   ptQueueCtrl;
    tagPUdpMsgCtrl  ptUdpMsgCtrl;

    WORD        wSave, wCounter;
    WORD        wHeadPos, wMsgLen;
    BYTE       *pbyReportBuf;
    BOOLEAN     bFlagReturn;

    ptQueueCtrl  = &g_tQueueUdpMsg.tQueueCtrl;
    pbyReportBuf =  g_tQueueUdpMsg.byReportBuf;
    wSave = ptQueueCtrl->wSave;
    if( wSave >=CONST_NUM_REPORT_CTRL_UP ) wSave = 0;           // !!! ERROR
    if( (*pwRead) >=CONST_NUM_REPORT_CTRL_UP ) (*pwRead) = 0;   // !!! ERROR

    ptUdpMsgCtrl = &g_tQueueUdpMsg.tUdpMsgCtrl[(*pwRead)];
    if( (*pwQueueIDRead) !=ptUdpMsgCtrl->wQueueID )
    { /* The Queue have been FULL since last reading */
        (*pwRead) = (wSave + 1 );
        if( (*pwRead) >=CONST_NUM_REPORT_CTRL_UP ) (*pwRead) = 0;
        ptUdpMsgCtrl = &g_tQueueUdpMsg.tUdpMsgCtrl[(*pwRead)];

        (*pwQueueIDRead) = ptUdpMsgCtrl->wQueueID;
    }

    if( wBufLen >CONST_VOL_BUF_RESP ) wBufLen = CONST_VOL_BUF_RESP;     // 响应报文允许长度
    for( wCounter=0; wCounter<CONST_NUM_REPORT_CTRL_UP; wCounter++ )    // 避免死循环
    {
        if( (*pwRead) ==wSave ) return 0;                               // NO more Cmd_Msg
        ptUdpMsgCtrl = &g_tQueueUdpMsg.tUdpMsgCtrl[(*pwRead)];          // Get the Report-Ctrl

        (*pwRead)++;                                                    // Update the Read-Pointer
        if( (*pwRead) >=CONST_NUM_REPORT_CTRL_UP ) (*pwRead) = 0;
        (*pwQueueIDRead) = ptUdpMsgCtrl->wQueueID+1;

        // Get the Message
        wMsgLen = ptUdpMsgCtrl->wLength;
        if( (0 ==wMsgLen) || (wMsgLen >wBufLen) )   continue;           // !!! ERROR: illegal length
        if( ptUdpMsgCtrl->wGroupNo !=iGroupNo )     continue;           // 报文目的地不同
        *piConnNo = ptUdpMsgCtrl->wConnNo;

        wHeadPos = ptUdpMsgCtrl->wHead;
        if( wHeadPos >= CONST_VOL_REPORT_BUF_UP )   continue;           // !!! ERROR
        bFlagReturn = Fun_QueueBufRead( pbyMsgBuf, pbyReportBuf, wHeadPos, wMsgLen, CONST_VOL_REPORT_BUF_UP );
        if( ptUdpMsgCtrl->wCRC !=Fun_CreateCrc16(pbyMsgBuf, wMsgLen) ) bFlagReturn = FALSE;
        if( bFlagReturn )  // 报文正确
        {
            return wMsgLen;
        }
    }

    return 0;
}