static int sprd_thm_probe()

in sprd_thermal.c [331:445]

• 92 lines of code
• 18 McCabe index (conditional complexity)

static int sprd_thm_probe(struct platform_device *pdev)
{
	struct device_node *np = pdev->dev.of_node;
	struct device_node *sen_child;
	struct sprd_thermal_data *thm;
	struct sprd_thermal_sensor *sen;
	const struct sprd_thm_variant_data *pdata;
	int ret, i;
	u32 val;

	pdata = of_device_get_match_data(&pdev->dev);
	if (!pdata) {
		dev_err(&pdev->dev, "No matching driver data found\n");
		return -EINVAL;
	}

	thm = devm_kzalloc(&pdev->dev, sizeof(*thm), GFP_KERNEL);
	if (!thm)
		return -ENOMEM;

	thm->var_data = pdata;
	thm->base = devm_platform_ioremap_resource(pdev, 0);
	if (IS_ERR(thm->base))
		return PTR_ERR(thm->base);

	thm->nr_sensors = of_get_child_count(np);
	if (thm->nr_sensors == 0 || thm->nr_sensors > SPRD_THM_MAX_SENSOR) {
		dev_err(&pdev->dev, "incorrect sensor count\n");
		return -EINVAL;
	}

	thm->clk = devm_clk_get(&pdev->dev, "enable");
	if (IS_ERR(thm->clk)) {
		dev_err(&pdev->dev, "failed to get enable clock\n");
		return PTR_ERR(thm->clk);
	}

	ret = clk_prepare_enable(thm->clk);
	if (ret)
		return ret;

	sprd_thm_para_config(thm);

	ret = sprd_thm_cal_read(np, "thm_sign_cal", &val);
	if (ret)
		goto disable_clk;

	if (val > 0)
		thm->ratio_sign = -1;
	else
		thm->ratio_sign = 1;

	ret = sprd_thm_cal_read(np, "thm_ratio_cal", &thm->ratio_off);
	if (ret)
		goto disable_clk;

	for_each_child_of_node(np, sen_child) {
		sen = devm_kzalloc(&pdev->dev, sizeof(*sen), GFP_KERNEL);
		if (!sen) {
			ret = -ENOMEM;
			goto of_put;
		}

		sen->data = thm;
		sen->dev = &pdev->dev;

		ret = of_property_read_u32(sen_child, "reg", &sen->id);
		if (ret) {
			dev_err(&pdev->dev, "get sensor reg failed");
			goto of_put;
		}

		ret = sprd_thm_sensor_calibration(sen_child, thm, sen);
		if (ret) {
			dev_err(&pdev->dev, "efuse cal analysis failed");
			goto of_put;
		}

		sprd_thm_sensor_init(thm, sen);

		sen->tzd = devm_thermal_zone_of_sensor_register(sen->dev,
								sen->id,
								sen,
								&sprd_thm_ops);
		if (IS_ERR(sen->tzd)) {
			dev_err(&pdev->dev, "register thermal zone failed %d\n",
				sen->id);
			ret = PTR_ERR(sen->tzd);
			goto of_put;
		}

		thm->sensor[sen->id] = sen;
	}
	/* sen_child set to NULL at this point */

	ret = sprd_thm_set_ready(thm);
	if (ret)
		goto of_put;

	ret = sprd_thm_wait_temp_ready(thm);
	if (ret)
		goto of_put;

	for (i = 0; i < thm->nr_sensors; i++)
		sprd_thm_toggle_sensor(thm->sensor[i], true);

	platform_set_drvdata(pdev, thm);
	return 0;

of_put:
	of_node_put(sen_child);
disable_clk:
	clk_disable_unprepare(thm->clk);
	return ret;
}