01 // vim: set ft=c:
02 
03 #define ICMP_TYPE_ECHO_REPLY      0
04 #define ICMP_TYPE_ECHO_REQUEST    8
05 
06 class CIcmpHeader {
07   U8 type;
08   U8 code;
09   U16 checksum;
10   U16 identifier;
11   U16 seq_number;
12 };
13 
14 I64 IcmpSendReply(U32 dest_ip, U16 identifier, U16 seq_number, U16 request_checksum, U8* payload, I64 length) {
15   U8* frame;
16   I64 index = IPv4PacketAlloc(&frame, IP_PROTO_ICMP, IPv4GetAddress(), dest_ip, sizeof(CIcmpHeader) + length);
17 
18   if (index < 0)
19     return index;
20 
21   CIcmpHeader* hdr = frame;
22   hdr->type = ICMP_TYPE_ECHO_REPLY;
23   hdr->code = 0;
24   hdr->checksum = htons(ntohs(request_checksum) + 0x0800);    // hack alert!
25   hdr->identifier = identifier;
26   hdr->seq_number = seq_number;
27 
28   MemCpy(frame + sizeof(CIcmpHeader), payload, length);
29   return IPv4PacketFinish(index);
30 }
31 
32 I64 IcmpHandler(CIPv4Packet* packet) {
33   if (packet->proto != IP_PROTO_ICMP)
34     return -1;
35 
36   if (packet->length < sizeof(CIcmpHeader))
37     return -1;
38 
39   CIcmpHeader* hdr = packet->data;
40 
41   if (hdr->type == ICMP_TYPE_ECHO_REQUEST && hdr->code == 0) {
42     // This also makes sure that we don't stall NetHandlerTask
43     ArpCachePut(packet->source_ip, packet->l2_frame->source_addr);
44 
45     IcmpSendReply(packet->source_ip, hdr->identifier, hdr->seq_number, hdr->checksum,
46         packet->data + sizeof(CIcmpHeader), packet->length - sizeof(CIcmpHeader));
47   }
48 
49   return 0;
50 }
51 
52 RegisterL4Protocol(IP_PROTO_ICMP, &IcmpHandler);