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