KernelLand/Modules/IPStack/icmp.c

   1 /*
   2  * Acess2 IP Stack
   3  * - ICMP Handling
   4  */
   5 #include "ipstack.h"
   6 #include "ipv4.h"
   7 #include "icmp.h"
   8
   9 // === CONSTANTS ===
  10 #define PING_SLOTS      64
  11
  12 // === PROTOTYPES ===
  13 void    ICMP_Initialise();
  14 void    ICMP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer);
  15
  16 // === GLOBALS ===
  17 struct {
  18         tInterface      *Interface;
  19          int    bArrived;
  20 }       gICMP_PingSlots[PING_SLOTS];
  21
  22 // === CODE ===
  23 /**
  24  * \fn void ICMP_Initialise()
  25  * \brief Initialise the ICMP Layer
  26  */
  27 void ICMP_Initialise()
  28 {
  29         IPv4_RegisterCallback(IP4PROT_ICMP, ICMP_GetPacket, NULL);
  30 }
  31
  32 /**
  33  * \fn void ICMP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
  34  * \brief Handles a packet from the IP Layer
  35  */
  36 void ICMP_GetPacket(tInterface *Interface, void *Address, int Length, void *Buffer)
  37 {
  38         tICMPHeader     *hdr = Buffer;
  39
  40         //Log_Debug("ICMPv4", "Length = %i", Length);
  41         Log_Debug("ICMPv4", "hdr->Type, hdr->Code = %i, %i", hdr->Type, hdr->Code);
  42         //Log_Debug("ICMPv4", "hdr->Checksum = 0x%x", ntohs(hdr->Checksum));
  43         Log_Debug("ICMPv4", "hdr->ID = 0x%x", ntohs(hdr->ID));
  44         Log_Debug("ICMPv4", "hdr->Sequence = 0x%x", ntohs(hdr->Sequence));
  45
  46         switch(hdr->Type)
  47         {
  48         // -- 0: Echo Reply
  49         case ICMP_ECHOREPLY:
  50                 if(hdr->Code != 0) {
  51                         Log_Warning("ICMPv4", "Code == %i for ICMP Echo Reply, should be 0", hdr->Code);
  52                         return ;
  53                 }
  54                 if(hdr->ID != (Uint16)~hdr->Sequence) {
  55                         Log_Warning("ICMPv4", "ID and Sequence values do not match");
  56                         //return ;
  57                 }
  58                 gICMP_PingSlots[hdr->ID].bArrived = 1;
  59                 break;
  60
  61         // -- 3: Destination Unreachable
  62         case ICMP_UNREACHABLE:
  63                 switch(hdr->Code)
  64                 {
  65                 case 3: // Port Unreachable
  66                         Log_Debug("ICMPv4", "Destination Unreachable (Port Unreachable)");
  67                         IPv4_HandleError( Interface, IPERR_PORT_UNREACHABLE,
  68                                 htons(Length)-sizeof(tICMPHeader), hdr->Data );
  69                         break;
  70                 default:
  71                         Log_Debug("ICMPv4", "Destination Unreachable (Code %i)", hdr->Code);
  72                         IPv4_HandleError( Interface, IPERR_MISC,
  73                                 htons(Length)-sizeof(tICMPHeader), hdr->Data );
  74                         break;
  75                 }
  76                 break;
  77
  78         // -- 8: Echo Request
  79         case ICMP_ECHOREQ:
  80                 if(hdr->Code != 0) {
  81                         Log_Warning("ICMPv4", "Code == %i for ICMP Echo Request, should be 0", hdr->Code);
  82                         return ;
  83                 }
  84                 //Log_Debug("ICMPv4", "Replying");
  85                 hdr->Type = ICMP_ECHOREPLY;
  86                 hdr->Checksum = 0;
  87                 hdr->Checksum = htons( IPv4_Checksum( (Uint16*)hdr, Length/2 ) );
  88                 //Log_Debug("ICMPv4", "Checksum = 0x%04x", hdr->Checksum);
  89
  90                 tIPStackBuffer  *buffer = IPStack_Buffer_CreateBuffer(1 + IPV4_BUFFERS);
  91                 IPStack_Buffer_AppendSubBuffer(buffer, Length, 0, hdr, NULL, NULL);
  92                 IPv4_SendPacket(Interface, *(tIPv4*)Address, 1, ntohs(hdr->Sequence), buffer);
  93                 break;
  94         default:
  95                 break;
  96         }
  97
  98 }
  99
 100 /**
 101  * \brief Sends ICMP Echo and waits for the reply
 102  * \note Times out after \a Interface->TimeoutDelay has elapsed
 103  */
 104 int ICMP_Ping(tInterface *Interface, tIPv4 Addr)
 105 {
 106         Sint64  ts;
 107         Sint64  end;
 108         char    buf[32] = "\x8\0\0\0\0\0\0\0Acess2 I"
 109                       "P/TCP Stack 1.0\0";
 110         tICMPHeader     *hdr = (void*)buf;
 111          int    i;
 112
 113         for(;;)
 114         {
 115                 for(i=0;i<PING_SLOTS;i++)
 116                 {
 117                         if(gICMP_PingSlots[i].Interface == NULL)        break;
 118                 }
 119                 if( i < PING_SLOTS )    break;
 120                 Threads_Yield();
 121         }
 122         gICMP_PingSlots[i].Interface = Interface;
 123         gICMP_PingSlots[i].bArrived = 0;
 124         hdr->ID = i;
 125         hdr->Sequence = ~i;
 126         hdr->Checksum = htons( IPv4_Checksum((Uint16*)buf, sizeof(buf)) );
 127
 128         ts = now();
 129
 130         tIPStackBuffer  *buffer = IPStack_Buffer_CreateBuffer(1 + IPV4_BUFFERS);
 131         IPStack_Buffer_AppendSubBuffer(buffer, sizeof(buf), 0, buf, NULL, NULL);
 132         IPv4_SendPacket(Interface, Addr, 1, i, buffer);
 133
 134         end = ts + Interface->TimeoutDelay;
 135         while( !gICMP_PingSlots[i].bArrived && now() < end)     Threads_Yield();
 136
 137         if( !gICMP_PingSlots[i].bArrived )
 138                 return -1;
 139
 140         return (int)( now() - ts );
 141 }