icmp.c

44B0+8019系统

//-----------------------------------------------------------------------------
// Net ICMP.C
//
// This module handles ICMP messages
// Refer to RFC 792, 896, 950, 1122, and 1191
//-----------------------------------------------------------------------------
#include <string.h>
#include <stdlib.h>
#include "net.h"
#include "cksum.h"
#include "ip.h"
#include "icmp.h"


void icmp__err_getHader(void *inbuf , void *h)
{
   unsigned char *pinbuf = (unsigned char*)inbuf;
   unsigned char *pBuf;
   ICMP_ERR_HEADER *ph = (ICMP_ERR_HEADER*)h;

   //=========================================
   ph->msg_type = pinbuf[34];
   ph->msg_code = pinbuf[35];

   //=========================================
   //pBuf = (unsigned char *)&ph->checksum;
   //pBuf[1] = pinbuf[36];
   //pBuf[0] = pinbuf[37];
   ph->checksum = get__int16(pinbuf+36);
   //=========================================
   //memcpy( (unsigned char *)&ph->msg_data , pinbuf + 38 , 4);
   ph->msg_data = get__int32(pinbuf+38);
   //=========================================
   ph->echo_data = pinbuf[42];
}

void icmp_err_setHader(void *inbuf , void *h)
{
   unsigned char *pinbuf = (unsigned char*)inbuf;
   unsigned char *pBuf;
   ICMP_ERR_HEADER *ph = (ICMP_ERR_HEADER*)h;

   //=========================================
   pinbuf[34] = ph->msg_type;
   pinbuf[35] = ph->msg_code;

   //=========================================
   //pBuf = (unsigned char *)&ph->checksum;
   //pinbuf[36] = pBuf[1];
   //pinbuf[37] = pBuf[0];
   set__int16(pinbuf+36,ph->checksum);
   //=========================================
   //memcpy( pinbuf + 38 , (unsigned char *)&ph->msg_data , 4);
   set__int32(pinbuf+38,ph->msg_data);
   //=========================================
   pinbuf[42] = ph->echo_data;
}

void ping_getHader(void *inbuf,void *h)
{
   unsigned char *pinbuf = (unsigned char*)inbuf;
   unsigned char *pBuf;
   PING_HEADER *ph = (PING_HEADER*)h;

   //=========================================
   ph->msg_type = pinbuf[34];
   ph->msg_code = pinbuf[35];

   //=========================================
   //pBuf = (unsigned char *)&ph->checksum;
   //pBuf[1] = pinbuf[36];
   //pBuf[0] = pinbuf[37];
   ph->checksum = get__int16(pinbuf+36);
   //=========================================
   //pBuf = (unsigned char *)&ph->identifier;
   //pBuf[1] = pinbuf[38];
   //pBuf[0] = pinbuf[39];
   ph->identifier = get__int16(pinbuf+38);
   //=========================================
   //pBuf = (unsigned char *)&ph->sequence;
   //pBuf[1] = pinbuf[40];
   //pBuf[0] = pinbuf[41];
   ph->sequence = get__int16(pinbuf+40);
   //=========================================
   ph->echo_data = pinbuf[42];
}
void ping_setHader(void *inbuf,void *h)
{
   unsigned char *pinbuf = (unsigned char*)inbuf;
   unsigned char *pBuf;
   PING_HEADER *ph = (PING_HEADER*)h;

   //=========================================
   pinbuf[34] = ph->msg_type;
   pinbuf[35] = ph->msg_code;

   //=========================================
   //pBuf = (unsigned char *)&ph->checksum;
   //pinbuf[36] = pBuf[1];
   //pinbuf[37] = pBuf[0];
   set__int16(pinbuf+36,ph->checksum);
   //=========================================
   //pBuf = (unsigned char *)&ph->identifier;
   //pinbuf[38] = pBuf[1];
   //pinbuf[39] = pBuf[0];
   set__int16(pinbuf+38,ph->identifier);
   //=========================================
   //pBuf = (unsigned char *)&ph->sequence;
   //pinbuf[40] = pBuf[1];
   //pinbuf[41] = pBuf[0];
   set__int16(pinbuf+40,ph->sequence);
   //=========================================
   pinbuf[42] = ph->echo_data;
}
//------------------------------------------------------------------------
// This builds a ping response message.  It allocates memory for the
// entire outgoing message, including Eth and IP headers.  See "TCP/IP
// Illustrated, Volume 1" Sect 7.2 for info on Ping messages
//------------------------------------------------------------------------
void ping_send(unsigned char *inbuf, unsigned int ipaddr, unsigned short int len)
{
   PING_HEADER *ping_in,pi;
   PING_HEADER *ping_out,po;
   unsigned char outbuf[1520];
            
   ping_in = &pi;
   ping_getHader(inbuf,ping_in);
      
   // Ping response message payload starts at offset 34
   ping_out = &po;

   ping_out->msg_type = 0;
   ping_out->msg_code = 0;
   ping_out->checksum = 0;
   ping_out->identifier = ping_in->identifier;
   ping_out->sequence = ping_in->sequence;
   
   memcpy(outbuf+42,inbuf + 42, len - 8);
   ping_out->echo_data = ping_in->echo_data;
            
   // Compute checksum over the ICMP header plus
	// optional data and insert complement
   ping_setHader(outbuf,ping_out);
   ping_out->checksum = ~cksum(outbuf + 34, len);
   ping_setHader(outbuf,ping_out);
                        

   ip_send(outbuf, ipaddr, ICMP_TYPE, len);
}



//------------------------------------------------------------------------
// This builds an outgoing ICMP destination port unreachable response
// message.  See See "TCP/IP Illustrated, Volume 1" Sect 6.5.  This
// message is typically sent in response to a UDP message directed
// to a port that has no corresponding application running. 
// Todo: The spec says we should return all options that were in
// the original incoming IP header.  Right now we cut off everything
// after the first 20 bytes. 
//------------------------------------------------------------------------
void dest_unreach_send(unsigned char *inbuf, unsigned int ipaddr)
{
   unsigned char outbuf[70];
   ICMP_ERR_HEADER *icmp,icmph;
            
   // Allocate memory for entire outgoing message
   // including eth and IP haders.  Always 70 bytes
      
   icmp = &icmph;
   
   // Fill in ICMP error message header
   icmp->msg_type = 3;   // destination unreachable
   icmp->msg_code = 3;   // port unreachable
   icmp->checksum = 0;
      
   // Fill in ICMP error message data
   icmp->msg_data = 0;
           
   // Copy in 20 byte original incoming IP header
   // plus 8 bytes of data
   memcpy( outbuf + 42, inbuf + 14, 28);
   icmp->echo_data = inbuf[14];
                               
   // Compute checksum over the 36 byte long ICMP
   // header plus data and insert complement
   icmp_err_setHader(outbuf , icmp);
   icmp->checksum = ~cksum(outbuf + 34, 36);
   icmp_err_setHader(outbuf , icmp);
   // Forward message to the IP layer
   ip_send(outbuf, ipaddr, ICMP_TYPE, 36);
}




//------------------------------------------------------------------------
// This handles incoming ICMP messages.  See "TCP/IP Illustrated,
// Volume 1" Sect 6.2 for discussion of the various ICMP messages
//------------------------------------------------------------------------
void icmp_rcve(unsigned char *inbuf, unsigned short int len)
{
   IP_HEADER *ip,iph;
   unsigned char msg_type;
   unsigned short int temp;
   
   // Allow for 14 bytes eth header
   ip = &iph;
   ip_getHader(inbuf,ip);
        
   // IP header has been adjusted if necessary to always be 
	// 20 bytes so message starts at an offset of 34
   // Validate checksum of entire ICMP message incl data 
	temp = cksum(inbuf + 34, len);
	
	if (temp != 0xFFFF)
	{
      return; 
   }
      
   // Switch on the message type
   msg_type = *(inbuf + 34);
   switch(msg_type)
   {
      case 3:
      //if (debug) serial_send("ICMP: Dest unreachable rcvd\r");
      break;

	   case 8:
      //if (debug) serial_send("ICMP: Ping rcvd\r");
      ping_send(inbuf, ip->source_ipaddr, len); 
      break;

      default:
      //if (debug) serial_send("ICMP: Error, unknown msg rcvd\r");
      break;
   }
}