--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/chipsetsw/drivers/drv_app/lcc/lcc_handle_message.c	Mon Jun 01 03:24:05 2015 +0000
@@ -0,0 +1,1082 @@
+/******************************************************************************
+ * Power Task (pwr)
+ * Design and coding by Svend Kristian Lindholm, skl@ti.com
+ *
+ * PWR Message Processing
+ *
+ * $Id: pwr_handle_message.c 1.3 Wed, 20 Aug 2003 15:17:25 +0200 skl $
+ *
+ ******************************************************************************/
+#include <string.h>
+#include "lcc/lcc.h"
+#include "lcc/lcc_api.h"
+#include "lcc/lcc_trace.h"
+#include "lcc/lcc_task.h"
+#include "lcc/lcc_tm_i.h"
+#include "lcc/lcc_cfg_i.h"
+#include "lcc/lcc_cfg.h"
+#include "lcc/lcc_modulate.h"
+
+#include "rv/rv_defined_swe.h"
+
+#include "ffs/ffs.h"
+
+#include "etm/etm_env.h"
+
+#include "abb/abb.h"
+/******************************************************************************
+ * Function prototypes
+ ******************************************************************************/
+
+void ttr(unsigned trmask, char *format, ...);
+void str(unsigned mask, char *string);
+
+void *pwr_malloc(int size);
+
+T_RVM_RETURN pwr_task_init(void);
+
+UINT32 pwr_timer_elapsed(UINT32 time_begin, UINT32 current_timer);
+void build_name(const char *ch_pre, char *cfg_id , UINT8 index, const char * ch_post,  char * name);
+
+void pwr_modulate_init(void);
+void pwr_modulate_on(void);
+int pwr_capacity(uint16 Vbat);
+
+void pwr_check_timers();
+void pwr_stop_timers();
+
+T_RVM_RETURN pwr_start_timer(UINT32 *timer_begin);
+T_RVM_RETURN pwr_stop_timer(UINT32 *timer_begin);
+
+T_RVM_RETURN pwr_check_files(void);
+T_RVM_RETURN pwr_read_files(void);
+T_RVM_RETURN pwr_read_chg_files(void);
+T_RVM_RETURN pwr_read_cal_files(void);
+
+void pwr_send_msg(uint32 msg_id ,T_RVF_ADDR_ID src_addr_id, T_RVF_ADDR_ID  dest_addr_id);
+void mmi_send_msg(struct mmi_info_ind_s *event);
+
+void start_q401_charge(void);
+void start_q402_charge(void);
+void start_pre_charge(void);
+void stop_q401_charge(void);
+void stop_q402_charge(void);
+void stop_pre_charge(void);
+void charger_unplug_house_keeping(void);
+void cv_charging_house_keeping(void);
+void end_charging_house_keeping(void);
+int check_chg_unplug(void);
+
+#if (TEST_PWR_MMI_INTERFACE == 1)
+    #include "lcc/pwr_mmi_api.c"
+#endif
+
+extern T_PWR_CTRL_BLOCK *pwr_ctrl;
+extern T_PWR_CFG_BLOCK  *pwr_cfg;
+
+/******************************************************************************
+ * Functions 
+ ******************************************************************************/
+
+void build_name(const char *ch_pre, char *cfg_id, UINT8 index, const char * ch_post,  char * name) 
+{
+    char tmp[2];
+
+    strcpy(name, ch_pre);
+    tmp[0] = *cfg_id;
+    tmp[1] = 0; // null-termination of strings!
+    strcpy(&(name[index]), tmp);
+    strcat(name, ch_post);
+    ttw(ttr(TTrInit, "build_name: '%s'" NL, name));
+}
+
+
+T_RVM_RETURN pwr_task_init()
+{
+// Perform battery initialization of the pwr task by
+// reading battery id and FFS configuration files
+    T_FFS_SIZE       error;
+
+    ttw(ttr(TTrInit, "pwr_task_init(%d)" NL, 0));
+
+    // Check configuration and calibration files
+    if ((error = pwr_check_files()) < 0)
+        return error;
+
+    // Read configuration files
+    if ((error = pwr_read_files()) < 0)
+        return error;
+
+    ttw(ttr(TTrInit, "pwr_task_init(%d)" NL, 0xFF));
+    return RV_OK;
+
+}
+
+int pwr_chg_start(int Tbat) {
+    int capacity;
+    // Tbat is the temperature measured in Celsius
+    // Check start conditions for charging - it is assumed that a charger is plugged
+    // This check is used in state SUP
+
+    // Charging start will _not_ be performed if ...
+    // 1) Battery temperature is outside limits (measurement)
+    // 2) Charger identity is unknown
+
+    ttw(ttr(TTrCharge, "pwr_chg_start(%d)" NL, 0));
+
+    // 1)
+    // Battery temperature is outside limits (measurement)
+    // FIXME: Check also low temperatures
+    //    if ((Tbat < pwr_cfg->temp.tbat_min) || (Tbat > pwr_cfg->temp.tbat_max)) {
+    if ((Tbat > pwr_cfg->temp.tbat_max)) {
+        ttw(ttr(TTrCharge, "pwr_chg_start(%d) Tbat=(%d)" NL, 0xFF, Tbat));
+        return FALSE;
+    }
+
+    // 2) 
+    // Charger identity unknown
+    if ((pwr_ctrl->flag_chg_unknown == 1) && (pwr_ctrl->flag_mmi_registered == 1)) {
+        ttw(ttr(TTrCharge, "pwr_chg_start(%d) Chg unknown=(%d)" NL, 0xFF, Tbat));
+        pwr_ctrl->mmi_ptr->header.msg_id = MMI_CHG_UNKNOWN_IND;
+        mmi_send_msg(pwr_ctrl->mmi_ptr);
+        // Send only once
+        pwr_ctrl->flag_bat_unknown = 0;
+        return FALSE;
+    }
+
+    ttw(ttr(TTrCharge, "pwr_chg_start(%d)" NL, 0xFF));
+    return TRUE;
+}
+
+int pwr_chg_stop(int Tbat, int Vbat) {
+    // Check stop conditions for charging
+    // I.e.
+    // - temperature (RF+Battery) thresholds
+    // - battery voltage thresholds
+    // - regulation loop gain (duty cycle)
+    //
+    // This check is used regularly in state CCI, LCI, CCV and LCV
+
+    // Charging will be stopped if ...
+    // 1) Battery temperature is outside limits
+    // 2) Battery voltage is equal to - or above - charging stop battery voltage
+
+    // Nickel checks:
+    // 3) The RF temperature versus battery temperature rise is above threshold (Possible?)
+    //
+    // Lithium checks:
+    // 4) The regulation parameter k is below configured threshold limit
+    // Returns TRUE if the charging algorithm should be stopped
+    // Returns FALSE if the charging algorithm should be continued
+
+
+    ttw(ttr(TTrCharge, "pwr_chg_stop(%d)" NL, 0));
+
+    // 1)
+    // Battery temperature is outside limits
+    // FIXME: Check also low temperatures
+    if (Tbat > pwr_cfg->temp.tbat_max) {
+        ttw(ttr(TTrCharge, "pwr_chg_stop(%d) Tbat=(%d)" NL, 0xFF, Tbat));
+        if (pwr_ctrl->flag_mmi_registered == 1) {
+            pwr_ctrl->mmi_ptr->header.msg_id = MMI_CHG_FAILED_IND;
+            pwr_ctrl->mmi_ptr->cause         = BATTERY_TEMPERATURE_HIGH;
+            mmi_send_msg(pwr_ctrl->mmi_ptr);
+        }
+        return TRUE;
+    }
+
+    // 2)
+    // Battery voltage is equal to - or above - charging stop battery voltage
+    if ((Vbat > pwr_cfg->bat.chg_stop_thr)) {
+        ttw(ttr(TTrCharge, "pwr_chg_stop(%d) Vbat=(%d)" NL, 0xFF, Vbat));
+        ttw(ttr(TTrCharge, "pwr_cfg->bat.chg_stop_thr=%d" NL, 0xFF, pwr_cfg->bat.chg_stop_thr));
+        return TRUE;
+    }
+#if 0
+#ifndef PWR_FFS_CFG
+    pwr_cfg->common.rise_thr = PWR_RISE_THR ;
+#endif
+
+    // 3)
+    // The RF temperature versus battery temperature rise is above threshold (Possible?)
+    // Did the battery temperature rise more than the RF temperature rise - stop for Nickel charging!
+    // Only for Nickel batteries
+    // FIXME: How big time difference should there be between the deltas? Configurable?
+    if ((pwr_cfg->data.dTbat - pwr_cfg->data.dT_rf > pwr_cfg->common.rise_thr) == TRUE) {
+        ttw(ttr(TTrCharge, "pwr_chg_stop(%d) d(Tbat-Trf)=(%d)" NL, 0xFF, pwr_cfg->data.dTbat - pwr_cfg->data.dT_rf));
+        return TRUE;
+    }
+#endif
+
+    // 4) FIXME: Remove states in this part of the function
+    // Regulation parameter k is below the configured limit
+    if ((pwr_cfg->bat.type == LITHIUM) &&
+        ((pwr_ctrl->state == LCV) || (pwr_ctrl->state == CCV)) &&
+        (pwr_cfg->data.k < pwr_cfg->bat.chg_ctrl_thr)) {
+        ttw(ttr(TTrCharge, "k=%d < %d" NL, pwr_cfg->data.k, pwr_cfg->bat.chg_ctrl_thr));
+        return TRUE;
+    }
+
+    ttw(ttr(TTrCharge, "pwr_chg_stop(%d)" NL, 0xFF));
+    return FALSE;
+}
+
+int pwr_chg_ci_cv_transition(int Vbat) {
+    // Check CI -> CV condition for charging
+    // Only for Lithium batteries
+    // Returns TRUE if CI-> CV transition should be made
+    // Returns FALSE if CI-> CV transition should NOT be made
+
+    ttw(ttr(TTrCharge, "pwr_chg_ci_cv_transition(%d)" NL, 0));
+
+    // 1) Check CV charging start threshold
+    if ((Vbat > pwr_cfg->bat.chg_start_thr)) {
+        ttw(ttr(TTrCharge, "pwr_chg_ci_cv_transition(%d) Vbat=(%d)" NL, 0xFF, Vbat));
+        return TRUE;
+    }
+
+    ttw(ttr(TTrCharge, "pwr_chg_ci_cv_transition(%d)" NL, 0xFF));
+    return FALSE;
+}
+
+int pwr_capacity(uint16 Vbat) {
+    // Calculate the capacity, C(T,V), of the battery based on
+    // - Battery Voltage Measurement
+    // - Battery Temperature Measurement
+    // - Configuration Parameters
+    // Returns a value in the interval 0..100 [%]
+    uint8 i;
+    uint8 cap;
+
+    ttw(ttr(TTrEventLow,"pwr_capacity (Vbat=%d)" NL, Vbat));
+    cap = 100; // 100%
+    for (i=0;i<=9;i++) {
+        if (Vbat < pwr_cfg->temp.cap[i] ) {
+            cap -= 10; // Count down 10% for each index
+            ttw(ttr(TTrEventLow,"Trying cap=%d" NL, cap));
+        } else {
+            // FIXME: Interpolate ... instead of truncate
+            ttw(ttr(TTrEventLow,"capacity=%d" NL, cap));
+            return cap;
+        }
+    }
+    ttw(ttr(TTrEventLow,"pwr_capacity (%d)" NL, 0xFF));
+    return 0; // Couldn't lookup capacity
+}
+
+int8 pwr_temp_lookup(uint16 ADC, uint8 i_meas) {
+// temperature = f(ADC, I_meas)
+// f(x) is table-lized using ETM command 'pww 4 ...'
+
+    int i;
+    int temp;
+
+#define MAX_TEMP_SIZE 9
+
+    i = 0;
+    temp = -20;
+    // FIXME: Always using the second current - should also use the first...
+    while (ADC < pwr_cfg->temp.v2t_2[i] && i < MAX_TEMP_SIZE) {
+        ttw(ttr(TTrEventLow,"ADC=%d < pwr_cfg->temp.v2t_2[%d]=0x%x" NL, ADC,i, pwr_cfg->temp.v2t_2[i]));
+        ttw(ttr(TTrEventLow,"temp=%d" NL, temp));
+        i++;
+        temp += 10;
+    }
+    if (i == MAX_TEMP_SIZE) {
+        // No temperature found - return NULL value (0xFF)
+        ttr(TTrWarning, "Temperature lookup failed %d" NL, ADC);
+        return 0xFF;
+    }
+    if (i != 0) {
+        // Interpolate
+        if (pwr_cfg->temp.v2t_2[i-1] == 0xFFFF)
+            temp += 10*(ADC - pwr_cfg->temp.v2t_2[i])/(pwr_cfg->temp.v2t_2[i] - 1024);
+        else
+            temp += 10*(ADC - pwr_cfg->temp.v2t_2[i])/(pwr_cfg->temp.v2t_2[i] - pwr_cfg->temp.v2t_2[i-1]);
+        ttw(ttr(TTrEventLow,"Interpolated temp=%d" NL, temp));
+    }
+    return ((int8)temp);
+}
+
+
+enum {
+    Vbat = 0,
+    Vchg = 1,
+    Ichg = 2,
+    Vbt2 = 3,
+    Type = 4,
+    Tbat = 5,
+    T_rf = 6,
+    Tc_x = 7,
+    Tc_y = 8,
+    State= 9
+} adc_index_e;
+
+// The driving element of the LCC task - the ADC measurements
+T_RV_RET process_spi_adc_indication (T_PWR_REQ *request)
+{
+    struct pwr_adc_ind_s *ind;
+    int error, i, index;
+    int8 temp;
+    UINT32 timer;
+    UINT16 status; // Result of charger plug - unplug - polled
+
+    ind = (struct pwr_adc_ind_s*)request;
+
+    ttw(ttr(TTrEventLow,"State=%d" NL, ind->data[State]));
+
+    // Basic trace - not for CAL
+    switch (pwr_ctrl->state) {
+    case PRE :
+        ttw(ttr(TTrEventLow,"Vbat=%d" NL, ADC_to_mV(ind->data[Vbat])));
+        ttw(ttr(TTrEventLow,"Vchg=%d" NL, ADC_to_mV(ind->data[Vchg])));
+        ttw(ttr(TTrEventLow,"Ichg=%d" NL, ind->data[Ichg]));
+        break;
+    case INI :
+    case SUP :
+    case CCI :
+    case LCI :
+    case CCV :
+    case LCV :
+        ttw(ttr(TTrEventLow,"Vbat=%d" NL, ADC_to_mV(ind->data[Vbat])));
+        ttw(ttr(TTrEventLow,"Vbat_adc=%d" NL, ind->data[Vbat]));
+#if (USE_Q402_CHG_CIRCUIT == 1)
+        // Vchg Scaling factor for Q402 circuit
+        pwr_cfg->data.Vchg = 5*ADC_to_mV(ind->data[Vchg])/4;
+        ttw(ttr(TTrEventLow,"Vchg=%d" NL, pwr_cfg->data.Vchg));
+#else
+        pwr_cfg->data.Vchg = ADC_to_mV(ind->data[Vchg]);
+        ttw(ttr(TTrEventLow,"Vchg=%d" NL, pwr_cfg->data.Vchg));
+#endif
+        ttw(ttr(TTrEventLow,"Ichg=%d" NL, ind->data[Ichg]));
+        ttw(ttr(TTrEventLow,"Type=%d" NL, ind->data[Type]));
+        ttw(ttr(TTrEventLow,"Tbat=%d" NL, ind->data[Tbat]));
+        ttw(ttr(TTrEventLow,"T_rf=%d" NL, ind->data[T_rf]));
+        ttw(ttr(TTrEventLow,"Vbt2=%d" NL, ADC_to_mV(ind->data[Vbt2])));
+        break;
+    }
+
+    // Updates variables in 'service' mode - not for CAL, PRE & INI
+    switch (pwr_ctrl->state) {
+    case SUP :
+    case CCI :
+    case LCI :
+    case CCV :
+    case LCV :
+        index = pwr_ctrl->index % CONSECUTIVE_CHG_UNPLUGS;
+        ttw(ttr(TTrEventLow,"Using index: (%d)" NL, index));
+        pwr_ctrl->index++;
+        // Get current nucleus time
+        timer  = rvf_get_tick_count();
+
+        // T0 timer expired?
+        pwr_ctrl->time_elapsed_T0  = pwr_timer_elapsed(pwr_ctrl->time_begin_T0, timer);
+        ttw(ttr(TTrTimerLow,"T0: %d ms elapsed " NL, pwr_ctrl->time_elapsed_T0));
+
+        // Compare T0 with configured sampling rate
+        if (pwr_ctrl->time_elapsed_T0 > pwr_cfg->common.sampling) {
+            ttw(ttr(TTrTimer, "T0 Reset: elapsed (%d)" NL, pwr_ctrl->time_elapsed_T0));
+            pwr_ctrl->time_begin_T0 = timer;
+            ttr(TTrAll,"Vbat_avg=%d" NL, ADC_to_mV(pwr_cfg->data.Vbat_avg));
+
+        }
+
+        pwr_cfg->data.Vbat = ind->data[Vbat]; 
+        pwr_cfg->data.Vbat_avg = (ind->data[Vbat] + pwr_cfg->common.alfa1 * pwr_cfg->data.Vbat_avg)/(pwr_cfg->common.alfa1 + 1);
+        pwr_cfg->data.Vbat_avg_mV = ADC_to_mV(pwr_cfg->data.Vbat_avg);
+        ttw(ttr(TTrEventLow,"Vbat_avg=%d" NL, pwr_cfg->data.Vbat_avg_mV));
+
+        pwr_cfg->data.Tbat = ind->data[Tbat];
+        pwr_cfg->data.Tbat_avg = (ind->data[Tbat] + pwr_cfg->common.alfa2 * pwr_cfg->data.Tbat_avg)/(pwr_cfg->common.alfa2 + 1);
+        ttw(ttr(TTrEventLow,"Tbat_avg=%d" NL, pwr_cfg->data.Tbat_avg));
+        temp = pwr_temp_lookup(pwr_cfg->data.Tbat_avg, 0x01);
+        ttw(ttr(TTrEventLow,"temp=%d" NL, temp));
+
+        pwr_cfg->data.T_rf_avg = ind->data[T_rf];
+        pwr_cfg->data.T_rf_avg = (ind->data[T_rf] + pwr_cfg->common.alfa3 * pwr_cfg->data.T_rf_avg)/(pwr_cfg->common.alfa3 + 1);
+        ttw(ttr(TTrEventLow,"T_rf_avg=%d" NL, pwr_cfg->data.T_rf_avg));
+
+        pwr_cfg->data.Ichg = ind->data[Ichg];
+        break;
+    }
+
+    // 'Main' switch
+    ttw(ttr(TTrEventLow,"state=%d" NL, pwr_ctrl->state));
+    switch (pwr_ctrl->state) {
+    case CAL :
+        // Read calibration files
+        // Decide if SW precharge should be started (state PRE) or if a normal configuration scheme should be inititated (state INI)
+        error = pwr_read_cal_files();
+        if (error >= 0) {
+            // Save measurements - also initial values for averaging
+            pwr_cfg->data.Vbat_avg = pwr_cfg->data.Vbat = ind->data[Vbat];
+            pwr_cfg->data.Vbat_avg_mV = ADC_to_mV(pwr_cfg->data.Vbat_avg);
+            ttw(ttr(TTrEventLow,"Vbat=%d" NL, ADC_to_mV(ind->data[Vbat])));
+            ttw(ttr(TTrEventLow,"Vchg=%d" NL, ADC_to_mV(ind->data[Vchg])));
+
+            // Charger was already inserted?
+            status = ind->data[State];
+
+            if (status & CHGPRES) {
+                // Charger plugged caused a wakeup
+                pwr_ctrl->flag_chg_plugged = 1;
+                pwr_ctrl->flag_chg_prev_plugged = 1;
+                ttw(ttr(TTrInitLow,"Polled - charger plugged  (%d)" NL, status));
+            }
+
+            // Check Pre-charge - immediately change state to PRE
+            if ((pwr_cfg->data.Vbat_avg_mV < VBAT_PRECHG_START) && (pwr_ctrl->flag_chg_plugged == 1)) {
+                ttw(ttr(TTrInitLow,"precharge (%d)" NL, ADC_to_mV(ind->data[Vbat])));
+                // Charger interrupts (plug/unplug) are already masked off - in case it was a linear charger
+                pwr_ctrl->state = PRE;
+            } else {
+                pwr_ctrl->state = INI;
+            }
+        }
+        break;
+    case PRE :
+        // Start fast charge immediately after 3.2V boot
+        // Enter INI state - in order to read FFS configuration files - when Vbat reaches 3.6V
+
+        // Hardcoding moving average to PRECHG_AVG_WINDOW_SIZE since we haven't read any configuration
+        pwr_cfg->data.Vbat = ind->data[Vbat];
+        pwr_cfg->data.Vbat_avg = (ind->data[Vbat] + PRECHG_AVG_WINDOW_SIZE  * pwr_cfg->data.Vbat_avg)/(PRECHG_AVG_WINDOW_SIZE + 1); 
+        pwr_cfg->data.Vbat_avg_mV = ADC_to_mV(pwr_cfg->data.Vbat_avg);
+        if (pwr_cfg->data.Vbat_avg_mV > VBAT_PRECHG_STOP) {
+            ttw(ttr(TTrInitLow,"state PRE (%d > VBAT_PRECHG_STOP)" NL, pwr_cfg->data.Vbat_avg_mV));
+            pwr_ctrl->state = INI;
+            pwr_ctrl->flag_prechg_started = 0;
+            stop_pre_charge();
+        } else {
+            ttw(ttr(TTrInitLow,"state PRE (%d < VBAT_PRECHG_STOP)" NL, pwr_cfg->data.Vbat_avg_mV));
+            // Start fast charging NOW
+            if (pwr_ctrl->flag_prechg_started == 0) {
+                pwr_ctrl->flag_prechg_started = 1;
+                start_pre_charge();
+            }
+        }
+        break;
+    case INI :
+        // Charger was already inserted?
+        status = ind->data[State];
+        pwr_ctrl->chg_unplug_vec[index] = (status & CHGPRES);
+        if (status & CHGPRES) {
+            // Charger plugged caused a wakeup
+            pwr_ctrl->flag_chg_plugged = 1;
+            pwr_ctrl->flag_chg_prev_plugged = 1;
+            ttw(ttr(TTrInitLow,"Polled - charger plugged  (%d)" NL, status));
+        }
+
+        if (pwr_ctrl->flag_ini_virgo == 0) {
+            // Perform some very basic initialization
+
+            /* Precharge (C/20) is switched OFF since it was finished in state PRE */
+            ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0002);
+
+            // Enable battery type identification
+            // Switch on battery type identification (TYPEN)
+            // NOTE: Battery type identification is ALWAYS done with 10uA (TYPEN)
+            ttw(ttr(TTrInitLow,"Enable type ident (%d)" NL, 0x00));
+            ABB_Write_Register_on_page(PAGE0, BCICTL1, THERMAL_SENSOR_30uA | TYPEN);
+            pwr_ctrl->flag_ini_virgo = 1;
+
+        }
+
+        // Don't read battery files before we know which battery id to use
+        // We will ignore the first BATTERY_TYPE_SLIP measurements of battery id
+        if ((pwr_ctrl->count_bat_type == BATTERY_TYPE_SLIP))  {
+
+            error = pwr_task_init();
+
+            // Change state
+            pwr_ctrl->state = SUP;
+
+            // Readout /pwr/common.cfg
+            ttw(ttr(TTrInitLow,"common.cfg: pins: %d" NL, pwr_cfg->common.pins));
+            ttw(ttr(TTrInitLow,"chg_dedic %d" NL, pwr_cfg->common.chg_dedic));
+            ttw(ttr(TTrInitLow,"sampling %d" NL, pwr_cfg->common.sampling));
+            ttw(ttr(TTrInitLow,"mod_cycle %d" NL, pwr_cfg->common.mod_cycle));
+            ttw(ttr(TTrInitLow,"alfa1 %d" NL, pwr_cfg->common.alfa1));
+            ttw(ttr(TTrInitLow,"alfa2 %d" NL, pwr_cfg->common.alfa2));
+            ttw(ttr(TTrInitLow,"alfa3 %d" NL, pwr_cfg->common.alfa3));
+            ttw(ttr(TTrInitLow,"rise_thr %d" NL, pwr_cfg->common.rise_thr));
+
+            // Readout /pwr/bat/bat.cfg
+            ttw(ttr(TTrInitLow,"bat.cfg: type: %d" NL, pwr_cfg->bat.type));
+            ttw(ttr(TTrInitLow,"rf_temp: %d" NL, pwr_cfg->bat.rf_temp));
+            ttw(ttr(TTrInitLow,"id_low: %d" NL, pwr_cfg->bat.id_low));
+            ttw(ttr(TTrInitLow,"id_high: %d" NL, pwr_cfg->bat.id_high));
+            ttw(ttr(TTrInitLow,"cbat: %d" NL, pwr_cfg->bat.cbat));
+            ttw(ttr(TTrInitLow,"ratio: %d" NL, pwr_cfg->bat.ratio));
+            ttw(ttr(TTrInitLow,"T1: %d" NL, pwr_cfg->bat.T1));
+            ttw(ttr(TTrInitLow,"T2: %d" NL, pwr_cfg->bat.T2));
+            ttw(ttr(TTrInitLow,"T3: %d" NL, pwr_cfg->bat.T3));
+            ttw(ttr(TTrInitLow,"chg_start_thr: %d" NL, pwr_cfg->bat.chg_start_thr));
+            ttw(ttr(TTrInitLow,"chg_stop_thr: %d" NL, pwr_cfg->bat.chg_stop_thr));
+            ttw(ttr(TTrInitLow,"chg_ctrl_thr: %d" NL, pwr_cfg->bat.chg_ctrl_thr));
+            ttw(ttr(TTrInitLow,"chg_again_thr: %d" NL, pwr_cfg->bat.chg_again_thr));
+
+            // Readout /pwr/bat/temp<N>.cfg
+            ttw(ttr(TTrInitLow,"sizeof(temp): %d" NL, sizeof(pwr_cfg->temp)));
+            ttw(ttr(TTrInitLow,"sizeof(T_PWR_BAT_TEMP_CFG_BLOCK): %d" NL, sizeof(T_PWR_BAT_TEMP_CFG_BLOCK)));
+            ttw(ttr(TTrInitLow,"temp.cfg: tbat_min: %d" NL, pwr_cfg->temp.tbat_min));
+            ttw(ttr(TTrInitLow,"tbat_max: %d" NL, pwr_cfg->temp.tbat_max));
+            ttw(ttr(TTrInitLow,"i_meas1: %d" NL, pwr_cfg->temp.i_meas1));
+            for (i=0;i<=8;i++) {
+                ttw(ttr(TTrInitLow,"v2t_1[]: %d" NL, pwr_cfg->temp.v2t_1[i]));
+            }
+            ttw(ttr(TTrInitLow,"i_meas2: %d" NL, pwr_cfg->temp.i_meas2));
+            for (i=0;i<=8;i++) {
+                ttw(ttr(TTrInitLow,"v2t_2[]: %d" NL, pwr_cfg->temp.v2t_2[i]));
+            }
+            for (i=0;i<=9;i++) {
+                ttw(ttr(TTrInitLow,"cap[]: %d" NL, pwr_cfg->temp.cap[i]));
+            }
+            ttw(ttr(TTrInitLow,"a0: %d" NL, pwr_cfg->temp.a0));
+            ttw(ttr(TTrInitLow,"a1: %d" NL, pwr_cfg->temp.a1));
+            ttw(ttr(TTrInitLow,"a2: %d" NL, pwr_cfg->temp.a2));
+
+            // Readout /mmi/pwr/bsie.cfg
+            ttw(ttr(TTrInitLow,"mmi repetition: %d" NL, pwr_cfg->mmi.repetition));
+
+            // Initialize battery temperature - must not be made in the virgo part since Tbat=0
+            pwr_cfg->data.Tbat_avg = ind->data[Tbat];
+
+            // Setup thermal sensor - don't re-enable TYPEN
+            if (pwr_cfg->temp.i_meas2 == 50) {
+                ttw(ttr(TTrInitLow,"ABB set i_current: %d" NL, pwr_cfg->temp.i_meas2));
+                ABB_Write_Register_on_page(PAGE0, BCICTL1, THERMAL_SENSOR_50uA);
+            } else  if (pwr_cfg->temp.i_meas2 == 30) {
+                ttw(ttr(TTrInitLow,"ABB set i_current: %d" NL, pwr_cfg->temp.i_meas2));
+                ABB_Write_Register_on_page(PAGE0, BCICTL1, THERMAL_SENSOR_30uA);
+            } else {
+                // Default 30uA
+                ttw(ttr(TTrInitLow,"ABB set i_current default: %d" NL, pwr_cfg->temp.i_meas2));
+                ABB_Write_Register_on_page(PAGE0, BCICTL1, THERMAL_SENSOR_30uA);
+            }
+
+        } else {
+            // Compare battery id with values found in FFS
+            // First value(s) are NOT to be trusted
+            pwr_ctrl->count_bat_type++;
+            pwr_cfg->data.bat_id = ind->data[Type];
+
+        }
+        break;
+    case SUP :
+        // Charger inserted?? Poll!!
+        // Reason: 100Hz interrupts will occur if an unregulated charger is inserted & charger interrupts are enabled
+        //         This will cause an immediate crash!
+        status = ind->data[State];
+        pwr_ctrl->chg_unplug_vec[index] = (status & CHGPRES);
+        if (status & CHGPRES) {
+            // Charger is plugged - continue
+            ttw(ttr(TTrEventLow,"Polled - chg plugged (%d)" NL, status));
+            pwr_ctrl->flag_chg_plugged = 1;
+            pwr_ctrl->capacity = pwr_capacity(pwr_cfg->data.Vbat_avg_mV);
+        } else {
+            pwr_ctrl->flag_chg_plugged = 0;
+        }
+
+        if (pwr_ctrl->flag_mmi_registered == 1)  {
+
+            // If the charger is plugged and no charging is initiated this message will be repeated until charging has started
+            // Charger plugged
+            if ((pwr_ctrl->flag_chg_plugged == 1) &&  (pwr_ctrl->flag_chg_prev_plugged == 0)) {
+                pwr_ctrl->flag_chg_prev_plugged = 1;
+                pwr_ctrl->mmi_ptr->header.msg_id = MMI_CHG_PLUG_IND;
+                mmi_send_msg(pwr_ctrl->mmi_ptr);
+            }
+            // Charger unplugged
+            if ((pwr_ctrl->flag_chg_plugged == 0) && (pwr_ctrl->flag_chg_prev_plugged == 1)) {
+                pwr_ctrl->flag_chg_prev_plugged = 0;
+                pwr_ctrl->mmi_ptr->header.msg_id = MMI_CHG_UNPLUG_IND;
+                mmi_send_msg(pwr_ctrl->mmi_ptr);
+            }
+
+            if (pwr_ctrl->flag_bat_unknown == 1) {
+                // Battery unknown - inform the MMI
+                pwr_ctrl->mmi_ptr->header.msg_id = MMI_BAT_UNKNOWN_IND;
+                mmi_send_msg(pwr_ctrl->mmi_ptr);
+                // Send only once
+                pwr_ctrl->flag_bat_unknown = 0;
+            }
+
+        }
+        pwr_check_timers();
+
+        // If charger is plugged AND charger configuration is not read AND capacity is below configured limit - charger id Vchg(Load=0) must be measured
+        if ((pwr_ctrl->flag_chg_plugged == 1) && 
+            (pwr_ctrl->flag_chg_cfg_read == 0) && 
+            (pwr_ctrl->capacity < pwr_cfg->bat.chg_again_thr)
+           ) {
+
+            // Measurement loop - Take the 2nd Vchg(0) measurements
+            if (pwr_ctrl->count_chg_type == CHARGER_TYPE_SLIP)  {
+                if (pwr_cfg->data.Vchg > CHARGER_TYPE_OUT_OF_BOUND_LOW)
+                    pwr_cfg->data.chg_id = pwr_cfg->data.Vchg;
+                else {
+                    // Out of bound!!
+                    ttr(TTrWarning, "Charger id out of bound! %d" NL, pwr_cfg->data.Vchg);
+                    pwr_cfg->data.chg_id = CHARGER_TYPE_TYPICAL;
+                }
+                ttw(ttr(TTrEventLow,"Using chg id=%d" NL, pwr_cfg->data.chg_id));
+                error = pwr_read_chg_files();
+                pwr_ctrl->flag_chg_cfg_read = 1;
+
+                // Initialize the circular buffer with charger plugs
+                for (i=0;i<= CONSECUTIVE_CHG_UNPLUGS-1 ;i++)
+                    pwr_ctrl->chg_unplug_vec[i] = CHGPRES;
+
+                if (pwr_chg_start(pwr_temp_lookup(pwr_cfg->data.Tbat_avg, 0x01)) == TRUE ) {
+                    if (pwr_cfg->chg.type == CI) {
+                        pwr_ctrl->state = CCI;
+                        // Enable charger plug/unplug interrupts for a CI charger
+                        ABB_Write_Register_on_page(PAGE0, ITMASK, ALL_IT_UMSK);
+                    } else {
+                        if (pwr_cfg->chg.type == UNREGULATED)
+                            pwr_ctrl->state = LCI;
+                        else {
+                            ttr(TTrWarning, "Unknown charger type! %d" NL, pwr_cfg->chg.type);
+                            pwr_ctrl->state = LCI;
+                        }
+                    }
+#if (USE_Q401_CHG_CIRCUIT == 1)
+                    start_q401_charge();
+#endif
+
+#if (USE_Q402_CHG_CIRCUIT == 1)
+                    start_q402_charge();
+#endif
+
+                    // Stop the mmi info repetition timer
+                    pwr_stop_timer(&pwr_ctrl->time_begin_mmi_rep);
+                }
+            } else {
+                pwr_ctrl->count_chg_type++;
+                ttw(ttr(TTrEventLow,"count_chg_type=%d" NL, pwr_ctrl->count_chg_type));
+            }
+        }
+        break;
+    case CCI :
+        pwr_check_timers();
+        // We are in the middle of a CI charging process - check stop criterias
+        //
+        if ( pwr_chg_stop(pwr_temp_lookup(pwr_cfg->data.Tbat_avg, 0x01), pwr_cfg->data.Vbat_avg_mV) == FALSE) {
+            // Continue to charge - change state?
+            if ((pwr_cfg->bat.type == LITHIUM) && (pwr_chg_ci_cv_transition(pwr_cfg->data.Vbat_avg_mV)== TRUE)) {
+                // Change state to CCV
+                // Calculate k value
+                // Start T2 timer
+                // Start DC timer
+                // Start T4 timer based on k value
+                // Modulation ON
+                pwr_ctrl->state = CCV;
+                cv_charging_house_keeping();
+            }
+        } else {
+            // Change state
+            pwr_ctrl->state = SUP;
+            end_charging_house_keeping();
+
+        }
+        break;
+    case LCI :
+        // Poll ABB for charger unplug - Linear - unregulated chargers ONLY
+        status = ind->data[State];
+        pwr_ctrl->chg_unplug_vec[index] = (status & CHGPRES);
+        if (status & CHGPRES) {
+            // Charger is still plugged - continue
+            ttw(ttr(TTrEventLow,"Polled - chg plugged (%d)" NL, status));
+        } else {
+            if (check_chg_unplug() == 1) {
+                // Charger is not plugged anymore - stop!!
+                ttw(ttr(TTrEventLow,"Verdict - chg not plugged (%d)" NL, status));
+                // Change state
+                pwr_ctrl->state = SUP;
+                charger_unplug_house_keeping(); 
+                pwr_free(request);
+                return;
+            } else {
+                // False alarm - don't set flags
+                ttw(ttr(TTrEventLow,"Polled - chg not plugged - FALSE alarm? (%d)" NL, status));
+#if (USE_Q401_CHG_CIRCUIT == 1)
+                if ((ind->data[Ichg] == 0) && (pwr_cfg->data.Vchg > 0)) {
+                    // Charging current has disappeared due to a fast unplug??? Bug in IOTA ABB?
+                    pwr_ctrl->state = SUP;
+                    charger_unplug_house_keeping(); 
+                    pwr_free(request);
+                    return;
+                }
+#endif
+#if (USE_Q402_CHG_CIRCUIT == 1)
+                // FIXME: Really no false alarm code for Q402??
+#endif
+
+            }
+        }
+        pwr_check_timers();
+        // We are in the middle of a CI charging process - check stop criterias
+        if ( pwr_chg_stop(pwr_temp_lookup(pwr_cfg->data.Tbat_avg, 0x01), pwr_cfg->data.Vbat_avg_mV) == FALSE) {
+            // Continue to charge - change state?
+            if ((pwr_cfg->bat.type == LITHIUM) && (pwr_chg_ci_cv_transition(pwr_cfg->data.Vbat_avg_mV)== TRUE)) {
+                // Change state to LCV
+                // Calculate k value
+                // Start T2 timer
+                // Start DC timer
+                // Start T4 timer based on k value
+                // Modulation ON
+                pwr_ctrl->state = LCV;
+                cv_charging_house_keeping();
+            }
+        } else {
+
+            // Change state
+            pwr_ctrl->state = SUP;
+            end_charging_house_keeping();
+
+        }
+        break;
+    case CCV :
+        pwr_check_timers();
+        // We are in the middle of a CV charging process - check stop criterias
+        if (pwr_chg_stop(pwr_temp_lookup(pwr_cfg->data.Tbat_avg, 0x01), pwr_cfg->data.Vbat_avg_mV) == FALSE) {
+           // EMPTY - waiting for T4 and DC timeouts OR T2 timeout
+        } else {
+            // Change state
+            pwr_ctrl->state = SUP;
+            end_charging_house_keeping();
+        }
+        break;
+    case LCV :
+        // Poll ABB for charger unplug - Linear - unregulated chargers ONLY
+        status = ind->data[State];
+        pwr_ctrl->chg_unplug_vec[index] = (status & CHGPRES);
+        if (status & CHGPRES) {
+            // Charger is still plugged - continue
+            ttw(ttr(TTrEventLow,"Polled - chg plugged (%d)" NL, status));
+        } else {
+#if (USE_Q401_CHG_CIRCUIT == 1)
+            // Charger is not plugged anymore - stop!!
+            ttw(ttr(TTrEventLow,"Verdict - chg not plugged (%d)" NL, status));
+            // Change state
+            pwr_ctrl->state = SUP;
+            charger_unplug_house_keeping(); 
+            pwr_free(request);
+            return;
+#endif
+#if (USE_Q402_CHG_CIRCUIT == 1)
+            if (check_chg_unplug() == 1) {
+                // Charger is not plugged anymore - stop!!
+                ttw(ttr(TTrEventLow,"Verdict - chg not plugged (%d)" NL, status));
+                // Change state
+                pwr_ctrl->state = SUP;
+                charger_unplug_house_keeping(); 
+                pwr_free(request);
+                return;
+            }
+#endif
+        }
+        pwr_check_timers();
+        // We are in the middle of a CV charging process - check stop criterias
+        if (pwr_chg_stop(pwr_temp_lookup(pwr_cfg->data.Tbat_avg, 0x01), pwr_cfg->data.Vbat_avg_mV) == FALSE) {
+           // EMPTY - waiting for T4 and DC timeouts OR T2 timeout
+        } else {
+            // Change state
+            pwr_ctrl->state = SUP;
+            end_charging_house_keeping();
+        }
+        break;
+    default :
+        {
+        // Exception Handling - Unknown State
+        ttr(TTrFatal, "process_spi_adc_indication: Unknown State: %d" NL, pwr_ctrl->state);
+        }
+    }
+    pwr_free(request);
+    return RV_OK;
+}
+
+T_RV_RET process_abb_chg_unplugged_ind       (T_PWR_REQ *request)
+{
+
+    charger_unplug_house_keeping(); 
+    // Disable charger interrupts - they where enabled when the CI charger was connected
+    ABB_Write_Register_on_page(PAGE0, ITMASK, CHARGER_IT_MSK);
+
+    // A charger can be unplugged in any state
+    switch (pwr_ctrl->state) {
+    case SUP :
+    case CCI :
+    case CCV :
+        // Change state - T3 is running
+        pwr_ctrl->state = SUP;
+        break;
+    default :
+        {
+        // Exception Handling - Unknown State
+        ttr(TTrFatal, "process_abb_chg_unplugged_ind: Unknown State: %d" NL, pwr_ctrl->state);
+        }
+    }
+    pwr_free(request);
+    return RV_OK;
+}
+
+void pwr_send_msg(uint32 msg_id, T_RVF_ADDR_ID src_addr_id, T_RVF_ADDR_ID  dest_addr_id) {
+    struct pwr_req_s *msg;
+
+    if ((msg = pwr_malloc(sizeof(struct pwr_req_s))) == NULL) {
+        return;
+    }
+    msg->header.msg_id        = msg_id;
+    msg->header.src_addr_id   = src_addr_id;
+    msg->header.dest_addr_id  = dest_addr_id;
+    msg->header.callback_func = NULL;
+    if (rvf_send_msg(dest_addr_id, msg) != RV_OK) {
+        ttr(TTrFatal, "PWR FATAL: Send failed! %d" NL, 0xFF);
+    }
+}
+
+void mmi_send_msg(struct mmi_info_ind_s *event) {
+    ttw(ttr(TTrInit,"mmi_send_msg(%d)" NL, 0));
+    if (pwr_ctrl->rpath.callback_func) {
+        ttw(ttr(TTrInit,"Using callback (0x%x)" NL, pwr_ctrl->rpath.callback_func));
+        (pwr_ctrl->rpath.callback_func) (event);
+    } else {
+        if (pwr_ctrl->rpath.addr_id) {
+            // TESTME
+            rvf_send_msg(pwr_ctrl->rpath.addr_id, event);
+        } else {
+            ttr(TTrFatal,"PWR FATAL: mmi_send_msg(%d) No return path" NL, 0xFF);
+            return;
+        }
+    }
+    ttw(ttr(TTrInit,"mmi_send_msg(%d)" NL, 0xFF));
+}
+
+void start_pre_charge(void) {
+    ttw(ttr(TTrInitLow,"start_pre_charge" NL, 0x00));
+#if (USE_Q401_CHG_CIRCUIT == 1)
+    /* Program the DAC with the constant current value */
+    // Value is hardcoded!! No configuration exist for state PRE
+    ABB_Write_Register_on_page(PAGE0, CHGREG, ICHG_PRECHG); 
+
+    /* Enable the charger  */
+    ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0003);
+#endif
+#if (USE_Q402_CHG_CIRCUIT == 1)
+    pwr_modulate_init();
+#endif
+    ttw(ttr(TTrInitLow,"start_pre_charge" NL, 0xFF));
+
+}
+
+void start_q401_charge(void) {
+
+    ttw(ttr(TTrInitLow,"start_q401_charge" NL, 0x00));
+    if (pwr_ctrl->flag_mmi_registered == 1) {
+        pwr_ctrl->mmi_ptr->header.msg_id = MMI_CHG_START_IND;
+        mmi_send_msg(pwr_ctrl->mmi_ptr);
+    }
+
+    /* Program the DAC with the constant current value taken from /pwr/chg/chg<N>.cfg */
+    ABB_Write_Register_on_page(PAGE0, CHGREG, pwr_cfg->chg.ichg_max);
+
+    /* Enable the CI charger  */
+    ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0003);
+
+    /* If not already started - start the overall charger timer T1 */
+    if (pwr_ctrl->time_begin_T1 == 0) {
+        pwr_start_timer(&pwr_ctrl->time_begin_T1);
+        ttw(ttr(TTrTimerLow,"T1 started(%d)" NL, 0));
+    }
+    ttw(ttr(TTrInitLow,"start_q401_charge" NL, 0xFF));
+
+}
+
+void start_q402_charge(void) {
+
+    ttw(ttr(TTrInitLow,"start_q402_charge(%d)" NL, 0x00));
+    if (pwr_ctrl->flag_mmi_registered == 1) {
+        pwr_ctrl->mmi_ptr->header.msg_id = MMI_CHG_START_IND;
+        mmi_send_msg(pwr_ctrl->mmi_ptr);
+    }
+
+    // Needed if we want Ichg measurements - else they are disabled!!
+    ABB_Write_Register_on_page(PAGE0, CHGREG, pwr_cfg->chg.ichg_max);
+    ABB_Write_Register_on_page(PAGE0, BCICTL2, 0x0003);
+
+    pwr_modulate_init();
+
+    /* If not already started - start the overall charger timer T1 */
+    if (pwr_ctrl->time_begin_T1 == 0) {
+        pwr_start_timer(&pwr_ctrl->time_begin_T1);
+        ttw(ttr(TTrTimerLow,"T1 started(%d)" NL, 0));
+    }
+    ttw(ttr(TTrInitLow,"start_q402_charge(%d)" NL, 0xFF));
+
+}
+
+void stop_q401_charge(void) {
+
+    ttw(ttr(TTrInitLow,"stop_q401_charge(%d)" NL, 0x00));
+    if (pwr_ctrl->flag_mmi_registered == 1) {
+        pwr_ctrl->mmi_ptr->header.msg_id = MMI_CHG_STOP_IND;
+        mmi_send_msg(pwr_ctrl->mmi_ptr);
+    }
+    ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
+    ttw(ttr(TTrInitLow,"stop_q401_charge(%d)" NL, 0xFF));
+
+}
+
+void stop_q402_charge(void) {
+
+    ttw(ttr(TTrInitLow,"stop_q402_charge(%d)" NL, 0x00));
+    if (pwr_ctrl->flag_mmi_registered == 1) {
+        pwr_ctrl->mmi_ptr->header.msg_id = MMI_CHG_STOP_IND;
+        mmi_send_msg(pwr_ctrl->mmi_ptr);
+    }
+    ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
+
+    pwr_modulate_off();
+    ttw(ttr(TTrInitLow,"stop_q402_charge(%d)" NL, 0xFF));
+
+}
+
+void stop_pre_charge(void) {
+
+    ABB_Write_Register_on_page(PAGE0, BCICTL2, 0);
+
+}
+
+int check_chg_unplug() {
+    int i;
+
+#if (USE_Q401_CHG_CIRCUIT == 1)
+    // Check that the latest values of charger status confirm that we have a charger unplug
+    // Search through the vector - charger is only assumed unplugged when ALL CONSECUTIVE_CHG_UNPLUGS elements are 0
+    // Returns 1 (TRUE) if the charger is assumed unplugged
+    // Returns 0 (FALSE) if the charger is assumed plugged
+    // Assume that the charger unplug was true if the battery voltage is sufficiently high
+    if (pwr_cfg->data.Vbat_avg_mV < VBAT_PRECHG_STOP) {
+        for (i=0;i<= CONSECUTIVE_CHG_UNPLUGS-1 ;i++) {
+            ttw(ttr(TTrTimerLow,"chg_unplug_vec(%d)" NL, pwr_ctrl->chg_unplug_vec[i]));
+            if (pwr_ctrl->chg_unplug_vec[i] != 0)
+                return 0;
+        }
+    }
+    return 1;
+#endif
+#if (USE_Q402_CHG_CIRCUIT == 1)
+    // Assume that the charger unplug was true if BOTH the charger voltage & charger current 
+    // are below configurable Vchg & Ichg threshold values
+    if ((pwr_cfg->data.Vchg < VCHG_Q402_THR) && (pwr_cfg->data.Ichg < ICHG_Q402_THR)) {
+        ttw(ttr(TTrInitLow,"check_chg_unplug: Vchg=(%d)" NL, pwr_cfg->data.Vchg));
+        ttw(ttr(TTrInitLow,"check_chg_unplug: Ichg=(%d)" NL, pwr_cfg->data.Ichg));
+        return 1;
+    }
+    return 0;
+#endif
+}
+
+void charger_unplug_house_keeping(void) {
+    // Flag the charger unplug
+    pwr_ctrl->flag_chg_plugged = 0;
+    pwr_ctrl->flag_chg_prev_plugged = 0;
+
+    // Read charger configuration next time charger is inserted
+    pwr_ctrl->flag_chg_cfg_read = 0;
+
+    // We must have more charger id measurements if the charger is re-connected
+    pwr_ctrl->count_chg_type = 0;
+
+    if (pwr_ctrl->flag_mmi_registered == 1) {
+        // Send charger unplug to the MMI
+        pwr_ctrl->mmi_ptr->header.msg_id = MMI_CHG_UNPLUG_IND;
+        mmi_send_msg(pwr_ctrl->mmi_ptr);
+
+        // Re-start the mmi info repetition timer
+        pwr_start_timer(&pwr_ctrl->time_begin_mmi_rep);
+    }
+
+    // if T1 or T2 is running (which mean we have NOT had a normal charging stop)
+    if ((pwr_ctrl->time_begin_T1 != 0) || (pwr_ctrl->time_begin_T2 != 0)) {
+        // Start T3 timer if not already started
+        if (pwr_ctrl->time_begin_T3 == 0) {
+            pwr_start_timer(&pwr_ctrl->time_begin_T3);
+            ttw(ttr(TTrTimerLow,"T3 started(%d)" NL, 0));
+        }
+        // Switch of charging circuit
+
+#if (USE_Q401_CHG_CIRCUIT == 1)
+        stop_q401_charge();
+#endif
+#if (USE_Q402_CHG_CIRCUIT == 1)
+        stop_q402_charge();
+#endif
+    }
+}
+
+void cv_charging_house_keeping(void) {
+    // Calculate k value
+    // Start DC timer
+    // Start T4 timer based on k value
+    // Modulation ON
+    ttw(ttr(TTrTimerLow,"Vbat_avg, chg_start_thr, chg_stop_thr (%d), (%d), (%d)" NL, pwr_cfg->data.Vbat_avg_mV, pwr_cfg->bat.chg_start_thr, pwr_cfg->bat.chg_stop_thr));
+    if ((pwr_cfg->data.Vbat_avg_mV - pwr_cfg->bat.chg_start_thr) <= 0) {
+        // Assign k to max value if Vbat_avg value is below start value
+        pwr_cfg->data.k = PWR_MAX_K;
+    } else {
+        pwr_cfg->data.k = (255 * (pwr_cfg->bat.chg_stop_thr - pwr_cfg->data.Vbat_avg_mV) / (pwr_cfg->bat.chg_stop_thr - pwr_cfg->bat.chg_start_thr));
+        if (pwr_cfg->data.k <= PWR_MIN_K)
+           pwr_cfg->data.k = PWR_MIN_K;
+    }
+    pwr_start_timer(&pwr_ctrl->time_begin_T2);
+    ttw(ttr(TTrTimerLow,"T2 started(%d)" NL, 0));
+
+    ttw(ttr(TTrTimerLow, "k=%d" NL, pwr_cfg->data.k));
+    pwr_cfg->data.T4 = pwr_cfg->data.k * pwr_cfg->common.mod_cycle/255;
+    ttw(ttr(TTrTimerLow,"T4 timer (%d)" NL, pwr_cfg->data.T4));
+
+    ttw(ttr(TTrTimerLow,"DC started(%d)" NL, 0));
+    pwr_start_timer(&pwr_ctrl->time_begin_mod_cycle);
+
+    ttw(ttr(TTrTimerLow,"T4 started(%d)" NL, 0));
+    pwr_start_timer(&pwr_ctrl->time_begin_T4);
+
+    pwr_modulate_on();
+}
+
+void end_charging_house_keeping(void) {
+    // Stop & reset timers
+    pwr_stop_timers();
+
+    // Stop charging
+#if (USE_Q401_CHG_CIRCUIT == 1)
+    stop_q401_charge();
+#endif
+#if (USE_Q402_CHG_CIRCUIT == 1)
+    stop_q402_charge();
+#endif
+
+    // Start the mmi info repetition timer
+    if (pwr_ctrl->flag_mmi_registered == 1)
+        pwr_start_timer(&pwr_ctrl->time_begin_mmi_rep);
+
+    // Read charger configuration next time charger is inserted
+    pwr_ctrl->flag_chg_cfg_read = 0;
+}
+
+#if (TEST_PWR_MMI_INTERFACE == 1)
+// Callback function used for testing MMI reporting
+static void mmi_test_cb_function(void *ptr) {
+T_PWR_MMI_INFO_IND_EVENT *event;
+
+
+    event = (T_PWR_MMI_INFO_IND_EVENT *)ptr;
+
+    ttw(ttr(TTrInit, "mmi_test_cb_function (%d)" NL, 0));
+    ttw(ttr(TTrInit, "MMI event: (%d)" NL, event->header.msg_id));
+    ttw(ttr(TTrInit, "mmi_test_cb_function (%d)" NL, 0xFF));
+}
+#endif