gwenhywfar
4.3.1
|
00001 /*************************************************************************** 00002 begin : Sun Jun 13 2004 00003 copyright : (C) 2004-2011 by Martin Preuss 00004 email : martin@libchipcard.de 00005 00006 *************************************************************************** 00007 * Please see toplevel file COPYING for license details * 00008 ***************************************************************************/ 00009 00010 00011 #ifdef HAVE_CONFIG_H 00012 # include <config.h> 00013 #endif 00014 00015 #define DISABLE_DEBUGLOG 00016 00017 00018 #include "tlv_p.h" 00019 #include <gwenhywfar/debug.h> 00020 #include <gwenhywfar/inherit.h> 00021 #include <gwenhywfar/misc.h> 00022 #include <gwenhywfar/text.h> 00023 00024 #include <stdlib.h> 00025 #include <assert.h> 00026 #include <string.h> 00027 00028 00029 GWEN_LIST_FUNCTIONS(GWEN_TLV, GWEN_TLV) 00030 00031 00032 GWEN_TLV *GWEN_TLV_new(void) { 00033 GWEN_TLV *tlv; 00034 00035 GWEN_NEW_OBJECT(GWEN_TLV, tlv); 00036 GWEN_LIST_INIT(GWEN_TLV, tlv); 00037 00038 return tlv; 00039 } 00040 00041 00042 00043 void GWEN_TLV_free(GWEN_TLV *tlv) { 00044 if (tlv) { 00045 free(tlv->tagData); 00046 GWEN_LIST_FINI(GWEN_TLV, tlv); 00047 GWEN_FREE_OBJECT(tlv); 00048 } 00049 } 00050 00051 00052 00053 GWEN_TLV *GWEN_TLV_create(unsigned int tagType, 00054 unsigned int tagMode, 00055 const void *p, 00056 unsigned int dlen, 00057 int isBerTlv) { 00058 GWEN_TLV *tlv; 00059 00060 /* some checks first */ 00061 if (tagType>255) { 00062 DBG_ERROR(GWEN_LOGDOMAIN, "Tag type too high"); 00063 abort(); 00064 } 00065 if (isBerTlv) { 00066 if (dlen>65535) { 00067 DBG_ERROR(GWEN_LOGDOMAIN, "Data too long"); 00068 abort(); 00069 } 00070 } 00071 else { 00072 if (dlen>255) { 00073 DBG_ERROR(GWEN_LOGDOMAIN, "Data too long"); 00074 abort(); 00075 } 00076 } 00077 00078 /* limits ok, create TLV */ 00079 tlv=GWEN_TLV_new(); 00080 tlv->tagType=tagType; 00081 tlv->tagMode=tagMode; 00082 tlv->isBerTlv=isBerTlv; 00083 00084 tlv->tagLength=dlen; 00085 if (dlen) { 00086 tlv->tagData=malloc(dlen); 00087 assert(tlv->tagData); 00088 memmove(tlv->tagData, p, dlen); 00089 } 00090 00091 return tlv; 00092 } 00093 00094 00095 00096 int GWEN_TLV_IsBerTlv(const GWEN_TLV *tlv){ 00097 assert(tlv); 00098 return tlv->isBerTlv; 00099 } 00100 00101 00102 00103 unsigned int GWEN_TLV_GetTagType(const GWEN_TLV *tlv){ 00104 assert(tlv); 00105 return tlv->tagType; 00106 } 00107 00108 00109 00110 unsigned int GWEN_TLV_GetTagLength(const GWEN_TLV *tlv){ 00111 assert(tlv); 00112 return tlv->tagLength; 00113 } 00114 00115 00116 00117 unsigned int GWEN_TLV_GetTagSize(const GWEN_TLV *tlv){ 00118 assert(tlv); 00119 return tlv->tagSize; 00120 } 00121 00122 00123 00124 const void *GWEN_TLV_GetTagData(const GWEN_TLV *tlv){ 00125 assert(tlv); 00126 return tlv->tagData; 00127 } 00128 00129 00130 00131 GWEN_TLV *GWEN_TLV_fromBuffer(GWEN_BUFFER *mbuf, int isBerTlv) { 00132 const char *p; 00133 unsigned int tagMode; 00134 unsigned int tagType; 00135 unsigned int tagLength; 00136 const char *tagData; 00137 unsigned int size; 00138 unsigned int pos; 00139 unsigned int j; 00140 GWEN_TLV *tlv; 00141 uint32_t startPos; 00142 00143 if (!GWEN_Buffer_GetBytesLeft(mbuf)) { 00144 DBG_ERROR(GWEN_LOGDOMAIN, "Buffer empty"); 00145 return 0; 00146 } 00147 00148 startPos=GWEN_Buffer_GetPos(mbuf); 00149 00150 tagMode=tagType=tagLength=0; 00151 00152 p=GWEN_Buffer_GetPosPointer(mbuf); 00153 pos=0; 00154 size=GWEN_Buffer_GetBytesLeft(mbuf); 00155 00156 /* get tag type */ 00157 if (size<2) { 00158 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes for BER-TLV"); 00159 return 0; 00160 } 00161 j=(unsigned char)(p[pos]); 00162 tagMode=(j & 0xe0); 00163 if (isBerTlv) { 00164 if ((j & 0x1f)==0x1f) { 00165 pos++; 00166 if (pos>=size) { 00167 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00168 return 0; 00169 } 00170 j=(unsigned char)(p[pos]); 00171 } 00172 else 00173 j&=0x1f; 00174 } 00175 DBG_DEBUG(GWEN_LOGDOMAIN, "Tag type %02x%s", j, 00176 isBerTlv?" (BER-TLV)":""); 00177 tagType=j; 00178 00179 /* get length */ 00180 pos++; 00181 if (pos>=size) { 00182 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00183 return 0; 00184 } 00185 j=(unsigned char)(p[pos]); 00186 if (isBerTlv) { 00187 if (j & 0x80) { 00188 if (j==0x81) { 00189 pos++; 00190 if (pos>=size) { 00191 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00192 return 0; 00193 } 00194 j=(unsigned char)(p[pos]); 00195 } /* 0x81 */ 00196 else if (j==0x82) { 00197 if (pos+1>=size) { 00198 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00199 return 0; 00200 } 00201 pos++; 00202 j=((unsigned char)(p[pos]))<<8; 00203 pos++; 00204 j+=(unsigned char)(p[pos]); 00205 } /* 0x82 */ 00206 else { 00207 DBG_ERROR(GWEN_LOGDOMAIN, "Unexpected tag length modifier %02x at %d", j, pos); 00208 return 0; 00209 } 00210 } /* if tag length modifier */ 00211 } 00212 else { 00213 if (j==255) { 00214 if (pos+2>=size) { 00215 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00216 return 0; 00217 } 00218 pos++; 00219 j=((unsigned char)(p[pos]))<<8; 00220 pos++; 00221 j+=(unsigned char)(p[pos]); 00222 } 00223 } 00224 pos++; 00225 tagLength=j; 00226 tagData=p+pos; 00227 GWEN_Buffer_IncrementPos(mbuf, pos); 00228 00229 DBG_DEBUG(GWEN_LOGDOMAIN, "Tag: %02x (%d bytes)", tagType, tagLength); 00230 if (pos+j>size) { 00231 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00232 return 0; 00233 } 00234 00235 tlv=GWEN_TLV_new(); 00236 assert(tlv); 00237 tlv->isBerTlv=isBerTlv; 00238 tlv->tagMode=tagMode; 00239 tlv->tagType=tagType; 00240 tlv->tagLength=tagLength; 00241 if (tagLength) { 00242 tlv->tagData=(void*)malloc(tagLength); 00243 memmove(tlv->tagData, tagData, tagLength); 00244 } 00245 00246 GWEN_Buffer_IncrementPos(mbuf, tagLength); 00247 tlv->tagSize=GWEN_Buffer_GetPos(mbuf)-startPos; 00248 return tlv; 00249 } 00250 00251 00252 00253 int GWEN_TLV_IsContructed(const GWEN_TLV *tlv){ 00254 assert(tlv); 00255 return (tlv->tagMode & 0x20); 00256 } 00257 00258 00259 00260 unsigned int GWEN_TLV_GetClass(const GWEN_TLV *tlv){ 00261 assert(tlv); 00262 return (tlv->tagMode & 0xc0); 00263 } 00264 00265 00266 00267 int GWEN_TLV_toBuffer(GWEN_TLV *tlv, GWEN_BUFFER *mbuf) { 00268 assert(tlv); 00269 return GWEN_TLV_DirectlyToBuffer(tlv->tagType, 00270 tlv->tagMode, 00271 tlv->tagData, 00272 tlv->tagLength, 00273 tlv->isBerTlv, 00274 mbuf); 00275 } 00276 00277 00278 00279 int GWEN_TLV_DirectlyToBuffer(unsigned int tagType, 00280 unsigned int tagMode, 00281 const void *tagData, 00282 int tagLength, 00283 int isBerTlv, 00284 GWEN_BUFFER *mbuf) { 00285 if (tagLength==-1) 00286 tagLength=strlen(tagData); 00287 00288 if (isBerTlv) { 00289 unsigned char j; 00290 00291 /* write tag type */ 00292 j=tagMode; 00293 if (tagType>=0x1f) { 00294 j|=0x1f; 00295 GWEN_Buffer_AppendByte(mbuf, j); 00296 GWEN_Buffer_AppendByte(mbuf, (unsigned char)tagType); 00297 } 00298 else { 00299 j|=tagType; 00300 GWEN_Buffer_AppendByte(mbuf, j); 00301 } 00302 00303 /* write tag length */ 00304 if (tagLength>255) { 00305 /* two byte size */ 00306 GWEN_Buffer_AppendByte(mbuf, 0x82); 00307 GWEN_Buffer_AppendByte(mbuf, ((tagLength>>8) & 0xff)); 00308 GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff)); 00309 } 00310 else if (tagLength>127) { 00311 /* one byte size */ 00312 GWEN_Buffer_AppendByte(mbuf, 0x81); 00313 GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff)); 00314 } 00315 else { 00316 GWEN_Buffer_AppendByte(mbuf, (tagLength & 0x7f)); 00317 } 00318 00319 /* write tag data */ 00320 if (tagLength) 00321 GWEN_Buffer_AppendBytes(mbuf, tagData, tagLength); 00322 } 00323 else { 00324 /* write tag type */ 00325 GWEN_Buffer_AppendByte(mbuf, (unsigned char)tagType); 00326 00327 /* write tag length */ 00328 GWEN_Buffer_AppendByte(mbuf, (tagLength && 0xff)); 00329 00330 /* write tag data */ 00331 if (tagLength) 00332 GWEN_Buffer_AppendBytes(mbuf, tagData, tagLength); 00333 } 00334 00335 return 0; 00336 } 00337 00338 00339 00340 int GWEN_TLV_ReadHeader(GWEN_TLV *tlv, const uint8_t *p, uint32_t size, int isBerTlv) { 00341 uint64_t tagMode; 00342 uint64_t tagType; 00343 uint64_t tagLength; 00344 unsigned int pos; 00345 uint64_t j; 00346 00347 tagMode=tagType=tagLength=0; 00348 00349 pos=0; 00350 00351 /* get tag type */ 00352 if (size<2) { 00353 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes for TLV"); 00354 return GWEN_ERROR_BAD_DATA; 00355 } 00356 j=(unsigned char)(p[pos]); 00357 tagMode=(j & 0xe0); 00358 if (isBerTlv) { 00359 if ((j & 0x1f)==0x1f) { 00360 pos++; 00361 if (pos>=size) { 00362 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00363 return 0; 00364 } 00365 j=(unsigned char)(p[pos]); 00366 } 00367 else 00368 j&=0x1f; 00369 } 00370 DBG_DEBUG(GWEN_LOGDOMAIN, "Tag type %02x%s", j, 00371 isBerTlv?" (BER-TLV)":""); 00372 tagType=j; 00373 00374 /* get length */ 00375 pos++; 00376 if (pos>=size) { 00377 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00378 return GWEN_ERROR_BAD_DATA; 00379 } 00380 j=(unsigned char)(p[pos]); 00381 if (isBerTlv) { 00382 if (j & 0x80) { 00383 if (j==0x81) { 00384 pos++; 00385 if (pos>=size) { 00386 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00387 return GWEN_ERROR_BAD_DATA; 00388 } 00389 j=(unsigned char)(p[pos]); 00390 } /* 0x81 */ 00391 else if (j==0x82) { 00392 if (pos+1>=size) { 00393 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00394 return GWEN_ERROR_BAD_DATA; 00395 } 00396 pos++; 00397 j=((unsigned char)(p[pos]))<<8; 00398 pos++; 00399 j+=(unsigned char)(p[pos]); 00400 } /* 0x82 */ 00401 else if (j==0x83) { 00402 if (pos+2>=size) { 00403 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00404 return GWEN_ERROR_BAD_DATA; 00405 } 00406 pos++; 00407 j=((unsigned char)(p[pos]))<<16; 00408 pos++; 00409 j+=((unsigned char)(p[pos]))<<8; 00410 pos++; 00411 j+=(unsigned char)(p[pos]); 00412 } /* 0x83 */ 00413 else if (j==0x84) { 00414 if (pos+3>=size) { 00415 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00416 return GWEN_ERROR_BAD_DATA; 00417 } 00418 pos++; 00419 j=((unsigned char)(p[pos]))<<24; 00420 pos++; 00421 j+=((unsigned char)(p[pos]))<<16; 00422 pos++; 00423 j+=((unsigned char)(p[pos]))<<8; 00424 pos++; 00425 j+=(unsigned char)(p[pos]); 00426 } /* 0x84 */ 00427 else if (j==0x85) { 00428 if (pos+4>=size) { 00429 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00430 return GWEN_ERROR_BAD_DATA; 00431 } 00432 pos++; 00433 j=((uint64_t) ((unsigned char)(p[pos])))<<32; 00434 pos++; 00435 j+=((uint64_t) ((unsigned char)(p[pos])))<<24; 00436 pos++; 00437 j+=((uint64_t) ((unsigned char)(p[pos])))<<16; 00438 pos++; 00439 j+=((uint64_t) ((unsigned char)(p[pos])))<<8; 00440 pos++; 00441 j+=(unsigned char)(p[pos]); 00442 } /* 0x85 */ 00443 else { 00444 DBG_ERROR(GWEN_LOGDOMAIN, "Unexpected tag length modifier %02x at %d", (int) j, pos); 00445 return GWEN_ERROR_BAD_DATA; 00446 } 00447 } /* if tag length modifier */ 00448 } 00449 else { 00450 if (j==255) { 00451 if (pos+2>=size) { 00452 DBG_ERROR(GWEN_LOGDOMAIN, "Too few bytes"); 00453 return GWEN_ERROR_BAD_DATA; 00454 } 00455 pos++; 00456 j=((unsigned char)(p[pos]))<<8; 00457 pos++; 00458 j+=(unsigned char)(p[pos]); 00459 } 00460 } 00461 pos++; 00462 tagLength=j; 00463 00464 DBG_DEBUG(GWEN_LOGDOMAIN, "Tag: %02x (%d bytes)", tagType, tagLength); 00465 00466 tlv->isBerTlv=isBerTlv; 00467 tlv->tagMode=tagMode; 00468 tlv->tagType=tagType; 00469 tlv->tagLength=tagLength; 00470 00471 tlv->tagSize=pos+tagLength; 00472 return (int) pos; 00473 } 00474 00475 00476 00477 int GWEN_TLV_WriteHeader(unsigned int tagType, 00478 unsigned int tagMode, 00479 uint64_t tagLength, 00480 int isBerTlv, 00481 GWEN_BUFFER *mbuf) { 00482 if (isBerTlv) { 00483 unsigned char j; 00484 00485 /* write tag type */ 00486 j=tagMode; 00487 if (tagType>=0x1f) { 00488 j|=0x1f; 00489 GWEN_Buffer_AppendByte(mbuf, j); 00490 GWEN_Buffer_AppendByte(mbuf, (unsigned char)tagType); 00491 } 00492 else { 00493 j|=tagType; 00494 GWEN_Buffer_AppendByte(mbuf, j); 00495 } 00496 00497 /* write tag length */ 00498 if (tagLength>0xffffffffLL) { 00499 /* five byte size */ 00500 GWEN_Buffer_AppendByte(mbuf, 0x85); 00501 GWEN_Buffer_AppendByte(mbuf, ((tagLength>>32) & 0xff)); 00502 GWEN_Buffer_AppendByte(mbuf, ((tagLength>>24) & 0xff)); 00503 GWEN_Buffer_AppendByte(mbuf, ((tagLength>>16) & 0xff)); 00504 GWEN_Buffer_AppendByte(mbuf, ((tagLength>>8) & 0xff)); 00505 GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff)); 00506 } 00507 else if (tagLength>0xffffffL) { 00508 /* four byte size */ 00509 GWEN_Buffer_AppendByte(mbuf, 0x84); 00510 GWEN_Buffer_AppendByte(mbuf, ((tagLength>>24) & 0xff)); 00511 GWEN_Buffer_AppendByte(mbuf, ((tagLength>>16) & 0xff)); 00512 GWEN_Buffer_AppendByte(mbuf, ((tagLength>>8) & 0xff)); 00513 GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff)); 00514 } 00515 else if (tagLength>0xffff) { 00516 /* three byte size */ 00517 GWEN_Buffer_AppendByte(mbuf, 0x83); 00518 GWEN_Buffer_AppendByte(mbuf, ((tagLength>>16) & 0xff)); 00519 GWEN_Buffer_AppendByte(mbuf, ((tagLength>>8) & 0xff)); 00520 GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff)); 00521 } 00522 else if (tagLength>0xff) { 00523 /* two byte size */ 00524 GWEN_Buffer_AppendByte(mbuf, 0x82); 00525 GWEN_Buffer_AppendByte(mbuf, ((tagLength>>8) & 0xff)); 00526 GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff)); 00527 } 00528 else if (tagLength>127) { 00529 /* one byte size */ 00530 GWEN_Buffer_AppendByte(mbuf, 0x81); 00531 GWEN_Buffer_AppendByte(mbuf, (tagLength & 0xff)); 00532 } 00533 else { 00534 GWEN_Buffer_AppendByte(mbuf, (tagLength & 0x7f)); 00535 } 00536 } 00537 else { 00538 /* write tag type */ 00539 GWEN_Buffer_AppendByte(mbuf, (unsigned char)tagType); 00540 00541 /* write tag length */ 00542 GWEN_Buffer_AppendByte(mbuf, (tagLength && 0xff)); 00543 } 00544 00545 return 0; 00546 } 00547 00548 00549 00550 00551 00552 00553