From acb9c37ee7a9a59f32492ee05bd994b74e60bdc6 Mon Sep 17 00:00:00 2001
From: Daniel Lublin <daniel@lublin.se>
Date: Mon, 21 Nov 2022 14:19:09 +0100
Subject: [PATCH] Add fw cmd to get UDI

---
 hw/application_fpga/fw/tk1/main.c  | 18 +++++++++++++++++-
 hw/application_fpga/fw/tk1/proto.c |  5 +++++
 hw/application_fpga/fw/tk1/proto.h |  2 ++
 3 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/hw/application_fpga/fw/tk1/main.c b/hw/application_fpga/fw/tk1/main.c
index b1bbcac..7e9d463 100644
--- a/hw/application_fpga/fw/tk1/main.c
+++ b/hw/application_fpga/fw/tk1/main.c
@@ -15,10 +15,11 @@ static volatile uint32_t *switch_app = (volatile uint32_t *)TK1_MMIO_TK1_SWITCH_
 static volatile uint32_t *name0 =      (volatile uint32_t *)TK1_MMIO_TK1_NAME0;
 static volatile uint32_t *name1 =      (volatile uint32_t *)TK1_MMIO_TK1_NAME1;
 static volatile uint32_t *ver =        (volatile uint32_t *)TK1_MMIO_TK1_VERSION;
+static volatile uint32_t *udi =        (volatile uint32_t *)TK1_MMIO_TK1_UDI_FIRST;
 static volatile uint32_t *cdi =        (volatile uint32_t *)TK1_MMIO_TK1_CDI_FIRST;
 static volatile uint32_t *app_addr =   (volatile uint32_t *)TK1_MMIO_TK1_APP_ADDR;
 static volatile uint32_t *app_size =   (volatile uint32_t *)TK1_MMIO_TK1_APP_SIZE;
-static volatile uint8_t *fw_ram =      (volatile uint8_t *)TK1_MMIO_FW_RAM_BASE;
+static volatile uint8_t  *fw_ram =     (volatile uint8_t  *)TK1_MMIO_FW_RAM_BASE;
 
 #define LED_RED   (1 << TK1_MMIO_TK1_LED_R_BIT)
 #define LED_GREEN (1 << TK1_MMIO_TK1_LED_G_BIT)
@@ -138,6 +139,21 @@ int main()
 			fwreply(hdr, FW_RSP_NAME_VERSION, rsp);
 			break;
 
+		case FW_CMD_GET_UDI:
+			puts("FW_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(udi_words, (void *)udi, 2);
+			memcpy(rsp + 1, udi_words, 2 * 4);
+			fwreply(hdr, FW_RSP_GET_UDI, rsp);
+			break;
+
 		case FW_CMD_LOAD_USS:
 			puts("cmd: load-uss\n");
 
diff --git a/hw/application_fpga/fw/tk1/proto.c b/hw/application_fpga/fw/tk1/proto.c
index 6b9b928..60e6d4a 100644
--- a/hw/application_fpga/fw/tk1/proto.c
+++ b/hw/application_fpga/fw/tk1/proto.c
@@ -96,6 +96,11 @@ void fwreply(struct frame_header hdr, enum fwcmd rspcode, uint8_t *buf)
 		nbytes = 128;
 		break;
 
+	case FW_RSP_GET_UDI:
+		len = LEN_32;
+		nbytes = 32;
+		break;
+
 	default:
 		puts("fwreply(): Unknown response code: 0x");
 		puthex(rspcode);
diff --git a/hw/application_fpga/fw/tk1/proto.h b/hw/application_fpga/fw/tk1/proto.h
index 898461b..6a29043 100644
--- a/hw/application_fpga/fw/tk1/proto.h
+++ b/hw/application_fpga/fw/tk1/proto.h
@@ -37,6 +37,8 @@ enum fwcmd {
 	FW_CMD_GET_APP_DIGEST	= 0x09,
 	FW_CMD_LOAD_USS		= 0x0a,
 	FW_RSP_LOAD_USS		= 0x0b,
+	FW_CMD_GET_UDI		= 0x0c,
+	FW_RSP_GET_UDI		= 0x0d,
 	/* ... */
 	FW_RSP_GET_APP_DIGEST	= 0x10, // encoded as 0x10 for backwards compatibility
 };