1 files changed, 53 insertions, 7 deletions

diff --git a/common/tpm_registers.c b/common/tpm_registers.c
index 52de4aa6f6..debfbfb613 100644
--- a/common/tpm_registers.c
+++ b/common/tpm_registers.c
@@ -10,9 +10,8 @@
  */
 
 #include "byteorder.h"
-#include "common.h"
 #include "console.h"
-#include "hooks.h"
+#include "extension.h"
 #include "task.h"
 #include "tpm_registers.h"
 #include "util.h"
@@ -412,6 +411,35 @@ static void tpm_init(void)
 	_plat__SetNvAvail();
 }
 
+#ifdef CONFIG_EXTENSION_COMMAND
+
+static void call_extension_command(struct tpm_cmd_header *tpmh,
+				  size_t *total_size)
+{
+	size_t command_size = be32toh(tpmh->size);
+
+	/* Verify there is room for at least the extension command header. */
+	if (command_size >= sizeof(struct tpm_cmd_header)) {
+		uint16_t subcommand_code;
+
+		/* The header takes room in the buffer. */
+		*total_size -= sizeof(struct tpm_cmd_header);
+
+		subcommand_code = be16toh(tpmh->subcommand_code);
+		extension_route_command(subcommand_code,
+				       tpmh + 1,
+				       command_size -
+				       sizeof(struct tpm_cmd_header),
+				       total_size);
+		/* Add the header size back. */
+		*total_size += sizeof(struct tpm_cmd_header);
+		tpmh->size = htobe32(*total_size);
+	} else {
+		*total_size = command_size;
+	}
+}
+#endif
+
 void tpm_task(void)
 {
 	tpm_init();
@@ -429,14 +457,32 @@ void tpm_task(void)
 		CPRINTF("%s: received fifo command 0x%04x\n",
 			__func__, command_code);
 
-		ExecuteCommand(tpm_.fifo_write_index,
-			       tpm_.regs.data_fifo,
-			       &response_size,
-			       &response);
+#ifdef CONFIG_EXTENSION_COMMAND
+		if (command_code == CONFIG_EXTENSION_COMMAND) {
+			response_size = sizeof(tpm_.regs.data_fifo);
+			call_extension_command(tpmh, &response_size);
+		} else
+#endif
+		{
+			ExecuteCommand(tpm_.fifo_write_index,
+				       tpm_.regs.data_fifo,
+				       &response_size,
+				       &response);
+		}
 		CPRINTF("got %d bytes in response\n", response_size);
 		if (response_size &&
 		    (response_size <= sizeof(tpm_.regs.data_fifo))) {
-			memcpy(tpm_.regs.data_fifo, response, response_size);
+#ifdef CONFIG_EXTENSION_COMMAND
+			if (command_code != CONFIG_EXTENSION_COMMAND)
+#endif
+			{
+				/*
+				 * Extension commands reuse FIFO buffer, the
+				 * rest need to copy.
+				 */
+				memcpy(tpm_.regs.data_fifo,
+				       response, response_size);
+			}
 			tpm_.fifo_read_index = 0;
 			tpm_.fifo_write_index = response_size;
 			tpm_.regs.sts |= data_avail;