/*
 *              Common Public License Version 0.5
 *
 *              THE ACCOMPANYING PROGRAM IS PROVIDED UNDER THE TERMS OF
 *              THIS COMMON PUBLIC LICENSE ("AGREEMENT"). ANY USE,
 *              REPRODUCTION OR DISTRIBUTION OF THE PROGRAM CONSTITUTES
 *              RECIPIENT'S ACCEPTANCE OF THIS AGREEMENT.
 *
 *              1. DEFINITIONS
 *
 *              "Contribution" means:
 *                    a) in the case of the initial Contributor, the
 *                    initial code and documentation distributed under
 *                    this Agreement, and
 *
 *                    b) in the case of each subsequent Contributor:
 *                    i) changes to the Program, and
 *                    ii) additions to the Program;
 *
 *                    where such changes and/or additions to the Program
 *                    originate from and are distributed by that
 *                    particular Contributor. A Contribution 'originates'
 *                    from a Contributor if it was added to the Program
 *                    by such Contributor itself or anyone acting on such
 *                    Contributor's behalf. Contributions do not include
 *                    additions to the Program which: (i) are separate
 *                    modules of software distributed in conjunction with
 *                    the Program under their own license agreement, and
 *                    (ii) are not derivative works of the Program.
 *
 *
 *              "Contributor" means any person or entity that distributes
 *              the Program.
 *
 *              "Licensed Patents " mean patent claims licensable by a
 *              Contributor which are necessarily infringed by the use or
 *              sale of its Contribution alone or when combined with the
 *              Program.
 *
 *              "Program" means the Contributions distributed in
 *              accordance with this Agreement.
 *
 *              "Recipient" means anyone who receives the Program under
 *              this Agreement, including all Contributors.
 *
 *              2. GRANT OF RIGHTS
 *
 *                    a) Subject to the terms of this Agreement, each
 *                    Contributor hereby grants Recipient a
 *                    no - exclusive, worldwide, royalt - free copyright
 *                    license to reproduce, prepare derivative works of,
 *                    publicly display, publicly perform, distribute and
 *                    sublicense the Contribution of such Contributor, if
 *                    any, and such derivative works, in source code and
 *                    object code form.
 *
 *                    b) Subject to the terms of this Agreement, each
 *                    Contributor hereby grants Recipient a
 *                    no - exclusive, worldwide, royalt - free patent
 *                    license under Licensed Patents to make, use, sell,
 *                    offer to sell, import and otherwise transfer the
 *                    Contribution of such Contributor, if any, in source
 *                    code and object code form. This patent license
 *                    shall apply to the combination of the Contribution
 *                    and the Program if, at the time the Contribution is
 *                    added by the Contributor, such addition of the
 *                    Contribution causes such combination to be covered
 *                    by the Licensed Patents. The patent license shall
 *                    not apply to any other combinations which include
 *                    the Contribution. No hardware per se is licensed
 *                    hereunder.
 *
 *                    c) Recipient understands that although each
 *                    Contributor grants the licenses to its
 *                    Contributions set forth herein, no assurances are
 *                    provided by any Contributor that the Program does
 *                    not infringe the patent or other intellectual
 *                    property rights of any other entity. Each
 *                    Contributor disclaims any liability to Recipient
 *                    for claims brought by any other entity based on
 *                    infringement of intellectual property rights or
 *                    otherwise. As a condition to exercising the rights
 *                    and licenses granted hereunder, each Recipient
 *                    hereby assumes sole responsibility to secure any
 *                    other intellectual property rights needed, if any.
 *
 *                    For example, if a third party patent license is
 *                    required to allow Recipient to distribute the
 *                    Program, it is Recipient's responsibility to
 *                    acquire that license before distributing the
 *                    Program.
 *
 *                    d) Each Contributor represents that to its
 *                    knowledge it has sufficient copyright rights in its
 *                    Contribution, if any, to grant the copyright
 *                    license set forth in this Agreement.
 *
 *              3. REQUIREMENTS
 *
 *              A Contributor may choose to distribute the Program in
 *              object code form under its own license agreement, provided
 *              that:
 *                    a) it complies with the terms and conditions of
 *                    this Agreement; and
 *
 *                    b) its license agreement:
 *                    i) effectively disclaims on behalf of all
 *                    Contributors all warranties and conditions, express
 *                    and implied, including warranties or conditions of
 *                    title and no - infringement, and implied warranties
 *                    or conditions of merchantability and fitness for a
 *                    particular purpose;
 *
 *                    ii) effectively excludes on behalf of all
 *                    Contributors all liability for damages, including
 *                    direct, indirect, special, incidental and
 *                    consequential damages, such as lost profits;
 *
 *                    iii) states that any provisions which differ from
 *                    this Agreement are offered by that Contributor
 *                    alone and not by any other party; and
 *
 *                    iv) states that source code for the Program is
 *                    available from such Contributor, and informs
 *                    licensees how to obtain it in a reasonable manner
 *                    on or through a medium customarily used for
 *                    software exchange.
 *
 *              When the Program is made available in source code form:
 *                    a) it must be made available under this Agreement;
 *                    and
 *                    b) a copy of this Agreement must be included with
 *                    each copy of the Program.
 *
 *              Contributors may not remove or alter any copyright notices
 *              contained within the Program.
 *
 *              Each Contributor must identify itself as the originator of
 *              its Contribution, if any, in a manner that reasonably
 *              allows subsequent Recipients to identify the originator of
 *              the Contribution.
 *
 *
 *              4. COMMERCIAL DISTRIBUTION
 *
 *              Commercial distributors of software may accept certain
 *              responsibilities with respect to end users, business
 *              partners and the like. While this license is intended to
 *              facilitate the commercial use of the Program, the
 *              Contributor who includes the Program in a commercial
 *              product offering should do so in a manner which does not
 *              create potential liability for other Contributors.
 *              Therefore, if a Contributor includes the Program in a
 *              commercial product offering, such Contributor ("Commercial
 *              Contributor") hereby agrees to defend and indemnify every
 *              other Contributor ("Indemnified Contributor") against any
 *              losses, damages and costs (collectively "Losses") arising
 *              from claims, lawsuits and other legal actions brought by a
 *              third party against the Indemnified Contributor to the
 *              extent caused by the acts or omissions of such Commercial
 *              Contributor in connection with its distribution of the
 *              Program in a commercial product offering. The obligations
 *              in this section do not apply to any claims or Losses
 *              relating to any actual or alleged intellectual property
 *              infringement. In order to qualify, an Indemnified
 *              Contributor must: a) promptly notify the Commercial
 *              Contributor in writing of such claim, and b) allow the
 *              Commercial Contributor to control, and cooperate with the
 *              Commercial Contributor in, the defense and any related
 *              settlement negotiations. The Indemnified Contributor may
 *              participate in any such claim at its own expense.
 *
 *
 *              For example, a Contributor might include the Program in a
 *              commercial product offering, Product X. That Contributor
 *              is then a Commercial Contributor. If that Commercial
 *              Contributor then makes performance claims, or offers
 *              warranties related to Product X, those performance claims
 *              and warranties are such Commercial Contributor's
 *              responsibility alone. Under this section, the Commercial
 *              Contributor would have to defend claims against the other
 *              Contributors related to those performance claims and
 *              warranties, and if a court requires any other Contributor
 *              to pay any damages as a result, the Commercial Contributor
 *              must pay those damages.
 *
 *
 *              5. NO WARRANTY
 *
 *              EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, THE
 *              PROGRAM IS PROVIDED ON AN "AS IS" BASIS, WITHOUT
 *              WARRANTIES OR CONDITIONS OF ANY KIND, EITHER EXPRESS OR
 *              IMPLIED INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OR
 *              CONDITIONS OF TITLE, NO - INFRINGEMENT, MERCHANTABILITY OR
 *              FITNESS FOR A PARTICULAR PURPOSE. Each Recipient is solely
 *              responsible for determining the appropriateness of using
 *              and distributing the Program and assumes all risks
 *              associated with its exercise of rights under this
 *              Agreement, including but not limited to the risks and
 *              costs of program errors, compliance with applicable laws,
 *              damage to or loss of data, programs or equipment, and
 *              unavailability or interruption of operations.
 *
 *              6. DISCLAIMER OF LIABILITY
 *              EXCEPT AS EXPRESSLY SET FORTH IN THIS AGREEMENT, NEITHER
 *              RECIPIENT NOR ANY CONTRIBUTORS SHALL HAVE ANY LIABILITY
 *              FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 *              OR CONSEQUENTIAL DAMAGES (INCLUDING WITHOUT LIMITATION
 *              LOST PROFITS), HOWEVER CAUSED AND ON ANY THEORY OF
 *              LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 *              (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 *              OF THE USE OR DISTRIBUTION OF THE PROGRAM OR THE EXERCISE
 *              OF ANY RIGHTS GRANTED HEREUNDER, EVEN IF ADVISED OF THE
 *              POSSIBILITY OF SUCH DAMAGES.
 *
 *              7. GENERAL
 *
 *              If any provision of this Agreement is invalid or
 *              unenforceable under applicable law, it shall not affect
 *              the validity or enforceability of the remainder of the
 *              terms of this Agreement, and without further action by the
 *              parties hereto, such provision shall be reformed to the
 *              minimum extent necessary to make such provision valid and
 *              enforceable.
 *
 *
 *              If Recipient institutes patent litigation against a
 *              Contributor with respect to a patent applicable to
 *              software (including a cros - claim or counterclaim in a
 *              lawsuit), then any patent licenses granted by that
 *              Contributor to such Recipient under this Agreement shall
 *              terminate as of the date such litigation is filed. In
 *              addition, If Recipient institutes patent litigation
 *              against any entity (including a cros - claim or
 *              counterclaim in a lawsuit) alleging that the Program
 *              itself (excluding combinations of the Program with other
 *              software or hardware) infringes such Recipient's
 *              patent(s), then such Recipient's rights granted under
 *              Section 2(b) shall terminate as of the date such
 *              litigation is filed.
 *
 *              All Recipient's rights under this Agreement shall
 *              terminate if it fails to comply with any of the material
 *              terms or conditions of this Agreement and does not cure
 *              such failure in a reasonable period of time after becoming
 *              aware of such noncompliance. If all Recipient's rights
 *              under this Agreement terminate, Recipient agrees to cease
 *              use and distribution of the Program as soon as reasonably
 *              practicable. However, Recipient's obligations under this
 *              Agreement and any licenses granted by Recipient relating
 *              to the Program shall continue and survive.
 *
 *              Everyone is permitted to copy and distribute copies of
 *              this Agreement, but in order to avoid inconsistency the
 *              Agreement is copyrighted and may only be modified in the
 *              following manner. The Agreement Steward reserves the right
 *              to publish new versions (including revisions) of this
 *              Agreement from time to time. No one other than the
 *              Agreement Steward has the right to modify this Agreement.
 *
 *              IBM is the initial Agreement Steward. IBM may assign the
 *              responsibility to serve as the Agreement Steward to a
 *              suitable separate entity. Each new version of the
 *              Agreement will be given a distinguishing version number.
 *              The Program (including Contributions) may always be
 *              distributed subject to the version of the Agreement under
 *              which it was received. In addition, after a new version of
 *              the Agreement is published, Contributor may elect to
 *              distribute the Program (including its Contributions) under
 *              the new version. Except as expressly stated in Sections
 *              2(a) and 2(b) above, Recipient receives no rights or
 *              licenses to the intellectual property of any Contributor
 *              under this Agreement, whether expressly, by implication,
 *              estoppel or otherwise. All rights in the Program not
 *              expressly granted under this Agreement are reserved.
 *
 *
 *              This Agreement is governed by the laws of the State of New
 *              York and the intellectual property laws of the United
 *              States of America. No party to this Agreement will bring a
 *              legal action under this Agreement more than one year after
 *              the cause of action arose. Each party waives its rights to
 *              a jury trial in any resulting litigation.
 *
 *
 *
 * (C) COPYRIGHT International Business Machines Corp. 2001, 2002
 */
/*
 * Copyright 2009 Sun Microsystems, Inc.  All rights reserved.
 * Use is subject to license terms.
 */
#include "tpmtok_int.h"

CK_ULONG
ber_encode_INTEGER(CK_BBOOL    length_only,
        CK_BYTE  ** ber_int,
        CK_ULONG  * ber_int_len,
        CK_BYTE   * data,
        CK_ULONG    data_len)
{
        CK_BYTE   *buf = NULL;
        CK_ULONG   len;

        /*
         * if data_len < 127 use short form length id
         * if data_len < 256 use long form length id with
         * byte length field
         * if data_len < 65536 use long form length id with
         * byte length field
         * if data_len < 16777216 use long form length id
         * with byte length field
         */
        if (data_len < 128)
                len = 1 + 1 + data_len;
        else if (data_len < 256)
                len = 1 + (1 + 1) + data_len;
        else if (data_len < (1 << 16))
                len = 1 + (1 + 2) + data_len;
        else if (data_len < (1 << 24))
                len = 1 + (1 + 3) + data_len;
        else
                return (CKR_FUNCTION_FAILED);

        if (length_only == TRUE) {
                *ber_int_len = len;
                return (CKR_OK);
        }

        buf = (CK_BYTE *)malloc(len);
        if (! buf) {
                return (CKR_HOST_MEMORY);
        }
        if (data_len < 128) {
                buf[0] = 0x02;
                buf[1] = data_len;
                (void) memcpy(&buf[2], data, data_len);

                *ber_int_len = len;
                *ber_int = buf;
                return (CKR_OK);
        }

        if (data_len < 256) {
                buf[0] = 0x02;
                buf[1] = 0x81;
                buf[2] = data_len;
                (void) memcpy(&buf[3], data, data_len);

                *ber_int_len = len;
                *ber_int = buf;
                return (CKR_OK);
        }

        if (data_len < (1 << 16)) {
                buf[0] = 0x02;
                buf[1] = 0x82;
                buf[2] = (data_len >> 8) & 0xFF;
                buf[3] = (data_len) & 0xFF;
                (void) memcpy(&buf[4], data, data_len);

                *ber_int_len = len;
                *ber_int = buf;
                return (CKR_OK);
        }

        if (data_len < (1 << 24)) {
                buf[0] = 0x02;
                buf[1] = 0x83;
                buf[2] = (data_len >> 16) & 0xFF;
                buf[3] = (data_len >>  8) & 0xFF;
                buf[4] = (data_len) & 0xFF;
                (void) memcpy(&buf[5], data, data_len);

                *ber_int_len = len;
                *ber_int = buf;
                return (CKR_OK);
        }

        free(buf);
        return (CKR_FUNCTION_FAILED);
}

CK_RV
ber_decode_INTEGER(CK_BYTE   * ber_int,
        CK_BYTE  ** data,
        CK_ULONG  * data_len,
        CK_ULONG  * field_len)
{
        CK_ULONG  len, length_octets;

        if (! ber_int) {
                return (CKR_FUNCTION_FAILED);
        }
        if (ber_int[0] != 0x02) {
                return (CKR_FUNCTION_FAILED);
        }
        if ((ber_int[1] & 0x80) == 0) {
                len = ber_int[1] & 0x7F;

                *data = &ber_int[2];
                *data_len  = len;
                *field_len = 1 + 1 + len;
                return (CKR_OK);
        }

        length_octets = ber_int[1] & 0x7F;

        if (length_octets == 1) {
                len = ber_int[2];

                *data = &ber_int[3];
                *data_len  = len;
                *field_len = 1 + (1 + 1) + len;
                return (CKR_OK);
        }

        if (length_octets == 2) {
                len = ber_int[2];
                len = len << 8;
                len |= ber_int[3];

                *data = &ber_int[4];
                *data_len  = len;
                *field_len = 1 + (1 + 2) + len;
                return (CKR_OK);
        }

        if (length_octets == 3) {
                len = ber_int[2];
                len = len << 8;
                len |= ber_int[3];
                len = len << 8;
                len |= ber_int[4];

                *data = &ber_int[5];
                *data_len  = len;
                *field_len = 1 + (1 + 3) + len;
                return (CKR_OK);
        }

        return (CKR_FUNCTION_FAILED);
}

CK_RV
ber_encode_OCTET_STRING(CK_BBOOL    length_only,
CK_BYTE  ** str,
CK_ULONG  * str_len,
CK_BYTE   * data,
CK_ULONG    data_len)
{
        CK_BYTE   *buf = NULL;
        CK_ULONG   len;


        /*
         * if data_len < 128 use shor - form length id
         * if data_len < 256 use lon - form length id with - byte length field
         * if data_len < 65536 use lon - form length id with - byte length field
         */
        if (data_len < 128)
                len = 1 + 1 + data_len;
        else if (data_len < 256)
                len = 1 + (1 + 1) + data_len;
        else if (data_len < (1 << 16))
                len = 1 + (1 + 2) + data_len;
        else if (data_len < (1 << 24))
                len = 1 + (1 + 3) + data_len;
        else
                return (CKR_FUNCTION_FAILED);

        if (length_only == TRUE) {
                *str_len = len;
                return (CKR_OK);
        }

        buf = (CK_BYTE *)malloc(len);
        if (! buf) {
                return (CKR_HOST_MEMORY);
        }

        if (data_len < 128) {
                buf[0] = 0x04; /* primitive, OCTET STRING */
                buf[1] = data_len;
                (void) memcpy(&buf[2], data, data_len);

                *str_len = len;
                *str = buf;
                return (CKR_OK);
        }

        if (data_len < 256) {
                buf[0] = 0x04;  /* primitive, OCTET STRING */
                buf[1] = 0x81;  /* length header -- 1 length octets */
                buf[2] = data_len;

                (void) memcpy(&buf[3], data, data_len);

                *str_len = len;
                *str = buf;
                return (CKR_OK);
        }

        if (data_len < (1 << 16)) {
                buf[0] = 0x04;  /* primitive, OCTET STRING */
                buf[1] = 0x82;  /* length header -- 2 length octets */
                buf[2] = (data_len >> 8) & 0xFF;
                buf[3] = (data_len) & 0xFF;

                (void) memcpy(&buf[4], data, data_len);

                *str_len = len;
                *str = buf;
                return (CKR_OK);
        }

        if (data_len < (1 << 24)) {
                buf[0] = 0x04;  /* primitive, OCTET STRING */
                buf[1] = 0x83;  /* length header -- 3 length octets */
                buf[2] = (data_len >> 16) & 0xFF;
                buf[3] = (data_len >>  8) & 0xFF;
                buf[4] = (data_len) & 0xFF;

                (void) memcpy(&buf[5], data, data_len);

                *str_len = len;
                *str = buf;
                return (CKR_OK);
        }

        free(buf);
        return (CKR_FUNCTION_FAILED);
}

CK_RV
ber_decode_OCTET_STRING(CK_BYTE  * str,
        CK_BYTE ** data,
        CK_ULONG * data_len,
        CK_ULONG * field_len)
{
        CK_ULONG  len, length_octets;

        if (! str) {
                return (CKR_FUNCTION_FAILED);
        }
        if (str[0] != 0x04) {
                return (CKR_FUNCTION_FAILED);
        }
        if ((str[1] & 0x80) == 0) {
                len = str[1] & 0x7F;

                *data = &str[2];
                *data_len  = len;
                *field_len = 1 + (1) + len;
                return (CKR_OK);
        }

        length_octets = str[1] & 0x7F;

        if (length_octets == 1) {
                len = str[2];

                *data = &str[3];
                *data_len  = len;
                *field_len = 1 + (1 + 1) + len;
                return (CKR_OK);
        }

        if (length_octets == 2) {
                len = str[2];
                len = len << 8;
                len |= str[3];

                *data = &str[4];
                *data_len  = len;
                *field_len = 1 + (1 + 2) + len;
                return (CKR_OK);
        }

        if (length_octets == 3) {
                len = str[2];
                len = len << 8;
                len |= str[3];
                len = len << 8;
                len |= str[4];

                *data = &str[5];
                *data_len  = len;
                *field_len = 1 + (1 + 3) + len;
                return (CKR_OK);
        }

        /* > 3 length octets implies a length > 16MB */
        return (CKR_FUNCTION_FAILED);
}

CK_RV
ber_encode_SEQUENCE(CK_BBOOL    length_only,
        CK_BYTE  ** seq,
        CK_ULONG  * seq_len,
        CK_BYTE   * data,
        CK_ULONG    data_len)
{
        CK_BYTE   *buf = NULL;
        CK_ULONG   len;

        if (data_len < 128)
                len = 1 + 1 + data_len;
        else if (data_len < 256)
                len = 1 + (1 + 1) + data_len;
        else if (data_len < (1 << 16))
                len = 1 + (1 + 2) + data_len;
        else if (data_len < (1 << 24))
                len = 1 + (1 + 3) + data_len;
        else
                return (CKR_FUNCTION_FAILED);

        if (length_only == TRUE) {
                *seq_len = len;
                return (CKR_OK);
        }

        buf = (CK_BYTE *)malloc(len);
        if (! buf) {
                return (CKR_HOST_MEMORY);
        }

        if (data_len < 128) {
                buf[0] = 0x30;  /* constructed, SEQUENCE */
                buf[1] = data_len;
                (void) memcpy(&buf[2], data, data_len);

                *seq_len = len;
                *seq = buf;
                return (CKR_OK);
        }

        if (data_len < 256) {
                buf[0] = 0x30;  /* constructed, SEQUENCE */
                buf[1] = 0x81;  /* length header -- 1 length octets */
                buf[2] = data_len;

                (void) memcpy(&buf[3], data, data_len);

                *seq_len = len;
                *seq = buf;
                return (CKR_OK);
        }

        if (data_len < (1 << 16)) {
                buf[0] = 0x30;  /* constructed, SEQUENCE */
                buf[1] = 0x82;  /* length header -- 2 length octets */
                buf[2] = (data_len >> 8) & 0xFF;
                buf[3] = (data_len) & 0xFF;

                (void) memcpy(&buf[4], data, data_len);

                *seq_len = len;
                *seq = buf;
                return (CKR_OK);
        }

        if (data_len < (1 << 24)) {
                buf[0] = 0x30;  /* constructed, SEQUENCE */
                buf[1] = 0x83;  /* length header -- 3 length octets */
                buf[2] = (data_len >> 16) & 0xFF;
                buf[3] = (data_len >>  8) & 0xFF;
                buf[4] = (data_len) & 0xFF;

                (void) memcpy(&buf[5], data, data_len);

                *seq_len = len;
                *seq = buf;
                return (CKR_OK);
        }

        return (CKR_FUNCTION_FAILED);
}

CK_RV
ber_decode_SEQUENCE(CK_BYTE  * seq,
        CK_BYTE ** data,
        CK_ULONG * data_len,
        CK_ULONG * field_len)
{
        CK_ULONG  len, length_octets;

        if (! seq) {
                return (CKR_FUNCTION_FAILED);
        }
        if (seq[0] != 0x30) {
                return (CKR_FUNCTION_FAILED);
        }
        if ((seq[1] & 0x80) == 0) {
                len = seq[1] & 0x7F;

                *data = &seq[2];
                *data_len  = len;
                *field_len = 1 + (1) + len;
                return (CKR_OK);
        }

        length_octets = seq[1] & 0x7F;

        if (length_octets == 1) {
                len = seq[2];

                *data = &seq[3];
                *data_len  = len;
                *field_len = 1 + (1 + 1) + len;
                return (CKR_OK);
        }

        if (length_octets == 2) {
                len = seq[2];
                len = len << 8;
                len |= seq[3];

                *data = &seq[4];
                *data_len  = len;
                *field_len = 1 + (1 + 2) + len;
                return (CKR_OK);
        }

        if (length_octets == 3) {
                len = seq[2];
                len = len << 8;
                len |= seq[3];
                len = len << 8;
                len |= seq[4];

                *data = &seq[5];
                *data_len  = len;
                *field_len = 1 + (1 + 3) + len;
                return (CKR_OK);
        }

        return (CKR_FUNCTION_FAILED);
}

/*
 *
 * PrivateKeyInfo ::= SEQUENCE {
 *    version  Version  -- always '0' for now
 *    privateKeyAlgorithm PrivateKeyAlgorithmIdentifier
 *    privateKey  PrivateKey
 *    attributes
 * }
 */
CK_RV
ber_encode_PrivateKeyInfo(CK_BBOOL    length_only,
CK_BYTE  ** data,
CK_ULONG  * data_len,
CK_BYTE   * algorithm_id,
CK_ULONG    algorithm_id_len,
CK_BYTE   * priv_key,
CK_ULONG    priv_key_len)
{
        CK_BYTE  * buf = NULL;
        CK_BYTE  * tmp = NULL;
        CK_BYTE    version[] = { 0 };
        CK_BYTE    attrib[]  = { 0x05, 0x00 };
        CK_ULONG   len, total;
        CK_RV rc;

        len = 0;

        rc = ber_encode_INTEGER(TRUE, NULL, &total, version, sizeof (version));
        if (rc != CKR_OK) {
                return (rc);
        }
        else
        len += total;

        len += algorithm_id_len;

        rc = ber_encode_OCTET_STRING(TRUE, NULL, &total,
            priv_key, priv_key_len);

        if (rc != CKR_OK)
                return (rc);

        len += total;

        /* for this stuff, attributes are always NULL == 05 00 */
        len += sizeof (attrib);

        if (length_only == TRUE) {
                rc = ber_encode_SEQUENCE(TRUE, NULL, &total, NULL, len);

                if (rc == CKR_OK)
                        *data_len = total;
                return (rc);
        }

        buf = (CK_BYTE *)malloc(len);
        if (! buf) {
                return (CKR_HOST_MEMORY);
        }
        len = 0;
        rc = ber_encode_INTEGER(FALSE, &tmp, &total, version, sizeof (version));
        if (rc != CKR_OK) {
                goto error;
        }
        (void) memcpy(buf + len, tmp, total);
        len += total;
        free(tmp);

        (void) memcpy(buf + len, algorithm_id, algorithm_id_len);
        len += algorithm_id_len;

        rc = ber_encode_OCTET_STRING(FALSE, &tmp, &total,
            priv_key, priv_key_len);
        if (rc != CKR_OK) {
                goto error;
        }
        (void) memcpy(buf + len, tmp, total);
        len += total;
        free(tmp);

        (void) memcpy(buf + len, attrib, sizeof (attrib));
        len += sizeof (attrib);

        rc = ber_encode_SEQUENCE(FALSE, data, data_len, buf, len);

error:
        free(buf);
        return (rc);
}

CK_RV
ber_decode_PrivateKeyInfo(CK_BYTE   * data,
        CK_ULONG    data_len,
        CK_BYTE  ** algorithm,
        CK_ULONG  * alg_len,
        CK_BYTE  ** priv_key)
{
        CK_BYTE  *buf = NULL;
        CK_BYTE  *alg = NULL;
        CK_BYTE  *ver = NULL;
        CK_ULONG  buf_len, offset, len, field_len;
        CK_RV rc;

        if (! data || (data_len == 0)) {
                return (CKR_FUNCTION_FAILED);
        }
        rc = ber_decode_SEQUENCE(data, &buf, &buf_len, &field_len);
        if (rc != CKR_OK) {
                return (rc);
        }
        offset = 0;
        rc = ber_decode_INTEGER(buf + offset, &ver, &len, &field_len);
        if (rc != CKR_OK) {
                return (rc);
        }
        offset += field_len;

        rc = ber_decode_SEQUENCE(buf + offset, &alg, &len, &field_len);
        if (rc != CKR_OK) {
                return (rc);
        }
        *algorithm = alg;
        *alg_len   = len;

        rc = ber_decode_OCTET_STRING(alg + len, priv_key, &buf_len, &field_len);
        return (rc);
}

/*
 * RSAPrivateKey ::= SEQUENCE {
 *    version  Version  -- always '0' for now
 *    modulus  INTEGER
 *    publicExponent  INTEGER
 *    privateExponent INTEGER
 *    prime1  INTEGER
 *    prime2  INTEGER
 *    exponent1  INTEGER
 *    exponent2  INTEGER
 *    coefficient INTEGER
 * }
 */
CK_RV
ber_encode_RSAPrivateKey(CK_BBOOL    length_only,
        CK_BYTE  ** data,
        CK_ULONG  * data_len,
        CK_ATTRIBUTE * modulus,
        CK_ATTRIBUTE * publ_exp,
        CK_ATTRIBUTE * priv_exp,
        CK_ATTRIBUTE * prime1,
        CK_ATTRIBUTE * prime2,
        CK_ATTRIBUTE * exponent1,
        CK_ATTRIBUTE * exponent2,
        CK_ATTRIBUTE * coeff)
{
        CK_BYTE   *buf = NULL;
        CK_BYTE   *buf2 = NULL;
        CK_ULONG   len, offset;
        CK_BYTE    version[] = { 0 };
        CK_RV rc;

        offset = 0;
        rc = 0;

        rc |= ber_encode_INTEGER(TRUE, NULL, &len, NULL,
            sizeof (version));
        offset += len;
        rc |= ber_encode_INTEGER(TRUE, NULL, &len, NULL,
            modulus->ulValueLen);
        offset += len;
        rc |= ber_encode_INTEGER(TRUE, NULL, &len, NULL,
            publ_exp->ulValueLen);
        offset += len;
        rc |= ber_encode_INTEGER(TRUE, NULL, &len, NULL,
            priv_exp->ulValueLen);
        offset += len;
        rc |= ber_encode_INTEGER(TRUE, NULL, &len, NULL,
            prime1->ulValueLen);
        offset += len;
        rc |= ber_encode_INTEGER(TRUE, NULL, &len, NULL,
            prime2->ulValueLen);
        offset += len;
        rc |= ber_encode_INTEGER(TRUE, NULL, &len, NULL,
            exponent1->ulValueLen);
        offset += len;
        rc |= ber_encode_INTEGER(TRUE, NULL, &len, NULL,
            exponent2->ulValueLen);
        offset += len;
        rc |= ber_encode_INTEGER(TRUE, NULL, &len, NULL,
            coeff->ulValueLen);
        offset += len;

        if (rc != CKR_OK) {
                return (CKR_FUNCTION_FAILED);
        }
        if (length_only == TRUE) {
                rc = ber_encode_SEQUENCE(TRUE, NULL, &len, NULL, offset);
                if (rc != CKR_OK)
                        return (rc);
                rc = ber_encode_PrivateKeyInfo(TRUE,
                    NULL, data_len,
                    NULL, ber_AlgIdRSAEncryptionLen,
                    NULL, len);
                if (rc != CKR_OK)
                        return (rc);
                return (rc);
        }

        buf = (CK_BYTE *)malloc(offset);
        if (! buf) {
                return (CKR_HOST_MEMORY);
        }
        offset = 0;
        rc = 0;

        rc = ber_encode_INTEGER(FALSE, &buf2, &len, version, sizeof (version));
        if (rc != CKR_OK) {
                goto error;
        }
        (void) memcpy(buf + offset, buf2, len);
        offset += len;
        free(buf2);

        rc = ber_encode_INTEGER(FALSE, &buf2, &len,
            (CK_BYTE *)modulus + sizeof (CK_ATTRIBUTE), modulus->ulValueLen);
        if (rc != CKR_OK) {
                goto error;
        }
        (void) memcpy(buf + offset, buf2, len);
        offset += len;
        free(buf2);

        rc = ber_encode_INTEGER(FALSE, &buf2, &len,
            (CK_BYTE *)publ_exp + sizeof (CK_ATTRIBUTE), publ_exp->ulValueLen);
        if (rc != CKR_OK) {
                goto error;
        }
        (void) memcpy(buf + offset, buf2, len);
        offset += len;
        free(buf2);

        rc = ber_encode_INTEGER(FALSE, &buf2, &len,
            (CK_BYTE *)priv_exp  + sizeof (CK_ATTRIBUTE),
            priv_exp->ulValueLen);
        if (rc != CKR_OK) {
                goto error;
        }
        (void) memcpy(buf + offset, buf2, len);
        offset += len;
        free(buf2);

        rc = ber_encode_INTEGER(FALSE, &buf2, &len,
            (CK_BYTE *)prime1 + sizeof (CK_ATTRIBUTE), prime1->ulValueLen);
        if (rc != CKR_OK) {
                goto error;
        }
        (void) memcpy(buf + offset, buf2, len);
        offset += len;
        free(buf2);

        rc = ber_encode_INTEGER(FALSE, &buf2, &len,
            (CK_BYTE *)prime2 + sizeof (CK_ATTRIBUTE), prime2->ulValueLen);
        if (rc != CKR_OK) {
                goto error;
        }
        (void) memcpy(buf + offset, buf2, len);
        offset += len;
        free(buf2);

        rc = ber_encode_INTEGER(FALSE, &buf2, &len,
            (CK_BYTE *)exponent1 + sizeof (CK_ATTRIBUTE),
            exponent1->ulValueLen);
        if (rc != CKR_OK) {
                goto error;
        }
        (void) memcpy(buf + offset, buf2, len);
        offset += len;
        free(buf2);

        rc = ber_encode_INTEGER(FALSE, &buf2, &len,
            (CK_BYTE *)exponent2 + sizeof (CK_ATTRIBUTE),
            exponent2->ulValueLen);
        if (rc != CKR_OK) {
                goto error;
        }
        (void) memcpy(buf + offset, buf2, len);
        offset += len;
        free(buf2);

        rc = ber_encode_INTEGER(FALSE, &buf2, &len,
            (CK_BYTE *)coeff + sizeof (CK_ATTRIBUTE), coeff->ulValueLen);
        if (rc != CKR_OK) {
                goto error;
        }
        (void) memcpy(buf + offset, buf2, len);
        offset += len;
        free(buf2);

        rc = ber_encode_SEQUENCE(FALSE, &buf2, &len, buf, offset);
        if (rc != CKR_OK) {
                goto error;
        }
        rc = ber_encode_PrivateKeyInfo(FALSE,
            data, data_len,
            ber_AlgIdRSAEncryption, ber_AlgIdRSAEncryptionLen,
            buf2, len);
error:
        if (buf2) free(buf2);
        if (buf)  free(buf);
        return (rc);
}

CK_RV
ber_decode_RSAPrivateKey(CK_BYTE    * data,
        CK_ULONG data_len,
        CK_ATTRIBUTE ** modulus,
        CK_ATTRIBUTE ** publ_exp,
        CK_ATTRIBUTE ** priv_exp,
        CK_ATTRIBUTE ** prime1,
        CK_ATTRIBUTE ** prime2,
        CK_ATTRIBUTE ** exponent1,
        CK_ATTRIBUTE ** exponent2,
        CK_ATTRIBUTE ** coeff)
{
        CK_ATTRIBUTE *n_attr = NULL;
        CK_ATTRIBUTE *e_attr = NULL;
        CK_ATTRIBUTE *d_attr = NULL;
        CK_ATTRIBUTE *p_attr = NULL;
        CK_ATTRIBUTE *q_attr = NULL;
        CK_ATTRIBUTE *e1_attr = NULL;
        CK_ATTRIBUTE *e2_attr = NULL;
        CK_ATTRIBUTE *coeff_attr = NULL;

        CK_BYTE  *alg = NULL;
        CK_BYTE  *rsa_priv_key = NULL;
        CK_BYTE  *buf = NULL;
        CK_BYTE  *tmp = NULL;
        CK_ULONG offset, buf_len, field_len, len;
        CK_RV rc;

        rc = ber_decode_PrivateKeyInfo(data, data_len, &alg,
            &len, &rsa_priv_key);
        if (rc != CKR_OK) {
                return (rc);
        }
        if (memcmp(alg, ber_rsaEncryption, ber_rsaEncryptionLen) != 0) {
                return (CKR_FUNCTION_FAILED);
        }
        rc = ber_decode_SEQUENCE(rsa_priv_key, &buf, &buf_len, &field_len);
        if (rc != CKR_OK)
                return (rc);
        offset = 0;

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        }
        offset += field_len;

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        }
        offset += field_len;

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        }
        offset += field_len;

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        }
        offset += field_len;

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        }
        offset += field_len;

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        }
        offset += field_len;

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        }
        offset += field_len;

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        }
        offset += field_len;

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        }
        offset += field_len;

        if (offset > buf_len) {
                return (CKR_FUNCTION_FAILED);
        }

        offset = 0;

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        }
        offset += field_len;

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        } else {
                rc = build_attribute(CKA_MODULUS, tmp, len, &n_attr);
                if (rc != CKR_OK) {
                        goto cleanup;
                }
                offset += field_len;
        }

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        } else {
                rc = build_attribute(CKA_PUBLIC_EXPONENT, tmp, len, &e_attr);
                if (rc != CKR_OK) {
                        goto cleanup;
                }
                offset += field_len;
        }

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        } else {
                rc = build_attribute(CKA_PRIVATE_EXPONENT, tmp, len, &d_attr);
                if (rc != CKR_OK) {
                        goto cleanup;
                }
                offset += field_len;
        }

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        } else {
                rc = build_attribute(CKA_PRIME_1, tmp, len, &p_attr);
                if (rc != CKR_OK) {
                        goto cleanup;
                }
                offset += field_len;
        }

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        } else {
                rc = build_attribute(CKA_PRIME_2, tmp, len, &q_attr);
                if (rc != CKR_OK) {
                        goto cleanup;
                }
                offset += field_len;
        }

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        } else {
                rc = build_attribute(CKA_EXPONENT_1, tmp, len, &e1_attr);
                if (rc != CKR_OK) {
                        goto cleanup;
                }
                offset += field_len;
        }

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        } else {
                rc = build_attribute(CKA_EXPONENT_2, tmp, len, &e2_attr);
                if (rc != CKR_OK) {
                        goto cleanup;
                }
                offset += field_len;
        }

        rc = ber_decode_INTEGER(buf + offset, &tmp, &len, &field_len);
        if (rc != CKR_OK) {
                goto cleanup;
        } else {
                rc = build_attribute(CKA_COEFFICIENT, tmp, len, &coeff_attr);
                if (rc != CKR_OK) {
                        goto cleanup;
                }
                offset += len;
        }

        *modulus   = n_attr;
        *publ_exp  = e_attr;
        *priv_exp  = d_attr;
        *prime1 = p_attr;
        *prime2 = q_attr;
        *exponent1 = e1_attr;
        *exponent2 = e2_attr;
        *coeff = coeff_attr;

        return (CKR_OK);

cleanup:
        if (n_attr) free(n_attr);
        if (e_attr) free(e_attr);
        if (d_attr) free(d_attr);
        if (p_attr) free(p_attr);
        if (q_attr) free(q_attr);
        if (e1_attr) free(e1_attr);
        if (e2_attr) free(e2_attr);
        if (coeff_attr) free(coeff_attr);

        return (rc);
}