1 files changed, 132 insertions, 144 deletions

diff --git a/drivers/pwm/pwm-atmel.c b/drivers/pwm/pwm-atmel.c
index 67a7023be5c2..530d7dc5f1b5 100644
--- a/drivers/pwm/pwm-atmel.c
+++ b/drivers/pwm/pwm-atmel.c
@@ -58,17 +58,22 @@
 #define PWM_MAX_PRD		0xFFFF
 #define PRD_MAX_PRES		10
 
+struct atmel_pwm_registers {
+	u8 period;
+	u8 period_upd;
+	u8 duty;
+	u8 duty_upd;
+};
+
 struct atmel_pwm_chip {
 	struct pwm_chip chip;
 	struct clk *clk;
 	void __iomem *base;
+	const struct atmel_pwm_registers *regs;
 
 	unsigned int updated_pwms;
 	/* ISR is cleared when read, ensure only one thread does that */
 	struct mutex isr_lock;
-
-	void (*config)(struct pwm_chip *chip, struct pwm_device *pwm,
-		       unsigned long dty, unsigned long prd);
 };
 
 static inline struct atmel_pwm_chip *to_atmel_pwm_chip(struct pwm_chip *chip)
@@ -105,153 +110,71 @@ static inline void atmel_pwm_ch_writel(struct atmel_pwm_chip *chip,
 	writel_relaxed(val, chip->base + base + offset);
 }
 
-static int atmel_pwm_config(struct pwm_chip *chip, struct pwm_device *pwm,
-			    int duty_ns, int period_ns)
+static int atmel_pwm_calculate_cprd_and_pres(struct pwm_chip *chip,
+					     const struct pwm_state *state,
+					     unsigned long *cprd, u32 *pres)
 {
 	struct atmel_pwm_chip *atmel_pwm = to_atmel_pwm_chip(chip);
-	unsigned long prd, dty;
-	unsigned long long div;
-	unsigned int pres = 0;
-	u32 val;
-	int ret;
-
-	if (pwm_is_enabled(pwm) && (period_ns != pwm_get_period(pwm))) {
-		dev_err(chip->dev, "cannot change PWM period while enabled\n");
-		return -EBUSY;
-	}
+	unsigned long long cycles = state->period;
 
 	/* Calculate the period cycles and prescale value */
-	div = (unsigned long long)clk_get_rate(atmel_pwm->clk) * period_ns;
-	do_div(div, NSEC_PER_SEC);
+	cycles *= clk_get_rate(atmel_pwm->clk);
+	do_div(cycles, NSEC_PER_SEC);
 
-	while (div > PWM_MAX_PRD) {
-		div >>= 1;
-		pres++;
-	}
+	for (*pres = 0; cycles > PWM_MAX_PRD; cycles >>= 1)
+		(*pres)++;
 
-	if (pres > PRD_MAX_PRES) {
+	if (*pres > PRD_MAX_PRES) {
 		dev_err(chip->dev, "pres exceeds the maximum value\n");
 		return -EINVAL;
 	}
 
-	/* Calculate the duty cycles */
-	prd = div;
-	div *= duty_ns;
-	do_div(div, period_ns);
-	dty = prd - div;
-
-	ret = clk_enable(atmel_pwm->clk);
-	if (ret) {
-		dev_err(chip->dev, "failed to enable PWM clock\n");
-		return ret;
-	}
-
-	/* It is necessary to preserve CPOL, inside CMR */
-	val = atmel_pwm_ch_readl(atmel_pwm, pwm->hwpwm, PWM_CMR);
-	val = (val & ~PWM_CMR_CPRE_MSK) | (pres & PWM_CMR_CPRE_MSK);
-	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWM_CMR, val);
-	atmel_pwm->config(chip, pwm, dty, prd);
-	mutex_lock(&atmel_pwm->isr_lock);
-	atmel_pwm->updated_pwms |= atmel_pwm_readl(atmel_pwm, PWM_ISR);
-	atmel_pwm->updated_pwms &= ~(1 << pwm->hwpwm);
-	mutex_unlock(&atmel_pwm->isr_lock);
-
-	clk_disable(atmel_pwm->clk);
-	return ret;
-}
-
-static void atmel_pwm_config_v1(struct pwm_chip *chip, struct pwm_device *pwm,
-				unsigned long dty, unsigned long prd)
-{
-	struct atmel_pwm_chip *atmel_pwm = to_atmel_pwm_chip(chip);
-	unsigned int val;
-
+	*cprd = cycles;
 
-	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV1_CUPD, dty);
-
-	val = atmel_pwm_ch_readl(atmel_pwm, pwm->hwpwm, PWM_CMR);
-	val &= ~PWM_CMR_UPD_CDTY;
-	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWM_CMR, val);
-
-	/*
-	 * If the PWM channel is enabled, only update CDTY by using the update
-	 * register, it needs to set bit 10 of CMR to 0
-	 */
-	if (pwm_is_enabled(pwm))
-		return;
-	/*
-	 * If the PWM channel is disabled, write value to duty and period
-	 * registers directly.
-	 */
-	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV1_CDTY, dty);
-	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV1_CPRD, prd);
+	return 0;
 }
 
-static void atmel_pwm_config_v2(struct pwm_chip *chip, struct pwm_device *pwm,
-				unsigned long dty, unsigned long prd)
+static void atmel_pwm_calculate_cdty(const struct pwm_state *state,
+				     unsigned long cprd, unsigned long *cdty)
 {
-	struct atmel_pwm_chip *atmel_pwm = to_atmel_pwm_chip(chip);
+	unsigned long long cycles = state->duty_cycle;
 
-	if (pwm_is_enabled(pwm)) {
-		/*
-		 * If the PWM channel is enabled, using the duty update register
-		 * to update the value.
-		 */
-		atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV2_CDTYUPD, dty);
-	} else {
-		/*
-		 * If the PWM channel is disabled, write value to duty and
-		 * period registers directly.
-		 */
-		atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV2_CDTY, dty);
-		atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWMV2_CPRD, prd);
-	}
+	cycles *= cprd;
+	do_div(cycles, state->period);
+	*cdty = cprd - cycles;
 }
 
-static int atmel_pwm_set_polarity(struct pwm_chip *chip, struct pwm_device *pwm,
-				  enum pwm_polarity polarity)
+static void atmel_pwm_update_cdty(struct pwm_chip *chip, struct pwm_device *pwm,
+				  unsigned long cdty)
 {
 	struct atmel_pwm_chip *atmel_pwm = to_atmel_pwm_chip(chip);
 	u32 val;
-	int ret;
-
-	val = atmel_pwm_ch_readl(atmel_pwm, pwm->hwpwm, PWM_CMR);
 
-	if (polarity == PWM_POLARITY_NORMAL)
-		val &= ~PWM_CMR_CPOL;
-	else
-		val |= PWM_CMR_CPOL;
-
-	ret = clk_enable(atmel_pwm->clk);
-	if (ret) {
-		dev_err(chip->dev, "failed to enable PWM clock\n");
-		return ret;
+	if (atmel_pwm->regs->duty_upd ==
+	    atmel_pwm->regs->period_upd) {
+		val = atmel_pwm_ch_readl(atmel_pwm, pwm->hwpwm, PWM_CMR);
+		val &= ~PWM_CMR_UPD_CDTY;
+		atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWM_CMR, val);
 	}
 
-	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWM_CMR, val);
-
-	clk_disable(atmel_pwm->clk);
-
-	return 0;
+	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm,
+			    atmel_pwm->regs->duty_upd, cdty);
 }
 
-static int atmel_pwm_enable(struct pwm_chip *chip, struct pwm_device *pwm)
+static void atmel_pwm_set_cprd_cdty(struct pwm_chip *chip,
+				    struct pwm_device *pwm,
+				    unsigned long cprd, unsigned long cdty)
 {
 	struct atmel_pwm_chip *atmel_pwm = to_atmel_pwm_chip(chip);
-	int ret;
-
-	ret = clk_enable(atmel_pwm->clk);
-	if (ret) {
-		dev_err(chip->dev, "failed to enable PWM clock\n");
-		return ret;
-	}
 
-	atmel_pwm_writel(atmel_pwm, PWM_ENA, 1 << pwm->hwpwm);
-
-	return 0;
+	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm,
+			    atmel_pwm->regs->duty, cdty);
+	atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm,
+			    atmel_pwm->regs->period, cprd);
 }
 
-static void atmel_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
+static void atmel_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm,
+			      bool disable_clk)
 {
 	struct atmel_pwm_chip *atmel_pwm = to_atmel_pwm_chip(chip);
 	unsigned long timeout = jiffies + 2 * HZ;
@@ -282,37 +205,99 @@ static void atmel_pwm_disable(struct pwm_chip *chip, struct pwm_device *pwm)
 	       time_before(jiffies, timeout))
 		usleep_range(10, 100);
 
-	clk_disable(atmel_pwm->clk);
+	if (disable_clk)
+		clk_disable(atmel_pwm->clk);
+}
+
+static int atmel_pwm_apply(struct pwm_chip *chip, struct pwm_device *pwm,
+			   struct pwm_state *state)
+{
+	struct atmel_pwm_chip *atmel_pwm = to_atmel_pwm_chip(chip);
+	struct pwm_state cstate;
+	unsigned long cprd, cdty;
+	u32 pres, val;
+	int ret;
+
+	pwm_get_state(pwm, &cstate);
+
+	if (state->enabled) {
+		if (cstate.enabled &&
+		    cstate.polarity == state->polarity &&
+		    cstate.period == state->period) {
+			cprd = atmel_pwm_ch_readl(atmel_pwm, pwm->hwpwm,
+						  atmel_pwm->regs->period);
+			atmel_pwm_calculate_cdty(state, cprd, &cdty);
+			atmel_pwm_update_cdty(chip, pwm, cdty);
+			return 0;
+		}
+
+		ret = atmel_pwm_calculate_cprd_and_pres(chip, state, &cprd,
+							&pres);
+		if (ret) {
+			dev_err(chip->dev,
+				"failed to calculate cprd and prescaler\n");
+			return ret;
+		}
+
+		atmel_pwm_calculate_cdty(state, cprd, &cdty);
+
+		if (cstate.enabled) {
+			atmel_pwm_disable(chip, pwm, false);
+		} else {
+			ret = clk_enable(atmel_pwm->clk);
+			if (ret) {
+				dev_err(chip->dev, "failed to enable clock\n");
+				return ret;
+			}
+		}
+
+		/* It is necessary to preserve CPOL, inside CMR */
+		val = atmel_pwm_ch_readl(atmel_pwm, pwm->hwpwm, PWM_CMR);
+		val = (val & ~PWM_CMR_CPRE_MSK) | (pres & PWM_CMR_CPRE_MSK);
+		if (state->polarity == PWM_POLARITY_NORMAL)
+			val &= ~PWM_CMR_CPOL;
+		else
+			val |= PWM_CMR_CPOL;
+		atmel_pwm_ch_writel(atmel_pwm, pwm->hwpwm, PWM_CMR, val);
+		atmel_pwm_set_cprd_cdty(chip, pwm, cprd, cdty);
+		mutex_lock(&atmel_pwm->isr_lock);
+		atmel_pwm->updated_pwms |= atmel_pwm_readl(atmel_pwm, PWM_ISR);
+		atmel_pwm->updated_pwms &= ~(1 << pwm->hwpwm);
+		mutex_unlock(&atmel_pwm->isr_lock);
+		atmel_pwm_writel(atmel_pwm, PWM_ENA, 1 << pwm->hwpwm);
+	} else if (cstate.enabled) {
+		atmel_pwm_disable(chip, pwm, true);
+	}
+
+	return 0;
 }
 
 static const struct pwm_ops atmel_pwm_ops = {
-	.config = atmel_pwm_config,
-	.set_polarity = atmel_pwm_set_polarity,
-	.enable = atmel_pwm_enable,
-	.disable = atmel_pwm_disable,
+	.apply = atmel_pwm_apply,
 	.owner = THIS_MODULE,
 };
 
-struct atmel_pwm_data {
-	void (*config)(struct pwm_chip *chip, struct pwm_device *pwm,
-		       unsigned long dty, unsigned long prd);
+static const struct atmel_pwm_registers atmel_pwm_regs_v1 = {
+	.period		= PWMV1_CPRD,
+	.period_upd	= PWMV1_CUPD,
+	.duty		= PWMV1_CDTY,
+	.duty_upd	= PWMV1_CUPD,
 };
 
-static const struct atmel_pwm_data atmel_pwm_data_v1 = {
-	.config = atmel_pwm_config_v1,
-};
-
-static const struct atmel_pwm_data atmel_pwm_data_v2 = {
-	.config = atmel_pwm_config_v2,
+static const struct atmel_pwm_registers atmel_pwm_regs_v2 = {
+	.period		= PWMV2_CPRD,
+	.period_upd	= PWMV2_CPRDUPD,
+	.duty		= PWMV2_CDTY,
+	.duty_upd	= PWMV2_CDTYUPD,
 };
 
 static const struct platform_device_id atmel_pwm_devtypes[] = {
 	{
 		.name = "at91sam9rl-pwm",
-		.driver_data = (kernel_ulong_t)&atmel_pwm_data_v1,
+		.driver_data = (kernel_ulong_t)&atmel_pwm_regs_v1,
 	}, {
 		.name = "sama5d3-pwm",
-		.driver_data = (kernel_ulong_t)&atmel_pwm_data_v2,
+		.driver_data = (kernel_ulong_t)&atmel_pwm_regs_v2,
 	}, {
 		/* sentinel */
 	},
@@ -322,17 +307,20 @@ MODULE_DEVICE_TABLE(platform, atmel_pwm_devtypes);
 static const struct of_device_id atmel_pwm_dt_ids[] = {
 	{
 		.compatible = "atmel,at91sam9rl-pwm",
-		.data = &atmel_pwm_data_v1,
+		.data = &atmel_pwm_regs_v1,
 	}, {
 		.compatible = "atmel,sama5d3-pwm",
-		.data = &atmel_pwm_data_v2,
+		.data = &atmel_pwm_regs_v2,
+	}, {
+		.compatible = "atmel,sama5d2-pwm",
+		.data = &atmel_pwm_regs_v2,
 	}, {
 		/* sentinel */
 	},
 };
 MODULE_DEVICE_TABLE(of, atmel_pwm_dt_ids);
 
-static inline const struct atmel_pwm_data *
+static inline const struct atmel_pwm_registers *
 atmel_pwm_get_driver_data(struct platform_device *pdev)
 {
 	const struct platform_device_id *id;
@@ -342,18 +330,18 @@ atmel_pwm_get_driver_data(struct platform_device *pdev)
 
 	id = platform_get_device_id(pdev);
 
-	return (struct atmel_pwm_data *)id->driver_data;
+	return (struct atmel_pwm_registers *)id->driver_data;
 }
 
 static int atmel_pwm_probe(struct platform_device *pdev)
 {
-	const struct atmel_pwm_data *data;
+	const struct atmel_pwm_registers *regs;
 	struct atmel_pwm_chip *atmel_pwm;
 	struct resource *res;
 	int ret;
 
-	data = atmel_pwm_get_driver_data(pdev);
-	if (!data)
+	regs = atmel_pwm_get_driver_data(pdev);
+	if (!regs)
 		return -ENODEV;
 
 	atmel_pwm = devm_kzalloc(&pdev->dev, sizeof(*atmel_pwm), GFP_KERNEL);
@@ -385,7 +373,7 @@ static int atmel_pwm_probe(struct platform_device *pdev)
 
 	atmel_pwm->chip.base = -1;
 	atmel_pwm->chip.npwm = 4;
-	atmel_pwm->config = data->config;
+	atmel_pwm->regs = regs;
 	atmel_pwm->updated_pwms = 0;
 	mutex_init(&atmel_pwm->isr_lock);