///////////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2000-2019 Ericsson Telecom AB
//
// All rights reserved. This program and the accompanying materials
// are made available under the terms of the Eclipse Public License v2.0
// which accompanies this distribution, and is available at
// https://www.eclipse.org/org/documents/epl-2.0/EPL-2.0.html
///////////////////////////////////////////////////////////////////////////////
//
//  File:               EAP_EncDec.cc
//  Rev:                R6A
//  Prodnr:             CNL 113 722
//  Updated:            2014-01-24
//  Contact:            http://ttcn.ericsson.se
//  Reference:          RFC 3748(Extensible Authentication Protocol)
//                      RFC 4187(EAP-AKA)
//                      RFC 5448(EAP-AKA')
//                      RFC 4186(EAP-SIM)
//                      RFC 5281(EAP-TTLS)
///////////////////////////////////////////////////////////////////////////////

#include "EAP_Types.hh"
#include <stdint.h>
#include <openssl/sha.h>
#include <openssl/bn.h>

#include <openssl/asn1.h>
#include <openssl/x509.h>


namespace EAP__Types {

OCTETSTRING enc__PDU__EAP(const PDU__EAP& pdu)
{
    if(TTCN_Logger::log_this_event(TTCN_DEBUG)) {
      TTCN_Logger::begin_event(TTCN_DEBUG);
      TTCN_Logger::log_event("Encoding PDU_EAP: ");
      pdu.log();
      TTCN_Logger::end_event();
    }

    TTCN_EncDec::error_type_t err;
    TTCN_Buffer buf;
    buf.clear();
    TTCN_EncDec::clear_error();
    TTCN_EncDec::set_error_behavior(TTCN_EncDec::ET_ALL, TTCN_EncDec::EB_WARNING);
    pdu.encode(PDU__EAP_descr_, buf, TTCN_EncDec::CT_RAW);
    err = TTCN_EncDec::get_last_error_type();
    if(err != TTCN_EncDec::ET_NONE)
      TTCN_warning("Encoding error: %s\n", TTCN_EncDec::get_error_str());

    OCTETSTRING ret_val(buf.get_len(), buf.get_data());
    if(TTCN_Logger::log_this_event(TTCN_DEBUG)) {
      TTCN_Logger::begin_event(TTCN_DEBUG);
      TTCN_Logger::log_event("PDU_EAP after encoding: ");
      ret_val.log();
      TTCN_Logger::end_event();
    }

    return ret_val;
}

PDU__EAP dec__PDU__EAP(const OCTETSTRING& stream)
{
    if(TTCN_Logger::log_this_event(TTCN_DEBUG)) {
      TTCN_Logger::begin_event(TTCN_DEBUG);
      TTCN_Logger::log_event("Decoding PDU_EAP: ");
      stream.log();
      TTCN_Logger::end_event();
    }

    TTCN_EncDec::error_type_t err;
    TTCN_EncDec::clear_error();
    TTCN_Buffer buf;
    buf.put_os(stream);
    PDU__EAP ret_val;
    TTCN_EncDec::set_error_behavior(TTCN_EncDec::ET_ALL, TTCN_EncDec::EB_WARNING);
    ret_val.decode(PDU__EAP_descr_, buf, TTCN_EncDec::CT_RAW);
    err = TTCN_EncDec::get_last_error_type();
    if(err != TTCN_EncDec::ET_NONE)
      TTCN_warning("Decoding error: %s\n", TTCN_EncDec::get_error_str());

    if(TTCN_Logger::log_this_event(TTCN_DEBUG)) {
      TTCN_Logger::begin_event(TTCN_DEBUG);
      TTCN_Logger::log_event("Decoded PDU_EAP: ");
      ret_val.log();
      TTCN_Logger::end_event();
    }

    return ret_val;
}

OCTETSTRING enc__AKA__Attrib(const EAP__AKA__Attrib__List& pdu)
{
    if(TTCN_Logger::log_this_event(TTCN_DEBUG)) {
      TTCN_Logger::begin_event(TTCN_DEBUG);
      TTCN_Logger::log_event("Encoding EAP_AKA_Attrib_List: ");
      pdu.log();
      TTCN_Logger::end_event();
    }

    TTCN_EncDec::error_type_t err;
    TTCN_Buffer buf;
    buf.clear();
    TTCN_EncDec::clear_error();
    TTCN_EncDec::set_error_behavior(TTCN_EncDec::ET_ALL, TTCN_EncDec::EB_WARNING);
    pdu.encode(EAP__AKA__Attrib__List_descr_, buf, TTCN_EncDec::CT_RAW);
    err = TTCN_EncDec::get_last_error_type();
    if(err != TTCN_EncDec::ET_NONE)
      TTCN_warning("Encoding error: %s\n", TTCN_EncDec::get_error_str());

    OCTETSTRING ret_val(buf.get_len(), buf.get_data());
    if(TTCN_Logger::log_this_event(TTCN_DEBUG)) {
      TTCN_Logger::begin_event(TTCN_DEBUG);
      TTCN_Logger::log_event("EAP_AKA_Attrib_List after encoding: ");
      ret_val.log();
      TTCN_Logger::end_event();
    }

    return ret_val;
}

EAP__AKA__Attrib__List dec__AKA__Attrib(const OCTETSTRING& stream)
{
    if(TTCN_Logger::log_this_event(TTCN_DEBUG)) {
      TTCN_Logger::begin_event(TTCN_DEBUG);
      TTCN_Logger::log_event("Decoding EAP_AKA_Attrib_List: ");
      stream.log();
      TTCN_Logger::end_event();
    }

    TTCN_EncDec::error_type_t err;
    TTCN_EncDec::clear_error();
    TTCN_Buffer buf;
    buf.put_os(stream);
    EAP__AKA__Attrib__List ret_val;
    TTCN_EncDec::set_error_behavior(TTCN_EncDec::ET_ALL, TTCN_EncDec::EB_WARNING);
    ret_val.decode(EAP__AKA__Attrib__List_descr_, buf, TTCN_EncDec::CT_RAW);
    err = TTCN_EncDec::get_last_error_type();
    if(err != TTCN_EncDec::ET_NONE)
      TTCN_warning("Decoding error: %s\n", TTCN_EncDec::get_error_str());

    if(TTCN_Logger::log_this_event(TTCN_DEBUG)) {
      TTCN_Logger::begin_event(TTCN_DEBUG);
      TTCN_Logger::log_event("Decoded EAP_AKA_Attrib_List: ");
      ret_val.log();
      TTCN_Logger::end_event();
    }

    return ret_val;
}


typedef unsigned char u8;


void sha1_vector(size_t num_elem, const unsigned char *addr[], const size_t *len, unsigned char *mac)
{
  SHA_CTX ctx;
  SHA_Init(&ctx);

  size_t i;

  for (i = 0; i < num_elem; i++) {

    SHA1_Update(&ctx, addr[i], len[i]);
  }

  SHA1_Final(mac, &ctx);
}


OCTETSTRING eap__sim__derive__mk(const OCTETSTRING& identity,
    const OCTETSTRING& nonce_mt, const INTEGER& selected_version,
    const OCTETSTRING& ver_list,
    const OCTETSTRING& kc)
{
  unsigned char mk[20];

  unsigned char sel_ver[2];
  const unsigned char *addr[5];
  size_t len[5];

  addr[0] = (const unsigned char*)identity;

  len[0] = identity.lengthof();

  addr[1] = (const unsigned char*)kc;

  len[1] = kc.lengthof();

  addr[2] = (const unsigned char*)nonce_mt;

  len[2] = nonce_mt.lengthof();

  addr[3] = (const unsigned char*)ver_list;

  len[3] = ver_list.lengthof();

  addr[4] = sel_ver;

  len[4] = 2;

  sel_ver[0] = ((unsigned short) (selected_version)) >> 8;
  sel_ver[1] = ((unsigned short) (selected_version)) & 0xff;
  
  
  /* MK = SHA1(Identity|n*Kc|NONCE_MT|Version List|Selected Version) */
  sha1_vector(5, addr, len, mk);

  OCTETSTRING mk_value(20,mk);
  return mk_value;
}

#define bswap_32(a) ((((unsigned int) (a) << 24) & 0xff000000) | \
             (((unsigned int) (a) << 8) & 0xff0000) | \
                  (((unsigned int) (a) >> 8) & 0xff00) | \
                  (((unsigned int) (a) >> 24) & 0xff))

#ifdef __linux__
#define host_to_be32(n) ((unsigned int) bswap_32((n)))
#else
#define host_to_be32(n) (n)
#endif



OCTETSTRING fips186__2__prf(const OCTETSTRING& input /**const unsigned char *seed, size_t seed_len, unsigned char *x, size_t xlen*/)
{
  const unsigned char* seed=(const unsigned char*)input;
  size_t seed_len=input.lengthof();
  size_t xlen=160;
  unsigned char x[xlen];

  unsigned char xkey[64];
  unsigned int _t[5];
  int i, j, m, k;
  unsigned char *xpos = x;

  unsigned int carry;

  if (seed_len > sizeof(xkey))
    seed_len = sizeof(xkey);


  /* FIPS 186-2 + change notice 1 */

  memcpy(xkey, seed, seed_len);

  memset(xkey + seed_len, 0, 64 - seed_len);

  SHA_CTX ctx;

  m = xlen / 40;

  for (j = 0; j < m; j++) {
    /* XSEED_j = 0 */
    for (i = 0; i < 2; i++) {
      /* XVAL = (XKEY + XSEED_j) mod 2^b */

      /* w_i = G(t, XVAL) */

      SHA_Init(&ctx);

      SHA1_Transform(&ctx , xkey);

      _t[0] = host_to_be32(ctx.h0);
      _t[1] = host_to_be32(ctx.h1);
      _t[2] = host_to_be32(ctx.h2);
      _t[3] = host_to_be32(ctx.h3);
      _t[4] = host_to_be32(ctx.h4);

      memcpy(xpos, _t, 20);


      /* XKEY = (1 + XKEY + w_i) mod 2^b */
      carry = 1;

      for (k = 19; k >= 0; k--) {
        carry += xkey[k] + xpos[k];

        xkey[k] = carry & 0xff;

        carry >>= 8;
      }

      xpos += 20;

    }
    /* x_j = w_0|w_1 */
  }

  OCTETSTRING output(xlen,x);
  return output;
}



OCTETSTRING eap__aka__derive__mk(const OCTETSTRING& p_identity, const OCTETSTRING& p_ik, const OCTETSTRING& p_ck)
{
  unsigned char mk[20];

  const u8 *identity=(const u8 *)p_identity;
  size_t identity_len=p_identity.lengthof();
  const u8 *ik=(const u8 *)p_ik;
  const u8 *ck=(const u8 *)p_ck;



  const u8 *addr[3];

  size_t len[3];

  addr[0] = identity;

  len[0] = identity_len;

  addr[1] = ik;

  len[1] = 16;

  addr[2] = ck;

  len[2] = 16;

  /* MK = SHA1(Identity|IK|CK) */
  sha1_vector(3, addr, len, mk);

  OCTETSTRING mk_value(20,mk);
  return mk_value;
}

OCTETSTRING eap__aka__derive__reauth__msk__emsk(const OCTETSTRING& p_identity, const OCTETSTRING& p_counter, const OCTETSTRING& p_nonce_s, const OCTETSTRING& p_mk)
{
  unsigned char reauth_msk_emsk[20];
  OCTETSTRING source_os = p_identity + p_counter + p_nonce_s + p_mk;
  const unsigned char *source = (const unsigned char*)source_os;

  /* MK = SHA1(Identity|counter|NONCE_S|MK) */
  SHA1(source, source_os.lengthof(), reauth_msk_emsk);

  OCTETSTRING emsk_value(20,reauth_msk_emsk);
  return emsk_value;
}

//Added for EPC


#define EAP_AKA_IK_LEN 16
#define EAP_AKA_CK_LEN 16
#define EAP_SIM_K_ENCR_LEN 16
#define EAP_AKA_PRIME_K_AUT_LEN 32
#define EAP_AKA_PRIME_K_RE_LEN 32
#define EAP_MSK_LEN 64
#define EAP_EMSK_LEN 64
#define SHA256_MAC_LEN 32
#define SHA256_CHECKCODE_LEN 32




/**
 * sha256_vector - SHA256 hash for data vector
 * @num_elem: Number of elements in the data vector
 * @addr: Pointers to the data areas
 * @len: Lengths of the data blocks
 * @mac: Buffer for the hash
 * Returns: 0 on success, -1 of failure
 */
int sha256_vector(size_t num_elem, const u8 *addr[], const size_t *len,
      u8 *mac)
{

        SHA256_CTX ctx; 
  size_t i;
  
  SHA256_Init(&ctx);
  for (i = 0; i < num_elem; i++) {    
                SHA256_Update(&ctx, addr[i], len[i]);
  }
  if (SHA256_Final(mac, &ctx))
    return -1;
  return 0;      
      
}

/**
 * hmac_sha256_vector - HMAC-SHA256 over data vector (RFC 2104)
 * @key: Key for HMAC operations
 * @key_len: Length of the key in bytes
 * @num_elem: Number of elements in the data vector
 * @addr: Pointers to the data areas
 * @len: Lengths of the data blocks
 * @mac: Buffer for the hash (32 bytes)
 */
void hmac_sha256_vector(const u8 *key, size_t key_len, size_t num_elem,
      const u8 *addr[], const size_t *len, u8 *mac)
{
  unsigned char k_pad[64]; /* padding - key XORd with ipad/opad */
  unsigned char tk[32];
  const u8 *_addr[6];
  size_t _len[6], i;

  if (num_elem > 5) {
    /*
     * Fixed limit on the number of fragments to avoid having to
     * allocate memory (which could fail).
     */
    return;
  }

        /* if key is longer than 64 bytes reset it to key = SHA256(key) */
        if (key_len > 64) {
    sha256_vector(1, &key, &key_len, tk);
    key = tk;
    key_len = 32;
        }

  /* the HMAC_SHA256 transform looks like:
   *
   * SHA256(K XOR opad, SHA256(K XOR ipad, text))
   *
   * where K is an n byte key
   * ipad is the byte 0x36 repeated 64 times
   * opad is the byte 0x5c repeated 64 times
   * and text is the data being protected */

  /* start out by storing key in ipad */
  memset(k_pad, 0, sizeof(k_pad));
  memcpy(k_pad, key, key_len);
  /* XOR key with ipad values */
  for (i = 0; i < 64; i++)
    k_pad[i] ^= 0x36;

  /* perform inner SHA256 */
  _addr[0] = k_pad;
  _len[0] = 64;
  for (i = 0; i < num_elem; i++) {
    _addr[i + 1] = addr[i];
    _len[i + 1] = len[i];
  }
  sha256_vector(1 + num_elem, _addr, _len, mac);

  memset(k_pad, 0, sizeof(k_pad));
  memcpy(k_pad, key, key_len);
  /* XOR key with opad values */
  for (i = 0; i < 64; i++)
    k_pad[i] ^= 0x5c;

  /* perform outer SHA256 */
  _addr[0] = k_pad;
  _len[0] = 64;
  _addr[1] = mac;
  _len[1] = SHA256_MAC_LEN;
  sha256_vector(2, _addr, _len, mac);
}


static void prf_prime(const u8 *k, const char *seed1,
          const u8 *seed2, size_t seed2_len,
          const u8 *seed3, size_t seed3_len,
          u8 *res, size_t res_len)
{
  const u8 *addr[5];
  size_t len[5];
  u8 hash[SHA256_MAC_LEN];
  u8 iter;
  /*
   * PRF'(K,S) = T1 | T2 | T3 | T4 | ...
   * T1 = HMAC-SHA-256 (K, S | 0x01)
   * T2 = HMAC-SHA-256 (K, T1 | S | 0x02)
   * T3 = HMAC-SHA-256 (K, T2 | S | 0x03)
   * T4 = HMAC-SHA-256 (K, T3 | S | 0x04)
   * ...
   */

        addr[0] = hash;
  len[0] = 0;
  addr[1] = (const u8 *) seed1;
  len[1] = strlen(seed1);
  addr[2] = seed2;
  len[2] = seed2_len;
  addr[3] = seed3;
  len[3] = seed3_len;
  addr[4] = &iter;
  len[4] = 1;

  iter = 0;
  while (res_len) {
    size_t hlen;
    iter++;
    hmac_sha256_vector(k, 32, 5, addr, len, hash);
    len[0] = SHA256_MAC_LEN;
    hlen = res_len > SHA256_MAC_LEN ? SHA256_MAC_LEN : res_len;
    memcpy(res, hash, hlen);
    res += hlen;
    res_len -= hlen;
  }

}

OCTETSTRING eap__akaprime__derive__mk(const OCTETSTRING& p_identity, const OCTETSTRING& p_ik, const OCTETSTRING& p_ck)
{
  OCTETSTRING mk;

  //const char *identity=oct2str(p_identity).c_str();
  //const char *identity=(const char *)oct2str(p_identity);
  //char *identity=oct2str(p_identity);

  //size_t identity_len=p_identity.lengthof();

  const u8 *identity=(const u8 *)p_identity;
  size_t identity_len=p_identity.lengthof();
  const u8 *ik=(const u8 *)p_ik;
  const u8 *ck=(const u8 *)p_ck;

  //char *ik=oct2str(p_ik);
  //char *ck=oct2str(p_ck);
  //const unsigned char *ik=(const unsigned char *)p_ik;

  //const unsigned char *ck=(const unsigned char *)p_ck;

        uint8_t key[EAP_AKA_IK_LEN + EAP_AKA_CK_LEN];
        uint8_t keys[EAP_SIM_K_ENCR_LEN + EAP_AKA_PRIME_K_AUT_LEN +
        EAP_AKA_PRIME_K_RE_LEN + EAP_MSK_LEN + EAP_EMSK_LEN];
    //    uint8_t *pos;

        memset(key, 0, sizeof(key));
        memset(keys, 0, sizeof(keys));
        memcpy(key, ik, EAP_AKA_IK_LEN);
      memcpy(key + EAP_AKA_IK_LEN, ck, EAP_AKA_CK_LEN);

      prf_prime(key, "EAP-AKA'", (const uint8_t*)identity, identity_len, NULL, 0,
        keys, sizeof(keys));
  OCTETSTRING mk_value(208,keys);
        return mk_value;
}

OCTETSTRING Calculate__AT__CheckCode(const OCTETSTRING& rcv_eappayload_octstring,const OCTETSTRING& send_eappayload_octstring)
{
  const u8 *addr;
  size_t len;
  u8 hash[SHA256_CHECKCODE_LEN];

  OCTETSTRING eappayload1=rcv_eappayload_octstring+send_eappayload_octstring;

  const u8* eappayload=(const u8 *)eappayload1;

  addr=eappayload;

  len=eappayload1.lengthof();;

  sha256_vector(1, &addr, &len, hash);

  OCTETSTRING output(32,hash);

  return output;

}


const unsigned char * change_ByteOrder(unsigned int in)
{
   static unsigned char out[4] ;
   out[0] = (unsigned char)((in & 0xFF000000) >> 24);
   out[1] = (unsigned char)((in & 0x00FF0000) >> 16);
   out[2] = (unsigned char)((in & 0x0000FF00) >> 8);
   out[3] = (unsigned char)(in & 0x000000FF);
   return out;
} // change_ByteOrder


OCTETSTRING f__calc__Kaut(const OCTETSTRING& input, OCTETSTRING& Kencr)
{  
   unsigned char xval[64];
   unsigned char xkey[SHA_DIGEST_LENGTH];
   SHA1((const unsigned char*)input, input.lengthof(), xkey);
   unsigned char dummy[128];
   unsigned char x_out[80];//40 bytes for each (m) iteration
   int m = 2;

   for(int j = 0; j < m; j++){
    for(int i = 0; i < 2; i++){
      memcpy(xval, xkey, 20);//xval = xkey mod 2^160
      memset(xval + 20, 0, 44);//padding xval to 64 bytes

      SHA_CTX c;
      SHA1_Init(&c);
      SHA1_Update(&c, xval, 64);

      memcpy(x_out + j * 40 + i * 20, change_ByteOrder(c.h0), 4);
      memcpy(x_out + j * 40 + i * 20 + 4, change_ByteOrder(c.h1), 4);
      memcpy(x_out + j * 40 + i * 20 + 8, change_ByteOrder(c.h2), 4);
      memcpy(x_out + j * 40 + i * 20 + 12, change_ByteOrder(c.h3), 4);
      memcpy(x_out + j * 40 + i * 20 + 16, change_ByteOrder(c.h4), 4);//w_i

      BIGNUM * bn1 = BN_new();
      BIGNUM * bn2 = BN_new();
      BIGNUM * bn3 = BN_new();
      if(!bn1 || !bn2 || !bn3)
        TTCN_error("Key generation failed");

      BN_bin2bn(xkey, 20, bn1);//xkey

      BN_one(bn2);//1

      BN_add(bn3, bn1, bn2);//bn3 = xkey + 1

      BN_bin2bn(x_out + j * 40, 20, bn2);//w_i

      BN_add(bn1, bn3, bn2);//bn1 = xkey + 1 + w_i

      BN_bn2bin(bn1, dummy);
      for(int k = 0; k < 20; k++){
        if(BN_num_bytes(bn1) - 1 - k < 0)
          xkey[19 - k] = '\0';
        else
          xkey[19 - k] = dummy[BN_num_bytes(bn1) - 1 - k];
      }//xkey = (xkey + 1 + w_i) mod 2 ^ 160

      BN_free(bn1);
      BN_free(bn2);
      BN_free(bn3);
    }
  }

 Kencr = OCTETSTRING(16, x_out);
 OCTETSTRING Kaut = OCTETSTRING(16, x_out + 16);
 // PMK = OCTETSTRING(32, x_out + 32);
            
  return Kaut;   
}

OCTETSTRING f__calc__AKA__Keys(const OCTETSTRING& pl_eap_identity, const OCTETSTRING& pl_AKA_K,const OCTETSTRING& pl_rand,OCTETSTRING& pl_AK,OCTETSTRING& pl_Kaut,OCTETSTRING& pl_Kencr)  
{
  const OCTETSTRING& xdout = pl_AKA_K ^ pl_rand;  
  const OCTETSTRING& IK = xdout <<= 2;
  const OCTETSTRING& CK = xdout <<= 1;
  pl_AK = OCTETSTRING(6, ((const unsigned char*)xdout) + 3);
  pl_Kaut = f__calc__Kaut(pl_eap_identity + IK + CK,pl_Kencr);
  return xdout;
}


void f__get__ServersPublicKey(const OCTETSTRING& pl_certificate, OCTETSTRING& pl_modules, OCTETSTRING& pl_exponent)
{
  X509 *x; 
  const unsigned char *cert = (const unsigned char *)pl_certificate;
 
  x = NULL;

  d2i_X509(&x, &cert, pl_certificate.lengthof()); 
 // X509_print_fp(stdout, x);
    
  if (x != NULL)
  {
    unsigned char *pkd  = x->cert_info->key->public_key->data;
    
    if(pkd != NULL)
    {
      //int structlength = 0;
      int structlengthlength = 0;
      int moduleslength = 0;
      int moduleslengthlength = 0;
      int exponentlength = 0;
      int exponentlengthlength = 0;
      if(pkd[1] & 0x80)
      {
        structlengthlength = (pkd[1] & 0x7f);
        //structlength = oct2int((OCTETSTRING(structlengthlength, (const unsigned char *)(pkd + 2))));
      }
      //else
      //{
        //structlength = pkd[1];
      //}
      //type,length,type
      if(pkd[structlengthlength + 3] & 0x80)
      {
        moduleslengthlength = (pkd[structlengthlength + 3] & 0x7f);
        moduleslength = oct2int((OCTETSTRING(moduleslengthlength, (const unsigned char *)(pkd + structlengthlength + 4))));
      }
      else
      {
        moduleslength = pkd[structlengthlength + 3];
      }
      pl_modules = (OCTETSTRING(moduleslength, (const unsigned char *)(pkd + 4 + structlengthlength + moduleslengthlength)));
      
      if(pkd[moduleslength + structlengthlength + moduleslengthlength + 5] & 0x80)
      {
        exponentlengthlength = (pkd[moduleslength + structlengthlength + moduleslengthlength + 5] & 0x7f);
        exponentlength = oct2int((OCTETSTRING(exponentlengthlength, (const unsigned char *)(pkd + moduleslength + structlengthlength + moduleslengthlength + 6))));
      }
      else
      {
        exponentlength = pkd[moduleslength + structlengthlength + moduleslengthlength + 5];
      }
      pl_exponent = (OCTETSTRING(exponentlength, (const unsigned char *)(pkd + 6 + structlengthlength + moduleslengthlength + moduleslength + exponentlengthlength)));
    }
  }  
}
#define MAX_MESSAGE_LENGTH 4096






void hmac_sha1(
                unsigned char *key,
                int key_length,
                unsigned char *data,
                int data_length,
                unsigned char *digest
                )

{
    int b = 64; /* blocksize */
    unsigned char ipad = 0x36;

    unsigned char opad = 0x5c;

    unsigned char k0[64];
    unsigned char k0xorIpad[64];
    unsigned char step7data[64];
    unsigned char step5data[MAX_MESSAGE_LENGTH+128];
    unsigned char step8data[64+20];
    int i;

    for (i=0; i<64; i++)
    {
        k0[i] = 0x00;
    }



    if (key_length != b)    /* Step 1 */
    {
        /* Step 2 */
        if (key_length > b)      
        {
      SHA1(key, key_length, digest);
            for (i=0;i<20;i++)
            {
                k0[i]=digest[i];
            }
        }
        else if (key_length < b)  /* Step 3 */
        {
            for (i=0; i<key_length; i++)
            {
                k0[i] = key[i];
            }
        }
    }
    else
    {
        for (i=0;i<b;i++)
        {
            k0[i] = key[i];
        }
    }
    /* Step 4 */
    for (i=0; i<64; i++)
    {
        k0xorIpad[i] = k0[i] ^ ipad;
    }
    /* Step 5 */
    for (i=0; i<64; i++)
    {
        step5data[i] = k0xorIpad[i];
    }
    for (i=0;i<data_length;i++)
    {
        step5data[i+64] = data[i];
    }

    /* Step 6 */ 
    SHA1(step5data, data_length+b, digest);

    /* Step 7 */
    for (i=0; i<64; i++)
    {
        step7data[i] = k0[i] ^ opad;
    }


    /* Step 8 */
    for (i=0;i<64;i++)
    {
        step8data[i] = step7data[i];
    }
    for (i=0;i<20;i++)
    {
        step8data[i+64] = digest[i];
    }


    /* Step 9 */
    SHA1(step8data, b+20, digest);   

}



OCTETSTRING f__prf(const OCTETSTRING& pl_key, const OCTETSTRING& pl_prefix,
         const OCTETSTRING& pl_data, const INTEGER& result_length_inOctets)
{
  unsigned char *key = (unsigned char*)(const unsigned char*)pl_key;
  int key_length = pl_key.lengthof();
  const unsigned char *prefix = (const unsigned char*)pl_prefix;
  int prefix_length = pl_prefix.lengthof();
  const unsigned char *data = (const unsigned char*)pl_data;
  int data_length = pl_data.lengthof();
  unsigned char r[MAX_MESSAGE_LENGTH*2+128];
  int r_length;
  unsigned char hmac_sha_result[20];
  unsigned char input_data[MAX_MESSAGE_LENGTH*2+128];
  int input_data_length;
  int i,j;
  r_length = 0;

  for (i=0; i<((result_length_inOctets*8+159)/160); i++)
  {
    for (j=0;j<prefix_length;j++)
    {
      input_data[j] = prefix[j];
    }
    input_data[prefix_length]= 0x00;
    for (j=0;j<data_length;j++)
    {
      input_data[prefix_length+j+1] = data[j];
    }
    input_data_length = prefix_length + data_length + 1;

    input_data[input_data_length] = (unsigned char)i;
    input_data_length++;
    input_data[input_data_length] = 0x00;
    OCTETSTRING vl_input(input_data_length,input_data);
    hmac_sha1(key, key_length, input_data, input_data_length,hmac_sha_result);
    for (j=0; j<20; j++)
    {
      r[r_length+j] = hmac_sha_result[j];
    }
    r_length = r_length+20;
  }
  OCTETSTRING result(result_length_inOctets,r);
  return result;
}





}