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

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