Simon Ford / NetServices

Version of http://mbed.org/cookbook/NetServicesTribute with setting set the same for LPC2368

Dependents:   UDPSocketExample 24LCxx_I2CApp WeatherPlatform_pachube HvZServerLib ... more

lwip/core/snmp/asn1_dec.c@0:350011bf8be7, 2010-11-23 (annotated)

Committer:
simon
Date:
Tue Nov 23 14:15:36 2010 +0000
Revision:
0:350011bf8be7
Experimental version for testing UDP

Who changed what in which revision?

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