git.sur5r.net Git - freertos/blob - FreeRTOS/Demo/Common/ethernet/lwIP_130/src/core/snmp/asn1_dec.c

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