Diff: wakaama/objects.c

Revision:

3:a280069151ac

Parent:

0:f9d13e09cf11

Child:

15:d0f20339c1ad

--- a/wakaama/objects.c	Fri Apr 28 09:53:26 2017 +0000
+++ b/wakaama/objects.c	Fri Apr 28 18:13:27 2017 +0800
@@ -15,6 +15,8 @@
  *    Fabien Fleutot - Please refer to git log
  *    Toby Jaffey - Please refer to git log
  *    Benjamin Cabé - Please refer to git log
+ *    Bosch Software Innovations GmbH - Please refer to git log
+ *    Pascal Rieux - Please refer to git log
  *    
  *******************************************************************************/
 
@@ -47,404 +49,540 @@
  David Navarro <david.navarro@intel.com>
 
 */
-
 #include "internals.h"
 
 #ifdef LWM2M_CLIENT_MODE
+
+
 #include <stdlib.h>
 #include <string.h>
 #include <stdio.h>
 
 
-static lwm2m_object_t * prv_find_object(lwm2m_context_t * contextP,
-                                        uint16_t Id)
+uint8_t object_checkReadable(lwm2m_context_t * contextP,
+                             lwm2m_uri_t * uriP)
 {
-    int i;
+    coap_status_t result;
+    lwm2m_object_t * targetP;
+    lwm2m_data_t * dataP = NULL;
+    int size;
+
+    LOG_URI(uriP);
+    targetP = (lwm2m_object_t *)LWM2M_LIST_FIND(contextP->objectList, uriP->objectId);
+    if (NULL == targetP) return COAP_404_NOT_FOUND;
+    if (NULL == targetP->readFunc) return COAP_405_METHOD_NOT_ALLOWED;
+
+    if (!LWM2M_URI_IS_SET_INSTANCE(uriP)) return COAP_205_CONTENT;
+
+    if (NULL == lwm2m_list_find(targetP->instanceList, uriP->instanceId)) return COAP_404_NOT_FOUND;
+
+    if (!LWM2M_URI_IS_SET_RESOURCE(uriP)) return COAP_205_CONTENT;
+
+    size = 1;
+    dataP = lwm2m_data_new(1);
+    if (dataP == NULL) return COAP_500_INTERNAL_SERVER_ERROR;
+
+    dataP->id = uriP->resourceId;
+
+    result = targetP->readFunc(uriP->instanceId, &size, &dataP, targetP);
+    lwm2m_data_free(1, dataP);
+
+    return result;
+}
 
-    for (i = 0 ; i < contextP->numObject ; i++)
+uint8_t object_checkNumeric(lwm2m_context_t * contextP,
+                            lwm2m_uri_t * uriP)
+{
+    coap_status_t result;
+    lwm2m_object_t * targetP;
+    lwm2m_data_t * dataP = NULL;
+    int size;
+
+    LOG_URI(uriP);
+    if (!LWM2M_URI_IS_SET_RESOURCE(uriP)) return COAP_405_METHOD_NOT_ALLOWED;
+
+    targetP = (lwm2m_object_t *)LWM2M_LIST_FIND(contextP->objectList, uriP->objectId);
+    if (NULL == targetP) return COAP_404_NOT_FOUND;
+    if (NULL == targetP->readFunc) return COAP_405_METHOD_NOT_ALLOWED;
+
+    size = 1;
+    dataP = lwm2m_data_new(1);
+    if (dataP == NULL) return COAP_500_INTERNAL_SERVER_ERROR;
+
+    dataP->id = uriP->resourceId;
+
+    result = targetP->readFunc(uriP->instanceId, &size, &dataP, targetP);
+    if (result == COAP_205_CONTENT)
     {
-        if (contextP->objectList[i]->objID == Id)
+        switch (dataP->type)
         {
-            return contextP->objectList[i];
+        case LWM2M_TYPE_INTEGER:
+        case LWM2M_TYPE_FLOAT:
+            break;
+        default:
+            result = COAP_405_METHOD_NOT_ALLOWED;
         }
     }
 
-    return NULL;
+    lwm2m_data_free(1, dataP);
+
+    return result;
+}
+
+coap_status_t object_readData(lwm2m_context_t * contextP,
+                               lwm2m_uri_t * uriP,
+                               int * sizeP,
+                               lwm2m_data_t ** dataP)
+{
+    coap_status_t result;
+    lwm2m_object_t * targetP;
+
+    LOG_URI(uriP);
+    targetP = (lwm2m_object_t *)LWM2M_LIST_FIND(contextP->objectList, uriP->objectId);
+    if (NULL == targetP) return COAP_404_NOT_FOUND;
+    if (NULL == targetP->readFunc) return COAP_405_METHOD_NOT_ALLOWED;
+
+    if (LWM2M_URI_IS_SET_INSTANCE(uriP))
+    {
+        if (NULL == lwm2m_list_find(targetP->instanceList, uriP->instanceId)) return COAP_404_NOT_FOUND;
+
+        // single instance read
+        if (LWM2M_URI_IS_SET_RESOURCE(uriP))
+        {
+            *sizeP = 1;
+            *dataP = lwm2m_data_new(*sizeP);
+            if (*dataP == NULL) return COAP_500_INTERNAL_SERVER_ERROR;
+
+            (*dataP)->id = uriP->resourceId;
+        }
+
+        result = targetP->readFunc(uriP->instanceId, sizeP, dataP, targetP);
+    }
+    else
+    {
+        // multiple object instances read
+        lwm2m_list_t * instanceP;
+        int i;
+
+        result = COAP_205_CONTENT;
+
+        *sizeP = 0;
+        for (instanceP = targetP->instanceList; instanceP != NULL ; instanceP = instanceP->next)
+        {
+            (*sizeP)++;
+        }
+
+        if (*sizeP == 0)
+        {
+            *dataP = NULL;
+        }
+        else
+        {
+            *dataP = lwm2m_data_new(*sizeP);
+            if (*dataP == NULL) return COAP_500_INTERNAL_SERVER_ERROR;
+
+            instanceP = targetP->instanceList;
+            i = 0;
+            while (instanceP != NULL && result == COAP_205_CONTENT)
+            {
+                result = targetP->readFunc(instanceP->id, (int*)&((*dataP)[i].value.asChildren.count), &((*dataP)[i].value.asChildren.array), targetP);
+                (*dataP)[i].type = LWM2M_TYPE_OBJECT_INSTANCE;
+                (*dataP)[i].id = instanceP->id;
+                i++;
+                instanceP = instanceP->next;
+            }
+        }
+    }
+
+    LOG_ARG("result: %u.%2u, size: %d", (result & 0xFF) >> 5, (result & 0x1F), *sizeP);
+    return result;
 }
 
 coap_status_t object_read(lwm2m_context_t * contextP,
                           lwm2m_uri_t * uriP,
-                          char ** bufferP,
-                          int * lengthP)
+                          lwm2m_media_type_t * formatP,
+                          uint8_t ** bufferP,
+                          size_t * lengthP)
 {
     coap_status_t result;
-
-    switch (uriP->objectId)
-    {
-    case LWM2M_SECURITY_OBJECT_ID:
-        result = NOT_FOUND_4_04;
-        break;
-
-    case LWM2M_SERVER_OBJECT_ID:
-        result = object_server_read(contextP, uriP, bufferP, lengthP);
-        break;
-
-    default:
-        {
-            lwm2m_object_t * targetP;
-            lwm2m_tlv_t * tlvP = NULL;
-            int size = 0;
+    lwm2m_data_t * dataP = NULL;
+    int size = 0;
+    int res;
 
-            targetP = prv_find_object(contextP, uriP->objectId);
-            if (NULL == targetP) return NOT_FOUND_4_04;
-            if (NULL == targetP->readFunc) return METHOD_NOT_ALLOWED_4_05;
-            if (targetP->instanceList == NULL)
-            {
-                // this is a single instance object
-                if (LWM2M_URI_IS_SET_INSTANCE(uriP) && (uriP->instanceId != 0))
-                {
-                    return COAP_404_NOT_FOUND;
-                }
-            }
-            else
-            {
-                if (LWM2M_URI_IS_SET_INSTANCE(uriP))
-                {
-                    if (NULL == lwm2m_list_find(targetP->instanceList, uriP->instanceId))
-                    {
-                        return COAP_404_NOT_FOUND;
-                    }
-                }
-                else
-                {
-                    // multiple object instances read
-                    lwm2m_list_t * instanceP;
-                    int i;
-
-                    size = 0;
-                    for (instanceP = targetP->instanceList; instanceP != NULL ; instanceP = instanceP->next)
-                    {
-                        size++;
-                    }
-
-                    tlvP = lwm2m_tlv_new(size);
-                    if (tlvP == NULL) return COAP_500_INTERNAL_SERVER_ERROR;
+    LOG_URI(uriP);
+    result = object_readData(contextP, uriP, &size, &dataP);
 
-                    result = COAP_205_CONTENT;
-                    instanceP = targetP->instanceList;
-                    i = 0;
-                    while (instanceP != NULL && result == COAP_205_CONTENT)
-                    {
-                        result = targetP->readFunc(instanceP->id, (int*)&(tlvP[i].length), (lwm2m_tlv_t **)&(tlvP[i].value), targetP);
-                        tlvP[i].type = LWM2M_TYPE_OBJECT_INSTANCE;
-                        tlvP[i].id = instanceP->id;
-                        i++;
-                        instanceP = instanceP->next;
-                    }
-
-                    if (result == COAP_205_CONTENT)
-                    {
-                        *lengthP = lwm2m_tlv_serialize(size, tlvP, bufferP);
-                        if (*lengthP == 0) result = COAP_500_INTERNAL_SERVER_ERROR;
-                    }
-                    lwm2m_tlv_free(size, tlvP);
-
-                    return result;
-                }
-            }
-
-            // single instance read
-            if (LWM2M_URI_IS_SET_RESOURCE(uriP))
-            {
-                size = 1;
-                tlvP = lwm2m_tlv_new(size);
-                if (tlvP == NULL) return COAP_500_INTERNAL_SERVER_ERROR;
-
-                tlvP->type = LWM2M_TYPE_RESSOURCE;
-                tlvP->flags = LWM2M_TLV_FLAG_TEXT_FORMAT;
-                tlvP->id = uriP->resourceId;
-            }
-            result = targetP->readFunc(uriP->instanceId, &size, &tlvP, targetP);
-            if (result == COAP_205_CONTENT)
-            {
-                if (size == 1
-                 && tlvP->type == LWM2M_TYPE_RESSOURCE
-                 && (tlvP->flags && LWM2M_TLV_FLAG_TEXT_FORMAT) != 0 )
-                {
-                    *bufferP = (char *)malloc(tlvP->length);
-                    if (*bufferP == NULL)
-                    {
-                        result = COAP_500_INTERNAL_SERVER_ERROR;
-                    }
-                    else
-                    {
-                        memcpy(*bufferP, tlvP->value, tlvP->length);
-                        *lengthP = tlvP->length;
-                    }
-                }
-                else
-                {
-                    *lengthP = lwm2m_tlv_serialize(size, tlvP, bufferP);
-                    if (*lengthP == 0) result = COAP_500_INTERNAL_SERVER_ERROR;
-                }
-            }
-            lwm2m_tlv_free(size, tlvP);
+    if (result == COAP_205_CONTENT)
+    {
+        res = lwm2m_data_serialize(uriP, size, dataP, formatP, bufferP);
+        if (res < 0)
+        {
+            result = COAP_500_INTERNAL_SERVER_ERROR;
+        }
+        else
+        {
+            *lengthP = (size_t)res;
         }
     }
+    lwm2m_data_free(size, dataP);
+
+    LOG_ARG("result: %u.%2u, length: %d", (result & 0xFF) >> 5, (result & 0x1F), *lengthP);
 
     return result;
 }
 
 coap_status_t object_write(lwm2m_context_t * contextP,
                            lwm2m_uri_t * uriP,
-                           char * buffer,
-                           int length)
+                           lwm2m_media_type_t format,
+                           uint8_t * buffer,
+                           size_t length)
 {
-    coap_status_t result;
-
-    switch (uriP->objectId)
-    {
-    case LWM2M_SECURITY_OBJECT_ID:
-        result = NOT_FOUND_4_04;
-        break;
-
-    case LWM2M_SERVER_OBJECT_ID:
-        result = object_server_write(contextP, uriP, buffer, length);
-        break;
-
-    default:
-        {
-            lwm2m_object_t * targetP;
-            lwm2m_tlv_t * tlvP = NULL;
-            int size = 0;
+    coap_status_t result = NO_ERROR;
+    lwm2m_object_t * targetP;
+    lwm2m_data_t * dataP = NULL;
+    int size = 0;
 
-            targetP = prv_find_object(contextP, uriP->objectId);
-            if (NULL == targetP) return NOT_FOUND_4_04;
-            if (NULL == targetP->writeFunc) return METHOD_NOT_ALLOWED_4_05;
-
-            if (LWM2M_URI_IS_SET_RESOURCE(uriP))
-            {
-                size = 1;
-                tlvP = lwm2m_tlv_new(size);
-                if (tlvP == NULL) return COAP_500_INTERNAL_SERVER_ERROR;
-
-                tlvP->flags = LWM2M_TLV_FLAG_TEXT_FORMAT | LWM2M_TLV_FLAG_STATIC_DATA;
-                tlvP->type = LWM2M_TYPE_RESSOURCE;
-                tlvP->id = uriP->resourceId;
-                tlvP->length = length;
-                tlvP->value = buffer;
-            }
-            else
-            {
-                size = lwm2m_tlv_parse(buffer, length, &tlvP);
-                if (size == 0) return COAP_500_INTERNAL_SERVER_ERROR;
-            }
-            result = targetP->writeFunc(uriP->instanceId, size, tlvP, targetP);
-            lwm2m_tlv_free(size, tlvP);
+    LOG_URI(uriP);
+    targetP = (lwm2m_object_t *)LWM2M_LIST_FIND(contextP->objectList, uriP->objectId);
+    if (NULL == targetP)
+    {
+        result = COAP_404_NOT_FOUND;
+    }
+    else if (NULL == targetP->writeFunc)
+    {
+        result = COAP_405_METHOD_NOT_ALLOWED;
+    }
+    else
+    {
+        size = lwm2m_data_parse(uriP, buffer, length, format, &dataP);
+        if (size == 0)
+        {
+            result = COAP_406_NOT_ACCEPTABLE;
         }
     }
+    if (result == NO_ERROR)
+    {
+        result = targetP->writeFunc(uriP->instanceId, size, dataP, targetP);
+        lwm2m_data_free(size, dataP);
+    }
+
+    LOG_ARG("result: %u.%2u", (result & 0xFF) >> 5, (result & 0x1F));
 
     return result;
 }
 
 coap_status_t object_execute(lwm2m_context_t * contextP,
                              lwm2m_uri_t * uriP,
-                             char * buffer,
-                             int length)
+                             uint8_t * buffer,
+                             size_t length)
 {
-    switch (uriP->objectId)
-    {
-    case LWM2M_SECURITY_OBJECT_ID:
-        return NOT_FOUND_4_04;
-
-    case LWM2M_SERVER_OBJECT_ID:
-        return object_server_execute(contextP, uriP, buffer, length);
+    lwm2m_object_t * targetP;
 
-    default:
-        {
-            lwm2m_object_t * targetP;
+    LOG_URI(uriP);
+    targetP = (lwm2m_object_t *)LWM2M_LIST_FIND(contextP->objectList, uriP->objectId);
+    if (NULL == targetP) return COAP_404_NOT_FOUND;
+    if (NULL == targetP->executeFunc) return COAP_405_METHOD_NOT_ALLOWED;
+    if (NULL == lwm2m_list_find(targetP->instanceList, uriP->instanceId)) return COAP_404_NOT_FOUND;
 
-            targetP = prv_find_object(contextP, uriP->objectId);
-            if (NULL == targetP) return NOT_FOUND_4_04;
-            if (NULL == targetP->executeFunc) return METHOD_NOT_ALLOWED_4_05;
-
-            return targetP->executeFunc(uriP->instanceId, uriP->resourceId, buffer, length, targetP);
-        }
-    }
+    return targetP->executeFunc(uriP->instanceId, uriP->resourceId, buffer, length, targetP);
 }
 
 coap_status_t object_create(lwm2m_context_t * contextP,
                             lwm2m_uri_t * uriP,
-                            char * buffer,
-                            int length)
+                            lwm2m_media_type_t format,
+                            uint8_t * buffer,
+                            size_t length)
 {
+    lwm2m_object_t * targetP;
+    lwm2m_data_t * dataP = NULL;
+    int size = 0;
+    uint8_t result;
+
+    LOG_URI(uriP);
+
     if (length == 0 || buffer == 0)
     {
-        return BAD_REQUEST_4_00;
+        return COAP_400_BAD_REQUEST;
     }
 
-    switch (uriP->objectId)
+    targetP = (lwm2m_object_t *)LWM2M_LIST_FIND(contextP->objectList, uriP->objectId);
+    if (NULL == targetP) return COAP_404_NOT_FOUND;
+    if (NULL == targetP->createFunc) return COAP_405_METHOD_NOT_ALLOWED;
+
+    size = lwm2m_data_parse(uriP, buffer, length, format, &dataP);
+    if (size <= 0) return COAP_400_BAD_REQUEST;
+
+    switch (dataP[0].type)
     {
-    case LWM2M_SECURITY_OBJECT_ID:
-        return object_security_create(contextP, uriP, buffer, length);
+    case LWM2M_TYPE_OBJECT:
+        result = COAP_400_BAD_REQUEST;
+        goto exit;
 
-    case LWM2M_SERVER_OBJECT_ID:
-        return object_server_create(contextP, uriP, buffer, length);
+    case LWM2M_TYPE_OBJECT_INSTANCE:
+        if (size != 1)
+        {
+            result = COAP_400_BAD_REQUEST;
+            goto exit;
+        }
+        if (NULL != lwm2m_list_find(targetP->instanceList, dataP[0].id))
+        {
+            // Instance already exists
+            result = COAP_406_NOT_ACCEPTABLE;
+            goto exit;
+        }
+        result = targetP->createFunc(dataP[0].id, dataP[0].value.asChildren.count, dataP[0].value.asChildren.array, targetP);
+        uriP->instanceId = dataP[0].id;
+        uriP->flag |= LWM2M_URI_FLAG_INSTANCE_ID;
+        break;
 
     default:
+        if (!LWM2M_URI_IS_SET_INSTANCE(uriP))
         {
-            lwm2m_object_t * targetP;
-            lwm2m_tlv_t * tlvP = NULL;
-            int size = 0;
-            uint8_t result;
-
-            targetP = prv_find_object(contextP, uriP->objectId);
-            if (NULL == targetP) return NOT_FOUND_4_04;
-            if (NULL == targetP->createFunc) return METHOD_NOT_ALLOWED_4_05;
+            uriP->instanceId = lwm2m_list_newId(targetP->instanceList);
+            uriP->flag |= LWM2M_URI_FLAG_INSTANCE_ID;
+        }
+        result = targetP->createFunc(uriP->instanceId, size, dataP, targetP);
+        break;
+    }
 
-            if (LWM2M_URI_IS_SET_INSTANCE(uriP))
-            {
-                if (NULL != lwm2m_list_find(targetP->instanceList, uriP->instanceId))
-                {
-                    // Instance already exists
-                    return COAP_406_NOT_ACCEPTABLE;
-                }
-            }
-            else
-            {
-                uriP->instanceId = lwm2m_list_newId(targetP->instanceList);
-                uriP->flag |= LWM2M_URI_FLAG_INSTANCE_ID;
-            }
+exit:
+    lwm2m_data_free(size, dataP);
 
-            targetP = prv_find_object(contextP, uriP->objectId);
-            if (NULL == targetP) return NOT_FOUND_4_04;
-            if (NULL == targetP->writeFunc) return METHOD_NOT_ALLOWED_4_05;
-
-            size = lwm2m_tlv_parse(buffer, length, &tlvP);
-            if (size == 0) return COAP_500_INTERNAL_SERVER_ERROR;
+    LOG_ARG("result: %u.%2u", (result & 0xFF) >> 5, (result & 0x1F));
 
-            result = targetP->createFunc(uriP->instanceId, size, tlvP, targetP);
-            lwm2m_tlv_free(size, tlvP);
-
-            return result;
-        }
-    }
+    return result;
 }
 
 coap_status_t object_delete(lwm2m_context_t * contextP,
                             lwm2m_uri_t * uriP)
 {
-    switch (uriP->objectId)
-    {
-    case LWM2M_SECURITY_OBJECT_ID:
-        return object_security_delete(contextP, uriP);
+    lwm2m_object_t * objectP;
+    coap_status_t result;
 
-    case LWM2M_SERVER_OBJECT_ID:
-        return object_server_delete(contextP, uriP);
+    LOG_URI(uriP);
+    objectP = (lwm2m_object_t *)LWM2M_LIST_FIND(contextP->objectList, uriP->objectId);
+    if (NULL == objectP) return COAP_404_NOT_FOUND;
+    if (NULL == objectP->deleteFunc) return COAP_405_METHOD_NOT_ALLOWED;
+
+    LOG("Entering");
 
-    default:
-        {
-            lwm2m_object_t * targetP;
+    if (LWM2M_URI_IS_SET_INSTANCE(uriP))
+    {
+        result = objectP->deleteFunc(uriP->instanceId, objectP);
+    }
+    else
+    {
+        lwm2m_list_t * instanceP;
 
-            targetP = prv_find_object(contextP, uriP->objectId);
-            if (NULL == targetP) return NOT_FOUND_4_04;
-            if (NULL == targetP->deleteFunc) return METHOD_NOT_ALLOWED_4_05;
-
-            return targetP->deleteFunc(uriP->instanceId, targetP);
+        result = COAP_202_DELETED;
+        instanceP = objectP->instanceList;
+        while (NULL != instanceP
+            && result == COAP_202_DELETED)
+        {
+            result = objectP->deleteFunc(instanceP->id, objectP);
+            instanceP = objectP->instanceList;
         }
     }
+
+    LOG_ARG("result: %u.%2u", (result & 0xFF) >> 5, (result & 0x1F));
+
+    return result;
+}
+
+coap_status_t object_discover(lwm2m_context_t * contextP,
+                              lwm2m_uri_t * uriP,
+                              lwm2m_server_t * serverP,
+                              uint8_t ** bufferP,
+                              size_t * lengthP)
+{
+    coap_status_t result;
+    lwm2m_object_t * targetP;
+    lwm2m_data_t * dataP = NULL;
+    int size = 0;
+
+    LOG_URI(uriP);
+    targetP = (lwm2m_object_t *)LWM2M_LIST_FIND(contextP->objectList, uriP->objectId);
+    if (NULL == targetP) return COAP_404_NOT_FOUND;
+    if (NULL == targetP->discoverFunc) return COAP_501_NOT_IMPLEMENTED;
+
+    if (LWM2M_URI_IS_SET_INSTANCE(uriP))
+    {
+        if (NULL == lwm2m_list_find(targetP->instanceList, uriP->instanceId)) return COAP_404_NOT_FOUND;
+
+        // single instance read
+        if (LWM2M_URI_IS_SET_RESOURCE(uriP))
+        {
+            size = 1;
+            dataP = lwm2m_data_new(size);
+            if (dataP == NULL) return COAP_500_INTERNAL_SERVER_ERROR;
+
+            dataP->id = uriP->resourceId;
+        }
+
+        result = targetP->discoverFunc(uriP->instanceId, &size, &dataP, targetP);
+    }
+    else
+    {
+        // multiple object instances read
+        lwm2m_list_t * instanceP;
+        int i;
+
+        result = COAP_205_CONTENT;
+
+        size = 0;
+        for (instanceP = targetP->instanceList; instanceP != NULL ; instanceP = instanceP->next)
+        {
+            size++;
+        }
+
+        if (size != 0)
+        {
+            dataP = lwm2m_data_new(size);
+            if (dataP == NULL) return COAP_500_INTERNAL_SERVER_ERROR;
+
+            instanceP = targetP->instanceList;
+            i = 0;
+            while (instanceP != NULL && result == COAP_205_CONTENT)
+            {
+                result = targetP->discoverFunc(instanceP->id, (int*)&(dataP[i].value.asChildren.count), &(dataP[i].value.asChildren.array), targetP);
+                dataP[i].type = LWM2M_TYPE_OBJECT_INSTANCE;
+                dataP[i].id = instanceP->id;
+                i++;
+                instanceP = instanceP->next;
+            }
+        }
+    }
+
+    if (result == COAP_205_CONTENT)
+    {
+        int len;
+
+        len = discover_serialize(contextP, uriP, serverP, size, dataP, bufferP);
+        if (len <= 0) result = COAP_500_INTERNAL_SERVER_ERROR;
+        else *lengthP = len;
+    }
+    lwm2m_data_free(size, dataP);
+
+    LOG_ARG("result: %u.%2u", (result & 0xFF) >> 5, (result & 0x1F));
+
+    return result;
 }
 
 bool object_isInstanceNew(lwm2m_context_t * contextP,
                           uint16_t objectId,
                           uint16_t instanceId)
 {
-    switch (objectId)
+    lwm2m_object_t * targetP;
+
+    LOG("Entering");
+    targetP = (lwm2m_object_t *)LWM2M_LIST_FIND(contextP->objectList, objectId);
+    if (targetP != NULL)
     {
-    case LWM2M_SECURITY_OBJECT_ID:
-    case LWM2M_SERVER_OBJECT_ID:
-        if (NULL != lwm2m_list_find((lwm2m_list_t *)contextP->serverList, instanceId))
+        if (NULL != lwm2m_list_find(targetP->instanceList, instanceId))
         {
             return false;
         }
-        break;
-
-    default:
-        {
-            lwm2m_object_t * targetP;
-
-            targetP = prv_find_object(contextP, objectId);
-            if (targetP != NULL)
-            {
-                if (NULL != lwm2m_list_find(targetP->instanceList, instanceId))
-                {
-                    return false;
-                }
-            }
-        }
-        break;
     }
 
     return true;
 }
 
-int prv_getRegisterPayload(lwm2m_context_t * contextP,
-                           char * buffer,
-                           size_t length)
+static int prv_getObjectTemplate(uint8_t * buffer,
+                                 size_t length,
+                                 uint16_t id)
 {
     int index;
-    int i;
     int result;
 
-    lwm2m_server_t * serverP;
+    if (length < REG_OBJECT_MIN_LEN) return -1;
+
+    buffer[0] = '<';
+    buffer[1] = '/';
+    index = 2;
+
+    result = utils_intCopy((char *)buffer + index, length - index, id);
+    if (result < 0) return -1;
+    index += result;
+
+    if (length - index < REG_OBJECT_MIN_LEN - 3) return -1;
+    buffer[index] = '/';
+    index++;
 
-    // index can not be greater than length
+    return index;
+}
+
+int object_getRegisterPayload(lwm2m_context_t * contextP,
+                           uint8_t * buffer,
+                           size_t bufferLen)
+{
+    size_t index;
+    int result;
+    lwm2m_object_t * objectP;
+
+    LOG("Entering");
+    // index can not be greater than bufferLen
     index = 0;
-    for (serverP = contextP->serverList;
-         serverP != NULL;
-         serverP = serverP->next)
+
+    result = utils_stringCopy((char *)buffer, bufferLen, REG_START);
+    if (result < 0) return 0;
+    index += result;
+
+    if ((contextP->altPath != NULL)
+     && (contextP->altPath[0] != 0))
+    {
+        result = utils_stringCopy((char *)buffer + index, bufferLen - index, contextP->altPath);
+    }
+    else
     {
-        result = snprintf(buffer + index, length - index, "</%hu/%hu>,", LWM2M_SERVER_OBJECT_ID, serverP->shortID);
-        if (result > 0 && result <= length - index)
+        result = utils_stringCopy((char *)buffer + index, bufferLen - index, REG_DEFAULT_PATH);
+    }
+    if (result < 0) return 0;
+    index += result;
+
+    result = utils_stringCopy((char *)buffer + index, bufferLen - index, REG_LWM2M_RESOURCE_TYPE);
+    if (result < 0) return 0;
+    index += result;
+
+    for (objectP = contextP->objectList; objectP != NULL; objectP = objectP->next)
+    {
+        size_t start;
+        size_t length;
+
+        if (objectP->objID == LWM2M_SECURITY_OBJECT_ID) continue;
+
+        start = index;
+        result = prv_getObjectTemplate(buffer + index, bufferLen - index, objectP->objID);
+        if (result < 0) return 0;
+        length = result;
+        index += length;
+
+        if (objectP->instanceList == NULL)
         {
+            index--;
+            result = utils_stringCopy((char *)buffer + index, bufferLen - index, REG_PATH_END);
+            if (result < 0) return 0;
             index += result;
         }
         else
         {
-            return 0;
-        }
-    }
-
-    for (i = 0 ; i < contextP->numObject ; i++)
-    {
-        if (contextP->objectList[i]->instanceList == NULL)
-        {
-            result = snprintf(buffer + index, length - index, "</%hu>,", contextP->objectList[i]->objID);
-            if (result > 0 && result <= length - index)
-            {
-                index += result;
-            }
-            else
+            lwm2m_list_t * targetP;
+            for (targetP = objectP->instanceList ; targetP != NULL ; targetP = targetP->next)
             {
-                return 0;
-            }
-        }
-        else
-        {
-            lwm2m_list_t * targetP;
-            for (targetP = contextP->objectList[i]->instanceList ; targetP != NULL ; targetP = targetP->next)
-            {
-                int result;
+                if (bufferLen - index <= length) return 0;
+
+                if (index != start + length)
+                {
+                    memcpy(buffer + index, buffer + start, length);
+                    index += length;
+                }
 
-                result = snprintf(buffer + index, length - index, "</%hu/%hu>,", contextP->objectList[i]->objID, targetP->id);
-                if (result > 0 && result <= length - index)
-                {
-                    index += result;
-                }
-                else
-                {
-                    return 0;
-                }
+                result = utils_intCopy((char *)buffer + index, bufferLen - index, targetP->id);
+                if (result < 0) return 0;
+                index += result;
+
+                result = utils_stringCopy((char *)buffer + index, bufferLen - index, REG_PATH_END);
+                if (result < 0) return 0;
+                index += result;
             }
         }
     }
@@ -458,4 +596,235 @@
 
     return index;
 }
+
+static lwm2m_list_t * prv_findServerInstance(lwm2m_object_t * objectP,
+                                             uint16_t shortID)
+{
+    lwm2m_list_t * instanceP;
+
+    instanceP = objectP->instanceList;
+    while (NULL != instanceP)
+    {
+        int64_t value;
+        lwm2m_data_t * dataP;
+        int size;
+
+        size = 1;
+        dataP = lwm2m_data_new(size);
+        if (dataP == NULL) return NULL;
+        dataP->id = LWM2M_SERVER_SHORT_ID_ID;
+
+        if (objectP->readFunc(instanceP->id, &size, &dataP, objectP) != COAP_205_CONTENT)
+        {
+            lwm2m_data_free(size, dataP);
+            return NULL;
+        }
+
+        if (1 == lwm2m_data_decode_int(dataP, &value))
+        {
+            if (value == shortID)
+            {
+                lwm2m_data_free(size, dataP);
+                break;
+            }
+        }
+        lwm2m_data_free(size, dataP);
+        instanceP = instanceP->next;
+    }
+
+    return instanceP;
+}
+
+static int prv_getMandatoryInfo(lwm2m_object_t * objectP,
+                                uint16_t instanceID,
+                                lwm2m_server_t * targetP)
+{
+    lwm2m_data_t * dataP;
+    int size;
+    int64_t value;
+
+    size = 2;
+    dataP = lwm2m_data_new(size);
+    if (dataP == NULL) return -1;
+    dataP[0].id = LWM2M_SERVER_LIFETIME_ID;
+    dataP[1].id = LWM2M_SERVER_BINDING_ID;
+
+    if (objectP->readFunc(instanceID, &size, &dataP, objectP) != COAP_205_CONTENT)
+    {
+        lwm2m_data_free(size, dataP);
+        return -1;
+    }
+
+    if (0 == lwm2m_data_decode_int(dataP, &value)
+     || value < 0 || value >0xFFFFFFFF)             // This is an implementation limit
+    {
+        lwm2m_data_free(size, dataP);
+        return -1;
+    }
+    targetP->lifetime = value;
+
+    targetP->binding = utils_stringToBinding(dataP[1].value.asBuffer.buffer, dataP[1].value.asBuffer.length);
+
+    lwm2m_data_free(size, dataP);
+
+    if (targetP->binding == BINDING_UNKNOWN)
+    {
+        return -1;
+    }
+
+    return 0;
+}
+
+int object_getServers(lwm2m_context_t * contextP)
+{
+    lwm2m_object_t * objectP;
+    lwm2m_object_t * securityObjP = NULL;
+    lwm2m_object_t * serverObjP = NULL;
+    lwm2m_list_t * securityInstP;   // instanceID of the server in the LWM2M Security Object
+
+    LOG("Entering");
+
+    for (objectP = contextP->objectList; objectP != NULL; objectP = objectP->next)
+    {
+        if (objectP->objID == LWM2M_SECURITY_OBJECT_ID)
+        {
+            securityObjP = objectP;
+        }
+        else if (objectP->objID == LWM2M_SERVER_OBJECT_ID)
+        {
+            serverObjP = objectP;
+        }
+    }
+
+    if (NULL == securityObjP) return -1;
+
+    securityInstP = securityObjP->instanceList;
+    while (securityInstP != NULL)
+    {
+        if (LWM2M_LIST_FIND(contextP->bootstrapServerList, securityInstP->id) == NULL
+         && LWM2M_LIST_FIND(contextP->serverList, securityInstP->id) == NULL)
+        {
+            // This server is new. eg created by last bootstrap
+
+            lwm2m_data_t * dataP;
+            int size;
+            lwm2m_server_t * targetP;
+            bool isBootstrap;
+            int64_t value = 0;
+
+            size = 3;
+            dataP = lwm2m_data_new(size);
+            if (dataP == NULL) return -1;
+            dataP[0].id = LWM2M_SECURITY_BOOTSTRAP_ID;
+            dataP[1].id = LWM2M_SECURITY_SHORT_SERVER_ID;
+            dataP[2].id = LWM2M_SECURITY_HOLD_OFF_ID;
+
+            if (securityObjP->readFunc(securityInstP->id, &size, &dataP, securityObjP) != COAP_205_CONTENT)
+            {
+                lwm2m_data_free(size, dataP);
+                return -1;
+            }
+
+            targetP = (lwm2m_server_t *)lwm2m_malloc(sizeof(lwm2m_server_t));
+            if (targetP == NULL) {
+                lwm2m_data_free(size, dataP);
+                return -1;
+            }
+            memset(targetP, 0, sizeof(lwm2m_server_t));
+            targetP->secObjInstID = securityInstP->id;
+
+            if (0 == lwm2m_data_decode_bool(dataP + 0, &isBootstrap))
+            {
+                lwm2m_free(targetP);
+                lwm2m_data_free(size, dataP);
+                return -1;
+            }
+
+            if (0 == lwm2m_data_decode_int(dataP + 1, &value)
+             || value < (isBootstrap ? 0 : 1) || value > 0xFFFF)                // 0 is forbidden as a Short Server ID
+            {
+                lwm2m_free(targetP);
+                lwm2m_data_free(size, dataP);
+                return -1;
+            }
+            targetP->shortID = value;
+
+            if (isBootstrap == true)
+            {
+                if (0 == lwm2m_data_decode_int(dataP + 2, &value)
+                 || value < 0 || value > 0xFFFFFFFF)             // This is an implementation limit
+                {
+                    lwm2m_free(targetP);
+                    lwm2m_data_free(size, dataP);
+                    return -1;
+                }
+                // lifetime of a bootstrap server is set to ClientHoldOffTime
+                targetP->lifetime = value;
+
+                contextP->bootstrapServerList = (lwm2m_server_t*)LWM2M_LIST_ADD(contextP->bootstrapServerList, targetP);
+            }
+            else
+            {
+                lwm2m_list_t * serverInstP;     // instanceID of the server in the LWM2M Server Object
+
+                serverInstP = prv_findServerInstance(serverObjP, targetP->shortID);
+                if (serverInstP == NULL)
+                {
+                    lwm2m_free(targetP);
+                    lwm2m_data_free(size, dataP);
+                    return -1;
+                }
+                if (0 != prv_getMandatoryInfo(serverObjP, serverInstP->id, targetP))
+                {
+                    lwm2m_free(targetP);
+                    lwm2m_data_free(size, dataP);
+                    return -1;
+                }
+                targetP->status = STATE_DEREGISTERED;
+                contextP->serverList = (lwm2m_server_t*)LWM2M_LIST_ADD(contextP->serverList, targetP);
+            }
+            lwm2m_data_free(size, dataP);
+        }
+        securityInstP = securityInstP->next;
+    }
+
+    return 0;
+}
+
+coap_status_t object_createInstance(lwm2m_context_t * contextP,
+                                    lwm2m_uri_t * uriP,
+                                    lwm2m_data_t * dataP)
+{
+    lwm2m_object_t * targetP;
+
+    LOG_URI(uriP);
+    targetP = (lwm2m_object_t *)LWM2M_LIST_FIND(contextP->objectList, uriP->objectId);
+    if (NULL == targetP) return COAP_404_NOT_FOUND;
+
+    if (NULL == targetP->createFunc) 
+    {
+        return COAP_405_METHOD_NOT_ALLOWED;
+    }
+
+    return targetP->createFunc(lwm2m_list_newId(targetP->instanceList), dataP->value.asChildren.count, dataP->value.asChildren.array, targetP);
+}
+
+coap_status_t object_writeInstance(lwm2m_context_t * contextP,
+                            lwm2m_uri_t * uriP,
+                            lwm2m_data_t * dataP)
+{
+    lwm2m_object_t * targetP;
+
+    LOG_URI(uriP);
+    targetP = (lwm2m_object_t *)LWM2M_LIST_FIND(contextP->objectList, uriP->objectId);
+    if (NULL == targetP) return COAP_404_NOT_FOUND;
+
+    if (NULL == targetP->writeFunc) 
+    {
+        return COAP_405_METHOD_NOT_ALLOWED;
+    }
+
+    return targetP->writeFunc(dataP->id, dataP->value.asChildren.count, dataP->value.asChildren.array, targetP);
+}
+
 #endif