static int imx_cpufreq_dt_probe()

in imx-cpufreq-dt.c [83:173]

• 70 lines of code
• 17 McCabe index (conditional complexity)

static int imx_cpufreq_dt_probe(struct platform_device *pdev)
{
	struct platform_device *dt_pdev;
	u32 cell_value, supported_hw[2];
	int speed_grade, mkt_segment;
	int ret;

	cpu_dev = get_cpu_device(0);

	if (!of_find_property(cpu_dev->of_node, "cpu-supply", NULL))
		return -ENODEV;

	if (of_machine_is_compatible("fsl,imx7ulp")) {
		ret = clk_bulk_get(cpu_dev, ARRAY_SIZE(imx7ulp_clks),
				   imx7ulp_clks);
		if (ret)
			return ret;

		dt_pdev = platform_device_register_data(NULL, "cpufreq-dt",
							-1, &imx7ulp_data,
							sizeof(imx7ulp_data));
		if (IS_ERR(dt_pdev)) {
			clk_bulk_put(ARRAY_SIZE(imx7ulp_clks), imx7ulp_clks);
			ret = PTR_ERR(dt_pdev);
			dev_err(&pdev->dev, "Failed to register cpufreq-dt: %d\n", ret);
			return ret;
		}

		cpufreq_dt_pdev = dt_pdev;

		return 0;
	}

	ret = nvmem_cell_read_u32(cpu_dev, "speed_grade", &cell_value);
	if (ret)
		return ret;

	if (of_machine_is_compatible("fsl,imx8mn") ||
	    of_machine_is_compatible("fsl,imx8mp"))
		speed_grade = (cell_value & IMX8MN_OCOTP_CFG3_SPEED_GRADE_MASK)
			      >> OCOTP_CFG3_SPEED_GRADE_SHIFT;
	else
		speed_grade = (cell_value & OCOTP_CFG3_SPEED_GRADE_MASK)
			      >> OCOTP_CFG3_SPEED_GRADE_SHIFT;

	if (of_machine_is_compatible("fsl,imx8mp"))
		mkt_segment = (cell_value & IMX8MP_OCOTP_CFG3_MKT_SEGMENT_MASK)
			       >> IMX8MP_OCOTP_CFG3_MKT_SEGMENT_SHIFT;
	else
		mkt_segment = (cell_value & OCOTP_CFG3_MKT_SEGMENT_MASK)
			       >> OCOTP_CFG3_MKT_SEGMENT_SHIFT;

	/*
	 * Early samples without fuses written report "0 0" which may NOT
	 * match any OPP defined in DT. So clamp to minimum OPP defined in
	 * DT to avoid warning for "no OPPs".
	 *
	 * Applies to i.MX8M series SoCs.
	 */
	if (mkt_segment == 0 && speed_grade == 0) {
		if (of_machine_is_compatible("fsl,imx8mm") ||
		    of_machine_is_compatible("fsl,imx8mq"))
			speed_grade = 1;
		if (of_machine_is_compatible("fsl,imx8mn") ||
		    of_machine_is_compatible("fsl,imx8mp"))
			speed_grade = 0xb;
	}

	supported_hw[0] = BIT(speed_grade);
	supported_hw[1] = BIT(mkt_segment);
	dev_info(&pdev->dev, "cpu speed grade %d mkt segment %d supported-hw %#x %#x\n",
			speed_grade, mkt_segment, supported_hw[0], supported_hw[1]);

	cpufreq_opp_table = dev_pm_opp_set_supported_hw(cpu_dev, supported_hw, 2);
	if (IS_ERR(cpufreq_opp_table)) {
		ret = PTR_ERR(cpufreq_opp_table);
		dev_err(&pdev->dev, "Failed to set supported opp: %d\n", ret);
		return ret;
	}

	cpufreq_dt_pdev = platform_device_register_data(
			&pdev->dev, "cpufreq-dt", -1, NULL, 0);
	if (IS_ERR(cpufreq_dt_pdev)) {
		dev_pm_opp_put_supported_hw(cpufreq_opp_table);
		ret = PTR_ERR(cpufreq_dt_pdev);
		dev_err(&pdev->dev, "Failed to register cpufreq-dt: %d\n", ret);
		return ret;
	}

	return 0;
}