udp.c

Hermit-at-1.1.3,一款bootloader

#include <target/io.h>

#include <target/net/eth.h>
#include <target/net/eth_util.h>
#include <target/net/ip.h>
#include <target/net/udp.h>

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
static unsigned short udp_checksum(const ip_pseudo_header *__pheader,
				   const void *__buf,
				   const unsigned int buflen){
  unsigned short *buf;
  unsigned long sum;
  int i;

  buf = (unsigned short *)__pheader;
  for(sum = 0, i = 0; i < 6; i++){
    sum += buf[i];
  }

  buf = (unsigned short *)__buf;
  for(i = 0; i < (buflen / 2); i++){
    sum += buf[i];
  }

  if(buflen % 2){
    sum += (buf[i] & 0x00ff);
  }

  sum = (sum & 0xffff) + (sum >> 16);
  sum = (sum & 0xffff) + (sum >> 16);

  return (unsigned short)~sum;
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
int udp_send(const unsigned char *dst_ipaddr,
	     const unsigned short dst_port,
	     const unsigned short src_port,
	     const void *pbuf, const unsigned int pbuflen){
  unsigned char buf[2048];
  udp_frame *udpfr = (udp_frame *)buf;
  unsigned int udpfr_len = sizeof(udp_frame);
  ip_pseudo_header pheader;
  unsigned short checksum;
  
  //create pseudo header
  safe_memcpy(pheader.dst_ipaddr, dst_ipaddr, 4);
  safe_memcpy(pheader.src_ipaddr, eth_ipaddr, 4);
  pheader.zero = 0;
  pheader.protocol = IP_PROTOCOL_UDP;
  pheader.data_len = htons((unsigned short)pbuflen + udpfr_len);

  //create udp header
  udpfr->src_port = htons(src_port);
  udpfr->dst_port = htons(dst_port);
  udpfr->data_len = htons((unsigned short)(pbuflen + udpfr_len));
  udpfr->checksum = 0;

  safe_memcpy(&buf[udpfr_len], pbuf, pbuflen);
  checksum = udp_checksum(&pheader, buf, udpfr_len + pbuflen);
  udpfr->checksum = checksum;

  return ip_send(dst_ipaddr, IP_PROTOCOL_UDP, buf, udpfr_len + pbuflen);
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
int udp_proc(const unsigned char *src_ipaddr,
	     const unsigned short dst_port, unsigned short *src_port, 
	     void **pbuf, unsigned int *pbuflen){
  udp_frame *udpfr;
  unsigned char *proc_pbuf = 0;
  unsigned int proc_pbuflen = 0;
  ip_pseudo_header pheader;
  unsigned short checksum;
  unsigned short tmp;
  int ret;

  while(1){
    ret = ip_proc(UDP_WAIT, (void *)&proc_pbuf, &proc_pbuflen);
    if(ret == -1){
      return -1;
    }

    if(proc_pbuflen < sizeof(udp_frame)){
      continue;
    }

    udpfr = (udp_frame *)proc_pbuf;

    if(pbuf != 0){
      *pbuf = (void *)(proc_pbuf + sizeof(udp_frame));
    }
    if(pbuflen != 0){
      *pbuflen = proc_pbuflen - sizeof(udp_frame);
    }
    if(src_port != 0){
      *src_port = ntohs(udpfr->src_port);
    }
    
    //create pseudo header
    safe_memcpy(pheader.dst_ipaddr, eth_ipaddr, 4); 
    safe_memcpy(pheader.src_ipaddr, src_ipaddr, 4);
    pheader.zero = 0;
    pheader.protocol = IP_PROTOCOL_UDP;
    pheader.data_len = htons(proc_pbuflen);

    checksum = udpfr->checksum;
    if(checksum != 0){
      udpfr->checksum = 0;
      
      tmp = udp_checksum(&pheader, proc_pbuf, proc_pbuflen);
      if(tmp == 0){
	tmp = 0xffff;
      }
      
      if(checksum != tmp){
	continue;
      }
    }

    _DEBUG("src_port: %d\n", ntohs(udpfr->src_port));
    _DEBUG("dst_port: %d\n", ntohs(udpfr->dst_port));

    if(dst_port == ntohs(udpfr->dst_port)){
      return 0;
    }
  }
  return 0;
}