Usermode/libc - Fix strchr and strrchr behavior
[tpg/acess2.git] / 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 }

UCC git Repository :: git.ucc.asn.au