ip.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/arp.h>
#include <target/net/ip.h>

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

  for(sum = 0, 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 ip_send(const unsigned char *dst_ipaddr, const unsigned char protocol,
	    const void *pbuf, const unsigned int pbuflen){
  static unsigned short packet_id = 0;
  unsigned char buf[2048];
  eth_frame ethfr;
  unsigned int ethfr_len = sizeof(eth_frame);
  ip_frame ipfr;
  unsigned int ipfr_len = sizeof(ip_frame);
  unsigned char dst_mac[6];
  int ret;

  if(safe_memcmp(dst_ipaddr, broadcast_ipaddr, 4) == 0){
    safe_memcpy(dst_mac, broadcast_mac, 6);
  }else{
    ret = arp_search(dst_mac, dst_ipaddr);
    if(ret == -1){
      return -1;
    }
  }

  safe_memset(&ipfr, 0, ipfr_len);
  ipfr.version = 4;
  ipfr.header_len = (LEN_IP_Header >> 2);
  ipfr.data_len = htons(ipfr_len + pbuflen);
  ipfr.id = htons(packet_id++);
#if 0
  ipfr.flag_offset = 0;
  ipfr.flag = 2;//Don't Flagment
#else
  ipfr.flag = htons(0x4000);//Don't Flagment
#endif
  ipfr.ttl = 64;
  ipfr.protocol = protocol;
  safe_memcpy(ipfr.src_ipaddr, eth_ipaddr, 4);
  safe_memcpy(ipfr.dst_ipaddr, dst_ipaddr, 4);

  ipfr.checksum = ip_checksum(&ipfr, ipfr_len);

  safe_memcpy(&buf[0], &ipfr, ipfr_len);
  safe_memcpy(&buf[ipfr_len], pbuf, pbuflen);

  safe_memset(&ethfr, 0 ,ethfr_len);
  safe_memcpy(ethfr.dmac, dst_mac, 6);
  ethfr.protocol = htons(ETH_PROTOCOL_IP);
  eth_send(&ethfr, buf, ipfr_len + pbuflen);

  return 0;
}

//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// 
//+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
int ip_proc(const int mode, void **pbuf, unsigned int *pbuflen){
  ip_frame *ipfr;
  unsigned char *proc_pbuf = 0;
  unsigned int proc_pbuflen = 0;
  unsigned short checksum;
  unsigned short tmp;
  int ret;

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

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

    ipfr = (ip_frame *)proc_pbuf;

    checksum = ipfr->checksum;
    if(checksum != 0){
      ipfr->checksum = 0;
      tmp = ip_checksum(ipfr, sizeof(ip_frame));
      if(tmp == 0){
	tmp = 0xffff;
      }

      if(checksum != tmp){
	continue;
      }
    }

    if(pbuf != 0){
      *pbuf = (void *)(proc_pbuf + (ipfr->header_len << 2));
    }
    if(pbuflen != 0){
      *pbuflen = ntohs(ipfr->data_len) - (ipfr->header_len << 2);
    }
    
    switch(ipfr->protocol){
    case IP_PROTOCOL_ICMP:
      _DEBUG("ICMP\n");
      if(mode == ICMP_WAIT) return 0;
      break;

    case IP_PROTOCOL_TCP:
      _DEBUG("TCP\n");
      if(mode == TCP_WAIT) return 0;
      break;
      
    case IP_PROTOCOL_UDP:
      _DEBUG("UDP\n");
      if(mode == UDP_WAIT) return 0;
      break;

    default:
      break;
    }
  }
  return 0;
}