3 files changed, 34 insertions, 14 deletions

diff --git a/drivers/thermal/samsung/exynos_tmu.c b/drivers/thermal/samsung/exynos_tmu.c
index 1b622ce0ee79..01aa5481c13e 100644
--- a/drivers/thermal/samsung/exynos_tmu.c
+++ b/drivers/thermal/samsung/exynos_tmu.c
@@ -55,6 +55,7 @@
  * @tmu_initialize: SoC specific TMU initialization method
  * @tmu_control: SoC specific TMU control method
  * @tmu_read: SoC specific TMU temperature read method
+ * @tmu_set_emulation: SoC specific TMU emulation setting method
  */
 struct exynos_tmu_data {
         int id;
@@ -72,6 +73,8 @@ struct exynos_tmu_data {
         int (*tmu_initialize)(struct platform_device *pdev);
         void (*tmu_control)(struct platform_device *pdev, bool on);
         int (*tmu_read)(struct exynos_tmu_data *data);
+        void (*tmu_set_emulation)(struct exynos_tmu_data *data,
+                                  unsigned long temp);
 };
 
 /*
@@ -460,12 +463,36 @@ static u32 get_emul_con_reg(struct exynos_tmu_data *data, unsigned int val,
         return val;
 }
 
+static void exynos4412_tmu_set_emulation(struct exynos_tmu_data *data,
+                                         unsigned long temp)
+{
+        unsigned int val;
+        u32 emul_con;
+
+        if (data->soc == SOC_ARCH_EXYNOS5260)
+                emul_con = EXYNOS5260_EMUL_CON;
+        else
+                emul_con = EXYNOS_EMUL_CON;
+
+        val = readl(data->base + emul_con);
+        val = get_emul_con_reg(data, val, temp);
+        writel(val, data->base + emul_con);
+}
+
+static void exynos5440_tmu_set_emulation(struct exynos_tmu_data *data,
+                                         unsigned long temp)
+{
+        unsigned int val;
+
+        val = readl(data->base + EXYNOS5440_TMU_S0_7_DEBUG);
+        val = get_emul_con_reg(data, val, temp);
+        writel(val, data->base + EXYNOS5440_TMU_S0_7_DEBUG);
+}
+
 static int exynos_tmu_set_emulation(void *drv_data, unsigned long temp)
 {
         struct exynos_tmu_data *data = drv_data;
         struct exynos_tmu_platform_data *pdata = data->pdata;
-        const struct exynos_tmu_registers *reg = pdata->registers;
-        unsigned int val;
         int ret = -EINVAL;
 
         if (!TMU_SUPPORTS(pdata, EMULATION))
@@ -476,11 +503,7 @@ static int exynos_tmu_set_emulation(void *drv_data, unsigned long temp)
 
         mutex_lock(&data->lock);
         clk_enable(data->clk);
-
-        val = readl(data->base + reg->emul_con);
-        val = get_emul_con_reg(data, val, temp);
-        writel(val, data->base + reg->emul_con);
-
+        data->tmu_set_emulation(data, temp);
         clk_disable(data->clk);
         mutex_unlock(&data->lock);
         return 0;
@@ -488,6 +511,8 @@ out:
         return ret;
 }
 #else
+#define exynos4412_tmu_set_emulation NULL
+#define exynos5440_tmu_set_emulation NULL
 static int exynos_tmu_set_emulation(void *drv_data,     unsigned long temp)
         { return -EINVAL; }
 #endif/*CONFIG_THERMAL_EMULATION*/
@@ -745,11 +770,13 @@ static int exynos_tmu_probe(struct platform_device *pdev)
                 data->tmu_initialize = exynos4412_tmu_initialize;
                 data->tmu_control = exynos4210_tmu_control;
                 data->tmu_read = exynos4412_tmu_read;
+                data->tmu_set_emulation = exynos4412_tmu_set_emulation;
                 break;
         case SOC_ARCH_EXYNOS5440:
                 data->tmu_initialize = exynos5440_tmu_initialize;
                 data->tmu_control = exynos5440_tmu_control;
                 data->tmu_read = exynos5440_tmu_read;
+                data->tmu_set_emulation = exynos5440_tmu_set_emulation;
                 break;
         default:
                 ret = -EINVAL;
diff --git a/drivers/thermal/samsung/exynos_tmu.h b/drivers/thermal/samsung/exynos_tmu.h
index 9460e6e55fe8..785eccf22c19 100644
--- a/drivers/thermal/samsung/exynos_tmu.h
+++ b/drivers/thermal/samsung/exynos_tmu.h
@@ -72,12 +72,10 @@ enum soc_type {
  * The register validity may vary slightly across different exynos SOC's.
  * @tmu_intstat: Register containing the interrupt status values.
  * @tmu_intclear: Register for clearing the raised interrupt status.
- * @emul_con: TMU emulation controller register.
  */
 struct exynos_tmu_registers {
         u32     tmu_intstat;
         u32     tmu_intclear;
-        u32     emul_con;
 };
 
 /**
diff --git a/drivers/thermal/samsung/exynos_tmu_data.c b/drivers/thermal/samsung/exynos_tmu_data.c
index 769b89d7d641..d0bb4b35d0b9 100644
--- a/drivers/thermal/samsung/exynos_tmu_data.c
+++ b/drivers/thermal/samsung/exynos_tmu_data.c
@@ -75,7 +75,6 @@ struct exynos_tmu_init_data const exynos4210_default_tmu_data = {
 static const struct exynos_tmu_registers exynos3250_tmu_registers = {
         .tmu_intstat = EXYNOS_TMU_REG_INTSTAT,
         .tmu_intclear = EXYNOS_TMU_REG_INTCLEAR,
-        .emul_con = EXYNOS_EMUL_CON,
 };
 
 #define EXYNOS3250_TMU_DATA \
@@ -135,7 +134,6 @@ struct exynos_tmu_init_data const exynos3250_default_tmu_data = {
 static const struct exynos_tmu_registers exynos4412_tmu_registers = {
         .tmu_intstat = EXYNOS_TMU_REG_INTSTAT,
         .tmu_intclear = EXYNOS_TMU_REG_INTCLEAR,
-        .emul_con = EXYNOS_EMUL_CON,
 };
 
 #define EXYNOS4412_TMU_DATA \
@@ -207,7 +205,6 @@ struct exynos_tmu_init_data const exynos5250_default_tmu_data = {
 static const struct exynos_tmu_registers exynos5260_tmu_registers = {
         .tmu_intstat = EXYNOS5260_TMU_REG_INTSTAT,
         .tmu_intclear = EXYNOS5260_TMU_REG_INTCLEAR,
-        .emul_con = EXYNOS5260_EMUL_CON,
 };
 
 #define __EXYNOS5260_TMU_DATA   \
@@ -269,7 +266,6 @@ struct exynos_tmu_init_data const exynos5260_default_tmu_data = {
 static const struct exynos_tmu_registers exynos5420_tmu_registers = {
         .tmu_intstat = EXYNOS_TMU_REG_INTSTAT,
         .tmu_intclear = EXYNOS_TMU_REG_INTCLEAR,
-        .emul_con = EXYNOS_EMUL_CON,
 };
 
 #define __EXYNOS5420_TMU_DATA   \
@@ -337,7 +333,6 @@ struct exynos_tmu_init_data const exynos5420_default_tmu_data = {
 static const struct exynos_tmu_registers exynos5440_tmu_registers = {
         .tmu_intstat = EXYNOS5440_TMU_S0_7_IRQ,
         .tmu_intclear = EXYNOS5440_TMU_S0_7_IRQ,
-        .emul_con = EXYNOS5440_TMU_S0_7_DEBUG,
 };
 
 #define EXYNOS5440_TMU_DATA \