tlv.c

00001 /*******************************************************************************
00002  *
00003  * Copyright (c) 2013, 2014 Intel Corporation and others.
00004  * All rights reserved. This program and the accompanying materials
00005  * are made available under the terms of the Eclipse Public License v1.0
00006  * and Eclipse Distribution License v1.0 which accompany this distribution.
00007  *
00008  * The Eclipse Public License is available at
00009  *    http://www.eclipse.org/legal/epl-v10.html
00010  * The Eclipse Distribution License is available at
00011  *    http://www.eclipse.org/org/documents/edl-v10.php.
00012  *
00013  * Contributors:
00014  *    David Navarro, Intel Corporation - initial API and implementation
00015  *    Fabien Fleutot - Please refer to git log
00016  *    Bosch Software Innovations GmbH - Please refer to git log
00017  *    
00018  *******************************************************************************/
00019 
00020 #include "internals.h"
00021 #include <stdlib.h>
00022 #include <string.h>
00023 #include <stdio.h>
00024 #include <inttypes.h>
00025 #include <float.h>
00026 
00027 #ifndef LWM2M_BIG_ENDIAN
00028 #ifndef LWM2M_LITTLE_ENDIAN
00029 #error Please define LWM2M_BIG_ENDIAN or LWM2M_LITTLE_ENDIAN
00030 #endif
00031 #endif
00032 
00033 #define _PRV_TLV_TYPE_MASK 0xC0
00034 #define _PRV_TLV_HEADER_MAX_LENGTH 6
00035 
00036 #define _PRV_TLV_TYPE_UNKNOWN           (uint8_t)0xFF
00037 #define _PRV_TLV_TYPE_OBJECT            (uint8_t)0x10
00038 #define _PRV_TLV_TYPE_OBJECT_INSTANCE   (uint8_t)0x00
00039 #define _PRV_TLV_TYPE_RESOURCE          (uint8_t)0xC0
00040 #define _PRV_TLV_TYPE_MULTIPLE_RESOURCE (uint8_t)0x80
00041 #define _PRV_TLV_TYPE_RESOURCE_INSTANCE (uint8_t)0x40
00042 
00043 
00044 static uint8_t prv_getHeaderType(lwm2m_data_type_t type)
00045 {
00046     switch (type)
00047     {
00048     case LWM2M_TYPE_OBJECT:
00049         return _PRV_TLV_TYPE_OBJECT;
00050 
00051     case LWM2M_TYPE_OBJECT_INSTANCE:
00052         return _PRV_TLV_TYPE_OBJECT_INSTANCE;
00053 
00054     case LWM2M_TYPE_MULTIPLE_RESOURCE:
00055         return _PRV_TLV_TYPE_MULTIPLE_RESOURCE;
00056 
00057 
00058     case LWM2M_TYPE_STRING:
00059     case LWM2M_TYPE_INTEGER:
00060     case LWM2M_TYPE_FLOAT:
00061     case LWM2M_TYPE_BOOLEAN:
00062     case LWM2M_TYPE_OPAQUE:
00063     case LWM2M_TYPE_OBJECT_LINK:
00064         return _PRV_TLV_TYPE_RESOURCE;
00065 
00066     case LWM2M_TYPE_UNDEFINED:
00067     default:
00068         return _PRV_TLV_TYPE_UNKNOWN;
00069     }
00070 }
00071 
00072 static lwm2m_data_type_t prv_getDataType(uint8_t type)
00073 {
00074     switch (type)
00075     {
00076     case _PRV_TLV_TYPE_OBJECT:
00077         return LWM2M_TYPE_OBJECT;
00078 
00079     case _PRV_TLV_TYPE_OBJECT_INSTANCE:
00080         return LWM2M_TYPE_OBJECT_INSTANCE;
00081 
00082     case _PRV_TLV_TYPE_MULTIPLE_RESOURCE:
00083         return LWM2M_TYPE_MULTIPLE_RESOURCE;
00084 
00085     case _PRV_TLV_TYPE_RESOURCE:
00086     case _PRV_TLV_TYPE_RESOURCE_INSTANCE:
00087         return LWM2M_TYPE_OPAQUE;
00088 
00089     default:
00090         return LWM2M_TYPE_UNDEFINED;
00091     }
00092 }
00093 
00094 static int prv_getHeaderLength(uint16_t id,
00095                                size_t dataLen)
00096 {
00097     int length;
00098 
00099     length = 2;
00100 
00101     if (id > 0xFF)
00102     {
00103         length += 1;
00104     }
00105 
00106     if (dataLen > 0xFFFF)
00107     {
00108         length += 3;
00109     }
00110     else if (dataLen > 0xFF)
00111     {
00112         length += 2;
00113     }
00114     else if (dataLen > 7)
00115     {
00116         length += 1;
00117     }
00118 
00119     return length;
00120 }
00121 
00122 static int prv_createHeader(uint8_t * header,
00123                             bool isInstance,
00124                             lwm2m_data_type_t type,
00125                             uint16_t id,
00126                             size_t data_len)
00127 {
00128     int header_len;
00129     int offset;
00130     uint8_t hdrType;
00131 
00132     header_len = prv_getHeaderLength(id, data_len);
00133     if (isInstance == true)
00134     {
00135         hdrType = _PRV_TLV_TYPE_RESOURCE_INSTANCE;
00136     }
00137     else
00138     {
00139         hdrType = prv_getHeaderType(type);
00140     }
00141 
00142     header[0] = 0;
00143     header[0] |= hdrType&_PRV_TLV_TYPE_MASK;
00144 
00145     if (id > 0xFF)
00146     {
00147         header[0] |= 0x20;
00148         header[1] = (id >> 8) & 0XFF;
00149         header[2] = id & 0XFF;
00150         offset = 3;
00151     }
00152     else
00153     {
00154         header[1] = id;
00155         offset = 2;
00156     }
00157     if (data_len <= 7)
00158     {
00159         header[0] += data_len;
00160     }
00161     else if (data_len <= 0xFF)
00162     {
00163         header[0] |= 0x08;
00164         header[offset] = data_len;
00165     }
00166     else if (data_len <= 0xFFFF)
00167     {
00168         header[0] |= 0x10;
00169         header[offset] = (data_len >> 8) & 0XFF;
00170         header[offset + 1] = data_len & 0XFF;
00171     }
00172     else if (data_len <= 0xFFFFFF)
00173     {
00174         header[0] |= 0x18;
00175         header[offset] = (data_len >> 16) & 0XFF;
00176         header[offset + 1] = (data_len >> 8) & 0XFF;
00177         header[offset + 2] = data_len & 0XFF;
00178     }
00179 
00180     return header_len;
00181 }
00182 
00183 int lwm2m_decode_TLV(const uint8_t * buffer,
00184                     size_t buffer_len,
00185                     lwm2m_data_type_t * oType,
00186                     uint16_t * oID,
00187                     size_t * oDataIndex,
00188                     size_t * oDataLen)
00189 {
00190 
00191     LOG_ARG("buffer_len: %d", buffer_len);
00192         ;
00193     if (buffer_len < 2) return 0;
00194 
00195     *oDataIndex = 2;
00196 
00197     *oType = prv_getDataType(buffer[0]&_PRV_TLV_TYPE_MASK);
00198 
00199     if ((buffer[0]&0x20) == 0x20)
00200     {
00201         // id is 16 bits long
00202         if (buffer_len < 3) return 0;
00203         *oDataIndex += 1;
00204         *oID = (buffer[1]<<8) + buffer[2];
00205     }
00206     else
00207     {
00208         // id is 8 bits long
00209         *oID = buffer[1];
00210     }
00211 
00212     switch (buffer[0]&0x18)
00213     {
00214     case 0x00:
00215         // no length field
00216         *oDataLen = buffer[0]&0x07;
00217         break;
00218     case 0x08:
00219         // length field is 8 bits long
00220         if (buffer_len < *oDataIndex + 1) return 0;
00221         *oDataLen = buffer[*oDataIndex];
00222         *oDataIndex += 1;
00223         break;
00224     case 0x10:
00225         // length field is 16 bits long
00226         if (buffer_len < *oDataIndex + 2) return 0;
00227         *oDataLen = (buffer[*oDataIndex]<<8) + buffer[*oDataIndex+1];
00228         *oDataIndex += 2;
00229         break;
00230     case 0x18:
00231         // length field is 24 bits long
00232         if (buffer_len < *oDataIndex + 3) return 0;
00233         *oDataLen = (buffer[*oDataIndex]<<16) + (buffer[*oDataIndex+1]<<8) + buffer[*oDataIndex+2];
00234         *oDataIndex += 3;
00235         break;
00236     default:
00237         // can't happen
00238         return 0;
00239     }
00240 
00241     if (*oDataIndex + *oDataLen > buffer_len) return 0;
00242 
00243     return *oDataIndex + *oDataLen;
00244 }
00245 
00246 
00247 int tlv_parse(uint8_t * buffer,
00248               size_t bufferLen,
00249               lwm2m_data_t ** dataP)
00250 {
00251     lwm2m_data_type_t type;
00252     uint16_t id;
00253     size_t dataIndex;
00254     size_t dataLen;
00255     int index = 0;
00256     int result;
00257     int size = 0;
00258 
00259     LOG_ARG("bufferLen: %d", bufferLen);
00260 
00261     *dataP = NULL;
00262 
00263     while (0 != (result = lwm2m_decode_TLV((uint8_t*)buffer + index, bufferLen - index, &type, &id, &dataIndex, &dataLen)))
00264     {
00265         lwm2m_data_t * newTlvP;
00266 
00267         newTlvP = lwm2m_data_new(size + 1);
00268         if (size >= 1)
00269         {
00270             if (newTlvP == NULL)
00271             {
00272                 lwm2m_data_free(size, *dataP);
00273                 return 0;
00274             }
00275             else
00276             {
00277                 memcpy(newTlvP, *dataP, size * sizeof(lwm2m_data_t));
00278                 lwm2m_free(*dataP);
00279             }
00280         }
00281         *dataP = newTlvP;
00282 
00283         (*dataP)[size].type = type;
00284         (*dataP)[size].id = id;
00285         if (type == LWM2M_TYPE_OBJECT_INSTANCE || type == LWM2M_TYPE_MULTIPLE_RESOURCE)
00286         {
00287             (*dataP)[size].value.asChildren.count = tlv_parse(buffer + index + dataIndex,
00288                                                           dataLen,
00289                                                           &((*dataP)[size].value.asChildren.array));
00290             if ((*dataP)[size].value.asChildren.count == 0)
00291             {
00292                 lwm2m_data_free(size + 1, *dataP);
00293                 return 0;
00294             }
00295         }
00296         else
00297         {
00298             lwm2m_data_encode_opaque(buffer + index + dataIndex, dataLen, (*dataP) + size);
00299         }
00300         size++;
00301         index += result;
00302     }
00303 
00304     return size;
00305 }
00306 
00307 
00308 static int prv_getLength(int size,
00309                          lwm2m_data_t * dataP)
00310 {
00311     int length;
00312     int i;
00313 
00314     length = 0;
00315 
00316     for (i = 0 ; i < size && length != -1 ; i++)
00317     {
00318         switch (dataP[i].type)
00319         {
00320         case LWM2M_TYPE_OBJECT_INSTANCE:
00321         case LWM2M_TYPE_MULTIPLE_RESOURCE:
00322             {
00323                 int subLength;
00324 
00325                 subLength = prv_getLength(dataP[i].value.asChildren.count, dataP[i].value.asChildren.array);
00326                 if (subLength == -1)
00327                 {
00328                     length = -1;
00329                 }
00330                 else
00331                 {
00332                     length += prv_getHeaderLength(dataP[i].id, subLength) + subLength;
00333                 }
00334             }
00335             break;
00336 
00337         case LWM2M_TYPE_STRING:
00338         case LWM2M_TYPE_OPAQUE:
00339             length += prv_getHeaderLength(dataP[i].id, dataP[i].value.asBuffer.length) + dataP[i].value.asBuffer.length;
00340             break;
00341 
00342         case LWM2M_TYPE_INTEGER:
00343             {
00344                 size_t data_len;
00345                 uint8_t unused_buffer[_PRV_64BIT_BUFFER_SIZE];
00346 
00347                 data_len = utils_encodeInt(dataP[i].value.asInteger, unused_buffer);
00348                 length += prv_getHeaderLength(dataP[i].id, data_len) + data_len;
00349             }
00350             break;
00351 
00352         case LWM2M_TYPE_FLOAT:
00353             {
00354                 size_t data_len;
00355 
00356                 if ((dataP[i].value.asFloat < 0.0 - (double)FLT_MAX)
00357                     || (dataP[i].value.asFloat >(double)FLT_MAX))
00358                 {
00359                     data_len = 8;
00360                 }
00361                 else
00362                 {
00363                     data_len = 4;
00364                 }
00365 
00366                 length += prv_getHeaderLength(dataP[i].id, data_len) + data_len;
00367             }
00368             break;
00369 
00370         case LWM2M_TYPE_BOOLEAN:
00371             // Booleans are always encoded on one byte
00372             length += prv_getHeaderLength(dataP[i].id, 1) + 1;
00373             break;
00374 
00375         case LWM2M_TYPE_OBJECT_LINK:
00376             // Object Link are always encoded on four bytes
00377             length += prv_getHeaderLength(dataP[i].id, 4) + 4;
00378             break;
00379 
00380         default:
00381             length = -1;
00382             break;
00383         }
00384     }
00385 
00386     return length;
00387 }
00388 
00389 
00390 int tlv_serialize(bool isResourceInstance, 
00391                   int size,
00392                   lwm2m_data_t * dataP,
00393                   uint8_t ** bufferP)
00394 {
00395     int length;
00396     int index;
00397     int i;
00398 
00399     LOG_ARG("isResourceInstance: %s, size: %d", isResourceInstance?"true":"false", size);
00400 
00401     *bufferP = NULL;
00402     length = prv_getLength(size, dataP);
00403     if (length <= 0) return length;
00404 
00405     *bufferP = (uint8_t *)lwm2m_malloc(length);
00406     if (*bufferP == NULL) return 0;
00407 
00408     index = 0;
00409     for (i = 0 ; i < size && length != 0 ; i++)
00410     {
00411         int headerLen;
00412         bool isInstance;
00413 
00414         isInstance = isResourceInstance;
00415         switch (dataP[i].type)
00416         {
00417         case LWM2M_TYPE_MULTIPLE_RESOURCE:
00418             isInstance = true;
00419             // fall through
00420         case LWM2M_TYPE_OBJECT_INSTANCE:
00421             {
00422                 uint8_t * tmpBuffer;
00423                 int res;
00424 
00425                 res = tlv_serialize(isInstance, dataP[i].value.asChildren.count, dataP[i].value.asChildren.array, &tmpBuffer);
00426                 if (res < 0)
00427                 {
00428                     length = -1;
00429                 }
00430                 else
00431                 {
00432                     size_t tmpLength;
00433 
00434                     tmpLength = (size_t)res;
00435                     headerLen = prv_createHeader(*bufferP + index, false, dataP[i].type, dataP[i].id, tmpLength);
00436                     index += headerLen;
00437                     if (tmpLength > 0)
00438                     {
00439                         memcpy(*bufferP + index, tmpBuffer, tmpLength);
00440                         index += tmpLength;
00441                         lwm2m_free(tmpBuffer);
00442                     }
00443                 }
00444             }
00445             break;
00446 
00447         case LWM2M_TYPE_OBJECT_LINK:
00448             {
00449                 int k;
00450                 uint8_t buf[4];
00451                 uint32_t v = dataP[i].value.asObjLink.objectId;
00452                 v <<= 16;
00453                 v |= dataP[i].value.asObjLink.objectInstanceId;
00454                 for (k = 3; k >= 0; --k) {
00455                     buf[k] = (uint8_t)(v & 0xFF);
00456                     v >>= 8;
00457                 }
00458                 // keep encoding as buffer
00459                 headerLen = prv_createHeader(*bufferP + index, isInstance, dataP[i].type, dataP[i].id, 4);
00460                 index += headerLen;
00461                 memcpy(*bufferP + index, buf, 4);
00462                 index += 4;
00463             }
00464             break;
00465 
00466         case LWM2M_TYPE_STRING:
00467         case LWM2M_TYPE_OPAQUE:
00468             headerLen = prv_createHeader(*bufferP + index, isInstance, dataP[i].type, dataP[i].id, dataP[i].value.asBuffer.length);
00469             index += headerLen;
00470             memcpy(*bufferP + index, dataP[i].value.asBuffer.buffer, dataP[i].value.asBuffer.length);
00471             index += dataP[i].value.asBuffer.length;
00472             break;
00473 
00474         case LWM2M_TYPE_INTEGER:
00475             {
00476                 size_t data_len;
00477                 uint8_t data_buffer[_PRV_64BIT_BUFFER_SIZE];
00478 
00479                 data_len = utils_encodeInt(dataP[i].value.asInteger, data_buffer);
00480                 headerLen = prv_createHeader(*bufferP + index, isInstance, dataP[i].type, dataP[i].id, data_len);
00481                 index += headerLen;
00482                 memcpy(*bufferP + index, data_buffer, data_len);
00483                 index += data_len;
00484             }
00485             break;
00486 
00487         case LWM2M_TYPE_FLOAT:
00488             {
00489                 size_t data_len;
00490                 uint8_t data_buffer[_PRV_64BIT_BUFFER_SIZE];
00491 
00492                 data_len = utils_encodeFloat(dataP[i].value.asFloat, data_buffer);
00493                 headerLen = prv_createHeader(*bufferP + index, isInstance, dataP[i].type, dataP[i].id, data_len);
00494                 index += headerLen;
00495                 memcpy(*bufferP + index, data_buffer, data_len);
00496                 index += data_len;
00497             }
00498             break;
00499 
00500         case LWM2M_TYPE_BOOLEAN:
00501             headerLen = prv_createHeader(*bufferP + index, isInstance, dataP[i].type, dataP[i].id, 1);
00502             index += headerLen;
00503             (*bufferP)[index] = dataP[i].value.asBoolean ? 1 : 0;
00504             index += 1;
00505             break;
00506 
00507         default:
00508             length = -1;
00509             break;
00510         }
00511     }
00512 
00513     if (length < 0)
00514     {
00515         lwm2m_free(*bufferP);
00516         *bufferP = NULL;
00517     }
00518 
00519     LOG_ARG("returning %u", length);
00520 
00521     return length;
00522 }
00523