/*-----------------------------------------------------------------------------------*/
void
uip_arp_arpin(void)
{
	if(uip_len < sizeof(struct arp_hdr)) {
		uip_len = 0;
		return;
	}
	uip_len = 0;

	switch(BUF->opcode) {
	case HTONS(ARP_HINT):
		/* Please note this is not a valid ARP type, this is just a 
		 * hint to implement prefetch/refresh of ARP mapping */

		/* This is a valid hint if we are the source of this request,
		 * the requested ipaddr is in dipaddress */
		if(uip_ipaddr_cmp(BUF->sipaddr, uip_hostaddr)) {
			/* We first try to check for the destination address 
			 * in our ARP table */
			if(uip_arp_update(BUF->dipaddr, &broadcast_ethaddr)) {
			/* If the destination address was not in our ARP table, 
			 * we send out an ARP request for the same */
				memset(BUF->ethhdr.dest.addr, 0xff, 6);
				BUF->opcode = HTONS(ARP_REQUEST);
				/* The other ARP fields of incoming hint are 
				 * supposed to be same as ARP broadcast except
				 * the opcode field */

				uip_len = sizeof(struct arp_hdr);
			}
		}
		break;

	case HTONS(ARP_REQUEST):
		/* ARP request. If it asked for our address, we send out a
		   reply. */
		if(uip_ipaddr_cmp(BUF->dipaddr, uip_hostaddr)) {
			/* First, we register the one who made the request in our ARP
			   table, since it is likely that we will do more communication
			   with this host in the future. */
			uip_arp_update(BUF->sipaddr, &BUF->shwaddr);

			BUF->opcode = HTONS(ARP_REPLY);

			memcpy(BUF->dhwaddr.addr, BUF->shwaddr.addr, 6);
			memcpy(BUF->shwaddr.addr, uip_ethaddr.addr, 6);
			memcpy(BUF->ethhdr.src.addr, uip_ethaddr.addr, 6);
			memcpy(BUF->ethhdr.dest.addr, BUF->dhwaddr.addr, 6);

			BUF->dipaddr[0] = BUF->sipaddr[0];
			BUF->dipaddr[1] = BUF->sipaddr[1];
			BUF->sipaddr[0] = uip_hostaddr[0];
			BUF->sipaddr[1] = uip_hostaddr[1];

			BUF->ethhdr.type = HTONS(UIP_ETHTYPE_ARP);
			uip_len = sizeof(struct arp_hdr);
		}
		break;
	case HTONS(ARP_REPLY):
		/* ARP reply. We insert or update the ARP table if it was meant
		   for us. */
		if(uip_ipaddr_cmp(BUF->dipaddr, uip_hostaddr)) {
			uip_arp_update(BUF->sipaddr, &BUF->shwaddr);
			vmm_completion_complete(&uip_arp_prefetch_done);
		}
		break;
	}

	return;
}

// called for http server app
void httpd_appcall(void){
	struct fs_file fsfile;
	unsigned int i;

	switch(uip_conn->lport){

		case HTONS(80):

			// app state
			hs = (struct httpd_state *)(uip_conn->appstate);

			// closed connection
			if(uip_closed()){
				httpd_state_reset();
				uip_close();
				return;
			}

			// aborted connection or time out occured
			if(uip_aborted() || uip_timedout()){
				httpd_state_reset();
				uip_abort();
				return;
			}

			// if we are pooled
			if(uip_poll()){
				if(hs->count++ >= 100){
					httpd_state_reset();
					uip_abort();
				}
				return;
			}

			// new connection
			if(uip_connected()){
				httpd_state_reset();
				return;
			}

			// new data in STATE_NONE
			if(uip_newdata() && hs->state == STATE_NONE){

				// GET or POST request?
				if(uip_appdata[0] == ISO_G && uip_appdata[1] == ISO_E && uip_appdata[2] == ISO_T && (uip_appdata[3] == ISO_space || uip_appdata[3] == ISO_tab)){
					hs->state = STATE_FILE_REQUEST;
				} else if(uip_appdata[0] == ISO_P && uip_appdata[1] == ISO_O && uip_appdata[2] == ISO_S && uip_appdata[3] == ISO_T && (uip_appdata[4] == ISO_space || uip_appdata[4] == ISO_tab)){
					hs->state = STATE_UPLOAD_REQUEST;
				}

				// anything else -> abort the connection!
				if(hs->state == STATE_NONE){
					httpd_state_reset();
					uip_abort();
					return;
				}

				// get file or firmware upload?
				if(hs->state == STATE_FILE_REQUEST){

					// we are looking for GET file name
					for(i = 4; i < 30; i++){
						if(uip_appdata[i] == ISO_space || uip_appdata[i] == ISO_cr || uip_appdata[i] == ISO_nl || uip_appdata[i] == ISO_tab){
							uip_appdata[i] = 0;
							i = 0;
							break;
						}
					}

					if(i != 0){
						printf("## Error: request file name too long!\n");
						httpd_state_reset();
						uip_abort();
						return;
					}

					printf("Request for: ");
					printf("%s\n", &uip_appdata[4]);

					// request for /
					if(uip_appdata[4] == ISO_slash && uip_appdata[5] == 0){
						fs_open(file_index_html.name, &fsfile);
					} else {
						// check if we have requested file
						if(!fs_open((const char *)&uip_appdata[4], &fsfile)){
							printf("## Error: file not found!\n");
							fs_open(file_404_html.name, &fsfile);
						}
					}

					hs->state = STATE_FILE_REQUEST;
					hs->dataptr = (u8_t *)fsfile.data;
					hs->upload = fsfile.len;

					// send first (and maybe the last) chunk of data
					uip_send(hs->dataptr, (hs->upload > uip_mss() ? uip_mss() : hs->upload));
					return;

				} else if(hs->state == STATE_UPLOAD_REQUEST){
//.........这里部分代码省略.........

// Task for reactivision output processing
void vServoTask( void)
{
    u16_t ripaddr[2];
    portBASE_TYPE xStatus;

    xTuioQueuePacket xValueRead;

    xTuioQueuePacket xDistanceValue[6];

    // Tracking Variables
    short sPosX = 0;
    short sPosY = 0;
    short sDegreesX = servoMIN_DEGREES;
    short sDegreesY = (servoMIN_DEGREES+servoMAX_DEGREES)/4;

    vServo_ConfigurePwm(sDegreesX, sDegreesY);

    sX = 0;
    sY = 0;
    sZ = 0;

    // Uip connect
    vTaskDelay(1000);
    uip_ipaddr(ripaddr, 192,168,0,1);
    uip_connect(ripaddr, HTONS(3000));

    xTuioQueue = xQueueCreate(20, sizeof(xTuioQueuePacket));

    vSemaphoreCreateBinary(xControlServo);

    short sFlightPlanStage = servotaskFLIGHT_PLAN_1;
    short sGoalPoint;
    short sGoalCounter;

    // Servo control loop
    for (;;) {

        //Read from TUIO queue.
        xStatus = xQueueReceive(xTuioQueue, &xValueRead, 10);     // Block task for 10ms when waiting for the Queue

        if (xStatus == pdPASS) {   // Process received value

            // values are between 0 and 1
            sPosX = (short) (xValueRead.position_x*100.0f);
            sPosY = (short) (xValueRead.position_y*100.0f);

            short sId = xValueRead.class_id-servoFIDUCIAL_SET;

            // If the middle fiducial marker, track it with the camera
            if (sId >= 0 && sId <= 5) {
                // Remember, position is taken from TOP LEFT
                if (sPosX < servoBOUNDING_BOX_MIN && sDegreesX < servoMAX_DEGREES) {
                    sDegreesX++;
                } else if (sPosX > servoBOUNDING_BOX_MAX && sDegreesX > servoMIN_DEGREES) {
                    sDegreesX--;
                }

                if (sPosY < servoBOUNDING_BOX_MIN && sDegreesY < servoMAX_DEGREES) {
                    sDegreesY++;
                } else if (sPosY > servoBOUNDING_BOX_MAX && sDegreesY > servoMIN_DEGREES) {
                    sDegreesY--;
                }

                // Set the fiducial to being used, and the value to the current packet
                sDistanceUsed[sId] = 1;
                xDistanceValue[sId] = xValueRead;

                // If there is an ID to compare to, calculate distance
                if (sGetId(sId) != -1 && sTask5) {
                    short sNextId = sGetId(sId);

                    // Print markers used for distancing
                    //debug_printf("markers: %d %d\r\n", xValueRead.class_id, xDistanceValue[sNextId].class_id);

                    // Compute the distance to the fiducial
                    double dD = sApproxSqrt(sPow(sPosX-(short) (xDistanceValue[sNextId].position_x*100.0f),2) +
                                           sPow(sPosY-(short) (xDistanceValue[sNextId].position_y*100.0f),2));

                    dD = (33379*sPow((short) dD,2) - 2288800*dD + 44475000)/10000;

                    //debug_printf(">> Distance: %d\r\n", (short) dD);

                    // Calculate the XYZ coordinates.
                    double dDegX = sDegreesX - servoMIN_DEGREES - (servoMIN_DEGREES+servoMAX_DEGREES)/2;
                    double dDegY = sDegreesY - servoMIN_DEGREES;

                    sX = (short) (dD*(sin((double) (dDegX/180.0f*3.14f))));
                    sY = (short) (dD*(sin((double) (dDegY/180.0f*3.14f))));
                    sZ = (short) (dD*(cos((double) (dDegX/180.0f*3.14f))));

                    //debug_printf(">> Angles: %d %d\r\n", (short) dDegX, (short) dDegY);
                    //debug_printf(">> Point: %d %d %d\r\n", sX, sY, sZ);

                    // On detecting the blimp, set the goal to 1.5m in X and flight plan to 2
                    if (sId < 3 && sFlightPlanStage == servotaskFLIGHT_PLAN_1) {
                        sGoalPoint = sX + 1500;
                        sGoalCounter = 0;
                        sFlightPlanStage = servotaskFLIGHT_PLAN_2;
                        debug_printf("Starting stage 2\t\t\ Goal: %d\r\n", sGoalPoint);
//.........这里部分代码省略.........

/**
  Extract the discover information and boot server entry from the
  cached packets if unspecified.

  @param[in]      Private      Pointer to PxeBc private data.
  @param[in]      Type         The type of bootstrap to perform.
  @param[in, out] DiscoverInfo Pointer to EFI_PXE_BASE_CODE_DISCOVER_INFO.
  @param[out]     BootEntry    Pointer to PXEBC_BOOT_SVR_ENTRY.
  @param[out]     SrvList      Pointer to EFI_PXE_BASE_CODE_SRVLIST.

  @retval EFI_SUCCESS       Successfully extracted the information.
  @retval EFI_DEVICE_ERROR  Failed to extract the information.

**/
EFI_STATUS
PxeBcExtractDiscoverInfo (
  IN     PXEBC_PRIVATE_DATA               *Private,
  IN     UINT16                           Type,
  IN OUT EFI_PXE_BASE_CODE_DISCOVER_INFO  **DiscoverInfo,
     OUT PXEBC_BOOT_SVR_ENTRY             **BootEntry,
     OUT EFI_PXE_BASE_CODE_SRVLIST        **SrvList
  )
{
  EFI_PXE_BASE_CODE_MODE          *Mode;
  PXEBC_DHCP4_PACKET_CACHE        *Cache4;
  PXEBC_VENDOR_OPTION             *VendorOpt;
  PXEBC_BOOT_SVR_ENTRY            *Entry;
  BOOLEAN                         IsFound;
  EFI_PXE_BASE_CODE_DISCOVER_INFO *Info;
  UINT16                          Index;

  Mode = Private->PxeBc.Mode;
  Info = *DiscoverInfo;

  if (Mode->UsingIpv6) {
    Info->IpCnt    = 1;
    Info->UseUCast = TRUE;

    Info->SrvList[0].Type              = Type;
    Info->SrvList[0].AcceptAnyResponse = FALSE;

    //
    // There is no vendor options specified in DHCPv6, so take BootFileUrl in the last cached packet.
    //
    CopyMem (&Info->SrvList[0].IpAddr, &Private->ServerIp, sizeof (EFI_IP_ADDRESS));

    *SrvList  = Info->SrvList;
  } else {
    Entry     = NULL;
    IsFound   = FALSE;
    Cache4    = (Mode->ProxyOfferReceived) ? &Private->ProxyOffer.Dhcp4 : &Private->DhcpAck.Dhcp4;
    VendorOpt = &Cache4->VendorOpt;

    if (!Mode->DhcpAckReceived || !IS_VALID_DISCOVER_VENDOR_OPTION (VendorOpt->BitMap)) {
      //
      // Address is not acquired or no discovery options.
      //
      return EFI_INVALID_PARAMETER;
    }

    //
    // Parse the boot server entry from the vendor option in the last cached packet.
    //
    Info->UseMCast    = (BOOLEAN) !IS_DISABLE_MCAST_DISCOVER (VendorOpt->DiscoverCtrl);
    Info->UseBCast    = (BOOLEAN) !IS_DISABLE_BCAST_DISCOVER (VendorOpt->DiscoverCtrl);
    Info->MustUseList = (BOOLEAN) IS_ENABLE_USE_SERVER_LIST (VendorOpt->DiscoverCtrl);
    Info->UseUCast    = (BOOLEAN) IS_VALID_BOOT_SERVERS (VendorOpt->BitMap);

    if (Info->UseMCast) {
      //
      // Get the multicast discover ip address from vendor option if has.
      //
      CopyMem (&Info->ServerMCastIp.v4, &VendorOpt->DiscoverMcastIp, sizeof (EFI_IPv4_ADDRESS));
    }

    Info->IpCnt = 0;

    if (Info->UseUCast) {
      Entry = VendorOpt->BootSvr;

      while (((UINT8) (Entry - VendorOpt->BootSvr)) < VendorOpt->BootSvrLen) {
        if (Entry->Type == HTONS (Type)) {
          IsFound = TRUE;
          break;
        }
        Entry = GET_NEXT_BOOT_SVR_ENTRY (Entry);
      }

      if (!IsFound) {
        return EFI_DEVICE_ERROR;
      }

      Info->IpCnt = Entry->IpCnt;
      if (Info->IpCnt >= 1) {
        *DiscoverInfo = AllocatePool (sizeof (*Info) + (Info->IpCnt - 1) * sizeof (**SrvList));
        if (*DiscoverInfo == NULL) {
          return EFI_OUT_OF_RESOURCES;       
        }     
        CopyMem (*DiscoverInfo, Info, sizeof (*Info));
        Info = *DiscoverInfo;
//.........这里部分代码省略.........

// obs³u¿ pakiet
void udp_config_handle_packet(unsigned char* data, unsigned int len) {

    // otrzymany pakiet
    udp_config_packet *packet = (udp_config_packet*) data;

    // ignoruj niepoprawne pakiety
    if (packet->start != UDP_CONFIG_MAGIC) {
        return;
    }

    // ignoruj pakiety z ustawionym adresem MAC innego urz¹dzenia
    if (memcmp(packet->mac, nic_get_mac(), 6) != 0) {
        // MAC niezgodny, sprawdŸ czy nie ustawiono adresu broadcast MAC (ff:ff:ff:ff:ff:ff)
        for (i=0; i<6; i++) {
            if (packet->mac[i] != 0xFF) {
                return;
            }
        }
    }

    switch(packet->type) {

        // wyszukiwanie urz¹dzeñ
        case UDP_CONFIG_TYPE_DISCOVERY:
            // wyszukiwanie okreœlonego typu urz¹dzeñ
            if ( (packet->length == 1) && (packet->data[0] != UDP_CONFIG_DEVICE_TYPE) ) {
                // szukany inny typ urz¹dzenia
                return;
            }

            // formuj pakiet
            packet->type = UDP_CONFIG_TYPE_MY_CONFIG;
            len = udp_config_fill(packet->data);
            
            break;

        // identyfikacja wybranego urz¹dzenia
        case UDP_CONFIG_TYPE_IDENTIFY:
            // mrugaj naprzemiennie diodami RX/TX przez 3 sekundy
            udp_identify_timer = 30;

            packet->type = UDP_CONFIG_TYPE_IDENTIFY_OK;
            len = 0;
            break;

        // niepoprawny typ
        default:
            return;
    }

    // odeœlij pakiet
    struct uip_udp_conn* conn;
    conn = uip_udp_new(&uip_udp_conn->ripaddr, uip_udp_conn->rport);

    if (!conn) {
        return;
    }

    // wyœlij z portu, na którym pakiet zosta³ odebrany
    uip_udp_bind(conn, HTONS(UDP_CONFIG_PORT));

    // nag³ówek    
    packet->start = UDP_CONFIG_MAGIC;
    packet->length = len;

    // nadawca
    memcpy(packet->mac, nic_get_mac(), 6);

    // wyœlij
    uip_udp_send(len + 10);

    // czekaj na wys³anie
    nic_wait_for_send();

    // zamknij po³¹czenia UDP (przychodz¹cy broadcast i wychodz¹cy unicast)
    uip_udp_remove(conn);
    uip_udp_remove(uip_udp_conn);
}

/** Initialization function for the simple TELNET webserver. */
void TELNETServerApp_Init(void)
{
	/* Listen on port 23 for TELNET connections from hosts */
	uip_listen(HTONS(TELNET_SERVER_PORT));
}

void ecmd_net_init()
{
  /* Without teensy support we use tcp */
    uip_listen(HTONS(ECMD_TCP_PORT), ecmd_net_main);
}

void
yport_net_init(void)
{
    uip_listen(HTONS(YPORT_PORT), yport_net_main);
}

static int dhcpc_sendmsg(struct dhcpc_state_s *pdhcpc,
                         struct dhcpc_state *presult, int msgtype)
{
  struct sockaddr_in addr;
  uint8_t *pend;
  in_addr_t serverid = INADDR_BROADCAST;
  int len;

  /* Create the common message header settings */

  memset(&pdhcpc->packet, 0, sizeof(struct dhcp_msg));
  pdhcpc->packet.op    = DHCP_REQUEST;
  pdhcpc->packet.htype = DHCP_HTYPE_ETHERNET;
  pdhcpc->packet.hlen  = pdhcpc->ds_maclen;
  memcpy(pdhcpc->packet.xid, xid, 4);
  memcpy(pdhcpc->packet.chaddr, pdhcpc->ds_macaddr, pdhcpc->ds_maclen);
  memset(&pdhcpc->packet.chaddr[pdhcpc->ds_maclen], 0, 16 - pdhcpc->ds_maclen);
  memcpy(pdhcpc->packet.options, magic_cookie, sizeof(magic_cookie));

  /* Add the common header options */

  pend = &pdhcpc->packet.options[4];
  pend = dhcpc_addmsgtype(pend, msgtype);

  /* Handle the message specific settings */

  switch (msgtype)
    {
      /* Broadcast DISCOVER message to all servers */

      case DHCPDISCOVER:
        pdhcpc->packet.flags = HTONS(BOOTP_BROADCAST); /*  Broadcast bit. */
        pend     = dhcpc_addreqoptions(pend);
        break;

      /* Send REQUEST message to the server that sent the *first* OFFER */

      case DHCPREQUEST:
        pdhcpc->packet.flags = HTONS(BOOTP_BROADCAST); /*  Broadcast bit. */
        memcpy(pdhcpc->packet.ciaddr, &pdhcpc->ipaddr.s_addr, 4);
        pend     = dhcpc_addserverid(&pdhcpc->serverid, pend);
        pend     = dhcpc_addreqipaddr(&pdhcpc->ipaddr, pend);
        break;

      /* Send DECLINE message to the server that sent the *last* OFFER */

      case DHCPDECLINE:
        memcpy(pdhcpc->packet.ciaddr, &presult->ipaddr.s_addr, 4);
        pend     = dhcpc_addserverid(&presult->serverid, pend);
        serverid = presult->serverid.s_addr;
        break;

      default:
        return ERROR;
    }

  pend = dhcpc_addend(pend);
  len  = pend - (uint8_t*)&pdhcpc->packet;

  /* Send the request */

  addr.sin_family      = AF_INET;
  addr.sin_port        = HTONS(DHCPC_SERVER_PORT);
  addr.sin_addr.s_addr = serverid;

  return sendto(pdhcpc->sockfd, &pdhcpc->packet, len, 0,
                (struct sockaddr*)&addr, sizeof(struct sockaddr_in));
}

static void e1000_receive(struct e1000_dev *e1000)
{
  int head = e1000->rx_ring.head;
  unsigned char *cp = (unsigned char *)
      (e1000->rx_ring.buf + head * CONFIG_E1000_BUFF_SIZE);
  int cnt;

  while (e1000->rx_ring.desc[head].desc_status)
    {
      /* Check for errors and update statistics */

      /* Here we do not handle packets that exceed packet-buffer size */

      if ((e1000->rx_ring.desc[head].desc_status & 3) == 1)
        {
          cprintf("NIC READ: Oversized packet\n");
          goto next;
        }

      /* Check if the packet is a valid size for the uIP buffer configuration */

      /* get the number of actual data-bytes in this packet */

      cnt = e1000->rx_ring.desc[head].packet_length;

      if (cnt > CONFIG_NET_BUFSIZE || cnt < 14)
        {
          cprintf("NIC READ: invalid package size\n");
          goto next;
        }

      /* Copy the data data from the hardware to e1000->uip_dev.d_buf.  Set
       * amount of data in e1000->uip_dev.d_len
       */

      /* now we try to copy these data-bytes to the UIP buffer */

      memcpy(e1000->uip_dev.d_buf, cp, cnt);
      e1000->uip_dev.d_len = cnt;

      /* We only accept IP packets of the configured type and ARP packets */

#ifdef CONFIG_NET_IPv6
      if (BUF->type == HTONS(UIP_ETHTYPE_IP6))
#else
        {
          if (BUF->type == HTONS(UIP_ETHTYPE_IP))
#endif
            {
              arp_ipin(&e1000->uip_dev);
              uip_input(&e1000->uip_dev);

              /* If the above function invocation resulted in data that should be
               * sent out on the network, the field  d_len will set to a value > 0.
               */

              if (e1000->uip_dev.d_len > 0)
                {
                  arp_out(&e1000->uip_dev);
                  e1000_transmit(e1000);
                }
            }
          else if (BUF->type == htons(UIP_ETHTYPE_ARP))
            {
              arp_arpin(&e1000->uip_dev);

              /* If the above function invocation resulted in data that should be
               * sent out on the network, the field  d_len will set to a value > 0.
               */

              if (e1000->uip_dev.d_len > 0)
                {
                  e1000_transmit(e1000);
                }
            }
        }

next:
      e1000->rx_ring.desc[head].desc_status = 0;
      e1000->rx_ring.head = (head + 1) % CONFIG_E1000_N_RX_DESC;
      e1000->rx_ring.free++;
      head = e1000->rx_ring.head;
      cp = (unsigned char *)(e1000->rx_ring.buf + head * CONFIG_E1000_BUFF_SIZE);
    }
}

/**
  * @brief  Main program.
  * @param  None
  * @retval None
  */
int main(void)
{	
	//Init system and pheripherals
	Platform_Config();
	printf("System init complete!\n");	

	//Init NetDev
	ENC28J60_Init(hostMac);		
	printf("NetDev init complete!\n");	
	
	//Init packet buffer of NIC
	arp_hdr = (struct net_arp_hdr *) malloc (sizeof(struct net_arp_hdr));
	icmp_hdr = (struct net_icmpip_hdr *) malloc (sizeof(struct net_icmpip_hdr));
	
	memset(pktBuffer, 0, sizeof(pktBuffer));
	for(int i=0; i<200; i++)
	{
		pktBuffer[i] = i;
	}
	
	//Init heart beat LED
	TIM1_PWM_Config(1000000);
	TIM1_PWM_SetDuty(1, 0);
	TIM1_PWM_SetDuty(2, 0);	
	TIM15_General_Config(1000);
	
	printf("=== ARP & ICMP Test Program ===\n  By Nights 2015-01-31\n");	
	
	while(1)
	{
		//Check Link
		{
			isLinkedNow = ENC28J60_GetLinkStatus();
			
			if(isLinkedBefore == ENC_LINK_DOWN && isLinkedNow == ENC_LINK_UP)
				printf("NetDev: link up!\n");
			else if(isLinkedBefore == ENC_LINK_UP && isLinkedNow == ENC_LINK_DOWN)
				printf("NetDev: link down!\n");	
			
			isLinkedBefore = isLinkedNow;
		}
		
		//ARP & ICMP responding.
		if(isLinkedNow == ENC_LINK_UP)
		{
			pktLen = ENC28J60_PacketReceive(pktBuffer, sizeof(pktBuffer));
			
			if(pktLen)
			{
//				// Dump the pkt.
//				printf("NetDev: received data:");
//				for(int i=0; i<pktLen; i++)
//				{
//					printf(" %02X", pktBuffer[i]);
//				}
//				printf("\n---\n");
				
				// Check if it's an ARP pkt.
				memcpy(arp_hdr, pktBuffer, sizeof(struct net_arp_hdr));				
				//arp_hdr = (struct net_arp_hdr *) &pktBuffer[0];
				if(arp_hdr->ethhdr.type == HTONS(UIP_ETHTYPE_ARP) && arp_hdr->opcode == HTONS(ARP_REQUEST))
				{
					printf("NetDev: received ARP request from %d.%d.%d.%d\n", 
						(arp_hdr->sipaddr[0] & 0xFF), (arp_hdr->sipaddr[0] >> 8), (arp_hdr->sipaddr[1] & 0xFF), (arp_hdr->sipaddr[0] >> 8));
					/* The reply opcode is 2. */
					arp_hdr->opcode = HTONS(2);

					memcpy(arp_hdr->dhwaddr.addr, arp_hdr->shwaddr.addr, 6);
					memcpy(arp_hdr->shwaddr.addr, hostMac, 6);
					memcpy(arp_hdr->ethhdr.src.addr, hostMac, 6);
					memcpy(arp_hdr->ethhdr.dest.addr, arp_hdr->dhwaddr.addr, 6);
					
					arp_hdr->dipaddr[0] = arp_hdr->sipaddr[0];
					arp_hdr->dipaddr[1] = arp_hdr->sipaddr[1];
					arp_hdr->sipaddr[0] = HTONS(hostIpaddr[0]);
					arp_hdr->sipaddr[1] = HTONS(hostIpaddr[1]);

					arp_hdr->opcode = HTONS(ARP_REPLY);
					
					memcpy(pktBuffer, arp_hdr, sizeof(struct net_arp_hdr));
					printf("NetDev: sending ARP reply...");
					ENC28J60_PacketSend(pktBuffer, sizeof(struct net_arp_hdr));
					printf("done!\n");
				}
				// Check if it's an ICMP pkt.
				else if(arp_hdr->ethhdr.type == HTONS(UIP_ETHTYPE_IP))
				{
					memcpy(icmp_hdr, pktBuffer, sizeof(struct net_icmpip_hdr));
					if(icmp_hdr->proto == UIP_PROTO_ICMP && icmp_hdr->type == ICMP_ECHO)
					{
						printf("NetDev: received ICMP request from %d.%d.%d.%d\n", 
						(icmp_hdr->srcipaddr[0] & 0xFF), (icmp_hdr->srcipaddr[0] >> 8), (icmp_hdr->srcipaddr[1] & 0xFF), (icmp_hdr->srcipaddr[0] >> 8));
						// Dill with MAC part
						memcpy(icmp_hdr->ethhdr.dest.addr, icmp_hdr->ethhdr.src.addr, 6);		
						memcpy(icmp_hdr->ethhdr.src.addr, hostMac, 6);						
//.........这里部分代码省略.........

NTSTATUS
DispatchRequest (
	IN PPRIMARY_SESSION	PrimarySession
	)
{
	NTSTATUS				status;
	IN PNDFS_REQUEST_HEADER	ndfsRequestHeader;


	ASSERT( NTOHS(PrimarySession->Thread.NdfsRequestHeader.Mid2) < PrimarySession->SessionContext.SessionSlotCount );
	ASSERT( PrimarySession->Thread.SessionSlot[NTOHS(PrimarySession->Thread.NdfsRequestHeader.Mid2)].State == SLOT_WAIT );
	ASSERT( PrimarySession->Thread.ReceiveOverlapped.Request[0].IoStatusBlock.Information == sizeof(NDFS_REQUEST_HEADER) );

	RtlCopyMemory( PrimarySession->Thread.SessionSlot[NTOHS(PrimarySession->Thread.NdfsRequestHeader.Mid2)].RequestMessageBuffer,
				   &PrimarySession->Thread.NdfsRequestHeader,
				   sizeof(NDFS_REQUEST_HEADER) );

	ndfsRequestHeader = (PNDFS_REQUEST_HEADER)PrimarySession->Thread.SessionSlot[NTOHS(PrimarySession->Thread.NdfsRequestHeader.Mid2)].RequestMessageBuffer;
   
    DebugTrace2( 0, Dbg,
				("DispatchRequest: ndfsRequestHeader->Command = %d\n", 
				ndfsRequestHeader->Command) );

	switch (ndfsRequestHeader->Command) {

	case NDFS_COMMAND_LOGOFF: {

		PNDFS_REQUEST_LOGOFF	ndfsRequestLogoff;
		PNDFS_REPLY_HEADER		ndfsReplyHeader;
		PNDFS_REPLY_LOGOFF		ndfsReplyLogoff;
		

		if (PrimarySession->Thread.SessionState != SESSION_TREE_CONNECT) {

			ASSERT(NDASFAT_BUG);
			status = STATUS_UNSUCCESSFUL;
			break;
		}

		if (!(NTOHS(ndfsRequestHeader->Uid2) == PrimarySession->SessionContext.Uid && 
			 NTOHS(ndfsRequestHeader->Tid2) == PrimarySession->SessionContext.Tid)) {

			ASSERT(NDASFAT_BUG);
			status = STATUS_UNSUCCESSFUL;
			break;
		}
		
		ASSERT( NTOHL(ndfsRequestHeader->MessageSize4) == sizeof(NDFS_REQUEST_HEADER) + sizeof(NDFS_REQUEST_LOGOFF) );

		ndfsRequestLogoff = (PNDFS_REQUEST_LOGOFF)(ndfsRequestHeader+1);
		
		status = RecvMessage( PrimarySession->ConnectionFileObject,
							  &PrimarySession->RecvNdasFcStatistics,
							  NULL,
							  (UINT8 *)ndfsRequestLogoff,
							  sizeof(NDFS_REQUEST_LOGOFF) );
	
		if (status != STATUS_SUCCESS) {

			ASSERT(NDASFAT_BUG);
			break;
		}

		ndfsReplyHeader = (PNDFS_REPLY_HEADER)(ndfsRequestLogoff+1);

		RtlCopyMemory( ndfsReplyHeader->Protocol, NDFS_PROTOCOL, sizeof(ndfsReplyHeader->Protocol) );
		ndfsReplyHeader->Status		= NDFS_SUCCESS;
		ndfsReplyHeader->Flags	    = 0;
		ndfsReplyHeader->Uid2		= HTONS(PrimarySession->SessionContext.Uid);
		ndfsReplyHeader->Tid2		= 0;
		ndfsReplyHeader->Mid2		= 0;
		ndfsReplyHeader->MessageSize4 = HTONL((UINT32)(sizeof(NDFS_REPLY_HEADER)+sizeof(NDFS_REPLY_LOGOFF)));

		ndfsReplyLogoff = (PNDFS_REPLY_LOGOFF)(ndfsReplyHeader+1);

		if (NTOHL(ndfsRequestLogoff->SessionKey4) != PrimarySession->SessionContext.SessionKey) {

			ndfsReplyLogoff->Status = NDFS_LOGOFF_UNSUCCESSFUL;
		
		} else {

			ndfsReplyLogoff->Status = NDFS_LOGOFF_SUCCESS;
		}

		status = SendMessage( PrimarySession->ConnectionFileObject,
							  &PrimarySession->SendNdasFcStatistics,
							  NULL,
							  (UINT8 *)ndfsReplyHeader,
							  NTOHL(ndfsReplyHeader->MessageSize4) );

		if (status != STATUS_SUCCESS) {

			break;
		}

		PrimarySession->Thread.SessionState = SESSION_CLOSED;

		status = STATUS_SUCCESS;

		break;
//.........这里部分代码省略.........

/*-----------------------------------------------------------------------------------*/
void
uip_arp_out(void)
{
	struct arp_entry *tabptr;

	/* Find the destination IP address in the ARP table and construct
	   the Ethernet header. If the destination IP addres isn't on the
	   local network, we use the default router's IP address instead.

	   If not ARP table entry is found, we overwrite the original IP
	   packet with an ARP request for the IP address. */

	/* First check if destination is a local broadcast. */
	if(uip_ipaddr_cmp(IPBUF->destipaddr, broadcast_ipaddr)) {
		memcpy(IPBUF->ethhdr.dest.addr, broadcast_ethaddr.addr, 6);
	} else {
		/* Check if the destination address is on the local network. */
		if(!uip_ipaddr_maskcmp(IPBUF->destipaddr, uip_hostaddr, uip_netmask)) {
			/* Destination address was not on the local network, so we need to
			   use the default router's IP address instead of the destination
			   address when determining the MAC address. */
			uip_ipaddr_copy(ipaddr, uip_draddr);
		} else {
			/* Else, we use the destination IP address. */
			uip_ipaddr_copy(ipaddr, IPBUF->destipaddr);
		}

		for(i = 0; i < UIP_ARPTAB_SIZE; ++i) {
			tabptr = &arp_table[i];
			if(uip_ipaddr_cmp(ipaddr, tabptr->ipaddr)) {
				break;
			}
		}

		if(i == UIP_ARPTAB_SIZE) {
			/* The destination address was not in our ARP table, so we
			   overwrite the IP packet with an ARP request. */

#if 0	
			memset(BUF->ethhdr.dest.addr, 0xff, 6);
			memset(BUF->dhwaddr.addr, 0x00, 6);
			memcpy(BUF->ethhdr.src.addr, uip_ethaddr.addr, 6);
			memcpy(BUF->shwaddr.addr, uip_ethaddr.addr, 6);

			uip_ipaddr_copy(BUF->dipaddr, ipaddr);
			uip_ipaddr_copy(BUF->sipaddr, uip_hostaddr);
			BUF->opcode = HTONS(ARP_REQUEST); /* ARP request. */
			BUF->hwtype = HTONS(ARP_HWTYPE_ETH);
			BUF->protocol = HTONS(UIP_ETHTYPE_IP);
			BUF->hwlen = 6;
			BUF->protolen = 4;
			BUF->ethhdr.type = HTONS(UIP_ETHTYPE_ARP);
#else
			uip_create_broadcast_eth_arp_pkt(BUF, ipaddr,
							 ARP_REQUEST);
#endif

			uip_appdata = &uip_buf[UIP_TCPIP_HLEN + UIP_LLH_LEN];

			uip_len = sizeof(struct arp_hdr);
			return;
		}

		/* Build an ethernet header. */
		memcpy(IPBUF->ethhdr.dest.addr, tabptr->ethaddr.addr, 6);
	}
	memcpy(IPBUF->ethhdr.src.addr, uip_ethaddr.addr, 6);

	IPBUF->ethhdr.type = HTONS(UIP_ETHTYPE_IP);

	uip_len += sizeof(struct uip_eth_hdr);
}

//.........这里部分代码省略.........
#if NBPFILTER > 0
	if (ifp->if_bpf)
		bpf_mtap_af(ifp->if_bpf, dst->sa_family, m, BPF_DIRECTION_OUT);
#endif

	if (sc->g_proto == IPPROTO_MOBILE) {
		if (ip_mobile_allow == 0) {
			IF_DROP(&ifp->if_snd);
			m_freem(m);
			error = EACCES;
			goto end;
		}

		if (dst->sa_family == AF_INET) {
			struct mbuf *m0;
			int msiz;

			/*
			 * Make sure the complete IP header (with options)
			 * is in the first mbuf.
			 */
			if (m->m_len < sizeof(struct ip)) {
				m = m_pullup(m, sizeof(struct ip));
				if (m == NULL) {
					IF_DROP(&ifp->if_snd);
					error = ENOBUFS;
					goto end;
				} else
					inp = mtod(m, struct ip *);

				if (m->m_len < inp->ip_hl << 2) {
					m = m_pullup(m, inp->ip_hl << 2);
					if (m == NULL) {
						IF_DROP(&ifp->if_snd);
						error = ENOBUFS;
						goto end;
					}
				}
			}

			inp = mtod(m, struct ip *);

			bzero(&mob_h, MOB_H_SIZ_L);
			mob_h.proto = (inp->ip_p) << 8;
			mob_h.odst = inp->ip_dst.s_addr;
			inp->ip_dst.s_addr = sc->g_dst.s_addr;

			/*
			 * If the packet comes from our host, we only change
			 * the destination address in the IP header.
			 * Otherwise we need to save and change the source.
			 */
			if (inp->ip_src.s_addr == sc->g_src.s_addr) {
				msiz = MOB_H_SIZ_S;
			} else {
				mob_h.proto |= MOB_H_SBIT;
				mob_h.osrc = inp->ip_src.s_addr;
				inp->ip_src.s_addr = sc->g_src.s_addr;
				msiz = MOB_H_SIZ_L;
			}

			HTONS(mob_h.proto);
			mob_h.hcrc = gre_in_cksum((u_int16_t *) &mob_h, msiz);

			/* Squeeze in the mobility header */
			if ((m->m_data - msiz) < m->m_pktdat) {
				/* Need new mbuf */
				MGETHDR(m0, M_DONTWAIT, MT_HEADER);
				if (m0 == NULL) {
					IF_DROP(&ifp->if_snd);
					m_freem(m);
					error = ENOBUFS;
					goto end;
				}
				M_MOVE_HDR(m0, m);

				m0->m_len = msiz + (inp->ip_hl << 2);
				m0->m_data += max_linkhdr;
				m0->m_pkthdr.len = m->m_pkthdr.len + msiz;
				m->m_data += inp->ip_hl << 2;
				m->m_len -= inp->ip_hl << 2;

				bcopy((caddr_t) inp, mtod(m0, caddr_t),
				    sizeof(struct ip));

				m0->m_next = m;
				m = m0;
			} else {  /* we have some space left in the old one */
				m->m_data -= msiz;
				m->m_len += msiz;
				m->m_pkthdr.len += msiz;
				bcopy(inp, mtod(m, caddr_t),
				    inp->ip_hl << 2);
			}

			/* Copy Mobility header */
			inp = mtod(m, struct ip *);
			bcopy(&mob_h, (caddr_t)(inp + 1), (unsigned) msiz);
			inp->ip_len = htons(ntohs(inp->ip_len) + msiz);
		} else {  /* AF_INET */

void rtos_vnet_recv(struct rgmp_vnet *rgmp_vnet, char *data, int len)
{
  struct vnet_driver_s *vnet = rgmp_vnet->priv;

  do
    {
      /* Check if the packet is a valid size for the network buffer
       * configuration.
       */

      if (len > CONFIG_NET_ETH_MTU || len < 14)
        {
#ifdef CONFIG_DEBUG
          cprintf("VNET: receive invalid packet of size %d\n", len);
#endif
          return;
        }

      /* Copy the data data from the hardware to vnet->sk_dev.d_buf.  Set
       * amount of data in vnet->sk_dev.d_len
       */

      memcpy(vnet->sk_dev.d_buf, data, len);
      vnet->sk_dev.d_len = len;

#ifdef CONFIG_NET_PKT
      /* When packet sockets are enabled, feed the frame into the packet tap */

       pkt_input(&vnet->sk_dev);
#endif

      /* We only accept IP packets of the configured type and ARP packets */

#ifdef CONFIG_NET_IPv4
      if (BUF->type == HTONS(ETHTYPE_IP))
        {
          nllvdbg("IPv4 frame\n");

          /* Handle ARP on input then give the IPv4 packet to the network
           * layer
           */

          arp_ipin(&vnet->sk_dev);
          ipv4_input(&vnet->sk_dev);

          /* If the above function invocation resulted in data that should be
           * sent out on the network, the field  d_len will set to a value > 0.
           */

          if (vnet->sk_dev.d_len > 0)
            {
              /* Update the Ethernet header with the correct MAC address */

#ifdef CONFIG_NET_IPv6
              if (IFF_IS_IPv4(vnet->sk_dev.d_flags))
#endif
                {
                  arp_out(&vnet->sk_dev);
                }
#ifdef CONFIG_NET_IPv6
              else
                {
                  neighbor_out(&vnet->sk_dev);
                }
#endif

              /* And send the packet */

              vnet_transmit(vnet);
            }
        }
      else
#endif
#ifdef CONFIG_NET_IPv6
      if (BUF->type == HTONS(ETHTYPE_IP6))
        {
          nllvdbg("Iv6 frame\n");

          /* Give the IPv6 packet to the network layer */

          ipv6_input(&vnet->sk_dev);

          /* If the above function invocation resulted in data that should be
           * sent out on the network, the field  d_len will set to a value > 0.
           */

          if (vnet->sk_dev.d_len > 0)
           {
              /* Update the Ethernet header with the correct MAC address */

#ifdef CONFIG_NET_IPv4
              if (IFF_IS_IPv4(vnet->sk_dev.d_flags))
                {
                  arp_out(&vnet->sk_dev);
                }
              else
#endif
#ifdef CONFIG_NET_IPv6
                {
                  neighbor_out(&vnet->sk_dev);
//.........这里部分代码省略.........

error_t dnsSendQuery(DnsCacheEntry *entry)
{
   error_t error;
   size_t length;
   size_t offset;
   ChunkedBuffer *buffer;
   DnsHeader *message;
   DnsQuestion *dnsQuestion;
   IpAddr destIpAddr;

#if (IPV4_SUPPORT == ENABLED)
   //An IPv4 address is expected?
   if(entry->type == HOST_TYPE_IPV4)
   {
      //Select the relevant DNS server
      destIpAddr.length = sizeof(Ipv4Addr);
      ipv4GetDnsServer(entry->interface, entry->dnsServerNum, &destIpAddr.ipv4Addr);

      //Make sure the IP address is valid
      if(destIpAddr.ipv4Addr == IPV4_UNSPECIFIED_ADDR)
         return ERROR_NO_DNS_SERVER;
   }
   else
#endif
#if (IPV6_SUPPORT == ENABLED)
   //An IPv6 address is expected?
   if(entry->type == HOST_TYPE_IPV6)
   {
      //Select the relevant DNS server
      destIpAddr.length = sizeof(Ipv6Addr);
      ipv6GetDnsServer(entry->interface, entry->dnsServerNum, &destIpAddr.ipv6Addr);

      //Make sure the IP address is valid
      if(ipv6CompAddr(&destIpAddr.ipv6Addr, &IPV6_UNSPECIFIED_ADDR))
         return ERROR_NO_DNS_SERVER;
   }
   else
#endif
   //Invalid host type?
   {
      //Report an error
      return ERROR_INVALID_PARAMETER;
   }

   //Allocate a memory buffer to hold the DNS query message
   buffer = udpAllocBuffer(DNS_MESSAGE_MAX_SIZE, &offset);
   //Failed to allocate buffer?
   if(!buffer) return ERROR_OUT_OF_MEMORY;

   //Point to the DNS header
   message = chunkedBufferAt(buffer, offset);

   //Format DNS query message
   message->id = htons(entry->id);
   message->qr = 0;
   message->opcode = DNS_OPCODE_QUERY;
   message->aa = 0;
   message->tc = 0;
   message->rd = 1;
   message->ra = 0;
   message->z = 0;
   message->rcode = DNS_RCODE_NO_ERROR;

   //The DNS query contains one question
   message->qdcount = HTONS(1);
   message->ancount = 0;
   message->nscount = 0;
   message->arcount = 0;

   //Length of the DNS query message
   length = sizeof(DnsHeader);

   //Encode the host name using the DNS name notation
   length += dnsEncodeName(entry->name, message->questions);

   //Point to the corresponding question structure
   dnsQuestion = DNS_GET_QUESTION(message, length);

#if (IPV4_SUPPORT == ENABLED)
   //An IPv4 address is expected?
   if(entry->type == HOST_TYPE_IPV4)
   {
      //Fill in question structure
      dnsQuestion->qtype = HTONS(DNS_RR_TYPE_A);
      dnsQuestion->qclass = HTONS(DNS_RR_CLASS_IN);
   }
#endif
#if (IPV6_SUPPORT == ENABLED)
   //An IPv6 address is expected?
   if(entry->type == HOST_TYPE_IPV6)
   {
      //Fill in question structure
      dnsQuestion->qtype = HTONS(DNS_RR_TYPE_AAAA);
      dnsQuestion->qclass = HTONS(DNS_RR_CLASS_IN);
   }
#endif

   //Update the length of the DNS query message
   length += sizeof(DnsQuestion);

//.........这里部分代码省略.........

/**
  Build the options buffer for the DHCPv6 request packet.

  @param[in]  Private             The pointer to HTTP BOOT driver private data.
  @param[out] OptList             The pointer to the option pointer array.
  @param[in]  Buffer              The pointer to the buffer to contain the option list.

  @return     Index               The count of the built-in options.

**/
UINT32
HttpBootBuildDhcp6Options (
  IN  HTTP_BOOT_PRIVATE_DATA       *Private,
  OUT EFI_DHCP6_PACKET_OPTION      **OptList,
  IN  UINT8                        *Buffer
  )
{
  HTTP_BOOT_DHCP6_OPTION_ENTRY     OptEnt;
  UINT16                           Value;
  UINT32                           Index;

  Index      = 0;
  OptList[0] = (EFI_DHCP6_PACKET_OPTION *) Buffer;

  //
  // Append client option request option
  //
  OptList[Index]->OpCode     = HTONS (HTTP_BOOT_DHCP6_OPT_ORO);
  OptList[Index]->OpLen      = HTONS (8);
  OptEnt.Oro                 = (HTTP_BOOT_DHCP6_OPTION_ORO *) OptList[Index]->Data;
  OptEnt.Oro->OpCode[0]      = HTONS(HTTP_BOOT_DHCP6_OPT_BOOT_FILE_URL);
  OptEnt.Oro->OpCode[1]      = HTONS(HTTP_BOOT_DHCP6_OPT_BOOT_FILE_PARAM);
  OptEnt.Oro->OpCode[2]      = HTONS(HTTP_BOOT_DHCP6_OPT_DNS_SERVERS);
  OptEnt.Oro->OpCode[3]      = HTONS(HTTP_BOOT_DHCP6_OPT_VENDOR_CLASS);
  Index++;
  OptList[Index]             = GET_NEXT_DHCP6_OPTION (OptList[Index - 1]);

  //
  // Append client network device interface option
  //
  OptList[Index]->OpCode     = HTONS (HTTP_BOOT_DHCP6_OPT_UNDI);
  OptList[Index]->OpLen      = HTONS ((UINT16)3);
  OptEnt.Undi                = (HTTP_BOOT_DHCP6_OPTION_UNDI *) OptList[Index]->Data;

  if (Private->Nii != NULL) {
    OptEnt.Undi->Type        = Private->Nii->Type;
    OptEnt.Undi->MajorVer    = Private->Nii->MajorVer;
    OptEnt.Undi->MinorVer    = Private->Nii->MinorVer;
  } else {
    OptEnt.Undi->Type        = DEFAULT_UNDI_TYPE;
    OptEnt.Undi->MajorVer    = DEFAULT_UNDI_MAJOR;
    OptEnt.Undi->MinorVer    = DEFAULT_UNDI_MINOR;
  }

  Index++;
  OptList[Index]             = GET_NEXT_DHCP6_OPTION (OptList[Index - 1]);

  //
  // Append client system architecture option
  //
  OptList[Index]->OpCode     = HTONS (HTTP_BOOT_DHCP6_OPT_ARCH);
  OptList[Index]->OpLen      = HTONS ((UINT16) sizeof (HTTP_BOOT_DHCP6_OPTION_ARCH));
  OptEnt.Arch                = (HTTP_BOOT_DHCP6_OPTION_ARCH *) OptList[Index]->Data;
  Value                      = HTONS (EFI_HTTP_BOOT_CLIENT_SYSTEM_ARCHITECTURE);
  CopyMem (&OptEnt.Arch->Type, &Value, sizeof (UINT16));
  Index++;
  OptList[Index]             = GET_NEXT_DHCP6_OPTION (OptList[Index - 1]);

  //
  // Append vendor class identify option.
  //
  OptList[