KernelLand/Modules/IPStack/icmpv6.c

   1 /*
   2  * Acess2 IP Stack
   3  *
   4  * icmpv6.c
   5  * - Internet Control Message Protocol v6
   6  */
   7 #include "ipstack.h"
   8 #include "ipv6.h"
   9 #include "ipv4.h"       // For the IP Checksum
  10 #include "icmpv6.h"
  11 #include "link.h"
  12 #include "hwaddr_cache.h"
  13 #include "include/adapters_int.h"
  14
  15 // === GLOBALS ===
  16 void    ICMPv6_GetPacket(tInterface *Interface, void *Address, int Length, const void *Buffer);
  17  int    ICMPv6_ND_GetOpt(size_t Length, const void *Buffer, Uint8 OptID, const void **Ptr);
  18
  19 // === CODE ===
  20 void ICMPv6_Initialise(void)
  21 {
  22
  23 }
  24
  25 void ICMPv6_GetPacket(tInterface *Interface, void *Address, int Length, const void *Buffer)
  26 {
  27         if( Length < sizeof(tICMPv6_Header))
  28                 return ;
  29         const tICMPv6_Header    *hdr = Buffer;
  30
  31         // Validate checksum
  32         if( IPv4_Checksum(Buffer, Length) != 0 )
  33                 return ;
  34
  35         if( hdr->Type == ICMPV6_INFO_NEIGHBOUR_ADVERTISMENT )
  36         {
  37                 if( Length < sizeof(tICMPv6_Header) + sizeof(tICMPv6_NA) )
  38                         return ;
  39                 // TODO: Check that IP.HopLimt == 255
  40                 const tICMPv6_NA        *na = (const void*)(hdr+1);
  41                 if( !(na->Flags & (1 << 1)) ) {
  42                         // Unsolicited, TODO
  43                         return ;
  44                 }
  45                 if( na->Flags & (1 << 2) ) {
  46                         // Override, force update
  47                 }
  48                 const tICMPv6_Opt_LinkAddr      *la = NULL;
  49                 if( ICMPv6_ND_GetOpt(Length-sizeof(*hdr)+sizeof(*na), na+1, ICMPV6_OPTION_SRCLINK, (const void**)&la) )
  50                         return ;
  51                 if( !la ) {
  52                         LOG("No link address on neighbor advertisement");
  53                         return;
  54                 }
  55
  56                 HWCache_Set(Interface->Adapter, 6, Address, (const tMacAddr*)la->Address);
  57         }
  58         else {
  59                 // Return unhandled?
  60         }
  61
  62         switch(hdr->Type)
  63         {
  64         case ICMPV6_INFO_ROUTER_SOLICITATION:
  65                 // TODO: If routing is active, send out RA
  66                 break;
  67         case ICMPV6_INFO_ROUTER_ADVERTISEMENT: {
  68                 // TODO: If routing INACTIVE, and interface is ::/0, set address
  69                 break; }
  70         case ICMPV6_INFO_NEIGHBOUR_SOLICITATION:
  71                 if( Length < sizeof(tICMPv6_Header) + sizeof(tICMPv6_NS) )
  72                         return ;
  73                 // TODO: Check that IP.HopLimt == 255
  74                 break;
  75         case ICMPV6_INFO_NEIGHBOUR_ADVERTISMENT:
  76                 //HWCache_Set(Interface->Adapter,
  77                 break;
  78         }
  79 }
  80
  81 int ICMPv6_ND_GetOpt(size_t Length, const void *Buffer, Uint8 OptID, const void **Ptr)
  82 {
  83         const struct {
  84                 Uint8   Type;
  85                 Uint8   Length;
  86                 Uint16  _resvd1;
  87                 Uint32  _resvd2;
  88         }       *opts = Buffer;
  89
  90         while( Length >= sizeof(*opts))
  91         {
  92                 if( opts->Length == 0 )
  93                         return 1;
  94                 if( opts->Length * 8 > Length )
  95                         return 1;
  96
  97                 if( opts->Type == OptID ) {
  98                         *Ptr = opts;
  99                         return 0;
 100                 }
 101                 opts += opts->Length;
 102         }
 103         return 0;
 104 }
 105
 106 void ICMPv6_SendNS(tInterface *Interface, const void *Address)
 107 {
 108         const Uint8     *addr8 = Address;
 109         struct {
 110                 tIPv6Header     ip;
 111                 tICMPv6_Header  icmp;
 112                 tICMPv6_NS      ns;
 113                 tICMPv6_Opt_LinkAddr    linkaddr;
 114                 Uint8   our_mac[6];
 115         } PACKED pkt;
 116         // Custom-crafted IPv6 header
 117         pkt.ip.Version = 6;
 118         pkt.ip.TrafficClass = 0;
 119         pkt.ip.FlowLabel = 0;
 120         pkt.ip.Head = htonl(pkt.ip.Head);
 121         pkt.ip.PayloadLength = htons(sizeof(pkt)-sizeof(pkt.ip));
 122         pkt.ip.NextHeader = IPV6PROT_ICMPV6;
 123         pkt.ip.HopLimit = 255;  // Max value
 124         pkt.ip.Source = *(tIPv6*)Interface->Address;
 125         pkt.ip.Destination = (tIPv6){.B={0xFF,0x02, 0,0, 0,0, 0,0,  0,0, 0,1,0xFF,addr8[13], addr8[14],addr8[15]}};
 126
 127         pkt.icmp.Type = ICMPV6_INFO_NEIGHBOUR_SOLICITATION;
 128         pkt.icmp.Code = 0;
 129         pkt.icmp.Checksum = 0;  // populated later
 130
 131         pkt.ns.Reserved = 0;
 132         pkt.ns.TargetAddress = *(const tIPv6*)Address;
 133
 134         pkt.linkaddr.Type = ICMPV6_OPTION_SRCLINK;
 135         pkt.linkaddr.Length = 1;        // 1 * 8 bytes
 136         memcpy(pkt.our_mac, Interface->Adapter->HWAddr, 6);
 137
 138         pkt.icmp.Checksum = IPv4_Checksum(&pkt, sizeof(pkt));
 139
 140
 141         tMacAddr        to = {.B={0x33, 0x33, addr8[12], addr8[13], addr8[14], addr8[15]}};
 142
 143         tIPStackBuffer *buffer = IPStack_Buffer_CreateBuffer(2);
 144         IPStack_Buffer_AppendSubBuffer(buffer, sizeof(pkt), 0, &pkt, NULL, NULL);
 145         Link_SendPacket(Interface->Adapter, IPV6_ETHERNET_ID, to, buffer);
 146         IPStack_Buffer_DestroyBuffer(buffer);
 147 }