//
//  bt_l2cap_channel_manager.c
//
//  Bluetooth Protocol Stack - L2CAP Channel Manager
//  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/>
//
//
//  Summary:
//      Creating the logical channel path
//      Message and task switcher
//
//  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
//
//  Update history
//  ---------- ----- -------------------------------------------
//  2013.01.21 v0.0  First cording
//  2013.04.21 v0.1  Commit
//  2013.04.25 v0.1  The ATT server was added to idle task scheduling
//  2014.02.23 v0.1a Bug fix
//  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 "bt_spec_l2cap.h"              // L2CAP format
#include "bt_l2cap_cb_channel.h"        // Logical channel
#include "bt_l2cap_cb_service.h"        // Service/protocol
#include "bt_l2cap_cb_link.h"           // ADD 2017.01.03
#include "bt_l2cap_channel_manager.h"   // ADD 2018.11.06

//#define DEBUG                         // Debug mode = on

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

size_t bt_l2capChannelManager(
//      unsigned short      handle,     // DEL 2017.01.03
        struct bt_l2capLink *link,      // ADD 2017.01.03
        unsigned char       *inBuff,
        size_t              inDataSize,
        unsigned char       *outBuff,
        size_t              outBuffSize
) {
    struct bt_l2cap_Data_Packet *in;
    struct bt_l2cap_Data_Packet *out;
    unsigned short              cid;
    size_t                      len;
    size_t                      outLen;

    in     = (struct bt_l2cap_Data_Packet *)inBuff;
    out    = (struct bt_l2cap_Data_Packet *)outBuff;
    outLen = 0;
    len    = 0;
    cid    = 0;

    // Initial process                  // ADD START 2017.01.08
    if (link->newCid < L2CAP_CID_Dynamically_allocated) {
        struct bt_l2capService  *a;
        struct bt_l2capService  *s;

        link->newCid = L2CAP_CID_Dynamically_allocated + 1;

        // Creating a default channel
        for(a = NULL; (s = bt_l2capServiceGetNext(&a)) != NULL; ) {                         // All services
            if ((s->psm & BT_L2CAP_SERVICE_CID_TO_PSM) == BT_L2CAP_SERVICE_CID_TO_PSM) {    // Connection less(fixed channel) service
                struct bt_l2capChannel  *c;

                cid = s->psm & (~BT_L2CAP_SERVICE_CID_TO_PSM);                              // psm to cid
                if ((c = bt_l2capChannelAddByLink(link, cid, cid)) != NULL) {
                    c->service = s;
            		DEBUGP("L2CAP-CM/%04x: Start cid=0x%02x,0x%02x psm=0x%04x fixed channel\n", link->handle, c->myCid, c->peerCid, s->psm);
                }
            }
        }
    }                                   // ADD END 2017.01.08
    
    // Data receive process
    if (in != NULL) {
        struct bt_l2capChannel  *p;

        // CID bundles, protocol switcher
        len = L2CAP_UINT16(in->Length);
        cid = L2CAP_UINT16(in->CID);
//      p = bt_l2capChannelFind(handle, 0, cid);        // DEL 2017.01.03
        p = bt_l2capChannelFindByLink(link, 0, cid);    // ADD 2017.01.03

#ifdef DELETE                           // DEL START 2017.01.08
        // Bulletin protocol is an auto create channel
        if ((p == NULL) && (cid < L2CAP_CID_Dynamically_allocated)) {
            struct bt_l2capService  *s;
            unsigned short          psm;

            psm = cid | BT_L2CAP_SERVICE_CID_TO_PSM;    // PSM .ne. CID
            if ((s = bt_l2capServiceFind(psm)) != NULL) {
//              if ((p = bt_l2capChannelAdd(handle, cid, cid)) != NULL) {       // DEL 2017.01.03
                if ((p = bt_l2capChannelAddByLink(link, cid, cid)) != NULL) {   // ADD 2017.01.03
//                  p->psm         = psm;                                       // DEL 2017.01.03
//                  p->serviceProg = s->serviceProg;                            // DEL 2017.01.03
                    p->service     = s;                                         // ADD 2017.01.03
                }
            }
        }
#endif                                  // DEL END 2017.01.08

        // Jump to protocol service
        if (p != NULL) {
//          len = p->serviceProg(handle, in->b_frame.Payload, len, out->b_frame.Payload, outBuffSize-L2CAP_Data_Packet_HeadSize);       // DEL 2017.01.03
            len = p->service->serviceProg(p, in->b_frame.Payload, len, out->b_frame.Payload, outBuffSize-L2CAP_Data_Packet_HeadSize);   // ADD 2017.01.03
            cid = p->peerCid;
        } else {                        // ADD 2014.02.23
            len = 0;                    // ADD 2014.02.23   Not answered
            DEBUGP("L2CAP-CM/%04x: Not found cid=0x%02x\n", link->handle, cid);
        }
        								// ADD START 2018.11.06
	// Connect notification
    } else if (inDataSize == BT_L2CAP_CM_inDataSize_CONNECT) {
        DEBUGP("L2CAP-CM/%04x: Start\n", link->handle);

	// Disconnect notification
    } else if (inDataSize == BT_L2CAP_CM_inDataSize_DISCONNECT) {
        struct bt_l2capChannel	*a;
    	struct bt_l2capChannel	*p;

    	for(a = NULL; (p = bt_l2capChannelGetNext(&a)) != NULL; ) {
            if (p->link == link) {
                p->service->serviceProg(p, NULL, BT_L2CAP_SERVICE_SERVICEPROG_inLen_DISCONNECT, NULL, 0);
                DEBUGP("L2CAP-CM/%04x: End cid=0x%02x\n", link->handle, p->myCid);
            }
        }
        DEBUGP("L2CAP-CM/%04x: End\n", link->handle);
										// ADD END 2018.11.06
    // Idle process
    } else {
//      static struct bt_l2capChannel   *id = NULL;                         // DEL 2017.01.03
        struct bt_l2capChannel          *p;

//      while((p = bt_l2capChannelGetNext(&id)) != NULL) {                  // DEL 2017.01.03
//          if (p->link->handle == handle) {                                // DEL 2017.01.03
        while((p = bt_l2capChannelGetNext(&(link->nowChannel))) != NULL) {  // ADD 2017.01.03
            if (p->link == link) {                                          // ADD 2017.01.03
                break;
            }
        }
        if (p != NULL) {
//          len = p->serviceProg(handle, NULL, 0, out->b_frame.Payload, outBuffSize-L2CAP_Data_Packet_HeadSize);    // DEL 2017.01.03
//          len = p->service->serviceProg(p, NULL, 0, out->b_frame.Payload, outBuffSize-L2CAP_Data_Packet_HeadSize);// ADD 2017.01.03 DEL 2018.10.06
            len = p->service->serviceProg(p, NULL, BT_L2CAP_SERVICE_SERVICEPROG_inLen_IDLE, out->b_frame.Payload, outBuffSize-L2CAP_Data_Packet_HeadSize);// ADD 2018.10.06
            cid = p->peerCid;
        }
    }

    if (len > 0) {
        L2CAP_STORE16(out->Length, len);
        L2CAP_STORE16(out->CID, cid);
        outLen = L2CAP_Data_Packet_HeadSize + len;
    } else {
        outLen = 0;
    }
    return(outLen);
}
