int ti_bandgap_probe()

in ti-soc-thermal/ti-bandgap.c [887:1069]

• 138 lines of code
• 35 McCabe index (conditional complexity)

int ti_bandgap_probe(struct platform_device *pdev)
{
	struct ti_bandgap *bgp;
	int clk_rate, ret, i;

	bgp = ti_bandgap_build(pdev);
	if (IS_ERR(bgp)) {
		dev_err(&pdev->dev, "failed to fetch platform data\n");
		return PTR_ERR(bgp);
	}
	bgp->dev = &pdev->dev;

	if (TI_BANDGAP_HAS(bgp, UNRELIABLE))
		dev_warn(&pdev->dev,
			 "This OMAP thermal sensor is unreliable. You've been warned\n");

	if (TI_BANDGAP_HAS(bgp, TSHUT)) {
		ret = ti_bandgap_tshut_init(bgp, pdev);
		if (ret) {
			dev_err(&pdev->dev,
				"failed to initialize system tshut IRQ\n");
			return ret;
		}
	}

	bgp->fclock = clk_get(NULL, bgp->conf->fclock_name);
	if (IS_ERR(bgp->fclock)) {
		dev_err(&pdev->dev, "failed to request fclock reference\n");
		ret = PTR_ERR(bgp->fclock);
		goto free_irqs;
	}

	bgp->div_clk = clk_get(NULL, bgp->conf->div_ck_name);
	if (IS_ERR(bgp->div_clk)) {
		dev_err(&pdev->dev, "failed to request div_ts_ck clock ref\n");
		ret = PTR_ERR(bgp->div_clk);
		goto put_fclock;
	}

	for (i = 0; i < bgp->conf->sensor_count; i++) {
		struct temp_sensor_registers *tsr;
		u32 val;

		tsr = bgp->conf->sensors[i].registers;
		/*
		 * check if the efuse has a non-zero value if not
		 * it is an untrimmed sample and the temperatures
		 * may not be accurate
		 */
		val = ti_bandgap_readl(bgp, tsr->bgap_efuse);
		if (!val)
			dev_info(&pdev->dev,
				 "Non-trimmed BGAP, Temp not accurate\n");
	}

	clk_rate = clk_round_rate(bgp->div_clk,
				  bgp->conf->sensors[0].ts_data->max_freq);
	if (clk_rate < bgp->conf->sensors[0].ts_data->min_freq ||
	    clk_rate <= 0) {
		ret = -ENODEV;
		dev_err(&pdev->dev, "wrong clock rate (%d)\n", clk_rate);
		goto put_clks;
	}

	ret = clk_set_rate(bgp->div_clk, clk_rate);
	if (ret)
		dev_err(&pdev->dev, "Cannot re-set clock rate. Continuing\n");

	bgp->clk_rate = clk_rate;
	if (TI_BANDGAP_HAS(bgp, CLK_CTRL))
		clk_prepare_enable(bgp->fclock);


	spin_lock_init(&bgp->lock);
	bgp->dev = &pdev->dev;
	platform_set_drvdata(pdev, bgp);

	ti_bandgap_power(bgp, true);

	/* Set default counter to 1 for now */
	if (TI_BANDGAP_HAS(bgp, COUNTER))
		for (i = 0; i < bgp->conf->sensor_count; i++)
			RMW_BITS(bgp, i, bgap_counter, counter_mask, 1);

	/* Set default thresholds for alert and shutdown */
	for (i = 0; i < bgp->conf->sensor_count; i++) {
		struct temp_sensor_data *ts_data;

		ts_data = bgp->conf->sensors[i].ts_data;

		if (TI_BANDGAP_HAS(bgp, TALERT)) {
			/* Set initial Talert thresholds */
			RMW_BITS(bgp, i, bgap_threshold,
				 threshold_tcold_mask, ts_data->t_cold);
			RMW_BITS(bgp, i, bgap_threshold,
				 threshold_thot_mask, ts_data->t_hot);
			/* Enable the alert events */
			RMW_BITS(bgp, i, bgap_mask_ctrl, mask_hot_mask, 1);
			RMW_BITS(bgp, i, bgap_mask_ctrl, mask_cold_mask, 1);
		}

		if (TI_BANDGAP_HAS(bgp, TSHUT_CONFIG)) {
			/* Set initial Tshut thresholds */
			RMW_BITS(bgp, i, tshut_threshold,
				 tshut_hot_mask, ts_data->tshut_hot);
			RMW_BITS(bgp, i, tshut_threshold,
				 tshut_cold_mask, ts_data->tshut_cold);
		}
	}

	if (TI_BANDGAP_HAS(bgp, MODE_CONFIG))
		ti_bandgap_set_continuous_mode(bgp);

	/* Set .250 seconds time as default counter */
	if (TI_BANDGAP_HAS(bgp, COUNTER))
		for (i = 0; i < bgp->conf->sensor_count; i++)
			RMW_BITS(bgp, i, bgap_counter, counter_mask,
				 bgp->clk_rate / 4);

	/* Every thing is good? Then expose the sensors */
	for (i = 0; i < bgp->conf->sensor_count; i++) {
		char *domain;

		if (bgp->conf->sensors[i].register_cooling) {
			ret = bgp->conf->sensors[i].register_cooling(bgp, i);
			if (ret)
				goto remove_sensors;
		}

		if (bgp->conf->expose_sensor) {
			domain = bgp->conf->sensors[i].domain;
			ret = bgp->conf->expose_sensor(bgp, i, domain);
			if (ret)
				goto remove_last_cooling;
		}
	}

	/*
	 * Enable the Interrupts once everything is set. Otherwise irq handler
	 * might be called as soon as it is enabled where as rest of framework
	 * is still getting initialised.
	 */
	if (TI_BANDGAP_HAS(bgp, TALERT)) {
		ret = ti_bandgap_talert_init(bgp, pdev);
		if (ret) {
			dev_err(&pdev->dev, "failed to initialize Talert IRQ\n");
			i = bgp->conf->sensor_count;
			goto disable_clk;
		}
	}

#ifdef CONFIG_PM_SLEEP
	bgp->nb.notifier_call = bandgap_omap_cpu_notifier;
	if (!soc_device_match(soc_no_cpu_notifier))
		cpu_pm_register_notifier(&bgp->nb);
#endif

	return 0;

remove_last_cooling:
	if (bgp->conf->sensors[i].unregister_cooling)
		bgp->conf->sensors[i].unregister_cooling(bgp, i);
remove_sensors:
	for (i--; i >= 0; i--) {
		if (bgp->conf->sensors[i].unregister_cooling)
			bgp->conf->sensors[i].unregister_cooling(bgp, i);
		if (bgp->conf->remove_sensor)
			bgp->conf->remove_sensor(bgp, i);
	}
	ti_bandgap_power(bgp, false);
disable_clk:
	if (TI_BANDGAP_HAS(bgp, CLK_CTRL))
		clk_disable_unprepare(bgp->fclock);
put_clks:
	clk_put(bgp->div_clk);
put_fclock:
	clk_put(bgp->fclock);
free_irqs:
	if (TI_BANDGAP_HAS(bgp, TSHUT))
		free_irq(gpiod_to_irq(bgp->tshut_gpiod), NULL);

	return ret;
}