#include "sm.h"
#include "utils.h"
#include <opensc/asn1.h>
#include <opensc/log.h>
#include <string.h>

static const struct sc_asn1_entry c_sm_capdu[] = {
    { "Padding-content indicator followed by cryptogram",
        SC_ASN1_INTEGER     , 0x87, SC_ASN1_OPTIONAL|SC_ASN1_UNSIGNED, NULL, NULL },
    { "Protected Le",
        SC_ASN1_INTEGER     , 0x97, SC_ASN1_OPTIONAL|SC_ASN1_UNSIGNED, NULL, NULL },
    { "Cryptographic Checksum",
        SC_ASN1_OCTET_STRING, 0x8E,                                 0, NULL, NULL },
    { NULL , 0 , 0 , 0 , NULL , NULL }
};

static const struct sc_asn1_entry c_sm_rapdu[] = {
    { "Padding-content indicator followed by cryptogram" ,
        SC_ASN1_INTEGER     , 0x87, SC_ASN1_OPTIONAL|SC_ASN1_UNSIGNED, NULL, NULL },
    { "Processing Status",
        SC_ASN1_INTEGER     , 0x99, SC_ASN1_OPTIONAL|SC_ASN1_UNSIGNED, NULL, NULL },
    { "Cryptographic Checksum",
        SC_ASN1_OCTET_STRING, 0x8E,                                 0, NULL, NULL },
    { NULL, 0, 0, 0, NULL, NULL }
};

BUF_MEM *add_padding(const struct sm_ctx *ctx, const char *data, size_t datalen)
{
    BUF_MEM *tmp = BUF_MEM_create_init(data, datalen);
    BUF_MEM *padded = NULL;

    switch (ctx->padding_indicator) {
        case SM_NO_PADDING:
            padded = tmp;
            tmp = NULL;
            break;
        case SM_ISO_PADDING:
            padded = add_iso_pad(tmp, EVP_CIPHER_block_size(ctx->cipher));
            if (tmp) {
                OPENSSL_cleanse(tmp->data, tmp->max);
                BUF_MEM_free(tmp);
            }
            break;
        default:
            return NULL;
    }

    return padded;
}

u8 * format_le(size_t le_len, size_t le, struct sc_asn1_entry *le_entry)
{
    u8 * lebuf = malloc(le_len);
    if (lebuf) {
        switch (le_len) {
            case 1:
                lebuf[0] = le;
                break;
            case 2:
                lebuf[0] = htons(le) >> 8;
                lebuf[1] = htons(le) & 0xff;
                break;
            case 3:
                lebuf[0] = 0x00;
                lebuf[1] = htons(le) >> 8;
                lebuf[2] = htons(le) & 0xff;
                break;
            default:
                free(lebuf);
                return NULL;
        }

        sc_format_asn1_entry(le_entry, lebuf, &le_len, SC_ASN1_PRESENT);
    }

    return lebuf;
}

BUF_MEM * prefix_buf(u8 prefix, BUF_MEM *buf)
{
    if (!buf) return NULL;
    BUF_MEM *cat = BUF_MEM_create(buf->length + 1);
    if (cat) {
        cat->data[0] = prefix;
        memcpy(cat->data + 1, buf->data, buf->length);
        cat->length = buf->length + 1;
    }
    return cat;
}

BUF_MEM * format_data(const struct sm_ctx *ctx, const u8 *data, size_t datalen,
        struct sc_asn1_entry *formatted_encrypted_data_entry)
{
    BUF_MEM *pad_data = add_padding(ctx, (char *) data, datalen);
    BUF_MEM *enc_data = cipher(ctx->cipher_ctx, ctx->cipher,
            ctx->cipher_engine, ctx->key_enc, ctx->iv, 1, pad_data, 1);

    if (pad_data) {
        OPENSSL_cleanse(pad_data->data, pad_data->max);
        BUF_MEM_free(pad_data);
    }

    BUF_MEM *indicator_encdata = prefix_buf(ctx->padding_indicator, enc_data);

    if (enc_data) {
        BUF_MEM_free(enc_data);
    }

    if (indicator_encdata)
        sc_format_asn1_entry(formatted_encrypted_data_entry,
                indicator_encdata->data, &(indicator_encdata->length),
                SC_ASN1_PRESENT);
    return indicator_encdata;
}

int sm_encrypt(const struct sm_ctx *ctx, sc_card_t *card, sc_apdu_t *apdu,
        sc_apdu_t *sm_apdu)
{
    struct sc_asn1_entry sm_capdu[4];
    u8 *le = NULL;
    size_t oldlen;
    BUF_MEM *formatted_data = NULL, *sm_data = NULL,
            *mac_data = NULL, *mac = NULL, *tmp = NULL;
    char head[4];
    int r;

    if (!apdu || !ctx || !card || !card->slot || !sm_apdu) {
        r = SC_ERROR_INVALID_ARGUMENTS;
        goto err;
    }


    sc_copy_asn1_entry(c_sm_capdu, sm_capdu);

    head[0] = apdu->cla;
    head[1] = apdu->ins;
    head[2] = apdu->p1;
    head[3] = apdu->p2;
    mac_data = add_padding(ctx, head, sizeof head);
    if (!mac_data) {
        r = SC_ERROR_WRONG_PADDING;
        goto err;
    }


    /* get asn1 encoding of le and padding indicator depending on the case of
     * the unsecure command */
    switch (apdu->cse) {
        case SC_APDU_CASE_1:
            break;
	case SC_APDU_CASE_2_SHORT:
            le = format_le(1, apdu->le, sm_capdu + 1);
            if (!le) {
                r = SC_ERROR_WRONG_LENGTH;
                goto err;
            }
            break;
	case SC_APDU_CASE_2_EXT:
            if (card->slot->active_protocol == SC_PROTO_T0) {
                /* T0 extended APDUs look just like short APDUs */
                le = format_le(1, apdu->le, sm_capdu + 1);
                if (!le) {
                    r = SC_ERROR_WRONG_LENGTH;
                    goto err;
                }
            } else {
                /* in case of T1 always use 3 bytes for length */
                le = format_le(3, apdu->le, sm_capdu + 1);
                if (!le) {
                    r = SC_ERROR_WRONG_LENGTH;
                    goto err;
                }
            }
            break;
        case SC_APDU_CASE_3_SHORT:
        case SC_APDU_CASE_3_EXT:
            formatted_data = format_data(ctx, apdu->data, apdu->datalen,
                    sm_capdu + 0);
            if (!formatted_data) {
                r = SC_ERROR_OUT_OF_MEMORY;
                goto err;
            }
            break;
        case SC_APDU_CASE_4_SHORT:
            /* in case of T0 no Le byte is added */
            if (card->slot->active_protocol != SC_PROTO_T0) {
                le = format_le(1, apdu->le, sm_capdu + 1);
                if (!le) {
                    r = SC_ERROR_WRONG_LENGTH;
                    goto err;
                }
            }

            formatted_data = format_data(ctx, apdu->data, apdu->datalen,
                    sm_capdu + 0);
            if (!formatted_data) {
                r = SC_ERROR_OUT_OF_MEMORY;
                goto err;
            }
            break;
        case SC_APDU_CASE_4_EXT:
            if (card->slot->active_protocol == SC_PROTO_T0) {
                /* again a T0 extended case 4 APDU looks just
                 * like a short APDU, the additional data is
                 * transferred using ENVELOPE and GET RESPONSE */
            } else {
                /* only 2 bytes are use to specify the length of the
                 * expected data */
                le = format_le(2, apdu->le, sm_capdu + 1);
                if (!le) {
                    r = SC_ERROR_WRONG_LENGTH;
                    goto err;
                }
            }

            formatted_data = format_data(ctx, apdu->data, apdu->datalen,
                    sm_capdu + 0);
            if (!formatted_data) {
                r = SC_ERROR_OUT_OF_MEMORY;
                goto err;
            }
            break;
        default:
            r = SC_ERROR_INVALID_DATA;
            goto err;
    }


    tmp = BUF_MEM_new();
    if (!tmp) {
        r = SC_ERROR_OUT_OF_MEMORY;
        goto err;
    }
    r = sc_asn1_encode(card->ctx, sm_capdu, (u8 **) &tmp->data, &tmp->length);
    if (r < 0) {
        goto err;
    }
    oldlen = mac_data->length;
    BUF_MEM_grow(mac_data, oldlen + tmp->length);
    memcpy(mac_data->data + oldlen, tmp->data, tmp->length);
    BUF_MEM_free(tmp);
    tmp = add_padding(ctx, mac_data->data, mac_data->length);
    BUF_MEM_free(mac_data);
    mac_data = hash(ctx->md, ctx->md_ctx, ctx->md_engine, tmp);
    mac = cipher(ctx->cipher_ctx, ctx->cipher, ctx->cipher_engine,
            ctx->key_mac, ctx->iv, 1, mac_data, 1);
    if (!mac) {
        r = SC_ERROR_INTERNAL;
        goto err;
    }
    sc_format_asn1_entry(sm_capdu + 2, mac->data, &(mac->length),
            SC_ASN1_PRESENT);


    /* format SM apdu */
    BUF_MEM_free(sm_data);
    sm_data = BUF_MEM_new();
    if (!sm_data) {
        r = SC_ERROR_OUT_OF_MEMORY;
        goto err;
    }
    r = sc_asn1_encode(card->ctx, sm_capdu, (u8 **) &sm_data->data, &sm_data->length);
    if (r < 0)
        goto err;
    memset(sm_apdu, 0, sizeof *sm_apdu);
    sm_apdu->cla = apdu->cla;
    sm_apdu->ins = apdu->ins;
    sm_apdu->p1 = apdu->p1;
    sm_apdu->p2 = apdu->p2;
    sm_apdu->data = (u8 *) sm_data->data;
    sm_apdu->lc = sm_data->length;
    sm_apdu->le = 0;
    sm_apdu->cse = SC_APDU_CASE_4;

err:
    if (formatted_data) {
        BUF_MEM_free(formatted_data);
    }
    if (tmp) {
        BUF_MEM_free(tmp);
    }
    if (mac_data) {
        BUF_MEM_free(mac_data);
    }
    if (mac) {
        BUF_MEM_free(mac);
    }
    if (le) {
        free(le);
    }

    SC_FUNC_RETURN(card->ctx, SC_LOG_TYPE_ERROR, r);
}