5 files changed, 189 insertions, 87 deletions

diff --git a/src/phFriNfc_LlcpMacNfcip.c b/src/phFriNfc_LlcpMacNfcip.c
index 6fc7cf6..611571a 100644
--- a/src/phFriNfc_LlcpMacNfcip.c
+++ b/src/phFriNfc_LlcpMacNfcip.c
@@ -104,9 +104,9 @@ static NFCSTATUS phFriNfc_LlcpMac_Nfcip_Chk(phFriNfc_LlcpMac_t
       {
          status = PHNFCSTVAL(CID_FRI_NFC_LLCP_MAC, NFCSTATUS_FAILED);
       }
+      ChkLlcpMac_Cb(pContext,status);
    }
 
-   ChkLlcpMac_Cb(pContext,status);
    return status;
 }
 
@@ -142,29 +142,27 @@ static NFCSTATUS phFriNfc_LlcpMac_Nfcip_Deactivate (phFriNfc_LlcpMac_t   *LlcpMa
    {
       /* Set the flag of LinkStatus to deactivate */
       LlcpMac->LinkState = phFriNfc_LlcpMac_eLinkDeactivated;
-   }
-
-   if (LlcpMac->SendPending)
-   {
-      /* Reset Flag */
-      LlcpMac->SendPending = FALSE;
 
-      phFriNfc_LlcpMac_Nfcip_TriggerSendCb(LlcpMac, NFCSTATUS_FAILED);
-   }
+      if (LlcpMac->SendPending)
+      {
+         /* Reset Flag */
+         LlcpMac->SendPending = FALSE;
+         phFriNfc_LlcpMac_Nfcip_TriggerSendCb(LlcpMac, NFCSTATUS_FAILED);
+      }
 
-   if (LlcpMac->RecvPending)
-   {
-      /* Reset Flag */
-      LlcpMac->RecvPending = FALSE;
+      if (LlcpMac->RecvPending)
+      {
+         /* Reset Flag */
+         LlcpMac->RecvPending = FALSE;
+         phFriNfc_LlcpMac_Nfcip_TriggerRecvCb(LlcpMac, NFCSTATUS_FAILED);
+      }
 
-      phFriNfc_LlcpMac_Nfcip_TriggerRecvCb(LlcpMac, NFCSTATUS_FAILED);
+      LlcpMac->LinkStatus_Cb(LlcpMac->LinkStatus_Context,
+                             LlcpMac->LinkState,
+                             NULL,
+                             LlcpMac->PeerRemoteDevType);
    }
 
-   LlcpMac->LinkStatus_Cb(LlcpMac->LinkStatus_Context,
-                          LlcpMac->LinkState,
-                          NULL,
-                          LlcpMac->PeerRemoteDevType);
-
    return status;
 }
 
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_Connection.c b/src/phFriNfc_LlcpTransport_Connection.c
index c3b92f3..346558c 100644
--- a/src/phFriNfc_LlcpTransport_Connection.c
+++ b/src/phFriNfc_LlcpTransport_Connection.c
@@ -1026,7 +1026,7 @@ static void Handle_Receive_IFrame(phFriNfc_LlcpTransport_t      *psTransport,
             if(psLocalLlcpSocket->bSocketRecvPending == TRUE && (psLocalLlcpSocket->indexRwWrite == psLocalLlcpSocket->indexRwRead))
             {
                /* Reset Flag */
-               psTransport->pSocketTable[psTransport->socketIndex].bSocketRecvPending = FALSE;
+               psLocalLlcpSocket->bSocketRecvPending = FALSE;
 
                /* Save I_FRAME into the Receive Buffer */
                memcpy(psLocalLlcpSocket->sSocketRecvBuffer->buffer,psData->buffer,psData->length);
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.
 *