Merge "LE link_xmit_quota is not correctly allocated" into lmp-dev

4 files changed, 220 insertions, 28 deletions

diff --git a/stack/l2cap/l2c_ble.c b/stack/l2cap/l2c_ble.c
index 8a609d7..673f5b7 100644
--- a/stack/l2cap/l2c_ble.c
+++ b/stack/l2cap/l2c_ble.c
@@ -765,6 +765,130 @@ void l2c_link_processs_ble_num_bufs (UINT16 num_lm_ble_bufs)
     l2cb.num_lm_ble_bufs = l2cb.controller_le_xmit_window = num_lm_ble_bufs;
 }
 
+/*******************************************************************************
+**
+** Function         l2c_ble_link_adjust_allocation
+**
+** Description      This function is called when a link is created or removed
+**                  to calculate the amount of packets each link may send to
+**                  the HCI without an ack coming back.
+**
+**                  Currently, this is a simple allocation, dividing the
+**                  number of Controller Packets by the number of links. In
+**                  the future, QOS configuration should be examined.
+**
+** Returns          void
+**
+*******************************************************************************/
+void l2c_ble_link_adjust_allocation (void)
+{
+    UINT16      qq, yy, qq_remainder;
+    tL2C_LCB    *p_lcb;
+    UINT16      hi_quota, low_quota;
+    UINT16      num_lowpri_links = 0;
+    UINT16      num_hipri_links  = 0;
+    UINT16      controller_xmit_quota = l2cb.num_lm_ble_bufs;
+    UINT16      high_pri_link_quota = L2CAP_HIGH_PRI_MIN_XMIT_QUOTA_A;
+
+    /* If no links active, reset buffer quotas and controller buffers */
+    if (l2cb.num_ble_links_active == 0)
+    {
+        l2cb.controller_le_xmit_window = l2cb.num_lm_ble_bufs;
+        l2cb.ble_round_robin_quota = l2cb.ble_round_robin_unacked = 0;
+        return;
+    }
+
+    /* First, count the links */
+    for (yy = 0, p_lcb = &l2cb.lcb_pool[0]; yy < MAX_L2CAP_LINKS; yy++, p_lcb++)
+    {
+        if (p_lcb->in_use && p_lcb->transport == BT_TRANSPORT_LE)
+        {
+            if (p_lcb->acl_priority == L2CAP_PRIORITY_HIGH)
+                num_hipri_links++;
+            else
+                num_lowpri_links++;
+        }
+    }
+
+    /* now adjust high priority link quota */
+    low_quota = num_lowpri_links ? 1 : 0;
+    while ( (num_hipri_links * high_pri_link_quota + low_quota) > controller_xmit_quota )
+        high_pri_link_quota--;
+
+
+    /* Work out the xmit quota and buffer quota high and low priorities */
+    hi_quota  = num_hipri_links * high_pri_link_quota;
+    low_quota = (hi_quota < controller_xmit_quota) ? controller_xmit_quota - hi_quota : 1;
+
+    /* Work out and save the HCI xmit quota for each low priority link */
+
+    /* If each low priority link cannot have at least one buffer */
+    if (num_lowpri_links > low_quota)
+    {
+        l2cb.ble_round_robin_quota = low_quota;
+        qq = qq_remainder = 0;
+    }
+    /* If each low priority link can have at least one buffer */
+    else if (num_lowpri_links > 0)
+    {
+        l2cb.ble_round_robin_quota = 0;
+        l2cb.ble_round_robin_unacked = 0;
+        qq = low_quota / num_lowpri_links;
+        qq_remainder = low_quota % num_lowpri_links;
+    }
+    /* If no low priority link */
+    else
+    {
+        l2cb.ble_round_robin_quota = 0;
+        l2cb.ble_round_robin_unacked = 0;
+        qq = qq_remainder = 0;
+    }
+    L2CAP_TRACE_EVENT ("l2c_ble_link_adjust_allocation  num_hipri: %u  num_lowpri: %u  low_quota: %u  round_robin_quota: %u  qq: %u",
+                        num_hipri_links, num_lowpri_links, low_quota,
+                        l2cb.ble_round_robin_quota, qq);
+
+    /* Now, assign the quotas to each link */
+    for (yy = 0, p_lcb = &l2cb.lcb_pool[0]; yy < MAX_L2CAP_LINKS; yy++, p_lcb++)
+    {
+        if (p_lcb->in_use && p_lcb->transport == BT_TRANSPORT_LE)
+        {
+            if (p_lcb->acl_priority == L2CAP_PRIORITY_HIGH)
+            {
+                p_lcb->link_xmit_quota   = high_pri_link_quota;
+            }
+            else
+            {
+                /* Safety check in case we switched to round-robin with something outstanding */
+                /* if sent_not_acked is added into round_robin_unacked then don't add it again */
+                /* l2cap keeps updating sent_not_acked for exiting from round robin */
+                if (( p_lcb->link_xmit_quota > 0 )&&( qq == 0 ))
+                    l2cb.ble_round_robin_unacked += p_lcb->sent_not_acked;
+
+                p_lcb->link_xmit_quota   = qq;
+                if (qq_remainder > 0)
+                {
+                    p_lcb->link_xmit_quota++;
+                    qq_remainder--;
+                }
+            }
+
+            L2CAP_TRACE_EVENT("l2c_ble_link_adjust_allocation LCB %d   Priority: %d  XmitQuota: %d",
+                                yy, p_lcb->acl_priority, p_lcb->link_xmit_quota);
+
+            L2CAP_TRACE_EVENT("        SentNotAcked: %d  RRUnacked: %d",
+                                p_lcb->sent_not_acked, l2cb.round_robin_unacked);
+
+            /* There is a special case where we have readjusted the link quotas and  */
+            /* this link may have sent anything but some other link sent packets so  */
+            /* so we may need a timer to kick off this link's transmissions.         */
+            if ( (p_lcb->link_state == LST_CONNECTED)
+              && (p_lcb->link_xmit_data_q.count)
+              && (p_lcb->sent_not_acked < p_lcb->link_xmit_quota) )
+                btu_start_timer (&p_lcb->timer_entry, BTU_TTYPE_L2CAP_LINK, L2CAP_LINK_FLOW_CONTROL_TOUT);
+        }
+    }
+}
+
 #if (defined BLE_LLT_INCLUDED) && (BLE_LLT_INCLUDED == TRUE)
 /*******************************************************************************
 **
diff --git a/stack/l2cap/l2c_int.h b/stack/l2cap/l2c_int.h
index c96c409..f5b3e08 100644
--- a/stack/l2cap/l2c_int.h
+++ b/stack/l2cap/l2c_int.h
@@ -507,10 +507,14 @@ typedef struct
 #endif
 
 #if (BLE_INCLUDED == TRUE)
+    UINT16                   num_ble_links_active;               /* Number of LE links active           */
     BOOLEAN                  is_ble_connecting;
     BD_ADDR                  ble_connecting_bda;
     UINT16                   controller_le_xmit_window;         /* Total ACL window for all links   */
     UINT16                   num_lm_ble_bufs;                   /* # of ACL buffers on controller   */
+    UINT16                   ble_round_robin_quota;              /* Round-robin link quota           */
+    UINT16                   ble_round_robin_unacked;            /* Round-robin unacked              */
+    BOOLEAN                  ble_check_round_robin;              /* Do a round robin check           */
 #endif
 
     tL2CA_ECHO_DATA_CB      *p_echo_data_cb;                /* Echo data callback */
@@ -769,6 +773,8 @@ extern void l2cble_conn_comp (UINT16 handle, UINT8 role, BD_ADDR bda, tBLE_ADDR_
 extern BOOLEAN l2cble_init_direct_conn (tL2C_LCB *p_lcb);
 extern void l2c_enable_conn_param_timeout(tL2C_LCB * p_lcb);
 extern void l2cble_notify_le_connection (BD_ADDR bda);
+extern void l2c_ble_link_adjust_allocation (void);
+
 #if (defined BLE_LLT_INCLUDED) && (BLE_LLT_INCLUDED == TRUE)
 extern void l2cble_process_rc_param_request_evt(UINT16 handle, UINT16 int_min, UINT16 int_max,
                                                         UINT16 latency, UINT16 timeout);
diff --git a/stack/l2cap/l2c_link.c b/stack/l2cap/l2c_link.c
index 0ff79df..3ebc81b 100644
--- a/stack/l2cap/l2c_link.c
+++ b/stack/l2cap/l2c_link.c
@@ -1127,7 +1127,14 @@ void l2c_link_check_send_pkts (tL2C_LCB *p_lcb, tL2C_CCB *p_ccb, BT_HDR *p_buf)
         GKI_enqueue (&p_lcb->link_xmit_data_q, p_buf);
 
         if (p_lcb->link_xmit_quota == 0)
-            l2cb.check_round_robin = TRUE;
+        {
+#if BLE_INCLUDED == TRUE
+            if (p_lcb->transport == BT_TRANSPORT_LE)
+                l2cb.ble_check_round_robin = TRUE;
+            else
+#endif
+                l2cb.check_round_robin = TRUE;
+        }
     }
 
     /* If this is called from uncongested callback context break recursive calling.
@@ -1190,16 +1197,20 @@ void l2c_link_check_send_pkts (tL2C_LCB *p_lcb, tL2C_CCB *p_ccb, BT_HDR *p_buf)
         }
 
         /* If we finished without using up our quota, no need for a safety check */
-#if (BLE_INCLUDED == TRUE)
-        if ( ((l2cb.controller_xmit_window > 0 && (p_lcb->transport == BT_TRANSPORT_BR_EDR)) ||
-             (l2cb.controller_le_xmit_window > 0 && (p_lcb->transport == BT_TRANSPORT_LE)))
-          && (l2cb.round_robin_unacked < l2cb.round_robin_quota) )
-#else
         if ( (l2cb.controller_xmit_window > 0)
-          && (l2cb.round_robin_unacked < l2cb.round_robin_quota) )
-
+          && (l2cb.round_robin_unacked < l2cb.round_robin_quota)
+#if (BLE_INCLUDED == TRUE)
+          && (p_lcb->transport == BT_TRANSPORT_BR_EDR)
 #endif
+          )
             l2cb.check_round_robin = FALSE;
+
+#if (BLE_INCLUDED == TRUE)
+        if ( (l2cb.controller_le_xmit_window > 0)
+          && (l2cb.ble_round_robin_unacked < l2cb.ble_round_robin_quota)
+          && (p_lcb->transport == BT_TRANSPORT_LE))
+            l2cb.ble_check_round_robin = FALSE;
+#endif
     }
     else /* if this is not round-robin service */
     {
@@ -1278,8 +1289,14 @@ static BOOLEAN l2c_link_send_to_lower (tL2C_LCB *p_lcb, BT_HDR *p_buf)
         )
     {
         if (p_lcb->link_xmit_quota == 0)
-            l2cb.round_robin_unacked++;
-
+        {
+#if (BLE_INCLUDED == TRUE)
+            if (p_lcb->transport == BT_TRANSPORT_LE)
+                l2cb.ble_round_robin_unacked++;
+            else
+#endif
+                l2cb.round_robin_unacked++;
+        }
         p_lcb->sent_not_acked++;
         p_buf->layer_specific = 0;
 
@@ -1341,14 +1358,17 @@ static BOOLEAN l2c_link_send_to_lower (tL2C_LCB *p_lcb, BT_HDR *p_buf)
         if (p_lcb->transport == BT_TRANSPORT_LE)
         {
             l2cb.controller_le_xmit_window -= num_segs;
-
+            if (p_lcb->link_xmit_quota == 0)
+                l2cb.ble_round_robin_unacked += num_segs;
         }
         else
 #endif
-        l2cb.controller_xmit_window -= num_segs;
+        {
+            l2cb.controller_xmit_window -= num_segs;
 
-        if (p_lcb->link_xmit_quota == 0)
-            l2cb.round_robin_unacked += num_segs;
+            if (p_lcb->link_xmit_quota == 0)
+                l2cb.round_robin_unacked += num_segs;
+        }
 
         p_lcb->sent_not_acked += num_segs;
 #if BLE_INCLUDED == TRUE
@@ -1371,7 +1391,7 @@ static BOOLEAN l2c_link_send_to_lower (tL2C_LCB *p_lcb, BT_HDR *p_buf)
                 l2cb.controller_le_xmit_window,
                 p_lcb->handle,
                 p_lcb->link_xmit_quota, p_lcb->sent_not_acked,
-                l2cb.round_robin_quota, l2cb.round_robin_unacked);
+                l2cb.ble_round_robin_quota, l2cb.ble_round_robin_unacked);
     }
     else
 #endif
@@ -1436,11 +1456,24 @@ void l2c_link_process_num_completed_pkts (UINT8 *p)
             /* If doing round-robin, adjust communal counts */
             if (p_lcb->link_xmit_quota == 0)
             {
-                /* Don't go negative */
-                if (l2cb.round_robin_unacked > num_sent)
-                    l2cb.round_robin_unacked -= num_sent;
+#if BLE_INCLUDED == TRUE
+                if (p_lcb->transport == BT_TRANSPORT_LE)
+                {
+                   /* Don't go negative */
+                    if (l2cb.ble_round_robin_unacked > num_sent)
+                        l2cb.ble_round_robin_unacked -= num_sent;
+                    else
+                        l2cb.ble_round_robin_unacked = 0;
+                }
                 else
-                    l2cb.round_robin_unacked = 0;
+#endif
+                {
+                    /* Don't go negative */
+                    if (l2cb.round_robin_unacked > num_sent)
+                        l2cb.round_robin_unacked -= num_sent;
+                    else
+                        l2cb.round_robin_unacked = 0;
+                }
             }
 
             /* Don't go negative */
@@ -1458,6 +1491,15 @@ void l2c_link_process_num_completed_pkts (UINT8 *p)
             {
               l2c_link_check_send_pkts (NULL, NULL, NULL);
             }
+#if BLE_INCLUDED == TRUE
+            if ((p_lcb->transport == BT_TRANSPORT_LE)
+                && (p_lcb->acl_priority == L2CAP_PRIORITY_HIGH)
+                && ((l2cb.ble_check_round_robin)
+                && (l2cb.ble_round_robin_unacked < l2cb.ble_round_robin_quota)))
+            {
+              l2c_link_check_send_pkts (NULL, NULL, NULL);
+            }
+#endif
         }
 
 #if (L2CAP_HCI_FLOW_CONTROL_DEBUG == TRUE)
@@ -1469,7 +1511,7 @@ void l2c_link_process_num_completed_pkts (UINT8 *p)
                 L2CAP_TRACE_DEBUG ("TotalWin=%d,LinkUnack(0x%x)=%d,RRCheck=%d,RRUnack=%d",
                     l2cb.controller_le_xmit_window,
                     p_lcb->handle, p_lcb->sent_not_acked,
-                    l2cb.check_round_robin, l2cb.round_robin_unacked);
+                    l2cb.ble_check_round_robin, l2cb.ble_round_robin_unacked);
             }
             else
 #endif
@@ -1488,7 +1530,7 @@ void l2c_link_process_num_completed_pkts (UINT8 *p)
                 l2cb.controller_xmit_window,
                 l2cb.controller_le_xmit_window,
                 handle,
-                l2cb.check_round_robin, l2cb.round_robin_unacked);
+                l2cb.ble_check_round_robin, l2cb.ble_round_robin_unacked);
 #else
             L2CAP_TRACE_DEBUG ("TotalWin=%d  Handle=0x%x  RRCheck=%d  RRUnack=%d",
                 l2cb.controller_xmit_window,
diff --git a/stack/l2cap/l2c_utils.c b/stack/l2cap/l2c_utils.c
index 305e02e..242b25f 100644
--- a/stack/l2cap/l2c_utils.c
+++ b/stack/l2cap/l2c_utils.c
@@ -70,12 +70,20 @@ tL2C_LCB *l2cu_allocate_lcb (BD_ADDR p_bd_addr, BOOLEAN is_bonding, tBT_TRANSPOR
             p_lcb->idle_timeout    = l2cb.idle_timeout;
             p_lcb->id              = 1;                     /* spec does not allow '0' */
             p_lcb->is_bonding      = is_bonding;
-#if BLE_INCLUDED == TRUE
+#if (BLE_INCLUDED == TRUE)
             p_lcb->transport       = transport;
-#endif
-            l2cb.num_links_active++;
 
-            l2c_link_adjust_allocation();
+            if (transport == BT_TRANSPORT_LE)
+            {
+                l2cb.num_ble_links_active++;
+                l2c_ble_link_adjust_allocation();
+            }
+            else
+#endif
+            {
+                l2cb.num_links_active++;
+                l2c_link_adjust_allocation();
+            }
             return (p_lcb);
         }
     }
@@ -210,11 +218,23 @@ void l2cu_release_lcb (tL2C_LCB *p_lcb)
     l2c_ucd_delete_sec_pending_q(p_lcb);
 #endif
 
+#if BLE_INCLUDED == TRUE
     /* Re-adjust flow control windows make sure it does not go negative */
-    if (l2cb.num_links_active >= 1)
-        l2cb.num_links_active--;
+    if (p_lcb->transport == BT_TRANSPORT_LE)
+    {
+        if (l2cb.num_ble_links_active >= 1)
+            l2cb.num_ble_links_active--;
 
-    l2c_link_adjust_allocation();
+        l2c_ble_link_adjust_allocation();
+    }
+    else
+#endif
+    {
+        if (l2cb.num_links_active >= 1)
+            l2cb.num_links_active--;
+
+        l2c_link_adjust_allocation();
+    }
 
     /* Check for ping outstanding */
     if (p_lcb->p_echo_rsp_cb)