static int lpc18xx_pwm_probe()

in pwm-lpc18xx-sct.c [324:435]

• 83 lines of code
• 9 McCabe index (conditional complexity)

static int lpc18xx_pwm_probe(struct platform_device *pdev)
{
	struct lpc18xx_pwm_chip *lpc18xx_pwm;
	struct pwm_device *pwm;
	int ret, i;
	u64 val;

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

	lpc18xx_pwm->dev = &pdev->dev;

	lpc18xx_pwm->base = devm_platform_ioremap_resource(pdev, 0);
	if (IS_ERR(lpc18xx_pwm->base))
		return PTR_ERR(lpc18xx_pwm->base);

	lpc18xx_pwm->pwm_clk = devm_clk_get(&pdev->dev, "pwm");
	if (IS_ERR(lpc18xx_pwm->pwm_clk)) {
		dev_err(&pdev->dev, "failed to get pwm clock\n");
		return PTR_ERR(lpc18xx_pwm->pwm_clk);
	}

	ret = clk_prepare_enable(lpc18xx_pwm->pwm_clk);
	if (ret < 0) {
		dev_err(&pdev->dev, "could not prepare or enable pwm clock\n");
		return ret;
	}

	lpc18xx_pwm->clk_rate = clk_get_rate(lpc18xx_pwm->pwm_clk);
	if (!lpc18xx_pwm->clk_rate) {
		dev_err(&pdev->dev, "pwm clock has no frequency\n");
		ret = -EINVAL;
		goto disable_pwmclk;
	}

	mutex_init(&lpc18xx_pwm->res_lock);
	mutex_init(&lpc18xx_pwm->period_lock);

	val = (u64)NSEC_PER_SEC * LPC18XX_PWM_TIMER_MAX;
	do_div(val, lpc18xx_pwm->clk_rate);
	lpc18xx_pwm->max_period_ns = val;

	lpc18xx_pwm->min_period_ns = DIV_ROUND_UP(NSEC_PER_SEC,
						  lpc18xx_pwm->clk_rate);

	lpc18xx_pwm->chip.dev = &pdev->dev;
	lpc18xx_pwm->chip.ops = &lpc18xx_pwm_ops;
	lpc18xx_pwm->chip.npwm = 16;

	/* SCT counter must be in unify (32 bit) mode */
	lpc18xx_pwm_writel(lpc18xx_pwm, LPC18XX_PWM_CONFIG,
			   LPC18XX_PWM_CONFIG_UNIFY);

	/*
	 * Everytime the timer counter reaches the period value, the related
	 * event will be triggered and the counter reset to 0.
	 */
	set_bit(LPC18XX_PWM_EVENT_PERIOD, &lpc18xx_pwm->event_map);
	lpc18xx_pwm->period_event = LPC18XX_PWM_EVENT_PERIOD;

	lpc18xx_pwm_writel(lpc18xx_pwm,
			   LPC18XX_PWM_EVSTATEMSK(lpc18xx_pwm->period_event),
			   LPC18XX_PWM_EVSTATEMSK_ALL);

	val = LPC18XX_PWM_EVCTRL_MATCH(lpc18xx_pwm->period_event) |
	      LPC18XX_PWM_EVCTRL_COMB_MATCH;
	lpc18xx_pwm_writel(lpc18xx_pwm,
			   LPC18XX_PWM_EVCTRL(lpc18xx_pwm->period_event), val);

	lpc18xx_pwm_writel(lpc18xx_pwm, LPC18XX_PWM_LIMIT,
			   BIT(lpc18xx_pwm->period_event));

	ret = pwmchip_add(&lpc18xx_pwm->chip);
	if (ret < 0) {
		dev_err(&pdev->dev, "pwmchip_add failed: %d\n", ret);
		goto disable_pwmclk;
	}

	for (i = 0; i < lpc18xx_pwm->chip.npwm; i++) {
		struct lpc18xx_pwm_data *data;

		pwm = &lpc18xx_pwm->chip.pwms[i];

		data = devm_kzalloc(lpc18xx_pwm->dev, sizeof(*data),
				    GFP_KERNEL);
		if (!data) {
			ret = -ENOMEM;
			goto remove_pwmchip;
		}

		pwm_set_chip_data(pwm, data);
	}

	platform_set_drvdata(pdev, lpc18xx_pwm);

	val = lpc18xx_pwm_readl(lpc18xx_pwm, LPC18XX_PWM_CTRL);
	val &= ~LPC18XX_PWM_BIDIR;
	val &= ~LPC18XX_PWM_CTRL_HALT;
	val &= ~LPC18XX_PWM_PRE_MASK;
	val |= LPC18XX_PWM_PRE(0);
	lpc18xx_pwm_writel(lpc18xx_pwm, LPC18XX_PWM_CTRL, val);

	return 0;

remove_pwmchip:
	pwmchip_remove(&lpc18xx_pwm->chip);
disable_pwmclk:
	clk_disable_unprepare(lpc18xx_pwm->pwm_clk);
	return ret;
}