Write all the needed pk1 code and a large part of the nxboot function

fusee/fusee-secondary/src/hwinit.h

@@ -37,4 +37,6 @@ void cluster_enable_cpu0(u64 entry, u32 ns_disable);
 
 void mc_enable_ahb_redirect();
 
+int tsec_query(u32 carveout, u8 *dst, u32 rev);
+
 #endif

fusee/fusee-secondary/src/key_derivation.c

@@ -3,6 +3,7 @@
 #include "se.h"
 #include "exocfg.h"
 #include "fuse.h"
+#include "hwinit.h"
 
 static const uint8_t keyblob_seeds[MASTERKEY_REVISION_MAX][0x10] = {
     {0xDF, 0x20, 0x6F, 0x59, 0x44, 0x54, 0xEF, 0xDC, 0x70, 0x74, 0x48, 0x3B, 0x0D, 0xED, 0x9F, 0xD3}, /* Keyblob seed 00. */
@@ -32,21 +33,26 @@ static const uint8_t masterkey_4x_seed[0x10] = {
     0x2D, 0xC1, 0xF4, 0x8D, 0xF3, 0x5B, 0x69, 0x33, 0x42, 0x10, 0xAC, 0x65, 0xDA, 0x90, 0x46, 0x66
 };
 
-static void get_tsec_key(void *dst, const void *tsec_fw, size_t tsec_fw_size) {
+static int get_tsec_key(void *dst, const void *tsec_fw, size_t tsec_fw_size, uint32_t revision) {
-    /* TODO: Implement this method. Attempt to read TSEC fw from NAND, or from SD if that fails. */
+    static uint8_t __attribute__((aligned(256))) tsec_dma_buf[0xF00];
+    memcpy(tsec_dma_buf, tsec_fw, tsec_fw_size);
+    return tsec_query((u32)tsec_dma_buf, dst, revision);
 }
 
-void get_keyblob(nx_keyblob_t *dst, uint32_t revision, const nx_keyblob_t *keyblobs, uint32_t available_revision) {
+static int get_keyblob(nx_keyblob_t *dst, uint32_t revision, const nx_keyblob_t *keyblobs, uint32_t available_revision) {
     if (revision >= 0x20) {
+        return -1;
         generic_panic();
     }
 
     if (keyblobs != NULL) {
         *dst = keyblobs[revision];
     } else {
-        generic_panic();
+        return -1;
         /* TODO: what should we do? */
     }
+
+    return 0;
 }
 
 static bool safe_memcmp(uint8_t *a, uint8_t *b, size_t sz) {
@@ -58,7 +64,7 @@ static bool safe_memcmp(uint8_t *a, uint8_t *b, size_t sz) {
 }
 
 /* Derive all Switch keys. */
-void derive_nx_keydata(uint32_t target_firmware, const nx_keyblob_t *keyblobs, uint32_t available_revision, const void *tsec_fw, size_t tsec_fw_size) {
+int derive_nx_keydata(uint32_t target_firmware, const nx_keyblob_t *keyblobs, uint32_t available_revision, const void *tsec_fw, size_t tsec_fw_size) {
     uint8_t work_buffer[0x10];
     nx_keyblob_t keyblob;
 
@@ -67,12 +73,16 @@ void derive_nx_keydata(uint32_t target_firmware, const nx_keyblob_t *keyblobs, u
     set_aes_keyslot_flags(0xD, 0x15);
 
     /* Set TSEC key. */
-    get_tsec_key(work_buffer, tsec_fw, tsec_fw_size);
+    if  (get_tsec_key(work_buffer, tsec_fw, tsec_fw_size, target_firmware) != 0) {
+        return -1;
+    }
     set_aes_keyslot(0xD, work_buffer, 0x10);
 
     /* Get keyblob, always try to set up the highest possible master key. */
     /* TODO: Should we iterate, trying lower keys on failure? */
-    get_keyblob(&keyblob, MASTERKEY_REVISION_500_CURRENT, keyblobs, available_revision);
+    if (get_keyblob(&keyblob, MASTERKEY_REVISION_500_CURRENT, keyblobs, available_revision) != 0) {
+        return -1;
+    }
 
     /* Derive both keyblob key 1, and keyblob key latest. */
     se_aes_ecb_decrypt_block(0xD, work_buffer, 0x10, keyblob_seeds[MASTERKEY_REVISION_100_230], 0x10);
@@ -88,7 +98,7 @@ void derive_nx_keydata(uint32_t target_firmware, const nx_keyblob_t *keyblobs, u
     /* Validate keyblob. */
     se_compute_aes_128_cmac(0xB, work_buffer, 0x10, keyblob.mac + sizeof(keyblob.mac), sizeof(keyblob) - sizeof(keyblob.mac));
     if (safe_memcmp(keyblob.mac, work_buffer, 0x10)) {
-        generic_panic();
+        return -1;
     }
 
     /* Decrypt keyblob. */
@@ -122,11 +132,12 @@ void derive_nx_keydata(uint32_t target_firmware, const nx_keyblob_t *keyblobs, u
             decrypt_data_into_keyslot(0xC, 0xC, masterkey_seed, 0x10);
             break;
         default:
-            generic_panic();
+            return -1;
     }
 
     /* Setup master key revision, derive older master keys for use. */
     mkey_detect_revision();
+    return 0;
 }
 
 /* Sets final keyslot flags, for handover to TZ/Exosphere. Setting these will prevent the BPMP from using the device key or master key. */

fusee/fusee-secondary/src/key_derivation.h

@@ -31,7 +31,7 @@ typedef struct nx_keyblob_t {
     };
 } nx_keyblob_t;
 
-void derive_nx_keydata(uint32_t target_firmware, const nx_keyblob_t *keyblobs, uint32_t available_revision, const void *tsec_fw, size_t tsec_fw_size);
+int derive_nx_keydata(uint32_t target_firmware, const nx_keyblob_t *keyblobs, uint32_t available_revision, const void *tsec_fw, size_t tsec_fw_size);
 void finalize_nx_keydata(uint32_t target_firmware);
 
 void derive_bis_key(void *dst, BisPartition partition_id, uint32_t target_firmware);

fusee/fusee-secondary/src/nxboot.c

@@ -1,5 +1,8 @@
 #include <stdio.h>
 #include <string.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <malloc.h>
 #include "utils.h"
 #include "nxboot.h"
 #include "key_derivation.h"
@@ -46,28 +49,95 @@ void nxboot_configure_exosphere(void) {
 }
 
 /* This is the main function responsible for booting Horizon. */
+static nx_keyblob_t __attribute__((aligned(16))) g_keyblobs[32];
 void nxboot_main(void) {
     loader_ctx_t *loader_ctx = get_loader_ctx();
+    package2_header_t *package2_src = (package2_header_t *)loader_ctx->package2_loadfile.load_address;
+    package2_header_t *package2 = NULL;
+    void *tsec_fw = (void *)loader_ctx->tsecfw_loadfile.load_address;
+    size_t tsec_fw_size = loader_ctx->tsecfw_loadfile.load_size;
+    void *warmboot_fw = (void *)loader_ctx->warmboot_loadfile.load_address;
+    size_t warmboot_fw_size = loader_ctx->warmboot_loadfile.load_size;
+    void *package1loader = NULL;
+    size_t package1loader_size = 0;
+    package1_header_t *package1 = NULL;
+    size_t package1_size = 0;
+    uint32_t revision = EXOSPHERE_TARGET_FIRMWARE_MAX;
+    FILE *boot0 = fopen("boot0:/", "rb");
+
+    if (boot0 == NULL || package1_read_and_parse_boot0(&package1loader, &package1loader_size, g_keyblobs, &revision, boot0) == -1) {
+        printf("Error: Couldn't parse boot0: %s!\n", strerror(errno));
+        generic_panic();
+    }
+    fclose(boot0);
+
+    if (tsec_fw == NULL) {
+        tsec_fw_size = package1_get_tsec_fw(&tsec_fw, package1loader, package1loader_size);
+        if (tsec_fw_size == 0) {
+            printf("Error: Failed to read the TSEC firmware from Package1loader!\n");
+            generic_panic();
+        }
+    }
 
     /* TODO: Validate that we're capable of booting. */
 
     /* TODO: Initialize Boot Reason. */
 
-    /* TODO: How should we deal with bootconfig? */
-
     /* Setup boot configuration for Exosphere. */
     nxboot_configure_exosphere();
 
     /* Derive keydata. */
-    //derive_nx_keydata(MAILBOX_EXOSPHERE_CONFIGURATION->target_firmware);
+    if (derive_nx_keydata(MAILBOX_EXOSPHERE_CONFIGURATION->target_firmware, g_keyblobs, revision, tsec_fw, tsec_fw_size) != 0) {
+        printf("Error: Key derivation failed!\n");
+        generic_panic();
+    }
 
-    if (loader_ctx->package2_loadfile.load_address == 0) {
+    if (warmboot_fw == NULL) {
+        uint8_t ctr[16];
+        package1_size = package1_get_encrypted_package1(&package1, ctr, package1loader, package1loader_size);
+        if(package1_decrypt(package1, package1_size, ctr)) {
+            warmboot_fw = package1_get_warmboot_fw(package1);
+            warmboot_fw_size = package1->warmboot_size;
+        } else {
+            warmboot_fw = NULL;
+            warmboot_fw_size = 0;
+        }
 
-        /* TODO: read package2 somewhere. */
+        if (warmboot_fw_size == 0) {
+            printf("Error: Failed to read the warmboot firmware from Package1!\n");
+            generic_panic();
+        }
+    }
+
+    free(package1loader);
+
+    package2 = memalign(16, PACKAGE2_SIZE_MAX);
+    if (package2 == NULL) {
+        printf("Error: nxboot: out of memory!\n");
+        generic_panic();
+    }
+
+    if (package2_src == NULL) {
+        /* TODO: How should we deal with bootconfig? */
+        FILE *bcpkg21 = fopen("bcpkg21:/", "rb");
+        if (bcpkg21 == NULL)  {
+            printf("Error: Failed to read Package2 from NAND!\n");
+            generic_panic();
+        }
+        if (fseek(bcpkg21, 0x4000, SEEK_SET) != 0 || fread(package2, 1, PACKAGE2_SIZE_MAX, bcpkg21) < sizeof(package2_header_t)) {
+            printf("Error: Failed to read Package2 from NAND!\n");
+            fclose(bcpkg21);
+            generic_panic();
+        }
+        fclose(bcpkg21);
+    } else {
+        memcpy(package2, package2_src, loader_ctx->package2_loadfile.load_size);
     }
 
     /* Patch package2, adding thermosphere + custom KIPs. */
-    package2_rebuild_and_copy((void *)loader_ctx->package2_loadfile.load_address);
+    package2_rebuild_and_copy(package2);
+
+    free(package2);
 
     /* Boot up Exosphere. */
     MAILBOX_NX_BOOTLOADER_IS_SECMON_AWAKE = 0;

fusee/fusee-secondary/src/package1.c

@@ -1,15 +1,18 @@
 #include <errno.h>
 #include <stdlib.h>
+#include <malloc.h>
+#include <string.h>
 #include "package1.h"
 #include "bct.h"
+#include "se.h"
 
-int package1_parse_boot0(void **package1, size_t *package1_size, nx_keyblob_t *keyblobs, uint32_t *revision, FILE *boot0) {
+int package1_read_and_parse_boot0(void **package1loader, size_t *package1loader_size, nx_keyblob_t *keyblobs, uint32_t *revision, FILE *boot0) {
     static nvboot_config_table bct = {0}; /* Normal firmware BCT, primary. TODO: check? */
     nv_bootloader_info *pk1_info = &bct.bootloader[0]; /* TODO: check? */
 
     size_t fpos, pk1_offset;
 
-    if (package1 == NULL || package1_size != NULL || keyblobs == NULL || revision == NULL || boot0 == NULL) {
+    if (package1loader == NULL || package1loader_size != NULL || keyblobs == NULL || revision == NULL || boot0 == NULL) {
         errno = EINVAL;
         return -1;
     }
@@ -26,13 +29,13 @@ int package1_parse_boot0(void **package1, size_t *package1_size, nx_keyblob_t *k
     }
 
     *revision = pk1_info->attribute;
-    *package1_size = pk1_info->length;
+    *package1loader_size = pk1_info->length;
 
     pk1_offset = 0x4000 * pk1_info->start_blk + 0x200 * pk1_info->start_page;
 
-    (*package1) = malloc(*package1_size);
+    (*package1loader) = memalign(16, *package1loader_size);
 
-    if (*package1 == NULL) {
+    if (*package1loader == NULL) {
         errno = ENOMEM;
         return -1;
     }
@@ -41,12 +44,12 @@ int package1_parse_boot0(void **package1, size_t *package1_size, nx_keyblob_t *k
     if (fseek(boot0, fpos + pk1_offset, SEEK_SET) != 0) {
         return -1;
     }
-    if (fread(*package1, *package1_size, 1, boot0) == 0) {
+    if (fread(*package1loader, *package1loader_size, 1, boot0) == 0) {
         return -1;
     }
 
     /* Skip the backup pk1/pk1l. */
-    if (fseek(boot0, *package1_size, SEEK_CUR) != 0) {
+    if (fseek(boot0, *package1loader_size, SEEK_CUR) != 0) {
         return -1;
     }
 
@@ -62,3 +65,66 @@ int package1_parse_boot0(void **package1, size_t *package1_size, nx_keyblob_t *k
 
     return 0;
 }
+
+size_t package1_get_tsec_fw(void **tsec_fw, const void *package1loader, size_t package1loader_size) {
+    /* The TSEC firmware is always located at a 256-byte aligned address. */
+    /* We're looking for its 4 first bytes. We assume its size is always 0xF00 bytes. */
+    const uint32_t *pos;
+    uintptr_t pk1l = (uintptr_t)package1loader;
+    for (pos = (const uint32_t *)pk1l; (uintptr_t)pos < pk1l + package1loader_size && *pos != 0xCF42004D; pos += 0x40);
+
+    (*tsec_fw) = (void *)pos;
+    return 0xF00;
+}
+
+size_t package1_get_encrypted_package1(package1_header_t **package1, uint8_t *ctr, const void *package1loader, size_t package1loader_size) {
+    const uint32_t *pos;
+    uintptr_t pk1l = (uintptr_t)package1loader;
+
+    if (package1loader_size < 0x4000) {
+        return 0; /* Shouldn't happen, ever. */
+    }
+
+    for (pos = (const uint32_t *)pk1l; (uintptr_t)pos < pk1l + 0x3FF8 && (pos[0] != 0x70012000 || pos[2] != 0x40007000); pos++);
+    pos = (const uint32_t *)(pk1l + pos[1] - 0x40010000);
+
+    memcpy(ctr, pos + 4, 0x10);
+    (*package1) = (package1_header_t *)(pos + 8);
+    return *pos;
+}
+
+bool package1_decrypt(package1_header_t *package1, size_t package1_size, const uint8_t *ctr) {
+    uint8_t __attribute__((aligned(16))) ctrbuf[16];
+    memcpy(ctrbuf, ctr, 16);
+    se_aes_ctr_crypt(0xB, package1, package1_size, package1, package1_size, ctr, 16);
+    return memcmp(package1->magic, "PK11", 4) == 0;
+}
+
+void *package1_get_warmboot_fw(const package1_header_t *package1) {
+    /*  
+        The layout of pk1 changes between versions.
+
+        However, the secmon always starts by this erratum code:
+        https://github.com/ARM-software/arm-trusted-firmware/blob/master/plat/nvidia/tegra/common/aarch64/tegra_helpers.S#L312
+        and thus by 0xD5034FDF.
+
+        Nx-bootloader seems to always start by 0xE328F0C0 (msr cpsr_f, 0xc0).
+    */
+    const uint32_t *data = (const uint32_t *)package1->data;
+
+    for (size_t i = 0; i < 3; i++) {
+        switch (*data) {
+            case 0xD5034FDFu:
+                data += package1->secmon_size / 4;
+                break;
+            case 0xE328F0C0:
+                data += package1->nx_bootloader_size / 4;
+                break;
+            default:
+                /* TODO: should we validate its signature? */
+                return (void *)data;
+        }
+    }
+
+    return NULL;
+}

fusee/fusee-secondary/src/package1.h

@@ -4,6 +4,25 @@
 #include <stdio.h>
 #include "key_derivation.h"
 
-int package1_parse_boot0(void **package1, size_t *package1_size, nx_keyblob_t *keyblobs, uint32_t *revision, FILE *boot0);
+typedef struct package1_header_t {
+    char magic[4];
+    uint32_t warmboot_size;
+    uint32_t _0x8;
+    uint32_t _0xC;
+    uint32_t nx_bootloader_size;
+    uint32_t _0x14;
+    uint32_t secmon_size;
+    uint32_t _0x1C;
+    uint8_t data[];
+} package1_header_t;
+
+int package1_read_and_parse_boot0(void **package1loader, size_t *package1loader_size, nx_keyblob_t *keyblobs, uint32_t *revision, FILE *boot0);
+
+size_t package1_get_tsec_fw(void **tsec_fw, const void *package1loader, size_t package1loader_size);
+size_t package1_get_encrypted_package1(package1_header_t **package1, uint8_t *ctr, const void *package1loader, size_t package1loader_size);
+
+/* Must be aligned to 16 bytes. */
+bool package1_decrypt(package1_header_t *package1, size_t package1_size, const uint8_t *ctr);
+void *package1_get_warmboot_fw(const package1_header_t *package1);
 
 #endif