1 files changed, 87 insertions, 0 deletions
diff --git a/arch/arm/cpu/armv7/mx6/mp.c b/arch/arm/cpu/armv7/mx6/mp.c
new file mode 100644
index 0000000000..9f034d6e13
--- /dev/null
+++ b/arch/arm/cpu/armv7/mx6/mp.c
@@ -0,0 +1,87 @@
+/*
+ * (C) Copyright 2014
+ * Gabriel Huau <contact@huau-gabriel.fr>
+ *
+ * (C) Copyright 2009 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/errno.h>
+#include <asm/arch/sys_proto.h>
+#include <asm/arch/imx-regs.h>
+
+#define MAX_CPUS 4
+static struct src *src = (struct src *)SRC_BASE_ADDR;
+
+static uint32_t cpu_reset_mask[MAX_CPUS] = {
+	0, /* We don't really want to modify the cpu0 */
+	SRC_SCR_CORE_1_RESET_MASK,
+	SRC_SCR_CORE_2_RESET_MASK,
+	SRC_SCR_CORE_3_RESET_MASK
+};
+
+static uint32_t cpu_ctrl_mask[MAX_CPUS] = {
+	0, /* We don't really want to modify the cpu0 */
+	SRC_SCR_CORE_1_ENABLE_MASK,
+	SRC_SCR_CORE_2_ENABLE_MASK,
+	SRC_SCR_CORE_3_ENABLE_MASK
+};
+
+int cpu_reset(int nr)
+{
+	/* Software reset of the CPU N */
+	src->scr |= cpu_reset_mask[nr];
+	return 0;
+}
+
+int cpu_status(int nr)
+{
+	printf("core %d => %d\n", nr, !!(src->scr & cpu_ctrl_mask[nr]));
+	return 0;
+}
+
+int cpu_release(int nr, int argc, char *const argv[])
+{
+	uint32_t boot_addr;
+
+	boot_addr = simple_strtoul(argv[0], NULL, 16);
+
+	switch (nr) {
+	case 1:
+		src->gpr3 = boot_addr;
+		break;
+	case 2:
+		src->gpr5 = boot_addr;
+		break;
+	case 3:
+		src->gpr7 = boot_addr;
+		break;
+	default:
+		return 1;
+	}
+
+	/* CPU N is ready to start */
+	src->scr |= cpu_ctrl_mask[nr];
+
+	return 0;
+}
+
+int is_core_valid(unsigned int core)
+{
+	uint32_t nr_cores = get_nr_cpus();
+
+	if (core > nr_cores)
+		return 0;
+
+	return 1;
+}
+
+int cpu_disable(int nr)
+{
+	/* Disable the CPU N */
+	src->scr &= ~cpu_ctrl_mask[nr];
+	return 0;
+}