cs40l26: merge CirrusLogic dsp v3.1.2 and cs40l26 v1.1.0

Branch: v5.10-cs40l26
Tag: cs40l26-v1.1.0_5.10
Files:
drivers/input/misc/cs40l26-i2c.c (No changes)
drivers/input/misc/cs40l26-spi.c (No changes)
drivers/input/misc/cs40l26-sysfs.c
drivers/input/misc/cs40l26-tables.c
drivers/input/misc/cs40l26.c
include/linux/mfd/cs40l26.h
sound/soc/codecs/cs40l26.c -> cs40l26-codec.c

Features:
- Increase A2H Volume Granularity
- Add more debug-level statements
- sysfs control to get amount of space left in wavetable
- DT control to set maximum boost peak current
- sysfs control to read die temperature
- sysfs control to swap firmware variants at runtime
- Load firmware synchronously
- Inverted streaming and waveform playback
- SVC enable for effects streamed via ASP
- Map Haptic Effects to GPI events
- Actuator Safe Mode workaround
- Remove support for A0 silicon

Bug fixes:
- Remove unused variables
- Cancel worker threads before deleting workqueue

commits:
c07047bd1087 ASoC: cs40l26: Add control to enable invert streaming data
7d0b08956875 input: cs40l26: Remove A0 silicon support
d6c20f8342fd input: cs40l26: Update firmware swap function
(Skip) 421500c4eb47 Documentation: cs40l26: Add option to load specified SVC tuning
a3fb83ecc3b3 input: cs40l26: Add option to load specified SVC tuning
736aa239110c ASoC: cs40l26: Add control to enable SVC for streaming data
7c060144b291 input: cs40l26: Implement actuator safe mode workaround
9b866d0e0362 input: cs40l26: Map Haptic Effects to GPI Events
e28e0a5131cc input: cs40l26: Add ability to invert waveform playback
961e4ba3a4bb input: cs40l26: Load firmware synchronously
23463ef1ab4a input: cs40l26: Add ability to swap firmware variants
05b2d67e3c66 input: cs40l26: Remove ENABLE/DISABLE macros
b6bd69389b17 input: cs40l26: Remove unneeded use of variable in cs40l26_pcm_ev
27af6399be8c input: cs40l26: Cancel worker threads before destroying workqueue
1794f854a2d0 input: cs40l26: Add control to read die temperature when DSP is active
50bf4051b3a8 input: cs40l26: Remove silicon rev. checks for algorithm IDs
(Skip) 3982e2db9db5 Documentation: cs40l26: Allow user to set peak boost current
756186d689c5 input: cs40l26: Allow user to set peak boost current
ed0f6bc72faa input: cs40l26: Control to read remaining space in wavetable
72510182f0f2 input: cs40l26: Use percentage value to set A0 gain
91d781c96e12 input: cs40l26: Make status registers readable via regmap
a8beb7e4c41f ASoC: cs40l26: Add dev_dbg statments to ALSA callbacks
c5b8320ebbce input: cs40l26: Add debug statements
17b3a65e18d1 input: cs40l26: Control to read Power On Sequence
8c4f68e0194a ASoC: cs40l26: Increase A2H Volume granularity

Branch: v5.10-cirrus-dsp-fw
Tag: cl-dsp-fw-v3.1.2_5.10
Files:
drivers/firmware/cirrus/cl_dsp.c
include/linux/firmware/cirrus/cl_dsp.h

Allocate array memory for .bin file data dynamically instead of using
a hard-coded value to avoid possibility of a kernel crash.

Allow firmware to load even if there are incompatibilities between
the expected firmware version, the loaded firmware, and the tuning
file revisions.

commits:
79a4a41e4fc6 firmware: cirrus: Allocate data array dynamically
19140c2d644b firmware: cirrus: Loosen restrictions on firmware loading
8e02f2e42990 firmware: cirrus: Don't explicitly free coefficient parent name

Bug: 191658078
Bug: 180110149
Bug: 194540033
Bug: 193793095
Test: Check idlcli vibrator commands.
Test: Back EMF for internal calibration.
Test: Firmware swap.
Signed-off-by: Tai Kuo <taikuo@google.com>
Change-Id: I7db82ce23663772a83e69490b88cfb47e4bf93f3

1 files changed, 592 insertions, 554 deletions

diff --git a/cs40l26/cs40l26.c b/cs40l26/cs40l26.c
index 4f10c6f..9b75a6f 100644
--- a/cs40l26/cs40l26.c
+++ b/cs40l26/cs40l26.c
@@ -128,34 +128,17 @@ EXPORT_SYMBOL(cs40l26_class_h_set);
 
 int cs40l26_dsp_state_get(struct cs40l26_private *cs40l26, u8 *state)
 {
-	u32 algo_id, reg, dsp_state;
-	int ret;
-
-	if (cs40l26->fw_loaded) {
-		if (cs40l26->fw_mode == CS40L26_FW_MODE_RAM)
-			algo_id = CS40L26_PM_ALGO_ID;
-		else
-			algo_id = CS40L26_PM_ROM_ALGO_ID;
+	u32 reg, dsp_state;
+	int ret = 0;
 
+	if (cs40l26->fw_loaded)
 		ret = cl_dsp_get_reg(cs40l26->dsp, "PM_CUR_STATE",
-				CL_DSP_XM_UNPACKED_TYPE, algo_id, &reg);
-		if (ret)
-			return ret;
-	} else {
-		switch (cs40l26->revid) {
-		case CS40L26_REVID_A0:
-			reg = CS40L26_A0_PM_CUR_STATE_STATIC_REG;
-			break;
-		case CS40L26_REVID_A1:
-			reg = CS40L26_A1_PM_CUR_STATE_STATIC_REG;
-			break;
-		default:
-			dev_err(cs40l26->dev,
-					"Revid ID not supported: 0x%02X\n",
-					cs40l26->revid);
-			return -EINVAL;
-		}
-	}
+			CL_DSP_XM_UNPACKED_TYPE, CS40L26_PM_ALGO_ID, &reg);
+	else
+		reg = CS40L26_A1_PM_CUR_STATE_STATIC_REG;
+
+	if (ret)
+		return ret;
 
 	ret = cs40l26_dsp_read(cs40l26, reg, &dsp_state);
 	if (ret)
@@ -185,7 +168,7 @@ int cs40l26_pm_timeout_ms_set(struct cs40l26_private *cs40l26,
 {
 	u32 timeout_ticks = timeout_ms * CS40L26_PM_TICKS_MS_DIV;
 	struct regmap *regmap = cs40l26->regmap;
-	u32 lower_val, reg, algo_id;
+	u32 lower_val, reg;
 	u8 upper_val;
 	int ret;
 
@@ -194,13 +177,8 @@ int cs40l26_pm_timeout_ms_set(struct cs40l26_private *cs40l26,
 
 	lower_val = timeout_ticks & CS40L26_PM_TIMEOUT_TICKS_LOWER_MASK;
 
-	if (cs40l26->fw_mode == CS40L26_FW_MODE_RAM)
-		algo_id = CS40L26_PM_ALGO_ID;
-	else
-		algo_id = CS40L26_PM_ROM_ALGO_ID;
-
 	ret = cl_dsp_get_reg(cs40l26->dsp, "PM_TIMER_TIMEOUT_TICKS",
-			CL_DSP_XM_UNPACKED_TYPE, algo_id, &reg);
+			CL_DSP_XM_UNPACKED_TYPE, CS40L26_PM_ALGO_ID, &reg);
 	if (ret)
 		return ret;
 
@@ -217,33 +195,17 @@ EXPORT_SYMBOL(cs40l26_pm_timeout_ms_set);
 int cs40l26_pm_timeout_ms_get(struct cs40l26_private *cs40l26,
 		u32 *timeout_ms)
 {
-	u32 lower_val, upper_val, algo_id, reg;
-	int ret;
-
-	if (cs40l26->fw_loaded) {
-		if (cs40l26->fw_mode == CS40L26_FW_MODE_RAM)
-			algo_id = CS40L26_PM_ALGO_ID;
-		else
-			algo_id = CS40L26_PM_ROM_ALGO_ID;
+	u32 lower_val, upper_val, reg;
+	int ret = 0;
 
+	if (cs40l26->fw_loaded)
 		ret = cl_dsp_get_reg(cs40l26->dsp, "PM_TIMER_TIMEOUT_TICKS",
-				CL_DSP_XM_UNPACKED_TYPE, algo_id, &reg);
-		if (ret)
-			return ret;
-	} else {
-		switch (cs40l26->revid) {
-		case CS40L26_REVID_A0:
-			reg = CS40L26_A0_PM_TIMEOUT_TICKS_STATIC_REG;
-			break;
-		case CS40L26_REVID_A1:
-			reg = CS40L26_A1_PM_TIMEOUT_TICKS_STATIC_REG;
-			break;
-		default:
-			dev_err(cs40l26->dev, "Revid ID not supported: %02X\n",
-				cs40l26->revid);
-			return -EINVAL;
-		}
-	}
+			CL_DSP_XM_UNPACKED_TYPE, CS40L26_PM_ALGO_ID, &reg);
+	else
+		reg = CS40L26_A1_PM_TIMEOUT_TICKS_STATIC_REG;
+
+	if (ret)
+		return ret;
 
 	ret = regmap_read(cs40l26->regmap, reg +
 			CS40L26_PM_STDBY_TIMEOUT_LOWER_OFFSET, &lower_val);
@@ -405,27 +367,21 @@ static int cs40l26_dsp_shutdown(struct cs40l26_private *cs40l26)
 
 static int cs40l26_dsp_pre_config(struct cs40l26_private *cs40l26)
 {
+	u32 halo_state, timeout_ms;
 	u8 dsp_state;
-	u32 halo_state, halo_state_reg;
-	int ret;
+	int ret, i;
 
-	switch (cs40l26->revid) {
-	case CS40L26_REVID_A0:
-		halo_state_reg = CS40L26_A0_DSP_HALO_STATE_REG;
-		break;
-	case CS40L26_REVID_A1:
-		halo_state_reg = CS40L26_A1_DSP_HALO_STATE_REG;
-		break;
-	default:
-		dev_err(cs40l26->dev, "Revid ID not supported: %02X\n",
-			cs40l26->revid);
-		return -EINVAL;
-	}
+	ret = cs40l26_pm_state_transition(cs40l26,
+			CS40L26_PM_STATE_PREVENT_HIBERNATE);
+	if (ret)
+		return ret;
 
-	ret = regmap_read(cs40l26->regmap, halo_state_reg,
+	ret = regmap_read(cs40l26->regmap, CS40L26_A1_DSP_HALO_STATE_REG,
 			&halo_state);
-	if (ret)
+	if (ret) {
+		dev_err(cs40l26->dev, "Failed to get HALO state\n");
 		return ret;
+	}
 
 	if (halo_state != CS40L26_DSP_HALO_STATE_RUN) {
 		dev_err(cs40l26->dev, "DSP not Ready: HALO_STATE: %08X\n",
@@ -433,24 +389,34 @@ static int cs40l26_dsp_pre_config(struct cs40l26_private *cs40l26)
 		return -EINVAL;
 	}
 
-	ret = cs40l26_pm_state_transition(cs40l26,
-			CS40L26_PM_STATE_PREVENT_HIBERNATE);
-	if (ret)
-		return ret;
 
-	ret = cs40l26_dsp_state_get(cs40l26, &dsp_state);
+	ret = cs40l26_pm_timeout_ms_get(cs40l26, &timeout_ms);
 	if (ret)
 		return ret;
 
-	if (dsp_state != CS40L26_DSP_STATE_SHUTDOWN &&
-			dsp_state != CS40L26_DSP_STATE_STANDBY) {
-		dev_err(cs40l26->dev, "DSP core not safe to kill\n");
+	for (i = 0; i < 10; i++) {
+		ret = cs40l26_dsp_state_get(cs40l26, &dsp_state);
+		if (ret)
+			return ret;
+
+		if (dsp_state != CS40L26_DSP_STATE_SHUTDOWN &&
+				dsp_state != CS40L26_DSP_STATE_STANDBY)
+			dev_warn(cs40l26->dev, "DSP core not safe to kill\n");
+		else
+			break;
+
+		usleep_range(CS40L26_MS_TO_US(timeout_ms),
+			CS40L26_MS_TO_US(timeout_ms) + 100);
+	}
+
+	if (i == 10) {
+		dev_err(cs40l26->dev, "DSP Core could not be shut down\n");
 		return -EINVAL;
 	}
 
 	/* errata write fixing indeterminent PLL lock time */
 	ret = regmap_update_bits(cs40l26->regmap, CS40L26_PLL_REFCLK_DETECT_0,
-			CS40L26_PLL_REFCLK_DET_EN_MASK, CS40L26_DISABLE);
+			CS40L26_PLL_REFCLK_DET_EN_MASK, 0);
 	if (ret) {
 		dev_err(cs40l26->dev, "Failed to disable PLL refclk detect\n");
 		return ret;
@@ -588,6 +554,12 @@ static int cs40l26_handle_mbox_buffer(struct cs40l26_private *cs40l26)
 				return -EINVAL;
 			}
 			break;
+		case CS40L26_DSP_MBOX_LE_EST_START:
+			dev_dbg(dev, "LE_EST_START\n");
+			break;
+		case CS40L26_DSP_MBOX_LE_EST_DONE:
+			dev_dbg(dev, "LE_EST_DONE\n");
+			break;
 		case CS40L26_DSP_MBOX_SYS_ACK:
 			dev_err(dev, "Mbox buffer value (0x%X) not supported\n",
 					val);
@@ -855,8 +827,7 @@ static int cs40l26_handle_irq1(struct cs40l26_private *cs40l26,
 				CS40L26_WKSRC_STS_SHIFT);
 
 		ret = cl_dsp_get_reg(cs40l26->dsp, "LAST_WAKESRC_CTL",
-				CL_DSP_XM_UNPACKED_TYPE,
-				CS40L26_FW_ID, &reg);
+				CL_DSP_XM_UNPACKED_TYPE, cs40l26->fw.id, &reg);
 		if (ret)
 			goto err;
 
@@ -1200,173 +1171,50 @@ err:
 	return (irq1_count + irq2_count) ? IRQ_HANDLED : IRQ_NONE;
 }
 
-static bool cs40l26_pseq_v1_addr_exists(struct cs40l26_private *cs40l26,
-							u16 addr, int *index)
-{
-	int i;
-
-	if (cs40l26->pseq_v1_len == 0)
-		return false;
-
-	for (i = 0; i < cs40l26->pseq_v1_len; i++) {
-		if (cs40l26->pseq_v1_table[i].addr == addr) {
-			*index = i;
-			return true;
-		}
-	}
-
-	*index = -1;
-
-	return false;
-}
-
-static int cs40l26_pseq_v1_write(struct cs40l26_private *cs40l26,
-		unsigned int pseq_v1_offset)
-{
-	struct regmap *regmap = cs40l26->regmap;
-	unsigned int len = cs40l26->pseq_v1_len;
-	struct device *dev = cs40l26->dev;
-	u32 val;
-	u16 addr;
-	int ret;
-
-	addr = cs40l26->pseq_v1_table[pseq_v1_offset].addr;
-	val = cs40l26->pseq_v1_table[pseq_v1_offset].val;
-
-	/* the "upper half" first 24-bit word of the sequence pair is written
-	 * to the write sequencer as: [23-16] addr{15-0},
-	 * [15-0] val{31-24} with bits [24-31] acting as a buffer
-	 */
-	ret = regmap_write(regmap, cs40l26->pseq_base +
-			(pseq_v1_offset * CS40L26_PSEQ_V1_STRIDE),
-			(addr << CS40L26_PSEQ_V1_ADDR_SHIFT) |
-			((val & ~CS40L26_PSEQ_V1_VAL_MASK)
-			>> CS40L26_PSEQ_V1_VAL_SHIFT));
-	if (ret) {
-		dev_err(dev, "Failed to write power on seq. (upper half)\n");
-		return ret;
-	}
-
-	/* the "lower half" of the address-value pair is written to the write
-	 * sequencer as: [23-0] data{23-0} with bits [24-31] acting as a buffer
-	 */
-	ret = regmap_write(regmap, cs40l26->pseq_base + CL_DSP_BYTES_PER_WORD
-			+ (pseq_v1_offset * CS40L26_PSEQ_V1_STRIDE),
-			val & CS40L26_PSEQ_V1_VAL_MASK);
-	if (ret) {
-		dev_err(dev, "Failed to write power on seq. (lower half)\n");
-		return ret;
-	}
-
-	/* end of sequence must be marked by list terminator */
-	ret = regmap_write(regmap, cs40l26->pseq_base +
-			(len * CS40L26_PSEQ_V1_STRIDE),
-			CS40L26_PSEQ_V1_LIST_TERM);
-	if (ret)
-		dev_err(dev, "Failed to write power on seq. terminator\n");
-
-	return ret;
-}
-
-static int cs40l26_pseq_v1_add_pair(struct cs40l26_private *cs40l26, u16 addr,
-		u32 val, bool replace)
-{
-	unsigned int len = cs40l26->pseq_v1_len;
-	struct device *dev = cs40l26->dev;
-	unsigned int pseq_v1_offset, prev_val;
-	int ret, index;
-
-	if (len >= CS40L26_PSEQ_V1_MAX_ENTRIES) {
-		dev_err(dev, "Power on seq. exceeded max number of entries\n");
-		return -E2BIG;
-	}
-
-	if (cs40l26_pseq_v1_addr_exists(cs40l26, addr, &index) && replace) {
-		prev_val = cs40l26->pseq_v1_table[index].val;
-		cs40l26->pseq_v1_table[index].val = val;
-		pseq_v1_offset = index;
-	} else {
-		cs40l26->pseq_v1_table[len].addr = addr;
-		cs40l26->pseq_v1_table[len].val = val;
-		cs40l26->pseq_v1_len++;
-		pseq_v1_offset = len;
-	}
-
-	ret = cs40l26_pseq_v1_write(cs40l26, pseq_v1_offset);
-	if (ret) { /* If an error occurs during write, reset the sequence */
-		if (index < 0) { /* No previous value for this address */
-			cs40l26->pseq_v1_table[len].addr = 0;
-			cs40l26->pseq_v1_table[len].val = 0;
-			cs40l26->pseq_v1_len--;
-		} else {
-			cs40l26->pseq_v1_table[index].val = prev_val;
-		}
-	}
-
-	return ret;
-}
-
-int cs40l26_pseq_v1_multi_add_pair(struct cs40l26_private *cs40l26,
-		const struct reg_sequence *reg_seq, int num_regs, bool replace)
-{
-	int ret, i;
-
-	for (i = 0; i < num_regs; i++) {
-		ret = cs40l26_pseq_v1_add_pair(cs40l26, (u16) (reg_seq[i].reg &
-				CS40L26_PSEQ_V1_ADDR_MASK), reg_seq[i].def,
-				replace);
-		if (ret)
-			return ret;
-	}
-
-	return 0;
-}
-EXPORT_SYMBOL(cs40l26_pseq_v1_multi_add_pair);
-
-static int cs40l26_pseq_v2_add_op(struct cs40l26_private *cs40l26,
+static int cs40l26_pseq_add_op(struct cs40l26_private *cs40l26,
 		int num_words, u32 *words)
 {
 	struct regmap *regmap = cs40l26->regmap;
 	struct device *dev = cs40l26->dev;
 	u32 offset_for_new_op, *op_words;
 	int ret;
-	struct cs40l26_pseq_v2_op *pseq_v2_op_end, *pseq_v2_op_new, *op;
+	struct cs40l26_pseq_op *pseq_op_end, *pseq_op_new, *op;
 
 	/* get location of the list terminator */
-	list_for_each_entry(pseq_v2_op_end, &cs40l26->pseq_v2_op_head, list) {
-		if (pseq_v2_op_end->operation == CS40L26_PSEQ_V2_OP_END)
+	list_for_each_entry(pseq_op_end, &cs40l26->pseq_op_head, list) {
+		if (pseq_op_end->operation == CS40L26_PSEQ_OP_END)
 			break;
 	}
 
-	if (pseq_v2_op_end->operation != CS40L26_PSEQ_V2_OP_END) {
+	if (pseq_op_end->operation != CS40L26_PSEQ_OP_END) {
 		dev_err(dev, "Failed to find END_OF_SCRIPT\n");
 		return -EINVAL;
 	}
 
-	offset_for_new_op = pseq_v2_op_end->offset;
+	offset_for_new_op = pseq_op_end->offset;
 
 	/* add new op to list */
 	op_words = kzalloc(num_words * CL_DSP_BYTES_PER_WORD, GFP_KERNEL);
 	if (!op_words)
 		return -ENOMEM;
 	memcpy(op_words, words, num_words * CL_DSP_BYTES_PER_WORD);
-	pseq_v2_op_new = devm_kzalloc(dev, sizeof(*pseq_v2_op_new), GFP_KERNEL);
-	if (!pseq_v2_op_new) {
+	pseq_op_new = devm_kzalloc(dev, sizeof(*pseq_op_new), GFP_KERNEL);
+	if (!pseq_op_new) {
 		ret = -ENOMEM;
 		goto err_free;
 	}
 
-	pseq_v2_op_new->size = num_words;
-	pseq_v2_op_new->offset = offset_for_new_op;
-	pseq_v2_op_new->words = op_words;
-	list_add(&pseq_v2_op_new->list, &cs40l26->pseq_v2_op_head);
+	pseq_op_new->size = num_words;
+	pseq_op_new->offset = offset_for_new_op;
+	pseq_op_new->words = op_words;
+	list_add(&pseq_op_new->list, &cs40l26->pseq_op_head);
 
-	cs40l26->pseq_v2_num_ops++;
+	cs40l26->pseq_num_ops++;
 
 	/* bump end of script offset to accomodate new operation */
-	pseq_v2_op_end->offset += num_words * CL_DSP_BYTES_PER_WORD;
+	pseq_op_end->offset += num_words * CL_DSP_BYTES_PER_WORD;
 
-	list_for_each_entry(op, &cs40l26->pseq_v2_op_head, list) {
+	list_for_each_entry(op, &cs40l26->pseq_op_head, list) {
 		if (op->offset >= offset_for_new_op) {
 			ret = regmap_bulk_write(regmap, cs40l26->pseq_base +
 							op->offset,
@@ -1387,22 +1235,22 @@ err_free:
 	return ret;
 }
 
-static int cs40l26_pseq_v2_add_write_reg_full(struct cs40l26_private *cs40l26,
+static int cs40l26_pseq_add_write_reg_full(struct cs40l26_private *cs40l26,
 		u32 addr, u32 data, bool update_if_op_already_in_seq)
 {
 	int ret;
-	struct cs40l26_pseq_v2_op *op;
-	u32 op_words[CS40L26_PSEQ_V2_OP_WRITE_REG_FULL_WORDS];
+	struct cs40l26_pseq_op *op;
+	u32 op_words[CS40L26_PSEQ_OP_WRITE_REG_FULL_WORDS];
 
-	op_words[0] = (CS40L26_PSEQ_V2_OP_WRITE_REG_FULL <<
-						CS40L26_PSEQ_V2_OP_SHIFT);
+	op_words[0] = (CS40L26_PSEQ_OP_WRITE_REG_FULL <<
+						CS40L26_PSEQ_OP_SHIFT);
 	op_words[0] |= addr >> 16;
 	op_words[1] = (addr & 0x0000FFFF) << 8;
 	op_words[1] |= (data & 0xFF000000) >> 24;
 	op_words[2] = (data & 0x00FFFFFF);
 
 	if (update_if_op_already_in_seq) {
-		list_for_each_entry(op, &cs40l26->pseq_v2_op_head, list) {
+		list_for_each_entry(op, &cs40l26->pseq_op_head, list) {
 			/* check if op with same op and addr already exists */
 			if ((op->words[0] == op_words[0]) &&
 				((op->words[1] & 0xFFFFFF00) ==
@@ -1420,21 +1268,21 @@ static int cs40l26_pseq_v2_add_write_reg_full(struct cs40l26_private *cs40l26,
 	}
 
 	/* if no matching op is found or !update, add the op */
-	ret = cs40l26_pseq_v2_add_op(cs40l26,
-				CS40L26_PSEQ_V2_OP_WRITE_REG_FULL_WORDS,
+	ret = cs40l26_pseq_add_op(cs40l26,
+				CS40L26_PSEQ_OP_WRITE_REG_FULL_WORDS,
 				op_words);
 
 	return ret;
 }
 
-int cs40l26_pseq_v2_multi_add_write_reg_full(struct cs40l26_private *cs40l26,
+int cs40l26_pseq_multi_add_write_reg_full(struct cs40l26_private *cs40l26,
 		const struct reg_sequence *reg_seq, int num_regs,
 		bool update_if_op_already_in_seq)
 {
 	int ret, i;
 
 	for (i = 0; i < num_regs; i++) {
-		ret = cs40l26_pseq_v2_add_write_reg_full(cs40l26,
+		ret = cs40l26_pseq_add_write_reg_full(cs40l26,
 						reg_seq[i].reg, reg_seq[i].def,
 						update_if_op_already_in_seq);
 		if (ret)
@@ -1443,94 +1291,77 @@ int cs40l26_pseq_v2_multi_add_write_reg_full(struct cs40l26_private *cs40l26,
 
 	return 0;
 }
-EXPORT_SYMBOL(cs40l26_pseq_v2_multi_add_write_reg_full);
+EXPORT_SYMBOL(cs40l26_pseq_multi_add_write_reg_full);
 
-static int cs40l26_pseq_v1_init(struct cs40l26_private *cs40l26)
+/* 24-bit address, 16-bit data */
+static int cs40l26_pseq_add_write_reg_h16(struct cs40l26_private *cs40l26,
+		u32 addr, u16 data, bool update_if_op_already_in_seq)
 {
+	int ret;
 	struct device *dev = cs40l26->dev;
-	int ret, i, index = 0;
-	u8 upper_val = 0;
-	u16 addr = 0;
-	u32 val, word, algo_id;
-
-	if (cs40l26->fw_mode == CS40L26_FW_MODE_RAM)
-		algo_id = CS40L26_PM_ALGO_ID;
-	else
-		algo_id = CS40L26_PM_ROM_ALGO_ID;
-
-	ret = cl_dsp_get_reg(cs40l26->dsp, "POWER_ON_SEQUENCE",
-			CL_DSP_XM_UNPACKED_TYPE, algo_id, &cs40l26->pseq_base);
-	if (ret)
-		return ret;
-
-	for (i = 0; i < CS40L26_PSEQ_V1_MAX_WRITES; i++) {
-		ret = regmap_read(cs40l26->regmap,
-				cs40l26->pseq_base +
-				(i * CL_DSP_BYTES_PER_WORD), &word);
-		if (ret) {
-			dev_err(dev, "Failed to read from power on seq.\n");
-			return ret;
-		}
-
-		if ((word & CS40L26_PSEQ_V1_LIST_TERM_MASK) ==
-				CS40L26_PSEQ_V1_LIST_TERM)
-			break;
+	struct cs40l26_pseq_op *op;
+	u32 op_words[CS40L26_PSEQ_OP_WRITE_REG_H16_WORDS];
 
-		if (i % CS40L26_PSEQ_V1_PAIR_NUM_WORDS) { /* lower half */
-			index = i / CS40L26_PSEQ_V1_PAIR_NUM_WORDS;
-			val = (upper_val << CS40L26_PSEQ_V1_VAL_SHIFT) |
-					(word & CS40L26_PSEQ_V1_VAL_MASK);
+	if (addr & 0xFF000000) {
+		dev_err(dev, "invalid address for pseq write_reg_h16\n");
+		return -EINVAL;
+	}
 
-			cs40l26->pseq_v1_table[index].addr = addr;
-			cs40l26->pseq_v1_table[index].val = val;
-		} else { /* upper half */
-			addr = (word & CS40L26_PSEQ_V1_ADDR_WORD_MASK) >>
-					CS40L26_PSEQ_V1_ADDR_SHIFT;
+	op_words[0] = (CS40L26_PSEQ_OP_WRITE_REG_H16 <<
+						CS40L26_PSEQ_OP_SHIFT);
+	op_words[0] |= (addr & 0x00FFFF00) >> 8;
+	op_words[1] = (addr & 0x000000FF) << 16;
+	op_words[1] |= data;
 
-			upper_val = word & CS40L26_PSEQ_V1_VAL_WORD_UPPER_MASK;
+	if (update_if_op_already_in_seq) {
+		list_for_each_entry(op, &cs40l26->pseq_op_head, list) {
+			/* check if op with same op and addr already exists */
+			if ((op->words[0] == op_words[0]) &&
+				((op->words[1] & 0x00FF0000) ==
+				(op_words[1] & 0x00FF0000))) {
+				/* update data in the existing op and return */
+				ret = regmap_bulk_write(cs40l26->regmap,
+						cs40l26->pseq_base + op->offset,
+						op_words, op->size);
+				if (ret)
+					dev_err(cs40l26->dev,
+						"Failed to update op\n");
+				return ret;
+			}
 		}
 	}
 
-	if (i >= CS40L26_PSEQ_V1_MAX_WRITES) {
-		dev_err(dev, "Original sequence exceeds max # of entries\n");
-		return -E2BIG;
-	}
-
-	ret = regmap_write(cs40l26->regmap, cs40l26->pseq_base +
-			(i * CL_DSP_BYTES_PER_WORD), CS40L26_PSEQ_V1_LIST_TERM);
-	if (ret)
-		return ret;
+	/* if no matching op is found or !update, add the op */
+	ret = cs40l26_pseq_add_op(cs40l26,
+				CS40L26_PSEQ_OP_WRITE_REG_H16_WORDS,
+				op_words);
 
-	cs40l26->pseq_v1_len = index + 1;
-	return 0;
+	return ret;
 }
 
-static int cs40l26_pseq_v2_init(struct cs40l26_private *cs40l26)
+static int cs40l26_pseq_init(struct cs40l26_private *cs40l26)
 {
 	struct device *dev = cs40l26->dev;
+	u32 words[CS40L26_PSEQ_MAX_WORDS], *op_words;
+	struct cs40l26_pseq_op *pseq_op;
 	int ret, i, j, num_words, read_size;
 	u8 operation;
-	u32 words[CS40L26_PSEQ_V2_MAX_WORDS], algo_id, *op_words;
-	struct cs40l26_pseq_v2_op *pseq_v2_op;
-
-	INIT_LIST_HEAD(&cs40l26->pseq_v2_op_head);
-	cs40l26->pseq_v2_num_ops = 0;
 
-	if (cs40l26->fw_mode == CS40L26_FW_MODE_RAM)
-		algo_id = CS40L26_PM_ALGO_ID;
-	else
-		algo_id = CS40L26_PM_ROM_ALGO_ID;
+	INIT_LIST_HEAD(&cs40l26->pseq_op_head);
+	cs40l26->pseq_num_ops = 0;
 
 	ret = cl_dsp_get_reg(cs40l26->dsp, "POWER_ON_SEQUENCE",
-			CL_DSP_XM_UNPACKED_TYPE, algo_id, &cs40l26->pseq_base);
+			CL_DSP_XM_UNPACKED_TYPE, CS40L26_PM_ALGO_ID,
+			&cs40l26->pseq_base);
 	if (ret)
 		return ret;
 
 	/* read pseq memory space */
 	i = 0;
-	while (i < CS40L26_PSEQ_V2_MAX_WORDS) {
+	while (i < CS40L26_PSEQ_MAX_WORDS) {
 		read_size = min(CS40L26_MAX_I2C_READ_SIZE_BYTES,
-			CS40L26_PSEQ_V2_MAX_WORDS - i);
+			CS40L26_PSEQ_MAX_WORDS - i);
+
 		ret = regmap_bulk_read(cs40l26->regmap,
 			cs40l26->pseq_base + i * CL_DSP_BYTES_PER_WORD,
 			words + i,
@@ -1543,20 +1374,20 @@ static int cs40l26_pseq_v2_init(struct cs40l26_private *cs40l26)
 	}
 
 	i = 0;
-	while (i < CS40L26_PSEQ_V2_MAX_WORDS) {
-		operation = (words[i] & CS40L26_PSEQ_V2_OP_MASK) >>
-			CS40L26_PSEQ_V2_OP_SHIFT;
+	while (i < CS40L26_PSEQ_MAX_WORDS) {
+		operation = (words[i] & CS40L26_PSEQ_OP_MASK) >>
+			CS40L26_PSEQ_OP_SHIFT;
 
 		/* get num words for given operation */
-		for (j = 0; j < CS40L26_PSEQ_V2_NUM_OPS; j++) {
-			if (cs40l26_pseq_v2_op_sizes[j][0] == operation) {
-				num_words = cs40l26_pseq_v2_op_sizes[j][1];
+		for (j = 0; j < CS40L26_PSEQ_NUM_OPS; j++) {
+			if (cs40l26_pseq_op_sizes[j][0] == operation) {
+				num_words = cs40l26_pseq_op_sizes[j][1];
 				break;
 			}
 		}
 
-		if (j == CS40L26_PSEQ_V2_NUM_OPS) {
-			dev_err(dev, "Failed to determine pseq_v2 op size\n");
+		if (j == CS40L26_PSEQ_NUM_OPS) {
+			dev_err(dev, "Failed to determine pseq op size\n");
 			return -EINVAL;
 		}
 
@@ -1566,37 +1397,28 @@ static int cs40l26_pseq_v2_init(struct cs40l26_private *cs40l26)
 			return -ENOMEM;
 		memcpy(op_words, &words[i], num_words * CL_DSP_BYTES_PER_WORD);
 
-		pseq_v2_op = devm_kzalloc(dev, sizeof(*pseq_v2_op), GFP_KERNEL);
-		if (!pseq_v2_op) {
+		pseq_op = devm_kzalloc(dev, sizeof(*pseq_op), GFP_KERNEL);
+		if (!pseq_op) {
 			ret = -ENOMEM;
 			goto err_free;
 		}
 
-		pseq_v2_op->size = num_words;
-		pseq_v2_op->offset = i * CL_DSP_BYTES_PER_WORD;
-		pseq_v2_op->operation = operation;
-		pseq_v2_op->words = op_words;
-		list_add(&pseq_v2_op->list, &cs40l26->pseq_v2_op_head);
+		pseq_op->size = num_words;
+		pseq_op->offset = i * CL_DSP_BYTES_PER_WORD;
+		pseq_op->operation = operation;
+		pseq_op->words = op_words;
+		list_add(&pseq_op->list, &cs40l26->pseq_op_head);
 
-		cs40l26->pseq_v2_num_ops++;
+		cs40l26->pseq_num_ops++;
 		i += num_words;
 
-		if (operation == CS40L26_PSEQ_V2_OP_END)
+		if (operation == CS40L26_PSEQ_OP_END)
 			break;
 
 	}
 
-	dev_dbg(dev, "PSEQ_V2 num ops: %d\n", cs40l26->pseq_v2_num_ops);
-	dev_dbg(dev, "offset\tsize\twords\n");
-	list_for_each_entry(pseq_v2_op, &cs40l26->pseq_v2_op_head, list) {
-		dev_dbg(dev, "0x%04X\t%d", pseq_v2_op->offset,
-							pseq_v2_op->size);
-		for (j = 0; j < pseq_v2_op->size; j++)
-			dev_dbg(dev, "0x%08X", *(pseq_v2_op->words + j));
-	}
-
-	if (operation != CS40L26_PSEQ_V2_OP_END) {
-		dev_err(dev, "PSEQ_V2 END_OF_SCRIPT not found\n");
+	if (operation != CS40L26_PSEQ_OP_END) {
+		dev_err(dev, "PSEQ END_OF_SCRIPT not found\n");
 		return -E2BIG;
 	}
 
@@ -1608,25 +1430,107 @@ err_free:
 	return ret;
 }
 
-static int cs40l26_pseq_init(struct cs40l26_private *cs40l26)
+static int cs40l26_update_reg_defaults_via_pseq(struct cs40l26_private *cs40l26)
 {
+	struct device *dev = cs40l26->dev;
 	int ret;
 
-	switch (cs40l26->revid) {
-	case CS40L26_REVID_A0:
-		ret = cs40l26_pseq_v1_init(cs40l26);
-		break;
-	case CS40L26_REVID_A1:
-		ret = cs40l26_pseq_v2_init(cs40l26);
-		break;
-	default:
-		dev_err(cs40l26->dev, "Revid ID not supported: %02X\n",
-			cs40l26->revid);
+	/* set SPK_DEFAULT_HIZ to 1 */
+	ret = cs40l26_pseq_add_write_reg_h16(cs40l26,
+			CS40L26_TST_DAC_MSM_CONFIG,
+			CS40L26_TST_DAC_MSM_CONFIG_DEFAULT_CHANGE_VALUE_H16,
+			true);
+	if (ret)
+		dev_err(dev, "Failed to sequence register default updates\n");
+
+	return ret;
+}
+
+static int cs40l26_buzzgen_set(struct cs40l26_private *cs40l26, u16 freq,
+		u16 level, u16 duration, u8 offset)
+{
+	int ret;
+	unsigned int base_reg, offset_reg;
+
+	ret = cl_dsp_get_reg(cs40l26->dsp, "BUZZ_EFFECTS1_BUZZ_FREQ",
+		CL_DSP_XM_UNPACKED_TYPE, CS40L26_BUZZGEN_ALGO_ID, &base_reg);
+	if (ret)
+		return ret;
+
+	offset_reg = base_reg + (offset * 12);
+
+	ret = regmap_write(cs40l26->regmap, offset_reg, freq);
+	if (ret) {
+		dev_err(cs40l26->dev, "Failed to write BUZZGEN frequency\n");
+		return ret;
+	}
+
+	ret = regmap_write(cs40l26->regmap, offset_reg + 4,
+			CS40L26_BUZZGEN_LEVEL_DEFAULT);
+	if (ret) {
+		dev_err(cs40l26->dev, "Failed to write BUZZGEN level\n");
+		return ret;
+	}
+
+	ret = regmap_write(cs40l26->regmap, offset_reg + 8, duration / 4);
+	if (ret)
+		dev_err(cs40l26->dev, "Failed to write BUZZGEN duration\n");
+
+	return ret;
+}
+
+static int cs40l26_gpio_index_set(struct cs40l26_private *cs40l26,
+		struct ff_effect *effect)
+{
+	u16 button = effect->trigger.button;
+	u8 bank = (button & CS40L26_BTN_BANK_MASK) >> CS40L26_BTN_BANK_SHIFT;
+	u8 edge = (button & CS40L26_BTN_EDGE_MASK) >> CS40L26_BTN_EDGE_SHIFT;
+	u8 gpio = (button & CS40L26_BTN_NUM_MASK) >> CS40L26_BTN_NUM_SHIFT;
+	u8 index = button & CS40L26_BTN_INDEX_MASK;
+	u8 buzz = button & CS40L26_BTN_BUZZ_MASK;
+	u16 write_val, freq;
+	u8 offset;
+	u32 reg;
+	int ret;
+
+	if (gpio != 1) {
+		dev_err(cs40l26->dev, "GPIO%u not supported on 0x%02X\n", gpio,
+				cs40l26->revid);
+		return -EINVAL;
+	}
+
+	if (edge > 1) {
+		dev_err(cs40l26->dev, "Invalid GPI edge %u\n", edge);
 		return -EINVAL;
 	}
 
+	offset = ((edge ^ 1)) * 4;
+	reg = cs40l26->event_map_base + offset;
+
+	pm_runtime_get_sync(cs40l26->dev);
+
+	if (buzz) {
+		freq = CS40L26_MS_TO_HZ(effect->u.periodic.period);
+
+		ret = cs40l26_buzzgen_set(cs40l26, freq,
+				CS40L26_BUZZGEN_LEVEL_DEFAULT,
+				effect->replay.length, 0);
+		if (ret)
+			goto pm_err;
+
+		write_val = 1 << CS40L26_BTN_BUZZ_SHIFT;
+	} else {
+		write_val = index | (bank << CS40L26_BTN_BANK_SHIFT);
+	}
+
+	ret = regmap_update_bits(cs40l26->regmap, reg,
+			CS40L26_EVENT_MAP_INDEX_MASK, write_val);
 	if (ret)
-		dev_err(cs40l26->dev, "Failed to init pseq\n");
+		dev_err(cs40l26->dev, "Failed to update event map\n");
+
+pm_err:
+	pm_runtime_mark_last_busy(cs40l26->dev);
+	pm_runtime_put_autosuspend(cs40l26->dev);
 
 	return ret;
 }
@@ -1635,53 +1539,22 @@ static void cs40l26_set_gain_worker(struct work_struct *work)
 {
 	struct cs40l26_private *cs40l26 =
 		container_of(work, struct cs40l26_private, set_gain_work);
-	u16 amp_vol_pcm;
-	u32 reg, val;
+	u32 reg;
 	int ret;
 
 	pm_runtime_get_sync(cs40l26->dev);
 	mutex_lock(&cs40l26->lock);
 
-	switch (cs40l26->revid) {
-	case CS40L26_REVID_A0:
-		amp_vol_pcm = CS40L26_AMP_VOL_PCM_MAX & cs40l26->gain_pct;
-
-		ret = regmap_update_bits(cs40l26->regmap, CS40L26_AMP_CTRL,
-				CS40L26_AMP_CTRL_VOL_PCM_MASK, amp_vol_pcm <<
-				CS40L26_AMP_CTRL_VOL_PCM_SHIFT);
-		if (ret) {
-			dev_err(cs40l26->dev, "Failed to update digtal gain\n");
-			goto err_mutex;
-		}
-
-		ret = regmap_read(cs40l26->regmap, CS40L26_AMP_CTRL, &val);
-		if (ret) {
-			dev_err(cs40l26->dev, "Failed to read AMP control\n");
-			goto err_mutex;
-		}
-
-		ret = cs40l26_pseq_v1_add_pair(cs40l26,
-						CS40L26_AMP_CTRL, val, true);
-		if (ret)
-			dev_err(cs40l26->dev, "Failed to set gain in pseq\n");
-		break;
-	case CS40L26_REVID_A1:
-		val = cs40l26_attn_q21_2_vals[cs40l26->gain_pct];
-
-		/* Write Q21.2 value to SOURCE_ATTENUATION */
-		ret = cl_dsp_get_reg(cs40l26->dsp, "SOURCE_ATTENUATION",
+	/* Write Q21.2 value to SOURCE_ATTENUATION */
+	ret = cl_dsp_get_reg(cs40l26->dsp, "SOURCE_ATTENUATION",
 			CL_DSP_XM_UNPACKED_TYPE, CS40L26_EXT_ALGO_ID, &reg);
-		if (ret)
-			goto err_mutex;
+	if (ret)
+		goto err_mutex;
 
-		ret = regmap_write(cs40l26->regmap, reg, val);
-		if (ret)
-			dev_err(cs40l26->dev, "Failed to set attenuation\n");
-		break;
-	default:
-		dev_err(cs40l26->dev, "Revid ID not supported: %02X\n",
-			cs40l26->revid);
-	}
+	ret = regmap_write(cs40l26->regmap, reg,
+			cs40l26_attn_q21_2_vals[cs40l26->gain_pct]);
+	if (ret)
+		dev_err(cs40l26->dev, "Failed to set attenuation\n");
 
 err_mutex:
 	mutex_unlock(&cs40l26->lock);
@@ -1694,15 +1567,13 @@ static void cs40l26_vibe_start_worker(struct work_struct *work)
 	struct cs40l26_private *cs40l26 = container_of(work,
 			struct cs40l26_private, vibe_start_work);
 	struct device *dev = cs40l26->dev;
+	u32 index = 0;
 	int ret = 0;
 	unsigned int reg, freq;
-	u32 index = 0, buzz_id;
+	bool invert;
 	u16 duration;
 
-	if (cs40l26->fw_mode == CS40L26_FW_MODE_RAM)
-		buzz_id = CS40L26_BUZZGEN_ALGO_ID;
-	else
-		buzz_id = CS40L26_BUZZGEN_ROM_ALGO_ID;
+	dev_dbg(dev, "%s\n", __func__);
 
 	if (cs40l26->effect->u.periodic.waveform == FF_CUSTOM)
 		index = cs40l26->trigger_indices[cs40l26->effect->id];
@@ -1721,6 +1592,29 @@ static void cs40l26_vibe_start_worker(struct work_struct *work)
 			ktime_set(CS40L26_MS_TO_SECS(duration),
 			CS40L26_MS_TO_NS(duration % 1000)), HRTIMER_MODE_REL);
 
+	ret = cl_dsp_get_reg(cs40l26->dsp, "SOURCE_INVERT",
+			CL_DSP_XM_UNPACKED_TYPE, CS40L26_EXT_ALGO_ID, &reg);
+	if (ret)
+		goto err_mutex;
+
+	switch (cs40l26->effect->direction) {
+	case 0x0000:
+		invert = false;
+		break;
+	case 0x8000:
+		invert = true;
+		break;
+	default:
+		dev_err(dev, "Invalid ff_effect direction: 0x%X\n",
+			cs40l26->effect->direction);
+		ret = -EINVAL;
+		goto err_mutex;
+	}
+
+	ret = regmap_write(cs40l26->regmap, reg, invert);
+	if (ret)
+		goto err_mutex;
+
 	cs40l26_vibe_state_set(cs40l26, CS40L26_VIBE_STATE_HAPTIC);
 
 	switch (cs40l26->effect->u.periodic.waveform) {
@@ -1731,26 +1625,10 @@ static void cs40l26_vibe_start_worker(struct work_struct *work)
 			goto err_mutex;
 		break;
 	case FF_SINE:
-		ret = cl_dsp_get_reg(cs40l26->dsp, "BUZZ_EFFECTS2_BUZZ_FREQ",
-				CL_DSP_XM_UNPACKED_TYPE, buzz_id, &reg);
-		if (ret)
-			goto err_mutex;
-
 		freq = CS40L26_MS_TO_HZ(cs40l26->effect->u.periodic.period);
 
-		ret = regmap_write(cs40l26->regmap, reg, freq);
-		if (ret)
-			goto err_mutex;
-
-		ret = regmap_write(cs40l26->regmap, reg +
-				CS40L26_BUZZGEN_LEVEL_OFFSET,
-				CS40L26_BUZZGEN_LEVEL_DEFAULT);
-		if (ret)
-			goto err_mutex;
-
-		ret = regmap_write(cs40l26->regmap, reg +
-				CS40L26_BUZZGEN_DURATION_OFFSET, duration /
-				CS40L26_BUZZGEN_DURATION_DIV_STEP);
+		ret = cs40l26_buzzgen_set(cs40l26, freq,
+				CS40L26_BUZZGEN_LEVEL_DEFAULT, duration, 1);
 		if (ret)
 			goto err_mutex;
 
@@ -1781,9 +1659,10 @@ static void cs40l26_vibe_stop_worker(struct work_struct *work)
 	struct cs40l26_private *cs40l26 = container_of(work,
 			struct cs40l26_private, vibe_stop_work);
 	unsigned int reg;
-	u32 algo_id;
 	int ret;
 
+	dev_dbg(cs40l26->dev, "%s\n", __func__);
+
 	pm_runtime_get_sync(cs40l26->dev);
 	mutex_lock(&cs40l26->lock);
 
@@ -1795,13 +1674,8 @@ static void cs40l26_vibe_stop_worker(struct work_struct *work)
 			goto mutex_exit;
 		}
 	} else {
-		if (cs40l26->fw_mode == CS40L26_FW_MODE_ROM)
-			algo_id = CS40L26_VIBEGEN_ROM_ALGO_ID;
-		else
-			algo_id = CS40L26_VIBEGEN_ALGO_ID;
-
 		ret = cl_dsp_get_reg(cs40l26->dsp, "END_PLAYBACK",
-				CL_DSP_XM_UNPACKED_TYPE, algo_id, &reg);
+			CL_DSP_XM_UNPACKED_TYPE, CS40L26_VIBEGEN_ALGO_ID, &reg);
 		if (ret)
 			goto mutex_exit;
 
@@ -1840,6 +1714,11 @@ static void cs40l26_set_gain(struct input_dev *dev, u16 gain)
 {
 	struct cs40l26_private *cs40l26 = input_get_drvdata(dev);
 
+	if (gain < 0 || gain >= CS40L26_NUM_PCT_MAP_VALUES) {
+		dev_err(cs40l26->dev, "Gain value %u out of bounds\n", gain);
+		return;
+	}
+
 	cs40l26->gain_pct = gain;
 
 	queue_work(cs40l26->vibe_workqueue, &cs40l26->set_gain_work);
@@ -1851,6 +1730,9 @@ static int cs40l26_playback_effect(struct input_dev *dev,
 	struct cs40l26_private *cs40l26 = input_get_drvdata(dev);
 	struct ff_effect *effect;
 
+	dev_dbg(cs40l26->dev, "%s: effect ID = %d, val = %d\n", __func__,
+			effect_id, val);
+
 	effect = &dev->ff->effects[effect_id];
 	if (!effect) {
 		dev_err(cs40l26->dev, "No such effect to playback\n");
@@ -2217,6 +2099,8 @@ static int cs40l26_upload_effect(struct input_dev *dev,
 			goto out_free;
 		}
 
+		dev_dbg(cdev, "%s: ID = %u, index = 0x%08X\n",
+				__func__, effect->id, trigger_index);
 		break;
 	case FF_SINE:
 		if (effect->u.periodic.period) {
@@ -2240,6 +2124,10 @@ static int cs40l26_upload_effect(struct input_dev *dev,
 		return -EINVAL;
 	}
 
+	if (effect->trigger.button)
+		ret = cs40l26_gpio_index_set(cs40l26, effect);
+
+
 out_free:
 	kfree(raw_custom_data);
 
@@ -2353,12 +2241,9 @@ static int cs40l26_part_num_resolve(struct cs40l26_private *cs40l26)
 	}
 
 	val &= CS40L26_REVID_MASK;
-	switch (val) {
-	case CS40L26_REVID_A0:
-	case CS40L26_REVID_A1:
+	if (val == CS40L26_REVID_A1) {
 		cs40l26->revid = val;
-		break;
-	default:
+	} else {
 		dev_err(dev, "Invalid device revision: 0x%02X\n", val);
 		return -EINVAL;
 	}
@@ -2369,31 +2254,46 @@ static int cs40l26_part_num_resolve(struct cs40l26_private *cs40l26)
 	return 0;
 }
 
-static int cs40l26_cl_dsp_init(struct cs40l26_private *cs40l26)
+static int cs40l26_cl_dsp_init(struct cs40l26_private *cs40l26, u32 id)
 {
 	int ret = 0;
 
+	if (cs40l26->dsp) {
+		ret = cl_dsp_destroy(cs40l26->dsp);
+		if (ret) {
+			dev_err(cs40l26->dev,
+				"Failed to destroy existing DSP structure\n");
+			return ret;
+		}
+		cs40l26->dsp = NULL;
+	}
+
 	cs40l26->dsp = cl_dsp_create(cs40l26->dev, cs40l26->regmap);
-	if (!cs40l26->dsp)
+	if (!cs40l26->dsp) {
+		dev_err(cs40l26->dev, "Failed to allocate space for DSP\n");
 		return -ENOMEM;
+	}
 
 	if (cs40l26->fw_mode == CS40L26_FW_MODE_ROM) {
+		cs40l26->fw.id = CS40L26_FW_ID;
 		cs40l26->fw.min_rev = CS40L26_FW_ROM_MIN_REV;
 		cs40l26->fw.num_coeff_files = 0;
-		cs40l26->fw.coeff_files = NULL;
 	} else {
-		if (cs40l26->revid == CS40L26_REVID_A1)
+		cs40l26->fw.id = id;
+
+		if (id == CS40L26_FW_ID)
 			cs40l26->fw.min_rev = CS40L26_FW_A1_RAM_MIN_REV;
-		else
-			cs40l26->fw.min_rev = CS40L26_FW_A0_RAM_MIN_REV;
+		if (id == CS40L26_FW_CALIB_ID)
+			cs40l26->fw.min_rev = CS40L26_FW_CALIB_MIN_REV;
 
-		cs40l26->fw.num_coeff_files =
-				ARRAY_SIZE(cs40l26_ram_coeff_files);
-		cs40l26->fw.coeff_files = cs40l26_ram_coeff_files;
+		cs40l26->fw.num_coeff_files = 3;
+		cs40l26->fw.coeff_files[0] = CS40L26_WT_FILE_NAME;
+		cs40l26->fw.coeff_files[1] = CS40L26_A2H_TUNING_FILE_NAME;
+		cs40l26->fw.coeff_files[2] = CS40L26_SVC_TUNING_FILE_NAME;
 
 		ret = cl_dsp_wavetable_create(cs40l26->dsp,
 				CS40L26_VIBEGEN_ALGO_ID, CS40L26_WT_NAME_XM,
-				CS40L26_WT_NAME_YM, "cs40l26.bin");
+				CS40L26_WT_NAME_YM, CS40L26_WT_FILE_NAME);
 	}
 
 	cs40l26->num_owt_effects = 0;
@@ -2434,6 +2334,13 @@ static int cs40l26_wksrc_config(struct cs40l26_private *cs40l26)
 static int cs40l26_gpio_config(struct cs40l26_private *cs40l26)
 {
 	u32 unmask_bits;
+	int ret;
+
+	ret = cl_dsp_get_reg(cs40l26->dsp, "ENT_MAP_TABLE_EVENT_DATA_PACKED",
+			CL_DSP_XM_UNPACKED_TYPE, CS40L26_EVENT_HANDLER_ALGO_ID,
+			&cs40l26->event_map_base);
+	if (ret)
+		return ret;
 
 	unmask_bits = BIT(CS40L26_IRQ1_GPIO1_RISE)
 			| BIT(CS40L26_IRQ1_GPIO1_FALL);
@@ -2474,21 +2381,8 @@ static int cs40l26_brownout_prevention_init(struct cs40l26_private *cs40l26)
 		return ret;
 	}
 
-	switch (cs40l26->revid) {
-	case CS40L26_REVID_A0:
-		ret = cs40l26_pseq_v1_add_pair(cs40l26,
-			CS40L26_BLOCK_ENABLES2, val, CS40L26_PSEQ_V1_REPLACE);
-		break;
-	case CS40L26_REVID_A1:
-		ret = cs40l26_pseq_v2_add_write_reg_full(cs40l26,
-			CS40L26_BLOCK_ENABLES2, val, true);
-		break;
-	default:
-		dev_err(cs40l26->dev, "Revid ID not supported: %02X\n",
-			cs40l26->revid);
-		return -EINVAL;
-	}
-
+	ret = cs40l26_pseq_add_write_reg_full(cs40l26, CS40L26_BLOCK_ENABLES2,
+			val, true);
 	if (ret) {
 		dev_err(dev, "Failed to sequence brownout prevention\n");
 		return ret;
@@ -2592,22 +2486,8 @@ static int cs40l26_brownout_prevention_init(struct cs40l26_private *cs40l26)
 			return ret;
 		}
 
-		switch (cs40l26->revid) {
-		case CS40L26_REVID_A0:
-			ret = cs40l26_pseq_v1_add_pair(cs40l26,
-				CS40L26_VBBR_CONFIG, val,
-				CS40L26_PSEQ_V1_REPLACE);
-			break;
-		case CS40L26_REVID_A1:
-			ret = cs40l26_pseq_v2_add_write_reg_full(cs40l26,
-				CS40L26_VBBR_CONFIG, val,
-				true);
-			break;
-		default:
-			dev_err(cs40l26->dev, "Revid ID not supported: %02X\n",
-				cs40l26->revid);
-			return -EINVAL;
-		}
+		ret = cs40l26_pseq_add_write_reg_full(cs40l26,
+				CS40L26_VBBR_CONFIG, val, true);
 		if (ret)
 			return ret;
 	}
@@ -2713,22 +2593,8 @@ static int cs40l26_brownout_prevention_init(struct cs40l26_private *cs40l26)
 			return ret;
 		}
 
-		switch (cs40l26->revid) {
-		case CS40L26_REVID_A0:
-			ret = cs40l26_pseq_v1_add_pair(cs40l26,
-				CS40L26_VPBR_CONFIG, val,
-				CS40L26_PSEQ_V1_REPLACE);
-			break;
-		case CS40L26_REVID_A1:
-			ret = cs40l26_pseq_v2_add_write_reg_full(cs40l26,
-				CS40L26_VPBR_CONFIG, val,
-				true);
-			break;
-		default:
-			dev_err(cs40l26->dev, "Revid ID not supported: %02X\n",
-				cs40l26->revid);
-			return -EINVAL;
-		}
+		ret = cs40l26_pseq_add_write_reg_full(cs40l26,
+				CS40L26_VPBR_CONFIG, val, true);
 		if (ret)
 			return ret;
 	}
@@ -2761,7 +2627,7 @@ static int cs40l26_verify_fw(struct cs40l26_private *cs40l26)
 	if (ret)
 		return ret;
 
-	if (val != CS40L26_FW_ID) {
+	if (val != cs40l26->fw.id) {
 		dev_err(cs40l26->dev, "Invalid firmware ID: 0x%X\n", val);
 		return -EINVAL;
 	}
@@ -2808,22 +2674,8 @@ static int cs40l26_asp_config(struct cs40l26_private *cs40l26)
 		goto err_free;
 	}
 
-	switch (cs40l26->revid) {
-	case CS40L26_REVID_A0:
-		ret = cs40l26_pseq_v1_multi_add_pair(cs40l26, dsp1rx_config, 2,
-				CS40L26_PSEQ_V1_REPLACE);
-		break;
-	case CS40L26_REVID_A1:
-		ret = cs40l26_pseq_v2_multi_add_write_reg_full(cs40l26,
-				dsp1rx_config, 2, true);
-		break;
-	default:
-		dev_err(cs40l26->dev, "Revid ID not supported: %02X\n",
-			cs40l26->revid);
-		ret = -EINVAL;
-		goto err_free;
-	}
-
+	ret = cs40l26_pseq_multi_add_write_reg_full(cs40l26, dsp1rx_config, 2,
+			true);
 	if (ret)
 		dev_err(cs40l26->dev, "Failed to add ASP config to pseq\n");
 
@@ -2854,25 +2706,36 @@ static int cs40l26_bst_dcm_config(struct cs40l26_private *cs40l26)
 			return ret;
 		}
 
-		switch (cs40l26->revid) {
-		case CS40L26_REVID_A0:
-			ret = cs40l26_pseq_v1_add_pair(cs40l26,
-						CS40L26_BST_DCM_CTL, val, true);
-			break;
-		case CS40L26_REVID_A1:
-			ret = cs40l26_pseq_v2_add_write_reg_full(cs40l26,
-						CS40L26_BST_DCM_CTL, val, true);
-			break;
-		default:
-			dev_err(cs40l26->dev, "Revid ID not supported: %02X\n",
-				cs40l26->revid);
-			ret = -EINVAL;
-		}
+		ret = cs40l26_pseq_add_write_reg_full(cs40l26,
+				CS40L26_BST_DCM_CTL, val, true);
 	}
 
 	return ret;
 }
 
+static int cs40l26_bst_ipk_config(struct cs40l26_private *cs40l26)
+{
+	u32 val, bst_ipk_ma = cs40l26->pdata.bst_ipk / 1000;
+	int ret;
+
+	if (bst_ipk_ma < CS40L26_BST_IPK_MILLIAMP_MIN ||
+			bst_ipk_ma > CS40L26_BST_IPK_MILLIAMP_MAX) {
+		val = CS40L26_BST_IPK_DEFAULT;
+		dev_dbg(cs40l26->dev, "Using default BST_IPK\n");
+	} else {
+		val = (bst_ipk_ma / 50) - 16;
+	}
+
+	ret = regmap_write(cs40l26->regmap, CS40L26_BST_IPK_CTL, val);
+	if (ret) {
+		dev_err(cs40l26->dev, "Failed to update BST peak current\n");
+		return ret;
+	}
+
+	return cs40l26_pseq_add_write_reg_full(cs40l26, CS40L26_BST_IPK_CTL,
+			val, true);
+}
+
 static int cs40l26_dsp_config(struct cs40l26_private *cs40l26)
 {
 	struct regmap *regmap = cs40l26->regmap;
@@ -2883,84 +2746,89 @@ static int cs40l26_dsp_config(struct cs40l26_private *cs40l26)
 
 	ret = cs40l26_verify_fw(cs40l26);
 	if (ret)
-		goto err_out;
+		return ret;
 
 	ret = regmap_update_bits(regmap, CS40L26_PWRMGT_CTL,
-			CS40L26_MEM_RDY_MASK,
-			CS40L26_ENABLE << CS40L26_MEM_RDY_SHIFT);
+			CS40L26_MEM_RDY_MASK, 1 << CS40L26_MEM_RDY_SHIFT);
 	if (ret) {
 		dev_err(dev, "Failed to set MEM_RDY to initialize RAM\n");
-		goto err_out;
+		return ret;
 	}
 
 	if (cs40l26->fw_mode == CS40L26_FW_MODE_RAM) {
 		ret = cl_dsp_get_reg(cs40l26->dsp, "CALL_RAM_INIT",
-				CL_DSP_XM_UNPACKED_TYPE,
-				CS40L26_FW_ID, &reg);
+				CL_DSP_XM_UNPACKED_TYPE, cs40l26->fw.id, &reg);
 		if (ret)
-			goto err_out;
+			return ret;
 
-		ret = cs40l26_dsp_write(cs40l26, reg, CS40L26_ENABLE);
+		ret = cs40l26_dsp_write(cs40l26, reg, 1);
 		if (ret)
-			goto err_out;
+			return ret;
 	}
 
 	cs40l26->fw_loaded = true;
 
 	ret = cs40l26_dsp_start(cs40l26);
 	if (ret)
-		goto err_out;
+		return ret;
 
 	ret = cs40l26_pseq_init(cs40l26);
 	if (ret)
-		goto err_out;
+		return ret;
+
+	ret = cs40l26_update_reg_defaults_via_pseq(cs40l26);
+	if (ret)
+		return ret;
 
 	ret = cs40l26_iseq_init(cs40l26);
 	if (ret)
-		goto err_out;
+		return ret;
 
 	ret = cs40l26_irq_update_mask(cs40l26, CS40L26_IRQ1_MASK_1,
 			BIT(CS40L26_IRQ1_VIRTUAL2_MBOX_WR), CS40L26_IRQ_UNMASK);
 	if (ret)
-		goto err_out;
+		return ret;
 
 	ret = cs40l26_wksrc_config(cs40l26);
 	if (ret)
-		goto err_out;
+		return ret;
 
 	ret = cs40l26_gpio_config(cs40l26);
 	if (ret)
-		goto err_out;
+		return ret;
 
 	ret = cs40l26_bst_dcm_config(cs40l26);
 	if (ret)
-		goto err_out;
+		return ret;
+
+	ret = cs40l26_bst_ipk_config(cs40l26);
+	if (ret)
+		return ret;
 
 	ret = cs40l26_brownout_prevention_init(cs40l26);
 	if (ret)
-		goto err_out;
+		return ret;
 
 	/* ensure firmware running */
 	ret = cl_dsp_get_reg(cs40l26->dsp, "HALO_STATE",
-			CL_DSP_XM_UNPACKED_TYPE, CS40L26_FW_ID,
-			&reg);
+			CL_DSP_XM_UNPACKED_TYPE, cs40l26->fw.id, &reg);
 	if (ret)
-		goto err_out;
+		return ret;
 
 	ret = regmap_read(regmap, reg, &val);
 	if (ret) {
 		dev_err(dev, "Failed to read HALO_STATE\n");
-		goto err_out;
+		return ret;
 	}
 
 	if (val != CS40L26_DSP_HALO_STATE_RUN) {
 		dev_err(dev, "Firmware in unexpected state: 0x%X\n", val);
-		goto err_out;
+		return ret;
 	}
 
 	ret = cs40l26_pm_runtime_setup(cs40l26);
 	if (ret)
-		goto err_out;
+		return ret;
 
 	pm_runtime_get_sync(dev);
 
@@ -2987,47 +2855,149 @@ static int cs40l26_dsp_config(struct cs40l26_private *cs40l26)
 pm_err:
 	pm_runtime_mark_last_busy(dev);
 	pm_runtime_put_autosuspend(dev);
-err_out:
-	enable_irq(cs40l26->irq);
+
 	return ret;
 }
 
-static void cs40l26_firmware_load(const struct firmware *fw, void *context)
+static int cs40l26_svc_le_select(struct cs40l26_private *cs40l26)
 {
-	struct cs40l26_private *cs40l26 = (struct cs40l26_private *)context;
-	struct device *dev = cs40l26->dev;
-	const struct firmware *coeff_fw;
+	unsigned int reg, le_est = 0;
 	int ret, i;
 
-	if (!fw) {
-		dev_err(dev, "Failed to request firmware file\n");
-		return;
-	}
+	pm_runtime_get_sync(cs40l26->dev);
 
-	cs40l26->pm_ready = false;
+	ret = cs40l26_ack_write(cs40l26, CS40L26_DSP_VIRTUAL1_MBOX_1,
+			CS40L26_DSP_MBOX_CMD_LE_EST, CS40L26_DSP_MBOX_RESET);
+	if (ret)
+		goto pm_err;
 
-	ret = cl_dsp_firmware_parse(cs40l26->dsp, fw,
-			cs40l26->fw_mode == CS40L26_FW_MODE_RAM);
-	release_firmware(fw);
+	ret = cl_dsp_get_reg(cs40l26->dsp, "LE_EST_STATUS",
+			CL_DSP_YM_UNPACKED_TYPE, CS40l26_SVC_ALGO_ID, &reg);
 	if (ret)
-		return;
+		goto pm_err;
+
+	for (i = 0; i < 2; i++) {
+		usleep_range(5000, 5100);
+		ret = regmap_read(cs40l26->regmap, reg, &le_est);
+		if (ret) {
+			dev_err(cs40l26->dev, "Failed to get LE_EST_STATUS\n");
+			goto pm_err;
+		}
+
+		if (le_est >= cs40l26->svc_le->le1_min &&
+				le_est <= cs40l26->svc_le->le1_max) {
+			cs40l26->fw.coeff_files[2] =
+					CS40L26_SVC_TUNING_FILE_NAME1;
+			break;
+		} else if (le_est >= cs40l26->svc_le->le2_min &&
+				le_est <= cs40l26->svc_le->le2_max) {
+			cs40l26->fw.coeff_files[2] =
+					CS40L26_SVC_TUNING_FILE_NAME2;
+			break;
+		}
+	}
+
+	if (i == 2) {
+		dev_warn(cs40l26->dev, "Falling back to default SVC tuning\n");
+		cs40l26->fw.coeff_files[2] = CS40L26_SVC_TUNING_FILE_NAME;
+	}
+
+pm_err:
+	pm_runtime_mark_last_busy(cs40l26->dev);
+	pm_runtime_put_autosuspend(cs40l26->dev);
+
+	return ret;
+}
+
+static void cs40l26_coeff_load(struct cs40l26_private *cs40l26)
+{
+	struct device *dev = cs40l26->dev;
+	const struct firmware *coeff;
+	int i;
 
 	for (i = 0; i < cs40l26->fw.num_coeff_files; i++) {
-		request_firmware(&coeff_fw, cs40l26->fw.coeff_files[i],
-				dev);
-		if (!coeff_fw) {
+		request_firmware(&coeff, cs40l26->fw.coeff_files[i], dev);
+		if (!coeff) {
 			dev_warn(dev, "Continuing...\n");
 			continue;
 		}
 
-		if (cl_dsp_coeff_file_parse(cs40l26->dsp, coeff_fw))
+		if (cl_dsp_coeff_file_parse(cs40l26->dsp, coeff))
 			dev_warn(dev, "Continuing...\n");
 		else
-			dev_dbg(dev, "%s Loaded Successfully\n",
-				cs40l26->fw.coeff_files[i]);
+			dev_info(dev, "%s Loaded Successfully\n",
+					cs40l26->fw.coeff_files[i]);
+	}
+}
+
+static int cs40l26_firmware_load(struct cs40l26_private *cs40l26, u32 id)
+{
+	struct device *dev = cs40l26->dev;
+	const struct firmware *fw;
+	int ret;
+
+	if (cs40l26->fw.id == CS40L26_FW_ID)
+		ret = request_firmware(&fw, CS40L26_FW_FILE_NAME, dev);
+	else
+		ret = request_firmware(&fw, CS40L26_FW_CALIB_NAME, dev);
+
+	if (ret)
+		return ret;
+
+	ret = cl_dsp_firmware_parse(cs40l26->dsp, fw, true);
+	release_firmware(fw);
+
+	return ret;
+}
+
+int cs40l26_fw_swap(struct cs40l26_private *cs40l26, u32 id)
+{
+	struct device *dev = cs40l26->dev;
+	int ret;
+
+	if (id == cs40l26->fw.id) {
+		dev_warn(dev, "Cannot swap to same ID as running firmware\n");
+		return 0;
 	}
 
-	cs40l26_dsp_config(cs40l26);
+	cs40l26_pm_runtime_teardown(cs40l26);
+
+	cs40l26->fw_loaded = false;
+
+	disable_irq(cs40l26->irq);
+
+	ret = cs40l26_cl_dsp_init(cs40l26, id);
+	if (ret)
+		goto irq_exit;
+
+	ret = cs40l26_dsp_pre_config(cs40l26);
+	if (ret)
+		goto irq_exit;
+
+	ret = cs40l26_firmware_load(cs40l26, id);
+	if (ret)
+		goto irq_exit;
+
+	cs40l26_coeff_load(cs40l26);
+
+	ret = cs40l26_dsp_config(cs40l26);
+
+irq_exit:
+	enable_irq(cs40l26->irq);
+
+	return ret;
+}
+EXPORT_SYMBOL(cs40l26_fw_swap);
+
+static int cs40l26_update_reg_defaults(struct cs40l26_private *cs40l26)
+{
+	int ret;
+
+	ret = regmap_update_bits(cs40l26->regmap, CS40L26_TST_DAC_MSM_CONFIG,
+			CS40L26_SPK_DEFAULT_HIZ_MASK, 1 <<
+			CS40L26_SPK_DEFAULT_HIZ_SHIFT);
+
+	return ret;
 }
 
 static int cs40l26_handle_platform_data(struct cs40l26_private *cs40l26)
@@ -3035,7 +3005,7 @@ static int cs40l26_handle_platform_data(struct cs40l26_private *cs40l26)
 	struct device *dev = cs40l26->dev;
 	struct device_node *np = dev->of_node;
 	const char *str = NULL;
-	u32 val;
+	u32 val, tmp;
 
 	if (!np) {
 		dev_err(dev, "No platform data found\n");
@@ -3113,6 +3083,35 @@ static int cs40l26_handle_platform_data(struct cs40l26_private *cs40l26)
 	else
 		cs40l26->pdata.bst_dcm_en = CS40L26_BST_DCM_EN_DEFAULT;
 
+	if (!of_property_read_u32(np, "cirrus,bst-ipk-microamp", &val))
+		cs40l26->pdata.bst_ipk = val;
+	else
+		cs40l26->pdata.bst_ipk = 0;
+
+	if (of_property_count_u32_elems(np, "cirrus,svc-le1") == 2 &&
+		of_property_count_u32_elems(np, "cirrus,svc-le2") == 2) {
+		of_property_read_u32_index(np, "cirrus,svc-le1", 0, &tmp);
+		of_property_read_u32_index(np, "cirrus,svc-le1", 1, &val);
+
+		cs40l26->svc_le = devm_kzalloc(dev,
+				sizeof(struct cs40l26_svc_le), GFP_KERNEL);
+		if (!cs40l26->svc_le)
+			return -ENOMEM;
+
+		cs40l26->svc_le->le1_min = min(tmp, val);
+		cs40l26->svc_le->le1_max = max(tmp, val);
+
+		of_property_read_u32_index(np, "cirrus,svc-le2", 0, &tmp);
+		of_property_read_u32_index(np, "cirrus,svc-le2", 1, &val);
+
+		cs40l26->svc_le->le2_min = min(tmp, val);
+		cs40l26->svc_le->le2_max = max(tmp, val);
+
+		cs40l26->pdata.svc_le_is_valid = true;
+	} else {
+		cs40l26->pdata.svc_le_is_valid = false;
+	}
+
 	return 0;
 }
 
@@ -3177,7 +3176,7 @@ int cs40l26_probe(struct cs40l26_private *cs40l26,
 	usleep_range(CS40L26_MIN_RESET_PULSE_WIDTH,
 			CS40L26_MIN_RESET_PULSE_WIDTH + 100);
 
-	gpiod_set_value_cansleep(cs40l26->reset_gpio, CS40L26_ENABLE);
+	gpiod_set_value_cansleep(cs40l26->reset_gpio, 1);
 
 	usleep_range(CS40L26_CONTROL_PORT_READY_DELAY,
 			CS40L26_CONTROL_PORT_READY_DELAY + 100);
@@ -3186,6 +3185,12 @@ int cs40l26_probe(struct cs40l26_private *cs40l26,
 	if (ret)
 		goto err;
 
+	ret = cs40l26_update_reg_defaults(cs40l26);
+	if (ret) {
+		dev_err(dev, "Failed to update reg defaults\n");
+		goto err;
+	}
+
 	ret = devm_request_threaded_irq(dev, cs40l26->irq, NULL, cs40l26_irq,
 			IRQF_ONESHOT | IRQF_SHARED | IRQF_TRIGGER_LOW,
 			"cs40l26", cs40l26);
@@ -3193,23 +3198,54 @@ int cs40l26_probe(struct cs40l26_private *cs40l26,
 		dev_err(dev, "Failed to request threaded IRQ\n");
 		goto err;
 	}
+
 	/* the /ALERT pin may be asserted prior to firmware initialization.
 	 * Disable the interrupt handler until firmware has downloaded
 	 * so erroneous interrupt requests are ignored
 	 */
 	disable_irq(cs40l26->irq);
 
-	ret = cs40l26_cl_dsp_init(cs40l26);
+	cs40l26->pm_ready = false;
+	cs40l26->fw_loaded = false;
+
+	ret = cs40l26_cl_dsp_init(cs40l26, CS40L26_FW_ID);
 	if (ret)
-		goto err;
+		goto irq_err;
 
 	ret = cs40l26_dsp_pre_config(cs40l26);
 	if (ret)
-		goto err;
+		goto irq_err;
+
+	ret = cs40l26_firmware_load(cs40l26, cs40l26->fw.id);
+	if (ret)
+		goto irq_err;
+
+	if (cs40l26->pdata.svc_le_is_valid) {
+		ret = cs40l26_dsp_config(cs40l26);
+		if (ret)
+			goto irq_err;
+
+		enable_irq(cs40l26->irq);
+
+		ret = cs40l26_svc_le_select(cs40l26);
+		if (ret)
+			goto err;
+
+		disable_irq(cs40l26->irq);
+		cs40l26_pm_runtime_teardown(cs40l26);
+
+		ret = cs40l26_dsp_pre_config(cs40l26);
+		if (ret)
+			goto err;
+	}
 
-	request_firmware_nowait(THIS_MODULE, FW_ACTION_HOTPLUG,
-			CS40L26_FW_FILE_NAME, dev, GFP_KERNEL, cs40l26,
-			cs40l26_firmware_load);
+	cs40l26_coeff_load(cs40l26);
+
+	ret = cs40l26_dsp_config(cs40l26);
+	if (ret)
+		goto irq_err;
+
+	enable_irq(cs40l26->irq);
 
 	ret = cs40l26_input_init(cs40l26);
 	if (ret)
@@ -3224,6 +3260,8 @@ int cs40l26_probe(struct cs40l26_private *cs40l26,
 
 	return 0;
 
+irq_err:
+	enable_irq(cs40l26->irq);
 err:
 	cs40l26_remove(cs40l26);
 
@@ -3246,10 +3284,10 @@ int cs40l26_remove(struct cs40l26_private *cs40l26)
 		cs40l26_pm_runtime_teardown(cs40l26);
 
 	if (cs40l26->vibe_workqueue) {
-		destroy_workqueue(cs40l26->vibe_workqueue);
 		cancel_work_sync(&cs40l26->vibe_start_work);
 		cancel_work_sync(&cs40l26->vibe_stop_work);
 		cancel_work_sync(&cs40l26->set_gain_work);
+		destroy_workqueue(cs40l26->vibe_workqueue);
 	}
 
 	if (vp_consumer)
@@ -3258,7 +3296,7 @@ int cs40l26_remove(struct cs40l26_private *cs40l26)
 	if (va_consumer)
 		regulator_disable(va_consumer);
 
-	gpiod_set_value_cansleep(cs40l26->reset_gpio, CS40L26_DISABLE);
+	gpiod_set_value_cansleep(cs40l26->reset_gpio, 0);
 
 	if (cs40l26->vibe_timer.function)
 		hrtimer_cancel(&cs40l26->vibe_timer);