4 files changed, 258 insertions, 0 deletions
diff --git a/src/soc/amd/sabrina/psp_verstage/Makefile.inc b/src/soc/amd/sabrina/psp_verstage/Makefile.inc
new file mode 100644
index 000000000000..27539dafbc6f
--- /dev/null
+++ b/src/soc/amd/sabrina/psp_verstage/Makefile.inc
@@ -0,0 +1,16 @@
+# SPDX-License-Identifier: GPL-2.0-only
+
+# TODO: Check if this is still correct
+
+verstage-generic-ccopts += -I$(src)/soc/amd/sabrina/psp_verstage/include
+verstage-generic-ccopts += -I$(src)/vendorcode/amd/fsp/sabrina/include
+
+verstage-generic-ccopts += -I$(src)/soc/amd/common/psp_verstage/include
+
+subdirs-$(CONFIG_VBOOT_STARTS_BEFORE_BOOTBLOCK) += ../../common/psp_verstage
+
+verstage-y += svc.c
+verstage-y += chipset.c
+
+verstage-y += $(top)/src/vendorcode/amd/fsp/sabrina/bl_uapp/bl_uapp_startup.S
+verstage-y += $(top)/src/vendorcode/amd/fsp/sabrina/bl_uapp/bl_uapp_end.S
diff --git a/src/soc/amd/sabrina/psp_verstage/chipset.c b/src/soc/amd/sabrina/psp_verstage/chipset.c
new file mode 100644
index 000000000000..e82a1328308b
--- /dev/null
+++ b/src/soc/amd/sabrina/psp_verstage/chipset.c
@@ -0,0 +1,46 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+
+/* TODO: Check if this is still correct */
+
+#include <bl_uapp/bl_syscall_public.h>
+#include <psp_verstage.h>
+
+uint32_t update_psp_bios_dir(uint32_t *psp_dir_offset, uint32_t *bios_dir_offset)
+{
+	return svc_update_psp_bios_dir(psp_dir_offset, bios_dir_offset);
+}
+
+uint32_t save_uapp_data(void *address, uint32_t size)
+{
+	return svc_save_uapp_data(address, size);
+}
+
+uint32_t get_bios_dir_addr(struct embedded_firmware *ef_table)
+{
+	return ef_table->bios3_entry;
+}
+
+int platform_set_sha_op(enum vb2_hash_algorithm hash_alg,
+			struct sha_generic_data *sha_op)
+{
+	if (hash_alg == VB2_HASH_SHA256) {
+		sha_op->SHAType = SHA_TYPE_256;
+		sha_op->DigestLen = 32;
+	} else if (hash_alg == VB2_HASH_SHA384) {
+		sha_op->SHAType = SHA_TYPE_384;
+		sha_op->DigestLen = 48;
+	} else {
+		return -1;
+	}
+	return 0;
+}
+
+
+/* Functions below are stub functions for not-yet-implemented PSP features.
+ * These functions should be replaced with proper implementations later.
+ */
+
+uint32_t svc_write_postcode(uint32_t postcode)
+{
+	return 0;
+}
diff --git a/src/soc/amd/sabrina/psp_verstage/svc.c b/src/soc/amd/sabrina/psp_verstage/svc.c
new file mode 100644
index 000000000000..bf1a1062399e
--- /dev/null
+++ b/src/soc/amd/sabrina/psp_verstage/svc.c
@@ -0,0 +1,137 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+
+/* TODO: Check if this is still correct */
+
+#include "svc.h"
+
+#include <assert.h>
+#include <bl_uapp/bl_syscall_public.h>
+#include <psp_verstage.h>
+#include <stddef.h>
+
+void svc_exit(uint32_t status)
+{
+	uint32_t unused = 0;
+	SVC_CALL0(SVC_EXIT, unused);
+}
+
+void svc_debug_print(const char *string)
+{
+	uint32_t unused = 0;
+	SVC_CALL1(SVC_DEBUG_PRINT, (uint32_t)string, unused);
+}
+
+void svc_debug_print_ex(uint32_t dword0,
+		uint32_t dword1, uint32_t dword2, uint32_t dword3)
+{
+	uint32_t unused = 0;
+	SVC_CALL4(SVC_DEBUG_PRINT_EX, dword0, dword1, dword2, dword3, unused);
+}
+
+uint32_t svc_get_boot_mode(uint32_t *boot_mode)
+{
+	uint32_t retval = 0;
+	SVC_CALL1(SVC_GET_BOOT_MODE, boot_mode, retval);
+	return retval;
+}
+
+void svc_delay_in_usec(uint32_t delay)
+{
+	uint32_t unused = 0;
+	SVC_CALL1(SVC_DELAY_IN_MICRO_SECONDS, delay, unused);
+}
+
+uint32_t svc_get_spi_rom_info(struct spirom_info *spi_rom_info)
+{
+	uint32_t retval = 0;
+	SVC_CALL1(SVC_GET_SPI_INFO, (uint32_t)spi_rom_info, retval);
+	return retval;
+}
+
+uint32_t svc_map_fch_dev(enum fch_io_device io_device,
+		uint32_t arg1, uint32_t arg2, void **io_device_axi_addr)
+{
+	uint32_t retval = 0;
+	assert(io_device < FCH_IO_DEVICE_END);
+	SVC_CALL4(SVC_MAP_FCH_IO_DEVICE, io_device, arg1, arg2,
+			(uint32_t)io_device_axi_addr, retval);
+	return retval;
+}
+
+uint32_t svc_unmap_fch_dev(enum fch_io_device io_device, void *io_device_axi_addr)
+{
+	uint32_t retval = 0;
+	assert(io_device < FCH_IO_DEVICE_END);
+	SVC_CALL2(SVC_UNMAP_FCH_IO_DEVICE, (uint32_t)io_device,
+			(uint32_t)io_device_axi_addr, retval);
+	return retval;
+}
+
+uint32_t svc_map_spi_rom(void *spi_rom_addr,
+		uint32_t size, void **spi_rom_axi_addr)
+{
+	uint32_t retval = 0;
+	SVC_CALL3(SVC_MAP_SPIROM_DEVICE, spi_rom_addr, size,
+			(uint32_t)spi_rom_axi_addr, retval);
+	return retval;
+}
+
+uint32_t svc_unmap_spi_rom(void *spi_rom_addr)
+{
+	uint32_t retval = 0;
+	SVC_CALL1(SVC_UNMAP_SPIROM_DEVICE, (uint32_t)spi_rom_addr, retval);
+	return retval;
+}
+
+uint32_t svc_update_psp_bios_dir(uint32_t *psp_dir_offset,
+		uint32_t *bios_dir_offset)
+{
+	uint32_t retval = 0;
+	SVC_CALL2(SVC_UPDATE_PSP_BIOS_DIR, (uint32_t)psp_dir_offset,
+			(uint32_t)bios_dir_offset, retval);
+	return retval;
+}
+
+uint32_t svc_save_uapp_data(void *address, uint32_t size)
+{
+	uint32_t retval = 0;
+	SVC_CALL2(SVC_COPY_DATA_FROM_UAPP, (uint32_t)address, size, retval);
+	return retval;
+}
+
+uint32_t svc_read_timer_val(enum psp_timer_type type, uint64_t *counter_value)
+{
+	unsigned int retval = 0;
+	assert(type < PSP_TIMER_TYPE_MAX);
+	SVC_CALL2(SVC_READ_TIMER_VAL, type, counter_value, retval);
+	return retval;
+}
+
+uint32_t svc_reset_system(enum reset_type reset_type)
+{
+	unsigned int retval = 0;
+	assert(reset_type < RESET_TYPE_MAX);
+	SVC_CALL1(SVC_RESET_SYSTEM, reset_type, retval);
+	return retval;
+}
+
+uint32_t svc_crypto_sha(struct sha_generic_data *sha_op, enum sha_operation_mode sha_mode)
+{
+	uint32_t retval = 0;
+	SVC_CALL2(SVC_SHA, sha_op, sha_mode, retval);
+	return retval;
+}
+
+uint32_t svc_modexp(struct mod_exp_params *mod_exp_param)
+{
+	uint32_t retval = 0;
+	SVC_CALL1(SVC_MODEXP, mod_exp_param, retval);
+	return retval;
+}
+
+uint32_t svc_ccp_dma(uint32_t spi_rom_offset, void *dest, uint32_t size)
+{
+	uint32_t retval = 0;
+	SVC_CALL3(SVC_CCP_DMA, spi_rom_offset, dest, size, retval);
+	return retval;
+}
diff --git a/src/soc/amd/sabrina/psp_verstage/svc.h b/src/soc/amd/sabrina/psp_verstage/svc.h
new file mode 100644
index 000000000000..86e533a88daa
--- /dev/null
+++ b/src/soc/amd/sabrina/psp_verstage/svc.h
@@ -0,0 +1,59 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+
+/* TODO: Check if this is still correct */
+
+#ifndef PSP_VERSTAGE_SVC_H
+#define PSP_VERSTAGE_SVC_H
+
+#define SVC_CALL4(SVC_ID, R0, R1, R2, R3, Ret) \
+	__asm__ __volatile__ ( \
+	"mov r0, %[reg0]\n\t" \
+	"mov r1, %[reg1]\n\t" \
+	"mov r2, %[reg2]\n\t" \
+	"mov r3, %[reg3]\n\t" \
+	"svc %[id]\n\t" \
+	"mov %[result], r0\n\t" \
+	: [result] "=r" (Ret) /* output */ \
+	: [id] "i" (SVC_ID), [reg0] "r" (R0), [reg1] "r" (R1), [reg2] "r" (R2), \
+		[reg3] "r" (R3) /* input(s) */ \
+	: "r0", "r1", "r2", "r3", "memory", "cc" /* list of clobbered registers */)
+
+#define SVC_CALL3(SVC_ID, R0,  R1, R2, Ret) \
+	__asm__ __volatile__ ( \
+	"mov r0, %[reg0]\n\t" \
+	"mov r1, %[reg1]\n\t" \
+	"mov r2, %[reg2]\n\t" \
+	"svc %[id]\n\t" \
+	"mov %[result], r0\n\t" \
+	: [result] "=r" (Ret) /* output */ \
+	: [id] "i" (SVC_ID), [reg0] "r" (R0), [reg1] "r" (R1), [reg2] "r" (R2) \
+	: "r0", "r1", "r2", "memory", "cc" /* list of clobbered registers */)
+
+#define SVC_CALL2(SVC_ID, R0, R1, Ret) \
+	__asm__ __volatile__ ( \
+	"mov r0, %[reg0]\n\t" \
+	"mov r1, %[reg1]\n\t" \
+	"svc %[id]\n\t" \
+	"mov %[result], r0\n\t" \
+	: [result] "=r" (Ret) /* output */ \
+	: [id] "i" (SVC_ID), [reg0] "r" (R0), [reg1] "r" (R1)/* input(s) */ \
+	: "r0", "r1", "memory", "cc" /* list of clobbered registers */)
+
+#define SVC_CALL1(SVC_ID, R0, Ret) \
+	__asm__ __volatile__ ( \
+	"mov r0, %[reg0]\n\t" \
+	"svc %[id]\n\t" \
+	"mov %[result], r0\n\t" \
+	: [result] "=r" (Ret) /* output */ \
+	: [id] "i" (SVC_ID), [reg0] "r" (R0) /* input(s) */ \
+	: "r0", "memory", "cc" /* list of clobbered registers */)
+
+#define SVC_CALL0(SVC_ID, Ret) \
+	__asm__ __volatile__ ( \
+	"svc %[id]\n\t" \
+	"mov %[result], r0\n\t" \
+	: [result] "=r" (Ret) /* output */ \
+	: [id] "I" (SVC_ID) /* input(s) */ \
+	: "memory", "cc" /* list of clobbered registers */)
+
+#endif /* PSP_VERSTAGE_SVC_H */