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 <inttypes.h>
48 #include "lwm2m-tlv.h"
49 
50 /* Log configuration */
51 #include "coap-log.h"
52 #define LOG_MODULE "lwm2m-tlv"
53 #define LOG_LEVEL LOG_LEVEL_NONE
54 
55 /*---------------------------------------------------------------------------*/
56 static inline uint8_t
57 get_len_type(const lwm2m_tlv_t *tlv)
58 {
59  if(tlv->length < 8) {
60  return 0;
61  } else if(tlv->length < 256) {
62  return 1;
63  } else if(tlv->length < 0x10000) {
64  return 2;
65  } else {
66  return 3;
67  }
68 }
69 /*---------------------------------------------------------------------------*/
70 size_t
71 lwm2m_tlv_read(lwm2m_tlv_t *tlv, const uint8_t *buffer, size_t len)
72 {
73  uint8_t len_type;
74  uint8_t len_pos = 1;
75  size_t tlv_len;
76 
77  tlv->type = (buffer[0] >> 6) & 3;
78  len_type = (buffer[0] >> 3) & 3;
79  len_pos = 1 + (((buffer[0] & (1 << 5)) != 0) ? 2 : 1);
80 
81  tlv->id = buffer[1];
82  /* if len_pos is larger than two it means that there is more ID to read */
83  if(len_pos > 2) {
84  tlv->id = (tlv->id << 8) + buffer[2];
85  }
86 
87  if(len_type == 0) {
88  tlv_len = buffer[0] & 7;
89  } else {
90  /* read the length */
91  tlv_len = 0;
92  while(len_type > 0) {
93  tlv_len = tlv_len << 8 | buffer[len_pos++];
94  len_type--;
95  }
96  }
97  /* and read out the data??? */
98  tlv->length = tlv_len;
99  tlv->value = &buffer[len_pos];
100 
101  return len_pos + tlv_len;
102 }
103 /*---------------------------------------------------------------------------*/
104 size_t
105 lwm2m_tlv_get_size(const lwm2m_tlv_t *tlv)
106 {
107  size_t size;
108  /* first hdr + len size */
109  size = 1 + get_len_type(tlv);
110  /* id size */
111  size += (tlv->id > 255) ? 2 : 1;
112 
113  /* and the length */
114  size += tlv->length;
115  return size;
116 }
117 /*---------------------------------------------------------------------------*/
118 /* If the tlv->value is NULL - only the header will be generated - useful for
119  * large strings or opaque streaming (block transfer)
120  */
121 size_t
122 lwm2m_tlv_write(const lwm2m_tlv_t *tlv, uint8_t *buffer, size_t buffersize)
123 {
124  int pos;
125  uint8_t len_type;
126 
127  /* len type is the same as number of bytes required for length */
128  len_type = get_len_type(tlv);
129  pos = 1 + len_type;
130  /* ensure that we do not write too much */
131  if(tlv->value != NULL && buffersize < tlv->length + pos) {
132  LOG_WARN("Could not write the TLV - buffer overflow.\n");
133  return 0;
134  }
135 
136  if(buffersize < pos + 2) {
137  return 0;
138  }
139 
140  /* first type byte in TLV header */
141  buffer[0] = (tlv->type << 6) |
142  (tlv->id > 255 ? (1 << 5) : 0) |
143  (len_type << 3) |
144  (len_type == 0 ? tlv->length : 0);
145 
146  pos = 1;
147  /* The ID */
148  if(tlv->id > 255) {
149  buffer[pos++] = (tlv->id >> 8) & 0xff;
150  }
151  buffer[pos++] = tlv->id & 0xff;
152  /* Add length if needed - unrolled loop ? */
153  if(len_type > 2) {
154  buffer[pos++] = (tlv->length >> 16) & 0xff;
155  }
156  if(len_type > 1) {
157  buffer[pos++] = (tlv->length >> 8) & 0xff;
158  }
159  if(len_type > 0) {
160  buffer[pos++] = tlv->length & 0xff;
161  }
162 
163  /* finally add the value */
164  if(tlv->value != NULL && tlv->length > 0) {
165  memcpy(&buffer[pos], tlv->value, tlv->length);
166  }
167 
168  if(LOG_DBG_ENABLED) {
169  int i;
170  LOG_DBG("TLV: ");
171  for(i = 0; i < pos + ((tlv->value != NULL) ? tlv->length : 0); i++) {
172  LOG_DBG_("%02x", buffer[i]);
173  }
174  LOG_DBG_("\n");
175  }
176 
177  return pos + ((tlv->value != NULL) ? tlv->length : 0);
178 }
179 /*---------------------------------------------------------------------------*/
180 int32_t
181 lwm2m_tlv_get_int32(const lwm2m_tlv_t *tlv)
182 {
183  int i;
184  int32_t value = 0;
185 
186  for(i = 0; i < tlv->length; i++) {
187  value = (value << 8) | tlv->value[i];
188  }
189 
190  /* Ensure that we set all the bits above what we read in */
191  if(tlv->value[0] & 0x80) {
192  while(i < 4) {
193  value = value | (0xff << (i * 8));
194  i++;
195  }
196  }
197 
198  return value;
199 }
200 /*---------------------------------------------------------------------------*/
201 size_t
202 lwm2m_tlv_write_int32(uint8_t type, int16_t id, int32_t value, uint8_t *buffer, size_t len)
203 {
204  lwm2m_tlv_t tlv;
205  uint8_t buf[4];
206  int i;
207  int v;
208  int last_bit;
209  LOG_DBG("Exporting int32 %d %"PRId32" ", id, value);
210 
211  v = value < 0 ? -1 : 0;
212  i = 0;
213  do {
214  buf[3 - i] = value & 0xff;
215  /* check if the last MSB indicates that we need another byte */
216  last_bit = (v == 0 && (value & 0x80) > 0) || (v == -1 && (value & 0x80) == 0);
217  value = value >> 8;
218  i++;
219  } while((value != v || last_bit) && i < 4);
220 
221  /* export INT as TLV */
222  LOG_DBG("len: %d\n", i);
223  tlv.type = type;
224  tlv.length = i;
225  tlv.value = &buf[3 - (i - 1)];
226  tlv.id = id;
227  return lwm2m_tlv_write(&tlv, buffer, len);
228 }
229 /*---------------------------------------------------------------------------*/
230 /* convert fixpoint 32-bit to a IEEE Float in the byte array*/
231 size_t
232 lwm2m_tlv_write_float32(uint8_t type, int16_t id, int32_t value, int bits,
233  uint8_t *buffer, size_t len)
234 {
235  int i;
236  int e = 0;
237  int32_t val = 0;
238  int32_t v;
239  uint8_t b[4];
240  lwm2m_tlv_t tlv;
241 
242  v = value;
243  if(v < 0) {
244  v = -v;
245  }
246 
247  while(v > 1) {
248  val = (val >> 1);
249  if (v & 1) {
250  val = val | (1L << 22);
251  }
252  v = (v >> 1);
253  e++;
254  }
255 
256  LOG_DBG("Sign: %d, Fraction: %06"PRIx32" 0b", value < 0, val);
257  for(i = 0; i < 23; i++) {
258  LOG_DBG_("%"PRId32"", ((val >> (22 - i)) & 1));
259  }
260  LOG_DBG_("\nExp:%d\n", e);
261 
262  /* convert to the thing we should have */
263  e = e - bits + 127;
264 
265  if(value == 0) {
266  e = 0;
267  }
268 
269  /* is this the right byte order? */
270  b[0] = (value < 0 ? 0x80 : 0) | (e >> 1);
271  b[1] = ((e & 1) << 7) | ((val >> 16) & 0x7f);
272  b[2] = (val >> 8) & 0xff;
273  b[3] = val & 0xff;
274 
275  LOG_DBG("B=%02x%02x%02x%02x\n", b[0], b[1], b[2], b[3]);
276  /* construct the TLV */
277  tlv.type = type;
278  tlv.length = 4;
279  tlv.value = b;
280  tlv.id = id;
281 
282  return lwm2m_tlv_write(&tlv, buffer, len);
283 }
284 /*---------------------------------------------------------------------------*/
285 /* convert float to fixpoint */
286 size_t
287 lwm2m_tlv_float32_to_fix(const lwm2m_tlv_t *tlv, int32_t *value, int bits)
288 {
289  /* TLV needs to be 4 bytes */
290  int e, i;
291  int32_t val;
292  int sign = (tlv->value[0] & 0x80) != 0;
293  e = ((tlv->value[0] << 1) & 0xff) | (tlv->value[1] >> 7);
294  val = (((long)tlv->value[1] & 0x7f) << 16) | (tlv->value[2] << 8) | tlv->value[3];
295 
296  LOG_DBG("Sign: %d, Fraction: %06"PRIx32" 0b", val < 0, val);
297  for(i = 0; i < 23; i++) {
298  LOG_DBG_("%"PRId32"", ((val >> (22 - i)) & 1));
299  }
300  LOG_DBG("\nExp:%d => %d\n", e, e - 127);
301 
302  e = e - 127 + bits;
303 
304  /* e corresponds to the number of times we need to roll the number */
305 
306  LOG_DBG("Actual e=%d\n", e);
307  e = e - 23;
308  LOG_DBG("E after sub %d\n", e);
309  val = val | 1L << 23;
310  if(e > 0) {
311  val = val << e;
312  } else {
313  val = val >> -e;
314  }
315 
316  *value = sign ? -val : val;
317  return 4;
318 }
319 /*---------------------------------------------------------------------------*/
320 /** @} */
321 
322 #if 0
323 int main(int argc, char *argv[])
324 {
325  lwm2m_tlv_t tlv;
326  uint8_t data[24];
327  /* Make -1 */
328  tlv.length = 2;
329  tlv.value = data;
330  data[0] = 0x00;
331  data[1] = 0x80,
332 
333  printf("TLV:%d\n", lwm2m_tlv_get_int32(&tlv));
334 
335  printf("Len: %d\n", lwm2m_tlv_write_int32(0, 1, -0x88987f, data, 24));
336 }
337 #endif
Log support for CoAP
Header file for the Contiki OMA LWM2M TLV