Yoonseok Pyo / Mbed 2 deprecated ORTP-L_WiiRemoteTest

ORTP-L_WiiRemoteTest

Dependencies:   Motordriver mbed FatFileSystem

Fork of WallbotTypeN by Junichi Katsu

BlueUSB/L2CAP.cpp@0:425791fe4b42, 2011-05-01 (annotated)

Committer:
jksoft
Date:
Sun May 01 11:12:52 2011 +0000
Revision:
0:425791fe4b42

        

        
Who changed what in which revision?

        

            
                
UserRevisionLine numberNew contents of line

            
            


jksoft

0:425791fe4b42


     1

/*





jksoft

0:425791fe4b42


     2

Copyright (c) 2010 Peter Barrett





jksoft

0:425791fe4b42


     3







jksoft

0:425791fe4b42


     4

Permission is hereby granted, free of charge, to any person obtaining a copy





jksoft

0:425791fe4b42


     5

of this software and associated documentation files (the "Software"), to deal





jksoft

0:425791fe4b42


     6

in the Software without restriction, including without limitation the rights





jksoft

0:425791fe4b42


     7

to use, copy, modify, merge, publish, distribute, sublicense, and/or sell





jksoft

0:425791fe4b42


     8

copies of the Software, and to permit persons to whom the Software is





jksoft

0:425791fe4b42


     9

furnished to do so, subject to the following conditions:





jksoft

0:425791fe4b42


    10







jksoft

0:425791fe4b42


    11

The above copyright notice and this permission notice shall be included in





jksoft

0:425791fe4b42


    12

all copies or substantial portions of the Software.





jksoft

0:425791fe4b42


    13







jksoft

0:425791fe4b42


    14

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR





jksoft

0:425791fe4b42


    15

IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,





jksoft

0:425791fe4b42


    16

FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE





jksoft

0:425791fe4b42


    17

AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER





jksoft

0:425791fe4b42


    18

LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,





jksoft

0:425791fe4b42


    19

OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN





jksoft

0:425791fe4b42


    20

THE SOFTWARE.





jksoft

0:425791fe4b42


    21

*/





jksoft

0:425791fe4b42


    22







jksoft

0:425791fe4b42


    23







jksoft

0:425791fe4b42


    24

#include <stdio.h>





jksoft

0:425791fe4b42


    25

#include <stdlib.h>





jksoft

0:425791fe4b42


    26

#include <stdio.h>





jksoft

0:425791fe4b42


    27

#include <string.h>





jksoft

0:425791fe4b42


    28







jksoft

0:425791fe4b42


    29

#include "Utils.h"





jksoft

0:425791fe4b42


    30

#include "hci.h"





jksoft

0:425791fe4b42


    31







jksoft

0:425791fe4b42


    32

#define L2CAP_COMMAND_REJ       0x01





jksoft

0:425791fe4b42


    33

#define L2CAP_CONN_REQ          0x02





jksoft

0:425791fe4b42


    34

#define L2CAP_CONN_RSP          0x03





jksoft

0:425791fe4b42


    35

#define L2CAP_CONF_REQ          0x04





jksoft

0:425791fe4b42


    36

#define L2CAP_CONF_RSP          0x05





jksoft

0:425791fe4b42


    37

#define L2CAP_DISCONN_REQ       0x06





jksoft

0:425791fe4b42


    38

#define L2CAP_DISCONN_RSP       0x07





jksoft

0:425791fe4b42


    39

#define L2CAP_ECHO_REQ          0x08





jksoft

0:425791fe4b42


    40

#define L2CAP_ECHO_RSP          0x09





jksoft

0:425791fe4b42


    41

#define L2CAP_INFO_REQ          0x0a





jksoft

0:425791fe4b42


    42

#define L2CAP_INFO_RSP          0x0b





jksoft

0:425791fe4b42


    43







jksoft

0:425791fe4b42


    44







jksoft

0:425791fe4b42


    45

 /* L2CAP command codes */





jksoft

0:425791fe4b42


    46

 const char* L2CAP_ComandCodeStr(int c)





jksoft

0:425791fe4b42


    47

 {





jksoft

0:425791fe4b42


    48

     switch (c)





jksoft

0:425791fe4b42


    49

     {





jksoft

0:425791fe4b42


    50

        case L2CAP_COMMAND_REJ:    return "L2CAP_COMMAND_REJ";





jksoft

0:425791fe4b42


    51

        case L2CAP_CONN_REQ:    return "L2CAP_CONN_REQ";





jksoft

0:425791fe4b42


    52

        case L2CAP_CONN_RSP:    return "L2CAP_CONN_RSP";





jksoft

0:425791fe4b42


    53

        case L2CAP_CONF_REQ:    return "L2CAP_CONF_REQ";





jksoft

0:425791fe4b42


    54

        case L2CAP_CONF_RSP:    return "L2CAP_CONF_RSP";





jksoft

0:425791fe4b42


    55

        case L2CAP_DISCONN_REQ:    return "L2CAP_DISCONN_REQ";





jksoft

0:425791fe4b42


    56

        case L2CAP_DISCONN_RSP:    return "L2CAP_DISCONN_RSP";





jksoft

0:425791fe4b42


    57

        case L2CAP_ECHO_REQ:    return "L2CAP_ECHO_REQ";





jksoft

0:425791fe4b42


    58

        case L2CAP_ECHO_RSP:    return "L2CAP_ECHO_RSP";





jksoft

0:425791fe4b42


    59

        case L2CAP_INFO_REQ:    return "L2CAP_INFO_REQ";





jksoft

0:425791fe4b42


    60

        case L2CAP_INFO_RSP:    return "L2CAP_INFO_RSP";





jksoft

0:425791fe4b42


    61

     }





jksoft

0:425791fe4b42


    62

     return "unknown";





jksoft

0:425791fe4b42


    63

 }





jksoft

0:425791fe4b42


    64







jksoft

0:425791fe4b42


    65

typedef struct





jksoft

0:425791fe4b42


    66

{





jksoft

0:425791fe4b42


    67

    u16    handle;





jksoft

0:425791fe4b42


    68

    u16    length;            // total





jksoft

0:425791fe4b42


    69

    u16    l2capLength;    // length -4





jksoft

0:425791fe4b42


    70

    u16    cid;            // Signaling packet CID = 1





jksoft

0:425791fe4b42


    71

    u8  data[64];       // Largest thing to send!!! todo





jksoft

0:425791fe4b42


    72

} L2CAPData;





jksoft

0:425791fe4b42


    73







jksoft

0:425791fe4b42


    74

typedef struct





jksoft

0:425791fe4b42


    75

{





jksoft

0:425791fe4b42


    76

    u16    handle;





jksoft

0:425791fe4b42


    77

    u16    length;            // total





jksoft

0:425791fe4b42


    78

    u16    l2capLength;    // length -4





jksoft

0:425791fe4b42


    79

    u16    cid;            // Signaling packet CID = 1





jksoft

0:425791fe4b42


    80







jksoft

0:425791fe4b42


    81

    // Payload





jksoft

0:425791fe4b42


    82

    u8    cmd;            //





jksoft

0:425791fe4b42


    83

    u8    id;





jksoft

0:425791fe4b42


    84

    u16    cmdLength;        // total-8





jksoft

0:425791fe4b42


    85

    u16 params[4];      // Params





jksoft

0:425791fe4b42


    86

} L2CAPCmd;





jksoft

0:425791fe4b42


    87







jksoft

0:425791fe4b42


    88

//





jksoft

0:425791fe4b42


    89

void BTDevice::Init()





jksoft

0:425791fe4b42


    90

{





jksoft

0:425791fe4b42


    91

    memset(&_info,0,sizeof(inquiry_info));





jksoft

0:425791fe4b42


    92

    _handle = 0;





jksoft

0:425791fe4b42


    93

    _name[0] = 0;





jksoft

0:425791fe4b42


    94

    _state = 0;





jksoft

0:425791fe4b42


    95

}





jksoft

0:425791fe4b42


    96







jksoft

0:425791fe4b42


    97

// virtual SocketHandler





jksoft

0:425791fe4b42


    98

int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr)





jksoft

0:425791fe4b42


    99

{





jksoft

0:425791fe4b42


   100

    L2CAPSocket* s = (L2CAPSocket*)sock;





jksoft

0:425791fe4b42


   101

    L2CAPAddr* a = (L2CAPAddr*)addr;





jksoft

0:425791fe4b42


   102

    s->scid = 0x40 + sock->ID-1;   // are these reserved?





jksoft

0:425791fe4b42


   103

    s->dcid = 0;





jksoft

0:425791fe4b42


   104

    Connect(s->scid,a->psm);





jksoft

0:425791fe4b42


   105

    return sock->ID;





jksoft

0:425791fe4b42


   106

}





jksoft

0:425791fe4b42


   107







jksoft

0:425791fe4b42


   108

// virtual SocketHandler





jksoft

0:425791fe4b42


   109

int BTDevice::Send(SocketInternal* sock, const u8* data, int len)





jksoft

0:425791fe4b42


   110

{





jksoft

0:425791fe4b42


   111

    L2CAPData d;





jksoft

0:425791fe4b42


   112

    L2CAPSocket* s = (L2CAPSocket*)sock;





jksoft

0:425791fe4b42


   113







jksoft

0:425791fe4b42


   114

    d.handle = _handle | 0x2000;





jksoft

0:425791fe4b42


   115

    d.length = 4 + len;





jksoft

0:425791fe4b42


   116

    d.l2capLength = len;





jksoft

0:425791fe4b42


   117

    d.cid = s->dcid;





jksoft

0:425791fe4b42


   118







jksoft

0:425791fe4b42


   119

    if (len > 64)





jksoft

0:425791fe4b42


   120

        return -1;





jksoft

0:425791fe4b42


   121

    memcpy(d.data,data,len);





jksoft

0:425791fe4b42


   122

    return Send((u8*)&d,len+8);





jksoft

0:425791fe4b42


   123

}





jksoft

0:425791fe4b42


   124







jksoft

0:425791fe4b42


   125

// virtual SocketHandler





jksoft

0:425791fe4b42


   126

int BTDevice::Close(SocketInternal* sock)





jksoft

0:425791fe4b42


   127

{





jksoft

0:425791fe4b42


   128

    printf("L2CAP close %d\n",sock->ID);





jksoft

0:425791fe4b42


   129

    L2CAPSocket* s = (L2CAPSocket*)sock;





jksoft

0:425791fe4b42


   130

    return Disconnect(s->scid,s->dcid);





jksoft

0:425791fe4b42


   131

}





jksoft

0:425791fe4b42


   132







jksoft

0:425791fe4b42


   133

L2CAPSocket* BTDevice::SCIDToSocket(int scid)





jksoft

0:425791fe4b42


   134

{





jksoft

0:425791fe4b42


   135

    return (L2CAPSocket*)GetSocketInternal(scid-0x40+1);





jksoft

0:425791fe4b42


   136

}





jksoft

0:425791fe4b42


   137







jksoft

0:425791fe4b42


   138

int BTDevice::Send(const u8* data, int len)





jksoft

0:425791fe4b42


   139

{





jksoft

0:425791fe4b42


   140

     _transport->ACLSend(data,len);





jksoft

0:425791fe4b42


   141

     return 0;





jksoft

0:425791fe4b42


   142

}





jksoft

0:425791fe4b42


   143







jksoft

0:425791fe4b42


   144

int BTDevice::Send(u8 c, u8 id, u16* params, int count)





jksoft

0:425791fe4b42


   145

{





jksoft

0:425791fe4b42


   146

    L2CAPCmd cmd;





jksoft

0:425791fe4b42


   147

    cmd.handle = _handle | 0x2000;





jksoft

0:425791fe4b42


   148

    cmd.length = 8 + count*2;





jksoft

0:425791fe4b42


   149







jksoft

0:425791fe4b42


   150

    cmd.l2capLength = cmd.length-4;





jksoft

0:425791fe4b42


   151

    cmd.cid = 1;    // Signaling packet





jksoft

0:425791fe4b42


   152







jksoft

0:425791fe4b42


   153

    cmd.cmd = c;





jksoft

0:425791fe4b42


   154

    cmd.id = id;





jksoft

0:425791fe4b42


   155

    cmd.cmdLength = count*2;





jksoft

0:425791fe4b42


   156

    for (int i = 0; i < count; i++)





jksoft

0:425791fe4b42


   157

        cmd.params[i] = params[i];





jksoft

0:425791fe4b42


   158

    return Send((u8*)&cmd,cmd.length+4);





jksoft

0:425791fe4b42


   159

}





jksoft

0:425791fe4b42


   160







jksoft

0:425791fe4b42


   161

int BTDevice::Connect(int scid, int psm)





jksoft

0:425791fe4b42


   162

{





jksoft

0:425791fe4b42


   163

    u16 p[2];





jksoft

0:425791fe4b42


   164

    p[0] = psm;





jksoft

0:425791fe4b42


   165

    p[1] = scid;





jksoft

0:425791fe4b42


   166

    return Send(L2CAP_CONN_REQ,_txid++,p,2);





jksoft

0:425791fe4b42


   167

}





jksoft

0:425791fe4b42


   168







jksoft

0:425791fe4b42


   169

int BTDevice::Disconnect(int scid, int dcid)





jksoft

0:425791fe4b42


   170

{





jksoft

0:425791fe4b42


   171

    u16 p[2];





jksoft

0:425791fe4b42


   172

    p[0] = dcid;





jksoft

0:425791fe4b42


   173

    p[1] = scid;





jksoft

0:425791fe4b42


   174

    return Send(L2CAP_DISCONN_REQ,_txid++,p,2);





jksoft

0:425791fe4b42


   175

}





jksoft

0:425791fe4b42


   176







jksoft

0:425791fe4b42


   177

int BTDevice::ConfigureRequest(int dcid)





jksoft

0:425791fe4b42


   178

{





jksoft

0:425791fe4b42


   179

    u16 p[4];





jksoft

0:425791fe4b42


   180

    p[0] = dcid;





jksoft

0:425791fe4b42


   181

    p[1] = 0;





jksoft

0:425791fe4b42


   182

    p[2] = 0x0201;  // Options





jksoft

0:425791fe4b42


   183

    p[3] = 0x02A0;  // 672





jksoft

0:425791fe4b42


   184

    return Send(L2CAP_CONF_REQ,_txid++,p,4);





jksoft

0:425791fe4b42


   185

}





jksoft

0:425791fe4b42


   186







jksoft

0:425791fe4b42


   187

int BTDevice::ConfigureResponse(u8 rxid, int dcid)





jksoft

0:425791fe4b42


   188

{





jksoft

0:425791fe4b42


   189

    u16 p[3];





jksoft

0:425791fe4b42


   190

    p[0] = dcid;





jksoft

0:425791fe4b42


   191

    p[1] = 0;





jksoft

0:425791fe4b42


   192

    p[2] = 0;





jksoft

0:425791fe4b42


   193

    return Send(L2CAP_CONF_RSP,rxid,p,3);





jksoft

0:425791fe4b42


   194

}





jksoft

0:425791fe4b42


   195







jksoft

0:425791fe4b42


   196

int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid)





jksoft

0:425791fe4b42


   197

{





jksoft

0:425791fe4b42


   198

    u16 p[2];





jksoft

0:425791fe4b42


   199

    p[0] = dcid;





jksoft

0:425791fe4b42


   200

    p[1] = scid;





jksoft

0:425791fe4b42


   201

    return Send(L2CAP_DISCONN_RSP,rxid,p,2);





jksoft

0:425791fe4b42


   202

}





jksoft

0:425791fe4b42


   203







jksoft

0:425791fe4b42


   204

void BTDevice::Control(const u8* data, int len)





jksoft

0:425791fe4b42


   205

{





jksoft

0:425791fe4b42


   206

    int cc = data[8];





jksoft

0:425791fe4b42


   207

    printf(L2CAP_ComandCodeStr(cc));





jksoft

0:425791fe4b42


   208

    int result = LE16(data+16);





jksoft

0:425791fe4b42


   209

    printf(" Result %d\n",result);





jksoft

0:425791fe4b42


   210

    switch (cc)





jksoft

0:425791fe4b42


   211

    {





jksoft

0:425791fe4b42


   212

        case L2CAP_COMMAND_REJ:





jksoft

0:425791fe4b42


   213

           break;





jksoft

0:425791fe4b42


   214

        case L2CAP_CONN_REQ:





jksoft

0:425791fe4b42


   215

            break;





jksoft

0:425791fe4b42


   216







jksoft

0:425791fe4b42


   217

        // Response to our initial connect from Remote





jksoft

0:425791fe4b42


   218

        case L2CAP_CONN_RSP:





jksoft

0:425791fe4b42


   219

            {





jksoft

0:425791fe4b42


   220

                if (result == 0)





jksoft

0:425791fe4b42


   221

                {





jksoft

0:425791fe4b42


   222

                    int dcid = LE16(data+12);





jksoft

0:425791fe4b42


   223

                    int scid = LE16(data+14);





jksoft

0:425791fe4b42


   224

                    L2CAPSocket* s = SCIDToSocket(scid);





jksoft

0:425791fe4b42


   225

                    if (s)





jksoft

0:425791fe4b42


   226

                    {





jksoft

0:425791fe4b42


   227

                        s->dcid = dcid;





jksoft

0:425791fe4b42


   228

                        ConfigureRequest(dcid);





jksoft

0:425791fe4b42


   229

                    }





jksoft

0:425791fe4b42


   230

                } else





jksoft

0:425791fe4b42


   231

                    printf("Connect failed?\n");





jksoft

0:425791fe4b42


   232

            }





jksoft

0:425791fe4b42


   233

            break;





jksoft

0:425791fe4b42


   234







jksoft

0:425791fe4b42


   235

        case L2CAP_CONF_RSP:





jksoft

0:425791fe4b42


   236

            {





jksoft

0:425791fe4b42


   237

                int scid = LE16(data+12);





jksoft

0:425791fe4b42


   238

                SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);





jksoft

0:425791fe4b42


   239

                if (s)





jksoft

0:425791fe4b42


   240

                    s->SetState(SocketState_Open);





jksoft

0:425791fe4b42


   241

            }





jksoft

0:425791fe4b42


   242

            break;





jksoft

0:425791fe4b42


   243







jksoft

0:425791fe4b42


   244

        case L2CAP_CONF_REQ:





jksoft

0:425791fe4b42


   245

            {





jksoft

0:425791fe4b42


   246

                int scid = LE16(data+12);





jksoft

0:425791fe4b42


   247

                L2CAPSocket* s = SCIDToSocket(scid);





jksoft

0:425791fe4b42


   248

                if (s)





jksoft

0:425791fe4b42


   249

                    ConfigureResponse(data[9],s->dcid);





jksoft

0:425791fe4b42


   250

            }





jksoft

0:425791fe4b42


   251

            break;





jksoft

0:425791fe4b42


   252

    }





jksoft

0:425791fe4b42


   253

}





jksoft

0:425791fe4b42


   254







jksoft

0:425791fe4b42


   255

void BTDevice::ACLRecv(const u8* data, int len)





jksoft

0:425791fe4b42


   256

{





jksoft

0:425791fe4b42


   257

   // printfBytes("L2CP",data,16);





jksoft

0:425791fe4b42


   258

    int handle = LE16(data);





jksoft

0:425791fe4b42


   259

    if (handle != (0x2000 | _handle))





jksoft

0:425791fe4b42


   260

        return;





jksoft

0:425791fe4b42


   261







jksoft

0:425791fe4b42


   262

    int cid = LE16(data+6);





jksoft

0:425791fe4b42


   263

    if (cid == 1)





jksoft

0:425791fe4b42


   264

    {





jksoft

0:425791fe4b42


   265

        Control(data,len);





jksoft

0:425791fe4b42


   266

        return;





jksoft

0:425791fe4b42


   267

    }





jksoft

0:425791fe4b42


   268







jksoft

0:425791fe4b42


   269

    SocketInternal* s = (SocketInternal*)SCIDToSocket(cid);





jksoft

0:425791fe4b42


   270

    if (s)





jksoft

0:425791fe4b42


   271

        s->Recv(data+8,LE16(data+2)-4);





jksoft

0:425791fe4b42


   272

    else





jksoft

0:425791fe4b42


   273

        printf("Bad event cid %d\n",cid);





jksoft

0:425791fe4b42


   274

}