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