diff --git a/drivers/misc/cs40l2x-tables.c b/drivers/misc/cs40l2x-tables.c
index d88ebb248454..210e3dcd41ce 100644
--- a/drivers/misc/cs40l2x-tables.c
+++ b/drivers/misc/cs40l2x-tables.c
@@ -853,107 +853,106 @@ const struct cs40l2x_otp_desc cs40l2x_otp_map[CS40L2X_NUM_OTP_MAPS] = {
 	},
 };
 
-const struct cs40l2x_dig_scale_map
-			cs40l2x_pbq_scale[CS40L2X_PBQ_SCALE_MAX + 1] = {
-	{0, 0},
-	{1, 320},
-	{2, 272},
-	{3, 244},
-	{4, 224},
-	{5, 208},
-	{6, 195},
-	{7, 185},
-	{8, 176},
-	{9, 167},
-	{10, 160},
-	{11, 153},
-	{12, 147},
-	{13, 142},
-	{14, 137},
-	{15, 132},
-	{16, 127},
-	{17, 123},
-	{18, 119},
-	{19, 115},
-	{20, 112},
-	{21, 108},
-	{22, 105},
-	{23, 102},
-	{24, 99},
-	{25, 96},
-	{26, 94},
-	{27, 91},
-	{28, 88},
-	{29, 86},
-	{30, 84},
-	{31, 81},
-	{32, 79},
-	{33, 77},
-	{34, 75},
-	{35, 73},
-	{36, 71},
-	{37, 69},
-	{38, 67},
-	{39, 65},
-	{40, 64},
-	{41, 62},
-	{42, 60},
-	{43, 59},
-	{44, 57},
-	{45, 55},
-	{46, 54},
-	{47, 52},
-	{48, 51},
-	{49, 50},
-	{50, 48},
-	{51, 47},
-	{52, 45},
-	{53, 44},
-	{54, 43},
-	{55, 42},
-	{56, 40},
-	{57, 39},
-	{58, 38},
-	{59, 37},
-	{60, 35},
-	{61, 34},
-	{62, 33},
-	{63, 32},
-	{64, 31},
-	{65, 30},
-	{66, 29},
-	{67, 28},
-	{68, 27},
-	{69, 26},
-	{70, 25},
-	{71, 24},
-	{72, 23},
-	{73, 22},
-	{74, 21},
-	{75, 20},
-	{76, 19},
-	{77, 18},
-	{78, 17},
-	{79, 16},
-	{80, 16},
-	{81, 15},
-	{82, 14},
-	{83, 13},
-	{84, 12},
-	{85, 11},
-	{86, 10},
-	{87, 10},
-	{88, 9},
-	{89, 8},
-	{90, 7},
-	{91, 7},
-	{92, 6},
-	{93, 5},
-	{94, 4},
-	{95, 4},
-	{96, 3},
-	{97, 2},
-	{98, 1},
-	{99, 1},
-	{100, 0},
+const unsigned int cs40l2x_pbq_dig_scale[CS40L2X_PBQ_SCALE_MAX + 1] = {
+	0,
+	320,	/* 1% */
+	272,
+	244,
+	224,
+	208,
+	195,
+	185,
+	176,
+	167,
+	160,	/* 10% */
+	153,
+	147,
+	142,
+	137,
+	132,
+	127,
+	123,
+	119,
+	115,
+	112,	/* 20% */
+	108,
+	105,
+	102,
+	99,
+	96,
+	94,
+	91,
+	88,
+	86,
+	84,	/* 30% */
+	81,
+	79,
+	77,
+	75,
+	73,
+	71,
+	69,
+	67,
+	65,
+	64,	/* 40% */
+	62,
+	60,
+	59,
+	57,
+	55,
+	54,
+	52,
+	51,
+	50,
+	48,	/* 50% */
+	47,
+	45,
+	44,
+	43,
+	42,
+	40,
+	39,
+	38,
+	37,
+	35,	/* 60% */
+	34,
+	33,
+	32,
+	31,
+	30,
+	29,
+	28,
+	27,
+	26,
+	25,	/* 70% */
+	24,
+	23,
+	22,
+	21,
+	20,
+	19,
+	18,
+	17,
+	16,
+	16,	/* 80% */
+	15,
+	14,
+	13,
+	12,
+	11,
+	10,
+	10,
+	9,
+	8,
+	7,	/* 90% */
+	7,
+	6,
+	5,
+	4,
+	4,
+	3,
+	2,
+	1,
+	1,
+	0,	/* 100% */
 };
diff --git a/drivers/misc/cs40l2x.c b/drivers/misc/cs40l2x.c
index 738d1783c113..f067f78588d2 100644
--- a/drivers/misc/cs40l2x.c
+++ b/drivers/misc/cs40l2x.c
@@ -73,6 +73,7 @@ struct cs40l2x_private {
 	unsigned int pbq_depth;
 	unsigned int pbq_index;
 	unsigned int pbq_state;
+	unsigned int pbq_cp_dig_scale;
 	int pbq_repeat;
 	int pbq_remain;
 #ifdef CONFIG_ANDROID_TIMED_OUTPUT
@@ -1151,7 +1152,7 @@ static int cs40l2x_pbq_cancel(struct cs40l2x_private *cs40l2x)
 	if (ret)
 		return ret;
 
-	ret = cs40l2x_dig_scale_set(cs40l2x, cs40l2x->dig_scale);
+	ret = cs40l2x_cp_dig_scale_set(cs40l2x, cs40l2x->pbq_cp_dig_scale);
 	if (ret)
 		return ret;
 
@@ -1188,9 +1189,7 @@ static int cs40l2x_pbq_poll(struct cs40l2x_private *cs40l2x)
 
 static int cs40l2x_pbq_pair_launch(struct cs40l2x_private *cs40l2x)
 {
-	struct regmap *regmap = cs40l2x->regmap;
-	unsigned int dig_scale = cs40l2x->dig_scale;
-	unsigned int tag, mag;
+	unsigned int tag, mag, cp_dig_scale;
 	int ret, i;
 
 	/* this function expects to be called from a locked worker function */
@@ -1214,7 +1213,8 @@ static int cs40l2x_pbq_pair_launch(struct cs40l2x_private *cs40l2x)
 				cs40l2x->cp_trailer_index = 0;
 				cs40l2x->pbq_state = CS40L2X_PBQ_STATE_IDLE;
 
-				ret = cs40l2x_dig_scale_set(cs40l2x, dig_scale);
+				ret = cs40l2x_cp_dig_scale_set(cs40l2x,
+						cs40l2x->pbq_cp_dig_scale);
 				return ret;
 			default:
 				/* loop once more */
@@ -1246,16 +1246,17 @@ static int cs40l2x_pbq_pair_launch(struct cs40l2x_private *cs40l2x)
 			}
 			break;
 		default:
-			dig_scale += cs40l2x_pbq_scale[mag].dig_scale;
-			if (dig_scale > CS40L2X_DIG_SCALE_MAX)
-				dig_scale = CS40L2X_DIG_SCALE_MAX;
+			cp_dig_scale = cs40l2x->pbq_cp_dig_scale
+					+ cs40l2x_pbq_dig_scale[mag];
+			if (cp_dig_scale > CS40L2X_DIG_SCALE_MAX)
+				cp_dig_scale = CS40L2X_DIG_SCALE_MAX;
 
-			ret = cs40l2x_dig_scale_set(cs40l2x, dig_scale);
+			ret = cs40l2x_cp_dig_scale_set(cs40l2x, cp_dig_scale);
 			if (ret)
 				return ret;
 
-			ret = regmap_write(regmap, CS40L2X_MBOX_TRIGGERINDEX,
-					tag);
+			ret = regmap_write(cs40l2x->regmap,
+					CS40L2X_MBOX_TRIGGERINDEX, tag);
 			if (ret)
 				return ret;
 
@@ -1413,6 +1414,15 @@ static void cs40l2x_vibe_start_worker(struct work_struct *work)
 		break;
 
 	case CS40L2X_INDEX_PBQ:
+		cs40l2x->pbq_cp_dig_scale = CS40L2X_DIG_SCALE_RESET;
+
+		ret = cs40l2x_cp_dig_scale_get(cs40l2x,
+				&cs40l2x->pbq_cp_dig_scale);
+		if (ret) {
+			dev_err(dev, "Failed to read digital scale\n");
+			goto err_mutex;
+		}
+
 		cs40l2x->pbq_index = 0;
 		cs40l2x->pbq_remain = cs40l2x->pbq_repeat;
 
diff --git a/drivers/misc/cs40l2x.h b/drivers/misc/cs40l2x.h
index bbf208b4bf85..6b567f64d394 100644
--- a/drivers/misc/cs40l2x.h
+++ b/drivers/misc/cs40l2x.h
@@ -774,18 +774,12 @@ struct cs40l2x_pbq_pair {
 	unsigned int remain;
 };
 
-struct cs40l2x_dig_scale_map {
-	unsigned int encoded;
-	unsigned int dig_scale;
-};
-
 extern const unsigned char cs40l2x_bst_k1_table[4][5];
 extern const unsigned char cs40l2x_bst_k2_table[4][5];
 extern const unsigned char cs40l2x_bst_slope_table[4];
 
 extern const struct cs40l2x_otp_desc cs40l2x_otp_map[CS40L2X_NUM_OTP_MAPS];
 
-extern const struct cs40l2x_dig_scale_map
-			cs40l2x_pbq_scale[CS40L2X_PBQ_SCALE_MAX + 1];
+extern const unsigned int cs40l2x_pbq_dig_scale[CS40L2X_PBQ_SCALE_MAX + 1];
 
 #endif /*__CS40L2X_H__*/