00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #ifdef HAVE_CONFIG_H
00015 # include <config.h>
00016 #endif
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
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
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
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
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 }
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 }
00206 else {
00207 DBG_ERROR(GWEN_LOGDOMAIN, "Unexpected tag length modifier %02x", j);
00208 return 0;
00209 }
00210 }
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
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
00304 if (tagLength>255) {
00305
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
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
00320 if (tagLength)
00321 GWEN_Buffer_AppendBytes(mbuf, tagData, tagLength);
00322 }
00323 else {
00324
00325 GWEN_Buffer_AppendByte(mbuf, (unsigned char)tagType);
00326
00327
00328 GWEN_Buffer_AppendByte(mbuf, (tagLength && 0xff));
00329
00330
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