From 5b72dbfc23e4f9a62cfe787816c5b5e75a0aa597 Mon Sep 17 00:00:00 2001
From: Stefan Roese <sr@denx.de>
Date: Wed, 1 Jul 2015 12:44:51 +0200
Subject: arm: mvebu: Setup the MBUS bridge registers

With this patch, the MBUS bridge registers (base and size) are
configured upon each call to mbus_dt_setup_win(). This is needed, since
the board code can also call this function in later boot stages. As
done in the maxbcm board.

This is needed to fix a problem with the secondary CPU's not booting
in Linux on AXP.

Signed-off-by: Stefan Roese <sr@denx.de>
Cc: Peter Morrow <peter@senient.com>
Cc: Luka Perkov <luka.perkov@sartura.hr>

diff --git a/arch/arm/mach-mvebu/include/mach/soc.h b/arch/arm/mach-mvebu/include/mach/soc.h
index 125b5f2..71254c5 100644
--- a/arch/arm/mach-mvebu/include/mach/soc.h
+++ b/arch/arm/mach-mvebu/include/mach/soc.h
@@ -66,6 +66,9 @@
 #define MVEBU_SATA0_BASE	(MVEBU_REGISTER(0xa8000))
 #define MVEBU_SDIO_BASE		(MVEBU_REGISTER(0xd8000))
 
+#define MBUS_BRIDGE_WIN_CTRL_REG (MVEBU_REGISTER(0x20250))
+#define MBUS_BRIDGE_WIN_BASE_REG (MVEBU_REGISTER(0x20254))
+
 #define SDRAM_MAX_CS		4
 #define SDRAM_ADDR_MASK		0xFF000000
 
diff --git a/arch/arm/mach-mvebu/mbus.c b/arch/arm/mach-mvebu/mbus.c
index 9b76bce..71fa693 100644
--- a/arch/arm/mach-mvebu/mbus.c
+++ b/arch/arm/mach-mvebu/mbus.c
@@ -52,6 +52,7 @@
 #include <asm/io.h>
 #include <asm/arch/cpu.h>
 #include <asm/arch/soc.h>
+#include <linux/compat.h>
 #include <linux/mbus.h>
 
 #define BIT(nr)			(1UL << (nr))
@@ -407,6 +408,53 @@ int mvebu_mbus_del_window(phys_addr_t base, size_t size)
 	return 0;
 }
 
+static void mvebu_mbus_get_lowest_base(struct mvebu_mbus_state *mbus,
+				       phys_addr_t *base)
+{
+	int win;
+	*base = 0xffffffff;
+
+	for (win = 0; win < mbus->soc->num_wins; win++) {
+		u64 wbase;
+		u32 wsize;
+		u8 wtarget, wattr;
+		int enabled;
+
+		mvebu_mbus_read_window(mbus, win,
+				       &enabled, &wbase, &wsize,
+				       &wtarget, &wattr, NULL);
+
+		if (!enabled)
+			continue;
+
+		if (wbase < *base)
+			*base = wbase;
+	}
+}
+
+static void mvebu_config_mbus_bridge(struct mvebu_mbus_state *mbus)
+{
+	phys_addr_t base;
+	u32 val;
+	u32 size;
+
+	/* Set MBUS bridge base/ctrl */
+	mvebu_mbus_get_lowest_base(&mbus_state, &base);
+
+	size = 0xffffffff - base + 1;
+	if (!is_power_of_2(size)) {
+		/* Round up to next power of 2 */
+		size = 1 << (ffs(base) + 1);
+		base = 0xffffffff - size + 1;
+	}
+
+	/* Now write base and size */
+	writel(base, MBUS_BRIDGE_WIN_BASE_REG);
+	/* Align window size to 64KiB */
+	val = (size / (64 << 10)) - 1;
+	writel((val << 16) | 0x1, MBUS_BRIDGE_WIN_CTRL_REG);
+}
+
 int mbus_dt_setup_win(struct mvebu_mbus_state *mbus,
 		      u32 base, u32 size, u8 target, u8 attr)
 {
@@ -426,6 +474,13 @@ int mbus_dt_setup_win(struct mvebu_mbus_state *mbus,
 			return -ENOMEM;
 	}
 
+	/*
+	 * Re-configure the mbus bridge registers each time this function
+	 * is called. Since it may get called from the board code in
+	 * later boot stages as well.
+	 */
+	mvebu_config_mbus_bridge(mbus);
+
 	return 0;
 }
 
-- 
cgit v0.10.2