outbound aes cbc encryption working.

Author: philljj

scripts/ip-xfrm/README.md

@@ -14,7 +14,7 @@ Some convience scripts and config for testing IPsec with wolfIP:
 Build wolfssl with:
 
 ```sh
-./configure --enable-cryptonly --enable-sha --enable-sha256 --enable-md5
+./configure --enable-cryptonly --enable-sha --enable-sha256 --enable-md5 --enable-des3
   make
   sudo make install
 ```

src/wolfesp.c

@@ -51,6 +51,25 @@ struct wolfIP_esp_sa {
     uint8_t    icv_len;
 };
 
+static WC_RNG  wc_rng;
+static uint8_t rng_inited = 0;
+
+static void
+esp_init(void)
+{
+    if (rng_inited == 0) {
+        int ret = wc_InitRng_ex(&wc_rng, NULL, INVALID_DEVID);
+
+        if (ret) {
+            printf("error: wc_InitRng_ex returned: %d\n", ret);
+        }
+
+        rng_inited = 1;
+    }
+
+    return;
+}
+
 /* Static pre-defined SA lists.*/
 
 struct wolfIP_esp_sa in_sa_list[WOLFIP_ESP_NUM_SA] =
@@ -91,7 +110,7 @@ struct wolfIP_esp_sa out_sa_list[WOLFIP_ESP_NUM_SA] =
     {
         {0xf6, 0xe9, 0xb8, 0x0d}, /* spi */
         0x020A0A0A, /* src */
-        0x010A0A0A, /* dst */
+        0x030A0A0A, /* dst */
         0,0, /* oseq, seq */
         0, /* iv len */
         0, {0}, 0, /* null enc */
@@ -258,6 +277,31 @@ static void wolfIP_print_esp(const struct wolfIP_esp_sa * esp_sa,
 }
 #endif /* WOLFIP_DEBUG_ESP */
 
+uint8_t
+esp_block_len_from_enc(esp_enc_t enc)
+{
+    uint8_t block_len = 0;
+
+    switch (enc) {
+    case ESP_ENC_NONE:
+        block_len = 0;
+        break;
+    case ESP_ENC_CBC_AES:
+        block_len = WC_AES_BLOCK_SIZE;
+        break;
+    case ESP_ENC_CBC_DES3:
+        block_len = DES_BLOCK_SIZE;
+        break;
+    case ESP_ENC_GCM_RFC4106:
+    case ESP_ENC_GCM_RFC4543:
+    default:
+        block_len = 0;
+        break;
+    }
+
+    return block_len;
+}
+
 /*
  * esp_data covers from start of ESP header to end of ESP trailer, but does not
  * include the ESP ICV after trailer.
@@ -415,6 +459,68 @@ esp_aes_rfc3602_dec(const struct wolfIP_esp_sa * esp_sa, uint8_t * esp_data,
     return ret;
 }
 
+static int
+esp_aes_rfc3602_enc(const struct wolfIP_esp_sa * esp_sa, uint8_t * esp_data,
+                    uint32_t esp_len)
+{
+    Aes          cbc_enc;
+    int          ret = -1;
+    uint8_t      icv_len = esp_sa->icv_len;
+    uint8_t      iv_len = esp_sa->iv_len;
+    uint8_t *    enc_payload = NULL;
+    uint8_t *    iv = NULL;
+    uint16_t     enc_len = 0;
+    uint8_t      inited = 0;
+
+    #ifdef WOLFIP_DEBUG_ESP
+    printf("info: aes cbc enc\n");
+    #endif /* WOLFIP_DEBUG_ESP */
+
+    enc_len = esp_enc_len(esp_len, iv_len, icv_len);
+    enc_payload = esp_enc_payload(esp_data, iv_len);
+    iv = esp_enc_iv(esp_data, iv_len);
+
+    /* Generate random iv block for cbc method. */
+    ret = wc_RNG_GenerateBlock(&wc_rng, iv, iv_len);
+
+    if (ret) {
+        printf("error: wc_RNG_GenerateBlock returned: %d\n", ret);
+        goto aes_enc_out;
+    }
+
+    ret = wc_AesInit(&cbc_enc, NULL, INVALID_DEVID);
+
+    if (ret != 0) {
+        printf("error: wc_AesInit returned: %d\n", ret);
+        goto aes_enc_out;
+    }
+
+    inited = 1;
+    ret = wc_AesSetKey(&cbc_enc, esp_sa->enc_key, AES_BLOCK_SIZE,
+                       iv, AES_ENCRYPTION);
+
+    if (ret != 0) {
+        printf("error: wc_AesSetKey returned: %d\n", ret);
+        goto aes_enc_out;
+    }
+
+    ret = wc_AesCbcEncrypt(&cbc_enc, enc_payload, enc_payload, enc_len);
+
+    if (ret != 0) {
+        printf("error: wc_AesCbcEncrypt returned: %d\n", ret);
+        goto aes_enc_out;
+    }
+
+aes_enc_out:
+    if (inited) {
+        wc_AesFree(&cbc_enc);
+        inited = 0;
+    }
+
+    return ret;
+}
+
+
 /**
  * esp_data covers from start of ESP header to end of ESP trailer, but does not
  * include the ESP ICV after trailer.
@@ -645,15 +751,17 @@ static int esp_unwrap(struct wolfIP *s, struct wolfIP_ip_packet *ip,
  * */
 static int esp_wrap(struct wolfIP_ip_packet *ip, uint16_t * ip_len)
 {
+    uint8_t   block_len = 0;
     uint16_t  orig_ip_len = *ip_len;
     uint16_t  orig_payload_len = orig_ip_len - IP_HEADER_LEN;
-    uint16_t  payload_len = orig_ip_len - IP_HEADER_LEN;
+    uint16_t  payload_len = 0;
     uint8_t * payload = ip->data;
     uint8_t   pad_len = 0;
     uint32_t  seq_n = 0; /* sequence num in network order */
     uint16_t  icv_offset = 0;
     struct wolfIP_esp_sa * esp_sa = NULL;
 
+    /* TODO: priority, tcp/udp port-filtering? */
     for (size_t i = 0; i < WOLFIP_ESP_NUM_SA; ++i) {
         if (ip->dst == out_sa_list[i].dst) {
             esp_sa = &out_sa_list[i];
@@ -685,7 +793,7 @@ static int esp_wrap(struct wolfIP_ip_packet *ip, uint16_t * ip_len)
     return 0;
     #endif
 
-   /* move ip payload back to make room for ESP header (SPI, SEQ, IV). */
+   /* move ip payload back to make room for ESP header (SPI, SEQ) + IV. */
     memmove(ip->data + ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len,
             ip->data, orig_payload_len);
 
@@ -699,25 +807,43 @@ static int esp_wrap(struct wolfIP_ip_packet *ip, uint16_t * ip_len)
     payload += ESP_SEQ_LEN;
 
     if (esp_sa->iv_len) {
+        /* skip iv field, will generate later. */
         payload += esp_sa->iv_len;
     }
 
-    payload_len += ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len;
+    block_len = esp_block_len_from_enc(esp_sa->enc);
+
+    if (block_len) {
+        /* Block cipher. Calculate padding and encrypted length, then
+         * icv_offset. */
+        uint32_t enc_len = 0;
+        enc_len = esp_sa->iv_len + orig_payload_len + pad_len
+                  + ESP_PADDING_LEN + ESP_NEXT_HEADER_LEN;
+
+        /* Determine padding. This needs to be flexible for
+         * des3 (8 byte) or aes (16 byte) block sizes.*/
+        if (enc_len % block_len) {
+            pad_len = block_len - (enc_len % block_len);
+        }
 
-    {
-        /* Stream cipher or auth-only. Calculate the icv offset. */
+        icv_offset = ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len
+                   + orig_payload_len + pad_len + ESP_PADDING_LEN
+                   + ESP_NEXT_HEADER_LEN;
+    }
+    else {
+        /* Stream cipher or auth-only. Calculate the icv offset directly. */
         icv_offset = ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len
                    + orig_payload_len + pad_len + ESP_PADDING_LEN
                    + ESP_NEXT_HEADER_LEN;
 
         /* Determine padding. */
         if (icv_offset % ESP_ICV_ALIGNMENT) {
-          pad_len = ESP_ICV_ALIGNMENT - (icv_offset % ESP_ICV_ALIGNMENT);
-          icv_offset += pad_len;
+            pad_len = ESP_ICV_ALIGNMENT - (icv_offset % ESP_ICV_ALIGNMENT);
+            icv_offset += pad_len;
         }
     }
 
-    /* Skip past the original payload. */
+    /* Skip past the original payload, add padding. */
     payload += orig_payload_len;
 
     if (pad_len) {
@@ -727,22 +853,48 @@ static int esp_wrap(struct wolfIP_ip_packet *ip, uint16_t * ip_len)
         }
 
         payload += pad_len;
-        payload_len += pad_len;
     }
 
-    /* Copy in padding len and next header. */
+    /* ESP trailer. Copy in padding len and next header fields. */
     memcpy(payload, &pad_len, ESP_PADDING_LEN);
     payload += ESP_PADDING_LEN;
-    payload_len += ESP_PADDING_LEN;
 
     memcpy(payload, &ip->proto, ESP_NEXT_HEADER_LEN);
     payload += ESP_NEXT_HEADER_LEN;
-    payload_len += ESP_NEXT_HEADER_LEN;
+
+    /* calculate final esp payload length. */
+    payload_len =  orig_ip_len - IP_HEADER_LEN;
+    payload_len += ESP_SPI_LEN + ESP_SEQ_LEN + esp_sa->iv_len +
+                   pad_len + ESP_PADDING_LEN + ESP_NEXT_HEADER_LEN +
+                   esp_sa->icv_len;
+
+    /* encrypt from payload to end of ESP trailer. */
+    if (esp_sa->iv_len) {
+        int err =  -1;
+
+        switch(esp_sa->enc) {
+        case ESP_ENC_CBC_AES:
+            err = esp_aes_rfc3602_enc(esp_sa, ip->data, payload_len);
+            break;
+
+        case ESP_ENC_NONE:
+        default:
+            printf("error: encrypt: invalid enc: %d\n", esp_sa->enc);
+            err = -1;
+            break;
+        }
+
+        if (err) {
+            printf("error: esp_encrypt(%02x) returned: %d\n", esp_sa->enc, err);
+            return -1;
+        }
+
+        /* Payload is now encrypted. Now calculate ICV.  */
+    }
 
     if (esp_sa->icv_len) {
         uint8_t * icv = NULL;
         int       rc = 0;
-        payload_len += esp_sa->icv_len;
 
         icv = ip->data + icv_offset;
 

src/wolfip.c

@@ -1993,6 +1993,10 @@ static int arp_lookup(struct wolfIP *s, ip4 ip, uint8_t *mac)
 void wolfIP_init(struct wolfIP *s)
 {
     memset(s, 0, sizeof(struct wolfIP));
+
+    #ifdef WOLFIP_ESP
+    esp_init();
+    #endif /* WOLFIP_ESP */
 }
 
 struct ll *wolfIP_getdev(struct wolfIP *s)
@@ -2007,6 +2011,10 @@ void wolfIP_init_static(struct wolfIP **s)
         return;
     wolfIP_init(&wolfIP_static);
     *s = &wolfIP_static;
+
+    #ifdef WOLFIP_ESP
+    esp_init();
+    #endif /* WOLFIP_ESP */
 }
 
 #ifdef DEBUG_IP