1 /******************************************************************************
2 * Copyright (c) 2013 IBM Corporation
4 * This program and the accompanying materials
5 * are made available under the terms of the BSD License
6 * which accompanies this distribution, and is available at
7 * http://www.opensource.org/licenses/bsd-license.php
10 * IBM Corporation - initial implementation
11 *****************************************************************************/
18 #include <sys/socket.h>
19 #include <netlib/ethernet.h>
20 #include <netlib/ipv6.h>
21 #include <netlib/icmpv6.h>
22 #include <netlib/ndp.h>
23 #include <netlib/udp.h>
28 #define dprintf(_x ...) do { printf(_x); } while (0)
30 #define dprintf(_x ...)
33 /****************************** PROTOTYPES *******************************/
34 int8_t ip6addr_add (struct ip6addr_list_entry *new_address);
35 static void ipv6_init(int fd);
36 static int ip6_is_multicast (ip6_addr_t * ip);
38 /****************************** LOCAL VARIABLES **************************/
40 /* List of Ipv6 Addresses */
41 static struct ip6addr_list_entry *first_ip6;
42 static struct ip6addr_list_entry *last_ip6;
44 /* Own IPv6 address */
45 static struct ip6addr_list_entry *own_ip6;
47 /* All nodes link-local address */
48 struct ip6addr_list_entry all_nodes_ll;
50 /* Null IPv6 address */
51 static ip6_addr_t null_ip6;
53 /* helper variables */
54 static uint8_t null_mac[] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
56 struct ip6_config ip6_state;
58 /****************************** IMPLEMENTATION ***************************/
61 * IPv6: Set the own IPv6 address.
63 * @param fd Socket descriptor
64 * @param _own_ip client IPv6 address (e.g. ::1)
66 void set_ipv6_address(int fd, ip6_addr_t *_own_ip6)
68 struct ip6addr_list_entry *ile;
70 own_ip6 = malloc (sizeof(struct ip6addr_list_entry));
72 /* If no address was passed as a parameter generate a link-local
73 * address from our MAC address.*/
75 memcpy(&(own_ip6->addr.addr),
76 ip6_create_ll_address(get_mac_address()),
79 memcpy (&(own_ip6->addr.addr), _own_ip6, 16);
81 /* Add to our list of IPv6 addresses */
82 ip6addr_add (own_ip6);
87 * Check whether we've got a non-link-local address during
88 * ipv6_init() and use that as preferred address if possible
90 if (_own_ip6 == NULL) {
91 for (ile = first_ip6; ile != NULL ; ile = ile->next) {
92 if (!ip6_is_multicast(&ile->addr) &&
93 !ip6_is_linklocal(&ile->addr)) {
102 * IPv6: Get pointer to own IPv6 address.
104 * @return pointer to client IPv6 address (e.g. ::1)
106 ip6_addr_t *get_ipv6_address(void)
108 return (ip6_addr_t *) &(own_ip6->addr);
112 * IPv6: Search for IPv6 address in list
114 * @return 0 - IPv6 address is not in list
115 * 1 - IPv6 address is in list
117 static int8_t find_ip6addr(ip6_addr_t *ip)
119 struct ip6addr_list_entry *n = NULL;
124 for (n = first_ip6; n != NULL ; n=n->next)
125 if (ip6_cmp (&(n->addr), ip))
126 return 1; /* IPv6 address is in our list*/
128 return 0; /* not one of our IPv6 addresses*/
132 * NET: Handles IPv6-packets
134 * @param fd - Socket descriptor
135 * @param ip6_packet - Pointer to IPv6 header
136 * @param packetsize - Size of Ipv6 packet
137 * @return ERROR - -1 if packet is too small or unknown protocol
138 * return value of handle_udp
143 int8_t handle_ipv6(int fd, uint8_t * ip6_packet, uint32_t packetsize)
146 struct ip6hdr *ip6 = NULL;
147 ip6 = (struct ip6hdr *) ip6_packet;
149 /* Only handle packets which are for us */
150 if (! find_ip6addr(&(ip6->dst)))
153 if (packetsize < sizeof(struct ip6hdr))
154 return -1; // packet is too small
158 return handle_udp (fd, ip6_packet + sizeof (struct ip6hdr),
161 return handle_icmpv6 (fd, (struct ethhdr *) ip6_packet - sizeof(struct ethhdr),
165 return -1; // unknown protocol
169 * NET: Creates IPv6-packet. Places IPv6-header in a packet and fills it
170 * with corresponding information.
172 * Use this function with similar functions for other network layers
173 * (fill_ethhdr, fill_udphdr, fill_dnshdr, fill_btphdr).
175 * @param packet Points to the place where IPv6-header must be placed.
176 * @param packetsize Size of payload (i.e. excluding ethhdr and ip6hdr)
177 * @param ip_proto Type of the next level protocol (e.g. UDP).
178 * @param ip6_src Sender IPv6 address
179 * @param ip6_dst Receiver IPv6 address
187 void fill_ip6hdr(uint8_t * packet, uint16_t packetsize,
188 uint8_t ip_proto, ip6_addr_t *ip6_src, ip6_addr_t *ip6_dst)
190 struct ip6hdr * ip6h = (struct ip6hdr *) packet;
192 ip6h->ver_tc_fl = 6 << 28; // set version to 6
193 ip6h->pl = packetsize; // IPv6 payload size
196 memcpy (&(ip6h->src), ip6_src, IPV6_ADDR_LENGTH);
197 memcpy (&(ip6h->dst), ip6_dst, IPV6_ADDR_LENGTH);
201 * NET: For a given MAC calculates EUI64-Identifier.
202 * See RFC 4291 "IP Version 6 Addressing Architecture"
205 uint64_t mac2eui64(const uint8_t *mac)
210 memcpy (eui64id, mac, 3);
211 memcpy (eui64id + 5, mac + 3, 3);
215 memcpy(&retid, eui64id, 8);
220 * NET: create link-local IPv6 address
222 * @param own_mac MAC of NIC
223 * @return ll_addr pointer to newly created link-local address
225 ip6_addr_t *ip6_create_ll_address(const uint8_t *own_mac)
229 ll_addr = malloc (sizeof (struct ip6addr_list_entry));
230 memset (ll_addr, 0, IPV6_ADDR_LENGTH);
231 ll_addr->part.prefix |= IPV6_LL_PREFIX;
232 ll_addr->part.interface_id |= mac2eui64((uint8_t *) own_mac);
238 * NET: check if we already have an address with the same prefix.
239 * @param struct ip6_addr_list_entry *ip6
240 * @return true or false
242 int8_t unknown_prefix(ip6_addr_t *ip)
244 struct ip6addr_list_entry *node;
246 for( node = first_ip6; node != NULL; node=node->next )
247 if( node->addr.part.prefix == ip->part.prefix )
248 return 0; /* address is one of ours */
250 return 1; /* prefix not yet in our list */
254 * NET: Create empty element for prefix list and return a pointer to it;
255 * @return NULL - malloc failed
256 * ! NULL - pointer to new prefix_info
258 struct prefix_info *ip6_create_prefix_info()
260 struct prefix_info *prfx_info;
262 prfx_info = malloc (sizeof(struct prefix_info));
270 * NET: create a new IPv6 address with a given network prefix
271 * and add it to our IPv6 address list
273 * @param ip6_addr prefix (as received in RA)
274 * @return NULL - pointer to new ip6addr_list entry
276 void *ip6_prefix2addr(ip6_addr_t prefix)
278 struct ip6addr_list_entry *new_address;
279 uint64_t interface_id;
281 new_address = malloc (sizeof(struct ip6addr_list_entry));
285 /* fill new addr struct */
286 /* extract prefix from Router Advertisement */
287 memcpy (&(new_address->addr.part.prefix), &prefix, 8 );
289 /* interface id is generated from MAC address */
290 interface_id = mac2eui64 (get_mac_address());
291 memcpy (&(new_address->addr.part.interface_id), &interface_id, 8);
297 * NET: add new IPv6 adress to list
299 * @param ip6_addr *new_address
300 * @return 0 - passed pointer = NULL;
303 int8_t ip6addr_add(struct ip6addr_list_entry *new_address)
305 struct ip6addr_list_entry *solicited_node;
308 if (new_address == NULL)
311 /* Don't add the same address twice */
312 if (find_ip6addr (&(new_address->addr)))
315 /* If address is a unicast address, we also have to process packets
316 * for its solicited-node multicast address.
317 * See RFC 2373 - IP Version 6 Adressing Architecture */
318 if (! ip6_is_multicast(&(new_address->addr))) {
321 solicited_node = malloc(sizeof(struct ip6addr_list_entry));
322 if (! solicited_node)
325 solicited_node->addr.part.prefix = IPV6_SOLIC_NODE_PREFIX;
326 solicited_node->addr.part.interface_id = IPV6_SOLIC_NODE_IFACE_ID;
327 solicited_node->addr.addr[13] = new_address->addr.addr[13];
328 solicited_node->addr.addr[14] = new_address->addr.addr[14];
329 solicited_node->addr.addr[15] = new_address->addr.addr[15];
330 ip6addr_add (solicited_node);
333 if (NULL == first_ip6)
334 first_ip6 = new_address;
335 last_ip6->next = new_address;
336 last_ip6 = new_address;
337 last_ip6->next = NULL;
339 return 1; /* no error */
343 * NET: Initialize IPv6
345 * @param fd socket fd
347 static void ipv6_init(int fd)
351 send_ip = &send_ipv6;
353 /* Address configuration parameters */
354 ip6_state.managed_mode = 0;
356 /* Null IPv6 address */
357 null_ip6.part.prefix = 0;
358 null_ip6.part.interface_id = 0;
360 /* Multicast addresses */
361 all_nodes_ll.addr.part.prefix = 0xff02000000000000;
362 all_nodes_ll.addr.part.interface_id = 1;
363 ip6addr_add(&all_nodes_ll);
367 send_router_solicitation (fd);
368 for(i=0; i < 4 && !is_ra_received(); i++) {
369 set_timer(TICKS_SEC);
372 if (is_ra_received())
374 } while (get_timer() > 0);
379 * NET: compare IPv6 adresses
381 * @param ip6_addr ip_1
382 * @param ip6_addr ip_2
384 int8_t ip6_cmp(ip6_addr_t *ip_1, ip6_addr_t *ip_2)
386 return ((int8_t) !memcmp( &(ip_1->addr[0]), &(ip_2->addr[0]),
391 * NET: Calculate checksum over IPv6 header and upper-layer protocol
392 * (e.g. UDP or ICMPv6)
394 * @param *ip - pointer to IPv6 address
395 * @return true or false
397 int ip6_is_multicast(ip6_addr_t * ip)
399 return ip->addr[0] == 0xFF;
403 * NET: Generate multicast MAC address from IPv6 address
404 * (e.g. UDP or ICMPv6)
406 * @param *ip - pointer to IPv6 address
407 * @param *mc_mac pointer to an array with 6 bytes (for the MAC address)
408 * @return pointer to Multicast MAC address
410 static uint8_t *ip6_to_multicast_mac(ip6_addr_t * ip, uint8_t *mc_mac)
414 memcpy (mc_mac+2, (uint8_t *) &(ip->addr)+12, 4);
420 * NET: calculate checksum over IPv6 header and upper-layer protocol
421 * (e.g. UDP or ICMPv6)
423 * @param struct ip6hdr *ip6h - pointer to IPv6 header
424 * @param unsigned short *packet - pointer to header of upper-layer
426 * @param int words - number of words (as in 2 bytes)
427 * starting from *packet
430 static unsigned short ip6_checksum(struct ip6hdr *ip6h, unsigned short *packet,
434 unsigned long checksum;
435 struct ip6hdr pseudo_ip6h;
436 unsigned short *pip6h;
438 memcpy (&pseudo_ip6h, ip6h, sizeof(struct ip6hdr));
439 pseudo_ip6h.hl = ip6h->nh;
440 pseudo_ip6h.ver_tc_fl = 0;
442 pip6h = (unsigned short *) &pseudo_ip6h;
444 for (checksum = 0; words > 0; words--) {
445 checksum += *packet++;
449 for (i = 0; i < 20; i++) {
450 checksum += *pip6h++;
453 checksum = (checksum >> 16) + (checksum & 0xffff);
454 checksum += (checksum >> 16);
460 * NET: Handles IPv6-packets
462 * @param fd socket fd
463 * @param ip6_packet Pointer to IPv6 header in packet
464 * @param packetsize Size of IPv6 packet
465 * @return -1 == ERRROR
466 * return of handle_udp() or handle_icmp6()
471 int send_ipv6(int fd, void* buffer, int len)
476 struct icmp6hdr *icmp6h;
478 uint8_t *mac_addr, mac[6];
482 ip6h = (struct ip6hdr *) buffer;
483 udph = (struct udphdr *) ((uint8_t *) ip6h + sizeof (struct ip6hdr));
484 icmp6h = (struct icmp6hdr *) ((uint8_t *) ip6h + sizeof (struct ip6hdr));
486 memcpy(&ip_dst, &ip6h->dst, 16);
488 if(len + sizeof(struct ethhdr) > 1500)
491 if ( ip6_cmp (&ip6h->src, &null_ip6))
492 memcpy (&(ip6h->src), get_ipv6_address(), IPV6_ADDR_LENGTH);
494 if (ip6h->nh == 17) {//UDP
495 udph->uh_sum = ip6_checksum (ip6h, (unsigned short *) udph ,
497 /* As per RFC 768, if the computed checksum is zero,
498 * it is transmitted as all ones (the equivalent in
499 * one's complement arithmetic).
501 if (udph->uh_sum == 0)
502 udph->uh_sum = ~udph->uh_sum;
504 else if (ip6h->nh == 0x3a) //ICMPv6
505 icmp6h->checksum = ip6_checksum (ip6h,
506 (unsigned short *) icmp6h,
509 n = find_neighbor (&ip_dst);
511 // If address is a multicast address, create a proper mac address
512 if (ip6_is_multicast (&ip_dst)) {
513 mac_addr = ip6_to_multicast_mac (&ip_dst, mac);
516 // Check if the MAC address is already cached
518 if (memcmp(n->mac, null_mac, ETH_ALEN) != 0)
519 memcpy (mac_addr, &(n->mac), ETH_ALEN); /* found it */
522 n = malloc(sizeof(struct neighbor));
523 memcpy(&(n->ip.addr[0]), &ip_dst, 16);
524 n->status = NB_PROBE;
529 if (! memcmp (mac_addr, &null_mac, 6)) {
530 if (n->eth_len == 0) {
531 send_neighbour_solicitation (fd, &ip_dst);
533 // Store the packet until we know the MAC address
534 memset(n->eth_frame, 0, 1500);
535 fill_ethhdr (n->eth_frame,
536 htons(ETHERTYPE_IPv6),
539 memcpy (&(n->eth_frame[sizeof(struct ethhdr)]),
542 set_timer(TICKS_SEC);
545 } while (get_timer() > 0);
550 fill_ethhdr (n->eth_frame, htons(ETHERTYPE_IPv6), get_mac_address(),
552 memcpy (&(n->eth_frame[sizeof(struct ethhdr)]), buffer, len);
553 return send_ether (fd, n->eth_frame, len + sizeof(struct ethhdr));
556 static int check_colons(const char *str)
562 dprintf("str : %s\n",str);
563 pch = strchr(str, ':');
566 pch = strchr(pch+1, ':');
570 col--; /* Its part of double colon */
575 dprintf("The number of col : %d \n",col);
576 dprintf("The number of dcol : %d \n",dcol);
578 if((dcol > 1) || /* Cannot have 2 "::" */
579 ((dcol == 1) && (col > 5)) || /* Too many ':'s */
580 ((dcol == 0) && (col != 7)) ) { /* Too few ':'s */
581 dprintf(" exiting for check_colons \n");
588 static int ipv6str_to_bytes(const char *str, char *ip)
593 uint32_t cnt = 0, len;
595 dprintf("str : %s \n",str);
598 if (cnt > 15 || !isxdigit(*str)){
601 if ((pos = strchr(str, ':')) != NULL) {
602 len = (int16_t) (pos - str);
603 dprintf("\t len is : %d \n",len);
606 strncpy(block, str, len);
608 dprintf("\t str : %s \n",str);
609 dprintf("\t block : %s \n",block);
612 strncpy(block, str, 4);
614 dprintf("\t str : %s \n",str);
615 dprintf("\t block : %s \n",block);
616 str += strlen(block);
618 res = strtol(block, NULL, 16);
619 dprintf("\t res : %x \n",res);
620 if ((res > 0xFFFF) || (res < 0))
622 ip[cnt++] = (res & 0xFF00) >> 8;
623 ip[cnt++] = (res & 0x00FF);
629 dprintf("cnt : %d\n",cnt);
633 int str_to_ipv6(const char *str, uint8_t *ip)
638 char tmp[30], buf[16];
642 if(!check_colons(str))
645 if ((ptr = strstr(str, "::")) != NULL) {
646 /* Handle the ::1 IPv6 loopback */
647 if(!strcmp(str,"::1")) {
652 dprintf(" len : %d \n",len);
653 if (len >= sizeof(tmp))
655 strncpy(tmp, str, len);
659 i = ipv6str_to_bytes(ptr, buf);
663 #if defined(ARGS_DEBUG)
665 dprintf("=========== bottom part i : %d \n",i);
667 dprintf("%02x \t",buf[j]);
670 /* Copy the bottom part i.e bytes following "::" */
671 memcpy(ip+(16-i), buf, i);
673 k = ipv6str_to_bytes(tmp, buf);
677 #if defined(ARGS_DEBUG)
678 dprintf("=========== top part k : %d \n",k);
680 printf("%02x \t",buf[j]);
683 /* Copy the top part i.e bytes before "::" */
685 #if defined(ARGS_DEBUG)
688 dprintf("%02x \t",ip[j]);
692 i = ipv6str_to_bytes(str, (char *)ip);
697 void ipv6_to_str(const uint8_t *ip, char *str)
700 uint8_t byte_even, byte_odd;
701 char *consec_zero, *strptr;
704 for (i = 0; i < 16; i+=2) {
708 sprintf(str, "%s%x%02x", str, byte_even, byte_odd);
710 sprintf(str, "%s%x", str, byte_odd);
718 consec_zero = strstr(strptr, "0:0:");
720 len = consec_zero - strptr;
723 else if (strptr[len-1] == ':')
726 strptr = consec_zero + 2;
728 } while (consec_zero);
730 len = consec_zero - str;
736 strptr = consec_zero + 4;
738 if (!strncmp(strptr, "0:", 2))
744 if (!strcmp(str, "::0"))