1 files changed, 50 insertions, 0 deletions

diff --git a/arch/arm/mach-mvebu/system-controller.c b/arch/arm/mach-mvebu/system-controller.c
index 0c5524ac75b7..a068cb5c2ce8 100644
--- a/arch/arm/mach-mvebu/system-controller.c
+++ b/arch/arm/mach-mvebu/system-controller.c
@@ -28,8 +28,14 @@
 #include <linux/io.h>
 #include <linux/reboot.h>
 #include "common.h"
+#include "mvebu-soc-id.h"
+#include "pmsu.h"
+
+#define ARMADA_375_CRYPT0_ENG_TARGET 41
+#define ARMADA_375_CRYPT0_ENG_ATTR    1
 
 static void __iomem *system_controller_base;
+static phys_addr_t system_controller_phys_base;
 
 struct mvebu_system_controller {
 	u32 rstoutn_mask_offset;
@@ -39,6 +45,9 @@ struct mvebu_system_controller {
 	u32 system_soft_reset;
 
 	u32 resume_boot_addr;
+
+	u32 dev_id;
+	u32 rev_id;
 };
 static struct mvebu_system_controller *mvebu_sc;
 
@@ -47,6 +56,8 @@ static const struct mvebu_system_controller armada_370_xp_system_controller = {
 	.system_soft_reset_offset = 0x64,
 	.rstoutn_mask_reset_out_en = 0x1,
 	.system_soft_reset = 0x1,
+	.dev_id = 0x38,
+	.rev_id = 0x3c,
 };
 
 static const struct mvebu_system_controller armada_375_system_controller = {
@@ -55,6 +66,8 @@ static const struct mvebu_system_controller armada_375_system_controller = {
 	.rstoutn_mask_reset_out_en = 0x1,
 	.system_soft_reset = 0x1,
 	.resume_boot_addr = 0xd4,
+	.dev_id = 0x38,
+	.rev_id = 0x3c,
 };
 
 static const struct mvebu_system_controller orion_system_controller = {
@@ -101,11 +114,45 @@ void mvebu_restart(enum reboot_mode mode, const char *cmd)
 		;
 }
 
+int mvebu_system_controller_get_soc_id(u32 *dev, u32 *rev)
+{
+	if (of_machine_is_compatible("marvell,armada380") &&
+		system_controller_base) {
+		*dev = readl(system_controller_base + mvebu_sc->dev_id) >> 16;
+		*rev = (readl(system_controller_base + mvebu_sc->rev_id) >> 8)
+			& 0xF;
+		return 0;
+	} else
+		return -ENODEV;
+}
+
 #ifdef CONFIG_SMP
+void mvebu_armada375_smp_wa_init(void)
+{
+	u32 dev, rev;
+	phys_addr_t resume_addr_reg;
+
+	if (mvebu_get_soc_id(&dev, &rev) != 0)
+		return;
+
+	if (rev != ARMADA_375_Z1_REV)
+		return;
+
+	resume_addr_reg = system_controller_phys_base +
+		mvebu_sc->resume_boot_addr;
+	mvebu_setup_boot_addr_wa(ARMADA_375_CRYPT0_ENG_TARGET,
+				 ARMADA_375_CRYPT0_ENG_ATTR,
+				 resume_addr_reg);
+}
+
 void mvebu_system_controller_set_cpu_boot_addr(void *boot_addr)
 {
 	BUG_ON(system_controller_base == NULL);
 	BUG_ON(mvebu_sc->resume_boot_addr == 0);
+
+	if (of_machine_is_compatible("marvell,armada375"))
+		mvebu_armada375_smp_wa_init();
+
 	writel(virt_to_phys(boot_addr), system_controller_base +
 	       mvebu_sc->resume_boot_addr);
 }
@@ -119,7 +166,10 @@ static int __init mvebu_system_controller_init(void)
 	np = of_find_matching_node_and_match(NULL, of_system_controller_table,
 					     &match);
 	if (np) {
+		struct resource res;
 		system_controller_base = of_iomap(np, 0);
+		of_address_to_resource(np, 0, &res);
+		system_controller_phys_base = res.start;
 		mvebu_sc = (struct mvebu_system_controller *)match->data;
 		of_node_put(np);
 	}