static int __init tcb_clksrc_init()

in timer-atmel-tcb.c [374:509]

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

static int __init tcb_clksrc_init(struct device_node *node)
{
	struct atmel_tc tc;
	struct clk *t0_clk;
	const struct of_device_id *match;
	u64 (*tc_sched_clock)(void);
	u32 rate, divided_rate = 0;
	int best_divisor_idx = -1;
	int bits;
	int i;
	int ret;

	/* Protect against multiple calls */
	if (tcaddr)
		return 0;

	tc.regs = of_iomap(node->parent, 0);
	if (!tc.regs)
		return -ENXIO;

	t0_clk = of_clk_get_by_name(node->parent, "t0_clk");
	if (IS_ERR(t0_clk))
		return PTR_ERR(t0_clk);

	tc.slow_clk = of_clk_get_by_name(node->parent, "slow_clk");
	if (IS_ERR(tc.slow_clk))
		return PTR_ERR(tc.slow_clk);

	tc.clk[0] = t0_clk;
	tc.clk[1] = of_clk_get_by_name(node->parent, "t1_clk");
	if (IS_ERR(tc.clk[1]))
		tc.clk[1] = t0_clk;
	tc.clk[2] = of_clk_get_by_name(node->parent, "t2_clk");
	if (IS_ERR(tc.clk[2]))
		tc.clk[2] = t0_clk;

	tc.irq[2] = of_irq_get(node->parent, 2);
	if (tc.irq[2] <= 0) {
		tc.irq[2] = of_irq_get(node->parent, 0);
		if (tc.irq[2] <= 0)
			return -EINVAL;
	}

	match = of_match_node(atmel_tcb_of_match, node->parent);
	if (!match)
		return -ENODEV;

	tc.tcb_config = match->data;
	bits = tc.tcb_config->counter_width;

	for (i = 0; i < ARRAY_SIZE(tc.irq); i++)
		writel(ATMEL_TC_ALL_IRQ, tc.regs + ATMEL_TC_REG(i, IDR));

	ret = clk_prepare_enable(t0_clk);
	if (ret) {
		pr_debug("can't enable T0 clk\n");
		return ret;
	}

	/* How fast will we be counting?  Pick something over 5 MHz.  */
	rate = (u32) clk_get_rate(t0_clk);
	i = 0;
	if (tc.tcb_config->has_gclk)
		i = 1;
	for (; i < ARRAY_SIZE(atmel_tcb_divisors); i++) {
		unsigned divisor = atmel_tcb_divisors[i];
		unsigned tmp;

		tmp = rate / divisor;
		pr_debug("TC: %u / %-3u [%d] --> %u\n", rate, divisor, i, tmp);
		if ((best_divisor_idx >= 0) && (tmp < 5 * 1000 * 1000))
			break;
		divided_rate = tmp;
		best_divisor_idx = i;
	}

	clksrc.name = kbasename(node->parent->full_name);
	clkevt.clkevt.name = kbasename(node->parent->full_name);
	pr_debug("%s at %d.%03d MHz\n", clksrc.name, divided_rate / 1000000,
			((divided_rate % 1000000) + 500) / 1000);

	tcaddr = tc.regs;

	if (bits == 32) {
		/* use appropriate function to read 32 bit counter */
		clksrc.read = tc_get_cycles32;
		/* setup only channel 0 */
		tcb_setup_single_chan(&tc, best_divisor_idx);
		tc_sched_clock = tc_sched_clock_read32;
		tc_delay_timer.read_current_timer = tc_delay_timer_read32;
	} else {
		/* we have three clocks no matter what the
		 * underlying platform supports.
		 */
		ret = clk_prepare_enable(tc.clk[1]);
		if (ret) {
			pr_debug("can't enable T1 clk\n");
			goto err_disable_t0;
		}
		/* setup both channel 0 & 1 */
		tcb_setup_dual_chan(&tc, best_divisor_idx);
		tc_sched_clock = tc_sched_clock_read;
		tc_delay_timer.read_current_timer = tc_delay_timer_read;
	}

	/* and away we go! */
	ret = clocksource_register_hz(&clksrc, divided_rate);
	if (ret)
		goto err_disable_t1;

	/* channel 2:  periodic and oneshot timer support */
	ret = setup_clkevents(&tc, best_divisor_idx);
	if (ret)
		goto err_unregister_clksrc;

	sched_clock_register(tc_sched_clock, 32, divided_rate);

	tc_delay_timer.freq = divided_rate;
	register_current_timer_delay(&tc_delay_timer);

	return 0;

err_unregister_clksrc:
	clocksource_unregister(&clksrc);

err_disable_t1:
	if (bits != 32)
		clk_disable_unprepare(tc.clk[1]);

err_disable_t0:
	clk_disable_unprepare(t0_clk);

	tcaddr = NULL;

	return ret;
}