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; }