Adding unwrap(), to decrypt and verify secure APDU.

Signed-off-by: Pol Henarejos <pol.henarejos@cttc.es>

src/hsm/eac.c

@@ -16,6 +16,7 @@
  */
 
 #include "eac.h"
+#include "sc_hsm.h"
 #include "crypto_utils.h"
 #include "random.h"
 #include "mbedtls/cmac.h"
@@ -24,7 +25,10 @@ static uint8_t nonce[8];
 static uint8_t auth_token[8];
 static uint8_t sm_kmac[16];
 static uint8_t sm_kenc[16];
-static MSE_protocol sm_protocol;
+static MSE_protocol sm_protocol = MSE_NONE;
+static mbedtls_mpi sm_mSSC;
+static uint8_t sm_blocksize = 0;
+static uint8_t sm_iv[16];
 
 bool is_secured_apdu() {
     return (CLA(apdu) & 0xC);
@@ -47,10 +51,18 @@ void sm_derive_all_keys(const uint8_t *derived, size_t derived_len) {
     memcpy(nonce, random_bytes_get(8), 8);
     sm_derive_key(derived, derived_len, 1, nonce, sizeof(nonce), sm_kenc);
     sm_derive_key(derived, derived_len, 2, nonce, sizeof(nonce), sm_kmac);
+    mbedtls_mpi_init(&sm_mSSC);
+    mbedtls_mpi_grow(&sm_mSSC, sm_blocksize);
+    mbedtls_mpi_lset(&sm_mSSC, 0);
+    memset(sm_iv, 0, sizeof(sm_iv));
 }
 
 void sm_set_protocol(MSE_protocol proto) {
     sm_protocol = proto;
+    if (proto == MSE_AES)
+        sm_blocksize = 16;
+    else if (proto == MSE_3DES)
+        sm_blocksize = 8;
 }
 
 MSE_protocol sm_get_protocol() {
@@ -65,3 +77,131 @@ int sm_sign(uint8_t *in, size_t in_len, uint8_t *out) {
     return mbedtls_cipher_cmac(mbedtls_cipher_info_from_type(MBEDTLS_CIPHER_AES_128_ECB), sm_kmac, 128, in, in_len, out);
 }
 
+int sm_unwrap() {
+    uint8_t sm_indicator = (CLA(apdu) >> 2) & 0x3;
+    if (sm_indicator == 0)
+        return HSM_OK;
+    int r = sm_verify();
+    if (r != HSM_OK)
+        return r;
+    int le = sm_get_le();
+    if (le >= 0)
+        apdu.expected_res_size = le;
+    const uint8_t *p = apdu.cmd_apdu_data;
+    uint8_t *body = NULL;
+    size_t body_size = 0;
+    bool is87 = false;
+    while (p-apdu.cmd_apdu_data < apdu.cmd_apdu_data_len) {
+        uint8_t tag = *p++;
+        uint8_t tag_len = *p++;
+        if (tag == 0x87 || tag == 0x85) {
+            body = (uint8_t *)p;
+            body_size = tag_len;
+            if (tag == 0x87) {
+                is87 = true;
+                body_size--;
+            }
+        }
+        p += tag_len;
+    }
+    if (!body)
+        return HSM_WRONG_DATA;
+    if (is87 && *body++ != 0x1) {
+        return HSM_WRONG_PADDING;
+    }
+    sm_update_iv();
+    aes_decrypt(sm_kenc, sm_iv, 128, HSM_AES_MODE_CBC, body, body_size);
+    memmove(apdu.cmd_apdu_data, body, body_size);
+    apdu.cmd_apdu_data_len = sm_remove_padding(apdu.cmd_apdu_data, body_size);
+    return HSM_OK;
+}
+
+int sm_get_le() {
+    const uint8_t *p = apdu.cmd_apdu_data;
+    while (p-apdu.cmd_apdu_data < apdu.cmd_apdu_data_len) {
+        uint8_t tag = *p++;
+        uint8_t tag_len = *p++;
+        if (tag == 0x97) {
+            uint32_t le = 0;
+            for (int t = 1; t <= tag_len; t++)
+                le |= (*p++) << (tag_len-t);
+            return le;
+        }
+        p += tag_len;
+    }
+    return -1;
+}
+
+void sm_update_iv() {
+    uint8_t tmp_iv[16];
+    mbedtls_mpi_write_binary(&sm_mSSC, tmp_iv, sizeof(tmp_iv));
+    aes_encrypt(sm_kenc, sm_iv, 128, HSM_AES_MODE_CBC, tmp_iv, sizeof(tmp_iv));
+    memcpy(sm_iv, tmp_iv, sizeof(tmp_iv));
+}
+
+int sm_verify() {
+    uint8_t input[1024];
+    memset(input, 0, sizeof(input));
+    int input_len = 0, r = 0;
+    bool add_header = (CLA(apdu) & 0xC) == 0xC;
+    int data_len = (int)(apdu.cmd_apdu_data_len/sm_blocksize)*sm_blocksize;
+    if (data_len % sm_blocksize)
+        data_len += sm_blocksize;
+    if (data_len+(add_header ? sm_blocksize : 0) > 1024)
+        return HSM_WRONG_LENGTH;
+    mbedtls_mpi ssc;
+    mbedtls_mpi_init(&ssc);
+    mbedtls_mpi_add_int(&ssc, &sm_mSSC, 1);
+    mbedtls_mpi_copy(&sm_mSSC, &ssc);
+    r = mbedtls_mpi_write_binary(&ssc, input, sm_blocksize);
+    input_len += sm_blocksize;
+    mbedtls_mpi_free(&ssc);
+    if (r != 0)
+        return HSM_EXEC_ERROR;
+    if (add_header) {
+        input[input_len++] = CLA(apdu);
+        input[input_len++] = INS(apdu);
+        input[input_len++] = P1(apdu);
+        input[input_len++] = P2(apdu);
+        input[input_len++] = 0x80;
+        input_len += sm_blocksize-5;
+    }
+    bool some_added = false;
+    const uint8_t *p = apdu.cmd_apdu_data, *mac = NULL;
+    size_t mac_len = 0;
+    while (p-apdu.cmd_apdu_data < apdu.cmd_apdu_data_len) {
+        uint8_t tag = *p++;
+        uint8_t tag_len = *p++;
+        if (tag & 0x1) {
+            memcpy(input+input_len, p-2, tag_len+2);
+            input_len += tag_len+2;
+            some_added = true;
+        }
+        if (tag == 0x8E) {
+            mac = p;
+            mac_len = tag_len;
+        }
+        p += tag_len;
+    }
+    if (!mac)
+        return HSM_WRONG_DATA;
+    if (some_added) {
+        input[input_len++] = 0x80;
+        input_len += (sm_blocksize - (input_len%sm_blocksize));
+    }
+    uint8_t signature[16];
+    r = sm_sign(input, input_len, signature);
+    if (r != 0)
+        return HSM_EXEC_ERROR;
+    if (memcmp(signature, mac, mac_len) == 0)
+        return HSM_OK;
+    return HSM_VERIFICATION_FAILED;
+}
+
+int sm_remove_padding(const uint8_t *data, size_t data_len) {
+    int i = data_len-1;
+    for (; i >= 0 && data[i] == 0; i--);
+    if (i < 0 || data[i] != 0x80)
+        return -1;
+    return i;
+}

src/hsm/eac.h

@@ -33,5 +33,10 @@ extern void sm_set_protocol(MSE_protocol proto);
 extern MSE_protocol sm_get_protocol();
 extern uint8_t *sm_get_nonce();
 extern int sm_sign(uint8_t *in, size_t in_len, uint8_t *out);
+int sm_verify();
+void sm_update_iv();
+int sm_get_le();
+extern int sm_unwrap();
+int sm_remove_padding(const uint8_t *data, size_t data_len);
 
 #endif

src/hsm/sc_hsm.c

@@ -1963,6 +1963,7 @@ static const cmd_t cmds[] = {
 };
 
 int sc_hsm_process_apdu() {
+    int r = sm_unwrap();
     for (const cmd_t *cmd = cmds; cmd->ins != 0x00; cmd++) {
         if (cmd->ins == INS(apdu))
             return cmd->cmd_handler();

src/hsm/sc_hsm.h

@@ -93,6 +93,7 @@ extern const uint8_t sc_hsm_aid[];
 #define HSM_WRONG_DKEK                      -1009
 #define HSM_WRONG_SIGNATURE                 -1010
 #define HSM_WRONG_PADDING                   -1011
+#define HSM_VERIFICATION_FAILED             -1012
 
 #define ALGO_RSA_RAW			0x20		/* RSA signature with external padding */
 #define ALGO_RSA_DECRYPT		0x21		/* RSA decrypt */