gwenhywfar  4.6.0beta
tlv.c
Go to the documentation of this file.
1 /***************************************************************************
2  begin : Sun Jun 13 2004
3  copyright : (C) 2004-2011 by Martin Preuss
4  email : martin@libchipcard.de
5 
6  ***************************************************************************
7  * Please see toplevel file COPYING for license details *
8  ***************************************************************************/
9 
10 
11 #ifdef HAVE_CONFIG_H
12 # include <config.h>
13 #endif
14 
15 #define DISABLE_DEBUGLOG
16 
17 
18 #include "tlv_p.h"
19 #include <gwenhywfar/debug.h>
20 #include <gwenhywfar/inherit.h>
21 #include <gwenhywfar/misc.h>
22 #include <gwenhywfar/text.h>
23 
24 #include <stdlib.h>
25 #include <assert.h>
26 #include <string.h>
27 
28 
30 
31 
33  GWEN_TLV *tlv;
34 
37 
38  return tlv;
39 }
40 
41 
42 
43 void GWEN_TLV_free(GWEN_TLV *tlv) {
44  if (tlv) {
45  free(tlv->tagData);
47  GWEN_FREE_OBJECT(tlv);
48  }
49 }
50 
51 
52 
53 GWEN_TLV *GWEN_TLV_create(unsigned int tagType,
54  unsigned int tagMode,
55  const void *p,
56  unsigned int dlen,
57  int isBerTlv) {
58  GWEN_TLV *tlv;
59 
60  /* some checks first */
61  if (tagType>255) {
62  DBG_ERROR(GWEN_LOGDOMAIN, "Tag type too high");
63  abort();
64  }
65  if (isBerTlv) {
66  if (dlen>65535) {
67  DBG_ERROR(GWEN_LOGDOMAIN, "Data too long");
68  abort();
69  }
70  }
71  else {
72  if (dlen>255) {
73  DBG_ERROR(GWEN_LOGDOMAIN, "Data too long");
74  abort();
75  }
76  }
77 
78  /* limits ok, create TLV */
79  tlv=GWEN_TLV_new();
80  tlv->tagType=tagType;
81  tlv->tagMode=tagMode;
82  tlv->isBerTlv=isBerTlv;
83 
84  tlv->tagLength=dlen;
85  if (dlen) {
86  tlv->tagData=malloc(dlen);
87  assert(tlv->tagData);
88  memmove(tlv->tagData, p, dlen);
89  }
90 
91  return tlv;
92 }
93 
94 
95 
96 int GWEN_TLV_IsBerTlv(const GWEN_TLV *tlv){
97  assert(tlv);
98  return tlv->isBerTlv;
99 }
100 
101 
102 
103 unsigned int GWEN_TLV_GetTagType(const GWEN_TLV *tlv){
104  assert(tlv);
105  return tlv->tagType;
106 }
107 
108 
109 
110 unsigned int GWEN_TLV_GetTagLength(const GWEN_TLV *tlv){
111  assert(tlv);
112  return tlv->tagLength;
113 }
114 
115 
116 
117 unsigned int GWEN_TLV_GetTagSize(const GWEN_TLV *tlv){
118  assert(tlv);
119  return tlv->tagSize;
120 }
121 
122 
123 
124 const void *GWEN_TLV_GetTagData(const GWEN_TLV *tlv){
125  assert(tlv);
126  return tlv->tagData;
127 }
128 
129 
130 
131 GWEN_TLV *GWEN_TLV_fromBuffer(GWEN_BUFFER *mbuf, int isBerTlv) {
132  const char *p;
133  unsigned int tagMode;
134  unsigned int tagType;
135  unsigned int tagLength;
136  const char *tagData;
137  unsigned int size;
138  unsigned int pos;
139  unsigned int j;
140  GWEN_TLV *tlv;
141  uint32_t startPos;
142 
143  if (!GWEN_Buffer_GetBytesLeft(mbuf)) {
144  DBG_ERROR(GWEN_LOGDOMAIN, "Buffer empty");
145  return 0;
146  }
147 
148  startPos=GWEN_Buffer_GetPos(mbuf);
149 
150  tagMode=tagType=tagLength=0;
151 
153  pos=0;
154  size=GWEN_Buffer_GetBytesLeft(mbuf);
155 
156  /* get tag type */
157  if (size<2) {
158  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes for BER-TLV");
159  return 0;
160  }
161  j=(unsigned char)(p[pos]);
162  tagMode=(j & 0xe0);
163  if (isBerTlv) {
164  if ((j & 0x1f)==0x1f) {
165  pos++;
166  if (pos>=size) {
167  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
168  return 0;
169  }
170  j=(unsigned char)(p[pos]);
171  }
172  else
173  j&=0x1f;
174  }
175  DBG_DEBUG(GWEN_LOGDOMAIN, "Tag type %02x%s", j,
176  isBerTlv?" (BER-TLV)":"");
177  tagType=j;
178 
179  /* get length */
180  pos++;
181  if (pos>=size) {
182  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
183  return 0;
184  }
185  j=(unsigned char)(p[pos]);
186  if (isBerTlv) {
187  if (j & 0x80) {
188  if (j==0x81) {
189  pos++;
190  if (pos>=size) {
191  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
192  return 0;
193  }
194  j=(unsigned char)(p[pos]);
195  } /* 0x81 */
196  else if (j==0x82) {
197  if (pos+1>=size) {
198  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
199  return 0;
200  }
201  pos++;
202  j=((unsigned char)(p[pos]))<<8;
203  pos++;
204  j+=(unsigned char)(p[pos]);
205  } /* 0x82 */
206  else {
207  DBG_ERROR(GWEN_LOGDOMAIN, "Unexpected tag length modifier %02x at %d", j, pos);
208  return 0;
209  }
210  } /* if tag length modifier */
211  }
212  else {
213  if (j==255) {
214  if (pos+2>=size) {
215  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
216  return 0;
217  }
218  pos++;
219  j=((unsigned char)(p[pos]))<<8;
220  pos++;
221  j+=(unsigned char)(p[pos]);
222  }
223  }
224  pos++;
225  tagLength=j;
226  tagData=p+pos;
227  GWEN_Buffer_IncrementPos(mbuf, pos);
228 
229  DBG_DEBUG(GWEN_LOGDOMAIN, "Tag: %02x (%d bytes)", tagType, tagLength);
230  if (pos+j>size) {
231  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
232  return 0;
233  }
234 
235  tlv=GWEN_TLV_new();
236  assert(tlv);
237  tlv->isBerTlv=isBerTlv;
238  tlv->tagMode=tagMode;
239  tlv->tagType=tagType;
240  tlv->tagLength=tagLength;
241  if (tagLength) {
242  tlv->tagData=(void*)malloc(tagLength);
243  memmove(tlv->tagData, tagData, tagLength);
244  }
245 
246  GWEN_Buffer_IncrementPos(mbuf, tagLength);
247  tlv->tagSize=GWEN_Buffer_GetPos(mbuf)-startPos;
248  return tlv;
249 }
250 
251 
252 
254  assert(tlv);
255  return (tlv->tagMode & 0x20);
256 }
257 
258 
259 
260 unsigned int GWEN_TLV_GetClass(const GWEN_TLV *tlv){
261  assert(tlv);
262  return (tlv->tagMode & 0xc0);
263 }
264 
265 
266 
268  assert(tlv);
269  return GWEN_TLV_DirectlyToBuffer(tlv->tagType,
270  tlv->tagMode,
271  tlv->tagData,
272  tlv->tagLength,
273  tlv->isBerTlv,
274  mbuf);
275 }
276 
277 
278 
279 int GWEN_TLV_DirectlyToBuffer(unsigned int tagType,
280  unsigned int tagMode,
281  const void *tagData,
282  int tagLength,
283  int isBerTlv,
284  GWEN_BUFFER *mbuf) {
285  if (tagLength==-1)
286  tagLength=strlen(tagData);
287 
288  if (isBerTlv) {
289  unsigned char j;
290 
291  /* write tag type */
292  j=tagMode;
293  if (tagType>=0x1f) {
294  j|=0x1f;
295  GWEN_Buffer_AppendByte(mbuf, j);
296  GWEN_Buffer_AppendByte(mbuf, (unsigned char)tagType);
297  }
298  else {
299  j|=tagType;
300  GWEN_Buffer_AppendByte(mbuf, j);
301  }
302 
303  /* write tag length */
304  if (tagLength>255) {
305  /* two byte size */
306  GWEN_Buffer_AppendByte(mbuf, 0x82);
307  GWEN_Buffer_AppendByte(mbuf, ((tagLength>>8) & 0xff));
308  GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff));
309  }
310  else if (tagLength>127) {
311  /* one byte size */
312  GWEN_Buffer_AppendByte(mbuf, 0x81);
313  GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff));
314  }
315  else {
316  GWEN_Buffer_AppendByte(mbuf, (tagLength & 0x7f));
317  }
318 
319  /* write tag data */
320  if (tagLength)
321  GWEN_Buffer_AppendBytes(mbuf, tagData, tagLength);
322  }
323  else {
324  /* write tag type */
325  GWEN_Buffer_AppendByte(mbuf, (unsigned char)tagType);
326 
327  /* write tag length */
328  GWEN_Buffer_AppendByte(mbuf, (tagLength && 0xff));
329 
330  /* write tag data */
331  if (tagLength)
332  GWEN_Buffer_AppendBytes(mbuf, tagData, tagLength);
333  }
334 
335  return 0;
336 }
337 
338 
339 
340 int GWEN_TLV_ReadHeader(GWEN_TLV *tlv, const uint8_t *p, uint32_t size, int isBerTlv) {
341  uint64_t tagMode;
342  uint64_t tagType;
343  uint64_t tagLength;
344  unsigned int pos;
345  uint64_t j;
346 
347  tagMode=tagType=tagLength=0;
348 
349  pos=0;
350 
351  /* get tag type */
352  if (size<2) {
353  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes for TLV");
354  return GWEN_ERROR_BAD_DATA;
355  }
356  j=(unsigned char)(p[pos]);
357  tagMode=(j & 0xe0);
358  if (isBerTlv) {
359  if ((j & 0x1f)==0x1f) {
360  pos++;
361  if (pos>=size) {
362  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
363  return 0;
364  }
365  j=(unsigned char)(p[pos]);
366  }
367  else
368  j&=0x1f;
369  }
370  DBG_DEBUG(GWEN_LOGDOMAIN, "Tag type %02x%s", j,
371  isBerTlv?" (BER-TLV)":"");
372  tagType=j;
373 
374  /* get length */
375  pos++;
376  if (pos>=size) {
377  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
378  return GWEN_ERROR_BAD_DATA;
379  }
380  j=(unsigned char)(p[pos]);
381  if (isBerTlv) {
382  if (j & 0x80) {
383  if (j==0x81) {
384  pos++;
385  if (pos>=size) {
386  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
387  return GWEN_ERROR_BAD_DATA;
388  }
389  j=(unsigned char)(p[pos]);
390  } /* 0x81 */
391  else if (j==0x82) {
392  if (pos+1>=size) {
393  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
394  return GWEN_ERROR_BAD_DATA;
395  }
396  pos++;
397  j=((unsigned char)(p[pos]))<<8;
398  pos++;
399  j+=(unsigned char)(p[pos]);
400  } /* 0x82 */
401  else if (j==0x83) {
402  if (pos+2>=size) {
403  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
404  return GWEN_ERROR_BAD_DATA;
405  }
406  pos++;
407  j=((unsigned char)(p[pos]))<<16;
408  pos++;
409  j+=((unsigned char)(p[pos]))<<8;
410  pos++;
411  j+=(unsigned char)(p[pos]);
412  } /* 0x83 */
413  else if (j==0x84) {
414  if (pos+3>=size) {
415  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
416  return GWEN_ERROR_BAD_DATA;
417  }
418  pos++;
419  j=((unsigned char)(p[pos]))<<24;
420  pos++;
421  j+=((unsigned char)(p[pos]))<<16;
422  pos++;
423  j+=((unsigned char)(p[pos]))<<8;
424  pos++;
425  j+=(unsigned char)(p[pos]);
426  } /* 0x84 */
427  else if (j==0x85) {
428  if (pos+4>=size) {
429  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
430  return GWEN_ERROR_BAD_DATA;
431  }
432  pos++;
433  j=((uint64_t) ((unsigned char)(p[pos])))<<32;
434  pos++;
435  j+=((uint64_t) ((unsigned char)(p[pos])))<<24;
436  pos++;
437  j+=((uint64_t) ((unsigned char)(p[pos])))<<16;
438  pos++;
439  j+=((uint64_t) ((unsigned char)(p[pos])))<<8;
440  pos++;
441  j+=(unsigned char)(p[pos]);
442  } /* 0x85 */
443  else {
444  DBG_ERROR(GWEN_LOGDOMAIN, "Unexpected tag length modifier %02x at %d", (int) j, pos);
445  return GWEN_ERROR_BAD_DATA;
446  }
447  } /* if tag length modifier */
448  }
449  else {
450  if (j==255) {
451  if (pos+2>=size) {
452  DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes");
453  return GWEN_ERROR_BAD_DATA;
454  }
455  pos++;
456  j=((unsigned char)(p[pos]))<<8;
457  pos++;
458  j+=(unsigned char)(p[pos]);
459  }
460  }
461  pos++;
462  tagLength=j;
463 
464  DBG_DEBUG(GWEN_LOGDOMAIN, "Tag: %02x (%d bytes)", tagType, tagLength);
465 
466  tlv->isBerTlv=isBerTlv;
467  tlv->tagMode=tagMode;
468  tlv->tagType=tagType;
469  tlv->tagLength=tagLength;
470 
471  tlv->tagSize=pos+tagLength;
472  return (int) pos;
473 }
474 
475 
476 
477 int GWEN_TLV_WriteHeader(unsigned int tagType,
478  unsigned int tagMode,
479  uint64_t tagLength,
480  int isBerTlv,
481  GWEN_BUFFER *mbuf) {
482  if (isBerTlv) {
483  unsigned char j;
484 
485  /* write tag type */
486  j=tagMode;
487  if (tagType>=0x1f) {
488  j|=0x1f;
489  GWEN_Buffer_AppendByte(mbuf, j);
490  GWEN_Buffer_AppendByte(mbuf, (unsigned char)tagType);
491  }
492  else {
493  j|=tagType;
494  GWEN_Buffer_AppendByte(mbuf, j);
495  }
496 
497  /* write tag length */
498  if (tagLength>0xffffffffLL) {
499  /* five byte size */
500  GWEN_Buffer_AppendByte(mbuf, 0x85);
501  GWEN_Buffer_AppendByte(mbuf, ((tagLength>>32) & 0xff));
502  GWEN_Buffer_AppendByte(mbuf, ((tagLength>>24) & 0xff));
503  GWEN_Buffer_AppendByte(mbuf, ((tagLength>>16) & 0xff));
504  GWEN_Buffer_AppendByte(mbuf, ((tagLength>>8) & 0xff));
505  GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff));
506  }
507  else if (tagLength>0xffffffL) {
508  /* four byte size */
509  GWEN_Buffer_AppendByte(mbuf, 0x84);
510  GWEN_Buffer_AppendByte(mbuf, ((tagLength>>24) & 0xff));
511  GWEN_Buffer_AppendByte(mbuf, ((tagLength>>16) & 0xff));
512  GWEN_Buffer_AppendByte(mbuf, ((tagLength>>8) & 0xff));
513  GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff));
514  }
515  else if (tagLength>0xffff) {
516  /* three byte size */
517  GWEN_Buffer_AppendByte(mbuf, 0x83);
518  GWEN_Buffer_AppendByte(mbuf, ((tagLength>>16) & 0xff));
519  GWEN_Buffer_AppendByte(mbuf, ((tagLength>>8) & 0xff));
520  GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff));
521  }
522  else if (tagLength>0xff) {
523  /* two byte size */
524  GWEN_Buffer_AppendByte(mbuf, 0x82);
525  GWEN_Buffer_AppendByte(mbuf, ((tagLength>>8) & 0xff));
526  GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff));
527  }
528  else if (tagLength>127) {
529  /* one byte size */
530  GWEN_Buffer_AppendByte(mbuf, 0x81);
531  GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff));
532  }
533  else {
534  GWEN_Buffer_AppendByte(mbuf, (tagLength & 0x7f));
535  }
536  }
537  else {
538  /* write tag type */
539  GWEN_Buffer_AppendByte(mbuf, (unsigned char)tagType);
540 
541  /* write tag length */
542  GWEN_Buffer_AppendByte(mbuf, (tagLength && 0xff));
543  }
544 
545  return 0;
546 }
547 
548 
549 
550 
551 
552 
553