[network-data] add Service/Server TLV support (#2256)

.travis/script.sh

@@ -63,6 +63,7 @@ set -x
         --enable-mac-filter               \
         --enable-mtd-network-diagnostic   \
         --enable-raw-link-api             \
+        --enable-service                  \
         --enable-tmf-proxy || die
     scan-build --status-bugs -analyze-headers -v make || die
 }
@@ -213,12 +214,13 @@ set -x
         --enable-ncp-app=all                \
         --with-ncp-bus=spi                  \
         --with-examples=posix               \
-        --enable-diag                       \
-        --enable-legacy                     \
-        --enable-jam-detection              \
-        --enable-child-supervision          \
         --enable-border-router              \
+        --enable-child-supervision          \
+        --enable-diag                       \
+        --enable-jam-detection              \
+        --enable-legacy                     \
         --enable-mac-filter                 \
+        --enable-service                    \
         --disable-docs                      \
         --disable-test || die
     make -j 8 || die
@@ -230,10 +232,11 @@ set -x
         --enable-cli-app=mtd                \
         --with-ncp-bus=spi                  \
         --with-examples=posix               \
-        --enable-legacy                     \
-        --enable-child-supervision          \
         --enable-border-router              \
+        --enable-child-supervision          \
+        --enable-legacy                     \
         --enable-mac-filter                 \
+        --enable-service                    \
         --disable-docs                      \
         --disable-test || die
     make -j 8 || die

Makefile.am

@@ -32,19 +32,20 @@ AM_MAKEFLAGS                        = --no-print-directory
 
 AM_DISTCHECK_CONFIGURE_FLAGS        = \
     --enable-address-sanitizer        \
-    --enable-cli-app=all              \
-    --enable-ncp-app=all              \
-    --with-ncp-bus=uart               \
-    --enable-diag                     \
-    --with-examples=posix             \
-    --enable-commissioner             \
-    --enable-joiner                   \
-    --enable-dhcp6-client             \
-    --enable-dhcp6-server             \
-    --enable-dns-client               \
     --enable-application-coap         \
     --enable-border-router            \
+    --enable-cli-app=all              \
+    --enable-commissioner             \
+    --enable-dhcp6-client             \
+    --enable-dhcp6-server             \
+    --enable-diag                     \
+    --enable-dns-client               \
+    --enable-joiner                   \
     --enable-mac-filter               \
+    --enable-ncp-app=all              \
+    --enable-service                  \
+    --with-examples=posix             \
+    --with-ncp-bus=uart               \
     $(NULL)
 
 SUBDIRS                             = \

configure.ac

@@ -1133,6 +1133,36 @@ AC_SUBST(OPENTHREAD_ENABLE_BORDER_ROUTER)
 AM_CONDITIONAL([OPENTHREAD_ENABLE_BORDER_ROUTER], [test "${enable_border_router}" = "yes"])
 AC_DEFINE_UNQUOTED([OPENTHREAD_ENABLE_BORDER_ROUTER],[${OPENTHREAD_ENABLE_BORDER_ROUTER}],[Define to 1 if you want to enable Border Router])
 
+#
+# Service
+#
+
+AC_ARG_ENABLE(service,
+    [AS_HELP_STRING([--enable-service],[Enable Service @<:@default=no@:>@.])],
+    [
+        case "${enableval}" in
+
+        no|yes)
+            enable_service=${enableval}
+            ;;
+
+        *)
+            AC_MSG_ERROR([Invalid value ${enable_service} for --enable-service])
+            ;;
+        esac
+    ],
+    [enable_service=no])
+
+if test "$enable_service" = "yes"; then
+    OPENTHREAD_ENABLE_SERVICE=1
+else
+    OPENTHREAD_ENABLE_SERVICE=0
+fi
+
+AC_SUBST(OPENTHREAD_ENABLE_SERVICE)
+AM_CONDITIONAL([OPENTHREAD_ENABLE_SERVICE], [test "${enable_service}" = "yes"])
+AC_DEFINE_UNQUOTED([OPENTHREAD_ENABLE_SERVICE],[${OPENTHREAD_ENABLE_SERVICE}],[Define to 1 if you want to enable Service])
+
 #
 # Linker Map Output
 #
@@ -1514,6 +1544,7 @@ AC_MSG_NOTICE([
   OpenThread Application CoAP support       : ${enable_application_coap}
   OpenThread Raw Link-Layer support         : ${enable_raw_link_api}
   OpenThread Border Router support          : ${enable_border_router}
+  OpenThread Service support                : ${enable_service}
   OpenThread examples                       : ${OPENTHREAD_EXAMPLES}
 
 ])

examples/Makefile-posix

@@ -90,14 +90,11 @@ TargetTuple                     = $(shell ${AbsTopSourceDir}/third_party/nlbuild
 # accordingly.
 
 configure_OPTIONS                   = \
-    --enable-cli-app=all              \
-    --enable-ncp-app=all              \
-    --with-ncp-bus=uart               \
-    --with-examples=posix             \
     --enable-application-coap         \
     --enable-border-router            \
     --enable-cert-log                 \
     --enable-child-supervision        \
+    --enable-cli-app=all              \
     --enable-commissioner             \
     --enable-dhcp6-client             \
     --enable-dhcp6-server             \
@@ -108,8 +105,12 @@ configure_OPTIONS                   = \
     --enable-legacy                   \
     --enable-mac-filter               \
     --enable-mtd-network-diagnostic   \
+    --enable-ncp-app=all              \
     --enable-raw-link-api             \
+    --enable-service                  \
     --enable-tmf-proxy                \
+    --with-examples=posix             \
+    --with-ncp-bus=uart               \
     $(NULL)
 
 include $(dir $(abspath $(lastword $(MAKEFILE_LIST))))/common-switches.mk

examples/common-switches.mk

@@ -26,10 +26,6 @@
 #  POSSIBILITY OF SUCH DAMAGE.
 #
 
-ifeq ($(TMF_PROXY),1)
-configure_OPTIONS              += --enable-tmf-proxy
-endif
-
 ifeq ($(BORDER_ROUTER),1)
 configure_OPTIONS              += --enable-border-router
 endif
@@ -90,6 +86,14 @@ ifeq ($(MTD_NETDIAG),1)
 configure_OPTIONS              += --enable-mtd-network-diagnostic
 endif
 
+ifeq ($(SERVICE),1)
+configure_OPTIONS              += --enable-service
+endif
+
+ifeq ($(TMF_PROXY),1)
+configure_OPTIONS              += --enable-tmf-proxy
+endif
+
 ifeq ($(DEBUG_UART),1)
 CFLAGS   += -DOPENTHREAD_CONFIG_ENABLE_DEBUG_UART=1
 CXXFLAGS += -DOPENTHREAD_CONFIG_ENABLE_DEBUG_UART=1

include/openthread-windows-config.h

@@ -82,6 +82,9 @@
 /* Define to 1 to enable Border Router feature. */
 #define OPENTHREAD_ENABLE_BORDER_ROUTER 1
 
+/* Define to 1 to enable Service feature. */
+#define OPENTHREAD_ENABLE_SERVICE 0
+
 /* Name of package */
 #define PACKAGE "openthread"
 

include/openthread/Makefile.am

@@ -71,6 +71,7 @@ openthread_headers                      = \
     ncp.h                                 \
     netdata.h                             \
     openthread.h                          \
+    server.h                              \
     tasklet.h                             \
     thread.h                              \
     thread_ftd.h                          \

include/openthread/server.h

@@ -0,0 +1,134 @@
+/*
+ *  Copyright (c) 2016-2017, The OpenThread Authors.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *  2. Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *  3. Neither the name of the copyright holder nor the
+ *     names of its contributors may be used to endorse or promote products
+ *     derived from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * @file
+ * @brief
+ *  This file defines the OpenThread Server API.
+ */
+
+#ifndef OPENTHREAD_SERVER_H_
+#define OPENTHREAD_SERVER_H_
+
+#include <openthread/types.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @addtogroup api-server
+ *
+ * @brief
+ *  This module includes functions to manage local network data with the OpenThread Server.
+ *
+ * @{
+ *
+ */
+
+/**
+ * This method provides a full or stable copy of the local Thread Network Data.
+ *
+ * @param[in]     aInstance    A pointer to an OpenThread instance.
+ * @param[in]     aStable      TRUE when copying the stable version, FALSE when copying the full version.
+ * @param[out]    aData        A pointer to the data buffer.
+ * @param[inout]  aDataLength  On entry, size of the data buffer pointed to by @p aData.
+ *                             On exit, number of copied bytes.
+ */
+OTAPI otError OTCALL otServerGetNetDataLocal(otInstance *aInstance, bool aStable, uint8_t *aData,
+                                             uint8_t *aDataLength);
+
+/**
+ * Add a service configuration to the local network data.
+ *
+ * @param[in]  aInstance A pointer to an OpenThread instance.
+ * @param[in]  aConfig   A pointer to the service configuration.
+ *
+ * @retval OT_ERROR_NONE          Successfully added the configuration to the local network data.
+ * @retval OT_ERROR_INVALID_ARGS  One or more configuration parameters were invalid.
+ * @retval OT_ERROR_NO_BUFS       Not enough room is available to add the configuration to the local network data.
+ *
+ * @sa otServerRemoveService
+ * @sa otServerRegister
+ */
+OTAPI otError OTCALL otServerAddService(otInstance *aInstance, const otServiceConfig *aConfig);
+
+/**
+ * Remove a service configuration from the local network data.
+ *
+ * @param[in]  aInstance          A pointer to an OpenThread instance.
+ * @param[in]  aEnterpriseNumber  Enterprise Number of the service entry to be deleted.
+ * @param[in]  aServiceData       A pointer to an Service Data to look for during deletion.
+ * @param[in]  aServiceDataLength The length of @p aServiceData in bytes.
+ *
+ * @retval OT_ERROR_NONE       Successfully removed the configuration from the local network data.
+ * @retval OT_ERROR_NOT_FOUND  Could not find the Border Router entry.
+ *
+ * @sa otServerAddService
+ * @sa otServerRegister
+ */
+OTAPI otError OTCALL otServerRemoveService(otInstance *aInstance, uint32_t aEnterpriseNumber, uint8_t *aServiceData,
+                                           uint8_t aServiceDataLength);
+
+/**
+ * This function gets the next service in the local Network Data.
+ *
+ * @param[in]     aInstance  A pointer to an OpenThread instance.
+ * @param[inout]  aIterator  A pointer to the Network Data iterator context. To get the first service entry
+                             it should be set to OT_NETWORK_DATA_ITERATOR_INIT.
+ * @param[out]    aConfig    A pointer to where the service information will be placed.
+ *
+ * @retval OT_ERROR_NONE       Successfully found the next service.
+ * @retval OT_ERROR_NOT_FOUND  No subsequent service exists in the Thread Network Data.
+ *
+ */
+OTAPI otError OTCALL otServerGetNextService(otInstance *aInstance, otNetworkDataIterator *aIterator,
+                                            otServiceConfig *aConfig);
+
+/**
+ * Immediately register the local network data with the Leader.
+ *
+ * @param[in]  aInstance A pointer to an OpenThread instance.
+ *
+ * retval OT_ERROR_NONE  Successfully queued a Server Data Request message for delivery.
+ *
+ * @sa otServerAddService
+ * @sa otServerRemoveService
+ */
+OTAPI otError OTCALL otServerRegister(otInstance *aInstance);
+
+/**
+ * @}
+ *
+ */
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
+#endif  // OPENTHREAD_SERVER_H_

include/openthread/types.h

@@ -812,6 +812,68 @@ typedef enum otRoutePreference
     OT_ROUTE_PREFERENCE_HIGH = 1,   ///< High route preference.
 } otRoutePreference;
 
+enum
+{
+    /**
+     * Maximum size of Service Data in bytes.
+     */
+    kMaxServiceDataSize = 252,
+
+    /**
+     * Maximum size of Server Data in bytes. This is theoretical limit, practical one is much lower.
+     */
+    kMaxServerDataSize = 248,
+};
+
+/**
+ * This structure represents a Server configuration.
+ */
+typedef struct otServerConfig
+{
+    /**
+     * TRUE, if this configuration is considered Stable Network Data.  FALSE, otherwise.
+     */
+    bool mStable : 1;
+
+    /**
+     * Length of server data.
+     */
+    uint8_t mServerDataLength;
+
+    /**
+     * Server data bytes
+     */
+    uint8_t mServerData[kMaxServerDataSize];
+
+    /**
+     * The Server Rloc.
+     */
+    uint16_t mRloc16;
+} otServerConfig;
+
+/**
+ * This structure represents a Service configuration.
+ */
+typedef struct otServiceConfig
+{
+    /**
+     * IANA Enterprise Number.
+     */
+    uint32_t mEnterpriseNumber;
+
+    /**
+     * Length of service data.
+     */
+    uint8_t mServiceDataLength;
+
+    /**
+     * Service data bytes
+     */
+    uint8_t mServiceData[kMaxServiceDataSize];
+
+    otServerConfig mServerConfig;
+} otServiceConfig;
+
 /**
  * Used to indicate no fixed received signal strength was set
  */

src/cli/README.md

@@ -43,6 +43,7 @@ OpenThread test scripts use the CLI to execute test cases.
 * [mode](#mode)
 * [neighbor](#neighbor-list)
 * [netdataregister](#netdataregister)
+* [netdatashow](#netdatashow)
 * [networkdiagnostic](#networkdiagnostic-get-addr-type-)
 * [networkidtimeout](#networkidtimeout)
 * [networkname](#networkname)
@@ -69,6 +70,7 @@ OpenThread test scripts use the CLI to execute test cases.
 * [txpowermax](#txpowermax)
 * [version](#version)
 * [diag](#diag)
+* [service](#service)
 
 ## OpenThread Command Details
 
@@ -1125,6 +1127,16 @@ Register local network data with Thread Leader.
 Done
 ```
 
+### netdatashow
+
+Show Thread Leader network data.
+
+```bash
+> netdatashow
+08040b020000
+Done
+```
+
 ### networkdiagnostic get \<addr\> \<type\> ..
 
 Send network diagnostic request to retrieve tlv of \<type\>s.
@@ -1767,4 +1779,35 @@ Done
 Diagnostics module is enabled only when building OpenThread with --enable-diag option.
 Go [diagnostics module][1] for more information.
 
+### service
+
+Module for controlling service registration in Network Data.
+Each change in service registration must be sent to leader by `netdataregister` command
+before taking effect.
+
+### service add \<enterpriseNumber\> \<serviceData\> \<serverData\>
+
+Add service to the Network Data.
+
+```bash
+> service add 44970 foo bar
+Done
+> ipaddr
+fdde:ad00:beef:0:0:ff:fe00:fc10
+fdde:ad00:beef:0:0:ff:fe00:fc00
+fdde:ad00:beef:0:0:ff:fe00:7c00
+fe80:0:0:0:1486:2f57:3c:6e10
+fdde:ad00:beef:0:8ca4:19ed:217a:eff9
+Done
+```
+
+### service remove \<enterpriseNumber\> \<serviceData\>
+
+Remove service from Network Data.
+
+```bash
+> service remove 44970 foo
+Done
+```
+
 [1]:../diag/README.md

src/cli/cli.cpp

@@ -55,6 +55,9 @@
 #if OPENTHREAD_ENABLE_BORDER_ROUTER
 #include <openthread/border_router.h>
 #endif
+#if OPENTHREAD_ENABLE_SERVICE
+#include <openthread/server.h>
+#endif
 
 #ifndef OTDLL
 #include <openthread/dhcp6_client.h>
@@ -163,9 +166,12 @@ const struct Command Interpreter::sCommands[] =
 #if OPENTHREAD_FTD
     { "neighbor", &Interpreter::ProcessNeighbor },
 #endif
-#if OPENTHREAD_ENABLE_BORDER_ROUTER
+#if OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
     { "netdataregister", &Interpreter::ProcessNetworkDataRegister },
 #endif
+#if OPENTHREAD_ENABLE_SERVICE
+    { "netdatashow", &Interpreter::ProcessNetworkDataShow },
+#endif
 #if OPENTHREAD_FTD || OPENTHREAD_ENABLE_MTD_NETWORK_DIAGNOSTIC
     { "networkdiagnostic", &Interpreter::ProcessNetworkDiagnostic },
 #endif // OPENTHREAD_FTD || OPENTHREAD_ENABLE_MTD_NETWORK_DIAGNOSTIC
@@ -205,6 +211,9 @@ const struct Command Interpreter::sCommands[] =
     { "routerupgradethreshold", &Interpreter::ProcessRouterUpgradeThreshold },
 #endif
     { "scan", &Interpreter::ProcessScan },
+#if OPENTHREAD_ENABLE_SERVICE
+    { "service", &Interpreter::ProcessService },
+#endif
     { "singleton", &Interpreter::ProcessSingleton },
     { "state", &Interpreter::ProcessState },
     { "thread", &Interpreter::ProcessThread },
@@ -1469,18 +1478,81 @@ exit:
 }
 #endif
 
-#if OPENTHREAD_ENABLE_BORDER_ROUTER
-void Interpreter::ProcessNetworkDataRegister(int argc, char *argv[])
+#if OPENTHREAD_ENABLE_SERVICE
+void Interpreter::ProcessNetworkDataShow(int argc, char *argv[])
 {
     otError error = OT_ERROR_NONE;
-    SuccessOrExit(error = otBorderRouterRegister(mInstance));
+    uint8_t data[255];
+    uint8_t len = sizeof(data);
+
+    SuccessOrExit(error = otNetDataGet(mInstance, false, data, &len));
+
+    this->OutputBytes(data, static_cast<uint8_t>(len));
+    mServer->OutputFormat("\r\n");
 
 exit:
     OT_UNUSED_VARIABLE(argc);
     OT_UNUSED_VARIABLE(argv);
     AppendResult(error);
 }
-#endif  // OPENTHREAD_ENABLE_BORDER_ROUTER
+
+void Interpreter::ProcessService(int argc, char *argv[])
+{
+    otError error = OT_ERROR_NONE;
+
+    VerifyOrExit(argc > 0, error = OT_ERROR_PARSE);
+
+    if (strcmp(argv[0], "add") == 0)
+    {
+        otServiceConfig cfg;
+        long enterpriseNumber = 0;
+
+        VerifyOrExit(argc > 3, error = OT_ERROR_PARSE);
+
+        SuccessOrExit(error = ParseLong(argv[1], enterpriseNumber));
+
+        cfg.mServiceDataLength = static_cast<uint8_t>(strlen(argv[2]));
+        memcpy(cfg.mServiceData, argv[2], cfg.mServiceDataLength);
+        cfg.mEnterpriseNumber = static_cast<uint32_t>(enterpriseNumber);
+        cfg.mServerConfig.mStable = true;
+        cfg.mServerConfig.mServerDataLength = static_cast<uint8_t>(strlen(argv[3]));
+        memcpy(cfg.mServerConfig.mServerData, argv[3], cfg.mServerConfig.mServerDataLength);
+
+        SuccessOrExit(error = otServerAddService(mInstance, &cfg));
+    }
+    else if (strcmp(argv[0], "remove") == 0)
+    {
+        long enterpriseNumber = 0;
+
+        VerifyOrExit(argc > 2, error = OT_ERROR_PARSE);
+
+        SuccessOrExit(error = ParseLong(argv[1], enterpriseNumber));
+
+        SuccessOrExit(error = otServerRemoveService(mInstance, static_cast<uint32_t>(enterpriseNumber),
+                                                    reinterpret_cast<uint8_t *>(argv[2]), static_cast<uint8_t>(strlen(argv[2]))));
+    }
+
+exit:
+    AppendResult(error);
+}
+#endif
+
+#if OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
+void Interpreter::ProcessNetworkDataRegister(int argc, char *argv[])
+{
+    otError error = OT_ERROR_NONE;
+#if OPENTHREAD_ENABLE_BORDER_ROUTER
+    SuccessOrExit(error = otBorderRouterRegister(mInstance));
+#else
+    SuccessOrExit(error = otServerRegister(mInstance));
+#endif
+
+exit:
+    OT_UNUSED_VARIABLE(argc);
+    OT_UNUSED_VARIABLE(argv);
+    AppendResult(error);
+}
+#endif  // OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
 
 #if OPENTHREAD_FTD
 void Interpreter::ProcessNetworkIdTimeout(int argc, char *argv[])

src/cli/cli.hpp

@@ -249,15 +249,18 @@ private:
     void ProcessLeaderPartitionId(int argc, char *argv[]);
     void ProcessLeaderWeight(int argc, char *argv[]);
 #endif
-    void ProcessLinkQuality(int argc, char *argv[]);
     void ProcessMasterKey(int argc, char *argv[]);
     void ProcessMode(int argc, char *argv[]);
 #if OPENTHREAD_FTD
     void ProcessNeighbor(int argc, char *argv[]);
 #endif
-#if OPENTHREAD_ENABLE_BORDER_ROUTER
+#if OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
     void ProcessNetworkDataRegister(int argc, char *argv[]);
 #endif
+#if OPENTHREAD_ENABLE_SERVICE
+    void ProcessNetworkDataShow(int argc, char *argv[]);
+    void ProcessService(int argc, char *argv[]);
+#endif
 #if OPENTHREAD_FTD || OPENTHREAD_ENABLE_MTD_NETWORK_DIAGNOSTIC
     void ProcessNetworkDiagnostic(int argc, char *argv[]);
 #endif // OPENTHREAD_FTD || OPENTHREAD_ENABLE_MTD_NETWORK_DIAGNOSTIC

src/core/Makefile.am

@@ -106,6 +106,7 @@ SOURCES_COMMON                      = \
     api/link_raw_api.cpp              \
     api/message_api.cpp               \
     api/netdata_api.cpp               \
+    api/server_api.cpp                \
     api/tasklet_api.cpp               \
     api/thread_api.cpp                \
     api/thread_ftd_api.cpp            \

src/core/api/server_api.cpp

@@ -0,0 +1,88 @@
+/*
+ *  Copyright (c) 2016-2017, The OpenThread Authors.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *  2. Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *  3. Neither the name of the copyright holder nor the
+ *     names of its contributors may be used to endorse or promote products
+ *     derived from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/**
+ * @file
+ *   This file implements the OpenThread Server API.
+ */
+
+#include <openthread/config.h>
+
+#if OPENTHREAD_ENABLE_SERVICE
+
+#include <openthread/server.h>
+
+#include "openthread-instance.h"
+
+using namespace ot;
+
+otError otServerGetNetDataLocal(otInstance *aInstance, bool aStable, uint8_t *aData, uint8_t *aDataLength)
+{
+    otError error = OT_ERROR_NONE;
+
+    VerifyOrExit(aData != NULL && aDataLength != NULL, error = OT_ERROR_INVALID_ARGS);
+
+    error = aInstance->mThreadNetif.GetNetworkDataLocal().GetNetworkData(aStable, aData, *aDataLength);
+
+exit:
+    return error;
+}
+
+otError otServerAddService(otInstance *aInstance, const otServiceConfig *aConfig)
+{
+    return aInstance->mThreadNetif.GetNetworkDataLocal().AddService(
+               aConfig->mEnterpriseNumber, &aConfig->mServiceData[0], aConfig->mServiceDataLength,
+               aConfig->mServerConfig.mStable, &aConfig->mServerConfig.mServerData[0], aConfig->mServerConfig.mServerDataLength
+           );
+}
+
+otError otServerRemoveService(otInstance *aInstance, uint32_t aEnterpriseNumber, uint8_t *aServiceData,
+                              uint8_t aServiceDataLength)
+{
+    return aInstance->mThreadNetif.GetNetworkDataLocal().RemoveService(aEnterpriseNumber, aServiceData, aServiceDataLength);
+}
+
+otError otServerGetNextService(otInstance *aInstance, otNetworkDataIterator *aIterator,
+                               otServiceConfig *aConfig)
+{
+    otError error = OT_ERROR_NONE;
+
+    VerifyOrExit(aIterator && aConfig, error = OT_ERROR_INVALID_ARGS);
+
+    error = aInstance->mThreadNetif.GetNetworkDataLocal().GetNextService(aIterator, aConfig);
+
+exit:
+    return error;
+}
+
+otError otServerRegister(otInstance *aInstance)
+{
+    return aInstance->mThreadNetif.GetNetworkDataLocal().SendServerDataNotification();
+}
+
+#endif  // OPENTHREAD_ENABLE_SERVICE

src/core/openthread-core-default-config.h

@@ -281,6 +281,16 @@
 #define OPENTHREAD_CONFIG_MAX_EXT_MULTICAST_IP_ADDRS            2
 #endif
 
+/**
+ * @def OPENTHREAD_CONFIG_MAX_SERVER_ALOCS
+ *
+ * The maximum number of supported Service ALOCs registrations for this device.
+ *
+ */
+#ifndef OPENTHREAD_CONFIG_MAX_SERVER_ALOCS
+#define OPENTHREAD_CONFIG_MAX_SERVER_ALOCS                      1
+#endif
+
 /**
  * @def OPENTHREAD_CONFIG_6LOWPAN_REASSEMBLY_TIMEOUT
  *

src/core/thread/mesh_forwarder.cpp

@@ -922,9 +922,16 @@ otError MeshForwarder::UpdateIp6Route(Message &aMessage)
             }
 
 #endif  // OPENTHREAD_ENABLE_DHCP6_SERVER || OPENTHREAD_ENABLE_DHCP6_CLIENT
+#if OPENTHREAD_ENABLE_SERVICE
+            else if ((aloc16 >= Mle::kAloc16ServiceStart) && (aloc16 <= Mle::kAloc16ServiceEnd))
+            {
+                SuccessOrExit(error = GetDestinationRlocByServiceAloc(aloc16, mMeshDest));
+            }
+
+#endif
             else
             {
-                // TODO: support ALOC for Service, Commissioner, Neighbor Discovery Agent
+                // TODO: support ALOC for Commissioner, Neighbor Discovery Agent
                 ExitNow(error = OT_ERROR_DROP);
             }
         }
@@ -981,6 +988,69 @@ exit:
     return error;
 }
 
+#if OPENTHREAD_FTD
+#if OPENTHREAD_ENABLE_SERVICE
+otError MeshForwarder::GetDestinationRlocByServiceAloc(uint16_t aServiceAloc, uint16_t &aMeshDest)
+{
+    otError error = OT_ERROR_NONE;
+    ThreadNetif &netif = GetNetif();
+    uint8_t serviceId = netif.GetMle().GetServiceIdFromAloc(aServiceAloc);
+    NetworkData::ServiceTlv *serviceTlv = netif.GetNetworkDataLeader().FindServiceById(serviceId);
+
+    if (serviceTlv != NULL)
+    {
+        NetworkData::NetworkDataTlv *cur = serviceTlv->GetSubTlvs();
+        NetworkData::NetworkDataTlv *end = serviceTlv->GetNext();
+        NetworkData::ServerTlv *server;
+        uint8_t bestCost = Mle::kMaxRouteCost;
+        uint8_t curCost = 0x00;
+        uint16_t bestDest = Mac::kShortAddrInvalid;
+
+        while (cur < end)
+        {
+            switch (cur->GetType())
+            {
+            case NetworkData::NetworkDataTlv::kTypeServer:
+                server = static_cast<NetworkData::ServerTlv *>(cur);
+                curCost = netif.GetMle().GetCost(server->GetServer16());
+
+                if ((bestDest == Mac::kShortAddrInvalid) || (curCost < bestCost))
+                {
+                    bestDest = server->GetServer16();
+                    bestCost = curCost;
+                }
+
+                break;
+
+            default:
+                break;
+            }
+
+            cur = cur->GetNext();
+        }
+
+        if (bestDest != Mac::kShortAddrInvalid)
+        {
+            aMeshDest = bestDest;
+        }
+        else
+        {
+            // ServiceTLV without ServerTLV? Can't forward packet anywhere.
+            ExitNow(error = OT_ERROR_DROP);
+        }
+    }
+    else
+    {
+        // Unknown service, can't forward
+        ExitNow(error = OT_ERROR_DROP);
+    }
+
+exit:
+    return error;
+}
+#endif // OPENTHREAD_ENABLE_SERVICE
+#endif // OPENTHREAD_FTD
+
 void MeshForwarder::SetRxOff(void)
 {
     ThreadNetif &netif = GetNetif();

src/core/thread/mesh_forwarder.hpp

@@ -310,9 +310,11 @@ private:
     void ScheduleTransmissionTask(void);
     static void HandleDataPollTimeout(Mac::Receiver &aReceiver);
 
-    otError AddPendingSrcMatchEntries(void);
-    otError AddSrcMatchEntry(Child &aChild);
-    void ClearSrcMatchEntry(Child &aChild);
+#if OPENTHREAD_FTD
+#if OPENTHREAD_ENABLE_SERVICE
+    otError GetDestinationRlocByServiceAloc(uint16_t aServiceAloc, uint16_t &aMeshDest);
+#endif // OPENTHREAD_ENABLE_SERVICE
+#endif // OPENTHREAD_FTD
 
     static MeshForwarder &GetOwner(const Context &aContext);
 

src/core/thread/mle.cpp

@@ -96,6 +96,7 @@ Mle::Mle(otInstance &aInstance) :
     mPreviousPanId(Mac::kPanIdBroadcast)
 {
     uint8_t meshLocalPrefix[8];
+    size_t i = 0;
 
     memset(&mLeaderData, 0, sizeof(mLeaderData));
     memset(&mParentLeaderData, 0, sizeof(mParentLeaderData));
@@ -124,6 +125,23 @@ Mle::Mle(otInstance &aInstance) :
     mLeaderAloc.mScopeOverride = Ip6::Address::kRealmLocalScope;
     mLeaderAloc.mScopeOverrideValid = true;
 
+#if OPENTHREAD_ENABLE_SERVICE
+
+    // Service Alocs
+    for (i = 0; i < sizeof(mServiceAlocs) / sizeof(mServiceAlocs[0]); i++)
+    {
+        memset(&mServiceAlocs[i], 0, sizeof(mServiceAlocs[i]));
+
+        mServiceAlocs[i].mPrefixLength = 128;
+        mServiceAlocs[i].mPreferred = true;
+        mServiceAlocs[i].mValid = true;
+        mServiceAlocs[i].mScopeOverride = Ip6::Address::kRealmLocalScope;
+        mServiceAlocs[i].mScopeOverrideValid = true;
+        mServiceAlocs[i].GetAddress().mFields.m16[7] = HostSwap16(Mac::kShortAddrInvalid);
+    }
+
+#endif
+
     // initialize Mesh Local Prefix
     meshLocalPrefix[0] = 0xfd;
     memcpy(meshLocalPrefix + 1, GetNetif().GetMac().GetExtendedPanId(), 5);
@@ -131,7 +149,7 @@ Mle::Mle(otInstance &aInstance) :
     meshLocalPrefix[7] = 0x00;
 
     // mesh-local 64
-    for (int i = OT_IP6_PREFIX_SIZE; i < OT_IP6_ADDRESS_SIZE; i++)
+    for (i = OT_IP6_PREFIX_SIZE; i < OT_IP6_ADDRESS_SIZE; i++)
     {
         mMeshLocal64.GetAddress().mFields.m8[i] = static_cast<uint8_t>(otPlatRandomGet());
     }
@@ -253,7 +271,7 @@ otError Mle::Stop(bool aClearNetworkDatasets)
     netif.GetKeyManager().Stop();
     SetStateDetached();
     netif.RemoveUnicastAddress(mMeshLocal16);
-#if OPENTHREAD_ENABLE_BORDER_ROUTER
+#if OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
     netif.GetNetworkDataLocal().Clear();
 #endif
     netif.GetNetworkDataLeader().Clear();
@@ -620,7 +638,7 @@ otError Mle::SetStateChild(uint16_t aRloc16)
         netif.GetMle().HandleChildStart(mParentRequestMode);
     }
 
-#if OPENTHREAD_ENABLE_BORDER_ROUTER
+#if OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
     netif.GetNetworkDataLocal().ClearResubmitDelayTimer();
 #endif
     netif.GetIp6().SetForwardingEnabled(false);
@@ -742,6 +760,20 @@ otError Mle::SetMeshLocalPrefix(const uint8_t *aMeshLocalPrefix)
     memcpy(mMeshLocal64.GetAddress().mFields.m8, aMeshLocalPrefix, 8);
     memcpy(mMeshLocal16.GetAddress().mFields.m8, mMeshLocal64.GetAddress().mFields.m8, 8);
 
+#if OPENTHREAD_ENABLE_SERVICE
+
+    for (uint8_t i = 0; i < sizeof(mServiceAlocs) / sizeof(mServiceAlocs[0]); i++)
+    {
+        if (HostSwap16(mServiceAlocs[i].GetAddress().mFields.m16[7]) != Mac::kShortAddrInvalid)
+        {
+            netif.RemoveUnicastAddress(mServiceAlocs[i]);
+            memcpy(mServiceAlocs[i].GetAddress().mFields.m8, mMeshLocal64.GetAddress().mFields.m8, 8);
+            netif.AddUnicastAddress(mServiceAlocs[i]);
+        }
+    }
+
+#endif
+
     mLinkLocalAllThreadNodes.GetAddress().mFields.m8[3] = 64;
     memcpy(mLinkLocalAllThreadNodes.GetAddress().mFields.m8 + 4, mMeshLocal64.GetAddress().mFields.m8, 8);
 
@@ -762,7 +794,8 @@ otError Mle::SetMeshLocalPrefix(const uint8_t *aMeshLocalPrefix)
     if (mRole == OT_DEVICE_ROLE_LEADER)
     {
         netif.RemoveUnicastAddress(mLeaderAloc);
-        AddLeaderAloc();
+        memcpy(mLeaderAloc.GetAddress().mFields.m8, mMeshLocal64.GetAddress().mFields.m8, 8);
+        netif.AddUnicastAddress(mLeaderAloc);
     }
 
     // Changing the prefix also causes the mesh local address to be different.
@@ -868,6 +901,24 @@ exit:
     return error;
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+otError Mle::GetServiceAloc(uint8_t aServiceId, Ip6::Address &aAddress) const
+{
+    otError error = OT_ERROR_NONE;
+
+    VerifyOrExit(GetRloc16() != Mac::kShortAddrInvalid, error = OT_ERROR_DETACHED);
+
+    memcpy(&aAddress, &mMeshLocal16.GetAddress(), 8);
+    aAddress.mFields.m16[4] = HostSwap16(0x0000);
+    aAddress.mFields.m16[5] = HostSwap16(0x00ff);
+    aAddress.mFields.m16[6] = HostSwap16(0xfe00);
+    aAddress.mFields.m16[7] = HostSwap16(GetServiceAlocFromId(aServiceId));
+
+exit:
+    return error;
+}
+#endif
+
 otError Mle::AddLeaderAloc(void)
 {
     otError error = OT_ERROR_NONE;
@@ -1263,8 +1314,11 @@ void Mle::HandleNetifStateChanged(uint32_t aFlags)
             mSendChildUpdateRequest.Post();
         }
 
-#if OPENTHREAD_ENABLE_BORDER_ROUTER
+#if OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
         netif.GetNetworkDataLocal().SendServerDataNotification();
+#if OPENTHREAD_ENABLE_SERVICE
+        this->UpdateServiceAlocs();
+#endif
 #endif
     }
 
@@ -1277,6 +1331,69 @@ exit:
     return;
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+void Mle::UpdateServiceAlocs(void)
+{
+    ThreadNetif &netif = GetNetif();
+    uint16_t rloc = GetRloc16();
+    uint16_t serviceAloc = 0;
+    uint8_t serviceId = 0;
+    int i = 0;
+    NetworkData::Leader &leaderData = netif.GetNetworkDataLeader();
+    otNetworkDataIterator serviceIterator = OT_NETWORK_DATA_ITERATOR_INIT;
+    int serviceAlocsLength = sizeof(mServiceAlocs) / sizeof(mServiceAlocs[0]);
+
+    VerifyOrExit(mRole != OT_DEVICE_ROLE_DISABLED);
+
+    // First remove all alocs which are no longer necessary, to free up space in mServiceAlocs
+    for (i = 0; i < serviceAlocsLength; i++)
+    {
+        serviceAloc = HostSwap16(mServiceAlocs[i].GetAddress().mFields.m16[7]);
+
+        if ((serviceAloc != Mac::kShortAddrInvalid) &&
+            (!leaderData.ContainsService(Mle::GetServiceIdFromAloc(serviceAloc), rloc)))
+        {
+            netif.RemoveUnicastAddress(mServiceAlocs[i]);
+            mServiceAlocs[i].GetAddress().mFields.m16[7] = HostSwap16(Mac::kShortAddrInvalid);
+        }
+    }
+
+    // Now add any missing service alocs which should be there, if there is enough space in mServiceAlocs
+    while (leaderData.GetNextServiceId(&serviceIterator, rloc, &serviceId) == OT_ERROR_NONE)
+    {
+        for (i = 0; i < serviceAlocsLength; i++)
+        {
+            serviceAloc = HostSwap16(mServiceAlocs[i].GetAddress().mFields.m16[7]);
+
+            if ((serviceAloc != Mac::kShortAddrInvalid) && (Mle::GetServiceIdFromAloc(serviceAloc) == serviceId))
+            {
+                break;
+            }
+        }
+
+        if (i >= serviceAlocsLength)
+        {
+            // Service Aloc is not there, but it should be. Lets add it into first empty space
+            for (i = 0; i < serviceAlocsLength; i++)
+            {
+                serviceAloc = HostSwap16(mServiceAlocs[i].GetAddress().mFields.m16[7]);
+
+                if (serviceAloc == Mac::kShortAddrInvalid)
+                {
+                    SuccessOrExit(GetServiceAloc(serviceId, mServiceAlocs[i].GetAddress()));
+                    netif.AddUnicastAddress(mServiceAlocs[i]);
+                    break;
+                }
+            }
+
+        }
+    }
+
+exit:
+    return;
+}
+#endif
+
 void Mle::HandleParentRequestTimer(Timer &aTimer)
 {
     GetOwner(aTimer).HandleParentRequestTimer();

src/core/thread/mle.hpp

@@ -123,6 +123,16 @@ enum AlocAllocation
     kAloc16NeighborDiscoveryAgentEnd    = 0xfc4e,
 };
 
+/**
+ * Service IDs
+ *
+ */
+enum ServiceID
+{
+    kServiceMinId = 0x00,  ///< Minimal Service ID.
+    kServiceMaxId = 0x0f,  ///< Maximal Service ID.
+};
+
 /**
  * This class implements MLE Header generation and parsing.
  *
@@ -795,6 +805,20 @@ public:
      */
     otError GetLeaderAloc(Ip6::Address &aAddress) const;
 
+#if OPENTHREAD_ENABLE_SERVICE
+    /**
+     * This method retrieves the Service ALOC for given Service ID.
+     *
+     * @param[in]   aServiceID Service ID to get ALOC for.
+     * @param[out]  aAddress   A reference to the Service ALOC.
+     *
+     * @retval OT_ERROR_NONE      Successfully retrieved the Service ALOC.
+     * @retval OT_ERROR_DETACHED  The Thread interface is not currently attached to a Thread Partition.
+     *
+     */
+    otError GetServiceAloc(uint8_t aServiceId, Ip6::Address &aAddress) const;
+#endif
+
     /**
      * This method adds Leader's ALOC to its Thread interface.
      *
@@ -844,6 +868,26 @@ public:
      */
     static uint8_t GetRouterId(uint16_t aRloc16) { return aRloc16 >> kRouterIdOffset; }
 
+    /**
+     * This method returns the Service ID corresponding to a Service ALOC16.
+     *
+     * @param[in]  aAloc16  The Servicer ALOC16 value.
+     *
+     * @returns The Service ID corresponding to given ALOC16.
+     *
+     */
+    static uint8_t GetServiceIdFromAloc(uint16_t aAloc16) { return static_cast<uint8_t>(aAloc16 - kAloc16ServiceStart); }
+
+    /**
+     * This method returns the Service Aloc corresponding to a Service ID.
+     *
+     * @param[in]  aAloc16  The Servicer ID value.
+     *
+     * @returns The Service ALOC16 corresponding to given ID.
+     *
+     */
+    static uint16_t GetServiceAlocFromId(uint8_t aServiceId) { return static_cast<uint16_t>(aServiceId + kAloc16ServiceStart); }
+
     /**
      * This method returns the RLOC16 of a given Router ID.
      *
@@ -1382,6 +1426,14 @@ private:
                         ConnectivityTlv &aConnectivityTlv);
     void ResetParentCandidate(void);
 
+#if OPENTHREAD_ENABLE_SERVICE
+    /**
+     * This method scans for network data from the leader and updates ip addresses assigned to this
+     * interface to make sure that all Service ALOCs (0xfc10-0xfc1f) are properly set.
+     */
+    void UpdateServiceAlocs(void);
+#endif
+
 #if OPENTHREAD_CONFIG_INFORM_PREVIOUS_PARENT_ON_REATTACH
     otError InformPreviousParent(void);
 #endif
@@ -1433,6 +1485,10 @@ private:
 
     Ip6::NetifUnicastAddress mLeaderAloc;
 
+#if OPENTHREAD_ENABLE_SERVICE
+    Ip6::NetifUnicastAddress mServiceAlocs[OPENTHREAD_CONFIG_MAX_SERVER_ALOCS];
+#endif
+
     Ip6::NetifUnicastAddress mLinkLocal64;
     Ip6::NetifUnicastAddress mMeshLocal64;
     Ip6::NetifUnicastAddress mMeshLocal16;

src/core/thread/mle_router.cpp

@@ -1541,7 +1541,7 @@ otError MleRouter::HandleAdvertisement(const Message &aMessage, const Ip6::Messa
 
     UpdateRoutes(route, routerId);
 
-#if OPENTHREAD_ENABLE_BORDER_ROUTER
+#if OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
     netif.GetNetworkDataLocal().SendServerDataNotification();
 #endif
 

src/core/thread/network_data.cpp

@@ -234,6 +234,150 @@ exit:
     return error;
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+otError NetworkData::GetNextService(otNetworkDataIterator *aIterator, otServiceConfig *aConfig)
+{
+    return GetNextService(aIterator, Mac::kShortAddrBroadcast, aConfig);
+}
+
+otError NetworkData::GetNextService(otNetworkDataIterator *aIterator, uint16_t aRloc16,
+                                    otServiceConfig *aConfig)
+{
+    otError error = OT_ERROR_NOT_FOUND;
+    NetworkDataIterator iterator(aIterator);
+    NetworkDataTlv *cur = reinterpret_cast<NetworkDataTlv *>(mTlvs + iterator.GetTlvOffset());
+    NetworkDataTlv *end = reinterpret_cast<NetworkDataTlv *>(mTlvs + mLength);
+
+    for (; cur < end; cur = cur->GetNext(), iterator.SetSubTlvOffset(0))
+    {
+        ServiceTlv *service;
+        NetworkDataTlv *subCur;
+        NetworkDataTlv *subEnd;
+
+        VerifyOrExit((cur + 1) <= end && cur->GetNext() <= end, error = OT_ERROR_PARSE);
+
+        if (cur->GetType() != NetworkDataTlv::kTypeService)
+        {
+            continue;
+        }
+
+        service = static_cast<ServiceTlv *>(cur);
+
+        subCur = reinterpret_cast<NetworkDataTlv *>(reinterpret_cast<uint8_t *>(service->GetSubTlvs())
+                                                    + iterator.GetSubTlvOffset());
+        subEnd = cur->GetNext();
+
+        for (; subCur < subEnd; subCur = subCur->GetNext())
+        {
+            ServerTlv *server;
+
+            VerifyOrExit((subCur + 1) <= subEnd && subCur->GetNext() <= subEnd, error = OT_ERROR_PARSE);
+
+            if (subCur->GetType() != NetworkDataTlv::kTypeServer)
+            {
+                continue;
+            }
+
+            server = static_cast<ServerTlv *>(subCur);
+
+            if ((aRloc16 == Mac::kShortAddrBroadcast) || (server->GetServer16() == aRloc16))
+            {
+                memset(aConfig, 0, sizeof(*aConfig));
+
+                aConfig->mEnterpriseNumber = service->GetEnterpriseNumber();
+                aConfig->mServiceDataLength = service->GetServiceDataLength();
+
+                memcpy(&aConfig->mServiceData, service->GetServiceData(), service->GetServiceDataLength());
+
+                aConfig->mServerConfig.mStable = server->IsStable();
+                aConfig->mServerConfig.mServerDataLength = server->GetServerDataLength();
+                memcpy(&aConfig->mServerConfig.mServerData, server->GetServerData(), server->GetServerDataLength());
+                aConfig->mServerConfig.mRloc16 = server->GetServer16();
+
+                if (subCur->GetNext() >= cur->GetNext())
+                {
+                    iterator.SaveTlvOffset(cur->GetNext(), mTlvs);
+                    iterator.SetSubTlvOffset(0);
+                }
+                else
+                {
+                    iterator.SaveTlvOffset(cur, mTlvs);
+                    iterator.SaveSubTlvOffset(subCur->GetNext(), service->GetSubTlvs());
+                }
+
+                ExitNow(error = OT_ERROR_NONE);
+            }
+        }
+    }
+
+exit:
+    return error;
+}
+
+otError NetworkData::GetNextServiceId(otNetworkDataIterator *aIterator, uint16_t aRloc16, uint8_t *aServiceId)
+{
+    otError error = OT_ERROR_NOT_FOUND;
+    NetworkDataIterator iterator(aIterator);
+    NetworkDataTlv *cur = reinterpret_cast<NetworkDataTlv *>(mTlvs + iterator.GetTlvOffset());
+    NetworkDataTlv *end = reinterpret_cast<NetworkDataTlv *>(mTlvs + mLength);
+
+    for (; cur < end; cur = cur->GetNext(), iterator.SetSubTlvOffset(0))
+    {
+        ServiceTlv *service;
+        NetworkDataTlv *subCur;
+        NetworkDataTlv *subEnd;
+
+        VerifyOrExit((cur + 1) <= end && cur->GetNext() <= end, error = OT_ERROR_PARSE);
+
+        if (cur->GetType() != NetworkDataTlv::kTypeService)
+        {
+            continue;
+        }
+
+        service = static_cast<ServiceTlv *>(cur);
+
+        subCur = reinterpret_cast<NetworkDataTlv *>(reinterpret_cast<uint8_t *>(service->GetSubTlvs())
+                                                    + iterator.GetSubTlvOffset());
+        subEnd = cur->GetNext();
+
+        for (; subCur < subEnd; subCur = subCur->GetNext())
+        {
+            ServerTlv *server;
+
+            VerifyOrExit((subCur + 1) <= subEnd && subCur->GetNext() <= subEnd, error = OT_ERROR_PARSE);
+
+            if (subCur->GetType() != NetworkDataTlv::kTypeServer)
+            {
+                continue;
+            }
+
+            server = static_cast<ServerTlv *>(subCur);
+
+            if ((aRloc16 == Mac::kShortAddrBroadcast) || (server->GetServer16() == aRloc16))
+            {
+                *aServiceId = service->GetServiceID();
+
+                if (subCur->GetNext() >= cur->GetNext())
+                {
+                    iterator.SaveTlvOffset(cur->GetNext(), mTlvs);
+                    iterator.SetSubTlvOffset(0);
+                }
+                else
+                {
+                    iterator.SaveTlvOffset(cur, mTlvs);
+                    iterator.SaveSubTlvOffset(subCur->GetNext(), service->GetSubTlvs());
+                }
+
+                ExitNow(error = OT_ERROR_NONE);
+            }
+        }
+    }
+
+exit:
+    return error;
+}
+#endif
+
 bool NetworkData::ContainsOnMeshPrefixes(NetworkData &aCompare, uint16_t aRloc16)
 {
     otNetworkDataIterator outerIterator = OT_NETWORK_DATA_ITERATOR_INIT;
@@ -294,11 +438,103 @@ exit:
     return rval;
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+bool NetworkData::ContainsServices(NetworkData &aCompare, uint16_t aRloc16)
+{
+    otNetworkDataIterator outerIterator = OT_NETWORK_DATA_ITERATOR_INIT;
+    otServiceConfig outerConfig;
+    bool rval = true;
+
+    while (aCompare.GetNextService(&outerIterator, aRloc16, &outerConfig) == OT_ERROR_NONE)
+    {
+        otNetworkDataIterator innerIterator = OT_NETWORK_DATA_ITERATOR_INIT;
+        otServiceConfig innerConfig;
+        otError error;
+
+        while ((error = GetNextService(&innerIterator, aRloc16, &innerConfig)) == OT_ERROR_NONE)
+        {
+            if ((outerConfig.mEnterpriseNumber == innerConfig.mEnterpriseNumber) &&
+                (outerConfig.mServiceDataLength == innerConfig.mServiceDataLength)
+                && (memcmp(outerConfig.mServiceData, innerConfig.mServiceData, outerConfig.mServiceDataLength) == 0) &&
+                (outerConfig.mServerConfig.mStable == innerConfig.mServerConfig.mStable)
+                && (outerConfig.mServerConfig.mServerDataLength == innerConfig.mServerConfig.mServerDataLength) &&
+                (memcmp(outerConfig.mServerConfig.mServerData, innerConfig.mServerConfig.mServerData,
+                        outerConfig.mServerConfig.mServerDataLength) == 0))
+            {
+                break;
+            }
+        }
+
+        if (error != OT_ERROR_NONE)
+        {
+            ExitNow(rval = false);
+        }
+    }
+
+exit:
+    return rval;
+}
+
+bool NetworkData::ContainsService(uint8_t aServiceId, uint16_t aRloc16)
+{
+    bool rval = false;
+    NetworkDataTlv *cur = reinterpret_cast<NetworkDataTlv *>(mTlvs);
+    NetworkDataTlv *end = reinterpret_cast<NetworkDataTlv *>(mTlvs + mLength);
+
+    for (; cur < end; cur = cur->GetNext())
+    {
+        ServiceTlv *service;
+        NetworkDataTlv *subCur;
+        NetworkDataTlv *subEnd;
+
+        VerifyOrExit((cur + 1) <= end && cur->GetNext() <= end);
+
+        if (cur->GetType() != NetworkDataTlv::kTypeService)
+        {
+            continue;
+        }
+
+        service = static_cast<ServiceTlv *>(cur);
+
+        if (service->GetServiceID() == aServiceId)
+        {
+            subCur = reinterpret_cast<NetworkDataTlv *>(reinterpret_cast<uint8_t *>(service->GetSubTlvs()));
+            subEnd = cur->GetNext();
+
+            for (; subCur < subEnd; subCur = subCur->GetNext())
+            {
+                ServerTlv *server;
+
+                VerifyOrExit((subCur + 1) <= subEnd && subCur->GetNext() <= subEnd);
+
+                if (subCur->GetType() != NetworkDataTlv::kTypeServer)
+                {
+                    continue;
+                }
+
+                server = static_cast<ServerTlv *>(subCur);
+
+                if (server->GetServer16() == aRloc16)
+                {
+                    ExitNow(rval = true);
+                }
+            }
+        }
+    }
+
+exit:
+    return rval;
+}
+#endif
+
 void NetworkData::RemoveTemporaryData(uint8_t *aData, uint8_t &aDataLength)
 {
     NetworkDataTlv *cur = reinterpret_cast<NetworkDataTlv *>(aData);
     NetworkDataTlv *end;
     PrefixTlv *prefix;
+#if OPENTHREAD_ENABLE_SERVICE
+    ServiceTlv *service;
+#endif
     uint8_t length;
     uint8_t *dst;
     uint8_t *src;
@@ -333,6 +569,29 @@ void NetworkData::RemoveTemporaryData(uint8_t *aData, uint8_t &aDataLength)
             break;
         }
 
+#if OPENTHREAD_ENABLE_SERVICE
+
+        case NetworkDataTlv::kTypeService:
+        {
+            service = reinterpret_cast<ServiceTlv *>(cur);
+            RemoveTemporaryData(aData, aDataLength, *service);
+
+            if (service->GetSubTlvsLength() == 0)
+            {
+                length = sizeof(NetworkDataTlv) + cur->GetLength();
+                dst = reinterpret_cast<uint8_t *>(cur);
+                src = reinterpret_cast<uint8_t *>(cur->GetNext());
+                memmove(dst, src, aDataLength - static_cast<size_t>(src - aData));
+                aDataLength -= length;
+                continue;
+            }
+
+            otDumpDebgNetData(GetInstance(), "remove service done", mTlvs, mLength);
+            break;
+        }
+
+#endif
+
         default:
         {
             // remove temporary tlv
@@ -448,6 +707,59 @@ void NetworkData::RemoveTemporaryData(uint8_t *aData, uint8_t &aDataLength, Pref
     }
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+void NetworkData::RemoveTemporaryData(uint8_t *aData, uint8_t &aDataLength, ServiceTlv &aService)
+{
+    NetworkDataTlv *cur = aService.GetSubTlvs();
+    NetworkDataTlv *end;
+    ServerTlv *server;
+    uint8_t length;
+    uint8_t *dst;
+    uint8_t *src;
+
+    while (1)
+    {
+        end = aService.GetNext();
+
+        if (cur >= end)
+        {
+            break;
+        }
+
+        if (cur->IsStable())
+        {
+            switch (cur->GetType())
+            {
+            case NetworkDataTlv::kTypeServer:
+            {
+                server = reinterpret_cast<ServerTlv *>(cur);
+                server->SetServer16(Mle::Mle::GetServiceAlocFromId(aService.GetServiceID()));
+                break;
+            }
+
+            default:
+            {
+                break;
+            }
+            }
+
+            // keep stable tlv
+            cur = cur->GetNext();
+        }
+        else
+        {
+            // remove temporary tlv
+            length = sizeof(NetworkDataTlv) + cur->GetLength();
+            dst = reinterpret_cast<uint8_t *>(cur);
+            src = reinterpret_cast<uint8_t *>(cur->GetNext());
+            memmove(dst, src, aDataLength - static_cast<size_t>(src - aData));
+            aService.SetSubTlvsLength(aService.GetSubTlvsLength() - length);
+            aDataLength -= length;
+        }
+    }
+}
+#endif
+
 BorderRouterTlv *NetworkData::FindBorderRouter(PrefixTlv &aPrefix)
 {
     BorderRouterTlv *rval = NULL;
@@ -622,6 +934,43 @@ int8_t NetworkData::PrefixMatch(const uint8_t *a, const uint8_t *b, uint8_t aLen
     return (rval >= aLength) ? rval : -1;
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+ServiceTlv *NetworkData::FindService(uint32_t aEnterpriseNumber, const uint8_t *aServiceData,
+                                     uint8_t aServiceDataLength)
+{
+    return FindService(aEnterpriseNumber, aServiceData, aServiceDataLength, mTlvs, mLength);
+}
+
+ServiceTlv *NetworkData::FindService(uint32_t aEnterpriseNumber, const uint8_t *aServiceData,
+                                     uint8_t aServiceDataLength, uint8_t *aTlvs, uint8_t aTlvsLength)
+{
+    NetworkDataTlv *cur = reinterpret_cast<NetworkDataTlv *>(aTlvs);
+    NetworkDataTlv *end = reinterpret_cast<NetworkDataTlv *>(aTlvs + aTlvsLength);
+    ServiceTlv *compare;
+
+    while (cur < end)
+    {
+        VerifyOrExit((cur + 1) <= end && cur->GetNext() <= end);
+
+        if (cur->GetType() == NetworkDataTlv::kTypeService)
+        {
+            compare = reinterpret_cast<ServiceTlv *>(cur);
+
+            if ((compare->GetEnterpriseNumber() == aEnterpriseNumber) && (compare->GetServiceDataLength() == aServiceDataLength)
+                && (memcmp(compare->GetServiceData(), aServiceData, aServiceDataLength) == 0))
+            {
+                return compare;
+            }
+        }
+
+        cur = cur->GetNext();
+    }
+
+exit:
+    return NULL;
+}
+#endif
+
 otError NetworkData::Insert(uint8_t *aStart, uint8_t aLength)
 {
     assert(aLength + mLength <= sizeof(mTlvs) &&

src/core/thread/network_data.hpp

@@ -174,6 +174,48 @@ public:
     otError GetNextExternalRoute(otNetworkDataIterator *aIterator, uint16_t aRloc16,
                                  otExternalRouteConfig *aConfig);
 
+#if OPENTHREAD_ENABLE_SERVICE
+    /**
+     * This method provides the next service in the Thread Network Data.
+     *
+     * @param[inout]  aIterator  A pointer to the Network Data iterator context.
+     * @param[out]    aConfig    A pointer to where the service information will be placed.
+     *
+     * @retval OT_ERROR_NONE       Successfully found the next service.
+     * @retval OT_ERROR_NOT_FOUND  No subsequent service exists in the Thread Network Data.
+     *
+     */
+    otError GetNextService(otNetworkDataIterator *aIterator, otServiceConfig *aConfig);
+
+    /**
+     * This method provides the next service in the Thread Network Data for a given RLOC16.
+     *
+     * @param[inout]  aIterator  A pointer to the Network Data iterator context.
+     * @param[in]     aRloc16    The RLOC16 value.
+     * @param[out]    aConfig    A pointer to where the service information will be placed.
+     *
+     * @retval OT_ERROR_NONE       Successfully found the next service.
+     * @retval OT_ERROR_NOT_FOUND  No subsequent service exists in the Thread Network Data.
+     *
+     */
+    otError GetNextService(otNetworkDataIterator *aIterator, uint16_t aRloc16,
+                           otServiceConfig *aConfig);
+
+    /**
+     * This method provides the next service ID in the Thread Network Data for a given RLOC16.
+     *
+     * @param[inout]  aIterator  A pointer to the Network Data iterator context.
+     * @param[in]     aRloc16    The RLOC16 value.
+     * @param[out]    aServiceID A pointer to where the service ID will be placed.
+     *
+     * @retval OT_ERROR_NONE       Successfully found the next service.
+     * @retval OT_ERROR_NOT_FOUND  No subsequent service exists in the Thread Network Data.
+     *
+     */
+    otError GetNextServiceId(otNetworkDataIterator *aIterator, uint16_t aRloc16,
+                             uint8_t *aServiceId);
+#endif
+
     /**
      * This method indicates whether or not the Thread Network Data contains all of the on mesh prefix information
      * in @p aCompare associated with @p aRloc16.
@@ -200,6 +242,34 @@ public:
      */
     bool ContainsExternalRoutes(NetworkData &aCompare, uint16_t aRloc16);
 
+#if OPENTHREAD_ENABLE_SERVICE
+    /**
+     * This method indicates whether or not the Thread Network Data contains all of the service information
+     * in @p aCompare associated with @p aRloc16.
+     *
+     * @param[in]  aCompare  The Network Data to use for the query.
+     * @param[in]  aRloc16   The RLOC16 to consider.
+     *
+     * @returns TRUE if this object contains all service information in @p aCompare associated with @p aRloc16,
+     *          FALSE otherwise.
+     *
+     */
+    bool ContainsServices(NetworkData &aCompare, uint16_t aRloc16);
+
+    /**
+     * This method indicates whether or not the Thread Network Data contains the service with given Service ID
+     * associated with @p aRloc16.
+     *
+     * @param[in]  aServiceID The Service ID to search for.
+     * @param[in]  aRloc16    The RLOC16 to consider.
+     *
+     * @returns TRUE if this object contains the service with given ID associated with @p aRloc16,
+     *          FALSE otherwise.
+     *
+     */
+    bool ContainsService(uint8_t aServiceId, uint16_t aRloc16);
+#endif
+
     /**
      * This method cancels the data resubmit delay timer.
      *
@@ -283,6 +353,35 @@ protected:
      */
     PrefixTlv *FindPrefix(const uint8_t *aPrefix, uint8_t aPrefixLength, uint8_t *aTlvs, uint8_t aTlvsLength);
 
+#if OPENTHREAD_ENABLE_SERVICE
+    /**
+     * This method returns a pointer to a matching Service TLV.
+     *
+     * @param[in]  aEnterpriseNumber  Enterprise Number.
+     * @param[in]  aServiceData       A pointer to a Service Data.
+     * @param[in]  aServiceDataLength The Service Data length pointed to by @p aServiceData.
+     *
+     * @returns A pointer to the Service TLV is one is found or NULL if no matching Service TLV exists.
+     *
+     */
+    ServiceTlv *FindService(uint32_t aEnterpriseNumber, const uint8_t *aServiceData, uint8_t aServiceDataLength);
+
+    /**
+     * This method returns a pointer to a Service TLV in a specified tlvs buffer.
+     *
+     * @param[in]  aEnterpriseNumber  Enterprise Number.
+     * @param[in]  aServiceData       A pointer to an Service Data.
+     * @param[in]  aServiceDataLength The Service Data length pointed to by @p aServiceData.
+     * @param[in]  aTlvs              A pointer to a specified tlvs buffer.
+     * @param[in]  aTlvsLength        The specified tlvs buffer length pointed to by @p aTlvs.
+     *
+     * @returns A pointer to the Service TLV is one is found or NULL if no matching Service TLV exists.
+     *
+     */
+    ServiceTlv *FindService(uint32_t aEnterpriseNumber, const uint8_t *aServiceData, uint8_t aServiceDataLength,
+                            uint8_t *aTlvs, uint8_t aTlvsLength);
+#endif
+
     /**
      * This method inserts bytes into the Network Data.
      *
@@ -327,6 +426,19 @@ protected:
      */
     void RemoveTemporaryData(uint8_t *aData, uint8_t &aDataLength, PrefixTlv &aPrefix);
 
+#if OPENTHREAD_ENABLE_SERVICE
+    /**
+     * This method strips non-stable Sub-TLVs from a Service TLV.
+     *
+     * @param[inout]  aData        A pointer to the Network Data to modify.
+     * @param[inout]  aDataLength  On entry, the size of the Network Data in bytes.  On exit, the size of the
+     *                             resulting Network Data in bytes.
+     * @param[inout]  aService     A reference to the Service TLV to modify.
+     *
+     */
+    void RemoveTemporaryData(uint8_t *aData, uint8_t &aDataLength, ServiceTlv &aService);
+#endif
+
     /**
      * This method computes the number of IPv6 Prefix bits that match.
      *
@@ -364,7 +476,7 @@ private:
     private:
         enum
         {
-            kTlvPoistion    = 0,
+            kTlvPosition    = 0,
             kSubTlvPosition = 1,
             kEntryPosition  = 2,
         };
@@ -373,10 +485,10 @@ private:
         NetworkDataIterator(otNetworkDataIterator *aIterator):
             mIteratorBuffer(reinterpret_cast<uint8_t *>(aIterator)) { }
 
-        uint8_t GetTlvOffset(void) const         { return mIteratorBuffer[kTlvPoistion];       }
+        uint8_t GetTlvOffset(void) const         { return mIteratorBuffer[kTlvPosition];       }
         uint8_t GetSubTlvOffset(void) const      { return mIteratorBuffer[kSubTlvPosition];    }
         uint8_t GetEntryIndex(void) const        { return mIteratorBuffer[kEntryPosition];     }
-        void    SetTlvOffset(uint8_t aOffset)    { mIteratorBuffer[kTlvPoistion] = aOffset;    }
+        void    SetTlvOffset(uint8_t aOffset)    { mIteratorBuffer[kTlvPosition] = aOffset;    }
         void    SetSubTlvOffset(uint8_t aOffset) { mIteratorBuffer[kSubTlvPosition] = aOffset; }
         void    SetEntryIndex(uint8_t aIndex)    { mIteratorBuffer[kEntryPosition]  = aIndex;  }
 

src/core/thread/network_data_leader_ftd.cpp

@@ -443,12 +443,18 @@ otError Leader::RlocLookup(uint16_t aRloc16, bool &aIn, bool &aStable, uint8_t *
     HasRouteTlv *hasRoute;
     BorderRouterEntry *borderRouterEntry;
     HasRouteEntry *hasRouteEntry;
+#if OPENTHREAD_ENABLE_SERVICE
+    ServiceTlv *service;
+    ServerTlv *server;
+#endif
 
     while (cur < end)
     {
         VerifyOrExit((cur + 1) <= end && cur->GetNext() <= end, error = OT_ERROR_PARSE);
 
-        if (cur->GetType() == NetworkDataTlv::kTypePrefix)
+        switch (cur->GetType())
+        {
+        case NetworkDataTlv::kTypePrefix:
         {
             prefix = static_cast<PrefixTlv *>(cur);
             VerifyOrExit(prefix->IsValid(), error = OT_ERROR_PARSE);
@@ -516,6 +522,60 @@ otError Leader::RlocLookup(uint16_t aRloc16, bool &aIn, bool &aStable, uint8_t *
                 subCur = subCur->GetNext();
             }
         }
+        break;
+
+#if OPENTHREAD_ENABLE_SERVICE
+
+        case NetworkDataTlv::kTypeService:
+        {
+            service = static_cast<ServiceTlv *>(cur);
+
+            subCur = service->GetSubTlvs();
+            subEnd = service->GetNext();
+
+            VerifyOrExit(subEnd <= end, error = OT_ERROR_PARSE);
+
+            while (subCur < subEnd)
+            {
+                VerifyOrExit((subCur + 1) <= subEnd && subCur->GetNext() <= subEnd, error = OT_ERROR_PARSE);
+
+                switch (subCur->GetType())
+                {
+                case NetworkDataTlv::kTypeServer:
+                    server = static_cast<ServerTlv *>(subCur);
+
+                    if (server->GetServer16() == aRloc16)
+                    {
+                        aIn = true;
+
+                        if (server->IsStable())
+                        {
+                            aStable = true;
+                        }
+                    }
+
+                    break;
+
+                default:
+                    break;
+                }
+
+                if (aIn && aStable)
+                {
+                    ExitNow();
+                }
+
+                subCur = subCur->GetNext();
+            }
+
+            break;
+        }
+
+#endif
+
+        default:
+            break;
+        }
 
         cur = cur->GetNext();
     }
@@ -529,12 +589,17 @@ bool Leader::IsStableUpdated(uint8_t *aTlvs, uint8_t aTlvsLength, uint8_t *aTlvs
     bool rval = false;
     NetworkDataTlv *cur = reinterpret_cast<NetworkDataTlv *>(aTlvs);
     NetworkDataTlv *end = reinterpret_cast<NetworkDataTlv *>(aTlvs + aTlvsLength);
+#if OPENTHREAD_ENABLE_SERVICE
+    ServiceTlv *service;
+#endif
 
     while (cur < end)
     {
         VerifyOrExit((cur + 1) <= end && cur->GetNext() <= end);
 
-        if (cur->GetType() == NetworkDataTlv::kTypePrefix)
+        switch (cur->GetType())
+        {
+        case NetworkDataTlv::kTypePrefix:
         {
             PrefixTlv *prefix = static_cast<PrefixTlv *>(cur);
             ContextTlv *context = FindContext(*prefix);
@@ -555,7 +620,8 @@ bool Leader::IsStableUpdated(uint8_t *aTlvs, uint8_t aTlvsLength, uint8_t *aTlvs
                 {
                     BorderRouterTlv *borderRouterBase = FindBorderRouter(*prefixBase, true);
 
-                    if (!borderRouterBase || memcmp(borderRouter, borderRouterBase, borderRouter->GetLength()) != 0)
+                    if (!borderRouterBase ||
+                        memcmp(borderRouter, borderRouterBase, borderRouter->GetLength()) != 0)
                     {
                         ExitNow(rval = true);
                     }
@@ -571,6 +637,83 @@ bool Leader::IsStableUpdated(uint8_t *aTlvs, uint8_t aTlvsLength, uint8_t *aTlvs
                     }
                 }
             }
+
+            break;
+        }
+
+#if OPENTHREAD_ENABLE_SERVICE
+
+        case NetworkDataTlv::kTypeService:
+            service = static_cast<ServiceTlv *>(cur);
+
+            if (cur->IsStable())
+            {
+                NetworkDataTlv *curInner;
+                NetworkDataTlv *endInner;
+
+                ServiceTlv *serviceBase = FindService(service->GetEnterpriseNumber(), service->GetServiceData(),
+                                                      service->GetServiceDataLength(),
+                                                      aTlvsBase, aTlvsBaseLength);
+
+                if (!serviceBase || !serviceBase->IsStable())
+                {
+                    ExitNow(rval = true);
+                }
+
+                curInner = service->GetSubTlvs();
+                endInner = service->GetNext();
+
+                while (curInner < endInner)
+                {
+                    if (curInner->IsStable())
+                    {
+                        switch (curInner->GetType())
+                        {
+                        case NetworkDataTlv::kTypeServer:
+                        {
+                            bool foundInBase = false;
+                            ServerTlv *server = reinterpret_cast<ServerTlv *>(curInner);
+
+                            NetworkDataTlv *curServerBase = service->GetSubTlvs();
+                            NetworkDataTlv *endServerBase = service->GetNext();
+
+                            while (curServerBase <= endServerBase)
+                            {
+                                ServerTlv *serverBase = reinterpret_cast<ServerTlv *>(curServerBase);
+
+                                if (curServerBase->IsStable() && (server->GetServer16() == serverBase->GetServer16())
+                                    && (server->GetServerDataLength() == serverBase->GetServerDataLength())
+                                    && (memcmp(server->GetServerData(), serverBase->GetServerData(), server->GetServerDataLength()) == 0))
+                                {
+                                    foundInBase = true;
+                                    break;
+                                }
+
+                                curServerBase = curServerBase->GetNext();
+                            }
+
+                            if (!foundInBase)
+                            {
+                                ExitNow(rval = true);
+                            }
+
+                            break;
+                        }
+
+                        default:
+                            break;
+                        }
+                    }
+
+                    curInner = curInner->GetNext();
+                }
+            }
+
+            break;
+#endif
+
+        default:
+            break;
         }
 
         cur = cur->GetNext();
@@ -586,6 +729,8 @@ otError Leader::RegisterNetworkData(uint16_t aRloc16, uint8_t *aTlvs, uint8_t aT
     bool rlocIn = false;
     bool rlocStable = false;
     bool stableUpdated = false;
+    uint8_t oldTlvs[NetworkData::kMaxSize];
+    uint8_t oldTlvsLength = NetworkData::kMaxSize;
 
     RlocLookup(aRloc16, rlocIn, rlocStable, mTlvs, mLength);
 
@@ -597,8 +742,11 @@ otError Leader::RegisterNetworkData(uint16_t aRloc16, uint8_t *aTlvs, uint8_t aT
             stableUpdated = true;
         }
 
+        // Store old Service IDs for given rloc16, so updates to server will reuse the same Service ID
+        SuccessOrExit(error = GetNetworkData(false, oldTlvs, oldTlvsLength));
+
         SuccessOrExit(error = RemoveRloc(aRloc16));
-        SuccessOrExit(error = AddNetworkData(aTlvs, aTlvsLength));
+        SuccessOrExit(error = AddNetworkData(aTlvs, aTlvsLength, oldTlvs, oldTlvsLength));
 
         mVersion++;
 
@@ -610,7 +758,9 @@ otError Leader::RegisterNetworkData(uint16_t aRloc16, uint8_t *aTlvs, uint8_t aT
     else
     {
         SuccessOrExit(error = RlocLookup(aRloc16, rlocIn, rlocStable, aTlvs, aTlvsLength));
-        SuccessOrExit(error = AddNetworkData(aTlvs, aTlvsLength));
+
+        // No old data to be preserved, lets avoid memcpy() & FindService calls.
+        SuccessOrExit(error = AddNetworkData(aTlvs, aTlvsLength, oldTlvs, 0));
 
         mVersion++;
 
@@ -626,7 +776,7 @@ exit:
     return error;
 }
 
-otError Leader::AddNetworkData(uint8_t *aTlvs, uint8_t aTlvsLength)
+otError Leader::AddNetworkData(uint8_t *aTlvs, uint8_t aTlvsLength, uint8_t *aOldTlvs, uint8_t aOldTlvsLength)
 {
     otError error = OT_ERROR_NONE;
     NetworkDataTlv *cur = reinterpret_cast<NetworkDataTlv *>(aTlvs);
@@ -643,6 +793,14 @@ otError Leader::AddNetworkData(uint8_t *aTlvs, uint8_t aTlvsLength)
             otDumpDebgNetData(GetInstance(), "add prefix done", mTlvs, mLength);
             break;
 
+#if OPENTHREAD_ENABLE_SERVICE
+
+        case NetworkDataTlv::kTypeService:
+            SuccessOrExit(error = AddService(*static_cast<ServiceTlv *>(cur), aOldTlvs, aOldTlvsLength));
+            otDumpDebgNetData(GetInstance(), "add service done", mTlvs, mLength);
+            break;
+#endif
+
         default:
             break;
         }
@@ -650,6 +808,11 @@ otError Leader::AddNetworkData(uint8_t *aTlvs, uint8_t aTlvsLength)
         cur = cur->GetNext();
     }
 
+#if !OPENTHREAD_ENABLE_SERVICE
+    OT_UNUSED_VARIABLE(aOldTlvs);
+    OT_UNUSED_VARIABLE(aOldTlvsLength);
+#endif
+
     otDumpDebgNetData(GetInstance(), "add done", mTlvs, mLength);
 
 exit:
@@ -691,6 +854,38 @@ exit:
     return error;
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+otError Leader::AddService(ServiceTlv &aService, uint8_t *aOldTlvs, uint8_t aOldTlvsLength)
+{
+    otError error = OT_ERROR_NONE;
+    NetworkDataTlv *cur;
+    NetworkDataTlv *end;
+
+    cur = aService.GetSubTlvs();
+    end = aService.GetNext();
+
+    while (cur < end)
+    {
+        VerifyOrExit((cur + 1) <= end && cur->GetNext() <= end, error = OT_ERROR_PARSE);
+
+        switch (cur->GetType())
+        {
+        case NetworkDataTlv::kTypeServer:
+            SuccessOrExit(error = AddServer(aService, *static_cast<ServerTlv *>(cur), aOldTlvs, aOldTlvsLength));
+            break;
+
+        default:
+            break;
+        }
+
+        cur = cur->GetNext();
+    }
+
+exit:
+    return error;
+}
+#endif
+
 otError Leader::AddHasRoute(PrefixTlv &aPrefix, HasRouteTlv &aHasRoute)
 {
     otError error = OT_ERROR_NONE;
@@ -754,6 +949,119 @@ exit:
     return error;
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+otError Leader::AddServer(ServiceTlv &aService, ServerTlv &aServer, uint8_t *aOldTlvs, uint8_t aOldTlvsLength)
+{
+    otError error = OT_ERROR_NONE;
+    ServiceTlv *dstService = NULL;
+    ServiceTlv *oldService = NULL;
+    ServerTlv *dstServer = NULL;
+    uint16_t appendLength = 0;
+    uint8_t serviceID = 0;
+    uint8_t serviceInsertLength = sizeof(ServiceTlv) + sizeof(uint8_t)/*mServiceDataLength*/ +
+                                  ServiceTlv::GetEnterpriseNumberFieldLength(aService.GetEnterpriseNumber()) +
+                                  aService.GetServiceDataLength();
+
+    dstService = FindService(aService.GetEnterpriseNumber(), aService.GetServiceData(), aService.GetServiceDataLength());
+
+    if (dstService == NULL)
+    {
+        appendLength += serviceInsertLength;
+    }
+
+    appendLength += sizeof(ServerTlv) + aServer.GetServerDataLength();
+
+    VerifyOrExit(mLength + appendLength <= sizeof(mTlvs), error = OT_ERROR_NO_BUFS);
+
+    if (dstService == NULL)
+    {
+        // Try to preserve old Service ID, if existing
+        oldService = FindService(aService.GetEnterpriseNumber(), aService.GetServiceData(), aService.GetServiceDataLength(),
+                                 aOldTlvs, aOldTlvsLength);
+
+        if (oldService != NULL)
+        {
+            // The same service is not found in current data, but was in old data. So, it had to be just removed by RemoveRloc()
+            // Lets use the same ServiceID
+            serviceID = oldService->GetServiceID();
+        }
+        else
+        {
+            uint8_t i;
+
+            // This seems like completely new service. Lets try to find new ServiceID for it. If all are taken, error out.
+            // Since we call FindServiceById() on mTlv, we need to execute this before Insert() call, otherwise we'll find
+            // uninitialized service as well.
+            for (i = Mle::kServiceMinId; i <= Mle::kServiceMaxId; i++)
+            {
+                if (FindServiceById(i) == NULL)
+                {
+                    serviceID = i;
+                    break;
+                }
+            }
+
+            otLogInfoNetData(GetInstance(), "Allocated Service ID = %d", i);
+
+            VerifyOrExit(i <= Mle::kServiceMaxId, error = OT_ERROR_NO_BUFS);
+        }
+
+        dstService = reinterpret_cast<ServiceTlv *>(mTlvs + mLength);
+        Insert(reinterpret_cast<uint8_t *>(dstService), serviceInsertLength);
+        dstService->Init();
+        dstService->SetServiceID(serviceID);
+        dstService->SetEnterpriseNumber(aService.GetEnterpriseNumber());
+        dstService->SetServiceData(aService.GetServiceData(), aService.GetServiceDataLength());
+        dstService->SetLength(serviceInsertLength - sizeof(NetworkDataTlv));
+    }
+
+    dstServer = reinterpret_cast<ServerTlv *>(dstService->GetNext());
+
+    Insert(reinterpret_cast<uint8_t *>(dstServer), sizeof(ServerTlv) + aServer.GetServerDataLength());
+    dstServer->Init();
+    dstServer->SetServer16(aServer.GetServer16());
+    dstServer->SetServerData(aServer.GetServerData(), aServer.GetServerDataLength());
+
+    if (aServer.IsStable())
+    {
+        dstService->SetStable();
+        dstServer->SetStable();
+    }
+
+    dstService->SetLength(dstService->GetLength() + sizeof(ServerTlv) + aServer.GetServerDataLength());
+
+exit:
+    return error;
+}
+
+ServiceTlv *Leader::FindServiceById(uint8_t aServiceId)
+{
+    NetworkDataTlv *cur = reinterpret_cast<NetworkDataTlv *>(mTlvs);
+    NetworkDataTlv *end = reinterpret_cast<NetworkDataTlv *>(mTlvs + mLength);
+    ServiceTlv *compare = NULL;
+
+    while (cur < end)
+    {
+        VerifyOrExit((cur + 1) <= end && cur->GetNext() <= end);
+
+        if (cur->GetType() == NetworkDataTlv::kTypeService)
+        {
+            compare = reinterpret_cast<ServiceTlv *>(cur);
+
+            if (compare->GetServiceID() == aServiceId)
+            {
+                return compare;
+            }
+        }
+
+        cur = cur->GetNext();
+    }
+
+exit:
+    return NULL;
+}
+#endif
+
 otError Leader::AddBorderRouter(PrefixTlv &aPrefix, BorderRouterTlv &aBorderRouter)
 {
     otError error = OT_ERROR_NONE;
@@ -893,6 +1201,9 @@ otError Leader::RemoveRloc(uint16_t aRloc16)
     NetworkDataTlv *cur = reinterpret_cast<NetworkDataTlv *>(mTlvs);
     NetworkDataTlv *end;
     PrefixTlv *prefix;
+#if OPENTHREAD_ENABLE_SERVICE
+    ServiceTlv *service;
+#endif
 
     while (1)
     {
@@ -920,6 +1231,26 @@ otError Leader::RemoveRloc(uint16_t aRloc16)
             break;
         }
 
+#if OPENTHREAD_ENABLE_SERVICE
+
+        case NetworkDataTlv::kTypeService:
+        {
+            service = static_cast<ServiceTlv *>(cur);
+            RemoveRloc(*service, aRloc16);
+
+            if (service->GetSubTlvsLength() == 0)
+            {
+                Remove(reinterpret_cast<uint8_t *>(service), sizeof(NetworkDataTlv) + service->GetLength());
+                continue;
+            }
+
+            otDumpDebgNetData(GetInstance(), "remove service done", mTlvs, mLength);
+
+            break;
+        }
+
+#endif
+
         default:
             break;
         }
@@ -1006,6 +1337,49 @@ otError Leader::RemoveRloc(PrefixTlv &prefix, uint16_t aRloc16)
     return OT_ERROR_NONE;
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+otError Leader::RemoveRloc(ServiceTlv &service, uint16_t aRloc16)
+{
+    NetworkDataTlv *cur = service.GetSubTlvs();
+    NetworkDataTlv *end;
+    ServerTlv *server;
+    uint8_t removeLength;
+
+    while (1)
+    {
+        end = service.GetNext();
+
+        if (cur >= end)
+        {
+            break;
+        }
+
+        switch (cur->GetType())
+        {
+        case NetworkDataTlv::kTypeServer:
+            server = static_cast<ServerTlv *>(cur);
+
+            if (server->GetServer16() == aRloc16)
+            {
+                removeLength = sizeof(ServerTlv) + server->GetServerDataLength();
+                service.SetSubTlvsLength(service.GetSubTlvsLength() - removeLength);
+                Remove(reinterpret_cast<uint8_t *>(cur), removeLength);
+                continue;
+            }
+
+            break;
+
+        default:
+            break;
+        }
+
+        cur = cur->GetNext();
+    }
+
+    return OT_ERROR_NONE;
+}
+#endif
+
 otError Leader::RemoveRloc(PrefixTlv &aPrefix, HasRouteTlv &aHasRoute, uint16_t aRloc16)
 {
     HasRouteEntry *entry;

src/core/thread/network_data_leader_ftd.hpp

@@ -142,6 +142,17 @@ public:
      */
     otError SendServerDataNotification(uint16_t aRloc16);
 
+#if OPENTHREAD_ENABLE_SERVICE
+    /**
+     * This method scans network data for given service ID and returns pointer to the respective TLV, if present.
+     *
+     * @param aServiceId Service ID to look for.
+     * @return Pointer to the Service TLV for given Service ID, or NULL if not present.
+     *
+     */
+    ServiceTlv *FindServiceById(uint8_t aServiceId);
+#endif
+
 private:
     static void HandleServerData(void *aContext, otCoapHeader *aHeader, otMessage *aMessage,
                                  const otMessageInfo *aMessageInfo);
@@ -154,8 +165,12 @@ private:
 
     otError AddHasRoute(PrefixTlv &aPrefix, HasRouteTlv &aHasRoute);
     otError AddBorderRouter(PrefixTlv &aPrefix, BorderRouterTlv &aBorderRouter);
-    otError AddNetworkData(uint8_t *aTlv, uint8_t aTlvLength);
+    otError AddNetworkData(uint8_t *aTlvs, uint8_t aTlvsLength, uint8_t *aOldTlvs, uint8_t aOldTlvsLength);
     otError AddPrefix(PrefixTlv &aTlv);
+#if OPENTHREAD_ENABLE_SERVICE
+    otError AddServer(ServiceTlv &aService, ServerTlv &aServer, uint8_t *aOldTlvs, uint8_t aOldTlvsLength);
+    otError AddService(ServiceTlv &aTlv, uint8_t *aOldTlvs, uint8_t aOldTlvsLength);
+#endif
 
     int AllocateContext(void);
     otError FreeContext(uint8_t aContextId);
@@ -167,6 +182,9 @@ private:
 
     otError RemoveRloc(uint16_t aRloc16);
     otError RemoveRloc(PrefixTlv &aPrefix, uint16_t aRloc16);
+#if OPENTHREAD_ENABLE_SERVICE
+    otError RemoveRloc(ServiceTlv &service, uint16_t aRloc16);
+#endif
     otError RemoveRloc(PrefixTlv &aPrefix, HasRouteTlv &aHasRoute, uint16_t aRloc16);
     otError RemoveRloc(PrefixTlv &aPrefix, BorderRouterTlv &aBorderRouter, uint16_t aRloc16);
 

src/core/thread/network_data_local.cpp

@@ -39,7 +39,7 @@
 #include "mac/mac_frame.hpp"
 #include "thread/thread_netif.hpp"
 
-#if OPENTHREAD_ENABLE_BORDER_ROUTER
+#if OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
 
 namespace ot {
 namespace NetworkData {
@@ -151,6 +151,68 @@ exit:
     return error;
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+otError Local::AddService(uint32_t aEnterpriseNumber, const uint8_t *aServiceData, uint8_t aServiceDataLength,
+                          bool aServerStable, const uint8_t *aServerData, uint8_t aServerDataLength)
+{
+    otError error = OT_ERROR_NONE;
+    ServiceTlv *serviceTlv;
+    ServerTlv *serverTlv;
+    uint8_t serviceTlvLength = (sizeof(ServiceTlv) - sizeof(NetworkDataTlv)) + aServiceDataLength +
+                               sizeof(uint8_t)/*mServiceDataLength*/ +
+                               ServiceTlv::GetEnterpriseNumberFieldLength(aEnterpriseNumber) +
+                               aServerDataLength + sizeof(ServerTlv);
+
+    RemoveService(aEnterpriseNumber, aServiceData, aServiceDataLength);
+
+    serviceTlv = reinterpret_cast<ServiceTlv *>(mTlvs + mLength);
+    Insert(reinterpret_cast<uint8_t *>(serviceTlv), serviceTlvLength + sizeof(NetworkDataTlv));
+
+    serviceTlv->Init();
+    serviceTlv->SetEnterpriseNumber(aEnterpriseNumber);
+    serviceTlv->SetServiceID(0);
+    serviceTlv->SetServiceData(aServiceData, aServiceDataLength);
+    serviceTlv->SetLength(serviceTlvLength);
+
+    serverTlv = reinterpret_cast<ServerTlv *>(serviceTlv->GetSubTlvs());
+    serverTlv->Init();
+
+    // According to Thread spec 1.1.1, section 5.18.6 Service TLV:
+    // "The Stable flag is set if any of the included sub-TLVs have their Stable flag set."
+    // The meaning also seems to be 'if and only if'.
+    if (aServerStable)
+    {
+        serviceTlv->SetStable();
+        serverTlv->SetStable();
+    }
+
+    serverTlv->SetServer16(GetNetif().GetMle().GetRloc16());
+    serverTlv->SetServerData(aServerData, aServerDataLength);
+
+    ClearResubmitDelayTimer();
+
+    otDumpDebgNetData(GetInstance(), "add service done", mTlvs, mLength);
+
+//exit:
+    return error;
+}
+
+otError Local::RemoveService(uint32_t aEnterpriseNumber, const uint8_t *aServiceData, uint8_t aServiceDataLength)
+{
+    otError error = OT_ERROR_NONE;
+    ServiceTlv *tlv;
+
+    VerifyOrExit((tlv = FindService(aEnterpriseNumber, aServiceData, aServiceDataLength)) != NULL,
+                 error = OT_ERROR_NOT_FOUND);
+    Remove(reinterpret_cast<uint8_t *>(tlv), sizeof(NetworkDataTlv) + tlv->GetLength());
+    ClearResubmitDelayTimer();
+
+exit:
+    otDumpDebgNetData(GetInstance(), "remove service done", mTlvs, mLength);
+    return error;
+}
+#endif
+
 otError Local::UpdateRloc(void)
 {
     for (NetworkDataTlv *cur = reinterpret_cast<NetworkDataTlv *>(mTlvs);
@@ -163,6 +225,13 @@ otError Local::UpdateRloc(void)
             UpdateRloc(*static_cast<PrefixTlv *>(cur));
             break;
 
+#if OPENTHREAD_ENABLE_SERVICE
+
+        case NetworkDataTlv::kTypeService:
+            UpdateRloc(*static_cast<ServiceTlv *>(cur));
+            break;
+#endif
+
         default:
             assert(false);
             break;
@@ -211,6 +280,33 @@ otError Local::UpdateRloc(BorderRouterTlv &aBorderRouter)
     return OT_ERROR_NONE;
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+otError Local::UpdateRloc(ServiceTlv &aService)
+{
+    for (NetworkDataTlv *cur = aService.GetSubTlvs(); cur < aService.GetNext(); cur = cur->GetNext())
+    {
+        switch (cur->GetType())
+        {
+        case NetworkDataTlv::kTypeServer:
+            UpdateRloc(*static_cast<ServerTlv *>(cur));
+            break;
+
+        default:
+            assert(false);
+            break;
+        }
+    }
+
+    return OT_ERROR_NONE;
+}
+
+otError Local::UpdateRloc(ServerTlv &aServer)
+{
+    aServer.SetServer16(GetNetif().GetMle().GetRloc16());
+    return OT_ERROR_NONE;
+}
+#endif
+
 bool Local::IsOnMeshPrefixConsistent(void)
 {
     ThreadNetif &netif = GetNetif();
@@ -227,6 +323,16 @@ bool Local::IsExternalRouteConsistent(void)
             ContainsExternalRoutes(netif.GetNetworkDataLeader(), netif.GetMle().GetRloc16()));
 }
 
+#if OPENTHREAD_ENABLE_SERVICE
+bool Local::IsServiceConsistent(void)
+{
+    ThreadNetif &netif = GetNetif();
+
+    return (netif.GetNetworkDataLeader().ContainsServices(*this, netif.GetMle().GetRloc16()) &&
+            ContainsServices(netif.GetNetworkDataLeader(), netif.GetMle().GetRloc16()));
+}
+#endif
+
 otError Local::SendServerDataNotification(void)
 {
     ThreadNetif &netif = GetNetif();
@@ -249,7 +355,12 @@ otError Local::SendServerDataNotification(void)
 
     UpdateRloc();
 
+#if OPENTHREAD_ENABLE_SERVICE
+    VerifyOrExit(!IsOnMeshPrefixConsistent() || !IsExternalRouteConsistent() ||
+                 !IsServiceConsistent(), ClearResubmitDelayTimer());
+#else
     VerifyOrExit(!IsOnMeshPrefixConsistent() || !IsExternalRouteConsistent(), ClearResubmitDelayTimer());
+#endif
 
     if (mOldRloc == rloc)
     {
@@ -266,4 +377,4 @@ exit:
 }  // namespace NetworkData
 }  // namespace ot
 
-#endif // OPENTHREAD_ENABLE_BORDER_ROUTER
+#endif // OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE

src/core/thread/network_data_local.hpp

@@ -123,6 +123,38 @@ public:
      */
     otError RemoveHasRoutePrefix(const uint8_t *aPrefix, uint8_t aPrefixLength);
 
+#if OPENTHREAD_ENABLE_SERVICE
+    /**
+     * This method adds a Service entry to the Thread Network local data.
+     *
+     * @param[in]  aEnterpriseNumber  Enterprise Number (IANA-assigned) for Service TLV
+     * @param[in]  aServiceData       A pointer to the Service Data
+     * @param[in]  aServiceDataLength The length of @p aServiceData in bytes.
+     * @param[in]  aServerStable      The Stable flag value for Server TLV
+     * @param[in]  aServerData        A pointer to the Server Data
+     * @param[in]  aServerDataLength  The length of @p aServerData in bytes.
+     *
+     * @retval OT_ERROR_NONE     Successfully added the Service entry.
+     * @retval OT_ERROR_NO_BUFS  Insufficient space to add the Service entry.
+     *
+     */
+    otError AddService(uint32_t aEnterpriseNumber, const uint8_t *aServiceData, uint8_t aServiceDataLength,
+                       bool aServerStable, const uint8_t *aServerData, uint8_t aServerDataLength);
+
+    /**
+     * This method removes a Service entry from the Thread Network local data.
+     *
+     * @param[in]  aEnterpriseNumber   Enterprise Number of the service to be deleted.
+     * @param[in]  aServiceData        A pointer to the service data.
+     * @param[in]  aServiceDataLength  The length of @p aServiceData in bytes.
+     *
+     * @retval OT_ERROR_NONE       Successfully removed the Border Router entry.
+     * @retval OT_ERROR_NOT_FOUND  Could not find the Border Router entry.
+     *
+     */
+    otError RemoveService(uint32_t aEnterpriseNumber, const uint8_t *aServiceData, uint8_t aServiceDataLength);
+#endif
+
     /**
      * This method sends a Server Data Notification message to the Leader.
      *
@@ -137,9 +169,16 @@ private:
     otError UpdateRloc(PrefixTlv &aPrefix);
     otError UpdateRloc(HasRouteTlv &aHasRoute);
     otError UpdateRloc(BorderRouterTlv &aBorderRouter);
+#if OPENTHREAD_ENABLE_SERVICE
+    otError UpdateRloc(ServiceTlv &aService);
+    otError UpdateRloc(ServerTlv &aService);
+#endif
 
     bool IsOnMeshPrefixConsistent(void);
     bool IsExternalRouteConsistent(void);
+#if OPENTHREAD_ENABLE_SERVICE
+    bool IsServiceConsistent(void);
+#endif
 
     uint16_t mOldRloc;
 };

src/core/thread/network_data_tlvs.hpp

@@ -43,6 +43,8 @@
 
 using ot::Encoding::BigEndian::HostSwap16;
 
+#define THREAD_ENTERPRISE_NUMBER 44970
+
 namespace ot {
 namespace NetworkData {
 
@@ -81,6 +83,8 @@ public:
         kTypeBorderRouter      = 2,  ///< Border Router TLV
         kTypeContext           = 3,  ///< Context TLV
         kTypeCommissioningData = 4,  ///< Commissioning Dataset TLV
+        kTypeService           = 5,  ///< Service TLV
+        kTypeServer            = 6,  ///< Server TLV
     };
 
     /**
@@ -694,6 +698,263 @@ public:
     void Init(void) { NetworkDataTlv::Init(); SetType(kTypeCommissioningData); SetLength(0); }
 } OT_TOOL_PACKED_END;
 
+/**
+ * This class implements Service Data TLV generation and parsing.
+ *
+ */
+OT_TOOL_PACKED_BEGIN
+class ServiceTlv: public NetworkDataTlv
+{
+public:
+    /**
+     * This method initializes the TLV.
+     * Initial length is set to 2, to hold S_service_data_length field.
+     */
+    void Init(void) { NetworkDataTlv::Init(); SetType(kTypeService); SetLength(2); mTResSId = kTMask; SetServiceDataLength(0); }
+
+    /**
+     * This method gets Service Data length.
+     *
+     * @returns length of the Service Data field in bytes.
+     */
+    uint8_t GetServiceDataLength(void) {
+        return *GetServiceDataLengthLocation();
+    }
+
+    /**
+     * This method sets Service Data length.
+     *
+     * @param aServiceDataLength desired length of the Service Data field in bytes.
+     */
+    void SetServiceDataLength(uint8_t aServiceDataLength) {
+        *GetServiceDataLengthLocation() = aServiceDataLength;
+    }
+
+    /**
+     * This method returns a pointer to the Service Data.
+     *
+     * @returns A pointer to the Service Data.
+     */
+    uint8_t *GetServiceData(void) { return GetServiceDataLengthLocation() + sizeof(uint8_t); }
+
+    /**
+     * This method sets Service Data to the given values.
+     *
+     * Caller must ensure that there is enough memory allocated.
+     *
+     * @param aServiceData       pointer to the service data to use
+     * @param aServiceDataLength length of the provided service data in bytes
+     */
+    void SetServiceData(const uint8_t *aServiceData, uint8_t aServiceDataLength) {
+        SetServiceDataLength(aServiceDataLength);
+
+        memcpy(GetServiceData(), aServiceData, aServiceDataLength);
+    }
+
+    /**
+     * This method returns Enterprise Number field.
+     *
+     * @returns Enterprise Number
+     */
+    uint32_t GetEnterpriseNumber(void) {
+        if (IsThreadEnterprise()) {
+            return THREAD_ENTERPRISE_NUMBER;
+        }
+        else {
+            // This memory access most likely will not be aligned to 4 bytes
+            return HostSwap32(*reinterpret_cast<uint32_t *>(GetEnterpriseNumberLocation()));
+        }
+    }
+
+    /**
+     * This method returns the T flag. It is set when Enterprise Number is equal to THREAD_ENTERPRISE_NUMBER.
+     *
+     * @returns Flag whether Enterprise Number is equal to THREAD_ENTERPRISE_NUMBER
+     */
+    bool IsThreadEnterprise(void) const {
+        return (mTResSId & kTMask) != 0;
+    }
+
+    /**
+     * This method sets Enterprise Number and updates the T flag.
+     *
+     * Note: this method does not preserve service data / sub-TLV fields. Changing the T flag and inserting
+     * few bytes in the middle of this TLV effectively destroys rest of the content of this TLV (and might lead to
+     * memory corruption) so modification of Enterprise Number must be done before adding any content to the TLV.
+     *
+     * @param [in] aEnterpriseNumber Enterprise Number
+     */
+    void SetEnterpriseNumber(uint32_t aEnterpriseNumber) {
+        if (aEnterpriseNumber == THREAD_ENTERPRISE_NUMBER) {
+            mTResSId |= kTMask;
+        }
+        else {
+            mTResSId &= ~kTMask;
+
+            // This memory access most likely will not be aligned to 4 bytes
+            *reinterpret_cast<uint32_t *>(GetEnterpriseNumberLocation()) = HostSwap32(aEnterpriseNumber);
+        }
+    }
+
+    /**
+     * This method returns length of the S_enterprise_number TLV field in bytes, for given Enterprise Number.
+     *
+     * @returns length of the S_enterprise_number field in bytes
+     */
+    static uint8_t GetEnterpriseNumberFieldLength(uint32_t aEnterpriseNumber) {
+        if (aEnterpriseNumber == THREAD_ENTERPRISE_NUMBER) {
+            return 0;
+        }
+        else {
+            return (sizeof(aEnterpriseNumber));
+        }
+    }
+
+    /**
+     * This method returns Service ID. It is in range 0x00-0x0f.
+     *
+     * @returns Service ID
+     */
+    uint8_t GetServiceID(void) const {
+        return (mTResSId & kSIdMask) >> kSIdOffset;
+    }
+
+    /**
+     * This method sets Service ID.
+     *
+     * @param [in] aServiceID Service ID to be set. Expected range: 0x00-0x0f.
+     */
+    void SetServiceID(uint8_t aServiceID) {
+        mTResSId = static_cast<uint8_t>((mTResSId & ~kSIdMask) | (aServiceID << kSIdOffset));
+    }
+
+    /**
+     * This method returns the Sub-TLVs length in bytes.
+     *
+     * @returns The Sub-TLVs length in bytes.
+     *
+     */
+    uint8_t GetSubTlvsLength(void) {
+        return GetLength() - (sizeof(*this) - sizeof(NetworkDataTlv)) - (IsThreadEnterprise() ? 0 : sizeof(uint32_t))
+               - sizeof(uint8_t)/* mServiceDataLength */ - GetServiceDataLength();
+    }
+
+    /**
+     * This method sets the Sub-TLVs length in bytes.
+     *
+     * @param[in]  aLength  The Sub-TLVs length in bytes.
+     *
+     */
+    void SetSubTlvsLength(uint8_t aLength) {
+        SetLength(sizeof(*this) - sizeof(NetworkDataTlv) + (IsThreadEnterprise() ? 0 : sizeof(uint32_t))
+                  + sizeof(uint8_t)/* mServiceDataLength */ + GetServiceDataLength() + aLength);
+    }
+
+    /**
+     * This method returns a pointer to the Sub-TLVs.
+     *
+     * @returns A pointer to the Sub-TLVs.
+     *
+     */
+    NetworkDataTlv *GetSubTlvs(void) {
+        return reinterpret_cast<NetworkDataTlv *>(GetServiceDataLengthLocation() + sizeof(uint8_t) + GetServiceDataLength());
+    }
+
+private:
+    /**
+     * This method returns pointer to where mServiceDataLength would be.
+     *
+     * @returns pointer to service data length location
+     */
+    uint8_t *GetServiceDataLengthLocation(void) {
+        return GetEnterpriseNumberLocation() + (IsThreadEnterprise() ? 0 : sizeof(uint32_t));
+    }
+
+    /**
+     * This method returns pointer to where mEnterpriseNumber would be.
+     *
+     * Note: this method returns uint8_t*, not uint32_t*.
+     *
+     * @returns pointer to enterprise number location
+     */
+    uint8_t *GetEnterpriseNumberLocation(void) {
+        return &mTResSId + sizeof(mTResSId);
+    }
+
+    enum
+    {
+        kTOffset = 7,
+        kTMask = 0x1 << kTOffset,
+        kSIdOffset = 0,
+        kSIdMask = 0xf << kSIdOffset,
+    };
+
+    uint8_t mTResSId;
+} OT_TOOL_PACKED_END;
+
+/**
+ * This class implements Server Data TLV generation and parsing.
+ *
+ */
+OT_TOOL_PACKED_BEGIN
+class ServerTlv: public NetworkDataTlv
+{
+public:
+    /**
+     * This method initializes the TLV.
+     *
+     */
+    void Init(void) { NetworkDataTlv::Init(); SetType(kTypeServer); SetLength(sizeof(*this) - sizeof(NetworkDataTlv)); }
+
+    /**
+     * This method returns the S_server_16 value.
+     *
+     * @returns The S_server_16 value.
+     */
+    uint16_t GetServer16(void) const { return HostSwap16(mServer16); }
+
+    /**
+     * This method sets the S_server_16 value.
+     *
+     * @param[in]  aServer16  The S_server_16 value.
+     *
+     */
+    void SetServer16(uint16_t aServer16) { mServer16 = HostSwap16(aServer16); }
+
+    /**
+     * This method returns a pointer to the Server Data.
+     *
+     * @returns A pointer to the Server Data.
+     */
+    const uint8_t *GetServerData(void) { return reinterpret_cast<uint8_t *>(this) + sizeof(*this); }
+
+    /**
+     * This method sets Server Data to the given values.
+     *
+     * Caller must ensure that there is enough memory allocated.
+     *
+     * @param aServerData       pointer to the server data to use
+     * @param aServerDataLength length of the provided server data in bytes
+     */
+    void SetServerData(const uint8_t *aServerData, uint8_t aServerDataLength) {
+        SetLength(sizeof(*this) - sizeof(NetworkDataTlv) + aServerDataLength);
+        memcpy(reinterpret_cast<uint8_t *>(this) + sizeof(*this), aServerData, aServerDataLength);
+    }
+
+    /**
+     * This method returns the Server Data length in bytes.
+     *
+     * @returns The Server Data length in bytes.
+     *
+     */
+    uint8_t GetServerDataLength(void) const {
+        return GetLength() - (sizeof(*this) - sizeof(NetworkDataTlv));
+    }
+
+private:
+    uint16_t mServer16;
+} OT_TOOL_PACKED_END;
+
 /**
  * @}
  *

src/core/thread/thread_netif.cpp

@@ -76,7 +76,7 @@ ThreadNetif::ThreadNetif(otInstance &aInstance):
     mMac(aInstance),
     mMeshForwarder(aInstance),
     mMleRouter(aInstance),
-#if OPENTHREAD_ENABLE_BORDER_ROUTER
+#if OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
     mNetworkDataLocal(aInstance),
 #endif
     mNetworkDataLeader(aInstance),

src/core/thread/thread_netif.hpp

@@ -254,7 +254,7 @@ public:
      */
     MeshForwarder &GetMeshForwarder(void) { return mMeshForwarder; }
 
-#if OPENTHREAD_ENABLE_BORDER_ROUTER
+#if OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
     /**
      * This method returns a reference to the network data local object.
      *
@@ -262,7 +262,7 @@ public:
      *
      */
     NetworkData::Local &GetNetworkDataLocal(void) { return mNetworkDataLocal; }
-#endif  // OPENTHREAD_ENABLE_BORDER_ROUTER
+#endif  // OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
 
     /**
      * This method returns a reference to the network data leader object.
@@ -442,9 +442,9 @@ private:
     Mac::Mac mMac;
     MeshForwarder mMeshForwarder;
     Mle::MleRouter mMleRouter;
-#if OPENTHREAD_ENABLE_BORDER_ROUTER
+#if OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
     NetworkData::Local mNetworkDataLocal;
-#endif  // OPENTHREAD_ENABLE_BORDER_ROUTER
+#endif  // OPENTHREAD_ENABLE_BORDER_ROUTER || OPENTHREAD_ENABLE_SERVICE
     NetworkData::Leader mNetworkDataLeader;
 #if OPENTHREAD_FTD || OPENTHREAD_ENABLE_MTD_NETWORK_DIAGNOSTIC
     NetworkDiagnostic::NetworkDiagnostic mNetworkDiagnostic;

tests/scripts/thread-cert/Makefile.am

@@ -143,6 +143,7 @@ EXTRA_DIST                                                         = \
     test_lowpan.py                                                   \
     test_mac802154.py                                                \
     test_mle.py                                                      \
+    test_service.py                                                  \
     test_network_data.py                                             \
     test_network_layer.py                                            \
     $(NULL)
@@ -158,6 +159,7 @@ check_SCRIPTS                                                      = \
     test_lowpan.py                                                   \
     test_mac802154.py                                                \
     test_mle.py                                                      \
+    test_service.py                                                  \
     test_network_data.py                                             \
     test_network_layer.py                                            \
     Cert_5_2_02_LeaderReject1Hop.py                                  \
@@ -263,6 +265,7 @@ TESTS                                                              = \
     $(NULL)
 
 XFAIL_NCP_TESTS                                                    = \
+    test_service.py                                                  \
     Cert_5_2_05_AddressQuery.py                                      \
     Cert_5_3_09_AddressQuery.py                                      \
     Cert_8_1_01_Commissioning.py                                     \

tests/scripts/thread-cert/node.py

@@ -188,6 +188,12 @@ class Node:
     def get_addrs(self):
         return self.interface.get_addrs()
 
+    def add_service(self, enterpriseNumber, serviceData, serverData):
+        self.interface.add_service(enterpriseNumber, serviceData, serverData)
+
+    def remove_service(self, enterpriseNumber, serviceData):
+        self.interface.remove_service(enterpriseNumber, serviceData)
+
     def get_ip6_address(self, address_type):
         return self.interface.get_ip6_address(address_type)
 

tests/scripts/thread-cert/node_api.py

@@ -197,6 +197,12 @@ class otApi:
     def get_addrs(self):
         return self.Api.otNodeGetAddrs(self.otNode).decode("utf-8").split("\n")
 
+    def add_service(self, enterpriseNumber, serviceData, serverData):
+        raise OSError("otServerAddService wrapper not implemented!")
+
+    def remove_service(self, enterpriseNumber, serviceData):
+        raise OSError("otServerRemoveService wrapper not implemented!")
+
     def get_context_reuse_delay(self):
         return int(self.Api.otNodeGetContextReuseDelay(self.otNode))
 

tests/scripts/thread-cert/node_cli.py

@@ -375,6 +375,16 @@ class otCli:
 
         return addrs
 
+    def add_service(self, enterpriseNumber, serviceData, serverData):
+        cmd = 'service add ' + enterpriseNumber + ' ' + serviceData+ ' '  + serverData
+        self.send_command(cmd)
+        self.pexpect.expect('Done')
+
+    def remove_service(self, enterpriseNumber, serviceData):
+        cmd = 'service remove ' + enterpriseNumber + ' ' + serviceData
+        self.send_command(cmd)
+        self.pexpect.expect('Done')
+
     def __getLinkLocalAddress(self):
         for ip6Addr in self.get_addrs():
             if re.match(config.LINK_LOCAL_REGEX_PATTERN, ip6Addr, re.I):

tests/scripts/thread-cert/test_service.py

@@ -0,0 +1,208 @@
+#!/usr/bin/env python
+#
+#  Copyright (c) 2017, The OpenThread Authors.
+#  All rights reserved.
+#
+#  Redistribution and use in source and binary forms, with or without
+#  modification, are permitted provided that the following conditions are met:
+#  1. Redistributions of source code must retain the above copyright
+#     notice, this list of conditions and the following disclaimer.
+#  2. Redistributions in binary form must reproduce the above copyright
+#     notice, this list of conditions and the following disclaimer in the
+#     documentation and/or other materials provided with the distribution.
+#  3. Neither the name of the copyright holder nor the
+#     names of its contributors may be used to endorse or promote products
+#     derived from this software without specific prior written permission.
+#
+#  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+#  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+#  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+#  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+#  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+#  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+#  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+#  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+#  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+#  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+#  POSSIBILITY OF SUCH DAMAGE.
+#
+
+import re
+import time
+import unittest
+
+import node
+import config
+
+LEADER = 1
+ROUTER1 = 2
+ROUTER2 = 3
+
+SRV_0_ID = 0
+SRV_0_ENT_NUMBER = '123'
+SRV_0_SERVICE_DATA = 'foo'
+SRV_0_SERVER_DATA = 'bar'
+
+SRV_1_ID = 1
+SRV_1_ENT_NUMBER = '234'
+SRV_1_SERVICE_DATA = 'baz'
+SRV_1_SERVER_DATA = 'qux'
+
+class Test_Service(unittest.TestCase):
+    def setUp(self):
+        self.nodes = {}
+        for i in range(1,4):
+            self.nodes[i] = node.Node(i)
+
+        self.nodes[LEADER].set_panid(0xface)
+        self.nodes[LEADER].set_mode('rsdn')
+        self.nodes[LEADER].add_whitelist(self.nodes[ROUTER1].get_addr64())
+        self.nodes[LEADER].add_whitelist(self.nodes[ROUTER2].get_addr64())
+        self.nodes[LEADER].enable_whitelist()
+        self.nodes[LEADER].set_channel(12)
+        self.nodes[LEADER].set_network_name('OpenThread')
+
+        self.nodes[ROUTER1].set_panid(0xface)
+        self.nodes[ROUTER1].set_mode('rsdn')
+        self.nodes[ROUTER1].add_whitelist(self.nodes[LEADER].get_addr64())
+        self.nodes[ROUTER1].add_whitelist(self.nodes[ROUTER2].get_addr64())
+        self.nodes[ROUTER1].enable_whitelist()
+        self.nodes[ROUTER1].set_channel(12)
+        self.nodes[ROUTER1].set_network_name('OpenThread')
+        self.nodes[ROUTER1].set_router_selection_jitter(1)
+
+        self.nodes[ROUTER2].set_panid(0xface)
+        self.nodes[ROUTER2].set_mode('rsdn')
+        self.nodes[ROUTER2].add_whitelist(self.nodes[LEADER].get_addr64())
+        self.nodes[ROUTER2].add_whitelist(self.nodes[ROUTER1].get_addr64())
+        self.nodes[ROUTER2].enable_whitelist()
+        self.nodes[ROUTER2].set_channel(12)
+        self.nodes[ROUTER2].set_network_name('OpenThread')
+        self.nodes[ROUTER2].set_router_selection_jitter(1)
+
+    def tearDown(self):
+        for node in list(self.nodes.values()):
+            node.stop()
+        del self.nodes
+
+    def hasAloc(self, node_id, service_id):
+        for addr in self.nodes[node_id].get_ip6_address(config.ADDRESS_TYPE.ALOC):
+            m = re.match('.*:fc(..)$', addr, re.I)
+            if m is not None:
+                if m.group(1) == str(service_id + 10): # for service_id=3 look for '...:fc13'
+                    return True
+
+        return False
+
+    def pingFromAll(self, addr):
+        for node in list(self.nodes.values()):
+            self.assertTrue(node.ping(addr))
+
+    def failToPingFromAll(self, addr):
+        for node in list(self.nodes.values()):
+            self.assertFalse(node.ping(addr, timeout=3000))
+
+    def test(self):
+        self.nodes[LEADER].start()
+        self.nodes[LEADER].set_state('leader')
+        self.assertEqual(self.nodes[LEADER].get_state(), 'leader')
+
+        self.nodes[ROUTER1].start()
+        self.nodes[ROUTER2].start()
+        time.sleep(5)
+        self.assertEqual(self.nodes[ROUTER1].get_state(), 'router')
+        self.assertEqual(self.nodes[ROUTER2].get_state(), 'router')
+
+        self.assertEqual(self.hasAloc(LEADER, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(LEADER, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_1_ID), False)
+
+        self.nodes[ROUTER1].add_service(SRV_0_ENT_NUMBER, SRV_0_SERVICE_DATA, SRV_0_SERVER_DATA)
+        self.nodes[ROUTER1].register_netdata()
+        time.sleep(2)
+
+        self.assertEqual(self.hasAloc(LEADER, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(LEADER, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_0_ID), True)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_1_ID), False)
+
+        aloc0 = self.nodes[ROUTER1].get_ip6_address(config.ADDRESS_TYPE.ALOC)[0]
+        self.pingFromAll(aloc0)
+
+        self.nodes[LEADER].add_service(SRV_0_ENT_NUMBER, SRV_0_SERVICE_DATA, SRV_0_SERVER_DATA)
+        self.nodes[LEADER].register_netdata()
+        time.sleep(2)
+
+        self.assertEqual(self.hasAloc(LEADER, SRV_0_ID), True)
+        self.assertEqual(self.hasAloc(LEADER, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_0_ID), True)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_1_ID), False)
+
+        self.pingFromAll(aloc0)
+
+        self.nodes[ROUTER2].add_service(SRV_1_ENT_NUMBER, SRV_1_SERVICE_DATA, SRV_1_SERVER_DATA)
+        self.nodes[ROUTER2].register_netdata()
+        time.sleep(2)
+
+        self.assertEqual(self.hasAloc(LEADER, SRV_0_ID), True)
+        self.assertEqual(self.hasAloc(LEADER, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_0_ID), True)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_1_ID), True)
+
+        aloc1 = self.nodes[ROUTER2].get_ip6_address(config.ADDRESS_TYPE.ALOC)[0]
+        self.pingFromAll(aloc0)
+        self.pingFromAll(aloc1)
+
+        self.nodes[ROUTER1].remove_service(SRV_0_ENT_NUMBER, SRV_0_SERVICE_DATA)
+        self.nodes[ROUTER1].register_netdata()
+        time.sleep(2)
+
+        self.assertEqual(self.hasAloc(LEADER, SRV_0_ID), True)
+        self.assertEqual(self.hasAloc(LEADER, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_1_ID), True)
+
+        self.pingFromAll(aloc0)
+        self.pingFromAll(aloc1)
+
+        self.nodes[LEADER].remove_service(SRV_0_ENT_NUMBER, SRV_0_SERVICE_DATA)
+        self.nodes[LEADER].register_netdata()
+        time.sleep(2)
+
+        self.assertEqual(self.hasAloc(LEADER, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(LEADER, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_1_ID), True)
+
+        self.failToPingFromAll(aloc0)
+        self.pingFromAll(aloc1)
+
+        self.nodes[ROUTER2].remove_service(SRV_1_ENT_NUMBER, SRV_1_SERVICE_DATA)
+        self.nodes[ROUTER2].register_netdata()
+        time.sleep(2)
+
+        self.assertEqual(self.hasAloc(LEADER, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(LEADER, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER1, SRV_1_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_0_ID), False)
+        self.assertEqual(self.hasAloc(ROUTER2, SRV_1_ID), False)
+
+        self.failToPingFromAll(aloc0)
+        self.failToPingFromAll(aloc1)
+
+if __name__ == '__main__':
+    unittest.main()