2 files changed, 75 insertions, 6 deletions

diff --git a/include/linux/mfd/arizona/core.h b/include/linux/mfd/arizona/core.h
index 5cf8b91ce996..11783b511b9a 100644
--- a/include/linux/mfd/arizona/core.h
+++ b/include/linux/mfd/arizona/core.h
@@ -110,6 +110,9 @@ struct arizona {
         int clk32k_ref;
 
         struct snd_soc_dapm_context *dapm;
+
+        int tdm_width[ARIZONA_MAX_AIF];
+        int tdm_slots[ARIZONA_MAX_AIF];
 };
 
 int arizona_clk32k_enable(struct arizona *arizona);
diff --git a/sound/soc/codecs/arizona.c b/sound/soc/codecs/arizona.c
index 29e198f57d4c..e77f61c387f7 100644
--- a/sound/soc/codecs/arizona.c
+++ b/sound/soc/codecs/arizona.c
@@ -1185,7 +1185,10 @@ static int arizona_hw_params(struct snd_pcm_substream *substream,
         int base = dai->driver->base;
         const int *rates;
         int i, ret, val;
+        int channels = params_channels(params);
         int chan_limit = arizona->pdata.max_channels_clocked[dai->id - 1];
+        int tdm_width = arizona->tdm_width[dai->id - 1];
+        int tdm_slots = arizona->tdm_slots[dai->id - 1];
         int bclk, lrclk, wl, frame, bclk_target;
 
         if (params_rate(params) % 8000)
@@ -1193,18 +1196,27 @@ static int arizona_hw_params(struct snd_pcm_substream *substream,
         else
                 rates = &arizona_48k_bclk_rates[0];
 
-        bclk_target = snd_soc_params_to_bclk(params);
-        if (chan_limit && chan_limit < params_channels(params)) {
+        if (tdm_slots) {
+                arizona_aif_dbg(dai, "Configuring for %d %d bit TDM slots\n",
+                                tdm_slots, tdm_width);
+                bclk_target = tdm_slots * tdm_width * params_rate(params);
+                channels = tdm_slots;
+        } else {
+                bclk_target = snd_soc_params_to_bclk(params);
+        }
+
+        if (chan_limit && chan_limit < channels) {
                 arizona_aif_dbg(dai, "Limiting to %d channels\n", chan_limit);
-                bclk_target /= params_channels(params);
+                bclk_target /= channels;
                 bclk_target *= chan_limit;
         }
 
-        /* Force stereo for I2S mode */
+        /* Force multiple of 2 channels for I2S mode */
         val = snd_soc_read(codec, base + ARIZONA_AIF_FORMAT);
-        if (params_channels(params) == 1 && (val & ARIZONA_AIF1_FMT_MASK)) {
+        if ((channels & 1) && (val & ARIZONA_AIF1_FMT_MASK)) {
                 arizona_aif_dbg(dai, "Forcing stereo mode\n");
-                bclk_target *= 2;
+                bclk_target /= channels;
+                bclk_target *= channels + 1;
         }
 
         for (i = 0; i < ARRAY_SIZE(arizona_44k1_bclk_rates); i++) {
@@ -1324,9 +1336,63 @@ static int arizona_set_tristate(struct snd_soc_dai *dai, int tristate)
                                    ARIZONA_AIF1_TRI, reg);
 }
 
+static void arizona_set_channels_to_mask(struct snd_soc_dai *dai,
+                                         unsigned int base,
+                                         int channels, unsigned int mask)
+{
+        struct snd_soc_codec *codec = dai->codec;
+        struct arizona_priv *priv = snd_soc_codec_get_drvdata(codec);
+        struct arizona *arizona = priv->arizona;
+        int slot, i;
+
+        for (i = 0; i < channels; ++i) {
+                slot = ffs(mask) - 1;
+                if (slot < 0)
+                        return;
+
+                regmap_write(arizona->regmap, base + i, slot);
+
+                mask &= ~(1 << slot);
+        }
+
+        if (mask)
+                arizona_aif_warn(dai, "Too many channels in TDM mask\n");
+}
+
+static int arizona_set_tdm_slot(struct snd_soc_dai *dai, unsigned int tx_mask,
+                                unsigned int rx_mask, int slots, int slot_width)
+{
+        struct snd_soc_codec *codec = dai->codec;
+        struct arizona_priv *priv = snd_soc_codec_get_drvdata(codec);
+        struct arizona *arizona = priv->arizona;
+        int base = dai->driver->base;
+        int rx_max_chan = dai->driver->playback.channels_max;
+        int tx_max_chan = dai->driver->capture.channels_max;
+
+        /* Only support TDM for the physical AIFs */
+        if (dai->id > ARIZONA_MAX_AIF)
+                return -ENOTSUPP;
+
+        if (slots == 0) {
+                tx_mask = (1 << tx_max_chan) - 1;
+                rx_mask = (1 << rx_max_chan) - 1;
+        }
+
+        arizona_set_channels_to_mask(dai, base + ARIZONA_AIF_FRAME_CTRL_3,
+                                     tx_max_chan, tx_mask);
+        arizona_set_channels_to_mask(dai, base + ARIZONA_AIF_FRAME_CTRL_11,
+                                     rx_max_chan, rx_mask);
+
+        arizona->tdm_width[dai->id - 1] = slot_width;
+        arizona->tdm_slots[dai->id - 1] = slots;
+
+        return 0;
+}
+
 const struct snd_soc_dai_ops arizona_dai_ops = {
         .startup = arizona_startup,
         .set_fmt = arizona_set_fmt,
+        .set_tdm_slot = arizona_set_tdm_slot,
         .hw_params = arizona_hw_params,
         .set_sysclk = arizona_dai_set_sysclk,
         .set_tristate = arizona_set_tristate,