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);
 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                 for (i = 0; i < ND_IPV6_ADDR_SIZE; i++)
 213                         dst_ipv6[i] = ipv6_h->dst_addr[i];
 214
 215                 multi_addr = dst_ipv6[0];
 216
 217                 /*  Check for Multicast Address */
 218                 if ((IPV6_MULTICAST & ((multi_addr << 8) | dst_ipv6[1]))
 219                                 || !memcmp(&port->macaddr[0], &eth_h->d_addr, 6)) {
 220                         populate_nd_entry(src_hw_addr, src_ipv6, port->pmdid,
 221                                                 DYNAMIC_ND);
 222
 223                         /* build a Neighbor Advertisement message */
 224                         for (i = 0; i < ND_IPV6_ADDR_SIZE; i++)
 225                                 req_tipv6[i] = icmpv6_nd_h->target_ipv6[i];
 226
 227                         if (!memcmp
 228                                         (&req_tipv6[0],
 229                                          &((ipv6list_t *) port->ipv6_list)->ipaddr[0],
 230                                          16)) {
 231
 232                                 ether_addr_copy(&eth_h->s_addr, &eth_h->d_addr);
 233                                 ether_addr_copy((struct ether_addr *)&port->
 234                                                 macaddr[0], &eth_h->s_addr);
 235
 236                                 /* set sender mac address */
 237                                 for (i = 0; i < ND_IPV6_ADDR_SIZE; i++)
 238                                         ipv6_h->dst_addr[i] =
 239                                                         ipv6_h->src_addr[i];
 240                                 for (i = 0; i < ND_IPV6_ADDR_SIZE; i++)
 241                                         ipv6_h->src_addr[i] = req_tipv6[i];
 242                                 icmpv6_h->icmpv6_type =
 243                                                 ICMPV6_NEIGHBOR_ADVERTISEMENT;
 244                                 icmpv6_nd_h->type = e_Target_Link_Layer_Address;
 245                                 icmpv6_nd_h->length = 1;
 246                                 memcpy(&icmpv6_nd_h->link_layer_addr[0],
 247                                                          &port->macaddr[0], 6);
 248                                 icmpv6_nd_h->icmpv6_reserved = 0;
 249                                 icmpv6_nd_h->icmpv6_reserved |=
 250                                                 rte_cpu_to_be_32
 251                                                 (NEIGHBOR_ROUTER_OVERRIDE_SET);
 252
 253                                 icmpv6_h->icmpv6_cksum = 0;
 254                                 icmpv6_h->icmpv6_cksum =
 255                                                 ~icmpv6_ipv6_nd_checksum(pkt);
 256
 257                                 port->transmit_bulk_pkts(port, &pkt, 1);
 258
 259                         } else if (ARPICMP_DEBUG) {
 260                                 printf
 261                                                 ("............Some one else is the target host here !!!\n");
 262                         }
 263
 264                         return;
 265                 } else {
 266                         if (ARPICMP_DEBUG) {
 267                                 printf
 268                                                 ("...............Malformed ND Solicitation message!!!\n");
 269                         }
 270                 }
 271
 272         } else if ((icmpv6_h->icmpv6_type == ICMPV6_NEIGHBOR_ADVERTISEMENT)
 273                          && (icmpv6_h->icmpv6_code == 0)) {
 274                 struct ether_addr *src_hw_addr = &eth_h->s_addr;
 275                 uint8_t ipv6[16];
 276                 for (i = 0; i < ND_IPV6_ADDR_SIZE; i++) {
 277                         ipv6[i] = ipv6_h->src_addr[i];
 278
 279                 }
 280                 populate_nd_entry(src_hw_addr, ipv6, port->pmdid, DYNAMIC_ND);
 281         } else {
 282                 if (ARPICMP_DEBUG) {
 283                         printf("ICMPv6 Type %d Not Supported yet !!!\n",
 284                                                  icmpv6_h->icmpv6_type);
 285                 }
 286         }
 287
 288         rte_pktmbuf_free(pkt);
 289 }
 290
 291 struct rte_mbuf *request_icmpv6_echo(uint8_t ipv6[], l2_phy_interface_t *port)
 292 {
 293         struct ether_hdr *eth_h;
 294         struct ipv6_hdr *ipv6_h;
 295         struct icmpv6_hdr *icmpv6_h;
 296         struct icmpv6_info_hdr *icmpv6_info_h;
 297         int i;
 298         uint8_t *icmp_data;
 299
 300         struct rte_mbuf *icmpv6_pkt = lib_icmpv6_pkt;
 301         if (icmpv6_pkt == NULL) {
 302                 if (ARPICMP_DEBUG)
 303                         printf("Error allocating icmpv6_pkt rte_mbuf\n");
 304                 return NULL;
 305         }
 306
 307         eth_h = rte_pktmbuf_mtod(icmpv6_pkt, struct ether_hdr *);
 308
 309         ipv6_h = (struct ipv6_hdr *)((char *)eth_h + sizeof(struct ether_hdr));
 310         icmpv6_h =
 311                         (struct icmpv6_hdr *)((char *)ipv6_h + sizeof(struct ipv6_hdr));
 312         icmpv6_info_h =
 313                         (struct icmpv6_info_hdr *)((char *)icmpv6_h +
 314                                                          sizeof(struct icmpv6_hdr));
 315
 316         ether_addr_copy((struct ether_addr *)&port->macaddr[0], &eth_h->s_addr);
 317         eth_h->ether_type = rte_bswap16(0x86dd);
 318         for (i = 0; i < 6; i++) {
 319                 eth_h->d_addr.addr_bytes[i] = 0;
 320         }
 321
 322         ipv6_h->vtc_flow = rte_bswap32(0x60000000);
 323         ipv6_h->payload_len = rte_bswap16(64);
 324         ipv6_h->proto = 58;
 325         ipv6_h->hop_limits = 64;
 326
 327         for (i = 0; i < 16; i++) {
 328                 ipv6_h->src_addr[i] = 0x0;
 329                 ipv6_h->dst_addr[i] = ipv6[i];
 330         }
 331
 332         icmpv6_h->icmpv6_type = ICMPV6_ECHO_REQUEST;
 333         icmpv6_h->icmpv6_code = 0;
 334         icmpv6_info_h->icmpv6_ident = rte_bswap16(0x5151);
 335         icmpv6_info_h->icmpv6_seq_nb = rte_bswap16(0x1);
 336
 337         icmp_data = (uint8_t *) icmpv6_h + 8;
 338         for (i = 0; i < 56; i++) {
 339                 *icmp_data = i + 1;
 340                 icmp_data++;
 341         }
 342         icmpv6_h->icmpv6_cksum = 0;
 343         icmpv6_h->icmpv6_cksum = ~icmpv6_ipv6_echo_checksum(icmpv6_pkt);
 344
 345         icmpv6_pkt->pkt_len =
 346                         sizeof(struct ether_hdr) + sizeof(struct ipv6_hdr) + 64;
 347         icmpv6_pkt->data_len = icmpv6_pkt->pkt_len;
 348
 349         return icmpv6_pkt;
 350 }
 351
 352 struct rte_mbuf *request_nd(uint8_t ipv6[], l2_phy_interface_t *port)
 353 {
 354         struct ether_hdr *eth_h;
 355         struct ipv6_hdr *ipv6_h;
 356         struct icmpv6_hdr *icmpv6_h;
 357         struct icmpv6_nd_hdr *icmpv6_nd_h;
 358         int i;
 359
 360         struct rte_mbuf *icmpv6_pkt = lib_icmpv6_pkt;
 361         if (icmpv6_pkt == NULL) {
 362                 if (ARPICMP_DEBUG)
 363                         printf("Error allocating icmpv6_pkt rte_mbuf\n");
 364                 return NULL;
 365         }
 366
 367         eth_h = rte_pktmbuf_mtod(icmpv6_pkt, struct ether_hdr *);
 368
 369         ipv6_h = (struct ipv6_hdr *)((char *)eth_h + sizeof(struct ether_hdr));
 370         icmpv6_h =
 371                         (struct icmpv6_hdr *)((char *)ipv6_h + sizeof(struct ipv6_hdr));
 372         icmpv6_nd_h =
 373                         (struct icmpv6_nd_hdr *)((char *)icmpv6_h +
 374                                                  sizeof(struct icmpv6_hdr));
 375
 376         ether_addr_copy((struct ether_addr *)&port->macaddr[0], &eth_h->s_addr);
 377         eth_h->ether_type = rte_bswap16(0x86dd);
 378         for (i = 0; i < 6; i++) {
 379                 eth_h->d_addr.addr_bytes[i] = 0;
 380         }
 381
 382         ipv6_h->vtc_flow = 0x60000000;
 383         ipv6_h->payload_len = rte_bswap16(32);
 384         ipv6_h->proto = 58;
 385         ipv6_h->hop_limits = 64;
 386
 387         for (i = 0; i < 16; i++) {
 388                 ipv6_h->src_addr[i] = 0x0;
 389                 ipv6_h->dst_addr[i] = ipv6[i];
 390         }
 391
 392         icmpv6_h->icmpv6_type = ICMPV6_NEIGHBOR_SOLICITATION;
 393         icmpv6_h->icmpv6_code = 0;
 394
 395         icmpv6_nd_h->icmpv6_reserved = 0x0;
 396         for (i = 0; i < ND_IPV6_ADDR_SIZE; i++)
 397                 icmpv6_nd_h->target_ipv6[i] = ipv6[i];
 398         icmpv6_nd_h->type = e_Source_Link_Layer_Address;
 399         icmpv6_nd_h->length = 1;
 400         memcpy(&icmpv6_nd_h->link_layer_addr[0], &port->macaddr[0], 6);
 401
 402         icmpv6_h->icmpv6_cksum = 0;
 403         icmpv6_h->icmpv6_cksum = ~icmpv6_ipv6_nd_checksum(icmpv6_pkt);
 404
 405         icmpv6_pkt->pkt_len =
 406                         sizeof(struct ether_hdr) + sizeof(struct ipv6_hdr) + 32;
 407         icmpv6_pkt->data_len = icmpv6_pkt->pkt_len;
 408
 409         return icmpv6_pkt;
 410 }