static int bcm2835_thermal_probe()

in broadcom/bcm2835_thermal.c [164:285]

• 84 lines of code
• 12 McCabe index (conditional complexity)

static int bcm2835_thermal_probe(struct platform_device *pdev)
{
	const struct of_device_id *match;
	struct thermal_zone_device *tz;
	struct bcm2835_thermal_data *data;
	struct resource *res;
	int err = 0;
	u32 val;
	unsigned long rate;

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

	match = of_match_device(bcm2835_thermal_of_match_table,
				&pdev->dev);
	if (!match)
		return -EINVAL;

	res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
	data->regs = devm_ioremap_resource(&pdev->dev, res);
	if (IS_ERR(data->regs)) {
		err = PTR_ERR(data->regs);
		return err;
	}

	data->clk = devm_clk_get(&pdev->dev, NULL);
	if (IS_ERR(data->clk)) {
		err = PTR_ERR(data->clk);
		if (err != -EPROBE_DEFER)
			dev_err(&pdev->dev, "Could not get clk: %d\n", err);
		return err;
	}

	err = clk_prepare_enable(data->clk);
	if (err)
		return err;

	rate = clk_get_rate(data->clk);
	if ((rate < 1920000) || (rate > 5000000))
		dev_warn(&pdev->dev,
			 "Clock %pCn running at %lu Hz is outside of the recommended range: 1.92 to 5MHz\n",
			 data->clk, rate);

	/* register of thermal sensor and get info from DT */
	tz = thermal_zone_of_sensor_register(&pdev->dev, 0, data,
					     &bcm2835_thermal_ops);
	if (IS_ERR(tz)) {
		err = PTR_ERR(tz);
		dev_err(&pdev->dev,
			"Failed to register the thermal device: %d\n",
			err);
		goto err_clk;
	}

	/*
	 * right now the FW does set up the HW-block, so we are not
	 * touching the configuration registers.
	 * But if the HW is not enabled, then set it up
	 * using "sane" values used by the firmware right now.
	 */
	val = readl(data->regs + BCM2835_TS_TSENSCTL);
	if (!(val & BCM2835_TS_TSENSCTL_RSTB)) {
		int trip_temp, offset, slope;

		slope = thermal_zone_get_slope(tz);
		offset = thermal_zone_get_offset(tz);
		/*
		 * For now we deal only with critical, otherwise
		 * would need to iterate
		 */
		err = tz->ops->get_trip_temp(tz, 0, &trip_temp);
		if (err < 0) {
			dev_err(&pdev->dev,
				"Not able to read trip_temp: %d\n",
				err);
			goto err_tz;
		}

		/* set bandgap reference voltage and enable voltage regulator */
		val = (BCM2835_TS_TSENSCTL_CTRL_DEFAULT <<
		       BCM2835_TS_TSENSCTL_CTRL_SHIFT) |
		      BCM2835_TS_TSENSCTL_REGULEN;

		/* use the recommended reset duration */
		val |= (0xFE << BCM2835_TS_TSENSCTL_RSTDELAY_SHIFT);

		/*  trip_adc value from info */
		val |= bcm2835_thermal_temp2adc(trip_temp,
						offset,
						slope)
			<< BCM2835_TS_TSENSCTL_THOLD_SHIFT;

		/* write the value back to the register as 2 steps */
		writel(val, data->regs + BCM2835_TS_TSENSCTL);
		val |= BCM2835_TS_TSENSCTL_RSTB;
		writel(val, data->regs + BCM2835_TS_TSENSCTL);
	}

	data->tz = tz;

	platform_set_drvdata(pdev, data);

	/*
	 * Thermal_zone doesn't enable hwmon as default,
	 * enable it here
	 */
	tz->tzp->no_hwmon = false;
	err = thermal_add_hwmon_sysfs(tz);
	if (err)
		goto err_tz;

	bcm2835_thermal_debugfs(pdev);

	return 0;
err_tz:
	thermal_zone_of_sensor_unregister(&pdev->dev, tz);
err_clk:
	clk_disable_unprepare(data->clk);

	return err;
}