static int ab8500_btemp_curr_source_enable()

in supply/ab8500_btemp.c [196:336]

• 94 lines of code
• 15 McCabe index (conditional complexity)

static int ab8500_btemp_curr_source_enable(struct ab8500_btemp *di,
	bool enable)
{
	int curr;
	int ret = 0;

	/*
	 * BATCTRL current sources are included on AB8500 cut2.0
	 * and future versions
	 */
	if (is_ab8500_1p1_or_earlier(di->parent))
		return 0;

	/* Only do this for batteries with internal NTC */
	if (di->bm->adc_therm == AB8500_ADC_THERM_BATCTRL && enable) {

		if (di->curr_source == BTEMP_BATCTRL_CURR_SRC_7UA)
			curr = BAT_CTRL_7U_ENA;
		else
			curr = BAT_CTRL_20U_ENA;

		dev_dbg(di->dev, "Set BATCTRL %duA\n", di->curr_source);

		ret = abx500_mask_and_set_register_interruptible(di->dev,
			AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
			FORCE_BAT_CTRL_CMP_HIGH, FORCE_BAT_CTRL_CMP_HIGH);
		if (ret) {
			dev_err(di->dev, "%s failed setting cmp_force\n",
				__func__);
			return ret;
		}

		/*
		 * We have to wait one 32kHz cycle before enabling
		 * the current source, since ForceBatCtrlCmpHigh needs
		 * to be written in a separate cycle
		 */
		udelay(32);

		ret = abx500_set_register_interruptible(di->dev,
			AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
			FORCE_BAT_CTRL_CMP_HIGH | curr);
		if (ret) {
			dev_err(di->dev, "%s failed enabling current source\n",
				__func__);
			goto disable_curr_source;
		}
	} else if (di->bm->adc_therm == AB8500_ADC_THERM_BATCTRL && !enable) {
		dev_dbg(di->dev, "Disable BATCTRL curr source\n");

		/* Write 0 to the curr bits */
		ret = abx500_mask_and_set_register_interruptible(
			di->dev,
			AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
			BAT_CTRL_7U_ENA | BAT_CTRL_20U_ENA,
			~(BAT_CTRL_7U_ENA | BAT_CTRL_20U_ENA));

		if (ret) {
			dev_err(di->dev, "%s failed disabling current source\n",
				__func__);
			goto disable_curr_source;
		}

		/* Enable Pull-Up and comparator */
		ret = abx500_mask_and_set_register_interruptible(di->dev,
			AB8500_CHARGER,	AB8500_BAT_CTRL_CURRENT_SOURCE,
			BAT_CTRL_PULL_UP_ENA | BAT_CTRL_CMP_ENA,
			BAT_CTRL_PULL_UP_ENA | BAT_CTRL_CMP_ENA);
		if (ret) {
			dev_err(di->dev, "%s failed enabling PU and comp\n",
				__func__);
			goto enable_pu_comp;
		}

		/*
		 * We have to wait one 32kHz cycle before disabling
		 * ForceBatCtrlCmpHigh since this needs to be written
		 * in a separate cycle
		 */
		udelay(32);

		/* Disable 'force comparator' */
		ret = abx500_mask_and_set_register_interruptible(di->dev,
			AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
			FORCE_BAT_CTRL_CMP_HIGH, ~FORCE_BAT_CTRL_CMP_HIGH);
		if (ret) {
			dev_err(di->dev, "%s failed disabling force comp\n",
				__func__);
			goto disable_force_comp;
		}
	}
	return ret;

	/*
	 * We have to try unsetting FORCE_BAT_CTRL_CMP_HIGH one more time
	 * if we got an error above
	 */
disable_curr_source:
	/* Write 0 to the curr bits */
	ret = abx500_mask_and_set_register_interruptible(di->dev,
		AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
		BAT_CTRL_7U_ENA | BAT_CTRL_20U_ENA,
		~(BAT_CTRL_7U_ENA | BAT_CTRL_20U_ENA));

	if (ret) {
		dev_err(di->dev, "%s failed disabling current source\n",
			__func__);
		return ret;
	}
enable_pu_comp:
	/* Enable Pull-Up and comparator */
	ret = abx500_mask_and_set_register_interruptible(di->dev,
		AB8500_CHARGER,	AB8500_BAT_CTRL_CURRENT_SOURCE,
		BAT_CTRL_PULL_UP_ENA | BAT_CTRL_CMP_ENA,
		BAT_CTRL_PULL_UP_ENA | BAT_CTRL_CMP_ENA);
	if (ret) {
		dev_err(di->dev, "%s failed enabling PU and comp\n",
			__func__);
		return ret;
	}

disable_force_comp:
	/*
	 * We have to wait one 32kHz cycle before disabling
	 * ForceBatCtrlCmpHigh since this needs to be written
	 * in a separate cycle
	 */
	udelay(32);

	/* Disable 'force comparator' */
	ret = abx500_mask_and_set_register_interruptible(di->dev,
		AB8500_CHARGER, AB8500_BAT_CTRL_CURRENT_SOURCE,
		FORCE_BAT_CTRL_CMP_HIGH, ~FORCE_BAT_CTRL_CMP_HIGH);
	if (ret) {
		dev_err(di->dev, "%s failed disabling force comp\n",
			__func__);
		return ret;
	}

	return ret;
}