2 files changed, 36 insertions, 9 deletions

diff --git a/include/linux/mfd/wm8994/registers.h b/include/linux/mfd/wm8994/registers.h
index ccf3a774fe36..be072faec6f0 100644
--- a/include/linux/mfd/wm8994/registers.h
+++ b/include/linux/mfd/wm8994/registers.h
@@ -64,6 +64,7 @@
 #define WM8994_LDO_1                            0x3B
 #define WM8994_LDO_2                            0x3C
 #define WM8994_CHARGE_PUMP_1                    0x4C
+#define WM8958_CHARGE_PUMP_2                    0x4D
 #define WM8994_CLASS_W_1                        0x51
 #define WM8994_DC_SERVO_1                       0x54
 #define WM8994_DC_SERVO_2                       0x55
@@ -1926,6 +1927,14 @@
 #define WM8994_CP_ENA_WIDTH                          1  /* CP_ENA */
 
 /*
+ * R77 (0x4D) - Charge Pump (2)
+ */
+#define WM8958_CP_DISCH                         0x8000  /* CP_DISCH */
+#define WM8958_CP_DISCH_MASK                    0x8000  /* CP_DISCH */
+#define WM8958_CP_DISCH_SHIFT                       15  /* CP_DISCH */
+#define WM8958_CP_DISCH_WIDTH                        1  /* CP_DISCH */
+
+/*
  * R81 (0x51) - Class W (1)
  */
 #define WM8994_CP_DYN_SRC_SEL_MASK              0x0300  /* CP_DYN_SRC_SEL - [9:8] */
diff --git a/sound/soc/codecs/wm8994.c b/sound/soc/codecs/wm8994.c
index 59d361145b15..af3a98ae0579 100644
--- a/sound/soc/codecs/wm8994.c
+++ b/sound/soc/codecs/wm8994.c
@@ -1858,15 +1858,33 @@ static int wm8994_set_bias_level(struct snd_soc_codec *codec,
                 if (codec->dapm.bias_level == SND_SOC_BIAS_OFF) {
                         pm_runtime_get_sync(codec->dev);
 
-                        /* Tweak DC servo and DSP configuration for
-                         * improved performance. */
-                        if (control->type == WM8994 && wm8994->revision < 4) {
-                                /* Tweak DC servo and DSP configuration for
-                                 * improved performance. */
-                                snd_soc_write(codec, 0x102, 0x3);
-                                snd_soc_write(codec, 0x56, 0x3);
-                                snd_soc_write(codec, 0x817, 0);
-                                snd_soc_write(codec, 0x102, 0);
+                        switch (control->type) {
+                        case WM8994:
+                                if (wm8994->revision < 4) {
+                                        /* Tweak DC servo and DSP
+                                         * configuration for improved
+                                         * performance. */
+                                        snd_soc_write(codec, 0x102, 0x3);
+                                        snd_soc_write(codec, 0x56, 0x3);
+                                        snd_soc_write(codec, 0x817, 0);
+                                        snd_soc_write(codec, 0x102, 0);
+                                }
+                                break;
+
+                        case WM8958:
+                                if (wm8994->revision == 0) {
+                                        /* Optimise performance for rev A */
+                                        snd_soc_write(codec, 0x102, 0x3);
+                                        snd_soc_write(codec, 0xcb, 0x81);
+                                        snd_soc_write(codec, 0x817, 0);
+                                        snd_soc_write(codec, 0x102, 0);
+
+                                        snd_soc_update_bits(codec,
+                                                            WM8958_CHARGE_PUMP_2,
+                                                            WM8958_CP_DISCH,
+                                                            WM8958_CP_DISCH);
+                                }
+                                break;
                         }
 
                         /* Discharge LINEOUT1 & 2 */