static int __init ingenic_tcu_init()

in ingenic-timer.c [273:358]

• 65 lines of code
• 7 McCabe index (conditional complexity)

static int __init ingenic_tcu_init(struct device_node *np)
{
	const struct of_device_id *id = of_match_node(ingenic_tcu_of_match, np);
	const struct ingenic_soc_info *soc_info = id->data;
	struct ingenic_tcu_timer *timer;
	struct ingenic_tcu *tcu;
	struct regmap *map;
	unsigned int cpu;
	int ret, last_bit = -1;
	long rate;

	of_node_clear_flag(np, OF_POPULATED);

	map = device_node_to_regmap(np);
	if (IS_ERR(map))
		return PTR_ERR(map);

	tcu = kzalloc(struct_size(tcu, timers, num_possible_cpus()),
		      GFP_KERNEL);
	if (!tcu)
		return -ENOMEM;

	/*
	 * Enable all TCU channels for PWM use by default except channels 0/1,
	 * and channel 2 if target CPU is JZ4780/X2000 and SMP is selected.
	 */
	tcu->pwm_channels_mask = GENMASK(soc_info->num_channels - 1,
					 num_possible_cpus() + 1);
	of_property_read_u32(np, "ingenic,pwm-channels-mask",
			     (u32 *)&tcu->pwm_channels_mask);

	/* Verify that we have at least num_possible_cpus() + 1 free channels */
	if (hweight8(tcu->pwm_channels_mask) >
			soc_info->num_channels - num_possible_cpus() + 1) {
		pr_crit("%s: Invalid PWM channel mask: 0x%02lx\n", __func__,
			tcu->pwm_channels_mask);
		ret = -EINVAL;
		goto err_free_ingenic_tcu;
	}

	tcu->map = map;
	tcu->np = np;
	ingenic_tcu = tcu;

	for (cpu = 0; cpu < num_possible_cpus(); cpu++) {
		timer = &tcu->timers[cpu];

		timer->cpu = cpu;
		timer->channel = find_next_zero_bit(&tcu->pwm_channels_mask,
						  soc_info->num_channels,
						  last_bit + 1);
		last_bit = timer->channel;
	}

	tcu->cs_channel = find_next_zero_bit(&tcu->pwm_channels_mask,
					     soc_info->num_channels,
					     last_bit + 1);

	ret = ingenic_tcu_clocksource_init(np, tcu);
	if (ret) {
		pr_crit("%s: Unable to init clocksource: %d\n", __func__, ret);
		goto err_free_ingenic_tcu;
	}

	/* Setup clock events on each CPU core */
	ret = cpuhp_setup_state(CPUHP_AP_ONLINE_DYN, "Ingenic XBurst: online",
				ingenic_tcu_setup_cevt, NULL);
	if (ret < 0) {
		pr_crit("%s: Unable to start CPU timers: %d\n", __func__, ret);
		goto err_tcu_clocksource_cleanup;
	}

	/* Register the sched_clock at the end as there's no way to undo it */
	rate = clk_get_rate(tcu->cs_clk);
	sched_clock_register(ingenic_tcu_timer_read, 16, rate);

	return 0;

err_tcu_clocksource_cleanup:
	clocksource_unregister(&tcu->cs);
	clk_disable_unprepare(tcu->cs_clk);
	clk_put(tcu->cs_clk);
err_free_ingenic_tcu:
	kfree(tcu);
	return ret;
}