hokuto Abe / Mbed 2 deprecated nisiBlutooth

L2CAP.cpp@0:db1ba09e8bfa, 2012-07-10 (annotated)

Committer:
hosei2
Date:
Tue Jul 10 11:36:47 2012 +0000
Revision:
0:db1ba09e8bfa
zzz

Who changed what in which revision?

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