/*
* Creation date : Tues Mar 20 09:00:00 2007
* Last modified : %modify_time%
*/
/** @file
* \brief This file contains implementation of 
* DH (Diffie-Hellman key exchange) low level functions. 
*
* \version LLF_DH.c#1:csrc:1
* \author Yermalayeu Ihar
* \remarks Copyright (C) 2007 by Discretix Technologies Ltd.
* All Rights reserved
*/

/************************ Include Files ***********************/

#include "LLF_ECPKI_Common.h"
#include "LLF_DH.h"

/************************ Defines *****************************/

#define LLF_DH_ASN1_OTHER_INFO_MAX_SIZE 6
#define LLF_DH_CONCAT_OTHER_INFO_MAX_SIZE 4

/************************ Enums *******************************/
/************************ Typedefs ****************************/
/************************ Global Data *************************/
/************************ Private function prototype **********/
/************************ Private Functions *******************/

/**
****************************************************************
* Function Name: 
*  LLF_DH_ANSI_X942_GeneratePubPrv
*
*  @param Generator_ptr [in] - Pointer to the Generator octet string
*  @param GeneratorSize [in] - Size of the Generator String (in bytes)
*  @param Prime_ptr [in] - Pointer to the Prime octet string
*  @param PrimeSize [in] - Size of the Prime string (in bytes)
*  @param Q_ptr [in] - Pointer to the Q octet string 1 <= Prv <= q-1 or 1 < Prv < q-1
*  @param QSize	[in] - Size of the Q string (in bytes)  				 				  
*  @param ClientPrvKey_ptr [out] - Pointer to the Private key octet string
*  @param ClientPrvKeySize_ptr [in/out] - The user should provide the size of the 
*                                        buffer indicated by ClientPrvKey_ptr
*                                        The function returns the actual size 
*                                        in bytes of the Private key Size
*  @param ClientPub_ptr [out] - Pointer to the Public key octet string
*                              This Buffer should be at least PrimeSize bytes
*  @param ClientPubSize_ptr [in/out] - The user should provide the size of the buffer 
*                                     indicated by ClientPub_ptr
*                                     The function returns the actual size in bytes 
*                                     of the client public buffer
*
* @returns \b
*  CE2Error_t 
*  - CE2_OK - on success
*  - Otherwise - error code defined in CE2_DH_error.h:
*
* \brief \b 
* Description:
*  LLF_DH_ANSI_X942_GeneratePubPrv has 2 functionalities:
*  -# randomly generates the Client private key 
*  -# compute the Client public key which is 
*     ClientPub = Generator ^ Prv mod Prime 
*
*  \b 
* Algorithm:
*  -# Client buffers size validation
*  -# Initialization of LibTomCrypt
*  -# Verify validity g, q, and p
*  -# Client private key generation
*  -# Calculate client public key
***************************************************************/
CE2Error_t LLF_DH_ANSI_X942_GeneratePubPrv(
         DxUint8_t *Generator_ptr, DxUint16_t GeneratorSize,
         DxUint8_t *Prime_ptr, DxUint16_t PrimeSize,
         DxUint8_t *Q_ptr, DxUint16_t QSize,
         DxUint8_t *ClientPrvKey_ptr, DxUint16_t *ClientPrvKeySize_ptr,
         DxUint8_t *ClientPub_ptr, DxUint16_t *ClientPubSize_ptr)
{
  prng_state prng;
  void *g, *p, *q, *t0, *t1, *t2, *prv, *pbl;
  int error_code, res, wprng;
  CE2Error_t error, result = CE2_OK;
  DxUint8_t *buff = NULL;
  DxUint32_t front_zero, size;

  /* Client buffers size validation */
  if (*ClientPrvKeySize_ptr < QSize || *ClientPubSize_ptr < PrimeSize) {
    *ClientPrvKeySize_ptr = QSize;
    *ClientPubSize_ptr = PrimeSize;
    return CE2_LLF_ECPKI_MODULE_ERROR_BASE;
  } else {
    *ClientPrvKeySize_ptr = QSize;
    *ClientPubSize_ptr = PrimeSize;
  }
  
  /* Initialize ltc_mp structure */
  ltc_mp = ltm_desc;

  error_code = ltc_init_multi(&g, &p, &q, &t0, &t1, &t2, &prv, &pbl, NULL);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /* Read g, p, and q LibTomCrypt primitives */
  error_code = ltc_mp.unsigned_read(g, Generator_ptr, GeneratorSize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(p, Prime_ptr, PrimeSize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(q, Q_ptr, QSize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

#ifdef LLF_VERIFY_G_P_Q
  /**************************/
  /* g, p, and q validation */
  /**************************/
  /* q - must be prime number */
  error_code = ltc_mp.isprime(q, &res);
  if(error_code != CRYPT_OK || res != LTC_MP_YES) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  /* p - must be prime number */
  error_code = ltc_mp.isprime(p, &res);
  if(error_code != CRYPT_OK || res != LTC_MP_YES) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /* p = j*q + 1 (j > 1); => (p - 1)/q = j > 1 */
  /* t0 = p - 1 */
  error_code = ltc_mp.subi(p, 1, t0);
  if(error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  /*  t1 = (p - 1)/q; t2 = (p - 1) mod q*/
  error_code = ltc_mp.mpdiv(t0, q, t1, t2);
  if(error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  /* t1 > 1 */
  res = ltc_mp.compare_d(t1, 1);
  if (res != LTC_MP_GT) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  /* t2 = 0 */
  res = ltc_mp.compare_d(t2, 0);
  if (res != LTC_MP_EQ) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /* g > 1 */
  res = ltc_mp.compare_d(g, 1);
  if (res != LTC_MP_GT) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /* g < p - 1 (t0 = p - 1)*/
  res = ltc_mp.compare(g, t0);
  if (res != LTC_MP_LT) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /* g^q mod p = 1 */
  error_code = ltc_mp.exptmod(g, q, p, t1);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  res = ltc_mp.compare_d(t1, 1);
  if (res != LTC_MP_EQ) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
#endif

  /*********************************/
  /* Client private key generation */
  /*********************************/

  /* Pseudo-random generator initialization */
  error = LLF_ECPKI_PRNG_Init(&prng, &wprng);
  if (error != CE2_OK) {
    fortuna_done(&prng);
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  } else {
    do {
      /* make up random string */
      error_code = prng_descriptor[wprng].read(ClientPrvKey_ptr, 
        (unsigned int)QSize, &prng);
      if (error_code != (unsigned int)QSize) {
        fortuna_done(&prng);
        result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
        goto error_case;
      }
      /* convert to LibTomCrypt primitive */
      error_code = ltc_mp.unsigned_read(prv, ClientPrvKey_ptr, 
        (unsigned int)QSize);
      if (error_code != CRYPT_OK) {
        fortuna_done(&prng);
        result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
        goto error_case;
      }
      res = ltc_mp.compare(prv, q);
    } while (res != LTC_MP_LT);
    fortuna_done(&prng);
  }

  /******************************************/
  /*  Calculate client public key           */
  /*  ClientPub = Generator ^ Prv mod Prime */ 
  /******************************************/

  /* pbl = g^prv mod p */
  error_code = ltc_mp.exptmod(g, prv, p, pbl);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  /* pbl write to big-endian ClientPub_ptr */
  size = ltc_mp.unsigned_size(pbl);
  if (size == *ClientPubSize_ptr) {
    error_code = ltc_mp.unsigned_write(pbl, ClientPub_ptr);
    if(error_code != CRYPT_OK) {
      result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
      goto error_case;
    }
  } else {
    buff = malloc(size);
	if (buff == NULL) {
		result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
		goto error_case;
	}
    error_code = ltc_mp.unsigned_write(pbl, buff);
    if(error_code != CRYPT_OK) {
      result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
      goto error_case;
    }
    for (front_zero = 0; front_zero + size < *ClientPubSize_ptr; front_zero++)
      ClientPub_ptr[front_zero] = 0x00;
    memcpy(ClientPub_ptr + front_zero, buff, size);
  }

error_case:
  if (buff != NULL)
    free(buff);
  ltc_deinit_multi(g, p, q, t0, t1, t2, prv, pbl, NULL);
  return result;
} /* End of LLF_DH_ANSI_X942_GeneratePubPrv */

/**
****************************************************************
* Function Name: 
*  LLF_DH_PVCS3_GeneratePubPrv
*
*  @param Generator_ptr [in] - Pointer to the Generator octet string
*  @param GeneratorSize [in] - Size of the Generator String (in bytes)
*  @param Prime_ptr [in] - Pointer to the Prime octet string
*  @param PrimeSize [in] - Size of the Prime string (in bytes)
*  @param L [in] - The private value length in bits
*                 If L != 0 then - force the private key to be 2^(L-1) <= Prv < 2^l
*                 if L 0 then: 0< Prv < P-1
*  @param ClientPrvKey_ptr [out] - Pointer to the Private key octet string - 
*                                 This buffer should be at least the 
*                                 following size:
*                                 2^L - 1 - if L is provided
*                                 P-1	  - if L is DX_NULL
*  @param ClientPrvKeySize_ptr [in/out] - The user should provide the size of the 
*                                        buffer indicated by ClientPrvKey_ptr
*                                        The function returns the actual size 
*                                        in bytes of the Private key Size
*  @param ClientPub_ptr [out] - Pointer to the Public key octet string
*                              This Buffer should be at least PrimeSize bytes
*  @param ClientPubSize_ptr [in/out] - The user should provide the size of the buffer 
*                                     indicated by ClientPub_ptr
*                                     The function returns the actual size in bytes 
*                                     of the client public buffer
*
* @returns \b
*  CE2Error_t 
*  - CE2_OK - on success
*  - Otherwise - error code defined in CE2_DH_error.h:
*
* \brief \b 
* Description:
*  LLF_DH_PVCS3_GeneratePubPrv has 2 functionalities:
*  -# randomly generates the Client private key 
*  -# compute the Client public key which is 
*     ClientPub = Generator ^ Prv mod Prime 
*
*  \b 
* Algorithm:
*  -# Initialization of LibTomCrypt
*  -# Initialize min(L) and max(L) values for private key
*  -# Client buffers size validation
*  -# Client private key generation
*  -# Calculate client public key
***************************************************************/
CE2Error_t LLF_DH_PVCS3_GeneratePubPrv(
  DxUint8_t *Generator_ptr,
  DxUint16_t GeneratorSize,
  DxUint8_t *Prime_ptr,
  DxUint16_t PrimeSize,
  DxUint16_t L,
  DxUint8_t *ClientPrvKey_ptr,
  DxUint16_t *ClientPrvKeySize_ptr,
  DxUint8_t *ClientPub_ptr,
  DxUint16_t *ClientPubSize_ptr)
{
  prng_state prng;
  void *p, *g, *prv, *pbl, *min, *max;
  CE2Error_t result = CE2_OK, error;
  int error_code, res_min, res_max, wprng;
  unsigned int k; 
  DxUint8_t *buff = NULL;
  DxUint32_t front_zero, size;

  /* Initialize ltc_mp structure */
  ltc_mp = ltm_desc;

  error_code = ltc_init_multi(&g, &p, &prv, &pbl, &min, &max, NULL);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /* Read g and p LibTomCrypt primitives */
  error_code = ltc_mp.unsigned_read(g, Generator_ptr, GeneratorSize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(p, Prime_ptr, PrimeSize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /* Initialize min(L) and max(L) values values for private key */
  if (L == 0) {
    /* min = 0 */
    error_code = ltc_mp.set_int(min, 0);
    if (error_code != CRYPT_OK) {
      result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
      goto error_case;
    }
    /* max = p - 1 */
    error_code = ltc_mp.subi(p, 1, max);
    if (error_code != CRYPT_OK) {
      result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
      goto error_case;
    }
    /* size of private key in byte */
    k = PrimeSize;
  } else {
    /* min = 2^(L - 1) */
    error_code = ltc_mp.twoexpt(min , L - 1);
    if (error_code != CRYPT_OK) {
      result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
      goto error_case;
    }
    /* max = 2^L */
    error_code = ltc_mp.twoexpt(max , L);
    if (error_code != CRYPT_OK) {
      result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
      goto error_case;
    }
    /* size of private key in byte */
	k = L/8 + (L%8 > 0 ? 1 : 0);
  }

  /* Client buffers size validation */
  if (*ClientPrvKeySize_ptr < k || *ClientPubSize_ptr < PrimeSize) {
    *ClientPrvKeySize_ptr = k;
    *ClientPubSize_ptr = PrimeSize;
    return CE2_LLF_ECPKI_MODULE_ERROR_BASE;
  } else {
    *ClientPrvKeySize_ptr = k;
    *ClientPubSize_ptr = PrimeSize;
  }

  /*********************************/
  /* Client private key generation */
  /*********************************/

  /* Pseudo-random generator initialization */
  error = LLF_ECPKI_PRNG_Init(&prng, &wprng);
  if (error != CE2_OK) {
    fortuna_done(&prng);
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  } else {
    do {
      /* make up random string */
      error_code = prng_descriptor[wprng].read(ClientPrvKey_ptr, 
        (unsigned int)k, &prng);
      if (error_code != (unsigned int)k) {
        fortuna_done(&prng);
        result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
        goto error_case;
      }
      /* convert to LibTomCrypt primitive */
      error_code = ltc_mp.unsigned_read(prv, ClientPrvKey_ptr, 
        (unsigned int)k);
      if (error_code != CRYPT_OK) {
        fortuna_done(&prng);
        result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
        goto error_case;
      }
      /* min < prv < max */
      res_min = ltc_mp.compare(prv, min);
      res_max = ltc_mp.compare(prv, max);
    } while (res_min == LTC_MP_LT || res_max != LTC_MP_LT);
    fortuna_done(&prng);
  }

  /******************************************/
  /*  Calculate client public key           */
  /*  ClientPub = Generator ^ Prv mod Prime */ 
  /******************************************/

  /* pbl = g^prv mod p */
  error_code = ltc_mp.exptmod(g, prv, p, pbl);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  /* pbl write to big-endian ClientPub_ptr */
  size = ltc_mp.unsigned_size(pbl);
  if (size == *ClientPubSize_ptr) {
    error_code = ltc_mp.unsigned_write(pbl, ClientPub_ptr);
    if(error_code != CRYPT_OK) {
      result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
      goto error_case;
    }
  } else {
    buff = malloc(size);
	if (buff == NULL) {
		result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
		goto error_case;
	}
    error_code = ltc_mp.unsigned_write(pbl, buff);
    if(error_code != CRYPT_OK) {
      result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
      goto error_case;
    }
    for (front_zero = 0; front_zero + size < *ClientPubSize_ptr; front_zero++)
      ClientPub_ptr[front_zero] = 0x00;
    memcpy(ClientPub_ptr + front_zero, buff, size);
  }

error_case:
  if (buff != NULL)
    free(buff);
 ltc_deinit_multi(g, p, prv, pbl, min, max, NULL);
  return result;
} /* End of LLF_DH_PVCS3_GeneratePubPrv */

/************************ Public Functions ********************/

/**
****************************************************************
* Function Name: 
*  LLF_DH_GeneratePubPrv
*
*  @param Generator_ptr [in] - Pointer to the Generator octet string
*  @param GeneratorSize [in] - Size of the Generator String (in bytes)
*  @param Prime_ptr [in] - Pointer to the Prime octet string
*  @param PrimeSize [in] - Size of the Prime string (in bytes)
*  @param L [in] - Relevant only for PKCS#3, the private value length in bits
*                 If L != 0 then - force the private key to be 2^(L-1) <= Prv < 2^l
*                 if L 0 then: 0< Prv < P-1
*                 In ANSI X9.42 L is irrelevant  
*  @param Q_ptr [in] - Relevant only for ANSI X9.42 - Pointer to the Q octet string
*                     1 <= Prv <= q-1   or   1 < Prv < q-1
*  @param QSize	[in] - Relevant only for ANSI X9.42 - Size of the Q string (in bytes)  				 				  
*  @param DH_mode [in] - enum declaring weather this is PKCS#3 or ANSI X9.42
*  @param ClientPrvKey_ptr [out] - Pointer to the Private key octet string - 
*                                 In PKCS#3 This buffer should be at least the 
*                                 following size:
*                                 2^L - 1 - if L is provided
*                                 P-1	- if L is DX_NULL
*  @param ClientPrvKeySize_ptr [in/out] - The user should provide the size of the 
*                                        buffer indicated by ClientPrvKey_ptr
*                                        The function returns the actual size 
*                                        in bytes of the Private key Size
*  @param ClientPub_ptr [out] - Pointer to the Public key octet string
*                              This Buffer should be at least PrimeSize bytes
*  @param ClientPubSize_ptr [in/out] - The user should provide the size of the buffer 
*                                     indicated by ClientPub_ptr
*                                     The function returns the actual size in bytes 
*                                     of the client public buffer
*
* @returns \b
*  CE2Error_t 
*  - CE2_OK - on success
*  - Otherwise - error code defined in CE2_DH_error.h:
*
* \brief \b 
* Description:
*  LLF_DH_GeneratePubPrv has 2 functionalities:
*  -# randomly generates the Client private key 
*  -# compute the Client public key which is 
*     ClientPub = Generator ^ Prv mod Prime 
*
*  \b 
* Algorithm:
*  -# Call corresponding subfunction for this mode;
***************************************************************/
CE2Error_t LLF_DH_GeneratePubPrv(
     DxUint8_t *Generator_ptr,
     DxUint16_t GeneratorSize,
     DxUint8_t *Prime_ptr,
     DxUint16_t PrimeSize,
     DxUint16_t L,
     DxUint8_t *Q_ptr,
     DxUint16_t QSize,
     CE2_DH_OpMode_t DH_mode,
     DxUint8_t *ClientPrvKey_ptr,
     DxUint16_t *ClientPrvKeySize_ptr,
     DxUint8_t *ClientPub_ptr,
     DxUint16_t *ClientPubSize_ptr)
{
  if (DH_mode == CE2_DH_ANSI_X942_mode) {
    return LLF_DH_ANSI_X942_GeneratePubPrv(Generator_ptr, GeneratorSize,
      Prime_ptr, PrimeSize, Q_ptr, QSize,    				                    
      ClientPrvKey_ptr, ClientPrvKeySize_ptr, ClientPub_ptr, ClientPubSize_ptr);
  } else {
    return LLF_DH_PVCS3_GeneratePubPrv(Generator_ptr, GeneratorSize,
      Prime_ptr, PrimeSize, L,    				                    
      ClientPrvKey_ptr, ClientPrvKeySize_ptr, ClientPub_ptr, ClientPubSize_ptr);
  }
  //return CE2_OK;
} /* LLF_DH_GeneratePubPrv */

/**
****************************************************************
* Function Name: 
*  LLF_DH_KeyDerivationFunc
*
*  @param ZZSecret_ptr [in] - A pointer to shared secret key octet string 
*  @param ZZSecretSize [in] - The shared secret key Size, in bytes
*  @param hashFunc [in] - The hash function to be used. The hash function 
*                        output must be at least 160 bits.
*  @param KeyLenInBits [in] - The size in Bits of the keying data to be generated - 
*                             KeyLenInBits < hashlen*(2^32-1) - in our implementation 
*                             the size should not be larger than the maximum input 
*                             size to the Hash function
*  @param derivation_mode [in] - Specifies whether we use an ASN.1-based derivation 
*                               function or a derivation based on concatenation.
*  @param KeyingData_ptr [out] - A pointer to the keying data derived from the secret 
*                               key, of length KeyLenInBits
*
* @returns \b
*  CE2Error_t 
*  - CE2_OK - on success
*  - Otherwise - error code defined in CE2_DH_error.h:
*
* \brief \b 
* Description:
*  LLF_DH_KeyDerivationFunc specifies the key derivation function, 
*  either based on ASN.1 DER encoding or based on the concatanation of fields as 
*  specified in ANSI X9.42-2001. The purpose of this function is to derive a key 
*  data from the shared secret value.
*
*  \b 
* Algorithm:
*  -# Init variables
*  -# Derivate key data
***************************************************************/
CE2Error_t LLF_DH_KeyDerivationFunc(
        DxUint8_t *ZZSecret_ptr,
        DxUint16_t ZZSecretSize,
        CE2_DH_HASH_OpMode_t hashFunc,				                    
        DxUint32_t KeyLenInBits,				                    
        DxUint8_t *KeyingData_ptr,
        CE2_DH_DerivationFunc_Mode derivation_mode)
{
  DxUint32_t hashSize, d, i, i_counter, bufferSize;
  CE2Error_t result = CE2_OK, error;
  DxUint8_t *pHashes = DX_NULL, *pBuffer = DX_NULL;
  DxUint8_t counter[4];
  CE2_HASH_OperationMode_t hashMode;
  CE2_HASH_Result_t hashResult;

  /******************/
  /* Init variables */
  /******************/
  switch (hashFunc) {
    case CE2_DH_HASH_SHA1_mode:
      hashSize = CE2_HASH_SHA1_DIGEST_SIZE_IN_BYTES;
      hashMode = CE2_HASH_SHA1_mode;
      break;
    default:
      return CE2_LLF_ECPKI_MODULE_ERROR_BASE;
  }
  switch (derivation_mode) {
    case CE2_DH_ASN1_Der_mode:
      bufferSize = ZZSecretSize;// + LLF_DH_ASN1_OTHER_INFO_MAX_SIZE;
      break;
    case CE2_DH_Concat_Der_mode:
      bufferSize = ZZSecretSize + LLF_DH_CONCAT_OTHER_INFO_MAX_SIZE;
      break;
    default:
      return CE2_LLF_ECPKI_MODULE_ERROR_BASE;
  }

  /* d = [keyLen/hashLen] */
  d = KeyLenInBits/8/hashSize + 1;

  pHashes = malloc(d*hashSize*sizeof(DxUint8_t));
  pBuffer = malloc(bufferSize*sizeof(DxUint8_t));
  if (pHashes == DX_NULL || pBuffer == DX_NULL) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /* copy ZZSecret to buffer */
  memcpy(pBuffer, ZZSecret_ptr, ZZSecretSize);

  /**********************************************************/                             
  /* counter = 1                                            */
  /* For i = 1 to d                                         */
  /*   a) ASN.1:         Hash_i = H(ZZ||OtherInfo)          */
  /*   b) Concatenation: Hash_i = H(ZZ||counter||OtherInfo) */
  /*   counter = counter + 1                                */ 
  /*   Increment i                                          */
  /**********************************************************/                             
  i_counter = 1;
  for (i = 0; i < d; i++) {
    /* ZZ||OtherInfo or ZZ||counter||OtherInfo */
    /* Convert i_counter to big-endian format */
    counter[0] = (DxUint8_t)(i_counter >> 24);
    counter[1] = (DxUint8_t)(i_counter >> 16);
    counter[2] = (DxUint8_t)(i_counter >> 8);
    counter[3] = (DxUint8_t)(i_counter);
    if (derivation_mode == CE2_DH_ASN1_Der_mode) {
      /* Octet string ASN.1 code */
      //pBuffer[ZZSecretSize] = 0x04;
      /* Octet string length */
      //pBuffer[ZZSecretSize + 1] = 0x04;
      //memcpy(pBuffer + ZZSecretSize + 2, counter, 4);
    } else {
      memcpy(pBuffer + ZZSecretSize, counter, 4);
    }
    /* Hash_i = H(ZZ||OtherInfo) or Hash_i = H(ZZ||counter||OtherInfo) */
    error = CE2_HASH(hashMode, pBuffer, bufferSize, hashResult);
    if (error != CE2_OK) {
      result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
      goto error_case;
    }
    memcpy(pHashes + i*hashSize, hashResult, hashSize);
    i_counter++;
  }

  /* KeyingData = leftmost keylen bits of Hash1||Hash2||…||Hashd */
  memcpy(KeyingData_ptr, pHashes, KeyLenInBits/8);

error_case:
  if (pHashes != DX_NULL)
    free(pHashes);
  if (pBuffer != DX_NULL)
    free(pBuffer);
  return result;
} /* End of LLF_DH_KeyDerivationFunc */

/**
****************************************************************
* Function Name: 
*  LLF_DH_X942_GetSecretData
*
*  @param hashFunc [in] - The hash function to be used
*  @param ClientPrvKey_ptr [in] - A pointer to the Private key octet string 
*  @param ClientPrvKeySize [in] - The Private key size, in bytes
*  @param ServerPubKey_ptr [in] - A pointer to the Server public key octet string - 
*  @param ServerPubKeySize [in] - The Server Public key size, in bytes
*  @param Prime_ptr [in] - A pointer to the Prime octet string
*  @param PrimeSize [in] - The size of the Prime string
*  @param DerFunc_mode [in] - The type of function to derive the secret key to 
*                            the key data. We use ASN.1-based function or Hash 
*                            concatenation function mode.
*  @param SecretKeyData_ptr [out] - A pointer to the secret key octet string. 
*                                  This buffer should be at least PrimeSize bytes.
*  @param SecretKeyDataSizeBitsNeeded [in] - Specifies the derived Secret Key data 
*                                           size needed in Bits. This value converted 
*                                           to bytes cannot be larger than PrimeSize
*
* @returns \b
*  CE2Error_t 
*  - CE2_OK - on success
*  - Otherwise - error code defined in CE2_DH_error.h:
*
* \brief \b 
* Description:
*  LLF_DH_X942_GetSecretData computes the shared secret key as follows:
*   -# OPTIONAL - validate the correctness of the argument keys
*   -# SecretKey = ServerPubKey ^ ClientPrvKey mod Prime
*   -# Use of Derivation function to derive a data key from the secret key 
*
*  \b 
* Algorithm:
*  -# Initialize LibTomCrypt primitives
*  -# Calculate SecretKey
*  -# Derivate secret key data from shared secret
***************************************************************/
CE2Error_t LLF_DH_X942_GetSecretData(
       CE2_DH_HASH_OpMode_t hashFunc,		
       DxUint8_t *ClientPrvKey_ptr,
       DxUint16_t ClientPrvKeySize,
       DxUint8_t *ServerPubKey_ptr,
       DxUint16_t ServerPubKeySize,				                    
       DxUint8_t *Prime_ptr,
       DxUint16_t PrimeSize,
       CE2_DH_DerivationFunc_Mode DerFunc_mode,
       DxUint8_t *SecretKeyData_ptr,
       DxUint16_t SecretKeyDataSizeBitsNeeded)
{
  CE2Error_t result = CE2_OK, error;
  void *zz, *client, *server, *p;
  DxUint8_t *pZZSecret = DX_NULL;
  int error_code, zzSize;

  /* Initialize ltc_mp structure */
  ltc_mp = ltm_desc;

  /*************************************/
  /* Initialize LibTomCrypt primitives */
  /*************************************/
  error_code = ltc_init_multi(&zz, &p, &client, &server, NULL);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(p, Prime_ptr, PrimeSize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(client, ClientPrvKey_ptr, ClientPrvKeySize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(server, ServerPubKey_ptr, ServerPubKeySize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /******************************************************/
  /*  Calculate SecretKey                               */
  /*  ZZSecret = ServerPubKey ^ ClientPrvKey mod Prime  */ 
  /******************************************************/

  /* zz = server^client mod p */
  error_code = ltc_mp.exptmod(server, client, p, zz);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  /* zz write to big-endian pZZSecret */
  zzSize = ltc_mp.unsigned_size(zz);
  pZZSecret = malloc(zzSize*sizeof(DxUint8_t));
  if (pZZSecret == DX_NULL) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_write(zz, pZZSecret);
  if(error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /******************************************************/
  /*  Derivate secret key data from shared secret       */
  /******************************************************/
  error = LLF_DH_KeyDerivationFunc(pZZSecret, (DxUint16_t)zzSize,
    hashFunc,	SecretKeyDataSizeBitsNeeded, SecretKeyData_ptr, DerFunc_mode);
  if (error != CE2_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

error_case:
  if (pZZSecret != DX_NULL)
    free(pZZSecret);
  ltc_deinit_multi(zz, p, client, server, NULL);
  return result;
} /* End of LLF_DH_X942_GetSecretData */

/**
****************************************************************
* Function Name: 
*  LLF_DH_X942_Hybrid_GetSecretData
*
*  @param hashFunc [in] - The hash function to be used
*  @param ClientPrvKey_ptr1 [in] - A pointer to the First Private key octet string 
*                                 number 
*  @param ClientPrvKeySize1 [in] - The First Private key Size, in bytes
*  @param ClientPrvKey_ptr2 [in] - A pointer to the Second Private key octet string
*  @param ClientPrvKeySize2 [in] - The Second Private key Size, in bytes
*  @param ServerPubKey_ptr1 [in] - A pointer to the First Server public key octet string
*  @param ServerPubKeySize1 [in] - The First Server Public key Size, in bytes
*  @param ServerPubKey_ptr2 [in] - A pointer to the Second Server public key octet string
*  @param ServerPubKeySize2 [in] - The Second Server Public key Size, in bytes
*  @param Prime_ptr [in] - A pointer to the Prime octet string
*  @param PrimeSize [in] - The size of the Prime string
*  @param DerFunc_mode [in] - The type of function to derive the secret key to the key 
*                            data. We use an ASN.1-based function or a Hash 
*                            concatenation function mode.
*  @param SecretKeyData_ptr [out] - A pointer to the secret key octet string.
*                                  This buffer should be at least 2*PrimeSize bytes.
*  @param SecretKeyDataSizeBitsNeeded [in] - Specifies the derived Secret Key data size 
*                                           needed in Bits. This value converted to bytes 
*                                           cannot be larger than PrimeSize
*
* @returns \b
*  CE2Error_t 
*  - CE2_OK - on success
*  - Otherwise - error code defined in CE2_DH_error.h:
*
* \brief \b 
* Description:
*  LLF_DH_X942_Hybrid_GetSecretData computes the shared secret key as follows:
*   -# OPTIONAL - validate the correctness of the argument keys
*   -# SecretKey1 = ServerPubKey1 ^ ClientPrvKey1 mod Prime
*   -# SecretKey2 = ServerPubKey2 ^ ClientPrvKey2 mod Prime
*   -# Use of Derivation function to derive a data key from the 2 secret keys 
*
*  \b 
* Algorithm:
*  -# Initialize LibTomCrypt primitives
*  -# SecretKey1 = ServerPubKey1 ^ ClientPrvKey1 mod Prime
*  -# SecretKey2 = ServerPubKey2 ^ ClientPrvKey2 mod Prime
*  -# SecretKey = SecretKey1 || SecretKey2
*  -# Use of Derivation function to derive a data key from SecretKey
***************************************************************/
CE2Error_t LLF_DH_X942_Hybrid_GetSecretData(
  CE2_DH_HASH_OpMode_t hashFunc,		
  DxUint8_t *ClientPrvKey_ptr1,
  DxUint16_t ClientPrvKeySize1,
  DxUint8_t *ClientPrvKey_ptr2,
  DxUint16_t ClientPrvKeySize2,				                    
  DxUint8_t *ServerPubKey_ptr1,
  DxUint16_t ServerPubKeySize1,
  DxUint8_t *ServerPubKey_ptr2,
  DxUint16_t ServerPubKeySize2,				                    				                    
  DxUint8_t *Prime_ptr,
  DxUint16_t PrimeSize,
  CE2_DH_DerivationFunc_Mode DerFunc_mode,
  DxUint8_t *SecretKeyData_ptr,
  DxUint16_t SecretKeyDataSizeBitsNeeded)
{
  CE2Error_t result = CE2_OK, error;
  void *zz1, *client1, *server1, *zz2, *client2, *server2, *p;
  DxUint8_t *pZZSecret1 = DX_NULL, *pZZSecret2 = DX_NULL, *pZZSecret = DX_NULL;
  int error_code, zzSize1, zzSize2;

  /* Initialize ltc_mp structure */
  ltc_mp = ltm_desc;

  /*************************************/
  /* Initialize LibTomCrypt primitives */
  /*************************************/
  error_code = ltc_init_multi(&zz1, &p, &client1, &server1, 
    &zz2, &server2, &client2, NULL);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(p, Prime_ptr, PrimeSize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(client1, ClientPrvKey_ptr1, ClientPrvKeySize1); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(server1, ServerPubKey_ptr1, ServerPubKeySize1); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(client2, ClientPrvKey_ptr2, ClientPrvKeySize2); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(server2, ServerPubKey_ptr2, ServerPubKeySize2); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /********************************************************/
  /*  Calculate SecretKey1                                */
  /*  ZZSecret1 = ServerPubKey1 ^ ClientPrvKey1 mod Prime */ 
  /********************************************************/

  /* zz1 = server1^client1 mod p */
  error_code = ltc_mp.exptmod(server1, client1, p, zz1);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  /* zz1 write to big-endian pZZSecret1 */
  zzSize1 = ltc_mp.unsigned_size(zz1);
  pZZSecret1 = malloc(zzSize1*sizeof(DxUint8_t));
  if (pZZSecret1 == DX_NULL) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_write(zz1, pZZSecret1);
  if(error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /********************************************************/
  /*  Calculate SecretKey2                                */
  /*  ZZSecret2 = ServerPubKey2 ^ ClientPrvKey2 mod Prime */ 
  /********************************************************/

  /* zz2 = server2^client2 mod p */
  error_code = ltc_mp.exptmod(server2, client2, p, zz2);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  /* zz2 write to big-endian pZZSecret2 */
  zzSize2 = ltc_mp.unsigned_size(zz2);
  pZZSecret2 = malloc(zzSize2*sizeof(DxUint8_t));
  if (pZZSecret2 == DX_NULL) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_write(zz2, pZZSecret2);
  if(error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /*********************************************************/
  /*  Make common buffer ZZSecret = ZZSecret1 || ZZSecret2 */
  /*********************************************************/
  pZZSecret = malloc((zzSize1 + zzSize2)*sizeof(DxUint8_t));
  if (pZZSecret == DX_NULL) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  memcpy(pZZSecret, pZZSecret1, zzSize1);
  memcpy(pZZSecret + zzSize1, pZZSecret2, zzSize2);


  /******************************************************/
  /*  Derivate secret key data from shared secret       */
  /******************************************************/
  error = LLF_DH_KeyDerivationFunc(pZZSecret, (DxUint16_t)(zzSize1 + zzSize2),
    hashFunc,	SecretKeyDataSizeBitsNeeded, SecretKeyData_ptr, DerFunc_mode);
  if (error != CE2_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

error_case:
  if (pZZSecret1 != DX_NULL)
    free(pZZSecret1);
  if (pZZSecret2 != DX_NULL)
    free(pZZSecret2);
  if (pZZSecret != DX_NULL)
    free(pZZSecret);
  ltc_deinit_multi(p, zz1, client1, server1, zz2, client2, server2, NULL);
  return result;
} /* End of LLF_DH_X942_Hybrid_GetSecretData */

/**
****************************************************************
* Function Name: 
*  LLF_DH_PKCS3_GetSecretKey
*
*  @param ClientPrvKey_ptr [in] - Pointer to the Private key octet string.
*  @param ClientPrvKeySize [in] - The Private key Size (in bytes)
*  @param ServerPubKey_ptr [in] - Pointer to the Server public key octet string.
*  @param ServerPubKeySize [in] - The Server Public key Size (in bytes)
*  @param Prime_ptr [in] - Pointer to the Prime octet string
*  @param PrimeSize [in] - Size of the Prime string
*  @param UserPubKey_ptr [in] - a pointer to the publick key structure. 
*                              Used for the Exp operation function
*                              the struct doesn't need to be initialized
*  @param PrimeData_ptr [in] - a pointer to a structure containing internal 
*                             buffers the struct doesn't need to be initialized    
*  @param SecretKey_ptr [out] - Pointer to the secret key octet string.
*                              This buffer should be at least PrimeSize Bytes
*  @param SecretKeySize_ptr [out] - Size of secret key octet string
*
* @returns \b
*  CE2Error_t 
*  - CE2_OK - on success
*  - Otherwise - error code defined in CE2_DH_error.h:
*
* \brief \b 
* Description:
*  LLF_DH_PKCS3_GetSecretKey computes the shared secret key in the 
*  following computation: SecretKey = ServerPubKey ^ ClientPrvKey mod Prime
*
*  \b 
* Algorithm:
*  -# Initialize LibTomCrypt primitives
*  -# Calculate SecretKey
*  -# Convert SecretKey to output big-endian SecretKey_ptr
***************************************************************/
CE2Error_t LLF_DH_PKCS3_GetSecretKey(
     DxUint8_t *ClientPrvKey_ptr,
     DxUint16_t ClientPrvKeySize,
     DxUint8_t *ServerPubKey_ptr,
     DxUint16_t ServerPubKeySize,				                    
     DxUint8_t *Prime_ptr,
     DxUint16_t PrimeSize,
     DxUint8_t *SecretKey_ptr,
     DxUint16_t *SecretKeySize_ptr)
{
  CE2Error_t result = CE2_OK;
  void *secret, *client, *server, *prime;
  int error_code, size;

  /* Initialize ltc_mp structure */
  ltc_mp = ltm_desc;

  /*************************************/
  /* Initialize LibTomCrypt primitives */
  /*************************************/
  error_code = ltc_init_multi(&secret, &prime, &client, &server, NULL);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(prime, Prime_ptr, PrimeSize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(client, ClientPrvKey_ptr, ClientPrvKeySize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }
  error_code = ltc_mp.unsigned_read(server, ServerPubKey_ptr, ServerPubKeySize); 
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /******************************************************/
  /*  Calculate SecretKey                               */
  /*  SecretKey = ServerPubKey ^ ClientPrvKey mod Prime */ 
  /******************************************************/

  /* secret = server^client mod prime */
  error_code = ltc_mp.exptmod(server, client, prime, secret);
  if (error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

  /* secret write to big-endian SecretKey_ptr */
  size = ltc_mp.unsigned_size(secret);
  if (size > *SecretKeySize_ptr) {
    *SecretKeySize_ptr = size; 
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  } 
  *SecretKeySize_ptr = size; 
  error_code = ltc_mp.unsigned_write(secret, SecretKey_ptr);
  if(error_code != CRYPT_OK) {
    result = CE2_LLF_ECPKI_MODULE_ERROR_BASE;
    goto error_case;
  }

error_case:
  ltc_deinit_multi(secret, prime, client, server, NULL);
  return result;
}
