git.sur5r.net Git - freertos/blob - Demo/Common/ethernet/lwIP/core/snmp/asn1_enc.c

   1 /**\r
   2  * @file\r
   3  * Abstract Syntax Notation One (ISO 8824, 8825) encoding\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  * Returns octet count for length.\r
  44  *\r
  45  * @param length\r
  46  * @param octets_needed points to the return value\r
  47  */\r
  48 void\r
  49 snmp_asn1_enc_length_cnt(u16_t length, u8_t *octets_needed)\r
  50 {\r
  51   if (length < 0x80U)\r
  52   {\r
  53     *octets_needed = 1;\r
  54   }\r
  55   else if (length < 0x100U)\r
  56   {\r
  57     *octets_needed = 2;\r
  58   }\r
  59   else\r
  60   {\r
  61     *octets_needed = 3;\r
  62   }\r
  63 }\r
  64 \r
  65 /**\r
  66  * Returns octet count for an u32_t.\r
  67  *\r
  68  * @param value\r
  69  * @param octets_needed points to the return value\r
  70  *\r
  71  * @note ASN coded integers are _always_ signed. E.g. +0xFFFF is coded\r
  72  * as 0x00,0xFF,0xFF. Note the leading sign octet. A positive value\r
  73  * of 0xFFFFFFFF is preceded with 0x00 and the length is 5 octets!!\r
  74  */\r
  75 void\r
  76 snmp_asn1_enc_u32t_cnt(u32_t value, u16_t *octets_needed)\r
  77 {\r
  78   if (value < 0x80UL)\r
  79   {\r
  80     *octets_needed = 1;\r
  81   }\r
  82   else if (value < 0x8000UL)\r
  83   {\r
  84     *octets_needed = 2;\r
  85   }\r
  86   else if (value < 0x800000UL)\r
  87   {\r
  88     *octets_needed = 3;\r
  89   }\r
  90   else if (value < 0x80000000UL)\r
  91   {\r
  92     *octets_needed = 4;\r
  93   }\r
  94   else\r
  95   {\r
  96     *octets_needed = 5;\r
  97   }\r
  98 }\r
  99 \r
 100 /**\r
 101  * Returns octet count for an s32_t.\r
 102  *\r
 103  * @param value\r
 104  * @param octets_needed points to the return value\r
 105  *\r
 106  * @note ASN coded integers are _always_ signed.\r
 107  */\r
 108 void\r
 109 snmp_asn1_enc_s32t_cnt(s32_t value, u16_t *octets_needed)\r
 110 {\r
 111   if (value < 0)\r
 112   {\r
 113     value = ~value;\r
 114   }\r
 115   if (value < 0x80L)\r
 116   {\r
 117     *octets_needed = 1;\r
 118   }\r
 119   else if (value < 0x8000L)\r
 120   {\r
 121     *octets_needed = 2;\r
 122   }\r
 123   else if (value < 0x800000L)\r
 124   {\r
 125     *octets_needed = 3;\r
 126   }\r
 127   else\r
 128   {\r
 129     *octets_needed = 4;\r
 130   }\r
 131 }\r
 132 \r
 133 /**\r
 134  * Returns octet count for an object identifier.\r
 135  *\r
 136  * @param ident_len object identifier array length\r
 137  * @param ident points to object identifier array\r
 138  * @param octets_needed points to the return value\r
 139  */\r
 140 void\r
 141 snmp_asn1_enc_oid_cnt(u8_t ident_len, s32_t *ident, u16_t *octets_needed)\r
 142 {\r
 143   s32_t sub_id;\r
 144   u8_t cnt;\r
 145 \r
 146   cnt = 0;\r
 147   if (ident_len > 1)\r
 148   {\r
 149     /* compressed prefix in one octet */\r
 150     cnt++;\r
 151     ident_len -= 2;\r
 152     ident += 2;\r
 153   }\r
 154   while(ident_len > 0)\r
 155   {\r
 156     ident_len--;\r
 157     sub_id = *ident;\r
 158 \r
 159     sub_id >>= 7;\r
 160     cnt++;\r
 161     while(sub_id > 0)\r
 162     {\r
 163       sub_id >>= 7;\r
 164       cnt++;\r
 165     }\r
 166     ident++;\r
 167   }\r
 168   *octets_needed = cnt;\r
 169 }\r
 170 \r
 171 /**\r
 172  * Encodes ASN type field into a pbuf chained ASN1 msg.\r
 173  *\r
 174  * @param p points to output pbuf to encode value into\r
 175  * @param ofs points to the offset within the pbuf chain\r
 176  * @param type input ASN1 type\r
 177  * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode\r
 178  */\r
 179 err_t\r
 180 snmp_asn1_enc_type(struct pbuf *p, u16_t ofs, u8_t type)\r
 181 {\r
 182   u16_t plen, base;\r
 183   u8_t *msg_ptr;\r
 184 \r
 185   plen = 0;\r
 186   while (p != NULL)\r
 187   {\r
 188     base = plen;\r
 189     plen += p->len;\r
 190     if (ofs < plen)\r
 191     {\r
 192       msg_ptr = p->payload;\r
 193       msg_ptr += ofs - base;\r
 194       *msg_ptr = type;\r
 195       return ERR_OK;\r
 196     }\r
 197     p = p->next;\r
 198   }\r
 199   /* p == NULL, ofs >= plen */\r
 200   return ERR_ARG;\r
 201 }\r
 202 \r
 203 /**\r
 204  * Encodes host order length field into a pbuf chained ASN1 msg.\r
 205  *\r
 206  * @param p points to output pbuf to encode length into\r
 207  * @param ofs points to the offset within the pbuf chain\r
 208  * @param length is the host order length to be encoded\r
 209  * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode\r
 210  */\r
 211 err_t\r
 212 snmp_asn1_enc_length(struct pbuf *p, u16_t ofs, u16_t length)\r
 213 {\r
 214   u16_t plen, base;\r
 215   u8_t *msg_ptr;\r
 216 \r
 217   plen = 0;\r
 218   while (p != NULL)\r
 219   {\r
 220     base = plen;\r
 221     plen += p->len;\r
 222     if (ofs < plen)\r
 223     {\r
 224       msg_ptr = p->payload;\r
 225       msg_ptr += ofs - base;\r
 226 \r
 227       if (length < 0x80)\r
 228       {\r
 229         *msg_ptr = length;\r
 230         return ERR_OK;\r
 231       }\r
 232       else if (length < 0x100)\r
 233       {\r
 234         *msg_ptr = 0x81;\r
 235         ofs += 1;\r
 236         if (ofs >= plen)\r
 237         {\r
 238           /* next octet in next pbuf */\r
 239           p = p->next;\r
 240           if (p == NULL) { return ERR_ARG; }\r
 241           msg_ptr = p->payload;\r
 242         }\r
 243         else\r
 244         {\r
 245           /* next octet in same pbuf */\r
 246           msg_ptr++;\r
 247         }\r
 248         *msg_ptr = length;\r
 249         return ERR_OK;\r
 250       }\r
 251       else\r
 252       {\r
 253         u8_t i;\r
 254 \r
 255         /* length >= 0x100 && length <= 0xFFFF */\r
 256         *msg_ptr = 0x82;\r
 257         i = 2;\r
 258         while (i > 0)\r
 259         {\r
 260           i--;\r
 261           ofs += 1;\r
 262           if (ofs >= plen)\r
 263           {\r
 264             /* next octet in next pbuf */\r
 265             p = p->next;\r
 266             if (p == NULL) { return ERR_ARG; }\r
 267             msg_ptr = p->payload;\r
 268             plen += p->len;\r
 269           }\r
 270           else\r
 271           {\r
 272             /* next octet in same pbuf */\r
 273             msg_ptr++;\r
 274           }\r
 275           if (i == 0)\r
 276           {\r
 277             /* least significant length octet */\r
 278             *msg_ptr = length;\r
 279           }\r
 280           else\r
 281           {\r
 282             /* most significant length octet */\r
 283             *msg_ptr = length >> 8;\r
 284           }\r
 285         }\r
 286         return ERR_OK;\r
 287       }\r
 288     }\r
 289     p = p->next;\r
 290   }\r
 291   /* p == NULL, ofs >= plen */\r
 292   return ERR_ARG;\r
 293 }\r
 294 \r
 295 /**\r
 296  * Encodes u32_t (counter, gauge, timeticks) into a pbuf chained ASN1 msg.\r
 297  *\r
 298  * @param p points to output pbuf to encode value into\r
 299  * @param ofs points to the offset within the pbuf chain\r
 300  * @param octets_needed encoding length (from snmp_asn1_enc_u32t_cnt())\r
 301  * @param value is the host order u32_t value to be encoded\r
 302  * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode\r
 303  *\r
 304  * @see snmp_asn1_enc_u32t_cnt()\r
 305  */\r
 306 err_t\r
 307 snmp_asn1_enc_u32t(struct pbuf *p, u16_t ofs, u8_t octets_needed, u32_t value)\r
 308 {\r
 309   u16_t plen, base;\r
 310   u8_t *msg_ptr;\r
 311 \r
 312   plen = 0;\r
 313   while (p != NULL)\r
 314   {\r
 315     base = plen;\r
 316     plen += p->len;\r
 317     if (ofs < plen)\r
 318     {\r
 319       msg_ptr = p->payload;\r
 320       msg_ptr += ofs - base;\r
 321 \r
 322       if (octets_needed == 5)\r
 323       {\r
 324         /* not enough bits in 'value' add leading 0x00 */\r
 325         octets_needed--;\r
 326         *msg_ptr = 0x00;\r
 327         ofs += 1;\r
 328         if (ofs >= plen)\r
 329         {\r
 330           /* next octet in next pbuf */\r
 331           p = p->next;\r
 332           if (p == NULL) { return ERR_ARG; }\r
 333           msg_ptr = p->payload;\r
 334           plen += p->len;\r
 335         }\r
 336         else\r
 337         {\r
 338           /* next octet in same pbuf */\r
 339           msg_ptr++;\r
 340         }\r
 341       }\r
 342       while (octets_needed > 1)\r
 343       {\r
 344         octets_needed--;\r
 345         *msg_ptr = value >> (octets_needed << 3);\r
 346         ofs += 1;\r
 347         if (ofs >= plen)\r
 348         {\r
 349           /* next octet in next pbuf */\r
 350           p = p->next;\r
 351           if (p == NULL) { return ERR_ARG; }\r
 352           msg_ptr = p->payload;\r
 353           plen += p->len;\r
 354         }\r
 355         else\r
 356         {\r
 357           /* next octet in same pbuf */\r
 358           msg_ptr++;\r
 359         }\r
 360       }\r
 361       /* (only) one least significant octet */\r
 362       *msg_ptr = value;\r
 363       return ERR_OK;\r
 364     }\r
 365     p = p->next;\r
 366   }\r
 367   /* p == NULL, ofs >= plen */\r
 368   return ERR_ARG;\r
 369 }\r
 370 \r
 371 /**\r
 372  * Encodes s32_t integer into a pbuf chained ASN1 msg.\r
 373  *\r
 374  * @param p points to output pbuf to encode value into\r
 375  * @param ofs points to the offset within the pbuf chain\r
 376  * @param octets_needed encoding length (from snmp_asn1_enc_s32t_cnt())\r
 377  * @param value is the host order s32_t value to be encoded\r
 378  * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode\r
 379  *\r
 380  * @see snmp_asn1_enc_s32t_cnt()\r
 381  */\r
 382 err_t\r
 383 snmp_asn1_enc_s32t(struct pbuf *p, u16_t ofs, u8_t octets_needed, s32_t value)\r
 384 {\r
 385   u16_t plen, base;\r
 386   u8_t *msg_ptr;\r
 387 \r
 388   plen = 0;\r
 389   while (p != NULL)\r
 390   {\r
 391     base = plen;\r
 392     plen += p->len;\r
 393     if (ofs < plen)\r
 394     {\r
 395       msg_ptr = p->payload;\r
 396       msg_ptr += ofs - base;\r
 397 \r
 398       while (octets_needed > 1)\r
 399       {\r
 400         octets_needed--;\r
 401         *msg_ptr = value >> (octets_needed << 3);\r
 402         ofs += 1;\r
 403         if (ofs >= plen)\r
 404         {\r
 405           /* next octet in next pbuf */\r
 406           p = p->next;\r
 407           if (p == NULL) { return ERR_ARG; }\r
 408           msg_ptr = p->payload;\r
 409           plen += p->len;\r
 410         }\r
 411         else\r
 412         {\r
 413           /* next octet in same pbuf */\r
 414           msg_ptr++;\r
 415         }\r
 416       }\r
 417       /* (only) one least significant octet */\r
 418       *msg_ptr = value;\r
 419       return ERR_OK;\r
 420     }\r
 421     p = p->next;\r
 422   }\r
 423   /* p == NULL, ofs >= plen */\r
 424   return ERR_ARG;\r
 425 }\r
 426 \r
 427 /**\r
 428  * Encodes object identifier into a pbuf chained ASN1 msg.\r
 429  *\r
 430  * @param p points to output pbuf to encode oid into\r
 431  * @param ofs points to the offset within the pbuf chain\r
 432  * @param ident_len object identifier array length\r
 433  * @param ident points to object identifier array\r
 434  * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode\r
 435  */\r
 436 err_t\r
 437 snmp_asn1_enc_oid(struct pbuf *p, u16_t ofs, u8_t ident_len, s32_t *ident)\r
 438 {\r
 439   u16_t plen, base;\r
 440   u8_t *msg_ptr;\r
 441 \r
 442   plen = 0;\r
 443   while (p != NULL)\r
 444   {\r
 445     base = plen;\r
 446     plen += p->len;\r
 447     if (ofs < plen)\r
 448     {\r
 449       msg_ptr = p->payload;\r
 450       msg_ptr += ofs - base;\r
 451 \r
 452       if (ident_len > 1)\r
 453       {\r
 454         if ((ident[0] == 1) && (ident[1] == 3))\r
 455         {\r
 456           /* compressed (most common) prefix .iso.org */\r
 457           *msg_ptr = 0x2b;\r
 458         }\r
 459         else\r
 460         {\r
 461           /* calculate prefix */\r
 462           *msg_ptr = (ident[0] * 40) + ident[1];\r
 463         }\r
 464         ofs += 1;\r
 465         if (ofs >= plen)\r
 466         {\r
 467           /* next octet in next pbuf */\r
 468           p = p->next;\r
 469           if (p == NULL) { return ERR_ARG; }\r
 470           msg_ptr = p->payload;\r
 471           plen += p->len;\r
 472         }\r
 473         else\r
 474         {\r
 475           /* next octet in same pbuf */\r
 476           msg_ptr++;\r
 477         }\r
 478         ident_len -= 2;\r
 479         ident += 2;\r
 480       }\r
 481       else\r
 482       {\r
 483 /* @bug:  allow empty varbinds for symmetry (we must decode them for getnext), allow partial compression??  */\r
 484         /* ident_len <= 1, at least we need zeroDotZero (0.0) (ident_len == 2) */\r
 485         return ERR_ARG;\r
 486       }\r
 487       while (ident_len > 0)\r
 488       {\r
 489         s32_t sub_id;\r
 490         u8_t shift, tail;\r
 491 \r
 492         ident_len--;\r
 493         sub_id = *ident;\r
 494         tail = 0;\r
 495         shift = 28;\r
 496         while(shift > 0)\r
 497         {\r
 498           u8_t code;\r
 499 \r
 500           code = sub_id >> shift;\r
 501           if ((code != 0) || (tail != 0))\r
 502           {\r
 503             tail = 1;\r
 504             *msg_ptr = code | 0x80;\r
 505             ofs += 1;\r
 506             if (ofs >= plen)\r
 507             {\r
 508               /* next octet in next pbuf */\r
 509               p = p->next;\r
 510               if (p == NULL) { return ERR_ARG; }\r
 511               msg_ptr = p->payload;\r
 512               plen += p->len;\r
 513             }\r
 514             else\r
 515             {\r
 516               /* next octet in same pbuf */\r
 517               msg_ptr++;\r
 518             }\r
 519           }\r
 520           shift -= 7;\r
 521         }\r
 522         *msg_ptr = (u8_t)sub_id & 0x7F;\r
 523         if (ident_len > 0)\r
 524         {\r
 525           ofs += 1;\r
 526           if (ofs >= plen)\r
 527           {\r
 528             /* next octet in next pbuf */\r
 529             p = p->next;\r
 530             if (p == NULL) { return ERR_ARG; }\r
 531             msg_ptr = p->payload;\r
 532             plen += p->len;\r
 533           }\r
 534           else\r
 535           {\r
 536             /* next octet in same pbuf */\r
 537             msg_ptr++;\r
 538           }\r
 539         }\r
 540         /* proceed to next sub-identifier */\r
 541         ident++;\r
 542       }\r
 543       return ERR_OK;\r
 544     }\r
 545     p = p->next;\r
 546   }\r
 547   /* p == NULL, ofs >= plen */\r
 548   return ERR_ARG;\r
 549 }\r
 550 \r
 551 /**\r
 552  * Encodes raw data (octet string, opaque) into a pbuf chained ASN1 msg.\r
 553  *\r
 554  * @param p points to output pbuf to encode raw data into\r
 555  * @param ofs points to the offset within the pbuf chain\r
 556  * @param raw_len raw data length\r
 557  * @param raw points raw data\r
 558  * @return ERR_OK if successfull, ERR_ARG if we can't (or won't) encode\r
 559  */\r
 560 err_t\r
 561 snmp_asn1_enc_raw(struct pbuf *p, u16_t ofs, u8_t raw_len, u8_t *raw)\r
 562 {\r
 563   u16_t plen, base;\r
 564   u8_t *msg_ptr;\r
 565 \r
 566   plen = 0;\r
 567   while (p != NULL)\r
 568   {\r
 569     base = plen;\r
 570     plen += p->len;\r
 571     if (ofs < plen)\r
 572     {\r
 573       msg_ptr = p->payload;\r
 574       msg_ptr += ofs - base;\r
 575 \r
 576       while (raw_len > 1)\r
 577       {\r
 578         /* copy raw_len - 1 octets */\r
 579         raw_len--;\r
 580         *msg_ptr = *raw;\r
 581         raw++;\r
 582         ofs += 1;\r
 583         if (ofs >= plen)\r
 584         {\r
 585           /* next octet in next pbuf */\r
 586           p = p->next;\r
 587           if (p == NULL) { return ERR_ARG; }\r
 588           msg_ptr = p->payload;\r
 589           plen += p->len;\r
 590         }\r
 591         else\r
 592         {\r
 593           /* next octet in same pbuf */\r
 594           msg_ptr++;\r
 595         }\r
 596       }\r
 597       if (raw_len > 0)\r
 598       {\r
 599         /* copy last or single octet */\r
 600         *msg_ptr = *raw;\r
 601       }\r
 602       return ERR_OK;\r
 603     }\r
 604     p = p->next;\r
 605   }\r
 606   /* p == NULL, ofs >= plen */\r
 607   return ERR_ARG;\r
 608 }\r
 609 \r
 610 #endif /* LWIP_SNMP */\r