el3.c

C++ 编写的EROS RTOS

#define ADDCARRY(x)  (x > 65535 ? x -= 65535 : x)
#define REDUCE {l_util.l = sum; sum = l_util.s[0] + l_util.s[1]; ADDCARRY(sum);}

typedef uint32_t u_int32_t;
typedef uint16_t u_int16_t;
typedef uint8_t u_int8_t;
typedef char int8_t;

int
in_cksum(register uint16_t *buf, register int len)
{
  register u_int16_t *w;
  register int sum = 0;
  register int mlen = len;
  int byte_swapped = 0;

  union {
    u_int8_t  c[2];
    u_int16_t s;
  } s_util;
  union {
    u_int16_t s[2];
    u_int32_t l;
  } l_util;

  w = buf;
	
  if (len < mlen)
    mlen = len;
  len -= mlen;

  /*
   * Force to even boundary.
   */
  if ((1 & (long) w) && (mlen > 0)) {
    REDUCE;
    sum <<= 8;
    s_util.c[0] = *(u_int8_t *)w;
    w = (u_int16_t *)((int8_t *)w + 1);
    mlen--;
    byte_swapped = 1;
  }
  /*
   * Unroll the loop to make overhead from
   * branches &c small.
   */
  while ((mlen -= 32) >= 0) {
    sum += w[0]; sum += w[1]; sum += w[2]; sum += w[3];
    sum += w[4]; sum += w[5]; sum += w[6]; sum += w[7];
    sum += w[8]; sum += w[9]; sum += w[10]; sum += w[11];
    sum += w[12]; sum += w[13]; sum += w[14]; sum += w[15];
    w += 16;
  }
  mlen += 32;
  while ((mlen -= 8) >= 0) {
    sum += w[0]; sum += w[1]; sum += w[2]; sum += w[3];
    w += 4;
  }
  mlen += 8;
  if (mlen == 0 && byte_swapped == 0)
    goto done;
  
  REDUCE;
  while ((mlen -= 2) >= 0) {
    sum += *w++;
  }
  if (byte_swapped) {
    REDUCE;
    sum <<= 8;
    byte_swapped = 0;
    if (mlen == -1) {
      s_util.c[1] = *(u_int8_t *)w;
      sum += s_util.s;
      mlen = 0;
    } else
      mlen = -1;
  } else if (mlen == -1)
    s_util.c[0] = *(u_int8_t *)w;

done:
  if (len)
    kprintf(KR_CONSOLEKEY_SLOT, "cksum: out of data\n");
  if (mlen == -1) {
    /* The last mbuf has odd # of bytes. Follow the
       standard (the odd byte may be shifted left by 8 bits
       or not as determined by endian-ness of the machine) */
    s_util.c[1] = 0;
    sum += s_util.s;
  }
  REDUCE;
  return (~sum & 0xffff);
}

#else

uint16_t
in_cksum(uint16_t *buf, int len)
{
  uint32_t cksum=0;

  if ((u_long)buf & 0x01) {
    cksum=(*(u_char *)buf)<<8;
    buf=(uint16_t *)((char *)buf+1);
    len--;
  }
  while (len>1) {
    cksum+=*buf++;
    if (cksum & 0x10000)
      cksum=(cksum & 0xFFFF)+1;
    len-=2;
  }
  if (len) {
    cksum+=*(u_char *)buf;
    if (cksum & 0x10000)
      cksum=(cksum & 0xFFFF)+1;
  }
  return ~cksum;
}

#endif

#define	ETHERTYPE_IP		0x0800	/* IP protocol */
#define	ICMP_ECHOREPLY		0		/* echo reply */
#define	ICMP_ECHO		8		/* echo service */
#define	IPPROTO_ICMP		1		/* control message protocol */
#define LEN_ICMP_HDR		16
#define LEN_ICMP_DATA		48
#define LEN_ICMP		(LEN_ICMP_HDR+LEN_ICMP_DATA)
#define LEN_IP			(sizeof(struct ip)+LEN_ICMP)

#define	ETHER_ADDR_LEN	6

struct ether_addr {
	uint8_t ether_addr_octet[6];
};

struct	ether_header {
	uint8_t  ether_dhost[ETHER_ADDR_LEN];
	uint8_t  ether_shost[ETHER_ADDR_LEN];
	uint16_t ether_type;
};

struct in_addr {
	Word s_addr;
};

struct ip {
#if BYTE_ORDER == LITTLE_ENDIAN
	uint8_t  ip_hl:4,		/* header length */
		  ip_v:4;		/* version */
#endif
#if BYTE_ORDER == BIG_ENDIAN
	uint8_t  ip_v:4,		/* version */
		  ip_hl:4;		/* header length */
#endif
	uint8_t  ip_tos;		/* type of service */
	short	  ip_len;		/* total length */
	uint16_t ip_id;		/* identification */
	short	  ip_off;		/* fragment offset field */
#define	IP_DF 0x4000			/* dont fragment flag */
#define	IP_MF 0x2000			/* more fragments flag */
#define	IP_OFFMASK 0x1fff		/* mask for fragmenting bits */
	uint8_t  ip_ttl;		/* time to live */
	uint8_t  ip_p;			/* protocol */
	uint16_t ip_sum;		/* checksum */
	struct	  in_addr ip_src, ip_dst; /* source and dest address */
};

/*
 * Structure of an icmp header.
 */
typedef short n_short;
typedef unsigned int n_time;
struct icmp {
	uint8_t  icmp_type;		/* type of message, see below */
	uint8_t  icmp_code;		/* type sub code */
	uint16_t icmp_cksum;		/* ones complement cksum of struct */
	union {
		uint8_t  ih_pptr;		/* ICMP_PARAMPROB */
		struct in_addr ih_gwaddr;	/* ICMP_REDIRECT */
		struct ih_idseq {
			  n_short icd_id;
			  n_short icd_seq;
		} ih_idseq;
		int   ih_void;

		/* ICMP_UNREACH_NEEDFRAG -- Path MTU Discovery (RFC1191) */
		struct ih_pmtu {
			  n_short ipm_void;    
			  n_short ipm_nextmtu;
		} ih_pmtu;
	} icmp_hun;
#define	icmp_pptr	  icmp_hun.ih_pptr
#define	icmp_gwaddr	  icmp_hun.ih_gwaddr
#define	icmp_id		  icmp_hun.ih_idseq.icd_id
#define	icmp_seq	  icmp_hun.ih_idseq.icd_seq
#define	icmp_void	  icmp_hun.ih_void
#define	icmp_pmvoid	  icmp_hun.ih_pmtu.ipm_void
#define	icmp_nextmtu	  icmp_hun.ih_pmtu.ipm_nextmtu
	union {
		struct id_ts {
			  n_time its_otime;
			  n_time its_rtime;
			  n_time its_ttime;
		} id_ts;
		struct id_ip  {
			  struct ip idi_ip;
			  /* options and then 64 bits of data */
		} id_ip;
		unsigned int id_mask;
		char	  id_data[1];
	} icmp_dun;
#define	icmp_otime	  icmp_dun.id_ts.its_otime
#define	icmp_rtime	  icmp_dun.id_ts.its_rtime
#define	icmp_ttime	  icmp_dun.id_ts.its_ttime
#define	icmp_ip		  icmp_dun.id_ip.idi_ip
#define	icmp_mask	  icmp_dun.id_mask
#define	icmp_data	  icmp_dun.id_data
};

static uint32_t IpIdNext = 0;

typedef int bool;
#define true 1
#define false 0

bool
PingPacket(uint8_t *pPkt, uint32_t len)
{
  struct ether_header *h_ether=(struct ether_header *)pPkt;
  struct ip *h_ip=(struct ip *)(pPkt+sizeof(struct ether_header));
  struct icmp *h_icmp=(struct icmp *)(((char *)h_ip)+(h_ip->ip_hl<<2));
  struct in_addr tmp;

  uint16_t the_ether_type = ETHERTYPE_IP;
  HTONS(the_ether_type);
  if (!(h_ether->ether_type == the_ether_type &&
	len>=LEN_IP &&
	h_ip->ip_p == IPPROTO_ICMP &&
	h_icmp->icmp_type == ICMP_ECHO))
    return false;

  if (h_ip->ip_ttl == 0)
    return false;
#if 0
  MsgLog::printf("It's icmp echo!\n");
#endif
  
  bcopy(h_ether->ether_shost, h_ether->ether_dhost, sizeof(struct ether_addr));
  bcopy(&enetAddr, h_ether->ether_shost, sizeof(struct ether_addr));

  if (in_cksum((uint16_t *)h_ip, (h_ip->ip_hl<<2)))
    return false;

#if 0
  MsgLog::printf("Cksum OK\n");
#endif

  tmp=h_ip->ip_dst;
  h_ip->ip_dst=h_ip->ip_src;
  h_ip->ip_sum=0;
  h_ip->ip_src=tmp;
  h_ip->ip_ttl = 64;		/* it's a new packet! */
  h_ip->ip_id=IpIdNext++;
  h_ip->ip_sum=in_cksum((uint16_t *)h_ip, h_ip->ip_hl<<2);

  h_icmp->icmp_cksum=0;
  h_icmp->icmp_type=ICMP_ECHOREPLY;
  h_icmp->icmp_cksum=in_cksum((uint16_t *)h_icmp,
			      h_ip->ip_len-(h_ip->ip_hl<<2));

  return true;
}
#endif

void
TestRead()
{
  static pktbuf[2048];		/* big enough! */
  
  Message	msg;

  msg.snd_key0 = KR_VOID;
  msg.snd_key1 = KR_VOID;
  msg.snd_key2 = KR_VOID;
  msg.snd_key3 = KR_VOID;

  msg.rcv_key0 = KR_VOID;
  msg.rcv_key1 = KR_VOID;
  msg.rcv_key2 = KR_VOID;
  msg.rcv_key3 = KR_VOID;

  msg.snd_len = 0;
  msg.rcv_len = 2048;
  msg.rcv_data = pktbuf;

  msg.snd_code = OC_READ;
  msg.snd_invKey = KR_EL3_SLOT;
  (void) CALL(&msg);

#if 0
  if (PingPacket(pktbuf, msg.rcv_len)) {
    kprintf(KR_CONSOLEKEY_SLOT, "Sending ping echo...\n");
    WritePacket(msg.rcv_data, msg.rcv.len);
  }
#endif

  {
    uint8_t *pkt = (uint8_t *) msg.rcv_data;
    kprintf(KR_CONSOLEKEY_SLOT, "Rcv pkt len was %d.\n", msg.rcv_len);
    kprintf(KR_CONSOLEKEY_SLOT, "Rcv pkt: from: %02x:%02x:%02x:%02x:%02x:%02x\n",
	    pkt[0], pkt[1], pkt[2], pkt[3], pkt[4], pkt[5]);
    pkt += 6;
    kprintf(KR_CONSOLEKEY_SLOT, "         to:   %02x:%02x:%02x:%02x:%02x:%02x\n",
	    pkt[0], pkt[1], pkt[2], pkt[3], pkt[4], pkt[5]);
    pkt += 6;
    kprintf(KR_CONSOLEKEY_SLOT, "       data:   %02x %02x %02x %02x %02x %02x\n",
	    pkt[0], pkt[1], pkt[2], pkt[3], pkt[4], pkt[5]);
  }
}

void
Startup()
{
  /* Message         msg; */
#if 0
  char		outputPkt[EL3_TEST_PKT_LEN + 1];
#endif
  char		outputAddr[6];
  uint16_t	h;

	/* Convert test packet to ASCII for console output. */
	/* for (loop = 0; loop < EL3_TEST_PKT_LEN; loop++)
		outputPkt[loop] = (char) pktData[loop] + '0';
	outputPkt[loop] = NULL; */

  ShowKey(KR_CONSOLEKEY_SLOT, KR_KEYBITS, KR_NETCREATOR_SLOT);

#if 0
  /* Print packet data for programmer verification. */
  WriteHexData(pktData, EL3_TEST_PKT_LEN);
#endif

  /* Obtain key for ELNK3. */
  GetELNK3Key();

  ShowKey(KR_CONSOLEKEY_SLOT, KR_KEYBITS, KR_EL3_SLOT);

  /* Obtain Ethernet addr of ELNK3. */
  GetEnetAddr(&enetAddr);

  /* Convert Ethernet hardware address to output format.
	Assuming conversion of endianness needed. */
  h = enetAddr.enetAddr[0];
  outputAddr[0] = (char) (h >> 8);
  outputAddr[1] = (char) (h & 0x00FFu);
  h = enetAddr.enetAddr[1];
  outputAddr[2] = (char) (h >> 8);
  outputAddr[3] = (char) (h & 0x00FFu);
  h = enetAddr.enetAddr[2];
  outputAddr[4] = (char) (h >> 8);
  outputAddr[5] = (char) (h & 0x00FFu);
  WriteHexData(outputAddr, 6);

  WritePacket(pktData, EL3_TEST_PKT_LEN);

  for(;;){
    TestRead();
  }

#if 0
  /* CALL(KR_EL3_SLOT, OC_READ, &msg); */

	/* CALL(KR_EL3_SLOT, OC_STAT, &msg); */
#endif
  kprintf(KR_CONSOLEKEY_SLOT, "At end of ELNK3 test domain.\n");
}