arp.c

Hermit-at-1.1.3,一款bootloader

#include <target/io.h>
#include <target/mem.h>

#include <target/net/eth_util.h>
#include <target/net/eth.h>
#include <target/net/arp.h>

#ifdef DEBUG_ETH
//#define DEBUG_ARP
#endif

#define LEN_ARP_Cache 16
static arp_cache_table arp_cache[LEN_ARP_Cache];
static int arp_cache_idx = 0;

static int arp_cache_add(const unsigned char *target_mac, 
			 const unsigned char *target_ipaddr){
  safe_memcpy(arp_cache[arp_cache_idx].mac, target_mac, 6);
  safe_memcpy(arp_cache[arp_cache_idx].ipaddr, target_ipaddr, 4);
  arp_cache_idx = ((arp_cache_idx + 1) % LEN_ARP_Cache);
  return 0;
}

static int arp_cache_search(unsigned char *target_mac, 
			    const unsigned char *target_ipaddr){
  int i;

  for(i=0; i<LEN_ARP_Cache; i++){
    if(safe_memcmp(arp_cache[i].ipaddr, target_ipaddr, 4) == 0){
      memcpy(target_mac, arp_cache[i].mac, 6);
      return 0;
    }
  }
  return -1;
}

static int arp_request(const unsigned char *target_ipaddr){
  eth_frame ethfr;
  arp_frame arpfr;

  safe_memset(&ethfr, 0, sizeof(eth_frame));
  safe_memset(&arpfr, 0, sizeof(arp_frame));

  arpfr.htype = htons(ARP_HTYPE_ETH);
  arpfr.ptype = htons(ARP_PTYPE_IP);
  arpfr.hlen = ARP_HLEN_ETH;
  arpfr.plen = ARP_PLEN_IPV4;
  arpfr.opecode = htons(ARP_OP_REQ);
  safe_memcpy(arpfr.host_mac, eth_mac, 6);
  safe_memcpy(arpfr.host_ipaddr, eth_ipaddr, 4);
  safe_memset(arpfr.target_mac, 0, 6);
  safe_memcpy(arpfr.target_ipaddr, target_ipaddr, 4);

  safe_memcpy(ethfr.dmac, broadcast_mac, 6);
  safe_memcpy(ethfr.smac, eth_mac, 6);
  ethfr.protocol = htons(ETH_PROTOCOL_ARP);

  return eth_send(&ethfr, &arpfr, sizeof(arp_frame));
}

int arp_search(unsigned char *target_mac, const unsigned char *target_ipaddr){
  int i;
  int ret;

  ret = arp_cache_search(target_mac, target_ipaddr);
  if(ret == 0){
    return 0;
  }

  for(i=0; i<2; i++){
    ret = arp_request(target_ipaddr);
    ret = eth_proc(ARP_WAIT, 0, 0);
    if(ret == -1) return -1;
    ret = arp_cache_search(target_mac, target_ipaddr);
    if(ret == 0) return 0;
  }
  return -1;
}

int arp_proc(const unsigned char *pbuf, const int pbuflen){
  arp_frame *recv_arpfr = (arp_frame *)pbuf;
  arp_frame send_arpfr;
  eth_frame send_ethfr;

  if(pbuflen < sizeof(arp_frame)){
    return -1;
  }

#ifdef DEBUG_ARP
  _DEBUG("htype     : %p\n", ntohs(recv_arpfr->htype));
  _DEBUG("ptype     : %p\n", ntohs(recv_arpfr->ptype));
  _DEBUG("hlen      : %p\n", recv_arpfr->hlen);
  _DEBUG("plen      : %p\n", recv_arpfr->plen);
  _DEBUG("opcode    : %p\n", ntohs(recv_arpfr->opecode));
  _DEBUG("hostmac   : %b:%b:%b:%b:%b:%b\n",
	 recv_arpfr->host_mac[0], recv_arpfr->host_mac[1],
	 recv_arpfr->host_mac[2], recv_arpfr->host_mac[3], 
	 recv_arpfr->host_mac[4], recv_arpfr->host_mac[5]);
  _DEBUG("hostip    : %d.%d.%d.%d\n",
	  recv_arpfr->host_ipaddr[0], recv_arpfr->host_ipaddr[1],
	  recv_arpfr->host_ipaddr[2], recv_arpfr->host_ipaddr[3]);
  _DEBUG("targetmac : %b:%b:%b:%b:%b:%b\n",
	  recv_arpfr->target_mac[0], recv_arpfr->target_mac[1],
	  recv_arpfr->target_mac[2], recv_arpfr->target_mac[3],
	  recv_arpfr->target_mac[4], recv_arpfr->target_mac[5]);
  _DEBUG("targetip  : %d.%d.%d.%d\n",
	  recv_arpfr->target_ipaddr[0], recv_arpfr->target_ipaddr[1],
	  recv_arpfr->target_ipaddr[2], recv_arpfr->target_ipaddr[3]);
#endif

  switch(ntohs(recv_arpfr->opecode)){
  case ARP_OP_REQ:
    if(safe_memcmp(recv_arpfr->target_ipaddr, eth_ipaddr, 4) == 0){
      safe_memset(&send_arpfr, 0 , sizeof(arp_frame));
      send_arpfr.htype = htons(ARP_HTYPE_ETH);
      send_arpfr.ptype = htons(ARP_PTYPE_IP);
      send_arpfr.hlen = ARP_HLEN_ETH;
      send_arpfr.plen = ARP_PLEN_IPV4;
      send_arpfr.opecode = htons(ARP_OP_REPLY);
      safe_memcpy(send_arpfr.host_mac, eth_mac, 6);
      safe_memcpy(send_arpfr.host_ipaddr, eth_ipaddr, 4);
      safe_memcpy(send_arpfr.target_mac, recv_arpfr->host_mac, 6);
      safe_memcpy(send_arpfr.target_ipaddr, recv_arpfr->host_ipaddr, 4);

      safe_memcpy(send_ethfr.dmac, recv_arpfr->host_mac, 6);
      safe_memcpy(send_ethfr.smac, eth_mac, 6);
      send_ethfr.protocol = htons(ETH_PROTOCOL_ARP);

      eth_send(&send_ethfr, &send_arpfr, sizeof(arp_frame));
    }
    return 0;

  case ARP_OP_REPLY:
    if(safe_memcmp(recv_arpfr->target_ipaddr, eth_ipaddr, 4) == 0){
      arp_cache_add(recv_arpfr->host_mac, recv_arpfr->host_ipaddr);
    }
    return 0;

  case ARP_OP_RARPREQ:
  case ARP_OP_RARPREPLY:
  default:
    return -1;
  }
}