Contiki-NG
lwm2m-tlv.c
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2015-2018, Yanzi Networks AB.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  * 1. Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * 3. Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived
15  * from this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDER AND CONTRIBUTORS
18  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
19  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
20  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
21  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
22  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
23  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
25  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
26  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
27  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
28  * OF THE POSSIBILITY OF SUCH DAMAGE.
29  */
30 
31 /**
32  * \addtogroup lwm2m
33  * @{
34  *
35  */
36 
37 /**
38  * \file
39  * Implementation of the Contiki OMA LWM2M TLV
40  * \author
41  * Joakim Eriksson <joakime@sics.se>
42  * Niclas Finne <nfi@sics.se>
43  */
44 
45 #include <string.h>
46 #include <stdint.h>
47 #include "lwm2m-tlv.h"
48 
49 /* Log configuration */
50 #include "coap-log.h"
51 #define LOG_MODULE "lwm2m-tlv"
52 #define LOG_LEVEL LOG_LEVEL_NONE
53 
54 /*---------------------------------------------------------------------------*/
55 static inline uint8_t
56 get_len_type(const lwm2m_tlv_t *tlv)
57 {
58  if(tlv->length < 8) {
59  return 0;
60  } else if(tlv->length < 256) {
61  return 1;
62  } else if(tlv->length < 0x10000) {
63  return 2;
64  } else {
65  return 3;
66  }
67 }
68 /*---------------------------------------------------------------------------*/
69 size_t
70 lwm2m_tlv_read(lwm2m_tlv_t *tlv, const uint8_t *buffer, size_t len)
71 {
72  uint8_t len_type;
73  uint8_t len_pos = 1;
74  size_t tlv_len;
75 
76  tlv->type = (buffer[0] >> 6) & 3;
77  len_type = (buffer[0] >> 3) & 3;
78  len_pos = 1 + (((buffer[0] & (1 << 5)) != 0) ? 2 : 1);
79 
80  tlv->id = buffer[1];
81  /* if len_pos is larger than two it means that there is more ID to read */
82  if(len_pos > 2) {
83  tlv->id = (tlv->id << 8) + buffer[2];
84  }
85 
86  if(len_type == 0) {
87  tlv_len = buffer[0] & 7;
88  } else {
89  /* read the length */
90  tlv_len = 0;
91  while(len_type > 0) {
92  tlv_len = tlv_len << 8 | buffer[len_pos++];
93  len_type--;
94  }
95  }
96  /* and read out the data??? */
97  tlv->length = tlv_len;
98  tlv->value = &buffer[len_pos];
99 
100  return len_pos + tlv_len;
101 }
102 /*---------------------------------------------------------------------------*/
103 size_t
104 lwm2m_tlv_get_size(const lwm2m_tlv_t *tlv)
105 {
106  size_t size;
107  /* first hdr + len size */
108  size = 1 + get_len_type(tlv);
109  /* id size */
110  size += (tlv->id > 255) ? 2 : 1;
111 
112  /* and the length */
113  size += tlv->length;
114  return size;
115 }
116 /*---------------------------------------------------------------------------*/
117 /* If the tlv->value is NULL - only the header will be generated - useful for
118  * large strings or opaque streaming (block transfer)
119  */
120 size_t
121 lwm2m_tlv_write(const lwm2m_tlv_t *tlv, uint8_t *buffer, size_t buffersize)
122 {
123  int pos;
124  uint8_t len_type;
125 
126  /* len type is the same as number of bytes required for length */
127  len_type = get_len_type(tlv);
128  pos = 1 + len_type;
129  /* ensure that we do not write too much */
130  if(tlv->value != NULL && buffersize < tlv->length + pos) {
131  LOG_WARN("Could not write the TLV - buffer overflow.\n");
132  return 0;
133  }
134 
135  if(buffersize < pos + 2) {
136  return 0;
137  }
138 
139  /* first type byte in TLV header */
140  buffer[0] = (tlv->type << 6) |
141  (tlv->id > 255 ? (1 << 5) : 0) |
142  (len_type << 3) |
143  (len_type == 0 ? tlv->length : 0);
144 
145  pos = 1;
146  /* The ID */
147  if(tlv->id > 255) {
148  buffer[pos++] = (tlv->id >> 8) & 0xff;
149  }
150  buffer[pos++] = tlv->id & 0xff;
151  /* Add length if needed - unrolled loop ? */
152  if(len_type > 2) {
153  buffer[pos++] = (tlv->length >> 16) & 0xff;
154  }
155  if(len_type > 1) {
156  buffer[pos++] = (tlv->length >> 8) & 0xff;
157  }
158  if(len_type > 0) {
159  buffer[pos++] = tlv->length & 0xff;
160  }
161 
162  /* finally add the value */
163  if(tlv->value != NULL && tlv->length > 0) {
164  memcpy(&buffer[pos], tlv->value, tlv->length);
165  }
166 
167  if(LOG_DBG_ENABLED) {
168  int i;
169  LOG_DBG("TLV: ");
170  for(i = 0; i < pos + ((tlv->value != NULL) ? tlv->length : 0); i++) {
171  LOG_DBG_("%02x", buffer[i]);
172  }
173  LOG_DBG_("\n");
174  }
175 
176  return pos + ((tlv->value != NULL) ? tlv->length : 0);
177 }
178 /*---------------------------------------------------------------------------*/
179 int32_t
180 lwm2m_tlv_get_int32(const lwm2m_tlv_t *tlv)
181 {
182  int i;
183  int32_t value = 0;
184 
185  for(i = 0; i < tlv->length; i++) {
186  value = (value << 8) | tlv->value[i];
187  }
188 
189  /* Ensure that we set all the bits above what we read in */
190  if(tlv->value[0] & 0x80) {
191  while(i < 4) {
192  value = value | (0xff << (i * 8));
193  i++;
194  }
195  }
196 
197  return value;
198 }
199 /*---------------------------------------------------------------------------*/
200 size_t
201 lwm2m_tlv_write_int32(uint8_t type, int16_t id, int32_t value, uint8_t *buffer, size_t len)
202 {
203  lwm2m_tlv_t tlv;
204  uint8_t buf[4];
205  int i;
206  int v;
207  int last_bit;
208  LOG_DBG("Exporting int32 %d %"PRId32" ", id, value);
209 
210  v = value < 0 ? -1 : 0;
211  i = 0;
212  do {
213  buf[3 - i] = value & 0xff;
214  /* check if the last MSB indicates that we need another byte */
215  last_bit = (v == 0 && (value & 0x80) > 0) || (v == -1 && (value & 0x80) == 0);
216  value = value >> 8;
217  i++;
218  } while((value != v || last_bit) && i < 4);
219 
220  /* export INT as TLV */
221  LOG_DBG("len: %d\n", i);
222  tlv.type = type;
223  tlv.length = i;
224  tlv.value = &buf[3 - (i - 1)];
225  tlv.id = id;
226  return lwm2m_tlv_write(&tlv, buffer, len);
227 }
228 /*---------------------------------------------------------------------------*/
229 /* convert fixpoint 32-bit to a IEEE Float in the byte array*/
230 size_t
231 lwm2m_tlv_write_float32(uint8_t type, int16_t id, int32_t value, int bits,
232  uint8_t *buffer, size_t len)
233 {
234  int i;
235  int e = 0;
236  int32_t val = 0;
237  int32_t v;
238  uint8_t b[4];
239  lwm2m_tlv_t tlv;
240 
241  v = value;
242  if(v < 0) {
243  v = -v;
244  }
245 
246  while(v > 1) {
247  val = (val >> 1);
248  if (v & 1) {
249  val = val | (1L << 22);
250  }
251  v = (v >> 1);
252  e++;
253  }
254 
255  LOG_DBG("Sign: %d, Fraction: %06"PRIx32" 0b", value < 0, val);
256  for(i = 0; i < 23; i++) {
257  LOG_DBG_("%"PRId32"", ((val >> (22 - i)) & 1));
258  }
259  LOG_DBG_("\nExp:%d\n", e);
260 
261  /* convert to the thing we should have */
262  e = e - bits + 127;
263 
264  if(value == 0) {
265  e = 0;
266  }
267 
268  /* is this the right byte order? */
269  b[0] = (value < 0 ? 0x80 : 0) | (e >> 1);
270  b[1] = ((e & 1) << 7) | ((val >> 16) & 0x7f);
271  b[2] = (val >> 8) & 0xff;
272  b[3] = val & 0xff;
273 
274  LOG_DBG("B=%02x%02x%02x%02x\n", b[0], b[1], b[2], b[3]);
275  /* construct the TLV */
276  tlv.type = type;
277  tlv.length = 4;
278  tlv.value = b;
279  tlv.id = id;
280 
281  return lwm2m_tlv_write(&tlv, buffer, len);
282 }
283 /*---------------------------------------------------------------------------*/
284 /* convert float to fixpoint */
285 size_t
286 lwm2m_tlv_float32_to_fix(const lwm2m_tlv_t *tlv, int32_t *value, int bits)
287 {
288  /* TLV needs to be 4 bytes */
289  int e, i;
290  int32_t val;
291  int sign = (tlv->value[0] & 0x80) != 0;
292  e = ((tlv->value[0] << 1) & 0xff) | (tlv->value[1] >> 7);
293  val = (((long)tlv->value[1] & 0x7f) << 16) | (tlv->value[2] << 8) | tlv->value[3];
294 
295  LOG_DBG("Sign: %d, Fraction: %06"PRIx32" 0b", val < 0, val);
296  for(i = 0; i < 23; i++) {
297  LOG_DBG_("%"PRId32"", ((val >> (22 - i)) & 1));
298  }
299  LOG_DBG("\nExp:%d => %d\n", e, e - 127);
300 
301  e = e - 127 + bits;
302 
303  /* e corresponds to the number of times we need to roll the number */
304 
305  LOG_DBG("Actual e=%d\n", e);
306  e = e - 23;
307  LOG_DBG("E after sub %d\n", e);
308  val = val | 1L << 23;
309  if(e > 0) {
310  val = val << e;
311  } else {
312  val = val >> -e;
313  }
314 
315  *value = sign ? -val : val;
316  return 4;
317 }
318 /*---------------------------------------------------------------------------*/
319 /** @} */
320 
321 #if 0
322 int main(int argc, char *argv[])
323 {
324  lwm2m_tlv_t tlv;
325  uint8_t data[24];
326  /* Make -1 */
327  tlv.length = 2;
328  tlv.value = data;
329  data[0] = 0x00;
330  data[1] = 0x80,
331 
332  printf("TLV:%d\n", lwm2m_tlv_get_int32(&tlv));
333 
334  printf("Len: %d\n", lwm2m_tlv_write_int32(0, 1, -0x88987f, data, 24));
335 }
336 #endif
Log support for CoAP
Header file for the Contiki OMA LWM2M TLV