leds: qpnp-flash-v2: Calculate the IRES current dynamically

Fix the out of bound ITARGET register programming by calculating the
LED_IRESOLUTION current dynamically based on the flash current requested
by the client.

CRs-Fixed: 2066263
Change-Id: Ifba232ab4af5493feef53e331bccdc55c6109246
Signed-off-by: Kiran Gunda <kgunda@codeaurora.org>

1 files changed, 36 insertions, 7 deletions

diff --git a/drivers/leds/leds-qpnp-flash-v2.c b/drivers/leds/leds-qpnp-flash-v2.c
index 564d20079715..15c931bbbf65 100644
--- a/drivers/leds/leds-qpnp-flash-v2.c
+++ b/drivers/leds/leds-qpnp-flash-v2.c
@@ -158,6 +158,11 @@
 #define	FLASH_LED_DISABLE			0x00
 #define	FLASH_LED_SAFETY_TMR_DISABLED		0x13
 #define	FLASH_LED_MAX_TOTAL_CURRENT_MA		3750
+#define	FLASH_LED_IRES5P0_MAX_CURR_MA		640
+#define	FLASH_LED_IRES7P5_MAX_CURR_MA		960
+#define	FLASH_LED_IRES10P0_MAX_CURR_MA		1280
+#define	FLASH_LED_IRES12P5_MAX_CURR_MA		1600
+#define	MAX_IRES_LEVELS				4
 
 /* notifier call chain for flash-led irqs */
 static ATOMIC_NOTIFIER_HEAD(irq_notifier_list);
@@ -196,13 +201,15 @@ struct flash_node_data {
 	struct pinctrl_state		*hw_strobe_state_suspend;
 	int				hw_strobe_gpio;
 	int				ires_ua;
+	int				default_ires_ua;
 	int				max_current;
 	int				current_ma;
 	int				prev_current_ma;
 	u8				duration;
 	u8				id;
 	u8				type;
-	u8				ires;
+	u8				ires_idx;
+	u8				default_ires_idx;
 	u8				hdrm_val;
 	u8				current_reg_val;
 	u8				strobe_ctrl;
@@ -305,6 +312,11 @@ static int otst3_threshold_table[] = {
 	125, 119, 113, 107, 149, 143, 137, 131,
 };
 
+static int max_ires_curr_ma_table[MAX_IRES_LEVELS] = {
+	FLASH_LED_IRES12P5_MAX_CURR_MA, FLASH_LED_IRES10P0_MAX_CURR_MA,
+	FLASH_LED_IRES7P5_MAX_CURR_MA, FLASH_LED_IRES5P0_MAX_CURR_MA
+};
+
 static int qpnp_flash_led_read(struct qpnp_flash_led *led, u16 addr, u8 *data)
 {
 	int rc;
@@ -935,6 +947,7 @@ static void qpnp_flash_led_aggregate_max_current(struct flash_node_data *fnode)
 
 static void qpnp_flash_led_node_set(struct flash_node_data *fnode, int value)
 {
+	int i = 0;
 	int prgm_current_ma = value;
 	int min_ma = fnode->ires_ua / 1000;
 	struct qpnp_flash_led *led = dev_get_drvdata(&fnode->pdev->dev);
@@ -944,7 +957,22 @@ static void qpnp_flash_led_node_set(struct flash_node_data *fnode, int value)
 	else if (value < min_ma)
 		prgm_current_ma = min_ma;
 
+	fnode->ires_idx = fnode->default_ires_idx;
+	fnode->ires_ua = fnode->default_ires_ua;
+
 	prgm_current_ma = min(prgm_current_ma, fnode->max_current);
+	if (prgm_current_ma > max_ires_curr_ma_table[fnode->ires_idx]) {
+		/* find the matching ires */
+		for (i = MAX_IRES_LEVELS - 1; i >= 0; i--) {
+			if (prgm_current_ma <= max_ires_curr_ma_table[i]) {
+				fnode->ires_idx = i;
+				fnode->ires_ua = FLASH_LED_IRES_MIN_UA +
+				      (FLASH_LED_IRES_BASE - fnode->ires_idx) *
+				      FLASH_LED_IRES_DIVISOR;
+				break;
+			}
+		}
+	}
 	fnode->current_ma = prgm_current_ma;
 	fnode->cdev.brightness = prgm_current_ma;
 	fnode->current_reg_val = CURRENT_MA_TO_REG_VAL(prgm_current_ma,
@@ -1062,7 +1090,7 @@ static int qpnp_flash_led_switch_set(struct flash_switch_data *snode, bool on)
 	val = 0;
 	for (i = 0; i < led->num_fnodes; i++)
 		if (snode->led_mask & BIT(led->fnode[i].id))
-			val |= led->fnode[i].ires << (led->fnode[i].id * 2);
+			val |= led->fnode[i].ires_idx << (led->fnode[i].id * 2);
 
 	rc = qpnp_flash_led_masked_write(led, FLASH_LED_REG_IRES(led->base),
 						FLASH_LED_CURRENT_MASK, val);
@@ -1434,13 +1462,14 @@ static int qpnp_flash_led_parse_each_led_dt(struct qpnp_flash_led *led,
 		return rc;
 	}
 
-	fnode->ires_ua = FLASH_LED_IRES_DEFAULT_UA;
-	fnode->ires = FLASH_LED_IRES_DEFAULT_VAL;
+	fnode->default_ires_ua = fnode->ires_ua = FLASH_LED_IRES_DEFAULT_UA;
+	fnode->default_ires_idx = fnode->ires_idx = FLASH_LED_IRES_DEFAULT_VAL;
 	rc = of_property_read_u32(node, "qcom,ires-ua", &val);
 	if (!rc) {
-		fnode->ires_ua = val;
-		fnode->ires = FLASH_LED_IRES_BASE -
-			(val - FLASH_LED_IRES_MIN_UA) / FLASH_LED_IRES_DIVISOR;
+		fnode->default_ires_ua = fnode->ires_ua = val;
+		fnode->default_ires_idx = fnode->ires_idx =
+			FLASH_LED_IRES_BASE - (val - FLASH_LED_IRES_MIN_UA) /
+			FLASH_LED_IRES_DIVISOR;
 	} else if (rc != -EINVAL) {
 		pr_err("Unable to read current resolution rc=%d\n", rc);
 		return rc;