From 347167bd9ac2a68870064edd921f2525bd569c6d Mon Sep 17 00:00:00 2001
From: Phil Elwell <phil@raspberrypi.org>
Date: Mon, 16 Jul 2018 14:40:13 +0100
Subject: [PATCH] dwc-otg: FIQ: Fix "bad mode in data abort handler"

Create a semi-static mapping for the USB registers early in the boot
process, before additional kernel threads are started, so all threads
will have the mappings from the start. This avoids the need for
data aborts to lazily update them.

See: https://github.com/raspberrypi/linux/issues/2450

Signed-off-by: Floris Bos <bos@je-eigen-domein.nl>
---
 arch/arm/mach-bcm/board_bcm2835.c | 69 +++++++++++++++++++++++++++++++
 1 file changed, 69 insertions(+)

--- a/arch/arm/mach-bcm/board_bcm2835.c
+++ b/arch/arm/mach-bcm/board_bcm2835.c
@@ -6,6 +6,7 @@
 #include <linux/init.h>
 #include <linux/irqchip.h>
 #include <linux/of_address.h>
+#include <linux/of_fdt.h>
 #include <asm/system_info.h>
 
 #include <asm/mach/arch.h>
@@ -13,6 +14,9 @@
 
 #include "platsmp.h"
 
+#define BCM2835_USB_VIRT_BASE   0xf0980000
+#define BCM2835_USB_VIRT_MPHI   0xf0006000
+
 static void __init bcm2835_init(void)
 {
 	struct device_node *np = of_find_node_by_path("/system");
@@ -25,6 +29,70 @@ static void __init bcm2835_init(void)
 		system_serial_low = val64;
 }
 
+/*
+ * We need to map registers that are going to be accessed by the FIQ
+ * very early, before any kernel threads are spawned. Because if done
+ * later, the mapping tables are not updated instantly but lazily upon
+ * first access through a data abort handler. While that is fine
+ * when executing regular kernel code, if the first access in a specific
+ * thread happens while running FIQ code this will result in a panic.
+ *
+ * For more background see the following old mailing list thread:
+ * https://www.spinics.net/lists/arm-kernel/msg325250.html
+ */
+static int __init bcm2835_map_usb(unsigned long node, const char *uname,
+					int depth, void *data)
+{
+	struct map_desc map[2];
+	const __be32 *reg;
+	int len;
+	unsigned long p2b_offset = *((unsigned long *) data);
+
+	if (!of_flat_dt_is_compatible(node, "brcm,bcm2708-usb"))
+		return 0;
+	reg = of_get_flat_dt_prop(node, "reg", &len);
+	if (!reg || len != (sizeof(unsigned long) * 4))
+		return 0;
+
+	/* Use information about the physical addresses of the
+	 * registers from the device tree, but use legacy
+	 * iotable_init() static mapping function to map them,
+	 * as ioremap() is not functional at this stage in boot.
+	 */
+	map[0].virtual = (unsigned long) BCM2835_USB_VIRT_BASE;
+	map[0].pfn = __phys_to_pfn(be32_to_cpu(reg[0]) - p2b_offset);
+	map[0].length = be32_to_cpu(reg[1]);
+	map[0].type = MT_DEVICE;
+	map[1].virtual = (unsigned long) BCM2835_USB_VIRT_MPHI;
+	map[1].pfn = __phys_to_pfn(be32_to_cpu(reg[2]) - p2b_offset);
+	map[1].length = be32_to_cpu(reg[3]);
+	map[1].type = MT_DEVICE;
+		iotable_init(map, 2);
+
+	return 1;
+}
+
+static void __init bcm2835_map_io(void)
+{
+	const __be32 *ranges;
+	int soc, len;
+	unsigned long p2b_offset;
+
+	debug_ll_io_init();
+
+	/* Find out how to map bus to physical address first from soc/ranges */
+	soc = of_get_flat_dt_subnode_by_name(of_get_flat_dt_root(), "soc");
+	if (soc < 0)
+		return;
+	ranges = of_get_flat_dt_prop(soc, "ranges", &len);
+	if (!ranges || len < (sizeof(unsigned long) * 3))
+		return;
+	p2b_offset = be32_to_cpu(ranges[0]) - be32_to_cpu(ranges[1]);
+
+	/* Now search for bcm2708-usb node in device tree */
+	of_scan_flat_dt(bcm2835_map_usb, &p2b_offset);
+}
+
 static const char * const bcm2835_compat[] = {
 #ifdef CONFIG_ARCH_MULTI_V6
 	"brcm,bcm2835",
@@ -37,6 +105,7 @@ static const char * const bcm2835_compat
 };
 
 DT_MACHINE_START(BCM2835, "BCM2835")
+	.map_io = bcm2835_map_io,
 	.init_machine = bcm2835_init,
 	.dt_compat = bcm2835_compat,
 	.smp = smp_ops(bcm2836_smp_ops),
