2 files changed, 114 insertions, 2 deletions

diff --git a/include/linux/mfd/twl6040.h b/include/linux/mfd/twl6040.h
index 94ac944d12f0..7e7fbce7a308 100644
--- a/include/linux/mfd/twl6040.h
+++ b/include/linux/mfd/twl6040.h
@@ -125,8 +125,15 @@
 
 #define TWL6040_HSDACENA                (1 << 0)
 #define TWL6040_HSDACMODE               (1 << 1)
+#define TWL6040_HSDRVENA                (1 << 2)
 #define TWL6040_HSDRVMODE               (1 << 3)
 
+/* HFLCTL/R (0x14/0x16) fields */
+
+#define TWL6040_HFDACENA                (1 << 0)
+#define TWL6040_HFPGAENA                (1 << 1)
+#define TWL6040_HFDRVENA                (1 << 4)
+
 /* VIBCTLL/R (0x18/0x1A) fields */
 
 #define TWL6040_VIBENA                  (1 << 0)
diff --git a/sound/soc/codecs/twl6040.c b/sound/soc/codecs/twl6040.c
index 9b9a6e587610..44621ddc332d 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)
 
@@ -67,6 +75,8 @@ struct twl6040_data {
         int pll_power_mode;
         int hs_power_mode;
         int hs_power_mode_locked;
+        bool dl1_unmuted;
+        bool dl2_unmuted;
         unsigned int clk_in;
         unsigned int sysclk;
         struct twl6040_jack_data hs_jack;
@@ -220,6 +230,25 @@ static int twl6040_read_reg_volatile(struct snd_soc_codec *codec,
         return value;
 }
 
+static bool twl6040_is_path_unmuted(struct snd_soc_codec *codec,
+                                    unsigned int reg)
+{
+        struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
+
+        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;
+        };
+}
+
 /*
  * write to the twl6040 register space
  */
@@ -232,7 +261,8 @@ static int twl6040_write(struct snd_soc_codec *codec,
                 return -EIO;
 
         twl6040_write_reg_cache(codec, reg, value);
-        if (likely(reg < TWL6040_REG_SW_SHADOW))
+        if (likely(reg < TWL6040_REG_SW_SHADOW) &&
+            twl6040_is_path_unmuted(codec, reg))
                 return twl6040_reg_write(twl6040, reg, value);
         else
                 return 0;
@@ -1026,16 +1056,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_reg_cache(codec, TWL6040_REG_HSLCTL);
+                hsrctl = twl6040_read_reg_cache(codec, TWL6040_REG_HSRCTL);
+                earctl = twl6040_read_reg_cache(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_reg_cache(codec, TWL6040_REG_HFLCTL);
+                hfrctl = twl6040_read_reg_cache(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,
@@ -1054,6 +1152,7 @@ static struct snd_soc_dai_driver twl6040_dai[] = {
 },
 {
         .name = "twl6040-ul",
+        .id = TWL6040_DAI_UL,
         .capture = {
                 .stream_name = "Capture",
                 .channels_min = 1,
@@ -1065,6 +1164,7 @@ static struct snd_soc_dai_driver twl6040_dai[] = {
 },
 {
         .name = "twl6040-dl1",
+        .id = TWL6040_DAI_DL1,
         .playback = {
                 .stream_name = "Headset Playback",
                 .channels_min = 1,
@@ -1076,6 +1176,7 @@ static struct snd_soc_dai_driver twl6040_dai[] = {
 },
 {
         .name = "twl6040-dl2",
+        .id = TWL6040_DAI_DL2,
         .playback = {
                 .stream_name = "Handsfree Playback",
                 .channels_min = 1,
@@ -1087,6 +1188,7 @@ static struct snd_soc_dai_driver twl6040_dai[] = {
 },
 {
         .name = "twl6040-vib",
+        .id = TWL6040_DAI_VIB,
         .playback = {
                 .stream_name = "Vibra Playback",
                 .channels_min = 1,
@@ -1143,7 +1245,7 @@ static int twl6040_probe(struct snd_soc_codec *codec)
 
         mutex_init(&priv->mutex);
 
-        ret = devm_request_threaded_irq(codec->dev, priv->plug_irq, NULL,
+        ret = request_threaded_irq(priv->plug_irq, NULL,
                                         twl6040_audio_handler, IRQF_NO_SUSPEND,
                                         "twl6040_irq_plug", codec);
         if (ret) {
@@ -1159,6 +1261,9 @@ static int twl6040_probe(struct snd_soc_codec *codec)
 
 static int twl6040_remove(struct snd_soc_codec *codec)
 {
+        struct twl6040_data *priv = snd_soc_codec_get_drvdata(codec);
+
+        free_irq(priv->plug_irq, codec);
         twl6040_set_bias_level(codec, SND_SOC_BIAS_OFF);
 
         return 0;