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