/*
 ******************************************************************************
 *                     SOURCE FILE
 *
 *     Document no: @(#) $Name: VXWORKS_ITER40_2019102207 $ $RCSfile: ipssh_keyex_dh.c,v $ $Revision: 1.62.10.5 $
 *     $Source: /home/interpeak/CVSRoot/ipssh/src/ipssh_keyex_dh.c,v $
 *     $Author: yli10 $ $Date: 2018-06-20 02:32:55 $
 *     $State: Exp $ $Locker:  $
 *
 *     INTERPEAK_COPYRIGHT_STRING
 *     Design and implementation by Roger Boden <roger@interpeak.se>
 ******************************************************************************
 */

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

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

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

#include "ipssh_h.h"
#include "ipssh_transport.h"
#include "ipssh_buffer.h"
#include "ipssh_keyex_dh.h"

#include <ipcom_clib.h>
#include <ipssh_config.h>
#include <ipcom_type.h>
#include <ipcom_cstyle.h>
#include <ipcom_buffer.h>
#include <ipcom_sock.h>
#include <ipcom_syslog.h>
#include <ipcom_err.h>

#include <openssl/bn.h>
#include <openssl/evp.h>


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

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

/*
 * Define if you want debug printouts of the buffers that are hashed as part
 * of the handshake.
 *
 * #define IPSSH_HASH_DBG
 */

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

/*
 ****************************************************************************
 * 7                    LOCAL PROTOTYPES
 ****************************************************************************
 */
IP_GLOBAL Ip_err ipssh_dss_sign(Ipssh_conn* ssh_conn, Ip_u8* data, Ip_u32 data_len, Ipcom_buffer* buf);
IP_GLOBAL Ip_err ipssh_send_kex_dh_reply(Ipssh_conn* ssh_conn);
IP_GLOBAL Ip_err ipssh_send_kex_dh_init(Ipssh_conn* ssh_conn);
IP_GLOBAL Ip_err ipssh_msg_kexdh_reply(Ipssh_conn* ssh_conn);

/*
 ****************************************************************************
 * 8                    DATA
 ****************************************************************************
 */
IP_STATIC const char *ipssh_dh_g_gr1 = "2";
IP_STATIC const char *ipssh_dh_p_gr1 =
"FFFFFFFF" "FFFFFFFF" "C90FDAA2" "2168C234" "C4C6628B" "80DC1CD1"
"29024E08" "8A67CC74" "020BBEA6" "3B139B22" "514A0879" "8E3404DD"
"EF9519B3" "CD3A431B" "302B0A6D" "F25F1437" "4FE1356D" "6D51C245"
"E485B576" "625E7EC6" "F44C42E9" "A637ED6B" "0BFF5CB6" "F406B7ED"
"EE386BFB" "5A899FA5" "AE9F2411" "7C4B1FE6" "49286651" "ECE65381"
"FFFFFFFF" "FFFFFFFF";

IP_STATIC const char *ipssh_dh_g_gr14 = "2";
IP_STATIC const char *ipssh_dh_p_gr14 =
    "FFFFFFFF" "FFFFFFFF" "C90FDAA2" "2168C234" "C4C6628B" "80DC1CD1"
    "29024E08" "8A67CC74" "020BBEA6" "3B139B22" "514A0879" "8E3404DD"
    "EF9519B3" "CD3A431B" "302B0A6D" "F25F1437" "4FE1356D" "6D51C245"
    "E485B576" "625E7EC6" "F44C42E9" "A637ED6B" "0BFF5CB6" "F406B7ED"
    "EE386BFB" "5A899FA5" "AE9F2411" "7C4B1FE6" "49286651" "ECE45B3D"
    "C2007CB8" "A163BF05" "98DA4836" "1C55D39A" "69163FA8" "FD24CF5F"
    "83655D23" "DCA3AD96" "1C62F356" "208552BB" "9ED52907" "7096966D"
    "670C354E" "4ABC9804" "F1746C08" "CA18217C" "32905E46" "2E36CE3B"
    "E39E772C" "180E8603" "9B2783A2" "EC07A28F" "B5C55DF0" "6F4C52C9"
    "DE2BCBF6" "95581718" "3995497C" "EA956AE5" "15D22618" "98FA0510"
    "15728E5A" "8AACAA68" "FFFFFFFF" "FFFFFFFF";


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


/*
 *===========================================================================
 *                    ipssh_create_dh_group
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 *
 */
IP_GLOBAL DH*
ipssh_create_dh_group(Ip_u8 dh_id)
{
    DH* dh;

    dh = DH_new();
    if(!dh)
        return IP_NULL;

    switch (dh_id)
    {
    case IPSSH_KEYEX_DH_GR1_SHA1_ID:
        if(BN_hex2bn(&(dh->p), ipssh_dh_p_gr1) == 0 ||
           BN_hex2bn(&(dh->g), ipssh_dh_g_gr1) == 0)
        {
            IPCOM_LOG0(ERR, "ipssh_create_dh_group :: BN_hex2bn() failed for group 1");
            DH_free(dh);
            return IP_NULL;
        }
        break;

    case IPSSH_KEYEX_DH_GR14_SHA1_ID:
        if(BN_hex2bn(&(dh->p), ipssh_dh_p_gr14) == 0 ||
           BN_hex2bn(&(dh->g), ipssh_dh_g_gr14) == 0)
        {
            IPCOM_LOG0(ERR, "ipssh_create_dh_group :: BN_hex2bn() failed for group 14");
            DH_free(dh);
            return IP_NULL;
        }
        break;

    default:
        IP_PANIC();
        DH_free(dh);
        return IP_NULL;
    }

    return dh;
}


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

/*
 *===========================================================================
 *                    ipssh_dh_priv_key_set_length
 *===========================================================================
 * Description: This function sets the length of the DH private key.
 *              If not set, DH_generate_key will generate a private key
 *              of the same size as the DH group. Especially for large groups
 *              this is unnecessarily big, which will give long connection
 *              time, in particular on weak CPUs. According to key length
 *              recommendations, a DH private key length of 256 bits will be
 *              sufficient until the year 2040. To be on the safe side, we
 *              use twice that length, i.e. 512 bits.
 * Parameters:
 * Returns:
 */
IP_GLOBAL Ip_err ipssh_dh_priv_key_set_length(Ipssh_conn* ssh_conn)
{
    if (BN_num_bits(ssh_conn->dh_data->dh->p) > 512)
    {
        ssh_conn->dh_data->dh->length = 512;
    }

    return IPCOM_SUCCESS;
}

/*
 *===========================================================================
 *                    ipssh_calc_hash
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 * TODO: change to one argument: Ipssh_conn*?
 */
IP_GLOBAL Ip_err
ipssh_calc_hash(Ipssh_msg_list* msgs, const EVP_MD* (*hash_func)(void), int hash_comps,
                Ip_u8** hash_value, Ip_u32* hash_len)
{
    Ipcom_list *list, *node;
    Ipssh_msg_list* msg;
    Ip_s32 i, rc = IPCOM_SUCCESS;
    Ipcom_buffer* buf;
    EVP_MD_CTX* md_ctx;
    Ip_size_t buf_len = 0;

    /* Calculate needed buffer length */
    list = (Ipcom_list*) msgs;
    for(node = ipcom_list_first(list); node; node = ipcom_list_next(node))
    {
        msg = (Ipssh_msg_list*) node;
        buf_len += msg->msg_len;
        switch(msg->type)
        {
        case IPSSH_STR:
            buf_len += 4;
            break;
        case IPSSH_MPINT:
            buf_len += 5;
            break;
        }
    }
    buf = ipcom_buffer_new(buf_len+16);
    md_ctx = EVP_MD_CTX_create();
    if(!buf || !md_ctx)
    {
        IPCOM_LOG0(ERR, "ipssh_calc_hash :: out of memory");
        rc = IPCOM_ERR_FAILED;
        goto cleanup;
    }
    (void)EVP_DigestInit(md_ctx, hash_func());

    list = (Ipcom_list*) msgs;
    for(i=0; i<hash_comps; i++)
    {
        Ip_s32 found = 0;
        for(node = ipcom_list_first(list); node; node = ipcom_list_next(node))
        {
            msg = (Ipssh_msg_list*) node;
            if(msg->hash_pos == i)
            {
                if(msg->type == IPSSH_STR)
                {
                    (void)ipssh_buffer_put_sshbuf(buf, msg->value, msg->msg_len);
                }
                else if(msg->type == IPSSH_MPINT)
                {
                    (void)ipssh_buffer_put_ssh_mpint(buf, msg->value, msg->msg_len);
                }
                else if(msg->type == IPSSH_RAW || msg->type == IPSSH_INT)
                {
                    (void)ipcom_buffer_put(buf, msg->value, msg->msg_len);
                }
#if defined(IPSSH_DEBUG) && defined(IPSSH_HASH_DBG)
                ipcom_debug_printf("dumping hash comp %d\n", i);
                ipcom_buffer_dump(buf);
                ipcom_debug_printf("\n");
#endif
                (void)EVP_DigestUpdate(md_ctx, ipcom_buffer_start_ptr(buf),ipcom_buffer_len(buf));
                found = 1;
                ipcom_buffer_reset(buf);
                break;
            }
        }
        if(!found)
        {
            IPCOM_LOG1(ERR, "ipssh_calc_hash :: message no %d not found", i);
            rc = IPCOM_ERR_FAILED;
            goto cleanup;
        }
    }

    *hash_len = md_ctx->digest->md_size;
    *hash_value = ipcom_malloc(*hash_len);
    if(!*hash_value)
    {
        IPCOM_LOG0(ERR, "ipssh_calc_hash :: out of memory");
        rc = IPCOM_ERR_FAILED;
        goto cleanup;
    }
    (void)EVP_DigestFinal(md_ctx, *hash_value, IP_NULL);
 cleanup:
    if(buf)
        ipcom_buffer_free(buf);
    if(md_ctx)
        (void)EVP_MD_CTX_destroy(md_ctx);

    return rc;
}

/*
 *===========================================================================
 *                    ipssh_verify_signature
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 *
 */
IP_GLOBAL Ip_err
ipssh_verify_signature(EVP_PKEY* srv_key, Ip_u8* data, Ip_u32 data_len, const char* sig_type, Ip_u8* signature, Ip_u32 sig_len)
{
    DSA_SIG* dsa_sig = IP_NULL;
    EVP_MD_CTX md;

    Ip_err rc = IPCOM_ERR_FAILED;

	EVP_MD_CTX_init(&md);

    switch(srv_key->type)
    {
    case NID_dsa:

        if(ipcom_strcmp(sig_type, IPSSH_DSA_STR) != 0 || sig_len != 2*IPSSH_SIG_LEN)
            return IPCOM_ERR_INVALID_ARG;
        dsa_sig = DSA_SIG_new();
        if(!dsa_sig)
            return IPCOM_ERR_NO_MEMORY;
        dsa_sig->r = BN_new();
        dsa_sig->s = BN_new();
        if(!dsa_sig->r || !dsa_sig->s)
        {
            rc = IPCOM_ERR_NO_MEMORY;
            goto cleanup;
        }
        (void)BN_bin2bn(signature, IPSSH_SIG_LEN, dsa_sig->r);
        (void)BN_bin2bn(signature+IPSSH_SIG_LEN, IPSSH_SIG_LEN, dsa_sig->s);

        if (!EVP_VerifyInit(&md, EVP_dss1()) ||
            !EVP_VerifyUpdate(&md, data, data_len) ||
            !EVP_VerifyFinal(&md, signature, sig_len, srv_key))
        {
            rc = IPCOM_ERR_FAILED;
            goto cleanup;
        }
        rc = IPCOM_SUCCESS;
        break;

#ifdef IPSSH_USE_RSA
    case NID_rsaEncryption:
        if(ipcom_strcmp(sig_type, IPSSH_RSA_STR) != 0)
            return IPCOM_ERR_INVALID_ARG;

        if (!EVP_VerifyInit(&md, EVP_sha1()) ||
            !EVP_VerifyUpdate(&md, data, data_len) ||
            !EVP_VerifyFinal(&md, signature, sig_len, srv_key))
        {
            rc = IPCOM_ERR_FAILED;
            goto cleanup;
        }
        rc = IPCOM_SUCCESS;
        break;
#endif
    default:
        IP_PANIC();
    }

 cleanup:
    if(dsa_sig)
        DSA_SIG_free(dsa_sig);
    EVP_MD_CTX_cleanup(&md);

    return rc;
}


/*
 *===========================================================================
 *                    ipssh_dss_sign
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 *
 */
IP_GLOBAL Ip_err
ipssh_dss_sign(Ipssh_conn* ssh_conn,
               Ip_u8* data,
               Ip_u32 data_len,
               Ipcom_buffer* buf)
{
    Ip_u8 *raw_sig;
    Ip_u8 *tmp_raw_sig;
    DSA_SIG *sig = IP_NULL;
    const EVP_MD *evp_md = EVP_dss1();
    EVP_MD_CTX md;
    Ip_s32 r_len, s_len, rc=0;
    Ip_u32 len;
    Ip_size_t blob_len_pos;
    int pad, i;
    char* len_ptr;
    EVP_PKEY* pkey = IP_NULL;
    unsigned int sig_size;

    IPCOM_LOG0(DEBUG2, "ipssh_dss_sign");

    if(ipcom_buffer_capacity(buf) < 60)
    {
        IPCOM_LOG0(ERR, "ipssh_dss_sign :: too little buffer space");
        return IPCOM_ERR_FAILED;
    }

    pkey = EVP_PKEY_new();
    if (!pkey)
    {
        IPCOM_LOG0(ERR, "ipssh_dss_sign :: EVP_PKEY_new() failed");
        return IPCOM_ERR_FAILED;
    }
    EVP_PKEY_set1_DSA(pkey, ssh_conn->srv_dsa_key);

    sig_size = EVP_PKEY_size(pkey);
    raw_sig = ipcom_malloc(sig_size);
    if (!raw_sig)
    {
        IPCOM_LOG0(ERR, "ipssh_dss_sign :: out of memory");
        return IPCOM_ERR_FAILED;
    }

    if (!EVP_SignInit(&md, evp_md)
        || !EVP_SignUpdate(&md, data, data_len)
        || !EVP_SignFinal(&md, raw_sig, &sig_size, pkey))
    {
        IPCOM_LOG0(ERR, "ipssh_dss_sign :: EVP_Sign*() failed");
        rc = IPCOM_ERR_FAILED;
        goto cleanup;
    }
    tmp_raw_sig = raw_sig;
    sig = d2i_DSA_SIG(IP_NULL, &tmp_raw_sig, sig_size);
    if (sig == IP_NULL) {
        IPCOM_LOG0(ERR, "ipssh_dss_sign :: d2i_DSA_SIG() failed");
        rc = IPCOM_ERR_FAILED;
        goto cleanup;
    }

    r_len = BN_num_bytes(sig->r);
    s_len = BN_num_bytes(sig->s);
    if (r_len > IPSSH_MAX_INT_LEN || s_len > IPSSH_MAX_INT_LEN) {
        IPCOM_LOG2(ERR, "ipssh_dss_sign :: bad sig size %d %d", r_len, s_len);
        rc = IPCOM_ERR_FAILED;
        goto cleanup;
    }

    len = (Ip_u32)ipcom_buffer_len(buf);
    blob_len_pos = ipcom_buffer_len(buf);
    (void)ipcom_buffer_put_int(buf, 0);

    (void)ipssh_buffer_put_sshstr(buf, IPSSH_DSA_STR);

    IPCOM_LOG2(DEBUG2, "ipssh_dss_sign :: r_len: %d, s_len: %d", r_len, s_len);

    (void)ipcom_buffer_put_int(buf, ip_htonl(2*IPSSH_SIG_LEN));

    for(i=0,pad=IPSSH_SIG_LEN-r_len; i<pad; i++)
        (void)ipcom_buffer_put_char(buf, 0);
    (void)BN_bn2bin(sig->r, ipcom_buffer_end_ptr(buf));
    (void)ipcom_buffer_decr_cap(buf, r_len);

    for(i=0,pad=IPSSH_SIG_LEN-s_len; i<pad; i++)
        (void)ipcom_buffer_put_char(buf, 0);
    (void)BN_bn2bin(sig->s, ipcom_buffer_end_ptr(buf));
    (void)ipcom_buffer_decr_cap(buf, s_len);

    len_ptr = ipcom_buffer_ptr2pos(buf, blob_len_pos);
    IP_SET_32ON8(len_ptr, ip_htonl(((Ip_u32)ipcom_buffer_len(buf)-len-4))); /*lint !e666*/

 cleanup:
    EVP_PKEY_free(pkey);
    (void)EVP_MD_CTX_cleanup(&md);
    if(raw_sig)
        ipcom_free(raw_sig);
    if(sig)
        DSA_SIG_free(sig);
    return rc;

}

#ifdef IPSSH_USE_RSA
/*
 *===========================================================================
 *                    ipssh_rsa_sign
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 *
 */
IP_GLOBAL Ip_err
ipssh_rsa_sign(Ipssh_conn* ssh_conn,
               Ip_u8* data,
               Ip_u32 data_len,
               Ipcom_buffer* buf)
{
    char* len_ptr;
    Ip_u32 len;
    Ip_size_t len2;
    int rsa_blob_len;
    unsigned int sig_len;
	EVP_MD_CTX md;
    EVP_PKEY* pkey = IP_NULL;

    IPCOM_LOG0(DEBUG2, "ipssh_rsa_sign");

    /* Length of whole signature */
    len = (Ip_u32)ipcom_buffer_len(buf);
    (void)ipcom_buffer_put_int(buf, 0);

    (void)ipssh_buffer_put_sshstr(buf, IPSSH_RSA_STR);

    /* Length of the RSA signature blob */
    len2 = ipcom_buffer_len(buf);
    (void)ipcom_buffer_put_int(buf, 0);

    if((int)ipcom_buffer_capacity(buf) < RSA_size(ssh_conn->srv_rsa_key))
    {
        IPCOM_LOG0(ERR, "ipssh_rsa_sign :: too little buffer space");
        return IPCOM_ERR_FAILED;
    }

    rsa_blob_len = RSA_size(ssh_conn->srv_rsa_key);
    pkey = EVP_PKEY_new();
    if (!pkey)
    {
        IPCOM_LOG0(ERR, "ipssh_rsa_sign :: EVP_PKEY_new() failed");
        return IPCOM_ERR_FAILED;
    }
    EVP_PKEY_set1_RSA(pkey, ssh_conn->srv_rsa_key);
	if (!EVP_SignInit(&md, EVP_sha1()) ||
        !EVP_SignUpdate(&md, data, data_len) ||
        !EVP_SignFinal(&md, ipcom_buffer_end_ptr(buf), &sig_len, pkey))
    {
        IPCOM_LOG0(ERR, "ipssh_rsa_sign :: EVP_Sign failed");
        EVP_PKEY_free(pkey);
        return IPCOM_ERR_FAILED;
    }
    EVP_PKEY_free(pkey);
    EVP_MD_CTX_cleanup(&md);
    (void)ipcom_buffer_decr_cap(buf, sig_len);

    /* Set length of RSA signature blob */
    len_ptr = ipcom_buffer_ptr2pos(buf, len2);
    IP_SET_32ON8(len_ptr, ip_htonl(rsa_blob_len)); /*lint !e666*/

    /* Set lenght of whole signature */
    len_ptr = ipcom_buffer_ptr2pos(buf, len);
    IP_SET_32ON8(len_ptr, ip_htonl(((Ip_u32)ipcom_buffer_len(buf)-len-4))); /*lint !e666*/

    return IPCOM_SUCCESS;
}
#endif /* IPSSH_USE_RSA */


/*
 *===========================================================================
 *                    ipssh_sign_hash
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 *
 */
IP_GLOBAL Ip_err
ipssh_sign_hash(Ipssh_conn* ssh_conn, Ipcom_buffer* pkt)
{
    /* Sign hash with server key */
    switch (ssh_conn->conn_prop_pending.srv_host_key_alg[0])
    {
    case IPSSH_DSA_ID:
        if(ipssh_dss_sign(ssh_conn, ssh_conn->hash, ssh_conn->hash_len, pkt))
        {
            IPCOM_LOG0(ERR, "ipssh_sign_hash :: ipssh_dss_sign() failed");
            return IPCOM_ERR_FAILED;
        }
        break;

#ifdef IPSSH_USE_RSA
    case IPSSH_RSA_ID:
        if(ipssh_rsa_sign(ssh_conn, ssh_conn->hash, ssh_conn->hash_len, pkt))
        {
            IPCOM_LOG0(ERR, "ipssh_sign_hash :: ipssh_rsa_sign() failed");
            return IPCOM_ERR_FAILED;
        }
        break;
#endif

    default:
        IP_PANIC();
        return IPCOM_ERR_FAILED;
    }

    return IPCOM_SUCCESS;
}

/*
 *===========================================================================
 *                    ipssh_send_kex_dh_reply
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 *
 */
IP_GLOBAL Ip_err
ipssh_send_kex_dh_reply(Ipssh_conn* ssh_conn)
{
    Ip_size_t len;
    Ip_size_t f_pos;
    Ip_size_t host_key_pos;
    Ipcom_buffer* pkt;
    Ip_err rc;

    IPCOM_LOG0(DEBUG, "ipssh_send_kex_dh_reply()");

    pkt = ipssh_buffer_new_v2_pkt(2048, IPSSH_MSG_KEXDH_REPLY);
    if(!pkt)
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: out of memory");
        return IPCOM_ERR_FAILED;
    }

    /* Add server key */
    len = ipcom_buffer_len(pkt);
    host_key_pos = ipcom_buffer_len(pkt);
    switch (ssh_conn->conn_prop_pending.srv_host_key_alg[0])
    {
    case IPSSH_DSA_ID:
        if(ipssh_buffer_put_dsa_key(pkt, ssh_conn->srv_dsa_key))
        {
            IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_buffer_put_dsa_key() failed");
            goto failed;
        }
        break;
#ifdef IPSSH_USE_RSA
    case IPSSH_RSA_ID:
        if(ipssh_buffer_put_rsa_key(pkt, ssh_conn->srv_rsa_key))
        {
            IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_buffer_put_rsa_key() failed");
            goto failed;
        }
        break;
#endif
    default:
        IP_PANIC();
        goto failed;
    }

    if(ipssh_msg2list((Ipssh_msg_list*)&(ssh_conn->handshake_msgs),
                      IPSSH_RAW,
                      (Ip_u8*)ipcom_buffer_ptr2pos(pkt, host_key_pos),
                      ipcom_buffer_len(pkt) - host_key_pos,
                      4))
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_msg2list() failed for DSA key");
        goto failed;
    }

    /* Add f */
    f_pos = ipcom_buffer_len(pkt);
    rc = ipssh_buffer_put_bn(pkt, ssh_conn->dh_data->dh->pub_key);
    if (rc < 0)
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_buffer_put_bn() failed");
        goto failed;
    }

    if(ipssh_msg2list((Ipssh_msg_list*)&(ssh_conn->handshake_msgs),
                      IPSSH_MPINT,
                      (Ip_u8*)IPSSH_GET_MPINT_VALUE(ipcom_buffer_ptr2pos(pkt, f_pos)),
                      ipcom_buffer_len(pkt) - f_pos - 4,
                      6))
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_msg2list() failed for f");
        goto failed;
    }

    /* Calculate hash */
    if (ssh_conn->hash)
        ipcom_free(ssh_conn->hash);
    if(ipssh_calc_hash((Ipssh_msg_list*)&(ssh_conn->handshake_msgs), ssh_conn->hash_func, 8,
                        &(ssh_conn->hash), &(ssh_conn->hash_len)))
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_calc_hash() failed");
        goto failed;
    }
    if (ssh_conn->sess_id_len == 0)
    {
        /* The session ID is only updated in the inital key exchange */
        ssh_conn->sess_id_len = ssh_conn->hash_len;
        ssh_conn->sess_id = ipcom_malloc(ssh_conn->hash_len);
        if(!ssh_conn->sess_id)
        {
            IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: out of memory");
            goto failed;
        }

        ipcom_memcpy(ssh_conn->sess_id, ssh_conn->hash, ssh_conn->sess_id_len);
    }

    /* Sign hash with server key */
    switch (ssh_conn->conn_prop_pending.srv_host_key_alg[0])
    {
    case IPSSH_DSA_ID:
        if(ipssh_dss_sign(ssh_conn, ssh_conn->hash, ssh_conn->hash_len, pkt))
        {
            IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_dss_sign() failed");
            goto failed;
        }
        break;

#ifdef IPSSH_USE_RSA
    case IPSSH_RSA_ID:
        if(ipssh_rsa_sign(ssh_conn, ssh_conn->hash, ssh_conn->hash_len, pkt))
        {
            IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_rsa_sign() failed");
            goto failed;
        }
        break;
#endif

    default:
        IP_PANIC();
        goto failed;
    }

    if(ipssh_send_pkt(ssh_conn, pkt, IP_TRUE))
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_send_pkt() failed");
        return IPCOM_ERR_FAILED;
    }


    if(ipssh_compute_keys(ssh_conn))
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_compute_keys() failed");
        return IPCOM_ERR_FAILED;
    }

    if(ipssh_send_new_key_msg(ssh_conn))
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_send_new_key() failed");
        return IPCOM_ERR_FAILED;
    }
    ssh_conn->conn_state = IPSSH_CONN_STATE_SRV_NEW_KEY;
    return IPCOM_SUCCESS;

failed:
    ipcom_buffer_free(pkt);
    return IPCOM_ERR_FAILED;
}


/*
 *===========================================================================
 *                    ipssh_send_kex_dh_init
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 *
 */
IP_GLOBAL Ip_err
ipssh_send_kex_dh_init(Ipssh_conn* ssh_conn)
{
    Ip_err rc = IPCOM_ERR_FAILED;
    Ipcom_buffer* pkt;
    Ip_size_t pos;
    Ip_size_t len;

    IPCOM_LOG1(DEBUG, "ipssh_send_kex_dh_init :: ssh_conn: %x", ssh_conn);

    pkt = ipssh_buffer_new_v2_pkt(1024, IPSSH_MSG_KEXDH_INIT);
    if(!pkt)
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_init :: out of memory");
        return IPCOM_ERR_FAILED;
    }

    ssh_conn->dh_data->dh = ipssh_create_dh_group(ssh_conn->conn_prop_pending.kex_alg[0]);
    if(!ssh_conn->dh_data->dh)
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_init :: ipssh_create_dh_group() failed");
        goto cleanup;
    }

    /* Reduce the size of the DH private key for large DH groups. It is unnecessarily big by
     * default, which increases the connection time */
    if (ipssh_dh_priv_key_set_length(ssh_conn) != IPCOM_SUCCESS)
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_init :: ipssh_dh_priv_key_set_length() failed");
        goto cleanup;
    }

    /* Generate random number y and create e = g^x mod p */
    if(!DH_generate_key(ssh_conn->dh_data->dh))
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_init :: DH_generate_key() failed");
        goto cleanup;
    }

    pos = ipcom_buffer_len(pkt);
    if(ipssh_buffer_put_bn(pkt, ssh_conn->dh_data->dh->pub_key) < 0)
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_init :: ipssh_buffer_put_bn() failed");
        goto cleanup;
    }
    pos += 4; /* proceed past length field */
    len = ipcom_buffer_len(pkt) - pos ;
    if(ipssh_msg2list((Ipssh_msg_list*)&(ssh_conn->handshake_msgs),
                      IPSSH_MPINT,
                      (Ip_u8*)ipcom_buffer_ptr2pos(pkt, pos),
                      len,
                      5))
    {
        IPCOM_LOG0(ERR, "ipssh_send_kex_dh_reply :: ipssh_msg2list() failed for f");
        goto cleanup;
    }

    if(ipssh_send_pkt(ssh_conn, pkt, IP_FALSE) == IPCOM_SUCCESS)
    {
        rc = IPCOM_SUCCESS;
    }

 cleanup:
    if(pkt)
        ipcom_buffer_free(pkt);

    return rc;
}


/*
 *===========================================================================
 *                    ipssh_msg_kex_dh_init
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 *
 */
IP_GLOBAL Ip_err
ipssh_msg_kex_dh_init(Ipssh_conn* ssh_conn)
{
    Ip_u8* mp_int_value;
    Ip_s32 len;
#ifdef IP_ASSERT
    int secr_len;
#endif

    IPCOM_LOG0(DEBUG, "ipssh_msg_kex_dh_init()");
    if(ssh_conn->skip_guessed_kex_pkt)
    {
        IPCOM_LOG0(DEBUG, "ipssh_msg_kex_dh_init :: skipping guessed packet");
        ssh_conn->skip_guessed_kex_pkt = 0;
        (void)ipssh_consume_pkt(ssh_conn->recv_plain_buf);
        return IPCOM_SUCCESS;
    }

    if (ssh_conn->dh_data->dh)
        DH_free(ssh_conn->dh_data->dh);
    ssh_conn->dh_data->dh = ipssh_create_dh_group(ssh_conn->conn_prop_pending.kex_alg[0]);
    if(!ssh_conn->dh_data->dh)
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kex_dh_init :: ipssh_create_dh_group() failed");
        return IPCOM_ERR_FAILED;
    }

    /* Reduce the size of the DH private key for large DH groups. It is unnecessarily big by
     * default, which increases the connection time */
    if (ipssh_dh_priv_key_set_length(ssh_conn) != IPCOM_SUCCESS)
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kex_dh_init :: ipssh_dh_priv_key_set_length() failed");
        return IPCOM_ERR_FAILED;
    }

    /* Generate random number y and create f = g^y mod p */
    if(!DH_generate_key(ssh_conn->dh_data->dh))
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kex_dh_init :: DH_generate_key() failed");
        return IPCOM_ERR_FAILED;
    }

    /* Get e from KEY_DH_INIT message */
    if (ipcom_buffer_consume(ssh_conn->recv_plain_buf, 6)
        || ipssh_buffer_get_int(ssh_conn->recv_plain_buf, &len))
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kex_dh_init :: Failed to unpack e length");
        return IPCOM_ERR_FAILED;
    }
    if (len == 0 || len != (Ip_s32)ipcom_buffer_len(ssh_conn->recv_plain_buf))
    {
        IPCOM_LOG1(ERR, "ipssh_msg_kex_dh_init :: Invalid length of e: %d", len);
        return IPCOM_ERR_FAILED;
    }
    mp_int_value = ipcom_buffer_start_ptr(ssh_conn->recv_plain_buf);
    if (ssh_conn->dh_data->e)
        BN_free(ssh_conn->dh_data->e);
    ssh_conn->dh_data->e = BN_bin2bn(mp_int_value, len, IP_NULL);
    if(!ssh_conn->dh_data->e)
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kex_dh_init :: BN_bin2bn() failed for e");
        return IPCOM_ERR_FAILED;
    }
    (void)ipcom_buffer_consume(ssh_conn->recv_plain_buf, len);
    if(ipssh_msg2list((Ipssh_msg_list*)&(ssh_conn->handshake_msgs), /* Store e for later hash calculation */
                       IPSSH_MPINT,
                       mp_int_value,
                       len,
                       5))
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kex_dh_init :: ipssh_msg2list() failed");
        return IPCOM_ERR_FAILED;
    }

    /* Calculate the shared secret */
    ssh_conn->secr_len = DH_size(ssh_conn->dh_data->dh);
#ifdef IP_ASSERT
    secr_len = ssh_conn->secr_len;
#endif
    if (ssh_conn->shared_secret)
        ipcom_free(ssh_conn->shared_secret); /* Free old shared secret, re-keying */
    ssh_conn->shared_secret = ipcom_malloc(ssh_conn->secr_len+1);
    if(!ssh_conn->shared_secret) {
        IPCOM_LOG0(ERR, "ipssh_msg_kex_dh_init :: out of memory");
        return IPCOM_ERR_FAILED;
    }
    ssh_conn->secr_len = DH_compute_key(ssh_conn->shared_secret, ssh_conn->dh_data->e, ssh_conn->dh_data->dh);
    if(ssh_conn->secr_len < 0)
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kex_dh_init :: DH_compute_key() failed");
        return IPCOM_ERR_FAILED;
    }
    ip_assert(ssh_conn->secr_len <= secr_len);
    /* If the shared secret is a negative number, we must add a zero byte */
    if (ssh_conn->shared_secret[0] & 0x80)
    {
        ipcom_memmove(ssh_conn->shared_secret+1, ssh_conn->shared_secret, ssh_conn->secr_len);
        ssh_conn->shared_secret[0] = 0;
        ssh_conn->secr_len++;
    }
    if(ipssh_msg2list((Ipssh_msg_list*)&(ssh_conn->handshake_msgs), /* Store shared secret for later hash calculation */
                       IPSSH_MPINT,
                       ssh_conn->shared_secret,
                       ssh_conn->secr_len,
                       7))
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kex_dh_init :: ipssh_msg2list() failed");
        return IPCOM_ERR_FAILED;
    }

    /* Send SSH_MSG_KEXDH_REPLY */
    return ipssh_send_kex_dh_reply(ssh_conn);
}

/*
 *===========================================================================
 *                    ipssh_msg_kexdh_reply
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 *
 */
IP_GLOBAL Ip_err
ipssh_msg_kexdh_reply(Ipssh_conn* ssh_conn)
{
    Ip_u8 pkt_type;
    EVP_PKEY* srv_pub_key;
    char* sig_type = IP_NULL;
    Ip_u8* signature = IP_NULL;
    Ip_u32 sign_len;
    Ip_err rc = IPCOM_ERR_FAILED;
    Ip_u8* ptr;
    Ip_ptrdiff_t len;
    union Ip_sockaddr_union peer_addr;
    Ip_socklen_t peer_len;

    pkt_type = *ipcom_buffer_ptr2pos(ssh_conn->recv_plain_buf, 5);
    if(pkt_type != IPSSH_MSG_KEXDH_REPLY)
    {
        IPCOM_LOG1(ERR, "ipssh_msg_kexdh_reply :: unexpected pkt type: %d", pkt_type);
        IP_PANIC();
        return IPCOM_ERR_FAILED;
    }

    /* Unpack message */
    (void)ipcom_buffer_consume(ssh_conn->recv_plain_buf, 6);
    ptr = ipcom_buffer_start_ptr(ssh_conn->recv_plain_buf);
    if((srv_pub_key=ipssh_buffer_get_pub_key(ssh_conn->recv_plain_buf)) == IP_NULL)
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: failed to unpack srv pub key");
        goto cleanup;
    }
    len = ipcom_buffer_start_ptr(ssh_conn->recv_plain_buf) - ptr;
    if(ipssh_msg2list((Ipssh_msg_list*)&(ssh_conn->handshake_msgs),
                       IPSSH_RAW,
                       ptr,
                       len,
                       4))
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: ipssh_msg2list() failed");
        goto cleanup;
    }

    ptr = ipcom_buffer_start_ptr(ssh_conn->recv_plain_buf);
    if (ssh_conn->dh_data->f)
        BN_free(ssh_conn->dh_data->f);
    if(ipssh_buffer_get_ssh_mpint2(ssh_conn->recv_plain_buf, &(ssh_conn->dh_data->f)) != IPCOM_SUCCESS)
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: failed to unpack f");
        goto cleanup;
    }
    ptr += 4; /* Skip length field */
    len = ipcom_buffer_start_ptr(ssh_conn->recv_plain_buf) - ptr;
    if(ipssh_msg2list((Ipssh_msg_list*)&(ssh_conn->handshake_msgs),
                       IPSSH_MPINT,
                       ptr,
                       len,
                       6))
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: ipssh_msg2list() failed for f");
        goto cleanup;
    }
    /* Calculate the shared secret */
    ssh_conn->secr_len = DH_size(ssh_conn->dh_data->dh);
    ssh_conn->shared_secret = ipcom_malloc(ssh_conn->secr_len+1);
    if(!ssh_conn->shared_secret) {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: out of memory");
        return IPCOM_ERR_FAILED;
    }
    ssh_conn->secr_len = DH_compute_key(ssh_conn->shared_secret, ssh_conn->dh_data->f, ssh_conn->dh_data->dh);
    if(ssh_conn->secr_len < 0)
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: DH_compute_key() failed");
        return IPCOM_ERR_FAILED;
    }
    /* If the shared secret is a negative number, we must add a zero byte */
    if (ssh_conn->shared_secret[0] & 0x80)
    {
        ipcom_memmove(ssh_conn->shared_secret+1, ssh_conn->shared_secret, ssh_conn->secr_len);
        ssh_conn->shared_secret[0] = 0;
        ssh_conn->secr_len++;
    }
    if(ipssh_msg2list((Ipssh_msg_list*)&(ssh_conn->handshake_msgs), /* Store shared secret for later hash calculation */
                       IPSSH_MPINT,
                       ssh_conn->shared_secret,
                       ssh_conn->secr_len,
                       7))
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: ipssh_msg2list() failed");
        return IPCOM_ERR_FAILED;
    }

    if(ipcom_buffer_consume(ssh_conn->recv_plain_buf, 4)) /* Skip blob len field */
        return IPCOM_ERR_FAILED;
    sig_type=ipssh_buffer_get_sshstr(ssh_conn->recv_plain_buf);
    signature=ipssh_buffer_get_sshbuf(ssh_conn->recv_plain_buf, &sign_len);
    if(!sig_type || !signature)
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: failed to unpack signature");
        goto cleanup;
    }

    /* Calculate hash */
    if (ssh_conn->hash)
        ipcom_free(ssh_conn->hash);
    if (ipssh_calc_hash((Ipssh_msg_list*)&(ssh_conn->handshake_msgs), ssh_conn->hash_func, 8,
                        &(ssh_conn->hash), &(ssh_conn->hash_len)))
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: ipssh_calc_hash() failed");
        goto cleanup;
    }

    /* Verify server key */
    ipcom_memset(&peer_addr, 0, sizeof(peer_addr));
    peer_len = sizeof(peer_addr);
    if(ipcom_getpeername(ssh_conn->sock, &peer_addr.sa, &peer_len) != IPCOM_SUCCESS)
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: ipcom_getpeername() failed");
        goto cleanup;
    }
    if(ipssh_validate_srv_public_key_cb(srv_pub_key, &peer_addr) != IPCOM_SUCCESS)
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: ipssh_validate_srv_public_key_cb() failed");
        goto cleanup;
    }
    /* Verify signature */
    if(ipssh_verify_signature(srv_pub_key, ssh_conn->hash, ssh_conn->hash_len, sig_type, signature, sign_len) != IPCOM_SUCCESS)
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: ipssh_verify_signature() failed");
        goto cleanup;
    }

    if(ssh_conn->sess_id_len == 0)
    {
        ssh_conn->sess_id = ipcom_malloc(ssh_conn->hash_len);
        if(!ssh_conn->sess_id)
        {
            IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: out of memory");
            goto cleanup;
        }
        ssh_conn->sess_id_len = ssh_conn->hash_len;
        ipcom_memcpy(ssh_conn->sess_id, ssh_conn->hash, ssh_conn->sess_id_len);
    }

    if(ipssh_compute_keys(ssh_conn))
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: ipssh_compute_keys() failed");
        goto cleanup;
    }

    if(ipssh_send_new_key_msg(ssh_conn))
    {
        IPCOM_LOG0(ERR, "ipssh_msg_kexdh_reply :: ipssh_send_new_key() failed");
        goto cleanup;
    }

    rc = IPCOM_SUCCESS;

 cleanup:
    if(srv_pub_key)
        EVP_PKEY_free(srv_pub_key);
    if(sig_type)
        ipcom_free(sig_type);
    if(signature)
        ipcom_free(signature);

    return rc;
}


/*
 *===========================================================================
 *                    ipssh_do_keyex_dh_clt
 *===========================================================================
 * Description:
 * Parameters:
 * Returns:
 *
 */
IP_GLOBAL Ip_err
ipssh_do_keyex_dh_clt(Ipssh_conn* ssh_conn)
{
    Ip_err rc = IPCOM_ERR_FAILED;

    if(ipssh_send_kex_dh_init(ssh_conn) != IPCOM_SUCCESS)
    {
        IPCOM_LOG0(ERR, "ipssh_do_keyex_dh_clt :: ipssh_send_kex_dh_init() failed");
    }

    if(ipssh_read_v2_pkt(ssh_conn) != IPCOM_SUCCESS)
        goto cleanup;
    if(ipssh_msg_kexdh_reply(ssh_conn) != IPCOM_SUCCESS)
        goto cleanup;
    if(ipssh_read_v2_pkt(ssh_conn) != IPCOM_SUCCESS)
        goto cleanup;
    if(ipssh_msg_new_key(ssh_conn) != IPCOM_SUCCESS)
        goto cleanup;

    rc = IPCOM_SUCCESS;

 cleanup:
    return rc;
}


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