AV rate correction for inaccurate GKI timers

Change-Id: I70bf6e759e46a49ac2ad31d024d8ac96bb76571e

6 files changed, 202 insertions, 54 deletions

diff --git a/audio_a2dp_hw/audio_a2dp_hw.c b/audio_a2dp_hw/audio_a2dp_hw.c
index c7c5810..2441f35 100644
--- a/audio_a2dp_hw/audio_a2dp_hw.c
+++ b/audio_a2dp_hw/audio_a2dp_hw.c
@@ -100,7 +100,7 @@ typedef enum {
     AUDIO_A2DP_STATE_STOPPED,
     AUDIO_A2DP_STATE_SUSPENDED, /* need explicit set param call to resume (suspend=false) */
     AUDIO_A2DP_STATE_STANDBY    /* allows write to autoresume */
-} a2dp_state;
+} a2dp_state_t;
 
 struct a2dp_stream_out;
 
@@ -111,7 +111,7 @@ struct a2dp_audio_device {
 
 struct a2dp_config {
     uint32_t                rate;
-    uint32_t                channels;
+    uint32_t                channel_flags;
     int                     format;
 };
 
@@ -122,9 +122,9 @@ struct a2dp_stream_out {
     pthread_mutex_t         lock;
     int                     ctrl_fd;
     int                     audio_fd;
-    a2dp_state              state;
-    struct a2dp_config      cfg;
     size_t                  buffer_sz;
+    a2dp_state_t            state;
+    struct a2dp_config      cfg;
 };
 
 struct a2dp_stream_in {
@@ -330,12 +330,12 @@ static void a2dp_stream_out_init(struct a2dp_stream_out *out)
     out->audio_fd = AUDIO_SKT_DISCONNECTED;
     out->state = AUDIO_A2DP_STATE_STOPPED;
 
-    out->cfg.channels = AUDIO_CHANNEL_OUT_STEREO;
-    out->cfg.format = AUDIO_CHANNEL_DEFAULT_FORMAT;
-    out->cfg.rate = AUDIO_CHANNEL_DEFAULT_RATE;
+    out->cfg.channel_flags = AUDIO_STREAM_DEFAULT_CHANNEL_FLAG;
+    out->cfg.format = AUDIO_STREAM_DEFAULT_FORMAT;
+    out->cfg.rate = AUDIO_STREAM_DEFAULT_RATE;
 
     /* manages max capacity of socket pipe */
-    out->buffer_sz = AUDIO_CHANNEL_OUTPUT_BUFFER_SZ;
+    out->buffer_sz = AUDIO_STREAM_OUTPUT_BUFFER_SZ;
 }
 
 static int start_audio_datapath(struct a2dp_stream_out *out)
@@ -504,9 +504,9 @@ static int out_set_sample_rate(struct audio_stream *stream, uint32_t rate)
 
     DEBUG("out_set_sample_rate : %d", rate);
 
-    if (rate != AUDIO_CHANNEL_DEFAULT_RATE)
+    if (rate != AUDIO_STREAM_DEFAULT_RATE)
     {
-        LOGE("only rate %d supported", AUDIO_CHANNEL_DEFAULT_RATE);
+        LOGE("only rate %d supported", AUDIO_STREAM_DEFAULT_RATE);
         return -1;
     }
 
@@ -528,22 +528,22 @@ static uint32_t out_get_channels(const struct audio_stream *stream)
 {
     struct a2dp_stream_out *out = (struct a2dp_stream_out *)stream;
 
-    DEBUG("channels %d", out->cfg.channels);
+    DEBUG("channels 0x%x", out->cfg.channel_flags);
 
-    return out->cfg.channels;
+    return out->cfg.channel_flags;
 }
 
 static int out_get_format(const struct audio_stream *stream)
 {
     struct a2dp_stream_out *out = (struct a2dp_stream_out *)stream;
-    DEBUG("format %x", out->cfg.format);
+    DEBUG("format 0x%x", out->cfg.format);
     return out->cfg.format;
 }
 
 static int out_set_format(struct audio_stream *stream, int format)
 {
     struct a2dp_stream_out *out = (struct a2dp_stream_out *)stream;
-    DEBUG("setting format not yet supported (%x)", format);
+    DEBUG("setting format not yet supported (0x%x)", format);
     return -ENOSYS;
 }
 
diff --git a/audio_a2dp_hw/audio_a2dp_hw.h b/audio_a2dp_hw/audio_a2dp_hw.h
index 77e113a..48aa0fa 100644
--- a/audio_a2dp_hw/audio_a2dp_hw.h
+++ b/audio_a2dp_hw/audio_a2dp_hw.h
@@ -64,10 +64,11 @@
 #define A2DP_CTRL_PATH "/data/misc/bluedroid/.a2dp_ctrl"
 #define A2DP_DATA_PATH "/data/misc/bluedroid/.a2dp_data"
 
-#define AUDIO_CHANNEL_DEFAULT_RATE          44100
-#define AUDIO_CHANNEL_DEFAULT_FORMAT        AUDIO_FORMAT_PCM_16_BIT
-#define AUDIO_CHANNEL_OUTPUT_BUFFER_SZ      (20*512)
-#define AUDIO_SKT_DISCONNECTED              (-1)
+#define AUDIO_STREAM_DEFAULT_RATE          44100
+#define AUDIO_STREAM_DEFAULT_FORMAT        AUDIO_FORMAT_PCM_16_BIT
+#define AUDIO_STREAM_DEFAULT_CHANNEL_FLAG  AUDIO_CHANNEL_OUT_STEREO
+#define AUDIO_STREAM_OUTPUT_BUFFER_SZ      (20*512)
+#define AUDIO_SKT_DISCONNECTED             (-1)
 
 typedef enum {
     A2DP_CTRL_CMD_NONE,
diff --git a/btif/include/btif_media.h b/btif/include/btif_media.h
index 5985580..64303c6 100644
--- a/btif/include/btif_media.h
+++ b/btif/include/btif_media.h
@@ -271,7 +271,6 @@ void btif_a2dp_stop_media_task(void);
 void btif_a2dp_on_init(void);
 void btif_a2dp_on_idle(void);
 void btif_a2dp_on_open(void);
-void btif_a2dp_on_start_req(void);
 void btif_a2dp_on_started(tBTA_AV_START *p_av);
 void btif_a2dp_on_stop_req(void);
 void btif_a2dp_on_stopped(tBTA_AV_SUSPEND *p_av);
diff --git a/btif/src/btif_av.c b/btif/src/btif_av.c
index 8ab884c..3c5afb4 100644
--- a/btif/src/btif_av.c
+++ b/btif/src/btif_av.c
@@ -466,8 +466,6 @@ static BOOLEAN btif_av_state_opened_handler(btif_sm_event_t event, void *p_data)
             break;
 
         case BTIF_AV_START_STREAM_REQ_EVT:
-            /* prepare media task */
-            btif_a2dp_on_start_req();
             BTA_AvStart();
             break;
 
diff --git a/btif/src/btif_media_task.c b/btif/src/btif_media_task.c
index 22893c9..70c48cc 100644
--- a/btif/src/btif_media_task.c
+++ b/btif/src/btif_media_task.c
@@ -201,6 +201,19 @@ static UINT32 a2dp_media_task_stack[(A2DP_MEDIA_TASK_STACK_SIZE + 3) / 4];
 /* fixme -- tune optimal value. For now set a large buffer capacity */
 #define MAX_OUTPUT_BUFFER_QUEUE_SZ 12
 
+/* trigger rate adjustment if deviation is more than threshold */
+#define BTIF_RA_OFFSET_TRIGGER_US 3000
+
+#define BTIF_MEDIA_VERBOSE_ENABLED
+
+#ifdef BTIF_MEDIA_VERBOSE_ENABLED
+#define VERBOSE(fmt, ...) \
+      LogMsg( TRACE_CTRL_GENERAL | TRACE_LAYER_NONE | TRACE_ORG_APPL | \
+              TRACE_TYPE_ERROR, fmt, ## __VA_ARGS__)
+#else
+#define VERBOSE(fmt, ...)
+#endif
+
 /*****************************************************************************
  **  Data types
  *****************************************************************************/
@@ -218,6 +231,14 @@ typedef union
     tBTIF_AV_MEDIA_FEEDINGS_PCM_STATE pcm;
 } tBTIF_AV_MEDIA_FEEDINGS_STATE;
 
+
+/* pcm rate adjustment */
+typedef struct {
+    struct timespec time_start;        /* offset used to calculate time elapsed */
+    unsigned long long tx_pcmtime_us;  /* track pcm time transmitted since stream start */
+    /* add latency tracker */
+} tBTIF_MEDIA_RA;
+
 typedef struct
 {
 #if (BTA_AV_INCLUDED == TRUE)
@@ -234,6 +255,7 @@ typedef struct
     void* av_sm_hdl;
     UINT8 a2dp_cmd_pending; /* we can have max one command pending */
     BOOLEAN tx_flush; /* discards any outgoing data when true */
+    tBTIF_MEDIA_RA ra; /* tx rate adjustment logic */
 
 #if ((defined(BTIF_MEDIA_OVERFEED_INCLUDED) && (BTIF_MEDIA_OVERFEED_INCLUDED == TRUE)) || \
      (defined(BTIF_MEDIA_UNDERFEED_INCLUDED) && (BTIF_MEDIA_UNDERFEED_INCLUDED == TRUE)))
@@ -376,7 +398,6 @@ const char* dump_media_event(UINT16 event)
     }
 }
 
-
 /*****************************************************************************
  **  A2DP CTRL PATH
  *****************************************************************************/
@@ -738,7 +759,7 @@ void btif_a2dp_on_open(void)
 
 /*****************************************************************************
 **
-** Function        btif_a2dp_on_start_req
+** Function        btif_a2dp_on_started
 **
 ** Description
 **
@@ -746,12 +767,12 @@ void btif_a2dp_on_open(void)
 **
 *******************************************************************************/
 
-void btif_a2dp_on_start_req(void)
+void btif_a2dp_on_started(tBTA_AV_START *p_av)
 {
     tBTIF_AV_MEDIA_FEEDINGS media_feeding;
     tBTIF_STATUS status;
 
-    APPL_TRACE_EVENT0("## ON A2DP START ##");
+    APPL_TRACE_EVENT0("## ON A2DP STARTED ##");
 
     GKI_disable();
 
@@ -776,22 +797,6 @@ void btif_a2dp_on_start_req(void)
     }
 
     GKI_enable();
-}
-
-
-/*****************************************************************************
-**
-** Function        btif_a2dp_on_started
-**
-** Description
-**
-** Returns
-**
-*******************************************************************************/
-
-void btif_a2dp_on_started(tBTA_AV_START *p_av)
-{
-    APPL_TRACE_EVENT0("## ON A2DP STARTED ##");
 
     if (p_av->status == BTA_AV_SUCCESS)
     {
@@ -891,6 +896,138 @@ void btif_a2dp_set_tx_flush(BOOLEAN enable)
     btif_media_cb.tx_flush = enable;
 }
 
+/*****************************************************************************
+**
+** Function        btif_calc_pcmtime
+**
+** Description     Calculates the pcmtime equivalent of a datapacket
+**
+** Returns         microseconds
+**
+*******************************************************************************/
+
+static int btif_calc_pcmtime(UINT32 bytes_processed)
+{
+    int pcm_time_us = 0;
+    tBTIF_AV_MEDIA_FEED_CFG *p_cfg;
+
+    p_cfg = &btif_media_cb.media_feeding.cfg;
+
+    /* calculate corresponding pcm time based on data processed */
+    switch(btif_media_cb.media_feeding.format)
+    {
+        case BTIF_AV_CODEC_PCM:
+            pcm_time_us = (bytes_processed*1000000)/
+                          (p_cfg->pcm.num_channel*p_cfg->pcm.sampling_freq*p_cfg->pcm.bit_per_sample/8);
+            break;
+
+        default :
+            APPL_TRACE_ERROR1("mediafeeding format invalid : %d", btif_media_cb.media_feeding.format);
+            break;
+    }
+
+    return pcm_time_us;
+}
+
+/*****************************************************************************
+**
+** Function        ra_reset
+**
+** Description     Media task tx rate adjustment
+**
+** Returns         void
+**
+*******************************************************************************/
+
+static void ra_reset(void)
+{
+    /* initialize start time of test interval */
+    btif_media_cb.ra.time_start.tv_nsec = 0;
+    btif_media_cb.ra.time_start.tv_sec = 0;
+    btif_media_cb.ra.tx_pcmtime_us= 0;
+}
+
+/*****************************************************************************
+**
+** Function        ra_update
+**
+** Description     Updates RA with amount of UIPC channel data processed
+**
+** Returns         void
+**
+*******************************************************************************/
+
+static void ra_update(UINT32 bytes_processed)
+{
+    /* if this is the first frame we will initialize tx start time */
+    if ( (btif_media_cb.ra.time_start.tv_sec == 0) && (btif_media_cb.ra.time_start.tv_nsec == 0) )
+        clock_gettime(CLOCK_MONOTONIC, &btif_media_cb.ra.time_start);
+
+    btif_media_cb.ra.tx_pcmtime_us += btif_calc_pcmtime(bytes_processed);
+}
+
+/*****************************************************************************
+**
+** Function        ra_adjust
+**
+** Description     Calculates deviation between PCM time processed across
+**                 UIPC audio channel and PCM time transmitted across the
+**                 A2DP stream. Is checked every time slice (media timer
+**                 tick).
+**
+** Returns         Returns a frame count offset used to compensate for
+**                 any drift versus the ideal clockrate
+**
+*******************************************************************************/
+
+static int ra_adjust(void)
+{
+    unsigned long long time_elapsed_us;
+    struct timespec now;
+    int adjust = 0;
+
+    /* don't start adjusting until we have read any pcm data */
+    if (btif_media_cb.ra.tx_pcmtime_us == 0)
+        return 0;
+
+    clock_gettime(CLOCK_MONOTONIC, &now);
+
+    time_elapsed_us = (now.tv_sec - btif_media_cb.ra.time_start.tv_sec) * USEC_PER_SEC + \
+                      (now.tv_nsec - btif_media_cb.ra.time_start.tv_nsec)/1000;
+
+    VERBOSE("tx_pcmtime_us : %d us, elapsed : %d us", btif_media_cb.ra.tx_pcmtime_us,
+                       time_elapsed_us);
+
+    /* compare elapsed time vs read pcm time */
+    if (btif_media_cb.ra.tx_pcmtime_us > time_elapsed_us)
+    {
+        //VERBOSE("TOO FAST : %06d us", btif_media_cb.ra.tx_pcmtime_us - time_elapsed_us);
+
+        if ((btif_media_cb.ra.tx_pcmtime_us - time_elapsed_us) > BTIF_RA_OFFSET_TRIGGER_US)
+        {
+            VERBOSE("RA : -1 FRAME (%06d us)", btif_media_cb.ra.tx_pcmtime_us - time_elapsed_us);
+
+            /* adjust by sending one frame less this time slice */
+            adjust--;
+        }
+    }
+    else if (btif_media_cb.ra.tx_pcmtime_us < time_elapsed_us)
+    {
+        //VERBOSE("TOO SLOW : %06d us", time_elapsed_us - btif_media_cb.ra.tx_pcmtime_us);
+
+        if ((time_elapsed_us - btif_media_cb.ra.tx_pcmtime_us) > BTIF_RA_OFFSET_TRIGGER_US)
+        {
+            VERBOSE("RA : +1 FRAME (%06d us)", time_elapsed_us - btif_media_cb.ra.tx_pcmtime_us);
+
+            /* adjust by sending one frame more this time slice */
+            adjust++;
+        }
+    }
+
+    return adjust;
+}
+
+
 /*******************************************************************************
  **
  ** Function         btif_media_task_aa_handle_timer
@@ -936,7 +1073,7 @@ static void btif_media_task_aa_handle_uipc_rx_rdy(void)
     btif_media_aa_prep_2_send(0xFF);
 
     /* send it */
-    APPL_TRACE_DEBUG0("btif_media_task_aa_handle_uipc_rx_rdy calls bta_av_ci_src_data_ready");
+    VERBOSE("btif_media_task_aa_handle_uipc_rx_rdy calls bta_av_ci_src_data_ready");
     bta_av_ci_src_data_ready(BTA_AV_CHNL_AUDIO);
 }
 #endif
@@ -960,6 +1097,8 @@ void btif_media_task_init(void)
 #if (BTA_AV_INCLUDED == TRUE)
     UIPC_Open(UIPC_CH_ID_AV_CTRL , btif_a2dp_ctrl_cb);
 #endif
+
+
 }
 /*******************************************************************************
  **
@@ -980,7 +1119,7 @@ int btif_media_task(void *p)
     UINT16 event;
     BT_HDR *p_msg;
 
-    APPL_TRACE_DEBUG0("================ MEDIA TASK STARTING ================");
+    VERBOSE("================ MEDIA TASK STARTING ================");
 
     btif_media_task_init();
 
@@ -988,7 +1127,7 @@ int btif_media_task(void *p)
     {
         event = GKI_wait(0xffff, 0);
 
-        APPL_TRACE_DEBUG1("================= MEDIA TASK EVENT %d ===============", event);
+        VERBOSE("================= MEDIA TASK EVENT %d ===============", event);
 
         if (event & BTIF_MEDIA_TASK_CMD)
         {
@@ -1015,7 +1154,7 @@ int btif_media_task(void *p)
         }
 
 
-        APPL_TRACE_DEBUG1("=============== MEDIA TASK EVENT %d DONE ============", event);
+        VERBOSE("=============== MEDIA TASK EVENT %d DONE ============", event);
 
         /* When we get this event we exit the task  - should only happen on GKI_shutdown  */
         if (event & BTIF_MEDIA_TASK_KILL)
@@ -1083,7 +1222,7 @@ static void btif_media_flush_q(BUFFER_Q *p_q)
  *******************************************************************************/
 static void btif_media_task_handle_cmd(BT_HDR *p_msg)
 {
-    APPL_TRACE_DEBUG2("btif_media_task_handle_cmd : %d %s", p_msg->event, dump_media_event(p_msg->event));
+    VERBOSE("btif_media_task_handle_cmd : %d %s", p_msg->event, dump_media_event(p_msg->event));
 
     switch (p_msg->event)
     {
@@ -1114,7 +1253,7 @@ static void btif_media_task_handle_cmd(BT_HDR *p_msg)
         APPL_TRACE_ERROR1("ERROR in btif_media_task_handle_cmd unknown event %d", p_msg->event);
     }
     GKI_freebuf(p_msg);
-    APPL_TRACE_EVENT1("btif_media_task_handle_cmd : %s DONE", dump_media_event(p_msg->event));
+    VERBOSE("btif_media_task_handle_cmd : %s DONE", dump_media_event(p_msg->event));
 }
 
 /*******************************************************************************
@@ -1371,7 +1510,6 @@ static void btif_media_task_enc_update(BT_HDR *p_msg)
 
         /* Set the initial target bit rate */
         pstrEncParams->u16BitRate = DEFAULT_SBC_BITRATE;
-
         if (pstrEncParams->s16SamplingFreq == SBC_sf16000)
             s16SamplingFreq = 16000;
         else if (pstrEncParams->s16SamplingFreq == SBC_sf32000)
@@ -1621,6 +1759,9 @@ static void btif_media_task_feeding_state_reset(void)
     /* By default, just clear the entire state */
     memset(&btif_media_cb.media_feeding_state, 0, sizeof(btif_media_cb.media_feeding_state));
 
+    /* reset pcm time tracker */
+    ra_reset();
+
 
 }
 /*******************************************************************************
@@ -1719,6 +1860,11 @@ static UINT8 btif_get_num_aa_frame(void)
                 }
                 break;
             }
+
+            result += ra_adjust();
+
+            VERBOSE("WRITE %d FRAMES", result);
+
 #if ((defined(BTIF_MEDIA_OVERFEED_INCLUDED) && (BTIF_MEDIA_OVERFEED_INCLUDED == TRUE)) || \
      (defined(BTIF_MEDIA_UNDERFEED_INCLUDED) && (BTIF_MEDIA_UNDERFEED_INCLUDED == TRUE)))
             btif_media_cb.tx_counter++;
@@ -1857,6 +2003,9 @@ BOOLEAN btif_media_aa_read_feeding(tUIPC_CH_ID channel_id)
     /* Read Data from UIPC channel */
     nb_byte_read = UIPC_Read(channel_id, &event, (UINT8 *)read_buffer, read_size);
 
+    /* update read 'pcmtime' */
+    ra_update(nb_byte_read);
+
     //tput_mon(TRUE, nb_byte_read, FALSE);
 
     if (nb_byte_read < read_size)
@@ -1993,7 +2142,7 @@ static void btif_media_aa_prep_sbc_2_send(UINT8 nb_frame)
         /* store the time stamp in the buffer to send */
         *((UINT32 *) (p_buf + 1)) = btif_media_cb.timestamp;
 
-        APPL_TRACE_EVENT1("TX QUEUE NOW %d", btif_media_cb.TxAaQ.count);
+        VERBOSE("TX QUEUE NOW %d", btif_media_cb.TxAaQ.count);
 
         if (btif_media_cb.tx_flush)
         {
@@ -2024,7 +2173,8 @@ static void btif_media_aa_prep_sbc_2_send(UINT8 nb_frame)
 
 static void btif_media_aa_prep_2_send(UINT8 nb_frame)
 {
-    APPL_TRACE_DEBUG1("btif_media_aa_prep_2_send : %d frames in queue", btif_media_cb.TxAaQ.count);
+    VERBOSE("btif_media_aa_prep_2_send : %d frames (queue %d)", nb_frame,
+                       btif_media_cb.TxAaQ.count);
 
     /* Remove all the buffers not sent until there are only 4 in the queue */
     while (btif_media_cb.TxAaQ.count >= MAX_OUTPUT_BUFFER_QUEUE_SZ)
@@ -2066,7 +2216,7 @@ static void btif_media_send_aa_frame(void)
     btif_media_aa_prep_2_send(nb_frame_2_send);
 
     /* send it */
-    APPL_TRACE_DEBUG0("btif_media_send_aa_frame calls bta_av_ci_src_data_ready");
+    VERBOSE("btif_media_send_aa_frame : send %d frames", nb_frame_2_send);
     bta_av_ci_src_data_ready(BTA_AV_CHNL_AUDIO);
 }
 
diff --git a/udrv/ulinux/uipc.c b/udrv/ulinux/uipc.c
index 8af45f1..6440323 100644
--- a/udrv/ulinux/uipc.c
+++ b/udrv/ulinux/uipc.c
@@ -777,8 +777,8 @@ UDRV_API UINT32 UIPC_Read(tUIPC_CH_ID ch_id, UINT16 *p_msg_evt, UINT8 *p_buf, UI
         return 0;
     }
 
-    BTIF_TRACE_DEBUG4("UIPC_Read : ch_id %d, len %d, fd %d, polltmo %d", ch_id, len,
-            fd, uipc_main.ch[ch_id].read_poll_tmo_ms);
+    //BTIF_TRACE_DEBUG4("UIPC_Read : ch_id %d, len %d, fd %d, polltmo %d", ch_id, len,
+    //        fd, uipc_main.ch[ch_id].read_poll_tmo_ms);
 
     while (n_read < (int)len)
     {
@@ -792,7 +792,7 @@ UDRV_API UINT32 UIPC_Read(tUIPC_CH_ID ch_id, UINT16 *p_msg_evt, UINT8 *p_buf, UI
             return 0;
         }
 
-        BTIF_TRACE_EVENT1("poll revents %x", pfd.revents);
+        //BTIF_TRACE_EVENT1("poll revents %x", pfd.revents);
 
         if (pfd.revents & (POLLHUP|POLLNVAL) )
         {