static int mtk_disp_pwm_apply()

in pwm-mtk-disp.c [71:173]

• 76 lines of code
• 11 McCabe index (conditional complexity)

static int mtk_disp_pwm_apply(struct pwm_chip *chip, struct pwm_device *pwm,
			      const struct pwm_state *state)
{
	struct mtk_disp_pwm *mdp = to_mtk_disp_pwm(chip);
	u32 clk_div, period, high_width, value;
	u64 div, rate;
	int err;

	if (state->polarity != PWM_POLARITY_NORMAL)
		return -EINVAL;

	if (!state->enabled) {
		mtk_disp_pwm_update_bits(mdp, DISP_PWM_EN, mdp->data->enable_mask,
					 0x0);

		if (mdp->enabled) {
			clk_disable_unprepare(mdp->clk_mm);
			clk_disable_unprepare(mdp->clk_main);
		}

		mdp->enabled = false;
		return 0;
	}

	if (!mdp->enabled) {
		err = clk_prepare_enable(mdp->clk_main);
		if (err < 0) {
			dev_err(chip->dev, "Can't enable mdp->clk_main: %pe\n",
				ERR_PTR(err));
			return err;
		}

		err = clk_prepare_enable(mdp->clk_mm);
		if (err < 0) {
			dev_err(chip->dev, "Can't enable mdp->clk_mm: %pe\n",
				ERR_PTR(err));
			clk_disable_unprepare(mdp->clk_main);
			return err;
		}
	}

	/*
	 * Find period, high_width and clk_div to suit duty_ns and period_ns.
	 * Calculate proper div value to keep period value in the bound.
	 *
	 * period_ns = 10^9 * (clk_div + 1) * (period + 1) / PWM_CLK_RATE
	 * duty_ns = 10^9 * (clk_div + 1) * high_width / PWM_CLK_RATE
	 *
	 * period = (PWM_CLK_RATE * period_ns) / (10^9 * (clk_div + 1)) - 1
	 * high_width = (PWM_CLK_RATE * duty_ns) / (10^9 * (clk_div + 1))
	 */
	rate = clk_get_rate(mdp->clk_main);
	clk_div = mul_u64_u64_div_u64(state->period, rate, NSEC_PER_SEC) >>
			  PWM_PERIOD_BIT_WIDTH;
	if (clk_div > PWM_CLKDIV_MAX) {
		if (!mdp->enabled) {
			clk_disable_unprepare(mdp->clk_mm);
			clk_disable_unprepare(mdp->clk_main);
		}
		return -EINVAL;
	}

	div = NSEC_PER_SEC * (clk_div + 1);
	period = mul_u64_u64_div_u64(state->period, rate, div);
	if (period > 0)
		period--;

	high_width = mul_u64_u64_div_u64(state->duty_cycle, rate, div);
	value = period | (high_width << PWM_HIGH_WIDTH_SHIFT);

	mtk_disp_pwm_update_bits(mdp, mdp->data->con0,
				 PWM_CLKDIV_MASK,
				 clk_div << PWM_CLKDIV_SHIFT);
	mtk_disp_pwm_update_bits(mdp, mdp->data->con1,
				 PWM_PERIOD_MASK | PWM_HIGH_WIDTH_MASK,
				 value);

	if (mdp->data->has_commit) {
		mtk_disp_pwm_update_bits(mdp, mdp->data->commit,
					 mdp->data->commit_mask,
					 mdp->data->commit_mask);
		mtk_disp_pwm_update_bits(mdp, mdp->data->commit,
					 mdp->data->commit_mask,
					 0x0);
	} else {
		/*
		 * For MT2701, disable double buffer before writing register
		 * and select manual mode and use PWM_PERIOD/PWM_HIGH_WIDTH.
		 */
		mtk_disp_pwm_update_bits(mdp, mdp->data->bls_debug,
					 mdp->data->bls_debug_mask,
					 mdp->data->bls_debug_mask);
		mtk_disp_pwm_update_bits(mdp, mdp->data->con0,
					 mdp->data->con0_sel,
					 mdp->data->con0_sel);
	}

	mtk_disp_pwm_update_bits(mdp, DISP_PWM_EN, mdp->data->enable_mask,
				 mdp->data->enable_mask);
	mdp->enabled = true;

	return 0;
}