From cc3419daf60159394cd310c2405a78775b3f84db Mon Sep 17 00:00:00 2001
From: Sanchayan Maity <sanchayan@asymptotic.io>
Date: Thu, 24 Feb 2022 20:28:23 +0530
Subject: [PATCH] rtp: ldac: Set frame count information in payload

The RTP payload seems to be required as it carries the frame count
information. Also, gst_rtp_base_payload_allocate_output_buffer had
the second argument incorrect.

Strangely some devices like Shanling MP4 and Sony XM3 would still
work without this while some like the Sony XM4 do not.

Part-of: <https://gitlab.freedesktop.org/gstreamer/gstreamer/-/merge_requests/1797>
---
 .../docs/gst_plugins_cache.json               |  2 +-
 .../gst-plugins-good/gst/rtp/gstrtpldacpay.c  | 63 ++++++++++++++++++-
 .../gst-plugins-good/gst/rtp/gstrtpldacpay.h  |  1 +
 3 files changed, 62 insertions(+), 4 deletions(-)

diff --git a/docs/gst_plugins_cache.json b/docs/gst_plugins_cache.json
index 88bff47243..003546d59d 100644
--- a/docs/gst_plugins_cache.json
+++ b/docs/gst_plugins_cache.json
@@ -14678,7 +14678,7 @@
                 "long-name": "RTP packet payloader",
                 "pad-templates": {
                     "sink": {
-                        "caps": "audio/x-ldac:\n       channels: [ 1, 2 ]\n           rate: { (int)44100, (int)48000, (int)88200, (int)96000 }\n",
+                        "caps": "audio/x-ldac:\n       channels: [ 1, 2 ]\n          eqmid: { (int)0, (int)1, (int)2 }\n           rate: { (int)44100, (int)48000, (int)88200, (int)96000 }\n",
                         "direction": "sink",
                         "presence": "always"
                     },
diff --git a/gst/rtp/gstrtpldacpay.c b/gst/rtp/gstrtpldacpay.c
index 2b14b746fe..aa30673e7e 100644
--- a/gst/rtp/gstrtpldacpay.c
+++ b/gst/rtp/gstrtpldacpay.c
@@ -48,7 +48,7 @@
 #include "gstrtpldacpay.h"
 #include "gstrtputils.h"
 
-#define GST_RTP_HEADER_LENGTH    12
+#define GST_RTP_LDAC_PAYLOAD_HEADER_SIZE 1
 /* MTU size required for LDAC A2DP streaming */
 #define GST_LDAC_MTU_REQUIRED    679
 
@@ -64,6 +64,7 @@ static GstStaticPadTemplate gst_rtp_ldac_pay_sink_factory =
 GST_STATIC_PAD_TEMPLATE ("sink", GST_PAD_SINK, GST_PAD_ALWAYS,
     GST_STATIC_CAPS ("audio/x-ldac, "
         "channels = (int) [ 1, 2 ], "
+        "eqmid = (int) { 0, 1, 2 }, "
         "rate = (int) { 44100, 48000, 88200, 96000 }")
     );
 
@@ -81,6 +82,38 @@ static gboolean gst_rtp_ldac_pay_set_caps (GstRTPBasePayload * payload,
 static GstFlowReturn gst_rtp_ldac_pay_handle_buffer (GstRTPBasePayload *
     payload, GstBuffer * buffer);
 
+/**
+ * gst_rtp_ldac_pay_get_num_frames
+ * @eqmid: Encode Quality Mode Index
+ * @channels: Number of channels
+ *
+ * Returns: Number of LDAC frames per packet.
+ */
+static guint8
+gst_rtp_ldac_pay_get_num_frames (gint eqmid, gint channels)
+{
+  g_assert (channels == 1 || channels == 2);
+
+  switch (eqmid) {
+      /* Encode setting for High Quality */
+    case 0:
+      return 4 / channels;
+      /* Encode setting for Standard Quality */
+    case 1:
+      return 6 / channels;
+      /* Encode setting for Mobile use Quality */
+    case 2:
+      return 12 / channels;
+    default:
+      break;
+  }
+
+  g_assert_not_reached ();
+
+  /* If assertion gets compiled out */
+  return 6 / channels;
+}
+
 static void
 gst_rtp_ldac_pay_class_init (GstRtpLdacPayClass * klass)
 {
@@ -115,7 +148,7 @@ gst_rtp_ldac_pay_set_caps (GstRTPBasePayload * payload, GstCaps * caps)
 {
   GstRtpLdacPay *ldacpay = GST_RTP_LDAC_PAY (payload);
   GstStructure *structure;
-  gint rate;
+  gint channels, eqmid, rate;
 
   if (GST_RTP_BASE_PAYLOAD_MTU (ldacpay) < GST_LDAC_MTU_REQUIRED) {
     GST_ERROR_OBJECT (ldacpay, "Invalid MTU %d, should be >= %d",
@@ -129,6 +162,18 @@ gst_rtp_ldac_pay_set_caps (GstRTPBasePayload * payload, GstCaps * caps)
     return FALSE;
   }
 
+  if (!gst_structure_get_int (structure, "channels", &channels)) {
+    GST_ERROR_OBJECT (ldacpay, "Failed to get audio rate from caps");
+    return FALSE;
+  }
+
+  if (!gst_structure_get_int (structure, "eqmid", &eqmid)) {
+    GST_ERROR_OBJECT (ldacpay, "Failed to get eqmid from caps");
+    return FALSE;
+  }
+
+  ldacpay->frame_count = gst_rtp_ldac_pay_get_num_frames (eqmid, channels);
+
   gst_rtp_base_payload_set_options (payload, "audio", TRUE, "X-GST-LDAC", rate);
 
   return gst_rtp_base_payload_set_outcaps (payload, NULL);
@@ -145,14 +190,26 @@ gst_rtp_ldac_pay_set_caps (GstRTPBasePayload * payload, GstCaps * caps)
 static GstFlowReturn
 gst_rtp_ldac_pay_handle_buffer (GstRTPBasePayload * payload, GstBuffer * buffer)
 {
+  GstRTPBuffer rtp = GST_RTP_BUFFER_INIT;
   GstRtpLdacPay *ldacpay = GST_RTP_LDAC_PAY (payload);
   GstBuffer *outbuf;
   GstClockTime outbuf_frame_duration, outbuf_pts;
+  guint8 *payload_data;
   gsize buf_sz;
 
   outbuf =
       gst_rtp_base_payload_allocate_output_buffer (GST_RTP_BASE_PAYLOAD
-      (ldacpay), GST_RTP_HEADER_LENGTH, 0, 0);
+      (ldacpay), GST_RTP_LDAC_PAYLOAD_HEADER_SIZE, 0, 0);
+
+  /* Get payload */
+  gst_rtp_buffer_map (outbuf, GST_MAP_WRITE, &rtp);
+
+  /* Write header and copy data into payload */
+  payload_data = gst_rtp_buffer_get_payload (&rtp);
+  /* Upper 3 fragment bits not used, ref A2DP v13, 4.3.4 */
+  payload_data[0] = ldacpay->frame_count & 0x0f;
+
+  gst_rtp_buffer_unmap (&rtp);
 
   outbuf_pts = GST_BUFFER_PTS (buffer);
   outbuf_frame_duration = GST_BUFFER_DURATION (buffer);
diff --git a/gst/rtp/gstrtpldacpay.h b/gst/rtp/gstrtpldacpay.h
index 0865ce7ade..0134491752 100644
--- a/gst/rtp/gstrtpldacpay.h
+++ b/gst/rtp/gstrtpldacpay.h
@@ -42,6 +42,7 @@ typedef struct _GstRtpLdacPayClass GstRtpLdacPayClass;
 
 struct _GstRtpLdacPay {
   GstRTPBasePayload base;
+  guint8 frame_count;
 };
 
 struct _GstRtpLdacPayClass {
-- 
GitLab