--- a/src/cs/drivers/drv_app/fchg/fchg_process.c	Fri Dec 29 05:51:08 2017 +0000
+++ b/src/cs/drivers/drv_app/fchg/fchg_process.c	Fri Dec 29 07:06:06 2017 +0000
@@ -13,6 +13,7 @@
 #include <stdio.h>
 
 extern UINT16 madc_vbat_2_physical(UINT16 adc_val);
+extern UINT16 madc_vbat_inverse(UINT16 mv);
 
 void pwr_init_discharge(void)
 {
@@ -45,16 +46,6 @@
 			RV_TRACE_LEVEL_WARNING, FCHG_USE_ID);
 }
 
-static void charge_progress_trace(char *mode, UINT16 ichg)
-{
-	char trace[64];
-
-	sprintf(trace, "%s charging: Vbat=%u Ichg=%u i2v=%u", mode,
-		pwr_ctrl->batt_mv, ichg, pwr_ctrl->i2v_offset);
-	rvf_send_trace(trace, strlen(trace), NULL_PARAM,
-			RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
-}
-
 static void start_i2v_cal(void)
 {
 	UINT16 bciconf;
@@ -88,24 +79,32 @@
 	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0003);
 }
 
-static void start_cv_charging(UINT16 code)
+static void start_cv_charging(void)
 {
+	UINT16 code;
+
 	rvf_send_trace("Start CV charging", 17, NULL_PARAM,
 			RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
 	pwr_ctrl->state = FCHG_STATE_CV_CHARGING;
 	/* Select constant voltage charging. The charger is disabled */
 	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
 	/* figure out the DAC code */
-	while (code &&
-	       madc_vbat_2_physical(code) >= pwr_ctrl->config.cv_init_set)
-		code--;
-	code++;
+	code = madc_vbat_inverse(pwr_ctrl->config.cv_init_set);
 	rvf_send_trace("Voltage (DAC code) ", 19, code,
 			RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
 	/* Program the DAC with the constant voltage value */
 	ABB_Write_Register_on_page(PAGE0, CHGREG, code);
 	/* Enable the charger */
 	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0001);
+	/* CV control loop state init */
+	pwr_ctrl->cv_dac_init = code;
+	pwr_ctrl->cv_dac_curr = code;
+	pwr_ctrl->cv_high_vbat_count = 0;
+	pwr_ctrl->cv_low_vbat_count = 0;
+	/* Ichg averaging state init */
+	pwr_ctrl->ichg_fill_level = 0;
+	pwr_ctrl->ichg_ring_ptr = 0;
+	pwr_ctrl->ichg_low_count = 0;
 }
 
 static void start_charge_condition_met(void)
@@ -120,10 +119,70 @@
 	}
 }
 
+static void ci_progress_trace(UINT16 ichg)
+{
+	char trace[64];
+
+	sprintf(trace, "CI charging: Vbat=%u Ichg=%u i2v=%u",
+		pwr_ctrl->batt_mv, ichg, pwr_ctrl->i2v_offset);
+	rvf_send_trace(trace, strlen(trace), NULL_PARAM,
+			RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
+}
+
+static int cv_ichg_process(UINT16 ichg_new)
+{
+	UINT16 ichg_clip, ichg_entry;
+	UINT32 ichg_accum;
+	UINT16 i;
+	char trace[64];
+
+	if (pwr_ctrl->ichg_fill_level < ICHG_AVG_WINDOW)
+		pwr_ctrl->ichg_avg_buf[pwr_ctrl->ichg_fill_level++] = ichg_new;
+	else {
+		ichg_clip = pwr_ctrl->ichg_average +
+				pwr_ctrl->config.ichg_max_spike;
+		if (ichg_new > ichg_clip)
+			ichg_entry = ichg_clip;
+		else
+			ichg_entry = ichg_new;
+		pwr_ctrl->ichg_avg_buf[pwr_ctrl->ichg_ring_ptr++] = ichg_entry;
+		if (pwr_ctrl->ichg_ring_ptr >= ICHG_AVG_WINDOW)
+			pwr_ctrl->ichg_ring_ptr = 0;
+	}
+	ichg_accum = 0;
+	for (i = 0; i < pwr_ctrl->ichg_fill_level; i++)
+		ichg_accum += pwr_ctrl->ichg_avg_buf[i];
+	pwr_ctrl->ichg_average = ichg_accum / pwr_ctrl->ichg_fill_level;
+	sprintf(trace, "CV charging: Vbat=%u Ichg=%u Ichg_avg=%u i2v=%u",
+		pwr_ctrl->batt_mv, ichg_new, pwr_ctrl->ichg_average,
+		pwr_ctrl->i2v_offset);
+	rvf_send_trace(trace, strlen(trace), NULL_PARAM,
+			RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
+	if (pwr_ctrl->ichg_average >
+	    (pwr_ctrl->config.end_current + pwr_ctrl->i2v_offset)) {
+		pwr_ctrl->ichg_low_count = 0;
+		return 0;
+	}
+	pwr_ctrl->ichg_low_count++;
+	if (pwr_ctrl->ichg_low_count < pwr_ctrl->config.ichg_samples_needed)
+		return 0;
+	rvf_send_trace("Stopping charge by low current condition", 40,
+			NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
+	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
+	pwr_init_discharge();
+	pwr_ctrl->state = FCHG_STATE_READY_TO_RECHARGE;
+	return 1;
+}
+
 static int overvoltage_end_charge_check(void)
 {
 	if (pwr_ctrl->batt_mv < pwr_ctrl->config.overvoltage)
 		return 0;
+	if (pwr_ctrl->cv_dac_curr >= pwr_ctrl->cv_dac_init)
+		return 0;
+	if ((pwr_ctrl->cv_dac_init - pwr_ctrl->cv_dac_curr) <
+	    pwr_ctrl->config.cv_dac_max_decr)
+		return 0;
 	rvf_send_trace("Stopping charge by overvoltage condition", 40,
 			NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
 	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
@@ -185,35 +244,26 @@
 		start_ci_charging();
 		return;
 	case FCHG_STATE_CI_CHARGING:
-		charge_progress_trace("CI", msg->data[2]);
+		ci_progress_trace(msg->data[2]);
 		if (!(msg->data[9] & CHGPRES)) {
 			ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
 			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
 			return;
 		}
-		if (overvoltage_end_charge_check())
-			return;
 		if (pwr_ctrl->batt_mv >= pwr_ctrl->config.ci2cv_thresh)
-			start_cv_charging(msg->data[0]);
+			start_cv_charging();
 		return;
 	case FCHG_STATE_CV_CHARGING:
-		charge_progress_trace("CV", msg->data[2]);
 		if (!(msg->data[9] & CHGPRES)) {
 			ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
 			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
 			return;
 		}
+		if (cv_ichg_process(msg->data[2]))
+			return;
 		if (overvoltage_end_charge_check())
 			return;
-		if (msg->data[2] >=
-		    pwr_ctrl->config.end_current + pwr_ctrl->i2v_offset)
-			return;
-		rvf_send_trace("Stopping charge by low current condition", 40,
-				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH,
-				FCHG_USE_ID);
-		ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
-		pwr_init_discharge();
-		pwr_ctrl->state = FCHG_STATE_READY_TO_RECHARGE;
+		/* DAC control loop will go here */
 		return;
 	default:
 		rvf_send_trace("Invalid state in pwr_process_adc()", 32,