From c0c4d2c0acb253b7eb8e73cbf5950618d30702f7 Mon Sep 17 00:00:00 2001
From: br <br@FreeBSD.org>
Date: Thu, 12 Jun 2014 11:37:38 +0000
Subject: Add machine-dependent SMP part for Exynos5420

---
 sys/arm/samsung/exynos/exynos5_machdep.c |  3 ++
 sys/arm/samsung/exynos/exynos5_mp.c      | 75 ++++++++++++++++++++++++++++----
 2 files changed, 70 insertions(+), 8 deletions(-)

diff --git a/sys/arm/samsung/exynos/exynos5_machdep.c b/sys/arm/samsung/exynos/exynos5_machdep.c
index 24fd3f4..9e99f68 100644
--- a/sys/arm/samsung/exynos/exynos5_machdep.c
+++ b/sys/arm/samsung/exynos/exynos5_machdep.c
@@ -74,6 +74,9 @@ int
 platform_devmap_init(void)
 {
 
+	/* CHIP ID */
+	arm_devmap_add_entry(0x10000000, 0x100000);
+
 	/* UART */
 	arm_devmap_add_entry(0x12C00000, 0x100000);
 
diff --git a/sys/arm/samsung/exynos/exynos5_mp.c b/sys/arm/samsung/exynos/exynos5_mp.c
index 7b97a6e..aaeaf80 100644
--- a/sys/arm/samsung/exynos/exynos5_mp.c
+++ b/sys/arm/samsung/exynos/exynos5_mp.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2013 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2013-2014 Ruslan Bukin <br@bsdpad.com>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -37,7 +37,34 @@ __FBSDID("$FreeBSD$");
 #include <machine/fdt.h>
 #include <machine/intr.h>
 
-#define	EXYNOS_SYSRAM			0x02020000
+#define	EXYNOS_CHIPID		0x10000000
+
+#define	EXYNOS5250_SOC_ID	0x43520000
+#define	EXYNOS5420_SOC_ID	0xE5420000
+#define	EXYNOS5_SOC_ID_MASK	0xFFFFF000
+
+#define	EXYNOS_SYSRAM		0x02020000
+#define	EXYNOS5420_SYSRAM_NS	(EXYNOS_SYSRAM + 0x53000 + 0x1c)
+
+#define	EXYNOS_PMU_BASE		0x10040000
+#define	CORE_CONFIG(n)		(0x2000 + (0x80 * (n)))
+#define	CORE_STATUS(n)		(CORE_CONFIG(n) + 0x4)
+#define	CORE_PWR_EN		0x3
+
+static int
+exynos_get_soc_id(void)
+{
+	bus_addr_t chipid;
+	int reg;
+
+	if (bus_space_map(fdtbus_bs_tag, EXYNOS_CHIPID,
+		0x1000, 0, &chipid) != 0)
+		panic("Couldn't map chipid\n");
+	reg = bus_space_read_4(fdtbus_bs_tag, chipid, 0x0);
+	bus_space_unmap(fdtbus_bs_tag, chipid, 0x1000);
+
+	return (reg & EXYNOS5_SOC_ID_MASK);
+}
 
 void
 platform_mp_init_secondary(void)
@@ -50,27 +77,58 @@ void
 platform_mp_setmaxid(void)
 {
 
-	mp_maxid = 1;
+	if (exynos_get_soc_id() == EXYNOS5420_SOC_ID)
+		mp_ncpus = 4;
+	else
+		mp_ncpus = 2;
+
+	mp_maxid = mp_ncpus - 1;
 }
 
 int
 platform_mp_probe(void)
 {
 
-	mp_ncpus = 2;
-	return (1);
+	return (mp_ncpus > 1);
 }
 
 void
 platform_mp_start_ap(void)
 {
-	bus_addr_t sysram;
-	int err;
+	bus_addr_t sysram, pmu;
+	int err, i, j;
+	int status;
+	int reg;
+
+	err = bus_space_map(fdtbus_bs_tag, EXYNOS_PMU_BASE, 0x20000, 0, &pmu);
+	if (err != 0)
+		panic("Couldn't map pmu\n");
+
+	if (exynos_get_soc_id() == EXYNOS5420_SOC_ID)
+		reg = EXYNOS5420_SYSRAM_NS;
+	else
+		reg = EXYNOS_SYSRAM;
 
-	err = bus_space_map(fdtbus_bs_tag, EXYNOS_SYSRAM, 0x100, 0, &sysram);
+	err = bus_space_map(fdtbus_bs_tag, reg, 0x100, 0, &sysram);
 	if (err != 0)
 		panic("Couldn't map sysram\n");
 
+	/* Give power to CPUs */
+	for (i = 1; i < mp_ncpus; i++) {
+		bus_space_write_4(fdtbus_bs_tag, pmu, CORE_CONFIG(i),
+		    CORE_PWR_EN);
+
+		for (j = 10; j >= 0; j--) {
+			status = bus_space_read_4(fdtbus_bs_tag, pmu,
+			    CORE_STATUS(i));
+			if ((status & CORE_PWR_EN) == CORE_PWR_EN)
+				break;
+			DELAY(10);
+			if (j == 0)
+				printf("Can't power on CPU%d\n", i);
+		}
+	}
+
 	bus_space_write_4(fdtbus_bs_tag, sysram, 0x0,
 	    pmap_kextract((vm_offset_t)mpentry));
 
@@ -79,6 +137,7 @@ platform_mp_start_ap(void)
 
 	armv7_sev();
 	bus_space_unmap(fdtbus_bs_tag, sysram, 0x100);
+	bus_space_unmap(fdtbus_bs_tag, pmu, 0x20000);
 }
 
 void
-- 
cgit v1.1