highbank: add of fixup

depending on the power domain register we need to disable sata or mmc
and update the cpu informations

take from Calxeda U-Boot git

Register the original dtb to /dev/firmware-dtb and the fixed dtb to /dev/dtb

Cc: Rob Herring <rob.herring@calxeda.com>
Signed-off-by: Jean-Christophe PLAGNIOL-VILLARD <plagnioj@jcrosoft.com>
Signed-off-by: Sascha Hauer <s.hauer@pengutronix.de>

1 files changed, 47 insertions, 1 deletions

diff --git a/arch/arm/boards/highbank/init.c b/arch/arm/boards/highbank/init.c
index bcd2783a72..e36674f0eb 100644
--- a/arch/arm/boards/highbank/init.c
+++ b/arch/arm/boards/highbank/init.c
@@ -10,15 +10,57 @@
 #include <asm/system_info.h>
 #include <generated/mach-types.h>
 #include <mach/devices.h>
+#include <mach/hardware.h>
+#include <mach/sysregs.h>
 #include <environment.h>
 #include <partition.h>
 #include <sizes.h>
 #include <io.h>
+#include <libfdt.h>
 
 #define FIRMWARE_DTB_BASE	0x1000
 
+#define HB_OPP_VERSION		0
+
 struct fdt_header *fdt = NULL;
 
+static int hb_fixup(struct fdt_header *fdt)
+{
+	u32 reg = readl(sregs_base + HB_SREG_A9_PWRDOM_DATA);
+	u32 *opp_table = (u32 *)HB_SYSRAM_OPP_TABLE_BASE;
+	u32 dtb_table[2*10];
+	u32 i;
+	u32 num_opps;
+	__be32 latency;
+
+	if (!(reg & HB_PWRDOM_STAT_SATA))
+		do_fixup_by_compatible_string(fdt, "calxeda,hb-ahci", "status",
+					           "disabled", 1);
+
+	if (!(reg & HB_PWRDOM_STAT_EMMC))
+		do_fixup_by_compatible_string(fdt, "calxeda,hb-sdhci", "status",
+					           "disabled", 1);
+
+	if ((opp_table[0] >> 16) != HB_OPP_VERSION)
+		return 0;
+
+	num_opps = opp_table[0] & 0xff;
+
+	for (i = 0; i < num_opps; i++) {
+		dtb_table[2 * i] = cpu_to_be32(opp_table[3 + 3 * i]);
+		dtb_table[2 * i + 1] = cpu_to_be32(opp_table[2 + 3 * i]);
+	}
+
+	latency = cpu_to_be32(opp_table[1]);
+
+	fdt_find_and_setprop(fdt, "/cpus/cpu@0", "transition-latency",
+				  &latency, 4, 1);
+	fdt_find_and_setprop(fdt, "/cpus/cpu@0", "operating-points",
+				  dtb_table, 8 * num_opps, 1);
+
+	return 0;
+}
+
 static int highbank_mem_init(void)
 {
 	struct device_node *np;
@@ -57,6 +99,7 @@ mem_initcall(highbank_mem_init);
 
 static int highbank_devices_init(void)
 {
+	of_register_fixup(hb_fixup);
 	if (!fdt) {
 		highbank_register_gpio(0);
 		highbank_register_gpio(1);
@@ -66,7 +109,10 @@ static int highbank_devices_init(void)
 		highbank_register_xgmac(0);
 		highbank_register_xgmac(1);
 	} else {
-		devfs_add_partition("ram0", FIRMWARE_DTB_BASE, SZ_64K, DEVFS_PARTITION_FIXED, "dtb");
+		fdt = of_get_fixed_tree(fdt);
+		add_mem_device("dtb", (unsigned long)fdt, fdt_totalsize(fdt),
+		       IORESOURCE_MEM_WRITEABLE);
+		devfs_add_partition("ram0", FIRMWARE_DTB_BASE, SZ_64K, DEVFS_PARTITION_FIXED, "firmware-dtb");
 	}
 
 	armlinux_set_bootparams((void *)(0x00000100));