//
//  bt_l2cap_signaling.c
//
//  Bluetooth Protocol Stack - L2CAP Signaling Process
//  Copyright (C) 2013-2018 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 A: Logical Link Control and Adaptation Protocol Specification
//              4 SIGNALING PACKET FORMATS
//              5 CONFIGURATION PARAMETER OPTIONS
//              6 STATE MACHINE
//
//  Update history
//  ---------- ----- -------------------------------------------
//  2013.01.21 v0.0  First cording
//  2013.04.21 v0.1  Commit
//  2016.04.21 v0.3d Bug fix (PIC32 "Store address error")
//  2017.01.03 v0.3e Changed a argument (handle -> cb_channel, BD_ADDR -> cb_link, psm -> cb_service)
//  2017.01.08 v0.3e Appended a authentication function for the classic services
//  2017.01.08 v0.3e Renamed scid,dcid -> peerCid,myCid
//  2018.11.06 v0.4c Supported a notification to server APP
//
#include <stddef.h>
#include <string.h>
#include "bt_spec_hci.h"            // HCI format
#include "bt_spec_l2cap.h"          // L2CAP format
#include "bt_l2cap_cb_service.h"    // Service manage table
#include "bt_l2cap_cb_channel.h"    // Channel manage table

#define MAX(a,b)    ((a)>(b)?(a):(b))
//#define DEBUG                     // Debug mode on
//#define DEBUG2                    // Verbose

//#define BT_CONFIG_TIMEOUT     100000  // DEL 2017.01.08
//#define BT_DISCONNECT_TIMEOUT 100000  // DEL 2017.01.08
#define BT_RTX_TIMEOUT          3       // ADD 2017.01.08  1-60[sec]
#define BT_ERTX_TIMEOUT         60      // ADD 2017.01.08  60-300[sec]

#ifdef DEBUG
#include <stdio.h>                      // Debug
#define DEBUGP(...)     printf(__VA_ARGS__)
#else
#define DEBUGP(...)
#endif

// Configuration option
#ifdef DEL                              // DEL START 2017.01.08
int bt_l2capConfigurationParameterParse(unsigned char *option, size_t len) {
    DEBUGP("L2CAP-SG: bt_l2capConfigurationParameterParse: len=%d\n", len);
    return(0);
}
int bt_l2capConfigurationParameterRequest(unsigned char *option, size_t len) {
    *option++ = 1;          // MAXIMUM TRANSMISSION UNIT (MTU)
    *option++ = 2;          // Parameter bytes
    L2CAP_STORE16(option, len);
    DEBUGP("L2CAP-SG: bt_l2capConfigurationParameterRequest: len=%d\n", len);
    return(4);
}
#else                                   // DEL END - ADD START 2017.01.08
int bt_l2capConfigurationParameterParse(unsigned char *peerOptions, size_t peerSize, unsigned char *answerOptions, size_t *answerSize, struct bt_l2capChannel *channel) {    // Length of answerOptions
    struct bt_l2cap_Configuration_Option    *in;
    struct bt_l2cap_Configuration_Option    *out;
    int inLen;
    int outLen;
    int result;

    result = L2CAP_PDU_Result_Configuration_Success;
    in     = (struct bt_l2cap_Configuration_Option *)peerOptions;
    out    = (struct bt_l2cap_Configuration_Option *)answerOptions;
    inLen  = peerSize;
    outLen = 0;
    while(inLen > 0) {
        size_t  s;
        int     f;

        s = in->Length + L2CAP_Configuration_Option_HeadSize;
        out->Length = in->Length;
        out->Type   = in->Type;
        f = 0;

        switch(in->Type){
        case L2CAP_PDU_Configuration_option_Type_MTU:
        {
            size_t  mtu;
            
            mtu = L2CAP_UINT16(in->Option.MTU.MTU);
            if (mtu > channel->link->buffSize) {
                mtu = channel->link->buffSize;
            } else {
                channel->link->buffSize = mtu;
            }
            L2CAP_STORE16(out->Option.MTU.MTU, mtu);
            f = 1;
            break;
        }
        case L2CAP_PDU_Configuration_option_Type_Flush_Timeout:
            break;
        case L2CAP_PDU_Configuration_option_Type_QoS:
            break;
        case L2CAP_PDU_Configuration_option_Type_Retransmission:
            out->Option.Retransmission.Mode = in->Option.Retransmission.Mode & 
                    L2CAP_PDU_Configuration_option_Retransmission_L2CAP_Basic_Mode;
            out->Option.Retransmission.TxWindow_size = 0;
            out->Option.Retransmission.Max_Transmit  = 0;
            L2CAP_STORE16(out->Option.Retransmission.Retransmission_TimeOut, 0);
            L2CAP_STORE16(out->Option.Retransmission.Monitor_TimeOut,        0);
            L2CAP_STORE16(out->Option.Retransmission.Maximum_PDU_size,       0);
            f = 1;
            break;
        case L2CAP_PDU_Configuration_option_Type_FCS:
            out->Option.FCS.FCS_Type = L2CAP_PDU_Configuration_option_FCS_Types_No_FCS;
            f = 1;
            break;
        case L2CAP_PDU_Configuration_option_Type_Extended_Flow:
            out->Option.Extended_Flow.Identifier   = 0x01;
            out->Option.Extended_Flow.Service_Type = L2CAP_PDU_Configuration_option_QoS_Service_Type_Best_effort;
            L2CAP_STORE16(out->Option.Extended_Flow.Maximum_SDU_Size,      0xffff);
            L2CAP_STORE32(out->Option.Extended_Flow.SDU_InterArrival_Time, 0xffffffff);
            L2CAP_STORE32(out->Option.Extended_Flow.Access_Latency,        0xffffffff);
            L2CAP_STORE32(out->Option.Extended_Flow.Flush_Timeout,         0xffffffff);
            f = 1;
            break;
        case L2CAP_PDU_Configuration_option_Type_Extended_Window_Size:
            break;
        default:
            result = L2CAP_PDU_Result_Configuration_Failure_unknown_options;
            break;
        }

        if (f) {
            outLen += s;
            out     = (struct bt_l2cap_Configuration_Option *)&(out->Option.Data[out->Length]);
        }
        inLen -= s;
        in     = (struct bt_l2cap_Configuration_Option *)&(in->Option.Data[in->Length]);
    }
    *answerSize = outLen;
    return(result);
}
size_t bt_l2capConfigurationParameterRequest(unsigned char *options, struct bt_l2capChannel *channel) {    // Length of options
    struct bt_l2cap_Configuration_Option    *out;
    size_t  outLen;
    int     s;
    int     i;

    out    = (struct bt_l2cap_Configuration_Option *)options;
    outLen = 0;
    for(i = 0; i < 1; i++) {
        switch(i) {
        case 0: // Maximum Transmission Unit
            out->Type   = L2CAP_PDU_Configuration_option_Type_MTU;
            out->Length = sizeof(out->Option.MTU);
            L2CAP_STORE16(out->Option.MTU.MTU, channel->link->buffSize);
            break;
        default:
            break;
        }
        s       = out->Length + L2CAP_Configuration_Option_HeadSize;
        outLen += s;
        out     = (struct bt_l2cap_Configuration_Option *)&(out->Option.Data[out->Length]);
    }
    return(outLen);
}
#endif                                  // ADD END 2017.01.08



size_t bt_l2capSignaling(
//      unsigned short          handle,     // DEL 2017.01.03
        struct bt_l2capChannel  *signaling, // ADD 2017.01.03
        unsigned char           *inbuff,
        size_t                  inLen,
        unsigned char           *outbuff,
        size_t                  maxSize
) {
    struct bt_l2cap_c_frame *in;
    struct bt_l2cap_c_frame *out;
    struct bt_l2capChannel  *p;             // Target channel
    size_t                  outLen;

    in  = (struct bt_l2cap_c_frame *)inbuff;
    out = (struct bt_l2cap_c_frame *)outbuff;

    //
    // Search of target channel
    //
#ifdef DEL                                  // DEL START 2017.01.08
    if ((handle != 0) && (in != NULL)) {
        unsigned short          peerCid;
        unsigned short          myCid;

        peerCid = 0;
        myCid   = 0;
        switch(in->Code){
        case L2CAP_PDU_Code_Command_reject:
            break;
//      case L2CAP_PDU_Code_Connection_request:
//          peerCid = L2CAP_UINT16(in->Data.Connection_Request.SourceCID);
//          break;
        case L2CAP_PDU_Code_Connection_response:
            peerCid = L2CAP_UINT16(in->Data.Connection_Response.DistinationCID);       // !
            myCid   = L2CAP_UINT16(in->Data.Connection_Response.SourceCID);            // !
            break;
        case L2CAP_PDU_Code_Configure_request:
            peerCid = 0;
            myCid   = L2CAP_UINT16(in->Data.Configuration_Request.DistinationCID);
            break;
        case L2CAP_PDU_Code_Configure_response:
            peerCid = 0;
            myCid   = L2CAP_UINT16(in->Data.Configuration_Response.SourceCID);         // !
            break;
        case L2CAP_PDU_Code_Disconnection_request:
            peerCid = L2CAP_UINT16(in->Data.Disconnection_Request.SourceCID);
            myCid   = L2CAP_UINT16(in->Data.Disconnection_Request.DistinationCID);
            break;
        case L2CAP_PDU_Code_Disconnection_response:
            peerCid = L2CAP_UINT16(in->Data.Disconnection_Response.DistinationCID);    // !
            myCid   = L2CAP_UINT16(in->Data.Disconnection_Response.SourceCID);         // !
            break;
//      case L2CAP_PDU_Code_Echo_request:
//          break;
//      case L2CAP_PDU_Code_Echo_response:
//          break;
//      case L2CAP_PDU_Code_Information_request:
//          break;
//      case L2CAP_PDU_Code_Information_response:
//          break;
        default:
            break;
        }
        if ((peerCid != 0) || (myCid != 0)) {
            p = bt_l2capChannelFind(handle, peerCid, myCid);
        } else {
            if ((p = bt_l2capChannelFind(handle, 0, L2CAP_CID_L2CAP_Signaling_channel)) == NULL) {
                p = bt_l2capChannelFind(handle, 0, L2CAP_CID_Low_Energy_L2CAP_Signaling_channel);
            }
        }

    //
    // Idle
    //
    } else {
        static struct bt_l2capChannel   *ch = NULL;

        p = bt_l2capChannelGetNext(&ch);        // Rotation
        if (p != NULL) {
#ifdef DEL
            if (p->state == CLOSED) {
                DEBUGP("L2CAP-SG: Close complete. %04x,%04x,%04x\n", (p->link)->handle, p->peerCid, p->myCid);
                bt_l2capChannelDelete(p);       // Table sweeper
                p = NULL;
            } else {
                p->event = NONE;
            }
#endif
            p->event = NONE;
        }
    }

#else                                   // DEL END - ADD START 2017.01.08

    if (in == NULL) {
        while((p = bt_l2capChannelGetNext(&(signaling->signaling.now))) != NULL) {
            if ((p->link            == signaling->link                ) &&
                (p->myCid           >= L2CAP_CID_Dynamically_allocated) &&
                (p->dynamic.creater == signaling                      )) {
                break;
            }
        }
    } else {
        unsigned short  peerCid;
        unsigned short  myCid;

        peerCid = 0;
        myCid   = 0;
        switch(in->Code){
        case L2CAP_PDU_Code_Command_reject:
            break;
        case L2CAP_PDU_Code_Echo_request:
            break;
        case L2CAP_PDU_Code_Echo_response:
            break;
        case L2CAP_PDU_Code_Information_request:
            break;
        case L2CAP_PDU_Code_Information_response:
            break;
        // Peripheral side
        case L2CAP_PDU_Code_Connection_request:
            break;
        case L2CAP_PDU_Code_Configure_request:
            myCid   = L2CAP_UINT16(in->Data.Configuration_Request.DistinationCID);
            break;
        case L2CAP_PDU_Code_Disconnection_request:
            peerCid = L2CAP_UINT16(in->Data.Disconnection_Request.SourceCID);
            myCid   = L2CAP_UINT16(in->Data.Disconnection_Request.DistinationCID);
            break;
        // Central side
        case L2CAP_PDU_Code_Connection_response:
            peerCid = L2CAP_UINT16(in->Data.Connection_Response.DistinationCID);       // !
            myCid   = L2CAP_UINT16(in->Data.Connection_Response.SourceCID);            // !
            break;
        case L2CAP_PDU_Code_Configure_response:
            myCid   = L2CAP_UINT16(in->Data.Configuration_Response.SourceCID);         // !
            break;
        case L2CAP_PDU_Code_Disconnection_response:
            peerCid = L2CAP_UINT16(in->Data.Disconnection_Response.DistinationCID);    // !
            myCid   = L2CAP_UINT16(in->Data.Disconnection_Response.SourceCID);         // !
            break;
        default:
            break;
        }
        if ((peerCid != 0) || (myCid != 0)) {
            p = bt_l2capChannelFindByLink(signaling->link, peerCid, myCid);
        } else {
            p = signaling;
        }
    }
#endif
    
#ifdef DEL                              // DEL START 2017.01.08
    //
    // State machine
    //
    {
        switch((p == NULL) ? CLOSED : p->state){

        case CLOSED:            // or Not have a target channel
            if (in != NULL) {   // Receive
                if (in->Code == L2CAP_PDU_Code_Connection_request) {
                    struct bt_l2capService  *s;
                    unsigned short          peerCid;
                    unsigned short          myCid;
                    unsigned short          psm;
                    unsigned char           r;

                    // Search service table
                    peerCid = L2CAP_UINT16(in->Data.Connection_Request.SourceCID);
                    psm  = L2CAP_UINT16(in->Data.Connection_Request.PSM);

                    // New CID
                    if (p->link->newCid < L2CAP_CID_Dynamically_allocated) {
                        p->link->newCid = L2CAP_CID_Dynamically_allocated;
                    }
                    myCid = p->link->newCid++;
//                  myCid = L2CAP_CID_Dynamically_allocated;
//                  f = NULL;
//                  while((n = bt_l2capChannelGetNext(&f)) != NULL) {
//                      myCid = MAX(myCid, n->myCid);
//                  }
//                  myCid++;

                    // New channel
                    s = bt_l2capServiceFind(psm);
                    p = bt_l2capChannelAdd(handle, peerCid, myCid);
                    if ((p != NULL) && (s != NULL)) {
//                      p->psm         = psm;               // DEL 2017.01.03
//                      p->serviceProg = s->serviceProg;    // DEL 2017.01.03
                        p->service     = s;                 // ADD 2017.01.03
                        p->state       = WAIT_CONFIG;
                        p->try         = 0;
                    } else {
                        DEBUGP("L2CAP-CM: ENOMEM or service not found\n");
                    }

                    // Make response message
                    if (s == NULL) {
                        r = L2CAP_PDU_Result_Connection_refused_PSM_not_supported;
                    } else if (p == NULL) {
                        r = L2CAP_PDU_Result_Connection_refused_no_resources_available;
                    } else {
                        r = L2CAP_PDU_Result_Connection_successful;
                    }

                    // Make response message
                    out->Code       = L2CAP_PDU_Code_Connection_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Connection_Response.SourceCID,      peerCid);
                    L2CAP_STORE16(out->Data.Connection_Response.DistinationCID, myCid);
                    L2CAP_STORE16(out->Data.Connection_Response.Result,         r);
                    L2CAP_STORE16(out->Data.Connection_Response.Status,         L2CAP_PDU_Status_No_further_information_available);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Connection_Response));
                    p->event = SEND_REQUEST;
                    DEBUGP("L2CAP-SG: CLOSED OPEN REQ psm=%d %04x %02x,%02x\n", psm, handle, p->peerCid, p->myCid);
                } else if (in->Code == L2CAP_PDU_Code_Echo_request) {
                    size_t  l;

                    l = L2CAP_UINT16(in->Length);
                    out->Code       = L2CAP_PDU_Code_Echo_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Length, l);
                    memcpy(out->Data.Echo_Request.Data, in->Data.Echo_Response.Data, l);
                    p->event = SEND_REQUEST;
                    DEBUGP("L2CAP-SG: CLOSED ECHO REQ %04x-%02x-%02x\n", handle, p->peerCid, p->myCid);
                } else if (in->Code == L2CAP_PDU_Code_Information_request) {
                    int     t;
//                  long    *l;         // DEL 2016.04.21

                    t = L2CAP_UINT16(in->Data.Information_Request.Info_Type);
                    out->Code       = L2CAP_PDU_Code_Information_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Information_Response.Info_Type, t);
                    L2CAP_STORE16(out->Data.Information_Response.Result, L2CAP_PDU_Result_Success);
                    switch(t){
                    case L2CAP_PDU_Info_Type_Connectionless_MTU:
                        L2CAP_STORE16(out->Data.Information_Response.Data, maxSize);
                        L2CAP_STORE16(out->Length, sizeof(out->Data.Information_Response)+2);
                        break;
                    case L2CAP_PDU_Info_Type_Extended_features_supported:
//                      L2CAP_STORE32(out->Data.Information_Response.Data, L2CAP_PDU_Extended_feature_Fixed_Channels);  // DEL 2016.04.21
                        L2CAP_STORE32(out->Data.Information_Response.Data,                              // ADD 2016.04.21
//                              L2CAP_PDU_Extended_feature_Flow_control_mode                      |     // ADD 2016.04.21
//                              L2CAP_PDU_Extended_feature_Retransmission_mode                    |     // ADD 2016.04.21
//                              L2CAP_PDU_Extended_feature_Bi_directional_QoS1                    |     // ADD 2016.04.21
//                              L2CAP_PDU_Extended_feature_Enhanced_Retransmission_Mode           |     // ADD 2016.04.21
//                              L2CAP_PDU_Extended_feature_Streaming_Mode                         |     // ADD 2016.04.21
//                              L2CAP_PDU_Extended_feature_FCS_Option                             |     // ADD 2016.04.21
//                              L2CAP_PDU_Extended_feature_Extended_Flow_Specification_for_BR_EDR |     // ADD 2016.04.21
                                L2CAP_PDU_Extended_feature_Fixed_Channels                         |     // ADD 2016.04.21
//                              L2CAP_PDU_Extended_feature_Extended_Window_Size                   |     // ADD 2016.04.21
//                              L2CAP_PDU_Extended_feature_Unicast_Connectionless_Data_Reception  |     // ADD 2016.04.21
                                0UL);                                                                   // ADD 2016.04.21
                        L2CAP_STORE16(out->Length, sizeof(out->Data.Information_Response)+4);
                        break;
                    case L2CAP_PDU_Info_Type_Fixed_Channels_Supported:
//                      L2CAP_STORE32(out->Data.Information_Response.Data, L2CAP_PDU_Fixed_channel_support_L2CAP_Signaling_channel);    // DEL 2016.04.21
//                      l = (long *)(&(out->Data.Information_Response.Data[4]));                // DEL 2016.04.21
//                      *l = 0;                                                                 // DEL 2016.04.21
                        L2CAP_STORE64(out->Data.Information_Response.Data,                      // ADD 2016.04.21
//                              L2CAP_PDU_Fixed_channel_support_Null_identifier              |  // ADD 2016.04.21
                                L2CAP_PDU_Fixed_channel_support_L2CAP_Signaling_channel      |  // ADD 2016.04.21
//                              L2CAP_PDU_Fixed_channel_support_Connectionless_reception     |  // ADD 2016.04.21
//                              L2CAP_PDU_Fixed_channel_support_AMP_Manager_Protocol_Channel |  // ADD 2016.04.21
//                              L2CAP_PDU_Fixed_channel_support_BR_EDR_Security_Manager      |  // ADD 2016.04.21
//                              L2CAP_PDU_Fixed_channel_support_AMP_Test_Manager             |  // ADD 2016.04.21
                                0ULL);                                                          // ADD 2016.04.21
                        L2CAP_STORE16(out->Length, sizeof(out->Data.Information_Response)+8);
                        break;
                    default:
                        break;
                    }
                    p->event = SEND_REQUEST;
                    DEBUGP("L2CAP-SG: CLOSED INFO REQ %04x-%02x-%02x\n", handle, p->peerCid, p->myCid);
                }
            }
            break;

        case WAIT_CONNECT_RSP:
            break;

        case WAIT_CONNECT:
            break;

        case WAIT_CONFIG:
            if (in != NULL) {       // Recieve
                // Configure event
                if (in->Code == L2CAP_PDU_Code_Configure_request) {
                    unsigned short      r;
                    int                 l;
                    int                 rc;

                    // Option parameter check
                    l = L2CAP_UINT16(in->Length) - sizeof(in->Data.Configuration_Request);
                    rc = bt_l2capConfigurationParameterParse(in->Data.Configuration_Request.Configuration_Options, l);
                    if (rc == 0) {
                        r = L2CAP_PDU_Result_Success;
                    } else {
                        r = L2CAP_PDU_Result_Failure_unknown_options;
                    }

                    // Get option parameter
                    l = bt_l2capConfigurationParameterRequest(out->Data.Configuration_Response.Config, maxSize);

                    // Create message
                    out->Code       = L2CAP_PDU_Code_Configure_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Configuration_Response.SourceCID, p->peerCid);
                    L2CAP_STORE16(out->Data.Configuration_Response.Flags, 0);
                    L2CAP_STORE16(out->Data.Configuration_Response.Result, r);
                    L2CAP_STORE16(out->Length, l + sizeof(out->Data.Configuration_Response));
                    p->event = SEND_REQUEST;
                    if (r == L2CAP_PDU_Result_Success) {
                        p->state = WAIT_SEND_CONFIG;
                    }
                    DEBUGP("L2CAP-SG: WAIT_CONFIG CONF REQ %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);

                // Disconnect event
                } else if (in->Code == L2CAP_PDU_Code_Disconnection_request) {

                    // Create message
                    out->Code       = L2CAP_PDU_Code_Disconnection_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Disconnection_Response.SourceCID,      p->peerCid);
                    L2CAP_STORE16(out->Data.Disconnection_Response.DistinationCID, p->myCid);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Disconnection_Response));
                    p->event = SEND_REQUEST;
                    p->state = CLOSED;
                    DEBUGP("L2CAP-SG: WAIT_CONFIG DISCONN %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
                }
            } else {
                if (p->try++ > BT_CONFIG_TIMEOUT) {
                    p->event = NONE;
                    p->state = CLOSED;
                    DEBUGP("L2CAP-SG: WAIT_CONFIG TIMEOUT %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
                }
            }
            break;

        case WAIT_CONFIG_REQ_RSP:
            break;

        case WAIT_CONFIG_REQ:
            break;

        case WAIT_SEND_CONFIG:
            if (in == NULL) {
                size_t  l;

                // Get option parameter
                l = bt_l2capConfigurationParameterRequest(out->Data.Configuration_Request.Configuration_Options, maxSize);

                // Create message
                (p->id)++;
                out->Code       = L2CAP_PDU_Code_Configure_request;
                out->Identifier = p->id;
                L2CAP_STORE16(out->Data.Configuration_Request.DistinationCID, p->peerCid); // !
                L2CAP_STORE16(out->Data.Configuration_Request.Flags,          0);
                L2CAP_STORE16(out->Length, l + sizeof(out->Data.Configuration_Request));
                p->event = SEND_REQUEST;
                p->state = WAIT_CONFIG_RSP;
                p->try   = 0;
                DEBUGP("L2CAP-SG: WAIT_SEND_CONFIG SEND %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
            } else {
                // Disconnect event
                if (in->Code == L2CAP_PDU_Code_Disconnection_request) {
                    out->Code       = L2CAP_PDU_Code_Disconnection_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Disconnection_Response.SourceCID,      p->peerCid);
                    L2CAP_STORE16(out->Data.Disconnection_Response.DistinationCID, p->myCid);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Disconnection_Response));
                    p->event = SEND_REQUEST;
                    p->state = CLOSED;
                    DEBUGP("L2CAP-SG: WAIT_SEND_CONFIG DISCONN REQ %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
                } else {
                    out->Code       = L2CAP_PDU_Code_Command_reject;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Command_Reject.Reason, L2CAP_PDU_Reason_Command_not_understood);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Command_Reject));
                    p->event = SEND_REQUEST;
                    DEBUGP("L2CAP-SG: WAIT_SEND_CONFIG REJECT %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
                }
            }
            break;

        case WAIT_CONFIG_RSP:
            if (in != NULL) {       // Receive
                // Configure event
                if (in->Code == L2CAP_PDU_Code_Configure_response) {
                    int     l;
                    int     rc;

                    // Option parameter check
                    if (in->Identifier == p->id) {
                        l = L2CAP_UINT16(in->Length) - sizeof(in->Data.Configuration_Response);
                        rc = bt_l2capConfigurationParameterParse(in->Data.Configuration_Response.Config, l);
                        if (rc == 0) {
                            p->event = NONE;
                            p->state = OPEN;
                            DEBUGP("L2CAP-SG: Open complete. %04x-%02x-%02x\n", p->link->handle,  p->peerCid, p->myCid);
                        }
                    }

                // Disconnect event
                } else if (in->Code == L2CAP_PDU_Code_Disconnection_request) {
                    out->Code       = L2CAP_PDU_Code_Disconnection_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Disconnection_Response.SourceCID,      p->peerCid);
                    L2CAP_STORE16(out->Data.Disconnection_Response.DistinationCID, p->myCid);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Disconnection_Response));
                    p->event = SEND_REQUEST;
                    p->state = CLOSED;
                    DEBUGP("L2CAP-SG: WAIT_CONFIG_RSP DISCONN %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
                }
            } else {
                if (p->try++ > BT_CONFIG_TIMEOUT) {
                    p->event = NONE;
                    p->state = CLOSED;
                    DEBUGP("L2CAP-SG: WAIT_CONFIG_RSP TIMEOUT %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
                }
            }
            break;

        case OPEN:
            if (in != NULL) {       // Recieve
                if (in->Code == L2CAP_PDU_Code_Disconnection_request) {
                    out->Code       = L2CAP_PDU_Code_Disconnection_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Disconnection_Response.SourceCID,      p->peerCid);
                    L2CAP_STORE16(out->Data.Disconnection_Response.DistinationCID, p->myCid);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Disconnection_Response));
                    p->event = SEND_REQUEST;
                    p->state = CLOSED;
                    DEBUGP("L2CAP-SG: OPEN DISCONN REQ %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
                } else {
                    out->Code       = L2CAP_PDU_Code_Command_reject;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Command_Reject.Reason, L2CAP_PDU_Reason_Command_not_understood);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Command_Reject));
                    p->event = SEND_REQUEST;
                    DEBUGP("L2CAP-SG: OPEN REJECT %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
                }
            }
            break;

        case WAIT_DISCONNECT:
            if (in != NULL) {       // Recieve
                if (in->Code == L2CAP_PDU_Code_Disconnection_response) {
                    if (in->Identifier == p->id) {
                        p->event = NONE;
                        p->state = CLOSED;
                        DEBUGP("L2CAP-SG: WAIT_DISCONNECT OK %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
                    }
                } else if (in->Code == L2CAP_PDU_Code_Disconnection_request) {
                    out->Code       = L2CAP_PDU_Code_Disconnection_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Disconnection_Response.SourceCID,      p->peerCid);
                    L2CAP_STORE16(out->Data.Disconnection_Response.DistinationCID, p->myCid);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Disconnection_Response));
                    p->event = SEND_REQUEST;
                    p->state = CLOSED;
                    DEBUGP("L2CAP-SG: WAIT_DISCONNECT OK %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
                }
            }
            if (p->try++ > BT_DISCONNECT_TIMEOUT) {
                p->state = CLOSED;
                DEBUGP("L2CAP-SG: WAIT_DISCONNECT TIMEOUT %04x-%02x-%02x\n", p->link->handle, p->peerCid, p->myCid);
            }
            break;

        default:
            break;
        }
    }
#else                                   // DEL END - ADD START 2017.01.08
            
    //
    // 6. State machine
    //
    L2CAP_STORE16(out->Length, 0);
    if (p != NULL) {
        if (in != NULL) {
            signaling->signaling.lastId = in->Identifier;
        }

        //
        // State less messages
        //
        if ((in != NULL) && (in->Code == L2CAP_PDU_Code_Echo_request)) {
            size_t  l;

            l = L2CAP_UINT16(in->Length);
            out->Code       = L2CAP_PDU_Code_Echo_response;
            out->Identifier = in->Identifier;
            L2CAP_STORE16(out->Length, l);
            memcpy(out->Data.Echo_Request.Data, in->Data.Echo_Response.Data, l);
            DEBUGP("L2CAP-SG:%x: Echo_request\n", p->myCid);

        } else if ((in != NULL) && (in->Code == L2CAP_PDU_Code_Information_request)) {
            int     t;

            t = L2CAP_UINT16(in->Data.Information_Request.Info_Type);
            out->Code       = L2CAP_PDU_Code_Information_response;
            out->Identifier = in->Identifier;
            L2CAP_STORE16(out->Data.Information_Response.Info_Type, t);
            L2CAP_STORE16(out->Data.Information_Response.Result, L2CAP_PDU_Result_Success);
            switch(t){
            case L2CAP_PDU_Info_Type_Connectionless_MTU:
                L2CAP_STORE16(out->Data.Information_Response.Data, p->link->buffSize);
                L2CAP_STORE16(out->Length, sizeof(out->Data.Information_Response)+2);
                DEBUGP("L2CAP-SG:%x: Information_request Connectionless_MTU=%d\n", p->myCid, p->link->buffSize);
                break;
            case L2CAP_PDU_Info_Type_Extended_features_supported:
                L2CAP_STORE32(out->Data.Information_Response.Data,
//                      L2CAP_PDU_Extended_feature_Flow_control_mode                      |
//                      L2CAP_PDU_Extended_feature_Retransmission_mode                    |
//                      L2CAP_PDU_Extended_feature_Bi_directional_QoS1                    |
//                      L2CAP_PDU_Extended_feature_Enhanced_Retransmission_Mode           |
//                      L2CAP_PDU_Extended_feature_Streaming_Mode                         |
//                      L2CAP_PDU_Extended_feature_FCS_Option                             |
                        L2CAP_PDU_Extended_feature_Extended_Flow_Specification_for_BR_EDR |
                        L2CAP_PDU_Extended_feature_Fixed_Channels                         |
//                      L2CAP_PDU_Extended_feature_Extended_Window_Size                   |
//                      L2CAP_PDU_Extended_feature_Unicast_Connectionless_Data_Reception  |
                        0UL);
                L2CAP_STORE16(out->Length, sizeof(out->Data.Information_Response)+4);
                DEBUGP("L2CAP-SG:%x: Information_request Extended_features_supported\n", p->myCid);
                break;
            case L2CAP_PDU_Info_Type_Fixed_Channels_Supported:
                L2CAP_STORE64(out->Data.Information_Response.Data,
//                      L2CAP_PDU_Fixed_channel_support_Null_identifier              |
                        L2CAP_PDU_Fixed_channel_support_L2CAP_Signaling_channel      |
//                      L2CAP_PDU_Fixed_channel_support_Connectionless_reception     |
//                      L2CAP_PDU_Fixed_channel_support_AMP_Manager_Protocol_Channel |
//                      L2CAP_PDU_Fixed_channel_support_BR_EDR_Security_Manager      |
//                      L2CAP_PDU_Fixed_channel_support_AMP_Test_Manager             |
                        0ULL);
                L2CAP_STORE16(out->Length, sizeof(out->Data.Information_Response)+8);
                DEBUGP("L2CAP-SG:%x: Information_request Fixed_Channels_Supported\n", p->myCid);
                break;
            default:
                L2CAP_STORE16(out->Data.Information_Response.Result, L2CAP_PDU_Result_Failure_unacceptable_parameters);
                L2CAP_STORE16(out->Length, sizeof(out->Data.Information_Response));
                DEBUGP("L2CAP-SG:%x: Information_request Type error %d\n", p->myCid, t);
                break;
            }

        //
        // 6.1.1 CLOSED state
        //
        } else if ((p == signaling) || (p->dynamic.state == CLOSED)) {
            if (in == NULL) {           // No message
//              if (OpenChannel_req) {          // Central function -> not implement
//                  Send(L2CAP_ConnectReq)      // Central function -> not implement
//                  state = WAIT_CONNECT_RSP    // Central function -> not implement
//              }                               // Central function -> not implement
            } else {                    // Receive
                switch(in->Code){
                case L2CAP_PDU_Code_Connection_request:
                {
                    struct bt_l2capService  *s;
                    struct bt_l2capChannel  *n;
                    unsigned short          peerCid;
                    unsigned short          myCid;
                    unsigned short          psm;
                    unsigned int            r;
                    unsigned int            t;

                    // Search service table
                    peerCid = L2CAP_UINT16(in->Data.Connection_Request.SourceCID);
                    psm     = L2CAP_UINT16(in->Data.Connection_Request.PSM);
                    s = bt_l2capServiceFind(psm);

                    // New CID#
                    if (p->link->newCid < L2CAP_CID_Dynamically_allocated) {
                        p->link->newCid = L2CAP_CID_Dynamically_allocated + 1;
                    }
                    myCid = p->link->newCid++;

                    // New channel
                    n = bt_l2capChannelAddByLink(p->link, peerCid, myCid);

                    // Make response message
                    if (n == NULL) {
                        r = L2CAP_PDU_Result_Connection_refused_no_resources_available;
                        t = L2CAP_PDU_Status_No_further_information_available;
                    } else if (s == NULL) {
                        r = L2CAP_PDU_Result_Connection_refused_PSM_not_supported;
                        t = L2CAP_PDU_Status_No_further_information_available;
                        bt_l2capChannelDelete(n);
                    } else {
                        r = L2CAP_PDU_Result_Connection_successful;
                        t = L2CAP_PDU_Status_No_further_information_available;
                        n->service              = s;
                        n->dynamic.creater      = signaling;
                        n->dynamic.connectionId = in->Identifier;
//                      n->dynamic.configured   = 1;    // Configure waiting mode
                        n->dynamic.timeOut      = time(NULL) + BT_RTX_TIMEOUT;
                        n->dynamic.state        = CONFIG;
                        n->dynamic.configState  = WAIT_CONFIG;

                        if (((s->permissions & BT_L2CAP_SERVICE_PERMISSIONS_Encryption_Required    ) && !(p->link->encryption    )) ||
                            ((s->permissions & BT_L2CAP_SERVICE_PERMISSIONS_Authentication_Required) && !(p->link->authentication))) {
                            r = L2CAP_PDU_Result_Connection_pending;
                            t = L2CAP_PDU_Status_Authentication_pending;
                            n->dynamic.timeOut  = time(NULL) + BT_ERTX_TIMEOUT;
                            n->dynamic.state    = WAIT_CONNECT;
                        }
                    }
                    out->Code       = L2CAP_PDU_Code_Connection_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Connection_Response.SourceCID,      peerCid);
                    L2CAP_STORE16(out->Data.Connection_Response.DistinationCID, myCid);
                    L2CAP_STORE16(out->Data.Connection_Response.Result,         r);
                    L2CAP_STORE16(out->Data.Connection_Response.Status,         t);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Connection_Response));
                    DEBUGP("L2CAP-SG:%x CLOSED: Connection_request(cid=0x%x,psm=%d) [New channel]-> Connection_Response(result=%d,stat=%d) newCid=0x%x\n", p->myCid, peerCid, psm, r, t, myCid);
                    break;
                }
                case L2CAP_PDU_Code_Connection_response:
                    DEBUGP("L2CAP-SG:%x CLOSED: Connection_response -> N/A\n", p->myCid);
                    break;
                case L2CAP_PDU_Code_Configure_request:
                    out->Code       = L2CAP_PDU_Code_Command_reject;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Command_Reject.Reason, L2CAP_PDU_Reason_invalid_CID_in_request);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Command_Reject));
                    DEBUGP("L2CAP-SG:%x CLOSED: Configure_request -> Command_Reject\n", p->myCid);
                    break;
                case L2CAP_PDU_Code_Configure_response:
                    DEBUGP("L2CAP-SG:%x CLOSED: Configure_response -> N/A\n", p->myCid);
                    break;
                case L2CAP_PDU_Code_Disconnection_request:
                    out->Code       = L2CAP_PDU_Code_Disconnection_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Disconnection_Response.SourceCID,      L2CAP_UINT16(in->Data.Disconnection_Request.SourceCID));
                    L2CAP_STORE16(out->Data.Disconnection_Response.DistinationCID, L2CAP_UINT16(in->Data.Disconnection_Request.DistinationCID));
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Disconnection_Response));
                    DEBUGP("L2CAP-SG:%x CLOSED: Disconnection_request -> Disconnection_Response\n", p->myCid);
                    break;
                case L2CAP_PDU_Code_Disconnection_response:
                    break;
                default:
                    break;
                }
            }

        //
        // 6.1.2 WAIT_CONNECT_RSP state
        //
        } else if (p->dynamic.state == WAIT_CONNECT_RSP) {
            if (in == NULL) {           // No message
                if (p->dynamic.timeOut < time(NULL)) {
                    p->dynamic.state = CLOSED;
                    DEBUGP("L2CAP-SG:%x WAIT_CONNECT_RSP: Timeout\n", p->myCid);
                }
            } else {                    // Receive
                switch(in->Code){
                case L2CAP_PDU_Code_Connection_response:
                    if (in->Identifier == p->dynamic.connectionId) {    // Message# is match
                        switch(L2CAP_UINT16(in->Data.Connection_Response.Result)) {
                        case L2CAP_PDU_Result_Connection_successful:
                            p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
                            p->dynamic.state       = CONFIG;
                            p->dynamic.configState = WAIT_CONFIG;
                            break;
                        case L2CAP_PDU_Result_Connection_pending:
                            break;
                        default:
                            p->dynamic.state = CLOSED;
                            break;
                        }
                    }
                    DEBUGP("L2CAP-SG:%x WAIT_CONNECT_RSP: Connection_response result=%d\n", p->myCid, L2CAP_UINT16(in->Data.Connection_Response.Result));
                    break;
                case L2CAP_PDU_Code_Configure_request:
                    out->Code       = L2CAP_PDU_Code_Command_reject;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Command_Reject.Reason, L2CAP_PDU_Reason_invalid_CID_in_request);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Command_Reject));
                    DEBUGP("L2CAP-SG:%x WAIT_CONNECT_RSP: Configure_request -> Command_Reject\n", p->myCid);
                    break;
                default:
                    break;
                }
            }

        //
        // 6.1.3 WAIT_CONNECT state
        //
        } else if (p->dynamic.state == WAIT_CONNECT) {
            if (in == NULL) {
                if (((p->service->permissions & BT_L2CAP_SERVICE_PERMISSIONS_Encryption_Required    ) && p->link->encryption    ) ||
                    ((p->service->permissions & BT_L2CAP_SERVICE_PERMISSIONS_Authentication_Required) && p->link->authentication)) {
                    out->Code       = L2CAP_PDU_Code_Connection_response;
                    out->Identifier = p->dynamic.connectionId;
                    L2CAP_STORE16(out->Data.Connection_Response.SourceCID,      p->peerCid);
                    L2CAP_STORE16(out->Data.Connection_Response.DistinationCID, p->myCid);
                    L2CAP_STORE16(out->Data.Connection_Response.Result,         L2CAP_PDU_Result_Connection_successful);
                    L2CAP_STORE16(out->Data.Connection_Response.Status,         L2CAP_PDU_Status_No_further_information_available);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Connection_Response));
                    p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
                    p->dynamic.state       = CONFIG;
                    p->dynamic.configState = WAIT_CONFIG;
                    DEBUGP("L2CAP-SG:%x WAIT_CONNECT: Permission release -> Connection_Response(0,0) WAIT_CONFIG\n", p->myCid);
                } else if (p->dynamic.timeOut < time(NULL)) {
                    out->Code       = L2CAP_PDU_Code_Connection_response;
                    out->Identifier = p->dynamic.connectionId;
                    L2CAP_STORE16(out->Data.Connection_Response.SourceCID,      p->peerCid);
                    L2CAP_STORE16(out->Data.Connection_Response.DistinationCID, p->myCid);
                    L2CAP_STORE16(out->Data.Connection_Response.Result,         L2CAP_PDU_Result_Connection_refused_security_block);
                    L2CAP_STORE16(out->Data.Connection_Response.Status,         L2CAP_PDU_Status_No_further_information_available);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Connection_Response));
                    p->dynamic.state = CLOSED;
                    DEBUGP("L2CAP-SG:%x WAIT_CONNECT: Timeout -> Connection_Response CLOSED\n", p->myCid);
                }
            } else {
                switch(in->Code) {
                case L2CAP_PDU_Code_Connection_response:
                    DEBUGP("L2CAP-SG:%x WAIT_CONNECT: Connection_response -> N/A\n", p->myCid);
                    break;
                case L2CAP_PDU_Code_Configure_response:
                    DEBUGP("L2CAP-SG:%x WAIT_CONNECT: Configure_response -> N/A\n", p->myCid);
                    break;
                case L2CAP_PDU_Code_Disconnection_response:
                    DEBUGP("L2CAP-SG:%x WAIT_CONNECT: Disconnection_response -> N/A\n", p->myCid);
                    break;
                default:
                    DEBUGP("L2CAP-SG:%x WAIT_CONNECT: ERROR%d -> N/A\n", p->myCid, in->Code);
                    break;
                }
            }

        //
        // 6.1.4 CONFIG state
        //
        } else if (p->dynamic.state == CONFIG) {
            
            //  General rule
            if (p->dynamic.timeOut < time(NULL)) {
                p->dynamic.state = CLOSED;
                DEBUGP("L2CAP-SG:%x CONFIG: Timeout\n", p->myCid);
            }
            if (in == NULL) {
                if (p->dynamic.event == DISCONNECT_REQUEST) {
                    p->dynamic.event = NONE;
                    p->dynamic.disconnectionId = signaling->signaling.lastId + 1;
                    out->Code       = L2CAP_PDU_Code_Disconnection_request;
                    out->Identifier = p->dynamic.disconnectionId;
                    L2CAP_STORE16(out->Data.Disconnection_Request.SourceCID,      p->myCid);
                    L2CAP_STORE16(out->Data.Disconnection_Request.DistinationCID, p->peerCid);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Disconnection_Request));
                    p->dynamic.timeOut = time(NULL) + BT_RTX_TIMEOUT;
                    p->dynamic.state   = WAIT_DISCONNECT;
                    DEBUGP("L2CAP-SG:%x CONFIG: DISCONNECT_REQUEST -> Disconnection_Request\n", p->myCid);
                }
            } else if (in->Code == L2CAP_PDU_Code_Disconnection_request) {
                out->Code       = L2CAP_PDU_Code_Disconnection_response;
                out->Identifier = in->Identifier;
                L2CAP_STORE16(out->Data.Disconnection_Response.SourceCID,      L2CAP_UINT16(in->Data.Disconnection_Request.SourceCID));
                L2CAP_STORE16(out->Data.Disconnection_Response.DistinationCID, L2CAP_UINT16(in->Data.Disconnection_Request.DistinationCID));
                L2CAP_STORE16(out->Length, sizeof(out->Data.Disconnection_Response));
                p->dynamic.state = CLOSED;
                DEBUGP("L2CAP-SG:%x CONFIG: Disconnection_request -> Disconnection_Response\n", p->myCid);
            } else if (in->Code == L2CAP_PDU_Code_Disconnection_response) {
                DEBUGP("L2CAP-SG:%x CONFIG: Disconnection_response -> N/A\n", p->myCid);
            }

            //                           =============
            if (p->dynamic.configState == WAIT_CONFIG) {
            //                           =============
                if (in == NULL) {
                    if (p->dynamic.timeOut < time(NULL)) {
                        p->dynamic.state = CLOSED;
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG: Timeout\n", p->myCid);
                    } else if (p->dynamic.configured == 0) {    // ConfigureChannel_Req
                        size_t  l;

                        p->dynamic.configured      = 1;
                        p->dynamic.configurationId = signaling->signaling.lastId + 1;
                        l = bt_l2capConfigurationParameterRequest(out->Data.Configuration_Request.Configuration_Options, p);
                        out->Code          = L2CAP_PDU_Code_Configure_request;
                        out->Identifier    = p->dynamic.configurationId;
                        L2CAP_STORE16(out->Data.Configuration_Request.DistinationCID, p->peerCid);
                        L2CAP_STORE16(out->Data.Configuration_Request.Flags,          0);
                        L2CAP_STORE16(out->Length, l + sizeof(out->Data.Configuration_Request));
                        p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
                        p->dynamic.configState = WAIT_CONFIG_REQ_RSP;
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG: --> Configuration_Request\n", p->myCid);
                    }
                } else {
                    switch(in->Code){
                    case L2CAP_PDU_Code_Configure_request:
                    {
                        unsigned short  r;
                        size_t          l;
                        size_t          m;

                        l = L2CAP_UINT16(in->Length) - sizeof(in->Data.Configuration_Request);
                        r = bt_l2capConfigurationParameterParse(in->Data.Configuration_Request.Configuration_Options, l,
                                                                out->Data.Configuration_Response.Config,              &m,
                                                                p);
                        out->Code       = L2CAP_PDU_Code_Configure_response;
                        out->Identifier = in->Identifier;
                        L2CAP_STORE16(out->Data.Configuration_Response.SourceCID, p->peerCid);
                        L2CAP_STORE16(out->Data.Configuration_Response.Flags,     0);
                        L2CAP_STORE16(out->Data.Configuration_Response.Result,    r);
                        L2CAP_STORE16(out->Length, m + sizeof(out->Data.Configuration_Response));
                        if (r == L2CAP_PDU_Result_Configuration_Success) {
                            p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
                            p->dynamic.configState = WAIT_SEND_CONFIG;
                        }
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG: Configure_request -> Configuration_Response(result=%d)\n", p->myCid, r);
                        break;
                    }
                    case L2CAP_PDU_Code_Configure_response:
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG: Configure_response -> N/A\n", p->myCid);
                        break;
                    default:
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG: ERROR %d\n", p->myCid, in->Code);
                        break;
                    }
                }

            //                                  =====================
            } else if (p->dynamic.configState == WAIT_CONFIG_REQ_RSP) {
            //                                  =====================
                if (in == NULL) {
                    if (p->dynamic.timeOut < time(NULL)) {
                        p->dynamic.configState = WAIT_CONFIG;
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG_REQ_RSP: Timeout\n", p->myCid);
                    }
                } else {
                    switch(in->Code) {
                    case L2CAP_PDU_Code_Configure_request:
                    {
                        unsigned short  r;
                        size_t          l;
                        size_t          m;

                        l = L2CAP_UINT16(in->Length) - sizeof(in->Data.Configuration_Request);
                        r = bt_l2capConfigurationParameterParse(in->Data.Configuration_Request.Configuration_Options, l,
                                                                out->Data.Configuration_Response.Config,              &m,
                                                                p);
                        out->Code       = L2CAP_PDU_Code_Configure_response;
                        out->Identifier = in->Identifier;
                        L2CAP_STORE16(out->Data.Configuration_Response.SourceCID, p->peerCid);
                        L2CAP_STORE16(out->Data.Configuration_Response.Flags,     0);
                        L2CAP_STORE16(out->Data.Configuration_Response.Result,    r);
                        L2CAP_STORE16(out->Length, m + sizeof(out->Data.Configuration_Response));
                        if (r == L2CAP_PDU_Result_Configuration_Success) {
                            p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
                            p->dynamic.configState = WAIT_CONFIG_RSP;
                        }
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG_REQ_RSP: Configure_request -> Configuration_Response\n", p->myCid);
                        break;
                    }
                    case L2CAP_PDU_Code_Configure_response:
                        if (L2CAP_UINT16(in->Data.Configuration_Response.Result) == L2CAP_PDU_Result_Configuration_Success) {
                            size_t  l;
                            size_t  m;

                            l = L2CAP_UINT16(in->Length) - sizeof(in->Data.Configuration_Response);
                            bt_l2capConfigurationParameterParse(in->Data.Configuration_Response.Config,  l,     // Update the control block
                                                                out->Data.Configuration_Response.Config, &m,    // Dummy
                                                                p);
                            p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
                            p->dynamic.configState = WAIT_CONFIG_REQ;
                            DEBUGP("L2CAP-SG:%x WAIT_CONFIG_REQ_RSP: Configure_response -> new state WAIT_CONFIG_REQ\n", p->myCid);
                        } else {
                            size_t  l;
                            
                            p->dynamic.configurationId = signaling->signaling.lastId + 1;
                            l = bt_l2capConfigurationParameterRequest(out->Data.Configuration_Request.Configuration_Options, p);
                            out->Code       = L2CAP_PDU_Code_Configure_request;
                            out->Identifier = p->dynamic.configurationId;
                            L2CAP_STORE16(out->Data.Configuration_Request.DistinationCID, p->peerCid);
                            L2CAP_STORE16(out->Data.Configuration_Request.Flags,          0);
                            L2CAP_STORE16(out->Length, l + sizeof(out->Data.Configuration_Request));
                            DEBUGP("L2CAP-SG:%x WAIT_CONFIG_REQ_RSP: Configure_response -> Configuration_Request\n", p->myCid);
                        }
                        break;
                    default:
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG_REQ_RSP: ERROR %d\n", p->myCid, in->Code);
                        break;
                    }
                }
                
            //                                  =================
            } else if (p->dynamic.configState == WAIT_CONFIG_REQ) {
            //                                  =================
                if (in == NULL) {
                    if (p->dynamic.timeOut < time(NULL)) {
                        p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
                        p->dynamic.configState = WAIT_CONFIG;
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG_REQ: Timeout\n", p->myCid);
                    }
                } else {
                    switch(in->Code) {
                    case L2CAP_PDU_Code_Configure_request:
                    {
                        unsigned short  r;
                        size_t          l;
                        size_t          m;

                        l = L2CAP_UINT16(in->Length) - sizeof(in->Data.Configuration_Request);
                        r = bt_l2capConfigurationParameterParse(in->Data.Configuration_Request.Configuration_Options, l,
                                                                out->Data.Configuration_Response.Config,              &m,
                                                                p);
                        out->Code       = L2CAP_PDU_Code_Configure_response;
                        out->Identifier = in->Identifier;
                        L2CAP_STORE16(out->Data.Configuration_Response.SourceCID, p->peerCid);
                        L2CAP_STORE16(out->Data.Configuration_Response.Flags,     0);
                        L2CAP_STORE16(out->Data.Configuration_Response.Result,    r);
                        L2CAP_STORE16(out->Length, m + sizeof(out->Data.Configuration_Response));
                        if (r == L2CAP_PDU_Result_Configuration_Success) {
                    		p->service->serviceProg(p, NULL, BT_L2CAP_SERVICE_SERVICEPROG_inLen_CONNECT, NULL, 0);	// ADD 2018.11.06
                            p->dynamic.state = OPEN;
                        } else if (r == L2CAP_PDU_Result_Configuration_Pending) {
                            p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
//                          p->dynamic.configState = WAIT_IND_FINAL_RSP;    // Extend flow function
                            p->dynamic.configState = WAIT_CONFIG;           // Not implement
                        }
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG_REQ: Configure_request -> Configuration_Response %d\n", p->myCid, r);
                        break;
                    }
                    default:
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG_REQ: ERROR %d\n", p->myCid, in->Code);
                        break;
                    }
                }

            //                                  =================
            } else if (p->dynamic.configState == WAIT_CONFIG_RSP) {
            //                                  =================
                if (in == NULL) {
                    if (p->dynamic.timeOut < time(NULL)) {
                        p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
                        p->dynamic.configState = WAIT_CONFIG;
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG_RSP: Timeout\n", p->myCid);
                    }
                } else {
                    switch(in->Code) {
                    case L2CAP_PDU_Code_Configure_response:
                        if (L2CAP_UINT16(in->Data.Configuration_Response.Result) == L2CAP_PDU_Result_Configuration_Success) {
                            size_t  l;
                            size_t  m;

                            l = L2CAP_UINT16(in->Length) - sizeof(in->Data.Configuration_Response);
                            bt_l2capConfigurationParameterParse(in->Data.Configuration_Response.Config,  l,     // Update the control block
                                                                out->Data.Configuration_Response.Config, &m,    // Dummy
                                                                p);
                    		p->service->serviceProg(p, NULL, BT_L2CAP_SERVICE_SERVICEPROG_inLen_CONNECT, NULL, 0);	// ADD 2018.11.06
                            p->dynamic.state = OPEN;
                            DEBUGP("L2CAP-SG:%x WAIT_CONFIG_RSP: Configure_response(result=%d) -> OPEN\n", p->myCid, L2CAP_UINT16(in->Data.Configuration_Response.Result));
                        } else {
                            p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
//                          p->dynamic.configState = WAIT_IND_FINAL_RSP;    // Extend flow function
                            p->dynamic.configState = WAIT_CONFIG;           // Not implement
                            DEBUGP("L2CAP-SG:%x WAIT_CONFIG_RSP: Configure_response(result=%d) -> WAIT_CONFIG\n", p->myCid, L2CAP_UINT16(in->Data.Configuration_Response.Result));
                        }
                        break;
                    default:
                        DEBUGP("L2CAP-SG:%x WAIT_CONFIG_RSP: ERROR %d -> N/A\n", p->myCid, in->Code);
                        break;
                    }
                }
            //                                  ==================
            } else if (p->dynamic.configState == WAIT_SEND_CONFIG) {
            //                                  ==================
                if (in == NULL) {   // Never rule
                    size_t  l;

                    p->dynamic.configurationId = signaling->signaling.lastId + 1;
                    l = bt_l2capConfigurationParameterRequest(out->Data.Configuration_Request.Configuration_Options, p);
                    out->Code       = L2CAP_PDU_Code_Configure_request;
                    out->Identifier = p->dynamic.configurationId;
                    L2CAP_STORE16(out->Data.Configuration_Request.DistinationCID, p->peerCid); // !
                    L2CAP_STORE16(out->Data.Configuration_Request.Flags,          0);
                    L2CAP_STORE16(out->Length, l + sizeof(out->Data.Configuration_Request));
                    p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
                    p->dynamic.configState = WAIT_CONFIG_RSP;
                    DEBUGP("L2CAP-SG:%x WAIT_SEND_CONFIG: --> Configuration_Request\n", p->myCid);
                }

//          } else if (p->dynamic.configState == WAIT_IND_FINAL_RSP) {
//                                      // Not implement (Extend flow function)
//          } else if (p->dynamic.configState == WAIT_CONTROL_IND) {
//                                      // Not implement (Extend flow function)
//          } else if (p->dynamic.configState == WAIT_FINAL_RSP) {
//                                      // Not implement (Extend flow function)
            }

        //
        // 6.1.5 OPEN state
        //
        } else if (p->dynamic.state == OPEN) {
            if (in == NULL) {   // No message
                switch(p->dynamic.event) {
                case DISCONNECT_REQUEST:
                    p->dynamic.event        = NONE;
                    p->dynamic.connectionId = signaling->signaling.lastId + 1;
                    out->Code       = L2CAP_PDU_Code_Disconnection_request;
                    out->Identifier = p->dynamic.connectionId;
                    L2CAP_STORE16(out->Data.Disconnection_Request.SourceCID,      p->myCid); // !
                    L2CAP_STORE16(out->Data.Disconnection_Request.DistinationCID, p->peerCid); // !
                    p->dynamic.timeOut = time(NULL) + BT_RTX_TIMEOUT;
                    p->dynamic.state   = WAIT_DISCONNECT;
                    DEBUGP("L2CAP-SG:%x OPEN: DISCONNECT_REQUEST -> Disconnection_Request\n", p->myCid);
                    break;
                case RECONFIGURE_REQUEST:
                {
                    size_t  l;

                    p->dynamic.event           = NONE;
                    p->dynamic.configurationId = signaling->signaling.lastId + 1;
                    l = bt_l2capConfigurationParameterRequest(out->Data.Configuration_Request.Configuration_Options, p);
                    out->Code          = L2CAP_PDU_Code_Configure_request;
                    out->Identifier    = p->dynamic.configurationId;
                    L2CAP_STORE16(out->Data.Configuration_Request.DistinationCID, p->peerCid); // !
                    L2CAP_STORE16(out->Data.Configuration_Request.Flags,          0);
                    L2CAP_STORE16(out->Length, l + sizeof(out->Data.Configuration_Request));
                    p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
                    p->dynamic.state       = CONFIG;
                    p->dynamic.configState = WAIT_CONFIG_REQ_RSP;
                    DEBUGP("L2CAP-SG:%x OPEN: RECONFIGURE_REQUEST -> Configuration_Request\n", p->myCid);
                    break;
                }
                default:
                    break;
                }
            } else {    // Receive
                switch(in->Code){
                case L2CAP_PDU_Code_Disconnection_request:
                    out->Code       = L2CAP_PDU_Code_Disconnection_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Disconnection_Response.SourceCID,      L2CAP_UINT16(in->Data.Disconnection_Request.SourceCID));
                    L2CAP_STORE16(out->Data.Disconnection_Response.DistinationCID, L2CAP_UINT16(in->Data.Disconnection_Request.DistinationCID));
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Disconnection_Response));
                    p->dynamic.state = CLOSED;
            		p->service->serviceProg(p, NULL, BT_L2CAP_SERVICE_SERVICEPROG_inLen_DISCONNECT, NULL, 0);	// ADD 2018.11.06
                    DEBUGP("L2CAP-SG:%x OPEN: Disconnection_request -> Disconnection_Response CLOSED\n", p->myCid);
                    break;
                case L2CAP_PDU_Code_Configure_request:
                {
                    unsigned short  r;
                    size_t          l;
                    size_t          m;

                    l = L2CAP_UINT16(in->Length) - sizeof(in->Data.Configuration_Request);
                    r = bt_l2capConfigurationParameterParse(in->Data.Configuration_Request.Configuration_Options, l,
                                                            out->Data.Configuration_Response.Config,              &m,
                                                            p);
                    out->Code       = L2CAP_PDU_Code_Configure_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Configuration_Response.SourceCID, p->peerCid);
                    L2CAP_STORE16(out->Data.Configuration_Response.Flags,     0);
                    L2CAP_STORE16(out->Data.Configuration_Response.Result,    r);
                    L2CAP_STORE16(out->Length, m + sizeof(out->Data.Configuration_Response));
                    if (r == L2CAP_PDU_Result_Configuration_Success) {
                        p->dynamic.timeOut     = time(NULL) + BT_RTX_TIMEOUT;
                        p->dynamic.state       = CONFIG;
                        p->dynamic.configState = WAIT_SEND_CONFIG;
                    }
                    DEBUGP("L2CAP-SG:%x OPEN: Configure_request -> Configuration_Response\n", p->myCid);
                    break;
                }
                default:
                    DEBUGP("L2CAP-SG:%x OPEN: ERROR %d -> N/A\n", p->myCid, in->Code);
                    break;
                }
            }

        //
        // 6.1.6 WAIT_DISCONNECT state
        //
        } else if (p->dynamic.state == WAIT_DISCONNECT) {
            if (in == NULL) {
                if (p->dynamic.timeOut < time(NULL)) {
            		p->service->serviceProg(p, NULL, BT_L2CAP_SERVICE_SERVICEPROG_inLen_DISCONNECT, NULL, 0);	// ADD 2018.11.06
                    p->dynamic.state = CLOSED;
                    DEBUGP("L2CAP-SG:%x WAIT_DISCONNECT: ERROR %d -> N/A\n", p->myCid, in->Code);
                }
            } else {
                switch(in->Code) {
                case L2CAP_PDU_Code_Disconnection_response:
                    if (in->Identifier == p->dynamic.disconnectionId) {
                		p->service->serviceProg(p, NULL, BT_L2CAP_SERVICE_SERVICEPROG_inLen_DISCONNECT, NULL, 0);	// ADD 2018.11.06
                        p->dynamic.state = CLOSED;
                    }
                    DEBUGP("L2CAP-SG:%x WAIT_DISCONNECT: Disconnection_response -> new state=%d\n", p->myCid, p->dynamic.state);
                    break;
                case L2CAP_PDU_Code_Disconnection_request:
                    out->Code       = L2CAP_PDU_Code_Disconnection_response;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Disconnection_Response.SourceCID,      L2CAP_UINT16(in->Data.Disconnection_Request.SourceCID));
                    L2CAP_STORE16(out->Data.Disconnection_Response.DistinationCID, L2CAP_UINT16(in->Data.Disconnection_Request.DistinationCID));
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Disconnection_Response));
            		p->service->serviceProg(p, NULL, BT_L2CAP_SERVICE_SERVICEPROG_inLen_DISCONNECT, NULL, 0);	// ADD 2018.11.06
                    p->dynamic.state = CLOSED;
                    DEBUGP("L2CAP-SG:%x WAIT_DISCONNECT: Disconnection_request -> Disconnection_Response\n", p->myCid);
                    break;
                case L2CAP_PDU_Code_Configure_request:
                    out->Code       = L2CAP_PDU_Code_Command_reject;
                    out->Identifier = in->Identifier;
                    L2CAP_STORE16(out->Data.Command_Reject.Reason, L2CAP_PDU_Reason_invalid_CID_in_request);
                    L2CAP_STORE16(out->Length, sizeof(out->Data.Command_Reject));
                    DEBUGP("L2CAP-SG:%x WAIT_DISCONNECT: Configure_request -> Command_Reject\n", p->myCid);
                    break;
                default:
                    DEBUGP("L2CAP-SG:%x WAIT_DISCONNECT: ERROR %d -> N/A\n", p->myCid, in->Code);
                    break;
                }
            }
        }
    }
#endif                                  // ADD END 2017.01.08

#ifdef DEBUG2
    if (p != NULL) {
        static struct bt_l2capChannel   *a = NULL;
        static int                      b = -1;

        if ((a != p) || (b != p->dynamic.state)) {
            a = p;
            b = p->dynamic.state;
            DEBUGP("L2CAP-SG: New state=%d evt=%d %04x-%02x-%02x\n", p->dynamic.state, p->dynamic.event, p->link->handle, p->peerCid, p->myCid);
        }
    }
#endif

//  if ((p != NULL) && (p->event == SEND_REQUEST)) {        // DEL 2017.01.08
    if ((p != NULL) && (L2CAP_UINT16(out->Length) > 0)) {   // ADD 2017.01.08
        outLen = L2CAP_UINT16(out->Length) + L2CAP_Data_Packet_C_Frame_HeadSize;
        signaling->signaling.lastId = out->Identifier;      // ADD 2017.01.08
    } else {
        outLen = 0;
    }
    return(outLen);
}


