diff --git a/drivers/leds/leds-qpnp-wled.c b/drivers/leds/leds-qpnp-wled.c
index 484c843..d2e576d 100644
--- a/drivers/leds/leds-qpnp-wled.c
+++ b/drivers/leds/leds-qpnp-wled.c
@@ -316,6 +316,7 @@
  *  @ cdev - led class device
  *  @ pdev - platform device
  *  @ work - worker for led operation
+ *  @ wq - workqueue for setting brightness level
  *  @ lock - mutex lock for exclusive access
  *  @ fdbk_op - output feedback mode
  *  @ dim_mode - dimming mode
@@ -359,6 +360,7 @@
  *  @ disp_type_amoled - type of display: LCD/AMOLED
  *  @ en_ext_pfet_sc_pro - enable sc protection on external pfet
  *  @ prev_state - previous state of WLED
+ *  @ stepper_en - Flag to enable stepper algorithm
  *  @ ovp_irq_disabled - OVP interrupt disable status
  *  @ auto_calib_enabled - Flag to enable auto calibration feature
  *  @ auto_calib_done - Flag to indicate auto calibration is done
@@ -371,6 +373,7 @@
 	struct regmap		*regmap;
 	struct pmic_revid_data	*pmic_rev_id;
 	struct work_struct	work;
+	struct workqueue_struct *wq;
 	struct mutex		lock;
 	struct mutex		bus_lock;
 	enum qpnp_wled_fdbk_op	fdbk_op;
@@ -415,6 +418,7 @@
 	bool			disp_type_amoled;
 	bool			en_ext_pfet_sc_pro;
 	bool			prev_state;
+	bool			stepper_en;
 	bool			ovp_irq_disabled;
 	bool			auto_calib_enabled;
 	bool			auto_calib_done;
@@ -422,6 +426,21 @@
 	ktime_t			start_ovp_fault_time;
 };
 
+static int qpnp_wled_step_delay_us = 52000;
+module_param_named(
+	total_step_delay_us, qpnp_wled_step_delay_us, int, 0600
+);
+
+static int qpnp_wled_step_size_threshold = 3;
+module_param_named(
+	step_size_threshold, qpnp_wled_step_size_threshold, int, 0600
+);
+
+static int qpnp_wled_step_delay_gain = 2;
+module_param_named(
+	step_delay_gain, qpnp_wled_step_delay_gain, int, 0600
+);
+
 /* helper to read a pmic register */
 static int qpnp_wled_read_reg(struct qpnp_wled *wled, u16 addr, u8 *data)
 {
@@ -583,6 +602,7 @@
 		return rc;
 	}
 
+	pr_debug("level:%d\n", level);
 	return 0;
 }
 
@@ -613,6 +633,65 @@
 	return 0;
 }
 
+static int qpnp_wled_set_step_level(struct qpnp_wled *wled, int new_level)
+{
+	int rc, i, num_steps, delay_us;
+	u16 level, start_level, end_level, step_size;
+	bool level_inc = false;
+
+	level = wled->prev_level;
+	start_level = wled->brt_map_table[level];
+	end_level = wled->brt_map_table[new_level];
+	level_inc = (new_level > level);
+
+	num_steps = abs(start_level - end_level);
+	if (!num_steps)
+		return 0;
+
+	delay_us = qpnp_wled_step_delay_us / num_steps;
+	pr_debug("level goes from [%d %d] num_steps: %d, delay: %d\n",
+		start_level, end_level, num_steps, delay_us);
+
+	if (delay_us < 500) {
+		step_size = 1000 / delay_us;
+		num_steps = num_steps / step_size;
+		delay_us = 1000;
+	} else {
+		if (num_steps < qpnp_wled_step_size_threshold)
+			delay_us *= qpnp_wled_step_delay_gain;
+
+		step_size = 1;
+	}
+
+	i = start_level;
+	while (num_steps--) {
+		if (level_inc)
+			i += step_size;
+		else
+			i -= step_size;
+
+		rc = qpnp_wled_set_level(wled, i);
+		if (rc < 0)
+			return rc;
+
+		if (delay_us > 0) {
+			if (delay_us < 20000)
+				usleep_range(delay_us, delay_us + 1);
+			else
+				msleep(delay_us / USEC_PER_MSEC);
+		}
+	}
+
+	if (i != end_level) {
+		i = end_level;
+		rc = qpnp_wled_set_level(wled, i);
+		if (rc < 0)
+			return rc;
+	}
+
+	return 0;
+}
+
 static int qpnp_wled_psm_config(struct qpnp_wled *wled, bool enable)
 {
 	int rc;
@@ -999,7 +1078,10 @@
 			level_255 = 255;
 
 		pr_debug("level: %d level_255: %d\n", level, level_255);
-		rc = qpnp_wled_set_map_level(wled, level_255);
+		if (wled->stepper_en)
+			rc = qpnp_wled_set_step_level(wled, level_255);
+		else
+			rc = qpnp_wled_set_map_level(wled, level_255);
 		if (rc) {
 			dev_err(&wled->pdev->dev, "wled set level failed\n");
 			goto unlock_mutex;
@@ -1064,7 +1146,7 @@
 		level = wled->cdev.max_brightness;
 
 	wled->cdev.brightness = level;
-	schedule_work(&wled->work);
+	queue_work(wled->wq, &wled->work);
 }
 
 static int qpnp_wled_set_disp(struct qpnp_wled *wled, u16 base_addr)
@@ -2226,6 +2308,8 @@
 		}
 	}
 
+	wled->stepper_en = of_property_read_bool(pdev->dev.of_node,
+				"qcom,wled-stepper-en");
 	wled->disp_type_amoled = of_property_read_bool(pdev->dev.of_node,
 				"qcom,disp-type-amoled");
 	if (wled->disp_type_amoled) {
@@ -2562,6 +2646,7 @@
 	}
 
 	wled->pmic_rev_id = get_revid_data(revid_node);
+	of_node_put(revid_node);
 	if (IS_ERR_OR_NULL(wled->pmic_rev_id)) {
 		pr_err("Unable to get pmic_revid rc=%ld\n",
 			PTR_ERR(wled->pmic_rev_id));
@@ -2576,6 +2661,12 @@
 	pr_debug("PMIC subtype %d Digital major %d\n",
 		wled->pmic_rev_id->pmic_subtype, wled->pmic_rev_id->rev4);
 
+	wled->wq = alloc_ordered_workqueue("qpnp_wled_wq", WQ_HIGHPRI);
+	if (!wled->wq) {
+		pr_err("Unable to alloc workqueue for WLED\n");
+		return -ENOMEM;
+	}
+
 	prop = of_get_address_by_name(pdev->dev.of_node, QPNP_WLED_SINK_BASE,
 			NULL, NULL);
 	if (!prop) {
@@ -2641,6 +2732,7 @@
 	led_classdev_unregister(&wled->cdev);
 wled_register_fail:
 	cancel_work_sync(&wled->work);
+	destroy_workqueue(wled->wq);
 	mutex_destroy(&wled->lock);
 	return rc;
 }
@@ -2656,6 +2748,7 @@
 
 	led_classdev_unregister(&wled->cdev);
 	cancel_work_sync(&wled->work);
+	destroy_workqueue(wled->wq);
 	mutex_destroy(&wled->lock);
 
 	return 0;