[PATCH] leds: rgb: leds-qcom-lpg: Add PWM frequency mode support

Certain PWM channels on a PMIC (e.g. PM8350C PWM4) support a Frequency
Mode (FM) that can generate waveforms with more frequency points than
the standard LPG PWM mode. The trade-off is that the duty cycle can
only be fixed at 50%. Add the FM support. When the PWM channel is
requested to set a duty cycle to exactly 50%, use FM mode by default
as it provides a finer-grained frequency resolution in that case.

Signed-off-by: Fenglin Wu <[email protected]>
---
Dependency:

This change was made based on the LED color balance fix change which
is under review and has not yet been merged:

https://lore.kernel.org/linux-arm-msm/20260716-lpg-rgb-color-balance-fix-v6-1-b49d51528f61@oss.qualcomm.com/

This change should be applied on top of that one.
---
 drivers/leds/rgb/leds-qcom-lpg.c | 199 +++++++++++++++++++++++++++++++++++----
 1 file changed, 183 insertions(+), 16 deletions(-)

diff --git a/drivers/leds/rgb/leds-qcom-lpg.c b/drivers/leds/rgb/leds-qcom-lpg.c
index 24b1f570f524..be3398b60445 100644
--- a/drivers/leds/rgb/leds-qcom-lpg.c
+++ b/drivers/leds/rgb/leds-qcom-lpg.c
@@ -21,6 +21,8 @@
 #define  LPG_SUBTYPE_PWM	0xb
 #define  LPG_SUBTYPE_HI_RES_PWM	0xc
 #define  LPG_SUBTYPE_LPG_LITE	0x11
+#define PWM_STATUS1_REG		0x08
+#define  PWM_FM_PRESENT		BIT(0)
 #define LPG_PATTERN_CONFIG_REG	0x40
 #define LPG_SIZE_CLK_REG	0x41
 #define  PWM_CLK_SELECT_MASK	GENMASK(1, 0)
@@ -42,6 +44,9 @@
 #define PWM_SEC_ACCESS_REG	0xd0
 #define PWM_DTEST_REG(x)	(0xe2 + (x) - 1)
 
+#define PWM_FM_MODE_REG		0x50
+#define  PWM_FM_ENABLE		BIT(7)
+
 #define SDAM_REG_PBS_SEQ_EN		0x42
 #define SDAM_PBS_TRIG_SET		0xe5
 #define SDAM_PBS_TRIG_CLR		0xe6
@@ -110,6 +115,8 @@ struct lpg_data;
  * @ramp_hi_pause_ms: pause (in milliseconds) after iterating over pattern
  * @pattern_lo_idx: start index of associated pattern
  * @pattern_hi_idx: last index of associated pattern
+ * @fm_capable: hardware supports Frequency Mode
+ * @use_fm: set the period using Frequency Mode
  */
 struct lpg_channel {
 	struct lpg *lpg;
@@ -146,6 +153,9 @@ struct lpg_channel {
 
 	unsigned int pattern_lo_idx;
 	unsigned int pattern_hi_idx;
+
+	bool fm_capable;
+	bool use_fm;
 };
 
 /**
@@ -238,11 +248,13 @@ struct lpg {
  * @sdam_offset:	Channel offset in LPG SDAM
  * @base:		base address for PWM channel registers
  * @triled_mask:	bitmask for controlling this channel in TRILED
+ * @fm_capable:		channel hardware supports Frequency Mode
  */
 struct lpg_channel_data {
 	unsigned int sdam_offset;
 	unsigned int base;
 	u8 triled_mask;
+	bool fm_capable;
 };
 
 /**
@@ -431,10 +443,105 @@ static int lpg_lut_sync(struct lpg *lpg, unsigned int mask)
 
 static const unsigned int lpg_clk_rates[] = {0, 1024, 32768, 19200000};
 static const unsigned int lpg_clk_rates_hi_res[] = {0, 1024, 32768, 19200000, 76800000};
+static const unsigned int lpg_clk_period_ns[] = {0, 976562, 30517, 52};
 static const unsigned int lpg_pre_divs[] = {1, 3, 5, 6};
 static const unsigned int lpg_pwm_resolution[] = {6, 9};
 static const unsigned int lpg_pwm_resolution_hi_res[] = {8, 9, 10, 11, 12, 13, 14, 15};
 
+static int lpg_calc_freq_fm(struct lpg_channel *chan, uint64_t period_ns)
+{
+	const unsigned int *clk_rate_arr, *clk_period_arr;
+	unsigned int best_clk = 0, best_exp = 0, best_lsb = 0;
+	unsigned int clk, exp, lsb;
+	unsigned int clk_len;
+	u64 lsb_tmp, period_actual;
+	u64 curr_err, last_err;
+	u64 min_err = U64_MAX;
+	bool found = false;
+
+	if (chan->subtype != LPG_SUBTYPE_PWM) {
+		dev_err(chan->lpg->dev, "Only SUBTYPE_PWM support frequency mode\n");
+		return -EOPNOTSUPP;
+	}
+
+	clk_rate_arr = lpg_clk_rates;
+	clk_len = ARRAY_SIZE(lpg_clk_rates);
+	clk_period_arr = lpg_clk_period_ns;
+
+	/*
+	 * Formula (rearranged to solve for pwm_value_lsb):
+	 *
+	 *                           period_ns
+	 * pwm_value_lsb = -------------------------------  - 1
+	 *                 2 * (2^pwm_exp) * clk_period_ns
+	 *
+	 * For each (clk, exp) combination, calculate pwm_value_lsb and then
+	 * use it to calculate the actual period. Store the combination that
+	 * yields the closest match to the desired period.
+	 *
+	 */
+
+	for (clk = 1; clk < clk_len; clk++) {
+		last_err = U64_MAX;
+
+		for (exp = 0; exp <= LPG_MAX_M; exp++) {
+			/* Calculate pwm_value_lsb for this (clk, exp) pair */
+			lsb_tmp = period_ns;
+			lsb_tmp = div64_u64(lsb_tmp, clk_period_arr[clk]);
+			lsb_tmp >>= (exp + 1);
+
+			if (lsb_tmp == 0 || lsb_tmp - 1 > U8_MAX)
+				continue;
+
+			lsb = lsb_tmp - 1;
+
+			period_actual = (u64)(lsb + 1);
+			period_actual <<= (exp + 1);
+			period_actual *= clk_period_arr[clk];
+
+			curr_err = period_ns - period_actual;
+			if (curr_err < min_err) {
+				min_err = curr_err;
+				best_clk = clk;
+				best_exp = exp;
+				best_lsb = lsb;
+				found = true;
+			}
+
+			if (curr_err > last_err)
+				break;
+
+			last_err = curr_err;
+		}
+	}
+
+	if (!found) {
+		dev_err(chan->lpg->dev,
+			"FM: Cannot generate period %llu ns\n", period_ns);
+		return -EINVAL;
+	}
+
+	chan->clk_sel = best_clk;
+	chan->pre_div_exp = best_exp;
+	chan->pwm_value = best_lsb;
+
+	chan->pre_div_sel = 0;
+	chan->pwm_resolution_sel = 0;
+
+	/* Calculate actual period for reference */
+	period_actual = (u64)(best_lsb + 1);
+	period_actual <<= (best_exp + 1);
+	period_actual *= clk_period_arr[best_clk];
+	chan->period = period_actual;
+
+	dev_dbg(chan->lpg->dev,
+		"Frequency mode: period=%llu ns -> clk=%u Hz (idx=%u), exp=%u, lsb=%u (actual=%llu ns, err=%llu ns)\n",
+		period_ns, clk_rate_arr[best_clk], best_clk, best_exp, best_lsb,
+		period_actual, min_err);
+
+	return 0;
+}
+
 static int lpg_calc_freq(struct lpg_channel *chan, uint64_t period)
 {
 	unsigned int i, pwm_resolution_count, best_pwm_resolution_sel = 0;
@@ -802,11 +909,23 @@ static void lpg_apply_dtest(struct lpg_channel *chan)
 		     chan->dtest_value);
 }
 
+static void lpg_apply_frequency_mode(struct lpg_channel *chan)
+{
+	struct lpg *lpg = chan->lpg;
+
+	if (!chan->fm_capable)
+		return;
+
+	regmap_write(lpg->map, chan->base + PWM_FM_MODE_REG,
+		     chan->use_fm ? PWM_FM_ENABLE : 0);
+}
+
 static void lpg_apply(struct lpg_channel *chan)
 {
 	lpg_disable_glitch(chan);
 	lpg_apply_freq(chan);
 	lpg_apply_pwm_value(chan);
+	lpg_apply_frequency_mode(chan);
 	lpg_apply_control(chan);
 	lpg_apply_sync(chan);
 	if (chan->lpg->lpg_chan_sdam)
@@ -1318,28 +1437,42 @@ static int lpg_pwm_apply(struct pwm_chip *chip, struct pwm_device *pwm,
 {
 	struct lpg *lpg = lpg_pwm_from_chip(chip);
 	struct lpg_channel *chan = &lpg->channels[pwm->hwpwm];
+	bool use_fm = false;
 	int ret = 0;
 
 	if (state->polarity != PWM_POLARITY_NORMAL)
 		return -EINVAL;
 
-	mutex_lock(&lpg->lock);
+	guard(mutex)(&lpg->lock);
 
 	if (state->enabled) {
-		ret = lpg_calc_freq(chan, state->period);
-		if (ret < 0)
-			goto out_unlock;
+		use_fm = chan->fm_capable && (state->duty_cycle == state->period / 2);
 
-		lpg_calc_duty(chan, state->duty_cycle);
+		if (use_fm) {
+			ret = lpg_calc_freq_fm(chan, state->period);
+			/*
+			 * fallback to use the standard mode if frequency
+			 * mode couldn't satisfy the requested period
+			 */
+			if (ret < 0)
+				use_fm = false;
+		}
+
+		if (!use_fm) {
+			ret = lpg_calc_freq(chan, state->period);
+			if (ret < 0)
+				return ret;
+
+			lpg_calc_duty(chan, state->duty_cycle);
+		}
 	}
+
+	chan->use_fm = use_fm;
 	chan->enabled = state->enabled;
 
 	lpg_apply(chan);
 
-out_unlock:
-	mutex_unlock(&lpg->lock);
-
-	return ret;
+	return 0;
 }
 
 static int lpg_pwm_get_state(struct pwm_chip *chip, struct pwm_device *pwm,
@@ -1347,8 +1480,10 @@ static int lpg_pwm_get_state(struct pwm_chip *chip, struct pwm_device *pwm,
 {
 	struct lpg *lpg = lpg_pwm_from_chip(chip);
 	struct lpg_channel *chan = &lpg->channels[pwm->hwpwm];
+	bool fm_active = false;
 	unsigned int resolution;
 	unsigned int pre_div;
+	unsigned int clk_idx;
 	unsigned int refclk;
 	unsigned int val;
 	unsigned int m;
@@ -1360,7 +1495,7 @@ static int lpg_pwm_get_state(struct pwm_chip *chip, struct pwm_device *pwm,
 		return ret;
 
 	if (chan->subtype == LPG_SUBTYPE_HI_RES_PWM) {
-		unsigned int clk_idx = FIELD_GET(PWM_CLK_SELECT_HI_RES_MASK, val);
+		clk_idx = FIELD_GET(PWM_CLK_SELECT_HI_RES_MASK, val);
 
 		if (clk_idx >= ARRAY_SIZE(lpg_clk_rates_hi_res))
 			return -EINVAL;
@@ -1368,7 +1503,8 @@ static int lpg_pwm_get_state(struct pwm_chip *chip, struct pwm_device *pwm,
 		refclk = lpg_clk_rates_hi_res[clk_idx];
 		resolution = lpg_pwm_resolution_hi_res[FIELD_GET(PWM_SIZE_HI_RES_MASK, val)];
 	} else {
-		refclk = lpg_clk_rates[FIELD_GET(PWM_CLK_SELECT_MASK, val)];
+		clk_idx = FIELD_GET(PWM_CLK_SELECT_MASK, val);
+		refclk = lpg_clk_rates[clk_idx];
 		resolution = lpg_pwm_resolution[FIELD_GET(PWM_SIZE_SELECT_MASK, val)];
 	}
 
@@ -1384,9 +1520,32 @@ static int lpg_pwm_get_state(struct pwm_chip *chip, struct pwm_device *pwm,
 		if (ret)
 			return ret;
 
-		state->period = DIV_ROUND_UP_ULL((u64)NSEC_PER_SEC * ((1 << resolution) - 1) *
-						 pre_div * (1 << m), refclk);
-		state->duty_cycle = DIV_ROUND_UP_ULL((u64)NSEC_PER_SEC * pwm_value * pre_div * (1 << m), refclk);
+		if (chan->fm_capable) {
+			ret = regmap_read(lpg->map, chan->base + PWM_FM_MODE_REG, &val);
+			if (ret)
+				return ret;
+
+			fm_active = !!(val & PWM_FM_ENABLE);
+		}
+
+		if (fm_active) {
+			if (clk_idx >= ARRAY_SIZE(lpg_clk_period_ns))
+				return -EINVAL;
+
+			/*
+			 * FM period formula:
+			 *   period = 2 * (pwm_value_lsb + 1) * (2^exp) * clk_period_ns
+			 */
+			pwm_value &= 0xff;
+			state->period = (u64)2 * (pwm_value + 1) * (1 << m) *
+					lpg_clk_period_ns[clk_idx];
+			state->duty_cycle = state->period / 2;
+		} else {
+			state->period = DIV_ROUND_UP_ULL((u64)NSEC_PER_SEC *
+					((1 << resolution) - 1) * pre_div * (1 << m), refclk);
+			state->duty_cycle = DIV_ROUND_UP_ULL((u64)NSEC_PER_SEC *
+					pwm_value * pre_div * (1 << m), refclk);
+		}
 	} else {
 		state->period = 0;
 		state->duty_cycle = 0;
@@ -1560,7 +1719,8 @@ static int lpg_init_channels(struct lpg *lpg)
 {
 	const struct lpg_data *data = lpg->data;
 	struct lpg_channel *chan;
-	int i;
+	unsigned int status;
+	int ret, i;
 
 	for (i = 0; i < data->num_channels; i++) {
 		chan = &lpg->channels[i];
@@ -1572,6 +1732,13 @@ static int lpg_init_channels(struct lpg *lpg)
 		chan->sdam_offset = data->channels[i].sdam_offset;
 
 		regmap_read(lpg->map, chan->base + LPG_SUBTYPE_REG, &chan->subtype);
+		if (data->channels[i].fm_capable) {
+			ret = regmap_read(lpg->map, chan->base + PWM_STATUS1_REG, &status);
+			if (ret < 0)
+				return ret;
+
+			chan->fm_capable = !!(status & PWM_FM_PRESENT);
+		}
 	}
 
 	return 0;
@@ -1895,7 +2062,7 @@ static const struct lpg_data pm8350c_pwm_data = {
 		{ .base = 0xe800, .triled_mask = BIT(7), .sdam_offset = 0x48 },
 		{ .base = 0xe900, .triled_mask = BIT(6), .sdam_offset = 0x56 },
 		{ .base = 0xea00, .triled_mask = BIT(5), .sdam_offset = 0x64 },
-		{ .base = 0xeb00 },
+		{ .base = 0xeb00, .fm_capable = true },
 	},
 };
 

---
base-commit: 1795fd2dbe84ef4d393b69a0b2a3b371f810bde5
change-id: 20260728-lpg-pwm-fm-support-5f0a4636d3dc
prerequisite-change-id: 20260605-lpg-rgb-color-balance-fix-82436649abf3:v6
prerequisite-patch-id: 931a69abc7b07f7f5b5603e8159c092d9a54703f

Best regards,
--  
Fenglin Wu <[email protected]>