2 files changed, 76 insertions, 7 deletions

diff --git a/include/sound/soc.h b/include/sound/soc.h
index 3e9cae001eab..82bd773f8ab1 100644
--- a/include/sound/soc.h
+++ b/include/sound/soc.h
@@ -192,6 +192,14 @@
                 ((unsigned long)&(struct soc_bytes)           \
                 {.base = xbase, .num_regs = xregs }) }
 
+#define SND_SOC_BYTES_MASK(xname, xbase, xregs, xmask)        \
+{       .iface = SNDRV_CTL_ELEM_IFACE_MIXER, .name = xname,   \
+        .info = snd_soc_bytes_info, .get = snd_soc_bytes_get, \
+        .put = snd_soc_bytes_put, .private_value =            \
+                ((unsigned long)&(struct soc_bytes)           \
+                {.base = xbase, .num_regs = xregs,            \
+                 .mask = xmask }) }
+
 /*
  * Simplified versions of above macros, declaring a struct and calculating
  * ARRAY_SIZE internally
@@ -904,6 +912,7 @@ struct soc_mixer_control {
 struct soc_bytes {
         int base;
         int num_regs;
+        u32 mask;
 };
 
 /* enumerated kcontrol */
diff --git a/sound/soc/soc-core.c b/sound/soc/soc-core.c
index a9786ab70504..fc0fd3485e7d 100644
--- a/sound/soc/soc-core.c
+++ b/sound/soc/soc-core.c
@@ -2763,6 +2763,25 @@ int snd_soc_bytes_get(struct snd_kcontrol *kcontrol,
         else
                 ret = -EINVAL;
 
+        /* Hide any masked bytes to ensure consistent data reporting */
+        if (ret == 0 && params->mask) {
+                switch (codec->val_bytes) {
+                case 1:
+                        ucontrol->value.bytes.data[0] &= ~params->mask;
+                        break;
+                case 2:
+                        ((u16 *)(&ucontrol->value.bytes.data))[0]
+                                &= ~params->mask;
+                        break;
+                case 4:
+                        ((u32 *)(&ucontrol->value.bytes.data))[0]
+                                &= ~params->mask;
+                        break;
+                default:
+                        return -EINVAL;
+                }
+        }
+
         return ret;
 }
 EXPORT_SYMBOL_GPL(snd_soc_bytes_get);
@@ -2772,14 +2791,55 @@ int snd_soc_bytes_put(struct snd_kcontrol *kcontrol,
 {
         struct soc_bytes *params = (void *)kcontrol->private_value;
         struct snd_soc_codec *codec = snd_kcontrol_chip(kcontrol);
-        int ret;
+        int ret, len;
+        unsigned int val;
+        void *data;
 
-        if (codec->using_regmap)
-                ret = regmap_raw_write(codec->control_data, params->base,
-                                       ucontrol->value.bytes.data,
-                                       params->num_regs * codec->val_bytes);
-        else
-                ret = -EINVAL;
+        if (!codec->using_regmap)
+                return -EINVAL;
+
+        data = ucontrol->value.bytes.data;
+        len = params->num_regs * codec->val_bytes;
+
+        /*
+         * If we've got a mask then we need to preserve the register
+         * bits.  We shouldn't modify the incoming data so take a
+         * copy.
+         */
+        if (params->mask) {
+                ret = regmap_read(codec->control_data, params->base, &val);
+                if (ret != 0)
+                        return ret;
+
+                val &= params->mask;
+
+                data = kmemdup(data, len, GFP_KERNEL);
+                if (!data)
+                        return -ENOMEM;
+
+                switch (codec->val_bytes) {
+                case 1:
+                        ((u8 *)data)[0] &= ~params->mask;
+                        ((u8 *)data)[0] |= val;
+                        break;
+                case 2:
+                        ((u16 *)data)[0] &= cpu_to_be16(~params->mask);
+                        ((u16 *)data)[0] |= cpu_to_be16(val);
+                        break;
+                case 4:
+                        ((u32 *)data)[0] &= cpu_to_be32(~params->mask);
+                        ((u32 *)data)[0] |= cpu_to_be32(val);
+                        break;
+                default:
+                        return -EINVAL;
+                }
+        }
+
+        ret = regmap_raw_write(codec->control_data, params->base,
+                               data, len);
+
+        if (params->mask)
+                kfree(data);
 
         return ret;
 }