lwip_snmp_asn1.c

00001 /**
00002  * @file
00003  * Abstract Syntax Notation One (ISO 8824, 8825) encoding
00004  *
00005  * @todo not optimised (yet), favor correctness over speed, favor speed over size
00006  */
00007 
00008 /*
00009  * Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
00010  * All rights reserved.
00011  *
00012  * Redistribution and use in source and binary forms, with or without modification,
00013  * are permitted provided that the following conditions are met:
00014  *
00015  * 1. Redistributions of source code must retain the above copyright notice,
00016  *    this list of conditions and the following disclaimer.
00017  * 2. Redistributions in binary form must reproduce the above copyright notice,
00018  *    this list of conditions and the following disclaimer in the documentation
00019  *    and/or other materials provided with the distribution.
00020  * 3. The name of the author may not be used to endorse or promote products
00021  *    derived from this software without specific prior written permission.
00022  *
00023  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
00024  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
00025  * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
00026  * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00027  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
00028  * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00029  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00030  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
00031  * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
00032  * OF SUCH DAMAGE.
00033  *
00034  * Author: Christiaan Simons <christiaan.simons@axon.tv>
00035  *         Martin Hentschel <info@cl-soft.de>
00036  */
00037 
00038 #include "lwip/apps/snmp_opts.h"
00039 
00040 #if LWIP_SNMP /* don't build if not configured for use in lwipopts.h */
00041 
00042 #include "snmp_asn1.h"
00043 
00044 #define PBUF_OP_EXEC(code) \
00045   if ((code) != ERR_OK) { \
00046     return ERR_BUF; \
00047   }
00048 
00049 /**
00050  * Encodes a TLV into a pbuf stream.
00051  *
00052  * @param pbuf_stream points to a pbuf stream
00053  * @param tlv TLV to encode
00054  * @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
00055  */
00056 err_t
00057 snmp_ans1_enc_tlv(struct snmp_pbuf_stream* pbuf_stream, struct snmp_asn1_tlv* tlv)
00058 {
00059   u8_t data;
00060   u8_t length_bytes_required;
00061 
00062   /* write type */
00063   if ((tlv->type & SNMP_ASN1_DATATYPE_MASK) == SNMP_ASN1_DATATYPE_EXTENDED) {
00064     /* extended format is not used by SNMP so we do not accept those values */
00065     return ERR_ARG;
00066   }
00067   if (tlv->type_len != 0) {
00068     /* any other value as auto is not accepted for type (we always use one byte because extended syntax is prohibited) */
00069     return ERR_ARG;
00070   }
00071 
00072   PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, tlv->type));
00073   tlv->type_len = 1;
00074 
00075   /* write length */
00076   if (tlv->value_len <= 127) {
00077     length_bytes_required = 1;
00078   } else if (tlv->value_len <= 255) {
00079     length_bytes_required = 2;
00080   } else  {
00081     length_bytes_required = 3;
00082   }
00083 
00084   /* check for forced min length */
00085   if (tlv->length_len > 0) {
00086     if (tlv->length_len < length_bytes_required) {
00087       /* unable to code requested length in requested number of bytes */
00088       return ERR_ARG;
00089     }
00090 
00091     length_bytes_required = tlv->length_len;
00092   } else {
00093     tlv->length_len = length_bytes_required;
00094   }
00095 
00096   if (length_bytes_required > 1) {
00097     /* multi byte representation required */
00098     length_bytes_required--;
00099     data = 0x80 | length_bytes_required; /* extended length definition, 1 length byte follows */
00100 
00101     PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, data));
00102 
00103     while (length_bytes_required > 1) {
00104       if (length_bytes_required == 2) {
00105         /* append high byte */
00106         data = (u8_t)(tlv->value_len >> 8);
00107       } else {
00108         /* append leading 0x00 */
00109         data = 0x00;
00110       }
00111 
00112       PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, data));
00113       length_bytes_required--;
00114     }
00115   }
00116 
00117   /* append low byte */
00118   data = (u8_t)(tlv->value_len & 0xFF);
00119   PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, data));
00120 
00121   return ERR_OK;
00122 }
00123 
00124 /**
00125  * Encodes raw data (octet string, opaque) into a pbuf chained ASN1 msg.
00126  *
00127  * @param pbuf_stream points to a pbuf stream
00128  * @param raw_len raw data length
00129  * @param raw points raw data
00130  * @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
00131  */
00132 err_t
00133 snmp_asn1_enc_raw(struct snmp_pbuf_stream* pbuf_stream, const u8_t *raw, u16_t raw_len)
00134 {
00135   PBUF_OP_EXEC(snmp_pbuf_stream_writebuf(pbuf_stream, raw, raw_len));
00136 
00137   return ERR_OK;
00138 }
00139 
00140 /**
00141  * Encodes u32_t (counter, gauge, timeticks) into a pbuf chained ASN1 msg.
00142  *
00143  * @param pbuf_stream points to a pbuf stream
00144  * @param octets_needed encoding length (from snmp_asn1_enc_u32t_cnt())
00145  * @param value is the host order u32_t value to be encoded
00146  * @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
00147  *
00148  * @see snmp_asn1_enc_u32t_cnt()
00149  */
00150 err_t
00151 snmp_asn1_enc_u32t(struct snmp_pbuf_stream* pbuf_stream, u16_t octets_needed, u32_t value)
00152 {
00153   if (octets_needed > 5) {
00154     return ERR_ARG;
00155   }
00156   if (octets_needed == 5) {
00157     /* not enough bits in 'value' add leading 0x00 */
00158     PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, 0x00));
00159     octets_needed--;
00160   }
00161 
00162   while (octets_needed > 1) {
00163     octets_needed--;
00164     PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)(value >> (octets_needed << 3))));
00165   }
00166 
00167   /* (only) one least significant octet */
00168   PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)value));
00169 
00170   return ERR_OK;
00171 }
00172 
00173 /**
00174  * Encodes u64_t (counter64) into a pbuf chained ASN1 msg.
00175  *
00176  * @param pbuf_stream points to a pbuf stream
00177  * @param octets_needed encoding length (from snmp_asn1_enc_u32t_cnt())
00178  * @param value is the host order u32_t value to be encoded
00179  * @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
00180  *
00181  * @see snmp_asn1_enc_u64t_cnt()
00182  */
00183 err_t
00184 snmp_asn1_enc_u64t(struct snmp_pbuf_stream* pbuf_stream, u16_t octets_needed, const u32_t* value)
00185 {
00186   if (octets_needed > 9) {
00187     return ERR_ARG;
00188   }
00189   if (octets_needed == 9) {
00190     /* not enough bits in 'value' add leading 0x00 */
00191     PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, 0x00));
00192     octets_needed--;
00193   }
00194 
00195   while (octets_needed > 4) {
00196     octets_needed--;
00197     PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)(*value >> ((octets_needed-4) << 3))));
00198   }
00199   
00200   /* skip to low u32 */
00201   value++;
00202   
00203   while (octets_needed > 1) {
00204     octets_needed--;
00205     PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)(*value >> (octets_needed << 3))));
00206   }
00207 
00208   /* always write at least one octet (also in case of value == 0) */
00209   PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)(*value)));
00210 
00211   return ERR_OK;
00212 }
00213 
00214 /**
00215  * Encodes s32_t integer into a pbuf chained ASN1 msg.
00216  *
00217  * @param pbuf_stream points to a pbuf stream
00218  * @param octets_needed encoding length (from snmp_asn1_enc_s32t_cnt())
00219  * @param value is the host order s32_t value to be encoded
00220  * @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
00221  *
00222  * @see snmp_asn1_enc_s32t_cnt()
00223  */
00224 err_t
00225 snmp_asn1_enc_s32t(struct snmp_pbuf_stream* pbuf_stream, u16_t octets_needed, s32_t value)
00226 {
00227   while (octets_needed > 1) {
00228     octets_needed--;
00229 
00230     PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)(value >> (octets_needed << 3))));
00231   }
00232 
00233   /* (only) one least significant octet */
00234   PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)value));
00235 
00236   return ERR_OK;
00237 }
00238 
00239 /**
00240  * Encodes object identifier into a pbuf chained ASN1 msg.
00241  *
00242  * @param pbuf_stream points to a pbuf stream
00243  * @param oid points to object identifier array
00244  * @param oid_len object identifier array length
00245  * @return ERR_OK if successful, ERR_ARG if we can't (or won't) encode
00246  */
00247 err_t
00248 snmp_asn1_enc_oid(struct snmp_pbuf_stream* pbuf_stream, const u32_t *oid, u16_t oid_len)
00249 {
00250   if (oid_len > 1) {
00251     /* write compressed first two sub id's */
00252     u32_t compressed_byte = ((oid[0] * 40) + oid[1]);
00253     PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)compressed_byte));
00254     oid_len -= 2;
00255     oid += 2;
00256   } else {
00257     /* @bug:  allow empty varbinds for symmetry (we must decode them for getnext), allow partial compression?? */
00258     /* ident_len <= 1, at least we need zeroDotZero (0.0) (ident_len == 2) */
00259     return ERR_ARG;
00260   }
00261 
00262   while (oid_len > 0) {
00263     u32_t sub_id;
00264     u8_t shift, tail;
00265 
00266     oid_len--;
00267     sub_id = *oid;
00268     tail = 0;
00269     shift = 28;
00270     while (shift > 0) {
00271       u8_t code;
00272 
00273       code = (u8_t)(sub_id >> shift);
00274       if ((code != 0) || (tail != 0)) {
00275         tail = 1;
00276         PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, code | 0x80));
00277       }
00278       shift -= 7;
00279     }
00280     PBUF_OP_EXEC(snmp_pbuf_stream_write(pbuf_stream, (u8_t)sub_id & 0x7F));
00281 
00282     /* proceed to next sub-identifier */
00283     oid++;
00284   }
00285   return ERR_OK;
00286 }
00287 
00288 /**
00289  * Returns octet count for length.
00290  *
00291  * @param length
00292  * @param octets_needed points to the return value
00293  */
00294 void
00295 snmp_asn1_enc_length_cnt(u16_t length, u8_t *octets_needed)
00296 {
00297   if (length < 0x80U) {
00298     *octets_needed = 1;
00299   } else if (length < 0x100U) {
00300     *octets_needed = 2;
00301   } else {
00302     *octets_needed = 3;
00303   }
00304 }
00305 
00306 /**
00307  * Returns octet count for an u32_t.
00308  *
00309  * @param value
00310  * @param octets_needed points to the return value
00311  *
00312  * @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
00313  * as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
00314  * of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!!
00315  */
00316 void
00317 snmp_asn1_enc_u32t_cnt(u32_t value, u16_t *octets_needed)
00318 {
00319   if (value < 0x80UL) {
00320     *octets_needed = 1;
00321   } else if (value < 0x8000UL) {
00322     *octets_needed = 2;
00323   } else if (value < 0x800000UL) {
00324     *octets_needed = 3;
00325   } else if (value < 0x80000000UL) {
00326     *octets_needed = 4;
00327   } else {
00328     *octets_needed = 5;
00329   }
00330 }
00331 
00332 /**
00333  * Returns octet count for an u64_t.
00334  *
00335  * @param value
00336  * @param octets_needed points to the return value
00337  *
00338  * @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
00339  * as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
00340  * of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!!
00341  */
00342 void
00343 snmp_asn1_enc_u64t_cnt(const u32_t *value, u16_t *octets_needed)
00344 {
00345   /* check if high u32 is 0 */
00346   if (*value == 0x00) {
00347     /* only low u32 is important */
00348     value++;
00349     snmp_asn1_enc_u32t_cnt(*value, octets_needed);
00350   } else {
00351     /* low u32 does not matter for length determination */
00352     snmp_asn1_enc_u32t_cnt(*value, octets_needed);
00353     *octets_needed = *octets_needed + 4; /* add the 4 bytes of low u32 */
00354   }
00355 }
00356 
00357 /**
00358  * Returns octet count for an s32_t.
00359  *
00360  * @param value
00361  * @param octets_needed points to the return value
00362  *
00363  * @note ASN coded integers are _always_ signed.
00364  */
00365 void
00366 snmp_asn1_enc_s32t_cnt(s32_t value, u16_t *octets_needed)
00367 {
00368   if (value < 0) {
00369     value = ~value;
00370   } 
00371   if (value < 0x80L) {
00372     *octets_needed = 1;
00373   } else if (value < 0x8000L) {
00374     *octets_needed = 2;
00375   } else if (value < 0x800000L) {
00376     *octets_needed = 3;
00377   } else {
00378     *octets_needed = 4;
00379   }
00380 }
00381 
00382 /**
00383  * Returns octet count for an object identifier.
00384  *
00385  * @param oid points to object identifier array
00386  * @param oid_len object identifier array length
00387  * @param octets_needed points to the return value
00388  */
00389 void
00390 snmp_asn1_enc_oid_cnt(const u32_t *oid, u16_t oid_len, u16_t *octets_needed)
00391 {
00392   u32_t sub_id;
00393 
00394   *octets_needed = 0;
00395   if (oid_len > 1) {
00396     /* compressed prefix in one octet */
00397     (*octets_needed)++;
00398     oid_len -= 2;
00399     oid += 2;
00400   }
00401   while (oid_len > 0) {
00402     oid_len--;
00403     sub_id = *oid;
00404 
00405     sub_id >>= 7;
00406     (*octets_needed)++;
00407     while (sub_id > 0) {
00408       sub_id >>= 7;
00409       (*octets_needed)++;
00410     }
00411     oid++;
00412   }
00413 }
00414 
00415 /**
00416  * Decodes a TLV from a pbuf stream.
00417  *
00418  * @param pbuf_stream points to a pbuf stream
00419  * @param tlv returns decoded TLV
00420  * @return ERR_OK if successful, ERR_VAL if we can't decode
00421  */
00422 err_t
00423 snmp_asn1_dec_tlv(struct snmp_pbuf_stream* pbuf_stream, struct snmp_asn1_tlv* tlv)
00424 {
00425   u8_t data;
00426 
00427   /* decode type first */
00428   PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00429   tlv->type = data;
00430 
00431   if ((tlv->type & SNMP_ASN1_DATATYPE_MASK) == SNMP_ASN1_DATATYPE_EXTENDED) {
00432     /* extended format is not used by SNMP so we do not accept those values */
00433     return ERR_VAL;
00434   }
00435   tlv->type_len = 1;
00436 
00437   /* now, decode length */
00438   PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00439 
00440   if (data < 0x80) { /* short form */
00441     tlv->length_len = 1;
00442     tlv->value_len  = data;
00443   } else if (data > 0x80) { /* long form */
00444     u8_t length_bytes = data - 0x80;
00445     tlv->length_len = length_bytes + 1; /* this byte + defined number of length bytes following */
00446     tlv->value_len = 0;
00447 
00448     while (length_bytes > 0) {
00449       /* we only support up to u16.maxvalue-1 (2 bytes) but have to accept leading zero bytes */
00450       if (tlv->value_len > 0xFF) {
00451         return ERR_VAL;
00452       }
00453       PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00454       tlv->value_len <<= 8;
00455       tlv->value_len |= data;
00456       
00457       /* take care for special value used for indefinite length */
00458       if (tlv->value_len == 0xFFFF) {
00459         return ERR_VAL;
00460       }
00461 
00462       length_bytes--;
00463     }
00464   } else { /* data == 0x80 indefinite length form */
00465     /* (not allowed for SNMP; RFC 1157, 3.2.2) */
00466     return ERR_VAL;
00467   }
00468 
00469   return ERR_OK;
00470 }
00471 
00472 /**
00473  * Decodes positive integer (counter, gauge, timeticks) into u32_t.
00474  *
00475  * @param pbuf_stream points to a pbuf stream
00476  * @param len length of the coded integer field
00477  * @param value return host order integer
00478  * @return ERR_OK if successful, ERR_ARG if we can't (or won't) decode
00479  *
00480  * @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
00481  * as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
00482  * of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!!
00483  */
00484 err_t
00485 snmp_asn1_dec_u32t(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u32_t *value)
00486 {
00487   u8_t data;
00488 
00489   if ((len > 0) && (len <= 5)) {
00490     PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00491 
00492     /* expecting sign bit to be zero, only unsigned please! */
00493     if (((len == 5) && (data == 0x00)) || ((len < 5) && ((data & 0x80) == 0))) {
00494       *value = data;
00495       len--;
00496 
00497       while (len > 0) {
00498         PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00499         len--;
00500 
00501         *value <<= 8;
00502         *value |= data;
00503       }
00504 
00505       return ERR_OK;
00506     }
00507   }
00508 
00509   return ERR_VAL;
00510 }
00511 
00512 /**
00513  * Decodes large positive integer (counter64) into 2x u32_t.
00514  *
00515  * @param pbuf_stream points to a pbuf stream
00516  * @param len length of the coded integer field
00517  * @param value return host order integer
00518  * @return ERR_OK if successful, ERR_ARG if we can't (or won't) decode
00519  *
00520  * @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
00521  * as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
00522  * of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!!
00523  */
00524 err_t
00525 snmp_asn1_dec_u64t(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u32_t *value)
00526 {
00527   u8_t data;
00528 
00529   if (len <= 4) {
00530     /* high u32 is 0 */
00531     *value = 0;
00532     /* directly skip to low u32 */
00533     value++;
00534   }
00535 
00536   if ((len > 0) && (len <= 9)) {
00537     PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00538 
00539     /* expecting sign bit to be zero, only unsigned please! */
00540     if (((len == 9) && (data == 0x00)) || ((len < 9) && ((data & 0x80) == 0))) {
00541       *value = data;
00542       len--;
00543 
00544       while (len > 0) {
00545         PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00546 
00547         if (len == 4) {
00548           /* skip to low u32 */
00549           value++;
00550           *value = 0;
00551         } else {
00552           *value <<= 8;
00553         }
00554         
00555         *value |= data;
00556         len--;
00557       }
00558 
00559       return ERR_OK;
00560     }
00561   }
00562 
00563   return ERR_VAL;
00564 }
00565 
00566 /**
00567  * Decodes integer into s32_t.
00568  *
00569  * @param pbuf_stream points to a pbuf stream
00570  * @param len length of the coded integer field
00571  * @param value return host order integer
00572  * @return ERR_OK if successful, ERR_ARG if we can't (or won't) decode
00573  *
00574  * @note ASN coded integers are _always_ signed!
00575  */
00576 err_t
00577 snmp_asn1_dec_s32t(struct snmp_pbuf_stream *pbuf_stream, u16_t len, s32_t *value)
00578 {
00579 #if BYTE_ORDER == LITTLE_ENDIAN
00580   u8_t *lsb_ptr = (u8_t*)value;
00581 #endif
00582 #if BYTE_ORDER == BIG_ENDIAN
00583   u8_t *lsb_ptr = (u8_t*)value + sizeof(s32_t) - 1;
00584 #endif
00585   u8_t sign;
00586   u8_t data;
00587 
00588   if ((len > 0) && (len < 5)) {
00589     PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00590     len--;
00591     
00592     if (data & 0x80) {
00593       /* negative, start from -1 */
00594       *value = -1;
00595       sign = 1;
00596       *lsb_ptr &= data;
00597     } else {
00598       /* positive, start from 0 */
00599       *value = 0;
00600       sign = 0;
00601       *lsb_ptr |= data;
00602     }
00603 
00604     /* OR/AND octets with value */
00605     while (len > 0) {
00606       PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00607       len--;
00608 
00609 #if BYTE_ORDER == LITTLE_ENDIAN
00610       *value <<= 8;
00611 #endif
00612 #if BYTE_ORDER == BIG_ENDIAN
00613       *value >>= 8;
00614 #endif
00615 
00616       if (sign) {
00617         *lsb_ptr |= 255;
00618         *lsb_ptr &= data;
00619       } else {
00620         *lsb_ptr |= data;
00621       }
00622     }
00623 
00624     return ERR_OK;
00625   }
00626 
00627   return ERR_VAL;
00628 }
00629 
00630 /**
00631  * Decodes object identifier from incoming message into array of u32_t.
00632  *
00633  * @param pbuf_stream points to a pbuf stream
00634  * @param len length of the coded object identifier
00635  * @param oid return decoded object identifier
00636  * @param oid_len return decoded object identifier length
00637  * @param oid_max_len size of oid buffer
00638  * @return ERR_OK if successful, ERR_ARG if we can't (or won't) decode
00639  */
00640 err_t
00641 snmp_asn1_dec_oid(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u32_t* oid, u8_t* oid_len, u8_t oid_max_len)
00642 {
00643   u32_t *oid_ptr;
00644   u8_t data;
00645 
00646   *oid_len = 0;
00647   oid_ptr = oid;
00648   if (len > 0) {
00649     if (oid_max_len < 2) {
00650       return ERR_MEM;
00651     }
00652     
00653     PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00654     len--;
00655 
00656     /* first compressed octet */
00657     if (data == 0x2B) {
00658       /* (most) common case 1.3 (iso.org) */
00659       *oid_ptr = 1;
00660       oid_ptr++;
00661       *oid_ptr = 3;
00662       oid_ptr++;
00663     } else if (data < 40) {
00664       *oid_ptr = 0;
00665       oid_ptr++;
00666       *oid_ptr = data;
00667       oid_ptr++;
00668     } else if (data < 80) {
00669       *oid_ptr = 1;
00670       oid_ptr++;
00671       *oid_ptr = data - 40;
00672       oid_ptr++;
00673     } else {
00674       *oid_ptr = 2;
00675       oid_ptr++;
00676       *oid_ptr = data - 80;
00677       oid_ptr++;
00678     }
00679     *oid_len = 2;
00680   } else {
00681     /* accepting zero length identifiers e.g. for getnext operation. uncommon but valid */
00682     return ERR_OK;
00683   }
00684 
00685   while ((len > 0) && (*oid_len < oid_max_len)) {
00686     PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00687     len--;
00688 
00689     if ((data & 0x80) == 0x00) {
00690       /* sub-identifier uses single octet */
00691       *oid_ptr = data;
00692     } else {
00693       /* sub-identifier uses multiple octets */
00694       u32_t sub_id = (data & ~0x80);
00695       while ((len > 0) && ((data & 0x80) != 0)) {
00696         PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, &data));
00697         len--;
00698 
00699         sub_id = (sub_id << 7) + (data & ~0x80);
00700       }
00701 
00702       if ((data & 0x80) != 0) {
00703         /* "more bytes following" bit still set at end of len */
00704         return ERR_VAL;
00705       }
00706       *oid_ptr = sub_id;
00707     }
00708     oid_ptr++;
00709     (*oid_len)++;
00710   }
00711 
00712   if (len > 0) {
00713     /* OID to long to fit in our buffer */
00714     return ERR_MEM;
00715   }
00716 
00717   return ERR_OK;
00718 }
00719 
00720 /**
00721  * Decodes (copies) raw data (ip-addresses, octet strings, opaque encoding)
00722  * from incoming message into array.
00723  *
00724  * @param pbuf_stream points to a pbuf stream
00725  * @param len length of the coded raw data (zero is valid, e.g. empty string!)
00726  * @param buf return raw bytes
00727  * @param buf_len returns length of the raw return value
00728  * @param buf_max_len buffer size
00729  * @return ERR_OK if successful, ERR_ARG if we can't (or won't) decode
00730  */
00731 err_t
00732 snmp_asn1_dec_raw(struct snmp_pbuf_stream *pbuf_stream, u16_t len, u8_t *buf, u16_t* buf_len, u16_t buf_max_len)
00733 {
00734   if (len > buf_max_len) {
00735     /* not enough dst space */
00736     return ERR_MEM;
00737   }
00738   *buf_len = len;
00739   
00740   while (len > 0) {
00741     PBUF_OP_EXEC(snmp_pbuf_stream_read(pbuf_stream, buf));
00742     buf++;
00743     len--;
00744   }
00745 
00746   return ERR_OK;
00747 }
00748 
00749 #endif /* LWIP_SNMP */