Handle inbound buffering in LLCP connectionless sockets

This enables the server to avoid losing packets, especially when
receiving them in an aggregated packet. The size of the buffer is
determined by the size of the working buffer provided by the application.

Change-Id: Ia3c72e19fdb74e2a43390af5607c497c2275fe46

1 files changed, 84 insertions, 27 deletions

diff --git a/src/phFriNfc_LlcpTransport_Connectionless.c b/src/phFriNfc_LlcpTransport_Connectionless.c
index 8d26d98..b27f6de 100644
--- a/src/phFriNfc_LlcpTransport_Connectionless.c
+++ b/src/phFriNfc_LlcpTransport_Connectionless.c
@@ -68,28 +68,56 @@ void Handle_Connectionless_IncommingFrame(phFriNfc_LlcpTransport_t      *pLlcpTr
                                           uint8_t                       dsap,
                                           uint8_t                       ssap)
 {
-   uint8_t i=0;
+   phFriNfc_LlcpTransport_Socket_t * pSocket = NULL;
+   uint8_t                           i       = 0;
+   uint8_t                           writeIndex;
 
+   /* Look through the socket table for a match */
    for(i=0;i<PHFRINFC_LLCP_NB_SOCKET_MAX;i++)
    {
-      /* Test if a socket is registered to get this packet */
-      if(pLlcpTransport->pSocketTable[i].socket_sSap == dsap && pLlcpTransport->pSocketTable[i].bSocketRecvPending == TRUE)
+      if(pLlcpTransport->pSocketTable[i].socket_sSap == dsap)
       {
-         pphFriNfc_LlcpTransportSocketRecvFromCb_t pfRecvFromCallback = pLlcpTransport->pSocketTable[i].pfSocketRecvFrom_Cb;
-         /* Reset the RecvPending variable */
-         pLlcpTransport->pSocketTable[i].bSocketRecvPending = FALSE;
-
-         /* Copy the received buffer into the receive buffer */   
-         memcpy(pLlcpTransport->pSocketTable[i].sSocketRecvBuffer->buffer,psData->buffer,psData->length);
-
-         /* Update the received length */
-         *pLlcpTransport->pSocketTable[i].receivedLength = psData->length;
-
-         /* Clear the Recv callback */
-         pLlcpTransport->pSocketTable[i].pfSocketRecvFrom_Cb = NULL;
-
-         /* call the Recv callback */
-         pfRecvFromCallback(pLlcpTransport->pSocketTable[i].pRecvContext,ssap,NFCSTATUS_SUCCESS);
+         /* Socket found ! */
+         pSocket = &pLlcpTransport->pSocketTable[i];
+
+         /* Forward directly to application if a read is pending */
+         if (pSocket->bSocketRecvPending == TRUE)
+         {
+            /* Reset the RecvPending variable */
+            pSocket->bSocketRecvPending = FALSE;
+
+            /* Copy the received buffer into the receive buffer */
+            memcpy(pSocket->sSocketRecvBuffer->buffer, psData->buffer, psData->length);
+
+            /* Update the received length */
+            *pSocket->receivedLength = psData->length;
+
+            /* call the recv callback */
+            pSocket->pfSocketRecvFrom_Cb(pSocket->pRecvContext, ssap, NFCSTATUS_SUCCESS);
+            pSocket->pfSocketRecvFrom_Cb = NULL;
+         }
+         /* If no read is pending, try to bufferize for later reading */
+         else
+         {
+            if((pSocket->indexRwWrite - pSocket->indexRwRead) < pSocket->localRW)
+            {
+               writeIndex = pSocket->indexRwWrite % pSocket->localRW;
+               /* Save SSAP */
+               pSocket->sSocketRwBufferTable[writeIndex].buffer[0] = ssap;
+               /* Save UI frame payload */
+               memcpy(pSocket->sSocketRwBufferTable[writeIndex].buffer + 1,
+                      psData->buffer,
+                      psData->length);
+               pSocket->sSocketRwBufferTable[writeIndex].length = psData->length;
+
+               /* Update the RW write index */
+               pSocket->indexRwWrite++;
+            }
+            else
+            {
+               /* Unable to bufferize the packet, drop it */
+            }
+         }
          break;
       }
    }
@@ -281,7 +309,9 @@ NFCSTATUS phLibNfc_LlcpTransport_Connectionless_RecvFrom(phFriNfc_LlcpTransport_
                                                          pphFriNfc_LlcpTransportSocketRecvFromCb_t         pRecv_Cb,
                                                          void                                              *pContext)
 {
-   NFCSTATUS status = NFCSTATUS_PENDING;
+   NFCSTATUS   status = NFCSTATUS_PENDING;
+   uint8_t     readIndex;
+   uint8_t     ssap;
 
    if(pLlcpSocket->bSocketRecvPending)
    {
@@ -289,16 +319,43 @@ NFCSTATUS phLibNfc_LlcpTransport_Connectionless_RecvFrom(phFriNfc_LlcpTransport_
    }
    else
    {
-      /* Store the callback and context*/
-      pLlcpSocket->pfSocketRecvFrom_Cb  = pRecv_Cb;
-      pLlcpSocket->pRecvContext         = pContext;
+      /* Check if pending packets in RW */
+      if(pLlcpSocket->indexRwRead != pLlcpSocket->indexRwWrite)
+      {
+         readIndex = pLlcpSocket->indexRwRead % pLlcpSocket->localRW;
+
+         /* Extract ssap and buffer from RW buffer */
+         ssap = pLlcpSocket->sSocketRwBufferTable[readIndex].buffer[0];
+         memcpy(psBuffer->buffer,
+                pLlcpSocket->sSocketRwBufferTable[readIndex].buffer + 1,
+                pLlcpSocket->sSocketRwBufferTable[readIndex].length);
+         psBuffer->length = pLlcpSocket->sSocketRwBufferTable[readIndex].length;
+
+         /* Reset RW buffer length */
+         pLlcpSocket->sSocketRwBufferTable[readIndex].length = 0;
 
-      /* Store the pointer to the receive buffer */
-      pLlcpSocket->sSocketRecvBuffer   =  psBuffer;
-      pLlcpSocket->receivedLength      =  &psBuffer->length;
+         /* Update Value Rw Read Index */
+         pLlcpSocket->indexRwRead++;
 
-      /* Set RecvPending to TRUE */
-      pLlcpSocket->bSocketRecvPending = TRUE;
+         /* call the recv callback */
+         pRecv_Cb(pContext, ssap, NFCSTATUS_SUCCESS);
+
+         status = NFCSTATUS_SUCCESS;
+      }
+      /* Otherwise, wait for a packet to come */
+      else
+      {
+         /* Store the callback and context*/
+         pLlcpSocket->pfSocketRecvFrom_Cb  = pRecv_Cb;
+         pLlcpSocket->pRecvContext         = pContext;
+
+         /* Store the pointer to the receive buffer */
+         pLlcpSocket->sSocketRecvBuffer   =  psBuffer;
+         pLlcpSocket->receivedLength      =  &psBuffer->length;
+
+         /* Set RecvPending to TRUE */
+         pLlcpSocket->bSocketRecvPending = TRUE;
+      }
    }
    return status;
 }