Merge "leds: qpnp-wled: Disable the current sink conditionally" (b7e3fcff) · Commits · e / devices / android_kernel_xiaomi_markw

drivers/leds/leds-qpnp-wled.c

+160 −94

Original line number	Diff line number	Diff line
		/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
		/* Copyright (c) 2014-2017, The Linux Foundation. All rights reserved.
		*
		* This program is free software; you can redistribute it and/or modify
		* it under the terms of the GNU General Public License version 2 and
		@@ -997,6 +997,162 @@ static irqreturn_t qpnp_wled_sc_irq(int irq, void *_wled)
		return IRQ_HANDLED;
		}

		static int qpnp_wled_sink_config(struct qpnp_wled *wled)
		{
		int rc, i, temp;
		u8 reg = 0, sink_reg = 0, sink_cfg = 0, fs_reg = 0, fs_temp = 0;
		u8 mod_reg = 0, mod_temp = 0, delay_reg = 0, delay_temp = 0, val = 0;
		bool module_enable;

		rc = qpnp_wled_read_reg(wled, &reg,
		QPNP_WLED_MODULE_EN_REG(wled->ctrl_base));
		if (rc < 0)
		return rc;

		module_enable = !!(reg & BIT(7));

		rc = qpnp_wled_read_reg(wled, &sink_reg,
		QPNP_WLED_CURR_SINK_REG(wled->sink_base));
		if (rc < 0)
		return rc;

		for (i = 0; i < wled->num_strings; i++) {
		if (wled->strings[i] >= QPNP_WLED_MAX_STRINGS) {
		dev_err(&wled->spmi->dev, "Invalid string number\n");
		return -EINVAL;
		}

		/* MODULATOR */
		rc = qpnp_wled_read_reg(wled, &mod_reg,
		QPNP_WLED_MOD_EN_REG(wled->sink_base,
		wled->strings[i]));
		if (rc < 0)
		return rc;

		mod_temp = (QPNP_WLED_MOD_EN << QPNP_WLED_MOD_EN_SHFT);

		if (wled->dim_mode == QPNP_WLED_DIM_HYBRID)
		mod_temp &= QPNP_WLED_GATE_DRV_MASK;
		else
		mod_temp \|= ~QPNP_WLED_GATE_DRV_MASK;

		/* SYNC DELAY */
		if (wled->sync_dly_us > QPNP_WLED_SYNC_DLY_MAX_US)
		wled->sync_dly_us = QPNP_WLED_SYNC_DLY_MAX_US;

		rc = qpnp_wled_read_reg(wled, &delay_reg,
		QPNP_WLED_SYNC_DLY_REG(wled->sink_base,
		wled->strings[i])
		);
		if (rc < 0)
		return rc;

		delay_temp = wled->sync_dly_us / QPNP_WLED_SYNC_DLY_STEP_US;

		/* FULL SCALE CURRENT */
		if (wled->fs_curr_ua > QPNP_WLED_FS_CURR_MAX_UA)
		wled->fs_curr_ua = QPNP_WLED_FS_CURR_MAX_UA;

		rc = qpnp_wled_read_reg(wled, &fs_reg,
		QPNP_WLED_FS_CURR_REG(wled->sink_base,
		wled->strings[i])
		);
		if (rc < 0)
		return rc;

		fs_temp = wled->fs_curr_ua / QPNP_WLED_FS_CURR_STEP_UA;

		if (mod_reg != mod_temp \|\|
		(delay_reg & ~QPNP_WLED_SYNC_DLY_MASK) != delay_temp \|\|
		(fs_reg & ~QPNP_WLED_FS_CURR_MASK) != fs_temp) {
		if (module_enable) {
		/* Disable module */
		rc = qpnp_wled_module_en(wled, wled->ctrl_base,
		false);
		if (rc < 0)
		return rc;

		module_enable = 0;

		val = 0;
		/* Disable all the sinks */
		rc = qpnp_wled_write_reg(wled, &val,
		QPNP_WLED_CURR_SINK_REG
		(wled->sink_base));
		if (rc < 0)
		return rc;
		}

		rc = qpnp_wled_write_reg(wled, &mod_temp,
		QPNP_WLED_MOD_EN_REG(
		wled->sink_base,
		wled->strings[i]));
		if (rc < 0)
		return rc;

		delay_reg &= QPNP_WLED_SYNC_DLY_MASK;
		delay_reg \|= delay_temp;
		rc = qpnp_wled_write_reg(wled, &delay_reg,
		QPNP_WLED_SYNC_DLY_REG(
		wled->sink_base,
		wled->strings[i]));
		if (rc < 0)
		return rc;

		fs_reg &= QPNP_WLED_FS_CURR_MASK;
		fs_reg \|= fs_temp;
		rc = qpnp_wled_write_reg(wled, &fs_reg,
		QPNP_WLED_FS_CURR_REG(
		wled->sink_base,
		wled->strings[i]));
		if (rc < 0)
		return rc;
		}

		/* CABC */
		rc = qpnp_wled_read_reg(wled, &reg,
		QPNP_WLED_CABC_REG(wled->sink_base,
		wled->strings[i]));
		if (rc < 0)
		return rc;

		reg &= QPNP_WLED_CABC_MASK;
		reg \|= (wled->en_cabc << QPNP_WLED_CABC_SHIFT);
		rc = qpnp_wled_write_reg(wled, &reg,
		QPNP_WLED_CABC_REG(wled->sink_base,
		wled->strings[i]));
		if (rc < 0)
		return rc;

		temp = wled->strings[i] + QPNP_WLED_CURR_SINK_SHIFT;
		sink_cfg \|= (1 << temp);
		}

		if (sink_reg != sink_cfg) {
		if (module_enable) {
		/* Disable module */
		rc = qpnp_wled_module_en(wled, wled->ctrl_base, false);
		if (rc < 0)
		return rc;

		module_enable = 0;
		}

		/* Disable all the sinks */
		rc = qpnp_wled_write_reg(wled, &sink_cfg,
		QPNP_WLED_CURR_SINK_REG(
		wled->sink_base));
		if (rc < 0)
		return rc;
		}

		/* Enable module */
		if (!module_enable)
		rc = qpnp_wled_module_en(wled, wled->ctrl_base, true);

		return rc;
		}

		/* Configure WLED registers */
		static int qpnp_wled_config(struct qpnp_wled *wled)
		{
		@@ -1258,99 +1414,9 @@ static int qpnp_wled_config(struct qpnp_wled *wled)
		if (rc)
		return rc;

		/* disable all current sinks and enable selected strings */
		reg = 0x00;
		rc = qpnp_wled_write_reg(wled, &reg,
		QPNP_WLED_CURR_SINK_REG(wled->sink_base));

		for (i = 0; i < wled->num_strings; i++) {
		if (wled->strings[i] >= QPNP_WLED_MAX_STRINGS) {
		dev_err(&wled->spmi->dev, "Invalid string number\n");
		return -EINVAL;
		}

		/* MODULATOR */
		rc = qpnp_wled_read_reg(wled, &reg,
		QPNP_WLED_MOD_EN_REG(wled->sink_base,
		wled->strings[i]));
		if (rc < 0)
		return rc;
		reg &= QPNP_WLED_MOD_EN_MASK;
		reg \|= (QPNP_WLED_MOD_EN << QPNP_WLED_MOD_EN_SHFT);

		if (wled->dim_mode == QPNP_WLED_DIM_HYBRID)
		reg &= QPNP_WLED_GATE_DRV_MASK;
		else
		reg \|= ~QPNP_WLED_GATE_DRV_MASK;

		rc = qpnp_wled_write_reg(wled, &reg,
		QPNP_WLED_MOD_EN_REG(wled->sink_base,
		wled->strings[i]));
		if (rc)
		return rc;

		/* SYNC DELAY */
		if (wled->sync_dly_us > QPNP_WLED_SYNC_DLY_MAX_US)
		wled->sync_dly_us = QPNP_WLED_SYNC_DLY_MAX_US;

		rc = qpnp_wled_read_reg(wled, &reg,
		QPNP_WLED_SYNC_DLY_REG(wled->sink_base,
		wled->strings[i]));
		if (rc < 0)
		return rc;
		reg &= QPNP_WLED_SYNC_DLY_MASK;
		temp = wled->sync_dly_us / QPNP_WLED_SYNC_DLY_STEP_US;
		reg \|= temp;
		rc = qpnp_wled_write_reg(wled, &reg,
		QPNP_WLED_SYNC_DLY_REG(wled->sink_base,
		wled->strings[i]));
		if (rc)
		return rc;

		/* FULL SCALE CURRENT */
		if (wled->fs_curr_ua > QPNP_WLED_FS_CURR_MAX_UA)
		wled->fs_curr_ua = QPNP_WLED_FS_CURR_MAX_UA;

		rc = qpnp_wled_read_reg(wled, &reg,
		QPNP_WLED_FS_CURR_REG(wled->sink_base,
		wled->strings[i]));
		rc = qpnp_wled_sink_config(wled);
		if (rc < 0)
		return rc;
		reg &= QPNP_WLED_FS_CURR_MASK;
		temp = wled->fs_curr_ua / QPNP_WLED_FS_CURR_STEP_UA;
		reg \|= temp;
		rc = qpnp_wled_write_reg(wled, &reg,
		QPNP_WLED_FS_CURR_REG(wled->sink_base,
		wled->strings[i]));
		if (rc)
		return rc;

		/* CABC */
		rc = qpnp_wled_read_reg(wled, &reg,
		QPNP_WLED_CABC_REG(wled->sink_base,
		wled->strings[i]));
		if (rc < 0)
		return rc;
		reg &= QPNP_WLED_CABC_MASK;
		reg \|= (wled->en_cabc << QPNP_WLED_CABC_SHIFT);
		rc = qpnp_wled_write_reg(wled, &reg,
		QPNP_WLED_CABC_REG(wled->sink_base,
		wled->strings[i]));
		if (rc)
		return rc;

		/* Enable CURRENT SINK */
		rc = qpnp_wled_read_reg(wled, &reg,
		QPNP_WLED_CURR_SINK_REG(wled->sink_base));
		if (rc < 0)
		return rc;
		temp = wled->strings[i] + QPNP_WLED_CURR_SINK_SHIFT;
		reg \|= (1 << temp);
		rc = qpnp_wled_write_reg(wled, &reg,
		QPNP_WLED_CURR_SINK_REG(wled->sink_base));
		if (rc)
		return rc;
		}

		rc = qpnp_wled_sync_reg_toggle(wled);
		if (rc < 0) {