/******************************************************************************
* Copyright (c) 2006 Altera Corporation, San Jose, California, USA.           *
* All rights reserved. All use of this software and documentation is          *
* subject to the License Agreement located at the end of this file below.     *
*******************************************************************************
* Date - October 24, 2006                                                     *
* Module - simple_socket_server.c                                             *
*                                                                             *
******************************************************************************/
 
/******************************************************************************
 * Unimation Server (US) example.
 * 
 * This example demonstrates the use of MicroC/OS-II running on NIOS II.       
 * In addition it is to serve as a good starting point for designs using       
 * MicroC/OS-II and Altera NicheStack TCP/IP Stack - NIOS II Edition.                                          
 *                                                                             
 * -Known Issues                                                             
 *     None.   
 *      
 * Please refer to the Altera NicheStack Tutorial documentation for details on this 
 * software example, as well as details on how to configure the NicheStack TCP/IP 
 * networking stack and MicroC/OS-II Real-Time Operating System.  
 */
 
#include <stdio.h>
#include <string.h>
#include <ctype.h> 
#include <time.h>
#include <time.h>
#include <sys/time.h>

/* MicroC/OS-II definitions */
#include "includes.h"

/* Common defines */
#include "common_defines.h"

/* Unimation Server definitions */
#include "unimation_client.h"
#include "client_fds.h"
#include "unimation_server.h"
#include "alt_error_handler.h"

/* Include debug log  header*/
#include "log_api.h"

/* Nichestack definitions */
#include "ipport.h"
#include "tcpport.h"
 
static INT32S WriteMessageHeader(INT16U u16Length, INT8U u8Cmd,INT32U dst,
                             INT8U *pData);

static INT32S SendToNetHost(INT16U u16DataLen, INT8U u8Cmd,
                        INT8U *data, UnimationConn *pCon);
static INT32S EnqueueSlaveTX(UNIMATION_CMD_HEADER *pCmdHdr, INT8U *pData, UnimationConn *pCon);
static INT32S ProcessReceiveMessage(UnimationConn *pstCon, INT8U * pBuffer, INT32S bufLength);

/*
 * This UnimationCommandQ MicroC/OS-II message queue will be used to communicate
 * between the simple socket server task and Nios Development Board LED control 
 * tasks.
 *
 * Handle to our MicroC/OS-II Command Queue and variable definitions related to 
 * the Q for sending commands received on the TCP-IP socket from the 
 * UnimationServerTask to the UnimationClientTask.
 */
OS_EVENT  *UnimationCommandQ;
void *UnimationCommandQTbl[UNIMATION_MSG_Q_SIZE]; /*Storage for UnimationCommandQ*/
struct MESSAGE_Q  msgQ[UNIMATION_MSG_Q_SIZE];

/* Unimation command response to the host machine*/
OS_EVENT  *UnimationResponseQ;
void *UnimationResponseQTbl[UNIMATION_MSG_Q_SIZE];
struct MESSAGE_Q   respQ[UNIMATION_MSG_Q_SIZE];


/*
 * Handle to our MicroC/OS-II LED Lightshow Semaphore. The semaphore is checked 
 * by the LED7SegLightshowTask each time it updates 7 segment LED displays, 
 * U8 and U9.  The UnimationClientTask grabs the semaphore away from the lightshow task to
 * toggle the lightshow off, and gives up the semaphore to turn the lightshow
 * back on.  The UnimationClientTask does this in response to the CMD_LEDS_LIGHTSHOW
 * command sent from the SSSSimpleSocketServerTask when the user sends a toggle 
 * lightshow command over the TCPIP socket.
 */
//OS_EVENT *UnimationReceiveSem;

/* Definition of Task Stacks for tasks not invoked by TK_NEWTASK 
 * (do not use NicheStack) 
 */
OS_STK    UnimationClientTaskStk[TASK_STACKSIZE];
OS_STK    UnimationSlaveTaskStk[TASK_STACKSIZE];

/*
 * Create our MicroC/OS-II resources. All of the resources beginning with 
 * "SSS" are declared in this file, and created in this function.
 */
void UnimationCreateOSDataStructs(void)
{
    int i=0;
    /*
    * Create the resource for our MicroC/OS-II Queue for sending commands
    * received on the TCP/IP socket from the UnimationServerTask()
    * to the UnimationClientTask().
    */
    fprintf(stdout, "Call UnimationCreateOSDataStructs!\r\n");
    for ( i=0; i < UNIMATION_MSG_Q_SIZE; i++)
    {
    	UnimationCommandQTbl[i] = (void *)&msgQ[i];
    }
    UnimationCommandQ = OSQCreate(&UnimationCommandQTbl[0], UNIMATION_MSG_Q_SIZE);
    if (!UnimationCommandQ)
    {
        alt_uCOSIIErrorHandler(EXPANDED_DIAGNOSIS_CODE,
                "Failed to create UnimationCommandQ.\r\n");
    }

    /*
    * Create the resource for our MicroC/OS-II Queue for sending response
    * received on the Unimation controller from the UnimationClientTask()
    * to the UnimationServerTask().
   */
    for ( i=0; i < UNIMATION_MSG_Q_SIZE; i++)
    {
    	UnimationResponseQTbl[i] = (void *)&respQ[i];
    }
     UnimationResponseQ = OSQCreate(&UnimationResponseQTbl[0], UNIMATION_MSG_Q_SIZE);
    if (!UnimationCommandQ)
    {
        alt_uCOSIIErrorHandler(EXPANDED_DIAGNOSIS_CODE,
                "Failed to create UnimationCommandQ.\r\n");
    }


    /* Create MicroC/OS-II  Semaphore.  The semaphore is for exclusively  receiving message
     *    from the lower controller.
     */
   // UnimationReceiveSem = OSSemCreate(1);
   // if (!UnimationReceiveSem)
   // {
   //     alt_uCOSIIErrorHandler(EXPANDED_DIAGNOSIS_CODE,
   //                         "Failed to create SSSLEDLightshowSem.\r\n");
   // }
  
}

/* This function creates tasks used in this example which do not use sockets.
 * Tasks which use Interniche sockets must be created with TK_NEWTASK.
 */
void UnimationCreateTasks(int fd)
{
    INT8U error_code;
    fprintf(stdout, "Create UnimationSlaveTask!\r\n");

    error_code = OSTaskCreateExt(UnimationSlaveTask,
                             &fd,
                             (void *)&UnimationSlaveTaskStk[TASK_STACKSIZE-1],
                             UNIMATION_SLAVE_TASK_PRIORITY,
                             UNIMATION_SLAVE_TASK_PRIORITY,
                             UnimationSlaveTaskStk,
                             TASK_STACKSIZE,
                             NULL,
                             0);
   
    alt_uCOSIIErrorHandler(error_code, 0);
      
    fprintf(stdout, "Create UnimationClientTask!\r\n");
    error_code = OSTaskCreateExt(UnimationClientTask,
                              &fd,
                              (void *)&UnimationClientTaskStk[TASK_STACKSIZE-1],
                              UNIMATION_CLIENT_TASK_PRIORITY,
                              UNIMATION_CLIENT_TASK_PRIORITY,
                              UnimationClientTaskStk,
                              TASK_STACKSIZE,
                              NULL,
                              0);

    alt_uCOSIIErrorHandler(error_code, 0);


}

/*
 * unimation_reset_connection()
 * 
 * This routine will, when called, reset our SSSConn struct's members 
 * to a reliable initial state. Note that we set our socket (FD) number to
 * -1 to easily determine whether the connection is in a "reset, ready to go" 
 * state.
 */
void unimation_reset_connection(UnimationConn* conn)
{
    memset(conn, 0, sizeof(UnimationConn));
    fprintf(stdout, "call unimation_reset_connection\r\n");
    free_clients(&conn->clients);
    conn->fd = -1; //FIXME
    conn->state = READY;
    conn->u32Seq = 0;
    conn->to_hal = 0;
    conn->q_id = 0 ;
    conn->r_wr = 0;
    conn->r_rd = 0;
    conn->t_wr = 0;
    conn->t_rd = 0;
    conn->socket_error = 0;
    return;
}

/*  unimation_send_ready()
 *     This routine will transmit the menu out to the telent client.
 */
void unimation_send_ready(UnimationConn* conn)
{
    int i;
	char sendData[UNIMATION_MSG_BUF_SIZE];
    char receiveData[UNIMATION_MSG_BUF_SIZE];
    short resp_status;
    INT8U resp_cmd;
    INT8U u8Len ;
    int rcv_size = 0;
    int send_size = 0;
    INT32S s32FrameLen = 0;
    INT32S s32checksum = 0;

    /* No newline received? Then ask the socket for data */
    memset(receiveData, 0, sizeof(receiveData));
    rcv_size = recv(conn->fd, (char*) receiveData,
                    UNIMATION_MSG_BUF_SIZE, MSG_DONTWAIT);   
    DbgHexdump(LOGAPP_DEBUG,"New connection msg  ",receiveData,  rcv_size);
    if ( rcv_size > sizeof(IpcMessageHeader) +  sizeof(UNIMATION_CMD_HEADER) )
    {
        IpcMessageHeader *  pIpcHdr = (IpcMessageHeader *)  receiveData;
        UNIMATION_CMD_HEADER * pCmdHdr = ( UNIMATION_CMD_HEADER *) (receiveData + sizeof(IpcMessageHeader)) ;
        if (pCmdHdr->u8Cmd == CMD_PROBE_REQ) 
        {
            resp_cmd = CMD_PROBE_RESP; 
            resp_status = 0;
        }
        else 
        {
            resp_cmd = CMD_OPEN_RESP;
            resp_status = 0; /* ERROR_OK*/
        }
                /* Compose payload */
        memset(sendData, 0, sizeof(sendData));
        s32FrameLen = WriteMessageHeader(2, resp_cmd, pIpcHdr->src, (unsigned char *)  sendData);

        memcpy(sendData + sizeof(IpcMessageHeader) +  sizeof(UNIMATION_CMD_HEADER) , (char *) &resp_status, 2);

 		INT32S * ps32Data = (INT32S *) sendData;
        for ( i = 0; i < (s32FrameLen - 4) / 4 ; i++)
        {
	        s32checksum += *(ps32Data ++ );
         }
        
        sendData[s32FrameLen-4] = s32checksum & 0xFF;
        sendData[s32FrameLen-3] = (s32checksum >> 8) & 0xFF;

        DbgHexdump(LOGAPP_DEBUG,"Enqueue TX message ",sendData, (int) s32FrameLen);


        send_size = send (conn->fd,                          // SOCKET s
                          sendData,   // const char* buf
                        s32FrameLen,               // int len
                        0);                 // int flags

        if( send_size < 0)
        {
            fprintf(stderr,"[%s]: socket send %d error: %d, err:%s\r\n", __FUNCTION__,
                send_size, errno, strerror(errno));

            if( (errno != EINTR) && (errno != EAGAIN) && (errno != EWOULDBLOCK))
            {
                close(conn->fd);
                conn->fd = -1;
            }
        }
        else
        {
             client_push_back(&conn->clients, conn->fd, pIpcHdr->src);
        }
	}
    else
    {
	    fprintf(stderr,"[%s]: Invalid new socket %ld  connection , size: %d\r\n", __FUNCTION__,
	    		conn->fd,  rcv_size);
        DbgHexdump(LOGAPP_DEBUG,"New connect message ",receiveData,  rcv_size);
	}

    return;
}

/*
 * unimation_handle_accept()
 * 
 * This routine is called when ever our listening socket has an incoming
 * connection request.
 * We'll also print out the client's IP address.
 */
void unimation_handle_accept(INT32S listen_socket, UnimationConn* conn)
{
    int      socket, len;
    struct sockaddr_in  incoming_addr;

    len = sizeof(incoming_addr);

    if ( conn->fd == -1)
    {
        if((socket=accept(listen_socket,(struct sockaddr*)&incoming_addr,&len))<0)
        {
            printf_error("[unimation_handle_accept] accept failed: %d", EXPANDED_DIAGNOSIS_CODE);
        }
        else
        {
            conn->fd = socket;
            unimation_send_ready(conn);
            printf("[sss_handle_accept] accepted connection request from %s\r\n",
               inet_ntoa(incoming_addr.sin_addr));
        }
    }
    else
    {
        printf("[sss_handle_accept] rejected connection request from %s\r\n",
           inet_ntoa(incoming_addr.sin_addr));
    }

    return;
}

/*****************************************************************************
    Function :      ProcessReceiveMessage
    Parameters :    None
    Returns :
    Description:   Parse web client remote command.
*****************************************************************************/
INT32S ProcessReceiveMessage(UnimationConn *pstCon, INT8U * pBuffer, INT32S bufLength)
{
    // UG! We got some data in from the Ethernet
    UNIMATION_CMD_HEADER * pMsgHeader;
    char dataBuf[255];

    DbgHexdump(LOGAPP_DEBUG,"Msg payload",pBuffer, (int) bufLength);

    if (bufLength < 0)
    {
        pstCon->close = 1;
    }
    else if ( bufLength <= sizeof(UNIMATION_CMD_HEADER))
    {
        fprintf(stderr,"%s: received message is too short:%ld discard\r\n", __FUNCTION__,
                bufLength);
        return 0;
    }

    pMsgHeader = (UNIMATION_CMD_HEADER *) pBuffer;

    if(pMsgHeader->u8Flag != CMD_FRM_START)
    {
        fprintf(stderr,"[Unimation Server]%s: Invalid Frame token: 0x%x\r\n",__FUNCTION__,
                pMsgHeader->u8Flag);
        return 0;
    }

    if  ( PROTO_TYPE != pMsgHeader->u8Proto )
    {
        fprintf(stderr, "[Unimation Server]%s: Invalid Proto type: 0x%x\r\n", __FUNCTION__,
                pMsgHeader->u8Proto);
        return 0;
    }

   /* first reset to_hal command */
    pstCon->to_hal = 0;
    switch (pMsgHeader->u8Cmd)
    {
        case CMD_QUIT:
        {
            sprintf(dataBuf, "[Unimation Server]Terminating connection\r\n");
            SendToNetHost( (INT8U) strlen(dataBuf), (INT8U) CMD_QUIT_RESP, (INT8U*) dataBuf , pstCon);
             pstCon->state = CLOSE;
            break;
        }
        case CMD_ATTACH_REQ:
        {
            /* state live play*/
            fprintf(stdout,"[%s]:Received CMD_ATTACH_REQ, len=%d\r\n",__FUNCTION__,
                        pMsgHeader->u16Len);
            /* Send to the controller*/
            EnqueueSlaveTX(pMsgHeader, NULL, pstCon);

            break;
        }
        case CMD_DETATCH_REQ:
        {
            fprintf(stdout,"[%s]Received CMD_DETATCH_REQ, len=%d \r\n",__FUNCTION__,
                        pMsgHeader->u16Len);
            /*Send to the controller*/
            EnqueueSlaveTX(pMsgHeader, NULL, pstCon);
            break;
        }
        case CMD_SIGNAL_REQ:
        {
            fprintf(stdout,"[Unimation_Server]Receive:CMD_SIGNAL_REQ, len=%d \r\n",
                        pMsgHeader->u16Len);

             /* Send to Unimation controller : Note: data payload offset is 6 not including cmd and cmdExt*/
             EnqueueSlaveTX(pMsgHeader, pBuffer + sizeof(UNIMATION_CMD_HEADER), pstCon);

            break;
        }
        case CMD_WRITE_REQ:
        {
            fprintf(stdout,"[Unimation_Server]Receive:CMD_WRITE_REQ, len=%d\r\n",
                    pMsgHeader->u16Len);
            /* Send to Unimation controller*/
            EnqueueSlaveTX(pMsgHeader, pBuffer + sizeof(UNIMATION_CMD_HEADER), pstCon );

            break;
        }
        case CMD_READ_REQ:
        {
            fprintf(stdout,"[Unimation_Server]Receive:CMD_READ_REQ, len=%d\r\n",
                    pMsgHeader->u16Len);
            /* Send to Unimation controller*/
            EnqueueSlaveTX(pMsgHeader, NULL, pstCon);

            break;
        }
        case CMD_IOCTL_REQ:
        {
            fprintf(stdout,"Unimation_Server:Receive:CMD_IOCTL_REQ, len=%d\r\n",
                                pMsgHeader->u16Len);
            /* Send to Unimaiton controller*/
            EnqueueSlaveTX(pMsgHeader, pBuffer + sizeof(UNIMATION_CMD_HEADER), pstCon);

            break;
        }

        default:
        {
            fprintf(stdout,"[Unimation_Server] unsupported command: 0x%02x\r\n",
                    pMsgHeader->u8Cmd);
            break;
        }
    }

    return 1;
}

/*
 * unimation_exec_command()
 *
 * This routine is called whenever we have new, valid receive data from our
 * network connection. It will parse through the data simply looking for valid
 * commands.
 */
void unimation_exec_command(UnimationConn* conn)
{
    int bytes_to_process = conn->r_wr - conn->r_rd;
    INT8U error_code;
    INT16U frame_len;
    unsigned char *pCmdHeader;
    int offset = sizeof(IpcMessageHeader) ;
    IpcMessageHeader *ipcMsg= NULL;

    while(bytes_to_process > 0)
    {
        conn->to_hal  = 0;
        ipcMsg = (IpcMessageHeader *) (conn->rx_buf + conn->r_rd);
        frame_len = ipcMsg->size;

        if ( (conn->rx_buf[conn->r_rd + offset]!= CMD_FRM_START ) ||
              (frame_len < offset + sizeof(UNIMATION_CMD_HEADER)+2))
        {
            fprintf(stderr, "%s: Invalid Command frame: 0x%x\r\n",__FUNCTION__,
                    conn->rx_buf[conn->r_rd ]);

            conn->r_rd += frame_len;
            bytes_to_process -= frame_len;
            continue;
        }

        /* incomplete frame */
        if ( (conn->r_rd > conn->r_wr) &&
                ( frame_len > (conn->r_wr - conn->r_rd )))
        {
            printf_error("Receiver buffer error, disconnect the socket");
        	break;
        }

        pCmdHeader = (conn->rx_buf + conn->r_rd + offset );
    
        if ( !ProcessReceiveMessage(conn, pCmdHeader, frame_len - offset))
        {
        	printf_error("Process receive message error, disconnect the socket");
            break;
        }

        /*Forward this command to Unimation controller */
        if (conn->to_hal)
        {
            /* Send message index number */
            fprintf(stdout,"Forward msg[%ld]:0x%02x to the client\r\n", conn->q_id, msgQ[conn->q_id].cmd);
            error_code = OSQPost(UnimationCommandQ, (void *)&msgQ[conn->q_id]);
            if (error_code == OS_Q_FULL)
            {
                printf_error("UnimationCommandQ is full!");
            }
        }

        conn->r_rd += frame_len ;
        bytes_to_process -= frame_len;
    }

    conn->r_wr = 0;
    conn->r_rd = 0;

    return;
}

/*
 * unimation_handle_receive()
 * 
 * This routine is called whenever there is a network connection established and
 * the socket associated with that connection has incoming data. We will first
 * look for a newline "\n" character to see if the user has entered something 
 * and pressed 'return'. If there is no newline in the buffer, we'll attempt
 * to receive data from the listening socket until there is.
 * 
 * The connection will remain open until the user enters "Q\n" or "q\n", as
 * deterimined by repeatedly calling recv(), and once a newline is found, 
 * calling sss_exec_command(), which will determine whether the quit 
 * command was received.
 * 
 * Finally, each time we receive data we must manage our receive-side buffer.
 * New data is received from the  socket onto the head of the buffer,
 * and popped off from the beginning of the buffer with the 
 * unimation_exec_command() routine. Aside from these, we must move incoming
 * (un-processed) data to buffer start as appropriate and keep track of 
 * associated pointers.
 */
void unimation_handle_receive(UnimationConn* conn, fd_set* rcvfdset)
{
    int i;
    INT32S data_used = 0;
    int rx_code = 0;
    IpcMessageHeader *ipcHdr = NULL;
	int head_tail_len = sizeof(IpcMessageHeader) + sizeof(UNIMATION_CMD_HEADER) + 4;
   // fprintf(stdout,"[unimation_handle_receive] processing RX data\n");
  
	for ( i = 0; i < conn->clients.size; i++)
	{
		if (FD_ISSET(conn->clients.fd[i], rcvfdset) )
	    {
	       	FD_CLR(conn->clients.fd[i], rcvfdset);
	        /* No newline received? Then ask the socket for data */
	        rx_code = recv(conn->clients.fd[i], (char*) (conn->rx_buf + conn->r_wr),
	                    UNIMATION_RX_BUF_SIZE - conn->r_wr -1, MSG_DONTWAIT);



	       	if(rx_code > 0)
	        {
	           			fprintf(stdout,"[%s] Received %d bytes data\r\n", __FUNCTION__,rx_code);
	           			DbgHexdump(LOGAPP_DEBUG,"Rcv",&(conn->rx_buf[conn->r_wr]),rx_code)
	            		conn->r_wr += rx_code;
	       	}
	       	else if (rx_code == 0)
	       	{
	       		fprintf(stderr,"[%s]: socket is closed by the client\n", __FUNCTION__);
	            close(conn->clients.fd[i]);
				delete_client(&conn->clients, i);
	         	continue;
	        }
	        else
	       	{
	           	//by default, socket is is non-block mode and the following errors
	    	   	// are not real errors.
	           	if ((errno == EWOULDBLOCK) || (errno == 0) || (errno == EINTR)
	                   			|| (errno == EAGAIN))
	           	{

	               	continue;
	           	}
	           	fprintf(stderr,"[%s]: recv()=%d : failed:%d %s\r\n", __FUNCTION__, rx_code,
	                         errno,strerror(errno));

	            close(conn->clients.fd[i]);
				delete_client(&conn->clients, i);

				if (conn->clients.size == 0 ) break;
	        }

	    }
	}

   /* Find the Carriage return which marks the end of the header */
   /*  (0x0D 0x0A, \r\n) */
   if ( conn->r_wr - conn->r_rd > head_tail_len)
   {
        ipcHdr = (IpcMessageHeader *) &(conn->rx_buf[conn->r_rd]);

        if (ipcHdr->sync == MSG_START_SYNC && ipcHdr->size > head_tail_len )
        {
            DbgPrintf(LOGAPP_DEBUG,"Msg Len=%lu, Cmd= %ld", ipcHdr->size,  ipcHdr->type);
                 /* go off and do whatever the user wanted us to do */
            unimation_exec_command(conn);
        }
        else if ( conn->r_wr - conn->r_rd > 0)
        {
            DbgHexdump(LOGAPP_DEBUG,"Out of Sync:flushed",&(conn->rx_buf[conn->r_rd]), (int) (conn->r_wr - conn->r_rd))
            conn->r_wr = 0;
            conn->r_rd = 0;
        }
   }


    /* Manage buffer */
    data_used = conn->r_wr - conn->r_rd;
    if ( conn->r_rd > 0)
    {
        if ( data_used > 0 )
        {
            memmove(conn->rx_buf, conn->rx_buf + conn->r_rd,
                                conn->r_wr - conn->r_rd);
            conn->r_rd = 0;
            conn->r_wr -= data_used;
            memset(conn->rx_buf+conn->r_wr, 0, data_used);
        }
        else
        {
            memset(conn->rx_buf,0, UNIMATION_RX_BUF_SIZE);
            conn->r_wr = 0;
            conn->r_rd = 0;
        }
    }
    /*
     * When the quit command is received, update our connection state so that
     * we can exit the while() loop and close the connection
     */
    conn->state = conn->clients.size == 0 ? CLOSE : READY;

    if ( conn-> state == CLOSE )
   {
        printf("[%s] closing connection\n",__FUNCTION__);
        unimation_reset_connection(conn);
    }
  
  return;
}

/*
 * UnimationServerTask()
 * 
 * This MicroC/OS-II thread spins forever after first establishing a listening
 * socket for the connection, binding it, and listening. Once setup,
 * it perpetually waits for incoming data to either the listening socket, or
 * (if a connection is active), the sss data socket. When data arrives, 
 * the approporiate routine is called to either accept/reject a connection
 * request, or process incoming data.
 */
void UnimationServerTask()
{
    int i;
    INT32S fd_listen, max_socket;
    struct sockaddr_in addr;
    static UnimationConn conn;
    fd_set readfds;
    struct timeval tv;
    INT8U error_code;
    INT32S retval;
    struct MESSAGE_Q  *msg = NULL;
    /*
     * Sockets primer...
     * The socket() call creates an endpoint for TCP of UDP communication. It
     * returns a descriptor (similar to a file descriptor) that we call fd_listen,
     * or, "the socket we're listening on for connection requests" in our sss
     * server example.
     *
     * Traditionally, in the Sockets API, PF_INET and AF_INET is used for the
     * protocol and address families respectively. However, there is usually only
     * 1 address per protocol family. Thus PF_INET and AF_INET can be interchanged.
     * In the case of NicheStack, only the use of AF_INET is supported.
     * PF_INET is not supported in NicheStack.
     */
    if ((fd_listen = socket(AF_INET, SOCK_STREAM, 0)) < 0)
    {
      printf_error("[UnimationServerTask] Socket creation failed!: %d", EXPANDED_DIAGNOSIS_CODE);
    }

    /*
    * Sockets primer, continued...
    * Calling bind() associates a socket created with socket() to a particular IP
    * port and incoming address. In this case we're binding to SSS_PORT and to
    * INADDR_ANY address (allowing anyone to connect to us. Bind may fail for
    * various reasons, but the most common is that some other socket is bound to
    * the port we're requesting.
    */
    addr.sin_family = AF_INET;
    addr.sin_port = htons(UNIMATION_PORT);
    addr.sin_addr.s_addr = INADDR_ANY;
  
    if ((bind(fd_listen,(struct sockaddr *)&addr,sizeof(addr))) < 0)
    {
    	printf_error("[UnimationServerTask] Bind failed! %d", EXPANDED_DIAGNOSIS_CODE);
    }
    
    /*
    * Sockets primer, continued...
    * The listen socket is a socket which is waiting for incoming connections.
    * This call to listen will block (i.e. not return) until someone tries to
    * connect to this port.
    */
    if ((listen(fd_listen,1)) < 0)
    {
        printf_error("[UnimationServerTask] Listen failed! %d ", EXPANDED_DIAGNOSIS_CODE);

    }

    /* At this point we have successfully created a socket which is listening
    * on SSS_PORT for connection requests from any remote address.
    */
    unimation_reset_connection(&conn);
    fprintf(stdout,"[%s] Unimation Server listening on port %d",__FUNCTION__,
                            UNIMATION_PORT);
    while(1)
    {
        /*
        * For those not familiar with sockets programming...
         * The select() call below basically tells the TCPIP stack to return
         * from this call when any of the events I have expressed an interest
         * in happen (it blocks until our call to select() is satisfied).
         *
         * In the call below we're only interested in either someone trying to
         * connect to us, or data being available to read on a socket, both of
         * these are a read event as far as select is called.
         *
         * The sockets we're interested in are passed in in the readfds
         * parameter, the format of the readfds is implementation dependant
         * Hence there are standard MACROs for setting/reading the values:
         *
         *   FD_ZERO  - Zero's out the sockets we're interested in
         *   FD_SET   - Adds a socket to those we're interested in
         *   FD_ISSET - Tests whether the chosen socket is set
        */
        FD_ZERO(&readfds);
        FD_SET(fd_listen, &readfds);
        max_socket = fd_listen;

        tv.tv_sec = 0;
        tv.tv_usec = 500000; /* Wait half second*/
        if (conn.clients.size > 0)
        {
            for ( i = 0 ; i < conn.clients.size; i++)
			{
            	FD_SET(conn.clients.fd[i], &readfds);
            	if (max_socket < conn.clients.fd[i])
            	{
                	max_socket = conn.clients.fd[i];
            	}
			}
           if ( conn.t_rd != conn.t_wr)
                tv.tv_usec = 20000; /* Wait 20 ms*/
            else
                tv.tv_usec = 50000; /* Wait 50 ms*/
        }

        retval = select(max_socket+1, &readfds, NULL, NULL, &tv);

        /*
         * If fd_listen (the listening socket we originally created in this thread
         * is "set" in readfs, then we have an incoming connection request. We'll
         * call a routine to explicitly accept or deny the incoming connection
         * request (in this example, we accept a single connection and reject any
         * others that come in while the connection is open).
        */
        if ( retval < 0)
        {
            fprintf(stdout,"[%s]:Socket elect status= SOCKET_ERROR, exit\r\n",__FUNCTION__);
            break;
        }
        else if (retval > 0)
        {

            if (FD_ISSET(fd_listen, &readfds))
            {
                fprintf(stdout,"[%s]: Found new connection!!!\r\n",__FUNCTION__);

                unimation_handle_accept(fd_listen, &conn);
            }

            /*
             * If unimation_handle_accept() accepts the connection, it creates *another*
             * socket for sending/receiving data over network. Note that this socket is
             * independent of the listening socket we created above. This socket's
             * descriptor is stored in conn.fd. If conn.fs is set in readfs... we have
             * incoming data for our network server, and we call our receiver routine
             * to process it.
             */
            else
            {
                if ( conn.clients.size > 0)
                {   
					conn.fd = -1;
                    for ( i= 0; i < conn.clients.size; i++)
                    {
                    	if (FD_ISSET(conn.clients.fd[i], &readfds))
                    	{  //at least one data from clients list
							conn.fd = conn.clients.fd[i];
                            break;
                        }

                    }
					if (conn.fd > 0)
 	                {
					    //fprintf(stdout,"[%s]: Socket received data\r\n",__FUNCTION__);
                        unimation_handle_receive(&conn, &readfds);
				    }
                }
            }
        }
      //  else //timeout
      //  {
      //        fprintf(stdout,"select time out\n");
      //  }
        //Send to net client: forward message from the device first
        msg = (struct MESSAGE_Q *)OSQAccept(UnimationResponseQ, &error_code);
        while ( msg  )
        {
            fprintf(stdout,"[%s]: Enqueue receive message: 0x%02X\r\n",
                            __FUNCTION__, msg->cmd);
            SendToNetHost(msg->u16Length, msg->cmd, msg->data, &conn);
            msg = (struct MESSAGE_Q *)OSQAccept(UnimationResponseQ, &error_code);
        }
    } /* while(1) */
}

/* Write Message header, including message terminator.
 * Note: Sequence number is not write here, it will be updated during sending package
 *     First DWORD : Frame start(bit31-24), dummy Frame start(bit28-16), Frame length ( 2 bytes) in bytes of following payload.
 *     Second DWORD: Uminimation Command header: '$'- Frag sequence, 01- Version, two bytes command length ( including 2 bytes commands). Note this command
 *     including 0D 0A
 *     Third DWORD : Command (31-16), ExtCommand(15-0)MSB, not used.
 *     Fourth DWORD: Command Payload. Payload length = Command Length -2. Note: some command payloads are GPIO_DVR11_BUFFER data structure.
 */
static INT32S WriteMessageHeader(INT16U u16Length, INT8U u8Cmd, INT32U dst,
                             INT8U *pData)
{
    struct timeval tmVal;
    static INT32U  u32Sn;
    static INT8U   u8Sn;
   //caculate padding bytes
    INT8U          u8Pad =  0;
    unsigned char i=0;
    if (( u16Length  & 0x03 )> 0)
    {
        u8Pad = 4 - ( u16Length & 0x03 ); 
    }
    INT32U *dwPtr = (INT32U *) pData;
    //First Dword ( Frame Header) 0x5AFAFA5A
    *(dwPtr++) = MSG_START_SYNC;
    //2nd Dword timestamp
    gettimeofday( &tmVal, NULL );
    *(dwPtr++) = tmVal.tv_sec;
    //3rd Dword
    *(dwPtr++) = tmVal.tv_usec * 1000 ; //nano seconds
    *(dwPtr++) = (u32Sn++);
    //5th Dword: src
    *(dwPtr++) = UNIMATION_MODULE_ID;
    //6th Dword: dst 
    *(dwPtr++) = dst;
    //7th Dword: type
    *(dwPtr++) = u8Cmd;
    //8th Dword: size : total message size = 32 (msg header) + 4 (HDLC frame header) + 4 (command header ) + command payload + pad + 4(CSC)
    *(dwPtr++) =  sizeof(IpcMessageHeader) +  sizeof(UNIMATION_CMD_HEADER) + u16Length  + 4 + u8Pad ;
    
     INT8U *ptr = (INT8U *) dwPtr;
    //HDLC frame Header DWord
    *(ptr++) = CMD_FRM_START; /* Binary frame*/
    *(ptr++) = PROTO_TYPE & 0xFF;   /* protocol version */
    *(ptr++) = (u8Sn++);
    *(ptr++) = u8Pad;
    // Command header DWORD
    *(ptr++) = u16Length & 0xFF; /* HDLC payload length : not include 2 bytes header*/
    *(ptr++) = (u16Length  >> 8) & 0xFF;
    *(ptr++) = u8Cmd ; /* little endian */
    *(ptr++) = 0x00;
    //Variable Command payload
    ptr += u16Length;
    
    for (i=0; i < u8Pad; i++)
    {
		*(ptr++) = 0xaa;
    }
    // add two bytes checksume
    ptr+= 2;

    // two characters terminators
    *(ptr++) = CMD_FRM_CR_END;
    *(ptr++) = CMD_FRM_LF_END;
    //return total frame length
    return (ptr - pData);
}

/*  Enqueue messages to be sent to the host */
static INT32S SendToNetHost(INT16U u16DataLen, INT8U u8Cmd,
                        INT8U *data, UnimationConn *pCon)
{

    int i = 0, j= 0;
    INT32S s32Sent = -1;
    INT32S s32FrameLen = 0;
    INT32S s32checksum = 0;
    char sendData[UNIMATION_TX_BUF_SIZE];

    DbgHexdump(LOGAPP_DEBUG,"Send msg to host",(char*) (data), (int) u16DataLen);

    if ( u16DataLen > 0 )
    {
		for ( i= 0; i < pCon->clients.size ; i++)
	   {
		    /* Compose Frame header */
		    memset(sendData, 0, sizeof(sendData));
		    s32FrameLen = WriteMessageHeader(u16DataLen,u8Cmd, (INT32U) pCon->clients.id[i], (unsigned char *) sendData);

		    memcpy(sendData + sizeof(IpcMessageHeader) +  sizeof(UNIMATION_CMD_HEADER) , data, u16DataLen);
		           INT32S * ps32Data = (INT32S *) sendData;
		    for ( j = 0; j < (s32FrameLen - 4) / 4 ; j++)
		    {
		   	   s32checksum += * (ps32Data ++ );
		    }

		    sendData[s32FrameLen-4] = s32checksum & 0xFF;
		    sendData[s32FrameLen-3] = (s32checksum >> 8) & 0xFF;

		    DbgHexdump(LOGAPP_DEBUG,"Send msge to host ",sendData,  (int) s32FrameLen);

			s32Sent = send (pCon->clients.fd[i],                          // SOCKET s
						 (char*) sendData,   // const char* buf
						 s32FrameLen,               // int len
						  0);                 // int flags

		    if( s32Sent < 0)
		    {
			  fprintf(stderr,"[%s]: socket send %ld error: %d, err:%s\r\n", __FUNCTION__,
				  s32Sent, errno, strerror(errno));

				if( (errno != EINTR) && (errno != EAGAIN) && (errno != EWOULDBLOCK))
				{
					pCon->socket_error = 1;
					close(pCon->clients.fd[i]);
					delete_client(&pCon->clients, i);
				}
			 }
         }
     }


    return s32Sent;


}

static INT32S EnqueueSlaveTX(UNIMATION_CMD_HEADER *pCmdHdr, INT8U *pData,
        UnimationConn *pCon)
{
    if ( !pCmdHdr)
        return 0;
    pCon->q_id = (pCon->q_id +1 )% UNIMATION_MSG_Q_SIZE;

    /* pass the command to unimation controller*/
    msgQ[pCon->q_id].cmd = pCmdHdr->u8Cmd;
    msgQ[pCon->q_id].u16Length = pCmdHdr->u16Len;
    if ( pData != NULL && (pCmdHdr->u16Len > 0))
    {
        memcpy(msgQ[pCon->q_id].data, pData, pCmdHdr->u16Len);
    }
    pCon->to_hal = 1;

    return 1;
}


/******************************************************************************
*                                                                             *
* License Agreement                                                           *
*                                                                             *
* Copyright (c) 2009 Altera Corporation, San Jose, California, USA.           *
* All rights reserved.                                                        *
*                                                                             *
* Permission is hereby granted, free of charge, to any person obtaining a     *
* copy of this software and associated documentation files (the "Software"),  *
* to deal in the Software without restriction, including without limitation   *
* the rights to use, copy, modify, merge, publish, distribute, sublicense,    *
* and/or sell copies of the Software, and to permit persons to whom the       *
* Software is furnished to do so, subject to the following conditions:        *
*                                                                             *
* The above copyright notice and this permission notice shall be included in  *
* all copies or substantial portions of the Software.                         *
*                                                                             *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR  *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,    *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER      *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING     *
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER         *
* DEALINGS IN THE SOFTWARE.                                                   *
*                                                                             *
* This agreement shall be governed in all respects by the laws of the State   *
* of California and by the laws of the United States of America.              *
* Altera does not recommend, suggest or require that this reference design    *
* file be used in conjunction or combination with any other product.          *
******************************************************************************/
