#include "vxWorks.h"
#include "netinet/in.h"
#include "netinet/ip.h"
#include "netinet/if_ether.h"
#include "netinet/udp.h"
#include "private/muxlibP.h"
 
#define ARRAY_BROADCAST_PORT  2020
#define ARRAY_ACQ_PORT        2021
#define EH_SIZE                 14
#define IPHL                    20
#define UDPHL                   8

struct myIpFrame      
{
 struct ip *iph;
 struct udphdr *udph;
 char   *pdata;
};
struct ps_udph        
{
    struct in_addr      srcip;
    struct in_addr      dstip;
    char                zero;
    char                prto;
    short               ulen;
};
struct myBuffer
{
 char *buf;
        u_short  size;
};
 
 
 
#define MAX_DATA_LEN    500
void *SendCookie=0;
long recvLen=0;
BOOL stackRcvRtn(
void * pCookie,
long type,
M_BLK_ID pMblk,
LL_HDR_INFO * pLinkHdr,
void * pSpare
)
{
 
 
int i=0;
/*printf("data len %d\n",pMblk->mBlkHdr.mLen);
 
   for(i=0;i<pMblk->mBlkHdr.mLen;i++)
    {
      printf("%3x",*((unsigned char *)pMblk->mBlkHdr.mData+i));
     }
  printf("\n");*/
   recvLen++;
 
return(FALSE);
}
void* myUdpBroadcast(char *pName, int unit)
{
 
   void *pSendCookie;

 
  if((pSendCookie = muxBind(pName,unit,(FUNCPTR)stackRcvRtn,NULL,NULL,NULL,  MUX_PROTO_SNARF,"baitao",NULL)) == NULL)
  {
  perror("muxBind");
  return(ERROR);
  }
 
 
  return pSendCookie;
}

void sendUdp(char *udp_data,int size  )
{
  
   END_OBJ *pEnd;
   M_BLK_ID  pMblk;
   struct ps_udph pudph;
   struct ether_header eth_head;
   struct myIpFrame myframe;
   struct myBuffer pbuf;
   char *pData;
   int length;
   const char dst_mac_addr[6]={0x00,0x17,0xA4,0xDD,0x88,0x6C};
   const char src_mac_addr[6]={0x00,0x30,0x40,0x10,0x00,0x40};
  if(udp_data == NULL || size<=0) return (ERROR);
   if((pEnd = ((MUX_ID)SendCookie)->pEnd) == NULL)
  {
   perror("pEnd");
   muxUnbind(SendCookie,MUX_PROTO_PROMISC,(FUNCPTR)stackRcvRtn);
   return(ERROR);
  }
  if ((pMblk = netTupleGet (pEnd->pNetPool, 1514, M_DONTWAIT, MT_DATA,
                            FALSE)) == NULL)
  {
    perror("pMblk");
    muxUnbind(SendCookie,MUX_PROTO_PROMISC,(FUNCPTR)stackRcvRtn);
    return(ERROR);
  }
  pMblk->mBlkHdr.mFlags |= M_PKTHDR;
  pMblk->mBlkHdr.mLen  = 0;
  pMblk->mBlkHdr.mData = pMblk->pClBlk->clNode.pClBuf;
 
  length = size;
  if(length > MAX_DATA_LEN)
     length = MAX_DATA_LEN;
  pbuf.size = IPHL + UDPHL + length;
  if((pbuf.buf = (char *) memalign (sizeof(long),pbuf.size))==NULL)
  {
    perror("pbuf");
    muxUnbind(SendCookie,MUX_PROTO_PROMISC,(FUNCPTR)stackRcvRtn);
    return(ERROR);
  } 
  bzero(pbuf.buf,pbuf.size);
 
  pData = pbuf.buf;
  myframe.iph  = (struct ip*)(pData);
  myframe.udph = (struct udphdr*)(pData+IPHL);
  myframe.pdata = (char *)(pData+UDPHL+IPHL);

  bcopy(udp_data, myframe.pdata, length);

  bcopy(dst_mac_addr, eth_head.ether_dhost,6);
  bcopy(src_mac_addr, eth_head.ether_shost,6);
  eth_head.ether_type =htons(0x0921); //htons(ETHERTYPE_IP);

  myframe.udph->uh_sport = htons(ARRAY_BROADCAST_PORT);
  myframe.udph->uh_dport = htons(ARRAY_ACQ_PORT);
  myframe.udph->uh_ulen  = htons (length + UDPHL);
  myframe.udph->uh_sum = 0;
  pudph.srcip.s_addr = INADDR_ANY;
  pudph.dstip.s_addr = INADDR_BROADCAST;
  pudph.zero = 0;
  pudph.prto = IPPROTO_UDP;
  pudph.ulen = myframe.udph->uh_ulen;
  myframe.udph->uh_sum = udp_cksum (&pudph, (char *) myframe.udph,
                                     ntohs (pudph.ulen));

  myframe.iph->ip_v = IPVERSION;
  myframe.iph->ip_hl = IPHL >> 2;
  myframe.iph->ip_tos = 0;
  myframe.iph->ip_len = htons (UDPHL + IPHL + length);
  myframe.iph->ip_id  = myframe.udph->uh_sum;
  myframe.iph->ip_off = htons (IP_DF);  
  myframe.iph->ip_ttl = 0x20;          
  myframe.iph->ip_p = IPPROTO_UDP;
  myframe.iph->ip_src.s_addr = INADDR_ANY;
  myframe.iph->ip_dst.s_addr = INADDR_BROADCAST;
  myframe.iph->ip_sum = 0;
  myframe.iph->ip_sum = checksum ((u_short *)myframe.iph,
                                  (myframe.iph->ip_hl) << 2);
  pData = pMblk->mBlkHdr.mData;
  bcopy((char*)&eth_head,pData,EH_SIZE);
  bcopy(pbuf.buf,pData+EH_SIZE,pbuf.size);
  pMblk->mBlkHdr.mLen  = EH_SIZE + pbuf.size;
  pMblk->mBlkPktHdr.len  = pMblk->mBlkHdr.mLen;
  
  if(muxSend(SendCookie,pMblk)!=OK)
  {
     perror("muxSend");
     netMblkClChainFree(pMblk);
     cfree(pbuf.buf);
     muxUnbind(SendCookie,MUX_PROTO_PROMISC,(FUNCPTR)stackRcvRtn);
     return (ERROR);
  }
 
}
void unBindMux()
{
  
  muxUnbind(SendCookie,MUX_PROTO_PROMISC,(FUNCPTR)stackRcvRtn);
}
void recvShow()
{
printf("recvShow len %d \n", recvLen);
}

void usrAppInit (void)
    {
#ifdef USER_APPL_INIT
 USER_APPL_INIT;  /* for backwards compatibility */
#endif
sys_maintask();
 SendCookie=myUdpBroadcast("motfcc", 0);
 sendUdp("ddddddddddddddddddddddddddd77777777777777",30  );
    /* add application specific code here */
    }