//
//  bt_security_manager.c
//
//  Bluetooth Protocol Stack - Security Manager
//  Copyright (C) 2014-2017 Toyohiko Togashi tog001@nifty.com
//
//
//  This program is free software; you can redistribute it and/or modify it under the terms of the
//  GNU General Public License as published by the Free Software Foundation; either version 3
//  of the License, or (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
//  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
//  See the GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License along with this program.
//  If not, see <http://www.gnu.org/licenses/>
//
//
//  Reference:
//      Bluetooth SIG (www.bluetooth.com)
//          BLUETOOTH SPECIFICATION Version 4.0
//          [Vol 3] Core System Package [Host volume]
//           Part H: SECURITY MANAGER SPECIFICATION
//      Federal Information Processing Standards Publication 197 (FIPS-197)
//          ADVANCED ENCRYPTION STANDARD (AES)
//          Rijndael Algorithm
//              http://csrc.nist.gov/archive/aes/rijndael/wsdindex.html
//              Reference source code
//                  rijndael-alg-ref.[ch]
//                  rijndael-api-ref.[ch]
//
//  Implement level:
//      Encryption       O
//      Authentication   O (OOB is not support)
//      Privacy feature  X (Random device address)
//      Data signing     X
//
//  Update history
//  ---------- ----- -------------------------------------------
//  2014.02.23 v0.0  First cording
//  2014.03.05 v0.2  Commit
//  2017.01.03 v0.3e argument handle -> cb_channel
//  2017.01.18 v0.3f Appended a authentication function (No comment of date)
//  2017.02.11 v0.3h The new "bt_config.h"
//

#include <stddef.h>
#include <string.h>
#include <time.h>
#include "bt_spec_smp.h"
#include "bt_l2cap_cb_common.h"
#include "bt_l2cap_cb_link.h"
#include "rijndael-api-ref.h"           // AES cryptographic routine
#include "bt_l2cap_cb_channel.h"        // Channel manage table     ADD 2017.01.03
#include "bt_config.h"                  // ADD 2017.02.11

// Config
#define SM_KEYSIZE              16
#define SM_PHASE3_IDLE_TIMEOUT  2
//#define DEBUG                         // Debug mode on

// Inline function
#define MEMCPY(d,s)     memcpy(d, s, sizeof(d))
#define MEMCMP(d,s)     memcmp(d, s, sizeof(d))
#define MEMSET(d,s)     memset(d, s, sizeof(d))

// DEBUG tools
#ifdef DEBUG
#include <stdio.h>                      // Debug
void bt_securityManagerDumpStr(char *t, unsigned char *p, int l){
    int i;

    printf(t);
    for(i = 0; i < l; i++) {
        if (((i % 4) == 0) && (i > 0)) {
            printf(",");
        }
        printf("%02x", *p++);
    }
    printf("\n");
}
void bt_securityManagerDumpNum(char *t, unsigned char *p, int l){
    printf(t);
    while(--l >= 0) {
        if (((l % 4) == 0) && (l > 0)) {
            printf(",");
        }
        printf("%02x", *(p+l));
    }
    printf("\n");
}
#define DEBUGP(...)     printf(__VA_ARGS__)
//#define DUMP(t,p)       bt_l2capResourceManagerDumpStr(t,p,sizeof(p))
#define DUMP(t,p)       bt_securityManagerDumpNum(t,p,sizeof(p))
#define DUMPS(t,p)      bt_securityManagerDumpStr(t,p,sizeof(p))
#define DUMPN(t,p)      bt_securityManagerDumpNum(t,p,sizeof(p))
#else
#define DEBUGP(...)
#define DUMP(...)
#define DUMPS(...)
#define DUMPN(...)
#endif

// Security function e
void bt_securityManagerCrypt_convCopy(unsigned char *out, unsigned char *in){
    out[ 0] = in[15];
    out[ 1] = in[14];
    out[ 2] = in[13];
    out[ 3] = in[12];
    out[ 4] = in[11];
    out[ 5] = in[10];
    out[ 6] = in[ 9];
    out[ 7] = in[ 8];
    out[ 8] = in[ 7];
    out[ 9] = in[ 6];
    out[10] = in[ 5];
    out[11] = in[ 4];
    out[12] = in[ 3];
    out[13] = in[ 2];
    out[14] = in[ 1];
    out[15] = in[ 0];
    return;
}
void bt_securityManagerCrypt_e(unsigned char *encorded, unsigned char *key, unsigned char *plaintextData){
    keyInstance     ki;
    cipherInstance  ci;
    char            keyStr[33];
    int             i;
    int             rc;
    unsigned char   in[SM_KEYSIZE];
    unsigned char   out[SM_KEYSIZE];

    //
    //  rijndael API convert
    //
    bt_securityManagerCrypt_convCopy(in, plaintextData);
    memset(&ki, 0, sizeof(ki));
    memset(&ci, 0, sizeof(ci));
    for(i = 0; i < SM_KEYSIZE; i++) {
        sprintf(&(keyStr[i*2]), "%02x", key[15 - i]);
    }
    keyStr[i*2] = '\0';
    ki.blockLen = 128;
    rc = makeKey(&ki, DIR_ENCRYPT, 128, keyStr);
    if (rc == FALSE) {
        DEBUGP("makeKey error=%d\n", rc);
    }
    ci.blockLen = 128;
    rc = cipherInit(&ci, MODE_ECB, "00000000000000000000000000000000");
    if (rc == FALSE) {
        DEBUGP("cipherInit error=%d\n", rc);
    }
    rc = blockEncrypt(&ci, &ki, in, 128, out);
//  DEBUGP("blockEncrypt error=%d\n", rc);
//  DUMP("txt=", in);
//  DUMP("enc=", out);
    bt_securityManagerCrypt_convCopy(encorded, out);
    return;
}

// Random Address Hash function ah
void bt_securityManagerCrypt_ah(unsigned char *out, unsigned char *k, unsigned char *r){
    unsigned char   rd[SM_KEYSIZE];

    rd[15] = 0;
    rd[14] = 0;
    rd[13] = 0;
    rd[12] = 0;
    rd[11] = 0;
    rd[10] = 0;
    rd[ 9] = 0;
    rd[ 8] = 0;
    rd[ 7] = 0;
    rd[ 6] = 0;
    rd[ 5] = 0;
    rd[ 4] = 0;
    rd[ 3] = 0;
    rd[ 2] = r[2];
    rd[ 1] = r[1];
    rd[ 0] = r[0];
    bt_securityManagerCrypt_e(out, k, rd);
    out[15] = 0;
    out[14] = 0;
    out[13] = 0;
    out[12] = 0;
    out[11] = 0;
    out[10] = 0;
    out[ 9] = 0;
    out[ 8] = 0;
    out[ 7] = 0;
    out[ 6] = 0;
    out[ 5] = 0;
    out[ 4] = 0;
    out[ 3] = 0;
    return;
}

// Confirm value generation function c1
void bt_securityManagerCrypt_c1(unsigned char *out,
        unsigned char *k, unsigned char *r, unsigned char *preq, unsigned char *pres, int iat, unsigned char *ia, int rat, unsigned char *ra){
    unsigned char   p1[SM_KEYSIZE];
    unsigned char   p2[SM_KEYSIZE];
    unsigned char   px[SM_KEYSIZE];
    int             i;
//  unsigned char   p1_test[SM_KEYSIZE] = {0x05,0x00,0x08,0x00,0x00,0x03,0x02,0x07,0x07,0x10,0x00,0x00,0x01,0x01,0x00,0x01};
//  unsigned char   p2_test[SM_KEYSIZE] = {0x00,0x00,0x00,0x00,0xA1,0xA2,0xA3,0xA4,0xA5,0xA6,0xB1,0xB2,0xB3,0xB4,0xB5,0xB6};

    p1[15] = pres[6];
    p1[14] = pres[5];
    p1[13] = pres[4];
    p1[12] = pres[3];
    p1[11] = pres[2];
    p1[10] = pres[1];
    p1[ 9] = pres[0];
    p1[ 8] = preq[6];
    p1[ 7] = preq[5];
    p1[ 6] = preq[4];
    p1[ 5] = preq[3];
    p1[ 4] = preq[2];
    p1[ 3] = preq[1];
    p1[ 2] = preq[0];
    p1[ 1] = rat;
    p1[ 0] = iat;
    p2[15] = 0;
    p2[14] = 0;
    p2[13] = 0;
    p2[12] = 0;
    p2[11] = ia[5];
    p2[10] = ia[4];
    p2[ 9] = ia[3];
    p2[ 8] = ia[2];
    p2[ 7] = ia[1];
    p2[ 6] = ia[0];
    p2[ 5] = ra[5];
    p2[ 4] = ra[4];
    p2[ 3] = ra[3];
    p2[ 2] = ra[2];
    p2[ 1] = ra[1];
    p2[ 0] = ra[0];
//  bt_securityManagerCrypt_convCopy(p1, p1_test);
//  bt_securityManagerCrypt_convCopy(p2, p2_test);
//  DUMP("p1=", p1);
//  DUMP("p2=", p2);
    for(i = 0; i < SM_KEYSIZE; i++) {
        p1[i] ^= r[i];
    }
    bt_securityManagerCrypt_e(px, k, p1);
    for(i = 0; i < SM_KEYSIZE; i++) {
        px[i] ^= p2[i];
    }
    bt_securityManagerCrypt_e(out, k, px);
    return;
}

// Key generation function s1
void bt_securityManagerCrypt_s1(unsigned char *out, unsigned char *k, unsigned char *r1, unsigned char *r2){
    unsigned char   rd[SM_KEYSIZE];

    rd[15] = r1[ 7];
    rd[14] = r1[ 6];
    rd[13] = r1[ 5];
    rd[12] = r1[ 4];
    rd[11] = r1[ 3];
    rd[10] = r1[ 2];
    rd[ 9] = r1[ 1];
    rd[ 8] = r1[ 0];
    rd[ 7] = r2[ 7];
    rd[ 6] = r2[ 6];
    rd[ 5] = r2[ 5];
    rd[ 4] = r2[ 4];
    rd[ 3] = r2[ 3];
    rd[ 2] = r2[ 2];
    rd[ 1] = r2[ 1];
    rd[ 0] = r2[ 0];
    bt_securityManagerCrypt_e(out, k, rd);
    return;
}

// 5.1.1.1 DIV Mask generation function dm
void bt_securityManagerCrypt_dm(unsigned char *out, unsigned char *k, unsigned char *r){
    unsigned char   rd[SM_KEYSIZE];

    rd[15] = 0;
    rd[14] = 0;
    rd[13] = 0;
    rd[12] = 0;
    rd[11] = 0;
    rd[10] = 0;
    rd[ 9] = 0;
    rd[ 8] = 0;
    rd[ 7] = r[ 7];
    rd[ 6] = r[ 6];
    rd[ 5] = r[ 5];
    rd[ 4] = r[ 4];
    rd[ 3] = r[ 3];
    rd[ 2] = r[ 2];
    rd[ 1] = r[ 1];
    rd[ 0] = r[ 0];
    bt_securityManagerCrypt_e(out, k, rd);
    out[15] = 0;
    out[14] = 0;
    out[13] = 0;
    out[12] = 0;
    out[11] = 0;
    out[10] = 0;
    out[ 9] = 0;
    out[ 8] = 0;
    return;
}

// 5.2.2.1 Diversifying function d1
void bt_securityManagerCrypt_d1(unsigned char *out, unsigned char *k, unsigned char *d, unsigned char *r){
    unsigned char   rd[SM_KEYSIZE];

    rd[15] = 0;
    rd[14] = 0;
    rd[13] = 0;
    rd[12] = 0;
    rd[11] = 0;
    rd[10] = 0;
    rd[ 9] = 0;
    rd[ 8] = 0;
    rd[ 7] = 0;
    rd[ 6] = 0;
    rd[ 5] = 0;
    rd[ 4] = 0;
    rd[ 3] = r[ 1];
    rd[ 2] = r[ 0];
    rd[ 1] = d[ 1];
    rd[ 0] = d[ 0];
    bt_securityManagerCrypt_e(out, k, rd);
    return;
}

// 5.2 Key management (EDIV+Rand to LTK,CSRK,IRK)
void bt_securityManagerKeyGeneration(unsigned char *ltk, unsigned char *irk, unsigned char *csrk,
                                     unsigned char *ediv, unsigned char *rand, unsigned char *newDiv) {
    static unsigned char    v0[2] = {0x00,0x00};
    static unsigned char    v1[2] = {0x01,0x00};
    static unsigned char    v3[2] = {0x03,0x00};
    unsigned char   dhk[SM_KEYSIZE];
    unsigned char   y[SM_KEYSIZE];
    unsigned char   div[2];

    bt_securityManagerCrypt_d1(dhk, bt_l2capCommon.ir, v3,  v0);
    bt_securityManagerCrypt_dm(y, dhk, rand);
    if (newDiv) {
        div[0]  = newDiv[0];
        div[1]  = newDiv[1];
        ediv[0] = div[0] ^ y[0];    // Out
        ediv[1] = div[1] ^ y[1];    // Put
    } else {
        div[0] = ediv[0] ^ y[0];
        div[1] = ediv[1] ^ y[1];
    }

    if (ltk) {
        bt_securityManagerCrypt_d1(ltk,  bt_l2capCommon.er, div, v0);
    }
    if (csrk) {
        bt_securityManagerCrypt_d1(csrk, bt_l2capCommon.er, div, v1);
    }
    if (irk) {
        bt_securityManagerCrypt_d1(irk,  bt_l2capCommon.ir, v1,  v0);
    }
    return;
}

//
// User own code (default)
//
long _bt_securityManagerPasskeyInput(struct bt_l2capChannel *ch) {
    // 0-999999:normal(end) -1:pending(continue)
//  return(123456);                     // DEL 2017.02.11
    return(BT_SMP_DEFAULT_PASSKEY);     // ADD 2017.02.11
}
long _bt_securityManagerPasskeyCreateDisplay(struct bt_l2capChannel *ch) {
//  return(123456);                     // DEL 2017.02.11
    return(BT_SMP_DEFAULT_PASSKEY);     // ADD 2017.02.11
}
long bt_securityManagerPasskeyInput()         __attribute__ ((weak, alias ("_bt_securityManagerPasskeyInput")));
long bt_securityManagerPasskeyCreateDisplay() __attribute__ ((weak, alias ("_bt_securityManagerPasskeyCreateDisplay")));


//
//  Server
//
//size_t bt_securityManager(unsigned short handle, unsigned char *inBuff, size_t inLen, unsigned char *outBuff, size_t outSize) {       // DEL 2017.01.03
size_t bt_securityManager(struct bt_l2capChannel *ch, unsigned char *inBuff, size_t inLen, unsigned char *outBuff, size_t outSize) {    // ADD 2017.01.03
    struct bt_smp_PDU_Format    *in;
    struct bt_smp_PDU_Format    *out;
    struct bt_smp_PDU_Format    *pres;
    struct bt_smp_PDU_Format    *preq;
    size_t                      len;
    struct bt_l2capLink         *cb;
    unsigned char               tk[SM_KEYSIZE];
//  static unsigned char        srand[SM_KEYSIZE] = {0xE0,0x2E,0x70,0xC6,0x4E,0x27,0x88,0x63,0x0E,0x6F,0xAD,0x56,0x21,0xD5,0x83,0x57};  // DEL 2017.02.11
    static unsigned char        srand[SM_KEYSIZE] = {BT_SMP_STK_SRAND};                                                                 // ADD 2017.02.11
#define PK_NONE      0x00               // Just Works
#define PK_DISPLAY   0x01
#define PK_INPUT     0x02
    const static unsigned char  action[SMP_IO_Capability_KeyboardDisplay+1][SMP_IO_Capability_KeyboardDisplay+1] = {
        /* Resp(me) \  Init  DisplayOnly  DisplayYesNo  KeyboardOnly  NoInputNoOutput  KeyboardDisplay */
        /*DisplayOnly*/     {PK_NONE,     PK_NONE,      PK_DISPLAY,   PK_NONE,         PK_DISPLAY      },
        /*DisplayYesNo*/    {PK_NONE,     PK_NONE,      PK_DISPLAY,   PK_NONE,         PK_DISPLAY      },
        /*KeyboardOnly*/    {PK_INPUT,    PK_INPUT,     PK_INPUT,     PK_NONE,         PK_INPUT        },
        /*NoInputNoOutput*/ {PK_NONE,     PK_NONE,      PK_NONE,      PK_NONE,         PK_NONE         },
        /*KeyboardDisplay*/ {PK_INPUT,    PK_INPUT,     PK_DISPLAY,   PK_NONE,         PK_INPUT        },
        };

    in  = (struct bt_smp_PDU_Format *)inBuff;
    out = (struct bt_smp_PDU_Format *)outBuff;
    len = 0;
//  cb  = bt_l2capLinkFind(NULL, handle);   // Not NULL // DEL 2017.01.03
    cb   = ch->link;                                    // ADD 2017.01.03
    pres = (struct bt_smp_PDU_Format *)&(ch->sm.pres);
    preq = (struct bt_smp_PDU_Format *)&(ch->sm.preq);
    MEMSET(tk, 0);
    tk[0] = ch->sm.passKey;
    tk[1] = ch->sm.passKey >> 8;
    tk[2] = ch->sm.passKey >> 16;
    tk[3] = ch->sm.passKey >> 24;
//  MEMCPY(srand, bt_l2capCommon.rand1);    // rand1+2

    switch(ch->sm.state) {
    case SM_IDLE:

    case SM_PHASE1:
        //
        // Phase 1
        //
        if (in != NULL) {
            switch(in->Code) {
            case SMP_Code_Pairing_Request:
                DEBUGP("SM: Pairing_Request io=0x%02x auth=%d OOB=%d\n",
                        in->Pairing_Request.IO_Capability,
                        in->Pairing_Request.AuthReq,
                        in->Pairing_Request.OOB_data_flag);
                if (in->Pairing_Request.IO_Capability > SMP_IO_Capability_KeyboardDisplay) {
                    out->Code                  = SMP_Code_Pairing_Failed;
                    out->Pairing_Failed.Reason = SMP_Reason_Invalid_Parameters;
                    len = SMP_PDU_Format_HeadSize + sizeof(out->Pairing_Failed);
                } else if (in->Pairing_Request.OOB_data_flag != SMP_OOB_data_flag_Authentication_data_not_present) {
                    out->Code                  = SMP_Code_Pairing_Failed;
                    out->Pairing_Failed.Reason = SMP_Reason_OOB_Not_Available;
                    len = SMP_PDU_Format_HeadSize + sizeof(out->Pairing_Failed);
                } else {
                    out->Code = SMP_Code_Pairing_Response;
                    out->Pairing_Response.IO_Capability               = bt_l2capCommon.responderIoCapability;
                    out->Pairing_Response.AuthReq                     = bt_l2capCommon.responderAuthReq;
                    out->Pairing_Response.OOB_data_flag               = bt_l2capCommon.responderOOBDataFlag;
                    out->Pairing_Response.Maximum_Encryption_Key_Size = SM_KEYSIZE;
                    out->Pairing_Response.Initiator_Key_Distribution  = in->Pairing_Request.Initiator_Key_Distribution & (0
                                                                      | SMP_Key_Distribution_EncKey
                                                                      | SMP_Key_Distribution_IdKey
//                                                                    | SMP_Key_Distribution_Sign
                                                                      );
                    out->Pairing_Response.Responder_Key_Distribution  = in->Pairing_Request.Responder_Key_Distribution & (0
                                                                      | SMP_Key_Distribution_EncKey
//                                                                    | SMP_Key_Distribution_IdKey
//                                                                    | SMP_Key_Distribution_Sign
                                                                      );
                    len = SMP_PDU_Format_HeadSize + sizeof(out->Pairing_Response);

                    MEMCPY(ch->sm.preq, in);
                    MEMCPY(ch->sm.pres, out);
                    ch->sm.action = (unsigned char)action[out->Pairing_Response.IO_Capability][in->Pairing_Request.IO_Capability];
                    if (ch->sm.action == PK_DISPLAY) {
                        ch->sm.passKey = bt_securityManagerPasskeyCreateDisplay(ch);
                    } else {
                        ch->sm.passKey = 0;
                    }
                    ch->sm.timeOut = time(NULL) + SMP_Timeout;
                    ch->sm.state   = SM_PHASE2;
                    
                    cb->encryption     = 0;
                    cb->authentication = 0;
                    cb->device->auth   = 0;     // Bonding
                    DEBUGP("SM: Pairing_Response send\n");
                }
                break;
            case SMP_Code_Pairing_Failed:
                DEBUGP("SM: Phase1 Faild!! Reason=0x%02x\n", in->Pairing_Failed.Reason);
                ch->sm.state = SM_IDLE;
                break;
            default:
                DEBUGP("SM: Unsupport Code=0x%02x phase1\n", in->Code);
//              out->Code                  = SMP_Code_Pairing_Failed;
//              out->Pairing_Failed.Reason = SMP_Reason_Command_Not_Supported;
//              len = SMP_PDU_Format_HeadSize + sizeof(out->Pairing_Failed);
                break;
            }
        }
        break;

    case SM_PHASE2:
        //
        // Phase 2
        //
        if (in != NULL) {
            switch(in->Code) {
            case SMP_Code_Pairing_Confirm:
            {
                DEBUGP("SM: Pairing_Confirm\n");
                if (preq->Pairing_Request.Maximum_Encryption_Key_Size != SM_KEYSIZE) {
                    out->Code                  = SMP_Code_Pairing_Failed;
                    out->Pairing_Failed.Reason = SMP_Reason_Encryption_Key_Size;
                    len = SMP_PDU_Format_HeadSize + sizeof(out->Pairing_Failed);
                    ch->sm.state = SM_IDLE;
                } else if ((ch->sm.action == PK_INPUT) && (ch->sm.passKey < 0)) {
                    out->Code                  = SMP_Code_Pairing_Failed;
                    out->Pairing_Failed.Reason = SMP_Reason_Passkey_Entry_Failed;
                    len = SMP_PDU_Format_HeadSize + sizeof(out->Pairing_Failed);
                    ch->sm.state = SM_IDLE;
                } else {
                    MEMCPY(ch->sm.mconfirm, in->Pairing_Confirm.Confirm_value); // Save
                    // Creating and sending the sconfirm
                    bt_securityManagerCrypt_c1(out->Pairing_Confirm.Confirm_value, tk, srand, ch->sm.preq, ch->sm.pres, cb->device->addrType, cb->device->bdAddr, bt_l2capCommon.myAddrType, bt_l2capCommon.myBdAddr);
                    len = SMP_PDU_Format_HeadSize + sizeof(out->Pairing_Confirm);
                }
                break;
            }
            case SMP_Code_Pairing_Random:
            {
                unsigned char   chk[sizeof(in->Pairing_Confirm.Confirm_value)];

                DEBUGP("SM: Pairing_Random\n");
                // Checking a mconfirm by the mrand
                bt_securityManagerCrypt_c1(chk, tk, in->Pairing_Random.Random_value, ch->sm.preq, ch->sm.pres, cb->device->addrType, cb->device->bdAddr, bt_l2capCommon.myAddrType, bt_l2capCommon.myBdAddr);
                if (MEMCMP(chk, ch->sm.mconfirm)) {
                    DEBUGP("SM: mrand unmatch!\n");
                    out->Code                  = SMP_Code_Pairing_Failed;
                    out->Pairing_Failed.Reason = SMP_Reason_Confirm_Value_Failed;
                    len = SMP_PDU_Format_HeadSize + sizeof(out->Pairing_Failed);
                    ch->sm.state = SM_IDLE;
                } else {
                    DEBUGP("SM: mrand ok\n");
                    // Creating a STK, Sending a srand
                    bt_securityManagerCrypt_s1(cb->ltk, tk, srand, in->Pairing_Random.Random_value);    // .Random_value=mrand
                    DUMP(" STK=", cb->ltk);

                    out->Code = SMP_Code_Pairing_Random;
                    MEMCPY(out->Pairing_Random.Random_value, srand);
                    len = SMP_PDU_Format_HeadSize + sizeof(out->Pairing_Random);
                    ch->sm.timeOut = time(NULL) + SMP_Timeout;
                    ch->sm.state = SM_PHASE3;
                }
                break;
            }
            case SMP_Code_Pairing_Failed:
                DEBUGP("SM: Phase2 Faild!! Reason=%02x\n", in->Pairing_Failed.Reason);
                ch->sm.state = SM_IDLE;
                break;
            default:
                DEBUGP("SM: Unsupport Code=%02x\n", in->Code);
                out->Code                  = SMP_Code_Pairing_Failed;
                out->Pairing_Failed.Reason = SMP_Reason_Command_Not_Supported;
                len = SMP_PDU_Format_HeadSize + sizeof(out->Pairing_Failed);
                break;
            }
        } else {
            if (ch->sm.timeOut < time(NULL)) {
                ch->sm.state = SM_IDLE;
            // Passkey entering
            } else if (ch->sm.action == PK_INPUT) {
                ch->sm.passKey = bt_securityManagerPasskeyInput(ch);
            }
        }
        break;

    case SM_PHASE3:
        //
        // Phase 3
        //
        if (cb->encryption) {
            unsigned char   div[2];

            DEBUGP("SM: Phase3 STK encryption on\n");

            // Creating the new keys(EDIV,Rand,LTK...)
            div[0] = cb->device->bdAddr[1];
            div[1] = cb->device->bdAddr[0];
            MEMCPY(cb->rand, bt_l2capCommon.rand1);
            bt_securityManagerKeyGeneration(cb->ltk, cb->irk, cb->csrk, cb->ediv, cb->rand, div);

            // Authenticated
            if (ch->sm.action != PK_NONE) {
                cb->authentication = 1;
            }

            ch->sm.state = SM_PHASE3_SEND_LTK;
        } else if (ch->sm.timeOut < time(NULL)) {
            ch->sm.state = SM_IDLE;
        }
        break;

    case SM_PHASE3_SEND_LTK:
        if (pres->Pairing_Response.Responder_Key_Distribution & SMP_Key_Distribution_EncKey) {
            DEBUGP("SM: Encryption_Information Send\n");
            DUMP(" LTK=", cb->ltk);
            out->Code = SMP_Code_Encryption_Information;
            MEMCPY(out->Encryption_Information.Long_Term_Key, cb->ltk);
            len = SMP_PDU_Format_HeadSize + sizeof(out->Encryption_Information);
        }
        ch->sm.state = SM_PHASE3_SEND_EDIV;
        break;
    case SM_PHASE3_SEND_EDIV:
        if (pres->Pairing_Response.Responder_Key_Distribution & SMP_Key_Distribution_EncKey) {
            DEBUGP("SM: Master_Identification Send\n");
            DEBUGP(" EDIV=%02x%02x", cb->ediv[1], cb->ediv[0]);
            DUMP(" RAND=", cb->rand);
            out->Code = SMP_Code_Master_Identification;
            MEMCPY(out->Master_Identification.EDIV, cb->ediv);
            MEMCPY(out->Master_Identification.Rand, cb->rand);
            len = SMP_PDU_Format_HeadSize + sizeof(out->Master_Identification);
        }
        ch->sm.state = SM_PHASE3_SEND_ID;
        break;
    case SM_PHASE3_SEND_ID:
        if (pres->Pairing_Response.Responder_Key_Distribution & SMP_Key_Distribution_IdKey) {
            DEBUGP("SM: Identity_Information Send\n");
            DUMP(" IRK=", cb->irk);
            out->Code = SMP_Code_Identity_Information;
            MEMCPY(out->Identity_Information.Identity_Resolving_Key, cb->irk);
            len = SMP_PDU_Format_HeadSize + sizeof(out->Identity_Information);
        }
        ch->sm.state = SM_PHASE3_SEND_ADDR;
        break;
    case SM_PHASE3_SEND_ADDR:
        if (pres->Pairing_Response.Responder_Key_Distribution & SMP_Key_Distribution_IdKey) {
            DEBUGP("SM: Identity_Address_Information Send\n");
            DEBUGP(" TYPE=%d", bt_l2capCommon.myAddrType);
            DUMP(" BDADDR=", bt_l2capCommon.myBdAddr);
            out->Code = SMP_Code_Identity_Address_Information;
            MEMCPY(out->Identity_Address_Information.BD_ADDR, bt_l2capCommon.myBdAddr);
            out->Identity_Address_Information.AddrType      = bt_l2capCommon.myAddrType;
            len = SMP_PDU_Format_HeadSize + sizeof(out->Identity_Address_Information);
        }
        ch->sm.state = SM_PHASE3_SEND_SIG;
        break;
    case SM_PHASE3_SEND_SIG:
        if (pres->Pairing_Response.Responder_Key_Distribution & SMP_Key_Distribution_Sign) {
            DEBUGP("SM: Signing_Information Send\n");
            DUMP(" CSRK=", cb->csrk);
            out->Code = SMP_Code_Signing_Information;
            MEMCPY(out->Signing_Information.Signature_Key, cb->csrk);
            len = SMP_PDU_Format_HeadSize + sizeof(out->Signing_Information);
        }
        ch->sm.timeOut = time(NULL) + SM_PHASE3_IDLE_TIMEOUT;
        ch->sm.state   = SM_PHASE3_IDLE;
        break;
    case SM_PHASE3_IDLE:
        if (in != NULL) {
            switch(in->Code) {
            case SMP_Code_Encryption_Information:   // Initiator_Key_Distribution : SMP_Key_Distribution_EncKey
                DEBUGP("SM: Encryption_Information Recv\n");
                DUMP(" ltk=", in->Encryption_Information.Long_Term_Key);
//              MEMCPY(cb->initLtk, in->Encryption_Information.Long_Term_Key);
                break;
            case SMP_Code_Master_Identification:    // Initiator_Key_Distribution : SMP_Key_Distribution_EncKey
                DEBUGP("SM: Master_Identification Recv\n");
                DEBUGP(" ediv=%02x%02x", in->Master_Identification.EDIV[1], in->Master_Identification.EDIV[0]);
                DUMP(" rand=", in->Master_Identification.Rand);
//              MEMCPY(cb->initEdiv, in->Master_Identification.EDIV);
//              MEMCPY(cb->initRand, in->Master_Identification.Rand);
                break;
            case SMP_Code_Identity_Information:     // Initiator_Key_Distribution : SMP_Key_Distribution_IdKey
                DEBUGP("SM: Identity_Information Recv\n");
                DUMP(" irk=", in->Identity_Information.Identity_Resolving_Key);
                MEMCPY(cb->initIrk, in->Identity_Information.Identity_Resolving_Key);
                break;
            case SMP_Code_Identity_Address_Information:   // Initiator_Key_Distribution : SMP_Key_Distribution_IdKey
                DEBUGP("SM: Identity_Address_Information Recv\n");
                DEBUGP(" type=%d", in->Identity_Address_Information.AddrType);
                DUMP(" addr=", in->Identity_Address_Information.BD_ADDR);
                MEMCPY(cb->device->bdAddr, in->Identity_Address_Information.BD_ADDR);	// True device address
                cb->device->addrType = in->Identity_Address_Information.AddrType;
                break;
            case SMP_Code_Signing_Information:      // Initiator_Key_Distribution : SMP_Key_Distribution_Sign
                DEBUGP("SM: Signing_Information Recv\n");
                DUMP(" key=", in->Signing_Information.Signature_Key);
//              MEMCPY(cb->initCsrk, in->Signing_Information.Signature_Key);
                break;
            case SMP_Code_Pairing_Failed:
                DEBUGP("SM: Phase3 Faild!! Reason=0x%02x\n", in->Pairing_Failed.Reason);
                ch->sm.state = SM_IDLE;
                break;
            default:
                DEBUGP("SM: Unsupport Code=0x%02x\n", in->Code);
                out->Code                  = SMP_Code_Pairing_Failed;
                out->Pairing_Failed.Reason = SMP_Reason_Command_Not_Supported;
                len = SMP_PDU_Format_HeadSize + sizeof(out->Pairing_Failed);
                break;
            }
        } else {
            if (!cb->encryption) {
                DEBUGP("SM: Phase3 encryption off\n");
                ch->sm.state = SM_IDLE;
            } else if (ch->sm.timeOut < time(NULL)) {
                DEBUGP("SM: Phase3 Timeout\n");
                if (pres->Pairing_Response.AuthReq & SMP_AuthReq_Bonding) {
                    DEBUGP("SM: Save the keys\n");
                    MEMCPY(cb->device->ediv, cb->ediv);
                    MEMCPY(cb->device->rand, cb->rand);
                    cb->device->auth = cb->authentication;
//                  MEMCPY(cb->device->initEdiv, cb->initEdiv);
//                  MEMCPY(cb->device->initRand, cb->initRand);
//                  MEMCPY(cb->device->initLtk,  cb->initLtk);
                    MEMCPY(cb->device->initIrk,  cb->initIrk);
//                  MEMCPY(cb->device->initCsrk, cb->initCsrk);
                }
                ch->sm.state = SM_IDLE;
            }
        }
        break;

    default:
        DEBUGP("SM: bad state=%d\n",ch->sm.state);
        ch->sm.state = SM_IDLE;
        break;
    }

    return(len);
}
