VNFs/DPPD-PROX/handle_qinq_decap4.c

   1 /*
   2 // Copyright (c) 2010-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
  17 #include <rte_byteorder.h>
  18 #include <rte_cycles.h>
  19 #include <rte_table_hash.h>
  20 #include <rte_lpm.h>
  21 #include <rte_version.h>
  22
  23 #include "prox_lua.h"
  24 #include "prox_lua_types.h"
  25 #include "handle_qinq_decap4.h"
  26 #include "handle_qinq_encap4.h"
  27 #include "stats.h"
  28 #include "tx_pkt.h"
  29 #include "defines.h"
  30 #include "handle_routing.h"
  31 #include "prox_assert.h"
  32 #include "task_init.h"
  33 #include "quit.h"
  34 #include "pkt_prototypes.h"
  35 #include "task_base.h"
  36 #include "task_init.h"
  37 #include "bng_pkts.h"
  38 #include "prox_cksum.h"
  39 #include "expire_cpe.h"
  40 #include "prox_port_cfg.h"
  41 #include "prefetch.h"
  42 #include "prox_cfg.h"
  43 #include "lconf.h"
  44 #include "prox_cfg.h"
  45 #include "prox_shared.h"
  46
  47 struct task_qinq_decap4 {
  48         struct task_base        base;
  49         struct rte_table_hash   *cpe_table;
  50         struct rte_table_hash   *qinq_gre_table;
  51         struct qinq_gre_data    *qinq_gre_data;
  52         struct next_hop         *next_hops;
  53         struct rte_lpm          *ipv4_lpm;
  54         uint32_t                local_ipv4;
  55         uint16_t                qinq_tag;
  56         uint8_t                 runtime_flags;
  57         int                     offload_crc;
  58         uint64_t                keys[64];
  59         uint64_t                src_mac[PROX_MAX_PORTS];
  60         struct rte_mbuf*        fake_packets[64];
  61         struct expire_cpe       expire_cpe;
  62         uint64_t                cpe_timeout;
  63         uint8_t                 mapping[PROX_MAX_PORTS];
  64 };
  65
  66 static uint8_t handle_qinq_decap4(struct task_qinq_decap4 *task, struct rte_mbuf *mbuf, struct qinq_gre_data* entry);
  67 /* Convert IPv4 packets to GRE and optionally store QinQ Tags */
  68 static void arp_update(struct task_base *tbase, struct rte_mbuf **mbufs, uint16_t n_pkts);
  69 static void arp_msg(struct task_base *tbase, void **data, uint16_t n_msgs);
  70
  71 static void init_task_qinq_decap4(struct task_base *tbase, struct task_args *targ)
  72 {
  73         struct task_qinq_decap4 *task = (struct task_qinq_decap4 *)tbase;
  74         const int socket_id = rte_lcore_to_socket_id(targ->lconf->id);
  75         struct lpm4 *lpm;
  76
  77         task->cpe_table = targ->cpe_table;
  78         task->cpe_timeout = msec_to_tsc(targ->cpe_table_timeout_ms);
  79
  80         PROX_PANIC(!strcmp(targ->route_table, ""), "route table not specified\n");
  81         lpm = prox_sh_find_socket(socket_id, targ->route_table);
  82         if (!lpm) {
  83                 int ret = lua_to_lpm4(prox_lua(), GLOBAL, targ->route_table, socket_id, &lpm);
  84                 PROX_PANIC(ret, "Failed to load IPv4 LPM:\n%s\n", get_lua_to_errors());
  85                 prox_sh_add_socket(socket_id, targ->route_table, lpm);
  86         }
  87         task->ipv4_lpm = lpm->rte_lpm;
  88         task->next_hops = lpm->next_hops;
  89
  90         task->qinq_tag = targ->qinq_tag;
  91         task->local_ipv4 = targ->local_ipv4;
  92         task->runtime_flags = targ->runtime_flags;
  93         if (strcmp(targ->task_init->sub_mode_str, "pe"))
  94                 PROX_PANIC(targ->qinq_gre_table == NULL, "can't set up qinq gre\n");
  95
  96         task->qinq_gre_table = targ->qinq_gre_table;
  97
  98         if (targ->cpe_table_timeout_ms) {
  99                 targ->lconf->period_func = check_expire_cpe;
 100                 task->expire_cpe.cpe_table = task->cpe_table;
 101                 targ->lconf->period_data = &task->expire_cpe;
 102                 targ->lconf->period_timeout = msec_to_tsc(500) / NUM_VCPES;
 103         }
 104
 105         for (uint32_t i = 0; i < 64; ++i) {
 106                 task->fake_packets[i] = (struct rte_mbuf*)((uint8_t*)&task->keys[i] - sizeof (struct rte_mbuf));
 107         }
 108         if (task->runtime_flags & TASK_ROUTING) {
 109                 if (targ->nb_txrings) {
 110                         struct task_args *dtarg;
 111                         struct core_task ct;
 112
 113                         for (uint32_t i = 0; i < targ->nb_txrings; ++i) {
 114                                 ct = targ->core_task_set[0].core_task[i];
 115                                 dtarg = core_targ_get(ct.core, ct.task);
 116                                 dtarg = find_reachable_task_sending_to_port(dtarg);
 117
 118                                 PROX_PANIC(dtarg == NULL, "Error finding destination port through other tasks for outgoing ring %u\n", i);
 119                                 task->src_mac[i] = *(uint64_t*)&prox_port_cfg[dtarg->tx_port_queue[0].port].eth_addr;
 120                         }
 121                 }
 122                 else {
 123                         for (uint32_t i = 0; i < targ->nb_txports; ++i) {
 124                                 task->src_mac[i] = *(uint64_t*)&prox_port_cfg[targ->tx_port_queue[i].port].eth_addr;
 125                         }
 126                 }
 127         }
 128
 129         if (targ->runtime_flags & TASK_CTRL_HANDLE_ARP) {
 130                 targ->lconf->ctrl_func_p[targ->task] = arp_update;
 131         }
 132
 133         /* Copy the mapping from a sibling task which is configured
 134            with mode encap4. The mapping is constant, so it is faster
 135            to apply it when entries are added (least common case)
 136            instead of re-applying it for every packet (most common
 137            case). */
 138
 139         for (uint8_t task_id = 0; task_id < targ->lconf->n_tasks_all; ++task_id) {
 140                 enum task_mode smode = targ->lconf->targs[task_id].mode;
 141                 if (QINQ_ENCAP4 == smode) {
 142                         for (uint8_t i = 0; i < PROX_MAX_PORTS; ++i) {
 143                                 task->mapping[i] = targ->lconf->targs[task_id].mapping[i];
 144                         }
 145                 }
 146         }
 147
 148         struct prox_port_cfg *port = find_reachable_port(targ);
 149         if (port) {
 150                 task->offload_crc = port->capabilities.tx_offload_cksum;
 151         }
 152
 153         // By default, calling this function 1K times per second => 64K ARP per second max
 154         // If 4 interfaces sending to here, = ~0.1% of workload.
 155         // If receiving more ARP, they will be dropped, or will dramatically slow down LB if in "no drop" mode.
 156         targ->lconf->ctrl_timeout = freq_to_tsc(targ->ctrl_freq);
 157         targ->lconf->ctrl_func_m[targ->task] = arp_msg;
 158 }
 159
 160 static void early_init_table(struct task_args *targ)
 161 {
 162         if (!targ->qinq_gre_table && !targ->cpe_table) {
 163                 init_qinq_gre_table(targ, get_qinq_gre_map(targ));
 164                 init_cpe4_table(targ);
 165         }
 166 }
 167
 168 static inline void extract_key_bulk(struct rte_mbuf **mbufs, uint16_t n_pkts, struct task_qinq_decap4 *task)
 169 {
 170         for (uint16_t j = 0; j < n_pkts; ++j) {
 171                 extract_key_cpe(mbufs[j], &task->keys[j]);
 172         }
 173 }
 174
 175 __attribute__((cold)) static void handle_error(struct rte_mbuf *mbuf)
 176 {
 177         struct cpe_pkt *packet = rte_pktmbuf_mtod(mbuf, struct cpe_pkt *);
 178 #ifdef USE_QINQ
 179         uint64_t key = (*(uint64_t*)(((uint8_t *)packet) + 12)) & 0xFF0FFFFFFF0FFFFF;
 180         uint32_t svlan = packet->qinq_hdr.svlan.vlan_tci;
 181         uint32_t cvlan = packet->qinq_hdr.cvlan.vlan_tci;
 182
 183         svlan = rte_be_to_cpu_16(svlan & 0xFF0F);
 184         cvlan = rte_be_to_cpu_16(cvlan & 0xFF0F);
 185 #if RTE_VERSION >= RTE_VERSION_NUM(2,1,0,0)
 186         plogx_err("Can't convert key %016lx qinq %d|%d (%x|%x) to gre_id, rss=%x flags=%lx, status_err_len=%lx, L2Tag=%d type=%d\n",
 187                   key, svlan, cvlan, svlan, cvlan, mbuf->hash.rss, mbuf->ol_flags, mbuf->udata64, mbuf->vlan_tci_outer, mbuf->packet_type);
 188 #else
 189 #if RTE_VERSION >= RTE_VERSION_NUM(1,8,0,0)
 190         plogx_err("Can't convert key %016lx qinq %d|%d (%x|%x) to gre_id, rss=%x flags=%lx, status_err_len=%lx, L2Tag=%d type=%d\n",
 191                   key, svlan, cvlan, svlan, cvlan, mbuf->hash.rss, mbuf->ol_flags, mbuf->udata64, mbuf->reserved, mbuf->packet_type);
 192 #else
 193         plogx_err("Can't convert key %016lx qinq %d|%d (%x|%x) to gre_id, flags=%x, L2Tag=%d\n",
 194                   key, svlan, cvlan, svlan, cvlan, mbuf->ol_flags, mbuf->reserved);
 195 #endif
 196 #endif
 197 #else
 198         plogx_err("Can't convert ip %x to gre_id\n", rte_bswap32(packet->ipv4_hdr.src_addr));
 199 #endif
 200 }
 201
 202 static int add_cpe_entry(struct rte_table_hash *hash, struct cpe_key *key, struct cpe_data *data)
 203 {
 204         void* entry_in_hash;
 205         int ret, key_found = 0;
 206
 207         ret = rte_table_hash_key8_ext_dosig_ops.
 208                 f_add(hash, key, data, &key_found, &entry_in_hash);
 209         if (unlikely(ret)) {
 210                 plogx_err("Failed to add key: ip %x, gre %x\n", key->ip, key->gre_id);
 211                 return 1;
 212         }
 213         return 0;
 214 }
 215
 216 static void extract_key_data_arp(struct rte_mbuf* mbuf, struct cpe_key* key, struct cpe_data* data, const struct qinq_gre_data* entry, uint64_t cpe_timeout, uint8_t* mapping)
 217 {
 218         const struct cpe_packet_arp *packet = rte_pktmbuf_mtod(mbuf, const struct cpe_packet_arp *);
 219         uint32_t svlan = packet->qinq_hdr.svlan.vlan_tci & 0xFF0F;
 220         uint32_t cvlan = packet->qinq_hdr.cvlan.vlan_tci & 0xFF0F;
 221         uint8_t port_id;
 222         key->ip = packet->arp.data.spa;
 223         key->gre_id = entry->gre_id;
 224
 225         data->mac_port_8bytes = *((const uint64_t *)(&packet->qinq_hdr.s_addr));
 226         data->qinq_svlan = svlan;
 227         data->qinq_cvlan = cvlan;
 228 #if RTE_VERSION >= RTE_VERSION_NUM(1,8,0,0)
 229         port_id = mbuf->port;
 230
 231 #else
 232         port_id = mbuf->pkt.in_port;
 233 #endif
 234         uint8_t mapped = mapping[port_id];
 235         data->mac_port.out_idx = mapping[port_id];
 236
 237         if (unlikely(mapped == 255)) {
 238                 /* This error only occurs if the system is configured incorrectly */
 239                 plog_warn("Failed adding packet: unknown mapping for port %d", port_id);
 240                 data->mac_port.out_idx = 0;
 241         }
 242
 243         data->user = entry->user;
 244         data->tsc = rte_rdtsc() + cpe_timeout;
 245 }
 246
 247 void arp_msg_to_str(char *str, struct arp_msg *msg)
 248 {
 249         sprintf(str, "%u %u %u %u %u.%u.%u.%u %x:%x:%x:%x:%x:%x %u\n",
 250                 msg->data.mac_port.out_idx, msg->key.gre_id, msg->data.qinq_svlan, msg->data.qinq_cvlan,
 251                 msg->key.ip_bytes[0], msg->key.ip_bytes[1], msg->key.ip_bytes[2], msg->key.ip_bytes[3],
 252                 msg->data.mac_port_b[0], msg->data.mac_port_b[1], msg->data.mac_port_b[2],
 253                 msg->data.mac_port_b[3], msg->data.mac_port_b[4], msg->data.mac_port_b[5], msg->data.user);
 254 }
 255
 256 int str_to_arp_msg(struct arp_msg *msg, const char *str)
 257 {
 258         uint32_t ip[4], interface, gre_id, svlan, cvlan, mac[6], user;
 259
 260         int ret = sscanf(str, "%u %u %u %u %u.%u.%u.%u %x:%x:%x:%x:%x:%x %u",
 261                          &interface, &gre_id, &svlan, &cvlan,
 262                          ip, ip + 1, ip + 2, ip + 3,
 263                          mac, mac + 1, mac + 2, mac + 3, mac + 4, mac + 5, &user);
 264
 265         for (uint8_t i = 0; i < 4; ++i)
 266                 msg->key.ip_bytes[i] = ip[i];
 267         msg->key.gre_id = gre_id;
 268
 269         for (uint8_t i = 0; i < 4; ++i)
 270                 msg->data.mac_port_b[i] = mac[i];
 271         msg->data.qinq_svlan = svlan;
 272         msg->data.qinq_cvlan = cvlan;
 273         msg->data.user = user;
 274         msg->data.mac_port.out_idx = interface;
 275
 276         return ret != 15;
 277 }
 278
 279 void arp_update_from_msg(struct rte_table_hash * cpe_table, struct arp_msg **msgs, uint16_t n_msgs, uint64_t cpe_timeout)
 280 {
 281         int ret, key_found = 0;
 282         void* entry_in_hash;
 283
 284         for (uint16_t i = 0; i < n_msgs; ++i) {
 285                 msgs[i]->data.tsc = rte_rdtsc() + cpe_timeout;
 286                 ret = rte_table_hash_key8_ext_dosig_ops.
 287                         f_add(cpe_table, &msgs[i]->key, &msgs[i]->data, &key_found, &entry_in_hash);
 288                 if (unlikely(ret)) {
 289                         plogx_err("Failed to add key %x, gre %x\n", msgs[i]->key.ip, msgs[i]->key.gre_id);
 290                 }
 291         }
 292 }
 293
 294 static void arp_msg(struct task_base *tbase, void **data, uint16_t n_msgs)
 295 {
 296         struct task_qinq_decap4 *task = (struct task_qinq_decap4 *)tbase;
 297         struct arp_msg **msgs = (struct arp_msg **)data;
 298
 299         arp_update_from_msg(task->cpe_table, msgs, n_msgs, task->cpe_timeout);
 300 }
 301
 302 static void arp_update(struct task_base *tbase, struct rte_mbuf **mbufs, uint16_t n_pkts)
 303 {
 304         struct task_qinq_decap4 *task = (struct task_qinq_decap4 *)tbase;
 305
 306         prefetch_pkts(mbufs, n_pkts);
 307         extract_key_bulk(mbufs, n_pkts, task);
 308
 309         uint64_t pkts_mask = RTE_LEN2MASK(n_pkts, uint64_t);
 310         uint64_t lookup_hit_mask = 0;
 311         struct qinq_gre_data* entries[64];
 312         rte_table_hash_key8_ext_dosig_ops.f_lookup(task->qinq_gre_table, task->fake_packets, pkts_mask, &lookup_hit_mask, (void**)entries);
 313
 314         TASK_STATS_ADD_RX(&task->base.aux->stats, n_pkts);
 315         for (uint16_t j = 0; j < n_pkts; ++j) {
 316                 if (unlikely(!((lookup_hit_mask >> j) & 0x1))) {
 317                         handle_error(mbufs[j]);
 318                         rte_pktmbuf_free(mbufs[j]);
 319                         continue;
 320                 }
 321
 322                 struct cpe_key key;
 323                 struct cpe_data data;
 324
 325                 extract_key_data_arp(mbufs[j], &key, &data, entries[j], task->cpe_timeout, task->mapping);
 326
 327                 void* entry_in_hash;
 328                 int ret, key_found = 0;
 329
 330                 ret = rte_table_hash_key8_ext_dosig_ops.
 331                         f_add(task->cpe_table, &key, &data, &key_found, &entry_in_hash);
 332
 333                 if (unlikely(ret)) {
 334                         plogx_err("Failed to add key %x, gre %x\n", key.ip, key.gre_id);
 335                         TASK_STATS_ADD_DROP_DISCARD(&task->base.aux->stats, 1);
 336                 }
 337
 338                 /* should do ARP reply */
 339                 TASK_STATS_ADD_DROP_HANDLED(&task->base.aux->stats, 1);
 340                 rte_pktmbuf_free(mbufs[j]);
 341         }
 342 }
 343
 344 static int handle_qinq_decap4_bulk(struct task_base *tbase, struct rte_mbuf **mbufs, uint16_t n_pkts)
 345 {
 346         struct task_qinq_decap4 *task = (struct task_qinq_decap4 *)tbase;
 347         uint64_t pkts_mask = RTE_LEN2MASK(n_pkts, uint64_t);
 348         struct qinq_gre_data* entries[64];
 349         uint8_t out[MAX_PKT_BURST];
 350         uint64_t lookup_hit_mask;
 351         prefetch_pkts(mbufs, n_pkts);
 352
 353         // Prefetch headroom, as we will prepend mbuf and write to this cache line
 354         for (uint16_t j = 0; j < n_pkts; ++j) {
 355                 PREFETCH0((rte_pktmbuf_mtod(mbufs[j], char*)-1));
 356         }
 357
 358         extract_key_bulk(mbufs, n_pkts, task);
 359         rte_table_hash_key8_ext_dosig_ops.f_lookup(task->qinq_gre_table, task->fake_packets, pkts_mask, &lookup_hit_mask, (void**)entries);
 360
 361         if (likely(lookup_hit_mask == pkts_mask)) {
 362                 for (uint16_t j = 0; j < n_pkts; ++j) {
 363                         out[j] = handle_qinq_decap4(task, mbufs[j], entries[j]);
 364                 }
 365         }
 366         else {
 367                 for (uint16_t j = 0; j < n_pkts; ++j) {
 368                         if (unlikely(!((lookup_hit_mask >> j) & 0x1))) {
 369                                 // This might fail as the packet has not the expected QinQ or it's not an IPv4 packet
 370                                 handle_error(mbufs[j]);
 371                                 out[j] = OUT_DISCARD;
 372                                 continue;
 373                         }
 374                         out[j] = handle_qinq_decap4(task, mbufs[j], entries[j]);
 375                 }
 376         }
 377
 378         return task->base.tx_pkt(&task->base, mbufs, n_pkts, out);
 379 }
 380
 381 /* add gre header */
 382 static inline void gre_encap(struct task_qinq_decap4 *task, uint32_t src_ipv4, struct rte_mbuf *mbuf, uint32_t gre_id)
 383 {
 384 #ifdef USE_QINQ
 385         struct ipv4_hdr *pip = (struct ipv4_hdr *)(1 + rte_pktmbuf_mtod(mbuf, struct qinq_hdr *));
 386 #else
 387         struct ipv4_hdr *pip = (struct ipv4_hdr *)(1 + rte_pktmbuf_mtod(mbuf, struct ether_hdr *));
 388 #endif
 389         uint16_t ip_len = rte_be_to_cpu_16(pip->total_length);
 390         uint16_t padlen = rte_pktmbuf_pkt_len(mbuf) - 20 - ip_len - sizeof(struct qinq_hdr);
 391
 392         if (padlen) {
 393                 rte_pktmbuf_trim(mbuf, padlen);
 394         }
 395
 396         PROX_PANIC(rte_pktmbuf_data_len(mbuf) - padlen + 20 > ETHER_MAX_LEN,
 397                    "Would need to fragment packet new size = %u - not implemented\n",
 398                    rte_pktmbuf_data_len(mbuf) - padlen + 20);
 399
 400 #ifdef USE_QINQ
 401         /* prepend only 20 bytes instead of 28, 8 bytes are present from the QinQ */
 402         struct ether_hdr *peth = (struct ether_hdr *)rte_pktmbuf_prepend(mbuf, 20);
 403 #else
 404         struct ether_hdr *peth = (struct ether_hdr *)rte_pktmbuf_prepend(mbuf, 28);
 405 #endif
 406
 407         PROX_ASSERT(peth);
 408         PREFETCH0(peth);
 409         if (task->runtime_flags & TASK_TX_CRC) {
 410                 /* calculate IP CRC here to avoid problems with -O3 flag with gcc */
 411 #ifdef MPLS_ROUTING
 412                 prox_ip_cksum(mbuf, pip, sizeof(struct ether_hdr) + sizeof(struct mpls_hdr), sizeof(struct ipv4_hdr), task->offload_crc);
 413 #else
 414                 prox_ip_cksum(mbuf, pip, sizeof(struct ether_hdr), sizeof(struct ipv4_hdr), task->offload_crc);
 415 #endif
 416         }
 417
 418         /* new IP header */
 419         struct ipv4_hdr *p_tunnel_ip = (struct ipv4_hdr *)(peth + 1);
 420         rte_memcpy(p_tunnel_ip, &tunnel_ip_proto, sizeof(struct ipv4_hdr));
 421         ip_len += sizeof(struct ipv4_hdr) + sizeof(struct gre_hdr);
 422         p_tunnel_ip->total_length = rte_cpu_to_be_16(ip_len);
 423         p_tunnel_ip->src_addr = src_ipv4;
 424
 425         /* Add GRE Header values */
 426         struct gre_hdr *pgre = (struct gre_hdr *)(p_tunnel_ip + 1);
 427
 428         rte_memcpy(pgre, &gre_hdr_proto, sizeof(struct gre_hdr));
 429         pgre->gre_id = gre_id;
 430         peth->ether_type = ETYPE_IPv4;
 431 }
 432
 433 static inline uint16_t calc_padlen(const struct rte_mbuf *mbuf, const uint16_t ip_len)
 434 {
 435         return rte_pktmbuf_pkt_len(mbuf) - DOWNSTREAM_DELTA - ip_len - offsetof(struct cpe_pkt, ipv4_hdr);
 436 }
 437
 438 static inline uint8_t gre_encap_route(uint32_t src_ipv4, struct rte_mbuf *mbuf, uint32_t gre_id, struct task_qinq_decap4 *task)
 439 {
 440         PROX_PANIC(rte_pktmbuf_data_len(mbuf) + DOWNSTREAM_DELTA  > ETHER_MAX_LEN,
 441                    "Would need to fragment packet new size = %u - not implemented\n",
 442                    rte_pktmbuf_data_len(mbuf) + DOWNSTREAM_DELTA);
 443
 444         struct core_net_pkt_m *packet = (struct core_net_pkt_m *)rte_pktmbuf_prepend(mbuf, DOWNSTREAM_DELTA);
 445         PROX_ASSERT(packet);
 446         PREFETCH0(packet);
 447
 448         struct ipv4_hdr *pip = &((struct cpe_pkt_delta *)packet)->pkt.ipv4_hdr;
 449         uint16_t ip_len = rte_be_to_cpu_16(pip->total_length);
 450
 451         /* returns 0 on success, returns -ENOENT of failure (or -EINVAL if first or last parameter is NULL) */
 452 #if RTE_VERSION >= RTE_VERSION_NUM(16,4,0,1)
 453         uint32_t next_hop_index;
 454 #else
 455         uint8_t next_hop_index;
 456 #endif
 457         if (unlikely(rte_lpm_lookup(task->ipv4_lpm, rte_bswap32(pip->dst_addr), &next_hop_index) != 0)) {
 458                 plog_warn("lpm_lookup failed for ip %x: rc = %d\n", rte_bswap32(pip->dst_addr), -ENOENT);
 459                 return ROUTE_ERR;
 460         }
 461         PREFETCH0(&task->next_hops[next_hop_index]);
 462
 463         /* calculate outer IP CRC here to avoid problems with -O3 flag with gcc */
 464         const uint16_t padlen = calc_padlen(mbuf, ip_len);
 465         if (padlen) {
 466                 rte_pktmbuf_trim(mbuf, padlen);
 467         }
 468         const uint8_t port_id = task->next_hops[next_hop_index].mac_port.out_idx;
 469
 470         *((uint64_t *)(&packet->ether_hdr.d_addr)) = task->next_hops[next_hop_index].mac_port_8bytes;
 471         *((uint64_t *)(&packet->ether_hdr.s_addr)) = task->src_mac[task->next_hops[next_hop_index].mac_port.out_idx];
 472
 473 #ifdef MPLS_ROUTING
 474         packet->mpls_bytes = task->next_hops[next_hop_index].mpls | 0x00010000; // Set BoS to 1
 475         packet->ether_hdr.ether_type = ETYPE_MPLSU;
 476 #else
 477         packet->ether_hdr.ether_type = ETYPE_IPv4;
 478 #endif
 479
 480         /* New IP header */
 481         rte_memcpy(&packet->tunnel_ip_hdr, &tunnel_ip_proto, sizeof(struct ipv4_hdr));
 482         ip_len += sizeof(struct ipv4_hdr) + sizeof(struct gre_hdr);
 483         packet->tunnel_ip_hdr.total_length = rte_cpu_to_be_16(ip_len);
 484         packet->tunnel_ip_hdr.src_addr = src_ipv4;
 485         packet->tunnel_ip_hdr.dst_addr = task->next_hops[next_hop_index].ip_dst;
 486         if (task->runtime_flags & TASK_TX_CRC) {
 487 #ifdef MPLS_ROUTING
 488                 prox_ip_cksum(mbuf, (void *)&(packet->tunnel_ip_hdr), sizeof(struct ether_hdr) + sizeof(struct mpls_hdr), sizeof(struct ipv4_hdr), task->offload_crc);
 489 #else
 490                 prox_ip_cksum(mbuf, (void *)&(packet->tunnel_ip_hdr), sizeof(struct ether_hdr), sizeof(struct ipv4_hdr), task->offload_crc);
 491 #endif
 492         }
 493
 494         /* Add GRE Header values */
 495         rte_memcpy(&packet->gre_hdr, &gre_hdr_proto, sizeof(struct gre_hdr));
 496         packet->gre_hdr.gre_id = rte_be_to_cpu_32(gre_id);
 497
 498         return port_id;
 499 }
 500
 501 static void extract_key_data(struct rte_mbuf* mbuf, struct cpe_key* key, struct cpe_data* data, const struct qinq_gre_data* entry, uint64_t cpe_timeout, uint8_t *mapping)
 502 {
 503         struct cpe_pkt *packet = rte_pktmbuf_mtod(mbuf, struct cpe_pkt *);
 504         uint8_t port_id;
 505
 506 #ifndef USE_QINQ
 507         const uint32_t tmp = rte_bswap32(packet->ipv4_hdr.src_addr) & 0x00FFFFFF;
 508         const uint32_t svlan = rte_bswap16(tmp >> 12);
 509         const uint32_t cvlan = rte_bswap16(tmp & 0x0FFF);
 510 #endif
 511
 512 #ifdef USE_QINQ
 513         key->ip = packet->ipv4_hdr.src_addr;
 514 #else
 515         key->ip = 0;
 516 #endif
 517         key->gre_id = entry->gre_id;
 518
 519 #ifdef USE_QINQ
 520         data->mac_port_8bytes = *((const uint64_t *)(&packet->qinq_hdr.s_addr));
 521         data->qinq_svlan      = packet->qinq_hdr.svlan.vlan_tci & 0xFF0F;
 522         data->qinq_cvlan      = packet->qinq_hdr.cvlan.vlan_tci & 0xFF0F;
 523 #else
 524         data->mac_port_8bytes = *((const uint64_t *)(&packet->ether_hdr.s_addr));
 525         data->qinq_svlan      = svlan;
 526         data->qinq_cvlan      = cvlan;
 527 #endif
 528
 529 #if RTE_VERSION >= RTE_VERSION_NUM(1,8,0,0)
 530         port_id = mbuf->port;
 531
 532 #else
 533         port_id = mbuf->pkt.in_port;
 534 #endif
 535         uint8_t mapped = mapping[port_id];
 536         data->mac_port.out_idx = mapped;
 537
 538         if (unlikely(mapped == 255)) {
 539                 /* This error only occurs if the system is configured incorrectly */
 540                 plog_warn("Failed adding packet: unknown mapping for port %d", port_id);
 541                 data->mac_port.out_idx = 0;
 542         }
 543         else {
 544                 data->mac_port.out_idx = mapped;
 545         }
 546
 547         data->user             = entry->user;
 548         data->tsc              = rte_rdtsc() + cpe_timeout;
 549 }
 550
 551 static uint8_t handle_qinq_decap4(struct task_qinq_decap4 *task, struct rte_mbuf *mbuf, struct qinq_gre_data* entry)
 552 {
 553         if (!(task->runtime_flags & (TASK_CTRL_HANDLE_ARP|TASK_FP_HANDLE_ARP))) {
 554                 // We learn CPE MAC addresses on every packets
 555                 struct cpe_key key;
 556                 struct cpe_data data;
 557                 extract_key_data(mbuf, &key, &data, entry, task->cpe_timeout, task->mapping);
 558                 //plogx_err("Adding key ip=%x/gre_id=%x data (svlan|cvlan)=%x|%x, rss=%x, gre_id=%x\n", key.ip, key.gre_id, data.qinq_svlan,data.qinq_cvlan, mbuf->hash.rss, entry->gre_id);
 559
 560                 if (add_cpe_entry(task->cpe_table, &key, &data)) {
 561                         plog_warn("Failed to add ARP entry\n");
 562                         return OUT_DISCARD;
 563                 }
 564         }
 565         if (task->runtime_flags & TASK_FP_HANDLE_ARP) {
 566                 // We learn CPE MAC addresses on ARP packets in Fast Path
 567 #if RTE_VERSION >= RTE_VERSION_NUM(1,8,0,0)
 568                 if (mbuf->packet_type == 0xB) {
 569                         struct cpe_key key;
 570                         struct cpe_data data;
 571                         extract_key_data_arp(mbuf, &key, &data, entry, task->cpe_timeout, task->mapping);
 572
 573                         if (add_cpe_entry(task->cpe_table, &key, &data)) {
 574                                 plog_warn("Failed to add ARP entry\n");
 575                                 return OUT_DISCARD;
 576                         }
 577                         return OUT_HANDLED;
 578                 } else
 579 #endif
 580                 {
 581 #ifdef USE_QINQ
 582                         struct cpe_pkt *packet = rte_pktmbuf_mtod(mbuf, struct cpe_pkt*);
 583                         if (packet->qinq_hdr.svlan.eth_proto == task->qinq_tag &&
 584                             packet->qinq_hdr.ether_type == ETYPE_ARP) {
 585                                 struct cpe_key key;
 586                                 struct cpe_data data;
 587                                 extract_key_data_arp(mbuf, &key, &data, entry, task->cpe_timeout, task->mapping);
 588
 589                                 if (add_cpe_entry(task->cpe_table, &key, &data)) {
 590                                         plog_warn("Failed to add ARP entry\n");
 591                                         return OUT_DISCARD;
 592                                 }
 593                                 return OUT_HANDLED;
 594                         }
 595 #endif
 596                 }
 597         }
 598         if (task->runtime_flags & TASK_ROUTING) {
 599                 uint8_t tx_portid;
 600                 tx_portid = gre_encap_route(task->local_ipv4, mbuf, entry->gre_id, task);
 601
 602                 return tx_portid == ROUTE_ERR? OUT_DISCARD : tx_portid;
 603         }
 604         else {
 605                 gre_encap(task, task->local_ipv4, mbuf, entry->gre_id);
 606                 return 0;
 607         }
 608 }
 609
 610 static void flow_iter_next(struct flow_iter *iter, struct task_args *targ)
 611 {
 612         do {
 613                 iter->idx++;
 614         } while (iter->idx < (int)get_qinq_gre_map(targ)->count &&
 615                  get_qinq_gre_map(targ)->entries[iter->idx].gre_id % targ->nb_slave_threads != targ->worker_thread_id);
 616 }
 617
 618 static void flow_iter_beg(struct flow_iter *iter, struct task_args *targ)
 619 {
 620         iter->idx = -1;
 621         flow_iter_next(iter, targ);
 622 }
 623
 624 static int flow_iter_is_end(struct flow_iter *iter, struct task_args *targ)
 625 {
 626         return iter->idx == (int)get_qinq_gre_map(targ)->count;
 627 }
 628
 629 static uint16_t flow_iter_get_svlan(struct flow_iter *iter, struct task_args *targ)
 630 {
 631         return get_qinq_gre_map(targ)->entries[iter->idx].svlan;
 632 }
 633
 634 static uint16_t flow_iter_get_cvlan(struct flow_iter *iter, struct task_args *targ)
 635 {
 636         return get_qinq_gre_map(targ)->entries[iter->idx].cvlan;
 637 }
 638
 639 static struct task_init task_init_qinq_decapv4_table = {
 640         .mode = QINQ_DECAP4,
 641         .mode_str = "qinqdecapv4",
 642         .early_init = early_init_table,
 643         .init = init_task_qinq_decap4,
 644         .handle = handle_qinq_decap4_bulk,
 645         .flag_features = TASK_FEATURE_ROUTING,
 646         .flow_iter = {
 647                 .beg       = flow_iter_beg,
 648                 .is_end    = flow_iter_is_end,
 649                 .next      = flow_iter_next,
 650                 .get_svlan = flow_iter_get_svlan,
 651                 .get_cvlan = flow_iter_get_cvlan,
 652         },
 653         .size = sizeof(struct task_qinq_decap4)
 654 };
 655
 656 __attribute__((constructor)) static void reg_task_qinq_decap4(void)
 657 {
 658         reg_task(&task_init_qinq_decapv4_table);
 659 }