UAC2: Implement feedback by fifo counting.

2 files changed, 114 insertions, 66 deletions

diff --git a/src/class/audio/audio_device.c b/src/class/audio/audio_device.c
index 9ba38a20c..4ca95d4e7 100644
--- a/src/class/audio/audio_device.c
+++ b/src/class/audio/audio_device.c
@@ -2,6 +2,7 @@
  * The MIT License (MIT)
  *
  * Copyright (c) 2020 Reinhard Panhuber, Jerzy Kasenberg
+ * Copyright (c) 2023 HiFiPhile
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -319,7 +320,8 @@ typedef struct
 
 #if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
   struct {
-    CFG_TUSB_MEM_ALIGN uint32_t value;  // Feedback value for asynchronous mode (in 16.16 format).
+    CFG_TUSB_MEM_ALIGN uint32_t send_buf;
+    uint32_t value;       // Feedback value for asynchronous mode (in 16.16 format).
     uint32_t min_value;   // min value according to UAC2 FMT-2.0 section 2.3.1.1.
     uint32_t max_value;   // max value according to UAC2 FMT-2.0 section 2.3.1.1.
 
@@ -335,12 +337,12 @@ typedef struct
         uint32_t mclk_freq;
       }fixed;
 
-#if 0 // implement later
       struct {
-        uint32_t nominal_value;
-        uint32_t threshold_bytes;
+        uint32_t nom_value;     // In 16.16 format
+        uint32_t fifo_lvl_avg;  // In 16.16 format
+        uint16_t fifo_lvl_thr;  // fifo level threshold
+        uint16_t rate_const[2]; // pre-computed feedback/fifo_depth rate
       }fifo_count;
-#endif
     }compute;
 
   } feedback;
@@ -473,7 +475,8 @@ static uint16_t audiod_tx_packet_size(const uint16_t* norminal_size, uint16_t da
 #endif
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-static bool set_fb_params_freq(audiod_function_t* audio, uint32_t sample_freq, uint32_t mclk_freq);
+static bool audiod_set_fb_params_freq(audiod_function_t* audio, uint32_t sample_freq, uint32_t mclk_freq);
+static void audiod_fb_fifo_count_update(audiod_function_t* audio, uint16_t lvl_new);
 #endif
 
 bool tud_audio_n_mounted(uint8_t func_id)
@@ -566,7 +569,7 @@ static bool audiod_rx_done_cb(uint8_t rhport, audiod_function_t* audio, uint16_t
     TU_VERIFY(tud_audio_rx_done_pre_read_cb(rhport, n_bytes_received, idx_audio_fct, audio->ep_out, audio->alt_setting[idxItf]));
   }
 
-#if CFG_TUD_AUDIO_ENABLE_DECODING && CFG_TUD_AUDIO_ENABLE_EP_OUT
+#if CFG_TUD_AUDIO_ENABLE_DECODING
 
   switch (audio->format_type_rx)
   {
@@ -615,6 +618,13 @@ static bool audiod_rx_done_cb(uint8_t rhport, audiod_function_t* audio, uint16_t
   TU_VERIFY(usbd_edpt_xfer_fifo(rhport, audio->ep_out, &audio->ep_out_ff, audio->ep_out_sz), false);
 #endif
 
+#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+  if(audio->feedback.compute_method == AUDIO_FEEDBACK_METHOD_FIFO_COUNT)
+  {
+    audiod_fb_fifo_count_update(audio, tu_fifo_count(&audio->ep_out_ff));
+  }
+#endif
+
 #endif
 
   // Call a weak callback here - a possibility for user to get informed decoding was completed
@@ -725,6 +735,13 @@ static bool audiod_decode_type_I_pcm(uint8_t rhport, audiod_function_t* audio, u
   // Number of bytes should be a multiple of CFG_TUD_AUDIO_N_BYTES_PER_SAMPLE_RX * CFG_TUD_AUDIO_N_CHANNELS_RX but checking makes no sense - no way to correct it
   // TU_VERIFY(cnt != n_bytes);
 
+#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
+  if(audio->feedback.compute_method == AUDIO_FEEDBACK_METHOD_FIFO_COUNT)
+  {
+    audiod_fb_fifo_count_update(audio, tu_fifo_count(&audio->rx_supp_ff[0]));
+  }
+#endif
+
   return true;
 }
 #endif //CFG_TUD_AUDIO_ENABLE_DECODING
@@ -1067,9 +1084,29 @@ static uint16_t audiod_encode_type_I_pcm(uint8_t rhport, audiod_function_t* audi
 // This function is called once a transmit of a feedback packet was successfully completed. Here, we get the next feedback value to be sent
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-static inline bool audiod_fb_send(uint8_t rhport, audiod_function_t *audio)
+static inline bool audiod_fb_send(audiod_function_t *audio)
 {
-  return usbd_edpt_xfer(rhport, audio->ep_fb, (uint8_t *) &audio->feedback.value, 4);
+  // Format the feedback value
+#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION
+  if ( TUSB_SPEED_FULL == tud_speed_get() )
+  {
+    uint8_t * fb = (uint8_t *) &audio->feedback.send_buf;
+
+    // For FS format is 10.14
+    *(fb++) = (audio->feedback.value >> 2) & 0xFF;
+    *(fb++) = (audio->feedback.value >> 10) & 0xFF;
+    *(fb++) = (audio->feedback.value >> 18) & 0xFF;
+    // 4th byte is needed to work correctly with MS Windows
+    *fb = 0;
+  } else
+  {
+    value = audio->feedback.value;
+  }
+#else
+  audio->feedback.send_buf = audio->feedback.value;
+#endif
+
+  return usbd_edpt_xfer(audio->rhport, audio->ep_fb, (uint8_t *) &audio->feedback.send_buf, 4);
 }
 #endif
 
@@ -1861,7 +1898,7 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
 
         // Minimal/Maximum value in 16.16 format for full speed (1ms per frame) or high speed (125 us per frame)
         uint32_t const frame_div  = (TUSB_SPEED_FULL == tud_speed_get()) ? 1000 : 8000;
-        audio->feedback.min_value = (fb_param.sample_freq/frame_div - 1) << 16;
+        audio->feedback.min_value = ((fb_param.sample_freq - 1)/frame_div) << 16;
         audio->feedback.max_value = (fb_param.sample_freq/frame_div + 1) << 16;
 
         switch(fb_param.method)
@@ -1869,20 +1906,27 @@ static bool audiod_set_interface(uint8_t rhport, tusb_control_request_t const *
           case AUDIO_FEEDBACK_METHOD_FREQUENCY_FIXED:
           case AUDIO_FEEDBACK_METHOD_FREQUENCY_FLOAT:
           case AUDIO_FEEDBACK_METHOD_FREQUENCY_POWER_OF_2:
-            set_fb_params_freq(audio, fb_param.sample_freq, fb_param.frequency.mclk_freq);
+            audiod_set_fb_params_freq(audio, fb_param.sample_freq, fb_param.frequency.mclk_freq);
           break;
 
-          #if 0 // implement later
           case AUDIO_FEEDBACK_METHOD_FIFO_COUNT:
           {
-            uint64_t fb64 = ((uint64_t) fb_param.sample_freq) << 16;
-            audio->feedback.compute.fifo_count.nominal_value = (uint32_t) (fb64 / frame_div);
-            audio->feedback.compute.fifo_count.threshold_bytes = fb_param.fifo_count.threshold_bytes;
-
-            tud_audio_fb_set(audio->feedback.compute.fifo_count.nominal_value);
+            /* Initialize the threshold level to half filled */
+            uint16_t fifo_lvl_thr;
+#if CFG_TUD_AUDIO_ENABLE_DECODING
+            fifo_lvl_thr = tu_fifo_depth(&audio->rx_supp_ff[0]) / 2;
+#else
+            fifo_lvl_thr = tu_fifo_depth(&audio->ep_out_ff) / 2;
+#endif
+            audio->feedback.compute.fifo_count.fifo_lvl_thr = fifo_lvl_thr;
+            audio->feedback.compute.fifo_count.fifo_lvl_avg = ((uint32_t)fifo_lvl_thr) << 16;
+            /* Avoid 64bit division */
+            uint32_t nominal = ((fb_param.sample_freq / 100) << 16) / (frame_div / 100);
+            audio->feedback.compute.fifo_count.nom_value = nominal;
+            audio->feedback.compute.fifo_count.rate_const[0] = (audio->feedback.max_value - nominal) / fifo_lvl_thr;
+            audio->feedback.compute.fifo_count.rate_const[1] = (nominal - audio->feedback.min_value) / fifo_lvl_thr;
           }
           break;
-          #endif
 
           // nothing to do
           default: break;
@@ -2198,7 +2242,7 @@ bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint3
       if (!usbd_edpt_busy(rhport, audio->ep_fb))
       {
         // Schedule next transmission - value is changed bytud_audio_n_fb_set() in the meantime or the old value gets sent
-        return audiod_fb_send(rhport, audio);
+        return audiod_fb_send(audio);
       }
     }
 #endif
@@ -2210,7 +2254,7 @@ bool audiod_xfer_cb(uint8_t rhport, uint8_t ep_addr, xfer_result_t result, uint3
 
 #if CFG_TUD_AUDIO_ENABLE_EP_OUT && CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
 
-static bool set_fb_params_freq(audiod_function_t* audio, uint32_t sample_freq, uint32_t mclk_freq)
+static bool audiod_set_fb_params_freq(audiod_function_t* audio, uint32_t sample_freq, uint32_t mclk_freq)
 {
   // Check if frame interval is within sane limits
   // The interval value n_frames was taken from the descriptors within audiod_set_interface()
@@ -2244,6 +2288,38 @@ static bool set_fb_params_freq(audiod_function_t* audio, uint32_t sample_freq, u
   return true;
 }
 
+static void audiod_fb_fifo_count_update(audiod_function_t* audio, uint16_t lvl_new)
+{
+  /* Low-pass (averaging) filter */
+  uint32_t lvl = audio->feedback.compute.fifo_count.fifo_lvl_avg;
+  lvl = (uint32_t)(((uint64_t)lvl * 63  + ((uint32_t)lvl_new << 16)) >> 6);
+  audio->feedback.compute.fifo_count.fifo_lvl_avg = lvl;
+
+  uint32_t const ff_lvl = lvl >> 16;
+  uint16_t const ff_thr = audio->feedback.compute.fifo_count.fifo_lvl_thr;
+  uint16_t const *rate   = audio->feedback.compute.fifo_count.rate_const;
+
+  uint32_t feedback;
+
+  if(ff_lvl < ff_thr)
+  {
+    feedback = audio->feedback.compute.fifo_count.nom_value + (ff_thr - ff_lvl) * rate[0];
+  } else
+  {
+    feedback = audio->feedback.compute.fifo_count.nom_value - (ff_lvl - ff_thr) * rate[1];
+  }
+
+  if ( feedback > audio->feedback.max_value ) feedback = audio->feedback.max_value;
+  if ( feedback < audio->feedback.min_value ) feedback = audio->feedback.min_value;
+  audio->feedback.value = feedback;
+
+  // Schedule a transmit with the new value if EP is not busy - this triggers repetitive scheduling of the feedback value
+  if (!usbd_edpt_busy(audio->rhport, audio->ep_fb))
+  {
+    audiod_fb_send(audio);
+  }
+}
+
 uint32_t tud_audio_feedback_update(uint8_t func_id, uint32_t cycles)
 {
   audiod_function_t* audio = &_audiod_fct[func_id];
@@ -2280,6 +2356,21 @@ uint32_t tud_audio_feedback_update(uint8_t func_id, uint32_t cycles)
 
   return feedback;
 }
+
+bool tud_audio_n_fb_set(uint8_t func_id, uint32_t feedback)
+{
+  TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
+
+  _audiod_fct[func_id].feedback.value = feedback;
+
+  // Schedule a transmit with the new value if EP is not busy - this triggers repetitive scheduling of the feedback value
+  if (!usbd_edpt_busy(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_fb))
+  {
+    return audiod_fb_send(&_audiod_fct[func_id]);
+  }
+
+  return true;
+}
 #endif
 
 TU_ATTR_FAST_FUNC void audiod_sof_isr (uint8_t rhport, uint32_t frame_count)
@@ -2691,44 +2782,8 @@ static uint16_t audiod_tx_packet_size(const uint16_t* norminal_size, uint16_t da
 
 #endif
 
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_EP
-
-bool tud_audio_n_fb_set(uint8_t func_id, uint32_t feedback)
-{
-  TU_VERIFY(func_id < CFG_TUD_AUDIO && _audiod_fct[func_id].p_desc != NULL);
-
-  // Format the feedback value
-#if CFG_TUD_AUDIO_ENABLE_FEEDBACK_FORMAT_CORRECTION
-  if ( TUSB_SPEED_FULL == tud_speed_get() )
-  {
-    uint8_t * fb = (uint8_t *) &_audiod_fct[func_id].feedback.value;
-
-    // For FS format is 10.14
-    *(fb++) = (feedback >> 2) & 0xFF;
-    *(fb++) = (feedback >> 10) & 0xFF;
-    *(fb++) = (feedback >> 18) & 0xFF;
-    // 4th byte is needed to work correctly with MS Windows
-    *fb = 0;
-  }else
-#else
-  {
-    // Send value as-is, caller will choose the appropriate format
-    _audiod_fct[func_id].feedback.value = feedback;
-  }
-#endif
-
-  // Schedule a transmit with the new value if EP is not busy - this triggers repetitive scheduling of the feedback value
-  if (!usbd_edpt_busy(_audiod_fct[func_id].rhport, _audiod_fct[func_id].ep_fb))
-  {
-    return audiod_fb_send(_audiod_fct[func_id].rhport, &_audiod_fct[func_id]);
-  }
-
-  return true;
-}
-#endif
-
 // No security checks here - internal function only which should always succeed
-uint8_t audiod_get_audio_fct_idx(audiod_function_t * audio)
+static uint8_t audiod_get_audio_fct_idx(audiod_function_t * audio)
 {
   for (uint8_t cnt=0; cnt < CFG_TUD_AUDIO; cnt++)
   {
diff --git a/src/class/audio/audio_device.h b/src/class/audio/audio_device.h
index b16514fd4..9c95ce1a9 100644
--- a/src/class/audio/audio_device.h
+++ b/src/class/audio/audio_device.h
@@ -3,6 +3,7 @@
  *
  * Copyright (c) 2020 Ha Thach (tinyusb.org)
  * Copyright (c) 2020 Reinhard Panhuber
+ * Copyright (c) 2023 HiFiPhile
  *
  * Permission is hereby granted, free of charge, to any person obtaining a copy
  * of this software and associated documentation files (the "Software"), to deal
@@ -487,7 +488,6 @@ TU_ATTR_WEAK void tud_audio_fb_done_cb(uint8_t func_id);
 // feedback = n_cycles / n_frames * f_s / f_m in 16.16 format, where n_cycles are the number of main clock cycles within fb_n_frames
 
 bool tud_audio_n_fb_set(uint8_t func_id, uint32_t feedback);
-static inline bool tud_audio_fb_set(uint32_t feedback);
 
 // Update feedback value with passed cycles since last time this update function is called.
 // Typically called within tud_audio_sof_isr(). Required tud_audio_feedback_params_cb() is implemented
@@ -500,9 +500,7 @@ enum {
   AUDIO_FEEDBACK_METHOD_FREQUENCY_FIXED,
   AUDIO_FEEDBACK_METHOD_FREQUENCY_FLOAT,
   AUDIO_FEEDBACK_METHOD_FREQUENCY_POWER_OF_2,
-
-  // impelemnt later
-  // AUDIO_FEEDBACK_METHOD_FIFO_COUNT
+  AUDIO_FEEDBACK_METHOD_FIFO_COUNT
 };
 
 typedef struct {
@@ -514,11 +512,6 @@ typedef struct {
       uint32_t mclk_freq; // Main clock frequency in Hz i.e. master clock to which sample clock is based on
     }frequency;
 
-#if 0 // implement later
-    struct {
-      uint32_t threshold_bytes; // minimum number of bytes received to be considered as filled/ready
-    }fifo_count;
-#endif
   };
 }audio_feedback_params_t;