/*
 ******************************************************************************
 *                     INTERPEAK SOURCE FILE
 *
 *   Document no: @(#) $Name: VXWORKS_ITER40_2019102207 $ $RCSfile: ipeap_sim_util.c,v $ $Revision: 1.21 $
 *   $Source: /home/interpeak/CVSRoot/ipeap/src/ipeap_sim_util.c,v $
 *   $Author: hchowdhu $
 *   $State: Exp $ $Locker:  $
 *
 *   INTERPEAK_COPYRIGHT_STRING
 *   Design and implementation by Roger Boden <roger@interpeak.se>
 ******************************************************************************
 */


/*
 ****************************************************************************
 * 1                    DESCRIPTION
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 2                    CONFIGURATION
 ****************************************************************************
 */

#include "ipeap_config.h"


/*
 ****************************************************************************
 * 3                    INCLUDE FILES
 ****************************************************************************
 */

#define IPCOM_USE_CLIB_PROTO
#include <ipcom_type.h>
#include <ipcom_cstyle.h>
#include <ipcom_clib.h>
#include <ipcom_err.h>
#include <ipcom_syslog.h>
#include <openssl/rand.h>
#include <openssl/aes.h>
#include <openssl/sha.h>
#include <openssl/hmac.h>

#include "ipeap.h"
#include "ipeap_h.h"
#include "ipeap_sim.h"
#include "ipeap_sim_util.h"


/*
 ****************************************************************************
 * 4                    DEFINES
 ****************************************************************************
 */
#ifndef AES_BLOCK_SIZE
#define AES_BLOCK_SIZE 16
#endif

/*
 ****************************************************************************
 * 5                    TYPES
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 6                    EXTERN PROTOTYPES
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 7                    LOCAL PROTOTYPES
 ****************************************************************************
 */

/*
 *===========================================================================
 *                    ipeap_sim_sha1_transform
 *===========================================================================
 * Description: Make a sha-1 transform
 * Parameters: state - the current state
 *             data - data buffer to transform
 * Returns:
 */
IP_STATIC void
ipeap_sim_sha1_transform(Ip_u8* state, Ip_u8 data[64]);

/*
 *===========================================================================
 *                    ipeap_sim_get_cipher
 *===========================================================================
 * Description: Get cipher context from cipher name and key length
 * Parameters: name - the cipher name
 *             key_len - key lengths in bytes
 * Returns: Cipher context or IP_NULL
 */
#ifndef IPMCRYPTO
IP_STATIC const EVP_CIPHER *
ipeap_sim_get_cipher(const char *name, Ip_u32 key_len);
#endif /* !IPMCRYPTO */

/*
 ****************************************************************************
 * 8                    DATA
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 9                    STATIC FUNCTIONS
 ****************************************************************************
 */

/*
 *===========================================================================
 *                    ipeap_sim_sha1_transform
 *===========================================================================
 */
IP_STATIC void
ipeap_sim_sha1_transform(Ip_u8* state, Ip_u8 data[64])
{
    SHA_CTX ctx;

    ipcom_memset(&ctx, 0, sizeof(ctx));
    ipcom_memcpy(&ctx.h0, state, 4);
    ipcom_memcpy(&ctx.h1, state+4, 4);
    ipcom_memcpy(&ctx.h2, state+8, 4);
    ipcom_memcpy(&ctx.h3, state+12, 4);
    ipcom_memcpy(&ctx.h4, state+16, 4);
    SHA1_Transform(&ctx, data);
    ipcom_memcpy(state, &ctx.h0, 4);
    ipcom_memcpy(state+4, &ctx.h1, 4);
    ipcom_memcpy(state+8, &ctx.h2, 4);
    ipcom_memcpy(state+12, &ctx.h3, 4);
    ipcom_memcpy(state+16, &ctx.h4, 4);

    return;
}

/*
 *===========================================================================
 *                    ipeap_sim_get_cipher
 *===========================================================================
 */
#ifndef IPMCRYPTO
IP_STATIC const EVP_CIPHER *
ipeap_sim_get_cipher(const char *name, Ip_u32 key_len)
{
    const EVP_CIPHER *cipher      = IP_NULL;
    char             alg_name[32] = {0};

    ipcom_snprintf(alg_name, sizeof(alg_name), "%s%d", name, key_len*8);
    cipher = EVP_get_cipherbyname(alg_name);
    if (cipher == IP_NULL)
        IPCOM_LOG1(ERR, "ipeap_sim_get_cipher: %s not found", name);
    
    return cipher;
}
#endif /* !IPMCRYPTO */

/*
 ****************************************************************************
 * 10                   GLOBAL FUNCTIONS
 ****************************************************************************
 */

/*
 *===========================================================================
 *                    ip_alloc_value
 *===========================================================================
 */
IP_GLOBAL Ip_err
ip_alloc_value(Ip_value* value, Ip_u32 len)
{
    value->p = ipcom_calloc(1, len);
    if (IP_NULL == value->p)
        return IPCOM_ERR_FAILED;
    value->n = len;

    return IPCOM_SUCCESS;
}

/*
 *===========================================================================
 *                    ip_free_value
 *===========================================================================
 */
IP_GLOBAL void
ip_free_value(Ip_value* value)
{
    if (IP_NULL != value->p)
        ipcom_free(value->p);
    value->p = IP_NULL;
    value->n = 0;

    return;
}

/*
 *===========================================================================
 *                    ip_set_value
 *===========================================================================
 */
IP_GLOBAL Ip_err
ip_set_value(Ip_value* value, Ip_u8* ptr, Ip_u32 len)
{
    Ip_err err = IPCOM_SUCCESS;

    ip_free_value(value);
    if (IPCOM_SUCCESS != (err = ip_alloc_value(value, len)))
        return err;
    ipcom_memcpy(value->p, ptr, len);
    value->n = len;

    return IPCOM_SUCCESS;
}

/*
 *===========================================================================
 *                    ip_append_value
 *===========================================================================
 */
IP_GLOBAL Ip_err
ip_append_value(Ip_value* value, Ip_u8* ptr, Ip_u32 len)
{
    value->p = ipcom_realloc(value->p, value->n + len);
    if (IP_NULL == value->p)
        return IPCOM_ERR_FAILED;
    ipcom_memcpy(value->p + value->n, ptr, len);
    value->n += len;

    return IPCOM_SUCCESS;
}

/*
 *===========================================================================
 *                    ipeap_alloc_pkt
 *===========================================================================
 */
IP_GLOBAL Ipeap_pkt*
ipeap_alloc_pkt(Ip_u32 size)
{
    Ipeap_pkt* pkt = IP_NULL;

    pkt = (Ipeap_pkt*)ipcom_calloc(1, size+sizeof(*pkt));
    if (IP_NULL != pkt)
    {
        pkt->start = 0;
        pkt->end = 0;
        pkt->max_len = (int)size;
        pkt->data = (Ip_u8*)pkt + sizeof(*pkt);
    }

    return pkt;
}

/*
 *===========================================================================
 *                    ipeap_free_pkt
 *===========================================================================
 */
IP_GLOBAL void
ipeap_free_pkt(Ipeap_pkt* pkt)
{
    if (IP_NULL != pkt)
        ipcom_free(pkt);

    return;
}

/*
 *===========================================================================
 *                    ipeap_set_pkt_data
 *===========================================================================
 */
IP_GLOBAL Ip_err
ipeap_set_pkt_data(Ipeap_pkt* pkt, Ip_u8* data, Ip_u32 len)
{
    if ((IP_NULL == pkt) ||
        ((pkt->start + (int)len) > pkt->max_len))
        return IPCOM_ERR_FAILED;
    ipcom_memcpy(pkt->data+pkt->start, data, len);
    pkt->end = pkt->start + (int)len;

    return IPCOM_SUCCESS;
}

/*
 *===========================================================================
 *                    ipeap_get_pkt_data_size
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 */
IP_GLOBAL Ip_u32
ipeap_get_pkt_data_size(
    Ipeap_pkt* pkt)
{
    return (Ip_u32)(pkt->max_len - pkt->end);
}

/*
 *===========================================================================
 *                    ipeap_get_pkt_data_buffer
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 */
IP_GLOBAL Ip_u8*
ipeap_get_pkt_data_buffer(
    Ipeap_pkt* pkt)
{
    return (Ip_u8*)pkt->data + pkt->start;
}

/*
 *===========================================================================
 *                    ipeap_compare_pkt_data
 *===========================================================================
 */
IP_GLOBAL Ip_bool
ipeap_compare_pkt_data(Ipeap_pkt* pkt, Ip_u8* data, Ip_u32 len)
{
    if ((IP_NULL == pkt) ||
        ((pkt->end - pkt->start) != (int)len) ||
        (ipcom_memcmp(pkt->data+pkt->start, data, len) != 0))
        return IP_FALSE;

    return IP_TRUE;
}

/*
 *===========================================================================
 *                    ipeap_memory_to_string
 *===========================================================================
 */
IP_GLOBAL char*
ipeap_memory_to_string(const Ip_u8* ptr, Ip_u32 size, Ip_u8 alignment)
{
    static const char hex_char[] = "0123456789abcdef";
    static const int byte_per_row = 32;
    char* buf;
    int row_size;
    int i, j, k = 0;
    int ws_per_row;
    int full_rows, blr;

    ws_per_row = byte_per_row / alignment;
    row_size = 2 * byte_per_row + ws_per_row;
    full_rows = (int)size / byte_per_row;
    blr = (int)size % byte_per_row;

    buf = ipcom_malloc( (Ip_size_t)(full_rows * row_size + 2 * blr + blr / alignment + 1) );
    if (IP_NULL == buf)
        return IP_NULL;

    for (i = 0; i <= full_rows; i++)
    {
        k = 0;
        for (j = 0; j < (i == full_rows ? blr : byte_per_row); j++)
        {
            if (j != 0 && j % alignment == 0)
                buf[i * row_size + 2 * j + k++] = ' ';
            buf[i * row_size + 2 * j + k] =
                hex_char[ptr[i * byte_per_row + j] >> 4];
            buf[i * row_size + 2 * j + k + 1] =
                hex_char[ptr[i * byte_per_row + j] & 0x0F];
        }
        buf[i * row_size + 2 * j + k] = '\n';
    }
    buf[full_rows * row_size + 2 * blr + k] = '\0';

    return buf;
}

/*
 *===========================================================================
 *                    ipeap_print_memory
 *===========================================================================
 */
IP_GLOBAL void
ipeap_print_memory(
    const char*  prefix,
    const Ip_u8* data,
    Ip_u32       len)
{
    char* hex_mem = IP_NULL;

    hex_mem = ipeap_memory_to_string(data, len, 4);
    if (hex_mem == IP_NULL)
    {
        IPCOM_LOG0(CRIT, "ipeap_print_memory: out of memory");
    }
    else
    {
        /* Make sure fit into syslog buffer */
        if ((ipcom_strlen(hex_mem) + ipcom_strlen(prefix)) >= IPCOM_SYSLOG_MAX_SIZE)
        {
            len = IPCOM_SYSLOG_MAX_SIZE - (Ip_u32)ipcom_strlen(prefix);
            hex_mem[len-4] = '.';
            hex_mem[len-3] = '.';
            hex_mem[len-2] = '.';
            hex_mem[len-1] = 0;
        }
        IPCOM_LOG2(DEBUG2, "%s%s", prefix, hex_mem);
        ipcom_free(hex_mem);
    }

    return;
}

/*
 *===========================================================================
 *                    ipeap_sim_prf
 *===========================================================================
 */
IP_GLOBAL Ip_err
ipeap_sim_prf(
    Ip_u8* key,
    Ip_u32 key_len,
    Ip_u8* buf,
    Ip_u32 buf_len)
{
    Ip_u8  xkey[64] = {0};
    Ip_u32 t[5]     = {0};
    Ip_u32 _t[5]    = {0};
    Ip_s32 i        = 0;
    Ip_s32 j        = 0;
    Ip_s32 m        = 0;
    Ip_s32 k        = 0;
    Ip_u8* xpos     = buf;
    Ip_u32 carry    = 0;

    /* See section 6.4
     *
     * Key derivation is based on the random number generation specified in
     * NIST Federal Information Processing Standards (FIPS) Publication
     * 186-2 [PRF].  The pseudo-random number generator is specified in the
     * change notice 1 (2001 October 5) of [PRF] (Algorithm 1) */

    if (key_len >= 64)
    {
        IPCOM_LOG1(ERR, "ipeap_sim_prf: key too long (%d >= 64)", key_len);
        return IPCOM_ERR_FAILED;
    }

    ipcom_memcpy(xkey, key, key_len);
    ipcom_memset(xkey + key_len, 0, 64 - key_len);
    t[0] = 0x67452301;
    t[1] = 0xEFCDAB89;
    t[2] = 0x98BADCFE;
    t[3] = 0x10325476;
    t[4] = 0xC3D2E1F0;

    m = (Ip_s32)buf_len / 40;
    for (j = 0; j < m; j++)
    {
        for (i = 0; i < 2; i++)
        {
            ipcom_memcpy(_t, t, 20);
            ipeap_sim_sha1_transform((Ip_u8*)_t, xkey);
            _t[0] = (Ip_u32)ip_htonl(_t[0]);
            _t[1] = (Ip_u32)ip_htonl(_t[1]);
            _t[2] = (Ip_u32)ip_htonl(_t[2]);
            _t[3] = (Ip_u32)ip_htonl(_t[3]);
            _t[4] = (Ip_u32)ip_htonl(_t[4]);
            ipcom_memcpy(xpos, _t, 20);

            carry = 1;
            for (k = 19; k >= 0; k--)
            {
                carry += (Ip_u32)(xkey[k] + xpos[k]);
                xkey[k] = (Ip_u8)carry;
                carry >>= 8;
            }

            xpos += 20;
        }
    }

    return IPCOM_SUCCESS;
}

/*
 *===========================================================================
 *                    ipeap_sim_generate_mac
 *===========================================================================
 */
IP_GLOBAL Ip_err
ipeap_sim_generate_mac(
    Ip_u8* key,
    Ip_u32 key_len,
    Ip_u8* data1,
    Ip_u32 data1_len,
    Ip_u8* data2,
    Ip_u32 data2_len,
    Ip_u8* mac,
    Ip_u32 mac_len)
{
    const EVP_MD* evp_md   = IP_NULL;
    HMAC_CTX      ctx      = {0};
    unsigned int  hmac_len = 0;
    Ip_u8*        hmac     = IP_NULL;

    evp_md = EVP_get_digestbyname("sha1");
    if (IP_NULL == evp_md)
    {
        IPCOM_LOG0(ERR, "ipeap_sim_generate_mac: sha1 algorithm not available");
        return IPCOM_ERR_FAILED;
    }
    hmac_len = (unsigned int)EVP_MD_size(evp_md);
    if (hmac_len < mac_len)
    {
        IPCOM_LOG2(ERR, "ipeap_sim_generate_mac: bad mac length (%d < %d)", hmac_len, mac_len);
        return IPCOM_ERR_FAILED;
    }
    hmac = ipcom_malloc(hmac_len);
    if (IP_NULL == hmac)
    {
        IPCOM_LOG0(ERR, "ipeap_sim_generate_mac: out of memory");
        return IPCOM_ERR_FAILED;
    }
    HMAC_CTX_init(&ctx);
    HMAC_Init(&ctx, key, (int)key_len, evp_md);
    if (data1_len > 0)
        HMAC_Update(&ctx, data1, data1_len);
    if (data2_len > 0)
        HMAC_Update(&ctx, data2, data2_len);
    HMAC_Final(&ctx, hmac, (unsigned int*)&hmac_len);
    HMAC_CTX_cleanup(&ctx);
    ipcom_memcpy(mac, hmac, mac_len);
    ipcom_free(hmac);

    return IPCOM_SUCCESS;
}

/*
 *===========================================================================
 *                    ipeap_sim_verify_mac
 *===========================================================================
 */
IP_GLOBAL Ip_err
ipeap_sim_verify_mac(
    Ip_u8* key,
    Ip_u32 key_len,
    Ip_u8* data1,
    Ip_u32 data1_len,
    Ip_u8* data2,
    Ip_u32 data2_len,
    Ip_u8* mac,
    Ip_u32 mac_len)
{
    Ip_err err = IPCOM_SUCCESS;
    Ip_u8* tmp = IP_NULL;

    tmp = ipcom_malloc(mac_len);
    if (IP_NULL == tmp)
        return IPCOM_ERR_FAILED;
    err = ipeap_sim_generate_mac(key,
                                 key_len,
                                 data1,
                                 data1_len,
                                 data2,
                                 data2_len,
                                 tmp,
                                 mac_len);
    if (IPCOM_SUCCESS != err)
    {
        IPCOM_LOG0(ERR, "ipeap_sim_verify_mac: failed to generate mac");
        ipcom_free(tmp);
        return err;
    }
    if (ipcom_memcmp(tmp, mac, mac_len) != 0)
    {
        IPCOM_LOG0(ERR, "ipeap_sim_verify_mac: bad mac value");
        ipcom_free(tmp);
        return IPCOM_ERR_FAILED;
    }

    ipcom_free(tmp);

    return IPCOM_SUCCESS;
}

/*
 *===========================================================================
 *                    ipeap_sim_aes_cbc_encrypt
 *===========================================================================
 */
IP_GLOBAL Ip_err
ipeap_sim_aes_cbc_encrypt(
    Ip_u8* key,
    Ip_u32 key_len,
    Ip_u8* iv,
    Ip_u32 iv_len,
    Ip_u8* data,
    Ip_u32 data_len)
{
    Ip_err err = IPCOM_ERR_FAILED;

    if (data_len == 0)
    {
        err = IPCOM_SUCCESS;
    }
    else if ((iv_len == AES_BLOCK_SIZE) && ((data_len % AES_BLOCK_SIZE) == 0))
    {
#if defined(IPMCRYPTO)
        AES_KEY ctx;
        ipcom_memset(&ctx, 0, sizeof(ctx));
        if (AES_set_encrypt_key(key, key_len*8, &ctx) < 0)
        {
            IPCOM_LOG0(ERR, "ipeap_sim_aes_cbc_encrypt: failed to set key");
            err = IPCOM_ERR_FAILED;
        }
        else
        {
            err = IPCOM_SUCCESS;
            AES_cbc_encrypt(data, data, data_len, &ctx, iv, AES_ENCRYPT);
        }
#else /* defined (IPCRYPTO) */
        const EVP_CIPHER *cipher = IP_NULL;
        EVP_CIPHER_CTX ctx;
        if ((cipher = ipeap_sim_get_cipher("aes", key_len)) != IP_NULL)
        {
            EVP_CIPHER_CTX_init(&ctx);
            if (EVP_EncryptInit(&ctx, cipher, key, iv) &&
                EVP_CIPHER_CTX_set_padding(&ctx, 0) &&
                EVP_EncryptUpdate(&ctx, data, (int*)&data_len, data, (int)data_len))
            {
                err = IPCOM_SUCCESS;
            }
            else
            {
                IPCOM_LOG0(ERR, "ipeap_sim_aes_cbc_encrypt: failed to encrypt");
            }
            (void)EVP_CIPHER_CTX_cleanup(&ctx);
        }
#endif
    }

    return err;
}

/*
 *===========================================================================
 *                    ipeap_sim_aes_cbc_decrypt
 *===========================================================================
 */
IP_GLOBAL Ip_err
ipeap_sim_aes_cbc_decrypt(
    Ip_u8* key,
    Ip_u32 key_len,
    Ip_u8* iv,
    Ip_u32 iv_len,
    Ip_u8* data,
    Ip_u32 data_len)
{
    Ip_err err = IPCOM_ERR_FAILED;

    if (data_len == 0)
    {
        err = IPCOM_SUCCESS;
    }
    else if ((iv_len == AES_BLOCK_SIZE) && ((data_len % AES_BLOCK_SIZE) == 0))
    {
#if defined(IPMCRYPTO)
        AES_KEY ctx;
        ipcom_memset(&ctx, 0, sizeof(ctx));
        if (AES_set_decrypt_key(key, key_len*8, &ctx) < 0)
        {
            IPCOM_LOG0(ERR, "ipeap_sim_aes_cbc_decrypt: failed to set key");
            err = IPCOM_ERR_FAILED;
        }
        else
        {
            err = IPCOM_SUCCESS;
            AES_cbc_encrypt(data, data, data_len, &ctx, iv, AES_DECRYPT);
        }
#else /* defined (IPCRYPTO) */
        const EVP_CIPHER *cipher = IP_NULL;
        EVP_CIPHER_CTX ctx;
        if ((cipher = ipeap_sim_get_cipher("aes", key_len)) != IP_NULL)
        {
            EVP_CIPHER_CTX_init(&ctx);
            if (EVP_DecryptInit(&ctx, cipher, key, iv) &&
                EVP_CIPHER_CTX_set_padding(&ctx, 0) &&
                EVP_DecryptUpdate(&ctx, data, (int*)&data_len, data, (int)data_len))
            {
                err = IPCOM_SUCCESS;
            }
            else
            {
                IPCOM_LOG0(ERR, "ipeap_sim_aes_cbc_decrypt: failed to decrypt");
            }
            (void)EVP_CIPHER_CTX_cleanup(&ctx);
        }
#endif
    }

    return err;
}

/*
 *===========================================================================
 *                     ipeap_sim_algorithm_test
 *===========================================================================
 */
IP_GLOBAL Ip_err
ipeap_sim_algorithm_test(void)
{
    Ip_err err = IPCOM_SUCCESS;

#ifdef IP_DEBUG
    Ip_u8 IV[16]  = {0};
    Ip_u8 KEY[16] = {0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0a,0x0b,0x0c,0x0d,0x0e,0x0f};
    Ip_u8 MSG[16] = {0x00,0x11,0x22,0x33,0x44,0x55,0x66,0x77,0x88,0x99,0xAA,0xBB,0xCC,0xDD,0xEE,0xFF};
    Ip_u8 ENC[16] = {0x69,0xC4,0xE0,0xD8,0x6A,0x7B,0x04,0x30,0xD8,0xCD,0xB7,0x80,0x70,0xB4,0xC5,0x5A};
    Ip_u8 tmp[16] = {0};

    ipcom_memcpy(tmp, MSG, sizeof(MSG)); 
    if (IPCOM_SUCCESS != (err = ipeap_sim_aes_cbc_encrypt(
        KEY, sizeof(KEY),
        IV, sizeof(IV),
        tmp, sizeof(tmp))))
    {
        IPCOM_LOG0(ERR, "ipeap_sim_algorithm_test - AES encrypt not available");
    }
    else if (ipcom_memcmp(ENC, tmp, sizeof(ENC)) != 0)
    {
        IPCOM_LOG0(ERR, "ipeap_sim_algorithm_test - AES bad encrypt value");
        err = IPCOM_ERR_FAILED;
    }
    else if (IPCOM_SUCCESS != (err = ipeap_sim_aes_cbc_decrypt(
        KEY, sizeof(KEY),
        IV, sizeof(IV),
        tmp, sizeof(tmp))))
    {
        IPCOM_LOG0(ERR, "ipeap_sim_algorithm_test - AES decrypt not available");
    }
    else if (ipcom_memcmp(MSG, tmp, sizeof(MSG)) != 0)
    {
        IPCOM_LOG0(ERR, "ipeap_sim_algorithm_test - AES bad decrypt value");
        err = IPCOM_ERR_FAILED;
    }
#endif /* IP_DEBUG */

    return err;
}

/*
 ****************************************************************************
 * 11                   PUBLIC FUNCTIONS
 ****************************************************************************
 */

/*
 ****************************************************************************
 *                      END OF FILE
 ****************************************************************************
 */