FG: Wrong current sample fix CSP 834010

Updated sampling rate to 4Hz
Added settling time before starting FG work
Avoid latch sample if read before sampling_t

Change-Id: I68f5e14212778d86c6871defc607ebbc8617af5d

1 files changed, 37 insertions, 45 deletions

diff --git a/drivers/power/bcmpmu-fg.c b/drivers/power/bcmpmu-fg.c
index 70b6a635b1f..f0c35edd6e8 100644
--- a/drivers/power/bcmpmu-fg.c
+++ b/drivers/power/bcmpmu-fg.c
@@ -72,6 +72,9 @@
 #define CRIT_BATT_POLL_TIME_MS		(2000)
 #define FAKE_BATT_POLL_TIME_MS		(60000)
 
+#define FG_SETTLING_TIME (bcmpmu_fg_sample_rate_to_time\
+			(fg->sample_rate) * 5)
+
 #define INTERPOLATE_LINEAR(X, Xa, Ya, Xb, Yb) \
 	(Ya + (((Yb - Ya) * (X - Xa))/(Xb - Xa)))
 
@@ -358,6 +361,7 @@ struct bcmpmu_fg_data {
 	int prev_cap_delta;
 	int max_discharge_current;
 	bool used_init_cap;
+	int init_notifier;
 
 	/* for debugging only */
 	int lock_cnt;
@@ -372,7 +376,8 @@ struct bcmpmu_fg_data {
 #endif
 
 #ifdef DEBUG
-#define DEBUG_MASK (BCMPMU_PRINT_ERROR | BCMPMU_PRINT_INIT | BCMPMU_PRINT_FLOW)
+#define DEBUG_MASK (BCMPMU_PRINT_ERROR | BCMPMU_PRINT_INIT |\
+			BCMPMU_PRINT_FLOW | BCMPMU_PRINT_VERBOSE)
 #else
 #define DEBUG_MASK (BCMPMU_PRINT_ERROR | BCMPMU_PRINT_INIT)
 #endif
@@ -398,6 +403,7 @@ static int bcmpmu_fg_get_curr_inst(struct bcmpmu_fg_data *fg);
 static int bcmpmu_fg_freeze_read(struct bcmpmu_fg_data *fg);
 static inline int bcmpmu_fg_sample_rate_to_time(
 		enum bcmpmu_fg_sample_rate rate);
+static int bcmpmu_fg_register_notifiers(struct bcmpmu_fg_data *fg);
 
 
 
@@ -420,9 +426,8 @@ static inline void fill_avg_sample_buff(struct bcmpmu_fg_data *fg)
 	fg->avg_sample.idx = 0;
 
 	for (i = 0; i < AVG_SAMPLES; i++) {
-		fg->avg_sample.volt[i] = bcmpmu_fg_get_batt_volt(fg->bcmpmu);
-		msleep(bcmpmu_fg_sample_rate_to_time(fg->sample_rate));
 		fg->avg_sample.curr[i] = bcmpmu_fg_get_curr_inst(fg);
+		fg->avg_sample.volt[i] = bcmpmu_fg_get_batt_volt(fg->bcmpmu);
 		fg->avg_sample.idx++;
 	}
 }
@@ -645,11 +650,6 @@ static int bcmpmu_fg_set_sample_rate(struct bcmpmu_fg_data *fg,
 
 	pr_fg(INIT, "set CIC sample rate to %d\n", rate);
 
-	/**
-	 * since we have changed the CIC filter rate, its
-	 * better to calibrate the offset for better accuracy
-	 */
-	bcmpmu_fg_calibrate_offset(fg, FG_HW_CAL_MODE_FAST);
 	return ret;
 }
 
@@ -708,21 +708,24 @@ static int bcmpmu_fg_reset(struct bcmpmu_fg_data *fg)
 	if (ret)
 		return ret;
 
+	fg->last_sample_tm = kona_hubtimer_get_counter();
+
 	return 0;
 }
 
 static int bcmpmu_fg_enable_coulb_counter(struct bcmpmu_fg_data *fg,
 		bool enable)
 {
-	bcmpmu_fg_reset(fg);
-	if (enable) {
-		bcmpmu_fg_enable(fg, true);
-		fg->flags.coulb_dis = false;
-	} else {
-		bcmpmu_fg_enable(fg, false);
-		fg->flags.coulb_dis = true;
+	if (fg->flags.coulb_dis == enable) {
+		bcmpmu_fg_reset(fg);
+		if (enable) {
+			bcmpmu_fg_enable(fg, true);
+			fg->flags.coulb_dis = false;
+		} else {
+			bcmpmu_fg_enable(fg, false);
+			fg->flags.coulb_dis = true;
+		}
 	}
-
 	return 0;
 }
 
@@ -1181,7 +1184,6 @@ static int bcmpmu_fg_get_curr_inst(struct bcmpmu_fg_data *fg)
 	int ret = 0;
 	u8 reg;
 	u8 smpl_cal[2];
-	int last_curr_sample;
 
 	FG_LOCK(fg);
 	/**
@@ -1191,19 +1193,11 @@ static int bcmpmu_fg_get_curr_inst(struct bcmpmu_fg_data *fg)
 	t_now = kona_hubtimer_get_counter();
 	t_ms = ((t_now - fg->last_curr_sample_tm) * 1000)/CLOCK_TICK_RATE;
 
-	if (t_ms < bcmpmu_fg_sample_rate_to_time(fg->sample_rate)) {
-		last_curr_sample = fg->last_curr_sample;
-		FG_UNLOCK(fg);
-		pr_fg(FLOW,
-			"Read time(%llu) < FG samp time(%d)\n",
-			t_ms, bcmpmu_fg_sample_rate_to_time(fg->sample_rate));
-		return last_curr_sample;
-	}
+	if (t_ms < bcmpmu_fg_sample_rate_to_time(fg->sample_rate))
+		msleep(bcmpmu_fg_sample_rate_to_time(fg->sample_rate) - t_ms);
 
-	fg->last_curr_sample_tm = t_now;
 
 	ret = fg->bcmpmu->read_dev(fg->bcmpmu, PMU_REG_FGCTRL2, &reg);
-
 	/**
 	 * Write 1  to FGFRZSMPL bit to latch the current sample to
 	 * FGSMPL_CAL registers
@@ -1215,9 +1209,12 @@ static int bcmpmu_fg_get_curr_inst(struct bcmpmu_fg_data *fg)
 			2);
 	BUG_ON(ret != 0);
 
+	fg->last_curr_sample_tm = kona_hubtimer_get_counter();
 	fg->last_curr_sample =
 		bcmpmu_fgsmpl_to_curr(fg, smpl_cal[1], smpl_cal[0]);
 	FG_UNLOCK(fg);
+	pr_fg(FLOW, "%s:fg->last_curr_sample=%d\n",
+		__func__, fg->last_curr_sample);
 	return fg->last_curr_sample;
 }
 
@@ -1304,8 +1301,8 @@ static int bcmpmu_fg_get_load_comp_capacity(struct bcmpmu_fg_data *fg,
 	int vbat_oc = 0;
 	int capacity_percentage = 0;
 
-	fg->adc_data.volt = bcmpmu_fg_get_batt_volt(fg->bcmpmu);
 	fg->adc_data.curr_inst = bcmpmu_fg_get_curr_inst(fg);
+	fg->adc_data.volt = bcmpmu_fg_get_batt_volt(fg->bcmpmu);
 	fg->adc_data.temp = bcmpmu_fg_get_batt_temp(fg);
 
 	if (abs(fg->adc_data.curr_inst) > FG_CURR_SAMPLE_MAX)
@@ -2027,7 +2024,6 @@ static int bcmpmu_fg_event_handler(struct notifier_block *nb,
 			fg->flags.chrgr_connected = false;
 			if (fg->bcmpmu->flags & BCMPMU_FG_VF_CTRL)
 				fg->vf_zone = -1;
-			bcmpmu_fg_calibrate_offset(fg, FG_HW_CAL_MODE_FAST);
 		} else if (fg->chrgr_type > PMU_CHRGR_TYPE_NONE &&
 				fg->chrgr_type < PMU_CHRGR_TYPE_MAX) {
 			pr_fg(FLOW, "charger connected!!\n");
@@ -2036,7 +2032,6 @@ static int bcmpmu_fg_event_handler(struct notifier_block *nb,
 			bcmpmu_fg_reset_cutoff_cnts(fg);
 			bcmpmu_fg_enable_coulb_counter(fg, true);
 			clear_avg_sample_buff(fg);
-			bcmpmu_fg_calibrate_offset(fg, FG_HW_CAL_MODE_FAST);
 			fg->flags.prev_batt_status = fg->flags.batt_status;
 			fg->flags.batt_status = POWER_SUPPLY_STATUS_CHARGING;
 		} else
@@ -2141,6 +2136,7 @@ static void bcmpmu_fg_hw_init(struct bcmpmu_fg_data *fg)
 	bcmpmu_fg_set_sync_mode(fg, true);
 	bcmpmu_fg_enable(fg, true);
 	bcmpmu_fg_calibrate_offset(fg, FG_HW_CAL_MODE_FAST);
+	bcmpmu_fg_reset(fg);
 }
 
 static int bcmpmu_fg_get_ocv_avg_capacity(struct bcmpmu_fg_data *fg,
@@ -2151,9 +2147,6 @@ static int bcmpmu_fg_get_ocv_avg_capacity(struct bcmpmu_fg_data *fg,
 	int cap_samples[50];
 
 	pr_fg(VERBOSE, "%s\n", __func__);
-	bcmpmu_fg_enable(fg, false);
-	bcmpmu_fg_set_sample_rate(fg, SAMPLE_RATE_8HZ);
-	bcmpmu_fg_enable(fg, true);
 
 	do {
 		msleep(FG_INIT_CAPACITY_SAMPLE_DELAY);
@@ -2165,11 +2158,6 @@ static int bcmpmu_fg_get_ocv_avg_capacity(struct bcmpmu_fg_data *fg,
 
 	BUG_ON((cap_percentage > 100) || (cap_percentage < 0));
 
-	bcmpmu_fg_enable(fg, false);
-	bcmpmu_fg_set_sample_rate(fg, SAMPLE_RATE_2HZ);
-	bcmpmu_fg_enable(fg, true);
-	bcmpmu_fg_reset(fg);
-
 	return cap_percentage;
 }
 
@@ -2868,11 +2856,17 @@ static void bcmpmu_fg_periodic_work(struct work_struct *work)
 	struct bcmpmu_fg_status_flags flags;
 	enum bcmpmu_chrgr_type_t chrgr_type;
 	int data;
+	int ret;
 
 	FG_LOCK(fg);
 	flags = fg->flags;
 	FG_UNLOCK(fg);
 
+	if (!fg->init_notifier) {
+		ret = bcmpmu_fg_register_notifiers(fg);
+		WARN_ON(ret);
+	}
+
 	/**
 	 * No battery Case (BSI/BCL pin is not connected)
 	 * if battery is not present (booting with power supply
@@ -3209,6 +3203,8 @@ static int bcmpmu_fg_register_notifiers(struct bcmpmu_fg_data *fg)
 	if (ret)
 		goto unreg_chrgr_current_nb;
 
+	fg->init_notifier = 1;
+
 	return 0;
 
 unreg_chrgr_current_nb:
@@ -3908,9 +3904,6 @@ static int bcmpmu_fg_probe(struct platform_device *pdev)
 	INIT_WORK(&fg->low_batt_irq_work, bcmpmu_fg_low_batt_irq_work);
 
 	mutex_init(&fg->mutex);
-	ret = bcmpmu_fg_register_notifiers(fg);
-	if (ret)
-		goto destroy_workq;
 #ifdef CONFIG_WD_TAPPER
 	ret = wd_tapper_add_timeout_req(&fg->wd_tap_node, "fg",
 			TAPPER_DEFAULT_TIMEOUT);
@@ -3941,10 +3934,9 @@ static int bcmpmu_fg_probe(struct platform_device *pdev)
 	} else
 		pr_fg(INIT, "booting with power supply\n");
 
-	bcmpmu_fg_set_sample_rate(fg, SAMPLE_RATE_2HZ);
+	bcmpmu_fg_set_sample_rate(fg, SAMPLE_RATE_4HZ);
 	bcmpmu_fg_hw_init(fg);
 
-
 	if (!(bcmpmu->flags & BCMPMU_SPA_EN)) {
 		ret = power_supply_register(&pdev->dev, &fg->psy);
 		if (ret) {
@@ -4002,8 +3994,8 @@ static int bcmpmu_fg_probe(struct platform_device *pdev)
 	/**
 	 * Run FG algorithm now
 	 */
-	queue_delayed_work(fg->fg_wq, &fg->fg_periodic_work, 0);
-
+	queue_delayed_work(fg->fg_wq, &fg->fg_periodic_work,
+				msecs_to_jiffies(FG_SETTLING_TIME));
 #ifdef CONFIG_DEBUG_FS
 	bcmpmu_fg_debugfs_init(fg);
 #endif