Fix some LLCP threading issues.

On Android we now start connecting SNEP/other LLCP
services as soon as the link comes up. That means
we will have connect requests incoming, accept
responses outgoing, connect requests outgoing and
accept responses incoming.

This lays bare a lot of threading issues in this lib.
Since the receives come in serialized, those typically
do not cause any issue. However, we may be sending
something on a thread coming from the NfcService,
while at the same time we'll receive for example
a connect frame from the remote, to which the receive
thread immediately wants to send a response.

This is a first attempt at making sends thread-safe:
already there was a lot of logic to deal with the fact
that another send was in progress, in the form of the
bSendPending variable. That variable was however
not checked atomically, and some operations did not
even set it correctly.

This change tests and sets that variable atomically,
more or less implementing a semaphore with try-acquires
and fallbacks for the failure case.

The generic structure is:

if (testAndSetSendPending()) {
   // Another thread is sending, stash data
   // and most importantly do *NOT* change shared data
} else {
   // We're now the only one sending, we're free
   // to change shared data until the callback is
   // called.
}

This is just a band-aid fix, but given the fact that
this stack will be deprecated it is hopefully enough
for our planned usecases.

A decent fix would involve switching to
a TX-queue per socket, and a generic TX-queue
for the whole LLCP link. This allows us to fully
decouple the connection layer from the transport
layer, as it should be.

Change-Id: Ibd8742f7350a58459771f5036a049af3a487f783

5 files changed, 181 insertions, 156 deletions

diff --git a/src/phFriNfc_Llcp.h b/src/phFriNfc_Llcp.h
index fb26d6a..728697f 100644
--- a/src/phFriNfc_Llcp.h
+++ b/src/phFriNfc_Llcp.h
@@ -221,6 +221,12 @@ typedef void (*phFriNfc_Llcp_LinkStatus_CB_t) (
    phFriNfc_Llcp_eLinkStatus_t      eLinkStatus
 );
 
+typedef void (*phFriNfc_Llcp_LinkSend_CB_t) (
+   void                             *pContext,
+   uint8_t                          socketIndex,
+   NFCSTATUS                        status
+);
+
 typedef void (*phFriNfc_Llcp_Send_CB_t) (
    void                             *pContext,
    NFCSTATUS                        status
diff --git a/src/phFriNfc_LlcpTransport.c b/src/phFriNfc_LlcpTransport.c
index 870aeb4..b3aee54 100644
--- a/src/phFriNfc_LlcpTransport.c
+++ b/src/phFriNfc_LlcpTransport.c
@@ -23,6 +23,7 @@
  */
 
 /*include files*/
+#include <cutils/log.h>
 #include <phOsalNfc.h>
 #include <phLibNfcStatus.h>
 #include <phLibNfc.h>
@@ -256,7 +257,7 @@ static NFCSTATUS phFriNfc_LlcpTransport_DiscoveryAnswer(phFriNfc_LlcpTransport_t
    uint8_t           nTid, nSap;
 
    /* Test if a send is pending */
-   if(!psTransport->bSendPending)
+   if(!testAndSetSendPending(psTransport))
    {
       /* Set the header */
       psTransport->sLlcpHeader.dsap  = PHFRINFC_LLCP_SAP_SDP;
@@ -293,9 +294,6 @@ static NFCSTATUS phFriNfc_LlcpTransport_DiscoveryAnswer(phFriNfc_LlcpTransport_t
       /* Update buffer length to match real TLV size */
       sInfoBuffer.length = nTlvOffset;
 
-      /* Send Pending */
-      psTransport->bSendPending = TRUE;
-
       /* Send SNL frame */
       result =  phFriNfc_Llcp_Send(psTransport->pLlcp,
                                    &psTransport->sLlcpHeader,
@@ -530,6 +528,20 @@ static void phFriNfc_LlcpTransport__Recv_CB(void            *pContext,
    }
 }
 
+bool_t testAndSetSendPending(phFriNfc_LlcpTransport_t* transport) {
+    bool_t currentValue;
+    pthread_mutex_lock(&transport->mutex);
+    currentValue = transport->bSendPending;
+    transport->bSendPending = TRUE;
+    pthread_mutex_unlock(&transport->mutex);
+    return currentValue;
+}
+
+void clearSendPending(phFriNfc_LlcpTransport_t* transport) {
+    pthread_mutex_lock(&transport->mutex);
+    transport->bSendPending = FALSE;
+    pthread_mutex_unlock(&transport->mutex);
+}
 
 /* TODO: comment function Transport recv CB */
 static void phFriNfc_LlcpTransport_Send_CB(void            *pContext,
@@ -538,35 +550,40 @@ static void phFriNfc_LlcpTransport_Send_CB(void            *pContext,
    phFriNfc_LlcpTransport_t         *psTransport = (phFriNfc_LlcpTransport_t*)pContext;
    NFCSTATUS                        result = NFCSTATUS_FAILED;
    phNfc_sData_t                    sFrmrBuffer;
-   phFriNfc_Llcp_Send_CB_t          pfSavedCb;
+   phFriNfc_Llcp_LinkSend_CB_t      pfSavedCb;
    void                             *pSavedContext;
    phFriNfc_LlcpTransport_Socket_t  *pCurrentSocket = NULL;
    uint8_t                          index;
 
+   // Store callbacks and socket index, so they can safely be
+   // overwritten by any code in the callback itself.
+   pfSavedCb = psTransport->pfLinkSendCb;
+   pSavedContext = psTransport->pLinkSendContext;
+   psTransport->pfLinkSendCb = NULL;
+   psTransport->pLinkSendContext = NULL;
+   index = psTransport->socketIndex;
+
    /* 1 - Reset the FLAG send pending*/
-   psTransport->bSendPending = FALSE;
+   clearSendPending(psTransport);
 
    /* 2 - Handle pending error responses */
-
    if(psTransport->bFrmrPending)
    {
-      /* Reset FRMR pending */
-      psTransport->bFrmrPending = FALSE;
-
-      /* Send Frmr */
-      sFrmrBuffer.buffer = psTransport->FrmrInfoBuffer;
-      sFrmrBuffer.length = 0x04; /* Size of FRMR Information field */
+      if (!testAndSetSendPending(psTransport)) {
+         /* Reset FRMR pending */
+         psTransport->bFrmrPending = FALSE;
 
-      /* Send Pending */
-      psTransport->bSendPending = TRUE;
+         /* Send Frmr */
+         sFrmrBuffer.buffer = psTransport->FrmrInfoBuffer;
+         sFrmrBuffer.length = 0x04; /* Size of FRMR Information field */
 
-      result =  phFriNfc_Llcp_Send(psTransport->pLlcp,
+         result =  phFriNfc_Llcp_Send(psTransport->pLlcp,
                                    &psTransport->sLlcpHeader,
                                    NULL,
                                    &sFrmrBuffer,
                                    phFriNfc_LlcpTransport_Send_CB,
                                    psTransport);
-
+      }
    }
    else if(psTransport->bDmPending)
    {
@@ -581,18 +598,12 @@ static void phFriNfc_LlcpTransport_Send_CB(void            *pContext,
    }
 
    /* 3 - Call the original callback */
-
-   if (psTransport->pfLinkSendCb != NULL)
+   if (pfSavedCb != NULL)
    {
-      pfSavedCb = psTransport->pfLinkSendCb;
-      pSavedContext = psTransport->pLinkSendContext;
-
-      psTransport->pfLinkSendCb = NULL;
-      psTransport->pLinkSendContext = NULL;
-
-      (*pfSavedCb)(pSavedContext, status);
+      (*pfSavedCb)(pSavedContext, index, status);
    }
 
+
    /* 4 - Handle pending send operations */
 
    /* Check for pending discovery requests/responses */
@@ -634,9 +645,6 @@ static void phFriNfc_LlcpTransport_Send_CB(void            *pContext,
       }
 
    } while(index != psTransport->socketIndex);
-
-   /* Save the new index */
-   psTransport->socketIndex = index;
 }
 
 
@@ -796,7 +804,8 @@ NFCSTATUS phFriNfc_LlcpTransport_LinkSend( phFriNfc_LlcpTransport_t         *Llc
                                            phFriNfc_Llcp_sPacketHeader_t    *psHeader,
                                            phFriNfc_Llcp_sPacketSequence_t  *psSequence,
                                            phNfc_sData_t                    *psInfo,
-                                           phFriNfc_Llcp_Send_CB_t          pfSend_CB,
+                                           phFriNfc_Llcp_LinkSend_CB_t      pfSend_CB,
+                                           uint8_t                          socketIndex,
                                            void                             *pContext )
 {
    NFCSTATUS status;
@@ -808,6 +817,7 @@ NFCSTATUS phFriNfc_LlcpTransport_LinkSend( phFriNfc_LlcpTransport_t         *Llc
    /* Save callback details */
    LlcpTransport->pfLinkSendCb = pfSend_CB;
    LlcpTransport->pLinkSendContext = pContext;
+   LlcpTransport->socketIndex = socketIndex;
 
    /* Call the link-level send function */
    status = phFriNfc_Llcp_Send(LlcpTransport->pLlcp, psHeader, psSequence, psInfo, phFriNfc_LlcpTransport_Send_CB, (void*)LlcpTransport);
@@ -885,7 +895,7 @@ NFCSTATUS phFriNfc_LlcpTransport_SendFrameReject(phFriNfc_LlcpTransport_t
       psTransport->FrmrInfoBuffer[3] = (vsa<<4)|vra ;
 
       /* Test if a send is pending */
-      if(psTransport->bSendPending)
+      if(testAndSetSendPending(psTransport))
       {
          psTransport->bFrmrPending = TRUE;
          status = NFCSTATUS_PENDING;
@@ -895,9 +905,6 @@ NFCSTATUS phFriNfc_LlcpTransport_SendFrameReject(phFriNfc_LlcpTransport_t
          sFrmrBuffer.buffer =  psTransport->FrmrInfoBuffer;
          sFrmrBuffer.length =  0x04; /* Size of FRMR Information field */
 
-         /* Send Pending */
-         psTransport->bSendPending = TRUE;
-
          /* Send FRMR frame */
          status =  phFriNfc_Llcp_Send(psTransport->pLlcp,
                                       &psTransport->sLlcpHeader,
@@ -927,7 +934,7 @@ NFCSTATUS phFriNfc_LlcpTransport_SendDisconnectMode(phFriNfc_LlcpTransport_t* ps
    NFCSTATUS                       status = NFCSTATUS_SUCCESS;
 
    /* Test if a send is pending */
-   if(psTransport->bSendPending)
+   if(testAndSetSendPending(psTransport))
    {
       /* DM pending */
       psTransport->bDmPending        = TRUE;
@@ -951,9 +958,6 @@ NFCSTATUS phFriNfc_LlcpTransport_SendDisconnectMode(phFriNfc_LlcpTransport_t* ps
       psTransport->sDmPayload.buffer    = &psTransport->DmInfoBuffer[2];
       psTransport->sDmPayload.length    = PHFRINFC_LLCP_DM_LENGTH;
 
-      /* Send Pending */
-      psTransport->bSendPending = TRUE;
-
       /* Send DM frame */
       status =  phFriNfc_Llcp_Send(psTransport->pLlcp,
                                    &psTransport->sDmHeader,
@@ -1075,7 +1079,7 @@ static NFCSTATUS phFriNfc_LlcpTransport_DiscoverServicesEx(phFriNfc_LlcpTranspor
    uint32_t          nTlvOffset;
 
    /* Test if a send is pending */
-   if(!psTransport->bSendPending)
+   if(!testAndSetSendPending(psTransport))
    {
       /* Set the header */
       psTransport->sLlcpHeader.dsap  = PHFRINFC_LLCP_SAP_SDP;
@@ -1111,9 +1115,6 @@ static NFCSTATUS phFriNfc_LlcpTransport_DiscoverServicesEx(phFriNfc_LlcpTranspor
       /* Update buffer length to match real TLV size */
       sInfoBuffer.length = nTlvOffset;
 
-      /* Send Pending */
-      psTransport->bSendPending = TRUE;
-
       /* Send SNL frame */
       result =  phFriNfc_Llcp_Send(psTransport->pLlcp,
                                    &psTransport->sLlcpHeader,
diff --git a/src/phFriNfc_LlcpTransport.h b/src/phFriNfc_LlcpTransport.h
index adcab9e..2bb0eac 100644
--- a/src/phFriNfc_LlcpTransport.h
+++ b/src/phFriNfc_LlcpTransport.h
@@ -32,6 +32,7 @@
 #include <phFriNfc_LlcpUtils.h>
 #ifdef ANDROID
 #include <string.h>
+#include <pthread.h>
 #endif
 
 
@@ -251,12 +252,13 @@ struct phFriNfc_LlcpTransport
    phFriNfc_LlcpTransport_Socket_t       pSocketTable[PHFRINFC_LLCP_NB_SOCKET_MAX];
    phFriNfc_Llcp_CachedServiceName_t     pCachedServiceNames[PHFRINFC_LLCP_SDP_ADVERTISED_NB];
    phFriNfc_Llcp_t                       *pLlcp;
+   pthread_mutex_t                       mutex;
    bool_t                                bSendPending;
    bool_t                                bRecvPending;
    bool_t                                bDmPending;
    bool_t                                bFrmrPending;
 
-   phFriNfc_Llcp_Send_CB_t               pfLinkSendCb;
+   phFriNfc_Llcp_LinkSend_CB_t           pfLinkSendCb;
    void                                  *pLinkSendContext;
 
    uint8_t                               socketIndex;
@@ -296,6 +298,10 @@ struct phFriNfc_LlcpTransport
 ################################################################################
 */
 
+bool_t testAndSetSendPending(phFriNfc_LlcpTransport_t* transport);
+
+void clearSendPending(phFriNfc_LlcpTransport_t* transport);
+
  /**
 * \ingroup grp_fri_nfc
 * \brief <b>Create a socket on a LLCP-connected device</b>.
@@ -322,7 +328,8 @@ NFCSTATUS phFriNfc_LlcpTransport_LinkSend( phFriNfc_LlcpTransport_t         *Llc
                                            phFriNfc_Llcp_sPacketHeader_t    *psHeader,
                                            phFriNfc_Llcp_sPacketSequence_t  *psSequence,
                                            phNfc_sData_t                    *psInfo,
-                                           phFriNfc_Llcp_Send_CB_t          pfSend_CB,
+                                           phFriNfc_Llcp_LinkSend_CB_t      pfSend_CB,
+                                           uint8_t                          socketIndex,
                                            void                             *pContext );
 
 
diff --git a/src/phFriNfc_LlcpTransport_Connection.c b/src/phFriNfc_LlcpTransport_Connection.c
index 63e2ff5..d23398f 100644
--- a/src/phFriNfc_LlcpTransport_Connection.c
+++ b/src/phFriNfc_LlcpTransport_Connection.c
@@ -22,6 +22,8 @@
  *
  */
 /*include files*/
+#define LOG_TAG "NFC"
+#include <cutils/log.h>
 #include <phOsalNfc.h>
 #include <phLibNfcStatus.h>
 #include <phLibNfc.h>
@@ -36,25 +38,11 @@ static NFCSTATUS phFriNfc_Llcp_Send_ReceiveReady_Frame(phFriNfc_LlcpTransport_So
 static NFCSTATUS phFriNfc_Llcp_Send_ReceiveNotReady_Frame(phFriNfc_LlcpTransport_Socket_t*   pLlcpSocket);
 
 static NFCSTATUS static_performSendInfo(phFriNfc_LlcpTransport_Socket_t * psLlcpSocket);
-
 /**********   End Function definition   ***********/
 
-NFCSTATUS phFriNfc_LlcpConnTransport_Send( phFriNfc_Llcp_t                  *Llcp,
-        phFriNfc_Llcp_sPacketHeader_t    *psHeader,
-        phFriNfc_Llcp_sPacketSequence_t  *psSequence,
-        phNfc_sData_t                    *psInfo,
-        phFriNfc_Llcp_Send_CB_t          pfSend_CB,
-        phFriNfc_LlcpTransport_t*        psTransport ) {
-    NFCSTATUS result = phFriNfc_Llcp_Send(Llcp, psHeader, psSequence, psInfo,
-            pfSend_CB, psTransport);
-    if (result == NFCSTATUS_PENDING) {
-        psTransport->bSendPending = TRUE;
-    }
-    return result;
-}
-
 /* TODO: comment functionphFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB */
 static void phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB(void*        pContext,
+                                                                  uint8_t      socketIndex,
                                                                   NFCSTATUS    status)
 {
    phFriNfc_LlcpTransport_t          *psTransport;
@@ -71,25 +59,25 @@ static void phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB(void*        p
    if(status == NFCSTATUS_SUCCESS)
    {
       /* Test the socket */
-      switch(psTransport->pSocketTable[psTransport->socketIndex].eSocket_State)
+      switch(psTransport->pSocketTable[socketIndex].eSocket_State)
       {
       case phFriNfc_LlcpTransportSocket_eSocketAccepted:
          {
             /* Set socket state to Connected */
-            psTransport->pSocketTable[psTransport->socketIndex].eSocket_State  = phFriNfc_LlcpTransportSocket_eSocketConnected;
+            psTransport->pSocketTable[socketIndex].eSocket_State  = phFriNfc_LlcpTransportSocket_eSocketConnected;
             /* Call the Accept Callback */
-            psTransport->pSocketTable[psTransport->socketIndex].pfSocketAccept_Cb(psTransport->pSocketTable[psTransport->socketIndex].pAcceptContext,status);
-            psTransport->pSocketTable[psTransport->socketIndex].pfSocketAccept_Cb = NULL;
-            psTransport->pSocketTable[psTransport->socketIndex].pAcceptContext = NULL;
+            psTransport->pSocketTable[socketIndex].pfSocketAccept_Cb(psTransport->pSocketTable[socketIndex].pAcceptContext,status);
+            psTransport->pSocketTable[socketIndex].pfSocketAccept_Cb = NULL;
+            psTransport->pSocketTable[socketIndex].pAcceptContext = NULL;
          }break;
 
       case phFriNfc_LlcpTransportSocket_eSocketRejected:
          {
             /* Store the Llcp socket in a local Llcp socket */
-            psTempLlcpSocket = psTransport->pSocketTable[psTransport->socketIndex];
+            psTempLlcpSocket = psTransport->pSocketTable[socketIndex];
 
             /* Reset the socket  and set the socket state to default */
-            result = phFriNfc_LlcpTransport_Close(&psTransport->pSocketTable[psTransport->socketIndex]);
+            result = phFriNfc_LlcpTransport_Close(&psTransport->pSocketTable[socketIndex]);
 
             /* Call the Reject Callback */
             psTempLlcpSocket.pfSocketSend_Cb(psTempLlcpSocket.pRejectContext,status);
@@ -98,10 +86,10 @@ static void phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB(void*        p
 
       case phFriNfc_LlcpTransportSocket_eSocketConnected:
          {
-            if(!psTransport->pSocketTable[psTransport->socketIndex].bSocketSendPending && psTransport->pSocketTable[psTransport->socketIndex].pfSocketSend_Cb != NULL)
+            if(!psTransport->pSocketTable[socketIndex].bSocketSendPending && psTransport->pSocketTable[socketIndex].pfSocketSend_Cb != NULL)
             {
-               psTransport->pSocketTable[psTransport->socketIndex].pfSocketSend_Cb(psTransport->pSocketTable[psTransport->socketIndex].pSendContext,status);
-               psTransport->pSocketTable[psTransport->socketIndex].pfSocketSend_Cb = NULL;
+               psTransport->pSocketTable[socketIndex].pfSocketSend_Cb(psTransport->pSocketTable[socketIndex].pSendContext,status);
+               psTransport->pSocketTable[socketIndex].pfSocketSend_Cb = NULL;
             }
          }break;
       default:
@@ -112,10 +100,10 @@ static void phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB(void*        p
    else
    {
       /* Send CB error */
-      if(!psTransport->pSocketTable[psTransport->socketIndex].bSocketSendPending && psTransport->pSocketTable[psTransport->socketIndex].pfSocketSend_Cb != NULL)
+      if(!psTransport->pSocketTable[socketIndex].bSocketSendPending && psTransport->pSocketTable[socketIndex].pfSocketSend_Cb != NULL)
       {
-         psTransport->pSocketTable[psTransport->socketIndex].pfSocketSend_Cb(psTransport->pSocketTable[psTransport->socketIndex].pSendContext,status);
-         psTransport->pSocketTable[psTransport->socketIndex].pfSocketSend_Cb = NULL;
+         psTransport->pSocketTable[socketIndex].pfSocketSend_Cb(psTransport->pSocketTable[socketIndex].pSendContext,status);
+         psTransport->pSocketTable[socketIndex].pfSocketSend_Cb = NULL;
       }
    }
 }
@@ -125,14 +113,18 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_HandlePendingOperations(phFr
 {
    NFCSTATUS                  result = NFCSTATUS_FAILED;
    phFriNfc_LlcpTransport_t   *psTransport = pSocket->psTransport;
-
    /* I FRAME */
    if(pSocket->bSocketSendPending == TRUE)
    {
       /* Test the RW window */
       if(CHECK_SEND_RW(pSocket))
       {
-         result = static_performSendInfo(pSocket);
+         if (!testAndSetSendPending(psTransport)) {
+            result = static_performSendInfo(pSocket);
+            if (result != NFCSTATUS_SUCCESS && result != NFCSTATUS_PENDING) {
+                clearSendPending(psTransport);
+            }
+         }
       }
    }
    /* RR FRAME */
@@ -156,70 +148,84 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_HandlePendingOperations(phFr
    /* CC Frame */
    else if(pSocket->bSocketAcceptPending == TRUE)
    {
-      /* Reset Accept pending */
-      pSocket->bSocketAcceptPending = FALSE;
-
-      /* Fill the psLlcpHeader stuture with the DSAP,CC PTYPE and the SSAP */
-      pSocket->sLlcpHeader.dsap  = pSocket->socket_dSap;
-      pSocket->sLlcpHeader.ptype = PHFRINFC_LLCP_PTYPE_CC;
-      pSocket->sLlcpHeader.ssap  = pSocket->socket_sSap;
+      if (!testAndSetSendPending(psTransport))
+      {
+         /* Reset Accept pending */
+         pSocket->bSocketAcceptPending = FALSE;
 
-      /* Send Pending */
-      pSocket->psTransport->bSendPending = TRUE;
+         /* Fill the psLlcpHeader stuture with the DSAP,CC PTYPE and the SSAP */
+         pSocket->sLlcpHeader.dsap  = pSocket->socket_dSap;
+         pSocket->sLlcpHeader.ptype = PHFRINFC_LLCP_PTYPE_CC;
+         pSocket->sLlcpHeader.ssap  = pSocket->socket_sSap;
 
-      /* Set the socket state to accepted */
-      pSocket->eSocket_State = phFriNfc_LlcpTransportSocket_eSocketAccepted;
+         /* Set the socket state to accepted */
+         pSocket->eSocket_State = phFriNfc_LlcpTransportSocket_eSocketAccepted;
 
-      /* Send a CC Frame */
-      result =  phFriNfc_LlcpTransport_LinkSend(psTransport,
+         /* Send a CC Frame */
+         result =  phFriNfc_LlcpTransport_LinkSend(psTransport,
                                    &pSocket->sLlcpHeader,
                                    NULL,
                                    &pSocket->sSocketSendBuffer,
                                    phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB,
+                                   pSocket->index,
                                    psTransport);
+
+         if (result != NFCSTATUS_SUCCESS && result != NFCSTATUS_PENDING) {
+            clearSendPending(psTransport);
+         }
+      }
    }
    /* CONNECT FRAME */
    else if(pSocket->bSocketConnectPending == TRUE)
    {
-      /* Reset Accept pending */
-      pSocket->bSocketConnectPending = FALSE;
-
-      /* Send Pending */
-      pSocket->psTransport->bSendPending = TRUE;
+      if (!testAndSetSendPending(psTransport))
+      {
+         /* Reset Accept pending */
+         pSocket->bSocketConnectPending = FALSE;
 
-      /* Set the socket in connecting state */
-      pSocket->eSocket_State = phFriNfc_LlcpTransportSocket_eSocketConnecting;
+         /* Set the socket in connecting state */
+         pSocket->eSocket_State = phFriNfc_LlcpTransportSocket_eSocketConnecting;
 
-      /* send CONNECT */
-      result =  phFriNfc_LlcpTransport_LinkSend(psTransport,
+         /* send CONNECT */
+         result =  phFriNfc_LlcpTransport_LinkSend(psTransport,
                                  &pSocket->sLlcpHeader,
                                  NULL,
                                  &pSocket->sSocketSendBuffer,
                                  phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB,
+                                 pSocket->index,
                                  psTransport);
+
+         if (result != NFCSTATUS_SUCCESS && result != NFCSTATUS_PENDING) {
+            clearSendPending(psTransport);
+         }
+      }
    }
    /* DISC FRAME */
    else if(pSocket->bSocketDiscPending == TRUE)
    {
-      /* Reset Disc Pending */
-      pSocket->bSocketDiscPending = FALSE;
-
-      /* Send Pending */
-      pSocket->psTransport->bSendPending = TRUE;
+      if (!testAndSetSendPending(psTransport))
+      {
+         /* Reset Disc Pending */
+         pSocket->bSocketDiscPending = FALSE;
 
-      /* Set the socket in connecting state */
-      pSocket->eSocket_State = phFriNfc_LlcpTransportSocket_eSocketDisconnecting;
+         /* Set the socket in connecting state */
+         pSocket->eSocket_State = phFriNfc_LlcpTransportSocket_eSocketDisconnecting;
 
-      /* Send DISC */
-      result =  phFriNfc_LlcpTransport_LinkSend(psTransport,
+         /* Send DISC */
+         result =  phFriNfc_LlcpTransport_LinkSend(psTransport,
                                    &pSocket->sLlcpHeader,
                                    NULL,
                                    &pSocket->sSocketSendBuffer,
                                    phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB,
+                                   pSocket->index,
                                    psTransport);
 
-      /* Call ErrCB due to a DISC */
-      pSocket->pSocketErrCb(pSocket->pContext, PHFRINFC_LLCP_ERR_DISCONNECTED);
+         if (result != NFCSTATUS_SUCCESS && result != NFCSTATUS_PENDING) {
+            clearSendPending(psTransport);
+         }
+         /* Call ErrCB due to a DISC */
+         pSocket->pSocketErrCb(pSocket->pContext, PHFRINFC_LLCP_ERR_DISCONNECTED);
+      }
    }
 
    return result;
@@ -230,9 +236,6 @@ static NFCSTATUS static_performSendInfo(phFriNfc_LlcpTransport_Socket_t * psLlcp
    phFriNfc_LlcpTransport_t   *psTransport = psLlcpSocket->psTransport;
    NFCSTATUS                  status;
 
-   /* Set transport send pending */
-   psTransport->bSendPending = TRUE;
-
    /* Set the Header */
    psLlcpSocket->sLlcpHeader.dsap   = psLlcpSocket->socket_dSap;
    psLlcpSocket->sLlcpHeader.ptype  = PHFRINFC_LLCP_PTYPE_I;
@@ -245,8 +248,6 @@ static NFCSTATUS static_performSendInfo(phFriNfc_LlcpTransport_Socket_t * psLlcp
    /* Update the VRA */
    psLlcpSocket->socket_VRA = psLlcpSocket->socket_VR;
 
-   /* Store the index of the socket */
-   psTransport->socketIndex = psLlcpSocket->index;
 
    /* Send I_PDU */
    status =  phFriNfc_LlcpTransport_LinkSend(psTransport,
@@ -254,13 +255,15 @@ static NFCSTATUS static_performSendInfo(phFriNfc_LlcpTransport_Socket_t * psLlcp
                                 &psLlcpSocket->sSequence,
                                 &psLlcpSocket->sSocketSendBuffer,
                                 phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB,
+                                psLlcpSocket->index,
                                 psLlcpSocket->psTransport);
+   if (status == NFCSTATUS_SUCCESS || status == NFCSTATUS_PENDING) {
+      /* Update VS */
+      psLlcpSocket->socket_VS = (psLlcpSocket->socket_VS+1)%16;
 
-   /* Update VS */
-   psLlcpSocket->socket_VS = (psLlcpSocket->socket_VS+1)%16;
-
-   /* Reset Send Pending */
-   psLlcpSocket->bSocketSendPending = FALSE;
+      /* Reset Send Pending */
+      psLlcpSocket->bSocketSendPending = FALSE;
+   }
 
    return status;
 }
@@ -309,16 +312,13 @@ static NFCSTATUS phFriNfc_Llcp_Send_ReceiveReady_Frame(phFriNfc_LlcpTransport_So
    NFCSTATUS   status = NFCSTATUS_SUCCESS;
 
    /* Test if a send is pending */
-   if(pLlcpSocket->psTransport->bSendPending == TRUE)
+   if(testAndSetSendPending(pLlcpSocket->psTransport))
    {
       pLlcpSocket->bSocketRRPending = TRUE;
       status = NFCSTATUS_PENDING;
    }
    else
    {
-      /* Set transport to pending */
-      pLlcpSocket->psTransport->bSendPending = TRUE;
-
       /* Set the header of the RR frame */
       pLlcpSocket->sLlcpHeader.dsap   = pLlcpSocket->socket_dSap;
       pLlcpSocket->sLlcpHeader.ptype  = PHFRINFC_LLCP_PTYPE_RR;
@@ -331,16 +331,17 @@ static NFCSTATUS phFriNfc_Llcp_Send_ReceiveReady_Frame(phFriNfc_LlcpTransport_So
       /* Update VRA */
       pLlcpSocket->socket_VRA = (uint8_t)pLlcpSocket->sSequence.nr;
 
-      /* Store the index of the socket */
-      pLlcpSocket->psTransport->socketIndex = pLlcpSocket->index;
-
       /* Send RR frame */
       status =  phFriNfc_LlcpTransport_LinkSend(pLlcpSocket->psTransport,
                                    &pLlcpSocket->sLlcpHeader,
                                    &pLlcpSocket->sSequence,
                                    NULL,
                                    phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB,
+                                   pLlcpSocket->index,
                                    pLlcpSocket->psTransport);
+      if (status != NFCSTATUS_SUCCESS && status != NFCSTATUS_PENDING) {
+         clearSendPending(pLlcpSocket->psTransport);
+      }
    }
 
    return status;
@@ -352,16 +353,13 @@ static NFCSTATUS phFriNfc_Llcp_Send_ReceiveNotReady_Frame(phFriNfc_LlcpTransport
 
 
    /* Test if a send is pending */
-   if(pLlcpSocket->psTransport->bSendPending == TRUE)
+   if(testAndSetSendPending(pLlcpSocket->psTransport))
    {
       pLlcpSocket->bSocketRNRPending = TRUE;
       status = NFCSTATUS_PENDING;
    }
    else
    {
-      /* Set transport to pending */
-      pLlcpSocket->psTransport->bSendPending = TRUE;
-
       /* Set the header of the RNR frame */
       pLlcpSocket->sLlcpHeader.dsap   = pLlcpSocket->socket_dSap;
       pLlcpSocket->sLlcpHeader.ptype  = PHFRINFC_LLCP_PTYPE_RNR;
@@ -374,16 +372,17 @@ static NFCSTATUS phFriNfc_Llcp_Send_ReceiveNotReady_Frame(phFriNfc_LlcpTransport
       /* Update VRA */
       pLlcpSocket->socket_VRA = (uint8_t)pLlcpSocket->sSequence.nr;
 
-      /* Store the index of the socket */
-      pLlcpSocket->psTransport->socketIndex = pLlcpSocket->index;
-
       /* Send RNR frame */
       status =  phFriNfc_LlcpTransport_LinkSend(pLlcpSocket->psTransport,
                                    &pLlcpSocket->sLlcpHeader,
                                    &pLlcpSocket->sSequence,
                                    NULL,
                                    phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB,
+                                   pLlcpSocket->index,
                                    pLlcpSocket->psTransport);
+      if (status != NFCSTATUS_SUCCESS && status != NFCSTATUS_PENDING) {
+         clearSendPending(pLlcpSocket->psTransport);
+      }
    }
    return status;
 }
@@ -566,7 +565,6 @@ static void Handle_ConnectionFrame(phFriNfc_LlcpTransport_t      *psTransport,
             socketFound = TRUE;
 
             psTransport->pSocketTable[index].index = index;
-            psTransport->socketIndex = psTransport->pSocketTable[index].index;
 
             /* Create a communication socket */
             pLlcpSocket = &psTransport->pSocketTable[index];
@@ -1043,9 +1041,12 @@ static void Handle_Receive_IFrame(phFriNfc_LlcpTransport_t      *psTransport,
                if(psLocalLlcpSocket->bSocketSendPending == TRUE && CHECK_SEND_RW(psLocalLlcpSocket))
                {
                   /* Test if a send is pending at LLC layer */
-                  if(psTransport->bSendPending != TRUE)
+                  if(!testAndSetSendPending(psLocalLlcpSocket->psTransport))
                   {
                      status = static_performSendInfo(psLocalLlcpSocket);
+                     if (status != NFCSTATUS_SUCCESS && status != NFCSTATUS_PENDING) {
+                         clearSendPending(psTransport);
+                     }
                   }
                }
                else
@@ -1119,9 +1120,12 @@ static void Handle_Receive_IFrame(phFriNfc_LlcpTransport_t      *psTransport,
               if((psLocalLlcpSocket->bSocketSendPending == TRUE) && CHECK_SEND_RW(psLocalLlcpSocket))
               {
                  /* Test if a send is pending at LLC layer */
-                 if(psTransport->bSendPending != TRUE)
+                 if(!testAndSetSendPending(psLocalLlcpSocket->psTransport))
                  {
                     status = static_performSendInfo(psLocalLlcpSocket);
+                    if (status != NFCSTATUS_SUCCESS && status != NFCSTATUS_PENDING) {
+                        clearSendPending(psTransport);
+                    }
                  }
               }
               else
@@ -1254,9 +1258,12 @@ static void Handle_ReceiveReady_Frame(phFriNfc_LlcpTransport_t      *psTransport
             if(CHECK_SEND_RW(psLocalLlcpSocket))
             {
                /* Test if a send is pending at LLC layer */
-               if(psTransport->bSendPending != TRUE)
+               if(!testAndSetSendPending(psLocalLlcpSocket->psTransport))
                {
                   status = static_performSendInfo(psLocalLlcpSocket);
+                  if (status != NFCSTATUS_SUCCESS && status != NFCSTATUS_PENDING) {
+                      clearSendPending(psTransport);
+                  }
                }
             }
          }
@@ -1363,9 +1370,12 @@ static void Handle_ReceiveNotReady_Frame(phFriNfc_LlcpTransport_t      *psTransp
          if(psLocalLlcpSocket->bSocketSendPending == TRUE && CHECK_SEND_RW(psLocalLlcpSocket))
          {
             /* Test if a send is pending at LLC layer */
-            if(psTransport->bSendPending != TRUE)
+            if(!testAndSetSendPending(psLocalLlcpSocket->psTransport))
             {
                status = static_performSendInfo(psLocalLlcpSocket);
+               if (status != NFCSTATUS_SUCCESS && status != NFCSTATUS_PENDING) {
+                   clearSendPending(psTransport);
+               }
             }
          }
       }
@@ -1654,7 +1664,6 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Accept(phFriNfc_LlcpTranspor
    uint32_t offset = 0;
    uint8_t miux[2];
    uint8_t  i;
-
    /* Store the options in the socket */
    memcpy(&pLlcpSocket->sSocketOption, psOptions, sizeof(phFriNfc_LlcpTransport_sSocketOptions_t));
 
@@ -1741,7 +1750,7 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Accept(phFriNfc_LlcpTranspor
 
 
    /* Test if a send is pending */
-   if(pLlcpSocket->psTransport->bSendPending == TRUE)
+   if(testAndSetSendPending(pLlcpSocket->psTransport))
    {
       pLlcpSocket->bSocketAcceptPending = TRUE;
 
@@ -1763,16 +1772,17 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Accept(phFriNfc_LlcpTranspor
       /* Update Send Buffer length value */
       pLlcpSocket->sSocketSendBuffer.length = offset;
 
-      /* Store the index of the socket */
-      pLlcpSocket->psTransport->socketIndex = pLlcpSocket->index;
-
       /* Send a CC Frame */
       status =  phFriNfc_LlcpTransport_LinkSend(pLlcpSocket->psTransport,
                                    &pLlcpSocket->sLlcpHeader,
                                    NULL,
                                    &pLlcpSocket->sSocketSendBuffer,
                                    phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB,
+                                   pLlcpSocket->index,
                                    pLlcpSocket->psTransport);
+      if (status != NFCSTATUS_SUCCESS && status != NFCSTATUS_PENDING) {
+          clearSendPending(pLlcpSocket->psTransport);
+      }
    }
 
 clean_and_return:
@@ -1814,9 +1824,6 @@ NFCSTATUS phLibNfc_LlcpTransport_ConnectionOriented_Reject( phFriNfc_LlcpTranspo
    pLlcpSocket->pfSocketSend_Cb = pReject_RspCb;
    pLlcpSocket->pRejectContext  = pContext;
 
-   /* Store the index of the socket */
-   pLlcpSocket->psTransport->socketIndex = pLlcpSocket->index;
-
    /* Send a DM*/
    status = phFriNfc_LlcpTransport_SendDisconnectMode(pLlcpSocket->psTransport,
                                                       pLlcpSocket->socket_dSap,
@@ -1857,7 +1864,6 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Connect( phFriNfc_LlcpTransp
                                                              void*                                      pContext)
 {
    NFCSTATUS status = NFCSTATUS_SUCCESS;
-
    uint32_t offset = 0;
    uint8_t miux[2];
 
@@ -1938,7 +1944,7 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Connect( phFriNfc_LlcpTransp
    }
 
    /* Test if a send is pending */
-   if(pLlcpSocket->psTransport->bSendPending == TRUE)
+   if(testAndSetSendPending(pLlcpSocket->psTransport))
    {
       pLlcpSocket->bSocketConnectPending =  TRUE;
 
@@ -1955,15 +1961,16 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Connect( phFriNfc_LlcpTransp
       /* Set the socket in connecting state */
       pLlcpSocket->eSocket_State = phFriNfc_LlcpTransportSocket_eSocketConnecting;
 
-      /* Store the index of the socket */
-      pLlcpSocket->psTransport->socketIndex = pLlcpSocket->index;
-
       status =  phFriNfc_LlcpTransport_LinkSend(pLlcpSocket->psTransport,
                                    &pLlcpSocket->sLlcpHeader,
                                    NULL,
                                    &pLlcpSocket->sSocketSendBuffer,
                                    phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB,
+                                   pLlcpSocket->index,
                                    pLlcpSocket->psTransport);
+      if (status != NFCSTATUS_SUCCESS && status != NFCSTATUS_PENDING) {
+          clearSendPending(pLlcpSocket->psTransport);
+      }
    }
 
 clean_and_return:
@@ -2052,24 +2059,23 @@ NFCSTATUS phLibNfc_LlcpTransport_ConnectionOriented_Disconnect(phFriNfc_LlcpTran
    pLlcpSocket->sLlcpHeader.ssap  = pLlcpSocket->socket_sSap;
 
    /* Test if a send is pending */
-   if( pLlcpSocket->psTransport->bSendPending == TRUE)
+   if( testAndSetSendPending(pLlcpSocket->psTransport))
    {
       pLlcpSocket->bSocketDiscPending =  TRUE;
       status = NFCSTATUS_PENDING;
    }
    else
    {
-      /* Store the index of the socket */
-      pLlcpSocket->psTransport->socketIndex = pLlcpSocket->index;
-
       status =  phFriNfc_LlcpTransport_LinkSend(pLlcpSocket->psTransport,
                                    &pLlcpSocket->sLlcpHeader,
                                    NULL,
                                    NULL,
                                    phFriNfc_LlcpTransport_ConnectionOriented_SendLlcp_CB,
+                                   pLlcpSocket->index,
                                    pLlcpSocket->psTransport);
-      if(status != NFCSTATUS_PENDING)
+      if(status != NFCSTATUS_SUCCESS && status != NFCSTATUS_PENDING)
       {
+         clearSendPending(pLlcpSocket->psTransport);
          LLCP_PRINT("Release Disconnect callback");
          pLlcpSocket->pfSocketConnect_Cb = NULL;
          pLlcpSocket->pConnectContext = NULL;
@@ -2243,7 +2249,7 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Send(phFriNfc_LlcpTransport_
       pLlcpSocket->pSendContext       = pContext;
 
       /* Test if a send is pending */
-      if(pLlcpSocket->psTransport->bSendPending == TRUE)
+      if(testAndSetSendPending(pLlcpSocket->psTransport))
       {
          /* Set Send pending */
          pLlcpSocket->bSocketSendPending = TRUE;
@@ -2259,8 +2265,9 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Send(phFriNfc_LlcpTransport_
 
          status = static_performSendInfo(pLlcpSocket);
 
-         if(status != NFCSTATUS_PENDING)
+         if(status != NFCSTATUS_SUCCESS && status != NFCSTATUS_PENDING)
          {
+            clearSendPending(pLlcpSocket->psTransport);
             LLCP_PRINT("Release Send callback");
             pLlcpSocket->pfSocketSend_Cb = NULL;
             pLlcpSocket->pSendContext = NULL;
diff --git a/src/phFriNfc_LlcpTransport_Connectionless.c b/src/phFriNfc_LlcpTransport_Connectionless.c
index b27f6de..99f01bc 100644
--- a/src/phFriNfc_LlcpTransport_Connectionless.c
+++ b/src/phFriNfc_LlcpTransport_Connectionless.c
@@ -30,6 +30,7 @@
 #include <phFriNfc_Llcp.h>
 
 static void phFriNfc_LlcpTransport_Connectionless_SendTo_CB(void*        pContext,
+                                                            uint8_t      socketIndex,
                                                             NFCSTATUS    status);
 
 NFCSTATUS phFriNfc_LlcpTransport_Connectionless_HandlePendingOperations(phFriNfc_LlcpTransport_Socket_t *pSocket)
@@ -51,6 +52,7 @@ NFCSTATUS phFriNfc_LlcpTransport_Connectionless_HandlePendingOperations(phFriNfc
                                    NULL,
                                    &pSocket->sSocketSendBuffer,
                                    phFriNfc_LlcpTransport_Connectionless_SendTo_CB,
+                                   pSocket->index,
                                    pSocket);
    }
    else
@@ -125,6 +127,7 @@ void Handle_Connectionless_IncommingFrame(phFriNfc_LlcpTransport_t      *pLlcpTr
 
 /* TODO: comment function phFriNfc_LlcpTransport_Connectionless_SendTo_CB */
 static void phFriNfc_LlcpTransport_Connectionless_SendTo_CB(void*        pContext,
+                                                            uint8_t      socketIndex,
                                                             NFCSTATUS    status)
 {
    phFriNfc_LlcpTransport_Socket_t *         pLlcpSocket = (phFriNfc_LlcpTransport_Socket_t*)pContext;
@@ -271,6 +274,7 @@ NFCSTATUS phFriNfc_LlcpTransport_Connectionless_SendTo(phFriNfc_LlcpTransport_So
                                    NULL,
                                    psBuffer,
                                    phFriNfc_LlcpTransport_Connectionless_SendTo_CB,
+                                   pLlcpSocket->index,
                                    pLlcpSocket);
    }