mirror of
https://github.com/tillitis/tillitis-key1.git
synced 2024-12-29 17:36:26 -05:00
fw: Move all command switches to their own functions
This commit is contained in:
Michael Cardell Widerkrantz
Daniel Lublin
parent
4e3f5469ef
commit
9de7f294df
@ -33,6 +33,15 @@ static volatile uint32_t *ram_aslr = (volatile uint32_t *)TK1_MMIO_TK1_RA
|
|||||||
static volatile uint32_t *ram_scramble = (volatile uint32_t *)TK1_MMIO_TK1_RAM_SCRAMBLE;
|
static volatile uint32_t *ram_scramble = (volatile uint32_t *)TK1_MMIO_TK1_RAM_SCRAMBLE;
|
||||||
// clang-format on
|
// clang-format on
|
||||||
|
|
||||||
|
// Context for the loading of a TKey program
|
||||||
|
struct context {
|
||||||
|
int left; // Bytes left to receive
|
||||||
|
uint8_t digest[32]; // Program digest
|
||||||
|
uint8_t *loadaddr; // Where we are currently loading a TKey program
|
||||||
|
uint8_t use_uss; // Use USS?
|
||||||
|
uint8_t uss[32]; // User Supplied Secret, if any
|
||||||
|
};
|
||||||
|
|
||||||
struct namever {
|
struct namever {
|
||||||
char name0[4];
|
char name0[4];
|
||||||
char name1[4];
|
char name1[4];
|
||||||
@ -101,7 +110,8 @@ uint32_t rnd_word()
|
|||||||
}
|
}
|
||||||
|
|
||||||
// CDI = blake2s(uds, blake2s(app), uss)
|
// CDI = blake2s(uds, blake2s(app), uss)
|
||||||
static void compute_cdi(uint8_t digest[32], uint8_t use_uss, uint8_t uss[32])
|
static void compute_cdi(const uint8_t digest[32], const uint8_t use_uss,
|
||||||
|
const uint8_t uss[32])
|
||||||
{
|
{
|
||||||
uint32_t local_cdi[8];
|
uint32_t local_cdi[8];
|
||||||
blake2s_ctx secure_ctx;
|
blake2s_ctx secure_ctx;
|
||||||
@ -141,6 +151,211 @@ static void compute_cdi(uint8_t digest[32], uint8_t use_uss, uint8_t uss[32])
|
|||||||
wordcpy_s((void *)cdi, 8, local_cdi, 8);
|
wordcpy_s((void *)cdi, 8, local_cdi, 8);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static int initial_commands(const struct frame_header *hdr, const uint8_t *cmd,
|
||||||
|
enum state state, struct context *ctx)
|
||||||
|
{
|
||||||
|
uint8_t rsp[CMDLEN_MAXBYTES] = {0};
|
||||||
|
|
||||||
|
switch (cmd[0]) {
|
||||||
|
case FW_CMD_NAME_VERSION: {
|
||||||
|
struct namever namever = get_hw_version(*name0, *name1, *ver);
|
||||||
|
|
||||||
|
htif_puts("cmd: name-version\n");
|
||||||
|
if (hdr->len != 1) {
|
||||||
|
// Bad length - give them an empty
|
||||||
|
// response
|
||||||
|
fwreply(*hdr, FW_RSP_NAME_VERSION, rsp);
|
||||||
|
return state;
|
||||||
|
}
|
||||||
|
|
||||||
|
memcpy_s(rsp, CMDLEN_MAXBYTES, &namever.name0, 4);
|
||||||
|
memcpy_s(&rsp[4], CMDLEN_MAXBYTES - 4, &namever.name1, 4);
|
||||||
|
memcpy_s(&rsp[8], CMDLEN_MAXBYTES - 8, &namever.version, 4);
|
||||||
|
htif_hexdump(rsp, 12);
|
||||||
|
fwreply(*hdr, FW_RSP_NAME_VERSION, rsp);
|
||||||
|
// state unchanged
|
||||||
|
return state;
|
||||||
|
} break;
|
||||||
|
|
||||||
|
case FW_CMD_GET_UDI:
|
||||||
|
htif_puts("cmd: get-udi\n");
|
||||||
|
if (hdr->len != 1) {
|
||||||
|
// Bad cmd length
|
||||||
|
rsp[0] = STATUS_BAD;
|
||||||
|
fwreply(*hdr, FW_RSP_GET_UDI, rsp);
|
||||||
|
return state;
|
||||||
|
}
|
||||||
|
|
||||||
|
rsp[0] = STATUS_OK;
|
||||||
|
uint32_t udi_words[2];
|
||||||
|
wordcpy_s(udi_words, 2, (void *)udi, 2);
|
||||||
|
memcpy_s(&rsp[1], CMDLEN_MAXBYTES - 1, udi_words, 2 * 4);
|
||||||
|
fwreply(*hdr, FW_RSP_GET_UDI, rsp);
|
||||||
|
// state unchanged
|
||||||
|
return state;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case FW_CMD_LOAD_APP:
|
||||||
|
htif_puts("cmd: load-app(size, uss)\n");
|
||||||
|
if (hdr->len != 512) {
|
||||||
|
// Bad length
|
||||||
|
rsp[0] = STATUS_BAD;
|
||||||
|
fwreply(*hdr, FW_RSP_LOAD_APP, rsp);
|
||||||
|
return state;
|
||||||
|
}
|
||||||
|
|
||||||
|
// cmd[1..4] contains the size.
|
||||||
|
uint32_t local_app_size =
|
||||||
|
cmd[1] + (cmd[2] << 8) + (cmd[3] << 16) + (cmd[4] << 24);
|
||||||
|
|
||||||
|
htif_puts("app size: ");
|
||||||
|
htif_putinthex(local_app_size);
|
||||||
|
htif_lf();
|
||||||
|
|
||||||
|
if (local_app_size == 0 || local_app_size > TK1_APP_MAX_SIZE) {
|
||||||
|
rsp[0] = STATUS_BAD;
|
||||||
|
fwreply(*hdr, FW_RSP_LOAD_APP, rsp);
|
||||||
|
return state;
|
||||||
|
}
|
||||||
|
|
||||||
|
*app_size = local_app_size;
|
||||||
|
|
||||||
|
// Do we have a USS at all?
|
||||||
|
if (cmd[5] != 0) {
|
||||||
|
// Yes
|
||||||
|
ctx->use_uss = TRUE;
|
||||||
|
memcpy_s(ctx->uss, 32, &cmd[6], 32);
|
||||||
|
} else {
|
||||||
|
ctx->use_uss = FALSE;
|
||||||
|
}
|
||||||
|
|
||||||
|
rsp[0] = STATUS_OK;
|
||||||
|
fwreply(*hdr, FW_RSP_LOAD_APP, rsp);
|
||||||
|
|
||||||
|
assert(*app_size != 0);
|
||||||
|
assert(*app_size <= TK1_APP_MAX_SIZE);
|
||||||
|
|
||||||
|
ctx->left = *app_size;
|
||||||
|
|
||||||
|
return FW_STATE_LOADING;
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
htif_puts("Got unknown firmware cmd: 0x");
|
||||||
|
htif_puthex(cmd[0]);
|
||||||
|
htif_lf();
|
||||||
|
}
|
||||||
|
|
||||||
|
return FW_STATE_FAIL;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int loading_commands(const struct frame_header *hdr, const uint8_t *cmd,
|
||||||
|
enum state state, struct context *ctx)
|
||||||
|
{
|
||||||
|
uint8_t rsp[CMDLEN_MAXBYTES] = {0};
|
||||||
|
|
||||||
|
switch (cmd[0]) {
|
||||||
|
case FW_CMD_LOAD_APP_DATA:
|
||||||
|
htif_puts("cmd: load-app-data\n");
|
||||||
|
if (hdr->len != 512) {
|
||||||
|
// Bad cmd length
|
||||||
|
rsp[0] = STATUS_BAD;
|
||||||
|
fwreply(*hdr, FW_RSP_LOAD_APP_DATA, rsp);
|
||||||
|
return state;
|
||||||
|
}
|
||||||
|
|
||||||
|
int nbytes;
|
||||||
|
if (ctx->left > (512 - 1)) {
|
||||||
|
nbytes = 512 - 1;
|
||||||
|
} else {
|
||||||
|
nbytes = ctx->left;
|
||||||
|
}
|
||||||
|
memcpy_s(ctx->loadaddr, ctx->left, cmd + 1, nbytes);
|
||||||
|
ctx->loadaddr += nbytes;
|
||||||
|
ctx->left -= nbytes;
|
||||||
|
|
||||||
|
if (ctx->left == 0) {
|
||||||
|
htif_puts("Fully loaded ");
|
||||||
|
htif_putinthex(*app_size);
|
||||||
|
htif_lf();
|
||||||
|
|
||||||
|
// Compute Blake2S digest of the app,
|
||||||
|
// storing it for FW_STATE_RUN
|
||||||
|
blake2s_ctx b2s_ctx;
|
||||||
|
|
||||||
|
blake2s(&ctx->digest, 32, NULL, 0,
|
||||||
|
(const void *)TK1_RAM_BASE, *app_size,
|
||||||
|
&b2s_ctx);
|
||||||
|
print_digest(ctx->digest);
|
||||||
|
|
||||||
|
// And return the digest in final
|
||||||
|
// response
|
||||||
|
rsp[0] = STATUS_OK;
|
||||||
|
memcpy_s(&rsp[1], CMDLEN_MAXBYTES - 1, &ctx->digest,
|
||||||
|
32);
|
||||||
|
fwreply(*hdr, FW_RSP_LOAD_APP_DATA_READY, rsp);
|
||||||
|
|
||||||
|
return FW_STATE_RUN;
|
||||||
|
}
|
||||||
|
|
||||||
|
rsp[0] = STATUS_OK;
|
||||||
|
fwreply(*hdr, FW_RSP_LOAD_APP_DATA, rsp);
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
htif_puts("Got unknown firmware cmd: 0x");
|
||||||
|
htif_puthex(cmd[0]);
|
||||||
|
htif_lf();
|
||||||
|
return FW_STATE_FAIL;
|
||||||
|
}
|
||||||
|
|
||||||
|
return state;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void run(const struct context *ctx)
|
||||||
|
{
|
||||||
|
*app_addr = TK1_RAM_BASE;
|
||||||
|
|
||||||
|
// CDI = hash(uds, hash(app), uss)
|
||||||
|
compute_cdi(ctx->digest, ctx->use_uss, ctx->uss);
|
||||||
|
|
||||||
|
htif_puts("Flipping to app mode!\n");
|
||||||
|
htif_puts("Jumping to ");
|
||||||
|
htif_putinthex(*app_addr);
|
||||||
|
htif_lf();
|
||||||
|
|
||||||
|
// Clear the firmware stack
|
||||||
|
// clang-format off
|
||||||
|
asm volatile(
|
||||||
|
"li a0, 0xd0000000;" // FW_RAM
|
||||||
|
"li a1, 0xd0000800;" // End of 2 KB FW_RAM (just past the end)
|
||||||
|
"loop:;"
|
||||||
|
"sw zero, 0(a0);"
|
||||||
|
"addi a0, a0, 4;"
|
||||||
|
"blt a0, a1, loop;"
|
||||||
|
::: "memory");
|
||||||
|
// clang-format on
|
||||||
|
|
||||||
|
// Flip over to application mode
|
||||||
|
*switch_app = 1;
|
||||||
|
|
||||||
|
// XXX Firmware stack now no longer available
|
||||||
|
// Don't use any function calls!
|
||||||
|
|
||||||
|
// Jump to app - doesn't return
|
||||||
|
// clang-format off
|
||||||
|
asm volatile(
|
||||||
|
// Get value at TK1_MMIO_TK1_APP_ADDR
|
||||||
|
"lui a0,0xff000;"
|
||||||
|
"lw a0,0x030(a0);"
|
||||||
|
// Jump to it
|
||||||
|
"jalr x0,0(a0);"
|
||||||
|
::: "memory");
|
||||||
|
// clang-format on
|
||||||
|
|
||||||
|
__builtin_unreachable();
|
||||||
|
}
|
||||||
|
|
||||||
int main()
|
int main()
|
||||||
{
|
{
|
||||||
// Set RAM address and data scrambling values
|
// Set RAM address and data scrambling values
|
||||||
@ -161,19 +376,16 @@ int main()
|
|||||||
*ram_aslr = rnd_word();
|
*ram_aslr = rnd_word();
|
||||||
*ram_scramble = rnd_word();
|
*ram_scramble = rnd_word();
|
||||||
|
|
||||||
struct namever namever = get_hw_version(*name0, *name1, *ver);
|
struct context ctx = {0};
|
||||||
struct frame_header hdr; // Used in both directions
|
struct frame_header hdr; // Used in both directions
|
||||||
uint8_t cmd[CMDLEN_MAXBYTES];
|
uint8_t cmd[CMDLEN_MAXBYTES];
|
||||||
uint8_t rsp[CMDLEN_MAXBYTES];
|
|
||||||
uint8_t *loadaddr = (uint8_t *)TK1_RAM_BASE;
|
|
||||||
int left = 0; // Bytes left to receive
|
|
||||||
uint8_t use_uss = FALSE;
|
|
||||||
uint8_t uss[32] = {0};
|
|
||||||
uint8_t digest[32] = {0};
|
|
||||||
enum state state = FW_STATE_INITIAL;
|
enum state state = FW_STATE_INITIAL;
|
||||||
// Let the app know the function adddress for blake2s()
|
// Let the app know the function adddress for blake2s()
|
||||||
*fw_blake2s_addr = (uint32_t)blake2s;
|
*fw_blake2s_addr = (uint32_t)blake2s;
|
||||||
print_hw_version(namever);
|
// print_hw_version(namever);
|
||||||
|
|
||||||
|
ctx.loadaddr = (uint8_t *)TK1_RAM_BASE;
|
||||||
|
ctx.use_uss = FALSE;
|
||||||
|
|
||||||
for (;;) {
|
for (;;) {
|
||||||
switch (state) {
|
switch (state) {
|
||||||
@ -183,93 +395,7 @@ int main()
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Reset response buffer
|
state = initial_commands(&hdr, cmd, state, &ctx);
|
||||||
memset(rsp, 0, CMDLEN_MAXBYTES);
|
|
||||||
|
|
||||||
switch (cmd[0]) {
|
|
||||||
case FW_CMD_NAME_VERSION:
|
|
||||||
htif_puts("cmd: name-version\n");
|
|
||||||
if (hdr.len != 1) {
|
|
||||||
// Bad length - give them an empty
|
|
||||||
// response
|
|
||||||
fwreply(hdr, FW_RSP_NAME_VERSION, rsp);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
memcpy_s(rsp, CMDLEN_MAXBYTES, &namever.name0,
|
|
||||||
4);
|
|
||||||
memcpy_s(&rsp[4], CMDLEN_MAXBYTES - 4,
|
|
||||||
&namever.name1, 4);
|
|
||||||
memcpy_s(&rsp[8], CMDLEN_MAXBYTES - 8,
|
|
||||||
&namever.version, 4);
|
|
||||||
htif_hexdump(rsp, 12);
|
|
||||||
fwreply(hdr, FW_RSP_NAME_VERSION, rsp);
|
|
||||||
// state unchanged
|
|
||||||
break;
|
|
||||||
case FW_CMD_GET_UDI:
|
|
||||||
htif_puts("cmd: get-udi\n");
|
|
||||||
if (hdr.len != 1) {
|
|
||||||
// Bad cmd length
|
|
||||||
rsp[0] = STATUS_BAD;
|
|
||||||
fwreply(hdr, FW_RSP_GET_UDI, rsp);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
rsp[0] = STATUS_OK;
|
|
||||||
uint32_t udi_words[2];
|
|
||||||
wordcpy_s(udi_words, 2, (void *)udi, 2);
|
|
||||||
memcpy_s(&rsp[1], CMDLEN_MAXBYTES - 1,
|
|
||||||
udi_words, 2 * 4);
|
|
||||||
fwreply(hdr, FW_RSP_GET_UDI, rsp);
|
|
||||||
// state unchanged
|
|
||||||
break;
|
|
||||||
case FW_CMD_LOAD_APP:
|
|
||||||
htif_puts("cmd: load-app(size, uss)\n");
|
|
||||||
if (hdr.len != 512) {
|
|
||||||
// Bad length
|
|
||||||
rsp[0] = STATUS_BAD;
|
|
||||||
fwreply(hdr, FW_RSP_LOAD_APP, rsp);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
// cmd[1..4] contains the size.
|
|
||||||
uint32_t local_app_size =
|
|
||||||
cmd[1] + (cmd[2] << 8) + (cmd[3] << 16) +
|
|
||||||
(cmd[4] << 24);
|
|
||||||
|
|
||||||
htif_puts("app size: ");
|
|
||||||
htif_putinthex(local_app_size);
|
|
||||||
htif_lf();
|
|
||||||
|
|
||||||
if (local_app_size == 0 ||
|
|
||||||
local_app_size > TK1_APP_MAX_SIZE) {
|
|
||||||
rsp[0] = STATUS_BAD;
|
|
||||||
fwreply(hdr, FW_RSP_LOAD_APP, rsp);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
*app_size = local_app_size;
|
|
||||||
|
|
||||||
// Do we have a USS at all?
|
|
||||||
if (cmd[5] != 0) {
|
|
||||||
// Yes
|
|
||||||
use_uss = TRUE;
|
|
||||||
memcpy_s(uss, 32, &cmd[6], 32);
|
|
||||||
} else {
|
|
||||||
use_uss = FALSE;
|
|
||||||
}
|
|
||||||
|
|
||||||
rsp[0] = STATUS_OK;
|
|
||||||
fwreply(hdr, FW_RSP_LOAD_APP, rsp);
|
|
||||||
|
|
||||||
assert(*app_size != 0);
|
|
||||||
assert(*app_size <= TK1_APP_MAX_SIZE);
|
|
||||||
|
|
||||||
left = *app_size;
|
|
||||||
|
|
||||||
state = FW_STATE_LOADING;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case FW_STATE_LOADING:
|
case FW_STATE_LOADING:
|
||||||
@ -278,109 +404,12 @@ int main()
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Reset response buffer
|
state = loading_commands(&hdr, cmd, state, &ctx);
|
||||||
memset(rsp, 0, CMDLEN_MAXBYTES);
|
|
||||||
|
|
||||||
switch (cmd[0]) {
|
|
||||||
|
|
||||||
case FW_CMD_LOAD_APP_DATA:
|
|
||||||
htif_puts("cmd: load-app-data\n");
|
|
||||||
if (hdr.len != 512) {
|
|
||||||
// Bad cmd length
|
|
||||||
rsp[0] = STATUS_BAD;
|
|
||||||
fwreply(hdr, FW_RSP_LOAD_APP_DATA, rsp);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
int nbytes;
|
|
||||||
if (left > (512 - 1)) {
|
|
||||||
nbytes = 512 - 1;
|
|
||||||
} else {
|
|
||||||
nbytes = left;
|
|
||||||
}
|
|
||||||
memcpy_s(loadaddr, left, cmd + 1, nbytes);
|
|
||||||
loadaddr += nbytes;
|
|
||||||
left -= nbytes;
|
|
||||||
|
|
||||||
if (left == 0) {
|
|
||||||
htif_puts("Fully loaded ");
|
|
||||||
htif_putinthex(*app_size);
|
|
||||||
htif_lf();
|
|
||||||
|
|
||||||
// Compute Blake2S digest of the app,
|
|
||||||
// storing it for FW_STATE_RUN
|
|
||||||
blake2s_ctx ctx;
|
|
||||||
|
|
||||||
blake2s(digest, 32, NULL, 0,
|
|
||||||
(const void *)TK1_RAM_BASE,
|
|
||||||
*app_size, &ctx);
|
|
||||||
print_digest(digest);
|
|
||||||
|
|
||||||
// And return the digest in final
|
|
||||||
// response
|
|
||||||
rsp[0] = STATUS_OK;
|
|
||||||
memcpy_s(&rsp[1], CMDLEN_MAXBYTES - 1,
|
|
||||||
&digest, 32);
|
|
||||||
fwreply(hdr, FW_RSP_LOAD_APP_DATA_READY,
|
|
||||||
rsp);
|
|
||||||
|
|
||||||
state = FW_STATE_RUN;
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
rsp[0] = STATUS_OK;
|
|
||||||
fwreply(hdr, FW_RSP_LOAD_APP_DATA, rsp);
|
|
||||||
|
|
||||||
break;
|
|
||||||
|
|
||||||
default:
|
|
||||||
htif_puts("Got unknown firmware cmd: 0x");
|
|
||||||
htif_puthex(cmd[0]);
|
|
||||||
htif_lf();
|
|
||||||
state = FW_STATE_FAIL;
|
|
||||||
}
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case FW_STATE_RUN:
|
case FW_STATE_RUN:
|
||||||
htif_puts("state_run\n");
|
htif_puts("state_run\n");
|
||||||
*app_addr = TK1_RAM_BASE;
|
run(&ctx);
|
||||||
|
|
||||||
// CDI = hash(uds, hash(app), uss)
|
|
||||||
compute_cdi(digest, use_uss, uss);
|
|
||||||
|
|
||||||
htif_puts("Flipping to app mode!\n");
|
|
||||||
htif_puts("Jumping to ");
|
|
||||||
htif_putinthex(*app_addr);
|
|
||||||
htif_lf();
|
|
||||||
|
|
||||||
// Clear the firmware stack
|
|
||||||
// clang-format off
|
|
||||||
asm volatile(
|
|
||||||
"li a0, 0xd0000000;" // FW_RAM
|
|
||||||
"li a1, 0xd0000800;" // End of 2 KB FW_RAM (just past the end)
|
|
||||||
"loop:;"
|
|
||||||
"sw zero, 0(a0);"
|
|
||||||
"addi a0, a0, 4;"
|
|
||||||
"blt a0, a1, loop;"
|
|
||||||
::: "memory");
|
|
||||||
// clang-format on
|
|
||||||
|
|
||||||
// Flip over to application mode
|
|
||||||
*switch_app = 1;
|
|
||||||
|
|
||||||
// XXX Firmware stack now no longer available
|
|
||||||
// Don't use any function calls!
|
|
||||||
|
|
||||||
// Jump to app - doesn't return
|
|
||||||
// clang-format off
|
|
||||||
asm volatile(
|
|
||||||
// Get value at TK1_MMIO_TK1_APP_ADDR
|
|
||||||
"lui a0,0xff000;"
|
|
||||||
"lw a0,0x030(a0);"
|
|
||||||
// Jump to it
|
|
||||||
"jalr x0,0(a0);"
|
|
||||||
::: "memory");
|
|
||||||
// clang-format on
|
|
||||||
break; // This is never reached!
|
break; // This is never reached!
|
||||||
|
|
||||||
case FW_STATE_FAIL:
|
case FW_STATE_FAIL:
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user