//
//  bt_rfcomm_server.c
//
//  Bluetooth Protocol Stack - RFCOMM Server
//  Copyright (C) 2013-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 1.1
//              PartF:1 RFCOMM with TS 07.10
//              Serial Port Emulation
//          ETSI TS 07.10 standard
//              GSM 07.10 (version 6.3.0 Release 1997)
//
//  Update history
//  ---------- ----- -------------------------------------------
//  2013.02.21 v0.0  First cording
//  2013.04.21 v0.1  Commit
//  2017.01.03 v0.3e argument handle -> cb_channel
//
#include <stddef.h>
#include <stdint.h>
#include <string.h>
#include "bt_rfcomm_server.h"
#include "bt_l2cap_cb_channel.h"        // Channel manage table     ADD 2017.01.03

//#define DEBUG

//
//  Protocol format
//
#define RFCOMM_ServerChannel        1
#define RFCOMM_ServerChannel_DLCI   (RFCOMM_ServerChannel << 1)
#define RFCOMM_ControlChannel_DLCI  0
//#define RFCOMM_BUFSIZE            1019
#define RFCOMM_BUFSIZE              1000

struct bt_rfcommFrame {
    uint8_t Address;
    uint8_t Control;
    uint8_t LengthIndicator[1];
//  uint8_t Information[];
//  uint8_t FCS;
}  __attribute__ ((packed));

#define convDLCI(Address)           ((Address) >> 2)
#define convAddress(DLCI)           ((DLCI) << 2)
#define Address_DLCI_MASK           0xfc
#define Address_CommandResponse     0x02
#define Address_EA                  0x01
#define Address_InitiatorCommand    0x03
#define Address_ResponderCommand    0x01
#define Address_InitiatorResponse   0x03
#define Address_ResponderResponse   0x01

#define Control_SABMcommand         0x2f
#define Control_UAresponse          0x63
#define Control_DMresponse          0x0f
#define Control_DISCcommand         0x43
#define Control_UIH                 0xef
#define Control_MASK                0xef
#define Control_Poll                0x10
#define Control_Final               0x10
#define Control_PollFinal           0x10

#define MultiplexerControlChannel_DLCI                  0
#define MultiplexerControlChannel_Type_TestCommand      0b00100011  // Test Command (Test)
#define MultiplexerControlChannel_Type_TestResponse     0b00100001  //
#define MultiplexerControlChannel_Type_FConCommand      0b10100011  // Flow Control On Command (FCon)
#define MultiplexerControlChannel_Type_FConResponse     0b10100001  //
#define MultiplexerControlChannel_Type_FCoffCommand     0b01100011  // Flow Control Off Command (FCoff)
#define MultiplexerControlChannel_Type_FCoffResponse    0b01100001  //
#define MultiplexerControlChannel_Type_MSCCommand       0b11100011  // Modem Status Command (MSC) +2or3bytes
#define MultiplexerControlChannel_Type_MSCResponse      0b11100001  //
#define MultiplexerControlChannel_Type_RPNCommand       0b10010011  // Remote Port Negotiation Command (RPN) +1or8bytes
#define MultiplexerControlChannel_Type_RPNResponse      0b10010001  //
#define MultiplexerControlChannel_Type_RLSCommand       0b01010011  // Remote Line Status Command(RLS) +2bytes
#define MultiplexerControlChannel_Type_RLSResponse      0b01010001  //
#define MultiplexerControlChannel_Type_PNCommand        0b10000011  // DLC parameter negotiation (PN) +8bytes
#define MultiplexerControlChannel_Type_PNResponse       0b10000001  //
#define MultiplexerControlChannel_Type_NSCResponse      0b00010001  // Non Supported Command Response (NSC) +1byte
#define MultiplexerControlChannel_Type_Command          0b00000010  //
#define MultiplexerControlChannel_getLength(a)          ((a) >> 1)  // max 127bytes, Not use EA bit
#define MultiplexerControlChannel_putLength(a)          ((a) << 1 | 0x01)

//
//  Debug
//
#ifdef DEBUG
#include <stdio.h>                      // Debug
#define DEBUGP(...)     printf(__VA_ARGS__)
void hexdump(char *title, uint8_t *p, size_t l){
    DEBUGP(title);
    for(; l > 0; --l) {
        DEBUGP("%02x", *p++);
    }
    DEBUGP("\n");
    return;
}

#else
#define DEBUGP(...)
#define hexdump(a,b,c)
#endif

//
//  Global area
//
unsigned char   *bt_rfcommServerSendRequestData = NULL;
size_t          bt_rfcommServerSendRequestLen = 0;

//
//  Internal subroutines
//
uint8_t *bt_rfcommGetInformation(struct bt_rfcommFrame *p, size_t *Length, size_t *HeadSize) {
    size_t  l;
    size_t  h;
    uint8_t *n;

    n = p->LengthIndicator;
    h = sizeof(struct bt_rfcommFrame);
    if (*n & 0x01) {
        l = *n++ >> 1;
    } else {
        l  = (*n++) >> 1;
        l += (*n++) << 7;
        h++;
    }
    *Length   = l;
    *HeadSize = h;
    return(n);
}

uint8_t *bt_rfcommPutLengthIndicator(struct bt_rfcommFrame *p, size_t Length, size_t *HeadSize) {
    size_t  h;
    uint8_t *n;

    n = p->LengthIndicator;
    h = sizeof(struct bt_rfcommFrame);
    if (Length < 128) {
        *n++ = (Length << 1) | 0x01;
    } else {
        *n++ = (uint8_t)(Length << 1);
        *n++ = (uint8_t)(Length >> 7);
        h++;
    }
    *HeadSize = h;
    return(n);
}


uint8_t bt_rfcommCalcFCS(struct bt_rfcommFrame *in, size_t len){
    const unsigned char crctable[256] = {   //reversed, 8-bit, poly=0x07
        0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75,  0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
        0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69,  0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
        0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D,  0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
        0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51,  0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
        0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05,  0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
        0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19,  0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
        0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D,  0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
        0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21,  0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
        0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95,  0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
        0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89,  0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
        0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD,  0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
        0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1,  0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
        0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5,  0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
        0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9,  0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
        0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD,  0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
        0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1,  0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF
    };
    uint8_t FCS;
    uint8_t *p;

    p = (uint8_t *)in;
    FCS = 0xFF;
    while (len--) {
        FCS = crctable[FCS ^ *p++];
    }
    FCS = 0xFF - FCS;
    return(FCS);
}

// User own code (default)                  // ADD START 2017.01.03
void _bt_rfcommServerDataRecived(unsigned char *inBuff, size_t inLen) {
    return;
}
void bt_rfcommServerDataRecived() __attribute__ ((weak, alias ("_bt_rfcommServerDataRecived")));
                                            // ADD END 2017.01.03

//
//  Server
//
//size_t bt_rfcommServer(unsigned short handle, unsigned char *inBuff, size_t inLen, unsigned char *outBuff, size_t outSize) {      // DEL 2017.01.03
size_t bt_rfcommServer(struct bt_l2capChannel *ch, unsigned char *inBuff, size_t inLen, unsigned char *outBuff, size_t outSize) {   // ADD 2017.01.03
    struct bt_rfcommFrame   *recv;
    struct bt_rfcommFrame   *send;
    size_t                  outLen;

    unsigned char   recvDlci;
    unsigned char   recvCr;
    unsigned char   recvCtrl;
    unsigned char   recvPf;
    size_t          recvDataLen;
    uint8_t         *recvData;
    static enum {
        DLC_CLOSE = 0,
        DLC_OPEN,
        PN_SEND,
        MSC_SEND,
        RPN_SEND
    }   state = DLC_CLOSE;

    recv   = (struct bt_rfcommFrame *)inBuff;
    send   = (struct bt_rfcommFrame *)outBuff;
    outLen = 0;

    // Receive data
    if (recv != NULL) {
        unsigned char   fcs;
        size_t          h;

        recvDlci = convDLCI(recv->Address);
        recvCr   = recv->Address & Address_CommandResponse;
        recvCtrl = recv->Control & Control_MASK;
        recvPf   = recv->Control & Control_PollFinal;
        if (recvCtrl == Control_UIH) {
            fcs = bt_rfcommCalcFCS(recv, 2);
        } else {
            fcs = bt_rfcommCalcFCS(recv, inLen - 1);
        }
        if (inBuff[inLen - 1] != fcs) {
            DEBUGP("RFCOMM: FCS error\n");
            recv      = NULL;
            recvCtrl = 0;
        }
        recvData = bt_rfcommGetInformation(recv, &recvDataLen, &h);

        DEBUGP("RFCOMM: DLCI=%d %s Control=0x%02x %s FCS=%02x:%02x\n",
                recvDlci, recvCr?"command":"response", recvCtrl, recvPf?"P/F":"", inBuff[inLen-1], fcs);
        hexdump("RFCOMM: Data=", recvData, recvDataLen);
    } else {
        recvDlci    = 0;
        recvCr      = 0;
        recvCtrl    = 0;
        recvPf      = 0;
        recvData    = NULL;
        recvDataLen = 0;
    }

    // State machine
#ifdef DEBUG
    {
        static int  s = -1;

        if (s != state) {
            DEBUGP("RFCOMM: new state %d\n", state);
            s = state;
        }
    }
#endif
    switch(state){
    case DLC_CLOSE:
        if ((recvCtrl == Control_SABMcommand) && (recvDlci == RFCOMM_ControlChannel_DLCI)) {
            DEBUGP("RFCOMM: SABM Establishment DLCI=%d\n", recvDlci);
            if (recvPf) {
                size_t  h;
                uint8_t *d;

                send->Address = recv->Address;
                send->Control = Control_UAresponse | Control_Final;
                d             = bt_rfcommPutLengthIndicator(send, 0, &h);
                *d            = bt_rfcommCalcFCS(send, h);
                outLen        = h + 1;
            }
            state = DLC_OPEN;
        } else if (recvCtrl != 0) {
            size_t  h;
            uint8_t *d;

            send->Address = recv->Address;
            send->Control = Control_DMresponse | Control_Final;
            d             = bt_rfcommPutLengthIndicator(send, 0, &h);
            *d            = bt_rfcommCalcFCS(send, h);
            outLen        = h + 1;
        }
        break;

    case DLC_OPEN:
        switch(recvCtrl){
        case Control_SABMcommand:
            DEBUGP("RFCOMM: SABM Establishment DLCI=%d\n", recvDlci);
            if (recvPf) {
                size_t  h;
                uint8_t *d;

                send->Address = recv->Address;
                send->Control = Control_UAresponse | Control_Final;
                d             = bt_rfcommPutLengthIndicator(send, 0, &h);
                *d            = bt_rfcommCalcFCS(send, h);
                outLen        = h + 1;
            }
            state = MSC_SEND;
            break;

        case Control_UAresponse:
            break;

        case Control_DMresponse:
            break;

        case Control_DISCcommand:
            DEBUGP("RFCOMM: DISC release DLCI=%d\n", recvDlci);
            if (recvPf) {
                size_t  h;
                uint8_t *d;

                send->Address = recv->Address;
                send->Control = Control_UAresponse | Control_Final;
                d             = bt_rfcommPutLengthIndicator(send, 0, &h);
                *d            = bt_rfcommCalcFCS(send, h);
                outLen        = h + 1;
            }
            if (recvDlci == RFCOMM_ControlChannel_DLCI) {
                state = DLC_CLOSE;
            }
            break;

        case Control_UIH:
            if (recvDlci == MultiplexerControlChannel_DLCI) {
                unsigned char   controlType;
                size_t          controlLen;

                controlType = *recvData++;
                controlLen  = MultiplexerControlChannel_getLength(*recvData++);
                switch(controlType) {
                case MultiplexerControlChannel_Type_TestCommand:
                    break;
                case MultiplexerControlChannel_Type_TestResponse:
                    break;
                case MultiplexerControlChannel_Type_FConCommand:
                    break;
                case MultiplexerControlChannel_Type_FConResponse:
                    break;
                case MultiplexerControlChannel_Type_FCoffCommand:
                    break;
                case MultiplexerControlChannel_Type_FCoffResponse:
                    break;
                case MultiplexerControlChannel_Type_MSCCommand:
                {
                    int     dlci;
                    int     v24;
                    int     br;
                    size_t  h;
                    uint8_t *d;

                    dlci = *recvData++ >> 2;
                    v24  = *recvData++;
                    if (controlLen > 2) {
                        br = *recvData++;
                    } else {
                        br = 0;
                    }
                    DEBUGP("RFCOMM: MSC recv dlci=%d v24=0x%02x br=%d\n", dlci, v24, br);

                    send->Address = convAddress(RFCOMM_ControlChannel_DLCI) | Address_ResponderCommand;
                    send->Control = Control_UIH;
                    d           = bt_rfcommPutLengthIndicator(send, 4, &h);
                    *d++        = MultiplexerControlChannel_Type_MSCResponse;
                    *d++        = MultiplexerControlChannel_putLength(2);
                    *d++        = (dlci << 2) | 0b00000011;
                    *d++        = v24;
                    *d          = bt_rfcommCalcFCS(send, 2);    // Address+Control only
                    outLen        = h + 4 + 1;
                    break;
                }
                case MultiplexerControlChannel_Type_MSCResponse:
                    break;
                case MultiplexerControlChannel_Type_RPNCommand:
                {
                    int     dlci;
                    size_t  h;
                    uint8_t *d;

                    dlci = *recvData++ >> 2;
                    DEBUGP("RFCOMM: RPN recv dlci=%d %02x %02x %02x %02x %02x %02x %02x\n",
                            dlci, *(recvData+0), *(recvData+1), *(recvData+2), *(recvData+3), *(recvData+4), *(recvData+5), *(recvData+6));

                    send->Address = convAddress(RFCOMM_ControlChannel_DLCI) | Address_ResponderCommand;
                    send->Control = Control_UIH;
                    d             = bt_rfcommPutLengthIndicator(send, recvDataLen, &h);
                    *d++          = MultiplexerControlChannel_Type_RPNResponse;
                    *d++          = MultiplexerControlChannel_putLength(controlLen);
                    *d++          = dlci;
                    memcpy(d, recvData, controlLen-1);  // Echo back
                    d            += (controlLen-1);
                    *d            = bt_rfcommCalcFCS(send, 2);  // Address+Control only
                    outLen        = h + recvDataLen + 1;
                    break;
                }
                case MultiplexerControlChannel_Type_RPNResponse:
                    break;
                case MultiplexerControlChannel_Type_RLSCommand:
                    break;
                case MultiplexerControlChannel_Type_RLSResponse:
                    break;
                case MultiplexerControlChannel_Type_PNCommand:
                {
                    int     d_bits;     // DLCI
                    int     i_bits;     // Type of frames, UIH=0
                    int     cl_bits;    // Convergence Layers controlType
                    int     p_bits;     // Priority
                    int     t_bits;     // Acknowledgement timer (T1)
                    int     n_bits;     // Maximum frame size (N1)
                    int     na_bits;    // Maximum number of retransmissions (N2)
                    int     k_bits;     // Window size for error recovery mode (k)
                    size_t  h;
                    uint8_t *d;

                    d_bits   = *recvData++;
                    i_bits   = *recvData & 0x0f;
                    cl_bits  = (*recvData++) >> 4;
                    p_bits   = *recvData++;
                    t_bits   = *recvData++;
                    n_bits   = *recvData++;
                    n_bits  |= (*recvData++) << 8;
                    na_bits  = *recvData++;
                    k_bits   = *recvData++;
                    DEBUGP("RFCOMM: PN recv d=%d i=%d cl=%d p=%d t=%d n=%d na=%d k=%d\n", d_bits, i_bits, cl_bits, p_bits, t_bits, n_bits, na_bits, k_bits);

                    send->Address = convAddress(RFCOMM_ControlChannel_DLCI) | Address_ResponderCommand;
                    send->Control = Control_UIH;
                    controlLen    = 8;
                    d             = bt_rfcommPutLengthIndicator(send, controlLen+2, &h);
                    *d++          = MultiplexerControlChannel_Type_PNResponse;
                    *d++          = MultiplexerControlChannel_putLength(controlLen);
// unsupport        cl_bits       = 14; // Type 15 credit based flow control
                    cl_bits       = 0;  // Type 1  Unstructured Octet Stream
                    *d++          = d_bits;
                    *d++          = i_bits | (cl_bits << 4);
                    *d++          = p_bits;
                    *d++          = t_bits;
                    *d++          = n_bits;
                    *d++          = n_bits >> 8;
                    *d++          = na_bits;
                    *d++          = k_bits;
                    *d            = bt_rfcommCalcFCS(send, 2);  // Address+Control only
                    outLen        = h + controlLen + 2 + 1;
                    break;
                }
                case MultiplexerControlChannel_Type_PNResponse:
                    break;
                default:
                    if (recvPf) {
                        size_t  h;
                        uint8_t *d;

                        DEBUGP("RFCOMM: MPX invalid %d\n", controlType);
                        send->Address = recv->Address;
                        send->Control = recv->Control | Control_Final;
                        d             = bt_rfcommPutLengthIndicator(send, 3, &h);
                        *d++          = MultiplexerControlChannel_Type_NSCResponse;
                        *d++          = MultiplexerControlChannel_putLength(1);
                        *d++          = controlType;
                        *d            = bt_rfcommCalcFCS(send, h + 3);
                        outLen        = h + 3 + 1;
                    }
                    break;
                }

            // In comming application data
            } else {
                DEBUGP("RFCOMM: Application data DLCI=%d\n", recvDlci);
#ifdef DEL
                // Echo back
                {
                    size_t          h;
                    uint8_t         *d;
                    size_t          l;

                    send->Address = convAddress(RFCOMM_ServerChannel_DLCI) | Address_ResponderCommand;
                    send->Control = Control_UIH;            // No poll
                    d             = bt_rfcommPutLengthIndicator(send, recvDataLen, &h);
                    memcpy(d, recvData, recvDataLen);
                    d            += recvDataLen;
                    *d            = bt_rfcommCalcFCS(send, 2);  // Address+Control only
                    outLen        = h + recvDataLen + 1;
                }
#endif
                bt_rfcommServerDataRecived(recvData, recvDataLen);
                outLen        = 0;
            }
            break;
        case 0:                         // Idle
            // Send Request
            if (bt_rfcommServerSendRequestData != NULL) {
                size_t          h;
                uint8_t         *d;

                send->Address = convAddress(RFCOMM_ServerChannel_DLCI) | Address_ResponderCommand;
                send->Control = Control_UIH;            // No poll
                d             = bt_rfcommPutLengthIndicator(send, bt_rfcommServerSendRequestLen, &h);
                memcpy(d, bt_rfcommServerSendRequestData, bt_rfcommServerSendRequestLen);
                d            += bt_rfcommServerSendRequestLen;
                *d            = bt_rfcommCalcFCS(send, 2);  // Address+Control only
                outLen        = h + bt_rfcommServerSendRequestLen + 1;
                bt_rfcommServerSendRequestData = NULL;
            }
            break;
        default:
            break;
        }
        break;

    case PN_SEND:
    {
        int     d_bits;
        int     i_bits;
        int     cl_bits;
        int     p_bits;
        int     t_bits;
        int     n_bits;
        int     na_bits;
        int     k_bits;
        size_t  h;
        uint8_t *d;

        d_bits  = RFCOMM_ServerChannel_DLCI;
        i_bits  = 0;
//      cl_bits = 14;
        cl_bits = 0;
        p_bits  = 0;
        t_bits  = 0;
        n_bits  = RFCOMM_BUFSIZE;
        na_bits = 0;
        k_bits  = 0;
        DEBUGP("RFCOMM: PN send d=%d i=%d cl=%d p=%d t=%d n=%d na=%d k=%d\n", d_bits, i_bits, cl_bits, p_bits, t_bits, n_bits, na_bits, k_bits);

        send->Address = convAddress(RFCOMM_ControlChannel_DLCI) | Address_ResponderCommand;
        send->Control = Control_UIH;    // no poll
        d             = bt_rfcommPutLengthIndicator(send, 8 + 2, &h);
        *d++          = MultiplexerControlChannel_Type_PNCommand;
        *d++          = MultiplexerControlChannel_putLength(8);
        *d++          = d_bits;
        *d++          = i_bits | (cl_bits << 4);
        *d++          = p_bits;
        *d++          = t_bits;
        *d++          = n_bits;
        *d++          = n_bits >> 8;
        *d++          = na_bits;
        *d++          = k_bits;
        *d            = bt_rfcommCalcFCS(send, 2);      // Address+Control only
        outLen        = h + 8 + 2 + 1;
        state         = DLC_OPEN;
        break;
    }

    case MSC_SEND:
    {
        int     dlci;
        int     v24;
        int     br;
        size_t  h;
        uint8_t *d;

        dlci = RFCOMM_ServerChannel_DLCI;
        v24  = 0b10001101;
        br   = 0;
        DEBUGP("RFCOMM: MSC send dlci=%d v24=0x%02x br=%d\n", dlci, v24, br);

        send->Address = convAddress(RFCOMM_ControlChannel_DLCI) | Address_ResponderCommand;
        send->Control = Control_UIH;            // No poll
        d             = bt_rfcommPutLengthIndicator(send, 2 + 2, &h);
        *d++          = MultiplexerControlChannel_Type_MSCCommand;
        *d++          = MultiplexerControlChannel_putLength(2);
        *d++          = (dlci << 2) | 0b00000011;
        *d++          = v24;
        *d            = bt_rfcommCalcFCS(send, 2);  // Address+Control only
        outLen        = h + 2 + 2 + 1;
        state         = DLC_OPEN;
        break;
    }

    default:
        break;
    }

    return(outLen);
}

int bt_rfcommServerDataSendRequest(unsigned char *outBuff, size_t outLen){
    int rc;

    if (bt_rfcommServerSendRequestData == NULL) {
        bt_rfcommServerSendRequestData = outBuff;
        bt_rfcommServerSendRequestLen  = outLen;
        rc = 0;
    } else {
        rc = -1;
    }
    return(rc);
}

