build/patch/kernel/archive/sunxi-6.5/patches.megous/drm-sun4i-Support-taking-over-display-pipeline-state-from-p-boo.patch

From 2435c6719c6eb37587b5678087b9ea2484dedf4f Mon Sep 17 00:00:00 2001
From: Ondrej Jirman <megi@xff.cz>
Date: Sun, 30 Apr 2023 18:19:16 +0200
Subject: [PATCH 207/464] drm/sun4i: Support taking over display pipeline state
 from p-boot

For perfect, flickerless and fast boot.

Signed-off-by: Ondrej Jirman <megi@xff.cz>
---
 drivers/clk/sunxi-ng/ccu-sun50i-a64.c         |  4 +-
 drivers/gpu/drm/drm_fbdev_generic.c           | 14 +++++
 drivers/gpu/drm/panel/panel-sitronix-st7703.c | 17 ++++++
 drivers/gpu/drm/sun4i/sun4i_tcon.c            | 23 ++++++++
 drivers/gpu/drm/sun4i/sun4i_tcon.h            |  2 +
 drivers/gpu/drm/sun4i/sun6i_mipi_dsi.c        | 13 +++++
 drivers/gpu/drm/sun4i/sun6i_mipi_dsi.h        |  2 +
 drivers/gpu/drm/sun4i/sun8i_mixer.c           | 52 +++++++++++++++++++
 drivers/gpu/drm/sun4i/sun8i_mixer.h           |  3 ++
 drivers/phy/allwinner/phy-sun6i-mipi-dphy.c   | 14 +++++
 drivers/video/backlight/pwm_bl.c              | 25 ++++++++-
 11 files changed, 167 insertions(+), 2 deletions(-)

diff --git a/drivers/clk/sunxi-ng/ccu-sun50i-a64.c b/drivers/clk/sunxi-ng/ccu-sun50i-a64.c
index eb36f8f77d55..ef567775fc95 100644
--- a/drivers/clk/sunxi-ng/ccu-sun50i-a64.c
+++ b/drivers/clk/sunxi-ng/ccu-sun50i-a64.c
@@ -958,7 +958,9 @@ static int sun50i_a64_ccu_probe(struct platform_device *pdev)
 	val &= ~GENMASK(19, 16);
 	writel(val | (0 << 16), reg + SUN50I_A64_PLL_AUDIO_REG);
 
-	writel(0x515, reg + SUN50I_A64_PLL_MIPI_REG);
+	ret = of_property_read_u32_index(of_chosen, "p-boot,framebuffer-start", 0, &val);
+	if (ret)
+		writel(0x515, reg + SUN50I_A64_PLL_MIPI_REG);
 
 	/* Set PLL MIPI as parent for TCON0 */
 	val = readl(reg + SUN50I_A64_TCON0_CLK_REG);
diff --git a/drivers/gpu/drm/drm_fbdev_generic.c b/drivers/gpu/drm/drm_fbdev_generic.c
index b9343fb6cf13..2e2913fa3ed0 100644
--- a/drivers/gpu/drm/drm_fbdev_generic.c
+++ b/drivers/gpu/drm/drm_fbdev_generic.c
@@ -78,6 +78,7 @@ static int drm_fbdev_generic_helper_fb_probe(struct drm_fb_helper *fb_helper,
 	size_t screen_size;
 	void *screen_buffer;
 	u32 format;
+	u32 fb_start;
 	int ret;
 
 	drm_dbg_kms(dev, "surface width(%d), height(%d) and bpp(%d)\n",
@@ -126,6 +127,19 @@ static int drm_fbdev_generic_helper_fb_probe(struct drm_fb_helper *fb_helper,
 	if (ret)
 		goto err_drm_fb_helper_release_info;
 
+	ret = of_property_read_u32_index(of_chosen, "p-boot,framebuffer-start", 0, &fb_start);
+	if (ret == 0) {
+		// copy framebuffer contents from p-boot if reasonable
+		if (screen_size != 720 * 1440 * 4) {
+			drm_err(dev, "surface width(%d), height(%d) and bpp(%d) does not match p-boot requirements\n",
+				    sizes->surface_width, sizes->surface_height,
+				    sizes->surface_bpp);
+			return 0;
+		}
+
+		memcpy(screen_buffer, __va(fb_start), screen_size);
+	}
+
 	return 0;
 
 err_drm_fb_helper_release_info:
diff --git a/drivers/gpu/drm/panel/panel-sitronix-st7703.c b/drivers/gpu/drm/panel/panel-sitronix-st7703.c
index 5b9689c6a351..955be40db545 100644
--- a/drivers/gpu/drm/panel/panel-sitronix-st7703.c
+++ b/drivers/gpu/drm/panel/panel-sitronix-st7703.c
@@ -62,6 +62,7 @@ struct st7703 {
 
 	struct dentry *debugfs;
 	const struct st7703_panel_desc *desc;
+	bool hw_preenabled;
 };
 
 struct st7703_panel_desc {
@@ -438,6 +439,11 @@ static int st7703_enable(struct drm_panel *panel)
 	struct mipi_dsi_device *dsi = to_mipi_dsi_device(ctx->dev);
 	int ret;
 
+	if (ctx->hw_preenabled) {
+		ctx->hw_preenabled = false;
+		return 0;
+	}
+
 	ret = ctx->desc->init_sequence(ctx);
 	if (ret < 0) {
 		dev_err(ctx->dev, "Panel init sequence failed: %d\n", ret);
@@ -505,8 +511,10 @@ static int st7703_prepare(struct drm_panel *panel)
 	if (ctx->prepared)
 		return 0;
 
+	if (!ctx->hw_preenabled) {
 	dev_dbg(ctx->dev, "Resetting the panel\n");
 	gpiod_set_value_cansleep(ctx->reset_gpio, 1);
+	}
 
 	ret = regulator_enable(ctx->iovcc);
 	if (ret < 0) {
@@ -522,10 +530,12 @@ static int st7703_prepare(struct drm_panel *panel)
 	}
 
 	/* Give power supplies time to stabilize before deasserting reset. */
+	if (!ctx->hw_preenabled) {
 	usleep_range(10000, 20000);
 
 	gpiod_set_value_cansleep(ctx->reset_gpio, 0);
 	usleep_range(15000, 20000);
+	}
 
 	ctx->prepared = true;
 
@@ -610,12 +620,19 @@ static int st7703_probe(struct mipi_dsi_device *dsi)
 {
 	struct device *dev = &dsi->dev;
 	struct st7703 *ctx;
+	u32 fb_start;
 	int ret;
 
 	ctx = devm_kzalloc(dev, sizeof(*ctx), GFP_KERNEL);
 	if (!ctx)
 		return -ENOMEM;
 
+	ret = of_property_read_u32_index(of_chosen, "p-boot,framebuffer-start", 0, &fb_start);
+	if (ret == 0) {
+		/* the display pipeline is already initialized by p-boot */
+		ctx->hw_preenabled = true;
+	}
+
 	ctx->reset_gpio = devm_gpiod_get(dev, "reset", GPIOD_OUT_LOW);
 	if (IS_ERR(ctx->reset_gpio))
 		return dev_err_probe(dev, PTR_ERR(ctx->reset_gpio), "Failed to get reset gpio\n");
diff --git a/drivers/gpu/drm/sun4i/sun4i_tcon.c b/drivers/gpu/drm/sun4i/sun4i_tcon.c
index 3b83920e0aec..c44d5f3350d1 100644
--- a/drivers/gpu/drm/sun4i/sun4i_tcon.c
+++ b/drivers/gpu/drm/sun4i/sun4i_tcon.c
@@ -40,6 +40,8 @@
 #include "sun8i_tcon_top.h"
 #include "sunxi_engine.h"
 
+static bool hw_preconfigured;
+
 static struct drm_connector *sun4i_tcon_get_connector(const struct drm_encoder *encoder)
 {
 	struct drm_connector *connector;
@@ -741,6 +743,13 @@ void sun4i_tcon_mode_set(struct sun4i_tcon *tcon,
 			 const struct drm_encoder *encoder,
 			 const struct drm_display_mode *mode)
 {
+	if (tcon->hw_preconfigured) {
+		// avoid the first modeset
+		tcon->hw_preconfigured = false;
+		hw_preconfigured = false;
+		return;
+	}
+
 	switch (encoder->encoder_type) {
 	case DRM_MODE_ENCODER_DSI:
 		/* DSI is tied to special case of CPU interface */
@@ -882,6 +891,7 @@ static int sun4i_tcon_init_regmap(struct device *dev,
 		return PTR_ERR(tcon->regs);
 	}
 
+	if (!tcon->hw_preconfigured) {
 	/* Make sure the TCON is disabled and all IRQs are off */
 	regmap_write(tcon->regs, SUN4I_TCON_GCTL_REG, 0);
 	regmap_write(tcon->regs, SUN4I_TCON_GINT0_REG, 0);
@@ -890,6 +900,7 @@ static int sun4i_tcon_init_regmap(struct device *dev,
 	/* Disable IO lines and set them to tristate */
 	regmap_write(tcon->regs, SUN4I_TCON0_IO_TRI_REG, ~0);
 	regmap_write(tcon->regs, SUN4I_TCON1_IO_TRI_REG, ~0);
+	}
 
 	return 0;
 }
@@ -1161,6 +1172,9 @@ static int sun4i_tcon_bind(struct device *dev, struct device *master,
 	tcon->dev = dev;
 	tcon->id = engine->id;
 	tcon->quirks = of_device_get_match_data(dev);
+	
+	if (tcon->id == 0)
+		tcon->hw_preconfigured = hw_preconfigured;
 
 	tcon->lcd_rst = devm_reset_control_get(dev, "lcd");
 	if (IS_ERR(tcon->lcd_rst)) {
@@ -1182,12 +1196,14 @@ static int sun4i_tcon_bind(struct device *dev, struct device *master,
 		}
 	}
 
+	if (!tcon->hw_preconfigured) {
 	/* Make sure our TCON is reset */
 	ret = reset_control_reset(tcon->lcd_rst);
 	if (ret) {
 		dev_err(dev, "Couldn't deassert our reset line\n");
 		return ret;
 	}
+	}
 
 	if (tcon->quirks->supports_lvds) {
 		/*
@@ -1351,8 +1367,15 @@ static int sun4i_tcon_probe(struct platform_device *pdev)
 	const struct sun4i_tcon_quirks *quirks;
 	struct drm_bridge *bridge;
 	struct drm_panel *panel;
+	u32 fb_start;
 	int ret;
 
+	ret = of_property_read_u32_index(of_chosen, "p-boot,framebuffer-start", 0, &fb_start);
+	if (ret == 0) {
+		/* the display pipeline is already initialized by p-boot */
+		hw_preconfigured = true;
+	}
+
 	quirks = of_device_get_match_data(&pdev->dev);
 
 	/* panels and bridges are present only on TCONs with channel 0 */
diff --git a/drivers/gpu/drm/sun4i/sun4i_tcon.h b/drivers/gpu/drm/sun4i/sun4i_tcon.h
index 97df39db2a31..864d70b9d242 100644
--- a/drivers/gpu/drm/sun4i/sun4i_tcon.h
+++ b/drivers/gpu/drm/sun4i/sun4i_tcon.h
@@ -293,6 +293,8 @@ struct sun4i_tcon {
 
 	/* TCON list management */
 	struct list_head		list;
+
+	bool hw_preconfigured;
 };
 
 struct drm_bridge *sun4i_tcon_find_bridge(struct device_node *node);
diff --git a/drivers/gpu/drm/sun4i/sun6i_mipi_dsi.c b/drivers/gpu/drm/sun4i/sun6i_mipi_dsi.c
index 760ff05eabf4..fac9b744c12d 100644
--- a/drivers/gpu/drm/sun4i/sun6i_mipi_dsi.c
+++ b/drivers/gpu/drm/sun4i/sun6i_mipi_dsi.c
@@ -732,6 +732,7 @@ static void sun6i_dsi_encoder_enable(struct drm_encoder *encoder)
 	reset_control_deassert(dsi->reset);
 	clk_prepare_enable(dsi->mod_clk);
 
+	if (!dsi->hw_preconfigured) {
 	/*
 	 * Enable the DSI block.
 	 */
@@ -758,6 +759,7 @@ static void sun6i_dsi_encoder_enable(struct drm_encoder *encoder)
 	sun6i_dsi_setup_inst_loop(dsi, mode);
 	sun6i_dsi_setup_format(dsi, mode);
 	sun6i_dsi_setup_timings(dsi, mode);
+	}
 
 	phy_init(dsi->dphy);
 
@@ -787,11 +789,15 @@ static void sun6i_dsi_encoder_enable(struct drm_encoder *encoder)
 	if (dsi->panel)
 		drm_panel_enable(dsi->panel);
 
+	if (!dsi->hw_preconfigured) {
 	sun6i_dsi_start(dsi, DSI_START_HSC);
 
 	udelay(1000);
 
 	sun6i_dsi_start(dsi, DSI_START_HSD);
+	}
+
+	dsi->hw_preconfigured = false;
 }
 
 static void sun6i_dsi_encoder_disable(struct drm_encoder *encoder)
@@ -1105,6 +1111,7 @@ static int sun6i_dsi_probe(struct platform_device *pdev)
 	struct device *dev = &pdev->dev;
 	struct sun6i_dsi *dsi;
 	void __iomem *base;
+	u32 fb_start;
 	int ret;
 
 	variant = device_get_match_data(dev);
@@ -1120,6 +1127,12 @@ static int sun6i_dsi_probe(struct platform_device *pdev)
 	dsi->host.dev = dev;
 	dsi->variant = variant;
 
+	ret = of_property_read_u32_index(of_chosen, "p-boot,framebuffer-start", 0, &fb_start);
+	if (ret == 0) {
+		/* the display pipeline is already initialized by p-boot */
+		dsi->hw_preconfigured = true;
+	}
+
 	base = devm_platform_ioremap_resource(pdev, 0);
 	if (IS_ERR(base)) {
 		dev_err(dev, "Couldn't map the DSI encoder registers\n");
diff --git a/drivers/gpu/drm/sun4i/sun6i_mipi_dsi.h b/drivers/gpu/drm/sun4i/sun6i_mipi_dsi.h
index f1ddefe0f554..958c2997ab43 100644
--- a/drivers/gpu/drm/sun4i/sun6i_mipi_dsi.h
+++ b/drivers/gpu/drm/sun4i/sun6i_mipi_dsi.h
@@ -38,6 +38,8 @@ struct sun6i_dsi {
 	struct drm_panel	*panel;
 
 	const struct sun6i_dsi_variant *variant;
+	
+	bool hw_preconfigured;
 };
 
 static inline struct sun6i_dsi *host_to_sun6i_dsi(struct mipi_dsi_host *host)
diff --git a/drivers/gpu/drm/sun4i/sun8i_mixer.c b/drivers/gpu/drm/sun4i/sun8i_mixer.c
index 95e2368982ea..f156a300b8c8 100644
--- a/drivers/gpu/drm/sun4i/sun8i_mixer.c
+++ b/drivers/gpu/drm/sun4i/sun8i_mixer.c
@@ -22,6 +22,7 @@
 #include <drm/drm_probe_helper.h>
 
 #include "sun4i_drv.h"
+#include "sun4i_tcon.h"
 #include "sun8i_mixer.h"
 #include "sun8i_ui_layer.h"
 #include "sun8i_vi_layer.h"
@@ -32,6 +33,8 @@ struct de2_fmt_info {
 	u32	de2_fmt;
 };
 
+static bool hw_preconfigured;
+
 static const struct de2_fmt_info de2_formats[] = {
 	{
 		.drm_fmt = DRM_FORMAT_ARGB8888,
@@ -276,6 +279,33 @@ static void sun8i_mixer_commit(struct sunxi_engine *engine,
 	struct drm_plane *plane;
 	u32 route = 0, pipe_en = 0;
 
+	if (mixer->hw_preconfigured && engine->id == 0) {
+		struct sun4i_tcon* tcon;
+		u32 val, saved, ret;
+
+		/*
+		 * This is the first commit, wait for vblank on tcon0 before continuing.
+		 */
+		list_for_each_entry(tcon, &mixer->drv->tcon_list, list) {
+			if (tcon->id == 0) {
+				regmap_read(tcon->regs, SUN4I_TCON_GINT0_REG, &saved);
+				saved &= 0xffff0000;
+
+				regmap_write(tcon->regs, SUN4I_TCON_GINT0_REG, 0);
+
+				ret = regmap_read_poll_timeout(tcon->regs, SUN4I_TCON_GINT0_REG, val,
+							 val & (SUN4I_TCON_GINT0_VBLANK_INT(0) |
+								SUN4I_TCON_GINT0_VBLANK_INT(1) |
+								SUN4I_TCON_GINT0_TCON0_TRI_FINISH_INT),
+							 100, 40000);
+
+				regmap_write(tcon->regs, SUN4I_TCON_GINT0_REG, saved);
+			}
+		}
+
+		mixer->hw_preconfigured = false;
+	}
+
 	DRM_DEBUG_DRIVER("Committing changes\n");
 
 	drm_for_each_plane(plane, state->dev) {
@@ -469,6 +499,7 @@ static int sun8i_mixer_bind(struct device *dev, struct device *master,
 	dev_set_drvdata(dev, mixer);
 	mixer->engine.ops = &sun8i_engine_ops;
 	mixer->engine.node = dev->of_node;
+	mixer->drv = drv;
 
 	if (of_property_present(dev->of_node, "iommus")) {
 		/*
@@ -493,6 +524,11 @@ static int sun8i_mixer_bind(struct device *dev, struct device *master,
 	 */
 	mixer->engine.id = sun8i_mixer_of_get_id(dev->of_node);
 
+	if (mixer->engine.id == 0) {
+		mixer->hw_preconfigured = hw_preconfigured;
+		hw_preconfigured = false;
+	}
+
 	mixer->cfg = of_device_get_match_data(dev);
 	if (!mixer->cfg)
 		return -EINVAL;
@@ -540,8 +576,11 @@ static int sun8i_mixer_bind(struct device *dev, struct device *master,
 	 * reason for the mixer to be functional. Make sure it's the
 	 * case.
 	 */
+
+	if (!mixer->hw_preconfigured) {
 	if (mixer->cfg->mod_rate)
 		clk_set_rate(mixer->mod_clk, mixer->cfg->mod_rate);
+	}
 
 	clk_prepare_enable(mixer->mod_clk);
 
@@ -549,6 +588,7 @@ static int sun8i_mixer_bind(struct device *dev, struct device *master,
 
 	base = sun8i_blender_base(mixer);
 
+	if (!mixer->hw_preconfigured) {
 	/* Reset registers and disable unused sub-engines */
 	if (mixer->cfg->is_de3) {
 		for (i = 0; i < DE3_MIXER_UNIT_SIZE; i += 4)
@@ -580,6 +620,7 @@ static int sun8i_mixer_bind(struct device *dev, struct device *master,
 	/* Enable the mixer */
 	regmap_write(mixer->engine.regs, SUN8I_MIXER_GLOBAL_CTL,
 		     SUN8I_MIXER_GLOBAL_CTL_RT_EN);
+	} /* hw_preconfigured */
 
 	/* Set background color to black */
 	regmap_write(mixer->engine.regs, SUN8I_MIXER_BLEND_BKCOLOR(base),
@@ -600,8 +641,10 @@ static int sun8i_mixer_bind(struct device *dev, struct device *master,
 			     SUN8I_MIXER_BLEND_MODE(base, i),
 			     SUN8I_MIXER_BLEND_MODE_DEF);
 
+	if (!mixer->hw_preconfigured) {
 	regmap_update_bits(mixer->engine.regs, SUN8I_MIXER_BLEND_PIPE_CTL(base),
 			   SUN8I_MIXER_BLEND_PIPE_CTL_EN_MSK, 0);
+	}
 
 	return 0;
 
@@ -631,6 +674,15 @@ static const struct component_ops sun8i_mixer_ops = {
 
 static int sun8i_mixer_probe(struct platform_device *pdev)
 {
+	int ret;
+	u32 fb_start;
+
+	ret = of_property_read_u32_index(of_chosen, "p-boot,framebuffer-start", 0, &fb_start);
+	if (ret == 0) {
+		/* the display pipeline is already initialized by p-boot */
+		hw_preconfigured = true;
+	}
+
 	return component_add(&pdev->dev, &sun8i_mixer_ops);
 }
 
diff --git a/drivers/gpu/drm/sun4i/sun8i_mixer.h b/drivers/gpu/drm/sun4i/sun8i_mixer.h
index d7898c9c9cc0..68e2741b0962 100644
--- a/drivers/gpu/drm/sun4i/sun8i_mixer.h
+++ b/drivers/gpu/drm/sun4i/sun8i_mixer.h
@@ -184,6 +184,9 @@ struct sun8i_mixer {
 
 	struct clk			*bus_clk;
 	struct clk			*mod_clk;
+
+	struct sun4i_drv		*drv;
+	bool				hw_preconfigured;
 };
 
 enum {
diff --git a/drivers/phy/allwinner/phy-sun6i-mipi-dphy.c b/drivers/phy/allwinner/phy-sun6i-mipi-dphy.c
index 36eab95271b2..a19a27cea860 100644
--- a/drivers/phy/allwinner/phy-sun6i-mipi-dphy.c
+++ b/drivers/phy/allwinner/phy-sun6i-mipi-dphy.c
@@ -195,6 +195,8 @@ struct sun6i_dphy {
 
 	const struct sun6i_dphy_variant		*variant;
 	enum sun6i_dphy_direction		direction;
+
+	bool hw_preconfigured;
 };
 
 static int sun6i_dphy_init(struct phy *phy)
@@ -226,6 +228,11 @@ static void sun6i_a31_mipi_dphy_tx_power_on(struct sun6i_dphy *dphy)
 {
 	u8 lanes_mask = GENMASK(dphy->config.lanes - 1, 0);
 
+	if (dphy->hw_preconfigured) {
+		dphy->hw_preconfigured = false;
+		return;
+	}
+
 	regmap_write(dphy->regs, SUN6I_DPHY_ANA0_REG,
 		     SUN6I_DPHY_ANA0_REG_PWS |
 		     SUN6I_DPHY_ANA0_REG_DMPC |
@@ -551,6 +558,7 @@ static int sun6i_dphy_probe(struct platform_device *pdev)
 	struct sun6i_dphy *dphy;
 	const char *direction;
 	void __iomem *regs;
+	u32 fb_start;
 	int ret;
 
 	dphy = devm_kzalloc(&pdev->dev, sizeof(*dphy), GFP_KERNEL);
@@ -561,6 +569,12 @@ static int sun6i_dphy_probe(struct platform_device *pdev)
 	if (!dphy->variant)
 		return -EINVAL;
 
+	ret = of_property_read_u32_index(of_chosen, "p-boot,framebuffer-start", 0, &fb_start);
+	if (ret == 0) {
+		/* the display pipeline is already initialized by p-boot */
+		dphy->hw_preconfigured = true;
+	}
+
 	regs = devm_platform_ioremap_resource(pdev, 0);
 	if (IS_ERR(regs)) {
 		dev_err(&pdev->dev, "Couldn't map the DPHY encoder registers\n");
diff --git a/drivers/video/backlight/pwm_bl.c b/drivers/video/backlight/pwm_bl.c
index a51fbab96368..4ee25b3ce944 100644
--- a/drivers/video/backlight/pwm_bl.c
+++ b/drivers/video/backlight/pwm_bl.c
@@ -455,7 +455,7 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	struct backlight_properties props;
 	struct backlight_device *bl;
 	struct pwm_bl_data *pb;
-	struct pwm_state state;
+	struct pwm_state state, state_real;
 	unsigned int i;
 	int ret;
 
@@ -519,6 +519,11 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 	/* Sync up PWM state. */
 	pwm_init_state(pb->pwm, &state);
 
+	/* Read real state, but only if the PWM is enabled. */
+	pwm_get_state(pb->pwm, &state_real);
+	if (state_real.enabled)
+		state = state_real;
+
 	/*
 	 * The DT case will set the pwm_period_ns field to 0 and store the
 	 * period, parsed from the DT, in the PWM device. For the non-DT case,
@@ -611,6 +616,24 @@ static int pwm_backlight_probe(struct platform_device *pdev)
 
 	bl->props.brightness = data->dft_brightness;
 	bl->props.power = pwm_backlight_initial_power_state(pb);
+	if (bl->props.power == FB_BLANK_UNBLANK && pb->levels) {
+		u64 level;
+
+		/* If the backlight is already on, determine the default
+		 * brightness from PWM duty cycle instead of forcing
+		 * the brightness determined by the driver 
+		 */
+		pwm_get_state(pb->pwm, &state);
+		level = (u64)state.duty_cycle * pb->scale;
+		do_div(level, (u64)state.period);
+
+		for (i = 0; i <= data->max_brightness; i++) {
+			if (data->levels[i] > level) {
+				bl->props.brightness = i;
+				break;
+			}
+		}
+	}
 	backlight_update_status(bl);
 
 	platform_set_drvdata(pdev, bl);
-- 
2.34.1