#include "common.h"

#include "b64.h"



static uint8_t BASE64_DECODE_TABLE[] = {

    -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,

    62, // +

    -1, -1, -1,

    63, // /

    52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, // 0-9

    -1, -1, -1, -1, -1, -1, 

    0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, // a-z

    -1, -1, -1, -1, -1, -1, 

    26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51 // A-Z

};



static char BASE64_ENCODE_TABLE[] = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H',

                                'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P',

                                'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X',

                                'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f',

                                'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n',

                                'o', 'p', 'q', 'r', 's', 't', 'u', 'v',

                                'w', 'x', 'y', 'z', '0', '1', '2', '3',

                                '4', '5', '6', '7', '8', '9', '+', '/'};

static int mod_table[] = {0, 2, 1};





TEE_Result 

base64_encode(const uint8_t* data, size_t input_length, uint8_t* output_data, size_t* output_length) 

{

    size_t output_data_space = *output_length;

    *output_length = B64_SIZE(input_length);

    

    if(output_data_space < *output_length) {

        MPOS_AUTH_LOG("%s: output_data_space(%d) is smaller than output_length(%d)", __func__, output_data_space, *output_length);

        return TEE_ERROR_SHORT_BUFFER;

    }



    for (unsigned i = 0, j = 0; i < input_length;) {

        uint32_t octet_a = i < input_length ? data[i++] : 0;

        uint32_t octet_b = i < input_length ? data[i++] : 0;

        uint32_t octet_c = i < input_length ? data[i++] : 0;



        uint32_t triple = (octet_a << 0x10) + (octet_b << 0x08) + octet_c;



        output_data[j++] = BASE64_ENCODE_TABLE[(triple >> (3 * 6)) & 0x3F];

        output_data[j++] = BASE64_ENCODE_TABLE[(triple >> (2 * 6)) & 0x3F];

        output_data[j++] = BASE64_ENCODE_TABLE[(triple >> (1 * 6)) & 0x3F];

        output_data[j++] = BASE64_ENCODE_TABLE[(triple >> (0 * 6)) & 0x3F];

    }



    for (int i = 0; i < mod_table[input_length % 3]; i++) {

        output_data[*output_length - 1 - i] = '=';

    }

    

    output_data[*output_length] = '\0';

    return TEE_SUCCESS;

}





TEE_Result 

base64_decode(const uint8_t* data, size_t input_length, uint8_t* output_data, size_t* output_length) 

{

    if (input_length % 4 != 0) { 

        return TEE_ERROR_BAD_FORMAT; 

        

    }

    

    *output_length = input_length / 4 * 3;

    if (data[input_length - 1] == '=') {

        (*output_length)--;

    }

    if (data[input_length - 2] == '=') {

        (*output_length)--;

    }

    

    for (unsigned i = 0, j = 0; i < input_length;) {

        uint32_t sextet_a = data[i] == '=' ? 0 & i++ : BASE64_DECODE_TABLE[data[i++]];

        uint32_t sextet_b = data[i] == '=' ? 0 & i++ : BASE64_DECODE_TABLE[data[i++]];

        uint32_t sextet_c = data[i] == '=' ? 0 & i++ : BASE64_DECODE_TABLE[data[i++]];

        uint32_t sextet_d = data[i] == '=' ? 0 & i++ : BASE64_DECODE_TABLE[data[i++]];

        

        uint32_t triple = (sextet_a << 3 * 6)

        + (sextet_b << 2 * 6)

        + (sextet_c << 1 * 6)

        + (sextet_d << 0 * 6);

        

        if (j < *output_length) {

            output_data[j++] = (triple >> 2 * 8) & 0xFF;

        }

        if (j < *output_length) {

            output_data[j++] = (triple >> 1 * 8) & 0xFF;

        }

        if (j < *output_length) {

            output_data[j++] = (triple >> 0 * 8) & 0xFF;

        }

    }

    

    return TEE_SUCCESS;

}