release/src/router/apcupsd/src/drivers/snmplite/asn.cpp

   1 /*
   2  * asn.cpp
   3  *
   4  * ASN.1 type classes
   5  */
   6
   7 /*
   8  * Copyright (C) 2009 Adam Kropelin
   9  *
  10  * This program is free software; you can redistribute it and/or
  11  * modify it under the terms of version 2 of the GNU General
  12  * Public License as published by the Free Software Foundation.
  13  *
  14  * This program is distributed in the hope that it will be useful,
  15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  17  * General Public License for more details.
  18  *
  19  * You should have received a copy of the GNU General Public
  20  * License along with this program; if not, write to the Free
  21  * Software Foundation, Inc., 59 Temple Place - Suite 330, Boston,
  22  * MA 02111-1307, USA.
  23  */
  24
  25 #include "asn.h"
  26 #include <stdlib.h>
  27 #include <stdio.h>
  28
  29 using namespace Asn;
  30
  31 void debug(const char *foo, int indent)
  32 {
  33    while (indent--)
  34       printf(" ");
  35    printf("%s\n", foo);
  36 }
  37
  38 // *****************************************************************************
  39 // Object
  40 // *****************************************************************************
  41
  42 Object *Object::Demarshal(unsigned char *&buffer, unsigned int &buflen)
  43 {
  44    static int indent = 0;
  45
  46    // Must have at least one byte to work with
  47    if (buflen < 1)
  48       return NULL;
  49
  50    // Extract type code from stream
  51    Identifier type = (Identifier)*buffer++;
  52    buflen--;
  53
  54    // Create proper object based on type code
  55    Object *obj;
  56
  57    switch (type)
  58    {
  59    case INTEGER:
  60    case COUNTER:
  61    case GAUGE:
  62    case TIMETICKS:
  63       debug("INTEGER", indent);
  64       obj = new Integer(type);
  65       break;
  66    case OCTETSTRING:
  67    case IPADDRESS:
  68       debug("OCTETSTRING", indent);
  69       obj = new OctetString();
  70       break;
  71    case OBJECTID:
  72       debug("OBJECTID", indent);
  73       obj = new ObjectId();
  74       break;
  75    case NULLL:
  76       debug("NULLL", indent);
  77       obj = new Null();
  78       break;
  79    case SEQUENCE:
  80    case GET_REQ_PDU:
  81    case GETNEXT_REQ_PDU:
  82    case GET_RSP_PDU:
  83    case TRAP_PDU:
  84       debug("SEQUENCE", indent);
  85       indent += 2;
  86       obj = new Sequence(type);
  87       break;
  88    default:
  89       printf("UNKNOWN ASN type=0x%02x\n", type);
  90       debug("UNKNOWN", indent);
  91       obj = NULL;
  92       break;
  93    }
  94
  95    // Have the object demarshal itself from the stream
  96    if (obj && !obj->demarshal(buffer, buflen))
  97    {
  98       delete obj;
  99       obj = NULL;
 100    }
 101
 102    if (type & CONSTRUCTED)
 103       indent -= 2;
 104
 105    return obj;
 106 }
 107
 108 int Object::numbits(unsigned int num) const
 109 {
 110    if (num == 0)
 111       return 1;
 112
 113    int bits = 0;
 114    while (num)
 115    {
 116       bits++;
 117       num >>= 1;
 118    }
 119
 120    return bits;
 121 }
 122
 123 bool Object::marshalLength(
 124    unsigned int len,
 125    unsigned char *&buffer,
 126    unsigned int &buflen) const
 127 {
 128    // Compute number of bytes required to store length
 129    unsigned int bits = numbits(len);
 130    unsigned int datalen;
 131    if (bits <= 7)
 132       datalen = 1;
 133    else
 134       datalen = (bits + 7) / 8 + 1;
 135
 136    // If no buffer provided, just return number of bytes required
 137    if (!buffer)
 138    {
 139       buflen += datalen;
 140       return true;
 141    }
 142
 143    // Fail if not enough space remaining in buffer
 144    if (buflen < datalen)
 145       return false;
 146
 147    // If using long form, begin with byte count
 148    if (datalen > 1)
 149       *buffer++ = (datalen - 1) | 0x80;
 150
 151    // Encode length bytes
 152    switch (datalen)
 153    {
 154    case 5:
 155       *buffer++ = (len >> 24) & 0xff;
 156    case 4:
 157       *buffer++ = (len >> 16) & 0xff;
 158    case 3:
 159       *buffer++ = (len >>  8) & 0xff;
 160    case 2:
 161    case 1:
 162       *buffer++ = len & 0xff;
 163       break;
 164    }
 165
 166    return true;
 167 }
 168
 169 bool Object::marshalType(unsigned char *&buffer, unsigned int &buflen) const
 170 {
 171    // If no buffer provided, just return number of bytes required
 172    if (!buffer)
 173    {
 174       buflen++;
 175       return true;
 176    }
 177
 178    // Fail if not enough room for data
 179    if (buflen < 1)
 180       return false;
 181    buflen--;
 182
 183    // Store type directly in stream
 184    *buffer++ = _type;
 185    return true;
 186 }
 187
 188 bool Object::demarshalLength(
 189    unsigned char *&buffer,
 190    unsigned int &buflen,
 191    unsigned int &vallen) const
 192 {
 193    // Must have at least one byte to work with
 194    if (buflen < 1)
 195       return false;
 196    unsigned int datalen = *buffer++;
 197    buflen--;
 198
 199    // Values less than 128 are stored directly in the first (only) byte
 200    if (datalen < 128)
 201    {
 202       vallen = datalen;
 203       return true;
 204    }
 205
 206    // For values > 128, first byte-128 indicates number of bytes to follow
 207    // Bail if not enough data for additional bytes
 208    datalen -= 128;
 209    if (buflen < datalen)
 210       return false;
 211    buflen -= datalen;
 212
 213    // Read data bytes
 214    vallen = 0;
 215    while (datalen--)
 216    {
 217       vallen <<= 8;
 218       vallen |= *buffer++;
 219    }
 220
 221    return true;
 222 }
 223
 224 // *****************************************************************************
 225 // Integer
 226 // *****************************************************************************
 227
 228 bool Integer::Marshal(unsigned char *&buffer, unsigned int &buflen) const
 229 {
 230    // Marshal the type code
 231    if (!marshalType(buffer, buflen))
 232       return false;
 233
 234    // Calculate the number of bytes it will require to store the value
 235    unsigned int datalen = 4;
 236    unsigned int tmp = _value;
 237    while (datalen > 1)
 238    {
 239       if (( _signed && (tmp & 0xff800000) != 0xff800000) ||
 240           (!_signed && (tmp & 0xff800000) != 0x00000000))
 241          break;
 242
 243        tmp <<= 8;
 244        datalen--;
 245    }
 246
 247    // Special case for unsigned value with MSb set
 248    if (!_signed && datalen == 4 && (tmp & 0x80000000))
 249       datalen = 5;
 250
 251    // Marshal the length
 252    if (!marshalLength(datalen, buffer, buflen))
 253       return false;
 254
 255    // If no buffer provided, just return number of bytes required
 256    if (!buffer)
 257    {
 258       buflen += datalen;
 259       return true;
 260    }
 261
 262    // Fail if not enough room for data
 263    if (buflen < datalen)
 264       return false;
 265    buflen -= datalen;
 266
 267    // Marshal the value itself
 268    switch (datalen)
 269    {
 270    case 5:
 271       *buffer++ = 0;
 272    case 4:
 273       *buffer++ = (_value >> 24) & 0xff;
 274    case 3:
 275       *buffer++ = (_value >> 16) & 0xff;
 276    case 2:
 277       *buffer++ = (_value >>  8) & 0xff;
 278    case 1:
 279       *buffer++ = _value & 0xff;
 280    }
 281
 282    return true;
 283 }
 284
 285 bool Integer::demarshal(unsigned char *&buffer, unsigned int &buflen)
 286 {
 287    // Unmarshal the data length
 288    unsigned int datalen;
 289    if (!demarshalLength(buffer, buflen, datalen) ||
 290        datalen < 1 || datalen > 4 || buflen < datalen)
 291    {
 292       return false;
 293    }
 294    buflen -= datalen;
 295
 296    // Determine signedness
 297    if (*buffer & 0x80)
 298    {
 299       // Start with all 1s so result will be sign-extended
 300       _value = (unsigned int)-1;
 301       _signed = true;
 302    }
 303    else
 304    {
 305       _value = 0;
 306       _signed = false;
 307    }
 308
 309    // Unmarshal the data
 310    while (datalen--)
 311    {
 312       _value <<= 8;
 313       _value |= *buffer++;
 314    }
 315
 316    return true;
 317 }
 318
 319 // *****************************************************************************
 320 // OctetString
 321 // *****************************************************************************
 322
 323 OctetString::OctetString(const char *value) :
 324    Object(OCTETSTRING),
 325    _data(NULL)
 326 {
 327    assign((const unsigned char *)value, strlen(value));
 328 }
 329
 330 OctetString::OctetString(const unsigned char *value, unsigned int len) :
 331    Object(OCTETSTRING),
 332    _data(NULL)
 333 {
 334    assign(value, len);
 335 }
 336
 337 OctetString::OctetString(const OctetString &rhs) :
 338    Object(OCTETSTRING),
 339    _data(NULL)
 340 {
 341    assign(rhs._data, rhs._len);
 342 }
 343
 344 OctetString &OctetString::operator=(const OctetString &rhs)
 345 {
 346    if (&rhs != this)
 347       assign(rhs._data, rhs._len);
 348    return *this;
 349 }
 350
 351 bool OctetString::Marshal(unsigned char *&buffer, unsigned int &buflen) const
 352 {
 353    // Marshal the type code
 354    if (!marshalType(buffer, buflen))
 355       return false;
 356
 357    // Marshal the length
 358    if (!marshalLength(_len, buffer, buflen))
 359       return false;
 360
 361    // If no buffer provided, just return number of bytes required
 362    if (!buffer)
 363    {
 364       buflen += _len;
 365       return true;
 366    }
 367
 368    // Bail if not enough room for data
 369    if ((unsigned int)buflen < _len)
 370       return false;
 371    buflen -= _len;
 372
 373    // Marshal data
 374    memcpy(buffer, _data, _len);
 375    buffer += _len;
 376
 377    return true;
 378 }
 379
 380 bool OctetString::demarshal(unsigned char *&buffer, unsigned int &buflen)
 381 {
 382    // Unmarshal the data length
 383    unsigned int datalen;
 384    if (!demarshalLength(buffer, buflen, datalen) ||
 385        datalen < 1 || buflen < datalen)
 386    {
 387       return false;
 388    }
 389    buflen -= datalen;
 390
 391    // Unmarshal the data
 392    assign(buffer, datalen);
 393    buffer += datalen;
 394
 395    return true;
 396 }
 397
 398 void OctetString::assign(const unsigned char *data, unsigned int len)
 399 {
 400    delete [] _data;
 401    _data = new unsigned char[len+1];
 402    memcpy(_data, data, len);
 403    _data[len] = '\0';
 404    _len = len;
 405 }
 406
 407 // *****************************************************************************
 408 // ObjectId
 409 // *****************************************************************************
 410
 411 ObjectId::ObjectId(const int oid[]) :
 412    Object(OBJECTID),
 413    _value(NULL)
 414 {
 415    assign(oid);
 416 }
 417
 418 ObjectId::ObjectId(const ObjectId &rhs) :
 419    Object(OBJECTID),
 420    _value(NULL)
 421 {
 422    assign(rhs._value, rhs._count);
 423 }
 424
 425 ObjectId &ObjectId::operator=(const ObjectId &rhs)
 426 {
 427    if (&rhs != this)
 428       assign(rhs._value, rhs._count);
 429    return *this;
 430 }
 431
 432 ObjectId &ObjectId::operator=(const int oid[])
 433 {
 434    if (oid != _value)
 435       assign(oid);
 436    return *this;
 437 }
 438
 439 void ObjectId::assign(const int oid[])
 440 {
 441    unsigned int count = 0;
 442    while (oid[count] != -1)
 443       count++;
 444    assign(oid, count);
 445 }
 446
 447 void ObjectId::assign(const int oid[], unsigned int count)
 448 {
 449    delete [] _value;
 450    _value = NULL;
 451    _count = count;
 452    if (_count)
 453    {
 454       _value = new int[_count];
 455       memcpy(_value, oid, _count*sizeof(int));
 456    }
 457 }
 458
 459 bool ObjectId::operator==(const ObjectId &rhs) const
 460 {
 461    if (_count != rhs._count)
 462       return false;
 463
 464    for (unsigned int i = 0; i < _count; i++)
 465    {
 466       if (_value[i] != rhs._value[i])
 467          return false;
 468    }
 469
 470    return true;
 471 }
 472
 473 bool ObjectId::operator==(const int oid[]) const
 474 {
 475    unsigned int i;
 476    for (i = 0; i < _count && oid[i] != -1; i++)
 477    {
 478       if (_value[i] != oid[i])
 479          return false;
 480    }
 481
 482    return i == _count && oid[i] == -1;
 483 }
 484
 485 // Same as operator== except we're allowed to be longer than the parameter
 486 bool ObjectId::IsChildOf(const int oid[])
 487 {
 488    unsigned int i;
 489    for (i = 0; i < _count && oid[i] != -1; i++)
 490    {
 491       if (_value[i] != oid[i])
 492          return false;
 493    }
 494
 495    return i < _count && oid[i] == -1;
 496 }
 497
 498 bool ObjectId::Marshal(unsigned char *&buffer, unsigned int &buflen) const
 499 {
 500    // Protect from trying to marshal an empty object
 501    if (!_value || _count < 2)
 502       return false;
 503
 504    // Marshal the type code
 505    if (!marshalType(buffer, buflen))
 506       return false;
 507
 508    // ASN.1 requires a special case for first two identifiers
 509    unsigned int cnt = _count-1;
 510    unsigned int vals[cnt];
 511    vals[0] = _value[0] * 40 + _value[1];
 512    for (unsigned int i = 2; i < _count; i++)
 513       vals[i-1] = _value[i];
 514
 515    // Calculate number of octets required to store data
 516    // We can only store 7 bits of data in each octet, so round accordingly
 517    unsigned int datalen = 0;
 518    for (unsigned int i = 0; i < cnt; i++)
 519       datalen += (numbits(vals[i]) + 6) / 7;
 520
 521    // Marshal the length
 522    if (!marshalLength(datalen, buffer, buflen))
 523       return false;
 524
 525    // If no buffer provided, just return number of bytes required
 526    if (!buffer)
 527    {
 528       buflen += datalen;
 529       return true;
 530    }
 531
 532    // Bail if data bytes will not fit
 533    if (buflen < datalen)
 534       return false;
 535    buflen -= datalen;
 536
 537    // Write data: 7 data bits per octet, bit 7 set on all but final octet
 538    for (unsigned int i = 0; i < cnt; i++)
 539    {
 540       unsigned int val = vals[i];
 541       unsigned int noctets = (numbits(val) + 6) / 7;
 542       switch (noctets)
 543       {
 544       case 5:
 545          *buffer++ = ((val >> 28) & 0x7f) | 0x80;
 546       case 4:
 547          *buffer++ = ((val >> 21) & 0x7f) | 0x80;
 548       case 3:
 549          *buffer++ = ((val >> 14) & 0x7f) | 0x80;
 550       case 2:
 551          *buffer++ = ((val >>  7) & 0x7f) | 0x80;
 552       case 1:
 553       case 0:
 554          *buffer++ = val & 0x7f;
 555       }
 556    }
 557
 558    return true;
 559 }
 560
 561 bool ObjectId::demarshal(unsigned char *&buffer, unsigned int &buflen)
 562 {
 563    // Unmarshal the data length
 564    unsigned int datalen;
 565    if (!demarshalLength(buffer, buflen, datalen) ||
 566        datalen < 1 || buflen < datalen)
 567    {
 568       return false;
 569    }
 570    buflen -= datalen;
 571
 572    // Allocate new value array, sized for worst case. +1 is because of
 573    // ASN.1 special case of compressing first two ids into one octet.
 574    delete [] _value;
 575    _value = new int[datalen+1];
 576
 577    // Unmarshal identifier values
 578    unsigned int i = 0;
 579    while (datalen)
 580    {
 581       // Accumulate octets into this identifier
 582       _value[i] = 0;
 583       unsigned int val;
 584       do
 585       {
 586          datalen--;
 587          val = *buffer++;
 588          _value[i] <<= 7;
 589          _value[i] |= val & 0x7f;
 590       }
 591       while (datalen && val & 0x80);
 592
 593       // Handle special case for first two ids
 594       if (i++ == 0)
 595       {
 596          _value[1] = _value[0] % 40;
 597          _value[0] /= 40;
 598          i++;
 599       }
 600    }
 601    _count = i;
 602
 603    return true;
 604 }
 605
 606 // *****************************************************************************
 607 // Null
 608 // *****************************************************************************
 609
 610 bool Null::Marshal(unsigned char *&buffer, unsigned int &buflen) const
 611 {
 612    // Marshal the type code
 613    if (!marshalType(buffer, buflen))
 614       return false;
 615
 616    // Marshal the length
 617    if (!marshalLength(0, buffer, buflen))
 618       return false;
 619
 620    return true;
 621 }
 622
 623 bool Null::demarshal(unsigned char *&buffer, unsigned int &buflen)
 624 {
 625    // Unmarshal the data length
 626    unsigned int datalen;
 627    return demarshalLength(buffer, buflen, datalen) && datalen == 0;
 628 }
 629
 630 // *****************************************************************************
 631 // Sequence
 632 // *****************************************************************************
 633
 634 Sequence::Sequence(Identifier type) :
 635    Object(type),
 636    _data(NULL),
 637    _size(0)
 638 {
 639 }
 640
 641 Sequence::~Sequence()
 642 {
 643    clear();
 644 }
 645
 646 Sequence::Sequence(const Sequence &rhs) :
 647    Object(rhs._type),
 648    _data(NULL)
 649 {
 650    assign(rhs);
 651 }
 652
 653 void Sequence::clear()
 654 {
 655    for (unsigned int i = 0; i < _size; i++)
 656       delete _data[i];
 657    delete [] _data;
 658    _size = 0;
 659 }
 660
 661 bool Sequence::Marshal(unsigned char *&buffer, unsigned int &buflen) const
 662 {
 663    // Marshal the type code
 664    if (!marshalType(buffer, buflen))
 665       return false;
 666
 667    // Need to calculate the length of the marshalled sequence.
 668    // Do so by passing a NULL buffer to Marshal() which will
 669    // accumulate total length in buflen parameter.
 670    unsigned int datalen = 0;
 671    unsigned char *buf = NULL;
 672    for (unsigned int i = 0; i < _size; ++i)
 673    {
 674       if (!_data[i]->Marshal(buf, datalen))
 675          return false;
 676    }
 677
 678    // Now marshal the length itself
 679    if (!marshalLength(datalen, buffer, buflen))
 680       return false;
 681
 682    // If no buffer provided, just return number of bytes required
 683    if (!buffer)
 684    {
 685       buflen += datalen;
 686       return true;
 687    }
 688
 689    // Marshal all items in the sequence
 690    for (unsigned int i = 0; i < _size; ++i)
 691    {
 692       if (!_data[i]->Marshal(buffer, buflen))
 693          return false;
 694    }
 695
 696    return true;
 697 }
 698
 699 bool Sequence::demarshal(unsigned char *&buffer, unsigned int &buflen)
 700 {
 701    // Free any existing data
 702    clear();
 703
 704    // Unmarshal the sequence data length
 705    unsigned int datalen;
 706    if (!demarshalLength(buffer, buflen, datalen) ||
 707        datalen < 1 || buflen < datalen)
 708    {
 709       return false;
 710    }
 711
 712    // Unmarshal items from the stream until sequence data is exhausted
 713    unsigned char *start = buffer;
 714    while (datalen)
 715    {
 716       Object *obj = Object::Demarshal(buffer, datalen);
 717       if (!obj)
 718          return false;
 719       Append(obj);
 720    }
 721
 722    buflen -= buffer-start;
 723    return true;
 724 }
 725
 726 void Sequence::Append(Object *obj)
 727 {
 728    // realloc ... not efficient, but easy
 729    Object **tmp = new Object *[_size+1];
 730    memcpy(tmp, _data, _size * sizeof(_data[0]));
 731    tmp[_size++] = obj;
 732    delete [] _data;
 733    _data = tmp;
 734 }
 735
 736 Object *Sequence::operator[](unsigned int idx)
 737 {
 738    if (idx >= _size)
 739       return NULL;
 740    return _data[idx];
 741 }
 742
 743 Sequence &Sequence::operator=(const Sequence &rhs)
 744 {
 745    if (this != &rhs)
 746       assign(rhs);
 747    return *this;
 748 }
 749
 750 void Sequence::assign(const Sequence &rhs)
 751 {
 752    clear();
 753
 754    _type = rhs._type;
 755    _size = rhs._size;
 756    _data = new Object *[_size];
 757
 758    for (unsigned int i = 0; i < _size; i++)
 759       _data[i] = rhs._data[i]->copy();
 760 }