common/VIL/l2l3_stack/lib_icmpv6.c

   1 /*
   2 // Copyright (c) 2017 Intel Corporation
   3 //
   4 // Licensed under the Apache License, Version 2.0 (the "License");
   5 // you may not use this file except in compliance with the License.
   6 // You may obtain a copy of the License at
   7 //
   8 //      http://www.apache.org/licenses/LICENSE-2.0
   9 //
  10 // Unless required by applicable law or agreed to in writing, software
  11 // distributed under the License is distributed on an "AS IS" BASIS,
  12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13 // See the License for the specific language governing permissions and
  14 // limitations under the License.
  15 */
  16 /*      Santosh Sethupathi*/
  17
  18 #include "lib_icmpv6.h"
  19
  20 static void print_pkt(uint8_t *rd)
  21 {
  22         int i = 0, j = 0;
  23
  24         printf("Packet Contents:\n");
  25
  26         for (i = 0; i < 20; i++) {
  27                 for (j = 0; j < 20; j++)
  28                         printf("%02x ", rd[(20 * i) + j]);
  29
  30                 printf("\n");
  31         }
  32 }
  33
  34 static uint16_t icmpv6_ipv6_nd_checksum(struct rte_mbuf *pkt)
  35 {
  36         struct ether_hdr *eth_h;
  37         struct ipv6_hdr *ipv6_h;
  38         struct icmpv6_hdr *icmpv6_h;
  39
  40         size_t tmplen, offset;
  41         uint8_t *tmppacket, *tpacket;
  42
  43         eth_h = rte_pktmbuf_mtod(pkt, struct ether_hdr *);
  44         ipv6_h = (struct ipv6_hdr *)((char *)eth_h + sizeof(struct ether_hdr));
  45         icmpv6_h =
  46                         (struct icmpv6_hdr *)((char *)ipv6_h + sizeof(struct ipv6_hdr));
  47
  48         uint32_t payloadlen = 0x20;
  49         payloadlen = rte_bswap32(payloadlen);
  50
  51         tmplen = 40 + sizeof(struct icmpv6_hdr) + sizeof(struct icmpv6_nd_hdr);
  52         tmplen = RTE_CACHE_LINE_ROUNDUP(tmplen);
  53         tmppacket = rte_zmalloc(NULL, tmplen, RTE_CACHE_LINE_SIZE);
  54         tpacket = tmppacket;
  55
  56         offset = 16;
  57         memcpy(tpacket, &ipv6_h->src_addr[0], offset);
  58         tpacket += offset;
  59         memcpy(tpacket, &ipv6_h->dst_addr[0], offset);
  60         tpacket += offset;
  61         *tpacket = 0;
  62         tpacket++;
  63         *tpacket = 0;
  64         tpacket++;
  65         *tpacket = 0;
  66         tpacket++;
  67         memcpy(tpacket, &ipv6_h->proto, 1);
  68         tpacket++;
  69         memcpy(tpacket, &payloadlen, 4);
  70         tpacket += 4;
  71         memcpy(tpacket, icmpv6_h,
  72                                  sizeof(struct icmpv6_hdr) + sizeof(struct icmpv6_nd_hdr));
  73
  74         if (ARPICMP_DEBUG)
  75                 print_pkt(tmppacket);
  76
  77         return rte_raw_cksum(tmppacket, tmplen);
  78 }
  79
  80 static uint16_t icmpv6_ipv6_echo_checksum(struct rte_mbuf *pkt)
  81 {
  82         struct ether_hdr *eth_h;
  83         struct ipv6_hdr *ipv6_h;
  84         struct icmpv6_hdr *icmpv6_h;
  85
  86         size_t tmplen, offset;
  87         uint8_t *tmppacket, *tpacket;
  88
  89         eth_h = rte_pktmbuf_mtod(pkt, struct ether_hdr *);
  90         ipv6_h = (struct ipv6_hdr *)((char *)eth_h + sizeof(struct ether_hdr));
  91         icmpv6_h =
  92                         (struct icmpv6_hdr *)((char *)ipv6_h + sizeof(struct ipv6_hdr));
  93
  94         uint32_t payloadlen = rte_bswap16(ipv6_h->payload_len);
  95         uint32_t payloadlen_swap = rte_bswap32(payloadlen);
  96
  97         if (ARPICMP_DEBUG)
  98                 printf("%s: payloadlen: %u\n", __FUNCTION__, payloadlen);
  99
 100         tmplen = 40 + payloadlen;
 101         tmplen = RTE_CACHE_LINE_ROUNDUP(tmplen);
 102         tmppacket = rte_zmalloc(NULL, tmplen, RTE_CACHE_LINE_SIZE);
 103         tpacket = tmppacket;
 104
 105         offset = 16;
 106         memcpy(tpacket, &ipv6_h->src_addr[0], offset);
 107         tpacket += offset;
 108         memcpy(tpacket, &ipv6_h->dst_addr[0], offset);
 109         tpacket += offset;
 110         *tpacket = 0;
 111         tpacket++;
 112         *tpacket = 0;
 113         tpacket++;
 114         *tpacket = 0;
 115         tpacket++;
 116         memcpy(tpacket, &ipv6_h->proto, 1);
 117         tpacket++;
 118         memcpy(tpacket, &payloadlen_swap, 4);
 119         tpacket += 4;
 120         memcpy(tpacket, icmpv6_h, payloadlen);
 121
 122         if (ARPICMP_DEBUG)
 123                 print_pkt(tmppacket);
 124
 125         return rte_raw_cksum(tmppacket, tmplen);
 126 }
 127
 128 void process_icmpv6_pkt(struct rte_mbuf *pkt, l2_phy_interface_t *port)
 129 {
 130
 131         struct ether_hdr *eth_h;
 132         struct ipv6_hdr *ipv6_h;
 133         struct icmpv6_hdr *icmpv6_h;
 134         struct icmpv6_nd_hdr *icmpv6_nd_h;
 135         uint8_t ipv6_addr[16];
 136         uint8_t i = 0;
 137         uint8_t req_tipv6[16];
 138         /* To drop the packet */
 139
 140         if (port == NULL) {
 141                 printf("port is NULL");
 142                 return;
 143         } else if (port->ipv6_list == NULL) {
 144                 printf("IPV6 address not configured on link\n");
 145                 return;
 146         }
 147
 148         eth_h = rte_pktmbuf_mtod(pkt, struct ether_hdr *);
 149         ipv6_h = (struct ipv6_hdr *)((char *)eth_h + sizeof(struct ether_hdr));
 150         icmpv6_h =
 151                         (struct icmpv6_hdr *)((char *)ipv6_h + sizeof(struct ipv6_hdr));
 152
 153         if ((icmpv6_h->icmpv6_type == ICMPV6_ECHO_REQUEST)
 154                         && (icmpv6_h->icmpv6_code == 0)) {
 155                 for (i = 0; i < 16; i++) {
 156                         ipv6_addr[i] = ipv6_h->src_addr[i];
 157                 }
 158
 159                 ether_addr_copy(&eth_h->s_addr, &eth_h->d_addr);
 160                 ether_addr_copy((struct ether_addr *)&port->macaddr[0],
 161                                 &eth_h->s_addr);
 162
 163                 for (i = 0; i < 16; i++)
 164                         ipv6_h->src_addr[i] = ipv6_h->dst_addr[i];
 165                 for (i = 0; i < 16; i++)
 166                         ipv6_h->dst_addr[i] = ipv6_addr[i];
 167
 168                 icmpv6_h->icmpv6_type = ICMPV6_ECHO_REPLY;
 169                 icmpv6_h->icmpv6_cksum = 0;
 170                 icmpv6_h->icmpv6_cksum = ~icmpv6_ipv6_echo_checksum(pkt);
 171                 port->transmit_bulk_pkts(port, &pkt, 1);
 172
 173                 return;
 174         } else if ((icmpv6_h->icmpv6_type == ICMPV6_ECHO_REPLY)
 175                          && (icmpv6_h->icmpv6_code == 0)) {
 176                 struct nd_key_ipv6 nd_key;
 177                 nd_key.port_id = port->pmdid;
 178                 for (i = 0; i < ND_IPV6_ADDR_SIZE; i++) {
 179                         nd_key.ipv6[i] = ipv6_h->src_addr[i];
 180
 181                 }
 182                 nd_key.filler1 = 0;
 183                 nd_key.filler2 = 0;
 184                 nd_key.filler3 = 0;
 185
 186                 /*Validate if key-value pair already exists in the hash table for ND IPv6 */
 187                 struct nd_entry_data *new_nd_data = retrieve_nd_entry(nd_key, DYNAMIC_ND);
 188                 if (new_nd_data == NULL) {
 189                         printf
 190                                         ("Received unsolicited ICMPv6 echo reply on port %d\n",
 191                                          nd_key.port_id);
 192                         for (i = 0; i < ND_IPV6_ADDR_SIZE; i += 2) {
 193                                 printf("%02X%02X ", nd_key.ipv6[i],
 194                                                          nd_key.ipv6[i + 1]);
 195                         }
 196                         return;
 197                 }
 198
 199                 new_nd_data->status = COMPLETE;
 200         } else if ((icmpv6_h->icmpv6_type == ICMPV6_NEIGHBOR_SOLICITATION)
 201                          && (icmpv6_h->icmpv6_code == 0)) {
 202
 203                 icmpv6_nd_h =
 204                                 (struct icmpv6_nd_hdr *)((char *)icmpv6_h +
 205                                                          sizeof(struct icmpv6_hdr));
 206                 struct ether_addr *src_hw_addr = &eth_h->s_addr;
 207                 uint8_t src_ipv6[16], dst_ipv6[16];
 208                 uint16_t multi_addr;
 209
 210                 for (i = 0; i < ND_IPV6_ADDR_SIZE; i++)
 211                         src_ipv6[i] = ipv6_h->src_addr[i];
 212
 213                 for (i = 0; i < ND_IPV6_ADDR_SIZE; i++)
 214                         dst_ipv6[i] = ipv6_h->dst_addr[i];
 215
 216                 multi_addr = dst_ipv6[0];
 217
 218                 /*  Check for Multicast Address */
 219                 if ((IPV6_MULTICAST & ((multi_addr << 8) | dst_ipv6[1]))
 220                                 || !memcmp(&port->macaddr[0], &eth_h->d_addr, 6)) {
 221
 222                         populate_nd_entry(src_hw_addr, src_ipv6, port->pmdid,
 223                                                 DYNAMIC_ND);
 224
 225                         /* build a Neighbor Advertisement message */
 226                         for (i = 0; i < ND_IPV6_ADDR_SIZE; i++)
 227                                 req_tipv6[i] = icmpv6_nd_h->target_ipv6[i];
 228
 229                         if (!memcmp
 230                                         (&req_tipv6[0],
 231                                          &((ipv6list_t *) port->ipv6_list)->ipaddr[0],
 232                                          16)) {
 233
 234                                 ether_addr_copy(&eth_h->s_addr, &eth_h->d_addr);
 235                                 ether_addr_copy((struct ether_addr *)&port->
 236                                                 macaddr[0], &eth_h->s_addr);
 237
 238                                 /* set sender mac address */
 239                                 for (i = 0; i < ND_IPV6_ADDR_SIZE; i++)
 240                                         ipv6_h->dst_addr[i] =
 241                                                         ipv6_h->src_addr[i];
 242                                 for (i = 0; i < ND_IPV6_ADDR_SIZE; i++)
 243                                         ipv6_h->src_addr[i] = req_tipv6[i];
 244                                 icmpv6_h->icmpv6_type =
 245                                                 ICMPV6_NEIGHBOR_ADVERTISEMENT;
 246                                 icmpv6_nd_h->type = e_Target_Link_Layer_Address;
 247                                 icmpv6_nd_h->length = 1;
 248                                 memcpy(&icmpv6_nd_h->link_layer_addr[0],
 249                                                          &port->macaddr[0], 6);
 250                                 icmpv6_nd_h->icmpv6_reserved = 0;
 251                                 icmpv6_nd_h->icmpv6_reserved |=
 252                                                 rte_cpu_to_be_32
 253                                                 (NEIGHBOR_ROUTER_OVERRIDE_SET);
 254
 255                                 icmpv6_h->icmpv6_cksum = 0;
 256                                 icmpv6_h->icmpv6_cksum =
 257                                                 ~icmpv6_ipv6_nd_checksum(pkt);
 258
 259                                 port->transmit_bulk_pkts(port, &pkt, 1);
 260
 261                         } else if (ARPICMP_DEBUG) {
 262                                 printf
 263                                                 ("............Some one else is the target host here !!!\n");
 264                         }
 265
 266                         return;
 267                 } else {
 268                         if (ARPICMP_DEBUG) {
 269                                 printf
 270                                                 ("...............Malformed ND Solicitation message!!!\n");
 271                         }
 272                 }
 273
 274         } else if ((icmpv6_h->icmpv6_type == ICMPV6_NEIGHBOR_ADVERTISEMENT)
 275                          && (icmpv6_h->icmpv6_code == 0)) {
 276                 struct ether_addr *src_hw_addr = &eth_h->s_addr;
 277                 uint8_t ipv6[16];
 278                 for (i = 0; i < ND_IPV6_ADDR_SIZE; i++) {
 279                         ipv6[i] = ipv6_h->src_addr[i];
 280
 281                 }
 282                 populate_nd_entry(src_hw_addr, ipv6, port->pmdid, DYNAMIC_ND);
 283         } else {
 284                 if (ARPICMP_DEBUG) {
 285                         printf("ICMPv6 Type %d Not Supported yet !!!\n",
 286                                                  icmpv6_h->icmpv6_type);
 287                 }
 288         }
 289
 290         rte_pktmbuf_free(pkt);
 291 }
 292
 293 struct rte_mbuf *request_icmpv6_echo(uint8_t ipv6[], l2_phy_interface_t *port)
 294 {
 295         struct ether_hdr *eth_h;
 296         struct ipv6_hdr *ipv6_h;
 297         struct icmpv6_hdr *icmpv6_h;
 298         struct icmpv6_info_hdr *icmpv6_info_h;
 299         int i;
 300         uint8_t *icmp_data;
 301
 302         struct rte_mbuf *icmpv6_pkt = lib_icmpv6_pkt;
 303         if (icmpv6_pkt == NULL) {
 304                 if (ARPICMP_DEBUG)
 305                         printf("Error allocating icmpv6_pkt rte_mbuf\n");
 306                 return NULL;
 307         }
 308
 309         eth_h = rte_pktmbuf_mtod(icmpv6_pkt, struct ether_hdr *);
 310
 311         ipv6_h = (struct ipv6_hdr *)((char *)eth_h + sizeof(struct ether_hdr));
 312         icmpv6_h =
 313                         (struct icmpv6_hdr *)((char *)ipv6_h + sizeof(struct ipv6_hdr));
 314         icmpv6_info_h =
 315                         (struct icmpv6_info_hdr *)((char *)icmpv6_h +
 316                                                          sizeof(struct icmpv6_hdr));
 317
 318         ether_addr_copy((struct ether_addr *)&port->macaddr[0], &eth_h->s_addr);
 319         eth_h->ether_type = rte_bswap16(0x86dd);
 320         for (i = 0; i < 6; i++) {
 321                 eth_h->d_addr.addr_bytes[i] = 0;
 322         }
 323
 324         ipv6_h->vtc_flow = rte_bswap32(0x60000000);
 325         ipv6_h->payload_len = rte_bswap16(64);
 326         ipv6_h->proto = 58;
 327         ipv6_h->hop_limits = 64;
 328
 329         for (i = 0; i < 16; i++) {
 330                 ipv6_h->src_addr[i] = 0x0;
 331                 ipv6_h->dst_addr[i] = ipv6[i];
 332         }
 333
 334         icmpv6_h->icmpv6_type = ICMPV6_ECHO_REQUEST;
 335         icmpv6_h->icmpv6_code = 0;
 336         icmpv6_info_h->icmpv6_ident = rte_bswap16(0x5151);
 337         icmpv6_info_h->icmpv6_seq_nb = rte_bswap16(0x1);
 338
 339         icmp_data = (uint8_t *) icmpv6_h + 8;
 340         for (i = 0; i < 56; i++) {
 341                 *icmp_data = i + 1;
 342                 icmp_data++;
 343         }
 344         icmpv6_h->icmpv6_cksum = 0;
 345         icmpv6_h->icmpv6_cksum = ~icmpv6_ipv6_echo_checksum(icmpv6_pkt);
 346
 347         icmpv6_pkt->pkt_len =
 348                         sizeof(struct ether_hdr) + sizeof(struct ipv6_hdr) + 64;
 349         icmpv6_pkt->data_len = icmpv6_pkt->pkt_len;
 350
 351         if (port)
 352                 port->transmit_single_pkt(port, icmpv6_pkt);
 353
 354         return icmpv6_pkt;
 355 }
 356
 357 struct rte_mbuf *request_nd(uint8_t ipv6[], l2_phy_interface_t *port)
 358 {
 359         struct ether_hdr *eth_h;
 360         struct ipv6_hdr *ipv6_h;
 361         struct icmpv6_hdr *icmpv6_h;
 362         struct icmpv6_nd_hdr *icmpv6_nd_h;
 363         int i;
 364
 365         struct rte_mbuf *icmpv6_pkt = lib_nd_pkt[port->pmdid];
 366         if (icmpv6_pkt == NULL) {
 367                 if (ARPICMP_DEBUG)
 368                         printf("Error allocating icmpv6_pkt rte_mbuf\n");
 369                 return NULL;
 370         }
 371
 372         uint8_t dst_ip[] = {255, 2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 255, 16, 100, 20};
 373         uint8_t dst_mac[] = {51,51,255, 16, 100, 20};
 374
 375         eth_h = rte_pktmbuf_mtod(icmpv6_pkt, struct ether_hdr *);
 376
 377         ipv6_h = (struct ipv6_hdr *)((char *)eth_h + sizeof(struct ether_hdr));
 378         icmpv6_h =
 379                         (struct icmpv6_hdr *)((char *)ipv6_h + sizeof(struct ipv6_hdr));
 380         icmpv6_nd_h =
 381                         (struct icmpv6_nd_hdr *)((char *)icmpv6_h +
 382                                                  sizeof(struct icmpv6_hdr));
 383
 384         ether_addr_copy((struct ether_addr *)&port->macaddr[0], &eth_h->s_addr);
 385         eth_h->ether_type = rte_bswap16(0x86dd);
 386
 387         for (i = 0; i < 6; i++) {
 388                 if (i < 3)
 389                         eth_h->d_addr.addr_bytes[i] = dst_mac[i];
 390                 else
 391                         eth_h->d_addr.addr_bytes[i] = ipv6[i];
 392         }
 393
 394         for (i=13; i<16; i++)
 395                 dst_ip[i] = ipv6[i];
 396
 397         uint8_t *addr = ((ipv6list_t *) (port->ipv6_list))->ipaddr;
 398
 399         ipv6_h->vtc_flow = rte_bswap32(0x60000000);
 400         ipv6_h->payload_len = rte_bswap16(32);
 401         ipv6_h->proto = 58;
 402         ipv6_h->hop_limits = 255;
 403
 404         for (i = 0; i < 16; i++) {
 405                 ipv6_h->src_addr[i] = *(addr + i);
 406                 ipv6_h->dst_addr[i] = dst_ip[i];
 407         }
 408
 409         icmpv6_h->icmpv6_type = ICMPV6_NEIGHBOR_SOLICITATION;
 410         icmpv6_h->icmpv6_code = 0;
 411
 412         icmpv6_nd_h->icmpv6_reserved = 0x0;
 413         icmpv6_nd_h->icmpv6_reserved |=
 414                         rte_cpu_to_be_32
 415                         (NEIGHBOR_ROUTER_OVERRIDE_SET);
 416
 417         for (i = 0; i < ND_IPV6_ADDR_SIZE; i++)
 418                 icmpv6_nd_h->target_ipv6[i] = ipv6[i];
 419         icmpv6_nd_h->type = e_Source_Link_Layer_Address;
 420         icmpv6_nd_h->length = 1;
 421         memcpy(&icmpv6_nd_h->link_layer_addr[0], &port->macaddr[0], 6);
 422
 423         icmpv6_h->icmpv6_cksum = 0;
 424         icmpv6_h->icmpv6_cksum = ~icmpv6_ipv6_nd_checksum(icmpv6_pkt);
 425         icmpv6_pkt->pkt_len =
 426                         sizeof(struct ether_hdr) + sizeof(struct ipv6_hdr) + 32;
 427         icmpv6_pkt->data_len = icmpv6_pkt->pkt_len;
 428
 429         if (port) {
 430                 port->transmit_single_pkt(port, icmpv6_pkt);
 431         }
 432
 433         return icmpv6_pkt;
 434 }