From b5acda292d565fbad8827d2988262fda4ad16a04 Mon Sep 17 00:00:00 2001
From: Pierre HUBERT <pierre.git@communiquons.org>
Date: Fri, 27 Sep 2024 21:28:05 +0200
Subject: [PATCH] Sign JWT

---
 esp32_device/main/crypto.c | 131 ++++++++++++++++++++++++++++++++++---
 esp32_device/main/crypto.h |   2 +-
 2 files changed, 124 insertions(+), 9 deletions(-)

diff --git a/esp32_device/main/crypto.c b/esp32_device/main/crypto.c
index 24a0985..f3d8953 100644
--- a/esp32_device/main/crypto.c
+++ b/esp32_device/main/crypto.c
@@ -135,6 +135,46 @@ void crypto_print_priv_key()
     mbedtls_entropy_free(&entropy);
 }
 
+/**
+ * Get secret point value of our private key
+ */
+static bool crypto_get_priv_key_mpi(mbedtls_mpi *dst)
+{
+    int ret;
+
+    unsigned char *key_buff = malloc(PRV_KEY_DER_MAX_BYTES);
+    size_t key_len = storage_get_priv_key(key_buff);
+    assert(key_len > 0);
+
+    mbedtls_pk_context key;
+    mbedtls_pk_init(&key);
+
+    mbedtls_entropy_context entropy;
+    mbedtls_ctr_drbg_context ctr_drbg;
+    seed_ctr_drbg_context(&entropy, &ctr_drbg);
+
+    ESP_LOGI(TAG, "Parse private key (len = %d)\n", key_len);
+    if ((ret = mbedtls_pk_parse_key(&key, key_buff, key_len, NULL, 0, mbedtls_ctr_drbg_random, &ctr_drbg)) != 0)
+    {
+        ESP_LOGE(TAG, " failed\n  !  mbedtls_pk_parse_key returned -0x%04x",
+                 (unsigned int)-ret);
+        reboot();
+    }
+    free(key_buff);
+
+    ESP_LOGI(TAG, "Extract private key");
+    mbedtls_ecp_keypair *kp = mbedtls_pk_ec(key);
+
+    mbedtls_mpi_init(dst);
+    mbedtls_mpi_copy(dst, &kp->private_d);
+
+    mbedtls_pk_free(&key);
+    mbedtls_ctr_drbg_free(&ctr_drbg);
+    mbedtls_entropy_free(&entropy);
+
+    return true;
+}
+
 char *crypto_get_csr()
 {
     int ret;
@@ -217,17 +257,92 @@ char *crypto_encode_base64_safe_url(const char *src, size_t srclen)
         return NULL;
     }
 
-    if (out[olen - 1] == '=')
-        out[olen - 1] = '\0';
-
-    if (out[olen - 2] == '=')
-        out[olen - 2] = '\0';
+    // Convert base64 encoding to base64URL
+    for (size_t i = 0; i < olen; i++)
+    {
+        switch (out[i])
+        {
+        case '+':
+            out[i] = '-';
+            break;
+        case '/':
+            out[i] = '_';
+            break;
+        case '=':
+            out[i] = '\0';
+            break;
+        }
+    }
 
     return out;
 }
 
-char *crypto_sign_sha256_payload(const char *src, const size_t src_len, size_t *srclen)
+#define HASH_LEN 32
+
+char *crypto_sign_sha256_payload(const char *src, const size_t src_len, size_t *dstlen)
 {
-    *srclen = 10;
-    return calloc(1, 10);
+    int ret;
+    uint8_t r_be[32] = {0};
+    uint8_t s_be[32] = {0};
+
+    // Load private key
+    mbedtls_mpi key_mpi;
+    if (!crypto_get_priv_key_mpi(&key_mpi))
+    {
+        ESP_LOGE(TAG, "Failed to load private key MPI!");
+        return NULL;
+    }
+
+    mbedtls_entropy_context entropy;
+    mbedtls_ctr_drbg_context ctr_drbg;
+    seed_ctr_drbg_context(&entropy, &ctr_drbg);
+
+    mbedtls_mpi r, s;
+    mbedtls_mpi_init(&r);
+    mbedtls_mpi_init(&s);
+
+    mbedtls_ecdsa_context ecdsa_context;
+    mbedtls_ecdsa_init(&ecdsa_context);
+
+    mbedtls_ecp_group_load(&ecdsa_context.MBEDTLS_PRIVATE(grp), MBEDTLS_ECP_DP_SECP256R1);
+
+    // Compute sha256
+    uint8_t sha256_out[HASH_LEN] = {0};
+    mbedtls_sha256((unsigned char *)src, src_len, sha256_out, 0);
+
+    // Compute signature
+    ret = mbedtls_ecdsa_sign(&ecdsa_context.MBEDTLS_PRIVATE(grp), &r, &s, &key_mpi, sha256_out, HASH_LEN, mbedtls_ctr_drbg_random, &ctr_drbg);
+
+    // Extract R & S (as per RFC 7518)
+    mbedtls_mpi_write_binary(&r, r_be, 32);
+    mbedtls_mpi_write_binary(&s, s_be, 32);
+
+    mbedtls_ctr_drbg_free(&ctr_drbg);
+    mbedtls_entropy_free(&entropy);
+
+    mbedtls_mpi_free(&r);
+    mbedtls_mpi_free(&s);
+    mbedtls_mpi_free(&key_mpi);
+
+    mbedtls_ecdsa_free(&ecdsa_context);
+
+    if (ret != 0)
+    {
+        ESP_LOGE(TAG, "Failed to perfom base64 encoding!");
+        return NULL;
+    }
+
+    // Prepare output
+    char *out = calloc(1, 64);
+    if (!out)
+    {
+        ESP_LOGE(TAG, "Failed to allocate memory to store signature!");
+        return NULL;
+    }
+
+    memcpy(out, &r_be, 32);
+    memcpy(out + 32, &s_be, 32);
+
+    *dstlen = 64;
+    return out;
 }
\ No newline at end of file
diff --git a/esp32_device/main/crypto.h b/esp32_device/main/crypto.h
index 63e5474..25d2f3d 100644
--- a/esp32_device/main/crypto.h
+++ b/esp32_device/main/crypto.h
@@ -42,7 +42,7 @@ extern "C"
     /**
      * Sign some data using sha256
      */
-    char *crypto_sign_sha256_payload(const char *src, const size_t src_len, size_t *srclen);
+    char *crypto_sign_sha256_payload(const char *src, const size_t src_len, size_t *dstlen);
 
 #ifdef __cplusplus
 }