SolarEnergy/esp32_device/main/crypto.c

#include "crypto.h"
#include "system.h"
#include "constants.h"
#include "storage.h"
#include "dev_name.h"

#include <string.h>
#include <mbedtls/build_info.h>
#include <mbedtls/platform.h>
#include <mbedtls/entropy.h>
#include <mbedtls/ctr_drbg.h>
#include <mbedtls/ecdsa.h>
#include <mbedtls/sha256.h>
#include <mbedtls/pk.h>
#include <mbedtls/x509_csr.h>
#include <mbedtls/base64.h>
#include "esp_log.h"

#define ECPARAMS MBEDTLS_ECP_DP_SECP256R1

static const char *TAG = "crypto";

static const char *pers = "ecdsa";

static void seed_ctr_drbg_context(mbedtls_entropy_context *entropy, mbedtls_ctr_drbg_context *ctr_drbg)
{
    int ret;

    mbedtls_entropy_init(entropy);
    mbedtls_ctr_drbg_init(ctr_drbg);

    ESP_LOGI(TAG, "Seed Mbedtls");
    if ((ret = mbedtls_ctr_drbg_seed(ctr_drbg, mbedtls_entropy_func, entropy,
                                     (const unsigned char *)pers,
                                     strlen(pers))) != 0)
    {
        ESP_LOGE(TAG, " failed\n  ! mbedtls_ctr_drbg_seed returned %d", ret);
        reboot();
    }
}

bool crypto_gen_priv_key()
{
    // Check if a private key has already been defined for this device
    if (storage_get_priv_key(NULL) > 0)
        return false;

    int ret = 1;

    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, "PK info from type");
    if ((ret = mbedtls_pk_setup(&key, mbedtls_pk_info_from_type(MBEDTLS_PK_ECKEY))) != 0)
    {
        ESP_LOGE(TAG, " failed\n  !  mbedtls_pk_setup returned -0x%04x", (unsigned int)-ret);
        reboot();
    }

    // Generate private key
    ESP_LOGI(TAG, "Generate private key");
    ret = mbedtls_ecp_gen_key(ECPARAMS,
                              mbedtls_pk_ec(key),
                              mbedtls_ctr_drbg_random, &ctr_drbg);
    if (ret != 0)
    {
        ESP_LOGE(TAG, " failed\n  !  mbedtls_ecp_gen_key returned -0x%04x",
                 (unsigned int)-ret);
        reboot();
    }

    // Export private key
    ESP_LOGI(TAG, "Export private key");
    unsigned char *key_buff = malloc(PRV_KEY_DER_MAX_BYTES);
    if ((ret = mbedtls_pk_write_key_der(&key, key_buff, PRV_KEY_DER_MAX_BYTES)) < 1)
    {
        ESP_LOGE(TAG, " failed\n  !  mbedtls_pk_write_key_der returned -0x%04x",
                 (unsigned int)-ret);
        reboot();
    }

    storage_set_priv_key(key_buff + PRV_KEY_DER_MAX_BYTES - ret, ret);
    free(key_buff);

    mbedtls_pk_free(&key);
    mbedtls_ctr_drbg_free(&ctr_drbg);
    mbedtls_entropy_free(&entropy);

    return true;
}

void crypto_print_priv_key()
{
    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)", 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, "Show private key");
    unsigned char *out = malloc(16000);
    memset(out, 0, 16000);
    if ((ret = mbedtls_pk_write_key_pem(&key, out, 16000)) != 0)
    {
        ESP_LOGE(TAG, " failed\n  !  mbedtls_pk_write_key_pem returned -0x%04x",
                 (unsigned int)-ret);
        reboot();
    }

    ESP_LOGI(TAG, "%s", out);
    free(out);

    mbedtls_pk_free(&key);
    mbedtls_ctr_drbg_free(&ctr_drbg);
    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)", 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;

    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)", 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);

    // Create CSR
    mbedtls_x509write_csr req;
    mbedtls_x509write_csr_init(&req);
    mbedtls_x509write_csr_set_md_alg(&req, MBEDTLS_MD_SHA256);

    char subj[DEV_NAME_LEN + 4];
    char *n = dev_name();
    sprintf(subj, "CN=%s", n);
    free(n);
    if ((ret = mbedtls_x509write_csr_set_subject_name(&req, subj)) != 0)
    {
        ESP_LOGE(TAG, " failed\n  !  mbedtls_x509write_csr_set_subject_name returned %d", ret);
        reboot();
    }

    ESP_LOGI(TAG, "Sign CSR with private key");
    mbedtls_x509write_csr_set_key(&req, &key);

    char *csr = malloc(4096);
    if ((ret = mbedtls_x509write_csr_pem(&req, (u_char *)csr, 4096, mbedtls_ctr_drbg_random, &ctr_drbg)) < 0)
    {
        ESP_LOGE(TAG, " failed\n  !  mbedtls_x509write_csr_pem returned %d", ret);
        reboot();
    }

    mbedtls_x509write_csr_free(&req);
    mbedtls_pk_free(&key);
    mbedtls_ctr_drbg_free(&ctr_drbg);
    mbedtls_entropy_free(&entropy);

    return csr;
}

char *crypto_encode_base64_safe_url(const char *src, size_t srclen)
{
    size_t olen = 0;
    mbedtls_base64_encode(NULL, 0, &olen, (unsigned char *)src, srclen);

    if (olen < 1)
    {
        ESP_LOGE(TAG, "Failed to determine base64 buffer size!");
        return NULL;
    }

    char *out = calloc(1, olen);
    if (!out)
    {
        ESP_LOGE(TAG, "Failed to allocate memory for destination buffer!");
        return NULL;
    }

    if (mbedtls_base64_encode((unsigned char *)out, olen, &olen, (unsigned char *)src, srclen) != 0)
    {
        ESP_LOGE(TAG, "Failed to perfom base64 encoding!");
        free(out);
        return NULL;
    }

    // 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;
}

#define HASH_LEN 32

char *crypto_sign_sha256_payload(const char *src, const size_t src_len, size_t *dstlen)
{
    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;
}