Andrew Schaeffer / Mbed 2 deprecated OU_Davis_Old_Robot

This is the project for the Old Model Robots for OU's Dr. Davis's Configurable Robots Research. This is being published so future robots can be set up easily.

Dependencies:   FatFileSystem MCP3008 Motor PinDetect QTR_8A SRF05 SSD1308_128x64_I2C mbed

blueusb/L2CAP.cpp@0:bcad524c1856, 2017-11-01 (annotated)

Committer:
DrewSchaef
Date:
Wed Nov 01 15:57:59 2017 +0000
Revision:
0:bcad524c1856
Published the project to allow access for future work on the Configurable Robots Research Project(s).

Who changed what in which revision?

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