Junichi Katsu / Mbed 2 deprecated WallbotR4

Test Ver

Dependencies:   mbed FatFileSystem

BlueUSB/L2CAP.cpp@0:269589d8d2c2, 2012-11-17 (annotated)

Committer:
jksoft
Date:
Sat Nov 17 13:22:00 2012 +0000
Revision:
0:269589d8d2c2
Test Program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jksoft 0:269589d8d2c2 1 /*
jksoft 0:269589d8d2c2 2 Copyright (c) 2010 Peter Barrett
jksoft 0:269589d8d2c2 3
jksoft 0:269589d8d2c2 4 Permission is hereby granted, free of charge, to any person obtaining a copy
jksoft 0:269589d8d2c2 5 of this software and associated documentation files (the "Software"), to deal
jksoft 0:269589d8d2c2 6 in the Software without restriction, including without limitation the rights
jksoft 0:269589d8d2c2 7 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
jksoft 0:269589d8d2c2 8 copies of the Software, and to permit persons to whom the Software is
jksoft 0:269589d8d2c2 9 furnished to do so, subject to the following conditions:
jksoft 0:269589d8d2c2 10
jksoft 0:269589d8d2c2 11 The above copyright notice and this permission notice shall be included in
jksoft 0:269589d8d2c2 12 all copies or substantial portions of the Software.
jksoft 0:269589d8d2c2 13
jksoft 0:269589d8d2c2 14 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
jksoft 0:269589d8d2c2 15 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
jksoft 0:269589d8d2c2 16 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
jksoft 0:269589d8d2c2 17 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
jksoft 0:269589d8d2c2 18 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
jksoft 0:269589d8d2c2 19 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
jksoft 0:269589d8d2c2 20 THE SOFTWARE.
jksoft 0:269589d8d2c2 21 */
jksoft 0:269589d8d2c2 22
jksoft 0:269589d8d2c2 23
jksoft 0:269589d8d2c2 24 #include <stdio.h>
jksoft 0:269589d8d2c2 25 #include <stdlib.h>
jksoft 0:269589d8d2c2 26 #include <stdio.h>
jksoft 0:269589d8d2c2 27 #include <string.h>
jksoft 0:269589d8d2c2 28
jksoft 0:269589d8d2c2 29 #include "Utils.h"
jksoft 0:269589d8d2c2 30 #include "hci.h"
jksoft 0:269589d8d2c2 31
jksoft 0:269589d8d2c2 32 #define L2CAP_COMMAND_REJ 0x01
jksoft 0:269589d8d2c2 33 #define L2CAP_CONN_REQ 0x02
jksoft 0:269589d8d2c2 34 #define L2CAP_CONN_RSP 0x03
jksoft 0:269589d8d2c2 35 #define L2CAP_CONF_REQ 0x04
jksoft 0:269589d8d2c2 36 #define L2CAP_CONF_RSP 0x05
jksoft 0:269589d8d2c2 37 #define L2CAP_DISCONN_REQ 0x06
jksoft 0:269589d8d2c2 38 #define L2CAP_DISCONN_RSP 0x07
jksoft 0:269589d8d2c2 39 #define L2CAP_ECHO_REQ 0x08
jksoft 0:269589d8d2c2 40 #define L2CAP_ECHO_RSP 0x09
jksoft 0:269589d8d2c2 41 #define L2CAP_INFO_REQ 0x0a
jksoft 0:269589d8d2c2 42 #define L2CAP_INFO_RSP 0x0b
jksoft 0:269589d8d2c2 43
jksoft 0:269589d8d2c2 44
jksoft 0:269589d8d2c2 45 /* L2CAP command codes */
jksoft 0:269589d8d2c2 46 const char* L2CAP_ComandCodeStr(int c)
jksoft 0:269589d8d2c2 47 {
jksoft 0:269589d8d2c2 48 switch (c)
jksoft 0:269589d8d2c2 49 {
jksoft 0:269589d8d2c2 50 case L2CAP_COMMAND_REJ: return "L2CAP_COMMAND_REJ";
jksoft 0:269589d8d2c2 51 case L2CAP_CONN_REQ: return "L2CAP_CONN_REQ";
jksoft 0:269589d8d2c2 52 case L2CAP_CONN_RSP: return "L2CAP_CONN_RSP";
jksoft 0:269589d8d2c2 53 case L2CAP_CONF_REQ: return "L2CAP_CONF_REQ";
jksoft 0:269589d8d2c2 54 case L2CAP_CONF_RSP: return "L2CAP_CONF_RSP";
jksoft 0:269589d8d2c2 55 case L2CAP_DISCONN_REQ: return "L2CAP_DISCONN_REQ";
jksoft 0:269589d8d2c2 56 case L2CAP_DISCONN_RSP: return "L2CAP_DISCONN_RSP";
jksoft 0:269589d8d2c2 57 case L2CAP_ECHO_REQ: return "L2CAP_ECHO_REQ";
jksoft 0:269589d8d2c2 58 case L2CAP_ECHO_RSP: return "L2CAP_ECHO_RSP";
jksoft 0:269589d8d2c2 59 case L2CAP_INFO_REQ: return "L2CAP_INFO_REQ";
jksoft 0:269589d8d2c2 60 case L2CAP_INFO_RSP: return "L2CAP_INFO_RSP";
jksoft 0:269589d8d2c2 61 }
jksoft 0:269589d8d2c2 62 return "unknown";
jksoft 0:269589d8d2c2 63 }
jksoft 0:269589d8d2c2 64
jksoft 0:269589d8d2c2 65 typedef struct
jksoft 0:269589d8d2c2 66 {
jksoft 0:269589d8d2c2 67 u16 handle;
jksoft 0:269589d8d2c2 68 u16 length; // total
jksoft 0:269589d8d2c2 69 u16 l2capLength; // length -4
jksoft 0:269589d8d2c2 70 u16 cid; // Signaling packet CID = 1
jksoft 0:269589d8d2c2 71 u8 data[64]; // Largest thing to send!!! todo
jksoft 0:269589d8d2c2 72 } L2CAPData;
jksoft 0:269589d8d2c2 73
jksoft 0:269589d8d2c2 74 typedef struct
jksoft 0:269589d8d2c2 75 {
jksoft 0:269589d8d2c2 76 u16 handle;
jksoft 0:269589d8d2c2 77 u16 length; // total
jksoft 0:269589d8d2c2 78 u16 l2capLength; // length -4
jksoft 0:269589d8d2c2 79 u16 cid; // Signaling packet CID = 1
jksoft 0:269589d8d2c2 80
jksoft 0:269589d8d2c2 81 // Payload
jksoft 0:269589d8d2c2 82 u8 cmd; //
jksoft 0:269589d8d2c2 83 u8 id;
jksoft 0:269589d8d2c2 84 u16 cmdLength; // total-8
jksoft 0:269589d8d2c2 85 u16 params[4]; // Params
jksoft 0:269589d8d2c2 86 } L2CAPCmd;
jksoft 0:269589d8d2c2 87
jksoft 0:269589d8d2c2 88 //
jksoft 0:269589d8d2c2 89 void BTDevice::Init()
jksoft 0:269589d8d2c2 90 {
jksoft 0:269589d8d2c2 91 memset(&_info,0,sizeof(inquiry_info));
jksoft 0:269589d8d2c2 92 _handle = 0;
jksoft 0:269589d8d2c2 93 _name[0] = 0;
jksoft 0:269589d8d2c2 94 _state = 0;
jksoft 0:269589d8d2c2 95 }
jksoft 0:269589d8d2c2 96
jksoft 0:269589d8d2c2 97 // virtual SocketHandler
jksoft 0:269589d8d2c2 98 int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr)
jksoft 0:269589d8d2c2 99 {
jksoft 0:269589d8d2c2 100 L2CAPSocket* s = (L2CAPSocket*)sock;
jksoft 0:269589d8d2c2 101 L2CAPAddr* a = (L2CAPAddr*)addr;
jksoft 0:269589d8d2c2 102 s->scid = 0x40 + sock->ID-1; // are these reserved?
jksoft 0:269589d8d2c2 103 s->dcid = 0;
jksoft 0:269589d8d2c2 104 Connect(s->scid,a->psm);
jksoft 0:269589d8d2c2 105 return sock->ID;
jksoft 0:269589d8d2c2 106 }
jksoft 0:269589d8d2c2 107
jksoft 0:269589d8d2c2 108 // virtual SocketHandler
jksoft 0:269589d8d2c2 109 int BTDevice::Send(SocketInternal* sock, const u8* data, int len)
jksoft 0:269589d8d2c2 110 {
jksoft 0:269589d8d2c2 111 L2CAPData d;
jksoft 0:269589d8d2c2 112 L2CAPSocket* s = (L2CAPSocket*)sock;
jksoft 0:269589d8d2c2 113
jksoft 0:269589d8d2c2 114 d.handle = _handle | 0x2000;
jksoft 0:269589d8d2c2 115 d.length = 4 + len;
jksoft 0:269589d8d2c2 116 d.l2capLength = len;
jksoft 0:269589d8d2c2 117 d.cid = s->dcid;
jksoft 0:269589d8d2c2 118
jksoft 0:269589d8d2c2 119 if (len > 64)
jksoft 0:269589d8d2c2 120 return -1;
jksoft 0:269589d8d2c2 121 memcpy(d.data,data,len);
jksoft 0:269589d8d2c2 122 return Send((u8*)&d,len+8);
jksoft 0:269589d8d2c2 123 }
jksoft 0:269589d8d2c2 124
jksoft 0:269589d8d2c2 125 // virtual SocketHandler
jksoft 0:269589d8d2c2 126 int BTDevice::Close(SocketInternal* sock)
jksoft 0:269589d8d2c2 127 {
jksoft 0:269589d8d2c2 128 printf("L2CAP close %d\n",sock->ID);
jksoft 0:269589d8d2c2 129 L2CAPSocket* s = (L2CAPSocket*)sock;
jksoft 0:269589d8d2c2 130 return Disconnect(s->scid,s->dcid);
jksoft 0:269589d8d2c2 131 }
jksoft 0:269589d8d2c2 132
jksoft 0:269589d8d2c2 133 L2CAPSocket* BTDevice::SCIDToSocket(int scid)
jksoft 0:269589d8d2c2 134 {
jksoft 0:269589d8d2c2 135 return (L2CAPSocket*)GetSocketInternal(scid-0x40+1);
jksoft 0:269589d8d2c2 136 }
jksoft 0:269589d8d2c2 137
jksoft 0:269589d8d2c2 138 int BTDevice::Send(const u8* data, int len)
jksoft 0:269589d8d2c2 139 {
jksoft 0:269589d8d2c2 140 _transport->ACLSend(data,len);
jksoft 0:269589d8d2c2 141 return 0;
jksoft 0:269589d8d2c2 142 }
jksoft 0:269589d8d2c2 143
jksoft 0:269589d8d2c2 144 int BTDevice::Send(u8 c, u8 id, u16* params, int count)
jksoft 0:269589d8d2c2 145 {
jksoft 0:269589d8d2c2 146 L2CAPCmd cmd;
jksoft 0:269589d8d2c2 147 cmd.handle = _handle | 0x2000;
jksoft 0:269589d8d2c2 148 cmd.length = 8 + count*2;
jksoft 0:269589d8d2c2 149
jksoft 0:269589d8d2c2 150 cmd.l2capLength = cmd.length-4;
jksoft 0:269589d8d2c2 151 cmd.cid = 1; // Signaling packet
jksoft 0:269589d8d2c2 152
jksoft 0:269589d8d2c2 153 cmd.cmd = c;
jksoft 0:269589d8d2c2 154 cmd.id = id;
jksoft 0:269589d8d2c2 155 cmd.cmdLength = count*2;
jksoft 0:269589d8d2c2 156 for (int i = 0; i < count; i++)
jksoft 0:269589d8d2c2 157 cmd.params[i] = params[i];
jksoft 0:269589d8d2c2 158 return Send((u8*)&cmd,cmd.length+4);
jksoft 0:269589d8d2c2 159 }
jksoft 0:269589d8d2c2 160
jksoft 0:269589d8d2c2 161 int BTDevice::Connect(int scid, int psm)
jksoft 0:269589d8d2c2 162 {
jksoft 0:269589d8d2c2 163 u16 p[2];
jksoft 0:269589d8d2c2 164 p[0] = psm;
jksoft 0:269589d8d2c2 165 p[1] = scid;
jksoft 0:269589d8d2c2 166 return Send(L2CAP_CONN_REQ,_txid++,p,2);
jksoft 0:269589d8d2c2 167 }
jksoft 0:269589d8d2c2 168
jksoft 0:269589d8d2c2 169 int BTDevice::Disconnect(int scid, int dcid)
jksoft 0:269589d8d2c2 170 {
jksoft 0:269589d8d2c2 171 u16 p[2];
jksoft 0:269589d8d2c2 172 p[0] = dcid;
jksoft 0:269589d8d2c2 173 p[1] = scid;
jksoft 0:269589d8d2c2 174 return Send(L2CAP_DISCONN_REQ,_txid++,p,2);
jksoft 0:269589d8d2c2 175 }
jksoft 0:269589d8d2c2 176
jksoft 0:269589d8d2c2 177 int BTDevice::ConfigureRequest(int dcid)
jksoft 0:269589d8d2c2 178 {
jksoft 0:269589d8d2c2 179 u16 p[4];
jksoft 0:269589d8d2c2 180 p[0] = dcid;
jksoft 0:269589d8d2c2 181 p[1] = 0;
jksoft 0:269589d8d2c2 182 p[2] = 0x0201; // Options
jksoft 0:269589d8d2c2 183 p[3] = 0x02A0; // 672
jksoft 0:269589d8d2c2 184 return Send(L2CAP_CONF_REQ,_txid++,p,4);
jksoft 0:269589d8d2c2 185 }
jksoft 0:269589d8d2c2 186
jksoft 0:269589d8d2c2 187 int BTDevice::ConfigureResponse(u8 rxid, int dcid)
jksoft 0:269589d8d2c2 188 {
jksoft 0:269589d8d2c2 189 u16 p[3];
jksoft 0:269589d8d2c2 190 p[0] = dcid;
jksoft 0:269589d8d2c2 191 p[1] = 0;
jksoft 0:269589d8d2c2 192 p[2] = 0;
jksoft 0:269589d8d2c2 193 return Send(L2CAP_CONF_RSP,rxid,p,3);
jksoft 0:269589d8d2c2 194 }
jksoft 0:269589d8d2c2 195
jksoft 0:269589d8d2c2 196 int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid)
jksoft 0:269589d8d2c2 197 {
jksoft 0:269589d8d2c2 198 u16 p[2];
jksoft 0:269589d8d2c2 199 p[0] = dcid;
jksoft 0:269589d8d2c2 200 p[1] = scid;
jksoft 0:269589d8d2c2 201 return Send(L2CAP_DISCONN_RSP,rxid,p,2);
jksoft 0:269589d8d2c2 202 }
jksoft 0:269589d8d2c2 203
jksoft 0:269589d8d2c2 204 void BTDevice::Control(const u8* data, int len)
jksoft 0:269589d8d2c2 205 {
jksoft 0:269589d8d2c2 206 int cc = data[8];
jksoft 0:269589d8d2c2 207 printf(L2CAP_ComandCodeStr(cc));
jksoft 0:269589d8d2c2 208 int result = LE16(data+16);
jksoft 0:269589d8d2c2 209 printf(" Result %d\n",result);
jksoft 0:269589d8d2c2 210 switch (cc)
jksoft 0:269589d8d2c2 211 {
jksoft 0:269589d8d2c2 212 case L2CAP_COMMAND_REJ:
jksoft 0:269589d8d2c2 213 break;
jksoft 0:269589d8d2c2 214 case L2CAP_CONN_REQ:
jksoft 0:269589d8d2c2 215 break;
jksoft 0:269589d8d2c2 216
jksoft 0:269589d8d2c2 217 // Response to our initial connect from Remote
jksoft 0:269589d8d2c2 218 case L2CAP_CONN_RSP:
jksoft 0:269589d8d2c2 219 {
jksoft 0:269589d8d2c2 220 if (result == 0)
jksoft 0:269589d8d2c2 221 {
jksoft 0:269589d8d2c2 222 int dcid = LE16(data+12);
jksoft 0:269589d8d2c2 223 int scid = LE16(data+14);
jksoft 0:269589d8d2c2 224 L2CAPSocket* s = SCIDToSocket(scid);
jksoft 0:269589d8d2c2 225 if (s)
jksoft 0:269589d8d2c2 226 {
jksoft 0:269589d8d2c2 227 s->dcid = dcid;
jksoft 0:269589d8d2c2 228 ConfigureRequest(dcid);
jksoft 0:269589d8d2c2 229 }
jksoft 0:269589d8d2c2 230 } else
jksoft 0:269589d8d2c2 231 printf("Connect failed?\n");
jksoft 0:269589d8d2c2 232 }
jksoft 0:269589d8d2c2 233 break;
jksoft 0:269589d8d2c2 234
jksoft 0:269589d8d2c2 235 case L2CAP_CONF_RSP:
jksoft 0:269589d8d2c2 236 {
jksoft 0:269589d8d2c2 237 int scid = LE16(data+12);
jksoft 0:269589d8d2c2 238 SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
jksoft 0:269589d8d2c2 239 if (s)
jksoft 0:269589d8d2c2 240 s->SetState(SocketState_Open);
jksoft 0:269589d8d2c2 241 }
jksoft 0:269589d8d2c2 242 break;
jksoft 0:269589d8d2c2 243
jksoft 0:269589d8d2c2 244 case L2CAP_CONF_REQ:
jksoft 0:269589d8d2c2 245 {
jksoft 0:269589d8d2c2 246 int scid = LE16(data+12);
jksoft 0:269589d8d2c2 247 L2CAPSocket* s = SCIDToSocket(scid);
jksoft 0:269589d8d2c2 248 if (s)
jksoft 0:269589d8d2c2 249 ConfigureResponse(data[9],s->dcid);
jksoft 0:269589d8d2c2 250 }
jksoft 0:269589d8d2c2 251 break;
jksoft 0:269589d8d2c2 252 }
jksoft 0:269589d8d2c2 253 }
jksoft 0:269589d8d2c2 254
jksoft 0:269589d8d2c2 255 void BTDevice::ACLRecv(const u8* data, int len)
jksoft 0:269589d8d2c2 256 {
jksoft 0:269589d8d2c2 257 // printfBytes("L2CP",data,16);
jksoft 0:269589d8d2c2 258 int handle = LE16(data);
jksoft 0:269589d8d2c2 259 if (handle != (0x2000 | _handle))
jksoft 0:269589d8d2c2 260 return;
jksoft 0:269589d8d2c2 261
jksoft 0:269589d8d2c2 262 int cid = LE16(data+6);
jksoft 0:269589d8d2c2 263 if (cid == 1)
jksoft 0:269589d8d2c2 264 {
jksoft 0:269589d8d2c2 265 Control(data,len);
jksoft 0:269589d8d2c2 266 return;
jksoft 0:269589d8d2c2 267 }
jksoft 0:269589d8d2c2 268
jksoft 0:269589d8d2c2 269 SocketInternal* s = (SocketInternal*)SCIDToSocket(cid);
jksoft 0:269589d8d2c2 270 if (s)
jksoft 0:269589d8d2c2 271 s->Recv(data+8,LE16(data+2)-4);
jksoft 0:269589d8d2c2 272 else
jksoft 0:269589d8d2c2 273 printf("Bad event cid %d\n",cid);
jksoft 0:269589d8d2c2 274 }