2 files changed, 127 insertions, 4 deletions

diff --git a/sound/soc/codecs/wm8903.c b/sound/soc/codecs/wm8903.c
index 119abceb1d85..2f912276a8f5 100644
--- a/sound/soc/codecs/wm8903.c
+++ b/sound/soc/codecs/wm8903.c
@@ -223,6 +223,9 @@ struct wm8903_priv {
         int fs;
         int deemph;
 
+        int dcs_pending;
+        int dcs_cache[4];
+
         /* Reference count */
         int class_w_users;
 
@@ -248,6 +251,10 @@ static int wm8903_volatile_register(struct snd_soc_codec *codec, unsigned int re
         case WM8903_WRITE_SEQUENCER_4:
         case WM8903_POWER_MANAGEMENT_3:
         case WM8903_POWER_MANAGEMENT_2:
+        case WM8903_DC_SERVO_READBACK_1:
+        case WM8903_DC_SERVO_READBACK_2:
+        case WM8903_DC_SERVO_READBACK_3:
+        case WM8903_DC_SERVO_READBACK_4:
                 return 1;
 
         default:
@@ -315,6 +322,103 @@ static int wm8903_cp_event(struct snd_soc_dapm_widget *w,
         return 0;
 }
 
+static int wm8903_dcs_event(struct snd_soc_dapm_widget *w,
+                            struct snd_kcontrol *kcontrol, int event)
+{
+        struct snd_soc_codec *codec = w->codec;
+        struct wm8903_priv *wm8903 = snd_soc_codec_get_drvdata(codec);
+
+        switch (event) {
+        case SND_SOC_DAPM_POST_PMU:
+                wm8903->dcs_pending |= 1 << w->shift;
+                break;
+        case SND_SOC_DAPM_PRE_PMD:
+                snd_soc_update_bits(codec, WM8903_DC_SERVO_0,
+                                    1 << w->shift, 0);
+                break;
+        }
+
+        return 0;
+}
+
+#define WM8903_DCS_MODE_WRITE_STOP 0
+#define WM8903_DCS_MODE_START_STOP 2
+
+static void wm8903_seq_notifier(struct snd_soc_dapm_context *dapm,
+                                enum snd_soc_dapm_type event, int subseq)
+{
+        struct snd_soc_codec *codec = container_of(dapm,
+                                                   struct snd_soc_codec, dapm);
+        struct wm8903_priv *wm8903 = snd_soc_codec_get_drvdata(codec);
+        int dcs_mode = WM8903_DCS_MODE_WRITE_STOP;
+        int i, val;
+
+        /* Complete any pending DC servo starts */
+        if (wm8903->dcs_pending) {
+                dev_dbg(codec->dev, "Starting DC servo for %x\n",
+                        wm8903->dcs_pending);
+
+                /* If we've no cached values then we need to do startup */
+                for (i = 0; i < ARRAY_SIZE(wm8903->dcs_cache); i++) {
+                        if (!(wm8903->dcs_pending & (1 << i)))
+                                continue;
+
+                        if (wm8903->dcs_cache[i]) {
+                                dev_dbg(codec->dev,
+                                        "Restore DC servo %d value %x\n",
+                                        3 - i, wm8903->dcs_cache[i]);
+
+                                snd_soc_write(codec, WM8903_DC_SERVO_4 + i,
+                                              wm8903->dcs_cache[i] & 0xff);
+                        } else {
+                                dev_dbg(codec->dev,
+                                        "Calibrate DC servo %d\n", 3 - i);
+                                dcs_mode = WM8903_DCS_MODE_START_STOP;
+                        }
+                }
+
+                /* Don't trust the cache for analogue */
+                if (wm8903->class_w_users)
+                        dcs_mode = WM8903_DCS_MODE_START_STOP;
+
+                snd_soc_update_bits(codec, WM8903_DC_SERVO_2,
+                                    WM8903_DCS_MODE_MASK, dcs_mode);
+
+                snd_soc_update_bits(codec, WM8903_DC_SERVO_0,
+                                    WM8903_DCS_ENA_MASK, wm8903->dcs_pending);
+
+                switch (dcs_mode) {
+                case WM8903_DCS_MODE_WRITE_STOP:
+                        break;
+
+                case WM8903_DCS_MODE_START_STOP:
+                        msleep(270);
+
+                        /* Cache the measured offsets for digital */
+                        if (wm8903->class_w_users)
+                                break;
+
+                        for (i = 0; i < ARRAY_SIZE(wm8903->dcs_cache); i++) {
+                                if (!(wm8903->dcs_pending & (1 << i)))
+                                        continue;
+
+                                val = snd_soc_read(codec,
+                                                   WM8903_DC_SERVO_READBACK_1 + i);
+                                dev_dbg(codec->dev, "DC servo %d: %x\n",
+                                        3 - i, val);
+                                wm8903->dcs_cache[i] = val;
+                        }
+                        break;
+
+                default:
+                        pr_warn("DCS mode %d delay not set\n", dcs_mode);
+                        break;
+                }
+
+                wm8903->dcs_pending = 0;
+        }
+}
+
 /*
  * When used with DAC outputs only the WM8903 charge pump supports
  * operation in class W mode, providing very low power consumption
@@ -847,10 +951,15 @@ SND_SOC_DAPM_PGA_S("LINEOUTR_ENA_OUTP", 3, WM8903_ANALOGUE_LINEOUT_0, 2, 0,
 SND_SOC_DAPM_PGA_S("LINEOUTR_ENA_DLY", 1, WM8903_ANALOGUE_LINEOUT_0, 1, 0,
                    NULL, 0),
 
-SND_SOC_DAPM_PGA_S("HPL_DCS", 3, WM8903_DC_SERVO_0, 3, 0, NULL, 0),
-SND_SOC_DAPM_PGA_S("HPR_DCS", 3, WM8903_DC_SERVO_0, 2, 0, NULL, 0),
-SND_SOC_DAPM_PGA_S("LINEOUTL_DCS", 3, WM8903_DC_SERVO_0, 1, 0, NULL, 0),
-SND_SOC_DAPM_PGA_S("LINEOUTR_DCS", 3, WM8903_DC_SERVO_0, 0, 0, NULL, 0),
+SND_SOC_DAPM_SUPPLY("DCS Master", WM8903_DC_SERVO_0, 4, 0, NULL, 0),
+SND_SOC_DAPM_PGA_S("HPL_DCS", 3, SND_SOC_NOPM, 3, 0, wm8903_dcs_event,
+                   SND_SOC_DAPM_POST_PMU | SND_SOC_DAPM_PRE_PMD),
+SND_SOC_DAPM_PGA_S("HPR_DCS", 3, SND_SOC_NOPM, 2, 0, wm8903_dcs_event,
+                   SND_SOC_DAPM_POST_PMU | SND_SOC_DAPM_PRE_PMD),
+SND_SOC_DAPM_PGA_S("LINEOUTL_DCS", 3, SND_SOC_NOPM, 1, 0, wm8903_dcs_event,
+                   SND_SOC_DAPM_POST_PMU | SND_SOC_DAPM_PRE_PMD),
+SND_SOC_DAPM_PGA_S("LINEOUTR_DCS", 3, SND_SOC_NOPM, 0, 0, wm8903_dcs_event,
+                   SND_SOC_DAPM_POST_PMU | SND_SOC_DAPM_PRE_PMD),
 
 SND_SOC_DAPM_PGA("Left Speaker PGA", WM8903_POWER_MANAGEMENT_5, 1, 0,
                  NULL, 0),
@@ -979,6 +1088,11 @@ static const struct snd_soc_dapm_route intercon[] = {
         { "LINEOUTL_ENA_DLY", NULL, "Left Line Output PGA" },
         { "LINEOUTR_ENA_DLY", NULL, "Right Line Output PGA" },
 
+        { "HPL_DCS", NULL, "DCS Master" },
+        { "HPR_DCS", NULL, "DCS Master" },
+        { "LINEOUTL_DCS", NULL, "DCS Master" },
+        { "LINEOUTR_DCS", NULL, "DCS Master" },
+
         { "HPL_DCS", NULL, "HPL_ENA_DLY" },
         { "HPR_DCS", NULL, "HPR_ENA_DLY" },
         { "LINEOUTL_DCS", NULL, "LINEOUTL_ENA_DLY" },
@@ -1901,6 +2015,7 @@ static struct snd_soc_codec_driver soc_codec_dev_wm8903 = {
         .reg_word_size = sizeof(u16),
         .reg_cache_default = wm8903_reg_defaults,
         .volatile_register = wm8903_volatile_register,
+        .seq_notifier = wm8903_seq_notifier,
 };
 
 #if defined(CONFIG_I2C) || defined(CONFIG_I2C_MODULE)
diff --git a/sound/soc/codecs/wm8903.h b/sound/soc/codecs/wm8903.h
index e8490f3edd03..9ef394ae0dd3 100644
--- a/sound/soc/codecs/wm8903.h
+++ b/sound/soc/codecs/wm8903.h
@@ -75,6 +75,14 @@ extern int wm8903_mic_detect(struct snd_soc_codec *codec,
 #define WM8903_ANALOGUE_SPK_OUTPUT_CONTROL_0    0x41
 #define WM8903_DC_SERVO_0                       0x43
 #define WM8903_DC_SERVO_2                       0x45
+#define WM8903_DC_SERVO_4                       0x47
+#define WM8903_DC_SERVO_5                       0x48
+#define WM8903_DC_SERVO_6                       0x49
+#define WM8903_DC_SERVO_7                       0x4A
+#define WM8903_DC_SERVO_READBACK_1              0x51
+#define WM8903_DC_SERVO_READBACK_2              0x52
+#define WM8903_DC_SERVO_READBACK_3              0x53
+#define WM8903_DC_SERVO_READBACK_4              0x54
 #define WM8903_ANALOGUE_HP_0                    0x5A
 #define WM8903_ANALOGUE_LINEOUT_0               0x5E
 #define WM8903_CHARGE_PUMP_0                    0x62