LLCP 1.1 implementation.

Previously, in LLCP v1.0, the only way to use the SDP (Service
Discovery Protocol) service was to send a CONNECT frame containing
the Service Name to the SDP service. This was implicitly
preforming a connection request to the requested service.

LLCP v1.1 introduced a way to discover services more efficiently
and without implicit connection. It also enables connectionless
services to have a service name. It is based on a new protocol
based on a new SNL frame containing discovery requests and
responses.

This update comes with new APIs:
- phLibNfc_Llcp_DiscoverServices() function can be used to discover
remote services. It can take multiple service names and resolve
all of them in a single call.

- Register service name at bind time. Cache LLCP service name/sap pairs.
In LLCP 1.1 specification defines at section 5.9 that any service
lookup answer must be valid for the whole LLCP session duration.
To enforce this, we cache the SAP/SN pairs locally and make sure
that the applications don't break the cache.

The stack remains fully retro-compatible with v1.0 devices.

Change-Id: I052edd3838013cee65e7415d0ed01fc3e9cad36d

11 files changed, 912 insertions, 108 deletions

diff --git a/inc/phNfcLlcpTypes.h b/inc/phNfcLlcpTypes.h
index 2954d00..fe25b7d 100644
--- a/inc/phNfcLlcpTypes.h
+++ b/inc/phNfcLlcpTypes.h
@@ -56,6 +56,7 @@
  */
  /*@{*/
 #define PHFRINFC_LLCP_NB_SOCKET_MAX          10                                /**< Max.number of simultaneous sockets */
+#define PHFRINFC_LLCP_SNL_RESPONSE_MAX       256                               /**< Max.number of simultaneous discovery requests */
 /*@}*/
 
 /**
diff --git a/src/phFriNfc_Llcp.h b/src/phFriNfc_Llcp.h
index 287f9f9..fb26d6a 100644
--- a/src/phFriNfc_Llcp.h
+++ b/src/phFriNfc_Llcp.h
@@ -83,7 +83,7 @@ extern char phOsalNfc_DbgTraceBuffer[];
  */
  /*@{*/
 #define PHFRINFC_LLCP_VERSION_MAJOR   0x01  /**< Major number of local LLCP version.*/
-#define PHFRINFC_LLCP_VERSION_MINOR   0x00  /**< Minor number of local LLCP version.*/
+#define PHFRINFC_LLCP_VERSION_MINOR   0x01  /**< Minor number of local LLCP version.*/
 #define PHFRINFC_LLCP_VERSION         ((PHFRINFC_LLCP_VERSION_MAJOR << 4) | PHFRINFC_LLCP_VERSION_MINOR) /**< Local LLCP version.*/
 /*@}*/
 
@@ -97,17 +97,17 @@ extern char phOsalNfc_DbgTraceBuffer[];
 #define PHFRINFC_LLCP_PTYPE_AGF        0x02 /**< AGgregated Frame.*/
 #define PHFRINFC_LLCP_PTYPE_UI         0x03 /**< Unnumbered Information.*/
 #define PHFRINFC_LLCP_PTYPE_CONNECT    0x04 /**< Connect.*/
-#define PHFRINFC_LLCP_PTYPE_DISC       0x05 /**< Didconnect.*/
+#define PHFRINFC_LLCP_PTYPE_DISC       0x05 /**< Disconnect.*/
 #define PHFRINFC_LLCP_PTYPE_CC         0x06 /**< Connection Complete.*/
 #define PHFRINFC_LLCP_PTYPE_DM         0x07 /**< Disconnected Mode.*/
 #define PHFRINFC_LLCP_PTYPE_FRMR       0x08 /**< FRaMe Reject.*/
-#define PHFRINFC_LLCP_PTYPE_RESERVED1  0x09 /**< Reserved.*/
-#define PHFRINFC_LLCP_PTYPE_RESERVED2  0x0A /**< Reserved.*/
-#define PHFRINFC_LLCP_PTYPE_RESERVED3  0x0B /**< Reserved.*/
+#define PHFRINFC_LLCP_PTYPE_SNL        0x09 /**< Service Name Lookup.*/
+#define PHFRINFC_LLCP_PTYPE_RESERVED1  0x0A /**< Reserved.*/
+#define PHFRINFC_LLCP_PTYPE_RESERVED2  0x0B /**< Reserved.*/
 #define PHFRINFC_LLCP_PTYPE_I          0x0C /**< Information.*/
 #define PHFRINFC_LLCP_PTYPE_RR         0x0D /**< Receive Ready.*/
 #define PHFRINFC_LLCP_PTYPE_RNR        0x0E /**< Receive Not Ready.*/
-#define PHFRINFC_LLCP_PTYPE_RESERVED4  0x0F /**< Reserved.*/
+#define PHFRINFC_LLCP_PTYPE_RESERVED3  0x0F /**< Reserved.*/
 /*@}*/
 
 /**
@@ -122,6 +122,15 @@ extern char phOsalNfc_DbgTraceBuffer[];
 #define PHFRINFC_LLCP_SAP_SDP_UNADVERTISED_FIRST   0x20 /**< First SAP number from SDP-unavertised SAP range.*/
 #define PHFRINFC_LLCP_SAP_NUMBER                   0x40 /**< Number of possible SAP values (also first invalid value).*/
 #define PHFRINFC_LLCP_SAP_DEFAULT                  0xFF /**< Default number when a socket is created or reset */
+#define PHFRINFC_LLCP_SDP_ADVERTISED_NB            0x10 /**< Number of SDP advertised SAP slots */
+/*@}*/
+
+/**
+ *  \name LLCP well-known SAPs.
+ *
+ */
+ /*@{*/
+#define PHFRINFC_LLCP_SERVICENAME_SDP              "urn:nfc:sn:sdp" /**< Service Discovery Protocol name.*/
 /*@}*/
 
 /**
@@ -158,6 +167,8 @@ extern char phOsalNfc_DbgTraceBuffer[];
 #define PHFRINFC_LLCP_TLV_TYPE_RW                    0x05   /**< \internal RW parameter Type code.*/
 #define PHFRINFC_LLCP_TLV_TYPE_SN                    0x06   /**< \internal SN parameter Type code.*/
 #define PHFRINFC_LLCP_TLV_TYPE_OPT                   0x07   /**< \internal OPT parameter Type code.*/
+#define PHFRINFC_LLCP_TLV_TYPE_SDREQ                 0x08   /**< \internal SDREQ parameter Type code.*/
+#define PHFRINFC_LLCP_TLV_TYPE_SDRES                 0x09   /**< \internal SDRES parameter Type code.*/
 /*@}*/
 
 /**
diff --git a/src/phFriNfc_LlcpTransport.c b/src/phFriNfc_LlcpTransport.c
index a7e195b..38c6807 100644
--- a/src/phFriNfc_LlcpTransport.c
+++ b/src/phFriNfc_LlcpTransport.c
@@ -37,15 +37,50 @@
 /* Check if (a <= x < b) */
 #define IS_BETWEEN(x, a, b) (((x)>=(a)) && ((x)<(b)))
 
+static NFCSTATUS phFriNfc_LlcpTransport_RegisterName(phFriNfc_LlcpTransport_Socket_t*   pLlcpSocket,
+                                                     uint8_t                            nSap,
+                                                     phNfc_sData_t                      *psServiceName);
 
-static NFCSTATUS phFriNfc_LlcpTransport_AutoBind(phFriNfc_LlcpTransport_Socket_t *pSocket)
+static NFCSTATUS phFriNfc_LlcpTransport_DiscoverServicesEx(phFriNfc_LlcpTransport_t *psTransport);
+
+static void phFriNfc_LlcpTransport_Send_CB(void            *pContext,
+                                           NFCSTATUS        status);
+
+static NFCSTATUS phFriNfc_LlcpTransport_GetFreeSap(phFriNfc_LlcpTransport_t * psTransport, phNfc_sData_t *psServiceName, uint8_t * pnSap)
 {
    uint8_t i;
    uint8_t sap;
-   phFriNfc_LlcpTransport_Socket_t* pSocketTable = pSocket->psTransport->pSocketTable;
+   uint8_t min_sap_range, max_sap_range;
+   phFriNfc_LlcpTransport_Socket_t* pSocketTable = psTransport->pSocketTable;
+
+   /* Calculate authorized SAP range */
+   if ((psServiceName != NULL) && (psServiceName->length > 0))
+   {
+      /* Make sure that we will return the same SAP if service name was already used in the past */
+      for(i=0 ; i<PHFRINFC_LLCP_SDP_ADVERTISED_NB ; i++)
+      {
+         if((psTransport->pCachedServiceNames[i].sServiceName.length > 0) &&
+            (memcmp(psTransport->pCachedServiceNames[i].sServiceName.buffer, psServiceName->buffer, psServiceName->length) == 0))
+         {
+            /* Service name matched in cached service names list */
+            *pnSap = psTransport->pCachedServiceNames[i].nSap;
+            return NFCSTATUS_SUCCESS;
+         }
+      }
+
+      /* SDP advertised service */
+      min_sap_range = PHFRINFC_LLCP_SAP_SDP_ADVERTISED_FIRST;
+      max_sap_range = PHFRINFC_LLCP_SAP_SDP_UNADVERTISED_FIRST;
+   }
+   else
+   {
+      /* Non-SDP advertised service */
+      min_sap_range = PHFRINFC_LLCP_SAP_SDP_UNADVERTISED_FIRST;
+      max_sap_range = PHFRINFC_LLCP_SAP_NUMBER;
+   }
 
    /* Try all possible SAPs */
-   for(sap=PHFRINFC_LLCP_SAP_SDP_UNADVERTISED_FIRST ; sap<PHFRINFC_LLCP_SAP_NUMBER ; sap++)
+   for(sap=min_sap_range ; sap<max_sap_range ; sap++)
    {
       /* Go through socket list to check if current SAP is in use */
       for(i=0 ; i<PHFRINFC_LLCP_NB_SOCKET_MAX ; i++)
@@ -61,8 +96,7 @@ static NFCSTATUS phFriNfc_LlcpTransport_AutoBind(phFriNfc_LlcpTransport_Socket_t
       if (i >= PHFRINFC_LLCP_NB_SOCKET_MAX)
       {
          /* No socket is using current SAP, proceed with binding */
-         pSocket->socket_sSap = sap;
-         pSocket->eSocket_State = phFriNfc_LlcpTransportSocket_eSocketBound;
+         *pnSap = sap;
          return NFCSTATUS_SUCCESS;
       }
    }
@@ -71,6 +105,343 @@ static NFCSTATUS phFriNfc_LlcpTransport_AutoBind(phFriNfc_LlcpTransport_Socket_t
    return NFCSTATUS_INSUFFICIENT_RESOURCES;
 }
 
+static NFCSTATUS phFriNfc_LlcpTransport_EncodeSdreqTlv(phNfc_sData_t  *psTlvData,
+                                                       uint32_t       *pOffset,
+                                                       uint8_t        nTid,
+                                                       phNfc_sData_t  *psServiceName)
+{
+   NFCSTATUS result;
+   uint32_t nTlvOffset = *pOffset;
+   uint32_t nTlvStartOffset = nTlvOffset;
+
+   /* Encode the TID */
+   result = phFriNfc_Llcp_EncodeTLV(psTlvData,
+                                    &nTlvOffset,
+                                    PHFRINFC_LLCP_TLV_TYPE_SDREQ,
+                                    1,
+                                    &nTid);
+   if (result != NFCSTATUS_SUCCESS)
+   {
+      goto clean_and_return;
+   }
+
+   /* Encode the service name itself */
+   result = phFriNfc_Llcp_AppendTLV(psTlvData,
+                                    nTlvStartOffset,
+                                    &nTlvOffset,
+                                    psServiceName->length,
+                                    psServiceName->buffer);
+   if (result != NFCSTATUS_SUCCESS)
+   {
+      goto clean_and_return;
+   }
+
+clean_and_return:
+   /* Save offset if no error occured */
+   if (result == NFCSTATUS_SUCCESS)
+   {
+      *pOffset = nTlvOffset;
+   }
+
+   return result;
+}
+
+static NFCSTATUS phFriNfc_LlcpTransport_EncodeSdresTlv(phNfc_sData_t  *psTlvData,
+                                                       uint32_t       *pOffset,
+                                                       uint8_t        nTid,
+                                                       uint8_t        nSap)
+{
+   NFCSTATUS result;
+   uint32_t nTlvStartOffset = *pOffset;
+
+   /* Encode the TID */
+   result = phFriNfc_Llcp_EncodeTLV(psTlvData,
+                                    pOffset,
+                                    PHFRINFC_LLCP_TLV_TYPE_SDRES,
+                                    1,
+                                    &nTid);
+   if (result != NFCSTATUS_SUCCESS)
+   {
+      goto clean_and_return;
+   }
+
+   /* Encode the service name itself */
+   result = phFriNfc_Llcp_AppendTLV(psTlvData,
+                                    nTlvStartOffset,
+                                    pOffset,
+                                    1,
+                                    &nSap);
+   if (result != NFCSTATUS_SUCCESS)
+   {
+      goto clean_and_return;
+   }
+
+clean_and_return:
+   /* Restore previous offset if an error occured */
+   if (result != NFCSTATUS_SUCCESS)
+   {
+      *pOffset = nTlvStartOffset;
+   }
+
+   return result;
+}
+
+static phFriNfc_LlcpTransport_Socket_t* phFriNfc_LlcpTransport_ServiceNameLoockup(phFriNfc_LlcpTransport_t *psTransport,
+                                                                                  phNfc_sData_t            *pServiceName)
+{
+   uint32_t                            index;
+   uint8_t                             cacheIndex;
+   phFriNfc_Llcp_CachedServiceName_t * pCachedServiceName;
+   phFriNfc_LlcpTransport_Socket_t *   pSocket;
+
+   /* Search a socket with the SN */
+   for(index=0;index<PHFRINFC_LLCP_NB_SOCKET_MAX;index++)
+   {
+      pSocket = &psTransport->pSocketTable[index];
+      /* Test if the CO socket is in Listen state or the CL socket is bound
+         and if its SN is the good one */
+      if((((pSocket->eSocket_Type == phFriNfc_LlcpTransport_eConnectionOriented)
+         && (pSocket->eSocket_State == phFriNfc_LlcpTransportSocket_eSocketRegistered))
+         || ((pSocket->eSocket_Type == phFriNfc_LlcpTransport_eConnectionLess)
+         && (pSocket->eSocket_State == phFriNfc_LlcpTransportSocket_eSocketBound)))
+         &&
+         (pServiceName->length == pSocket->sServiceName.length)
+         && !memcmp(pServiceName->buffer, pSocket->sServiceName.buffer, pServiceName->length))
+      {
+         /* Add new entry to cached service name/sap if not already in table */
+         for(cacheIndex=0;cacheIndex<PHFRINFC_LLCP_SDP_ADVERTISED_NB;cacheIndex++)
+         {
+            pCachedServiceName = &psTransport->pCachedServiceNames[cacheIndex];
+            if (pCachedServiceName->sServiceName.buffer != NULL)
+            {
+               if ((pCachedServiceName->sServiceName.length == pServiceName->length) &&
+                   (memcmp(pCachedServiceName->sServiceName.buffer, pServiceName->buffer, pServiceName->length) == 0))
+               {
+                  /* Already registered */
+                  break;
+               }
+            }
+            else
+            {
+               /* Reached end of existing entries and not found the service name,
+                * => Add the new entry
+                */
+               pCachedServiceName->nSap = pSocket->socket_sSap;
+               pCachedServiceName->sServiceName.buffer = phOsalNfc_GetMemory(pServiceName->length);
+               if (pCachedServiceName->sServiceName.buffer == NULL)
+               {
+                  /* Unable to cache this entry, so report this service as not found */
+                  return NULL;
+               }
+               memcpy(pCachedServiceName->sServiceName.buffer, pServiceName->buffer, pServiceName->length);
+               pCachedServiceName->sServiceName.length = pServiceName->length;
+               break;
+            }
+         }
+
+         return pSocket;
+      }
+   }
+
+   return NULL;
+}
+
+
+static NFCSTATUS phFriNfc_LlcpTransport_DiscoveryAnswer(phFriNfc_LlcpTransport_t *psTransport)
+{
+   NFCSTATUS         result = NFCSTATUS_PENDING;
+   phNfc_sData_t     sInfoBuffer;
+   uint32_t          nTlvOffset;
+   uint8_t           index;
+   uint8_t           nTid, nSap;
+
+   /* Test if a send is pending */
+   if(!psTransport->bSendPending)
+   {
+      /* Set the header */
+      psTransport->sLlcpHeader.dsap  = PHFRINFC_LLCP_SAP_SDP;
+      psTransport->sLlcpHeader.ptype = PHFRINFC_LLCP_PTYPE_SNL;
+      psTransport->sLlcpHeader.ssap  = PHFRINFC_LLCP_SAP_SDP;
+
+      /* Prepare the info buffer */
+      sInfoBuffer.buffer = psTransport->pDiscoveryBuffer;
+      sInfoBuffer.length = sizeof(psTransport->pDiscoveryBuffer);
+
+      /* Encode as many requests as possible */
+      nTlvOffset = 0;
+      for(index=0 ; index<psTransport->nDiscoveryResListSize ; index++)
+      {
+         /* Get current TID/SAP and try to encode them in SNL frame */
+         nTid = psTransport->nDiscoveryResTidList[index];
+         nSap = psTransport->nDiscoveryResSapList[index];
+         /* Encode response */
+         result = phFriNfc_LlcpTransport_EncodeSdresTlv(&sInfoBuffer,
+                                                        &nTlvOffset,
+                                                        nTid,
+                                                        nSap);
+         if (result != NFCSTATUS_SUCCESS)
+         {
+            /* Impossible to fit the entire response */
+            /* TODO: support reponse framgentation */
+            break;
+         }
+      }
+
+      /* Reset list size to be able to handle a new request */
+      psTransport->nDiscoveryResListSize = 0;
+
+      /* 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,
+                                   NULL,
+                                   &sInfoBuffer,
+                                   phFriNfc_LlcpTransport_Send_CB,
+                                   psTransport);
+   }
+   else
+   {
+      /* Impossible to send now, this function will be called again on next opportunity */
+   }
+
+   return result;
+}
+
+
+static void Handle_Discovery_IncomingFrame(phFriNfc_LlcpTransport_t           *psTransport,
+                                           phNfc_sData_t                      *psData)
+{
+   NFCSTATUS                        result;
+   phNfc_sData_t                    sValue;
+   phNfc_sData_t                    sResponseData;
+   phNfc_sData_t                    sServiceName;
+   uint32_t                         nInTlvOffset;
+   uint8_t                          nType;
+   uint8_t                          nTid;
+   uint8_t                          nSap;
+   pphFriNfc_Cr_t                   pfSavedCb;
+   void                             *pfSavedContext;
+   phFriNfc_LlcpTransport_Socket_t  *pSocket;
+
+
+   /* Prepare buffer */
+   sResponseData.buffer = psTransport->pDiscoveryBuffer;
+   sResponseData.length = sizeof(psTransport->pDiscoveryBuffer);
+
+   /* Parse all TLVs in frame */
+   nInTlvOffset = 0;
+   while(nInTlvOffset < psData->length)
+   {
+      result = phFriNfc_Llcp_DecodeTLV(psData,
+                                       &nInTlvOffset,
+                                       &nType,
+                                       &sValue );
+      switch(nType)
+      {
+         case PHFRINFC_LLCP_TLV_TYPE_SDREQ:
+            if (sValue.length < 2)
+            {
+               /* Erroneous request, ignore */
+               break;
+            }
+            /* Decode TID */
+            nTid = sValue.buffer[0];
+            /* Decode service name */
+            sServiceName.buffer = sValue.buffer + 1;
+            sServiceName.length = sValue.length - 1;
+
+            /* Handle SDP service name */
+            if((sServiceName.length == sizeof(PHFRINFC_LLCP_SERVICENAME_SDP)-1)
+               && !memcmp(sServiceName.buffer, PHFRINFC_LLCP_SERVICENAME_SDP, sServiceName.length))
+            {
+               nSap = PHFRINFC_LLCP_SAP_SDP;
+            }
+            else
+            {
+               /* Match service name in socket list */
+               pSocket = phFriNfc_LlcpTransport_ServiceNameLoockup(psTransport, &sServiceName);
+               if (pSocket != NULL)
+               {
+                  nSap = pSocket->socket_sSap;
+               }
+               else
+               {
+                  nSap = 0;
+               }
+            }
+
+            /* Encode response */
+            if (psTransport->nDiscoveryResListSize < PHFRINFC_LLCP_SNL_RESPONSE_MAX)
+            {
+               psTransport->nDiscoveryResSapList[psTransport->nDiscoveryResListSize] = nSap;
+               psTransport->nDiscoveryResTidList[psTransport->nDiscoveryResListSize] = nTid;
+               psTransport->nDiscoveryResListSize++;
+            }
+            else
+            {
+               /* Remote peer is sending more than max. allowed requests (max. 256
+                  different TID values), drop invalid requests to avoid buffer overflow
+               */
+            }
+            break;
+
+         case PHFRINFC_LLCP_TLV_TYPE_SDRES:
+            if (psTransport->pfDiscover_Cb == NULL)
+            {
+               /* Ignore response when no requests are pending */
+               break;
+            }
+            if (sValue.length != 2)
+            {
+               /* Erroneous response, ignore it */
+               break;
+            }
+            /* Decode TID and SAP */
+            nTid = sValue.buffer[0];
+            if (nTid >= psTransport->nDiscoveryListSize)
+            {
+               /* Unkown TID, ignore it */
+               break;
+            }
+            nSap = sValue.buffer[1];
+            /* Save response */
+            psTransport->pnDiscoverySapList[nTid] = nSap;
+            /* Update response counter */
+            psTransport->nDiscoveryResOffset++;
+            break;
+
+         default:
+            /* Ignored */
+            break;
+      }
+   }
+
+   /* If discovery requests have been received, send response */
+   if (psTransport->nDiscoveryResListSize > 0)
+   {
+      phFriNfc_LlcpTransport_DiscoveryAnswer(psTransport);
+   }
+
+   /* If all discovery responses have been received, trigger callback (if any) */
+   if ((psTransport->pfDiscover_Cb != NULL) &&
+       (psTransport->nDiscoveryResOffset >= psTransport->nDiscoveryListSize))
+   {
+      pfSavedCb = psTransport->pfDiscover_Cb;
+      pfSavedContext = psTransport->pDiscoverContext;
+
+      psTransport->pfDiscover_Cb = NULL;
+      psTransport->pDiscoverContext = NULL;
+
+      pfSavedCb(pfSavedContext, NFCSTATUS_SUCCESS);
+   }
+}
+
+
 /* TODO: comment function Transport recv CB */
 static void phFriNfc_LlcpTransport__Recv_CB(void            *pContext,
                                             phNfc_sData_t   *psData,
@@ -112,6 +483,20 @@ static void phFriNfc_LlcpTransport__Recv_CB(void            *pContext,
                                                  ssap);
          }break;
 
+      /* Service Discovery Protocol */
+      case PHFRINFC_LLCP_PTYPE_SNL:
+         {
+            if ((ssap == PHFRINFC_LLCP_SAP_SDP) && (dsap == PHFRINFC_LLCP_SAP_SDP))
+            {
+               Handle_Discovery_IncomingFrame(pLlcpTransport,
+                                              psData);
+            }
+            else
+            {
+               /* Ignore frame if source and destination are not the SDP service */
+            }
+         }break;
+
       /* Connection oriented */
       /* NOTE: forward reserved PTYPE to enable FRMR sending */
       case PHFRINFC_LLCP_PTYPE_CONNECT:
@@ -125,7 +510,6 @@ static void phFriNfc_LlcpTransport__Recv_CB(void            *pContext,
       case PHFRINFC_LLCP_PTYPE_RESERVED1:
       case PHFRINFC_LLCP_PTYPE_RESERVED2:
       case PHFRINFC_LLCP_PTYPE_RESERVED3:
-      case PHFRINFC_LLCP_PTYPE_RESERVED4:
          {
             Handle_ConnectionOriented_IncommingFrame(pLlcpTransport,
                                                      psData,
@@ -212,6 +596,17 @@ static void phFriNfc_LlcpTransport_Send_CB(void            *pContext,
 
    /* 4 - Handle pending send operations */
 
+   /* Check for pending discovery requests/responses */
+   if (psTransport->nDiscoveryResListSize > 0)
+   {
+      phFriNfc_LlcpTransport_DiscoveryAnswer(psTransport);
+   }
+   if ( (psTransport->pfDiscover_Cb != NULL) &&
+        (psTransport->nDiscoveryReqOffset < psTransport->nDiscoveryListSize) )
+   {
+      result = phFriNfc_LlcpTransport_DiscoverServicesEx(psTransport);
+   }
+
    /* Init index */
    index = psTransport->socketIndex;
 
@@ -269,7 +664,10 @@ NFCSTATUS phFriNfc_LlcpTransport_Reset (phFriNfc_LlcpTransport_t      *pLlcpTran
       pLlcpTransport->bFrmrPending     = FALSE;
       pLlcpTransport->socketIndex      = FALSE;
       pLlcpTransport->LinkStatusError  = 0;
+      pLlcpTransport->pfDiscover_Cb    = NULL;
 
+      /* Initialize cached service name/sap table */
+      memset(pLlcpTransport->pCachedServiceNames, 0x00, sizeof(phFriNfc_Llcp_CachedServiceName_t)*PHFRINFC_LLCP_SDP_ADVERTISED_NB);
 
       /* Reset all the socket info in the table */
       for(i=0;i<PHFRINFC_LLCP_NB_SOCKET_MAX;i++)
@@ -340,8 +738,9 @@ NFCSTATUS phFriNfc_LlcpTransport_Reset (phFriNfc_LlcpTransport_t      *pLlcpTran
 /* TODO: comment function Transport CloseAll */
 NFCSTATUS phFriNfc_LlcpTransport_CloseAll (phFriNfc_LlcpTransport_t *pLlcpTransport)
 {
-   NFCSTATUS status = NFCSTATUS_SUCCESS;
-   uint8_t i;
+   NFCSTATUS                           status = NFCSTATUS_SUCCESS;
+   phFriNfc_Llcp_CachedServiceName_t * pCachedServiceName;
+   uint8_t                             i;
 
    /* Check for NULL pointers */
    if(pLlcpTransport == NULL)
@@ -374,6 +773,21 @@ NFCSTATUS phFriNfc_LlcpTransport_CloseAll (phFriNfc_LlcpTransport_t *pLlcpTransp
          phFriNfc_LlcpTransport_Close(&pLlcpTransport->pSocketTable[i]);
       }
    }
+
+   /* Reset cached service name/sap table */
+   for(i=0;i<PHFRINFC_LLCP_SDP_ADVERTISED_NB;i++)
+   {
+      pCachedServiceName = &pLlcpTransport->pCachedServiceNames[i];
+
+      pCachedServiceName->nSap = 0;
+      if (pCachedServiceName->sServiceName.buffer != NULL)
+      {
+         phOsalNfc_FreeMemory(pCachedServiceName->sServiceName.buffer);
+         pCachedServiceName->sServiceName.buffer = NULL;
+      }
+      pCachedServiceName->sServiceName.length = 0;
+   }
+
    return status;
 }
 
@@ -647,6 +1061,101 @@ NFCSTATUS phFriNfc_LlcpTransport_SocketGetRemoteOptions(phFriNfc_LlcpTransport_S
    return status;
 }
 
+
+static NFCSTATUS phFriNfc_LlcpTransport_DiscoverServicesEx(phFriNfc_LlcpTransport_t *psTransport)
+{
+   NFCSTATUS         result = NFCSTATUS_PENDING;
+   phNfc_sData_t     sInfoBuffer;
+   phNfc_sData_t     *psServiceName;
+   uint32_t          nTlvOffset;
+
+   /* Test if a send is pending */
+   if(!psTransport->bSendPending)
+   {
+      /* Set the header */
+      psTransport->sLlcpHeader.dsap  = PHFRINFC_LLCP_SAP_SDP;
+      psTransport->sLlcpHeader.ptype = PHFRINFC_LLCP_PTYPE_SNL;
+      psTransport->sLlcpHeader.ssap  = PHFRINFC_LLCP_SAP_SDP;
+
+      /* Prepare the info buffer */
+      sInfoBuffer.buffer = psTransport->pDiscoveryBuffer;
+      sInfoBuffer.length = sizeof(psTransport->pDiscoveryBuffer);
+
+      /* Encode as many requests as possible */
+      nTlvOffset = 0;
+      while(psTransport->nDiscoveryReqOffset < psTransport->nDiscoveryListSize)
+      {
+         /* Get current service name and try to encode it in SNL frame */
+         psServiceName = &psTransport->psDiscoveryServiceNameList[psTransport->nDiscoveryReqOffset];
+         result = phFriNfc_LlcpTransport_EncodeSdreqTlv(&sInfoBuffer,
+                                                        &nTlvOffset,
+                                                        psTransport->nDiscoveryReqOffset,
+                                                        psServiceName);
+         if (result != NFCSTATUS_SUCCESS)
+         {
+            /* Impossible to fit more requests in a single frame,
+             * will be continued on next opportunity
+             */
+            break;
+         }
+
+         /* Update request counter */
+         psTransport->nDiscoveryReqOffset++;
+      }
+
+      /* 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,
+                                   NULL,
+                                   &sInfoBuffer,
+                                   phFriNfc_LlcpTransport_Send_CB,
+                                   psTransport);
+   }
+   else
+   {
+      /* Impossible to send now, this function will be called again on next opportunity */
+   }
+
+   return result;
+}
+
+/*!
+* \ingroup grp_fri_nfc
+* \brief <b>Discover remote services SAP using SDP protocol</b>.
+ */
+NFCSTATUS phFriNfc_LlcpTransport_DiscoverServices( phFriNfc_LlcpTransport_t  *pLlcpTransport,
+                                                   phNfc_sData_t             *psServiceNameList,
+                                                   uint8_t                   *pnSapList,
+                                                   uint8_t                   nListSize,
+                                                   pphFriNfc_Cr_t            pDiscover_Cb,
+                                                   void                      *pContext )
+{
+   NFCSTATUS         result = NFCSTATUS_FAILED;
+
+   /* Save request details */
+   pLlcpTransport->psDiscoveryServiceNameList = psServiceNameList;
+   pLlcpTransport->pnDiscoverySapList = pnSapList;
+   pLlcpTransport->nDiscoveryListSize = nListSize;
+   pLlcpTransport->pfDiscover_Cb = pDiscover_Cb;
+   pLlcpTransport->pDiscoverContext = pContext;
+
+   /* Reset internal counters */
+   pLlcpTransport->nDiscoveryReqOffset = 0;
+   pLlcpTransport->nDiscoveryResOffset = 0;
+
+   /* Perform request */
+   result = phFriNfc_LlcpTransport_DiscoverServicesEx(pLlcpTransport);
+
+   return result;
+}
+
+
  /**
 * \ingroup grp_fri_nfc
 * \brief <b>Create a socket on a LLCP-connected device</b>.
@@ -841,8 +1350,9 @@ NFCSTATUS phFriNfc_LlcpTransport_Close(phFriNfc_LlcpTransport_Socket_t*   pLlcpS
 *
 * This function binds the socket to a local Service Access Point.
 *
-* \param[out] pLlcpSocket           A pointer to a phFriNfc_LlcpTransport_Socket_t.
-* \param[in]  pConfigInfo           A port number for a specific socket
+* \param[in]  pLlcpSocket           A pointer to a phFriNfc_LlcpTransport_Socket_t.
+* \param[in]  nSap                  The SAP number to bind with, or 0 for auto-bind to a free SAP.
+* \param[in]  psServiceName         A pointer to Service Name, or NULL if no service name.
 *
 * \retval NFCSTATUS_SUCCESS                  Operation successful.
 * \retval NFCSTATUS_INVALID_PARAMETER        One or more of the supplied parameters
@@ -855,10 +1365,13 @@ NFCSTATUS phFriNfc_LlcpTransport_Close(phFriNfc_LlcpTransport_Socket_t*   pLlcpS
 */
 
 NFCSTATUS phFriNfc_LlcpTransport_Bind(phFriNfc_LlcpTransport_Socket_t    *pLlcpSocket,
-                                      uint8_t                            nSap)
+                                      uint8_t                            nSap,
+                                      phNfc_sData_t                      *psServiceName)
 {
    NFCSTATUS status = NFCSTATUS_SUCCESS;
    uint8_t i;
+   uint8_t min_sap_range;
+   uint8_t max_sap_range;
 
    /* Check for NULL pointers */
    if(pLlcpSocket == NULL)
@@ -869,25 +1382,59 @@ NFCSTATUS phFriNfc_LlcpTransport_Bind(phFriNfc_LlcpTransport_Socket_t    *pLlcpS
    {
       status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_STATE);
    }
-   else if(nSap<2 || nSap>63)
-   {
-      status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
-   }
    else
    {
-      /* Test if the nSap it is useb by another socket */
-      for(i=0;i<PHFRINFC_LLCP_NB_SOCKET_MAX;i++)
+      /* Calculate authorized SAP range */
+      if ((psServiceName != NULL) && (psServiceName->length > 0))
+      {
+         /* SDP advertised service */
+         min_sap_range = PHFRINFC_LLCP_SAP_SDP_ADVERTISED_FIRST;
+         max_sap_range = PHFRINFC_LLCP_SAP_SDP_UNADVERTISED_FIRST;
+      }
+      else
+      {
+         /* Non-SDP advertised service */
+         min_sap_range = PHFRINFC_LLCP_SAP_SDP_UNADVERTISED_FIRST;
+         max_sap_range = PHFRINFC_LLCP_SAP_NUMBER;
+      }
+
+      /* Handle dynamic SAP allocation */
+      if (nSap == 0)
+      {
+         status = phFriNfc_LlcpTransport_GetFreeSap(pLlcpSocket->psTransport, psServiceName, &nSap);
+         if (status != NFCSTATUS_SUCCESS)
+         {
+            return status;
+         }
+      }
+
+      /* Test the SAP range */
+      if(!IS_BETWEEN(nSap, min_sap_range, max_sap_range) &&
+         !IS_BETWEEN(nSap, PHFRINFC_LLCP_SAP_WKS_FIRST, PHFRINFC_LLCP_SAP_SDP_ADVERTISED_FIRST))
       {
-         if((pLlcpSocket->psTransport->pSocketTable[i].socket_sSap == nSap) 
-            && (pLlcpSocket->psTransport->pSocketTable[i].eSocket_Type == pLlcpSocket->eSocket_Type))
+         status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
+      }
+      else
+      {
+         /* Test if the nSap it is used by another socket */
+         for(i=0;i<PHFRINFC_LLCP_NB_SOCKET_MAX;i++)
+         {
+            if(pLlcpSocket->psTransport->pSocketTable[i].socket_sSap == nSap)
+            {
+               return status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_ALREADY_REGISTERED);
+            }
+         }
+         /* Set service name */
+         status = phFriNfc_LlcpTransport_RegisterName(pLlcpSocket, nSap, psServiceName);
+         if (status != NFCSTATUS_SUCCESS)
          {
-            return status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_ALREADY_REGISTERED);
+            return status;
          }
+         /* Set the nSap value of the socket */
+         pLlcpSocket->socket_sSap = nSap;
+         /* Set the socket state */
+         pLlcpSocket->eSocket_State = phFriNfc_LlcpTransportSocket_eSocketBound;
       }
-      /* Set the nSap value of the socket */
-      pLlcpSocket->socket_sSap = nSap;
-      /* Set the socket state */
-      pLlcpSocket->eSocket_State = phFriNfc_LlcpTransportSocket_eSocketBound;
    }
    return status;
 }
@@ -908,7 +1455,6 @@ NFCSTATUS phFriNfc_LlcpTransport_Bind(phFriNfc_LlcpTransport_Socket_t    *pLlcpS
 *
 *
 * \param[in]  pLlcpSocket        A pointer to a phFriNfc_LlcpTransport_Socket_t.
-* \param[in]  psServiceName      A pointer to Service Name 
 * \param[in]  pListen_Cb         The callback to be called each time the
 *                                socket receive a connection request.
 * \param[in]  pContext           Upper layer context to be returned in
@@ -922,14 +1468,13 @@ NFCSTATUS phFriNfc_LlcpTransport_Bind(phFriNfc_LlcpTransport_Socket_t    *pLlcpS
 * \retval NFCSTATUS_FAILED                   Operation failed.
 */
 NFCSTATUS phFriNfc_LlcpTransport_Listen(phFriNfc_LlcpTransport_Socket_t*          pLlcpSocket,
-                                        phNfc_sData_t                             *psServiceName,
                                         pphFriNfc_LlcpTransportSocketListenCb_t   pListen_Cb,
                                         void*                                     pContext)
 {
    NFCSTATUS status = NFCSTATUS_SUCCESS;
 
    /* Check for NULL pointers */
-   if(pLlcpSocket == NULL || pListen_Cb == NULL|| pContext == NULL || psServiceName == NULL)
+   if(pLlcpSocket == NULL || pListen_Cb == NULL|| pContext == NULL )
    {
       status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
    }
@@ -948,33 +1493,107 @@ NFCSTATUS phFriNfc_LlcpTransport_Listen(phFriNfc_LlcpTransport_Socket_t*
    {
       status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
    }
-   /* Test the length of the SN */
-   else if(psServiceName->length > PHFRINFC_LLCP_SN_MAX_LENGTH)
+   else
    {
-      status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
+      status = phFriNfc_LlcpTransport_ConnectionOriented_Listen(pLlcpSocket,
+                                                                pListen_Cb,
+                                                                pContext);
    }
-   /* Test the SAP range for SDP-advertised services */
-   else if((psServiceName->length > 0) &&
-           (!IS_BETWEEN(pLlcpSocket->socket_sSap, PHFRINFC_LLCP_SAP_SDP_ADVERTISED_FIRST, PHFRINFC_LLCP_SAP_SDP_UNADVERTISED_FIRST)) &&
-           (!IS_BETWEEN(pLlcpSocket->socket_sSap, PHFRINFC_LLCP_SAP_WKS_FIRST, PHFRINFC_LLCP_SAP_SDP_ADVERTISED_FIRST)))
+   return status;
+}
+
+
+/**
+* \ingroup grp_fri_nfc
+* \brief <b>Register the socket service name</b>.
+*
+* This function changes the service name of the corresponding socket.
+*
+* \param[in]  pLlcpSocket        A pointer to a phFriNfc_LlcpTransport_Socket_t.
+* \param[in]  nSap               SAP number associated to the service name.
+* \param[in]  psServiceName      A pointer to a Service Name.
+*
+* \retval NFCSTATUS_SUCCESS                  Operation successful.
+* \retval NFCSTATUS_INVALID_PARAMETER        One or more of the supplied parameters
+*                                            could not be properly interpreted.
+* \retval NFCSTATUS_FAILED                   Operation failed.
+*/
+static NFCSTATUS phFriNfc_LlcpTransport_RegisterName(phFriNfc_LlcpTransport_Socket_t*   pLlcpSocket,
+                                                     uint8_t                            nSap,
+                                                     phNfc_sData_t                      *psServiceName)
+{
+   phFriNfc_LlcpTransport_t *       psTransport = pLlcpSocket->psTransport;
+   uint8_t                          index;
+   uint8_t                          bSnMatch, bSapMatch;
+
+   /* Check in cache if sap has been used for different service name */
+   for(index=0 ; index<PHFRINFC_LLCP_SDP_ADVERTISED_NB ; index++)
    {
-      status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
+      if(psTransport->pCachedServiceNames[index].sServiceName.length == 0)
+      {
+         /* Reached end of table */
+         break;
+      }
+
+      bSnMatch = (memcmp(psTransport->pCachedServiceNames[index].sServiceName.buffer, psServiceName->buffer, psServiceName->length) == 0);
+      bSapMatch = psTransport->pCachedServiceNames[index].nSap == nSap;
+      if(bSnMatch && bSapMatch)
+      {
+         /* Request match cache */
+         break;
+      }
+      else if((bSnMatch && !bSapMatch) || (!bSnMatch && bSapMatch))
+      {
+         /* Request mismatch with cache */
+         return NFCSTATUS_INVALID_PARAMETER;
+      }
    }
-   /* Test the SAP range for non SDP-advertised services */
-   else if((psServiceName->length == 0) &&
-           (!IS_BETWEEN(pLlcpSocket->socket_sSap, PHFRINFC_LLCP_SAP_SDP_UNADVERTISED_FIRST, PHFRINFC_LLCP_SAP_NUMBER)) &&
-           (!IS_BETWEEN(pLlcpSocket->socket_sSap, PHFRINFC_LLCP_SAP_WKS_FIRST, PHFRINFC_LLCP_SAP_SDP_ADVERTISED_FIRST)))
+
+   /* Handle service with no name */
+   if (psServiceName == NULL)
    {
-      status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
+      if (pLlcpSocket->sServiceName.buffer != NULL)
+      {
+         phOsalNfc_FreeMemory(pLlcpSocket->sServiceName.buffer);
+      }
+      pLlcpSocket->sServiceName.buffer = NULL;
+      pLlcpSocket->sServiceName.length = 0;
    }
    else
    {
-      status = phFriNfc_LlcpTransport_ConnectionOriented_Listen(pLlcpSocket,
-                                                                psServiceName,
-                                                                pListen_Cb,
-                                                                pContext);
+      /* Check if name already in use */
+      for(index=0;index<PHFRINFC_LLCP_NB_SOCKET_MAX;index++)
+      {
+         phFriNfc_LlcpTransport_Socket_t* pCurrentSocket = &pLlcpSocket->psTransport->pSocketTable[index];
+
+         if(   (pCurrentSocket->eSocket_State != phFriNfc_LlcpTransportSocket_eSocketBound)
+            && (pCurrentSocket->eSocket_State != phFriNfc_LlcpTransportSocket_eSocketRegistered))
+         {
+            /* Only bound or listening sockets may have a service name */
+            continue;
+         }
+         if(pCurrentSocket->sServiceName.length != psServiceName->length) {
+            /* Service name do not match, check next */
+            continue;
+         }
+         if(memcmp(pCurrentSocket->sServiceName.buffer, psServiceName->buffer, psServiceName->length) == 0)
+         {
+            /* Service name already in use */
+            return NFCSTATUS_INVALID_PARAMETER;
+         }
+      }
+
+      /* Store the listen socket SN */
+      pLlcpSocket->sServiceName.length = psServiceName->length;
+      pLlcpSocket->sServiceName.buffer = phOsalNfc_GetMemory(psServiceName->length);
+      if (pLlcpSocket->sServiceName.buffer == NULL)
+      {
+          return NFCSTATUS_NOT_ENOUGH_MEMORY;
+      }
+      memcpy(pLlcpSocket->sServiceName.buffer, psServiceName->buffer, psServiceName->length);
    }
-   return status;
+
+   return NFCSTATUS_SUCCESS;
 }
 
 /**
@@ -1133,6 +1752,7 @@ NFCSTATUS phFriNfc_LlcpTransport_Connect( phFriNfc_LlcpTransport_Socket_t*
                                           void*                                      pContext)
 {
    NFCSTATUS status = NFCSTATUS_SUCCESS;
+   uint8_t nLocalSap;
    uint8_t i;
 
    /* Check for NULL pointers */
@@ -1150,7 +1770,11 @@ NFCSTATUS phFriNfc_LlcpTransport_Connect( phFriNfc_LlcpTransport_Socket_t*
    {
       status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_PARAMETER);
    }
-
+   /* Test if the socket has a service name */
+   else if(pLlcpSocket->sServiceName.length != 0)
+   {
+      status = PHNFCSTVAL(CID_FRI_NFC_LLCP_TRANSPORT, NFCSTATUS_INVALID_STATE);
+   }
    /* Test if the socket is not in connecting or connected state*/
    else if(pLlcpSocket->eSocket_State != phFriNfc_LlcpTransportSocket_eSocketCreated && pLlcpSocket->eSocket_State != phFriNfc_LlcpTransportSocket_eSocketBound)
    {
@@ -1161,11 +1785,13 @@ NFCSTATUS phFriNfc_LlcpTransport_Connect( phFriNfc_LlcpTransport_Socket_t*
       /* Implicit bind if socket is not already bound */
       if(pLlcpSocket->eSocket_State != phFriNfc_LlcpTransportSocket_eSocketBound)
       {
-         status = phFriNfc_LlcpTransport_AutoBind(pLlcpSocket);
+         /* Bind to a free sap */
+         status = phFriNfc_LlcpTransport_GetFreeSap(pLlcpSocket->psTransport, NULL, &nLocalSap);
          if (status != NFCSTATUS_SUCCESS)
          {
             return status;
          }
+         pLlcpSocket->socket_sSap = nLocalSap;
       }
 
       /* Test the SAP range for non SDP-advertised services */
@@ -1218,6 +1844,7 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectByUri(phFriNfc_LlcpTransport_Socket_t*
 {
    NFCSTATUS status = NFCSTATUS_SUCCESS;
    uint8_t i;
+   uint8_t nLocalSap;
 
    /* Check for NULL pointers */
    if(pLlcpSocket == NULL || pConnect_RspCb == NULL || pContext == NULL)
@@ -1244,11 +1871,13 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectByUri(phFriNfc_LlcpTransport_Socket_t*
       /* Implicit bind if socket is not already bound */
       if(pLlcpSocket->eSocket_State != phFriNfc_LlcpTransportSocket_eSocketBound)
       {
-         status = phFriNfc_LlcpTransport_AutoBind(pLlcpSocket);
+         /* Bind to a free sap */
+         status = phFriNfc_LlcpTransport_GetFreeSap(pLlcpSocket->psTransport, NULL, &nLocalSap);
          if (status != NFCSTATUS_SUCCESS)
          {
             return status;
          }
+         pLlcpSocket->socket_sSap = nLocalSap;
       }
 
       /* Test the SAP range for non SDP-advertised services */
diff --git a/src/phFriNfc_LlcpTransport.h b/src/phFriNfc_LlcpTransport.h
index 2fbabf6..adcab9e 100644
--- a/src/phFriNfc_LlcpTransport.h
+++ b/src/phFriNfc_LlcpTransport.h
@@ -47,6 +47,9 @@ typedef struct phFriNfc_LlcpTransport phFriNfc_LlcpTransport_t;
 struct phFriNfc_LlcpTransport_Socket;
 typedef struct phFriNfc_LlcpTransport_Socket phFriNfc_LlcpTransport_Socket_t;
 
+struct phFriNfc_Llcp_CachedServiceName;
+typedef struct phFriNfc_Llcp_CachedServiceName phFriNfc_Llcp_CachedServiceName_t;
+
 /*========== ENUMERATES ===========*/
 
 /* Enum reperesents the different LLCP Link status*/
@@ -167,6 +170,9 @@ struct phFriNfc_LlcpTransport_Socket
    uint16_t                                       localMIUX;
    uint8_t                                        index;
 
+   /* SDP related fields */
+   uint8_t                                       nTid;
+
    /* Information Flags */
    bool_t                                        bSocketRecvPending;
    bool_t                                        bSocketSendPending;
@@ -226,12 +232,24 @@ struct phFriNfc_LlcpTransport_Socket
 
 /**
  * \ingroup grp_fri_nfc_llcp_mac
+ * \brief TODO
+ */
+struct phFriNfc_Llcp_CachedServiceName
+{
+   phNfc_sData_t                         sServiceName;
+   uint8_t                               nSap;
+};
+
+
+/**
+ * \ingroup grp_fri_nfc_llcp_mac
  * \brief Declaration of a TRANSPORT Type with a table of PHFRINFC_LLCP_NB_SOCKET_DEFAULT sockets
  *        and a pointer a Llcp layer
  */
 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;
    bool_t                                bSendPending;
    bool_t                                bRecvPending;
@@ -254,6 +272,22 @@ struct phFriNfc_LlcpTransport
    uint8_t                               DmInfoBuffer[3];
 
    uint8_t                               LinkStatusError;
+
+   /**< Service discovery related infos */
+   phNfc_sData_t                         *psDiscoveryServiceNameList;
+   uint8_t                               *pnDiscoverySapList;
+   uint8_t                               nDiscoveryListSize;
+   uint8_t                               nDiscoveryReqOffset;
+   uint8_t                               nDiscoveryResOffset;
+
+   uint8_t                               nDiscoveryResTidList[PHFRINFC_LLCP_SNL_RESPONSE_MAX];
+   uint8_t                               nDiscoveryResSapList[PHFRINFC_LLCP_SNL_RESPONSE_MAX];
+   uint8_t                               nDiscoveryResListSize;
+
+   uint8_t                               pDiscoveryBuffer[PHFRINFC_LLCP_MIU_DEFAULT];
+   pphFriNfc_Cr_t                        pfDiscover_Cb;
+   void                                  *pDiscoverContext;
+
 };
 
 /*
@@ -322,6 +356,16 @@ NFCSTATUS phFriNfc_LlcpTransport_SendFrameReject(phFriNfc_LlcpTransport_t
                                                  uint8_t                            vr,
                                                  uint8_t                            vra);
 
+/*!
+* \ingroup grp_fri_nfc
+* \brief <b>Discover remote services SAP using SDP protocol</b>.
+ */
+NFCSTATUS phFriNfc_LlcpTransport_DiscoverServices( phFriNfc_LlcpTransport_t  *pLlcpTransport,
+                                                   phNfc_sData_t             *psServiceNameList,
+                                                   uint8_t                   *pnSapList,
+                                                   uint8_t                   nListSize,
+                                                   pphFriNfc_Cr_t            pDiscover_Cb,
+                                                   void                      *pContext );
 
 /**
 * \ingroup grp_lib_nfc
@@ -435,6 +479,7 @@ NFCSTATUS phFriNfc_LlcpTransport_Close(phFriNfc_LlcpTransport_Socket_t*   pLlcpS
 *
 * \param[out] pLlcpSocket           A pointer to a phFriNfc_LlcpTransport_Socket_t.
 * \param[in]  pConfigInfo           A port number for a specific socket
+* \param TODO
 *
 * \retval NFCSTATUS_SUCCESS                  Operation successful.
 * \retval NFCSTATUS_INVALID_PARAMETER        One or more of the supplied parameters
@@ -446,7 +491,8 @@ NFCSTATUS phFriNfc_LlcpTransport_Close(phFriNfc_LlcpTransport_Socket_t*   pLlcpS
 * \retval NFCSTATUS_FAILED                   Operation failed.
 */
 NFCSTATUS phFriNfc_LlcpTransport_Bind(phFriNfc_LlcpTransport_Socket_t    *pLlcpSocket,
-                                      uint8_t                            nSap);
+                                      uint8_t                            nSap,
+                                      phNfc_sData_t                      *psServiceName);
 
 /**
 * \ingroup grp_fri_nfc
@@ -460,7 +506,6 @@ NFCSTATUS phFriNfc_LlcpTransport_Bind(phFriNfc_LlcpTransport_Socket_t    *pLlcpS
 *
 *
 * \param[in]  pLlcpSocket        A pointer to a phFriNfc_LlcpTransport_Socket_t.
-* \param[in]  psServiceName      A pointer to a Service Name 
 * \param[in]  pListen_Cb         The callback to be called each time the
 *                                socket receive a connection request.
 * \param[in]  pContext           Upper layer context to be returned in
@@ -474,9 +519,9 @@ NFCSTATUS phFriNfc_LlcpTransport_Bind(phFriNfc_LlcpTransport_Socket_t    *pLlcpS
 * \retval NFCSTATUS_FAILED                   Operation failed.
 */
 NFCSTATUS phFriNfc_LlcpTransport_Listen(phFriNfc_LlcpTransport_Socket_t*          pLlcpSocket,
-                                        phNfc_sData_t                             *psServiceName,
                                         pphFriNfc_LlcpTransportSocketListenCb_t   pListen_Cb,
                                         void*                                     pContext);
+
 /**
 * \ingroup grp_fri_nfc
 * \brief <b>Accept an incoming connection request for a socket</b>.
diff --git a/src/phFriNfc_LlcpTransport_Connection.c b/src/phFriNfc_LlcpTransport_Connection.c
index f58b33f..a11945f 100644
--- a/src/phFriNfc_LlcpTransport_Connection.c
+++ b/src/phFriNfc_LlcpTransport_Connection.c
@@ -1581,7 +1581,6 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_SocketGetRemoteOptions(phFri
 *
 *
 * \param[in]  pLlcpSocket        A pointer to a phFriNfc_LlcpTransport_Socket_t.
-* \param[in]  psServiceName      A pointer to a Service Name 
 * \param[in]  pListen_Cb         The callback to be called each time the
 *                                socket receive a connection request.
 * \param[in]  pContext           Upper layer context to be returned in
@@ -1595,37 +1594,11 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_SocketGetRemoteOptions(phFri
 * \retval NFCSTATUS_FAILED                   Operation failed.
 */
 NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Listen(phFriNfc_LlcpTransport_Socket_t*          pLlcpSocket,
-                                                           phNfc_sData_t                             *psServiceName,
                                                            pphFriNfc_LlcpTransportSocketListenCb_t   pListen_Cb,
                                                            void*                                     pContext)
 {
    NFCSTATUS status = NFCSTATUS_SUCCESS;
    uint8_t   index;
-   
-   /* Check if the service name is already registered */
-   if (psServiceName != NULL)
-   {
-      for(index=0;index<PHFRINFC_LLCP_NB_SOCKET_MAX;index++)
-      {
-         phFriNfc_LlcpTransport_Socket_t* pCurrentSocket = &pLlcpSocket->psTransport->pSocketTable[index];
-
-         if((pCurrentSocket->sServiceName.length == 0) ||
-            (pCurrentSocket->eSocket_State != phFriNfc_LlcpTransportSocket_eSocketRegistered))
-         {
-            /* Do not check inactive or non-SDP registered sockets */
-            continue;
-         }
-         if(pCurrentSocket->sServiceName.length != psServiceName->length) {
-            /* Service name do not match, check next */
-            continue;
-         }
-         if(memcmp(pCurrentSocket->sServiceName.buffer, psServiceName->buffer, psServiceName->length) == 0)
-         {
-            /* Service name already in use */
-            return NFCSTATUS_INVALID_PARAMETER;
-         }
-      }
-   }
 
    /* Store the listen callback */
    pLlcpSocket->pfSocketListen_Cb = pListen_Cb;
@@ -1636,15 +1609,6 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Listen(phFriNfc_LlcpTranspor
    /* Set RecvPending to TRUE */
    pLlcpSocket->bSocketListenPending = TRUE;
 
-   /* Store the listen socket SN */
-   pLlcpSocket->sServiceName.length = psServiceName->length;
-   pLlcpSocket->sServiceName.buffer = phOsalNfc_GetMemory(psServiceName->length);
-   if (pLlcpSocket->sServiceName.buffer == NULL)
-   {
-       return NFCSTATUS_NOT_ENOUGH_MEMORY;
-   }
-   memcpy(pLlcpSocket->sServiceName.buffer, psServiceName->buffer, psServiceName->length);
-
    /* Set the socket state*/
    pLlcpSocket->eSocket_State = phFriNfc_LlcpTransportSocket_eSocketRegistered;
 
diff --git a/src/phFriNfc_LlcpTransport_Connection.h b/src/phFriNfc_LlcpTransport_Connection.h
index 4a15821..07ec1fb 100644
--- a/src/phFriNfc_LlcpTransport_Connection.h
+++ b/src/phFriNfc_LlcpTransport_Connection.h
@@ -112,7 +112,6 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Close(phFriNfc_LlcpTransport
 *
 *
 * \param[in]  pLlcpSocket        A pointer to a phFriNfc_LlcpTransport_Socket_t.
-* \param[in]  psServiceName      A pointer to a Service Name 
 * \param[in]  pListen_Cb         The callback to be called each time the
 *                                socket receive a connection request.
 * \param[in]  pContext           Upper layer context to be returned in
@@ -126,7 +125,6 @@ NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Close(phFriNfc_LlcpTransport
 * \retval NFCSTATUS_FAILED                   Operation failed.
 */
 NFCSTATUS phFriNfc_LlcpTransport_ConnectionOriented_Listen(phFriNfc_LlcpTransport_Socket_t*          pLlcpSocket,
-                                                           phNfc_sData_t                             *psServiceName,
                                                            pphFriNfc_LlcpTransportSocketListenCb_t   pListen_Cb,
                                                            void*                                     pContext);
 
diff --git a/src/phFriNfc_LlcpUtils.c b/src/phFriNfc_LlcpUtils.c
index 872fd51..750f513 100644
--- a/src/phFriNfc_LlcpUtils.c
+++ b/src/phFriNfc_LlcpUtils.c
@@ -86,6 +86,7 @@ NFCSTATUS phFriNfc_Llcp_EncodeTLV( phNfc_sData_t  *psValueBuffer,
                                    uint8_t        *pValue)
 {
    uint32_t offset = *pOffset;
+   uint32_t finalOffset = offset + 2 + length; /* 2 stands for Type and Length fields size */
    uint8_t i;
 
    /* Check for NULL pointers */
@@ -100,8 +101,8 @@ NFCSTATUS phFriNfc_Llcp_EncodeTLV( phNfc_sData_t  *psValueBuffer,
       return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
    }
 
-   /* Check if enough room for Type and Length (with overflow check) */
-   if ((offset+2 > psValueBuffer->length) && (offset+2 > offset))
+   /* Check if enough room for Type, Length and Value (with overflow check) */
+   if ((finalOffset > psValueBuffer->length) || (finalOffset < offset))
    {
       return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
    }
@@ -126,6 +127,47 @@ NFCSTATUS phFriNfc_Llcp_EncodeTLV( phNfc_sData_t  *psValueBuffer,
    return NFCSTATUS_SUCCESS;
 }
 
+NFCSTATUS phFriNfc_Llcp_AppendTLV( phNfc_sData_t  *psValueBuffer,
+                                   uint32_t       nTlvOffset,
+                                   uint32_t       *pCurrentOffset,
+                                   uint8_t        length,
+                                   uint8_t        *pValue)
+{
+   uint32_t offset = *pCurrentOffset;
+   uint32_t finalOffset = offset + length;
+
+   /* Check for NULL pointers */
+   if ((psValueBuffer == NULL) || (pCurrentOffset == NULL) || (pValue == NULL))
+   {
+      return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
+   }
+
+   /* Check offset */
+   if (offset > psValueBuffer->length)
+   {
+      return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
+   }
+
+   /* Check if enough room for Type and Length (with overflow check) */
+   if ((finalOffset > psValueBuffer->length) || (finalOffset < offset))
+   {
+      return PHNFCSTVAL(CID_FRI_NFC_LLCP, NFCSTATUS_INVALID_PARAMETER);
+   }
+
+   /* Update the LENGTH */
+   psValueBuffer->buffer[nTlvOffset+1] += length;
+
+   /* Set the VALUE */
+   memcpy(psValueBuffer->buffer + offset, pValue, length);
+   offset += length;
+
+   /* Save updated offset */
+   *pCurrentOffset = offset;
+
+   return NFCSTATUS_SUCCESS;
+}
+
+
 /* TODO: comment function EncodeMIUX */
 void phFriNfc_Llcp_EncodeMIUX(uint16_t miux,
                               uint8_t* pMiuxEncoded)
diff --git a/src/phFriNfc_LlcpUtils.h b/src/phFriNfc_LlcpUtils.h
index a1004a1..9dcb95a 100644
--- a/src/phFriNfc_LlcpUtils.h
+++ b/src/phFriNfc_LlcpUtils.h
@@ -70,6 +70,12 @@ NFCSTATUS phFriNfc_Llcp_EncodeTLV( phNfc_sData_t  *psValueBuffer,
                                    uint8_t        length,
                                    uint8_t        *pValue);
 
+NFCSTATUS phFriNfc_Llcp_AppendTLV( phNfc_sData_t  *psValueBuffer,
+                                   uint32_t       nTlvOffset,
+                                   uint32_t       *pCurrentOffset,
+                                   uint8_t        length,
+                                   uint8_t        *pValue);
+
 void phFriNfc_Llcp_EncodeMIUX(uint16_t pMiux,
                               uint8_t* pMiuxEncoded);
 
diff --git a/src/phLibNfc.h b/src/phLibNfc.h
index 8621361..7a5c3f0 100644
--- a/src/phLibNfc.h
+++ b/src/phLibNfc.h
@@ -2611,6 +2611,47 @@ extern NFCSTATUS phLibNfc_Llcp_Socket( phLibNfc_Llcp_eSocketType_t      eType,
 
 /**
 * \ingroup grp_lib_nfc
+* \brief <b>Get SAP of remote services using their names</b>.
+*
+* This function sends SDP queries to the remote peer to get the SAP to address for a given
+* service name. The queries are aggregated as much as possible for efficiency, but if all
+* the queries cannot fit in a single packet, they will be splitted in multiple packets.
+* The callback will be called only when all of the requested services names SAP will be
+* gathered. As mentionned in LLCP specification, a SAP of 0 means that the service name
+* as not been found.
+*
+* This feature is available only since LLCP v1.1, both devices must be at least v1.1 in
+* order to be able to use this function.
+*
+* \param[in]  hRemoteDevice      Peer handle obtained during device discovery process.
+* \param[in]  psServiceNameList  The list of the service names to discover.
+* \param[out] pnSapList          The list of the corresponding SAP numbers, in the same
+*                                order than the service names list.
+* \param[in]  nListSize          The size of both service names and SAP list.
+* \param[in]  pDiscover_Cb       The callback to be called once LibNfc matched SAP for
+*                                all of the provided service names.
+* \param[in]  pContext           Upper layer context to be returned in the callback.
+*
+* \retval NFCSTATUS_SUCCESS                  Operation successful.
+* \retval NFCSTATUS_INVALID_PARAMETER        One or more of the supplied parameters
+*                                            could not be properly interpreted.
+* \retval NFCSTATUS_NOT_INITIALISED          Indicates stack is not yet initialized.
+* \retval NFCSTATUS_SHUTDOWN                 Shutdown in progress.
+* \retval NFCSTATUS_FAILED                   Operation failed.
+* \retval NFCSTATUS_FEATURE_NOT_SUPPORTED    Remote peer does not support this feature (e.g.: is v1.0).
+* \retval NFCSTATUS_BUSY                     Previous request in progress can not accept new request.
+*/
+extern NFCSTATUS phLibNfc_Llcp_DiscoverServices( phLibNfc_Handle     hRemoteDevice,
+                                                 phNfc_sData_t       *psServiceNameList,
+                                                 uint8_t             *pnSapList,
+                                                 uint8_t             nListSize,
+                                                 pphLibNfc_RspCb_t   pDiscover_Cb,
+                                                 void                *pContext
+                                               );
+
+
+/**
+* \ingroup grp_lib_nfc
 * \brief <b>Close a socket on a LLCP-connected device</b>.
 *
 * This function closes a LLCP socket previously created using phLibNfc_Llcp_Socket.
@@ -2686,9 +2727,8 @@ extern NFCSTATUS phLibNfc_Llcp_SocketGetRemoteOptions( phLibNfc_Handle
 * This function binds the socket to a local Service Access Point.
 *
 * \param[in]  hSocket               Peer handle obtained during device discovery process.
-* \param[out] pConfigInfo           Pointer on the variable to be filled with the configuration
-*                                   parameters used during activation.
-*
+* \param TODO (nSap + sn)
+
 * \retval NFCSTATUS_SUCCESS                  Operation successful.
 * \retval NFCSTATUS_INVALID_PARAMETER        One or more of the supplied parameters
 *                                            could not be properly interpreted.
@@ -2701,7 +2741,8 @@ extern NFCSTATUS phLibNfc_Llcp_SocketGetRemoteOptions( phLibNfc_Handle
 * \retval NFCSTATUS_FAILED                   Operation failed.
 */
 extern NFCSTATUS phLibNfc_Llcp_Bind( phLibNfc_Handle hSocket,
-                                     uint8_t         nSap
+                                     uint8_t         nSap,
+                                     phNfc_sData_t * psServiceName
                                      );
 
 
@@ -2718,8 +2759,6 @@ extern NFCSTATUS phLibNfc_Llcp_Bind( phLibNfc_Handle hSocket,
 *
 *
 * \param[in]  hSocket            Socket handle obtained during socket creation.
-* \param[in]  psServiceName      A buffer containing the name of the service for SDP. No SDP
-*                                advertising if set to NULL.
 * \param[in]  pListen_Cb         The callback to be called each time the
 *                                socket receive a connection request.
 * \param[in]  pContext           Upper layer context to be returned in
@@ -2735,7 +2774,6 @@ extern NFCSTATUS phLibNfc_Llcp_Bind( phLibNfc_Handle hSocket,
 * \retval NFCSTATUS_FAILED                   Operation failed.
 */
 extern NFCSTATUS phLibNfc_Llcp_Listen( phLibNfc_Handle                  hSocket,
-                                       phNfc_sData_t                    *psServiceName,
                                        pphLibNfc_LlcpSocketListenCb_t   pListen_Cb,
                                        void*                            pContext
                                        );
diff --git a/src/phLibNfc_Internal.h b/src/phLibNfc_Internal.h
index 304ac22..f791b02 100644
--- a/src/phLibNfc_Internal.h
+++ b/src/phLibNfc_Internal.h
@@ -134,6 +134,10 @@ typedef struct phLibNfc_Hal_CB_Info
     pphLibNfc_LlcpLinkStatusCb_t   pClientLlcpLinkCb;
     void                           *pClientLlcpLinkCntx;
 
+    /*LLCP service discovery*/
+    pphLibNfc_RspCb_t              pClientLlcpDiscoveryCb;
+    void                           *pClientLlcpDiscoveryCntx;
+
 }phLibNfc_Hal_CB_Info_t;
 
 typedef struct phLibNfc_NdefInfo
diff --git a/src/phLibNfc_llcp.c b/src/phLibNfc_llcp.c
index ab518a2..799aae0 100644
--- a/src/phLibNfc_llcp.c
+++ b/src/phLibNfc_llcp.c
@@ -478,6 +478,73 @@ NFCSTATUS phLibNfc_Llcp_GetRemoteInfo( phLibNfc_Handle                    hRemot
    return PHNFCSTATUS(result);
 }
 
+NFCSTATUS phLibNfc_Llcp_DiscoverServices( phLibNfc_Handle     hRemoteDevice,
+                                          phNfc_sData_t       *psServiceNameList,
+                                          uint8_t             *pnSapList,
+                                          uint8_t             nListSize,
+                                          pphLibNfc_RspCb_t   pDiscover_Cb,
+                                          void                *pContext
+                                          )
+{
+   NFCSTATUS                           result;
+   PHNFC_UNUSED_VARIABLE(hRemoteDevice);
+
+   /* State checking */
+   result = static_CheckState();
+   if (result != NFCSTATUS_SUCCESS)
+   {
+      return result;
+   }
+
+   /* Parameters checking */
+   if ((hRemoteDevice == 0)       ||
+       (psServiceNameList == NULL) ||
+       (pnSapList == NULL) ||
+       (nListSize == 0) ||
+       (pDiscover_Cb == NULL))
+   {
+      return NFCSTATUS_INVALID_PARAMETER;
+   }
+
+   /* Check device */
+   result = static_CheckDevice(hRemoteDevice);
+   if (result != NFCSTATUS_SUCCESS)
+   {
+      return result;
+   }
+
+   /* Prepare callback */
+   gpphLibContext->CBInfo.pClientLlcpDiscoveryCb = pDiscover_Cb;
+   gpphLibContext->CBInfo.pClientLlcpDiscoveryCntx = pContext;
+
+   /* Update state */
+   result = phLibNfc_UpdateNextState(gpphLibContext, eLibNfcHalStateTransaction);
+   if (result != NFCSTATUS_SUCCESS)
+   {
+      return result;
+   }
+
+   /* Call the component function */
+   result = phFriNfc_LlcpTransport_DiscoverServices( &gpphLibContext->llcp_cntx.sLlcpTransportContext,
+                                                     psServiceNameList,
+                                                     pnSapList,
+                                                     nListSize,
+                                                     pDiscover_Cb,
+                                                     pContext
+                                                     );
+   result = PHNFCSTATUS(result);
+   if ((result == NFCSTATUS_PENDING) || (result == NFCSTATUS_SUCCESS))
+   {
+      /* Nothing to do */
+   }
+   else if (result != NFCSTATUS_FAILED)
+   {
+      result = NFCSTATUS_TARGET_LOST;
+   }
+
+   return result;
+}
+
 NFCSTATUS phLibNfc_Llcp_Socket( phLibNfc_Llcp_eSocketType_t      eType,
                                 phLibNfc_Llcp_sSocketOptions_t*  psOptions,
                                 phNfc_sData_t*                   psWorkingBuffer,
@@ -610,7 +677,8 @@ NFCSTATUS phLibNfc_Llcp_SocketGetRemoteOptions( phLibNfc_Handle
 }
 
 NFCSTATUS phLibNfc_Llcp_Bind( phLibNfc_Handle hSocket,
-                              uint8_t         nSap
+                              uint8_t         nSap,
+                              phNfc_sData_t * psServiceName
                               )
 {
    NFCSTATUS                        result;
@@ -632,13 +700,12 @@ NFCSTATUS phLibNfc_Llcp_Bind( phLibNfc_Handle hSocket,
    }
 
    /* Bind the socket to the designated port */
-   result = phFriNfc_LlcpTransport_Bind(psSocket, nSap);
+   result = phFriNfc_LlcpTransport_Bind(psSocket, nSap, psServiceName);
 
    return PHNFCSTATUS(result);
 }
 
 NFCSTATUS phLibNfc_Llcp_Listen( phLibNfc_Handle                  hSocket,
-                                phNfc_sData_t                    *psServiceName,
                                 pphLibNfc_LlcpSocketListenCb_t   pListen_Cb,
                                 void*                            pContext
                                 )
@@ -665,7 +732,6 @@ NFCSTATUS phLibNfc_Llcp_Listen( phLibNfc_Handle                  hSocket,
 
    /* Start listening for incoming connections */
    result = phFriNfc_LlcpTransport_Listen( psSocket,
-                                           psServiceName,
                                            (pphFriNfc_LlcpTransportSocketListenCb_t)pListen_Cb,
                                            pContext );