3 files changed, 163 insertions, 16 deletions

diff --git a/src/arch/arm64/armv8/secmon/Makefile.inc b/src/arch/arm64/armv8/secmon/Makefile.inc
index 5dadfa9a63..040f56829a 100644
--- a/src/arch/arm64/armv8/secmon/Makefile.inc
+++ b/src/arch/arm64/armv8/secmon/Makefile.inc
@@ -32,6 +32,7 @@ secmon-c-ccopts += -I$(src)/arch/arm64/include/armv8/ -include $(src)/include/kc
 secmon-S-ccopts += -I$(src)/arch/arm64/include/armv8/ -include $(src)/include/kconfig.h -D__SECMON__
 
 secmon-y += secmon_init.c
+secmon-y += smc.c
 secmon-y += ../exception.c
 secmon-y += ../../cpu.c
 secmon-y += ../../transition_asm.S ../../transition.c
@@ -48,4 +49,4 @@ $(SECMON_SRC): $(SECMON_RMOD)
 
 $(SECMON_RAMSTAGE): $(SECMON_SRC)
 		    @printf "    OBJCOPY $(subst $(obj)/,,$(@))\n"
-		    cd $(dir $@); $(OBJCOPY_secmon) -I binary $(notdir $<) -O elf64-littleaarch64 -B aarch64 $(notdir $@)
\ No newline at end of file
+		    cd $(dir $@); $(OBJCOPY_secmon) -I binary $(notdir $<) -O elf64-littleaarch64 -B aarch64 $(notdir $@)
diff --git a/src/arch/arm64/armv8/secmon/secmon_init.c b/src/arch/arm64/armv8/secmon/secmon_init.c
index 660d6d432b..de433bb9ef 100644
--- a/src/arch/arm64/armv8/secmon/secmon_init.c
+++ b/src/arch/arm64/armv8/secmon/secmon_init.c
@@ -24,6 +24,7 @@
 #include <arch/exception.h>
 #include <arch/lib_helpers.h>
 #include <arch/secmon.h>
+#include <arch/smc.h>
 #include <arch/transition.h>
 #include <console/console.h>
 #include <rmodule.h>
@@ -40,20 +41,6 @@ static void cpu_init(int bsp)
 		cpu_set_bsp();
 }
 
-static void secmon_el3_init(void)
-{
-	uint32_t scr;
-
-	scr = raw_read_scr_el3();
-
-	/* Enable SMC */
-	scr &= ~(SCR_SMC_MASK);
-	scr |= SCR_SMC_ENABLE;
-
-	raw_write_scr_el3(scr);
-	isb();
-}
-
 static void secmon_init(struct secmon_params *params, int bsp)
 {
 	struct exc_state exc_state;
@@ -61,7 +48,7 @@ static void secmon_init(struct secmon_params *params, int bsp)
 	exception_hwinit();
 	cpu_init(bsp);
 
-	secmon_el3_init();
+	smc_init();
 
 	/*
 	 * Check if the arg is non-NULL
diff --git a/src/arch/arm64/armv8/secmon/smc.c b/src/arch/arm64/armv8/secmon/smc.c
new file mode 100644
index 0000000000..3bb52f0311
--- /dev/null
+++ b/src/arch/arm64/armv8/secmon/smc.c
@@ -0,0 +1,159 @@
+/*
+ * This file is part of the coreboot project.
+ *
+ * Copyright 2014 Google Inc.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; version 2 of the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA, 02110-1301 USA
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <arch/cpu.h>
+#include <arch/smc.h>
+#include <arch/exception.h>
+#include <arch/lib_helpers.h>
+#include <console/console.h>
+
+enum {
+	EC_SMC32 = 0x13,
+	EC_SMC64 = 0x17,
+
+	SMC_NUM_RANGES = 8,
+};
+
+struct smc_range {
+	uint32_t func_begin;
+	uint32_t func_end;
+	int (*handler)(struct smc_call *);
+};
+
+struct smc_ranges {
+	size_t used;
+	struct smc_range handlers[SMC_NUM_RANGES];
+};
+
+static struct smc_ranges smc_functions;
+
+static struct smc_range *smc_handler_by_function(uint32_t fid)
+{
+	int i;
+
+	for (i = 0; i < smc_functions.used; i++) {
+		struct smc_range *r = &smc_functions.handlers[i];
+
+		if (fid >= r->func_begin && fid <= r->func_end)
+			return r;
+	}
+
+	return NULL;
+}
+
+int smc_register_range(uint32_t min, uint32_t max, int (*h)(struct smc_call *))
+{
+	struct smc_range *r;
+
+	if (smc_functions.used == SMC_NUM_RANGES)
+		return -1;
+
+	if (min > max)
+		return -1;
+
+	/* This check isn't exhaustive but it's fairly quick. */
+	if (smc_handler_by_function(min) || smc_handler_by_function(max))
+		return -1;
+
+	r = &smc_functions.handlers[smc_functions.used];
+	r->func_begin = min;
+	r->func_end = max;
+	r->handler = h;
+	smc_functions.used++;
+
+	return 0;
+}
+
+static int smc_cleanup(struct exc_state *state, struct smc_call *smc, int ret)
+{
+	memcpy(&state->regs.x, &smc->results, ARRAY_SIZE(smc->results));
+
+	return ret;
+}
+
+static int smc_return_with_error(struct exc_state *state, struct smc_call *smc)
+{
+	smc32_return(smc, SMC_UNKNOWN_FUNC);
+	return smc_cleanup(state, smc, EXC_RET_HANDLED);
+}
+
+static int smc_handler(struct exc_state *state, uint64_t vector_id)
+{
+	struct smc_call smc_storage;
+	struct smc_call *smc = &smc_storage;
+	uint32_t exception_class;
+	uint32_t esr;
+	struct smc_range *r;
+
+	memcpy(&smc->args, &state->regs.x, ARRAY_SIZE(smc->args));
+	memcpy(&smc->results, &state->regs.x, ARRAY_SIZE(smc->results));
+
+	esr = raw_read_esr_el3();
+	exception_class = (esr >> 26) & 0x3f;
+
+	/* No support for 32-bit SMC calls. */
+	if (exception_class == EC_SMC32)
+		smc_return_with_error(state, smc);
+
+	/* Check to ensure this is an SMC from aarch64. */
+	if (exception_class != EC_SMC64)
+		return EXC_RET_IGNORED;
+
+	/* Ensure immediate value is 0. */
+	if ((esr & 0xffff) != 0)
+		smc_return_with_error(state, smc);
+
+	r = smc_handler_by_function(smc_function_id(smc));
+
+	if (r != NULL) {
+		if (!r->handler(smc))
+			return smc_cleanup(state, smc, EXC_RET_HANDLED);
+	}
+
+	return smc_return_with_error(state, smc);
+}
+
+/* SMC calls can be generated by 32-bit or 64-bit code. */
+static struct exception_handler smc_handler64 = {
+	.handler = &smc_handler,
+};
+
+static struct exception_handler smc_handler32 = {
+	.handler = &smc_handler,
+};
+
+void smc_init(void)
+{
+	uint32_t scr;
+
+	/* Enable SMC */
+	scr = raw_read_scr_el3();
+	scr &= ~(SCR_SMC_MASK);
+	scr |= SCR_SMC_ENABLE;
+	raw_write_scr_el3(scr);
+
+	if (!cpu_is_bsp())
+		return;
+
+	/* Register SMC handlers. */
+	exception_handler_register(EXC_VID_LOW64_SYNC, &smc_handler64);
+	exception_handler_register(EXC_VID_LOW32_SYNC, &smc_handler32);
+}