tlv.c

Go to the documentation of this file.
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