VNFs/DPPD-PROX/handle_esp.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 /*
  18  * Non compatible implementation of RFC3686(CTR-AES 128 bit key), RFC4303 (tunnel ipv4 ESP)
  19  * Limitations:
  20  * 1. Crypto not safe!!!!! (underlying AES-CTR implementation is OK, but ESP implementation is lousy)
  21  * 2. Only ESP/tunnel/ipv4/AES-CTR
  22  * 3. Not fully implemented
  23  * 4. No proper key / SADB
  24  * So performance demonstrator only
  25  */
  26
  27 #include "task_init.h"
  28 #include "task_base.h"
  29 #include "etypes.h"
  30 #include "stats.h"
  31 #include "cfgfile.h"
  32 #include "log.h"
  33 #include "prox_cksum.h"
  34 #include "defines.h"
  35 #include <rte_ip.h>
  36 #include <rte_cryptodev.h>
  37 #include <rte_cryptodev_pmd.h>
  38 #include <rte_bus_vdev.h>
  39 #include "prox_port_cfg.h"
  40 #include "prox_compat.h"
  41
  42 typedef unsigned int u32;
  43 typedef unsigned char u8;
  44
  45 #define BYTE_LENGTH(x) (x/8)
  46 #define DIGEST_BYTE_LENGTH_SHA1 (BYTE_LENGTH(160))
  47
  48 //#define CIPHER_KEY_LENGTH_AES_CBC (32)
  49 #define CIPHER_KEY_LENGTH_AES_CBC (16)//==TEST
  50 #define CIPHER_IV_LENGTH_AES_CBC 16
  51
  52 #define MAXIMUM_IV_LENGTH 16
  53 #define IV_OFFSET (sizeof(struct rte_crypto_op) + sizeof(struct rte_crypto_sym_op))
  54
  55 #define MAX_SESSIONS 1024
  56 #define POOL_CACHE_SIZE 128
  57
  58 #define NUM_OPS 256
  59
  60 struct task_esp_enc {
  61         struct task_base base;
  62         uint8_t cdev_id;
  63         uint16_t qp_id;
  64         uint32_t local_ipv4;
  65         struct ether_addr local_mac;
  66         uint32_t remote_ipv4;
  67         struct ether_addr dst_mac;
  68         struct rte_mempool *crypto_op_pool;
  69         struct rte_mempool *session_pool;
  70         struct rte_cryptodev_sym_session *sess;
  71         struct rte_crypto_op *ops_burst[NUM_OPS];
  72 };
  73
  74 struct task_esp_dec {
  75         struct task_base base;
  76         uint8_t cdev_id;
  77         uint16_t qp_id;
  78         uint32_t local_ipv4;
  79         struct ether_addr local_mac;
  80         struct ether_addr dst_mac;
  81         struct rte_mempool *crypto_op_pool;
  82         struct rte_mempool *session_pool;
  83         struct rte_cryptodev_sym_session *sess;
  84         struct rte_crypto_op *ops_burst[NUM_OPS];
  85 };
  86
  87 static uint8_t hmac_sha1_key[] = {
  88         0xF8, 0x2A, 0xC7, 0x54, 0xDB, 0x96, 0x18, 0xAA,
  89         0xC3, 0xA1, 0x53, 0xF6, 0x1F, 0x17, 0x60, 0xBD,
  90         0xDE, 0xF4, 0xDE, 0xAD };
  91
  92 static uint8_t aes_cbc_key[] = {
  93         0xE4, 0x23, 0x33, 0x8A, 0x35, 0x64, 0x61, 0xE2,
  94         0x49, 0x03, 0xDD, 0xC6, 0xB8, 0xCA, 0x55, 0x7A,
  95         0xE4, 0x23, 0x33, 0x8A, 0x35, 0x64, 0x61, 0xE2,
  96         0x49, 0x03, 0xDD, 0xC6, 0xB8, 0xCA, 0x55, 0x7A };
  97
  98 static uint8_t aes_cbc_iv[] = {
  99         0xE4, 0x23, 0x33, 0x8A, 0x35, 0x64, 0x61, 0xE2,
 100         0x49, 0x03, 0xDD, 0xC6, 0xB8, 0xCA, 0x55, 0x7A };
 101
 102 //RFC4303
 103 struct esp_hdr {
 104         uint32_t spi;
 105         uint32_t sn;
 106 };
 107
 108 static void printf_cdev_info(uint8_t cdev_id)
 109 {
 110         struct rte_cryptodev_info dev_info;
 111         rte_cryptodev_info_get(cdev_id, &dev_info);
 112         plog_info("!!!numdevs:%d\n", rte_cryptodev_count());
 113         //uint16_t rte_cryptodev_queue_pair_count(uint8_t dev_id);
 114         plog_info("dev:%d name:%s nb_queue_pairs:%d max_nb_sessions:%d\n",
 115                 cdev_id, dev_info.driver_name, dev_info.max_nb_queue_pairs, dev_info.sym.max_nb_sessions);
 116         const struct rte_cryptodev_capabilities *cap = &dev_info.capabilities[0];
 117         int i=0;
 118         while (cap->op != RTE_CRYPTO_OP_TYPE_UNDEFINED) {
 119                 //plog_info("cap->sym.xform_type:%d,");
 120                 if (cap->sym.xform_type == RTE_CRYPTO_SYM_XFORM_CIPHER)
 121                         plog_info("RTE_CRYPTO_SYM_XFORM_CIPHER: %d\n", cap->sym.cipher.algo);
 122                 cap = &dev_info.capabilities[++i];
 123         }
 124 }
 125
 126 #if 0
 127 static uint8_t get_cdev_id(void)
 128 {
 129         //crypto devices must be configured in the config file
 130         //eal=-b 0000:00:03.0 --vdev crypto_aesni_mb0 --vdev crypto_aesni_mb1
 131
 132         static uint8_t cdev_id=0;
 133         PROX_PANIC(cdev_id+1 > rte_cryptodev_count(), "not enough crypto devices\n");
 134         //eal=-b 0000:00:03.0 --vdev crypto_aesni_mb0 --vdev crypto_aesni_mb1
 135         return cdev_id++;
 136 }
 137 #else
 138 static uint8_t get_cdev_id(void)
 139 {
 140         static uint8_t cdev_id=0;
 141         char name[64]={0};
 142
 143         sprintf(name, "crypto_aesni_mb%d", cdev_id);
 144
 145         int cdev_id1 = rte_cryptodev_get_dev_id(name);
 146         if (cdev_id1 >= 0){
 147                 plog_info("crypto dev %d preconfigured\n", cdev_id1);
 148                 ++cdev_id;
 149                 return cdev_id1;
 150         }
 151 #if RTE_VERSION < RTE_VERSION_NUM(18,8,0,0)
 152         int ret = rte_vdev_init(name, "max_nb_queue_pairs=8,max_nb_sessions=1024,socket_id=0");
 153 #else
 154         int ret = rte_vdev_init(name, "max_nb_queue_pairs=8,socket_id=0");
 155 #endif
 156         PROX_PANIC(ret != 0, "Failed rte_vdev_init\n");
 157
 158         return cdev_id++;
 159 }
 160 #endif
 161
 162 static void init_task_esp_enc(struct task_base *tbase, struct task_args *targ)
 163 {
 164         struct task_esp_enc *task = (struct task_esp_enc *)tbase;
 165
 166         tbase->flags |= FLAG_NEVER_FLUSH;
 167
 168         uint8_t lcore_id = targ->lconf->id;
 169         char name[64];
 170         sprintf(name, "core_%03u_crypto_pool", lcore_id);
 171         task->crypto_op_pool = rte_crypto_op_pool_create(name, RTE_CRYPTO_OP_TYPE_SYMMETRIC,
 172                 8192, 128, MAXIMUM_IV_LENGTH, rte_socket_id());
 173         PROX_PANIC(task->crypto_op_pool == NULL, "Can't create ENC CRYPTO_OP_POOL\n");
 174
 175         task->cdev_id = get_cdev_id();
 176
 177         struct rte_cryptodev_config cdev_conf;
 178         cdev_conf.nb_queue_pairs = 2;
 179         //cdev_conf.socket_id = SOCKET_ID_ANY;
 180         cdev_conf.socket_id = rte_socket_id();
 181         rte_cryptodev_configure(task->cdev_id, &cdev_conf);
 182
 183         unsigned int session_size = rte_cryptodev_sym_get_private_session_size(task->cdev_id);
 184         plog_info("rte_cryptodev_sym_get_private_session_size=%d\n", session_size);
 185         sprintf(name, "core_%03u_session_pool", lcore_id);
 186         task->session_pool = rte_mempool_create(name,
 187                                 MAX_SESSIONS,
 188                                 session_size,
 189                                 POOL_CACHE_SIZE,
 190                                 0, NULL, NULL, NULL,
 191                                 NULL, rte_socket_id(),
 192                                 0);
 193         PROX_PANIC(task->session_pool == NULL, "Failed rte_mempool_create\n");
 194
 195         task->qp_id=0;
 196         plog_info("enc: task->qp_id=%u\n", task->qp_id);
 197         struct rte_cryptodev_qp_conf qp_conf;
 198         //qp_conf.nb_descriptors = 4096;
 199         qp_conf.nb_descriptors = 128;
 200         rte_cryptodev_queue_pair_setup(task->cdev_id, task->qp_id,
 201                 &qp_conf, rte_cryptodev_socket_id(task->cdev_id), task->session_pool);
 202
 203         int ret = rte_cryptodev_start(task->cdev_id);
 204         PROX_PANIC(ret < 0, "Failed to start device\n");
 205
 206         struct rte_cryptodev *dev;
 207         dev = rte_cryptodev_pmd_get_dev(task->cdev_id);
 208         PROX_PANIC(dev->attached != RTE_CRYPTODEV_ATTACHED, "No ENC cryptodev attached\n");
 209
 210         //Setup Cipher Parameters
 211         struct rte_crypto_sym_xform cipher_xform = {0};
 212         struct rte_crypto_sym_xform auth_xform = {0};
 213
 214         cipher_xform.type = RTE_CRYPTO_SYM_XFORM_CIPHER;
 215         cipher_xform.next = &auth_xform;
 216
 217         cipher_xform.cipher.algo = RTE_CRYPTO_CIPHER_AES_CBC;
 218         cipher_xform.cipher.op = RTE_CRYPTO_CIPHER_OP_ENCRYPT;
 219         cipher_xform.cipher.key.data = aes_cbc_key;
 220         cipher_xform.cipher.key.length = CIPHER_KEY_LENGTH_AES_CBC;
 221
 222         cipher_xform.cipher.iv.offset = IV_OFFSET;
 223         cipher_xform.cipher.iv.length = CIPHER_IV_LENGTH_AES_CBC;
 224
 225         //Setup HMAC Parameters
 226         auth_xform.type = RTE_CRYPTO_SYM_XFORM_AUTH;
 227         auth_xform.next = NULL;
 228         auth_xform.auth.op = RTE_CRYPTO_AUTH_OP_GENERATE;
 229         auth_xform.auth.algo = RTE_CRYPTO_AUTH_SHA1_HMAC;
 230         auth_xform.auth.key.length = DIGEST_BYTE_LENGTH_SHA1;
 231         auth_xform.auth.key.data = hmac_sha1_key;
 232         auth_xform.auth.digest_length = DIGEST_BYTE_LENGTH_SHA1;
 233
 234         auth_xform.auth.iv.offset = 0;
 235         auth_xform.auth.iv.length = 0;
 236
 237         task->sess = rte_cryptodev_sym_session_create(task->session_pool);
 238         PROX_PANIC(task->sess == NULL, "Failed to create ENC session\n");
 239
 240         ret = rte_cryptodev_sym_session_init(task->cdev_id, task->sess, &cipher_xform, task->session_pool);
 241         PROX_PANIC(ret < 0, "Failed sym_session_init\n");
 242
 243         //TODO: doublecheck task->ops_burst lifecycle!
 244         if (rte_crypto_op_bulk_alloc(task->crypto_op_pool,
 245                         RTE_CRYPTO_OP_TYPE_SYMMETRIC,
 246                         task->ops_burst, NUM_OPS) != NUM_OPS) {
 247                 PROX_PANIC(1, "Failed to allocate ENC crypto operations\n");
 248         }
 249
 250         task->local_ipv4 = rte_cpu_to_be_32(targ->local_ipv4);
 251         task->remote_ipv4 = rte_cpu_to_be_32(targ->remote_ipv4);
 252         //memcpy(&task->src_mac, &prox_port_cfg[task->base.tx_params_hw.tx_port_queue->port].eth_addr, sizeof(struct ether_addr));
 253         struct prox_port_cfg *port = find_reachable_port(targ);
 254         memcpy(&task->local_mac, &port->eth_addr, sizeof(struct ether_addr));
 255
 256         if (targ->flags & TASK_ARG_DST_MAC_SET){
 257                 memcpy(&task->dst_mac, &targ->edaddr, sizeof(task->dst_mac));
 258                 plog_info("TASK_ARG_DST_MAC_SET ("MAC_BYTES_FMT")\n", MAC_BYTES(task->dst_mac.addr_bytes));
 259                 //ether_addr_copy(&ptask->dst_mac, &peth->d_addr);
 260                 //rte_memcpy(hdr, task->src_dst_mac, sizeof(task->src_dst_mac));
 261         }
 262 }
 263
 264 static void init_task_esp_dec(struct task_base *tbase, struct task_args *targ)
 265 {
 266         struct task_esp_dec *task = (struct task_esp_dec *)tbase;
 267
 268         tbase->flags |= FLAG_NEVER_FLUSH;
 269
 270         uint8_t lcore_id = targ->lconf->id;
 271         char name[64];
 272         sprintf(name, "core_%03u_crypto_pool", lcore_id);
 273         task->crypto_op_pool = rte_crypto_op_pool_create(name, RTE_CRYPTO_OP_TYPE_SYMMETRIC,
 274                 8192, 128, MAXIMUM_IV_LENGTH, rte_socket_id());
 275         PROX_PANIC(task->crypto_op_pool == NULL, "Can't create DEC CRYPTO_OP_POOL\n");
 276
 277         task->cdev_id = get_cdev_id();
 278         struct rte_cryptodev_config cdev_conf;
 279         cdev_conf.nb_queue_pairs = 2;
 280         cdev_conf.socket_id = SOCKET_ID_ANY;
 281         cdev_conf.socket_id = rte_socket_id();
 282         rte_cryptodev_configure(task->cdev_id, &cdev_conf);
 283
 284         unsigned int session_size = rte_cryptodev_sym_get_private_session_size(task->cdev_id);
 285         plog_info("rte_cryptodev_sym_get_private_session_size=%d\n", session_size);
 286         sprintf(name, "core_%03u_session_pool", lcore_id);
 287         task->session_pool = rte_mempool_create(name,
 288                                 MAX_SESSIONS,
 289                                 session_size,
 290                                 POOL_CACHE_SIZE,
 291                                 0, NULL, NULL, NULL,
 292                                 NULL, rte_socket_id(),
 293                                 0);
 294         PROX_PANIC(task->session_pool == NULL, "Failed rte_mempool_create\n");
 295
 296         task->qp_id=0;
 297         plog_info("dec: task->qp_id=%u\n", task->qp_id);
 298         struct rte_cryptodev_qp_conf qp_conf;
 299         //qp_conf.nb_descriptors = 4096;
 300         qp_conf.nb_descriptors = 128;
 301         rte_cryptodev_queue_pair_setup(task->cdev_id, task->qp_id,
 302                 &qp_conf, rte_cryptodev_socket_id(task->cdev_id), task->session_pool);
 303
 304         int ret = rte_cryptodev_start(task->cdev_id);
 305         PROX_PANIC(ret < 0, "Failed to start device\n");
 306
 307         struct rte_cryptodev *dev;
 308         dev = rte_cryptodev_pmd_get_dev(task->cdev_id);
 309         PROX_PANIC(dev->attached != RTE_CRYPTODEV_ATTACHED, "No ENC cryptodev attached\n");
 310
 311         //Setup Cipher Parameters
 312         struct rte_crypto_sym_xform cipher_xform = {0};
 313         struct rte_crypto_sym_xform auth_xform = {0};
 314
 315         cipher_xform.type = RTE_CRYPTO_SYM_XFORM_CIPHER;
 316         cipher_xform.next = NULL;
 317         cipher_xform.cipher.algo = RTE_CRYPTO_CIPHER_AES_CBC;
 318         cipher_xform.cipher.op = RTE_CRYPTO_CIPHER_OP_DECRYPT;
 319         cipher_xform.cipher.key.data = aes_cbc_key;
 320         cipher_xform.cipher.key.length = CIPHER_KEY_LENGTH_AES_CBC;
 321
 322         cipher_xform.cipher.iv.offset = IV_OFFSET;
 323         cipher_xform.cipher.iv.length = CIPHER_IV_LENGTH_AES_CBC;
 324
 325         //Setup HMAC Parameters
 326         auth_xform.type = RTE_CRYPTO_SYM_XFORM_AUTH;
 327         auth_xform.next = &cipher_xform;
 328         auth_xform.auth.op = RTE_CRYPTO_AUTH_OP_VERIFY;
 329         auth_xform.auth.algo = RTE_CRYPTO_AUTH_SHA1_HMAC;
 330         auth_xform.auth.key.length = DIGEST_BYTE_LENGTH_SHA1;
 331         auth_xform.auth.key.data = hmac_sha1_key;
 332         auth_xform.auth.digest_length = DIGEST_BYTE_LENGTH_SHA1;
 333
 334         auth_xform.auth.iv.offset = 0;
 335         auth_xform.auth.iv.length = 0;
 336
 337         task->sess = rte_cryptodev_sym_session_create(task->session_pool);
 338         PROX_PANIC(task->sess == NULL, "Failed to create ENC session\n");
 339
 340         ret = rte_cryptodev_sym_session_init(task->cdev_id, task->sess, &cipher_xform, task->session_pool);
 341         PROX_PANIC(ret < 0, "Failed sym_session_init\n");
 342
 343         //TODO: doublecheck task->ops_burst lifecycle!
 344         if (rte_crypto_op_bulk_alloc(task->crypto_op_pool,
 345                         RTE_CRYPTO_OP_TYPE_SYMMETRIC,
 346                         task->ops_burst, NUM_OPS) != NUM_OPS) {
 347                 PROX_PANIC(1, "Failed to allocate DEC crypto operations\n");
 348         }
 349
 350         task->local_ipv4 = rte_cpu_to_be_32(targ->local_ipv4);
 351         //memcpy(&task->src_mac, &prox_port_cfg[task->base.tx_params_hw.tx_port_queue->port].eth_addr, sizeof(struct ether_addr));
 352         struct prox_port_cfg *port = find_reachable_port(targ);
 353         memcpy(&task->local_mac, &port->eth_addr, sizeof(struct ether_addr));
 354
 355         if (targ->flags & TASK_ARG_DST_MAC_SET){
 356                 memcpy(&task->dst_mac, &targ->edaddr, sizeof(task->dst_mac));
 357                 plog_info("TASK_ARG_DST_MAC_SET ("MAC_BYTES_FMT")\n", MAC_BYTES(task->dst_mac.addr_bytes));
 358                 //ether_addr_copy(&ptask->dst_mac, &peth->d_addr);
 359                 //rte_memcpy(hdr, task->src_dst_mac, sizeof(task->src_dst_mac));
 360         }
 361
 362 }
 363
 364 static inline uint8_t handle_esp_ah_enc(struct task_esp_enc *task, struct rte_mbuf *mbuf, struct rte_crypto_op *cop)
 365 {
 366         u8 *data;
 367         struct ether_hdr *peth = rte_pktmbuf_mtod(mbuf, struct ether_hdr *);
 368         struct ipv4_hdr* pip4 = (struct ipv4_hdr *)(peth + 1);
 369         uint16_t ipv4_length = rte_be_to_cpu_16(pip4->total_length);
 370         struct rte_crypto_sym_op *sym_cop = cop->sym;
 371
 372         if (unlikely((pip4->version_ihl >> 4) != 4)) {
 373                 plog_info("Received non IPv4 packet at esp enc %i\n", pip4->version_ihl);
 374                 plogdx_info(mbuf, "ENC RX: ");
 375                 return OUT_DISCARD;
 376         }
 377         if (pip4->time_to_live) {
 378                 pip4->time_to_live--;
 379         }
 380         else {
 381                 plog_info("TTL = 0 => Dropping\n");
 382                 return OUT_DISCARD;
 383         }
 384
 385         // Remove padding if any (we don't want to encapsulate garbage at end of IPv4 packet)
 386         int l1 = rte_pktmbuf_pkt_len(mbuf);
 387         int padding = l1 - (ipv4_length + sizeof(struct ether_hdr));
 388         if (unlikely(padding > 0)) {
 389                 rte_pktmbuf_trim(mbuf, padding);
 390         }
 391
 392         l1 = rte_pktmbuf_pkt_len(mbuf);
 393         int encrypt_len = l1 - sizeof(struct ether_hdr) + 2; // According to RFC4303 table 1, encrypt len is ip+tfc_pad(o)+pad+pad len(1) + next header(1)
 394         padding = 0;
 395         if ((encrypt_len & 0xf) != 0){
 396                 padding = 16 - (encrypt_len % 16);
 397                 encrypt_len += padding;
 398         }
 399
 400         const int extra_space = sizeof(struct ipv4_hdr) + sizeof(struct esp_hdr) + CIPHER_IV_LENGTH_AES_CBC;
 401
 402         struct ether_addr src_mac = peth->s_addr;
 403         struct ether_addr dst_mac = peth->d_addr;
 404         uint32_t src_addr = pip4->src_addr;
 405         uint32_t dst_addr = pip4->dst_addr;
 406         uint8_t ttl = pip4->time_to_live;
 407         uint8_t version_ihl = pip4->version_ihl;
 408
 409         peth = (struct ether_hdr *)rte_pktmbuf_prepend(mbuf, extra_space); // encap + prefix
 410         peth = (struct ether_hdr *)rte_pktmbuf_append(mbuf, 0 + 1 + 1 + padding + 4 + DIGEST_BYTE_LENGTH_SHA1); // padding + pad_len + next_head + seqn + ICV pad + ICV
 411         peth = rte_pktmbuf_mtod(mbuf, struct ether_hdr *);
 412         l1 = rte_pktmbuf_pkt_len(mbuf);
 413         peth->ether_type = ETYPE_IPv4;
 414 #if 0
 415         //send it back
 416         ether_addr_copy(&dst_mac, &peth->s_addr);
 417         ether_addr_copy(&src_mac, &peth->d_addr);
 418 #else
 419         ether_addr_copy(&task->local_mac, &peth->s_addr);
 420         //ether_addr_copy(&dst_mac, &peth->d_addr);//IS: dstmac should be rewritten by arp
 421         ether_addr_copy(&task->dst_mac, &peth->d_addr);
 422 #endif
 423
 424         pip4 = (struct ipv4_hdr *)(peth + 1);
 425         pip4->src_addr = task->local_ipv4;
 426         pip4->dst_addr = task->remote_ipv4;
 427         pip4->time_to_live = ttl;
 428         pip4->next_proto_id = IPPROTO_ESP; // 50 for ESP, ip in ip next proto trailer
 429         pip4->version_ihl = version_ihl; // 20 bytes, ipv4
 430         pip4->total_length = rte_cpu_to_be_16(ipv4_length + sizeof(struct ipv4_hdr) + sizeof(struct esp_hdr) + CIPHER_IV_LENGTH_AES_CBC + padding + 1 + 1 + DIGEST_BYTE_LENGTH_SHA1); // iphdr+SPI+SN+IV+payload+padding+padlen+next header + crc + auth
 431         pip4->packet_id = 0x0101;
 432         pip4->type_of_service = 0;
 433         pip4->time_to_live = 64;
 434         prox_ip_cksum(mbuf, pip4, sizeof(struct ether_hdr), sizeof(struct ipv4_hdr), 1);
 435
 436         data = (u8*)(pip4 + 1);
 437 #if 0
 438         *((u32*) data) = 0x2016; // FIXME SPI
 439         *((u32*) data + 1) = 0x2; // FIXME SN
 440 #else
 441         struct esp_hdr *pesp = (struct esp_hdr*)(pip4+1);
 442         pesp->spi = src_addr;//for simplicity assume 1 tunnel per source ip
 443         static u32 sn = 0;
 444         pesp->sn = ++sn;
 445         pesp->spi=0xAAAAAAAA;//debug
 446         pesp->sn =0xBBBBBBBB;//debug
 447 #endif
 448         u8 *padl = (u8*)data + (8 + encrypt_len - 2 + CIPHER_IV_LENGTH_AES_CBC); // No ESN yet. (-2 means NH is crypted)
 449         //padl += CIPHER_IV_LENGTH_AES_CBC;
 450         *padl = padding;
 451         *(padl + 1) = 4; // ipv4 in 4
 452
 453         sym_cop->auth.digest.data = data + 8 + CIPHER_IV_LENGTH_AES_CBC + encrypt_len;
 454         //sym_cop->auth.digest.phys_addr = rte_pktmbuf_mtophys_offset(mbuf, (sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr) + 8 + CIPHER_IV_LENGTH_AES_CBC + encrypt_len));
 455         sym_cop->auth.digest.phys_addr = rte_pktmbuf_iova_offset(mbuf, (sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr) + 8 + CIPHER_IV_LENGTH_AES_CBC + encrypt_len));
 456         //sym_cop->auth.digest.length = DIGEST_BYTE_LENGTH_SHA1;
 457
 458         //sym_cop->cipher.iv.data = data + 8;
 459         //sym_cop->cipher.iv.phys_addr = rte_pktmbuf_mtophys(mbuf) + sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr) + 4 + 4;
 460         //sym_cop->cipher.iv.length = CIPHER_IV_LENGTH_AES_CBC;
 461
 462         //rte_memcpy(sym_cop->cipher.iv.data, aes_cbc_iv, CIPHER_IV_LENGTH_AES_CBC);
 463
 464         uint8_t *iv_ptr = rte_crypto_op_ctod_offset(cop, uint8_t *, IV_OFFSET);
 465         rte_memcpy(iv_ptr, aes_cbc_iv, CIPHER_IV_LENGTH_AES_CBC);
 466
 467 #if 0//old
 468         sym_cop->cipher.data.offset = sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr) + 4 + 4 + CIPHER_IV_LENGTH_AES_CBC;
 469         sym_cop->cipher.data.length = encrypt_len;
 470
 471         uint64_t *iv = (uint64_t *)(pesp + 1);
 472         memset(iv, 0, CIPHER_IV_LENGTH_AES_CBC);
 473 #else
 474         //uint64_t *iv = (uint64_t *)(pesp + 1);
 475         //memset(iv, 0, CIPHER_IV_LENGTH_AES_CBC);
 476         sym_cop->cipher.data.offset = sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr) + sizeof(struct esp_hdr);
 477         sym_cop->cipher.data.length = encrypt_len + CIPHER_IV_LENGTH_AES_CBC;
 478 #endif
 479
 480         sym_cop->auth.data.offset = sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr);
 481         sym_cop->auth.data.length = sizeof(struct esp_hdr) + CIPHER_IV_LENGTH_AES_CBC + encrypt_len;// + 4;// FIXME
 482
 483         sym_cop->m_src = mbuf;
 484         rte_crypto_op_attach_sym_session(cop, task->sess);
 485         //cop->type = RTE_CRYPTO_OP_TYPE_SYMMETRIC;
 486         //cop->status = RTE_CRYPTO_OP_STATUS_NOT_PROCESSED;
 487
 488         return 0;
 489 }
 490
 491 static inline uint8_t handle_esp_ah_dec(struct task_esp_dec *task, struct rte_mbuf *mbuf, struct rte_crypto_op *cop)
 492 {
 493         struct rte_crypto_sym_op *sym_cop = cop->sym;
 494         struct ether_hdr *peth = rte_pktmbuf_mtod(mbuf, struct ether_hdr *);
 495         struct ipv4_hdr* pip4 = (struct ipv4_hdr *)(peth + 1);
 496         uint16_t ipv4_length = rte_be_to_cpu_16(pip4->total_length);
 497         u8 *data = (u8*)(pip4 + 1);
 498
 499         if (pip4->next_proto_id != IPPROTO_ESP){
 500                 plog_info("Received non ESP packet on esp dec\n");
 501                 plogdx_info(mbuf, "DEC RX: ");
 502                 return OUT_DISCARD;
 503         }
 504
 505         rte_crypto_op_attach_sym_session(cop, task->sess);
 506
 507         sym_cop->auth.digest.data = (unsigned char *)((unsigned char*)pip4 + ipv4_length - DIGEST_BYTE_LENGTH_SHA1);
 508         //sym_cop->auth.digest.phys_addr = rte_pktmbuf_mtophys_offset(mbuf, sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr) + sizeof(struct esp_hdr)); // FIXME
 509         sym_cop->auth.digest.phys_addr = rte_pktmbuf_iova_offset(mbuf, sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr) + sizeof(struct esp_hdr));
 510         //sym_cop->auth.digest.length = DIGEST_BYTE_LENGTH_SHA1;
 511
 512         //sym_cop->cipher.iv.data = (uint8_t *)data + 8;
 513         //sym_cop->cipher.iv.phys_addr = rte_pktmbuf_mtophys(mbuf) + sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr) + 4 + 4;
 514         //sym_cop->cipher.iv.length = CIPHER_IV_LENGTH_AES_CBC;
 515
 516 #if 0
 517         rte_memcpy(rte_crypto_op_ctod_offset(cop, uint8_t *, IV_OFFSET),
 518                                 aes_cbc_iv,
 519                                 CIPHER_IV_LENGTH_AES_CBC);
 520 #else
 521         uint8_t * iv = (uint8_t *)(pip4 + 1) + sizeof(struct esp_hdr);
 522         rte_memcpy(rte_crypto_op_ctod_offset(cop, uint8_t *, IV_OFFSET),
 523                                 iv,
 524                                 CIPHER_IV_LENGTH_AES_CBC);
 525 #endif
 526
 527         sym_cop->auth.data.offset = sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr);
 528         sym_cop->auth.data.length = ipv4_length - sizeof(struct ipv4_hdr) - 4 - CIPHER_IV_LENGTH_AES_CBC;
 529
 530         sym_cop->cipher.data.offset = sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr) + sizeof(struct esp_hdr) + CIPHER_IV_LENGTH_AES_CBC;
 531         sym_cop->cipher.data.length = ipv4_length - sizeof(struct ipv4_hdr) - CIPHER_IV_LENGTH_AES_CBC - 28; // FIXME
 532
 533         sym_cop->m_src = mbuf;
 534         return 0;
 535 }
 536
 537 static inline void do_ipv4_swap(struct task_esp_dec *task, struct rte_mbuf *mbuf)
 538 {
 539         struct ether_hdr *peth = rte_pktmbuf_mtod(mbuf, struct ether_hdr *);
 540         struct ether_addr src_mac = peth->s_addr;
 541         struct ether_addr dst_mac = peth->d_addr;
 542         uint32_t src_ip, dst_ip;
 543
 544         struct ipv4_hdr* pip4 = (struct ipv4_hdr *)(peth + 1);
 545         src_ip = pip4->src_addr;
 546         dst_ip = pip4->dst_addr;
 547
 548         //peth->s_addr = dst_mac;
 549         peth->d_addr = src_mac;//should be replaced by arp
 550         pip4->src_addr = dst_ip;
 551         pip4->dst_addr = src_ip;
 552         ether_addr_copy(&task->local_mac, &peth->s_addr);
 553 }
 554
 555 static inline uint8_t handle_esp_ah_dec_finish(struct task_esp_dec *task, struct rte_mbuf *mbuf)
 556 {
 557         struct ether_hdr *peth = rte_pktmbuf_mtod(mbuf, struct ether_hdr *);
 558         rte_memcpy(((u8*)peth) + sizeof(struct ether_hdr), ((u8*)peth) + sizeof(struct ether_hdr) +
 559                         + sizeof(struct ipv4_hdr) + 4 + 4 + CIPHER_IV_LENGTH_AES_CBC, sizeof(struct ipv4_hdr));// next hdr, padding
 560         struct ipv4_hdr* pip4 = (struct ipv4_hdr *)(peth + 1);
 561
 562         if (unlikely((pip4->version_ihl >> 4) != 4)) {
 563                 plog_info("non IPv4 packet after esp dec %i\n", pip4->version_ihl);
 564                 plogdx_info(mbuf, "DEC TX: ");
 565                 return OUT_DISCARD;
 566         }
 567         if (pip4->time_to_live) {
 568                 pip4->time_to_live--;
 569         }
 570         else {
 571                 plog_info("TTL = 0 => Dropping\n");
 572                 return OUT_DISCARD;
 573         }
 574         uint16_t ipv4_length = rte_be_to_cpu_16(pip4->total_length);
 575         rte_memcpy(((u8*)peth) + sizeof(struct ether_hdr) + sizeof(struct ipv4_hdr),
 576                 ((u8*)peth) + sizeof(struct ether_hdr) +
 577                 + 2 * sizeof(struct ipv4_hdr) + 4 + 4 + CIPHER_IV_LENGTH_AES_CBC, ipv4_length - sizeof(struct ipv4_hdr));
 578
 579         int len = rte_pktmbuf_pkt_len(mbuf);
 580         rte_pktmbuf_trim(mbuf, len - sizeof(struct ether_hdr) - ipv4_length);
 581         peth = rte_pktmbuf_mtod(mbuf, struct ether_hdr *);
 582
 583 #if 0
 584         do_ipv4_swap(task, mbuf);
 585 #else
 586         ether_addr_copy(&task->local_mac, &peth->s_addr);
 587         ether_addr_copy(&task->dst_mac, &peth->d_addr);
 588         //rte_memcpy(peth, task->dst_mac, sizeof(task->dst_mac));
 589 #endif
 590         prox_ip_cksum(mbuf, pip4, sizeof(struct ether_hdr), sizeof(struct ipv4_hdr), 1);
 591
 592         return 0;
 593 }
 594
 595 static inline uint8_t handle_esp_ah_dec_finish2(struct task_esp_dec *task, struct rte_mbuf *mbuf)
 596 {
 597         u8* m = rte_pktmbuf_mtod(mbuf, u8*);
 598         rte_memcpy(m+sizeof(struct ipv4_hdr)+sizeof(struct esp_hdr)+CIPHER_IV_LENGTH_AES_CBC,
 599                 m, sizeof(struct ether_hdr));
 600         m = (u8*)rte_pktmbuf_adj(mbuf, sizeof(struct ipv4_hdr)+sizeof(struct esp_hdr)+CIPHER_IV_LENGTH_AES_CBC);
 601         struct ipv4_hdr* pip4 = (struct ipv4_hdr *)(m+sizeof(struct ether_hdr));
 602
 603         if (unlikely((pip4->version_ihl >> 4) != 4)) {
 604                 plog_info("non IPv4 packet after esp dec %i\n", pip4->version_ihl);
 605                 plogdx_info(mbuf, "DEC TX: ");
 606                 return OUT_DISCARD;
 607         }
 608         if (pip4->time_to_live) {
 609                 pip4->time_to_live--;
 610         }
 611         else {
 612                 plog_info("TTL = 0 => Dropping\n");
 613                 return OUT_DISCARD;
 614         }
 615         uint16_t ipv4_length = rte_be_to_cpu_16(pip4->total_length);
 616         int len = rte_pktmbuf_pkt_len(mbuf);
 617         rte_pktmbuf_trim(mbuf, len - sizeof(struct ether_hdr) - ipv4_length);
 618
 619 #if 0
 620         do_ipv4_swap(task, mbuf);
 621 #else
 622         struct ether_hdr *peth = rte_pktmbuf_mtod(mbuf, struct ether_hdr *);
 623         ether_addr_copy(&task->local_mac, &peth->s_addr);
 624         ether_addr_copy(&task->dst_mac, &peth->d_addr);
 625         //rte_memcpy(peth, task->dst_mac, sizeof(task->dst_mac));
 626 #endif
 627
 628         prox_ip_cksum(mbuf, pip4, sizeof(struct ether_hdr), sizeof(struct ipv4_hdr), 1);
 629         return 0;
 630 }
 631
 632 static int handle_esp_enc_bulk(struct task_base *tbase, struct rte_mbuf **mbufs, uint16_t n_pkts)
 633 {
 634         struct task_esp_enc *task = (struct task_esp_enc *)tbase;
 635         uint8_t out[MAX_PKT_BURST];
 636         uint16_t i = 0, nb_rx = 0, nb_enc=0, j = 0;
 637
 638         for (uint16_t j = 0; j < n_pkts; ++j) {
 639                 out[j] = handle_esp_ah_enc(task, mbufs[j], task->ops_burst[nb_enc]);
 640                 if (out[j] != OUT_DISCARD)
 641                         ++nb_enc;
 642         }
 643
 644         if (rte_cryptodev_enqueue_burst(task->cdev_id, task->qp_id, task->ops_burst, nb_enc) != nb_enc) {
 645                 plog_info("Error enc enqueue_burst\n");
 646                 return -1;
 647         }
 648
 649         do {
 650                 nb_rx = rte_cryptodev_dequeue_burst(task->cdev_id, task->qp_id, task->ops_burst+i, nb_enc-i);
 651                 i += nb_rx;
 652         } while (i < nb_enc);
 653
 654         return task->base.tx_pkt(&task->base, mbufs, n_pkts, out);
 655 }
 656
 657 static int handle_esp_dec_bulk(struct task_base *tbase, struct rte_mbuf **mbufs, uint16_t n_pkts)
 658 {
 659         struct task_esp_dec *task = (struct task_esp_dec *)tbase;
 660         uint8_t out[MAX_PKT_BURST];
 661         uint16_t j, nb_dec=0, nb_rx=0;
 662
 663         for (j = 0; j < n_pkts; ++j) {
 664                 out[j] = handle_esp_ah_dec(task, mbufs[j], task->ops_burst[nb_dec]);
 665                 if (out[j] != OUT_DISCARD)
 666                         ++nb_dec;
 667         }
 668
 669         if (rte_cryptodev_enqueue_burst(task->cdev_id, task->qp_id, task->ops_burst, nb_dec) != nb_dec) {
 670                 plog_info("Error dec enqueue_burst\n");
 671                 return -1;
 672         }
 673
 674         j=0;
 675         do {
 676                 nb_rx = rte_cryptodev_dequeue_burst(task->cdev_id, task->qp_id,
 677                                         task->ops_burst+j, nb_dec-j);
 678                 j += nb_rx;
 679         } while (j < nb_dec);
 680
 681         for (j = 0; j < nb_dec; ++j) {
 682                 if (task->ops_burst[j]->status != RTE_CRYPTO_OP_STATUS_SUCCESS){
 683                         plog_info("err: task->ops_burst[%d].status=%d\n", j, task->ops_burst[j]->status);
 684                         //!!!TODO!!! find mbuf and discard it!!!
 685                         //for now just send it further
 686                         //plogdx_info(mbufs[j], "RX: ");
 687                 }
 688                 if (task->ops_burst[j]->status == RTE_CRYPTO_OP_STATUS_SUCCESS) {
 689                         struct rte_mbuf *mbuf = task->ops_burst[j]->sym->m_src;
 690                         handle_esp_ah_dec_finish2(task, mbuf);//TODO set out[j] properly
 691                 }
 692         }
 693
 694         return task->base.tx_pkt(&task->base, mbufs, n_pkts, out);
 695 }
 696
 697 struct task_init task_init_esp_enc = {
 698         .mode = ESP_ENC,
 699         .mode_str = "esp_enc",
 700         .init = init_task_esp_enc,
 701         .handle = handle_esp_enc_bulk,
 702         .size = sizeof(struct task_esp_enc),
 703 };
 704
 705 struct task_init task_init_esp_dec = {
 706         .mode = ESP_ENC,
 707         .mode_str = "esp_dec",
 708         .init = init_task_esp_dec,
 709         .handle = handle_esp_dec_bulk,
 710         .size = sizeof(struct task_esp_dec),
 711 };
 712
 713 __attribute__((constructor)) static void reg_task_esp_enc(void)
 714 {
 715         reg_task(&task_init_esp_enc);
 716 }
 717
 718 __attribute__((constructor)) static void reg_task_esp_dec(void)
 719 {
 720         reg_task(&task_init_esp_dec);
 721 }