[ipv6] Add a dedicated function for prefix matching
[people/pcmattman/gpxe.git] / src / net / ipv6.c
1 #include <errno.h>
2 #include <stdint.h>
3 #include <string.h>
4 #include <stdlib.h>
5 #include <stdio.h>
6 #include <byteswap.h>
7 #include <gpxe/in.h>
8 #include <gpxe/ip6.h>
9 #include <gpxe/ndp.h>
10 #include <gpxe/list.h>
11 #include <gpxe/icmp6.h>
12 #include <gpxe/tcpip.h>
13 #include <gpxe/socket.h>
14 #include <gpxe/iobuf.h>
15 #include <gpxe/netdevice.h>
16 #include <gpxe/if_ether.h>
17
18 struct net_protocol ipv6_protocol;
19
20 char * inet6_ntoa ( struct in6_addr in6 );
21
22 /* Unspecified IP6 address */
23 static struct in6_addr ip6_none = {
24         .in6_u.u6_addr32 = { 0,0,0,0 }
25 };
26
27 /** An IPv6 routing table entry */
28 struct ipv6_miniroute {
29         /* List of miniroutes */
30         struct list_head list;
31
32         /* Network device */
33         struct net_device *netdev;
34
35         /* Destination prefix */
36         struct in6_addr prefix;
37         /* Prefix length */
38         int prefix_len;
39         /* IPv6 address of interface */
40         struct in6_addr address;
41         /* Gateway address */
42         struct in6_addr gateway;
43 };
44
45 /** List of IPv6 miniroutes */
46 static LIST_HEAD ( miniroutes );
47
48 /**
49  * Generate an EUI-64 from a given link-local address.
50  *
51  * @v out               pointer to buffer to receive the EUI-64
52  * @v ll                pointer to buffer containing the link-local address
53  */
54 void ipv6_generate_eui64 ( uint8_t *out, uint8_t *ll ) {
55         assert ( out != 0 );
56         assert ( ll != 0 );
57         
58         /* Create an EUI-64 identifier. */
59         memcpy( out, ll, 3 );
60         memcpy( out + 5, ll + 3, 3 );
61         out[3] = 0xFF;
62         out[4] = 0xFE;
63         
64         /* Designate that this is in fact an EUI-64. */
65         out[0] |= 0x2;
66 }
67
68 /**
69  * Verifies that a prefix matches another one.
70  *
71  * @v p1                first prefix
72  * @v p2                second prefix
73  * @v len               prefix length in bits to compare
74  * @ret int             0 if a match, nonzero otherwise
75  */
76 int ipv6_match_prefix ( struct in6_addr *p1, struct in6_addr *p2, size_t len ) {
77         uint8_t ip1, ip2;
78         size_t offset, bits;
79         int rc = 0;
80
81         /* Check for a prefix match on the route. */
82         if ( ! memcmp ( p1, p2, len / 8 ) ) {
83                 rc = 0;
84
85                 /* Handle extra bits in the prefix. */
86                 if ( ( len % 8 ) ||
87                      ( len < 8 ) ) {
88                         DBG ( "ipv6: prefix is not aligned to a byte.\n" );
89
90                         /* Compare the remaining bits. */
91                         offset = len / 8;
92                         bits = len % 8;
93
94                         ip1 = p1->in6_u.u6_addr8[offset];
95                         ip2 = p2->in6_u.u6_addr8[offset];
96                         if ( ! ( ( ip1 & (0xFF >> (8 - bits)) ) &
97                              ( ip2 ) ) ) {
98                                 rc = 1;
99                         }
100                 }
101         } else {
102                 rc = 1;
103         }
104
105         return rc;
106 }
107
108 /**
109  * Add IPv6 minirouting table entry
110  *
111  * @v netdev            Network device
112  * @v prefix            Destination prefix (in bits)
113  * @v address           Address of the interface
114  * @v gateway           Gateway address (or ::0 for no gateway)
115  * @ret miniroute       Routing table entry, or NULL
116  */
117 static struct ipv6_miniroute * __malloc
118 add_ipv6_miniroute ( struct net_device *netdev, struct in6_addr prefix,
119                      int prefix_len, struct in6_addr address,
120                      struct in6_addr gateway ) {
121         struct ipv6_miniroute *miniroute;
122         
123         DBG( "ipv6 add: %s/%d ", inet6_ntoa ( address ), prefix_len );
124         DBG( "gw %s\n", inet6_ntoa( gateway ) );
125
126         miniroute = malloc ( sizeof ( *miniroute ) );
127         if ( miniroute ) {
128                 /* Record routing information */
129                 miniroute->netdev = netdev_get ( netdev );
130                 miniroute->prefix = prefix;
131                 miniroute->prefix_len = prefix_len;
132                 miniroute->address = address;
133                 miniroute->gateway = gateway;
134
135                 /* Add miniroute to list of miniroutes */
136                 if ( !IP6_EQUAL ( gateway, ip6_none ) ) {
137                         list_add_tail ( &miniroute->list, &miniroutes );
138                 } else {
139                         list_add ( &miniroute->list, &miniroutes );
140                 }
141         }
142
143         return miniroute;
144 }
145
146 /**
147  * Delete IPv6 minirouting table entry
148  *
149  * @v miniroute         Routing table entry
150  */
151 static void del_ipv6_miniroute ( struct ipv6_miniroute *miniroute ) {
152         
153         DBG ( "ipv6 del: %s/%d\n", inet6_ntoa(miniroute->address),
154                                    miniroute->prefix_len );
155         
156         netdev_put ( miniroute->netdev );
157         list_del ( &miniroute->list );
158         free ( miniroute );
159 }
160
161 /**
162  * Add IPv6 interface
163  *
164  * @v netdev    Network device
165  * @v prefix    Destination prefix
166  * @v address   Address of the interface
167  * @v gateway   Gateway address (or ::0 for no gateway)
168  */
169 int add_ipv6_address ( struct net_device *netdev, struct in6_addr prefix,
170                        int prefix_len, struct in6_addr address,
171                        struct in6_addr gateway ) {
172         struct ipv6_miniroute *miniroute;
173
174         /* Clear any existing address for this net device */
175         /* del_ipv6_address ( netdev ); */
176
177         /* Add new miniroute */
178         miniroute = add_ipv6_miniroute ( netdev, prefix, prefix_len, address,
179                                          gateway );
180         if ( ! miniroute )
181                 return -ENOMEM;
182
183         return 0;
184 }
185
186 /**
187  * Remove IPv6 interface
188  *
189  * @v netdev    Network device
190  */
191 void del_ipv6_address ( struct net_device *netdev ) {
192         struct ipv6_miniroute *miniroute;
193
194         list_for_each_entry ( miniroute, &miniroutes, list ) {
195                 if ( miniroute->netdev == netdev ) {
196                         del_ipv6_miniroute ( miniroute );
197                         break;
198                 }
199         }
200 }
201
202 /**
203  * Calculate TCPIP checksum
204  *
205  * @v iobuf     I/O buffer
206  * @v tcpip     TCP/IP protocol
207  *
208  * This function constructs the pseudo header and completes the checksum in the
209  * upper layer header.
210  */
211 static uint16_t ipv6_tx_csum ( struct io_buffer *iobuf, uint16_t csum ) {
212         struct ip6_header *ip6hdr = iobuf->data;
213         struct ipv6_pseudo_header pshdr;
214
215         /* Calculate pseudo header */
216         memset ( &pshdr, 0, sizeof ( pshdr ) );
217         pshdr.src = ip6hdr->src;
218         pshdr.dest = ip6hdr->dest;
219         pshdr.len = htons ( iob_len ( iobuf ) - sizeof ( *ip6hdr ) );
220         pshdr.nxt_hdr = ip6hdr->nxt_hdr;
221
222         /* Update checksum value */
223         return tcpip_continue_chksum ( csum, &pshdr, sizeof ( pshdr ) );
224 }
225
226 /**
227  * Dump IP6 header for debugging
228  *
229  * ip6hdr       IPv6 header
230  */
231 void ipv6_dump ( struct ip6_header *ip6hdr ) {
232         /* Because inet6_ntoa returns a static char[16], each call needs to be
233          * separate. */
234         DBG ( "IP6 %p src %s ", ip6hdr, inet6_ntoa( ip6hdr->src ) );
235         DBG ( "dest %s nxt_hdr %d len %d\n", inet6_ntoa ( ip6hdr->dest ),
236                   ip6hdr->nxt_hdr, ntohs ( ip6hdr->payload_len ) );
237 }
238
239 /**
240  * Transmit IP6 packet
241  *
242  * iobuf                I/O buffer
243  * tcpip        TCP/IP protocol
244  * st_dest      Destination socket address
245  *
246  * This function prepends the IPv6 headers to the payload an transmits it.
247  */
248 static int ipv6_tx ( struct io_buffer *iobuf,
249                      struct tcpip_protocol *tcpip,
250                      struct sockaddr_tcpip *st_src __unused,
251                      struct sockaddr_tcpip *st_dest,
252                      struct net_device *netdev,
253                      uint16_t *trans_csum ) {
254         struct sockaddr_in6 *dest = ( struct sockaddr_in6* ) st_dest;
255         struct in6_addr next_hop, gateway = ip6_none;
256         struct ipv6_miniroute *miniroute;
257         uint8_t ll_dest_buf[MAX_LL_ADDR_LEN];
258         const uint8_t *ll_dest = ll_dest_buf;
259         int rc, multicast, linklocal;
260         
261         /* Check for multicast transmission. */
262         multicast = dest->sin6_addr.in6_u.u6_addr8[0] == 0xFF;
263
264         /* Construct the IPv6 packet */
265         struct ip6_header *ip6hdr = iob_push ( iobuf, sizeof ( *ip6hdr ) );
266         memset ( ip6hdr, 0, sizeof ( *ip6hdr) );
267         ip6hdr->ver_traffic_class_flow_label = htonl ( 0x60000000 );//IP6_VERSION;
268         ip6hdr->payload_len = htons ( iob_len ( iobuf ) - sizeof ( *ip6hdr ) );
269         ip6hdr->nxt_hdr = tcpip->tcpip_proto;
270         ip6hdr->hop_limit = IP6_HOP_LIMIT; // 255
271         
272         /* Determine the next hop address and interface. */
273         next_hop = dest->sin6_addr;
274         list_for_each_entry ( miniroute, &miniroutes, list ) {
275                 /* Check for specific netdev */
276                 if ( netdev && ( miniroute->netdev != netdev ) ) {
277                         continue;
278                 }
279                 
280                 /* Link-local route? */
281                 linklocal = (miniroute->address.in6_u.u6_addr16[0] & htons(0xFE80)) == htons(0xFE80);
282
283                 /* Handle link-local for multicast. */
284                 if ( multicast )
285                 {
286                         /* Link-local scope? */
287                         if ( next_hop.in6_u.u6_addr8[0] & 0x2 ) {
288                                 if ( linklocal ) {
289                                         netdev = miniroute->netdev;
290                                         ip6hdr->src = miniroute->address;
291                                         break;
292                                 } else {
293                                         /* Should be link-local address. */
294                                         continue;
295                                 }
296                         } else {
297                                 /* Can we route on this interface?
298                                    (assume non-link-local means routable) */
299                                 if ( ! linklocal ) {
300                                         netdev = miniroute->netdev;
301                                         ip6hdr->src = miniroute->address;
302                                         break;
303                                 }
304                         }
305                 }
306                 
307                 /* Check for a prefix match on the route. */
308                 rc = ipv6_match_prefix ( &next_hop, &miniroute->prefix, miniroute->prefix_len );
309                 
310                 /* Matched? */
311                 if( rc == 0 ) {
312                         netdev = miniroute->netdev;
313                         ip6hdr->src = miniroute->address;
314                         break;
315                 }
316                 
317                 if ( ( ! ( IP6_EQUAL ( miniroute->gateway, ip6_none ) ) ) &&
318                      ( IP6_EQUAL ( gateway, ip6_none ) ) ) {
319                         netdev = miniroute->netdev;
320                         ip6hdr->src = miniroute->address;
321                         gateway = miniroute->gateway;
322                 }
323         }
324         /* No network interface identified */
325         if ( ( ! netdev ) ) {
326                 DBG ( "No route to host %s\n", inet6_ntoa ( ip6hdr->dest ) );
327                 rc = -ENETUNREACH;
328                 goto err;
329         } else if ( ! IP6_EQUAL ( gateway, ip6_none ) ) {
330                 next_hop = gateway;
331         }
332         
333         /* Add the next hop to the packet. */
334         ip6hdr->dest = dest->sin6_addr;
335
336         /* Complete the transport layer checksum */
337         if ( trans_csum )
338                 *trans_csum = ipv6_tx_csum ( iobuf, *trans_csum );
339
340         /* Print IPv6 header */
341         /* ipv6_dump ( ip6hdr ); */
342
343         /* Resolve link layer address */
344         if ( next_hop.in6_u.u6_addr8[0] == 0xFF ) {
345                 ll_dest_buf[0] = 0x33;
346                 ll_dest_buf[1] = 0x33;
347                 ll_dest_buf[2] = next_hop.in6_u.u6_addr8[12];
348                 ll_dest_buf[3] = next_hop.in6_u.u6_addr8[13];
349                 ll_dest_buf[4] = next_hop.in6_u.u6_addr8[14];
350                 ll_dest_buf[5] = next_hop.in6_u.u6_addr8[15];
351         } else {
352                 /* Unicast address needs to be resolved by NDP */
353                 if ( ( rc = ndp_resolve ( netdev, &next_hop, &ip6hdr->src,
354                                           ll_dest_buf ) ) != 0 ) {
355                         DBG ( "No entry for %s\n", inet6_ntoa ( next_hop ) );
356                         goto err;
357                 }
358         }
359
360         /* Transmit packet */
361         return net_tx ( iobuf, netdev, &ipv6_protocol, ll_dest );
362
363   err:
364         free_iob ( iobuf );
365         return rc;
366 }
367
368 /**
369  * Process next IP6 header
370  *
371  * @v iobuf     I/O buffer
372  * @v nxt_hdr   Next header number
373  * @v src       Source socket address
374  * @v dest      Destination socket address
375  * @v netdev    Net device the packet arrived on
376  * @v phcsm Partial checksum over the IPv6 psuedo-header.
377  *
378  * Refer http://www.iana.org/assignments/ipv6-parameters for the numbers
379  */
380 static int ipv6_process_nxt_hdr ( struct io_buffer *iobuf, uint8_t nxt_hdr,
381                 struct sockaddr_tcpip *src, struct sockaddr_tcpip *dest,
382                 struct net_device *netdev, uint16_t phcsm ) {
383         switch ( nxt_hdr ) {
384         case IP6_HOPBYHOP:
385         case IP6_ROUTING:
386         case IP6_FRAGMENT:
387         case IP6_AUTHENTICATION:
388         case IP6_DEST_OPTS:
389         case IP6_ESP:
390                 DBG ( "Function not implemented for header %d\n", nxt_hdr );
391                 return -ENOSYS;
392         case IP6_ICMP6:
393                 return icmp6_rx ( iobuf, src, dest, netdev, phcsm );
394         case IP6_NO_HEADER:
395                 DBG ( "No next header\n" );
396                 return 0;
397         }
398         /* Next header is not a IPv6 extension header */
399         return tcpip_rx ( iobuf, nxt_hdr, src, dest, phcsm );
400 }
401
402 /**
403  * Process incoming IP6 packets
404  *
405  * @v iobuf             I/O buffer
406  * @v netdev            Network device
407  * @v ll_source         Link-layer source address
408  *
409  * This function processes a IPv6 packet
410  */
411 static int ipv6_rx ( struct io_buffer *iobuf,
412                      __unused struct net_device *netdev,
413                      __unused const void *ll_source ) {
414
415         struct ip6_header *ip6hdr = iobuf->data;
416         union {
417                 struct sockaddr_in6 sin6;
418                 struct sockaddr_tcpip st;
419         } src, dest;
420         uint16_t phcsm = 0;
421
422         /* Sanity check */
423         if ( iob_len ( iobuf ) < sizeof ( *ip6hdr ) ) {
424                 DBG ( "Packet too short (%zd bytes)\n", iob_len ( iobuf ) );
425                 goto drop;
426         }
427
428         /* Print IP6 header for debugging */
429         /* ipv6_dump ( ip6hdr ); */
430
431         /* Check header version */
432         if ( ( ntohl( ip6hdr->ver_traffic_class_flow_label ) & 0xf0000000 ) != 0x60000000 ) {
433                 DBG ( "Invalid protocol version\n" );
434                 goto drop;
435         }
436
437         /* Check the payload length */
438         if ( ntohs ( ip6hdr->payload_len ) > iob_len ( iobuf ) ) {
439                 DBG ( "Inconsistent packet length (%d bytes)\n",
440                         ip6hdr->payload_len );
441                 goto drop;
442         }
443
444         /* Ignore the traffic class and flow control values */
445
446         /* Construct socket address */
447         memset ( &src, 0, sizeof ( src ) );
448         src.sin6.sin_family = AF_INET6;
449         src.sin6.sin6_addr = ip6hdr->src;
450         memset ( &dest, 0, sizeof ( dest ) );
451         dest.sin6.sin_family = AF_INET6;
452         dest.sin6.sin6_addr = ip6hdr->dest;
453
454         /* Calculate the psuedo-header checksum before the IP6 header is
455          * stripped away. */
456         phcsm = ipv6_tx_csum ( iobuf, 0 );
457
458         /* Strip header */
459         iob_unput ( iobuf, iob_len ( iobuf ) - ntohs ( ip6hdr->payload_len ) -
460                                                         sizeof ( *ip6hdr ) );
461         iob_pull ( iobuf, sizeof ( *ip6hdr ) );
462
463         /* Send it to the transport layer */
464         return ipv6_process_nxt_hdr ( iobuf, ip6hdr->nxt_hdr, &src.st, &dest.st,
465                                       netdev, phcsm );
466
467   drop:
468         DBG ( "IP6 packet dropped\n" );
469         free_iob ( iobuf );
470         return -1;
471 }
472
473 /**
474  * Convert an IPv6 address to a string.
475  *
476  * @v in6   Address to convert to string.
477  *
478  * Converts an IPv6 address to a string, and applies zero-compression as needed
479  * to condense the address for easier reading/typing.
480  */
481 char * inet6_ntoa ( struct in6_addr in6 ) {
482         static char buf[40];
483         uint16_t *bytes = ( uint16_t* ) &in6;
484         size_t i = 0, longest = 0, tmp = 0, long_idx = ~0;
485         
486         /* ::0 */
487         if ( IP6_EQUAL ( in6, ip6_none ) ) {
488                 tmp = sprintf ( buf, "::0" );
489                 buf[tmp] = 0;
490                 return buf;
491         }
492
493         /* Determine the longest string of zeroes for zero-compression. */
494         for ( ; i < 8; i++ ) {
495                 if ( !bytes[i] )
496                         tmp++;
497                 else if(tmp > longest) {
498                         longest = tmp;
499                         long_idx = i - longest;
500                         
501                         tmp = 0;
502                 }
503         }
504         
505         /* Check for last word being zero. This will cause long_idx to be zero,
506          * which confuses the actual buffer fill code. */
507         if(tmp && (tmp > longest)) {
508                 longest = tmp;
509                 long_idx = 8 - longest;
510         }
511
512         /* Inject into the buffer. */
513         tmp = 0;
514         for ( i = 0; i < 8; i++ ) {
515                 /* Should we skip over a string of zeroes? */
516                 if ( i == long_idx ) {
517                         i += longest;
518                         tmp += sprintf( buf + tmp, ":" );
519
520                         /* Handle end-of-string. */
521                         if(i > 7)
522                                 break;
523                 }
524
525                 /* Insert this component of the address. */
526                 tmp += sprintf(buf + tmp, "%x", ntohs(bytes[i]));
527
528                 /* Add the next colon, if needed. */
529                 if ( i < 7 )
530                         tmp += sprintf( buf + tmp, ":" );
531         }
532
533         buf[tmp] = 0;
534
535         return buf;
536 }
537
538 /**
539  * Convert a string to an IPv6 address.
540  *
541  * @v in6   String to convert to an address.
542  */
543 int inet6_aton ( const char *cp, struct in6_addr *inp ) {
544         char convbuf[40];
545         char *tmp = convbuf, *next = convbuf;
546         size_t i = 0, len = strlen ( cp );
547         int ok;
548         char c;
549         
550         /* Verify a valid address. */
551         if ( ! len ) {
552                 return 0;
553         }
554         
555         for ( ; i < len; i++ ) {
556                 c = cp[i];
557                 
558                 ok = c == ':';
559                 ok = ok || ( ( c >= '0' ) && ( c <= '9' ) );
560                 ok = ok || ( ( c >= 'a' ) && ( c <= 'f' ) );
561                 ok = ok || ( ( c >= 'A' ) && ( c <= 'F' ) );
562                 
563                 if ( ! ok ) {
564                         return 0;
565                 }
566         }
567         
568         strcpy ( convbuf, cp );
569         
570         DBG ( "ipv6 converting %s to an in6_addr\n", cp );
571         
572         /* Handle the first part of the address (or all of it if no zero-compression. */
573         i = 0;
574         while ( ( next = strchr ( next, ':' ) ) ) {
575                 /* Cater for zero-compression. */
576                 if ( *tmp == ':' )
577                         break;
578                 
579                 /* Convert to integer. */
580                 inp->s6_addr16[i++] = htons( strtoul ( tmp, 0, 16 ) );
581                 
582                 *next++ = 0;
583                 tmp = next;
584         }
585         
586         /* Handle the case where no zero-compression is needed, but every word
587          * was filled in the address. */
588         if ( ( i == 7 ) && ( *tmp != ':' ) ) {
589                 inp->s6_addr16[i++] = htons( strtoul ( tmp, 0, 16 ) );
590         }
591         else
592         {
593                 /* Handle zero-compression now (go backwards). */
594                 i = 7;
595                 if ( i && ( *tmp == ':' ) ) {
596                         next = strrchr ( next, ':' );
597                         do
598                         {
599                                 tmp = next + 1;
600                                 *next-- = 0;
601         
602                                 /* Convert to integer. */
603                                 inp->s6_addr16[i--] = htons( strtoul ( tmp, 0, 16 ) );
604                         } while ( ( next = strrchr ( next, ':' ) ) );
605                 }
606         }
607         
608         return 1;
609 }
610
611 static const char * ipv6_ntoa ( const void *net_addr ) {
612         return inet6_ntoa ( * ( ( struct in6_addr * ) net_addr ) );
613 }
614
615 static int ipv6_check ( struct net_device *netdev, const void *net_addr ) {
616         const struct in6_addr *address = net_addr;
617         struct ipv6_miniroute *miniroute;
618
619         list_for_each_entry ( miniroute, &miniroutes, list ) {
620                 if ( ( miniroute->netdev == netdev ) &&
621                      ( ! memcmp ( &miniroute->address, address, 16 ) ) ) {
622                         /* Found matching address */
623                         return 0;
624                 }
625         }
626         return -ENOENT;
627 }
628
629 /** IPv6 protocol */
630 struct net_protocol ipv6_protocol __net_protocol = {
631         .name = "IPV6",
632         .net_proto = htons ( ETH_P_IPV6 ),
633         .net_addr_len = sizeof ( struct in6_addr ),
634         .rx = ipv6_rx,
635         .ntoa = ipv6_ntoa,
636 };
637
638 /** IPv6 TCPIP net protocol */
639 struct tcpip_net_protocol ipv6_tcpip_protocol __tcpip_net_protocol = {
640         .name = "IPv6",
641         .sa_family = AF_INET6,
642         .tx = ipv6_tx,
643 };
644
645 /** IPv6 ICMPv6 protocol, for NDP */
646 struct icmp6_net_protocol ipv6_icmp6_protocol __icmp6_net_protocol = {
647         .net_protocol = &ipv6_protocol,
648         .check = ipv6_check,
649 };
650