From 99efb78ed8ba69799045cff6c42c4a71bffdb703 Mon Sep 17 00:00:00 2001
From: Daniel Lublin <daniel@lublin.se>
Date: Wed, 21 Sep 2022 10:39:53 +0200
Subject: [PATCH] Receive USS and hash into CDI

- We're OK with USS not being loaded, and use an all-zero USS if so.
- We require USS to be loaded before app_size (if at all).
---
 hw/application_fpga/fw/mta1_mkdf/main.c  | 27 +++++++++++++++++++++---
 hw/application_fpga/fw/mta1_mkdf/proto.c |  5 +++++
 hw/application_fpga/fw/mta1_mkdf/proto.h |  5 ++++-
 3 files changed, 33 insertions(+), 4 deletions(-)

diff --git a/hw/application_fpga/fw/mta1_mkdf/main.c b/hw/application_fpga/fw/mta1_mkdf/main.c
index a1ab66d..ad5b7be 100644
--- a/hw/application_fpga/fw/mta1_mkdf/main.c
+++ b/hw/application_fpga/fw/mta1_mkdf/main.c
@@ -71,12 +71,16 @@ int main()
 	uint8_t *loadaddr = (uint8_t *)APP_RAM_ADDR;
 	int left = 0;	// Bytes left to read
 	int nbytes = 0; // Bytes to write to memory
+	uint8_t uss[32];
 	uint32_t local_app_size = 0;
 	uint8_t in;
 	uint8_t digest[32];
 
 	print_hw_version(local_name0, local_name1, local_ver);
 
+	// If host does not load USS, we use an all zero USS
+	memset(uss, 0, 32);
+
 	for (;;) {
 		// blocking; fw flashing white while waiting for cmd
 		in = readbyte_ledflash(LED_RED | LED_BLUE | LED_GREEN, 500000);
@@ -117,6 +121,22 @@ int main()
 			fwreply(hdr, FW_RSP_NAME_VERSION, rsp);
 			break;
 
+		case FW_CMD_LOAD_USS:
+			puts("request: load-uss\n");
+
+			if (hdr.len != 128 || *app_size != 0) {
+				// Bad cmd length, or app_size already set
+				rsp[0] = STATUS_BAD;
+				fwreply(hdr, FW_RSP_LOAD_USS, rsp);
+				break;
+			}
+
+			memcpy(uss, cmd + 1, 32);
+
+			rsp[0] = STATUS_OK;
+			fwreply(hdr, FW_RSP_LOAD_USS, rsp);
+			break;
+
 		case FW_CMD_LOAD_APP_SIZE:
 			puts("request: load-app-size\n");
 
@@ -173,7 +193,7 @@ int main()
 			left -= nbytes;
 
 			if (left == 0) {
-				uint8_t scratch[64];
+				uint8_t scratch[96];
 
 				puts("Fully loaded ");
 				putinthex(*app_size);
@@ -186,14 +206,15 @@ int main()
 					(const void *)*app_addr, *app_size);
 				print_digest(digest);
 
-				// CDI = hash(uds, hash(app))
+				// CDI = hash(uds, hash(app), uss)
 				uint32_t local_cdi[8];
 
 				// Only word aligned access to UDS
 				wordcpy(scratch, (void *)uds, 8);
 				memcpy(scratch + 32, digest, 32);
+				memcpy(scratch + 64, uss, 32);
 				blake2s((void *)local_cdi, 32, NULL, 0,
-					(const void *)scratch, 64);
+					(const void *)scratch, 96);
 				// Only word aligned access to CDI
 				wordcpy((void *)cdi, (void *)local_cdi, 8);
 			}
diff --git a/hw/application_fpga/fw/mta1_mkdf/proto.c b/hw/application_fpga/fw/mta1_mkdf/proto.c
index 73a645b..8689c61 100644
--- a/hw/application_fpga/fw/mta1_mkdf/proto.c
+++ b/hw/application_fpga/fw/mta1_mkdf/proto.c
@@ -71,6 +71,11 @@ void fwreply(struct frame_header hdr, enum fwcmd rspcode, uint8_t *buf)
 		nbytes = 32;
 		break;
 
+	case FW_RSP_LOAD_USS:
+		len = LEN_4;
+		nbytes = 4;
+		break;
+
 	case FW_RSP_LOAD_APP_SIZE:
 		len = LEN_4;
 		nbytes = 4;
diff --git a/hw/application_fpga/fw/mta1_mkdf/proto.h b/hw/application_fpga/fw/mta1_mkdf/proto.h
index 7523261..898461b 100644
--- a/hw/application_fpga/fw/mta1_mkdf/proto.h
+++ b/hw/application_fpga/fw/mta1_mkdf/proto.h
@@ -35,7 +35,10 @@ enum fwcmd {
 	FW_CMD_RUN_APP		= 0x07,
 	FW_RSP_RUN_APP		= 0x08,
 	FW_CMD_GET_APP_DIGEST	= 0x09,
-	FW_RSP_GET_APP_DIGEST	= 0x10
+	FW_CMD_LOAD_USS		= 0x0a,
+	FW_RSP_LOAD_USS		= 0x0b,
+	/* ... */
+	FW_RSP_GET_APP_DIGEST	= 0x10, // encoded as 0x10 for backwards compatibility
 };
 // clang-format on