WIP: pwm: New functions for rounding state

2 files changed, 218 insertions, 0 deletions

diff --git a/drivers/pwm/core.c b/drivers/pwm/core.c
index cbba6eccad90..c43c63313619 100644
--- a/drivers/pwm/core.c
+++ b/drivers/pwm/core.c
@@ -421,6 +421,220 @@ void pwm_free(struct pwm_device *pwm)
 }
 EXPORT_SYMBOL_GPL(pwm_free);
 
+void pwm_round_down_state_debug(struct pwm_chip *chip,
+				const struct pwm_state *pre, const struct pwm_state *post)
+{
+	if (!IS_ENABLED(CONFIG_PWM_DEBUG))
+		return;
+
+	if (pre->polarity != post->polarity) {
+		dev_warn(chip->dev, ".round_down is supposed to not modify polarity\n");
+		return;
+	}
+
+	/*
+	 * period and duty_cycle don't matter for the emitted wave form if the
+	 * PWM is off, so skip further tests.
+	 */
+	if (!post->enabled)
+		return;
+
+	/*
+	 * If disabled was requested returning an enabled setting is fine if
+	 * .duty_cycle is zero
+	 */
+	if (!pre->enabled) {
+		if (post->enabled && post->duty_cycle > 0)
+			dev_warn(chip->dev,
+				 ".round_down returned an enabled state when disabled was requested\n");
+
+		return;
+	}
+
+	if (pre->period < post->period) {
+		dev_warn(chip->dev, ".round_down didn't round down period\n");
+		return;
+	}
+
+	if (pre->duty_cycle < post->duty_cycle) {
+		dev_warn(chip->dev, ".round_down didn't round down duty_cycle\n");
+		return;
+	}
+}
+
+/*
+ * pwm_round_down_state() - round down state to the next implementable state
+ * @pwm: PWM device
+ * @state: state to round, also contains the rounded state on successful return
+ *
+ * Typically PWMs cannot implement a state passed to pwm_apply_state() exactly
+ * and so have to program a state "near" the requested one.
+ * pwm_round_down_state() returns the following "near" state with the same
+ * polarity that can actually be implemented by the given hardware:
+ *
+ *  - state->period is the biggest period not bigger than the input period
+ *  - state->duty_cycle is the biggest duty cycle not bigger than the input
+ *    duty_cycle that is possible with the picked period.
+ */
+int pwm_round_down_state(struct pwm_device *pwm, struct pwm_state *state)
+{
+	struct pwm_chip *chip;
+	struct pwm_state pre;
+	int ret;
+
+	if (!pwm || !state || !state->period ||
+	    state->duty_cycle > state->period)
+		return -EINVAL;
+
+	chip = pwm->chip;
+	pre = *state;
+
+	if (!chip->ops->round_down_state)
+		return -ENOSYS;
+
+	ret = chip->ops->round_down_state(chip, pwm, state);
+
+	if (!ret)
+		pwm_round_down_state_debug(chip, &pre, state);
+
+	return ret;
+}
+
+/*
+ * pwm_round_nearest_state() - round nearest state to the next implementable state
+ * @pwm: PWM device
+ * @state: state to round, also contains the rounded state on successful return
+ *
+ * Typically PWMs cannot implement a state passed to pwm_apply_state() exactly
+ * and so have to program a state "near" the requested one.
+ * pwm_round_nearest_state() returns the following "near" state with the same
+ * polarity that can actually be implemented by the given hardware:
+ *
+ *  - state->period is the period nearest to the requested one
+ *  - state->duty_cycle is the duty cycle nearest to the input
+ *    duty_cycle that is possible with the picked period.
+ *
+ * Note that when the actually implemented values are not integer this algorithm
+ * assumes that .apply() rounds down and so if to hit period P P+x and P-x are
+ * possible choices (for x > 0), it prefers P+x.
+ */
+int pwm_round_nearest_state(struct pwm_device *pwm, struct pwm_state *state)
+{
+	struct pwm_state s, s_down, s_up;
+	int ret;
+
+	/*
+	 * First determine the nearest period. To be sure that if the first call
+	 * to pwm_round_down_state fails this is because of an impossible period
+	 * and not an impossible duty_cycle, use duty_cycle = period here.
+	 */
+	s_down = *state;
+	s_down.duty_cycle = s_down.period;
+
+	ret = pwm_round_down_state(pwm, &s_down);
+	if (ret) {
+		/*
+		 * XXX: actually we're not done here, instead we have to find
+		 * the smallest implementable period. To be implemented ..
+		 */
+		return ret;
+	}
+
+	/*
+	 * Sanity check: s_down.period must not be bigger than state->period.
+	 * Otherwise our maths go wrong and .round_down_state() is buggy.
+	 */
+	if (s_down.period > state->period)
+		return -EINVAL;
+
+	/*
+	 * s_down.period is the nearest implementable period below the
+	 * requested period. Now check if there is a nearer period above
+	 * the requested period.
+	 */
+	s_up = s_down;
+
+	if (U64_MAX - state->period >= state->period - s_down.period)
+		s_up.period = state->period + (state->period - s_down.period);
+	else
+		s_up.period = U64_MAX;
+
+	s_up.duty_cycle = s_up.period;
+	ret = pwm_round_down_state(pwm, &s_up);
+	if (ret) {
+		/*
+		 * This should not happen because s_down is a valid
+		 * result.
+		 */
+		return ret;
+	}
+
+	/* There might be further values between state->period and s_up.period. */
+	s = s_up;
+	while (s.period > state->period) {
+		s_up = s;
+		s.period = s.period - 1;
+		s.duty_cycle = s.period;
+		ret = pwm_round_down_state(pwm, &s);
+		if (ret)
+			/* As above, this should not happen. */
+			return ret;
+	}
+
+	/*
+	 * Now s_up.period contains the period nearest to state->period.
+	 * Do the same iteration to find the nearest duty_cycle.
+	 */
+
+	s_down = s_up;
+	s_down.duty_cycle = min(state->duty_cycle, s_down.period);
+	ret = pwm_round_down_state(pwm, &s_down);
+	if (ret)
+		/*
+		 * XXX: actually we're not done here, instead we have to find
+		 * the smallest implementable duty_cycle. To be implemented ..
+		 */
+		return ret;
+
+	/*
+	 * Sanity check: The period was determined above to be implementable, so
+	 * it should be returned unmodified here.
+	 */
+	if (s_down.period != s_up.period)
+		return -EINVAL;
+
+	/*
+	 * Sanity check: s_down.duty_cycle must not be bigger than
+	 * state->duty_cycle. Otherwise our maths go wrong and
+	 * .round_down_state() is buggy.
+	 */
+	if (s_down.period > state->period)
+		return -EINVAL;
+
+	s_up = s_down;
+	if (s_up.period > state->duty_cycle &&
+	    s_up.period - state->duty_cycle >= state->duty_cycle - s_down.duty_cycle)
+		s_up.duty_cycle = state->duty_cycle + (state->duty_cycle - s_down.duty_cycle);
+	else
+		s_up.duty_cycle = s_up.period;
+
+	ret = pwm_round_down_state(pwm, &s_up);
+	if (ret)
+		return ret;
+
+	s = s_up;
+	while (s.duty_cycle > state->duty_cycle) {
+		s_up = s;
+		s.duty_cycle = s.duty_cycle - 1;
+		ret = pwm_round_down_state(pwm, &s);
+		if (ret)
+			return ret;
+	}
+
+	*state = s_up;
+	return 0;
+}
+
 static void pwm_apply_state_debug(struct pwm_device *pwm,
 				  const struct pwm_state *state)
 {
diff --git a/include/linux/pwm.h b/include/linux/pwm.h
index e6dac95e4960..450fcfb3fcbc 100644
--- a/include/linux/pwm.h
+++ b/include/linux/pwm.h
@@ -273,6 +273,8 @@ struct pwm_ops {
 		       struct pwm_capture *result, unsigned long timeout);
 	int (*apply)(struct pwm_chip *chip, struct pwm_device *pwm,
 		     const struct pwm_state *state);
+	int (*round_down_state)(struct pwm_chip *chip, struct pwm_device *pwm,
+				struct pwm_state *state);
 	void (*get_state)(struct pwm_chip *chip, struct pwm_device *pwm,
 			  struct pwm_state *state);
 	struct module *owner;
@@ -326,6 +328,8 @@ struct pwm_capture {
 /* PWM user APIs */
 struct pwm_device *pwm_request(int pwm_id, const char *label);
 void pwm_free(struct pwm_device *pwm);
+int pwm_round_down_state(struct pwm_device *pwm, struct pwm_state *state);
+int pwm_round_nearest_state(struct pwm_device *pwm, struct pwm_state *state);
 int pwm_apply_state(struct pwm_device *pwm, const struct pwm_state *state);
 int pwm_adjust_config(struct pwm_device *pwm);