gwenhywfar 4.0.3
|
00001 /*************************************************************************** 00002 begin : Sun Jun 13 2004 00003 copyright : (C) 2004 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() { 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 00341 00342 00343 00344 00345 00346