MLK-12623-03 ARM: imx: Add cpu speed grading check for imx6ul

In the OCOTP fuse map, the speed grading[1:0] define the MAX
CPU speed the chip can run. The detailed definition is below:
2b'00: Reserved;
2b'01: 528000000Hz;
2b'10: 696000000Hz;
2b'11: Reserved;

We need to disable the illegal setpoints according to the fuse map.

Signed-off-by: Bai Ping <ping.bai@nxp.com>
(cherry picked from commit 1fc5419ba08a8da302cfcddb0ea76226d7bdc8c3)

1 files changed, 76 insertions, 2 deletions

diff --git a/arch/arm/mach-imx/mach-imx6ul.c b/arch/arm/mach-imx/mach-imx6ul.c
index 5dc1feacbeb5..76054ac798b0 100644
--- a/arch/arm/mach-imx/mach-imx6ul.c
+++ b/arch/arm/mach-imx/mach-imx6ul.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * Copyright (C) 2015-2016 Freescale Semiconductor, Inc.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -15,6 +15,7 @@
 #include <linux/of_gpio.h>
 #include <linux/of_platform.h>
 #include <linux/phy.h>
+#include <linux/pm_opp.h>
 #include <linux/regmap.h>
 #include <asm/mach/arch.h>
 #include <asm/mach/map.h>
@@ -54,6 +55,76 @@ static void __init imx6ul_enet_phy_init(void)
 	phy_register_fixup_for_uid(PHY_ID_KSZ8081, 0xffffffff,	ksz8081_phy_fixup);
 }
 
+#define OCOTP_CFG3			0x440
+#define OCOTP_CFG3_SPEED_SHIFT		16
+#define OCOTP_CFG3_SPEED_696MHZ		0x2
+
+static void __init imx6ul_opp_check_speed_grading(struct device *cpu_dev)
+{
+	struct device_node *np;
+	void __iomem *base;
+	u32 val;
+
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx6ul-ocotp");
+	if (!np) {
+		pr_warn("failed to find ocotp node\n");
+		return;
+	}
+
+	base = of_iomap(np, 0);
+	if (!base) {
+		pr_warn("failed to map ocotp\n");
+		goto put_node;
+	}
+
+	/*
+	 * Speed GRADING[1:0] defines the max speed of ARM:
+	 * 2b'00: Reserved;
+	 * 2b'01: 528000000Hz;
+	 * 2b'10: 700000000Hz;
+	 * 2b'11: Reserved;
+	 * We need to set the max speed of ARM according to fuse map.
+	 */
+	val = readl_relaxed(base + OCOTP_CFG3);
+	val >>= OCOTP_CFG3_SPEED_SHIFT;
+	val &= 0x3;
+
+	if (val != OCOTP_CFG3_SPEED_696MHZ) {
+		if (dev_pm_opp_disable(cpu_dev, 696000000))
+			pr_warn("Failed to disable 696MHz OPP\n");
+	}
+	iounmap(base);
+
+put_node:
+	of_node_put(np);
+}
+
+static void __init imx6ul_opp_init(void)
+{
+	struct device_node *np;
+	struct device *cpu_dev = get_cpu_device(0);
+
+	if (!cpu_dev) {
+		pr_warn("failed to get cpu0 device\n");
+		return;
+	}
+	np = of_node_get(cpu_dev->of_node);
+	if (!np) {
+		pr_warn("failed to find cpu0 node\n");
+		return;
+	}
+
+	if (of_init_opp_table(cpu_dev)) {
+		pr_warn("failed to init OPP table\n");
+		goto put_node;
+	}
+
+	imx6ul_opp_check_speed_grading(cpu_dev);
+
+put_node:
+	of_node_put(np);
+}
+
 static inline void imx6ul_enet_init(void)
 {
 	imx6ul_enet_clk_init();
@@ -85,7 +156,10 @@ static void __init imx6ul_init_irq(void)
 
 static void __init imx6ul_init_late(void)
 {
-	platform_device_register_simple("imx6q-cpufreq", -1, NULL, 0);
+	if (IS_ENABLED(CONFIG_ARM_IMX6Q_CPUFREQ)) {
+		imx6ul_opp_init();
+		platform_device_register_simple("imx6q-cpufreq", -1, NULL, 0);
+	}
 
 	imx6ul_cpuidle_init();
 }