/*
 ******************************************************************************
 *                     SOURCE FILE
 *
 *     Document no: @(#) $Name: VXWORKS_ITER40_RCPL8_2024030303 $ $RCSfile: ipike2_payload_encrypted.c,v $ $Revision: 1.15.10.7 $
 *     $Source: /home/interpeak/CVSRoot/ipike/src/ipike2_payload_encrypted.c,v $
 *     $Author: jren $ $Date: 2022-01-15 07:11:43 $
 *     $State: Exp $ $Locker:  $
 *
 *     INTERPEAK_COPYRIGHT_STRING
 *        Design and implementation by Kenneth Jonsson <kenneth@interpeak.se>
 ******************************************************************************
 */

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

/* Encrypted payload (IKE version 2) */

/*
 ****************************************************************************
 * 2                    CONFIGURATION
 ****************************************************************************
 */
#include "ipike_config.h"

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

#ifdef IPIKE_USE_VERSION2

#include "ipike_h.h"
#include "ipike_attributes.h"
#include "ipike2_payload_encrypted.h"
#include "ipike_util.h"
#include "ipike_crypto.h"
#include "ipike_sadb.h"

#include <ipcom_clib.h>
#include <ipcom_syslog.h>

/*
 ****************************************************************************
 * 4                    EXTERN PROTOTYPES
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 5                    DEFINES
 ****************************************************************************
 */

/*
 ****************************************************************************
 * 6                    TYPES
 ****************************************************************************
 */

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

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

/*
 ****************************************************************************
 * 9                    LOCAL FUNCTIONS
 ****************************************************************************
 */

/*
 *===========================================================================
 *                    ipike_payload_encrypted_data
 *===========================================================================
 * Description: Returns a pointer to the encrypted data in the payload.
 * Parameters:  e - a encrypted payload.
 * Returns:     A pointer to the encrypted data.
 */
IP_STATIC Ip_u8*
ipike_payload_encrypted_data(Ipike2_e_payload* e)
{
    return (Ip_u8*)e + sizeof(Ipike2_e_payload);
}

/*
 *===========================================================================
 *                    ipike_payload_encrypted_data_length
 *===========================================================================
 * Description: Returns the length of the encrypted data.
 * Parameters:  e - encrypted payload.
 * Returns:     The length of the encrypted data (in bytes).
 */
IP_STATIC Ip_u16
ipike_payload_encrypted_data_length(Ipike2_e_payload* e)
{
    return IPIKE_PAYLOAD_DATA_LENGTH(e->header.length, sizeof(Ipike2_e_payload));
}

/*
 ****************************************************************************
 * 10                   PUBLIC FUNCTIONS
 ****************************************************************************
 */

/*
 *===========================================================================
 *                    ipike_payload_encrypted_create
 *===========================================================================
 */
IP_GLOBAL Ipike_error
ipike_payload_encrypted_create(
    Ipike_isakmp_exchange* ie,
    Ipike_message         *msg,
    Ip_u32                 offset)
{
    Ipike_error          err;
    Ipike2_sa*           sa;
    Ipike_value*         enc_key;
    Ipike_value*         auth_key;
    Ip_u16               block_size;
    Ip_u16               checksum_size;
    Ip_u8                pad_length;
    Ip_u32               length;
    Ip_u8*               buffer;
    Ipike2_e_payload*    payload;
    Ipike_isakmp_header* msghdr_out;
    Ipike_value          org_data;
    Ip_u8*               data;
    Ip_u8*               iv;
    Ip_u8*               checksum;
	
    sa = &ie->ike_sa->v2.sa;
    if (ie->ike_sa->is_initiator)
    {
        enc_key  = &ie->ike_sa->v2.keys.SK_ei;
        auth_key = &ie->ike_sa->v2.keys.SK_ai;
    }
    else
    {
        enc_key  = &ie->ike_sa->v2.keys.SK_er;
        auth_key = &ie->ike_sa->v2.keys.SK_ar;
    }

    /* Get the data to be encrypted, skip header and any offset (non-ESP marker) */
    org_data.len    = msg->data.len - sizeof(Ipike_isakmp_header) - offset;
    org_data.value  = msg->data.value + sizeof(Ipike_isakmp_header) + offset;

    /* Get block size */
    block_size = ipike_attribute_get_cipher_block_size_v2(sa->selected_encr,
                                                     sa->selected_key_length);
    
    ip_assert(block_size == ie->iv.len);

    /* Calculate the encrypted data size */
    checksum_size  = ipike_crypto_hmac_size(sa->selected_integ);
    pad_length     = (Ip_u8)(block_size - (org_data.len % block_size));
    length         = sizeof(Ipike_isakmp_header) + sizeof(Ipike2_e_payload);
    length        += block_size + (Ip_u32)org_data.len + pad_length + checksum_size;

    /* Create a new buffer for the encrypted message */
    if ( IP_NULL == (buffer = ipcom_malloc(length + offset)) )
	{
		IP_PANIC();
        return IPIKE_OUT_OF_MEMORY;
    }
   
    /* Copy the header to the new buffer */
    ipcom_memcpy((void*)buffer, (void*)msg->data.value, (sizeof(Ipike_isakmp_header) + offset));
    msghdr_out = (Ipike_isakmp_header *) (buffer + offset);
    payload    = (Ipike2_e_payload *)(msghdr_out + 1);
    iv         = (Ip_u8 *)(payload + 1);
    data       = iv + block_size;
    checksum   = data + org_data.len + pad_length;

    /* Update isakmp and encrypted header with correct data */
    payload->header.next_payload = msghdr_out->next_payload;
    msghdr_out->next_payload     = IPIKE2_PAYLOAD_ENCRYPTED;
    payload->header.length 	 = (Ip_u16)(length - sizeof(Ipike_isakmp_header));
    msghdr_out->length     	 = (Ip_u32)(payload->header.length + sizeof(*msghdr_out));
    payload->header.length       = ip_htons(payload->header.length);
    msghdr_out->length           = ip_htonl(msghdr_out->length);
    payload->header.flags        = 0;    

    /* Copy iv, data and pad to new buffer */
    ipcom_memcpy(iv, ie->iv.value, ie->iv.len);
    ipcom_memcpy(data, org_data.value, org_data.len);
    data[org_data.len+pad_length-1] = (Ip_u8)(pad_length - 1);

    /* Encrypt the data (plus padding chars) */
    if (IPIKE_SUCCESS != (err = ipike_crypto_encrypt_buffer(
                                    sa->selected_encr,
                                    enc_key->value,
                                    (Ip_u16)enc_key->len,
                                    iv,
                                    data,
                                    (Ip_u32)org_data.len + pad_length)))
    {
        ipcom_free(buffer);
        return IPIKE_FAILED; /* Should never happen */
    }

    /* Calculate checksum */
    if (IPIKE_SUCCESS != (err = ipike_crypto_calculate_checksum(
                                  sa->selected_integ,
                                  auth_key,
                                  (Ip_u8*)msghdr_out,
                                  ip_htonl(msghdr_out->length) - checksum_size,
                                  checksum)))
    {
        ipcom_free(buffer);
        return IPIKE_FAILED; /* Should never happen */
    }

    /* All success, free old plaintext data and replace it
       with the encrypted data */
    ipcom_free(msg->data.value);
    msg->data.value = buffer;
    msg->data.len   = length + offset;

    return err;
}

/*
 *===========================================================================
 *                    ipike_payload_encrypted_extract
 *===========================================================================
 */
IP_GLOBAL Ipike_error
ipike_payload_encrypted_extract(
    Ipike_isakmp_exchange* ie,
    Ipike_isakmp_header*   msg)
{
    Ipike_error       err;
    Ip_u16            block_size;
    Ip_u16            checksum_size;
    Ipike2_e_payload* payload;
    Ip_u8*            iv;
    Ip_u8*            checksum;
    Ip_u8*            data;
    Ip_size_t         data_len;
    Ipike2_sa*        sa;
    Ipike_value*      enc_key;
    Ipike_value*      auth_key;

    sa = &ie->ike_sa->v2.sa;
    if (ie->ike_sa->is_initiator)
    {
        enc_key  = &ie->ike_sa->v2.keys.SK_er;
        auth_key = &ie->ike_sa->v2.keys.SK_ar;
    }
    else
    {
        enc_key  = &ie->ike_sa->v2.keys.SK_ei;
        auth_key = &ie->ike_sa->v2.keys.SK_ai;
    }

    block_size = ipike_attribute_get_cipher_block_size_v2(sa->selected_encr,
                                                          sa->selected_key_length);

    checksum_size = ipike_crypto_hmac_size(sa->selected_integ);
    payload       = (Ipike2_e_payload*)(msg + 1);

    if ((sizeof(Ipike_payload_header) + IPIKE_ISAKMP_HEADER_LENGTH)  > (ip_htonl(msg->length)))
    {
        return IPIKE_ISAKMP_INVALID_PAYLOAD;
    }

    if ((ip_htons(payload->header.length) + IPIKE_ISAKMP_HEADER_LENGTH)  > (ip_htonl(msg->length)))
    {
        return IPIKE_ISAKMP_INVALID_PAYLOAD;
    }

    /* At least one block of iv, one block of encrypted data and one
     * checksum */
    if (ip_htons(payload->header.length) <
        (2u * block_size + checksum_size + sizeof(Ipike_payload_header)))
    {
        return IPIKE_ISAKMP_INVALID_PAYLOAD;
    }

    data_len    = (ip_htons(payload->header.length) - sizeof(*payload) - checksum_size - block_size);
    iv          = (Ip_u8*)(payload + 1);
    data        = iv + block_size;
    checksum    = data + data_len;

    err = ipike_crypto_verify_checksum(sa->selected_integ,
                                       auth_key,
                                       (Ip_u8*)msg,
                                       ip_htonl(msg->length) - checksum_size,
                                       checksum);

    if (IPIKE_SUCCESS != err)
        return IPIKE_ISAKMP_INVALID_PAYLOAD;

    err = ipike_crypto_decrypt_buffer(sa->selected_encr,
                                      enc_key->value,
                                      (Ip_u16)enc_key->len,
                                      iv,
                                      data,
                                      data_len);

    if (IPIKE_SUCCESS != err)
        return err;

    /* Remove padding */
    if ((data[data_len-1] > 0) ||
        (data[data_len-1] < block_size))
        data_len -= data[data_len-1] + 1u;

    /* Update the msg structure to reflect the decoded values */
    msg->next_payload = payload->header.next_payload;
    msg->length = ip_htonl(data_len + sizeof(*msg));
    /* Move decrypted data to begining of buffer */
    ipcom_memmove(payload, data, data_len);

    return IPIKE_SUCCESS;
}

/*
 *===========================================================================
 *                    ipike_payload_encrypted_print
 *===========================================================================
 */
IP_GLOBAL void
ipike_payload_encrypted_print(Ipike2_e_payload* e)
{
    (void)ipike_print_memory(IPCOM_LOG_DEBUG2, "  Encrypted data:\n",
        ipike_payload_encrypted_data(e),
        ipike_payload_encrypted_data_length(e));

    return;
}

#else

int ipike_payload_encrypted_empty_file = 0;

#endif /* IPIKE_USE_VERSION2 */

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