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

3 files changed, 171 insertions, 67 deletions

diff --git a/src/phFriNfc_LlcpTransport.c b/src/phFriNfc_LlcpTransport.c
index 158dbc2..61284bd 100644
--- a/src/phFriNfc_LlcpTransport.c
+++ b/src/phFriNfc_LlcpTransport.c
@@ -1206,7 +1206,12 @@ NFCSTATUS phFriNfc_LlcpTransport_Socket(phFriNfc_LlcpTransport_t
    uint8_t cpt;
 
    /* Check for NULL pointers */
-   if (((NULL == psOptions) && (eType != phFriNfc_LlcpTransport_eConnectionLess)) || ((psWorkingBuffer == NULL) && (eType != phFriNfc_LlcpTransport_eConnectionLess)) || pLlcpSocket == NULL || pErr_Cb == NULL || pContext == NULL || pLlcpTransport == NULL)
+   if (   ((psOptions == NULL) && (eType == phFriNfc_LlcpTransport_eConnectionOriented))
+       || ((psWorkingBuffer == NULL) && (eType == phFriNfc_LlcpTransport_eConnectionOriented))
+       || (pLlcpSocket == NULL)
+       || (pErr_Cb == NULL)
+       || (pContext == NULL)
+       || (pLlcpTransport == NULL))
    {
       status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
       return status;
@@ -1217,12 +1222,19 @@ NFCSTATUS phFriNfc_LlcpTransport_Socket(phFriNfc_LlcpTransport_t
       status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
       return status;
    }
+   /* Connectionless sockets don't support options */
+   else if ((psOptions != NULL) && (eType == phFriNfc_LlcpTransport_eConnectionLess))
+   {
+      status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
+      return status;
+   }
 
    /* Get the local parameters of the LLCP Link */
    status = phFriNfc_Llcp_GetLocalInfo(pLlcpTransport->pLlcp,&LlcpLinkParamInfo);
    if(status != NFCSTATUS_SUCCESS)
    {
       status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_FAILED);
+      return status;
    }
    else
    {
@@ -1242,56 +1254,89 @@ NFCSTATUS phFriNfc_LlcpTransport_Socket(phFriNfc_LlcpTransport_t
             pLlcpTransport->pSocketTable[index].pContext   = pContext;
 
             /* Set the pointers to the different working buffers */
-            if(pLlcpTransport->pSocketTable[index].eSocket_Type != phFriNfc_LlcpTransport_eConnectionLess)
+            if (eType == phFriNfc_LlcpTransport_eConnectionOriented)
             {
-               /* Test the socket options */
-               if((psOptions->rw > PHFRINFC_LLCP_RW_MAX) && (eType == phFriNfc_LlcpTransport_eConnectionOriented))
+                /* Test the socket options */
+                if (psOptions->rw > PHFRINFC_LLCP_RW_MAX)
+                {
+                    status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
+                    return status;
+                }
+
+                /* Set socket options */
+                memcpy(&pLlcpTransport->pSocketTable[index].sSocketOption, psOptions, sizeof(phFriNfc_LlcpTransport_sSocketOptions_t));
+
+                /* Set socket local params (MIUX & RW) */
+                pLlcpTransport->pSocketTable[index].localMIUX = (pLlcpTransport->pSocketTable[index].sSocketOption.miu - PHFRINFC_LLCP_MIU_DEFAULT) & PHFRINFC_LLCP_TLV_MIUX_MASK;
+                pLlcpTransport->pSocketTable[index].localRW   = pLlcpTransport->pSocketTable[index].sSocketOption.rw & PHFRINFC_LLCP_TLV_RW_MASK;
+
+                /* Set the Max length for the Send and Receive Window Buffer */
+                pLlcpTransport->pSocketTable[index].bufferSendMaxLength   = pLlcpTransport->pSocketTable[index].sSocketOption.miu;
+                pLlcpTransport->pSocketTable[index].bufferRwMaxLength     = pLlcpTransport->pSocketTable[index].sSocketOption.miu * ((pLlcpTransport->pSocketTable[index].sSocketOption.rw & PHFRINFC_LLCP_TLV_RW_MASK));
+                pLlcpTransport->pSocketTable[index].bufferLinearLength    = psWorkingBuffer->length - pLlcpTransport->pSocketTable[index].bufferSendMaxLength - pLlcpTransport->pSocketTable[index].bufferRwMaxLength;
+
+                /* Test the connection oriented buffers length */
+                if((pLlcpTransport->pSocketTable[index].bufferSendMaxLength + pLlcpTransport->pSocketTable[index].bufferRwMaxLength) > psWorkingBuffer->length  
+                    || ((pLlcpTransport->pSocketTable[index].bufferLinearLength < PHFRINFC_LLCP_MIU_DEFAULT) && (pLlcpTransport->pSocketTable[index].bufferLinearLength != 0)))
+                {
+                    status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_BUFFER_TOO_SMALL);
+                    return status;
+                }
+
+                /* Set the pointer and the length for the Receive Window Buffer */
+                for(cpt=0;cpt<pLlcpTransport->pSocketTable[index].localRW;cpt++)
+                {
+                    pLlcpTransport->pSocketTable[index].sSocketRwBufferTable[cpt].buffer = psWorkingBuffer->buffer + (cpt*pLlcpTransport->pSocketTable[index].sSocketOption.miu);
+                    pLlcpTransport->pSocketTable[index].sSocketRwBufferTable[cpt].length = 0;
+                }
+
+                /* Set the pointer and the length for the Send Buffer */
+                pLlcpTransport->pSocketTable[index].sSocketSendBuffer.buffer     = psWorkingBuffer->buffer + pLlcpTransport->pSocketTable[index].bufferRwMaxLength;
+                pLlcpTransport->pSocketTable[index].sSocketSendBuffer.length     = pLlcpTransport->pSocketTable[index].bufferSendMaxLength;
+
+                /** Set the pointer and the length for the Linear Buffer */
+                pLlcpTransport->pSocketTable[index].sSocketLinearBuffer.buffer   = psWorkingBuffer->buffer + pLlcpTransport->pSocketTable[index].bufferRwMaxLength + pLlcpTransport->pSocketTable[index].bufferSendMaxLength;
+                pLlcpTransport->pSocketTable[index].sSocketLinearBuffer.length   = pLlcpTransport->pSocketTable[index].bufferLinearLength;
+
+                if(pLlcpTransport->pSocketTable[index].sSocketLinearBuffer.length != 0)
+                {
+                    /* Init Cyclic Fifo */
+                    phFriNfc_Llcp_CyclicFifoInit(&pLlcpTransport->pSocketTable[index].sCyclicFifoBuffer,
+                                                pLlcpTransport->pSocketTable[index].sSocketLinearBuffer.buffer,
+                                                pLlcpTransport->pSocketTable[index].sSocketLinearBuffer.length);
+                }
+            }
+            /* Handle connectionless socket with buffering option */
+            else if (eType == phFriNfc_LlcpTransport_eConnectionLess)
+            {
+               /* Determine how many packets can be bufferized in working buffer */
+               if (psWorkingBuffer != NULL)
                {
-                  status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
+                  /* NOTE: the extra byte is used to store SSAP */
+                  pLlcpTransport->pSocketTable[index].localRW = psWorkingBuffer->length / (pLlcpTransport->pLlcp->sLocalParams.miu + 1);
                }
-               /* Set socket options */
-               memcpy(&pLlcpTransport->pSocketTable[index].sSocketOption, psOptions, sizeof(phFriNfc_LlcpTransport_sSocketOptions_t));
-
-               /* Set socket local params (MIUX & RW) */
-               pLlcpTransport->pSocketTable[index].localMIUX = (pLlcpTransport->pSocketTable[index].sSocketOption.miu - PHFRINFC_LLCP_MIU_DEFAULT) & PHFRINFC_LLCP_TLV_MIUX_MASK;
-               pLlcpTransport->pSocketTable[index].localRW   = pLlcpTransport->pSocketTable[index].sSocketOption.rw & PHFRINFC_LLCP_TLV_RW_MASK;
-
-               /* Set the Max length for the Send and Receive Window Buffer */
-               pLlcpTransport->pSocketTable[index].bufferSendMaxLength   = pLlcpTransport->pSocketTable[index].sSocketOption.miu;
-               pLlcpTransport->pSocketTable[index].bufferRwMaxLength     = pLlcpTransport->pSocketTable[index].sSocketOption.miu * ((pLlcpTransport->pSocketTable[index].sSocketOption.rw & PHFRINFC_LLCP_TLV_RW_MASK));
-               pLlcpTransport->pSocketTable[index].bufferLinearLength    = psWorkingBuffer->length - pLlcpTransport->pSocketTable[index].bufferSendMaxLength - pLlcpTransport->pSocketTable[index].bufferRwMaxLength;
-
-               /* Test the connection oriented buffers length */
-               if((pLlcpTransport->pSocketTable[index].bufferSendMaxLength + pLlcpTransport->pSocketTable[index].bufferRwMaxLength) > psWorkingBuffer->length  
-                   || ((pLlcpTransport->pSocketTable[index].bufferLinearLength < PHFRINFC_LLCP_MIU_DEFAULT) && (pLlcpTransport->pSocketTable[index].bufferLinearLength != 0)))
+               else
                {
-                  status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_BUFFER_TOO_SMALL);
-                  return status;
+                  pLlcpTransport->pSocketTable[index].localRW = 0;
                }
 
-               /* Set the pointer and the length for the Receive Window Buffer */
-               for(cpt=0;cpt<pLlcpTransport->pSocketTable[index].localRW;cpt++)
+               if (pLlcpTransport->pSocketTable[index].localRW > PHFRINFC_LLCP_RW_MAX)
                {
-                  pLlcpTransport->pSocketTable[index].sSocketRwBufferTable[cpt].buffer = psWorkingBuffer->buffer + (cpt*pLlcpTransport->pSocketTable[index].sSocketOption.miu);
-                  pLlcpTransport->pSocketTable[index].sSocketRwBufferTable[cpt].length = 0;
+                  pLlcpTransport->pSocketTable[index].localRW = PHFRINFC_LLCP_RW_MAX;
                }
 
-               /* Set the pointer and the length for the Send Buffer */
-               pLlcpTransport->pSocketTable[index].sSocketSendBuffer.buffer     = psWorkingBuffer->buffer + pLlcpTransport->pSocketTable[index].bufferRwMaxLength;
-               pLlcpTransport->pSocketTable[index].sSocketSendBuffer.length     = pLlcpTransport->pSocketTable[index].bufferSendMaxLength;
-
-               /** Set the pointer and the length for the Linear Buffer */
-               pLlcpTransport->pSocketTable[index].sSocketLinearBuffer.buffer   = psWorkingBuffer->buffer + pLlcpTransport->pSocketTable[index].bufferRwMaxLength + pLlcpTransport->pSocketTable[index].bufferSendMaxLength;
-               pLlcpTransport->pSocketTable[index].sSocketLinearBuffer.length   = pLlcpTransport->pSocketTable[index].bufferLinearLength;
-
-               if(pLlcpTransport->pSocketTable[index].sSocketLinearBuffer.length != 0)
+               /* Set the pointers and the lengths for buffering */
+               for(cpt=0 ; cpt<pLlcpTransport->pSocketTable[index].localRW ; cpt++)
                {
-                  /* Init Cyclic Fifo */
-                  phFriNfc_Llcp_CyclicFifoInit(&pLlcpTransport->pSocketTable[index].sCyclicFifoBuffer,
-                                               pLlcpTransport->pSocketTable[index].sSocketLinearBuffer.buffer,
-                                               pLlcpTransport->pSocketTable[index].sSocketLinearBuffer.length);
+                  pLlcpTransport->pSocketTable[index].sSocketRwBufferTable[cpt].buffer = psWorkingBuffer->buffer + (cpt*(pLlcpTransport->pLlcp->sLocalParams.miu + 1));
+                  pLlcpTransport->pSocketTable[index].sSocketRwBufferTable[cpt].length = 0;
                }
+
+               /* Set other socket internals */
+               pLlcpTransport->pSocketTable[index].indexRwRead      = 0;
+               pLlcpTransport->pSocketTable[index].indexRwWrite     = 0;
             }
+
             /* Store index of the socket */
             pLlcpTransport->pSocketTable[index].index = index;
 
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;
 }
diff --git a/src/phLibNfc.h b/src/phLibNfc.h
index 7a5c3f0..e4e41cf 100644
--- a/src/phLibNfc.h
+++ b/src/phLibNfc.h
@@ -2577,7 +2577,9 @@ extern NFCSTATUS phLibNfc_Llcp_GetRemoteInfo( phLibNfc_Handle
 * must provide a working buffer to the socket in order to handle incoming data. This buffer
 * must be large enough to fit the receive window (RW * MIU), the remaining space being
 * used as a linear buffer to store incoming data as a stream. Data will be readable later
-* using the phLibNfc_Llcp_Recv function.
+* using the phLibNfc_Llcp_Recv function. If the socket is connectionless, the caller may
+* provide a working buffer to the socket in order to bufferize as many packets as the buffer
+* can contain (each packet needs MIU + 1 bytes).
 * The options and working buffer are not required if the socket is used as a listening socket,
 * since it cannot be directly used for communication.
 *