static int tegra_soctherm_probe()

in tegra/soctherm.c [2115:2258]

• 113 lines of code
• 21 McCabe index (conditional complexity)

static int tegra_soctherm_probe(struct platform_device *pdev)
{
	const struct of_device_id *match;
	struct tegra_soctherm *tegra;
	struct thermal_zone_device *z;
	struct tsensor_shared_calib shared_calib;
	struct tegra_soctherm_soc *soc;
	unsigned int i;
	int err;

	match = of_match_node(tegra_soctherm_of_match, pdev->dev.of_node);
	if (!match)
		return -ENODEV;

	soc = (struct tegra_soctherm_soc *)match->data;
	if (soc->num_ttgs > TEGRA124_SOCTHERM_SENSOR_NUM)
		return -EINVAL;

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

	mutex_init(&tegra->thermctl_lock);
	dev_set_drvdata(&pdev->dev, tegra);

	tegra->soc = soc;

	tegra->regs = devm_platform_ioremap_resource_byname(pdev, "soctherm-reg");
	if (IS_ERR(tegra->regs)) {
		dev_err(&pdev->dev, "can't get soctherm registers");
		return PTR_ERR(tegra->regs);
	}

	if (!tegra->soc->use_ccroc) {
		tegra->clk_regs = devm_platform_ioremap_resource_byname(pdev, "car-reg");
		if (IS_ERR(tegra->clk_regs)) {
			dev_err(&pdev->dev, "can't get car clk registers");
			return PTR_ERR(tegra->clk_regs);
		}
	} else {
		tegra->ccroc_regs = devm_platform_ioremap_resource_byname(pdev, "ccroc-reg");
		if (IS_ERR(tegra->ccroc_regs)) {
			dev_err(&pdev->dev, "can't get ccroc registers");
			return PTR_ERR(tegra->ccroc_regs);
		}
	}

	tegra->reset = devm_reset_control_get(&pdev->dev, "soctherm");
	if (IS_ERR(tegra->reset)) {
		dev_err(&pdev->dev, "can't get soctherm reset\n");
		return PTR_ERR(tegra->reset);
	}

	tegra->clock_tsensor = devm_clk_get(&pdev->dev, "tsensor");
	if (IS_ERR(tegra->clock_tsensor)) {
		dev_err(&pdev->dev, "can't get tsensor clock\n");
		return PTR_ERR(tegra->clock_tsensor);
	}

	tegra->clock_soctherm = devm_clk_get(&pdev->dev, "soctherm");
	if (IS_ERR(tegra->clock_soctherm)) {
		dev_err(&pdev->dev, "can't get soctherm clock\n");
		return PTR_ERR(tegra->clock_soctherm);
	}

	tegra->calib = devm_kcalloc(&pdev->dev,
				    soc->num_tsensors, sizeof(u32),
				    GFP_KERNEL);
	if (!tegra->calib)
		return -ENOMEM;

	/* calculate shared calibration data */
	err = tegra_calc_shared_calib(soc->tfuse, &shared_calib);
	if (err)
		return err;

	/* calculate tsensor calibration data */
	for (i = 0; i < soc->num_tsensors; ++i) {
		err = tegra_calc_tsensor_calib(&soc->tsensors[i],
					       &shared_calib,
					       &tegra->calib[i]);
		if (err)
			return err;
	}

	tegra->thermctl_tzs = devm_kcalloc(&pdev->dev,
					   soc->num_ttgs, sizeof(z),
					   GFP_KERNEL);
	if (!tegra->thermctl_tzs)
		return -ENOMEM;

	err = soctherm_clk_enable(pdev, true);
	if (err)
		return err;

	soctherm_thermtrips_parse(pdev);

	soctherm_init_hw_throt_cdev(pdev);

	soctherm_init(pdev);

	for (i = 0; i < soc->num_ttgs; ++i) {
		struct tegra_thermctl_zone *zone =
			devm_kzalloc(&pdev->dev, sizeof(*zone), GFP_KERNEL);
		if (!zone) {
			err = -ENOMEM;
			goto disable_clocks;
		}

		zone->reg = tegra->regs + soc->ttgs[i]->sensor_temp_offset;
		zone->dev = &pdev->dev;
		zone->sg = soc->ttgs[i];
		zone->ts = tegra;

		z = devm_thermal_zone_of_sensor_register(&pdev->dev,
							 soc->ttgs[i]->id, zone,
							 &tegra_of_thermal_ops);
		if (IS_ERR(z)) {
			err = PTR_ERR(z);
			dev_err(&pdev->dev, "failed to register sensor: %d\n",
				err);
			goto disable_clocks;
		}

		zone->tz = z;
		tegra->thermctl_tzs[soc->ttgs[i]->id] = z;

		/* Configure hw trip points */
		err = tegra_soctherm_set_hwtrips(&pdev->dev, soc->ttgs[i], z);
		if (err)
			goto disable_clocks;
	}

	err = soctherm_interrupts_init(pdev, tegra);

	soctherm_debug_init(pdev);

	return 0;

disable_clocks:
	soctherm_clk_enable(pdev, false);

	return err;
}