00001 #include <stdint.h>
00002 #include <stdlib.h>
00003 #include <stdio.h>
00004 #include <string.h>
00005 #include <ctype.h>
00006
00007 #include <ber.h>
00008
00009 static void hex_dump(const uint8_t *tmp, size_t len,
00010 size_t llen, unsigned int depth)
00011 {
00012 size_t i, j;
00013 size_t line;
00014
00015 for(j = 0; j < len; j += line, tmp += line) {
00016 if ( j + llen > len ) {
00017 line = len - j;
00018 }else{
00019 line = llen;
00020 }
00021
00022 printf("%*c%05x : ", depth, ' ', j);
00023
00024 for(i = 0; i < line; i++) {
00025 if ( isprint(tmp[i]) ) {
00026 printf("%c", tmp[i]);
00027 }else{
00028 printf(".");
00029 }
00030 }
00031
00032 for(; i < llen; i++)
00033 printf(" ");
00034
00035 for(i=0; i < line; i++)
00036 printf(" %02x", tmp[i]);
00037
00038 printf("\n");
00039 }
00040 printf("\n");
00041 }
00042
00043 #if 0
00044 static unsigned int ber_id_octet_class(const uint8_t cls)
00045 {
00046 return (cls & 0xc0) >> 6;
00047 }
00048 #endif
00049
00050 static unsigned int ber_id_octet_constructed(const uint8_t cls)
00051 {
00052 return (cls & 0x20) >> 5;
00053 }
00054
00055 static unsigned int ber_id_octet_tag(const uint8_t cls)
00056 {
00057 return cls & 0x1f;
00058 }
00059
00060 static unsigned int ber_len_form_short(const uint8_t cls)
00061 {
00062 return !(cls & 0x80);
00063 }
00064
00065 static unsigned int ber_len_short(const uint8_t cls)
00066 {
00067 return cls & 0x7f;
00068 }
00069
00070 void ber_dump(const uint8_t *buf, size_t len, unsigned int depth)
00071 {
00072 const uint8_t *end = buf + len;
00073 uint32_t clen, num, i;
00074 uint8_t idb;
00075 #if 0
00076 const char * const clsname[]={
00077 "universal",
00078 "application",
00079 "context-specific",
00080 "private",
00081 };
00082 #endif
00083 again:
00084 if ( buf >= end )
00085 return;
00086
00087 idb = *buf;
00088 num = ber_id_octet_tag(*buf);
00089 buf++;
00090
00091
00092 if ( num >= 0x1f ) {
00093 for(num = 0, i = 0; buf < end; i++) {
00094 num <<= 7;
00095 num |= *buf & 0x7f;
00096 buf++;
00097 if ( !(*buf & 0x80) )
00098 break;
00099 }
00100 }
00101
00102 printf("%*c.tag = %u (0x%x)\n", depth, ' ', num, num);
00103
00104 if ( buf >= end )
00105 return;
00106
00107 if ( ber_len_form_short(*buf) ) {
00108 clen = ber_len_short(*buf);
00109 buf++;
00110 }else{
00111 uint32_t l;
00112
00113 l = ber_len_short(*buf);
00114 if ( l > 4 || buf + l > end )
00115 return;
00116 buf++;
00117 for(clen = i = 0; i < l; i++, buf++) {
00118 clen <<= 8;
00119 clen |= *buf;
00120 }
00121
00122 }
00123
00124 if ( buf + clen > end )
00125 return;
00126
00127 printf("%*c.len = %u (0x%x)",
00128 depth, ' ', len, len);
00129
00130 if ( ber_id_octet_constructed(idb) ) {
00131 printf(" {\n");
00132 ber_dump(buf, clen, depth + 2);
00133 printf("%*c}\n", depth, ' ');
00134 }else{
00135 printf("\n");
00136 hex_dump(buf, clen, 16, depth);
00137 }
00138
00139 buf += clen;
00140 goto again;
00141 }
00142
00143 static int tag_cmp(const struct ber_tag *tag, const uint8_t *idb, size_t len)
00144 {
00145 if ( tag->tag_len < len )
00146 return 1;
00147 if ( tag->tag_len > len )
00148 return -1;
00149 return memcmp(idb, tag->tag, len);
00150 }
00151
00152 static const struct ber_tag *find_tag(const struct ber_tag *tags,
00153 unsigned int num_tags,
00154 const uint8_t *idb,
00155 size_t tag_len)
00156 {
00157 while ( num_tags ) {
00158 unsigned int i;
00159 int cmp;
00160
00161 i = num_tags / 2U;
00162 cmp = tag_cmp(tags + i, idb, tag_len);
00163 if ( cmp < 0 ) {
00164 num_tags = i;
00165 }else if ( cmp > 0 ) {
00166 tags = tags + (i + 1U);
00167 num_tags = num_tags - (i + 1U);
00168 }else
00169 return tags + i;
00170 }
00171
00172 return NULL;
00173 }
00174
00175 static const uint8_t *decode_tag(const uint8_t **ptr,
00176 const uint8_t *end,
00177 size_t *tag_len)
00178 {
00179 const uint8_t *tmp = *ptr;
00180
00181 if ( tmp >= end ) {
00182 *ptr = end;
00183 return NULL;
00184 }
00185
00186 if ( ber_id_octet_tag(*tmp) != 0x1f ) {
00187 tmp++;
00188 }else{
00189 for( tmp++; tmp < end; tmp++) {
00190 if ( 0 == (*tmp & 0x80) ) {
00191 tmp++;
00192 break;
00193 }
00194 }
00195 }
00196
00197 *tag_len = tmp - *ptr;
00198 end = *ptr;
00199 *ptr = tmp;
00200 return end;
00201 }
00202
00203 size_t ber_tag_len(const uint8_t *ptr, const uint8_t *end)
00204 {
00205 size_t ret;
00206 if ( NULL == decode_tag(&ptr, end, &ret) )
00207 return 0;
00208 return ret;
00209 }
00210
00211 const uint8_t *ber_decode_tag(const uint8_t **ptr,
00212 const uint8_t *end,
00213 size_t *tag_len)
00214 {
00215 return decode_tag(ptr, end, tag_len);
00216 }
00217
00218 static size_t decode_len(const uint8_t **ptr, const uint8_t *end)
00219 {
00220 const uint8_t *tmp = *ptr;
00221 size_t ret;
00222
00223 if ( ber_len_form_short(*tmp) ) {
00224 ret = ber_len_short(*tmp);
00225 tmp++;
00226 }else{
00227 size_t i, l;
00228
00229 l = ber_len_short(*tmp);
00230 if ( l > 4 || tmp + l > end ) {
00231 *ptr = end;
00232 return 1;
00233 }
00234
00235 tmp++;
00236 for(ret = i = 0; i < l; i++, tmp++) {
00237 ret <<= 8;
00238 ret |= *tmp;
00239 }
00240
00241 }
00242
00243 *ptr = tmp;
00244 return ret;
00245 }
00246
00247 size_t ber_decode_len(const uint8_t **ptr, const uint8_t *end)
00248 {
00249 return decode_len(ptr, end);
00250 }
00251
00252 int ber_decode(const struct ber_tag *tags, unsigned int num_tags,
00253 const uint8_t *ptr, size_t len, void *priv)
00254 {
00255 const uint8_t *end = ptr + len;
00256 unsigned int i;
00257 uint32_t clen;
00258
00259
00260
00261
00262 for(i = 0; ptr < end; ptr += clen) {
00263 const uint8_t *idb;
00264 const struct ber_tag *tag;
00265 size_t tag_len;
00266
00267 idb = decode_tag(&ptr, end, &tag_len);
00268 if ( ptr >= end )
00269 return 0;
00270
00271 clen = decode_len(&ptr, end);
00272 if ( ptr + clen > end )
00273 return 0;
00274
00275 tag = find_tag(tags, num_tags, idb, tag_len);
00276 if ( tag ) {
00277 if ( tag->op && !(*tag->op)(ptr, clen, priv) )
00278 return 0;
00279 i++;
00280 }else{
00281 size_t i;
00282 printf("unknown tag: ");
00283 for(i = 0; i < tag_len; i++)
00284 printf("%.2x ", idb[i]);
00285 printf("\n");
00286 hex_dump(ptr, clen, 16, 0);
00287 }
00288 }
00289
00290 return i;
00291 }