From c9ba62745ba17d21542db28bf7dc28a39e19f7c2 Mon Sep 17 00:00:00 2001
From: popcornmix <popcornmix@gmail.com>
Date: Tue, 10 Mar 2020 22:21:15 +0000
Subject: [PATCH] vc4_hdmi: Set HDMI_MAI_FMT

The hardware uses this for generating the right audio
data island packets when using formats other than PCM

Signed-off-by: Dom Cobley <popcornmix@gmail.com>
---
 drivers/gpu/drm/vc4/vc4_hdmi.c | 47 ++++++++++++++++++++++++++++++++++
 drivers/gpu/drm/vc4/vc4_regs.h | 31 ++++++++++++++++++++++
 2 files changed, 78 insertions(+)

--- a/drivers/gpu/drm/vc4/vc4_hdmi.c
+++ b/drivers/gpu/drm/vc4/vc4_hdmi.c
@@ -917,6 +917,45 @@ static void vc4_hdmi_audio_shutdown(stru
 	vc4_hdmi->audio.substream = NULL;
 }
 
+static int sample_rate_to_mai_fmt(int samplerate)
+{
+   switch(samplerate)
+   {
+      case 8000:
+         return VC4_HDMI_MAI_SAMPLE_RATE_8000;
+      case 11025:
+         return VC4_HDMI_MAI_SAMPLE_RATE_11025;
+      case 12000:
+         return VC4_HDMI_MAI_SAMPLE_RATE_12000;
+      case 16000:
+         return VC4_HDMI_MAI_SAMPLE_RATE_16000;
+      case 22050:
+         return VC4_HDMI_MAI_SAMPLE_RATE_22050;
+      case 24000:
+         return VC4_HDMI_MAI_SAMPLE_RATE_24000;
+      case 32000:
+         return VC4_HDMI_MAI_SAMPLE_RATE_32000;
+      case 44100:
+         return VC4_HDMI_MAI_SAMPLE_RATE_44100;
+      case 48000:
+         return VC4_HDMI_MAI_SAMPLE_RATE_48000;
+      case 64000:
+         return VC4_HDMI_MAI_SAMPLE_RATE_64000;
+      case 88200:
+         return VC4_HDMI_MAI_SAMPLE_RATE_88200;
+      case 96000:
+         return VC4_HDMI_MAI_SAMPLE_RATE_96000;
+      case 128000:
+         return VC4_HDMI_MAI_SAMPLE_RATE_128000;
+      case 176400:
+         return VC4_HDMI_MAI_SAMPLE_RATE_176400;
+      case 192000:
+         return VC4_HDMI_MAI_SAMPLE_RATE_192000;
+      default:
+         return VC4_HDMI_MAI_SAMPLE_RATE_NOT_INDICATED;
+   }
+}
+
 /* HDMI audio codec callbacks */
 static int vc4_hdmi_audio_hw_params(struct snd_pcm_substream *substream,
 				    struct snd_pcm_hw_params *params,
@@ -926,6 +965,8 @@ static int vc4_hdmi_audio_hw_params(stru
 	struct device *dev = &vc4_hdmi->pdev->dev;
 	u32 audio_packet_config, channel_mask;
 	u32 channel_map;
+	u32 mai_audio_format;
+	u32 mai_sample_rate;
 
 	if (substream != vc4_hdmi->audio.substream)
 		return -EINVAL;
@@ -946,6 +987,12 @@ static int vc4_hdmi_audio_hw_params(stru
 
 	vc4_hdmi_audio_set_mai_clock(vc4_hdmi);
 
+	mai_sample_rate = sample_rate_to_mai_fmt(vc4_hdmi->audio.samplerate);
+	mai_audio_format = VC4_HDMI_MAI_FORMAT_PCM;
+	HDMI_WRITE(HDMI_MAI_FMT,
+		VC4_SET_FIELD(mai_sample_rate, VC4_HDMI_MAI_FORMAT_SAMPLE_RATE) |
+		VC4_SET_FIELD(mai_audio_format, VC4_HDMI_MAI_FORMAT_AUDIO_FORMAT));
+
 	/* The B frame identifier should match the value used by alsa-lib (8) */
 	audio_packet_config =
 		VC4_HDMI_AUDIO_PACKET_ZERO_DATA_ON_SAMPLE_FLAT |
--- a/drivers/gpu/drm/vc4/vc4_regs.h
+++ b/drivers/gpu/drm/vc4/vc4_regs.h
@@ -516,6 +516,37 @@
 # define VC4_HDMI_AUDIO_PACKET_CEA_MASK_MASK			VC4_MASK(7, 0)
 # define VC4_HDMI_AUDIO_PACKET_CEA_MASK_SHIFT			0
 
+
+# define VC4_HDMI_MAI_FORMAT_AUDIO_FORMAT_MASK		VC4_MASK(23, 16)
+# define VC4_HDMI_MAI_FORMAT_AUDIO_FORMAT_SHIFT		16
+
+enum {
+	VC4_HDMI_MAI_FORMAT_PCM = 2,
+	VC4_HDMI_MAI_FORMAT_HBR = 200,
+};
+
+# define VC4_HDMI_MAI_FORMAT_SAMPLE_RATE_MASK		VC4_MASK(15, 8)
+# define VC4_HDMI_MAI_FORMAT_SAMPLE_RATE_SHIFT		8
+
+enum {
+	VC4_HDMI_MAI_SAMPLE_RATE_NOT_INDICATED = 0,
+	VC4_HDMI_MAI_SAMPLE_RATE_8000 = 1,
+	VC4_HDMI_MAI_SAMPLE_RATE_11025 = 2,
+	VC4_HDMI_MAI_SAMPLE_RATE_12000 = 3,
+	VC4_HDMI_MAI_SAMPLE_RATE_16000 = 4,
+	VC4_HDMI_MAI_SAMPLE_RATE_22050 = 5,
+	VC4_HDMI_MAI_SAMPLE_RATE_24000 = 6,
+	VC4_HDMI_MAI_SAMPLE_RATE_32000 = 7,
+	VC4_HDMI_MAI_SAMPLE_RATE_44100 = 8,
+	VC4_HDMI_MAI_SAMPLE_RATE_48000 = 9,
+	VC4_HDMI_MAI_SAMPLE_RATE_64000 = 10,
+	VC4_HDMI_MAI_SAMPLE_RATE_88200 = 11,
+	VC4_HDMI_MAI_SAMPLE_RATE_96000 = 12,
+	VC4_HDMI_MAI_SAMPLE_RATE_128000 = 13,
+	VC4_HDMI_MAI_SAMPLE_RATE_176400 = 14,
+	VC4_HDMI_MAI_SAMPLE_RATE_192000 = 15,
+};
+
 # define VC4_HDMI_RAM_PACKET_ENABLE		BIT(16)
 
 /* When set, the CTS_PERIOD counts based on MAI bus sync pulse instead