1 files changed, 181 insertions, 227 deletions

diff --git a/sound/soc/codecs/twl6040.c b/sound/soc/codecs/twl6040.c
index 3fc3fc64dd8..0f6067f04e2 100644
--- a/sound/soc/codecs/twl6040.c
+++ b/sound/soc/codecs/twl6040.c
@@ -38,6 +38,14 @@
 
 #include "twl6040.h"
 
+enum twl6040_dai_id {
+	TWL6040_DAI_LEGACY = 0,
+	TWL6040_DAI_UL,
+	TWL6040_DAI_DL1,
+	TWL6040_DAI_DL2,
+	TWL6040_DAI_VIB,
+};
+
 #define TWL6040_RATES		SNDRV_PCM_RATE_8000_96000
 #define TWL6040_FORMATS	(SNDRV_PCM_FMTBIT_S32_LE)
 
@@ -46,12 +54,7 @@
 #define TWL6040_OUTHF_0dB 0x03
 #define TWL6040_OUTHF_M52dB 0x1D
 
-/* Shadow register used by the driver */
-#define TWL6040_REG_SW_SHADOW	0x2F
-#define TWL6040_CACHEREGNUM	(TWL6040_REG_SW_SHADOW + 1)
-
-/* TWL6040_REG_SW_SHADOW (0x2F) fields */
-#define TWL6040_EAR_PATH_ENABLE	0x01
+#define TWL6040_CACHEREGNUM	(TWL6040_REG_STATUS + 1)
 
 struct twl6040_jack_data {
 	struct snd_soc_jack *jack;
@@ -67,91 +70,18 @@ struct twl6040_data {
 	int pll_power_mode;
 	int hs_power_mode;
 	int hs_power_mode_locked;
+	bool dl1_unmuted;
+	bool dl2_unmuted;
+	u8 dl12_cache[TWL6040_REG_HFRCTL - TWL6040_REG_HSLCTL + 1];
 	unsigned int clk_in;
 	unsigned int sysclk;
-	u16 hs_left_step;
-	u16 hs_right_step;
-	u16 hf_left_step;
-	u16 hf_right_step;
 	struct twl6040_jack_data hs_jack;
 	struct snd_soc_codec *codec;
-	struct workqueue_struct *workqueue;
 	struct mutex mutex;
 };
 
-/*
- * twl6040 register cache & default register settings
- */
-static const u8 twl6040_reg[TWL6040_CACHEREGNUM] = {
-	0x00, /* not used	0x00	*/
-	0x4B, /* REG_ASICID	0x01 (ro) */
-	0x00, /* REG_ASICREV	0x02 (ro) */
-	0x00, /* REG_INTID	0x03	*/
-	0x00, /* REG_INTMR	0x04	*/
-	0x00, /* REG_NCPCTRL	0x05	*/
-	0x00, /* REG_LDOCTL	0x06	*/
-	0x60, /* REG_HPPLLCTL	0x07	*/
-	0x00, /* REG_LPPLLCTL	0x08	*/
-	0x4A, /* REG_LPPLLDIV	0x09	*/
-	0x00, /* REG_AMICBCTL	0x0A	*/
-	0x00, /* REG_DMICBCTL	0x0B	*/
-	0x00, /* REG_MICLCTL	0x0C	*/
-	0x00, /* REG_MICRCTL	0x0D	*/
-	0x00, /* REG_MICGAIN	0x0E	*/
-	0x1B, /* REG_LINEGAIN	0x0F	*/
-	0x00, /* REG_HSLCTL	0x10	*/
-	0x00, /* REG_HSRCTL	0x11	*/
-	0x00, /* REG_HSGAIN	0x12	*/
-	0x00, /* REG_EARCTL	0x13	*/
-	0x00, /* REG_HFLCTL	0x14	*/
-	0x00, /* REG_HFLGAIN	0x15	*/
-	0x00, /* REG_HFRCTL	0x16	*/
-	0x00, /* REG_HFRGAIN	0x17	*/
-	0x00, /* REG_VIBCTLL	0x18	*/
-	0x00, /* REG_VIBDATL	0x19	*/
-	0x00, /* REG_VIBCTLR	0x1A	*/
-	0x00, /* REG_VIBDATR	0x1B	*/
-	0x00, /* REG_HKCTL1	0x1C	*/
-	0x00, /* REG_HKCTL2	0x1D	*/
-	0x00, /* REG_GPOCTL	0x1E	*/
-	0x00, /* REG_ALB	0x1F	*/
-	0x00, /* REG_DLB	0x20	*/
-	0x00, /* not used	0x21	*/
-	0x00, /* not used	0x22	*/
-	0x00, /* not used	0x23	*/
-	0x00, /* not used	0x24	*/
-	0x00, /* not used	0x25	*/
-	0x00, /* not used	0x26	*/
-	0x00, /* not used	0x27	*/
-	0x00, /* REG_TRIM1	0x28	*/
-	0x00, /* REG_TRIM2	0x29	*/
-	0x00, /* REG_TRIM3	0x2A	*/
-	0x00, /* REG_HSOTRIM	0x2B	*/
-	0x00, /* REG_HFOTRIM	0x2C	*/
-	0x09, /* REG_ACCCTL	0x2D	*/
-	0x00, /* REG_STATUS	0x2E (ro) */
-
-	0x00, /* REG_SW_SHADOW	0x2F - Shadow, non HW register */
-};
-
-/* List of registers to be restored after power up */
-static const int twl6040_restore_list[] = {
-	TWL6040_REG_MICLCTL,
-	TWL6040_REG_MICRCTL,
-	TWL6040_REG_MICGAIN,
-	TWL6040_REG_LINEGAIN,
-	TWL6040_REG_HSLCTL,
-	TWL6040_REG_HSRCTL,
-	TWL6040_REG_HSGAIN,
-	TWL6040_REG_EARCTL,
-	TWL6040_REG_HFLCTL,
-	TWL6040_REG_HFLGAIN,
-	TWL6040_REG_HFRCTL,
-	TWL6040_REG_HFRGAIN,
-};
-
 /* set of rates for each pll: low-power and high-performance */
-static unsigned int lp_rates[] = {
+static const unsigned int lp_rates[] = {
 	8000,
 	11250,
 	16000,
@@ -163,7 +93,7 @@ static unsigned int lp_rates[] = {
 	96000,
 };
 
-static unsigned int hp_rates[] = {
+static const unsigned int hp_rates[] = {
 	8000,
 	16000,
 	32000,
@@ -171,63 +101,73 @@ static unsigned int hp_rates[] = {
 	96000,
 };
 
-static struct snd_pcm_hw_constraint_list sysclk_constraints[] = {
+static const struct snd_pcm_hw_constraint_list sysclk_constraints[] = {
 	{ .count = ARRAY_SIZE(lp_rates), .list = lp_rates, },
 	{ .count = ARRAY_SIZE(hp_rates), .list = hp_rates, },
 };
 
-/*
- * read twl6040 register cache
- */
-static inline unsigned int twl6040_read_reg_cache(struct snd_soc_codec *codec,
-						unsigned int reg)
+static unsigned int twl6040_read(struct snd_soc_codec *codec, unsigned int reg)
 {
-	u8 *cache = codec->reg_cache;
+	struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
+	struct twl6040 *twl6040 = codec->control_data;
+	u8 value;
 
 	if (reg >= TWL6040_CACHEREGNUM)
 		return -EIO;
 
-	return cache[reg];
+	switch (reg) {
+	case TWL6040_REG_HSLCTL:
+	case TWL6040_REG_HSRCTL:
+	case TWL6040_REG_EARCTL:
+	case TWL6040_REG_HFLCTL:
+	case TWL6040_REG_HFRCTL:
+		value = priv->dl12_cache[reg - TWL6040_REG_HSLCTL];
+		break;
+	default:
+		value = twl6040_reg_read(twl6040, reg);
+		break;
+	}
+
+	return value;
 }
 
-/*
- * write twl6040 register cache
- */
-static inline void twl6040_write_reg_cache(struct snd_soc_codec *codec,
-						u8 reg, u8 value)
+static bool twl6040_can_write_to_chip(struct snd_soc_codec *codec,
+				  unsigned int reg)
 {
-	u8 *cache = codec->reg_cache;
+	struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
 
-	if (reg >= TWL6040_CACHEREGNUM)
-		return;
-	cache[reg] = value;
+	switch (reg) {
+	case TWL6040_REG_HSLCTL:
+	case TWL6040_REG_HSRCTL:
+	case TWL6040_REG_EARCTL:
+		/* DL1 path */
+		return priv->dl1_unmuted;
+	case TWL6040_REG_HFLCTL:
+	case TWL6040_REG_HFRCTL:
+		return priv->dl2_unmuted;
+	default:
+		return 1;
+	}
 }
 
-/*
- * read from twl6040 hardware register
- */
-static int twl6040_read_reg_volatile(struct snd_soc_codec *codec,
-			unsigned int reg)
+static inline void twl6040_update_dl12_cache(struct snd_soc_codec *codec,
+					     u8 reg, u8 value)
 {
-	struct twl6040 *twl6040 = codec->control_data;
-	u8 value;
-
-	if (reg >= TWL6040_CACHEREGNUM)
-		return -EIO;
+	struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
 
-	if (likely(reg < TWL6040_REG_SW_SHADOW)) {
-		value = twl6040_reg_read(twl6040, reg);
-		twl6040_write_reg_cache(codec, reg, value);
-	} else {
-		value = twl6040_read_reg_cache(codec, reg);
+	switch (reg) {
+	case TWL6040_REG_HSLCTL:
+	case TWL6040_REG_HSRCTL:
+	case TWL6040_REG_EARCTL:
+	case TWL6040_REG_HFLCTL:
+	case TWL6040_REG_HFRCTL:
+		priv->dl12_cache[reg - TWL6040_REG_HSLCTL] = value;
+		break;
+	default:
+		break;
 	}
-
-	return value;
 }
 
-/*
- * write to the twl6040 register space
- */
 static int twl6040_write(struct snd_soc_codec *codec,
 			unsigned int reg, unsigned int value)
 {
@@ -236,8 +176,8 @@ static int twl6040_write(struct snd_soc_codec *codec,
 	if (reg >= TWL6040_CACHEREGNUM)
 		return -EIO;
 
-	twl6040_write_reg_cache(codec, reg, value);
-	if (likely(reg < TWL6040_REG_SW_SHADOW))
+	twl6040_update_dl12_cache(codec, reg, value);
+	if (twl6040_can_write_to_chip(codec, reg))
 		return twl6040_reg_write(twl6040, reg, value);
 	else
 		return 0;
@@ -245,45 +185,27 @@ static int twl6040_write(struct snd_soc_codec *codec,
 
 static void twl6040_init_chip(struct snd_soc_codec *codec)
 {
-	struct twl6040 *twl6040 = codec->control_data;
-	u8 val;
-
-	/* Update reg_cache: ASICREV, and TRIM values */
-	val = twl6040_get_revid(twl6040);
-	twl6040_write_reg_cache(codec, TWL6040_REG_ASICREV, val);
-
-	twl6040_read_reg_volatile(codec, TWL6040_REG_TRIM1);
-	twl6040_read_reg_volatile(codec, TWL6040_REG_TRIM2);
-	twl6040_read_reg_volatile(codec, TWL6040_REG_TRIM3);
-	twl6040_read_reg_volatile(codec, TWL6040_REG_HSOTRIM);
-	twl6040_read_reg_volatile(codec, TWL6040_REG_HFOTRIM);
+	twl6040_read(codec, TWL6040_REG_TRIM1);
+	twl6040_read(codec, TWL6040_REG_TRIM2);
+	twl6040_read(codec, TWL6040_REG_TRIM3);
+	twl6040_read(codec, TWL6040_REG_HSOTRIM);
+	twl6040_read(codec, TWL6040_REG_HFOTRIM);
 
 	/* Change chip defaults */
 	/* No imput selected for microphone amplifiers */
-	twl6040_write_reg_cache(codec, TWL6040_REG_MICLCTL, 0x18);
-	twl6040_write_reg_cache(codec, TWL6040_REG_MICRCTL, 0x18);
+	twl6040_write(codec, TWL6040_REG_MICLCTL, 0x18);
+	twl6040_write(codec, TWL6040_REG_MICRCTL, 0x18);
 
 	/*
 	 * We need to lower the default gain values, so the ramp code
 	 * can work correctly for the first playback.
 	 * This reduces the pop noise heard at the first playback.
 	 */
-	twl6040_write_reg_cache(codec, TWL6040_REG_HSGAIN, 0xff);
-	twl6040_write_reg_cache(codec, TWL6040_REG_EARCTL, 0x1e);
-	twl6040_write_reg_cache(codec, TWL6040_REG_HFLGAIN, 0x1d);
-	twl6040_write_reg_cache(codec, TWL6040_REG_HFRGAIN, 0x1d);
-	twl6040_write_reg_cache(codec, TWL6040_REG_LINEGAIN, 0);
-}
-
-static void twl6040_restore_regs(struct snd_soc_codec *codec)
-{
-	u8 *cache = codec->reg_cache;
-	int reg, i;
-
-	for (i = 0; i < ARRAY_SIZE(twl6040_restore_list); i++) {
-		reg = twl6040_restore_list[i];
-		twl6040_write(codec, reg, cache[reg]);
-	}
+	twl6040_write(codec, TWL6040_REG_HSGAIN, 0xff);
+	twl6040_write(codec, TWL6040_REG_EARCTL, 0x1e);
+	twl6040_write(codec, TWL6040_REG_HFLGAIN, 0x1d);
+	twl6040_write(codec, TWL6040_REG_HFRGAIN, 0x1d);
+	twl6040_write(codec, TWL6040_REG_LINEGAIN, 0);
 }
 
 /* set headset dac and driver power mode */
@@ -292,8 +214,8 @@ static int headset_power_mode(struct snd_soc_codec *codec, int high_perf)
 	int hslctl, hsrctl;
 	int mask = TWL6040_HSDRVMODE | TWL6040_HSDACMODE;
 
-	hslctl = twl6040_read_reg_cache(codec, TWL6040_REG_HSLCTL);
-	hsrctl = twl6040_read_reg_cache(codec, TWL6040_REG_HSRCTL);
+	hslctl = twl6040_read(codec, TWL6040_REG_HSLCTL);
+	hsrctl = twl6040_read(codec, TWL6040_REG_HSRCTL);
 
 	if (high_perf) {
 		hslctl &= ~mask;
@@ -320,8 +242,8 @@ static int twl6040_hs_dac_event(struct snd_soc_dapm_widget *w,
 	 * Both HS DAC need to be turned on (before the HS driver) and off at
 	 * the same time.
 	 */
-	hslctl = twl6040_read_reg_cache(codec, TWL6040_REG_HSLCTL);
-	hsrctl = twl6040_read_reg_cache(codec, TWL6040_REG_HSRCTL);
+	hslctl = twl6040_read(codec, TWL6040_REG_HSLCTL);
+	hsrctl = twl6040_read(codec, TWL6040_REG_HSRCTL);
 	if (SND_SOC_DAPM_EVENT_ON(event)) {
 		hslctl |= TWL6040_HSDACENA;
 		hsrctl |= TWL6040_HSDACENA;
@@ -366,7 +288,7 @@ static void twl6040_hs_jack_report(struct snd_soc_codec *codec,
 	mutex_lock(&priv->mutex);
 
 	/* Sync status */
-	status = twl6040_read_reg_volatile(codec, TWL6040_REG_STATUS);
+	status = twl6040_read(codec, TWL6040_REG_STATUS);
 	if (status & TWL6040_PLUGCOMP)
 		snd_soc_jack_report(jack, report, report);
 	else
@@ -404,8 +326,8 @@ static irqreturn_t twl6040_audio_handler(int irq, void *data)
 	struct snd_soc_codec *codec = data;
 	struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
 
-	queue_delayed_work(priv->workqueue, &priv->hs_jack.work,
-			   msecs_to_jiffies(200));
+	queue_delayed_work(system_power_efficient_wq,
+			   &priv->hs_jack.work, msecs_to_jiffies(200));
 
 	return IRQ_HANDLED;
 }
@@ -413,14 +335,12 @@ static irqreturn_t twl6040_audio_handler(int irq, void *data)
 static int twl6040_soc_dapm_put_vibra_enum(struct snd_kcontrol *kcontrol,
 	struct snd_ctl_elem_value *ucontrol)
 {
-	struct snd_soc_dapm_widget_list *wlist = snd_kcontrol_chip(kcontrol);
-	struct snd_soc_dapm_widget *widget = wlist->widgets[0];
-	struct snd_soc_codec *codec = widget->codec;
+	struct snd_soc_codec *codec = snd_soc_dapm_kcontrol_codec(kcontrol);
 	struct soc_enum *e = (struct soc_enum *)kcontrol->private_value;
 	unsigned int val;
 
 	/* Do not allow changes while Input/FF efect is running */
-	val = twl6040_read_reg_volatile(codec, e->reg);
+	val = twl6040_read(codec, e->reg);
 	if (val & TWL6040_VIBENA && !(val & TWL6040_VIBSEL))
 		return -EBUSY;
 
@@ -472,8 +392,10 @@ static const char *twl6040_amicr_texts[] =
 	{"Headset Mic", "Sub Mic", "Aux/FM Right", "Off"};
 
 static const struct soc_enum twl6040_enum[] = {
-	SOC_ENUM_SINGLE(TWL6040_REG_MICLCTL, 3, 4, twl6040_amicl_texts),
-	SOC_ENUM_SINGLE(TWL6040_REG_MICRCTL, 3, 4, twl6040_amicr_texts),
+	SOC_ENUM_SINGLE(TWL6040_REG_MICLCTL, 3,
+			ARRAY_SIZE(twl6040_amicl_texts), twl6040_amicl_texts),
+	SOC_ENUM_SINGLE(TWL6040_REG_MICRCTL, 3,
+			ARRAY_SIZE(twl6040_amicr_texts), twl6040_amicr_texts),
 };
 
 static const char *twl6040_hs_texts[] = {
@@ -532,7 +454,7 @@ static const struct snd_kcontrol_new hfr_mux_controls =
 	SOC_DAPM_ENUM("Route", twl6040_hf_enum[1]);
 
 static const struct snd_kcontrol_new ep_path_enable_control =
-	SOC_DAPM_SINGLE("Switch", TWL6040_REG_SW_SHADOW, 0, 1, 0);
+	SOC_DAPM_SINGLE_VIRT("Switch", 1);
 
 static const struct snd_kcontrol_new auxl_switch_control =
 	SOC_DAPM_SINGLE("Switch", TWL6040_REG_HFLCTL, 6, 1, 0);
@@ -556,14 +478,13 @@ static const char *twl6040_power_mode_texts[] = {
 	"Low-Power", "High-Performance",
 };
 
-static const struct soc_enum twl6040_power_mode_enum =
-	SOC_ENUM_SINGLE_EXT(ARRAY_SIZE(twl6040_power_mode_texts),
-			twl6040_power_mode_texts);
+static SOC_ENUM_SINGLE_EXT_DECL(twl6040_power_mode_enum,
+				twl6040_power_mode_texts);
 
 static int twl6040_headset_power_get_enum(struct snd_kcontrol *kcontrol,
 	struct snd_ctl_elem_value *ucontrol)
 {
-	struct snd_soc_codec *codec = snd_kcontrol_chip(kcontrol);
+	struct snd_soc_codec *codec = snd_soc_kcontrol_codec(kcontrol);
 	struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
 
 	ucontrol->value.enumerated.item[0] = priv->hs_power_mode;
@@ -574,7 +495,7 @@ static int twl6040_headset_power_get_enum(struct snd_kcontrol *kcontrol,
 static int twl6040_headset_power_put_enum(struct snd_kcontrol *kcontrol,
 	struct snd_ctl_elem_value *ucontrol)
 {
-	struct snd_soc_codec *codec = snd_kcontrol_chip(kcontrol);
+	struct snd_soc_codec *codec = snd_soc_kcontrol_codec(kcontrol);
 	struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
 	int high_perf = ucontrol->value.enumerated.item[0];
 	int ret = 0;
@@ -591,7 +512,7 @@ static int twl6040_headset_power_put_enum(struct snd_kcontrol *kcontrol,
 static int twl6040_pll_get_enum(struct snd_kcontrol *kcontrol,
 	struct snd_ctl_elem_value *ucontrol)
 {
-	struct snd_soc_codec *codec = snd_kcontrol_chip(kcontrol);
+	struct snd_soc_codec *codec = snd_soc_kcontrol_codec(kcontrol);
 	struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
 
 	ucontrol->value.enumerated.item[0] = priv->pll_power_mode;
@@ -602,7 +523,7 @@ static int twl6040_pll_get_enum(struct snd_kcontrol *kcontrol,
 static int twl6040_pll_put_enum(struct snd_kcontrol *kcontrol,
 	struct snd_ctl_elem_value *ucontrol)
 {
-	struct snd_soc_codec *codec = snd_kcontrol_chip(kcontrol);
+	struct snd_soc_codec *codec = snd_soc_kcontrol_codec(kcontrol);
 	struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
 
 	priv->pll_power_mode = ucontrol->value.enumerated.item[0];
@@ -645,7 +566,7 @@ int twl6040_get_trim_value(struct snd_soc_codec *codec, enum twl6040_trim trim)
 	if (unlikely(trim >= TWL6040_TRIM_INVAL))
 		return -EINVAL;
 
-	return twl6040_read_reg_cache(codec, TWL6040_REG_TRIM1 + trim);
+	return twl6040_read(codec, TWL6040_REG_TRIM1 + trim);
 }
 EXPORT_SYMBOL_GPL(twl6040_get_trim_value);
 
@@ -920,8 +841,6 @@ static int twl6040_set_bias_level(struct snd_soc_codec *codec,
 
 		priv->codec_powered = 1;
 
-		twl6040_restore_regs(codec);
-
 		/* Set external boost GPO */
 		twl6040_write(codec, TWL6040_REG_GPOCTL, 0x02);
 		break;
@@ -1032,16 +951,84 @@ static int twl6040_set_dai_sysclk(struct snd_soc_dai *codec_dai,
 	return 0;
 }
 
+static void twl6040_mute_path(struct snd_soc_codec *codec, enum twl6040_dai_id id,
+			     int mute)
+{
+	struct twl6040 *twl6040 = codec->control_data;
+	struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
+	int hslctl, hsrctl, earctl;
+	int hflctl, hfrctl;
+
+	switch (id) {
+	case TWL6040_DAI_DL1:
+		hslctl = twl6040_read(codec, TWL6040_REG_HSLCTL);
+		hsrctl = twl6040_read(codec, TWL6040_REG_HSRCTL);
+		earctl = twl6040_read(codec, TWL6040_REG_EARCTL);
+
+		if (mute) {
+			/* Power down drivers and DACs */
+			earctl &= ~0x01;
+			hslctl &= ~(TWL6040_HSDRVENA | TWL6040_HSDACENA);
+			hsrctl &= ~(TWL6040_HSDRVENA | TWL6040_HSDACENA);
+
+		}
+
+		twl6040_reg_write(twl6040, TWL6040_REG_EARCTL, earctl);
+		twl6040_reg_write(twl6040, TWL6040_REG_HSLCTL, hslctl);
+		twl6040_reg_write(twl6040, TWL6040_REG_HSRCTL, hsrctl);
+		priv->dl1_unmuted = !mute;
+		break;
+	case TWL6040_DAI_DL2:
+		hflctl = twl6040_read(codec, TWL6040_REG_HFLCTL);
+		hfrctl = twl6040_read(codec, TWL6040_REG_HFRCTL);
+
+		if (mute) {
+			/* Power down drivers and DACs */
+			hflctl &= ~(TWL6040_HFDACENA | TWL6040_HFPGAENA |
+				    TWL6040_HFDRVENA);
+			hfrctl &= ~(TWL6040_HFDACENA | TWL6040_HFPGAENA |
+				    TWL6040_HFDRVENA);
+		}
+
+		twl6040_reg_write(twl6040, TWL6040_REG_HFLCTL, hflctl);
+		twl6040_reg_write(twl6040, TWL6040_REG_HFRCTL, hfrctl);
+		priv->dl2_unmuted = !mute;
+		break;
+	default:
+		break;
+	}
+}
+
+static int twl6040_digital_mute(struct snd_soc_dai *dai, int mute)
+{
+	switch (dai->id) {
+	case TWL6040_DAI_LEGACY:
+		twl6040_mute_path(dai->codec, TWL6040_DAI_DL1, mute);
+		twl6040_mute_path(dai->codec, TWL6040_DAI_DL2, mute);
+		break;
+	case TWL6040_DAI_DL1:
+	case TWL6040_DAI_DL2:
+		twl6040_mute_path(dai->codec, dai->id, mute);
+		break;
+	default:
+		break;
+	}
+
+	return 0;
+}
+
 static const struct snd_soc_dai_ops twl6040_dai_ops = {
 	.startup	= twl6040_startup,
 	.hw_params	= twl6040_hw_params,
 	.prepare	= twl6040_prepare,
 	.set_sysclk	= twl6040_set_dai_sysclk,
+	.digital_mute	= twl6040_digital_mute,
 };
 
 static struct snd_soc_dai_driver twl6040_dai[] = {
 {
 	.name = "twl6040-legacy",
+	.id = TWL6040_DAI_LEGACY,
 	.playback = {
 		.stream_name = "Legacy Playback",
 		.channels_min = 1,
@@ -1060,6 +1047,7 @@ static struct snd_soc_dai_driver twl6040_dai[] = {
 },
 {
 	.name = "twl6040-ul",
+	.id = TWL6040_DAI_UL,
 	.capture = {
 		.stream_name = "Capture",
 		.channels_min = 1,
@@ -1071,6 +1059,7 @@ static struct snd_soc_dai_driver twl6040_dai[] = {
 },
 {
 	.name = "twl6040-dl1",
+	.id = TWL6040_DAI_DL1,
 	.playback = {
 		.stream_name = "Headset Playback",
 		.channels_min = 1,
@@ -1082,6 +1071,7 @@ static struct snd_soc_dai_driver twl6040_dai[] = {
 },
 {
 	.name = "twl6040-dl2",
+	.id = TWL6040_DAI_DL2,
 	.playback = {
 		.stream_name = "Handsfree Playback",
 		.channels_min = 1,
@@ -1093,6 +1083,7 @@ static struct snd_soc_dai_driver twl6040_dai[] = {
 },
 {
 	.name = "twl6040-vib",
+	.id = TWL6040_DAI_VIB,
 	.playback = {
 		.stream_name = "Vibra Playback",
 		.channels_min = 1,
@@ -1115,7 +1106,6 @@ static int twl6040_suspend(struct snd_soc_codec *codec)
 static int twl6040_resume(struct snd_soc_codec *codec)
 {
 	twl6040_set_bias_level(codec, SND_SOC_BIAS_STANDBY);
-	twl6040_set_bias_level(codec, codec->dapm.suspend_bias_level);
 
 	return 0;
 }
@@ -1127,83 +1117,50 @@ static int twl6040_resume(struct snd_soc_codec *codec)
 static int twl6040_probe(struct snd_soc_codec *codec)
 {
 	struct twl6040_data *priv;
-	struct twl6040_codec_data *pdata = dev_get_platdata(codec->dev);
+	struct twl6040 *twl6040 = dev_get_drvdata(codec->dev->parent);
 	struct platform_device *pdev = container_of(codec->dev,
 						   struct platform_device, dev);
 	int ret = 0;
 
-	priv = kzalloc(sizeof(struct twl6040_data), GFP_KERNEL);
+	priv = devm_kzalloc(codec->dev, sizeof(*priv), GFP_KERNEL);
 	if (priv == NULL)
 		return -ENOMEM;
+
 	snd_soc_codec_set_drvdata(codec, priv);
 
 	priv->codec = codec;
-	codec->control_data = dev_get_drvdata(codec->dev->parent);
-
-	if (pdata && pdata->hs_left_step && pdata->hs_right_step) {
-		priv->hs_left_step = pdata->hs_left_step;
-		priv->hs_right_step = pdata->hs_right_step;
-	} else {
-		priv->hs_left_step = 1;
-		priv->hs_right_step = 1;
-	}
-
-	if (pdata && pdata->hf_left_step && pdata->hf_right_step) {
-		priv->hf_left_step = pdata->hf_left_step;
-		priv->hf_right_step = pdata->hf_right_step;
-	} else {
-		priv->hf_left_step = 1;
-		priv->hf_right_step = 1;
-	}
+	codec->control_data = twl6040;
 
 	priv->plug_irq = platform_get_irq(pdev, 0);
 	if (priv->plug_irq < 0) {
 		dev_err(codec->dev, "invalid irq\n");
-		ret = -EINVAL;
-		goto work_err;
-	}
-
-	priv->workqueue = alloc_workqueue("twl6040-codec", 0, 0);
-	if (!priv->workqueue) {
-		ret = -ENOMEM;
-		goto work_err;
+		return -EINVAL;
 	}
 
 	INIT_DELAYED_WORK(&priv->hs_jack.work, twl6040_accessory_work);
 
 	mutex_init(&priv->mutex);
 
-	ret = request_threaded_irq(priv->plug_irq, NULL, twl6040_audio_handler,
-				   0, "twl6040_irq_plug", codec);
+	ret = request_threaded_irq(priv->plug_irq, NULL,
+					twl6040_audio_handler, IRQF_NO_SUSPEND,
+					"twl6040_irq_plug", codec);
 	if (ret) {
 		dev_err(codec->dev, "PLUG IRQ request failed: %d\n", ret);
-		goto plugirq_err;
+		return ret;
 	}
 
+	twl6040_set_bias_level(codec, SND_SOC_BIAS_STANDBY);
 	twl6040_init_chip(codec);
 
-	/* power on device */
-	ret = twl6040_set_bias_level(codec, SND_SOC_BIAS_STANDBY);
-	if (!ret)
-		return 0;
-
-	/* Error path */
-	free_irq(priv->plug_irq, codec);
-plugirq_err:
-	destroy_workqueue(priv->workqueue);
-work_err:
-	kfree(priv);
-	return ret;
+	return 0;
 }
 
 static int twl6040_remove(struct snd_soc_codec *codec)
 {
 	struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
 
-	twl6040_set_bias_level(codec, SND_SOC_BIAS_OFF);
 	free_irq(priv->plug_irq, codec);
-	destroy_workqueue(priv->workqueue);
-	kfree(priv);
+	twl6040_set_bias_level(codec, SND_SOC_BIAS_OFF);
 
 	return 0;
 }
@@ -1213,12 +1170,9 @@ static struct snd_soc_codec_driver soc_codec_dev_twl6040 = {
 	.remove = twl6040_remove,
 	.suspend = twl6040_suspend,
 	.resume = twl6040_resume,
-	.read = twl6040_read_reg_cache,
+	.read = twl6040_read,
 	.write = twl6040_write,
 	.set_bias_level = twl6040_set_bias_level,
-	.reg_cache_size = ARRAY_SIZE(twl6040_reg),
-	.reg_word_size = sizeof(u8),
-	.reg_cache_default = twl6040_reg,
 	.ignore_pmdown_time = true,
 
 	.controls = twl6040_snd_controls,