diff --git a/drivers/power/mmi_parallel_charger/mmi_charger_core.c b/drivers/power/mmi_parallel_charger/mmi_charger_core.c
index 5ae6a9d694bf..dc3ad22e70dd 100644
--- a/drivers/power/mmi_parallel_charger/mmi_charger_core.c
+++ b/drivers/power/mmi_parallel_charger/mmi_charger_core.c
@@ -165,12 +165,16 @@ bool mmi_find_chrg_step(struct mmi_charger_manager *chip, int temp_zone, int vba
 	batt_volt = vbatt_volt;
 	chrg_step_inline.temp_c = zone.temp_c;
 
-	mmi_chrg_info(chip, "batt_volt %d, step num %d\n",
-						batt_volt, chip->chrg_step_nums);
+	mmi_chrg_info(chip, "batt_volt %d, chrg step %d, step nums %d\n",
+						batt_volt, prev_step.pres_chrg_step,
+						chip->chrg_step_nums);
+
+	/*In the first search cycle, find out the vbatt is less than step volt*/
 
 	for (i = 0; i < chip->chrg_step_nums; i++) {
-		mmi_chrg_info(chip, "i %d, step volt %d, batt_volt %d\n",
-						i, chrg_steps[i].chrg_step_volt, batt_volt);
+		mmi_chrg_info(chip,
+					"first cycle,i %d, step volt %d, batt_volt %d\n",
+					i, chrg_steps[i].chrg_step_volt, batt_volt);
 		if (chrg_steps[i].chrg_step_volt > 0
 			&& batt_volt < chrg_steps[i].chrg_step_volt) {
 			if ( (i + 1) < chip->chrg_step_nums
@@ -186,6 +190,7 @@ bool mmi_find_chrg_step(struct mmi_charger_manager *chip, int temp_zone, int vba
 				chrg_steps[i].chrg_step_volt;
 			chrg_step_inline.pres_chrg_step = i;
 			find_step = true;
+			mmi_chrg_info(chip, "find chrg step\n");
 			break;
 		}
 	}
@@ -199,19 +204,70 @@ bool mmi_find_chrg_step(struct mmi_charger_manager *chip, int temp_zone, int vba
 					chrg_step_inline.chrg_step_cv_volt,
 					chrg_step_inline.chrg_step_cv_tapper_curr);
 		chip->chrg_step = chrg_step_inline;
+	} else {
+
+	/*If can't find out any chrg step in the first search cycle,*/
+	/*it means that vbatt is already greater than all chrg step volt, */
+	/*therefore, start to enter second search cycle, */
+	/*to find out the maximal chrg step volt*/
+		for (i = 0; i < chip->chrg_step_nums; i++) {
+			mmi_chrg_info(chip,
+						"second cycle, i %d, step volt %d, batt_volt %d\n",
+						i, chrg_steps[i].chrg_step_volt, batt_volt);
+			if (chrg_steps[i].chrg_step_volt > 0
+				&& batt_volt > chrg_steps[i].chrg_step_volt) {
+				if ( (i + 1) < chip->chrg_step_nums
+					&& chrg_steps[i + 1].chrg_step_volt > 0) {
+					chrg_step_inline.chrg_step_cv_tapper_curr =
+						chrg_steps[i + 1].chrg_step_curr;
+				} else
+					chrg_step_inline.chrg_step_cv_tapper_curr =
+						chrg_steps[i].chrg_step_curr;
+				chrg_step_inline.chrg_step_cc_curr =
+					chrg_steps[i].chrg_step_curr;
+				chrg_step_inline.chrg_step_cv_volt =
+					chrg_steps[i].chrg_step_volt;
+				chrg_step_inline.pres_chrg_step = i;
+				find_step = true;
+				mmi_chrg_info(chip, "find chrg step\n");
+			}
+		}
+
+		if (find_step) {
+			mmi_chrg_info(chip, "chrg step %d, "
+					"step cc curr %d, step cv volt %d, "
+					"step cv tapper curr %d\n",
+					chrg_step_inline.pres_chrg_step,
+					chrg_step_inline.chrg_step_cc_curr,
+					chrg_step_inline.chrg_step_cv_volt,
+					chrg_step_inline.chrg_step_cv_tapper_curr);
+			chip->chrg_step = chrg_step_inline;
+		}
 	}
 
-	if (find_step &&
-		prev_step.pres_chrg_step != chip->chrg_step.pres_chrg_step) {
-	mmi_chrg_info(chip,"temp zone %d, "
-				"Select chrg step %d, step cc curr %d,"
-				"step cv volt %d, step cv tapper curr %d\n",
+
+	if (find_step) {
+		if (chip->chrg_step.chrg_step_cc_curr ==
+			chip->chrg_step.chrg_step_cv_tapper_curr)
+			chip->chrg_step.last_step = true;
+		else
+			chip->chrg_step.last_step = false;
+
+		mmi_chrg_info(chip,"Temp zone %d, "
+				"select chrg step %d, step cc curr %d,"
+				"step cv volt %d, step cv tapper curr %d, "
+				"is the last chrg step %d\n",
 				chip->pres_temp_zone,
 				chip->chrg_step.pres_chrg_step,
 				chip->chrg_step.chrg_step_cc_curr,
 				chip->chrg_step.chrg_step_cv_volt,
-				chip->chrg_step.chrg_step_cv_tapper_curr);
-		return true;
+				chip->chrg_step.chrg_step_cv_tapper_curr,
+				chip->chrg_step.last_step);
+
+		if (prev_step.pres_chrg_step != chip->chrg_step.pres_chrg_step) {
+			mmi_chrg_info(chip, "Find the next chrg step\n");
+			return true;
+		}
 	}
 	return false;
 }
diff --git a/drivers/power/mmi_parallel_charger/mmi_charger_core.h b/drivers/power/mmi_parallel_charger/mmi_charger_core.h
index 518b804f9ef7..0ffc09b88638 100644
--- a/drivers/power/mmi_parallel_charger/mmi_charger_core.h
+++ b/drivers/power/mmi_parallel_charger/mmi_charger_core.h
@@ -111,6 +111,7 @@ struct mmi_chrg_step_info {
 	int	chrg_step_cc_curr;
 	int	chrg_step_cv_volt;
 	int	chrg_step_cv_tapper_curr;
+	bool	last_step;
 };
 
 struct mmi_chrg_dev_ops {
diff --git a/drivers/power/mmi_parallel_charger/mmi_charger_pump_policy.c b/drivers/power/mmi_parallel_charger/mmi_charger_pump_policy.c
index 80611e6732c3..6d63a9115b2b 100644
--- a/drivers/power/mmi_parallel_charger/mmi_charger_pump_policy.c
+++ b/drivers/power/mmi_parallel_charger/mmi_charger_pump_policy.c
@@ -126,6 +126,8 @@ static void mmi_chrg_sm_move_state(struct mmi_charger_manager *chip, pm_sm_state
 	mmi_chrg_dbg(chip, PR_INTERRUPT, "pm_state change:%s -> %s\n",
 		pm_state_str[sm_state], pm_state_str[state]);
 	sm_state = state;
+	pd_constant_power_cnt = 0;
+	batt_curr_roof = 0;
 }
 
 static void chrg_dev_init(struct mmi_charger_manager *chip, struct mmi_cp_policy_dev *chrg_list)
@@ -474,6 +476,16 @@ static void mmi_chrg_sm_work_func(struct work_struct *work)
 		heartbeat_dely_ms = HEARTBEAT_CANCEL;
 		break;
 	case PM_STATE_ENTRY:
+		if (chrg_list->cp_slave
+			&& chrg_list->chrg_dev[CP_SLAVE]->charger_enabled) {
+			mmi_enable_charging(chrg_list->chrg_dev[CP_SLAVE], false);
+		}
+
+		if (chrg_list->cp_master
+			&& chrg_list->chrg_dev[CP_MASTER]->charger_enabled) {
+			mmi_enable_charging(chrg_list->chrg_dev[CP_MASTER], false);
+		}
+
 		if (chip->pd_pps_support
 			&& chrg_list->cp_master
 			&& vbatt_volt > chip->pl_chrg_vbatt_min
@@ -969,8 +981,9 @@ static void mmi_chrg_sm_work_func(struct work_struct *work)
 		}
 
 		if (vbatt_volt >= chrg_step->chrg_step_cv_volt
-			&& (ibatt_curr < chrg_step->chrg_step_cv_tapper_curr
-			|| ibatt_curr < chrg_list->chrg_dev[CP_MASTER]->charging_curr_min)) {
+			&& ((!chrg_step->last_step &&
+				ibatt_curr < chrg_step->chrg_step_cv_tapper_curr)
+				|| ibatt_curr < chrg_list->chrg_dev[CP_MASTER]->charging_curr_min)) {
 			if (chrg_cv_taper_tunning_cnt >= CV_TAPPER_COUNT) {
 				if (ibatt_curr <
 					chrg_list->chrg_dev[CP_MASTER]->charging_curr_min) {
@@ -1265,7 +1278,7 @@ schedule:
 						"battery temp %d\n",
 						chip->pd_request_curr, batt_temp);
 
-				} else if (ibatt_curr < TYPEC_HIGH_CURRENT_UA
+				} else if (ibatt_curr > TYPEC_HIGH_CURRENT_UA
 						&& chip->pd_request_volt > cooling_volt) {
 					chip->pd_request_volt -= COOLING_DELTA_POWER;
 					mmi_chrg_info(chip, "Do chrg power cooling"