NetServices Stack source

Dependents:   HelloWorld ServoInterfaceBoardExample1 4180_Lab4

Committer:
donatien
Date:
Fri Jun 11 16:05:15 2010 +0000
Revision:
0:632c9925f013

        

Who changed what in which revision?

UserRevisionLine numberNew contents of line
donatien 0:632c9925f013 1 /**
donatien 0:632c9925f013 2 * @file
donatien 0:632c9925f013 3 * Abstract Syntax Notation One (ISO 8824, 8825) decoding
donatien 0:632c9925f013 4 *
donatien 0:632c9925f013 5 * @todo not optimised (yet), favor correctness over speed, favor speed over size
donatien 0:632c9925f013 6 */
donatien 0:632c9925f013 7
donatien 0:632c9925f013 8 /*
donatien 0:632c9925f013 9 * Copyright (c) 2006 Axon Digital Design B.V., The Netherlands.
donatien 0:632c9925f013 10 * All rights reserved.
donatien 0:632c9925f013 11 *
donatien 0:632c9925f013 12 * Redistribution and use in source and binary forms, with or without modification,
donatien 0:632c9925f013 13 * are permitted provided that the following conditions are met:
donatien 0:632c9925f013 14 *
donatien 0:632c9925f013 15 * 1. Redistributions of source code must retain the above copyright notice,
donatien 0:632c9925f013 16 * this list of conditions and the following disclaimer.
donatien 0:632c9925f013 17 * 2. Redistributions in binary form must reproduce the above copyright notice,
donatien 0:632c9925f013 18 * this list of conditions and the following disclaimer in the documentation
donatien 0:632c9925f013 19 * and/or other materials provided with the distribution.
donatien 0:632c9925f013 20 * 3. The name of the author may not be used to endorse or promote products
donatien 0:632c9925f013 21 * derived from this software without specific prior written permission.
donatien 0:632c9925f013 22 *
donatien 0:632c9925f013 23 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
donatien 0:632c9925f013 24 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
donatien 0:632c9925f013 25 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
donatien 0:632c9925f013 26 * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
donatien 0:632c9925f013 27 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT
donatien 0:632c9925f013 28 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
donatien 0:632c9925f013 29 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
donatien 0:632c9925f013 30 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
donatien 0:632c9925f013 31 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
donatien 0:632c9925f013 32 * OF SUCH DAMAGE.
donatien 0:632c9925f013 33 *
donatien 0:632c9925f013 34 * Author: Christiaan Simons <christiaan.simons@axon.tv>
donatien 0:632c9925f013 35 */
donatien 0:632c9925f013 36
donatien 0:632c9925f013 37 #include "lwip/opt.h"
donatien 0:632c9925f013 38
donatien 0:632c9925f013 39 #if LWIP_SNMP /* don't build if not configured for use in lwipopts.h */
donatien 0:632c9925f013 40
donatien 0:632c9925f013 41 #include "lwip/snmp_asn1.h"
donatien 0:632c9925f013 42
donatien 0:632c9925f013 43 /**
donatien 0:632c9925f013 44 * Retrieves type field from incoming pbuf chain.
donatien 0:632c9925f013 45 *
donatien 0:632c9925f013 46 * @param p points to a pbuf holding an ASN1 coded type field
donatien 0:632c9925f013 47 * @param ofs points to the offset within the pbuf chain of the ASN1 coded type field
donatien 0:632c9925f013 48 * @param type return ASN1 type
donatien 0:632c9925f013 49 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
donatien 0:632c9925f013 50 */
donatien 0:632c9925f013 51 err_t
donatien 0:632c9925f013 52 snmp_asn1_dec_type(struct pbuf *p, u16_t ofs, u8_t *type)
donatien 0:632c9925f013 53 {
donatien 0:632c9925f013 54 u16_t plen, base;
donatien 0:632c9925f013 55 u8_t *msg_ptr;
donatien 0:632c9925f013 56
donatien 0:632c9925f013 57 plen = 0;
donatien 0:632c9925f013 58 while (p != NULL)
donatien 0:632c9925f013 59 {
donatien 0:632c9925f013 60 base = plen;
donatien 0:632c9925f013 61 plen += p->len;
donatien 0:632c9925f013 62 if (ofs < plen)
donatien 0:632c9925f013 63 {
donatien 0:632c9925f013 64 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 65 msg_ptr += ofs - base;
donatien 0:632c9925f013 66 *type = *msg_ptr;
donatien 0:632c9925f013 67 return ERR_OK;
donatien 0:632c9925f013 68 }
donatien 0:632c9925f013 69 p = p->next;
donatien 0:632c9925f013 70 }
donatien 0:632c9925f013 71 /* p == NULL, ofs >= plen */
donatien 0:632c9925f013 72 return ERR_ARG;
donatien 0:632c9925f013 73 }
donatien 0:632c9925f013 74
donatien 0:632c9925f013 75 /**
donatien 0:632c9925f013 76 * Decodes length field from incoming pbuf chain into host length.
donatien 0:632c9925f013 77 *
donatien 0:632c9925f013 78 * @param p points to a pbuf holding an ASN1 coded length
donatien 0:632c9925f013 79 * @param ofs points to the offset within the pbuf chain of the ASN1 coded length
donatien 0:632c9925f013 80 * @param octets_used returns number of octets used by the length code
donatien 0:632c9925f013 81 * @param length return host order length, upto 64k
donatien 0:632c9925f013 82 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
donatien 0:632c9925f013 83 */
donatien 0:632c9925f013 84 err_t
donatien 0:632c9925f013 85 snmp_asn1_dec_length(struct pbuf *p, u16_t ofs, u8_t *octets_used, u16_t *length)
donatien 0:632c9925f013 86 {
donatien 0:632c9925f013 87 u16_t plen, base;
donatien 0:632c9925f013 88 u8_t *msg_ptr;
donatien 0:632c9925f013 89
donatien 0:632c9925f013 90 plen = 0;
donatien 0:632c9925f013 91 while (p != NULL)
donatien 0:632c9925f013 92 {
donatien 0:632c9925f013 93 base = plen;
donatien 0:632c9925f013 94 plen += p->len;
donatien 0:632c9925f013 95 if (ofs < plen)
donatien 0:632c9925f013 96 {
donatien 0:632c9925f013 97 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 98 msg_ptr += ofs - base;
donatien 0:632c9925f013 99
donatien 0:632c9925f013 100 if (*msg_ptr < 0x80)
donatien 0:632c9925f013 101 {
donatien 0:632c9925f013 102 /* primitive definite length format */
donatien 0:632c9925f013 103 *octets_used = 1;
donatien 0:632c9925f013 104 *length = *msg_ptr;
donatien 0:632c9925f013 105 return ERR_OK;
donatien 0:632c9925f013 106 }
donatien 0:632c9925f013 107 else if (*msg_ptr == 0x80)
donatien 0:632c9925f013 108 {
donatien 0:632c9925f013 109 /* constructed indefinite length format, termination with two zero octets */
donatien 0:632c9925f013 110 u8_t zeros;
donatien 0:632c9925f013 111 u8_t i;
donatien 0:632c9925f013 112
donatien 0:632c9925f013 113 *length = 0;
donatien 0:632c9925f013 114 zeros = 0;
donatien 0:632c9925f013 115 while (zeros != 2)
donatien 0:632c9925f013 116 {
donatien 0:632c9925f013 117 i = 2;
donatien 0:632c9925f013 118 while (i > 0)
donatien 0:632c9925f013 119 {
donatien 0:632c9925f013 120 i--;
donatien 0:632c9925f013 121 (*length) += 1;
donatien 0:632c9925f013 122 ofs += 1;
donatien 0:632c9925f013 123 if (ofs >= plen)
donatien 0:632c9925f013 124 {
donatien 0:632c9925f013 125 /* next octet in next pbuf */
donatien 0:632c9925f013 126 p = p->next;
donatien 0:632c9925f013 127 if (p == NULL) { return ERR_ARG; }
donatien 0:632c9925f013 128 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 129 plen += p->len;
donatien 0:632c9925f013 130 }
donatien 0:632c9925f013 131 else
donatien 0:632c9925f013 132 {
donatien 0:632c9925f013 133 /* next octet in same pbuf */
donatien 0:632c9925f013 134 msg_ptr++;
donatien 0:632c9925f013 135 }
donatien 0:632c9925f013 136 if (*msg_ptr == 0)
donatien 0:632c9925f013 137 {
donatien 0:632c9925f013 138 zeros++;
donatien 0:632c9925f013 139 if (zeros == 2)
donatien 0:632c9925f013 140 {
donatien 0:632c9925f013 141 /* stop while (i > 0) */
donatien 0:632c9925f013 142 i = 0;
donatien 0:632c9925f013 143 }
donatien 0:632c9925f013 144 }
donatien 0:632c9925f013 145 else
donatien 0:632c9925f013 146 {
donatien 0:632c9925f013 147 zeros = 0;
donatien 0:632c9925f013 148 }
donatien 0:632c9925f013 149 }
donatien 0:632c9925f013 150 }
donatien 0:632c9925f013 151 *octets_used = 1;
donatien 0:632c9925f013 152 return ERR_OK;
donatien 0:632c9925f013 153 }
donatien 0:632c9925f013 154 else if (*msg_ptr == 0x81)
donatien 0:632c9925f013 155 {
donatien 0:632c9925f013 156 /* constructed definite length format, one octet */
donatien 0:632c9925f013 157 ofs += 1;
donatien 0:632c9925f013 158 if (ofs >= plen)
donatien 0:632c9925f013 159 {
donatien 0:632c9925f013 160 /* next octet in next pbuf */
donatien 0:632c9925f013 161 p = p->next;
donatien 0:632c9925f013 162 if (p == NULL) { return ERR_ARG; }
donatien 0:632c9925f013 163 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 164 }
donatien 0:632c9925f013 165 else
donatien 0:632c9925f013 166 {
donatien 0:632c9925f013 167 /* next octet in same pbuf */
donatien 0:632c9925f013 168 msg_ptr++;
donatien 0:632c9925f013 169 }
donatien 0:632c9925f013 170 *length = *msg_ptr;
donatien 0:632c9925f013 171 *octets_used = 2;
donatien 0:632c9925f013 172 return ERR_OK;
donatien 0:632c9925f013 173 }
donatien 0:632c9925f013 174 else if (*msg_ptr == 0x82)
donatien 0:632c9925f013 175 {
donatien 0:632c9925f013 176 u8_t i;
donatien 0:632c9925f013 177
donatien 0:632c9925f013 178 /* constructed definite length format, two octets */
donatien 0:632c9925f013 179 i = 2;
donatien 0:632c9925f013 180 while (i > 0)
donatien 0:632c9925f013 181 {
donatien 0:632c9925f013 182 i--;
donatien 0:632c9925f013 183 ofs += 1;
donatien 0:632c9925f013 184 if (ofs >= plen)
donatien 0:632c9925f013 185 {
donatien 0:632c9925f013 186 /* next octet in next pbuf */
donatien 0:632c9925f013 187 p = p->next;
donatien 0:632c9925f013 188 if (p == NULL) { return ERR_ARG; }
donatien 0:632c9925f013 189 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 190 plen += p->len;
donatien 0:632c9925f013 191 }
donatien 0:632c9925f013 192 else
donatien 0:632c9925f013 193 {
donatien 0:632c9925f013 194 /* next octet in same pbuf */
donatien 0:632c9925f013 195 msg_ptr++;
donatien 0:632c9925f013 196 }
donatien 0:632c9925f013 197 if (i == 0)
donatien 0:632c9925f013 198 {
donatien 0:632c9925f013 199 /* least significant length octet */
donatien 0:632c9925f013 200 *length |= *msg_ptr;
donatien 0:632c9925f013 201 }
donatien 0:632c9925f013 202 else
donatien 0:632c9925f013 203 {
donatien 0:632c9925f013 204 /* most significant length octet */
donatien 0:632c9925f013 205 *length = (*msg_ptr) << 8;
donatien 0:632c9925f013 206 }
donatien 0:632c9925f013 207 }
donatien 0:632c9925f013 208 *octets_used = 3;
donatien 0:632c9925f013 209 return ERR_OK;
donatien 0:632c9925f013 210 }
donatien 0:632c9925f013 211 else
donatien 0:632c9925f013 212 {
donatien 0:632c9925f013 213 /* constructed definite length format 3..127 octets, this is too big (>64k) */
donatien 0:632c9925f013 214 /** @todo: do we need to accept inefficient codings with many leading zero's? */
donatien 0:632c9925f013 215 *octets_used = 1 + ((*msg_ptr) & 0x7f);
donatien 0:632c9925f013 216 return ERR_ARG;
donatien 0:632c9925f013 217 }
donatien 0:632c9925f013 218 }
donatien 0:632c9925f013 219 p = p->next;
donatien 0:632c9925f013 220 }
donatien 0:632c9925f013 221
donatien 0:632c9925f013 222 /* p == NULL, ofs >= plen */
donatien 0:632c9925f013 223 return ERR_ARG;
donatien 0:632c9925f013 224 }
donatien 0:632c9925f013 225
donatien 0:632c9925f013 226 /**
donatien 0:632c9925f013 227 * Decodes positive integer (counter, gauge, timeticks) into u32_t.
donatien 0:632c9925f013 228 *
donatien 0:632c9925f013 229 * @param p points to a pbuf holding an ASN1 coded integer
donatien 0:632c9925f013 230 * @param ofs points to the offset within the pbuf chain of the ASN1 coded integer
donatien 0:632c9925f013 231 * @param len length of the coded integer field
donatien 0:632c9925f013 232 * @param value return host order integer
donatien 0:632c9925f013 233 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
donatien 0:632c9925f013 234 *
donatien 0:632c9925f013 235 * @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded
donatien 0:632c9925f013 236 * as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value
donatien 0:632c9925f013 237 * of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!!
donatien 0:632c9925f013 238 */
donatien 0:632c9925f013 239 err_t
donatien 0:632c9925f013 240 snmp_asn1_dec_u32t(struct pbuf *p, u16_t ofs, u16_t len, u32_t *value)
donatien 0:632c9925f013 241 {
donatien 0:632c9925f013 242 u16_t plen, base;
donatien 0:632c9925f013 243 u8_t *msg_ptr;
donatien 0:632c9925f013 244
donatien 0:632c9925f013 245 plen = 0;
donatien 0:632c9925f013 246 while (p != NULL)
donatien 0:632c9925f013 247 {
donatien 0:632c9925f013 248 base = plen;
donatien 0:632c9925f013 249 plen += p->len;
donatien 0:632c9925f013 250 if (ofs < plen)
donatien 0:632c9925f013 251 {
donatien 0:632c9925f013 252 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 253 msg_ptr += ofs - base;
donatien 0:632c9925f013 254 if ((len > 0) && (len < 6))
donatien 0:632c9925f013 255 {
donatien 0:632c9925f013 256 /* start from zero */
donatien 0:632c9925f013 257 *value = 0;
donatien 0:632c9925f013 258 if (*msg_ptr & 0x80)
donatien 0:632c9925f013 259 {
donatien 0:632c9925f013 260 /* negative, expecting zero sign bit! */
donatien 0:632c9925f013 261 return ERR_ARG;
donatien 0:632c9925f013 262 }
donatien 0:632c9925f013 263 else
donatien 0:632c9925f013 264 {
donatien 0:632c9925f013 265 /* positive */
donatien 0:632c9925f013 266 if ((len > 1) && (*msg_ptr == 0))
donatien 0:632c9925f013 267 {
donatien 0:632c9925f013 268 /* skip leading "sign byte" octet 0x00 */
donatien 0:632c9925f013 269 len--;
donatien 0:632c9925f013 270 ofs += 1;
donatien 0:632c9925f013 271 if (ofs >= plen)
donatien 0:632c9925f013 272 {
donatien 0:632c9925f013 273 /* next octet in next pbuf */
donatien 0:632c9925f013 274 p = p->next;
donatien 0:632c9925f013 275 if (p == NULL) { return ERR_ARG; }
donatien 0:632c9925f013 276 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 277 plen += p->len;
donatien 0:632c9925f013 278 }
donatien 0:632c9925f013 279 else
donatien 0:632c9925f013 280 {
donatien 0:632c9925f013 281 /* next octet in same pbuf */
donatien 0:632c9925f013 282 msg_ptr++;
donatien 0:632c9925f013 283 }
donatien 0:632c9925f013 284 }
donatien 0:632c9925f013 285 }
donatien 0:632c9925f013 286 /* OR octets with value */
donatien 0:632c9925f013 287 while (len > 1)
donatien 0:632c9925f013 288 {
donatien 0:632c9925f013 289 len--;
donatien 0:632c9925f013 290 *value |= *msg_ptr;
donatien 0:632c9925f013 291 *value <<= 8;
donatien 0:632c9925f013 292 ofs += 1;
donatien 0:632c9925f013 293 if (ofs >= plen)
donatien 0:632c9925f013 294 {
donatien 0:632c9925f013 295 /* next octet in next pbuf */
donatien 0:632c9925f013 296 p = p->next;
donatien 0:632c9925f013 297 if (p == NULL) { return ERR_ARG; }
donatien 0:632c9925f013 298 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 299 plen += p->len;
donatien 0:632c9925f013 300 }
donatien 0:632c9925f013 301 else
donatien 0:632c9925f013 302 {
donatien 0:632c9925f013 303 /* next octet in same pbuf */
donatien 0:632c9925f013 304 msg_ptr++;
donatien 0:632c9925f013 305 }
donatien 0:632c9925f013 306 }
donatien 0:632c9925f013 307 *value |= *msg_ptr;
donatien 0:632c9925f013 308 return ERR_OK;
donatien 0:632c9925f013 309 }
donatien 0:632c9925f013 310 else
donatien 0:632c9925f013 311 {
donatien 0:632c9925f013 312 return ERR_ARG;
donatien 0:632c9925f013 313 }
donatien 0:632c9925f013 314 }
donatien 0:632c9925f013 315 p = p->next;
donatien 0:632c9925f013 316 }
donatien 0:632c9925f013 317 /* p == NULL, ofs >= plen */
donatien 0:632c9925f013 318 return ERR_ARG;
donatien 0:632c9925f013 319 }
donatien 0:632c9925f013 320
donatien 0:632c9925f013 321 /**
donatien 0:632c9925f013 322 * Decodes integer into s32_t.
donatien 0:632c9925f013 323 *
donatien 0:632c9925f013 324 * @param p points to a pbuf holding an ASN1 coded integer
donatien 0:632c9925f013 325 * @param ofs points to the offset within the pbuf chain of the ASN1 coded integer
donatien 0:632c9925f013 326 * @param len length of the coded integer field
donatien 0:632c9925f013 327 * @param value return host order integer
donatien 0:632c9925f013 328 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
donatien 0:632c9925f013 329 *
donatien 0:632c9925f013 330 * @note ASN coded integers are _always_ signed!
donatien 0:632c9925f013 331 */
donatien 0:632c9925f013 332 err_t
donatien 0:632c9925f013 333 snmp_asn1_dec_s32t(struct pbuf *p, u16_t ofs, u16_t len, s32_t *value)
donatien 0:632c9925f013 334 {
donatien 0:632c9925f013 335 u16_t plen, base;
donatien 0:632c9925f013 336 u8_t *msg_ptr;
donatien 0:632c9925f013 337 #if BYTE_ORDER == LITTLE_ENDIAN
donatien 0:632c9925f013 338 u8_t *lsb_ptr = (u8_t*)value;
donatien 0:632c9925f013 339 #endif
donatien 0:632c9925f013 340 #if BYTE_ORDER == BIG_ENDIAN
donatien 0:632c9925f013 341 u8_t *lsb_ptr = (u8_t*)value + sizeof(s32_t) - 1;
donatien 0:632c9925f013 342 #endif
donatien 0:632c9925f013 343 u8_t sign;
donatien 0:632c9925f013 344
donatien 0:632c9925f013 345 plen = 0;
donatien 0:632c9925f013 346 while (p != NULL)
donatien 0:632c9925f013 347 {
donatien 0:632c9925f013 348 base = plen;
donatien 0:632c9925f013 349 plen += p->len;
donatien 0:632c9925f013 350 if (ofs < plen)
donatien 0:632c9925f013 351 {
donatien 0:632c9925f013 352 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 353 msg_ptr += ofs - base;
donatien 0:632c9925f013 354 if ((len > 0) && (len < 5))
donatien 0:632c9925f013 355 {
donatien 0:632c9925f013 356 if (*msg_ptr & 0x80)
donatien 0:632c9925f013 357 {
donatien 0:632c9925f013 358 /* negative, start from -1 */
donatien 0:632c9925f013 359 *value = -1;
donatien 0:632c9925f013 360 sign = 1;
donatien 0:632c9925f013 361 }
donatien 0:632c9925f013 362 else
donatien 0:632c9925f013 363 {
donatien 0:632c9925f013 364 /* positive, start from 0 */
donatien 0:632c9925f013 365 *value = 0;
donatien 0:632c9925f013 366 sign = 0;
donatien 0:632c9925f013 367 }
donatien 0:632c9925f013 368 /* OR/AND octets with value */
donatien 0:632c9925f013 369 while (len > 1)
donatien 0:632c9925f013 370 {
donatien 0:632c9925f013 371 len--;
donatien 0:632c9925f013 372 if (sign)
donatien 0:632c9925f013 373 {
donatien 0:632c9925f013 374 *lsb_ptr &= *msg_ptr;
donatien 0:632c9925f013 375 *value <<= 8;
donatien 0:632c9925f013 376 *lsb_ptr |= 255;
donatien 0:632c9925f013 377 }
donatien 0:632c9925f013 378 else
donatien 0:632c9925f013 379 {
donatien 0:632c9925f013 380 *lsb_ptr |= *msg_ptr;
donatien 0:632c9925f013 381 *value <<= 8;
donatien 0:632c9925f013 382 }
donatien 0:632c9925f013 383 ofs += 1;
donatien 0:632c9925f013 384 if (ofs >= plen)
donatien 0:632c9925f013 385 {
donatien 0:632c9925f013 386 /* next octet in next pbuf */
donatien 0:632c9925f013 387 p = p->next;
donatien 0:632c9925f013 388 if (p == NULL) { return ERR_ARG; }
donatien 0:632c9925f013 389 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 390 plen += p->len;
donatien 0:632c9925f013 391 }
donatien 0:632c9925f013 392 else
donatien 0:632c9925f013 393 {
donatien 0:632c9925f013 394 /* next octet in same pbuf */
donatien 0:632c9925f013 395 msg_ptr++;
donatien 0:632c9925f013 396 }
donatien 0:632c9925f013 397 }
donatien 0:632c9925f013 398 if (sign)
donatien 0:632c9925f013 399 {
donatien 0:632c9925f013 400 *lsb_ptr &= *msg_ptr;
donatien 0:632c9925f013 401 }
donatien 0:632c9925f013 402 else
donatien 0:632c9925f013 403 {
donatien 0:632c9925f013 404 *lsb_ptr |= *msg_ptr;
donatien 0:632c9925f013 405 }
donatien 0:632c9925f013 406 return ERR_OK;
donatien 0:632c9925f013 407 }
donatien 0:632c9925f013 408 else
donatien 0:632c9925f013 409 {
donatien 0:632c9925f013 410 return ERR_ARG;
donatien 0:632c9925f013 411 }
donatien 0:632c9925f013 412 }
donatien 0:632c9925f013 413 p = p->next;
donatien 0:632c9925f013 414 }
donatien 0:632c9925f013 415 /* p == NULL, ofs >= plen */
donatien 0:632c9925f013 416 return ERR_ARG;
donatien 0:632c9925f013 417 }
donatien 0:632c9925f013 418
donatien 0:632c9925f013 419 /**
donatien 0:632c9925f013 420 * Decodes object identifier from incoming message into array of s32_t.
donatien 0:632c9925f013 421 *
donatien 0:632c9925f013 422 * @param p points to a pbuf holding an ASN1 coded object identifier
donatien 0:632c9925f013 423 * @param ofs points to the offset within the pbuf chain of the ASN1 coded object identifier
donatien 0:632c9925f013 424 * @param len length of the coded object identifier
donatien 0:632c9925f013 425 * @param oid return object identifier struct
donatien 0:632c9925f013 426 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
donatien 0:632c9925f013 427 */
donatien 0:632c9925f013 428 err_t
donatien 0:632c9925f013 429 snmp_asn1_dec_oid(struct pbuf *p, u16_t ofs, u16_t len, struct snmp_obj_id *oid)
donatien 0:632c9925f013 430 {
donatien 0:632c9925f013 431 u16_t plen, base;
donatien 0:632c9925f013 432 u8_t *msg_ptr;
donatien 0:632c9925f013 433 s32_t *oid_ptr;
donatien 0:632c9925f013 434
donatien 0:632c9925f013 435 plen = 0;
donatien 0:632c9925f013 436 while (p != NULL)
donatien 0:632c9925f013 437 {
donatien 0:632c9925f013 438 base = plen;
donatien 0:632c9925f013 439 plen += p->len;
donatien 0:632c9925f013 440 if (ofs < plen)
donatien 0:632c9925f013 441 {
donatien 0:632c9925f013 442 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 443 msg_ptr += ofs - base;
donatien 0:632c9925f013 444
donatien 0:632c9925f013 445 oid->len = 0;
donatien 0:632c9925f013 446 oid_ptr = &oid->id[0];
donatien 0:632c9925f013 447 if (len > 0)
donatien 0:632c9925f013 448 {
donatien 0:632c9925f013 449 /* first compressed octet */
donatien 0:632c9925f013 450 if (*msg_ptr == 0x2B)
donatien 0:632c9925f013 451 {
donatien 0:632c9925f013 452 /* (most) common case 1.3 (iso.org) */
donatien 0:632c9925f013 453 *oid_ptr = 1;
donatien 0:632c9925f013 454 oid_ptr++;
donatien 0:632c9925f013 455 *oid_ptr = 3;
donatien 0:632c9925f013 456 oid_ptr++;
donatien 0:632c9925f013 457 }
donatien 0:632c9925f013 458 else if (*msg_ptr < 40)
donatien 0:632c9925f013 459 {
donatien 0:632c9925f013 460 *oid_ptr = 0;
donatien 0:632c9925f013 461 oid_ptr++;
donatien 0:632c9925f013 462 *oid_ptr = *msg_ptr;
donatien 0:632c9925f013 463 oid_ptr++;
donatien 0:632c9925f013 464 }
donatien 0:632c9925f013 465 else if (*msg_ptr < 80)
donatien 0:632c9925f013 466 {
donatien 0:632c9925f013 467 *oid_ptr = 1;
donatien 0:632c9925f013 468 oid_ptr++;
donatien 0:632c9925f013 469 *oid_ptr = (*msg_ptr) - 40;
donatien 0:632c9925f013 470 oid_ptr++;
donatien 0:632c9925f013 471 }
donatien 0:632c9925f013 472 else
donatien 0:632c9925f013 473 {
donatien 0:632c9925f013 474 *oid_ptr = 2;
donatien 0:632c9925f013 475 oid_ptr++;
donatien 0:632c9925f013 476 *oid_ptr = (*msg_ptr) - 80;
donatien 0:632c9925f013 477 oid_ptr++;
donatien 0:632c9925f013 478 }
donatien 0:632c9925f013 479 oid->len = 2;
donatien 0:632c9925f013 480 }
donatien 0:632c9925f013 481 else
donatien 0:632c9925f013 482 {
donatien 0:632c9925f013 483 /* accepting zero length identifiers e.g. for
donatien 0:632c9925f013 484 getnext operation. uncommon but valid */
donatien 0:632c9925f013 485 return ERR_OK;
donatien 0:632c9925f013 486 }
donatien 0:632c9925f013 487 len--;
donatien 0:632c9925f013 488 if (len > 0)
donatien 0:632c9925f013 489 {
donatien 0:632c9925f013 490 ofs += 1;
donatien 0:632c9925f013 491 if (ofs >= plen)
donatien 0:632c9925f013 492 {
donatien 0:632c9925f013 493 /* next octet in next pbuf */
donatien 0:632c9925f013 494 p = p->next;
donatien 0:632c9925f013 495 if (p == NULL) { return ERR_ARG; }
donatien 0:632c9925f013 496 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 497 plen += p->len;
donatien 0:632c9925f013 498 }
donatien 0:632c9925f013 499 else
donatien 0:632c9925f013 500 {
donatien 0:632c9925f013 501 /* next octet in same pbuf */
donatien 0:632c9925f013 502 msg_ptr++;
donatien 0:632c9925f013 503 }
donatien 0:632c9925f013 504 }
donatien 0:632c9925f013 505 while ((len > 0) && (oid->len < LWIP_SNMP_OBJ_ID_LEN))
donatien 0:632c9925f013 506 {
donatien 0:632c9925f013 507 /* sub-identifier uses multiple octets */
donatien 0:632c9925f013 508 if (*msg_ptr & 0x80)
donatien 0:632c9925f013 509 {
donatien 0:632c9925f013 510 s32_t sub_id = 0;
donatien 0:632c9925f013 511
donatien 0:632c9925f013 512 while ((*msg_ptr & 0x80) && (len > 1))
donatien 0:632c9925f013 513 {
donatien 0:632c9925f013 514 len--;
donatien 0:632c9925f013 515 sub_id = (sub_id << 7) + (*msg_ptr & ~0x80);
donatien 0:632c9925f013 516 ofs += 1;
donatien 0:632c9925f013 517 if (ofs >= plen)
donatien 0:632c9925f013 518 {
donatien 0:632c9925f013 519 /* next octet in next pbuf */
donatien 0:632c9925f013 520 p = p->next;
donatien 0:632c9925f013 521 if (p == NULL) { return ERR_ARG; }
donatien 0:632c9925f013 522 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 523 plen += p->len;
donatien 0:632c9925f013 524 }
donatien 0:632c9925f013 525 else
donatien 0:632c9925f013 526 {
donatien 0:632c9925f013 527 /* next octet in same pbuf */
donatien 0:632c9925f013 528 msg_ptr++;
donatien 0:632c9925f013 529 }
donatien 0:632c9925f013 530 }
donatien 0:632c9925f013 531 if (!(*msg_ptr & 0x80) && (len > 0))
donatien 0:632c9925f013 532 {
donatien 0:632c9925f013 533 /* last octet sub-identifier */
donatien 0:632c9925f013 534 len--;
donatien 0:632c9925f013 535 sub_id = (sub_id << 7) + *msg_ptr;
donatien 0:632c9925f013 536 *oid_ptr = sub_id;
donatien 0:632c9925f013 537 }
donatien 0:632c9925f013 538 }
donatien 0:632c9925f013 539 else
donatien 0:632c9925f013 540 {
donatien 0:632c9925f013 541 /* !(*msg_ptr & 0x80) sub-identifier uses single octet */
donatien 0:632c9925f013 542 len--;
donatien 0:632c9925f013 543 *oid_ptr = *msg_ptr;
donatien 0:632c9925f013 544 }
donatien 0:632c9925f013 545 if (len > 0)
donatien 0:632c9925f013 546 {
donatien 0:632c9925f013 547 /* remaining oid bytes available ... */
donatien 0:632c9925f013 548 ofs += 1;
donatien 0:632c9925f013 549 if (ofs >= plen)
donatien 0:632c9925f013 550 {
donatien 0:632c9925f013 551 /* next octet in next pbuf */
donatien 0:632c9925f013 552 p = p->next;
donatien 0:632c9925f013 553 if (p == NULL) { return ERR_ARG; }
donatien 0:632c9925f013 554 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 555 plen += p->len;
donatien 0:632c9925f013 556 }
donatien 0:632c9925f013 557 else
donatien 0:632c9925f013 558 {
donatien 0:632c9925f013 559 /* next octet in same pbuf */
donatien 0:632c9925f013 560 msg_ptr++;
donatien 0:632c9925f013 561 }
donatien 0:632c9925f013 562 }
donatien 0:632c9925f013 563 oid_ptr++;
donatien 0:632c9925f013 564 oid->len++;
donatien 0:632c9925f013 565 }
donatien 0:632c9925f013 566 if (len == 0)
donatien 0:632c9925f013 567 {
donatien 0:632c9925f013 568 /* len == 0, end of oid */
donatien 0:632c9925f013 569 return ERR_OK;
donatien 0:632c9925f013 570 }
donatien 0:632c9925f013 571 else
donatien 0:632c9925f013 572 {
donatien 0:632c9925f013 573 /* len > 0, oid->len == LWIP_SNMP_OBJ_ID_LEN or malformed encoding */
donatien 0:632c9925f013 574 return ERR_ARG;
donatien 0:632c9925f013 575 }
donatien 0:632c9925f013 576
donatien 0:632c9925f013 577 }
donatien 0:632c9925f013 578 p = p->next;
donatien 0:632c9925f013 579 }
donatien 0:632c9925f013 580 /* p == NULL, ofs >= plen */
donatien 0:632c9925f013 581 return ERR_ARG;
donatien 0:632c9925f013 582 }
donatien 0:632c9925f013 583
donatien 0:632c9925f013 584 /**
donatien 0:632c9925f013 585 * Decodes (copies) raw data (ip-addresses, octet strings, opaque encoding)
donatien 0:632c9925f013 586 * from incoming message into array.
donatien 0:632c9925f013 587 *
donatien 0:632c9925f013 588 * @param p points to a pbuf holding an ASN1 coded raw data
donatien 0:632c9925f013 589 * @param ofs points to the offset within the pbuf chain of the ASN1 coded raw data
donatien 0:632c9925f013 590 * @param len length of the coded raw data (zero is valid, e.g. empty string!)
donatien 0:632c9925f013 591 * @param raw_len length of the raw return value
donatien 0:632c9925f013 592 * @param raw return raw bytes
donatien 0:632c9925f013 593 * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) decode
donatien 0:632c9925f013 594 */
donatien 0:632c9925f013 595 err_t
donatien 0:632c9925f013 596 snmp_asn1_dec_raw(struct pbuf *p, u16_t ofs, u16_t len, u16_t raw_len, u8_t *raw)
donatien 0:632c9925f013 597 {
donatien 0:632c9925f013 598 u16_t plen, base;
donatien 0:632c9925f013 599 u8_t *msg_ptr;
donatien 0:632c9925f013 600
donatien 0:632c9925f013 601 if (len > 0)
donatien 0:632c9925f013 602 {
donatien 0:632c9925f013 603 plen = 0;
donatien 0:632c9925f013 604 while (p != NULL)
donatien 0:632c9925f013 605 {
donatien 0:632c9925f013 606 base = plen;
donatien 0:632c9925f013 607 plen += p->len;
donatien 0:632c9925f013 608 if (ofs < plen)
donatien 0:632c9925f013 609 {
donatien 0:632c9925f013 610 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 611 msg_ptr += ofs - base;
donatien 0:632c9925f013 612 if (raw_len >= len)
donatien 0:632c9925f013 613 {
donatien 0:632c9925f013 614 while (len > 1)
donatien 0:632c9925f013 615 {
donatien 0:632c9925f013 616 /* copy len - 1 octets */
donatien 0:632c9925f013 617 len--;
donatien 0:632c9925f013 618 *raw = *msg_ptr;
donatien 0:632c9925f013 619 raw++;
donatien 0:632c9925f013 620 ofs += 1;
donatien 0:632c9925f013 621 if (ofs >= plen)
donatien 0:632c9925f013 622 {
donatien 0:632c9925f013 623 /* next octet in next pbuf */
donatien 0:632c9925f013 624 p = p->next;
donatien 0:632c9925f013 625 if (p == NULL) { return ERR_ARG; }
donatien 0:632c9925f013 626 msg_ptr = (u8_t*)p->payload;
donatien 0:632c9925f013 627 plen += p->len;
donatien 0:632c9925f013 628 }
donatien 0:632c9925f013 629 else
donatien 0:632c9925f013 630 {
donatien 0:632c9925f013 631 /* next octet in same pbuf */
donatien 0:632c9925f013 632 msg_ptr++;
donatien 0:632c9925f013 633 }
donatien 0:632c9925f013 634 }
donatien 0:632c9925f013 635 /* copy last octet */
donatien 0:632c9925f013 636 *raw = *msg_ptr;
donatien 0:632c9925f013 637 return ERR_OK;
donatien 0:632c9925f013 638 }
donatien 0:632c9925f013 639 else
donatien 0:632c9925f013 640 {
donatien 0:632c9925f013 641 /* raw_len < len, not enough dst space */
donatien 0:632c9925f013 642 return ERR_ARG;
donatien 0:632c9925f013 643 }
donatien 0:632c9925f013 644 }
donatien 0:632c9925f013 645 p = p->next;
donatien 0:632c9925f013 646 }
donatien 0:632c9925f013 647 /* p == NULL, ofs >= plen */
donatien 0:632c9925f013 648 return ERR_ARG;
donatien 0:632c9925f013 649 }
donatien 0:632c9925f013 650 else
donatien 0:632c9925f013 651 {
donatien 0:632c9925f013 652 /* len == 0, empty string */
donatien 0:632c9925f013 653 return ERR_OK;
donatien 0:632c9925f013 654 }
donatien 0:632c9925f013 655 }
donatien 0:632c9925f013 656
donatien 0:632c9925f013 657 #endif /* LWIP_SNMP */