path: root/cfe/cfe/net/net_nmrp.c

#include "bsp_config.h"

#include "lib_types.h"
#include "lib_string.h"
#include "lib_queue.h"
#include "lib_malloc.h"
#include "lib_printf.h"

#include "cfe_iocb.h"
#include "cfe_devfuncs.h"
#include "cfe_ioctl.h"
#include "cfe_timer.h"

#include "cfe_error.h"

#include "net_ebuf.h"
#include "net_ether.h"

#include "cfe_timer.h"

#include "net_ip.h"
#include "net_ip_internal.h"
#include "net_api.h"

#include "env_subr.h"

#if CFG_TCP
#include "net_tcp.h"
#endif

#include "ui_command.h"

typedef struct net_ctx_s {
    /* Global info */
    int64_t timer;

    /* device name */
    char *devname;

    /* Run-time info for IP interface */
    ip_info_t *ipinfo;

    /* Info for Ethernet interface */
    ether_info_t *ethinfo;

    /* Info specific to UDP */
    udp_info_t *udpinfo;

    /* Info specific to ICMP */
    icmp_info_t *icmpinfo;

#if CFG_TCP
    /* Info specific to TCP */
    tcp_info_t *tcpinfo;
#endif
} net_ctx_t;


#pragma pack(1)

typedef uint8_t BYTE;
typedef uint16_t WORD;
typedef uint32_t DWORD;

/* NMRP timeouts */
enum _nmrp_timeouts_ {
    NMRP_TIMEOUT_REQ		=   1/2, /* 0.5 sec */
    NMRP_TIMEOUT_LISTEN		=     3, /* 3 sec */
    NMRP_TIMEOUT_ACTIVE		= 60000, /* 1 minute */
    NMRP_TIMEOUT_CLOSE		=  6000, /* 6 sec */
    NMRP_TIMEOUT_ADVERTISE	=   500  /* 0.5 sec */
};

enum _nmrp_errors_ {
	NMRP_ERR_NONE = 0,
	NMRP_ERR_MSG_INVALID_LEN,
	NMRP_ERR_MSG_TOO_LONG,
	NMRP_ERR_MSG_TOO_MANY_OPT,
	NMRP_ERR_MSG_UNKNOWN_OPT,
	NMRP_ERR_MSG_INVALID_OPT,
	NMRP_ERR_NO_BUF,
	NMRP_ERR_GENERAL
};

/* NMRP codes */
enum _nmrp_codes_ {
	NMRP_CODE_ADVERTISE		= 0x01,
	NMRP_CODE_CONF_REQ		= 0x02,
	NMRP_CODE_CONF_ACK		= 0x03,
	NMRP_CODE_CLOSE_REQ		= 0x04,
	NMRP_CODE_CLOSE_ACK		= 0x05,
	NMRP_CODE_KEEP_ALIVE_REQ = 0x06,
	NMRP_CODE_KEEP_ALIVE_ACK = 0x07,
	NMRP_CODE_TFTP_UL_REQ	= 0x10
};

/* NMRP option types */
enum _nmrp_option_types_ {
	NMRP_OPT_MAGIC_NO	= 0x0001,
	NMRP_OPT_DEV_IP		= 0x0002,
	NMRP_OPT_FW_UP		= 0x0101
};

typedef enum {
	NMRPC_L2_NO_ERROR = 0,
	NMRPC_L2_TX_PKT_TOO_LONG,
	NMRPC_L2_TX_ETHER_PREPEND_FAIL,
	NMRPC_L2_TX_ERROR,
	NMRPC_L2_GEN_ERR
} NMRPC_L2_RET_CODE;

/* NMRP REQ max retries */
enum _nmrp_req_max_retries_ {
	NMRP_MAX_RETRY_CONF		= 5,
	NMRP_MAX_RETRY_CLOSE	= 9999,
	NMRP_MAX_RETRY_TFTP_UL	= 4
};


enum _nmrp_state_ {
	NMRP_STATE_LISTEN		= 0,
	NMRP_STATE_CONFIGURE,
	NMRP_STATE_TRANSFER,
	NMRP_STATE_CLOSE
};

#define NMRP_MAX_OPT_PER_MSG	(6)
#define ETHER_NMRP		(0x0912)

#define NMRP_HDR_LEN		(sizeof(NMRP_MSG) - sizeof(NMRP_OPT))
#define NMRP_MIN_OPT_LEN	(sizeof(NMRP_OPT) - 1)

#define NMRP_MAGIC_NO	"\x4E\x54\x47\x52"
#define NMRP_MAGIC_NO_LEN	(sizeof(NMRP_MAGIC_NO) - 1)

typedef struct {
	uint8_t destMAC[6];
	uint8_t srcMAC[6];
	uint16_t etherType;
} ETHER_HDR;

typedef struct {
	uint16_t type;
	uint16_t len;
	uint8_t value;
} NMRP_OPT;

typedef struct {
    uint16_t reserved;
    uint8_t code;
    uint8_t id;
    uint16_t length;

	NMRP_OPT opt;
} NMRP_MSG;

typedef struct {
    ETHER_HDR etherHdr;
    NMRP_MSG nmrpMsg;
} ETHER_NMRP_PKT;

typedef struct {
	uint16_t type;
	uint16_t len;

	union {
		uint8_t byteVal;
		uint16_t wordVal;
		uint32_t dwordVal;
		const uint8_t *streamVal;
	} value;
} NMRP_PARSED_OPT;

typedef struct {
	uint8_t code;
	uint8_t id;
	uint16_t length;

	int numOptions;
	NMRP_PARSED_OPT options[NMRP_MAX_OPT_PER_MSG];
} NMRP_PARSED_MSG;

typedef struct {
	int reqCode;
	const char reqName[32], respName[32];

	int (* RespParser) (const void *pkt, int pktLen, const BYTE *serverMac, void *userData);
} NMRP_REQ_CUSTOM;

typedef struct {
    
	BYTE peerMac[6];
	BYTE myMac[6];

	DWORD peerIp;
	DWORD myIp;
	WORD ipId;

	WORD peerPort;
	WORD myPort;
} NET_INFO;

#pragma pack()

#define MIN_ETHER_NMRP_LEN	(sizeof(ETHER_HDR) + NMRP_HDR_LEN)

#if 0
#define HTONCS(constWord)	((((constWord) & 0xff00) >> 8) | (((constWord) & 0xff) << 8))
#define NTOHCS(constWord)	HTONCS(constWord)
#define HTONCL(constDword)	((((constDword) & 0xff000000) >> 24) | (((constDword) & 0xff0000) >> 8) | (((constDword) & 0xff00) << 8) | (((constDword) & 0xff) << 24))
#define NTOHCL(constDword)	HTONCL(constDword)
#endif

#define HTONCS(constWord)	constWord
#define NTOHCS(constWord)	constWord
#define HTONCL(constDword)	constDword
#define NTOHCL(constDword)	constDword
/*
#define IS_NMRP_PKT(etherNmrpPkt, pktLen, parsedNmrpMsg, sourceMAC) \
	((pktLen) > MIN_ETHER_NMRP_LEN && (etherNmrpPkt)->etherHdr.etherType == HTONCS(ETHER_NMRP) && \
	((sourceMAC) == NULL || memcmp((sourceMAC), (etherNmrpPkt)->etherHdr.srcMAC, 6) == 0) && \
	NMRP_MsgParsing(&(etherNmrpPkt)->nmrpMsg, (pktLen) - sizeof(ETHER_HDR), (parsedNmrpMsg)) == NMRP_ERR_NONE)
*/

#define IS_NMRP_PKT(etherNmrpPkt, pktLen, parsedNmrpMsg, sourceMAC) \
	((pktLen) > MIN_ETHER_NMRP_LEN && (etherNmrpPkt)->etherHdr.etherType == HTONCS(ETHER_NMRP) && \
	((sourceMAC) == NULL || memcmp((sourceMAC), (etherNmrpPkt)->etherHdr.srcMAC, 6) == 0))

static int nmrp_rx_callback(ebuf_t *buf,void *ref);
int NMRP_MsgParsing(const NMRP_MSG *pkt, int pktLen, NMRP_PARSED_MSG *msg);
NMRP_PARSED_OPT *NMRP_MsgGetOpt(NMRP_PARSED_MSG *msg, uint16_t optType);
//int NMRPRequest( const BYTE *serverMac, const BYTE *clientMac, NMRP_REQ_CUSTOM *custom, void *userData);
static int NMRPConfiguring( const BYTE *serverMac, const BYTE *clientMac, DWORD *ipAddr, DWORD *ipSubnetMask, int *fwUpgrade);
static int NMRPConfAckParser(const void *pkt, int pktLen, const BYTE *serverMac, void *userData);
//int NMRPTFTPWaiting(const NET_INFO *netInfo, DWORD *peerIp, WORD *peerPort, WORD *ipId);
int NMRPTFTPWaiting(void);
int NMRPKeepAlive(void);
static int NMRPClosing(void);
static int NMRPSend(NMRP_REQ_CUSTOM *custom);

int nmrp_server_detected;
int g_nmrp_config_acked;
int g_nmrp_close_acked;
//int g_Listening;
int g_NMRP_State;
extern net_ctx_t *netctx;

NET_INFO g_NetInfo;
//ebuf_t *g_nmrp_txbuf = NULL;
int g_portal_number;
NMRP_PARSED_MSG g_nmrpMsg;
int g_tftp_upgrade_success;
int g_nmrp_keepalive;//jenny add for fixed NMRP server timeout issue 
unsigned char g_received_packet[1600];

int gpio_control(int gpio, int value);
extern int32_t _getticks(void);		/* return value of CP0 COUNT */
static unsigned long seed = 1;

void srand(unsigned long local_seed);
unsigned long rand(void);
int _start_nmrp(void);

void srand(unsigned long local_seed)
{
    seed = local_seed;
}

unsigned long rand(void)
{
	
	long x, hi, lo, t;

	x = seed;
	hi = x / 127773;
	lo = x % 127773;
	t = 16807 * lo - 2836 * hi;
	if (t <= 0) t += 0x7fffffff;
	seed = t;
	return t;
}

int _start_nmrp (void)
{ 
    uint8_t nmrpproto[2];
    
    int ret = 0;
    int ui_cmd_status;
    int fwUpgrade;
    int64_t timer;
    DWORD ipSubnetMask;
    char buf[512];
    unsigned char *pIP, *pMask;   
        
    srand( (unsigned long)_getticks());
        
    //net_init("eth0");
    nmrpproto[0] = (PROTOSPACE_NMRP >> 8) & 0xFF;
    nmrpproto[1] = (PROTOSPACE_NMRP & 0xFF);
    g_tftp_upgrade_success = 0;    
    g_nmrp_keepalive = 0;//jenny add for timeout
    
    g_portal_number = eth_open(netctx->ethinfo, ETH_PTYPE_DIX, (char*)nmrpproto, nmrp_rx_callback, NULL);
    if(g_portal_number < 0)
    {
        ret = -1;
        goto _clean_and_exit;
    }
    
    eth_gethwaddr(netctx->ethinfo, g_NetInfo.myMac);
//_retry:    
    g_NMRP_State = NMRP_STATE_LISTEN;
    nmrp_server_detected = 0;
    TIMER_SET(timer, NMRP_TIMEOUT_LISTEN*CFE_HZ);
    while (!TIMER_EXPIRED(timer)) 
    {  
         POLL();
	 if(nmrp_server_detected)
	 {
             xprintf ("find server.\n");	        
             break;
	 }
    }    	
    
    if(!nmrp_server_detected)
    {
        ret = -1;
        goto _clean_and_exit;
    }
    
    g_nmrp_config_acked = 0;
    
    if (NMRPConfiguring( g_NetInfo.peerMac, g_NetInfo.myMac, &g_NetInfo.myIp, &ipSubnetMask, &fwUpgrade) == -1) 
    {
        ret = -1;
        goto _clean_and_exit;
    }
	
    pIP = (unsigned char *)&g_NetInfo.myIp;
    pMask = (unsigned char *)&ipSubnetMask;

    sprintf(buf, "ifconfig eth0 -addr=%u.%u.%u.%u -mask=%u.%u.%u.%u", *(pIP), *(pIP+1), *(pIP+2), *(pIP+3),
	                                                             *(pMask), *(pMask+1), *(pMask+2), *(pMask+3) );
    ui_docommand(buf);
	

_retry_tftp:
	
    NMRPTFTPWaiting();
    //eth_close(netctx->ethinfo, g_portal_number);
    //g_portal_number = 0;
    //net_init("eth0");
    ui_cmd_status = ui_docommand("tftpd nmrp");
	

    sprintf(buf, "ifconfig eth0 -addr=%u.%u.%u.%u -mask=%u.%u.%u.%u", *(pIP), *(pIP+1), *(pIP+2), *(pIP+3),
	                                                             *(pMask), *(pMask+1), *(pMask+2), *(pMask+3) );

    ui_docommand(buf);

    //net_init("eth0");
    g_portal_number = eth_open(netctx->ethinfo, ETH_PTYPE_DIX, (char*)nmrpproto, nmrp_rx_callback, NULL);

    if(ui_cmd_status != 0)
    {
        goto _retry_tftp;
    }
	
    g_nmrp_close_acked = 0;
    /* Foxconn modified start, zacker,04/07/2008 */
    /* ui_docommand("nvram erase"); */
    // nvram_unset("restore_defaults");
    // nvram_commit();
    /* Foxconn modified end, zacker,04/07/2008 */

    NMRPClosing();
    g_tftp_upgrade_success = 1;
    // gpio_control(7, 1);     //turn off power led       
	
_clean_and_exit:
   
    if(g_portal_number > 0)
       eth_close(netctx->ethinfo, g_portal_number);

    return ret;
}

extern void setPowerOnLedOn(void);
static int nmrp_rx_callback(ebuf_t *buf,void *ref)
{
    int pktLen = 0;
    NMRP_PARSED_MSG nmrpMsg;
    const ETHER_NMRP_PKT *pkt;
    unsigned char *p;
    
    pkt = (const ETHER_NMRP_PKT *)buf->eb_data;
    pktLen = buf->eb_length + sizeof(ETHER_HDR) - 4;
    
    p=(unsigned char *)pkt;

    if(g_NMRP_State == NMRP_STATE_LISTEN)
    {   
        if (IS_NMRP_PKT(pkt, pktLen, &nmrpMsg, NULL) && (*(p+16) == NMRP_CODE_ADVERTISE ) )
        {         
            if (NMRP_MsgParsing(&(pkt)->nmrpMsg, (pktLen) - sizeof(ETHER_HDR), &nmrpMsg) == NMRP_ERR_NONE)
            {
      	        if(NMRP_MsgGetOpt(&nmrpMsg, NMRP_OPT_MAGIC_NO) != NULL)
        	{
            	    memcpy(g_NetInfo.peerMac, buf->eb_data+6, 6);
            	    g_NMRP_State = NMRP_STATE_CONFIGURE;
            	    nmrp_server_detected = 1;
                    setPowerOnLedOn();  /* Foxconn added for U12H154, turn on TEST LED */
            	}
            }      
        }
    }
    else if(g_NMRP_State == NMRP_STATE_CONFIGURE)
    {   
        if (IS_NMRP_PKT(pkt, pktLen, &nmrpMsg, NULL) && (*(p+16) == NMRP_CODE_CONF_ACK ) )
	{
	    memcpy(g_received_packet, pkt, pktLen);
	    pkt = (const ETHER_NMRP_PKT *)g_received_packet;
            if (NMRP_MsgParsing(&(pkt)->nmrpMsg, (pktLen) - sizeof(ETHER_HDR), &nmrpMsg) == NMRP_ERR_NONE)
    	    {
                if(NMRPConfAckParser(pkt, pktLen, g_NetInfo.peerMac, &nmrpMsg))
        	{
        	    g_nmrpMsg = nmrpMsg;
                    g_nmrp_config_acked = 1;
                    g_NMRP_State = NMRP_STATE_TRANSFER;
        	}
            }
        }
    }
    else if(g_NMRP_State == NMRP_STATE_TRANSFER)
    {
        if (IS_NMRP_PKT(pkt, pktLen, &nmrpMsg, NULL) && (*(p+16) == NMRP_CODE_CLOSE_ACK ) )
        {
            if (NMRP_MsgParsing(&(pkt)->nmrpMsg, (pktLen) - sizeof(ETHER_HDR), &nmrpMsg) == NMRP_ERR_NONE)
            {
                g_NMRP_State =NMRP_STATE_CLOSE;
                g_nmrp_close_acked = 1;
            }
        }
    }
  
    return ETH_DROP;
}

static int NMRP_OptValParsing(NMRP_PARSED_OPT *optParsed, const uint8_t *value)
{
	int retVal = NMRP_ERR_NONE;
	
	switch (optParsed->type) {
		case NMRP_OPT_MAGIC_NO: /* We require the lenght MUST be correct for the MAGIC-NO option */
			if (optParsed->len != (NMRP_MIN_OPT_LEN + NMRP_MAGIC_NO_LEN) || memcmp(value, NMRP_MAGIC_NO, NMRP_MAGIC_NO_LEN))
			{
				retVal = NMRP_ERR_MSG_INVALID_OPT;
			}
			else
			{
				optParsed->value.streamVal = value;
			}
			break;

		case NMRP_OPT_DEV_IP: /* save the value of IP in the byte stream format, the actual IP is streamVal[0].streamVal[1].streamVal[2].streamVal[3]. */
			//if ( optParsed->len != (NMRP_MIN_OPT_LEN + 8) || !IsIPAddrValid(ntohl(*(DWORD *) value), ntohl(*(DWORD *) (value + 4))) )
			if ( optParsed->len != (NMRP_MIN_OPT_LEN + 8) )
				retVal = NMRP_ERR_MSG_INVALID_OPT;
			else
				optParsed->value.streamVal = value;
			break;

		case NMRP_OPT_FW_UP: /* don't check the length of FW-UP option for future compatibility */
			break;

		default:
			retVal = NMRP_ERR_MSG_UNKNOWN_OPT;
			break;
	}

	return retVal;
}

int NMRP_MsgParsing(const NMRP_MSG *pkt, int pktLen, NMRP_PARSED_MSG *msg)
{
	if (pktLen < NMRP_HDR_LEN)
		return NMRP_ERR_MSG_INVALID_LEN;

	msg->code = pkt->code;
	msg->id = pkt->id;

	if ((msg->length = NTOHCS(pkt->length)) <= pktLen && msg->length >= NMRP_HDR_LEN) {
		int retVal;
		uint16_t optLen;
		NMRP_OPT *opt;
		NMRP_PARSED_OPT *optParsed;
		
		opt = (NMRP_OPT *)(((uint8_t *) pkt) + NMRP_HDR_LEN);
		optParsed = msg->options;
		msg->numOptions = 0;		

		/* use the length indicated in the NMRP header */
		for (pktLen = msg->length - NMRP_HDR_LEN; pktLen >= NMRP_MIN_OPT_LEN; ) 
		{
			optParsed->type = NTOHCS(opt->type);
			optParsed->len = optLen = NTOHCS(opt->len);
			
			if (optParsed->len > pktLen)
				return NMRP_ERR_MSG_INVALID_OPT;

			if ((retVal = NMRP_OptValParsing(optParsed, &opt->value)) == NMRP_ERR_NONE) 
			{
				optParsed++;

				if (++msg->numOptions >= NMRP_MAX_OPT_PER_MSG)
					return NMRP_ERR_MSG_TOO_MANY_OPT;
			}
			else if (retVal != NMRP_ERR_MSG_UNKNOWN_OPT)
			{
				return NMRP_ERR_MSG_INVALID_OPT;
			}

			pktLen -= optLen;
			opt = (NMRP_OPT *)(((uint8_t *) opt) + optLen);
		}
	} /* end if the msg->length is larger than the received packet length */

	return pktLen == 0 ? NMRP_ERR_NONE : NMRP_ERR_MSG_INVALID_LEN;
}

NMRP_PARSED_OPT *NMRP_MsgGetOpt(NMRP_PARSED_MSG *msg, uint16_t optType)
{
	NMRP_PARSED_OPT *opt, *optEnd;

	optEnd = &msg->options[msg->numOptions];

	for (opt = msg->options; opt != optEnd; opt++)
		if (opt->type == optType)
			break;

	return msg->numOptions == 0 ? NULL : (opt == optEnd ? NULL : opt);
}

static int NMRPClosing(void)
{
    int ret, retries=0;
    static NMRP_REQ_CUSTOM reqClose = { NMRP_CODE_CLOSE_REQ, "CLOSE-REQ", "CLOSE-ACK", NULL };

    do {
	    ret = NMRPSend(&reqClose);
	    if( !ret)
	    {
	        cfe_sleep(CFE_HZ/2);
                if(g_nmrp_close_acked)
                {
                    break;
                }
            }	    
    } while(++retries <= NMRP_MAX_RETRY_CLOSE);

    return 0;
}

int NMRPTFTPWaiting(void)
{
	static NMRP_REQ_CUSTOM reqTftpUl = { NMRP_CODE_TFTP_UL_REQ, "TFTP-UL-REQ", "TFTP-WRQ", NULL };
	
	return NMRPSend(&reqTftpUl);
}

/* Foxconn modify start by Jenny Zhao, 08/07/2008, for fixed timeout issue*/
int NMRPKeepAlive(void)
{
	static NMRP_REQ_CUSTOM reqkeepalive = { NMRP_CODE_KEEP_ALIVE_REQ, "KEEPALIVE_REQ", "KEEPALIVE-ACK", NULL };
	
	return NMRPSend(&reqkeepalive);
}
/* Foxconn modify end by Jenny Zhao, 08/07/2008*/

static int NMRPConfAckParser(const void *pkt, int pktLen, const BYTE *serverMac, void *userData)
{
	NMRP_PARSED_MSG *nmrpMsg = (NMRP_PARSED_MSG *) userData;

	return NMRP_MsgGetOpt(nmrpMsg, NMRP_OPT_DEV_IP) != NULL;
	/*
	return IS_NMRP_PKT((const ETHER_NMRP_PKT *)pkt, pktLen, nmrpMsg, serverMac) &&
	       nmrpMsg->code == NMRP_CODE_CONF_ACK && 
		   NMRP_MsgGetOpt(nmrpMsg, NMRP_OPT_DEV_IP) != NULL;
		   */
}


static int NMRPConfiguring( const BYTE *serverMac, 
                            const BYTE *clientMac, 
                            DWORD *ipAddr, DWORD *ipSubnetMask, 
                            int *fwUpgrade)
{
     //int ret, retries=0;
     int ret;
     NMRP_PARSED_OPT *devIp;
     static NMRP_REQ_CUSTOM reqConf = { NMRP_CODE_CONF_REQ, "CONF-REQ", "CONF-ACK", NMRPConfAckParser };
	
     do {
	
	    ret = NMRPSend(&reqConf);
	    if( !ret)
	    {
	        cfe_sleep(CFE_HZ/2);
            if(g_nmrp_config_acked)
            {
                break;
            }
        }
	    
	//}while(++retries <= NMRP_MAX_RETRY_CLOSE);
    } while(1);

    if(g_nmrp_config_acked)
    {
	    devIp = NMRP_MsgGetOpt(&g_nmrpMsg, NMRP_OPT_DEV_IP);
	    *ipAddr = (* (DWORD *) devIp->value.streamVal);
	    *ipSubnetMask = (* (DWORD *) (devIp->value.streamVal + 4));
	    *fwUpgrade = NMRP_MsgGetOpt(&g_nmrpMsg, NMRP_OPT_FW_UP) != NULL;
	    return 0;
    }
    return -1;
}

static int NMRPSend(NMRP_REQ_CUSTOM *custom)
{
    ebuf_t *txbuf;
    NMRP_MSG *msg;
    unsigned long random_backoff;

    txbuf = eth_alloc(netctx->ethinfo, g_portal_number);
    if (!txbuf) 
    {
        return -1;
    }
    msg = (NMRP_MSG *)(txbuf->eb_data + 14);
    msg->reserved = 0;
    msg->code = custom->reqCode;
    msg->id = 0;
    msg->length = HTONCS(NMRP_HDR_LEN); /* header only, no option */
    txbuf->eb_length = NMRP_HDR_LEN;

    random_backoff = rand();
    random_backoff = random_backoff % 30;
    xprintf("%s sent...Waiting for %s...\n", custom->reqName, custom->respName);
    cfe_sleep(random_backoff);
    eth_send(txbuf, (uint8_t *)g_NetInfo.peerMac);
    eth_free(txbuf);
        
    return 0;
}