--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/cs/drivers/drv_app/fchg/fchg_process.c	Sun Jul 15 04:39:26 2018 +0000
@@ -0,0 +1,487 @@
+/*
+ * In this module we are going to implement the main process functions
+ * for FCHG.
+ */
+
+#include "fchg/fchg_env.h"
+#include "fchg/fchg_func_i.h"
+#include "rv/rv_general.h"
+#include "rvf/rvf_api.h"
+#include "rvm/rvm_use_id_list.h"
+#include "abb/abb.h"
+#include <string.h>
+#include <stdio.h>
+
+extern UINT16 madc_vbat_2_physical(UINT16 adc_val);
+extern UINT16 madc_vbat_inverse(UINT16 mv);
+
+void pwr_init_discharge(void)
+{
+	pwr_ctrl->curr_disch_thresh = 0;
+}
+
+static void handle_discharge(void)
+{
+	UINT16 i;
+	char trace[64];
+
+	/* first we need to find the current threshold we are at */
+	i = pwr_ctrl->curr_disch_thresh;
+	/* is there one below? */
+	if (++i == pwr_ctrl->nb_thresholds)
+		return;
+	/* are we crossing it? */
+	if (pwr_ctrl->batt_mv >= pwr_ctrl->batt_thresholds[i].bat_voltage)
+		return;
+	/* yes, we crossed it - see if we fell even further down */
+	while (i < pwr_ctrl->nb_thresholds &&
+	       pwr_ctrl->batt_mv < pwr_ctrl->batt_thresholds[i].bat_voltage)
+		i++;
+	/* the last one was it */
+	i--;
+	pwr_ctrl->curr_disch_thresh = i;
+	sprintf(trace, "Battery fell through %u%% mark",
+		pwr_ctrl->batt_thresholds[i].remain_capa);
+	rvf_send_trace(trace, strlen(trace), NULL_PARAM,
+			RV_TRACE_LEVEL_WARNING, FCHG_USE_ID);
+}
+
+static void start_i2v_cal(void)
+{
+	UINT16 bciconf;
+
+	rvf_send_trace("Calibrating i2v offset", 22, NULL_PARAM,
+			RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
+	pwr_ctrl->state = FCHG_STATE_I2V_CAL_2;
+	bciconf = ABB_Read_Register_on_page(PAGE1, BCICONF);
+	bciconf &= 0x3E0;
+	bciconf |= pwr_ctrl->config.bciconf;
+	ABB_Write_Register_on_page(PAGE1, BCICONF, bciconf);
+	/*
+	 * Set the CHDISPA bit and start the zero calibration routine
+	 * of the I to V converter
+	 */
+	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0010);
+	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0019);
+}
+
+static void start_ci_charging(void)
+{
+	rvf_send_trace("Start CI charging", 17, NULL_PARAM,
+			RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
+	pwr_ctrl->state = FCHG_STATE_CI_CHARGING;
+	/* Select constant current charging. The charger is disabled */
+	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0002);
+	/* Program the DAC with the constant current value */
+	ABB_Write_Register_on_page(PAGE0, CHGREG,
+			pwr_ctrl->config.ci_current + pwr_ctrl->i2v_offset);
+	/* Enable the charger */
+	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0003);
+	/* The total charging time starts now */
+	pwr_ctrl->start_time = rvf_get_tick_count();
+}
+
+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 */
+	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)
+{
+	rvf_send_trace("Charge start condition met", 26, NULL_PARAM,
+			RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
+	if (pwr_ctrl->config.bciconf)
+		start_i2v_cal();
+	else {
+		pwr_ctrl->i2v_offset = 0;
+		start_ci_charging();
+	}
+}
+
+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 - pwr_ctrl->config.cv_dac_max_decr))
+		return 0;
+	rvf_send_trace("Stopping charge by overvoltage condition", 40,
+			NULL_PARAM, RV_TRACE_LEVEL_WARNING, 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 void cv_ctrl_loop_high_check(void)
+{
+	if (pwr_ctrl->batt_mv < pwr_ctrl->config.cv_ctrl_loop_high) {
+		pwr_ctrl->cv_high_vbat_count = 0;
+		return;
+	}
+	pwr_ctrl->cv_high_vbat_count++;
+	if (pwr_ctrl->cv_high_vbat_count < pwr_ctrl->config.cv_samples_needed)
+		return;
+	if (pwr_ctrl->cv_dac_curr ==
+	    (pwr_ctrl->cv_dac_init - pwr_ctrl->config.cv_dac_max_decr))
+		return;
+	pwr_ctrl->cv_dac_curr--;
+	ABB_Write_Register_on_page(PAGE0, CHGREG, pwr_ctrl->cv_dac_curr);
+	rvf_send_trace("Sub CV DAC", 10, pwr_ctrl->cv_dac_curr,
+			RV_TRACE_LEVEL_DEBUG_MEDIUM, FCHG_USE_ID);
+	pwr_ctrl->cv_high_vbat_count = 0;
+}
+
+static void cv_ctrl_loop_low_check(void)
+{
+	if (pwr_ctrl->batt_mv >= pwr_ctrl->config.cv_ctrl_loop_low) {
+		pwr_ctrl->cv_low_vbat_count = 0;
+		return;
+	}
+	pwr_ctrl->cv_low_vbat_count++;
+	if (pwr_ctrl->cv_low_vbat_count < pwr_ctrl->config.cv_samples_needed)
+		return;
+	if (pwr_ctrl->cv_dac_curr ==
+	    (pwr_ctrl->cv_dac_init + pwr_ctrl->config.cv_dac_max_incr))
+		return;
+	pwr_ctrl->cv_dac_curr++;
+	ABB_Write_Register_on_page(PAGE0, CHGREG, pwr_ctrl->cv_dac_curr);
+	rvf_send_trace("Add CV DAC", 10, pwr_ctrl->cv_dac_curr,
+			RV_TRACE_LEVEL_DEBUG_MEDIUM, FCHG_USE_ID);
+	pwr_ctrl->cv_low_vbat_count = 0;
+}
+
+static int charging_time_limit_check(void)
+{
+	if ((rvf_get_tick_count() - pwr_ctrl->start_time) <
+	    RVF_SECS_TO_TICKS(pwr_ctrl->config.charge_time_limit))
+		return 0;
+	rvf_send_trace("Stopping charge by time exceeded condition", 42,
+			NULL_PARAM, RV_TRACE_LEVEL_WARNING, FCHG_USE_ID);
+	ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
+	pwr_init_discharge();
+	pwr_ctrl->state = FCHG_STATE_RECHARGE_TIMER;
+	pwr_ctrl->start_time = rvf_get_tick_count();
+	return 1;
+}
+
+void pwr_process_adc(struct pwr_adc_ind_s *msg)
+{
+	pwr_ctrl->batt_mv = madc_vbat_2_physical(msg->data[0]);
+
+	switch (pwr_ctrl->state) {
+	case FCHG_STATE_NO_EXT_PWR:
+	case FCHG_STATE_PWR_PLUG_TIMER:
+	case FCHG_STATE_NO_CHARGING:
+		handle_discharge();
+		return;
+	case FCHG_STATE_READY_TO_CHARGE:
+		handle_discharge();
+		if (!(msg->data[9] & CHGPRES)) {
+			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
+			return;
+		}
+		if (pwr_ctrl->batt_mv < pwr_ctrl->config.start_thresh)
+			start_charge_condition_met();
+		return;
+	case FCHG_STATE_READY_TO_RECHARGE:
+		handle_discharge();
+		if (!(msg->data[9] & CHGPRES)) {
+			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
+			return;
+		}
+		if (pwr_ctrl->batt_mv < pwr_ctrl->config.restart_thresh)
+			start_charge_condition_met();
+		return;
+	case FCHG_STATE_I2V_CAL_1:
+		if (!(msg->data[9] & CHGPRES)) {
+			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
+			return;
+		}
+		if (pwr_ctrl->config.bciconf)
+			start_i2v_cal();
+		else {
+			pwr_ctrl->i2v_offset = 0;
+			start_ci_charging();
+		}
+		return;
+	case FCHG_STATE_I2V_CAL_2:
+		pwr_ctrl->i2v_offset = msg->data[2];
+		ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
+		rvf_send_trace("i2v offset (MADC code) ", 23,
+				pwr_ctrl->i2v_offset,
+				RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
+		if (!(msg->data[9] & CHGPRES)) {
+			pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
+			return;
+		}
+		start_ci_charging();
+		return;
+	case FCHG_STATE_CI_CHARGING:
+		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 (charging_time_limit_check())
+			return;
+		if (pwr_ctrl->batt_mv >= pwr_ctrl->config.ci2cv_thresh)
+			start_cv_charging();
+		return;
+	case FCHG_STATE_CV_CHARGING:
+		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 (charging_time_limit_check())
+			return;
+		cv_ctrl_loop_high_check();
+		cv_ctrl_loop_low_check();
+		return;
+	case FCHG_STATE_RECHARGE_TIMER:
+		handle_discharge();
+		if ((rvf_get_tick_count() - pwr_ctrl->start_time) <
+		    RVF_SECS_TO_TICKS(pwr_ctrl->config.recharge_delay))
+			return;
+		rvf_send_trace("Restart time met, allowing new charging", 39,
+				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH,
+				FCHG_USE_ID);
+		pwr_ctrl->state = FCHG_STATE_READY_TO_RECHARGE;
+		return;
+	default:
+		rvf_send_trace("Invalid state in pwr_process_adc()", 32,
+				pwr_ctrl->state, RV_TRACE_LEVEL_ERROR,
+				FCHG_USE_ID);
+	}
+}
+
+void pwr_handle_timer(void)
+{
+	if (pwr_ctrl->state != FCHG_STATE_PWR_PLUG_TIMER)
+		return;
+	rvf_send_trace("Timer expired, ready to charge", 30, NULL_PARAM,
+			RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
+	pwr_ctrl->state = FCHG_STATE_READY_TO_CHARGE;
+}
+
+void pwr_charger_plug(void)
+{
+	if (pwr_ctrl->state != FCHG_STATE_NO_EXT_PWR) {
+		rvf_send_trace("Charger plug event in unexpected state", 38,
+				pwr_ctrl->state, RV_TRACE_LEVEL_ERROR,
+				FCHG_USE_ID);
+		return;
+	}
+	if (!pwr_ctrl->config_present) {
+		rvf_send_trace(
+			"Charger plugged in, but no config: won't charge", 47,
+			NULL_PARAM, RV_TRACE_LEVEL_ERROR, FCHG_USE_ID);
+		pwr_ctrl->state = FCHG_STATE_NO_CHARGING;
+		return;
+	}
+	if (pwr_ctrl->config.start_delay) {
+		rvf_send_trace("Charger plug, starting timer", 28, NULL_PARAM,
+				RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
+		rvf_start_timer(FCHG_TIMER,
+				RVF_MS_TO_TICKS(pwr_ctrl->config.start_delay),
+				FALSE);
+		pwr_ctrl->state = FCHG_STATE_PWR_PLUG_TIMER;
+	} else {
+		rvf_send_trace("Charger plug, ready to charge", 29, NULL_PARAM,
+				RV_TRACE_LEVEL_DEBUG_HIGH, FCHG_USE_ID);
+		pwr_ctrl->state = FCHG_STATE_READY_TO_CHARGE;
+	}
+}
+
+void pwr_charger_unplug(void)
+{
+	switch (pwr_ctrl->state) {
+	case FCHG_STATE_NO_EXT_PWR:
+		rvf_send_trace("Charger unplug, already handled", 31,
+				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_LOW,
+				FCHG_USE_ID);
+		/* nothing to do */
+		return;
+	case FCHG_STATE_PWR_PLUG_TIMER:
+	case FCHG_STATE_READY_TO_CHARGE:
+	case FCHG_STATE_READY_TO_RECHARGE:
+	case FCHG_STATE_I2V_CAL_1:
+	case FCHG_STATE_RECHARGE_TIMER:
+	case FCHG_STATE_NO_CHARGING:
+		rvf_send_trace("Charger unplug", 14, NULL_PARAM,
+				RV_TRACE_LEVEL_DEBUG_LOW, FCHG_USE_ID);
+		pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
+		return;
+	case FCHG_STATE_I2V_CAL_2:
+	case FCHG_STATE_CI_CHARGING:
+	case FCHG_STATE_CV_CHARGING:
+		ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
+		rvf_send_trace("Charger unplug, charging stopped", 32,
+				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH,
+				FCHG_USE_ID);
+		pwr_ctrl->state = FCHG_STATE_NO_EXT_PWR;
+		pwr_init_discharge();
+		return;
+	default:
+		rvf_send_trace("Invalid state in pwr_charger_unplug()", 35,
+				pwr_ctrl->state, RV_TRACE_LEVEL_ERROR,
+				FCHG_USE_ID);
+	}
+}
+
+void pwr_charge_start_req(void)
+{
+	switch (pwr_ctrl->state) {
+	case FCHG_STATE_NO_EXT_PWR:
+		rvf_send_trace("Cannot charge without a power source", 36,
+				NULL_PARAM, RV_TRACE_LEVEL_ERROR, FCHG_USE_ID);
+		return;
+	case FCHG_STATE_NO_CHARGING:
+		if (!pwr_ctrl->config_present) {
+			rvf_send_trace("No config set, cannot charge", 28,
+					NULL_PARAM, RV_TRACE_LEVEL_ERROR,
+					FCHG_USE_ID);
+			return;
+		}
+		/* FALL THRU */
+	case FCHG_STATE_PWR_PLUG_TIMER:
+	case FCHG_STATE_READY_TO_CHARGE:
+	case FCHG_STATE_READY_TO_RECHARGE:
+	case FCHG_STATE_RECHARGE_TIMER:
+		rvf_send_trace("Starting charge on user request", 31,
+				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH,
+				FCHG_USE_ID);
+		pwr_ctrl->state = FCHG_STATE_I2V_CAL_1;
+		return;
+	case FCHG_STATE_I2V_CAL_1:
+	case FCHG_STATE_I2V_CAL_2:
+	case FCHG_STATE_CI_CHARGING:
+	case FCHG_STATE_CV_CHARGING:
+		rvf_send_trace(
+			"Charging already in progress, start request ignored",
+			51, NULL_PARAM, RV_TRACE_LEVEL_WARNING, FCHG_USE_ID);
+		return;
+	default:
+		rvf_send_trace("Invalid state in pwr_charge_start_req()", 37,
+				pwr_ctrl->state, RV_TRACE_LEVEL_ERROR,
+				FCHG_USE_ID);
+	}
+}
+
+void pwr_charge_stop_req(void)
+{
+	switch (pwr_ctrl->state) {
+	case FCHG_STATE_NO_EXT_PWR:
+	case FCHG_STATE_NO_CHARGING:
+		/* nothing to do */
+		return;
+	case FCHG_STATE_PWR_PLUG_TIMER:
+	case FCHG_STATE_READY_TO_CHARGE:
+	case FCHG_STATE_READY_TO_RECHARGE:
+	case FCHG_STATE_I2V_CAL_1:
+	case FCHG_STATE_RECHARGE_TIMER:
+		rvf_send_trace("Charging disabled by user request", 33,
+				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH,
+				FCHG_USE_ID);
+		pwr_ctrl->state = FCHG_STATE_NO_CHARGING;
+		return;
+	case FCHG_STATE_I2V_CAL_2:
+	case FCHG_STATE_CI_CHARGING:
+	case FCHG_STATE_CV_CHARGING:
+		ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
+		rvf_send_trace("Charging stopped by user request", 32,
+				NULL_PARAM, RV_TRACE_LEVEL_DEBUG_HIGH,
+				FCHG_USE_ID);
+		pwr_ctrl->state = FCHG_STATE_NO_CHARGING;
+		pwr_init_discharge();
+		return;
+	default:
+		rvf_send_trace("Invalid state in pwr_charge_stop_req()", 36,
+				pwr_ctrl->state, RV_TRACE_LEVEL_ERROR,
+				FCHG_USE_ID);
+	}
+}