This is a simplification of the initialization sequence of the omap temp sensor. Now all code related to cpu cooling is concentrated in only one place. This patch also properly handle the fail path, by freeing the right resources, in case the initialization fails.
Signed-off-by: Eduardo Valentin eduardo.valentin@ti.com --- drivers/mfd/omap4460plus_temp_sensor.c | 125 ++++++++++++++++++-------------- 1 files changed, 69 insertions(+), 56 deletions(-)
diff --git a/drivers/mfd/omap4460plus_temp_sensor.c b/drivers/mfd/omap4460plus_temp_sensor.c index 18140b7..ef2b091 100644 --- a/drivers/mfd/omap4460plus_temp_sensor.c +++ b/drivers/mfd/omap4460plus_temp_sensor.c @@ -904,13 +904,57 @@ static int omap_read_temp(void *private_data) return temp; }
+static int omap4460plus_cpu_cooling_init(struct scm *scm_ptr) +{ + scm_ptr->cpu_therm = kzalloc(sizeof(struct thermal_sensor_conf) * + scm_ptr->cnt, GFP_KERNEL); + if (!scm_ptr->cpu_therm) { + pr_err("Unable to allocate memory for ts data\n"); + + return -ENOMEM; + } + + if (scm_ptr->rev == 1) { + strncpy(scm_ptr->cpu_therm[0].name, "omap_ondie_sensor", + SENSOR_NAME_LEN); + scm_ptr->cpu_therm[0].private_data = scm_ptr; + scm_ptr->cpu_therm[0].read_temperature = omap_read_temp; + if (omap4_has_mpu_1_5ghz()) + scm_ptr->cpu_therm[0].sensor_data = + &omap4_1500mhz_bandgap_data; + else + scm_ptr->cpu_therm[0].sensor_data = + &omap4_1200mhz_bandgap_data; + omap_zones[0] = omap4_register_thermal(scm_ptr->cpu_therm); + } else if (scm_ptr->rev == 2) { + strncpy(scm_ptr->cpu_therm[0].name, "omap_ondie_sensor", + SENSOR_NAME_LEN); + scm_ptr->cpu_therm[0].private_data = scm_ptr; + scm_ptr->cpu_therm[0].read_temperature = omap_read_temp; + scm_ptr->cpu_therm[0].sensor_data = &omap5_1500mhz_bandgap_data; + omap_zones[0] = omap4_register_thermal(&scm_ptr->cpu_therm[0]); + strncpy(scm_ptr->cpu_therm[1].name, "omap_ondie_gpu_sensor", + SENSOR_NAME_LEN); + scm_ptr->cpu_therm[1].private_data = scm_ptr; + scm_ptr->cpu_therm[1].read_temperature = omap_read_temp; + scm_ptr->cpu_therm[1].sensor_data = &omap5_1500mhz_bandgap_data; + strncpy(scm_ptr->cpu_therm[2].name, "omap_ondie_core_sensor", + SENSOR_NAME_LEN); + scm_ptr->cpu_therm[2].private_data = scm_ptr; + scm_ptr->cpu_therm[2].read_temperature = omap_read_temp; + scm_ptr->cpu_therm[2].sensor_data = &omap5_1500mhz_bandgap_data; + } + + return 0; +} +#else +static int omap4460plus_cpu_cooling_init(struct scm *scm_ptr) { return 0; } #endif
int omap4460plus_temp_sensor_init(struct scm *scm_ptr) { struct omap_temp_sensor_registers *reg_ptr; struct omap4460plus_temp_sensor_data *ts_ptr; - struct scm_regval *regval_ptr; int clk_rate, ret, i;
scm_ptr->registers = kzalloc(sizeof(reg_ptr) * @@ -924,29 +968,25 @@ int omap4460plus_temp_sensor_init(struct scm *scm_ptr) if (!scm_ptr->ts_data) { pr_err("Unable to allocate memory for ts data\n"); ret = -ENOMEM; - goto fail_alloc; + goto free_registers; }
- scm_ptr->regval = kzalloc(sizeof(regval_ptr) * scm_ptr->cnt, + scm_ptr->regval = kzalloc(scm_ptr->cnt * sizeof(struct scm_regval *), GFP_KERNEL); if (!scm_ptr->regval) { pr_err("Unable to allocate memory for regval\n"); ret = -ENOMEM; - goto fail_alloc; + goto free_ts_data; }
-#if defined(CONFIG_CPU_THERMAL) - scm_ptr->cpu_therm = kzalloc(sizeof(struct thermal_sensor_conf) - * scm_ptr->cnt, GFP_KERNEL); - if (!scm_ptr->cpu_therm) { - pr_err("Unable to allocate memory for ts data\n"); - goto clk_err; - } -#endif - for (i = 0; i < scm_ptr->cnt; i++) { scm_ptr->regval[i] = kzalloc(sizeof(struct scm_regval), GFP_KERNEL); + if (!scm_ptr->regval[i]) { + pr_err("Unable to allocate memory for regval\n"); + ret = -ENOMEM; + goto free_regval; + } }
if (scm_ptr->rev == 1) { @@ -961,19 +1001,6 @@ int omap4460plus_temp_sensor_init(struct scm *scm_ptr) if (!omap4plus_scm_readl(scm_ptr, scm_ptr->registers[0]->bgap_efuse)) pr_info("Non-trimmed BGAP, Temp not accurate\n"); -#if defined(CONFIG_CPU_THERMAL) - strncpy(scm_ptr->cpu_therm[0].name, "omap_ondie_sensor", - SENSOR_NAME_LEN); - scm_ptr->cpu_therm[0].private_data = scm_ptr; - scm_ptr->cpu_therm[0].read_temperature = omap_read_temp; - if (omap4_has_mpu_1_5ghz()) - scm_ptr->cpu_therm[0].sensor_data = - &omap4_1500mhz_bandgap_data; - else - scm_ptr->cpu_therm[0].sensor_data = - &omap4_1200mhz_bandgap_data; - omap_zones[0] = omap4_register_thermal(scm_ptr->cpu_therm); -#endif } else if (scm_ptr->rev == 2) { scm_ptr->registers[0] = &omap5430_mpu_temp_sensor_registers; scm_ptr->ts_data[0] = &omap5430_mpu_temp_sensor_data; @@ -982,24 +1009,6 @@ int omap4460plus_temp_sensor_init(struct scm *scm_ptr) scm_ptr->registers[2] = &omap5430_core_temp_sensor_registers; scm_ptr->ts_data[2] = &omap5430_core_temp_sensor_data; scm_ptr->conv_table = omap5430_adc_to_temp; -#if defined(CONFIG_CPU_THERMAL) - strncpy(scm_ptr->cpu_therm[0].name, "omap_ondie_sensor", - SENSOR_NAME_LEN); - scm_ptr->cpu_therm[0].private_data = scm_ptr; - scm_ptr->cpu_therm[0].read_temperature = omap_read_temp; - scm_ptr->cpu_therm[0].sensor_data = &omap5_1500mhz_bandgap_data; - omap_zones[0] = omap4_register_thermal(&scm_ptr->cpu_therm[0]); - strncpy(scm_ptr->cpu_therm[1].name, "omap_ondie_gpu_sensor", - SENSOR_NAME_LEN); - scm_ptr->cpu_therm[1].private_data = scm_ptr; - scm_ptr->cpu_therm[1].read_temperature = omap_read_temp; - scm_ptr->cpu_therm[1].sensor_data = &omap5_1500mhz_bandgap_data; - strncpy(scm_ptr->cpu_therm[2].name, "omap_ondie_core_sensor", - SENSOR_NAME_LEN); - scm_ptr->cpu_therm[2].private_data = scm_ptr; - scm_ptr->cpu_therm[2].read_temperature = omap_read_temp; - scm_ptr->cpu_therm[2].sensor_data = &omap5_1500mhz_bandgap_data; -#endif }
if (scm_ptr->rev == 1) { @@ -1009,40 +1018,39 @@ int omap4460plus_temp_sensor_init(struct scm *scm_ptr) if (clk_rate < scm_ptr->ts_data[0]->min_freq || clk_rate == 0xffffffff) { ret = -ENODEV; - goto clk_err; + goto free_regval; }
ret = clk_set_rate(scm_ptr->div_clk, clk_rate); if (ret) { pr_err("Cannot set clock rate\n"); - goto clk_err; + goto free_regval; }
scm_ptr->clk_rate = clk_rate; } else if (scm_ptr->rev == 2) { - /* add clock rate code for omap5430 */ -#ifdef CONFIG_MACH_OMAP_5430ZEBU - scm_ptr->clk_rate = 1200; -#else - + /* add clock rate code for omap5430 */ clk_rate = clk_round_rate(scm_ptr->fclock, scm_ptr->ts_data[0]->max_freq);
if (clk_rate < scm_ptr->ts_data[0]->min_freq || clk_rate == 0xffffffff) { ret = -ENODEV; - goto clk_err; + goto free_regval; }
ret = clk_set_rate(scm_ptr->fclock, clk_rate); if (ret) { pr_err("Cannot set clock rate\n"); - goto clk_err; + goto free_regval; } scm_ptr->clk_rate = clk_rate; -#endif }
+ ret = omap4460plus_cpu_cooling_init(scm_ptr); + if (ret) + goto free_regval; + clk_enable(scm_ptr->fclock); /* 1 clk cycle */ for (i = 0; i < scm_ptr->cnt; i++) @@ -1067,9 +1075,14 @@ int omap4460plus_temp_sensor_init(struct scm *scm_ptr) }
return 0; -clk_err: + +free_regval: + for (i = scm_ptr->cnt - 1; i >= 0; i--) + kfree(scm_ptr->regval[i]); + kfree(scm_ptr->regval); +free_ts_data: kfree(scm_ptr->ts_data); -fail_alloc: +free_registers: kfree(scm_ptr->registers); return ret; }