3 files changed, 33 insertions, 12 deletions

diff --git a/sound/soc/codecs/wm8994.c b/sound/soc/codecs/wm8994.c
index d10d65191fd2..c80218f23bb9 100644
--- a/sound/soc/codecs/wm8994.c
+++ b/sound/soc/codecs/wm8994.c
@@ -3730,11 +3730,12 @@ static int wm8994_codec_probe(struct platform_device *pdev)
         case 3:
                 wm8994->hubs.dcs_codes = -5;
                 wm8994->hubs.hp_startup_mode = 1;
+                wm8994->hubs.dcs_readback_mode = 1;
                 break;
         default:
+                wm8994->hubs.dcs_readback_mode = 1;
                 break;
         }
-                           
 
         /* Remember if AIFnLRCLK is configured as a GPIO.  This should be
          * configured on init - if a system wants to do this dynamically
diff --git a/sound/soc/codecs/wm_hubs.c b/sound/soc/codecs/wm_hubs.c
index 3729a12b151f..2b5c0924f615 100644
--- a/sound/soc/codecs/wm_hubs.c
+++ b/sound/soc/codecs/wm_hubs.c
@@ -86,7 +86,7 @@ static void wait_for_dc_servo(struct snd_soc_codec *codec)
 static void calibrate_dc_servo(struct snd_soc_codec *codec)
 {
         struct wm_hubs_data *hubs = codec->private_data;
-        u16 reg, dcs_cfg;
+        u16 reg, reg_l, reg_r, dcs_cfg;
 
         /* Set for 32 series updates */
         snd_soc_update_bits(codec, WM8993_DC_SERVO_1,
@@ -110,19 +110,38 @@ static void calibrate_dc_servo(struct snd_soc_codec *codec)
                 dev_dbg(codec->dev, "Applying %d code DC servo correction\n",
                         hubs->dcs_codes);
 
+                /* Different chips in the family support different
+                 * readback methods.
+                 */
+                switch (hubs->dcs_readback_mode) {
+                case 0:
+                        reg_l = snd_soc_read(codec, WM8993_DC_SERVO_READBACK_1)
+                                & WM8993_DCS_INTEG_CHAN_0_MASK;;
+                        reg_r = snd_soc_read(codec, WM8993_DC_SERVO_READBACK_2)
+                                & WM8993_DCS_INTEG_CHAN_1_MASK;
+                        break;
+                case 1:
+                        reg = snd_soc_read(codec, WM8993_DC_SERVO_3);
+                        reg_l = (reg & WM8993_DCS_DAC_WR_VAL_1_MASK)
+                                >> WM8993_DCS_DAC_WR_VAL_1_SHIFT;
+                        reg_r = reg & WM8993_DCS_DAC_WR_VAL_0_MASK;
+                        break;
+                default:
+                        WARN(1, "Unknown DCS readback method");
+                        break;
+                }
+
                 /* HPOUT1L */
-                reg = snd_soc_read(codec, WM8993_DC_SERVO_READBACK_1) &
-                        WM8993_DCS_INTEG_CHAN_0_MASK;;
-                if (reg + hubs->dcs_codes > 0 && reg + hubs->dcs_codes < 0xff)
-                        reg += hubs->dcs_codes;
-                dcs_cfg = reg << WM8993_DCS_DAC_WR_VAL_1_SHIFT;
+                if (reg_l + hubs->dcs_codes > 0 &&
+                    reg_l + hubs->dcs_codes < 0xff)
+                        reg_l += hubs->dcs_codes;
+                dcs_cfg = reg_l << WM8993_DCS_DAC_WR_VAL_1_SHIFT;
 
                 /* HPOUT1R */
-                reg = snd_soc_read(codec, WM8993_DC_SERVO_READBACK_2) &
-                        WM8993_DCS_INTEG_CHAN_1_MASK;
-                if (reg + hubs->dcs_codes > 0 && reg + hubs->dcs_codes < 0xff)
-                        reg += hubs->dcs_codes;
-                dcs_cfg |= reg;
+                if (reg_r + hubs->dcs_codes > 0 &&
+                    reg_r + hubs->dcs_codes < 0xff)
+                        reg_r += hubs->dcs_codes;
+                dcs_cfg |= reg_r;
 
                 /* Do it */
                 snd_soc_write(codec, WM8993_DC_SERVO_3, dcs_cfg);
diff --git a/sound/soc/codecs/wm_hubs.h b/sound/soc/codecs/wm_hubs.h
index 420104fe9c90..e51c16683589 100644
--- a/sound/soc/codecs/wm_hubs.h
+++ b/sound/soc/codecs/wm_hubs.h
@@ -21,6 +21,7 @@ extern const unsigned int wm_hubs_spkmix_tlv[];
 /* This *must* be the first element of the codec->private_data struct */
 struct wm_hubs_data {
         int dcs_codes;
+        int dcs_readback_mode;
         int hp_startup_mode;
 };