diff --git a/arch/arm/boot/dts/imx6q.dtsi b/arch/arm/boot/dts/imx6q.dtsi
index 7dda599..d62b88d 100644
--- a/arch/arm/boot/dts/imx6q.dtsi
+++ b/arch/arm/boot/dts/imx6q.dtsi
@@ -508,6 +508,7 @@
 			};
 
 			ocotp@021bc000 {
+				compatible = "fsl,imx6q-ocotp";
 				reg = <0x021bc000 0x4000>;
 			};
 
diff --git a/drivers/thermal/imx6q_thermal.c b/drivers/thermal/imx6q_thermal.c
index 161a1a9..c7174b5 100644
--- a/drivers/thermal/imx6q_thermal.c
+++ b/drivers/thermal/imx6q_thermal.c
@@ -27,70 +27,60 @@
 #include <linux/of_address.h>
 #include <linux/smp.h>
 #include <linux/cpu_cooling.h>
+#include <linux/platform_device.h>
 
 /* register define of anatop */
-#define HW_ANADIG_ANA_MISC0	(0x00000150)
-#define HW_ANADIG_ANA_MISC0_SET	(0x00000154)
-#define HW_ANADIG_ANA_MISC0_CLR	(0x00000158)
-#define HW_ANADIG_ANA_MISC0_TOG	(0x0000015c)
-
-#define HW_ANADIG_TEMPSENSE0	(0x00000180)
-#define HW_ANADIG_TEMPSENSE0_SET	(0x00000184)
-#define HW_ANADIG_TEMPSENSE0_CLR	(0x00000188)
-#define HW_ANADIG_TEMPSENSE0_TOG	(0x0000018c)
-
-#define HW_ANADIG_TEMPSENSE1	(0x00000190)
-#define HW_ANADIG_TEMPSENSE1_SET	(0x00000194)
-#define HW_ANADIG_TEMPSENSE1_CLR	(0x00000198)
-
-#define BM_ANADIG_ANA_MISC0_REFTOP_SELBIASOFF 0x00000008
-
-#define BP_ANADIG_TEMPSENSE0_TEMP_VALUE      8
-#define BM_ANADIG_TEMPSENSE0_TEMP_VALUE 0x000FFF00
-#define BF_ANADIG_TEMPSENSE0_TEMP_VALUE(v)  \
-	(((v) << 8) & BM_ANADIG_TEMPSENSE0_TEMP_VALUE)
-#define BM_ANADIG_TEMPSENSE0_FINISHED 0x00000004
-#define BM_ANADIG_TEMPSENSE0_MEASURE_TEMP 0x00000002
-#define BM_ANADIG_TEMPSENSE0_POWER_DOWN 0x00000001
-
-#define BP_ANADIG_TEMPSENSE1_MEASURE_FREQ      0
-#define BM_ANADIG_TEMPSENSE1_MEASURE_FREQ 0x0000FFFF
-#define BF_ANADIG_TEMPSENSE1_MEASURE_FREQ(v)  \
-	(((v) << 0) & BM_ANADIG_TEMPSENSE1_MEASURE_FREQ)
-
-#define CONVER_CONST			14113  /* need to add calibration */
-#define CONVER_DIV				17259
-#define REG_VALUE_TO_CEL(val) (((CONVER_CONST - val * 10)\
-			* 1000) / CONVER_DIV);
-
-#define IMX6Q_THERMAL_POLLING_FREQUENCY_MS 1000
+#define HW_ANADIG_ANA_MISC0			0x00000150
+#define HW_ANADIG_ANA_MISC0_SET			0x00000154
+#define HW_ANADIG_ANA_MISC0_CLR			0x00000158
+#define HW_ANADIG_ANA_MISC0_TOG			0x0000015c
+#define BM_ANADIG_ANA_MISC0_REFTOP_SELBIASOFF	0x00000008
+
+#define HW_ANADIG_TEMPSENSE0			0x00000180
+#define HW_ANADIG_TEMPSENSE0_SET		0x00000184
+#define HW_ANADIG_TEMPSENSE0_CLR		0x00000188
+#define HW_ANADIG_TEMPSENSE0_TOG		0x0000018c
+
+#define BP_ANADIG_TEMPSENSE0_TEMP_VALUE		8
+#define BM_ANADIG_TEMPSENSE0_TEMP_VALUE		0x000FFF00
+#define BM_ANADIG_TEMPSENSE0_FINISHED		0x00000004
+#define BM_ANADIG_TEMPSENSE0_MEASURE_TEMP	0x00000002
+#define BM_ANADIG_TEMPSENSE0_POWER_DOWN		0x00000001
+
+#define HW_ANADIG_TEMPSENSE1			0x00000190
+#define HW_ANADIG_TEMPSENSE1_SET		0x00000194
+#define HW_ANADIG_TEMPSENSE1_CLR		0x00000198
+#define BP_ANADIG_TEMPSENSE1_MEASURE_FREQ	0
+#define BM_ANADIG_TEMPSENSE1_MEASURE_FREQ	0x0000FFFF
+
+#define HW_OCOTP_ANA1				0x000004E0
+
+#define IMX6Q_THERMAL_POLLING_FREQUENCY_MS 10000
 #define IMX6Q_THERMAL_ACT_TRP_PTS 3
 /* assumption: always one critical trip point */
 #define IMX6Q_THERMAL_TOTAL_TRP_PTS (IMX6Q_THERMAL_ACT_TRP_PTS + 1)
-#define IMX6Q_THERMAL_DEBUG 1
 
-struct trip_point {
-	u8 temp; /* in celcius */
-	u8 type;
+struct th_sys_trip_point {
+	u32 temp; /* in celcius */
+	enum thermal_trip_type type;
 };
 
 struct imx6q_thermal_data {
-	struct trip_point trp_pts[IMX6Q_THERMAL_TOTAL_TRP_PTS];
+	struct th_sys_trip_point trp_pts[IMX6Q_THERMAL_TOTAL_TRP_PTS];
 	struct freq_pctg_table freq_tab[IMX6Q_THERMAL_ACT_TRP_PTS];
 };
 
-struct thermal_sensor_conf {
+struct imx6q_sensor_data {
+	int	c1, c2;
 	char	*name;
-	int	(*read_temperature)(void *data);
+	bool	was_suspended;
 };
 
-static int imx6q_get_temp(struct thermal_zone_device *thermal,
-				  unsigned long *temp);
-
-static struct thermal_sensor_conf imx6q_sensor_conf = {
-	.name			= "imx6q-temp_sens",
-	.read_temperature	= (int (*)(void *))imx6q_get_temp,
-
+struct imx6q_thermal_zone {
+	struct thermal_zone_device *therm_dev;
+	struct thermal_cooling_device *cool_dev;
+	struct imx6q_sensor_data sensor_data;
+	struct imx6q_thermal_data *thermal_data;
 };
 
 /*
@@ -99,51 +89,137 @@ static struct thermal_sensor_conf imx6q_sensor_conf = {
  */
 static struct imx6q_thermal_data thermal_data = {
 	.trp_pts[0] = {
-		.temp	 = 85,
+		.temp	 = 85000,
 		.type	 = THERMAL_TRIP_STATE_ACTIVE,
 	},
 	.freq_tab[0] = {
 		.freq_clip_pctg[0] = 25,
 	},
 	.trp_pts[1] = {
-		.temp	 = 90,
+		.temp	 = 90000,
 		.type	 = THERMAL_TRIP_STATE_ACTIVE,
 	},
 	.freq_tab[1] = {
 		.freq_clip_pctg[0] = 65,
 	},
 	.trp_pts[2] = {
-		.temp	 = 95,
+		.temp	 = 95000,
 		.type	 = THERMAL_TRIP_STATE_ACTIVE,
 	},
 	.freq_tab[2] = {
 		.freq_clip_pctg[0] = 99,
 	},
 	.trp_pts[3] = {
-		.temp	 = 100,
+		.temp	 = 100000,
 		.type	 = THERMAL_TRIP_CRITICAL,
 	},
 };
 
-struct imx6q_thermal_zone {
-	struct thermal_zone_device *therm_dev;
-	struct thermal_cooling_device *cool_dev;
-	struct thermal_sensor_conf *sensor_conf;
-	struct imx6q_thermal_data *thermal_data;
-};
+static int th_sys_get_temp(struct thermal_zone_device *thermal,
+				  unsigned long *temp);
+
+static struct platform_device		*dev;
+static struct imx6q_thermal_zone	*th_zone;
+static void __iomem			*anatop_base;
+
+static inline int imx6q_get_temp(int *temp)
+{
+	unsigned int n_meas;
+	unsigned int reg;
+	struct imx6q_sensor_data *p = &th_zone->sensor_data;
+
+	/*
+	 * For now we only using single measure.  Every time we measure
+	 * the temperature, we will power on/down the anadig module
+	 */
+	writel_relaxed(BM_ANADIG_TEMPSENSE0_POWER_DOWN,
+		anatop_base + HW_ANADIG_TEMPSENSE0_CLR);
+
+	writel_relaxed(BM_ANADIG_TEMPSENSE0_FINISHED,
+		anatop_base + HW_ANADIG_TEMPSENSE0_CLR);
+
+	p->was_suspended = false;
+
+	writel_relaxed(BM_ANADIG_TEMPSENSE0_MEASURE_TEMP,
+		anatop_base + HW_ANADIG_TEMPSENSE0_SET);
 
-static struct imx6q_thermal_zone *th_zone;
+	/*
+	 * According to SoC designers, it may require up to ~17us to complete
+	 * a measurement.  But we have a 'finished' status bit, so we
+	 * check it just in case the designers are liars.
+	 */
+	do {
+		msleep(1);
 
-static void __iomem *anatop_base;
+		/*
+		 * if system was possibly suspended while measurement
+		 * was being taken, we take another measurement to make
+		 * sure the measurement is valid.
+		 */
+		if (p->was_suspended) {
+			writel_relaxed(BM_ANADIG_TEMPSENSE0_MEASURE_TEMP,
+				anatop_base + HW_ANADIG_TEMPSENSE0_CLR);
 
-static int imx6q_get_mode(struct thermal_zone_device *thermal,
+			writel_relaxed(BM_ANADIG_TEMPSENSE0_FINISHED,
+				anatop_base + HW_ANADIG_TEMPSENSE0_CLR);
+
+			p->was_suspended = false;
+
+			writel_relaxed(BM_ANADIG_TEMPSENSE0_MEASURE_TEMP,
+				anatop_base + HW_ANADIG_TEMPSENSE0_SET);
+
+			continue;
+		}
+	} while (!(readl_relaxed(anatop_base + HW_ANADIG_TEMPSENSE0)
+		& BM_ANADIG_TEMPSENSE0_FINISHED));
+
+	reg = readl_relaxed(anatop_base + HW_ANADIG_TEMPSENSE0);
+
+	n_meas = (reg & BM_ANADIG_TEMPSENSE0_TEMP_VALUE)
+			>> BP_ANADIG_TEMPSENSE0_TEMP_VALUE;
+
+	writel_relaxed(BM_ANADIG_TEMPSENSE0_MEASURE_TEMP,
+		anatop_base + HW_ANADIG_TEMPSENSE0_CLR);
+
+	writel_relaxed(BM_ANADIG_TEMPSENSE0_POWER_DOWN,
+			anatop_base + HW_ANADIG_TEMPSENSE0_SET);
+
+	/* See imx6q_thermal_process_fuse_data for forumla derivation. */
+	*temp = p->c2 + (p->c1 * n_meas);
+
+	pr_debug("Temperature: %d\n", *temp / 1000);
+
+	return 0;
+}
+
+static int th_sys_get_temp(struct thermal_zone_device *thermal,
+				  unsigned long *temp)
+{
+	int tmp = 0;
+
+	imx6q_get_temp(&tmp);
+
+	/*
+	 * The thermal framework code stores temperature in unsigned long. Also,
+	 * it has references to "millicelcius" which limits the lowest
+	 * temperature possible (compared to Kelvin).
+	 */
+	if (likely(tmp > 0))
+		*temp = tmp;
+	else
+		*temp = 0;
+
+	return 0;
+}
+
+static int th_sys_get_mode(struct thermal_zone_device *thermal,
 			    enum thermal_device_mode *mode)
 {
 	*mode = THERMAL_DEVICE_ENABLED;
 	return 0;
 }
 
-static int imx6q_get_trip_type(struct thermal_zone_device *thermal, int trip,
+static int th_sys_get_trip_type(struct thermal_zone_device *thermal, int trip,
 				 enum thermal_trip_type *type)
 {
 	if (trip >= IMX6Q_THERMAL_TOTAL_TRP_PTS)
@@ -154,134 +230,144 @@ static int imx6q_get_trip_type(struct thermal_zone_device *thermal, int trip,
 	return 0;
 }
 
-static int imx6q_get_trip_temp(struct thermal_zone_device *thermal, int trip,
+static int th_sys_get_trip_temp(struct thermal_zone_device *thermal, int trip,
 				 unsigned long *temp)
 {
 	if (trip >= IMX6Q_THERMAL_TOTAL_TRP_PTS)
 		return -EINVAL;
 
 	*temp = th_zone->thermal_data->trp_pts[trip].temp;
-
-	/*convert the temperature into millicelsius*/
-	*temp = *temp * 1000;
 	return 0;
 }
 
-static int imx6q_get_crit_temp(struct thermal_zone_device *thermal,
+static int th_sys_get_crit_temp(struct thermal_zone_device *thermal,
 				 unsigned long *temp)
 {
 
 	*temp = th_zone->thermal_data->trp_pts[
 		IMX6Q_THERMAL_TOTAL_TRP_PTS - 1].temp;
-	/*convert the temperature into millicelsius*/
-	*temp = *temp * 1000;
 	return 0;
 }
 
-static int imx6q_bind(struct thermal_zone_device *thermal,
+static int th_sys_bind(struct thermal_zone_device *thermal,
 			struct thermal_cooling_device *cdev)
 {
+	int i;
+
 	/* if the cooling device is the one from imx6 bind it */
 	if (cdev != th_zone->cool_dev)
 		return 0;
 
-	if (thermal_zone_bind_cooling_device(thermal, 0, cdev)) {
-		pr_err("error binding cooling dev\n");
-		return -EINVAL;
-	}
-	if (thermal_zone_bind_cooling_device(thermal, 1, cdev)) {
-		pr_err("error binding cooling dev\n");
-		return -EINVAL;
+	for (i = 0; i < IMX6Q_THERMAL_ACT_TRP_PTS; i++) {
+		if (thermal_zone_bind_cooling_device(thermal, i, cdev)) {
+			pr_err("error binding cooling dev\n");
+			return -EINVAL;
+		}
 	}
 
 	return 0;
 }
 
-static int imx6q_unbind(struct thermal_zone_device *thermal,
+static int th_sys_unbind(struct thermal_zone_device *thermal,
 			  struct thermal_cooling_device *cdev)
 {
+	int i;
+
 	if (cdev != th_zone->cool_dev)
 		return 0;
-	if (thermal_zone_unbind_cooling_device(thermal, 0, cdev)) {
-		pr_err("error unbinding cooling dev\n");
-		return -EINVAL;
-	}
-	return 0;
-}
 
-static int imx6q_temp_sens_reg_dump(void)
-{
-	if (!anatop_base) {
-		pr_info("anatop_base is not initialized!!!\n");
-		return -EINVAL;
+	for (i = 0; i < IMX6Q_THERMAL_ACT_TRP_PTS; i++) {
+		if (thermal_zone_unbind_cooling_device(thermal, i, cdev)) {
+			pr_err("error unbinding cooling dev\n");
+			return -EINVAL;
+		}
 	}
-	pr_info("HW_ANADIG_TEMPSENSE0 = 0x%x\n",
-			readl_relaxed(anatop_base + HW_ANADIG_TEMPSENSE0));
-	pr_info("HW_ANADIG_TEMPSENSE1 = 0x%x\n",
-			readl_relaxed(anatop_base + HW_ANADIG_TEMPSENSE1));
+
 	return 0;
 }
 
-static int imx6q_get_temp(struct thermal_zone_device *thermal,
-				  unsigned long *temp)
+static struct thermal_zone_device_ops imx6q_dev_ops = {
+	.bind = th_sys_bind,
+	.unbind = th_sys_unbind,
+	.get_temp = th_sys_get_temp,
+	.get_mode = th_sys_get_mode,
+	.get_trip_type = th_sys_get_trip_type,
+	.get_trip_temp = th_sys_get_trip_temp,
+	.get_crit_temp = th_sys_get_crit_temp,
+};
+
+static int imx6q_thermal_process_fuse_data(unsigned int fuse_data)
 {
-	unsigned int tmp;
-	unsigned int reg;
+	int t1, t2, n1, n2;
+	struct imx6q_sensor_data *p = &th_zone->sensor_data;
+
+	if (fuse_data == 0 || fuse_data == 0xffffffff)
+		return -EINVAL;
 
 	/*
-	 * For now we only using single measure.  Every time we measure
-	 * the temperature, we will power on/down the anadig module
+	 * Fuse data layout:
+	 * [31:20] sensor value @ 25C
+	 * [19:8] sensor value of hot
+	 * [7:0] hot temperature value
 	 */
-	writel_relaxed(BM_ANADIG_TEMPSENSE0_POWER_DOWN,
-		anatop_base + HW_ANADIG_TEMPSENSE0_CLR);
+	n1 = fuse_data >> 20;
+	n2 = (fuse_data & 0xfff00) >> 8;
+	t2 = fuse_data & 0xff;
+	t1 = 25; /* t1 always 25C */
 
-	writel_relaxed(BM_ANADIG_TEMPSENSE0_FINISHED,
-		anatop_base + HW_ANADIG_TEMPSENSE0_CLR);
+	pr_debug(" -- temperature sensor calibration data --\n");
+	pr_debug("HW_OCOTP_ANA1: %x\n", fuse_data);
+	pr_debug("n1: %d\nn2: %d\nt1: %d\nt2: %d\n", n1, n2, t1, t2);
 
-	writel_relaxed(BM_ANADIG_TEMPSENSE0_MEASURE_TEMP,
-		anatop_base + HW_ANADIG_TEMPSENSE0_SET);
 	/*
-	 * According to designers, may take up to ~17us for hardware to make
-	 * a measurement.  But because we have a 'finished' status bit, so we
-	 * check it just in case the designers are liars.
+	 * From reference manual (derived from linear interpolation),
+	 * Tmeas = T2 + (Nmeas - N2) * (T1 - T2) / (N1 - N2)
+	 * We want to reduce this down to the minimum computation necessary
+	 * for each temperature read.  Also, we want Tmeas in millicelcius
+	 * and we don't want to lose precision from integer division. So...
+	 * milli_Tmeas = 1000 * T2 + 1000 * (Nmeas - N2) * (T1 - T2) / (N1 - N2)
+	 * Let constant c1 = 1000 * (T1 - T2) / (N1 - N2)
+	 * milli_Tmeas = (1000 * T2) + c1 * (Nmeas - N2)
+	 * milli_Tmeas = (1000 * T2) + (c1 * Nmeas) - (c1 * N2)
+	 * Let constant c2 = (1000 * T2) - (c1 * N2)
+	 * milli_Tmeas = c2 + (c1 * Nmeas)
 	 */
-	do  {
-		msleep(1);
-	} while (!(readl_relaxed(anatop_base + HW_ANADIG_TEMPSENSE0)
-		& BM_ANADIG_TEMPSENSE0_FINISHED));
+	p->c1 = (1000 * (t1 - t2)) / (n1 - n2);
+	p->c2 = (1000 * t2) - (p->c1 * n2);
 
-	reg = readl_relaxed(anatop_base + HW_ANADIG_TEMPSENSE0);
+	pr_debug("c1: %i\n", p->c1);
+	pr_debug("c2: %i\n", p->c2);
 
-	tmp = (reg & BM_ANADIG_TEMPSENSE0_TEMP_VALUE)
-			>> BP_ANADIG_TEMPSENSE0_TEMP_VALUE;
+	return 0;
+}
 
-#if IMX6Q_THERMAL_DEBUG
-	imx6q_temp_sens_reg_dump();
-#endif
-	writel_relaxed(BM_ANADIG_TEMPSENSE0_MEASURE_TEMP,
-		anatop_base + HW_ANADIG_TEMPSENSE0_CLR);
 
-	writel_relaxed(BM_ANADIG_TEMPSENSE0_POWER_DOWN,
-			anatop_base + HW_ANADIG_TEMPSENSE0_SET);
+static int imx6q_thermal_probe(struct platform_device *pdev)
+{
+	return 0;
+}
 
-	*temp = REG_VALUE_TO_CEL(tmp);
+static int imx6q_thermal_remove(struct platform_device *pdev)
+{
+	return 0;
+}
 
-#if IMX6Q_THERMAL_DEBUG
-	pr_info("Temperature is %lu C\n", *temp);
-#endif
+static int imx6q_thermal_suspend(struct platform_device *pdev,
+						pm_message_t state)
+{
+	/*
+	 * according to imx6q thermal sensor designers, system problems will
+	 * not occur if low power modes are entered while temp_sensor is active,
+	 * so do nothing here.
+	 */
 	return 0;
 }
 
-/* bind callback functions to thermalzone */
-static struct thermal_zone_device_ops imx6q_dev_ops = {
-	.bind = imx6q_bind,
-	.unbind = imx6q_unbind,
-	.get_temp = imx6q_get_temp,
-	.get_mode = imx6q_get_mode,
-	.get_trip_type = imx6q_get_trip_type,
-	.get_trip_temp = imx6q_get_trip_temp,
-	.get_crit_temp = imx6q_get_crit_temp,
-};
+static int imx6q_thermal_resume(struct platform_device *pdev)
+{
+	th_zone->sensor_data.was_suspended = true;
+	return 0;
+}
 
 void imx6q_unregister_thermal(void)
 {
@@ -295,19 +381,48 @@ void imx6q_unregister_thermal(void)
 
 	pr_info("i.MX6Q: Kernel Thermal management unregistered\n");
 }
-EXPORT_SYMBOL(imx6q_unregister_thermal);
 
-int __init imx6q_register_thermal(void)
+static int __init imx6q_thermal_register(void)
 {
+	void __iomem *ocotp_base;
 	struct device_node *np;
+	unsigned int fuse_data;
 	int ret;
 
+	np = of_find_compatible_node(NULL, NULL, "fsl,imx6q-ocotp");
+	ocotp_base = of_iomap(np, 0);
+
+	if (!ocotp_base) {
+		pr_err("Could not retrieve ocotp-base\n");
+		ret = -EINVAL;
+		goto err_unregister;
+	}
+
 	np = of_find_compatible_node(NULL, NULL, "fsl,imx6q-anatop");
 	anatop_base = of_iomap(np, 0);
 
 	if (!anatop_base) {
 		pr_err("Could not retrieve anantop-base\n");
-		return -EINVAL;
+		ret = -EINVAL;
+		goto err_unregister;
+	}
+
+	fuse_data = readl_relaxed(ocotp_base + HW_OCOTP_ANA1);
+
+	th_zone = kzalloc(sizeof(struct imx6q_thermal_zone), GFP_KERNEL);
+
+	if (!th_zone) {
+		ret = -ENOMEM;
+		goto err_unregister;
+	}
+
+	th_zone->sensor_data.name = "imx6q-temp_sens",
+
+	ret = imx6q_thermal_process_fuse_data(fuse_data);
+
+	if (ret) {
+		pr_err("Invalid temperature calibration data.\n");
+		goto err_unregister;
 	}
 
 	/* Make sure sensor is in known good state for measurements */
@@ -322,14 +437,6 @@ int __init imx6q_register_thermal(void)
 	writel_relaxed(BM_ANADIG_TEMPSENSE0_POWER_DOWN,
 			anatop_base + HW_ANADIG_TEMPSENSE0_SET);
 
-	th_zone = kzalloc(sizeof(struct imx6q_thermal_zone), GFP_KERNEL);
-	if (!th_zone) {
-		ret = -ENOMEM;
-		goto err_unregister;
-	}
-
-	th_zone->sensor_conf = &imx6q_sensor_conf;
-
 	th_zone->thermal_data = &thermal_data;
 	if (!th_zone->thermal_data) {
 		pr_err("Temperature sensor data not initialised\n");
@@ -348,8 +455,9 @@ int __init imx6q_register_thermal(void)
 	}
 
 	th_zone->therm_dev = thermal_zone_device_register(
-		th_zone->sensor_conf->name, 3, NULL, &imx6q_dev_ops,
-		0, 0, 0, IMX6Q_THERMAL_POLLING_FREQUENCY_MS);
+		th_zone->sensor_data.name, IMX6Q_THERMAL_TOTAL_TRP_PTS,
+		NULL, &imx6q_dev_ops, 0, 0, 0,
+		IMX6Q_THERMAL_POLLING_FREQUENCY_MS);
 
 	if (IS_ERR(th_zone->therm_dev)) {
 		pr_err("Failed to register thermal zone device\n");
@@ -357,14 +465,83 @@ int __init imx6q_register_thermal(void)
 		goto err_unregister;
 	}
 
-	pr_info("i.MX6: Kernel Thermal management registered\n");
-
 	return 0;
 
 err_unregister:
 	imx6q_unregister_thermal();
 	return ret;
 }
-EXPORT_SYMBOL(imx6q_register_thermal);
 
-module_init(imx6q_register_thermal);
+static struct platform_driver imx6q_thermal_driver = {
+	.probe		= imx6q_thermal_probe,
+	.remove		= imx6q_thermal_remove,
+	.suspend	= imx6q_thermal_suspend,
+	.resume		= imx6q_thermal_resume,
+	.driver		= {
+			.name	= "imx6q-thermal",
+			.owner	= THIS_MODULE,
+			},
+};
+
+static int imx6q_th_platform_register(void)
+{
+	int ret;
+
+	ret = platform_driver_register(&imx6q_thermal_driver);
+	if (ret)
+		return ret;
+
+	dev = platform_device_alloc("imx6q-thermal-cpu", -1);
+
+	if (!dev) {
+		ret = -ENOMEM;
+		goto err_device_alloc;
+	}
+	ret = platform_device_add(dev);
+	if (ret)
+		goto err_device_add;
+
+	return ret;
+
+err_device_add:
+	platform_device_put(dev);
+err_device_alloc:
+	platform_driver_unregister(&imx6q_thermal_driver);
+
+	return ret;
+}
+
+static int __init imx6q_thermal_init(void)
+{
+	int ret;
+
+	ret = imx6q_th_platform_register();
+	if (ret)
+		goto err;
+
+	ret = imx6q_thermal_register();
+	if (ret)
+		goto err;
+
+	pr_info("i.MX Thermal management enabled.\n");
+	return ret;
+
+err:
+	pr_info("WARNING: Thermal management NOT enabled due to errors.\n");
+	return ret;
+}
+
+static void __exit imx6q_thermal_driver_exit(void)
+{
+	imx6q_unregister_thermal();
+	platform_device_unregister(dev);
+	platform_driver_unregister(&imx6q_thermal_driver);
+}
+
+module_init(imx6q_thermal_init);
+module_exit(imx6q_thermal_driver_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor");
+MODULE_DESCRIPTION("i.MX6Q SoC thermal driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:imx6q-thermal");
