Added non-volatile keystore

keystore.c

@@ -1,10 +1,18 @@
+/*
+ * Copyright (C) 2020 Dyne.org foundation
+ *
+ * This file is subject to the terms and conditions of the GNU
+ * General Public License (GPL) version 2. See the file LICENSE
+ * for more details.
+ *
+ */
 #include <stdint.h>
 #include <string.h>
 #include <stdio.h>
 #include "dp3t-config.h"
 #include "dp3t.h"
 #include "random.h"
-
+#include "nvmc.h"
 
 static uint8_t SKT_0[SK_LEN] = {};
 static int keystore_initialized = 0;
@@ -18,6 +26,15 @@ static uint32_t broadcast_key_len = BROADCAST_KEY_LEN;
 static uint8_t local_beacons_buffer[BEACONS_OBJ_SIZE];
 static beacons_t *beacons = (beacons_t *)(local_beacons_buffer);
 
+#define CONFIG_FLASH_ADDR ((512 - 4)  * (1024)) /* Last sector of flash considering a 512KB boundary */
+
+const char dp3t_config_signature[4] = "DP3T";
+
+struct __attribute__((packed)) dp3t_config_store {
+    char signature[4]; /* spells "DP3T" if the store is in use */
+    sk_t     key;      /* oldest key stored */
+    uint32_t age;      /* age in days */
+};
 
 static void print_hex(const uint8_t *x, int len)
 {
@@ -48,14 +65,6 @@ void dp3t_print_ephids(void)
     }
 }
 
-uint8_t *dp3t_get_skt_0(void)
-{
-    if (!keystore_initialized) {
-        sys_random(SKT_0, SK_LEN);
-        keystore_initialized = 1;
-    }
-    return SKT_0;
-}
 
 beacons_t *dp3t_generate_beacons(sk_t key, int day)
 {
@@ -68,9 +77,130 @@ beacons_t *dp3t_generate_beacons(sk_t key, int day)
     return beacons;
 }
 
+
 uint8_t *dp3t_get_ephid(uint8_t idx)
 {
+    static uint8_t zero16[16] = {};
+    if (!keystore_initialized)
+        return zero16;
     return beacons->ephids[idx];
 }
 
 
+static struct dp3t_config_store *load_config(void)
+{
+    struct dp3t_config_store *store = 
+        (struct dp3t_config_store *)(CONFIG_FLASH_ADDR);
+    if (strncmp(store->signature, dp3t_config_signature, 4) == 0) {
+        printf("DP-3T configuration loaded from NVMC\n");
+        print_sk(store->key);
+        return store;
+    }
+    return NULL;
+}
+
+static void store_config(struct dp3t_config_store *sto)
+{
+    flash_erase(CONFIG_FLASH_ADDR, FLASH_PAGE_SIZE);
+    flash_write(CONFIG_FLASH_ADDR, (uint8_t *)sto, sizeof(struct dp3t_config_store));
+    printf("DP-3T configuration stored to NVMC\n");
+}
+
+uint8_t *dp3t_get_skt_0(void)
+{
+    if (keystore_initialized)
+        return SKT_0;
+    else return NULL;
+}
+
+
+static int rekey(void)
+{
+    struct dp3t_config_store *sto, newsto;
+    sys_random(newsto.key, SK_LEN);
+    memcpy(newsto.signature, dp3t_config_signature, 4);
+    memcpy(newsto.key, newsto.key, SK_LEN);
+    newsto.age = 0;
+    store_config(&newsto);
+    /* Sanity check for the new configuration */
+    sto = load_config();
+    if (!sto)
+        return -1;
+    return 0;
+}
+
+
+/* Initialize dp3t subsystem.
+ * returns: 0 : Okay
+ *         -1 : Problems with nvm store
+ *         -2 : Could not generate beacons
+ */
+int dp3t_start(void)
+{
+    struct dp3t_config_store *sto;
+    int ret;
+    if (keystore_initialized)
+        return 0;
+    sto = load_config();
+    if (!sto) {
+        /* Create new random SK0, set age to 0 */
+        rekey();
+        sto = load_config();
+        if (!sto)
+            return -1;
+    }
+    memset(beacons, 0, BEACONS_OBJ_SIZE);
+    ret = generate_beacons(beacons, EPOCHS, sto->key, sto->age, TTL, broadcast_key, broadcast_key_len); 
+    if (ret != 0)
+        return -2;
+    keystore_initialized = 1;
+    return 0;
+}
+
+int dp3t_restart(unsigned int day)
+{
+    struct dp3t_config_store *sto, newsto;
+    int ret;
+    if (keystore_initialized) {
+        sto = load_config();
+        if (sto != NULL) {
+            /* don't update if the age is the same */
+            if (sto->age == day)
+                return 0;
+            memcpy(&newsto, sto, sizeof(struct dp3t_config_store));
+        } else {
+            /* Generate new random key, don't set age */
+            sys_random(SKT_0, SK_LEN);
+            memcpy(newsto.key, SKT_0, SK_LEN);
+        }
+        keystore_initialized = 0;
+    } else {
+        sto = &newsto;
+    }
+    /* Store new configuration */
+    memcpy(newsto.signature, dp3t_config_signature, 4);
+    newsto.age = day;
+    store_config(&newsto);
+    /* Sanity check */
+    if (!load_config())
+        return -1;
+    /* Generate beacons for the day */
+    ret = generate_beacons(beacons, EPOCHS, sto->key, sto->age, TTL, broadcast_key, broadcast_key_len); 
+    if (ret != 0)
+        return -2;
+    keystore_initialized = 1;
+    return 0;
+}
+
+
+int dp3t_shellcmd_testvec(int argc, char **argv)
+{
+    uint8_t testkey[32] = { }; /* zeros */
+    dp3t_generate_beacons(testkey, 0);
+    return 0;
+}
+
+int dp3t_shellcmd_rekey(int argc, char **argv)
+{
+    return rekey();
+}

keystore.h

@@ -4,7 +4,14 @@
 #include "dp3t-config.h"
 #include <stdint.h>
 
+int dp3t_start(void);
+int dp3t_restart(unsigned int day);
+uint8_t *dp3t_get_ephid(uint8_t idx);
+
 uint8_t *dp3t_get_skt_0(void);
 beacons_t *dp3t_generate_beacons(sk_t key, int day);
-uint8_t *dp3t_get_ephid(uint8_t idx);
+
+
+int dp3t_shellcmd_testvec(int argc, char **argv);
+int dp3t_shellcmd_rekey(int argc, char **argv);
 #endif

main.c

@@ -95,12 +95,6 @@ int cmd_scan(int argc, char **argv)
     return 0;
 }
 
-int cmd_testvec(int argc, char **argv)
-{
-    uint8_t testkey[32] = { }; /* zeros */
-    dp3t_generate_beacons(testkey, 0);
-    return 0;
-}
 
 #define MAIN_QUEUE_SIZE     (8)
 static msg_t _main_msg_queue[MAIN_QUEUE_SIZE];
@@ -124,7 +118,8 @@ static const shell_command_t shell_commands[] = {
     { "dtlsc", "Start a DTLS client", dtls_client },
     { "dtlss", "Start and stop a DTLS server", dtls_server },
     { "scan", "trigger a BLE scan", cmd_scan },
-    { "testvec", "print test vectors", cmd_testvec },
+    { "testvec", "print test vectors", dp3t_shellcmd_testvec },
+    { "rekey", "regenerate DP3T secure key", dp3t_shellcmd_rekey },
 #ifdef MODULE_WOLFCRYPT_TEST
     { "wolftest", "Perform wolfcrypt porting test", wolftest },
 #endif
@@ -155,11 +150,10 @@ int main(void)
     wolfSSL_Debugging_ON();
 
     /* dp3t */
-    sk_t0 = dp3t_get_skt_0();
-    dp3t_generate_beacons(sk_t0, 0);
+    dp3t_start();
 
     /* Start Bluetooth service by default */
-    gatt_server();
+    //gatt_server();
 
     /* start shell */
     printf( "All up, running the shell now\r\n");

nvmc.c

@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2020 Dyne.org foundation
+ *
+ * This file is subject to the terms and conditions of the GNU
+ * General Public License (GPL) version 2. See the file LICENSE
+ * for more details.
+ *
+ */
+#include <stdint.h>
+#include "nvmc.h"
+
+/* Flash write/erase control */
+#define NVMC_BASE (0x4001E000)
+#define NVMC_CONFIG *((volatile uint32_t *)(NVMC_BASE + 0x504))
+#define NVMC_ERASEPAGE *((volatile uint32_t *)(NVMC_BASE + 0x508))
+#define NVMC_READY *((volatile uint32_t *)(NVMC_BASE + 0x400))
+#define NVMC_CONFIG_REN 0
+#define NVMC_CONFIG_WEN 1
+#define NVMC_CONFIG_EEN 2
+
+
+
+static void flash_wait_complete(void)
+{
+    while (NVMC_READY == 0)
+        ;
+}
+
+int flash_write(uint32_t address, const uint8_t *data, int len)
+{
+    int i = 0;
+    uint32_t *src, *dst;
+
+    while (i < len) {
+        if ((len - i > 3) && ((((address + i) & 0x03) == 0)  && ((((uint32_t)data) + i) & 0x03) == 0)) {
+            src = (uint32_t *)data;
+            dst = (uint32_t *)address;
+            NVMC_CONFIG = NVMC_CONFIG_WEN;
+            flash_wait_complete();
+            dst[i >> 2] = src[i >> 2];
+            flash_wait_complete();
+            i+=4;
+        } else {
+            uint32_t val;
+            uint8_t *vbytes = (uint8_t *)(&val);
+            int off = (address + i) - (((address + i) >> 2) << 2);
+            dst = (uint32_t *)(address - off);
+            val = dst[i >> 2];
+            vbytes[off] = data[i];
+            NVMC_CONFIG = NVMC_CONFIG_WEN;
+            flash_wait_complete();
+            dst[i >> 2] = val;
+            flash_wait_complete();
+            i++;
+        }
+    }
+    return 0;
+}
+
+
+int flash_erase(uint32_t address, int len)
+{
+    uint32_t end = address + len - 1;
+    uint32_t p;
+    for (p = address; p <= end; p += FLASH_PAGE_SIZE) {
+        NVMC_CONFIG = NVMC_CONFIG_EEN;
+        flash_wait_complete();
+        NVMC_ERASEPAGE = p;
+        flash_wait_complete();
+    }
+    return 0;
+}
+

nvmc.h

@@ -0,0 +1,6 @@
+#ifndef NVMC_H
+#define NVMC_H
+#define FLASH_PAGE_SIZE (4096)
+int flash_write(uint32_t address, const uint8_t *data, int len);
+int flash_erase(uint32_t address, int len);
+#endif