Test version of BlueUSB stack. Includes SDP and RFCOMM. As Client it allows to connect to my fischertechnik TX Controller. As Server it echo\\\\\\\'s characters to Putty. PIN=1234
Dependencies: mbed myUSBHost AvailableMemory
Dependents: mbed_TANK_Kinect myBlueUSB_ros ftusbClass
L2CAP.cpp@0:81ed8b6e4a8b, 2011-04-04 (annotated)
- Committer:
- networker
- Date:
- Mon Apr 04 16:41:03 2011 +0000
- Revision:
- 0:81ed8b6e4a8b
initial revision
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
networker | 0:81ed8b6e4a8b | 1 | /* |
networker | 0:81ed8b6e4a8b | 2 | Copyright (c) 2010 Peter Barrett |
networker | 0:81ed8b6e4a8b | 3 | |
networker | 0:81ed8b6e4a8b | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy |
networker | 0:81ed8b6e4a8b | 5 | of this software and associated documentation files (the "Software"), to deal |
networker | 0:81ed8b6e4a8b | 6 | in the Software without restriction, including without limitation the rights |
networker | 0:81ed8b6e4a8b | 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
networker | 0:81ed8b6e4a8b | 8 | copies of the Software, and to permit persons to whom the Software is |
networker | 0:81ed8b6e4a8b | 9 | furnished to do so, subject to the following conditions: |
networker | 0:81ed8b6e4a8b | 10 | |
networker | 0:81ed8b6e4a8b | 11 | The above copyright notice and this permission notice shall be included in |
networker | 0:81ed8b6e4a8b | 12 | all copies or substantial portions of the Software. |
networker | 0:81ed8b6e4a8b | 13 | |
networker | 0:81ed8b6e4a8b | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
networker | 0:81ed8b6e4a8b | 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
networker | 0:81ed8b6e4a8b | 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
networker | 0:81ed8b6e4a8b | 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
networker | 0:81ed8b6e4a8b | 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
networker | 0:81ed8b6e4a8b | 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
networker | 0:81ed8b6e4a8b | 20 | THE SOFTWARE. |
networker | 0:81ed8b6e4a8b | 21 | */ |
networker | 0:81ed8b6e4a8b | 22 | |
networker | 0:81ed8b6e4a8b | 23 | |
networker | 0:81ed8b6e4a8b | 24 | #include <stdio.h> |
networker | 0:81ed8b6e4a8b | 25 | #include <stdlib.h> |
networker | 0:81ed8b6e4a8b | 26 | #include <stdio.h> |
networker | 0:81ed8b6e4a8b | 27 | #include <string.h> |
networker | 0:81ed8b6e4a8b | 28 | |
networker | 0:81ed8b6e4a8b | 29 | #include "Utils.h" |
networker | 0:81ed8b6e4a8b | 30 | #include "hci.h" |
networker | 0:81ed8b6e4a8b | 31 | #include "HCITransportUSB.h" |
networker | 0:81ed8b6e4a8b | 32 | |
networker | 0:81ed8b6e4a8b | 33 | #define L2CAP_COMMAND_REJ 0x01 |
networker | 0:81ed8b6e4a8b | 34 | #define L2CAP_CONN_REQ 0x02 |
networker | 0:81ed8b6e4a8b | 35 | #define L2CAP_CONN_RSP 0x03 |
networker | 0:81ed8b6e4a8b | 36 | #define L2CAP_CONF_REQ 0x04 |
networker | 0:81ed8b6e4a8b | 37 | #define L2CAP_CONF_RSP 0x05 |
networker | 0:81ed8b6e4a8b | 38 | #define L2CAP_DISCONN_REQ 0x06 |
networker | 0:81ed8b6e4a8b | 39 | #define L2CAP_DISCONN_RSP 0x07 |
networker | 0:81ed8b6e4a8b | 40 | #define L2CAP_ECHO_REQ 0x08 |
networker | 0:81ed8b6e4a8b | 41 | #define L2CAP_ECHO_RSP 0x09 |
networker | 0:81ed8b6e4a8b | 42 | #define L2CAP_INFO_REQ 0x0a |
networker | 0:81ed8b6e4a8b | 43 | #define L2CAP_INFO_RSP 0x0b |
networker | 0:81ed8b6e4a8b | 44 | |
networker | 0:81ed8b6e4a8b | 45 | //template <class T> T min(T a, T b) { return a<b ? a : b;} |
networker | 0:81ed8b6e4a8b | 46 | |
networker | 0:81ed8b6e4a8b | 47 | /* L2CAP command codes */ |
networker | 0:81ed8b6e4a8b | 48 | const char* L2CAP_ComandCodeStr(int c) { |
networker | 0:81ed8b6e4a8b | 49 | switch (c) { |
networker | 0:81ed8b6e4a8b | 50 | case L2CAP_COMMAND_REJ: |
networker | 0:81ed8b6e4a8b | 51 | return "L2CAP_COMMAND_REJ"; |
networker | 0:81ed8b6e4a8b | 52 | case L2CAP_CONN_REQ: |
networker | 0:81ed8b6e4a8b | 53 | return "L2CAP_CONN_REQ"; |
networker | 0:81ed8b6e4a8b | 54 | case L2CAP_CONN_RSP: |
networker | 0:81ed8b6e4a8b | 55 | return "L2CAP_CONN_RSP"; |
networker | 0:81ed8b6e4a8b | 56 | case L2CAP_CONF_REQ: |
networker | 0:81ed8b6e4a8b | 57 | return "L2CAP_CONF_REQ"; |
networker | 0:81ed8b6e4a8b | 58 | case L2CAP_CONF_RSP: |
networker | 0:81ed8b6e4a8b | 59 | return "L2CAP_CONF_RSP"; |
networker | 0:81ed8b6e4a8b | 60 | case L2CAP_DISCONN_REQ: |
networker | 0:81ed8b6e4a8b | 61 | return "L2CAP_DISCONN_REQ"; |
networker | 0:81ed8b6e4a8b | 62 | case L2CAP_DISCONN_RSP: |
networker | 0:81ed8b6e4a8b | 63 | return "L2CAP_DISCONN_RSP"; |
networker | 0:81ed8b6e4a8b | 64 | case L2CAP_ECHO_REQ: |
networker | 0:81ed8b6e4a8b | 65 | return "L2CAP_ECHO_REQ"; |
networker | 0:81ed8b6e4a8b | 66 | case L2CAP_ECHO_RSP: |
networker | 0:81ed8b6e4a8b | 67 | return "L2CAP_ECHO_RSP"; |
networker | 0:81ed8b6e4a8b | 68 | case L2CAP_INFO_REQ: |
networker | 0:81ed8b6e4a8b | 69 | return "L2CAP_INFO_REQ"; |
networker | 0:81ed8b6e4a8b | 70 | case L2CAP_INFO_RSP: |
networker | 0:81ed8b6e4a8b | 71 | return "L2CAP_INFO_RSP"; |
networker | 0:81ed8b6e4a8b | 72 | } |
networker | 0:81ed8b6e4a8b | 73 | return "unknown"; |
networker | 0:81ed8b6e4a8b | 74 | } |
networker | 0:81ed8b6e4a8b | 75 | |
networker | 0:81ed8b6e4a8b | 76 | #define OFFSET 8 //means the buffer also has space for the l2cap/hci headers and need not be allocated and copied |
networker | 0:81ed8b6e4a8b | 77 | //#define OFFSET 0 //means the buffer only has space for the payload which need to be copied |
networker | 0:81ed8b6e4a8b | 78 | #if OFFSET == 0 |
networker | 0:81ed8b6e4a8b | 79 | #define L2CAPBUFSIZE 128 |
networker | 0:81ed8b6e4a8b | 80 | #else |
networker | 0:81ed8b6e4a8b | 81 | #define L2CAPBUFSIZE 0 |
networker | 0:81ed8b6e4a8b | 82 | #endif |
networker | 0:81ed8b6e4a8b | 83 | |
networker | 0:81ed8b6e4a8b | 84 | typedef struct { |
networker | 0:81ed8b6e4a8b | 85 | u16 handle; |
networker | 0:81ed8b6e4a8b | 86 | u16 length; // total |
networker | 0:81ed8b6e4a8b | 87 | u16 l2capLength; // length -4 |
networker | 0:81ed8b6e4a8b | 88 | u16 cid; // Signaling packet CID = 1 |
networker | 0:81ed8b6e4a8b | 89 | u8 data[L2CAPBUFSIZE]; // Largest thing to send!!! todo |
networker | 0:81ed8b6e4a8b | 90 | } L2CAPData; |
networker | 0:81ed8b6e4a8b | 91 | |
networker | 0:81ed8b6e4a8b | 92 | // |
networker | 0:81ed8b6e4a8b | 93 | void BTDevice::Init() { |
networker | 0:81ed8b6e4a8b | 94 | memset(&_info,0,sizeof(inquiry_info)); |
networker | 0:81ed8b6e4a8b | 95 | _handle = 0; |
networker | 0:81ed8b6e4a8b | 96 | _name[0] = 0; |
networker | 0:81ed8b6e4a8b | 97 | _state = 0; |
networker | 0:81ed8b6e4a8b | 98 | } |
networker | 0:81ed8b6e4a8b | 99 | |
networker | 0:81ed8b6e4a8b | 100 | // virtual SocketHandler |
networker | 0:81ed8b6e4a8b | 101 | int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr) { |
networker | 0:81ed8b6e4a8b | 102 | L2CAPSocket* s = (L2CAPSocket*)sock; |
networker | 0:81ed8b6e4a8b | 103 | L2CAPAddr* a = (L2CAPAddr*)addr; |
networker | 0:81ed8b6e4a8b | 104 | s->scid = 0x40 + sock->ID-1; // are these reserved? |
networker | 0:81ed8b6e4a8b | 105 | s->dcid = 0; |
networker | 0:81ed8b6e4a8b | 106 | Connect(s->scid,a->psm); |
networker | 0:81ed8b6e4a8b | 107 | sock->State = SocketState_L2CAP_WaitConnectRsp; |
networker | 0:81ed8b6e4a8b | 108 | contState = 0; |
networker | 0:81ed8b6e4a8b | 109 | return sock->ID; |
networker | 0:81ed8b6e4a8b | 110 | } |
networker | 0:81ed8b6e4a8b | 111 | |
networker | 0:81ed8b6e4a8b | 112 | // virtual SocketHandler, called from HCI which is ABOVE L2CAP |
networker | 0:81ed8b6e4a8b | 113 | int BTDevice::Send(SocketInternal* sock, const u8* data, int len) { |
networker | 0:81ed8b6e4a8b | 114 | L2CAPSocket* s = (L2CAPSocket*)sock; |
networker | 0:81ed8b6e4a8b | 115 | #if OFFSET == 8 //sizeof L2CAPData header |
networker | 0:81ed8b6e4a8b | 116 | L2CAPData &d = *const_cast<L2CAPData*>((L2CAPData*)data); |
networker | 0:81ed8b6e4a8b | 117 | #else |
networker | 0:81ed8b6e4a8b | 118 | L2CAPData d; |
networker | 0:81ed8b6e4a8b | 119 | #endif |
networker | 0:81ed8b6e4a8b | 120 | if (len > peer_mtu) {//mtu concerns the l2cap mtu, because we use basic mode we cannot segment |
networker | 0:81ed8b6e4a8b | 121 | printf("MTU (%d) for outgoing packet (%d) exceeded\n", peer_mtu, len); |
networker | 0:81ed8b6e4a8b | 122 | return 0; |
networker | 0:81ed8b6e4a8b | 123 | } |
networker | 0:81ed8b6e4a8b | 124 | d.handle = _handle | 0x2000; |
networker | 0:81ed8b6e4a8b | 125 | d.length = 4 + len - OFFSET; |
networker | 0:81ed8b6e4a8b | 126 | d.l2capLength = len - OFFSET; |
networker | 0:81ed8b6e4a8b | 127 | d.cid = s->dcid; |
networker | 0:81ed8b6e4a8b | 128 | printf("cid=%d: ", d.cid); |
networker | 0:81ed8b6e4a8b | 129 | printfBytes("sending: ", data, len); |
networker | 0:81ed8b6e4a8b | 130 | |
networker | 0:81ed8b6e4a8b | 131 | #if OFFSET == 0 |
networker | 0:81ed8b6e4a8b | 132 | if (len > L2CAPBUFSIZE) |
networker | 0:81ed8b6e4a8b | 133 | return -1; |
networker | 0:81ed8b6e4a8b | 134 | memcpy(d.data,data,len); |
networker | 0:81ed8b6e4a8b | 135 | return Send((u8*)&d,len+8); |
networker | 0:81ed8b6e4a8b | 136 | #else |
networker | 0:81ed8b6e4a8b | 137 | return Send(data, len); |
networker | 0:81ed8b6e4a8b | 138 | #endif |
networker | 0:81ed8b6e4a8b | 139 | } |
networker | 0:81ed8b6e4a8b | 140 | |
networker | 0:81ed8b6e4a8b | 141 | // virtual SocketHandler |
networker | 0:81ed8b6e4a8b | 142 | int BTDevice::Close(SocketInternal* sock) { |
networker | 0:81ed8b6e4a8b | 143 | printf("L2CAP close %d\n",sock->ID); |
networker | 0:81ed8b6e4a8b | 144 | sock->State = SocketState_L2CAP_WaitDisconnect; |
networker | 0:81ed8b6e4a8b | 145 | L2CAPSocket* s = (L2CAPSocket*)sock; |
networker | 0:81ed8b6e4a8b | 146 | return Disconnect(s->scid,s->dcid); |
networker | 0:81ed8b6e4a8b | 147 | } |
networker | 0:81ed8b6e4a8b | 148 | |
networker | 0:81ed8b6e4a8b | 149 | L2CAPSocket* BTDevice::SCIDToSocket(int scid) { |
networker | 0:81ed8b6e4a8b | 150 | return (L2CAPSocket*)GetSocketInternal(scid-0x40+1); |
networker | 0:81ed8b6e4a8b | 151 | } |
networker | 0:81ed8b6e4a8b | 152 | |
networker | 0:81ed8b6e4a8b | 153 | int BTDevice::Send(const u8* data, int len) {//printfBytes("Transport : ", data, len); |
networker | 0:81ed8b6e4a8b | 154 | _transport->ACLSend(data,len); |
networker | 0:81ed8b6e4a8b | 155 | return 0; |
networker | 0:81ed8b6e4a8b | 156 | } |
networker | 0:81ed8b6e4a8b | 157 | |
networker | 0:81ed8b6e4a8b | 158 | void BTDevice::repeat_cmd() { |
networker | 0:81ed8b6e4a8b | 159 | printf("Cmd on handle %#x timed out, resending txid=%d\n", _handle, last_req.id); |
networker | 0:81ed8b6e4a8b | 160 | Send ((u8*)&last_req, last_req.length+4);//danger! interrupt context, Send is not reentrant |
networker | 0:81ed8b6e4a8b | 161 | //optionally set new larger timeout |
networker | 0:81ed8b6e4a8b | 162 | } |
networker | 0:81ed8b6e4a8b | 163 | |
networker | 0:81ed8b6e4a8b | 164 | int BTDevice::Send(u8 c, u8 id, u16* params, int count) { |
networker | 0:81ed8b6e4a8b | 165 | L2CAPCmd cmd; |
networker | 0:81ed8b6e4a8b | 166 | cmd.handle = _handle | 0x2000; |
networker | 0:81ed8b6e4a8b | 167 | cmd.length = 8 + count*2; |
networker | 0:81ed8b6e4a8b | 168 | |
networker | 0:81ed8b6e4a8b | 169 | cmd.l2capLength = cmd.length-4; |
networker | 0:81ed8b6e4a8b | 170 | cmd.cid = 1; // Signaling packet |
networker | 0:81ed8b6e4a8b | 171 | |
networker | 0:81ed8b6e4a8b | 172 | cmd.cmd = c; |
networker | 0:81ed8b6e4a8b | 173 | cmd.id = id; |
networker | 0:81ed8b6e4a8b | 174 | cmd.cmdLength = count*2; |
networker | 0:81ed8b6e4a8b | 175 | for (int i = 0; i < count; i++) |
networker | 0:81ed8b6e4a8b | 176 | cmd.params[i] = params[i]; |
networker | 0:81ed8b6e4a8b | 177 | if ((c & 1) == 0) { //this is a request |
networker | 0:81ed8b6e4a8b | 178 | last_req = cmd; |
networker | 0:81ed8b6e4a8b | 179 | rtx.attach(this, &BTDevice::repeat_cmd, 5.0); |
networker | 0:81ed8b6e4a8b | 180 | printf("Starting timeout for %#x, txid=%d\n", _handle, id); |
networker | 0:81ed8b6e4a8b | 181 | } |
networker | 0:81ed8b6e4a8b | 182 | return Send((u8*)&cmd,cmd.length+4); |
networker | 0:81ed8b6e4a8b | 183 | } |
networker | 0:81ed8b6e4a8b | 184 | |
networker | 0:81ed8b6e4a8b | 185 | int BTDevice::Connect(int scid, int psm) { |
networker | 0:81ed8b6e4a8b | 186 | u16 p[2]; |
networker | 0:81ed8b6e4a8b | 187 | p[0] = psm; |
networker | 0:81ed8b6e4a8b | 188 | p[1] = scid; |
networker | 0:81ed8b6e4a8b | 189 | return Send(L2CAP_CONN_REQ,_txid++,p,2); |
networker | 0:81ed8b6e4a8b | 190 | } |
networker | 0:81ed8b6e4a8b | 191 | |
networker | 0:81ed8b6e4a8b | 192 | int BTDevice::Disconnect(int scid, int dcid) { |
networker | 0:81ed8b6e4a8b | 193 | u16 p[2]; |
networker | 0:81ed8b6e4a8b | 194 | p[0] = dcid; |
networker | 0:81ed8b6e4a8b | 195 | p[1] = scid; |
networker | 0:81ed8b6e4a8b | 196 | return Send(L2CAP_DISCONN_REQ,_txid++,p,2); |
networker | 0:81ed8b6e4a8b | 197 | } |
networker | 0:81ed8b6e4a8b | 198 | |
networker | 0:81ed8b6e4a8b | 199 | int BTDevice::ConfigureRequest(int dcid) { |
networker | 0:81ed8b6e4a8b | 200 | u16 p[4]; |
networker | 0:81ed8b6e4a8b | 201 | p[0] = dcid; |
networker | 0:81ed8b6e4a8b | 202 | p[1] = 0; |
networker | 0:81ed8b6e4a8b | 203 | p[2] = 0x0201; // Options |
networker | 0:81ed8b6e4a8b | 204 | p[3] = min(0x02A0, MAX_ACL_SIZE); // my receiving MTU 672 |
networker | 0:81ed8b6e4a8b | 205 | return Send(L2CAP_CONF_REQ,_txid++,p,4); |
networker | 0:81ed8b6e4a8b | 206 | } |
networker | 0:81ed8b6e4a8b | 207 | |
networker | 0:81ed8b6e4a8b | 208 | int BTDevice::ConfigureResponse(u8 rxid, int dcid) { |
networker | 0:81ed8b6e4a8b | 209 | u16 p[3]; |
networker | 0:81ed8b6e4a8b | 210 | p[0] = dcid; //source cid |
networker | 0:81ed8b6e4a8b | 211 | p[1] = 0; //flags (no continuation) |
networker | 0:81ed8b6e4a8b | 212 | p[2] = 0; //result (success) |
networker | 0:81ed8b6e4a8b | 213 | return Send(L2CAP_CONF_RSP,rxid,p,3); |
networker | 0:81ed8b6e4a8b | 214 | } |
networker | 0:81ed8b6e4a8b | 215 | |
networker | 0:81ed8b6e4a8b | 216 | int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid) { |
networker | 0:81ed8b6e4a8b | 217 | u16 p[2]; |
networker | 0:81ed8b6e4a8b | 218 | p[0] = dcid; |
networker | 0:81ed8b6e4a8b | 219 | p[1] = scid; |
networker | 0:81ed8b6e4a8b | 220 | return Send(L2CAP_DISCONN_RSP,rxid,p,2); |
networker | 0:81ed8b6e4a8b | 221 | } |
networker | 0:81ed8b6e4a8b | 222 | #if 0 |
networker | 0:81ed8b6e4a8b | 223 | //handle16, length16, lengthL2CAP16, cid16, code8, tid8, lengthData16 |
networker | 0:81ed8b6e4a8b | 224 | // 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11 |
networker | 0:81ed8b6e4a8b | 225 | void BTDevice::Control(const u8* data, int len) { //control channel receive |
networker | 0:81ed8b6e4a8b | 226 | printf("\x1B[%dm", 31); |
networker | 0:81ed8b6e4a8b | 227 | int cc = data[8];//command code |
networker | 0:81ed8b6e4a8b | 228 | printf(L2CAP_ComandCodeStr(cc)); |
networker | 0:81ed8b6e4a8b | 229 | //int result = LE16(data+16);//conn_rsp, and conf_resp only |
networker | 0:81ed8b6e4a8b | 230 | //printf(" Result %d\n",result); |
networker | 0:81ed8b6e4a8b | 231 | switch (cc) { |
networker | 0:81ed8b6e4a8b | 232 | case L2CAP_COMMAND_REJ://bad command, eg. MTU, check (reason) |
networker | 0:81ed8b6e4a8b | 233 | printf(" rejection reason=%d\n", LE16(data+12)); |
networker | 0:81ed8b6e4a8b | 234 | break; |
networker | 0:81ed8b6e4a8b | 235 | case L2CAP_CONN_REQ://incoming connection request, not expected but should reply with proper rejection (or accept) |
networker | 0:81ed8b6e4a8b | 236 | //when a connection is accepted a new socket must be opened |
networker | 0:81ed8b6e4a8b | 237 | break; |
networker | 0:81ed8b6e4a8b | 238 | // Response to our initial connect from Remote |
networker | 0:81ed8b6e4a8b | 239 | case L2CAP_CONN_RSP: { |
networker | 0:81ed8b6e4a8b | 240 | int dcid = LE16(data+12); |
networker | 0:81ed8b6e4a8b | 241 | int scid = LE16(data+14); |
networker | 0:81ed8b6e4a8b | 242 | L2CAPSocket* s = SCIDToSocket(scid); |
networker | 0:81ed8b6e4a8b | 243 | int result = LE16(data+16); |
networker | 0:81ed8b6e4a8b | 244 | printf("Result=%d, Status = %d\n", result, LE16(data+18)); |
networker | 0:81ed8b6e4a8b | 245 | if (s->si.State != SocketState_L2CAP_WaitConnectRsp) { |
networker | 0:81ed8b6e4a8b | 246 | printf("Unexpected event ignored\n"); |
networker | 0:81ed8b6e4a8b | 247 | break; |
networker | 0:81ed8b6e4a8b | 248 | } |
networker | 0:81ed8b6e4a8b | 249 | if (result == 0) { |
networker | 0:81ed8b6e4a8b | 250 | if (s) { |
networker | 0:81ed8b6e4a8b | 251 | s->si.State = SocketState_L2CAP_Config_wait; |
networker | 0:81ed8b6e4a8b | 252 | s->dcid = dcid; |
networker | 0:81ed8b6e4a8b | 253 | ConfigureRequest(dcid); |
networker | 0:81ed8b6e4a8b | 254 | s->si.State = SocketState_L2CAP_Config_wait_reqrsp; |
networker | 0:81ed8b6e4a8b | 255 | printf("Sent ConfigureRequest, state=WAIT_CONFIG_REQ-RSP\n"); |
networker | 0:81ed8b6e4a8b | 256 | } |
networker | 0:81ed8b6e4a8b | 257 | } else { |
networker | 0:81ed8b6e4a8b | 258 | s->si.SetState(SocketState_Closed); |
networker | 0:81ed8b6e4a8b | 259 | printf("Connect failed?\n"); |
networker | 0:81ed8b6e4a8b | 260 | } |
networker | 0:81ed8b6e4a8b | 261 | } |
networker | 0:81ed8b6e4a8b | 262 | break; |
networker | 0:81ed8b6e4a8b | 263 | |
networker | 0:81ed8b6e4a8b | 264 | case L2CAP_CONF_RSP: { |
networker | 0:81ed8b6e4a8b | 265 | int result = LE16(data+16); |
networker | 0:81ed8b6e4a8b | 266 | printf("Result=%d, datalen=%d, %smore conf to follow\n", result, LE16(data+10), LE16(data+14)?"":"No "); |
networker | 0:81ed8b6e4a8b | 267 | //should parse the config |
networker | 0:81ed8b6e4a8b | 268 | printfBytes("CONF RSP:", data+8, LE16(data+10)+4); |
networker | 0:81ed8b6e4a8b | 269 | int scid = LE16(data+12); |
networker | 0:81ed8b6e4a8b | 270 | SocketInternal* s = (SocketInternal*)SCIDToSocket(scid); |
networker | 0:81ed8b6e4a8b | 271 | if (s == 0) break; |
networker | 0:81ed8b6e4a8b | 272 | if (s->State != SocketState_L2CAP_Config_wait_reqrsp && s->State != SocketState_L2CAP_Config_wait_rsp) { |
networker | 0:81ed8b6e4a8b | 273 | printf("Unexpected event ignored\n"); |
networker | 0:81ed8b6e4a8b | 274 | break; |
networker | 0:81ed8b6e4a8b | 275 | } |
networker | 0:81ed8b6e4a8b | 276 | if (result == 0) { //configuration acceptable |
networker | 0:81ed8b6e4a8b | 277 | if (s->State == SocketState_L2CAP_Config_wait_reqrsp) { |
networker | 0:81ed8b6e4a8b | 278 | s->State = SocketState_L2CAP_Config_wait_req; |
networker | 0:81ed8b6e4a8b | 279 | printf("State=WAIT_CONFIG_REQ\n"); |
networker | 0:81ed8b6e4a8b | 280 | } else { |
networker | 0:81ed8b6e4a8b | 281 | ConfigureResponse(data[9],((L2CAPSocket*)s)->dcid);//data[9]==txid |
networker | 0:81ed8b6e4a8b | 282 | printf("Sent ConfigureResponse, state=Open\n"); |
networker | 0:81ed8b6e4a8b | 283 | s->SetState(SocketState_Open); |
networker | 0:81ed8b6e4a8b | 284 | } |
networker | 0:81ed8b6e4a8b | 285 | } else { |
networker | 0:81ed8b6e4a8b | 286 | printf("Renegotiate configuration\n"); |
networker | 0:81ed8b6e4a8b | 287 | } |
networker | 0:81ed8b6e4a8b | 288 | } |
networker | 0:81ed8b6e4a8b | 289 | break; |
networker | 0:81ed8b6e4a8b | 290 | |
networker | 0:81ed8b6e4a8b | 291 | case L2CAP_CONF_REQ: { |
networker | 0:81ed8b6e4a8b | 292 | int scid = LE16(data+12);//flags (data[14] LSB is continuation flag, data[18],[19] are the MTU |
networker | 0:81ed8b6e4a8b | 293 | L2CAPSocket* s = SCIDToSocket(scid); |
networker | 0:81ed8b6e4a8b | 294 | printfBytes("CONF REQ: ", data+8, LE16(data+10)+4);//data+16 contains option type 1-4 1=MTU, 2=flush timeout, 3=QoS, 4=FCM |
networker | 0:81ed8b6e4a8b | 295 | if (s == 0) break; |
networker | 0:81ed8b6e4a8b | 296 | if (s->si.State == SocketState_Closed || |
networker | 0:81ed8b6e4a8b | 297 | s->si.State == SocketState_L2CAP_WaitConnectRsp || |
networker | 0:81ed8b6e4a8b | 298 | s->si.State == SocketState_L2CAP_WaitDisconnect) { |
networker | 0:81ed8b6e4a8b | 299 | //Send Reject command |
networker | 0:81ed8b6e4a8b | 300 | break; |
networker | 0:81ed8b6e4a8b | 301 | } |
networker | 0:81ed8b6e4a8b | 302 | switch (data[16]) { |
networker | 0:81ed8b6e4a8b | 303 | case 1: |
networker | 0:81ed8b6e4a8b | 304 | peer_mtu = LE16(data+18); |
networker | 0:81ed8b6e4a8b | 305 | printf("MTU = %d bytes\n", peer_mtu); |
networker | 0:81ed8b6e4a8b | 306 | break; |
networker | 0:81ed8b6e4a8b | 307 | default: |
networker | 0:81ed8b6e4a8b | 308 | printf("Unsupported configuration option %d, value = %#X\n", data[16], LE16(data+18)); |
networker | 0:81ed8b6e4a8b | 309 | break; |
networker | 0:81ed8b6e4a8b | 310 | } |
networker | 0:81ed8b6e4a8b | 311 | if (1 /* options acceptable */) { |
networker | 0:81ed8b6e4a8b | 312 | printf("Sending ConfigureResponse, old state=%d ", s->si.State); |
networker | 0:81ed8b6e4a8b | 313 | ConfigureResponse(data[9],s->dcid);//data[9]==txid, success |
networker | 0:81ed8b6e4a8b | 314 | switch (s->si.State) { |
networker | 0:81ed8b6e4a8b | 315 | case SocketState_L2CAP_Config_wait: |
networker | 0:81ed8b6e4a8b | 316 | s->si.State = SocketState_L2CAP_Config_wait_send; |
networker | 0:81ed8b6e4a8b | 317 | break; |
networker | 0:81ed8b6e4a8b | 318 | case SocketState_L2CAP_Config_wait_req: |
networker | 0:81ed8b6e4a8b | 319 | ((SocketInternal*)s)->SetState(SocketState_Open); |
networker | 0:81ed8b6e4a8b | 320 | break; |
networker | 0:81ed8b6e4a8b | 321 | case SocketState_L2CAP_Config_wait_rsp: |
networker | 0:81ed8b6e4a8b | 322 | break; |
networker | 0:81ed8b6e4a8b | 323 | case SocketState_L2CAP_Config_wait_reqrsp: |
networker | 0:81ed8b6e4a8b | 324 | s->si.State = SocketState_L2CAP_Config_wait_rsp; |
networker | 0:81ed8b6e4a8b | 325 | break; |
networker | 0:81ed8b6e4a8b | 326 | } |
networker | 0:81ed8b6e4a8b | 327 | printf("new state=%d\n", s->si.State); |
networker | 0:81ed8b6e4a8b | 328 | } else { //options not acceptable |
networker | 0:81ed8b6e4a8b | 329 | ConfigureResponse(data[9],s->dcid);//indicates success but should indicate fail |
networker | 0:81ed8b6e4a8b | 330 | } |
networker | 0:81ed8b6e4a8b | 331 | } |
networker | 0:81ed8b6e4a8b | 332 | break; |
networker | 0:81ed8b6e4a8b | 333 | case L2CAP_DISCONN_REQ: { |
networker | 0:81ed8b6e4a8b | 334 | int dcid = LE16(data+12); |
networker | 0:81ed8b6e4a8b | 335 | int scid = LE16(data+14); |
networker | 0:81ed8b6e4a8b | 336 | L2CAPSocket* s = SCIDToSocket(scid); |
networker | 0:81ed8b6e4a8b | 337 | s->si.SetState(SocketState_Closed); |
networker | 0:81ed8b6e4a8b | 338 | DisconnectResponse(data[9], scid, dcid); |
networker | 0:81ed8b6e4a8b | 339 | } |
networker | 0:81ed8b6e4a8b | 340 | break; |
networker | 0:81ed8b6e4a8b | 341 | case L2CAP_DISCONN_RSP: { |
networker | 0:81ed8b6e4a8b | 342 | int scid = LE16(data+14); |
networker | 0:81ed8b6e4a8b | 343 | L2CAPSocket* s = SCIDToSocket(scid); |
networker | 0:81ed8b6e4a8b | 344 | if (s->si.State == SocketState_L2CAP_WaitDisconnect) |
networker | 0:81ed8b6e4a8b | 345 | s->si.SetState(SocketState_Closed); |
networker | 0:81ed8b6e4a8b | 346 | } |
networker | 0:81ed8b6e4a8b | 347 | break; |
networker | 0:81ed8b6e4a8b | 348 | } |
networker | 0:81ed8b6e4a8b | 349 | printf("\x1b[0m"); |
networker | 0:81ed8b6e4a8b | 350 | } |
networker | 0:81ed8b6e4a8b | 351 | #else |
networker | 0:81ed8b6e4a8b | 352 | //code8, tid8, lengthData16 |
networker | 0:81ed8b6e4a8b | 353 | // 0, 1, 2, 3 |
networker | 0:81ed8b6e4a8b | 354 | void BTDevice::Control(const u8* data, int len) { //control channel receive |
networker | 0:81ed8b6e4a8b | 355 | printf("\x1B[%dm", 31); |
networker | 0:81ed8b6e4a8b | 356 | int cc = data[0];//command code |
networker | 0:81ed8b6e4a8b | 357 | if (cc & 1) { //it is a response or a reject |
networker | 0:81ed8b6e4a8b | 358 | rtx.detach(); //kill the timeout |
networker | 0:81ed8b6e4a8b | 359 | printf("timeout cancelled for handle %#x, txid=%d\n", _handle, data[1]); |
networker | 0:81ed8b6e4a8b | 360 | } |
networker | 0:81ed8b6e4a8b | 361 | printf(L2CAP_ComandCodeStr(cc)); |
networker | 0:81ed8b6e4a8b | 362 | switch (cc) { |
networker | 0:81ed8b6e4a8b | 363 | case L2CAP_COMMAND_REJ://bad command, eg. MTU, check (reason) |
networker | 0:81ed8b6e4a8b | 364 | printf(" rejection reason=%d\n", LE16(data+4)); |
networker | 0:81ed8b6e4a8b | 365 | break; |
networker | 0:81ed8b6e4a8b | 366 | case L2CAP_CONN_REQ://incoming connection request, not expected but should reply with proper rejection (or accept) |
networker | 0:81ed8b6e4a8b | 367 | //when a connection is accepted a new socket must be opened |
networker | 0:81ed8b6e4a8b | 368 | printf("Remote side requested a connection\n"); |
networker | 0:81ed8b6e4a8b | 369 | break; |
networker | 0:81ed8b6e4a8b | 370 | // Response to our initial connect from Remote |
networker | 0:81ed8b6e4a8b | 371 | case L2CAP_CONN_RSP: { |
networker | 0:81ed8b6e4a8b | 372 | int dcid = LE16(data+4); |
networker | 0:81ed8b6e4a8b | 373 | int scid = LE16(data+6); |
networker | 0:81ed8b6e4a8b | 374 | L2CAPSocket* s = SCIDToSocket(scid); |
networker | 0:81ed8b6e4a8b | 375 | int result = LE16(data+10); |
networker | 0:81ed8b6e4a8b | 376 | printf(" Result=%d, Status = %d\n", result, LE16(data+10)); |
networker | 0:81ed8b6e4a8b | 377 | if (s->si.State != SocketState_L2CAP_WaitConnectRsp) { |
networker | 0:81ed8b6e4a8b | 378 | printf("Unexpected event ignored\n"); |
networker | 0:81ed8b6e4a8b | 379 | break; |
networker | 0:81ed8b6e4a8b | 380 | } |
networker | 0:81ed8b6e4a8b | 381 | if (result == 0) { |
networker | 0:81ed8b6e4a8b | 382 | if (s) { |
networker | 0:81ed8b6e4a8b | 383 | s->si.State = SocketState_L2CAP_Config_wait; |
networker | 0:81ed8b6e4a8b | 384 | s->dcid = dcid; |
networker | 0:81ed8b6e4a8b | 385 | ConfigureRequest(dcid); |
networker | 0:81ed8b6e4a8b | 386 | s->si.State = SocketState_L2CAP_Config_wait_reqrsp; |
networker | 0:81ed8b6e4a8b | 387 | printf("Sent ConfigureRequest, state=WAIT_CONFIG_REQ_RSP\n"); |
networker | 0:81ed8b6e4a8b | 388 | } |
networker | 0:81ed8b6e4a8b | 389 | } else { |
networker | 0:81ed8b6e4a8b | 390 | s->si.SetState(SocketState_Closed); |
networker | 0:81ed8b6e4a8b | 391 | printf("Connect failed?\n"); |
networker | 0:81ed8b6e4a8b | 392 | } |
networker | 0:81ed8b6e4a8b | 393 | } |
networker | 0:81ed8b6e4a8b | 394 | break; |
networker | 0:81ed8b6e4a8b | 395 | |
networker | 0:81ed8b6e4a8b | 396 | case L2CAP_CONF_RSP: { |
networker | 0:81ed8b6e4a8b | 397 | int result = LE16(data+8); |
networker | 0:81ed8b6e4a8b | 398 | printf("Result=%d, datalen=%d, %smore conf to follow\n", result, LE16(data+2), LE16(data+6)?"":"No "); |
networker | 0:81ed8b6e4a8b | 399 | //should parse the config |
networker | 0:81ed8b6e4a8b | 400 | printfBytes("CONF RSP:", data, LE16(data+2)+4); |
networker | 0:81ed8b6e4a8b | 401 | int scid = LE16(data+4); |
networker | 0:81ed8b6e4a8b | 402 | SocketInternal* s = (SocketInternal*)SCIDToSocket(scid); |
networker | 0:81ed8b6e4a8b | 403 | if (s == 0) break; |
networker | 0:81ed8b6e4a8b | 404 | if (s->State != SocketState_L2CAP_Config_wait_reqrsp && s->State != SocketState_L2CAP_Config_wait_rsp) { |
networker | 0:81ed8b6e4a8b | 405 | printf("Unexpected event ignored\n"); |
networker | 0:81ed8b6e4a8b | 406 | break; |
networker | 0:81ed8b6e4a8b | 407 | } |
networker | 0:81ed8b6e4a8b | 408 | if (result == 0) { //configuration acceptable |
networker | 0:81ed8b6e4a8b | 409 | if (s->State == SocketState_L2CAP_Config_wait_reqrsp) { |
networker | 0:81ed8b6e4a8b | 410 | s->State = SocketState_L2CAP_Config_wait_req; |
networker | 0:81ed8b6e4a8b | 411 | printf("State=WAIT_CONFIG_REQ\n"); |
networker | 0:81ed8b6e4a8b | 412 | } else { |
networker | 0:81ed8b6e4a8b | 413 | ConfigureResponse(data[1],((L2CAPSocket*)s)->dcid);//data[1]==txid |
networker | 0:81ed8b6e4a8b | 414 | printf("Sent ConfigureResponse, state=Open\n"); |
networker | 0:81ed8b6e4a8b | 415 | s->SetState(SocketState_Open); |
networker | 0:81ed8b6e4a8b | 416 | } |
networker | 0:81ed8b6e4a8b | 417 | } else { |
networker | 0:81ed8b6e4a8b | 418 | printf("Renegotiate configuration\n"); |
networker | 0:81ed8b6e4a8b | 419 | } |
networker | 0:81ed8b6e4a8b | 420 | } |
networker | 0:81ed8b6e4a8b | 421 | break; |
networker | 0:81ed8b6e4a8b | 422 | |
networker | 0:81ed8b6e4a8b | 423 | case L2CAP_CONF_REQ: { |
networker | 0:81ed8b6e4a8b | 424 | int scid = LE16(data+4);//flags (data[6] LSB is continuation flag, data[10],[11] are the MTU |
networker | 0:81ed8b6e4a8b | 425 | L2CAPSocket* s = SCIDToSocket(scid); |
networker | 0:81ed8b6e4a8b | 426 | printfBytes("CONF REQ: ", data, LE16(data+2)+4);//data+8 contains option type 1-4 1=MTU, 2=flush timeout, 3=QoS, 4=FCM |
networker | 0:81ed8b6e4a8b | 427 | if (s == 0) break; |
networker | 0:81ed8b6e4a8b | 428 | if (s->si.State == SocketState_Closed || |
networker | 0:81ed8b6e4a8b | 429 | s->si.State == SocketState_L2CAP_WaitConnectRsp || |
networker | 0:81ed8b6e4a8b | 430 | s->si.State == SocketState_L2CAP_WaitDisconnect) { |
networker | 0:81ed8b6e4a8b | 431 | //Send Reject command |
networker | 0:81ed8b6e4a8b | 432 | break; |
networker | 0:81ed8b6e4a8b | 433 | } |
networker | 0:81ed8b6e4a8b | 434 | switch (data[8]) { |
networker | 0:81ed8b6e4a8b | 435 | case 1: |
networker | 0:81ed8b6e4a8b | 436 | peer_mtu = LE16(data+10); |
networker | 0:81ed8b6e4a8b | 437 | printf("MTU = %d bytes\n", peer_mtu); |
networker | 0:81ed8b6e4a8b | 438 | break; |
networker | 0:81ed8b6e4a8b | 439 | default: |
networker | 0:81ed8b6e4a8b | 440 | printf("Unsupported configuration option %d, value = %#X\n", data[8], LE16(data+10)); |
networker | 0:81ed8b6e4a8b | 441 | break; |
networker | 0:81ed8b6e4a8b | 442 | } |
networker | 0:81ed8b6e4a8b | 443 | if (1 /* options acceptable */) { |
networker | 0:81ed8b6e4a8b | 444 | printf("Sending ConfigureResponse, old state=%d ", s->si.State); |
networker | 0:81ed8b6e4a8b | 445 | ConfigureResponse(data[1],s->dcid);//data[1]==txid, success |
networker | 0:81ed8b6e4a8b | 446 | switch (s->si.State) { |
networker | 0:81ed8b6e4a8b | 447 | case SocketState_L2CAP_Config_wait: |
networker | 0:81ed8b6e4a8b | 448 | s->si.State = SocketState_L2CAP_Config_wait_send; |
networker | 0:81ed8b6e4a8b | 449 | break; |
networker | 0:81ed8b6e4a8b | 450 | case SocketState_L2CAP_Config_wait_req: |
networker | 0:81ed8b6e4a8b | 451 | ((SocketInternal*)s)->SetState(SocketState_Open); |
networker | 0:81ed8b6e4a8b | 452 | break; |
networker | 0:81ed8b6e4a8b | 453 | case SocketState_L2CAP_Config_wait_rsp: |
networker | 0:81ed8b6e4a8b | 454 | break; |
networker | 0:81ed8b6e4a8b | 455 | case SocketState_L2CAP_Config_wait_reqrsp: |
networker | 0:81ed8b6e4a8b | 456 | s->si.State = SocketState_L2CAP_Config_wait_rsp; |
networker | 0:81ed8b6e4a8b | 457 | break; |
networker | 0:81ed8b6e4a8b | 458 | } |
networker | 0:81ed8b6e4a8b | 459 | printf("new state=%d\n", s->si.State); |
networker | 0:81ed8b6e4a8b | 460 | } else { //options not acceptable |
networker | 0:81ed8b6e4a8b | 461 | ConfigureResponse(data[1],s->dcid);//indicates success but should indicate fail |
networker | 0:81ed8b6e4a8b | 462 | } |
networker | 0:81ed8b6e4a8b | 463 | } |
networker | 0:81ed8b6e4a8b | 464 | break; |
networker | 0:81ed8b6e4a8b | 465 | case L2CAP_DISCONN_REQ: { |
networker | 0:81ed8b6e4a8b | 466 | int dcid = LE16(data+4); |
networker | 0:81ed8b6e4a8b | 467 | int scid = LE16(data+6); |
networker | 0:81ed8b6e4a8b | 468 | L2CAPSocket* s = SCIDToSocket(scid); |
networker | 0:81ed8b6e4a8b | 469 | s->si.SetState(SocketState_Closed); |
networker | 0:81ed8b6e4a8b | 470 | DisconnectResponse(data[1], scid, dcid); |
networker | 0:81ed8b6e4a8b | 471 | } |
networker | 0:81ed8b6e4a8b | 472 | break; |
networker | 0:81ed8b6e4a8b | 473 | case L2CAP_DISCONN_RSP: { |
networker | 0:81ed8b6e4a8b | 474 | int scid = LE16(data+6); |
networker | 0:81ed8b6e4a8b | 475 | L2CAPSocket* s = SCIDToSocket(scid); |
networker | 0:81ed8b6e4a8b | 476 | if (s->si.State == SocketState_L2CAP_WaitDisconnect) |
networker | 0:81ed8b6e4a8b | 477 | s->si.SetState(SocketState_Closed); |
networker | 0:81ed8b6e4a8b | 478 | } |
networker | 0:81ed8b6e4a8b | 479 | break; |
networker | 0:81ed8b6e4a8b | 480 | default: printf("Unsupported L2CAP message %d\n", cc); |
networker | 0:81ed8b6e4a8b | 481 | } |
networker | 0:81ed8b6e4a8b | 482 | printf("\x1b[0m"); |
networker | 0:81ed8b6e4a8b | 483 | } |
networker | 0:81ed8b6e4a8b | 484 | #endif |
networker | 0:81ed8b6e4a8b | 485 | |
networker | 0:81ed8b6e4a8b | 486 | void BTDevice::ACLFwd(const u8* data, int len) { |
networker | 0:81ed8b6e4a8b | 487 | if (l2cap_sock == 1) { |
networker | 0:81ed8b6e4a8b | 488 | //printf("cannot handle segmented ACL control packets\n"); |
networker | 0:81ed8b6e4a8b | 489 | Control(data, len); |
networker | 0:81ed8b6e4a8b | 490 | return; |
networker | 0:81ed8b6e4a8b | 491 | } |
networker | 0:81ed8b6e4a8b | 492 | SocketInternal* s = (SocketInternal*)SCIDToSocket(l2cap_sock); |
networker | 0:81ed8b6e4a8b | 493 | if (s) |
networker | 0:81ed8b6e4a8b | 494 | s->Recv(data,len); |
networker | 0:81ed8b6e4a8b | 495 | else |
networker | 0:81ed8b6e4a8b | 496 | printf("Bad event cid %d\n",l2cap_sock); |
networker | 0:81ed8b6e4a8b | 497 | } |
networker | 0:81ed8b6e4a8b | 498 | //sometimes acl packets are segmented, in that case the l2cap payload length does not correspond to the acl pkt length |
networker | 0:81ed8b6e4a8b | 499 | //and the l2cap packet length. L2CAP works in basic mode and cannot be segmented hence the l2cap pkt size corresponds to |
networker | 0:81ed8b6e4a8b | 500 | //the acl pkt size |
networker | 0:81ed8b6e4a8b | 501 | void BTDevice::ACLRecv(const u8* data, int acllen) { |
networker | 0:81ed8b6e4a8b | 502 | printfBytes("L2CP",data,acllen); |
networker | 0:81ed8b6e4a8b | 503 | u16 handle = LE16(data); |
networker | 0:81ed8b6e4a8b | 504 | if ((handle&0x0fff) != _handle) { |
networker | 0:81ed8b6e4a8b | 505 | printf("unexpected handle %#x, this _handle=%#x\n", handle, _handle); |
networker | 0:81ed8b6e4a8b | 506 | return; |
networker | 0:81ed8b6e4a8b | 507 | } |
networker | 0:81ed8b6e4a8b | 508 | char pb = (handle>>12) & 3; |
networker | 0:81ed8b6e4a8b | 509 | int p = 4; //start of l2cap packet |
networker | 0:81ed8b6e4a8b | 510 | int len = LE16(data+2); //length of l2cap pkt |
networker | 0:81ed8b6e4a8b | 511 | while (p < len) |
networker | 0:81ed8b6e4a8b | 512 | switch (contState) { |
networker | 0:81ed8b6e4a8b | 513 | case 0: |
networker | 0:81ed8b6e4a8b | 514 | plen = data[p++]; |
networker | 0:81ed8b6e4a8b | 515 | contState = 1; |
networker | 0:81ed8b6e4a8b | 516 | break; |
networker | 0:81ed8b6e4a8b | 517 | case 1: |
networker | 0:81ed8b6e4a8b | 518 | plen += data[p++]<<8; |
networker | 0:81ed8b6e4a8b | 519 | if (pb == 2 && plen == acllen-8) {//normal case, l2cap pkt is contained completely in this hci pkt |
networker | 0:81ed8b6e4a8b | 520 | l2cap_sock = data[p] + (data[p+1]<<8); |
networker | 0:81ed8b6e4a8b | 521 | contState = 0; |
networker | 0:81ed8b6e4a8b | 522 | ACLFwd(data+8, plen); //forward the packet in its original buffer |
networker | 0:81ed8b6e4a8b | 523 | return; //all data was dealt with |
networker | 0:81ed8b6e4a8b | 524 | } else { //packet is segmented |
networker | 0:81ed8b6e4a8b | 525 | printf("ACL packet is segmented\n"); |
networker | 0:81ed8b6e4a8b | 526 | contState = 2; |
networker | 0:81ed8b6e4a8b | 527 | contBuf = new unsigned char[plen];//allocate recombination buffer |
networker | 0:81ed8b6e4a8b | 528 | contPos = 0; |
networker | 0:81ed8b6e4a8b | 529 | } |
networker | 0:81ed8b6e4a8b | 530 | break; |
networker | 0:81ed8b6e4a8b | 531 | case 2: |
networker | 0:81ed8b6e4a8b | 532 | l2cap_sock = data[p++]; |
networker | 0:81ed8b6e4a8b | 533 | contState = 3; |
networker | 0:81ed8b6e4a8b | 534 | break; |
networker | 0:81ed8b6e4a8b | 535 | case 3: |
networker | 0:81ed8b6e4a8b | 536 | l2cap_sock += data[p++]<<8; |
networker | 0:81ed8b6e4a8b | 537 | contState = 4; |
networker | 0:81ed8b6e4a8b | 538 | break; |
networker | 0:81ed8b6e4a8b | 539 | case 4: //data, recombine segmented ACL (not l2cap!) frames |
networker | 0:81ed8b6e4a8b | 540 | if (contPos < plen) {//buffer not yet full |
networker | 0:81ed8b6e4a8b | 541 | int datalen = acllen - p; //data in this incoming pkt |
networker | 0:81ed8b6e4a8b | 542 | int remcap = plen - contPos; //remaining capacity in the recombination buffer |
networker | 0:81ed8b6e4a8b | 543 | if (datalen <= remcap) { |
networker | 0:81ed8b6e4a8b | 544 | memcpy(contBuf+contPos, data+p, datalen); |
networker | 0:81ed8b6e4a8b | 545 | contPos += datalen; |
networker | 0:81ed8b6e4a8b | 546 | p = acllen;//end of data, stop the while loop |
networker | 0:81ed8b6e4a8b | 547 | if (contPos == plen) {//buffer is full now |
networker | 0:81ed8b6e4a8b | 548 | printfBytes("Recombined packet is:", contBuf, plen); |
networker | 0:81ed8b6e4a8b | 549 | ACLFwd(contBuf, plen); //forward the recombination buffer |
networker | 0:81ed8b6e4a8b | 550 | delete[] contBuf;//and free the buffer |
networker | 0:81ed8b6e4a8b | 551 | contState = 0; |
networker | 0:81ed8b6e4a8b | 552 | }//else stay in this state to wait for the rest |
networker | 0:81ed8b6e4a8b | 553 | } else {//data contains (part of) next packet |
networker | 0:81ed8b6e4a8b | 554 | memcpy(contBuf+contPos, data+p, plen-contPos);//this packet is complete |
networker | 0:81ed8b6e4a8b | 555 | p += plen-contPos; |
networker | 0:81ed8b6e4a8b | 556 | printfBytes("Recombined packet is:", contBuf, plen); |
networker | 0:81ed8b6e4a8b | 557 | printfBytes("Next packet starts with:", data+p, acllen-p); |
networker | 0:81ed8b6e4a8b | 558 | ACLFwd(contBuf, plen); //forward the recombination buffer |
networker | 0:81ed8b6e4a8b | 559 | delete[] contBuf;//and free the buffer |
networker | 0:81ed8b6e4a8b | 560 | contState = 0; //continue with the next packet |
networker | 0:81ed8b6e4a8b | 561 | } |
networker | 0:81ed8b6e4a8b | 562 | } else { |
networker | 0:81ed8b6e4a8b | 563 | printf("Cannot append to buffer (size=%d, pos=%d, datalen = %d)\n", plen, contPos, len-p); |
networker | 0:81ed8b6e4a8b | 564 | contState = 0; |
networker | 0:81ed8b6e4a8b | 565 | return; |
networker | 0:81ed8b6e4a8b | 566 | } |
networker | 0:81ed8b6e4a8b | 567 | break; |
networker | 0:81ed8b6e4a8b | 568 | }//switch (and while) |
networker | 0:81ed8b6e4a8b | 569 | } |