Ad van der Weiden
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
Revision 13:327622e38551, committed 2011-07-01
- Comitter:
- networker
- Date:
- Fri Jul 01 09:16:00 2011 +0000
- Parent:
- 12:57f7679dd651
- Commit message:
- made some improvements to get massstorage functioning
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutoEvents.cpp Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,202 @@
+
+/*
+Copyright (c) 2010 Peter Barrett
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*/
+
+#include "mbed.h"
+#include "USBHost.h"
+#include "Utils.h"
+
+#define AUTOEVT(_class,_subclass,_protocol) (((_class) << 16) | ((_subclass) << 8) | _protocol)
+#define AUTO_KEYBOARD AUTOEVT(CLASS_HID,1,1)
+#define AUTO_MOUSE AUTOEVT(CLASS_HID,1,2)
+
+u8 auto_mouse[4]; // buttons,dx,dy,scroll
+u8 auto_keyboard[8]; // modifiers,reserved,keycode1..keycode6
+u8 auto_joystick[4]; // x,y,buttons,throttle
+
+void AutoEventCallback(int device, int endpoint, int status, u8* data, int len, void* userData) {
+ int evt = (int)userData;
+ switch (evt) {
+ case AUTO_KEYBOARD:
+ printf("AUTO_KEYBOARD ");
+ break;
+ case AUTO_MOUSE:
+ printf("AUTO_MOUSE ");
+ break;
+ default:
+ printf("HUH ");
+ }
+ printfBytes("data",data,len);
+ USBInterruptTransfer(device,endpoint,data,len,AutoEventCallback,userData);
+}
+
+// Establish transfers for interrupt events
+void AddAutoEvent(int device, InterfaceDescriptor* id, EndpointDescriptor* ed) {
+ if ((ed->bmAttributes & 3) != ENDPOINT_INTERRUPT || !(ed->bEndpointAddress & 0x80))
+ return;
+
+ // Make automatic interrupt enpoints for known devices
+ u32 evt = AUTOEVT(id->bInterfaceClass,id->bInterfaceSubClass,id->bInterfaceProtocol);
+ u8* dst = 0;
+ int len;
+ switch (evt) {
+ case AUTO_MOUSE:
+ dst = auto_mouse;
+ len = sizeof(auto_mouse);
+ break;
+ case AUTO_KEYBOARD:
+ dst = auto_keyboard;
+ len = sizeof(auto_keyboard);
+ break;
+ default:
+ printf("Interrupt endpoint %02X %08X\n",ed->bEndpointAddress,evt);
+ break;
+ }
+ if (dst) {
+ printf("Auto Event for %02X %08X\n",ed->bEndpointAddress,evt);
+ USBInterruptTransfer(device,ed->bEndpointAddress,dst,len,AutoEventCallback,(void*)evt);
+ }
+}
+
+void PrintString(int device, int i) {
+ u8 buffer[256];
+ int le = GetDescriptor(device,DESCRIPTOR_TYPE_STRING,i,buffer,255);
+ if (le < 0)
+ return;
+ char* dst = (char*)buffer;
+ for (int j = 2; j < le; j += 2)
+ *dst++ = buffer[j];
+ *dst = 0;
+ printf("%d:%s\n",i,(const char*)buffer);
+}
+
+// Walk descriptors and create endpoints for a given device
+int StartAutoEvent(int device, int configuration, int interfaceNumber) {
+ u8 buffer[255];
+ int err = GetDescriptor(device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,255);
+ if (err < 0)
+ return err;
+
+ int len = buffer[2] | (buffer[3] << 8);
+ u8* d = buffer;
+ u8* end = d + len;
+ while (d < end) {
+ if (d[1] == DESCRIPTOR_TYPE_INTERFACE) {
+ InterfaceDescriptor* id = (InterfaceDescriptor*)d;
+ if (id->bInterfaceNumber == interfaceNumber) {
+ d += d[0];
+ while (d < end && d[1] != DESCRIPTOR_TYPE_INTERFACE) {
+ if (d[1] == DESCRIPTOR_TYPE_ENDPOINT)
+ AddAutoEvent(device,id,(EndpointDescriptor*)d);
+ d += d[0];
+ }
+ }
+ }
+ d += d[0];
+ }
+ return 0;
+}
+
+// Implemented in main.cpp
+int OnDiskInsert(int device, unsigned char in, unsigned char out);
+
+// Implemented in TestShell.cpp
+int OnBluetoothInsert(int device);
+
+EndpointDescriptor* GetNextEndpoint(unsigned char *d) {
+ while (d) {
+ switch (d[1]) {
+ case DESCRIPTOR_TYPE_INTERFACE:
+ case DESCRIPTOR_TYPE_CONFIGURATION:
+ return 0;
+ case DESCRIPTOR_TYPE_ENDPOINT:
+ return (EndpointDescriptor*)d;
+ default:
+ printf("Skipping descriptor %02X (%d bytes)\n",d[1],d[0]);
+ }
+ d += d[0];
+ }
+ return 0;
+}
+
+EndpointDescriptor* GetEndpoint(InterfaceDescriptor* interfaceDesc, int i) {
+ unsigned char *d = (unsigned char*)interfaceDesc;
+ int n = interfaceDesc->bNumEndpoints;
+ int j = 0;
+ if (i >= n)
+ return 0;
+ d += d[0];//move to first descriptor after the interface descriptor
+ while (j < n) {
+ switch (d[1]) {
+ case DESCRIPTOR_TYPE_INTERFACE:
+ case DESCRIPTOR_TYPE_CONFIGURATION:
+ return 0;
+ case DESCRIPTOR_TYPE_ENDPOINT:
+ if (i == j)
+ return (EndpointDescriptor*)d;
+ j++;
+ break;
+ default:
+ printf("Skipping descriptor %02X (%d bytes)\n",d[1],d[0]);
+ }
+ d += d[0];
+ }
+ return 0;
+}
+
+void OnLoadDevice(int device, DeviceDescriptor* deviceDesc, InterfaceDescriptor* interfaceDesc) {
+ printf("LoadDevice %d %02X:%02X:%02X\n",device,interfaceDesc->bInterfaceClass,interfaceDesc->bInterfaceSubClass,interfaceDesc->bInterfaceProtocol);
+ char s[128];
+ for (int i = 1; i < 3; i++) {
+ if (GetString(device,i,s,sizeof(s)) < 0)
+ break;
+ printf("%d: %s\n",i,s);
+ }
+
+ switch (interfaceDesc->bInterfaceClass) {
+ case CLASS_MASS_STORAGE:
+ if (interfaceDesc->bInterfaceSubClass == 0x06 && interfaceDesc->bInterfaceProtocol == 0x50) {
+ unsigned char epin = 0x81;
+ unsigned char epout = 0x01;
+ for (int i = 0; i < interfaceDesc->bNumEndpoints; i++){
+ EndpointDescriptor* ep = GetEndpoint(interfaceDesc, i);
+ if (ep){
+ if (ep->bEndpointAddress & 0x7F)
+ if (ep->bEndpointAddress & 0x80)
+ epin = ep->bEndpointAddress;
+ else
+ epout = ep->bEndpointAddress;
+ }else break;
+ }
+ printf("In = %02X, Out = %02X\n", epin, epout);
+ OnDiskInsert(device, epin, epout); // it's SCSI!
+ }
+ break;
+ case CLASS_WIRELESS_CONTROLLER:
+ if (interfaceDesc->bInterfaceSubClass == 0x01 && interfaceDesc->bInterfaceProtocol == 0x01)
+ OnBluetoothInsert(device); // it's bluetooth!
+ break;
+ default:
+ StartAutoEvent(device,1,0);
+ break;
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/AvailableMemory.lib Fri Jul 01 09:16:00 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/networker/code/AvailableMemory/#2b2aa11cebd7
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/FATFileSystem.lib Fri Jul 01 09:16:00 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_unsupported/code/fatfilesystem/ \ No newline at end of file
--- a/HCITransportUSB.h Sun Jun 19 19:32:51 2011 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,65 +0,0 @@
-#ifndef HCITRANSPORTUSB_H
-#define HCITRANSPORTUSB_H
-#define MAX_HCL_SIZE 260
-#define MAX_ACL_SIZE 400
-#include "USBHost.h"
-
-extern int bulk;
-
-class HCITransportUSB : public HCITransport {
- int _device;
- u8* _hciBuffer;
- u8* _aclBuffer;
-
-public:
- void Open(int device, u8* hciBuffer, u8* aclBuffer) {
- _device = device;
- _hciBuffer = hciBuffer;
- _aclBuffer = aclBuffer;
- USBInterruptTransfer(_device,0x81,_hciBuffer,MAX_HCL_SIZE,HciCallback,this);
- USBBulkTransfer(_device,0x82,_aclBuffer,MAX_ACL_SIZE,AclCallback,this);
- }
-
- static void HciCallback(int device, int endpoint, int status, u8* data, int len, void* userData) {
- HCI* t = ((HCITransportUSB*)userData)->_target; //printf("HCI: %d bytes avail\n", len);
- if (t){
- printfBytes("HCICallback:", data, len);
- t->HCIRecv(data,len);
- }
- USBInterruptTransfer(device,0x81,data,MAX_HCL_SIZE,HciCallback,userData);
- }
-
- static void AclCallback(int device, int endpoint, int status, u8* data, int len, void* userData) {
- HCI* t = ((HCITransportUSB*)userData)->_target; printf("ACL: %d bytes avail\n", len);
- if (t){
- printfBytes("ACLCallback:", data, len);
- t->ACLRecv(data,len);
- }
- printf("ACL Read pending..\n");
- USBBulkTransfer(device,0x82,data,MAX_ACL_SIZE,AclCallback,userData);
- }
-
- virtual void HCISend(const u8* data, int len) {
- printfBytes("HCISend:", data, len);
- USBControlTransfer(_device,REQUEST_TYPE_CLASS, 0, 0, 0,(u8*)data,len);
- }
-
- virtual int ACLSend(const u8* data, int len) { //printf("send %d bytes to usb\n", len);
- if (len > _acl_mtu) {
- printf("Max outgoing packet(%d) size exceeded, segmenting necessary, pktlen = %d\n", _acl_mtu, len);
- return 0;
- }
-#ifdef HOST_CONTR_FLOW
- if (data_credits == 0)
- printf("Waiting for ACL buffers...\n");
- while (data_credits < 1) {
- USBLoop();
- }
- data_credits--;
-#endif
- printfBytes("ACLSend:", data, len);
- return USBBulkTransfer(_device,0x02,(u8*)data,len);
- }
-};
-
-#endif
--- a/L2CAP.cpp Sun Jun 19 19:32:51 2011 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,614 +0,0 @@
-/*
-Copyright (c) 2010 Peter Barrett
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-*/
-
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-
-#include "Utils.h"
-#include "hci.h"
-#include "HCITransportUSB.h"
-#include "sdp.h"
-#include "RFCOMM.h"
-
-#define L2CAP_COMMAND_REJ 0x01
-#define L2CAP_CONN_REQ 0x02
-#define L2CAP_CONN_RSP 0x03
-#define L2CAP_CONF_REQ 0x04
-#define L2CAP_CONF_RSP 0x05
-#define L2CAP_DISCONN_REQ 0x06
-#define L2CAP_DISCONN_RSP 0x07
-#define L2CAP_ECHO_REQ 0x08
-#define L2CAP_ECHO_RSP 0x09
-#define L2CAP_INFO_REQ 0x0a
-#define L2CAP_INFO_RSP 0x0b
-
-#define TXID (++_txid?_txid:1)
-//template <class T> T min(T a, T b) { return a<b ? a : b;}
-
-/* L2CAP command codes */
-const char* L2CAP_ComandCodeStr(int c) {
- switch (c) {
- case L2CAP_COMMAND_REJ:
- return "L2CAP_COMMAND_REJ";
- case L2CAP_CONN_REQ:
- return "L2CAP_CONN_REQ";
- case L2CAP_CONN_RSP:
- return "L2CAP_CONN_RSP";
- case L2CAP_CONF_REQ:
- return "L2CAP_CONF_REQ";
- case L2CAP_CONF_RSP:
- return "L2CAP_CONF_RSP";
- case L2CAP_DISCONN_REQ:
- return "L2CAP_DISCONN_REQ";
- case L2CAP_DISCONN_RSP:
- return "L2CAP_DISCONN_RSP";
- case L2CAP_ECHO_REQ:
- return "L2CAP_ECHO_REQ";
- case L2CAP_ECHO_RSP:
- return "L2CAP_ECHO_RSP";
- case L2CAP_INFO_REQ:
- return "L2CAP_INFO_REQ";
- case L2CAP_INFO_RSP:
- return "L2CAP_INFO_RSP";
- }
- return "unknown";
-}
-
-#define OFFSET 8 //means the buffer also has space for the l2cap/hci headers and need not be allocated and copied
-//#define OFFSET 0 //means the buffer only has space for the payload which need to be copied
-#if OFFSET == 0
-#define L2CAPBUFSIZE 128
-#else
-#define L2CAPBUFSIZE 0
-#endif
-
-typedef struct {
- u16 handle;
- u16 length; // total
- u16 l2capLength; // length -4
- u16 cid; // Signaling packet CID = 1
- u8 data[L2CAPBUFSIZE]; // Largest thing to send!!! todo
-} L2CAPData;
-
-//
-void BTDevice::Init() {
- memset(&_info,0,sizeof(inquiry_info));
- _handle = 0;
- _name[0] = 0;
- _state = 0;
- _txid = 1;
- //cntr_cred = 1;
-}
-
-// virtual SocketHandler
-int BTDevice::Open(SocketInternal* sock, SocketAddrHdr* addr) {
- L2CAPSocket* s = (L2CAPSocket*)sock;
- L2CAPAddr* a = (L2CAPAddr*)addr;
- s->scid = 0x40 + sock->ID-1; // are these reserved?
- s->dcid = 0;
- Connect(s->scid,a->psm);
- sock->State = SocketState_L2CAP_WaitConnectRsp;
- contState = 0;
- return sock->ID;
-}
-
-// virtual SocketHandler
-int BTDevice::Accept(SocketInternal* sock, int scid, int rxid) {
- L2CAPSocket* s = (L2CAPSocket*)sock;
- s->scid = 0x40 + sock->ID-1; // are these reserved?
- s->dcid = scid;
- u16 p[4];
- p[0] = s->scid;
- p[1] = scid;
- p[2] = 0; //success
- p[3] = 0; //no further information
- Send(L2CAP_CONN_RSP,rxid,p,4);
- printf("send conn_rsp with dcid=%#x and scid=%#x\n", p[0],p[1]);
- sock->State = SocketState_L2CAP_Config_wait;
- contState = 0;
- return sock->ID;
-}
-
-// virtual SocketHandler, called from HCI which is ABOVE L2CAP
-int BTDevice::Send(SocketInternal* sock, const u8* data, int len) {
- L2CAPSocket* s = (L2CAPSocket*)sock;
-#if OFFSET == 8 //sizeof L2CAPData header
- L2CAPData &d = *const_cast<L2CAPData*>((L2CAPData*)data);
-#else
- L2CAPData d;
-#endif
- if (len > peer_mtu) {//mtu concerns the l2cap mtu, because we use basic mode we cannot segment
- printf("MTU (%d) for outgoing packet (%d) exceeded\n", peer_mtu, len);
- return 0;
- }
- d.handle = _handle | 0x2000;
- d.length = 4 + len - OFFSET;
- d.l2capLength = len - OFFSET;
- d.cid = s->dcid;
- //printf("cid=%d: ", d.cid);
- //printfBytes("sending: ", data, len);
-#if OFFSET == 0
- if (len > L2CAPBUFSIZE)
- return -1;
- memcpy(d.data,data,len);
- return Send((u8*)&d,len+8);
-#else
- return Send(data, len);
-#endif
-}
-
-// virtual SocketHandler
-int BTDevice::Close(SocketInternal* sock) {
- printf("L2CAP close %d\n",sock->ID);
- sock->State = SocketState_L2CAP_WaitDisconnect;
- L2CAPSocket* s = (L2CAPSocket*)sock;
- return Disconnect(s->scid,s->dcid);
-}
-
-L2CAPSocket* BTDevice::SCIDToSocket(int scid) {
- return (L2CAPSocket*)GetSocketInternal(scid-0x40+1);
-}
-
-int BTDevice::Send(const u8* data, int len) {//printfBytes("Transport : ", data, len);
-#ifdef HOST_CONTR_FLOW
- pkts_sent++;
-#endif
- _transport->ACLSend(data,len);
- return 0;
-}
-
-void BTDevice::repeat_cmd() {
- printf("Cmd on handle %#x timed out, resending txid=%d\n", _handle, last_req.id);
-// Send ((u8*)&last_req, last_req.length+4);//danger! interrupt context, Send is not reentrant
- //optionally set new larger timeout
-}
-
-int BTDevice::Send(u8 c, u8 id, u16* params, int count) {
- L2CAPCmd cmd;
- cmd.handle = _handle | 0x2000;
- cmd.length = 8 + count*2;
-
- cmd.l2capLength = cmd.length-4;
- cmd.cid = 1; // Signaling packet
-
- cmd.cmd = c;
- cmd.id = id;
- cmd.cmdLength = count*2;
- for (int i = 0; i < count; i++)
- cmd.params[i] = params[i];
- if ((c & 1) == 0) { //this is a request
- last_req = cmd;
- rtx.attach(this, &BTDevice::repeat_cmd, 30.0);
- //printf("Starting timeout for %#x, txid=%d\n", _handle, id);
- }
- return Send((u8*)&cmd,cmd.length+4);
-}
-
-int BTDevice::Connect(int scid, int psm) {
- u16 p[2];
- p[0] = psm;
- p[1] = scid;
- return Send(L2CAP_CONN_REQ,TXID,p,2);
-}
-
-int BTDevice::Disconnect(int scid, int dcid) {
- u16 p[2];
- p[0] = dcid;
- p[1] = scid;
- return Send(L2CAP_DISCONN_REQ,TXID,p,2);
-}
-
-int BTDevice::ConfigureRequest(int dcid) {
- u16 p[4];
- p[0] = dcid;
- p[1] = 0;
- p[2] = 0x0201; // Options
- p[3] = min(0x02A0, MAX_ACL_SIZE); // my receiving MTU 672
- return Send(L2CAP_CONF_REQ,TXID,p,4);
-}
-
-int BTDevice::CommandReject(u16 reason, u16 data0, u16 data1) {
- u16 p[3];
- p[0] = reason;
- p[1] = data0;
- p[2] = data1;
- int parlen = 2;
- switch (reason) {
- case 0: //command not understood
- break;
- case 1: //MTU exceeded
- parlen = 4; //return actual mtu in data
- break;
- case 2: //invalid CID
- parlen = 6; //return local, remote cid
- break;
- }
- return Send(L2CAP_COMMAND_REJ,TXID,p,parlen);
-}
-
-int BTDevice::ConfigureResponse(u8 rxid, int dcid) {
- u16 p[3];
- p[0] = dcid; //source cid
- p[1] = 0; //flags (no continuation)
- p[2] = 0; //result (success)
- return Send(L2CAP_CONF_RSP,rxid,p,3);
-}
-
-int BTDevice::DisconnectResponse(u8 rxid, int scid, int dcid) {
- u16 p[2];
- p[0] = dcid;
- p[1] = scid;
- return Send(L2CAP_DISCONN_RSP,rxid,p,2);
-}
-
-void server(int socket, SocketState state, const u8* data, int len, void* userData) {
- // printfBytes("Server: ", data, len);
- if (state==SocketState_Open && len>0)
- SDP.SDPServer(socket, state, data, len, userData);
-}
-
-void serserver(int socket, SocketState state, const u8* data, int len, void* userData) {
- printfBytes("serserver: ", data, len);
- SocketHandler *h = (SocketHandler*)userData;
- printf("userData refers to %s, state = %d\n", h->Name(), state);
- if (state==SocketState_Open) {
- if (len == 0) { //assume that the socket has just been opened and bind it to a new rfcomm server entity
- printf("Calling RFCOMMManager::BindSocket\n");
- rfcomm_manager.BindSocket(socket);
- } else {
- printf("Calling RFCOMMManager::SerServer\n");
- rfcomm_manager.SerServer(socket, state, data, len, userData);
- }
- } else if (state==SocketState_L2CAP_WaitDisconnect) {
- printf("Calling RFCOMMManager::SerServer\n");
- rfcomm_manager.SerServer(socket, state, data, len, userData);
- }
-}
-
-//code8, tid8, lengthData16
-// 0, 1, 2, 3
-void BTDevice::Control(const u8* data, int len) { //control channel receive
- printf("\x1B[%dm", 31);
- int cc = data[0];//command code
- if (cc & 1) { //it is a response or a reject
- rtx.detach(); //kill the timeout
- //printf("timeout cancelled for handle %#x, txid=%d\n", _handle, data[1]);
- }
- printf(L2CAP_ComandCodeStr(cc));
- switch (cc) {
- case L2CAP_COMMAND_REJ://bad command, eg. MTU, check (reason)
- printf(" rejection reason=%d\n", LE16(data+4));
- break;
- case L2CAP_CONN_REQ://incoming connection request, not expected but should reply with proper rejection (or accept)
- //when a connection is accepted a new socket must be opened
- printf(" Remote side requested a connection\n");
- {
- int scid = LE16(data+6);
- int psm = LE16(data+4);
- int rxid = data[1];
- u16 p[4];
- p[0] = 0; //no dcid
- p[1] = scid;
- p[3] = 0; //no further information
- printf(" scid=%d, psm=%d\n", scid, psm);
- peer_mtu = 672; //default mtu
- int s = 0;
- switch (psm) {
- case L2CAP_PSM_SDP:
- s = Socket_Accept(SOCKET_SDP, scid, rxid, server, this);//allocate an sdp socket but use it as L2CAP
- break;
- case L2CAP_PSM_RFCOMM: //SOCKET_RFCOM;
-#if 0
- s = Socket_Accept(SOCKET_RFCOM, scid, rxid, serserver, this);//allocate an rfcomm socket
- //using L2CAP i.o. RFCOM makes little difference in processing but it also changes the handler to HCI i.o. RFCOMMManager
-#else
-//an RFCOMM requests comes in from a known (this) device
-//the channel is not yet known
- s = rfcomm_manager.FindSocket(this);//this should return 0 otherwise the remote device was asking a second rfcomm on the same device
- if (s==0) {
- printf("No connection to this device yet, allocate L2CAP Socket and accept\n");
- //accept the connection, even though there may be no listener???
- //have no choice because w/o acceptance no rfcomm req.
- s = Socket_Accept(SOCKET_L2CAP, scid, rxid, serserver, this);//allocate an l2cap socket
- //get a new l2cap socket, call HCI::Accept (fill in btdevice internals), then call BTDevice::Accept (send accept message)
- //serserver is called on state changes (SocketInternal::SetState) and on received packets from the peer device to the new l2cap handle
- //after sending the accept message, the devices will execute the normal l2cap connection state-machine
- //ending in a call to SetState(Open) which will invoke 'serserver' for the first time
-//or something like:
-// s = Socket_Create(SOCKET_L2CAP, serserver, this);//allocate an l2cap socket
-// Accept(GetSocketInternal(s), scid, rxid);//send accept response, this would bypass HCI::Accept()
- } else {
- printf("Already had an L2CAP connection on socket %d\n", s);
- }
-#endif
- break;
- default:
- printf("PSM %d not supported\n", psm);
- }
- switch (s) {
- case 0:
- printf("Not a valid socket\n");
- break;
- case ERR_SOCKET_TYPE_NOT_FOUND:
- p[2] = 2; //psm not supported
- Send(L2CAP_CONN_RSP,rxid,p,4);
- break;
- case ERR_SOCKET_NONE_LEFT:
- p[2] = 4; //no resources available
- Send(L2CAP_CONN_RSP,rxid,p,4);
- break;
- }
- }
- break;
- // Response to our initial connect from Remote
- case L2CAP_CONN_RSP: {
- int dcid = LE16(data+4);
- int scid = LE16(data+6);
- L2CAPSocket* s = SCIDToSocket(scid);
- int result = LE16(data+8);
- printf(" Result=%d, Status = %d\n", result, LE16(data+10));
- if (s->si.State != SocketState_L2CAP_WaitConnectRsp) {
- printf("Unexpected event ignored\n");
- break;
- }
- if (result == 0) {
- if (s) {
- s->si.State = SocketState_L2CAP_Config_wait;
- s->dcid = dcid;
- ConfigureRequest(dcid);
- s->si.State = SocketState_L2CAP_Config_wait_reqrsp;
- printf("Sent ConfigureRequest, state=WAIT_CONFIG_REQ_RSP\n");
- }
- } else if (result == 1) {//pending, stay in the present state
- } else {
- s->si.SetState(SocketState_Closed);
- printf("Connect failed\n");
- }
- }
- break;
-
- case L2CAP_CONF_RSP: {
- int result = LE16(data+8);
- printf("Result=%d, datalen=%d, %smore conf to follow\n", result, LE16(data+2), LE16(data+6)?"":"No ");
- //should parse the config
- printfBytes("CONF RSP:", data, LE16(data+2)+4);
- int scid = LE16(data+4);
- SocketInternal* s = (SocketInternal*)SCIDToSocket(scid);
- if (s == 0) break;
- if (s->State != SocketState_L2CAP_Config_wait_reqrsp && s->State != SocketState_L2CAP_Config_wait_rsp) {
- printf("Unexpected event ignored\n");
- break;
- }
- if (result == 0) { //configuration acceptable
- if (s->State == SocketState_L2CAP_Config_wait_reqrsp) {
- s->State = SocketState_L2CAP_Config_wait_req;
- printf("State=WAIT_CONFIG_REQ\n");
- } else {
- ConfigureResponse(data[1],((L2CAPSocket*)s)->dcid);//data[1]==txid
- printf("Sent ConfigureResponse, state=Open\n");
- s->SetState(SocketState_Open);
- }
- } else {
- printf("Renegotiate configuration\n");
- }
- }
- break;
-
- case L2CAP_CONF_REQ: {
- int len = LE16(data+2);
- int scid = LE16(data+4);//flags (data[6] LSB is continuation flag, data[10],[11] are the MTU
- int flags = LE16(data+6);
- const u8* conf = data+8;
- if (flags)
- printf("Warning! Continuation flag in L2CAP configuration not supported\n");
- L2CAPSocket* s = SCIDToSocket(scid);
- printfBytes("CONF REQ: ", data, LE16(data+2)+4);//data+8 contains option type 1-4 1=MTU, 2=flush timeout, 3=QoS, 4=FCM
- if (s == 0) break;
- if (s->si.State == SocketState_Closed ||
- s->si.State == SocketState_L2CAP_WaitConnectRsp ||
- s->si.State == SocketState_L2CAP_WaitDisconnect) {
- //Send Reject command
- printf("Connection should be rejected\n");
- break;
- }
-#if 0
- if (len > 4)
- switch (data[8]) {
- case 1:
- peer_mtu = LE16(data+10);
- printf("Peer L2CAP MTU = %d bytes\n", peer_mtu);
- break;
- case 2: //flush timeout
- case 3: //QOS
- case 4: //retrans and FC option
- default:
- printf("Unsupported configuration option %d, value = %#X\n", data[8], LE16(data+10));
- break;
- }
- else
- printf("Empty config req. peer_mtu = %d\n", peer_mtu);
-#else
- while (conf < data+len+4) {
- bool hint = conf[0] & 0x80;
- switch (conf[0] & 0x7F) {
- case 1:
- peer_mtu = LE16(conf+2);
- printf("Peer L2CAP MTU = %d bytes\n", peer_mtu);
- break;
- case 2: //flush timeout
- case 3: //QOS
- case 4: //retrans and FC option
- default:
- printf("Unsupported configuration option %d, value = %#X\n", conf[0], LE16(conf+2));
- break;
- }
- conf += conf[1]+2;
- }
-#endif
- if (1 /* options acceptable */) {
- if (flags == 0) {
- printf("Sending ConfigureResponse, old state=%d ", s->si.State);
- ConfigureResponse(data[1],s->dcid);//data[1]==txid, success
- switch (s->si.State) {
- case SocketState_L2CAP_Config_wait:
- s->si.State = SocketState_L2CAP_Config_wait_send;
- ConfigureRequest(s->dcid);
- s->si.State = SocketState_L2CAP_Config_wait_rsp;
- break;
- case SocketState_L2CAP_Config_wait_req:
- ((SocketInternal*)s)->SetState(SocketState_Open);
- break;
- case SocketState_L2CAP_Config_wait_rsp:
- break;
- case SocketState_L2CAP_Config_wait_reqrsp:
- s->si.State = SocketState_L2CAP_Config_wait_rsp;
- break;
- }
- printf("new state=%d\n", s->si.State);
- } else
- printf("L2CAP config continuation, delaying response...\n");
- } else { //options not acceptable
- printf("Configure failure should be indicated\n");
- ConfigureResponse(data[1],s->dcid);//indicates success but should indicate fail
- }
- }
- break;
- case L2CAP_DISCONN_REQ: {
- int dcid = LE16(data+4);
- int scid = LE16(data+6);
- L2CAPSocket* s = SCIDToSocket(dcid);
- if (s) {
- s->si.SetState(SocketState_Closed);
- DisconnectResponse(data[1], scid, dcid);
- } else {
- printf("request to disconnect cid %d fails, no such cid\n", dcid);
- CommandReject(0, dcid, scid);
- }
- }
- break;
- case L2CAP_DISCONN_RSP: {
- int scid = LE16(data+6);
- L2CAPSocket* s = SCIDToSocket(scid);
- if (s->si.State == SocketState_L2CAP_WaitDisconnect)
- s->si.SetState(SocketState_Closed);
- }
- break;
- default:
- printf("Unsupported L2CAP message %d\n", cc);
- }
- printf("\x1b[0m");
-}
-
-void BTDevice::ACLFwd(const u8* data, int len) {
- if (l2cap_sock == 1)
- Control(data, len);
- else {
- SocketInternal* s = (SocketInternal*)SCIDToSocket(l2cap_sock);//in fact cid in the l2cap header
- if (s)
- s->Recv(data,len);//forward to the sockethandler for the type
- else
- printf("Bad event cid %d\n",l2cap_sock);
- }
-}
-//sometimes acl packets are segmented, in that case the l2cap payload length does not correspond to the acl pkt length
-//and the l2cap packet length. L2CAP works in basic mode and cannot be segmented hence the l2cap pkt size corresponds to
-//the acl pkt size
-int BTDevice::ACLRecv(const u8* data, int acllen) {
- //printfBytes("L2CP",data,acllen);
- //cntr_cred--;
- u16 handle = LE16(data);
- if ((handle&0x0fff) != _handle) {
- printf("unexpected handle %#x, this _handle=%#x\n", handle, _handle);
- return 1;
- }
- //below is the ACL packet recombination engine
- char pb = (handle>>12) & 3;
- if (pb == 2)
- segments = 1;
- else
- segments++;
- int p = 4; //start of l2cap packet
- int len = LE16(data+2); //length of l2cap pkt
- while (p < acllen)
- switch (contState) {
- case 0://allow even for fragmented length field
- plen = data[p++];//payload length lsb
- contState = 1;
- break;
- case 1:
- plen += data[p++]<<8; //payload length msb
- if (pb == 2 && plen == acllen-8) {//normal case, l2cap pkt is contained completely in this hci pkt
- l2cap_sock = data[p] + (data[p+1]<<8);
- contState = 0;
- ACLFwd(data+8, plen); //forward the packet in its original buffer
- return segments; //all data was dealt with
- } else { //packet is segmented
- printf("ACL packet is segmented\n");
- contState = 2;
- contBuf = new unsigned char[plen];//allocate recombination buffer
- contPos = 0;
- }
- break;
- case 2:
- l2cap_sock = data[p++];
- contState = 3;
- break;
- case 3:
- l2cap_sock += data[p++]<<8;
- contState = 4;
- break;
- case 4: //data, recombine segmented ACL (not l2cap!) frames
- if (contPos < plen) {//buffer not yet full
- int datalen = acllen - p; //data in this incoming pkt
- int remcap = plen - contPos; //remaining capacity in the recombination buffer
- if (datalen <= remcap) {
- memcpy(contBuf+contPos, data+p, datalen);
- contPos += datalen;
- p = acllen;//end of data, stop the while loop
- if (contPos == plen) {//buffer is full now
- printfBytes("Recombined packet is:", contBuf, plen);
- ACLFwd(contBuf, plen); //forward the recombination buffer
- delete[] contBuf;//and free the buffer
- contState = 0;
- return segments;
- }//else stay in this state to wait for the rest
- } else {//data contains (part of) next packet, never seen this happen
- memcpy(contBuf+contPos, data+p, plen-contPos);//this packet is complete
- p += plen-contPos;
- printfBytes("Recombined packet is:", contBuf, plen);
- printfBytes("Next packet starts with:", data+p, acllen-p);
- ACLFwd(contBuf, plen); //forward the recombination buffer
- delete[] contBuf;//and free the buffer
- contState = 0; //continue with the next packet
- }
- } else {
- printf("Cannot append to buffer (size=%d, pos=%d, datalen = %d)\n", plen, contPos, len-p);
- contState = 0;
- return segments;//flushed
- }
- break;
- }//switch (and while)
- return 0;//the buffers are not processed yet
-}
--- a/Socket.cpp Sun Jun 19 19:32:51 2011 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,253 +0,0 @@
-/*
-Copyright (c) 2010 Peter Barrett
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-*/
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-
-#include "Utils.h"
-#include "Socket.h"
-
-#define MAX_SOCKET_HANDLERS 4
-#define MAX_SOCKETS 16
-
-class SocketInternalPad
-{
- public:
- SocketInternal si;
- u8 pad[8];
-};
-
-class SocketManager
-{
- SocketHandler* _handlers[MAX_SOCKET_HANDLERS];
- SocketInternalPad _sockets[MAX_SOCKETS];
-
- public:
- SocketManager()
- {
- memset(_handlers,0,sizeof(_handlers));
- memset(_sockets,0,sizeof(_sockets));
- }
-
- SocketHandler* GetHandler(int type)
- {
- if (type < 1 || type > MAX_SOCKET_HANDLERS)
- return 0;
- return _handlers[type - 1];
- }
-
- SocketInternal* GetInternal(int s)
- {
- if (s < 1 || s > MAX_SOCKETS)
- return 0;
- return &_sockets[s - 1].si;
- }
-
- int RegisterSocketHandler(int type, SocketHandler* handler)
- {
- if (type < 1 || type > MAX_SOCKET_HANDLERS)
- return ERR_SOCKET_TYPE_NOT_FOUND;
- _handlers[type - 1] = handler;
- return 0;
- }
-
- int Create(int type, SocketCallback callback, void* userData)
- {
- SocketHandler* h = GetHandler(type);
- if (!h)
- return ERR_SOCKET_TYPE_NOT_FOUND;
-
- for (int i = 0; i < MAX_SOCKETS; i++)
- {
- SocketInternal* si = (SocketInternal*)(_sockets+i);
- if (si->ID == 0)
- {
- si->ID = i+1;
- si->Type = type;
- si->Callback = callback;
- si->userData = userData;
- printf("Creating socket %d for type %d, invoking 'Open' on %p (=%s)\n", si->ID, type, h, h->Name());
- return h->Create(si);
- }
- }
- return ERR_SOCKET_NONE_LEFT;
- }
-
- int Open(int type, SocketAddrHdr* addr, SocketCallback callback, void* userData)
- {
- SocketHandler* h = GetHandler(type);
- if (!h)
- return ERR_SOCKET_TYPE_NOT_FOUND;
-
- for (int i = 0; i < MAX_SOCKETS; i++)
- {
- SocketInternal* si = (SocketInternal*)(_sockets+i);
- if (si->ID == 0)
- {
- si->ID = i+1;
- si->Type = type;
- si->Callback = callback;
- si->userData = userData;
- printf("Opening socket %d for type %d, invoking 'Open' on %p (=%s)\n", si->ID, type, h, h->Name());
- return h->Open(si,addr);
- }
- }
- return ERR_SOCKET_NONE_LEFT;
- }
-
- int Listen(int type, int channel, SocketCallback callback, void* userData)
- {
- SocketHandler* h = GetHandler(type);
- if (!h)
- return ERR_SOCKET_TYPE_NOT_FOUND;
-
- for (int i = 0; i < MAX_SOCKETS; i++)
- {
- SocketInternal* si = (SocketInternal*)(_sockets+i);
- if (si->ID == 0)
- {
- si->ID = i+1;
- si->Type = type;
- si->Callback = callback;
- si->userData = userData;
- printf("Passively opening socket %d for type %d, invoking 'Listen' on %p (=%s)\n", si->ID, type, h, h->Name());
- int sn = h->Listen(si, channel);
- if (sn < 0)
- si->ID = 0;//free the socket when error
- return sn;
- }
- }
- return ERR_SOCKET_NONE_LEFT;
- }
-
- int Accept(int type, int scid, int rxid, SocketCallback callback, void* userData)
- {
- SocketHandler* h = GetHandler(type);
- if (!h)
- return ERR_SOCKET_TYPE_NOT_FOUND;
-
- for (int i = 0; i < MAX_SOCKETS; i++)
- {
- SocketInternal* si = (SocketInternal*)(_sockets+i);
- if (si->ID == 0)
- {
- si->ID = i+1;
- si->Type = type;
- si->Callback = callback;
- si->userData = userData;
- printf("Accepting socket %d for type %d, invoking 'Accept' on %p (=%s)\n", si->ID, type, h, h->Name());
- return h->Accept(si, scid, rxid);
- }
- }
- return ERR_SOCKET_NONE_LEFT;
- }
-
- int Send(int socket, const u8* data, int len)
- {
- SocketInternal* si = GetInternal(socket);
- if (!si || si->ID != socket)
- return ERR_SOCKET_NOT_FOUND;
- // printf("sending %d bytes to socket %d (ID=%d)\n", len, socket, si->ID);
- return GetHandler(si->Type)->Send(si,data,len);
- }
-
- int SetOpt(int socket, int so, int* data, int len)
- {
- SocketInternal* si = GetInternal(socket);
- if (!si || si->ID != socket)
- return ERR_SOCKET_NOT_FOUND;
- return GetHandler(si->Type)->SetOpt(si, so, data,len);
- }
-
- int GetOpt(int socket, int so, int* data, int len)
- {
- SocketInternal* si = GetInternal(socket);
- if (!si || si->ID != socket)
- return ERR_SOCKET_NOT_FOUND;
- return GetHandler(si->Type)->GetOpt(si, so, data,len);
- }
-
- int Close(int socket)
- {
- SocketInternal* si = GetInternal(socket);
- if (!si || si->ID != socket){
- printf("Close: socket %d not found\n", socket);
- return ERR_SOCKET_NOT_FOUND;
- }
- printf("Close: socket %d (type=%d)\n", socket, si->Type);
- si->SetState(SocketState_Closing);
- int retval = GetHandler(si->Type)->Close(si);
- //si->SetState(Socket_Closed);
- //si->ID = 0;
- return retval;
- }
-};
-
-SocketManager gSocketManager;
-
-int Socket_Open(int type, SocketAddrHdr* addr, SocketCallback callback, void* userData)
-{
- return gSocketManager.Open(type,addr,callback,userData);
-}
-
-int Socket_Listen(int type, int channel, SocketCallback callback, void* userData) // Open a socket for listening
-{
- return gSocketManager.Listen(type,channel,callback,userData);
-}
-
-int Socket_Accept(int type, int scid, int rxid, SocketCallback callback, void* userData) // Open a socket for an incoming connection
-{
- return gSocketManager.Accept(type,scid,rxid,callback,userData);
-}
-
-int Socket_Send(int socket, const u8* data, int len)
-{
- return gSocketManager.Send(socket,data,len);
-}
-
-int Socket_SetOpt(int socket, int so, int* data, int len)
-{
- return gSocketManager.SetOpt(socket, so, data,len);
-}
-
-int Socket_GetOpt(int socket, int so, int* data, int len)
-{
- return gSocketManager.GetOpt(socket, so, data,len);
-}
-
-int Socket_Close(int socket)
-{
- return gSocketManager.Close(socket);
-}
-
-int RegisterSocketHandler(int type, SocketHandler* handler)
-{
- return gSocketManager.RegisterSocketHandler(type,handler);
-}
-
-SocketInternal* GetSocketInternal(int socket)
-{
- return gSocketManager.GetInternal(socket);
-}
-
--- a/Socket.h Sun Jun 19 19:32:51 2011 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,127 +0,0 @@
-/*
-Copyright (c) 2010 Peter Barrett
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-*/
-
-#ifndef SOCKET_H_INCLUDED
-#define SOCKET_H_INCLUDED
-
-#define SOCKET_HCI 1
-#define SOCKET_L2CAP 2
-#define SOCKET_RFCOM 3
-#define SOCKET_SDP 4
-
-#define SO_RECBUF 1
-#define SO_SNDBUF 2
-#define NOPROTOOPT -100
-
-typedef struct
-{
- u8 AddressSpecific[0]; // BDADDR,psm etc
-} SocketAddrHdr;
-
-enum SocketState
-{
- SocketState_Unknown,
- SocketState_Opening,
- SocketState_Open,
- SocketState_Closing,
- SocketState_Closed,
- SocketState_Listen,
- SocketState_Accepting,
- SocketState_L2CAP_WaitConnect = 8,
- SocketState_L2CAP_WaitConnectRsp,
- SocketState_L2CAP_WaitDisconnect,
- SocketState_L2CAP_Config_wait = 16,
- SocketState_L2CAP_Config_wait_send,
- SocketState_L2CAP_Config_wait_req,
- SocketState_L2CAP_Config_wait_rsp,
- SocketState_L2CAP_Config_wait_reqrsp
-};
-
-typedef void (*SocketCallback)(int socket, SocketState state, const u8* data, int len, void* userData);
-
-int Socket_Create(int type, SocketCallback callback, void* userData); // Allocate a socket
-int Socket_Open(int type, SocketAddrHdr* addr, SocketCallback callback, void* userData); // Open a socket
-int Socket_Listen(int type, int channel, SocketCallback callback, void* userData); // Open a socket passive
-int Socket_Accept(int type, int scid, int rxid, SocketCallback callback, void* userData); // Open a socket for an incoming connection
-int Socket_SetOpt(int socket, int so, int* data, int len);
-int Socket_GetOpt(int socket, int so, int* data, int len);
-int Socket_Send(int socket, const u8* data, int len);
-int Socket_State(int socket);
-int Socket_Close(int socket);
-
-//===========================================================================
-//===========================================================================
-// Don't need to look at or use anything below this line:
-// Internal representation of socket
-
-class SocketHandler;
-class SocketInternal
-{
- public:
-
- u8 ID;
- u8 State;
- u8 Type;
- u8 B0;
- SocketCallback Callback;
- void* userData;
- u8 Data[0]; // Extra socket data starts here
-
- void Recv(const u8* data, int len)
- {
- Callback(ID,(SocketState)State,data,len,userData);
- }
-
- void SetState(SocketState state)
- {
- State = state;
- Callback(ID,(SocketState)State,0,0,userData);
- if (state == SocketState_Closed) {
- printf("Socket %d has been freed\n", ID);
- ID = 0;
- }
- }
-};
-
-class SocketHandler
-{
- public:
- virtual int Create(SocketInternal* sock) { printf("SocketHandler::Create: not implemented for %s\n", Name()); return sock->ID;}
- virtual int Open(SocketInternal* sock, SocketAddrHdr* addr) = 0;
- virtual int Send(SocketInternal* sock, const u8* data, int len) = 0;
- virtual int Close(SocketInternal* sock) = 0;
- virtual int Listen(SocketInternal* sock, int channel) { printf("SocketHandler::Listen: not implemented for %s\n", Name());return 0;}
- virtual int Accept(SocketInternal* sock, int scid, int rxid) { printf("SocketHandler::Accept: not implemented for %s\n", Name());return 0;}
- virtual int SetOpt(SocketInternal* sock, int so, int* data, int len) { printf("SocketHandler::SetOpt: not implemented for %s\n", Name());return 0;}
- virtual int GetOpt(SocketInternal* sock, int so, int* data, int len) { printf("SocketHandler::GetOpt: not implemented for %s\n", Name());return 0;}
- virtual char* Name() { return "Base_SocketHandler";}
-};
-
-int RegisterSocketHandler(int type, SocketHandler* handler);
-SocketInternal* GetSocketInternal(int socket);
-
-#define ERR_SOCKET_TYPE_NOT_FOUND -200
-#define ERR_SOCKET_NOT_FOUND -201
-#define ERR_SOCKET_NONE_LEFT -202
-#define ERR_SOCKET_CANT_LISTEN -203
-
-#endif // SOCKET_H_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TestShell.cpp Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,314 @@
+
+/*
+Copyright (c) 2010 Peter Barrett
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*/
+#include "mbed.h"
+#include <vector>
+#include "Utils.h"
+#include "USBHost.h"
+#include "hci.h"
+#include "HCITransportUSB.h"
+#include "RFCOMM.h"
+#include "ftclasslibusbdevbt.h"
+#include "sdp_data.h"
+#include "sdp.h"
+#include "btserial.h"
+#include "neighbourhood.h"
+
+/************************************************
+TODO:
+mtu and credits are completely unhandled - in progress
+multiple rfcomm sessions should be possible - done
+SDP would be nice - beta
+multiple rfcomm channels are untested
+decoupling of rfcomm and application - much better
+packets are not reassembled - some are (HCI and ft application level)
+disconnect and timeouts
+************************************************/
+#define DEBUG 1
+int state = 0;
+
+//int bulk = 0;
+void printf(const BD_ADDR* addr) {
+ const u8* a = addr->addr;
+ printf("%02X:%02X:%02X:%02X:%02X:%02X",a[5],a[4],a[3],a[2],a[1],a[0]);
+}
+
+const char FtDevClass[3] = {0x00, 0x1F, 0x82 };
+const char SerDevClass[3] = {4, 1, 0x00};
+// Instance
+//RFCOMMManager rfcomm_manager;
+
+class application : public HCI {
+ BTDevice* devs[8];
+ int count, i, pending;
+public:
+ // We have connected to a device
+ void ConnectionComplete(connection_info* info) {
+ printf("ConnectionComplete ");
+ BD_ADDR* a = &info->bdaddr;
+ printf(a);
+ printf("\n");
+ RemoteNameRequest(a);
+ for (i++; i < count; i++) {//find the next device to open
+ printfBytes("DEVICE CLASS",devs[i]->_info.dev_class,3);
+ if (devs[i]->_handle == 0 && memcmp(devs[i]->_info.dev_class, FtDevClass, 3)==0) {//or some other way to connect to RFCOMM devices
+ BD_ADDR* bd = &devs[i]->_info.bdaddr;
+ printf("Connecting to ");
+ printf(bd);
+ printf("\n");
+ pending++;
+ CreateConnection(bd); //some low level connect, just let it happen for now (sets pin, mtu etc.)
+ printf("connection cmd was sent\n");
+ return;
+ }
+ }
+ //state = 1; //start the real application
+ }
+
+ void ConnectDevices() {
+ count = GetDevices(devs,8);//get pointers to all bluetooth devices
+ pending = 0;
+ for (i = 0; i < count; i++) {
+ printfBytes("DEVICE CLASS",devs[i]->_info.dev_class,3);
+ if (devs[i]->_handle == 0 /*&& memcmp(devs[i]->_info.dev_class, FtDevClass, 3)==0*/) {//or some other way to connect to RFCOMM devices
+ BD_ADDR* bd = &devs[i]->_info.bdaddr;
+ printf("Connecting to ");
+ printf(bd);
+ printf("\n");
+ pending++;
+ CreateConnection(bd); //some low level connect, just let it happen for now (sets pin, mtu etc.)
+ printf("connection cmd was sent\n");
+ return;
+ }
+ }
+ if (pending == 0) state = 1;//for the case when there are no ft devices
+ }
+ virtual void Callback(HCI_CALLBACK_EVENT c, const u8* data, int len);
+ int csr_write_bd_addr(BD_ADDR *bdaddr, bool transient=true);
+ int csr_reset_device(bool transient=true);
+} App; //application instance
+
+extern "C" void mbed_reset();
+
+
+void application::Callback(HCI_CALLBACK_EVENT evt, const u8* data, int len) {//these events are forwarded (in)directly from HCIRecv
+ unsigned char pin[] = "1234";
+ unsigned char newaddr[] = {0x2c, 0x07, 0x54, 0x7b, 0x13, 0x00};//possible ft TX address (ROBO TX-277)
+// unsigned char newaddr[] = {0x57, 0x0a, 0x3d, 0x83, 0x15, 0x00};//original address of the cheap round BT dongle
+ printf("\x1b[%dm", 33);
+ switch (evt) {
+ case CALLBACK_READY:
+ printf("CALLBACK_READY\n");
+ printf("my address = ");
+ printf((BD_ADDR*)data);
+ if (memcmp(newaddr, data, 6) != 0) { //csr_write_bd_addr((BD_ADDR*)newaddr, false);
+ }
+ Inquiry();//start the second phase of the discovery
+ break;
+
+ case CALLBACK_INQUIRY_RESULT: //optionally build the list of FT devices here
+ printf("CALLBACK_INQUIRY_RESULT ");
+ printf((BD_ADDR*)data);
+ printf("\n");//data points to inquiry_info struct
+// RemoteNameRequest((BD_ADDR*)data);
+ break;
+
+ case CALLBACK_INQUIRY_DONE:
+ printf("CALLBACK_INQUIRY_DONE\n");
+ neighbors = new neighbourhood(&App);
+ neighbors->read();
+ ConnectDevices();
+ break;
+
+ case CALLBACK_REMOTE_NAME: {
+ BD_ADDR* addr = (BD_ADDR*)data;
+ const char* name = (const char*)(data + 6);
+ printf(addr);
+ printf(" = % s\n",name);
+ pending--;
+ if (pending == 0) state = 1;
+ }
+ break;
+
+ case CALLBACK_CONNECTION_COMPLETE: {
+ connection_info *ci = (connection_info*)data;
+ if (ci->status>0) {
+ printf("Connection failed, status=0x%02X\n", ci->status);
+ break;
+ }
+ ConnectionComplete(ci);
+ printf("Going to open sdp socket\n");
+ L2CAPAddr addr;
+ memcpy(&addr.bdaddr, &ci->bdaddr, 6);
+ //int s = SDP.Open(&addr.hdr);
+ }
+ break;
+ case CALLBACK_PIN_REQ:
+ printf("Enter PIN for ");
+ printf((BD_ADDR*)data);
+ printf(" : submitting %s\n", pin);
+ //USBLoop(); wait(1.0); USBLoop();
+ //for(int k=0; k<2000000;k++) USBLoop();
+ PinCodeReply(data, pin);
+ break;
+ case CALLBACK_VENDOR:
+ printfBytes("Vendor Reply:", data, len);
+ //mbed_reset();
+ if (data[0] == 0xc2)
+ csr_reset_device(false);
+ break;
+ default:
+ printf("Unhandled HCI Callback %d\n", evt);
+ };
+ printf("\x1b[%dm", 0);
+}
+
+#define CSR_WRITE 0xFC00
+
+int application::csr_write_bd_addr(BD_ADDR *bdaddr, bool transient) {
+ unsigned char cmd[] = { 0xc2,
+ 0x02, 0x00, 0x0c, 0x00, 0x11, 0x47, 0x03, 0x70,
+ 0x00, 0x00, 0x01, 0x00, 0x04, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+ };
+
+ if (transient)
+ cmd[15] = 0x08;
+
+ cmd[17] = bdaddr->addr[2];
+ cmd[18] = 0x00;
+ cmd[19] = bdaddr->addr[0];
+ cmd[20] = bdaddr->addr[1];
+ cmd[21] = bdaddr->addr[3];
+ cmd[22] = 0x00;
+ cmd[23] = bdaddr->addr[4];
+ cmd[24] = bdaddr->addr[5];
+
+ return SendCmd(CSR_WRITE, cmd, sizeof(cmd));
+}
+
+int application::csr_reset_device(bool transient) {
+ unsigned char cmd[] = { 0xc2, 0x02, 0x00, 0x09, 0x00,
+ 0x00, 0x00, 0x01, 0x40, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
+ };
+
+ if (transient)
+ cmd[7] = 0x02;
+
+ return SendCmd(CSR_WRITE, cmd, sizeof(cmd));
+}
+
+
+// these should be placed in the DMA SRAM
+typedef struct {
+ u8 _hciBuffer[MAX_HCL_SIZE];
+ u8 _aclBuffer[MAX_ACL_SIZE];
+} SRAMPlacement;
+
+HCITransportUSB _HCITransportUSB; //use USB as the transport to the radio
+
+int OnBluetoothInsert(int device) {//install the HCI and start discovery, user callbacks are made to HciCalback
+ printf("Bluetooth inserted of %d\n",device);
+ u32 sramLen;
+ u8* sram = USBGetBuffer(&sramLen);
+ sram = (u8*)(((u32)sram + 1023) & ~1023);
+ SRAMPlacement* s = (SRAMPlacement*)sram;
+ _HCITransportUSB.Open(device,s->_hciBuffer,s->_aclBuffer);//setup buffers for USB host, incoming data goes first to HCIRecv and ACLRecv
+ RegisterSocketHandler(SOCKET_L2CAP,&App); //register the application::hci as handler for L2CAP events
+ RegisterSocketHandler(SOCKET_RFCOM, &rfcomm_manager);//set the RFCOMMManager as the RFCOM socket handler
+ if (RegisterSocketHandler(SOCKET_SDP, &SDP))
+ printf("Could not register SDP socket type\n");
+ App.Open(&_HCITransportUSB);//the callback is virtual
+ App.Inquiry();//start discovery of BT devices phase 0
+ return 0;
+}
+
+DigitalOut led(LED1), loop(LED2);
+
+int comm = 0;
+btserial *incoming, *outgoing;
+
+void echo(int socket, SocketState state, const u8* data, int len, void* userData) {
+ const u8 connack[] = { 0xbe, 0xef, 8, 'C','O','N','N','_','A','C','K', 0x11};
+ printf("Echo: socket %d, state %d, len=%d\n", socket, state, len);
+ if (state==SocketState_Open) {
+ if (len == 0) {
+ printf("Sending CONN_ACK\n");
+ Socket_Send(socket, connack, sizeof(connack));
+ } else {
+ //Socket_Send(socket, data, len);
+ printfBytes("echo:", data, len);
+ }
+ }
+}
+
+void TestShell() {
+ int n=0;
+ USBInit();
+ for (;;) {
+ switch (state) {
+ case 0: //inquiry and low-level connection
+ break;
+ case 1: {//initialisation
+ printf("Ready to open ports\n");
+ InitFtBtDeviceList();
+ int n = GetNrOfFtBtDevices();
+ printf("%d ft BT devices have been found\n", n);
+ if (n > 0) {
+ ftbtdev *d = GetFtUsbDeviceHandle(0);
+ if (d==0) printf("could not get device handle\n");
+ int sock = OpenFtBtDevice(d);
+ }
+ state = 2;
+ //comm = Socket_Listen(SOCKET_RFCOM, 1, echo, 0);
+ //incoming = new btserial(1);
+ }
+ break;
+ case 2://main loop
+ /* if (incoming->readable()>0){
+ int c= incoming->getc();
+ putc(c, stderr);
+ incoming->putc(c);
+ }
+ else if (incoming->readable()<0){
+ state = 3;
+ printf("end of session");
+ delete incoming;
+ }*/
+ break;
+ default:
+ break;
+ }
+ loop=1;
+ USBLoop();
+ loop=0;
+ n++;
+ if (n>=500000) {
+ n=0;
+ led = !led;
+ }
+ }
+ //printf("Dropped out of main loop!\n");
+}
+
+//********************************************************************************************************************************
--- a/Utils.cpp Sun Jun 19 19:32:51 2011 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,49 +0,0 @@
-
-
-#include "mbed.h"
-#include "Utils.h"
-
-void printfBytes(const char* s, const u8* data, int len)
-{
- printf("%s %d:",s,len);
- if (len > 256)
- len = 256;
- while (len-- > 0)
- printf(" %02X",*data++);
- printf("\n");
-}
-
-void printHexLine(const u8* d, int addr, int len)
-{
- printf("%04X ",addr);
- int i;
- for (i = 0; i < len; i++)
- printf("%02X ",d[i]);
- for (;i < 16; i++)
- printf(" ");
- char s[16+1];
- memset(s,0,sizeof(s));
- for (i = 0; i < len; i++)
- {
- int c = d[i];
- if (c < 0x20 || c > 0x7E)
- c = '.';
- s[i] = c;
- }
- printf("%s\n",s);
-}
-
-void printHex(const u8* d, int len)
-{
- int addr = 0;
- while (len)
- {
- int count = len;
- if (count > 16)
- count = 16;
- printHexLine(d+addr,addr,count);
- addr += 16;
- len -= count;
- }
-}
-
--- a/Utils.h Sun Jun 19 19:32:51 2011 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,42 +0,0 @@
-#ifndef UTILS_H
-#define UTILS_H
-
-typedef unsigned char u8;
-typedef unsigned short u16;
-typedef unsigned long u32;
-
-void DelayMS(int ms);
-
-void printfBytes(const char* label,const u8* data, int len);
-void printHex(const u8* d, int len);
-//void printf(const BD_ADDR* addr);
-
-#ifndef min
-#define min(_a,_b) ((_a) < (_b) ? (_a) : (_b))
-#endif
-
-inline int LE16(const u8* d)
-{
- return d[0] | (d[1] << 8);
-}
-
-inline u32 BE32(const u8* d)
-{
- return (d[0] << 24) | (d[1] << 16) | (d[2] << 8) | d[3];
-}
-
-inline void BE32(u32 n, u8* d)
-{
- d[0] = (u8)(n >> 24);
- d[1] = (u8)(n >> 16);
- d[2] = (u8)(n >> 8);
- d[3] = (u8)n;
-}
-
-inline void BE16(u32 n, u8* d)
-{
- d[0] = (u8)(n >> 8);
- d[1] = (u8)n;
-}
-
-#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/btserial.cpp Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,116 @@
+#include "btserial.h"
+
+btserial::btserial(char ba[6], char ch) {
+ L2CAPAddr a;
+ memcpy(&a.bdaddr, ba, 6);
+ a.psm = ch;
+ sendptr = 0;
+ recptrin = 0;
+ recptrout = bufsize - 1;
+ free = bufsize;
+ sock = Socket_Open(SOCKET_RFCOM, &a.hdr, cb, this);
+}
+
+btserial::btserial(char ch) {
+ sendptr = 0;
+ recptrin = 0;
+ recptrout = bufsize - 1;
+ free = bufsize;
+ sock = Socket_Listen(SOCKET_RFCOM, ch, cb, this);
+}
+
+void btserial::cb(int socket, SocketState state, const unsigned char *data, int len, void* userData) {
+ btserial *self = (btserial*)userData;
+ if (state == SocketState_Open)
+ if (len > 0)
+ self->stash(data, len);
+ else {
+ //Socket_GetOpt(sock, SO_RECBUF, &recbufsize, sizeof(recbufsize));
+ //Socket_GetOpt(sock, SO_SNDBUF, &sndbufsize, sizeof(sndbufsize));
+ port_settings ps;//defaults are ok
+ ps.baud = 3; //9600 baud
+ ps.bits = 3;//8 bits
+ ps.stop = 0;//1 bit
+ ps.par = 0; //no parity
+ ps.mask = MASK_BITRATE|MASK_DATABITS|MASK_STOPBITS|MASK_PARITYBITS;
+ //set_remote_port_parameters(sock, &ps);
+ self->open = true;
+ }
+ else if (state == SocketState_Closed)
+ self->open = false;
+}
+
+void btserial::stash(const unsigned char *data, int len) {
+ int i = 0;
+ while (i < len && free>0) {
+ recbuf[recptrin++] = data[i++];
+ if (recptrin == bufsize) recptrin = 0;
+ free--;
+ }
+}
+
+int btserial::getc() {
+ if (free == bufsize || !open)
+ return -1;
+ free++;
+ recptrout++;
+ if (recptrout == bufsize) recptrout = 0;
+ return recbuf[recptrout];
+}
+
+int btserial::putc(int c) {
+ if (sendptr==bufsize || !open)
+ return -1;
+ sendbuf[sendptr++] = c;
+ if (sendptr==bufsize || c=='\n' || c=='\r') {
+ Socket_Send(sock, sendbuf, sendptr);
+ sendptr = 0;
+ }
+ return c;
+}
+
+void btserial::baud(int br) {
+ int rates[] = {2400,4800,7200,9600,19200,38400,57600,115200,230400};
+ if (!open) return;
+ for (int i = 0; i < sizeof(rates)/sizeof(int);i++)
+ if (rates[i] == br) {
+ port_settings ps;
+ ps.baud = i;
+ ps.mask = MASK_BITRATE;
+ set_remote_port_parameters(sock, &ps);
+ return;
+ }
+ printf("Illegal baudrate requested %d\n", br);
+}
+
+void btserial::format(int bits, Serial::Parity par, int stop) {
+if (!open) return;
+ port_settings ps;
+ ps.bits = bits-5;
+ ps.stop = stop-1;
+ switch (par) {
+ case Serial::None:
+ ps.par = 0;
+ ps.par_t = 0;
+ break;
+ case Serial::Odd:
+ ps.par = 1;
+ ps.par_t = 0;
+ break;
+ case Serial::Even:
+ ps.par = 1;
+ ps.par_t = 1;
+ break;
+ case Serial::Forced0:
+ ps.par = 1;
+ ps.par_t = 3;
+ break;
+ case Serial::Forced1:
+ ps.par = 1;
+ ps.par_t = 2;
+ break;
+ }
+ ps.mask = MASK_DATABITS|MASK_STOPBITS|MASK_PARITYBITS|MASK_PARITYTYPE;
+ set_remote_port_parameters(sock, &ps);
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/btserial.h Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,32 @@
+#ifndef BTSERIAL_H
+#define BTSERIAL_H
+#include "mbed.h"
+#include "RFCOMM.h"
+
+class btserial {
+ static const int bufsize = 350;
+ int recbufsize, sndbufsize;
+ int sock;
+ unsigned char sendbuf[bufsize], recbuf[bufsize];
+ int sendptr, recptrin, recptrout, free;
+ static void cb(int socket, SocketState state, const unsigned char *data, int len, void* userData);
+ void stash(const unsigned char *data, int len);
+ bool open;
+public:
+ btserial(char ba[6], char ch);//outgoing
+ btserial(char ch);//incoming
+ void baud(int);
+ void format(int, Serial::Parity, int);
+ int putc(int);
+ int getc();
+ int readable() {
+ if (!open) return -1;
+ return bufsize-free;
+ }
+ int writeable() {
+ if (!open) return -1;
+ return bufsize - sendptr;
+ }
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ftclasslibusbdevbt.cpp Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,134 @@
+#include "mbed.h"
+#include <vector>
+#include "Utils.h"
+#include "hci.h"
+#include "ftclasslibusbdevbt.h"
+
+//extern HCI* gHCI;
+class application;
+extern application App;
+
+void printf(const BD_ADDR* addr);
+
+void ftdev::receive(int socket, SocketState state, const u8* data, int len) {
+ printf("ftdev::receive was called: socket %d, state=%d, length=%d\n", socket, state, len);
+// unsigned char req[] = "\xdget_ser_num\xd";
+ unsigned char req[] = {0xbe, 0xef, 6, 0xfc, 0,0,0,0,0, 0xaf};
+ if (len==0) {
+ switch (state) {
+ case SocketState_Opening:
+ break;
+ case SocketState_Open:
+ printf("sending request \n%s\n", req);
+ Socket_Send(sock, req, sizeof(req));
+ break;
+ case SocketState_Closing:
+ case SocketState_Closed:
+ return;
+ }
+ } else {
+ //printHex(data, len);
+ parse(data, len);
+ if (state==SocketState_Open)
+ ;//Socket_Close(sock);//replace with ft primitive
+ }
+}
+
+unsigned short ftdev::chksum() {
+ unsigned short sum = (X1_len & 0xFF) + (X1_len >> 8);
+ for (int i = 0; i < X1_len; i++)
+ sum += X1_pkt[i];
+ return -sum;
+}
+
+void ftdev::parse (const unsigned char *buf, unsigned len) {
+ unsigned i = 0;
+ while (i < len) {
+ char c = buf[i++];
+ switch (parseState) {
+ case 0: //ascii state
+ if (c==2)
+ parseState = 1;
+ else
+ putc(c, stdout);
+ break;
+ case 1:
+ if (c==0x55)
+ parseState = 2;
+ else {
+ parseState = 0;
+ printf("expected 0x55 in X1 header, found %02X\n", c);
+ }
+ break;
+ case 2:
+ X1_len = c<<8;
+ parseState= 3;
+ break;
+ case 3:
+ X1_len += c;
+ parseState= 4;
+ X1_pkt = new unsigned char[X1_len];
+ X1_pos = 0;
+ break;
+ case 4:
+ if (X1_pos < X1_len) X1_pkt[X1_pos++] = c;
+ else parseState = 5;
+ break;
+ case 5:
+ X1_crc = c<<8;
+ parseState= 6;
+ break;
+ case 6:
+ X1_crc += c;
+ parseState= 7;
+ break;
+ case 7:
+ if (c == 3 && X1_crc == chksum()) {
+ //handlePkt();
+ printHex(X1_pkt, X1_len);
+ }else
+ printf("framing or checksum error, end char = %02X\n", c);
+
+ parseState = 0;
+ break;
+ }
+ }
+}
+
+vector<ftbtdev*> ft_devs;
+ftdev _ftdev; //single instance, just for test
+
+int GetNrOfFtBtDevices() {
+ return ft_devs.size();
+}
+
+unsigned InitFtBtDeviceList() {//assume inquiry has been done
+ static char FtDevClass[3] = {0x00, 0x1F, 0x82 };
+ BTDevice* devs[8];
+ int count = ((HCI*)&App)->GetDevices(devs,8);
+ int n = 0;
+ for (int i = 0; i < count; i++) {
+ if (memcmp(devs[i]->_info.dev_class, FtDevClass, 3)==0) {
+ ft_devs.push_back(new ftbtdev(&devs[i]->_info));
+ printf("%d: %s\n", n++, devs[i]->_name);
+ }
+// printf("device %d (handle=%d) ", i, devs[i]->_handle);
+// printfBytes("devclass: ", devs[i]->_info.dev_class, 3);
+ }
+ return n;
+}
+
+ftbtdev* GetFtUsbDeviceHandle(unsigned Num) {
+ if (Num < ft_devs.size()) {
+ return ft_devs[Num];
+ }
+ return 0;
+}
+
+unsigned OpenFtBtDevice(ftbtdev* d) {
+ BD_ADDR* bd = d->BtAddr();
+ printf("Connecting to ");
+ printf(bd);
+ printf("\n");
+ return _ftdev.Open(bd, 1); //TODO: everything can go wrong here
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ftclasslibusbdevbt.h Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,47 @@
+#ifndef FTCLASSLIBUSBDEVBT_H
+#define FTCLASSLIBUSBDEVBT_H
+
+
+class ftbtdev {//small object for ft BT enumeration
+ inquiry_info info;
+public:
+ ftbtdev(inquiry_info* ii) {
+ info = *ii;
+ }
+ BD_ADDR* BtAddr() {
+ return &info.bdaddr;
+ }
+};
+
+class ftdev {//this should in the future encapsulate the real TXC
+ int sock;
+ int parseState;
+ unsigned short X1_crc, X1_len, X1_pos;
+ unsigned char *X1_pkt;
+ unsigned short chksum();
+ void parse(const unsigned char *, unsigned);
+public:
+ ftdev(): sock(0) { parseState = 0;}
+ int Open(BD_ADDR *bt_addr, int chan=1, SocketCallback cb=&ftdev::recv) {
+ L2CAPAddr s;
+ s.bdaddr = *bt_addr;
+ s.psm = chan;//abuse the psm for the channelID
+ sock = Socket_Open(SOCKET_RFCOM, &s.hdr, cb, this);//Open the serial connection via RFCOMM
+ if (sock<=0)
+ printf("Opening of RFCOMM socket for ftdevice failed (%d)\n", sock);
+ return sock;
+ }
+ static void recv(int socket, SocketState state, const u8* data, int len, void* userData) {
+ if (userData) ((ftdev*)userData)->receive(socket, state, data, len);
+ }
+ void receive(int socket, SocketState state, const u8* data, int len);// {printf("ftdev::receive was called: socket %d, state=%d, length=%d\n", socket, state, len);}
+};
+
+extern ftdev _ftdev;
+
+unsigned InitFtBtDeviceList();
+int GetNrOfFtBtDevices();
+ftbtdev* GetFtUsbDeviceHandle(unsigned Num);
+unsigned OpenFtBtDevice(ftbtdev* d);
+
+#endif
--- a/hci.cpp Sun Jun 19 19:32:51 2011 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,605 +0,0 @@
-
-/*
-Copyright (c) 2010 Peter Barrett
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-*/
-
-#include <stdio.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-
-#include "Utils.h"
-#include "hci.h"
-#include "hci_private.h"
-#include "USBHost.h" //for USBLoop
-#include "HCITransportUSB.h" //for ACL/HCL buffer size
-#include "neighbourhood.h"
-
-extern const char FtDevClass[];
-const char FtDevClass[3] = {0x00, 0x1F, 0x82 };
-
-enum hci_callback_evt {
- NONE,
- CONNECT,
- DISCONECT,
- INQUIRYRESULT
-};
-
-#define MAX_BLUETOOTH_ADAPTERS 1
-
-enum StateMask {
- MASK_RESET = 1,
- MASK_READ_BUFFER_SIZE = 2,
- MASK_READ_BD_ADDR = 4,
- MASK_INITED = 8,
- MASK_INQUIRY = 16,
- MASK_REMOTE_NAME = 32,
- MASK_CREATE_CONNECTION = 64
-};
-
-//static const u8 local_name[] = "MBED";
-static const u8 local_name[] = "ROBO TX-599";
-
-int HCI::Open(HCITransport* transport, HCICallback callback) {
- _transport = transport;
- _transport->Set(this);
- _callback = callback;
- _state = 0;
- for (int i = 0; i < MAX_BTDEVICES; i++) {
- _devices[i].Init();
- _devices[i]._transport = transport;
- }
-#ifdef COMMAND_FLOW
- cmd_credits = 1;
-#endif
- return SendCmd(HCI_OP_RESET);
-}
-
-void printf(const BD_ADDR* addr);
-
-BTDevice* HCI::Find(const BD_ADDR* addr) {
- for (int i = 0; i < MAX_BTDEVICES; i++)
- if (_devices[i]._state != 0 && memcmp(addr,&_devices[i]._info.bdaddr,6) == 0)
- return &_devices[i];
- return 0;
-}
-
-BTDevice* HCI::Find(int handle) {
- for (int i = 0; i < MAX_BTDEVICES; i++)
- if (_devices[i]._state != 0 && handle == _devices[i]._handle)
- return &_devices[i];
- return 0;
-}
-
-//reports that some commands are still in progress
-bool HCI::Busy() {
- return (_state & (MASK_INQUIRY | MASK_REMOTE_NAME | MASK_CREATE_CONNECTION)) != 0;
-}
-
-int HCI::Inquiry(int duration) {
- _state |= MASK_INQUIRY;
- u8 buf[5];
- buf[0] = 0x33;//LAP=0x9e8b33
- buf[1] = 0x8B;
- buf[2] = 0x9E;
- buf[3] = duration;
- buf[4] = 5; // 5 results
- SendCmd(HCI_OP_INQUIRY,buf,sizeof(buf));
- return 0;
-}
-
-int HCI::SetEventFilter(u8 filterType, u8 filterConditionType, u8* condition) {
- int len = 2;
- u8 buf[8];
- buf[0] = filterType;
- buf[1] = filterConditionType;
- switch (filterConditionType) {
- case 0://all devices
- if (filterType==2) { //connection setup
- buf[2] = condition[0];
- len++;
- }
- break;
- case 1: //filter by class
- case 2: //filter by BDADDR
- memcpy(buf+2, condition, 6);
- len += 6;
- break;
- default:
- printf("Unknown filter condition type %d, filter type=%d\n", filterConditionType, filterType);
- }
- SendCmd(HCI_OP_SET_EVENT_FLT, buf, len);
- return 0;
-}
-
-int HCI::SendCmd(int cmd, const u8* params, int len) {
- u8 b[256];
- b[0] = cmd;
- b[1] = (cmd >> 8);
- b[2] = len;
- if (params)
- memcpy(b+3,params,len);
-#ifdef COMMAND_FLOW
- //printf("%d cmd_credits\n", cmd_credits);
- while (cmd_credits == 0) {//blocks when command credits run out
- USBLoop();
- putc('_', stdout);
- }
-#endif
- _transport->HCISend(b,len+3);
- return 0;
-}
-
-void HCI::OnCommandComplete(int cmd, const u8* data, int len) {//data is exclusive the status byte
- //printf("%04X %s",cmd,CmdStr(cmd));
- if (len < 0)
- return;
- //printfBytes(" complete",data,len/*min(16,len)*/);
-
- switch (cmd) {
- case 0: //NOP
- printf("Received NOP command (for cmd_credits)\n");
- break;
- // Init phase 0
- case HCI_OP_RESET: // Reset done, init chain to HCI_OP_READ_LOCAL_NAME
- SendCmd(HCI_OP_READ_BUFFER_SIZE);
- _state |= MASK_RESET;
- break;
-
- // Init phase 1
- case HCI_OP_READ_BUFFER_SIZE:
- _acl_mtu = LE16(data);
- _sco_mtu = data[2];
- _acl_max_pkt = LE16(data+3);
- _sco_max_pkt = LE16(data+5);
- printf("acl_mtu=%d, acl_max_pkt=%d\n", _acl_mtu, _acl_max_pkt);
-#ifdef HOST_CONTR_FLOW
- _transport->data_credits = _acl_max_pkt;
- _transport->_acl_mtu = _acl_mtu;
-#endif
- SendCmd(HCI_OP_READ_BD_ADDR);
- _state |= MASK_READ_BUFFER_SIZE;
- break;
-
- // Init phase 2
- case HCI_OP_READ_BD_ADDR:
- _localAddr = *((BD_ADDR*)data); // Local Address
- _state |= MASK_READ_BD_ADDR;
- _state |= MASK_INITED;
- {
-#ifdef CONTR_HOST_FLOW
- unsigned char param[7];
- param[0] = (u8)(MAX_ACL_SIZE-8);
- param[1] = (u8)((MAX_ACL_SIZE-8)>>8);
- param[2] = 0;//MAX_HCL_SIZE-8;
- param[3] = 10;
- param[4] = 0; //1 ACL buffer
- param[5] = 0;
- param[6] = 0; //0 Synchronous buffers
- SendCmd(HCI_OP_HOST_BUFFER_SIZE, param, 7);
- const unsigned char flow = 1;//ACL on, Synchonous off
- SendCmd(HCI_OP_CONTR_TO_HOST_FLOW, &flow, 1);
-#endif
- const unsigned char scan_enable = 3;
- SendCmd(HCI_OP_WRITE_SCAN_ENABLE, &scan_enable, 1);
- SendCmd(HCI_OP_WRITE_CLASS_OF_DEV, (const u8*)FtDevClass, 3);
- //SendCmd(HCI_OP_READ_LOCAL_VERSION, 0, 0);
- SendCmd(HCI_OP_WRITE_LOCAL_NAME, local_name, 248);
- }
- Callback(CALLBACK_READY,data,6);
- break;
-
- // 0CXX
- case HCI_OP_READ_LOCAL_NAME:
- case HCI_OP_LINK_KEY_NEG_REPLY:
- case HCI_OP_WRITE_SCAN_ENABLE:
- case HCI_OP_WRITE_LOCAL_NAME:
- break;
-#ifdef CONTR_HOST_FLOW
- case HCI_OP_CONTR_TO_HOST_FLOW:
- case HCI_OP_HOST_BUFFER_SIZE:
- break;
- case HCI_OP_NUM_COMP_PKTS:
- printf("Host number of Completed Packets: Invalid HCI Command Parameter\n");
- break;
-#endif
- case HCI_OP_READ_LOCAL_VERSION:
- // params
- //SendCmd(HCI_OP_READ_LOCAL_NAME);
- break;
-
- case HCI_OP_READ_LOCAL_COMMANDS:
- break;
-
- case HCI_OP_READ_LOCAL_FEATURES:
- //SendCmd(HCI_OP_READ_LOCAL_VERSION);
- break;
-
- case HCI_OP_READ_LOCAL_EXT_FEATURES:
- break;
-
- case HCI_OP_PIN_CODE_REPLY:
- printf("Got pin reply\n");
- break;
- case HCI_READ_STORED_LINK_KEY:
- neighbors->set_cap(LE16(data), LE16(data+2));
- break;
-
- default:
- printf("Unrecognized Command Completion %04X\n",cmd);
- break;
- }
-}
-
-void HCI::Callback(HCI_CALLBACK_EVENT c, const u8* data, int len) {
- if (_callback) _callback(this,c,data,len);
-// (this->*_callback)(c, data, len);
-}
-
-int HCI::RemoteNameRequest(const BD_ADDR* addr) {
- _state |= MASK_REMOTE_NAME;
- u8 buf[6+4];
- memset(buf,0,sizeof(buf));
- memcpy(buf,addr,6);
- //buf[7] = 1;
- return SendCmd(HCI_OP_REMOTE_NAME_REQ,buf,sizeof(buf));
-}
-
-int HCI::RemoteNameRequest(inquiry_info *ii) {
- _state |= MASK_REMOTE_NAME;
- u8 buf[6+4];
- //memset(buf,0,sizeof(buf));
- memcpy(buf,&ii->bdaddr,6);
- buf[6] = ii->pscan_rep_mode;
- buf[7] = 0;
- *(unsigned short*)(buf+8) = 0;
- return SendCmd(HCI_OP_REMOTE_NAME_REQ,buf,sizeof(buf));
-}
-
-int HCI::CreateConnection(const BD_ADDR* remoteAddr) {
- _state |= MASK_CREATE_CONNECTION;
- u8 buf[6+7];
- memset(buf,0,sizeof(buf));
- memcpy(buf,remoteAddr,6);
- buf[6] = 0x18; // DM1,DH1
- buf[7] = 0xCC; // DM3, DH3, DM5, DH5
- buf[8] = 1; // Page Repetition R1
- return SendCmd(HCI_OP_CREATE_CONN,buf,sizeof(buf));
-}
-
-int HCI::Disconnect(const BD_ADDR* bdaddr) {
- BTDevice* d = Find(bdaddr);
- if (!d)
- return ERR_HCI_DEVICE_NOT_FOUND;
- int handle = d->_handle;
- printf("Disconnect from %d\n",handle);
- _state |= MASK_CREATE_CONNECTION;
- u8 buf[3];
- buf[0] = handle;
- buf[1] = (handle >> 8);
- buf[2] = 0x13;
- return SendCmd(HCI_OP_DISCONNECT,buf,sizeof(buf));
-}
-
-void HCI::DisconnectComplete(int handle) {
- BTDevice* d = Find(handle);
- if (!d)
- return;
- d->_handle = 0;
-}
-
-int HCI::DisconnectAll() {
- BTDevice* devs[8];
- int count = GetDevices(devs,8);
- for (int i = 0; i < count; i++)
- Disconnect(&devs[i]->_info.bdaddr);
- return 0;
-}
-
-int HCI::PinCodeReply(const u8* data, const u8* pin) {
- u8 b[6+1+16];
- memset(b,0,sizeof(b));
- memcpy(b,data,6);
- b[6] = 4;
- memcpy(b+7, pin, 4);
- return SendCmd(HCI_OP_PIN_CODE_REPLY,b,sizeof(b));
-}
-
-void HCI::InquiryResult(const inquiry_info* info) {
- BTDevice* bt = Find(&info->bdaddr);
- if (!bt) { // new device
- for (int i = 0; i < MAX_BTDEVICES; i++) {
- if (_devices[i]._state == 0) {
- bt = _devices + i;
- bt->_state = 1;
- break;
- }
- }
- if (!bt) {
- printf("HCI::InquiryResult too many devices\n");
- return; // Too many devices!
- }
- }
-
- bt->_info = *info;
-}
-
-int HCI::GetDevices(BTDevice** devices, int maxDevices) {
- int j = 0;
- for (int i = 0; i < MAX_BTDEVICES; i++) {
- if (_devices[i]._state != 0) {
- devices[j++] = _devices + i;
- if (j == maxDevices)
- break;
- }
- }
- return j;
-}
-
-void HCI::RemoteName(const BD_ADDR* addr, const char* name) {
- BTDevice* d = Find(addr);
- if (d) {
- strncpy(d->_name,name,sizeof(d->_name)-1);
- d->_name[sizeof(d->_name)-1] = 0;
- }
-}
-
-void HCI::ConnectComplete(const connection_info* info) {
- BTDevice* d = Find(&info->bdaddr);
- if (!d) {
- printf("BT Device not known!?! ");
- printf(&info->bdaddr);
- printf("\n");
- return;
- }
- if (info->status == 0) {
- d->_handle = info->handle;
-#ifdef HOST_CONTR_FLOW
- d->pkts_sent = 0;
-#endif
- printf("Connected on %04X\n",info->handle);
- } else
- printf("Connection failed with %d\n",info->status);
-}
-
-void HCI::Accept_Connection(const BD_ADDR* addr, bool slave) {
- unsigned char b[7];
- memcpy(b, addr, 6);
- b[6] = slave;
- BTDevice* bt = Find(addr);
- if (!bt) {
- printf("Received connection request from undiscovered device\n");
- for (int i = 0; i < MAX_BTDEVICES; i++) {
- if (_devices[i]._state == 0) {
- bt = _devices + i;
- bt->_state = 1;
- memcpy(&(bt->_info.bdaddr), addr, 6);//rest of inquiry info unknown!!!
- break;
- }
- }
- if (!bt) {
- printf("HCI::InquiryResult too many devices\n");
- return; // Too many devices!
- }
- }
- SendCmd(HCI_OP_ACCEPT_CONN_REQ, b , 7);
-}
-
-void HCI::HCIRecv(const u8* data, int len) {//[0]=event, [1]=parlen, [2...]=pars
- printfBytes(EvtStr(data[0]),data,min(len,16));
- switch (data[0]) {
- case HCI_EV_INQUIRY_COMPLETE:
- printfBytes("Inquiry Complete",data,data[1]);
- _state &= ~MASK_INQUIRY;
- Callback(CALLBACK_INQUIRY_DONE,0,0);
- break;
-
- case HCI_EV_INQUIRY_RESULT: {
- const u8* end = data[1] + data + 2;
- data += 3;
- while (data < end) {
- inquiry_info align;
- memcpy(&align,data,sizeof(inquiry_info));
- InquiryResult(&align);
- Callback(CALLBACK_INQUIRY_RESULT,(u8*)&align,sizeof(inquiry_info));
- data += 14;
- }
- }
- break;
-
- case HCI_EV_CONN_COMPLETE:
- _state &= ~MASK_CREATE_CONNECTION;
- {
- connection_info align;
- memcpy(&align,data+2,sizeof(connection_info));
- ConnectComplete(&align);
- Callback(CALLBACK_CONNECTION_COMPLETE,(u8*)&align,sizeof(connection_info));
- }
- break;
-
- case HCI_EV_CONN_REQUEST:
- printf("Got Connection request \n");
- Callback(CALLBACK_CONNECTION_REQUEST, data+2, data[1]);
- Accept_Connection((BD_ADDR*)(data+2));
- break;
-
- case HCI_EV_DISCONN_COMPLETE:
- DisconnectComplete(LE16(data+3));
- break;
-
- case HCI_EV_REMOTE_NAME: {
- BD_ADDR* addr = (BD_ADDR*)(data+3);
- const char* name = (const char*)(data + 9);
- RemoteName(addr,name);
- }
- Callback(CALLBACK_REMOTE_NAME,data+3,LE16(data+1)); // addr is in here too
- _state &= ~MASK_REMOTE_NAME;
- break;
-
- case HCI_EV_CMD_STATUS: {
- const char* errs = HCIErrStr(data[2]);
- printf("Status %s %s %d cmd pkts\n",CmdStr(LE16(data+4)),errs, data[3]);
-#ifdef COMMAND_FLOW
- cmd_credits = data[3];
-#endif
- }
- Callback(CALLBACK_CMD_STATUS, data+2, 4);
- break;
-
- case HCI_EV_CMD_COMPLETE://[2]=cmd-pkts, [3-4]=cmd, [5...]=pars
- if (data[5]) { //[5]=usually status
- printf("HCIRecv error status: %s\n", HCIErrStr(data[5]));
- }
- OnCommandComplete(data[3] | (data[4] << 8), data+6, data[1]-4);
-#ifdef COMMAND_FLOW
- cmd_credits = data[2];
-#endif
- break;
-
- case HCI_EV_PIN_CODE_REQ:
- Callback(CALLBACK_PIN_REQ, data+2, 6);
- //PinCodeReply(data+2);
- break;
-
- case HCI_EV_LINK_KEY_REQ: {
- u8 param[22];
- if (neighbors->get((BD_ADDR*)(data+2), param+sizeof(BD_ADDR))){
- memcpy(param, data+2, sizeof(BD_ADDR));
- SendCmd(HCI_OP_LINK_KEY_REPLY,param,sizeof(param));
- } else
- SendCmd(HCI_OP_LINK_KEY_NEG_REPLY,data+2,6);
- }
- break;
-#ifdef HOST_CONTR_FLOW
- case HCI_EV_NUM_COMP_PKTS:
- for (int k = 0; k < data[2]; k++) {//data[2] and 'c' are usually 1
- u16 h = LE16(data+3+2*k);
- u16 c = LE16(data+5+2*k);
- BTDevice *d = Find(h);
- if (!d)
- continue;//skip no existing devices
- if (d->pkts_sent >= c) {
- d->pkts_sent -= c;
- _transport->data_credits += c;
- } else
- d->pkts_sent = 0;
- //printf("%d Outstanding pkts for handle %03X (total credits=%d)\n", d->pkts_sent, h, _transport->data_credits);
- }
- break;
-#endif
- case HCI_EV_LINK_KEY_NOTIFY:
- neighbors->add((BD_ADDR*)(data+2), data+8);
- break;
- case HCI_EV_RETURN_LINK_KEYS:
- for (int i = 0; i < data[2]; i++)
- neighbors->add((BD_ADDR*)(data+3+22*i), data+9+22*i, true);
- break;
- case HCI_EV_ENCRYPT_CHANGE:
- //for(int k=0; k<1000000;k++) USBLoop();
- break;
- case HCI_EV_VENDOR:
- Callback(CALLBACK_VENDOR, data+2, data[1]);
- break;
- default:
- printfBytes("HCIRecv:",data,data[1]+2);
- break;
- }
-}
-
-int HCI::Open(SocketInternal* sock, SocketAddrHdr* addr) {
- L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
- L2CAPAddr* l2capaddr = (L2CAPAddr*)addr;
- BTDevice* bt = Find(&l2capaddr->bdaddr);
- if (!bt) {
- printf("Can't open l2cap %d on ",l2capaddr->psm);
- printf(&l2capaddr->bdaddr);
- printf("\n");
- return ERR_HCI_DEVICE_NOT_FOUND;
- }
- l2capsock->btdevice = bt;
- return bt->Open(sock,addr);
-}
-
-int HCI::Accept(SocketInternal* sock, int scid, int rxid) {
- L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
- BTDevice* bt = (BTDevice*)sock->userData;
- if (!bt) {
- printf("Can't accept l2cap on socket %d\n", sock->ID);
- return ERR_HCI_DEVICE_NOT_FOUND;
- }
- l2capsock->btdevice = bt;
- return bt->Accept(sock, scid, rxid);
-}
-
-int HCI::Send(SocketInternal* sock, const u8* data, int len) {//check here for appropriate buffersize on the device
- /* these checks are HCI functions but this 'Send' does not catch all ACL traffic, so it is better done in L2CAP or transport
- //assume acl packet
- //FIXME: treatment of OFFSET is dubious, OFFSET is not defined?!
- #if OFFSET==8 //sizeof ACL/L2CAP is include in data/len
- if (len > _acl_mtu)
- #else //OFFSET==0, data is bare application frame
- if (len+8 > _acl_mtu)
- #endif
- { printf("Max outgoing packet(%d) size exceeded, segmenting necessary, pktlen = %d\n", _acl_mtu, len);
- }
- if (data_credits == 0) {
- printf("Out of ACL data credits\n");
- return 0;
- }
- data_credits--;
- */
- L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
- return l2capsock->btdevice->Send(sock,data,len); // Pointless double dispatch
-}
-
-int HCI::Close(SocketInternal* sock) {
- L2CAPSocket* l2capsock = (L2CAPSocket*)sock;
- return l2capsock->btdevice->Close(sock); // Pointless double dispatch
-}
-
-void HCI::Compl_pkts(int handle, u8 p) {
- u8 b[8] = {(u8)HCI_OP_NUM_COMP_PKTS, HCI_OP_NUM_COMP_PKTS >> 8, 5, 1, 0, 0, 1, 0};
- b[4] = handle;
- b[5] = (handle&0x0f00)>>8;
- b[6] = p;//only one packet
- _transport->HCISend(b, 8);//directly call the transport layer to prevent the command flow control from interfering
-}
-
-void HCI::ACLRecv(const u8* data, int len) {
- int handle = LE16(data);
- BTDevice* d = Find(handle & 0x0FFF);
- int bufs = 1;
- if (!d) {
- printfBytes("unk. dest. HCI:ACLRecv ", data, len);
- } else
- bufs = d->ACLRecv(data,len);
- //controller to host flow control
-#ifdef CONTR_HOST_FLOW
-//the ACLRecv function returned so we assume that the buffer is free, and tell this to the controller
- if (bufs) {
- Compl_pkts(handle, bufs);//this packet is completed
- printf("%d ACL buffers completed\n", bufs);
- }
-#endif
-}
-
-//===================================================================
-//===================================================================
--- a/hci.h Sun Jun 19 19:32:51 2011 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,280 +0,0 @@
-/*
-Copyright (c) 2010 Peter Barrett
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-*/
-
-#ifndef HCI_H_INCLUDED
-#define HCI_H_INCLUDED
-
-//#define CONTR_HOST_FLOW //Controller to Host flow control
-#define HOST_CONTR_FLOW //Host to Controller flow control
-#define COMMAND_FLOW //Command flow control
-
-#include "mbed.h"
-#include "Socket.h"
-
-#pragma pack(1)
-
-#define ERR_HCI_DEVICE_NOT_FOUND -300
-
-class HCI;
-class HCITransport;
-class BTDevice;
-
-typedef struct
-{
- u8 addr[6];
-} BD_ADDR;
-
-typedef struct
-{
- BD_ADDR bdaddr;
- u8 pscan_rep_mode;
- u8 pscan_period_mode;
- u8 pscan_mode;
- u8 dev_class[3];
- u16 clock_offset;
-} inquiry_info;
-
-typedef struct
-{
- u8 status;
- u16 handle;
- BD_ADDR bdaddr;
- u8 link_type;
- u8 encr_mode;
-} connection_info;
-
-// Address struct for creating L2CAP sockets
-typedef struct {
- SocketAddrHdr hdr;
- BD_ADDR bdaddr;
- u16 psm;
-} L2CAPAddr;
-
-typedef struct {
- u16 handle;
- u16 length; // total
- u16 l2capLength; // length -4
- u16 cid; // Signaling packet CID = 1
-
- // Payload
- u8 cmd; //
- u8 id;
- u16 cmdLength; // total-8
- u16 params[4]; // Params
-} L2CAPCmd;
-
-#pragma pack(4)
-
-class BTDevice;
-
-typedef struct
-{
- public:
- SocketInternal si;
- BTDevice* btdevice;
- u16 scid;
- u16 dcid;
-} L2CAPSocket;
-
-#define MAX_HCL_NAME_LENGTH 20 // TODO - BTDevice wants to be a multiple of 4
-
-// BTDevice encapsulates individual device state
-// It provides L2CAP layer sockets
-
-class BTDevice : public SocketHandler
-{
- public:
- HCITransport* _transport;
- inquiry_info _info;
- u16 _handle; // acl connection handle
- u8 _state; // connection state
- u8 _txid;
- u16 peer_mtu;
-#ifdef HOST_CONTR_FLOW
- u8 pkts_sent; //host to controller flow control
-#endif
-//u8 cntr_cred;
-u8 segments;
- char _name[MAX_HCL_NAME_LENGTH];
-
- void Init();
-
- BD_ADDR* GetAddress() { return &_info.bdaddr; }
-
- // Called from HCI
- int ACLRecv(const u8* data, int len);
- void ACLFwd(const u8* data, int len);
-
- // SocketHandler
- virtual int Open(SocketInternal* sock, SocketAddrHdr* addr);
- virtual int Accept(SocketInternal* sock, int scid, int rxid);
- virtual int Send(SocketInternal* sock, const u8* data, int len);
- virtual int Close(SocketInternal* sock);
- virtual char* Name() { return "BTDevice SocketHandler";}
-
-private:
- int l2cap_sock, plen, contPos, contState;
- unsigned char *contBuf;
- Timeout rtx;
- L2CAPCmd last_req;
- void repeat_cmd();
-
- L2CAPSocket* SCIDToSocket(int scid);
- int Send(const u8* data, int len);
- int Send(u8 c, u8 id, u16* params, int count);
- int Connect(int scid, int psm);
- int Disconnect(int scid, int dcid);
- int ConfigureRequest(int dcid);
- int CommandReject(u16 reason=0, u16 data0=0, u16 data1=0);
- int ConfigureResponse(u8 rxid, int dcid);
- int DisconnectResponse(u8 rxid, int scid, int dcid);
- void Control(const u8* data, int len);
-};
-
-enum HCI_CALLBACK_EVENT
-{
- CALLBACK_NONE,
- CALLBACK_READY,
- CALLBACK_INQUIRY_RESULT,
- CALLBACK_INQUIRY_DONE,
- CALLBACK_REMOTE_NAME,
- CALLBACK_CONNECTION_COMPLETE,
- CALLBACK_CONNECTION_FAILED,
- CALLBACK_PIN_REQ,
- CALLBACK_CMD_STATUS,
- CALLBACK_CONNECTION_REQUEST,
- CALLBACK_VENDOR
-};
-
-// L2CAP Protocol/Service Multiplexor (PSM) values
-
-#define L2CAP_PSM_ANY 0x0000 /* Any/Invalid PSM */
-#define L2CAP_PSM_SDP 0x0001 /* Service Discovery Protocol */
-#define L2CAP_PSM_RFCOMM 0x0003 /* RFCOMM protocol */
-#define L2CAP_PSM_TCP 0x0005 /* Telephony Control Protocol */
-#define L2CAP_PSM_TCS 0x0007 /* TCS cordless */
-#define L2CAP_PSM_BNEP 0x000f /* Bluetooth Network Encapsulation Protocol*/
-#define L2CAP_PSM_HID_CNTL 0x0011 /* HID Control */
-#define L2CAP_PSM_HID_INTR 0x0013 /* HID Interrupt */
-#define L2CAP_PSM_ESDP 0x0015 /* Extended Service Discovery Profile */
-#define L2CAP_PSM_AVCTP 0x0017 /* Audio/Visual Control Transport Protocol */
-#define L2CAP_PSM_AVDTP 0x0019 /* Audio/Visual Distribution */
-
-// Callback from inquiry
-typedef int (*HCICallback)(HCI* hci, HCI_CALLBACK_EVENT evt, const u8* data, int len);
-//typedef int (HCI::*HCICallback)(HCI_CALLBACK_EVENT evt, const u8* data, int len);
-
-#define MAX_BTDEVICES 8
-
-class HCITransport;
-class HCI : public SocketHandler
-{
- HCITransport* _transport;
- HCICallback _callback;
- BD_ADDR _localAddr;
-
- BTDevice _devices[MAX_BTDEVICES];
- int _deviceCount;
-
- int _acl_mtu;
- int _acl_max_pkt;
- int _sco_mtu;
- int _sco_max_pkt;
-
- int _state;
-#ifdef COMMAND_FLOW
- char cmd_credits;//command flow control
-#endif
-//, data_credits;//host to controller flow control is per handle, hence is handled in BTDevice
- public:
-
- // Open a local adapter
- int Open(HCITransport* transport, HCICallback callback=0);
-
- // Return list of discovered addreses
- int GetDevices(BTDevice** devices, int maxDevices);
-
- // Lookup a device by address or handle
- BTDevice* Find(const BD_ADDR* addr);
- BTDevice* Find(int handle);
-
- // Disconnect from a remote device
- int Disconnect(const BD_ADDR* addr);
- int DisconnectAll();
-
- // see what devies are in the system
- int Inquiry(int duration = 10);
-
- int SetEventFilter(u8 filterType, u8 filterConditionType, u8* condition);
- // get a name, delivered in callback
- int RemoteNameRequest(const BD_ADDR* addr);
- int RemoteNameRequest(inquiry_info* ii);
-
- // Connect to a remote device
- int CreateConnection(const BD_ADDR* remoteAddr);
-
- bool Busy();
-
- // called from transport
- void HCIRecv(const u8* data, int len);
-
- // called from transport
- void ACLRecv(const u8* data, int len);
-
- // SocketHandler methods for maintaining L2CAP sockets
- virtual int Open(SocketInternal* sock, SocketAddrHdr* addr);
- virtual int Accept(SocketInternal* sock, int scid, int rxid);
- virtual int Send(SocketInternal* sock, const u8* data, int len);
- virtual int Close(SocketInternal* sock);
- virtual char* Name() { return "HCI SocketHandler";}
-
-private:
- void InquiryResult(const inquiry_info* info);
- void RemoteName(const BD_ADDR* addr, const char* name);
- void ConnectComplete(const connection_info* info);
- void DisconnectComplete(int handle);
- void OnCommandComplete(int cmd, const u8* data, int len);
- virtual void Callback(HCI_CALLBACK_EVENT c, const u8* data, int len);
- void Compl_pkts(int handle, u8 p = 1);
-protected:
-// int SendCmd(int cmd, const u8* params = 0, int len = 0);
- int PinCodeReply(const u8* data, const u8* pin = "0000");
- void Accept_Connection(const BD_ADDR* addr, bool slave=true);
-public:
- int SendCmd(int cmd, const u8* params = 0, int len = 0);
-};
-
-class HCITransport
-{
-protected:
- HCI* _target;
-public:
-#ifdef HOST_CONTR_FLOW
- u8 data_credits;
-#endif
- u16 _acl_mtu;
- void Set(HCI* target) { _target = target; };
- virtual void HCISend(const u8* data, int len) = 0;
- virtual int ACLSend(const u8* data, int len) = 0;
-};
-
-#endif
--- a/hci_private.h Sun Jun 19 19:32:51 2011 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,327 +0,0 @@
-/*
-Copyright (c) 2010 Peter Barrett
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-*/
-
-#ifndef HCI_PRIVATE_H_INCLUDED
-#define HCI_PRIVATE_H_INCLUDED
-
-#define HCI_OP_INQUIRY 0x0401
-#define HCI_OP_INQUIRY_CANCEL 0x0402
-#define HCI_OP_EXIT_PERIODIC_INQ 0x0404
-#define HCI_OP_CREATE_CONN 0x0405
-#define HCI_OP_DISCONNECT 0x0406
-#define HCI_OP_ADD_SCO 0x0407
-#define HCI_OP_CREATE_CONN_CANCEL 0x0408
-#define HCI_OP_ACCEPT_CONN_REQ 0x0409
-#define HCI_OP_REJECT_CONN_REQ 0x040a
-#define HCI_OP_LINK_KEY_REPLY 0x040b
-#define HCI_OP_LINK_KEY_NEG_REPLY 0x040c
-#define HCI_OP_PIN_CODE_REPLY 0x040d
-#define HCI_OP_PIN_CODE_NEG_REPLY 0x040e
-#define HCI_OP_CHANGE_CONN_PTYPE 0x040f
-#define HCI_OP_AUTH_REQUESTED 0x0411
-#define HCI_OP_SET_CONN_ENCRYPT 0x0413
-#define HCI_OP_CHANGE_CONN_LINK_KEY 0x0415
-#define HCI_OP_REMOTE_NAME_REQ 0x0419
-#define HCI_OP_REMOTE_NAME_REQ_CANCEL 0x041a
-#define HCI_OP_READ_REMOTE_FEATURES 0x041b
-#define HCI_OP_READ_REMOTE_EXT_FEATURES 0x041c
-#define HCI_OP_READ_REMOTE_VERSION 0x041d
-#define HCI_OP_SETUP_SYNC_CONN 0x0428
-#define HCI_OP_ACCEPT_SYNC_CONN_REQ 0x0429
-#define HCI_OP_REJECT_SYNC_CONN_REQ 0x042a
-
-#define HCI_OP_SNIFF_MODE 0x0803
-#define HCI_OP_EXIT_SNIFF_MODE 0x0804
-#define HCI_OP_ROLE_DISCOVERY 0x0809
-#define HCI_OP_SWITCH_ROLE 0x080b
-#define HCI_OP_READ_LINK_POLICY 0x080c
-#define HCI_OP_WRITE_LINK_POLICY 0x080d
-#define HCI_OP_READ_DEF_LINK_POLICY 0x080e
-#define HCI_OP_WRITE_DEF_LINK_POLICY 0x080f
-#define HCI_OP_SNIFF_SUBRATE 0x0811
-
-
-#define HCI_OP_SET_EVENT_MASK 0x0c01
-#define HCI_OP_RESET 0x0c03
-#define HCI_OP_SET_EVENT_FLT 0x0c05
-#define HCI_OP_WRITE_LOCAL_NAME 0x0c13
-#define HCI_OP_READ_LOCAL_NAME 0x0c14
-#define HCI_OP_WRITE_CA_TIMEOUT 0x0c16
-#define HCI_OP_WRITE_PG_TIMEOUT 0x0c18
-#define HCI_OP_WRITE_SCAN_ENABLE 0x0c1a
-#define HCI_OP_READ_AUTH_ENABLE 0x0c1f
-#define HCI_OP_WRITE_AUTH_ENABLE 0x0c20
-#define HCI_OP_READ_ENCRYPT_MODE 0x0c21
-#define HCI_OP_WRITE_ENCRYPT_MODE 0x0c22
- #define ENCRYPT_DISABLED 0x00
- #define ENCRYPT_P2P 0x01
- #define ENCRYPT_BOTH 0x02
-#define HCI_OP_READ_CLASS_OF_DEV 0x0c23
-#define HCI_OP_WRITE_CLASS_OF_DEV 0x0c24
-#define HCI_OP_READ_VOICE_SETTING 0x0c25
-#define HCI_OP_WRITE_VOICE_SETTING 0x0c26
-#define HCI_OP_CONTR_TO_HOST_FLOW 0x0c31
-#define HCI_OP_HOST_BUFFER_SIZE 0x0c33
-#define HCI_OP_NUM_COMP_PKTS 0x0c35
-#define HCI_OP_READ_SSP_MODE 0x0c55
-#define HCI_OP_WRITE_SSP_MODE 0x0c56
-
-#define HCI_OP_READ_LOCAL_VERSION 0x1001
-#define HCI_OP_READ_LOCAL_COMMANDS 0x1002
-#define HCI_OP_READ_LOCAL_FEATURES 0x1003
-#define HCI_OP_READ_LOCAL_EXT_FEATURES 0x1004
-#define HCI_OP_READ_BUFFER_SIZE 0x1005
-#define HCI_OP_READ_BD_ADDR 0x1009
-
-// events
-#define HCI_EV_INQUIRY_COMPLETE 0x01
-#define HCI_EV_INQUIRY_RESULT 0x02
-#define HCI_EV_CONN_COMPLETE 0x03
-#define HCI_EV_CONN_REQUEST 0x04
-#define HCI_EV_DISCONN_COMPLETE 0x05
-#define HCI_EV_AUTH_COMPLETE 0x06
-#define HCI_EV_REMOTE_NAME 0x07
-#define HCI_EV_ENCRYPT_CHANGE 0x08
-#define HCI_EV_CHANGE_LINK_KEY_COMPLETE 0x09
-#define HCI_EV_REMOTE_FEATURES 0x0b
-#define HCI_EV_REMOTE_VERSION 0x0c
-#define HCI_EV_QOS_SETUP_COMPLETE 0x0d
-#define HCI_EV_CMD_COMPLETE 0x0e
-#define HCI_EV_CMD_STATUS 0x0f
-#define HCI_EV_ROLE_CHANGE 0x12
-#define HCI_EV_NUM_COMP_PKTS 0x13
-#define HCI_EV_MODE_CHANGE 0x14
-#define HCI_EV_RETURN_LINK_KEYS 0x15
-#define HCI_EV_PIN_CODE_REQ 0x16
-#define HCI_EV_LINK_KEY_REQ 0x17
-#define HCI_EV_LINK_KEY_NOTIFY 0x18
-#define HCI_EV_CLOCK_OFFSET 0x1c
-#define HCI_EV_PKT_TYPE_CHANGE 0x1d
-#define HCI_EV_PSCAN_REP_MODE 0x20
-#define HCI_EV_INQUIRY_RESULT_WITH_RSSI 0x22
-#define HCI_EV_REMOTE_EXT_FEATURES 0x23
-#define HCI_EV_SYNC_CONN_COMPLETE 0x2c
-#define HCI_EV_SYNC_CONN_CHANGED 0x2d
-#define HCI_EV_SNIFF_SUBRATE 0x2e
-#define HCI_EV_EXTENDED_INQUIRY_RESULT 0x2f
-#define HCI_EV_IO_CAPA_REQUEST 0x31
-#define HCI_EV_SIMPLE_PAIR_COMPLETE 0x36
-#define HCI_EV_REMOTE_HOST_FEATURES 0x3d
-#define HCI_EV_VENDOR 0xff
-
-/* Possible error codes */
-#define HCI_UNKNOWN_HCI_COMMAND 0x01
-#define HCI_NO_CONNECTION 0x02
-#define HCI_HW_FAILURE 0x03
-#define HCI_PAGE_TIMEOUT 0x04
-#define HCI_AUTHENTICATION_FAILURE 0x05
-#define HCI_KEY_MISSING 0x06
-#define HCI_MEMORY_FULL 0x07
-#define HCI_CONN_TIMEOUT 0x08
-#define HCI_MAX_NUMBER_OF_CONNECTIONS 0x09
-#define HCI_MAX_NUMBER_OF_SCO_CONNECTIONS_TO_DEVICE 0x0A
-#define HCI_ACL_CONNECTION_EXISTS 0x0B
-#define HCI_COMMAND_DISSALLOWED 0x0C
-#define HCI_HOST_REJECTED_DUE_TO_LIMITED_RESOURCES 0x0D
-#define HCI_HOST_REJECTED_DUE_TO_SECURITY_REASONS 0x0E
-#define HCI_HOST_REJECTED_DUE_TO_REMOTE_DEVICE_ONLY_PERSONAL_SERVICE 0x0F
-#define HCI_HOST_TIMEOUT 0x10
-#define HCI_UNSUPPORTED_FEATURE_OR_PARAMETER_VALUE 0x11
-#define HCI_INVALID_HCI_COMMAND_PARAMETERS 0x12
-#define HCI_OTHER_END_TERMINATED_CONN_USER_ENDED 0x13
-#define HCI_OTHER_END_TERMINATED_CONN_LOW_RESOURCES 0x14
-#define HCI_OTHER_END_TERMINATED_CONN_ABOUT_TO_POWER_OFF 0x15
-#define HCI_CONN_TERMINATED_BY_LOCAL_HOST 0x16
-#define HCI_REPETED_ATTEMPTS 0x17
-#define HCI_PAIRING_NOT_ALLOWED 0x18
-#define HCI_UNKNOWN_LMP_PDU 0x19
-#define HCI_UNSUPPORTED_REMOTE_FEATURE 0x1A
-#define HCI_SCO_OFFSET_REJECTED 0x1B
-#define HCI_SCO_INTERVAL_REJECTED 0x1C
-#define HCI_SCO_AIR_MODE_REJECTED 0x1D
-#define HCI_INVALID_LMP_PARAMETERS 0x1E
-#define HCI_UNSPECIFIED_ERROR 0x1F
-#define HCI_UNSUPPORTED_LMP_PARAMETER_VALUE 0x20
-#define HCI_ROLE_CHANGE_NOT_ALLOWED 0x21
-#define HCI_LMP_RESPONSE_TIMEOUT 0x22
-#define HCI_LMP_ERROR_TRANSACTION_COLLISION 0x23
-#define HCI_LMP_PDU_NOT_ALLOWED 0x24
-#define HCI_ENCRYPTION_MODE_NOT_ACCEPTABLE 0x25
-#define HCI_UNIT_KEY_USED 0x26
-#define HCI_QOS_NOT_SUPPORTED 0x27
-#define HCI_INSTANT_PASSED 0x28
-#define HCI_PAIRING_UNIT_KEY_NOT_SUPPORTED 0x29
-
-const char* EvtStr(int evt)
-{
- switch (evt)
- {
- case HCI_EV_INQUIRY_COMPLETE: return "HCI_EV_INQUIRY_COMPLETE";
- case HCI_EV_INQUIRY_RESULT: return "HCI_EV_INQUIRY_RESULT";
- case HCI_EV_CONN_COMPLETE: return "HCI_EV_CONN_COMPLETE";
- case HCI_EV_CONN_REQUEST: return "HCI_EV_CONN_REQUEST";
- case HCI_EV_DISCONN_COMPLETE: return "HCI_EV_DISCONN_COMPLETE";
- case HCI_EV_AUTH_COMPLETE: return "HCI_EV_AUTH_COMPLETE";
- case HCI_EV_REMOTE_NAME: return "HCI_EV_REMOTE_NAME";
- case HCI_EV_ENCRYPT_CHANGE: return "HCI_EV_ENCRYPT_CHANGE";
- case HCI_EV_CHANGE_LINK_KEY_COMPLETE : return "HCI_EV_CHANGE_LINK_KEY_COMPLETE";
- case HCI_EV_REMOTE_FEATURES: return "HCI_EV_REMOTE_FEATURES";
- case HCI_EV_REMOTE_VERSION: return "HCI_EV_REMOTE_VERSION";
- case HCI_EV_QOS_SETUP_COMPLETE : return "HCI_EV_QOS_SETUP_COMPLETE";
- case HCI_EV_CMD_COMPLETE: return "HCI_EV_CMD_COMPLETE";
- case HCI_EV_CMD_STATUS: return "HCI_EV_CMD_STATUS";
- case HCI_EV_ROLE_CHANGE: return "HCI_EV_ROLE_CHANGE";
- case HCI_EV_NUM_COMP_PKTS: return "HCI_EV_NUM_COMP_PKTS";
- case HCI_EV_MODE_CHANGE: return "HCI_EV_MODE_CHANGE";
- case HCI_EV_PIN_CODE_REQ: return "HCI_EV_PIN_CODE_REQ";
- case HCI_EV_LINK_KEY_REQ: return "HCI_EV_LINK_KEY_REQ";
- case HCI_EV_LINK_KEY_NOTIFY: return "HCI_EV_LINK_KEY_NOTIFY";
- case HCI_EV_CLOCK_OFFSET: return "HCI_EV_CLOCK_OFFSET";
- case HCI_EV_PKT_TYPE_CHANGE: return "HCI_EV_PKT_TYPE_CHANGE";
- case HCI_EV_PSCAN_REP_MODE: return "HCI_EV_PSCAN_REP_MODE";
- case HCI_EV_INQUIRY_RESULT_WITH_RSSI : return "HCI_EV_INQUIRY_RESULT_WITH_RSSI";
- case HCI_EV_REMOTE_EXT_FEATURES: return "HCI_EV_REMOTE_EXT_FEATURES";
- case HCI_EV_SYNC_CONN_COMPLETE: return "HCI_EV_SYNC_CONN_COMPLETE";
- case HCI_EV_SYNC_CONN_CHANGED: return "HCI_EV_SYNC_CONN_CHANGED";
- case HCI_EV_SNIFF_SUBRATE: return "HCI_EV_SNIFF_SUBRATE";
- case HCI_EV_EXTENDED_INQUIRY_RESULT: return "HCI_EV_EXTENDED_INQUIRY_RESULT";
- case HCI_EV_IO_CAPA_REQUEST: return "HCI_EV_IO_CAPA_REQUEST";
- case HCI_EV_SIMPLE_PAIR_COMPLETE: return "HCI_EV_SIMPLE_PAIR_COMPLETE";
- case HCI_EV_REMOTE_HOST_FEATURES: return "HCI_EV_REMOTE_HOST_FEATURES";
- }
- return "Unknown Event";
-}
-
-const char* CmdStr(int cmd)
-{
- switch (cmd)
- {
- // 0x04XX
- case HCI_OP_INQUIRY: return "HCI_OP_INQUIRY";
- case HCI_OP_INQUIRY_CANCEL: return "HCI_OP_INQUIRY_CANCEL";
- case HCI_OP_EXIT_PERIODIC_INQ: return "HCI_OP_EXIT_PERIODIC_INQ";
- case HCI_OP_CREATE_CONN: return "HCI_OP_CREATE_CONN";
- case HCI_OP_DISCONNECT: return "HCI_OP_DISCONNECT";
- case HCI_OP_ADD_SCO: return "HCI_OP_ADD_SCO";
- case HCI_OP_CREATE_CONN_CANCEL: return "HCI_OP_CREATE_CONN_CANCEL";
- case HCI_OP_ACCEPT_CONN_REQ: return "HCI_OP_ACCEPT_CONN_REQ";
- case HCI_OP_REJECT_CONN_REQ: return "HCI_OP_REJECT_CONN_REQ";
- case HCI_OP_LINK_KEY_REPLY: return "HCI_OP_LINK_KEY_REPLY";
- case HCI_OP_LINK_KEY_NEG_REPLY: return "HCI_OP_LINK_KEY_NEG_REPLY";
- case HCI_OP_PIN_CODE_REPLY: return "HCI_OP_PIN_CODE_REPLY";
- case HCI_OP_PIN_CODE_NEG_REPLY: return "HCI_OP_PIN_CODE_NEG_REPLY";
- case HCI_OP_CHANGE_CONN_PTYPE: return "HCI_OP_CHANGE_CONN_PTYPE";
- case HCI_OP_AUTH_REQUESTED: return "HCI_OP_AUTH_REQUESTED";
- case HCI_OP_SET_CONN_ENCRYPT: return "HCI_OP_SET_CONN_ENCRYPT";
- case HCI_OP_CHANGE_CONN_LINK_KEY: return "HCI_OP_CHANGE_CONN_LINK_KEY";
- case HCI_OP_REMOTE_NAME_REQ: return "HCI_OP_REMOTE_NAME_REQ";
- case HCI_OP_REMOTE_NAME_REQ_CANCEL: return "HCI_OP_REMOTE_NAME_REQ_CANCEL";
- case HCI_OP_READ_REMOTE_FEATURES: return "HCI_OP_READ_REMOTE_FEATURES";
- case HCI_OP_READ_REMOTE_EXT_FEATURES: return "HCI_OP_READ_REMOTE_EXT_FEATURES";
- case HCI_OP_READ_REMOTE_VERSION: return "HCI_OP_READ_REMOTE_VERSION";
- case HCI_OP_SETUP_SYNC_CONN: return "HCI_OP_SETUP_SYNC_CONN";
- case HCI_OP_ACCEPT_SYNC_CONN_REQ: return "HCI_OP_ACCEPT_SYNC_CONN_REQ";
- case HCI_OP_REJECT_SYNC_CONN_REQ: return "HCI_OP_REJECT_SYNC_CONN_REQ";
- // 0x0CXX
- case HCI_OP_SET_EVENT_MASK: return "HCI_OP_SET_EVENT_MASK";
- case HCI_OP_RESET: return "HCI_OP_RESET";
- case HCI_OP_SET_EVENT_FLT: return "HCI_OP_SET_EVENT_FLT";
- case HCI_OP_WRITE_LOCAL_NAME: return "HCI_OP_WRITE_LOCAL_NAME";
- case HCI_OP_READ_LOCAL_NAME: return "HCI_OP_READ_LOCAL_NAME";
- case HCI_OP_WRITE_CA_TIMEOUT: return "HCI_OP_WRITE_CA_TIMEOUT";
- case HCI_OP_WRITE_PG_TIMEOUT: return "HCI_OP_WRITE_PG_TIMEOUT";
- case HCI_OP_WRITE_SCAN_ENABLE: return "HCI_OP_WRITE_SCAN_ENABLE";
- case HCI_OP_READ_AUTH_ENABLE: return "HCI_OP_READ_AUTH_ENABLE";
- case HCI_OP_WRITE_AUTH_ENABLE: return "HCI_OP_WRITE_AUTH_ENABLE";
- case HCI_OP_READ_ENCRYPT_MODE: return "HCI_OP_READ_ENCRYPT_MODE";
- case HCI_OP_WRITE_ENCRYPT_MODE: return "HCI_OP_WRITE_ENCRYPT_MODE";
- case HCI_OP_READ_CLASS_OF_DEV: return "HCI_OP_READ_CLASS_OF_DEV";
- case HCI_OP_WRITE_CLASS_OF_DEV: return "HCI_OP_WRITE_CLASS_OF_DEV";
- case HCI_OP_READ_VOICE_SETTING: return "HCI_OP_READ_VOICE_SETTING";
- case HCI_OP_WRITE_VOICE_SETTING: return "HCI_OP_WRITE_VOICE_SETTING";
- case HCI_OP_HOST_BUFFER_SIZE: return "HCI_OP_HOST_BUFFER_SIZE";
- case HCI_OP_READ_SSP_MODE: return "HCI_OP_READ_SSP_MODE";
- case HCI_OP_WRITE_SSP_MODE: return "HCI_OP_WRITE_SSP_MODE";
-
- // 10xx
- case HCI_OP_READ_LOCAL_VERSION: return "HCI_OP_READ_LOCAL_VERSION";
- case HCI_OP_READ_LOCAL_COMMANDS: return "HCI_OP_READ_LOCAL_COMMANDS";
- case HCI_OP_READ_LOCAL_FEATURES: return "HCI_OP_READ_LOCAL_FEATURES";
- case HCI_OP_READ_LOCAL_EXT_FEATURES: return "HCI_OP_READ_LOCAL_EXT_FEATURES";
- case HCI_OP_READ_BUFFER_SIZE: return "HCI_OP_READ_BUFFER_SIZE";
- case HCI_OP_READ_BD_ADDR: return "HCI_OP_READ_BD_ADDR";
- }
- return "Unknown Cmd";
-}
-
-const char* HCIErrStr(int err)
-{
- switch (err)
- {
- case 0: return "OK";
- case HCI_UNKNOWN_HCI_COMMAND: return "HCI_UNKNOWN_HCI_COMMAND";
- case HCI_NO_CONNECTION: return "HCI_NO_CONNECTION";
- case HCI_HW_FAILURE: return "HCI_HW_FAILURE";
- case HCI_PAGE_TIMEOUT: return "HCI_PAGE_TIMEOUT";
- case HCI_AUTHENTICATION_FAILURE: return "HCI_AUTHENTICATION_FAILURE";
- case HCI_KEY_MISSING: return "HCI_KEY_MISSING";
- case HCI_MEMORY_FULL: return "HCI_MEMORY_FULL";
- case HCI_CONN_TIMEOUT: return "HCI_CONN_TIMEOUT";
- case HCI_MAX_NUMBER_OF_CONNECTIONS: return "HCI_CONN_TIMEOUT";
- case HCI_MAX_NUMBER_OF_SCO_CONNECTIONS_TO_DEVICE: return "HCI_MAX_NUMBER_OF_SCO_CONNECTIONS_TO_DEVICE";
- case HCI_ACL_CONNECTION_EXISTS: return "HCI_ACL_CONNECTION_EXISTS";
- case HCI_COMMAND_DISSALLOWED: return "HCI_COMMAND_DISSALLOWED";
- case HCI_HOST_REJECTED_DUE_TO_LIMITED_RESOURCES: return "HCI_HOST_REJECTED_DUE_TO_LIMITED_RESOURCES";
- case HCI_HOST_REJECTED_DUE_TO_SECURITY_REASONS: return "HCI_HOST_REJECTED_DUE_TO_SECURITY_REASONS";
- case HCI_HOST_REJECTED_DUE_TO_REMOTE_DEVICE_ONLY_PERSONAL_SERVICE: return "HCI_HOST_REJECTED_DUE_TO_REMOTE_DEVICE_ONLY_PERSONAL_SERVICE";
- case HCI_HOST_TIMEOUT: return "HCI_HOST_TIMEOUT";
- case HCI_UNSUPPORTED_FEATURE_OR_PARAMETER_VALUE: return "HCI_UNSUPPORTED_FEATURE_OR_PARAMETER_VALUE";
- case HCI_INVALID_HCI_COMMAND_PARAMETERS: return "HCI_INVALID_HCI_COMMAND_PARAMETERS";
- case HCI_OTHER_END_TERMINATED_CONN_USER_ENDED: return "HCI_OTHER_END_TERMINATED_CONN_USER_ENDED";
- case HCI_OTHER_END_TERMINATED_CONN_LOW_RESOURCES: return "HCI_OTHER_END_TERMINATED_CONN_LOW_RESOURCES";
- case HCI_OTHER_END_TERMINATED_CONN_ABOUT_TO_POWER_OFF: return "HCI_OTHER_END_TERMINATED_CONN_ABOUT_TO_POWER_OFF";
- case HCI_CONN_TERMINATED_BY_LOCAL_HOST: return "HCI_CONN_TERMINATED_BY_LOCAL_HOST";
- case HCI_REPETED_ATTEMPTS: return "HCI_REPEATED_ATTEMPTS";
- case HCI_PAIRING_NOT_ALLOWED: return "HCI_PAIRING_NOT_ALLOWED";
- case HCI_UNKNOWN_LMP_PDU: return "HCI_UNKNOWN_LMP_PDU";
- case HCI_UNSUPPORTED_REMOTE_FEATURE: return "HCI_UNSUPPORTED_REMOTE_FEATURE";
- case HCI_SCO_OFFSET_REJECTED: return "HCI_SCO_OFFSET_REJECTED";
- case HCI_SCO_INTERVAL_REJECTED: return "HCI_SCO_INTERVAL_REJECTED";
- case HCI_SCO_AIR_MODE_REJECTED: return "HCI_SCO_AIR_MODE_REJECTED";
- case HCI_INVALID_LMP_PARAMETERS: return "HCI_INVALID_LMP_PARAMETERS";
- case HCI_UNSPECIFIED_ERROR: return "HCI_UNSPECIFIED_ERROR";
- case HCI_UNSUPPORTED_LMP_PARAMETER_VALUE: return "HCI_UNSUPPORTED_LMP_PARAMETER_VALUE";
- case HCI_ROLE_CHANGE_NOT_ALLOWED: return "HCI_ROLE_CHANGE_NOT_ALLOWED";
- case HCI_LMP_RESPONSE_TIMEOUT: return "HCI_LMP_RESPONSE_TIMEOUT";
- case HCI_LMP_ERROR_TRANSACTION_COLLISION: return "HCI_LMP_ERROR_TRANSACTION_COLLISION";
- case HCI_LMP_PDU_NOT_ALLOWED: return "HCI_LMP_PDU_NOT_ALLOWED";
- case HCI_ENCRYPTION_MODE_NOT_ACCEPTABLE: return "HCI_ENCRYPTION_MODE_NOT_ACCEPTABLE";
- case HCI_UNIT_KEY_USED: return "HCI_UNIT_KEY_USED";
- case HCI_QOS_NOT_SUPPORTED: return "HCI_QOS_NOT_SUPPORTED";
- case HCI_INSTANT_PASSED: return "HCI_INSTANT_PASSED";
- case HCI_PAIRING_UNIT_KEY_NOT_SUPPORTED: return "HCI_PAIRING_UNIT_KEY_NOT_SUPPORTED";
- };
- return "Unknow HCI err";
-};
-
-
-#endif // HCI_PRIVATE_H_INCLUDED
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,125 @@
+/*
+Copyright (c) 2010 Peter Barrett
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+*/
+
+#include "mbed.h"
+#include "USBHost.h"
+#include "Utils.h"
+#include "FATFileSystem.h"
+#include "MassStorage.h"
+
+int MassStorage_ReadCapacity(int device, u32* blockCount, u32* blockSize);
+int MassStorage_Read(int device, u32 blockAddr, u32 blockCount, u8* dst, u32 blockSize);
+int MassStorage_Write(int device, u32 blockAddr, u32 blockCount, u8* dst, u32 blockSize);
+
+class USBFileSystem : public FATFileSystem, public USBSCSI
+{
+ //int _device;
+ u32 _blockSize;
+ u32 _blockCount;
+
+public:
+ USBFileSystem() : FATFileSystem("usb")/*,_device(0)*/,_blockSize(0),_blockCount(0)
+ {
+ }
+/*
+ void SetDevice(int device, unsigned char in, unsigned char out)
+ {
+ _device = device;
+ }
+*/
+ virtual int disk_initialize()
+ {
+ return SCSIReadCapacity(&_blockCount,&_blockSize);
+ //return MassStorage_ReadCapacity(_device,&_blockCount,&_blockSize);
+ }
+
+ virtual int disk_write(const char *buffer, int block_number)
+ {
+ return SCSITransfer(block_number, 1,(u8*)buffer,_blockSize,HOST_TO_DEVICE);
+ //return MassStorage_Write(_device,block_number,1,(u8*)buffer,_blockSize);
+ }
+
+ virtual int disk_read(char *buffer, int block_number)
+ {
+ return SCSITransfer(block_number, 1, (u8*)buffer, _blockSize, DEVICE_TO_HOST);
+ //return MassStorage_Read(_device,block_number,1,(u8*)buffer,_blockSize);
+ }
+
+ virtual int disk_sectors()
+ {
+ return _blockCount;
+ }
+};
+
+void DumpFS(int depth, int count)
+{
+ DIR *d = opendir("/usb");
+ if (!d)
+ {
+ printf("USB file system borked\n");
+ return;
+ }
+
+ printf("\nDumping root dir\n");
+ struct dirent *p;
+ for(;;)
+ {
+ p = readdir(d);
+ if (!p)
+ break;
+ int len = sizeof( dirent);
+ printf("%s %d\n", p->d_name, len);
+ }
+ closedir(d);
+}
+
+int OnDiskInsert(int device, unsigned char in, unsigned char out)
+{
+ USBFileSystem fs;
+ fs.SetDevice(device, in, out);
+ DumpFS(0,0);
+ return 0;
+}
+
+/*
+ Simple test shell to exercise mouse,keyboard,mass storage and hubs.
+ Add 2 15k pulldown resistors between D+/D- and ground, attach a usb socket and have at it.
+*/
+
+Serial pc(USBTX, USBRX);
+int GetConsoleChar()
+{
+ if (!pc.readable())
+ return -1;
+ char c = pc.getc();
+ pc.putc(c); // echo
+ return c;
+}
+
+void TestShell();
+
+int main()
+{
+ pc.baud(460800);
+ printf("BlueUSB\nNow get a bunch of usb or bluetooth things and plug them in\n");
+ TestShell();
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Jul 01 09:16:00 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/63bcd7ba4912
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/myUSBHost.lib Fri Jul 01 09:16:00 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/networker/code/myUSBHost/#58c785c2b381
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/neigbourhood/neighbourhood.cpp Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,69 @@
+#include "Utils.h"
+#include "neighbourhood.h"
+
+neighbourhood *neighbors = 0;
+
+int neighbourhood::get(BD_ADDR *a, unsigned char *key) {
+ for (list<item>::iterator i = keys.begin(); i != keys.end(); i++)
+ if (memcmp(a, &(*i).a, sizeof(BD_ADDR)) == 0) {
+ memcpy(key, (*i).lk, lksize);
+#ifdef STRICT_MRU
+ if (i != keys.begin()) {
+ keys.push_front(*i);
+ keys.erase(i);
+ dirty = true;
+ }
+#endif
+ return 1;
+ }
+ return 0;
+}
+
+int neighbourhood::add(BD_ADDR *a, const unsigned char *key, bool init) {
+ for (list<item>::iterator i = keys.begin(); i != keys.end(); i++)
+ if (memcmp(a, &(*i).a, sizeof(BD_ADDR)) == 0) {
+ memcpy((*i).lk, key, lksize); //assume key has changed, update key
+ (*i).used = true;
+ return 1;
+ }
+ //new key
+ printf("Neighbourhood: "); printf(a); printf("\n");
+ if (keys.size() < cap) {
+ keys.push_back(item(a, key, !init));//append as long as there is space
+ } else {
+ keys.push_front(item(a, key, true));//otherwise prepend
+ dirty = true;
+ }
+ return 0;
+}
+
+void neighbourhood::write() {
+ int n = 0;
+ static const int maxkey = 11;
+ unsigned char param[maxkey*(lksize+sizeof(BD_ADDR))+1];
+ int k = keys.size()-cap;
+ list<item>::iterator i = keys.begin();
+ while (i != keys.end()) {
+ if (k>0) {
+ if (!(*i).used) {
+ delete_link_key(&(*i).a);//try to make some room
+ keys.erase(i);
+ k--;
+ } else
+ i++;
+ } else
+ break;
+ }
+ //hci->delete_link_keys();
+ unsigned char *p = ¶m[1];
+ for (list<item>::iterator i = keys.begin(); i != keys.end() && n<maxkey; i++, n++) {
+ memcpy(p, &(*i).a, sizeof(BD_ADDR));
+ p += sizeof(BD_ADDR);
+ memcpy(p, (*i).lk, lksize);
+ p += lksize;
+ }
+ param[0] = n;
+ if (n > 0)
+ write_link_keys(param);
+}
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/neigbourhood/neighbourhood.h Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,77 @@
+#ifndef NEIGHBOURHOOD_H
+#define NEIGHBOURHOOD_H
+
+#include <list>
+#include "hci.h"
+
+//#define STRICT_MRU
+
+/******************************************************************************************
+call 'read' as part of the startup
+on HCI_READ-STORED-LINK-KEY -COMPLETED event, call 'set_cap'
+on RETURN_LINK_KEYS_EVENT call 'add' for each key with init=true
+on LINK_KEY_NOTIFICATION_EVENT call 'add' with init=false (default)
+on LINK_KEY_REQUEST_EVENT call 'get' and send the proper reply
+call 'write' as part of shutdown
+
+a simpler approach could be to check for a link if it exists (single read) and try to add it.
+when it fails just delete all.
+********************************************************************************************/
+
+#define HCI_DELETE_STORED_LINK_KEY 0x0C12
+#define HCI_WRITE_STORED_LINK_KEY 0x0C11
+#define HCI_READ_STORED_LINK_KEY 0x0C0D
+
+void printf(const BD_ADDR* addr);
+
+class neighbourhood {
+ static const int lksize = 16;
+ struct item {
+ BD_ADDR a;
+ unsigned char lk[lksize];
+ bool used;
+ item (BD_ADDR *ad, const unsigned char *k, bool d) {
+ memcpy(&a, ad, sizeof(BD_ADDR));
+ memcpy(lk, k, lksize);
+ used=d;
+ }
+ };
+ int cap, initsize, used;
+ list<item> keys;
+ bool dirty;
+ HCI *hci;
+ void delete_link_key(BD_ADDR *a) {
+ unsigned char param[sizeof(BD_ADDR)+1];
+ memcpy(param, a, sizeof(BD_ADDR));
+ param[sizeof(BD_ADDR)] = 0;
+ hci->SendCmd(HCI_DELETE_STORED_LINK_KEY, param, sizeof(param));
+ }
+ void write_link_keys(unsigned char param[]) {
+ hci->SendCmd(HCI_WRITE_STORED_LINK_KEY, param, param[0]*(sizeof(BD_ADDR)+lksize)+1);
+ }
+public:
+ neighbourhood(HCI *h): hci(h) {
+ dirty = false;
+ used = 0;
+ cap=0;
+ initsize=0;
+ }
+ void read() {
+ unsigned char param[sizeof(BD_ADDR)+1];
+ memset(param, 0, sizeof(BD_ADDR));
+ param[sizeof(BD_ADDR)] = 1;
+ hci->SendCmd(HCI_READ_STORED_LINK_KEY, param, sizeof(param));
+ }
+ void write();
+ void set_cap(int c, int s) {
+ cap = c;
+ initsize = s;
+ printf("Neighbourhood: capacity=%d, used=%d\n", c, s);
+ }
+ int get(BD_ADDR *a, unsigned char *key);
+ int add(BD_ADDR *a, const unsigned char *key, bool init=false);
+};
+
+extern neighbourhood *neighbors;
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rfcomm/RFCOMM.cpp Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,682 @@
+#include "mbed.h"
+#include "Utils.h"
+#include "RFCOMM.h"
+
+// Control field values bit no. 1 2 3 4 5 6 7 8
+#define BT_RFCOMM_SABM 0x3F // 1 1 1 1 P/F 1 0 0
+#define BT_RFCOMM_UA 0x73 // 1 1 0 0 P/F 1 1 0
+#define BT_RFCOMM_DM 0x0F // 1 1 1 1 P/F 0 0 0
+#define BT_RFCOMM_DM_PF 0x1F
+#define BT_RFCOMM_DISC 0x53 // 1 1 0 0 P/F 0 1 0
+#define BT_RFCOMM_UIH 0xEF // 1 1 1 1 P/F 1 1 1
+#define BT_RFCOMM_UIH_PF 0xFF
+
+// Multiplexer message types
+#define BT_RFCOMM_PN_CMD 0x83
+#define BT_RFCOMM_PN_RSP 0x81
+#define BT_RFCOMM_TEST_CMD 0x23
+#define BT_RFCOMM_TEST_RSP 0x21
+#define BT_RFCOMM_FCON_CMD 0xA3
+#define BT_RFCOMM_FCON_RSP 0xA1
+#define BT_RFCOMM_FCOFF_CMD 0x63
+#define BT_RFCOMM_FCOFF_RSP 0x61
+#define BT_RFCOMM_MSC_CMD 0xE3
+#define BT_RFCOMM_MSC_RSP 0xE1
+#define BT_RFCOMM_RPN_CMD 0x93
+#define BT_RFCOMM_RPN_RSP 0x91
+#define BT_RFCOMM_RLS_CMD 0x53
+#define BT_RFCOMM_RLS_RSP 0x51
+#define BT_RFCOMM_NSC_RSP 0x11
+
+// FCS calc
+#define BT_RFCOMM_CODE_WORD 0xE0 // pol = x8+x2+x1+1
+#define BT_RFCOMM_CRC_CHECK_LEN 3
+#define BT_RFCOMM_UIHCRC_CHECK_LEN 2
+
+#define NR_CREDITS 1
+#define INITIAL_CREDITS 1 //0...7
+
+#define DCD (1<<7) //DV data carrier detect
+#define RI (1<<6) //IC ring indicator
+#define RTS (1<<3) //RTR request to send
+#define DSR (1<<2) //RTC data set ready
+#define FC (1<<1) //Flow Control
+#define EA (1<<0) //extended address (always 1)
+#define SIGNALS (DCD | RTS | DSR | EA)
+
+#define DEBUG 1
+#define TAKE_INITIATIVE
+
+RFCOMMManager rfcomm_manager;
+
+//uint8_t rfcomm_out_buffer[1000];//seems a bit big as default max framesize is 127
+//unsigned rfcomm::maxframesize = MAX_FRAME_SIZE; //only initial value
+
+//these functions are obtained from rfcomm.c on google code
+void _bt_rfcomm_send_sabm(unsigned short source_cid, unsigned char initiator, unsigned char channel);
+void _bt_rfcomm_send_uih_pn_command(unsigned short source_cid, unsigned char initiator, unsigned char channel, unsigned short max_frame_size);
+void _bt_rfcomm_send_uih_msc_cmd(unsigned short source_cid, unsigned char initiator, unsigned char channel, unsigned char signals);
+void _bt_rfcomm_send_uih_rpn_cmd(uint16_t source_cid, uint8_t initiator, uint8_t dlci, port_settings *val);
+int rfcomm_send_packet(unsigned short source_cid, unsigned char address, unsigned char control, unsigned char credits, const unsigned char *data, unsigned short len);
+uint8_t crc8_calc(uint8_t *data, uint16_t len);
+
+
+//find a free socket slot for channel ch
+int rfcomm::find_slot(unsigned ch) {
+ for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) {
+ if (sckts[i] != 0) { //socket is in use
+ RFCOMMSocket *s = (RFCOMMSocket*)GetSocketInternal(sckts[i]);
+ if (s==0) {
+ printf("find_slot: socket %d not found\n", sckts[i]);
+ continue;
+ }
+ if (s->dlci >> 1 == ch) {
+ printf("Channel %d is already in use on socket %d\n", ch, sckts[i]);
+ return -1;
+ }
+ } else //slot is free
+ return i;
+ }
+ return -2; //no free slots
+}
+
+//find the rfcomm socket for dlci
+RFCOMMSocket* rfcomm::find_socket(unsigned dlci) {
+ for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) {
+ if (sckts[i] != 0) { //socket is in use
+ RFCOMMSocket *s = (RFCOMMSocket*)GetSocketInternal(sckts[i]);
+ if (s==0) {
+ printf("find_socket: socket %d not found\n", sckts[i]);
+ continue;
+ }
+ if (s->dlci == dlci) {
+ return s;
+ }
+ }
+ }
+ printf("RFCOMMSocket for dlci %d was not found!\n", dlci);
+ return 0; //socket not found
+}
+
+//send a PN command to all sockets waiting to be opened
+void rfcomm::initChannels(int socket) {
+ for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) {
+ if (sckts[i] != 0) { //socket is in use
+ RFCOMMSocket *s = (RFCOMMSocket*)GetSocketInternal(sckts[i]);
+ if (s==0) {
+ printf("initChannels: socket %d not found\n", sckts[i]);
+ continue;
+ }
+ if (s->State == SocketState_Opening) {
+ printf("Sending PN for DLCI %d on socket %d\n", s->dlci, sckts[i]);
+ _bt_rfcomm_send_uih_pn_command(socket, 1, s->dlci, maxframesize);
+ s->State = SocketState_L2CAP_Config_wait;
+ }
+ }
+ }
+}
+
+unsigned rfcomm::release_channel(unsigned dlci) {
+ int n = 0;
+ for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) {
+ if (sckts[i] != 0) { //socket is in use
+ RFCOMMSocket *s = (RFCOMMSocket*)GetSocketInternal(sckts[i]);
+ if (s==0) {
+ printf("Release: socket for dlci %d not found\n", dlci);
+ continue;
+ }
+ if (s->dlci == dlci)
+ sckts[i] = 0;
+ else
+ n++;
+ }
+ }
+ return n;
+}
+
+int rfcomm::Send(SocketInternal *sock, const u8* data, int len) {//also see if credits need to be send
+ RFCOMMSocket *s = (RFCOMMSocket*)sock;
+ char credits = 0;
+ char control = BT_RFCOMM_UIH;
+ if (len + 14 > maxframesize) //hci/l2cap header =8, rfcomm header ~ 6
+ printf("Error! packetsize = %d, maxframesize = %d\n", len, maxframesize);
+ if (s->peer_credits == 0) {//peer is low on credits
+ credits = NR_CREDITS;
+ control = BT_RFCOMM_UIH_PF;
+ s->peer_credits += NR_CREDITS;//so provide some more
+ }
+ unsigned char address = (1 << 0) | (initiator << 1) | (s->dlci << 2);
+ if (s->my_credits) {
+ s->my_credits--;
+ return rfcomm_send_packet(_l2cap, address, control, credits, data, len);
+ } else
+ return rfcomm_send_packet(_l2cap, address, control, credits, data, 0);//send an empty packet when credits run out
+}
+
+int rfcomm::Close(SocketInternal* sock) {
+ RFCOMMSocket *s = (RFCOMMSocket*)sock;
+ int id = s->dlci;
+ printf("Closing rfcomm dlci %d state=%d\n", id, s->State);
+ Disconnect(s);
+ int n = release_channel(id);
+ printf("%d channels are still open\n", n);
+ if (n == 0) {//all rfcomm channels are closed
+ printf("....L2CAP must be closed as well\n");
+ rfcomm_send_packet(_l2cap, (1 << 0) | (initiator << 1), BT_RFCOMM_DISC, 0, 0, 0); //close dlci0
+ Socket_Close(_l2cap);
+ _l2cap = 0; //return rfcomm to the pool
+ }
+ return 0;
+}
+
+int rfcomm::SetOpt(SocketInternal *sock, int so, int* data, int len) {
+ switch (so) {
+ case SO_RECBUF:
+ case SO_SNDBUF:
+ maxframesize = *data; //pointless because setting takes effect only before socket is opened (before PN)
+ break;
+ default: return NOPROTOOPT;
+ }
+ return 0;
+}
+
+int rfcomm::GetOpt(SocketInternal *sock, int so, int* data, int len) {
+ switch (so) {
+ case SO_RECBUF:
+ case SO_SNDBUF:
+ if (len >= sizeof(int))
+ *data = maxframesize;
+ break;
+ default: return NOPROTOOPT;
+ }
+ return 0;
+}
+
+int rfcomm::Disconnect(RFCOMMSocket *s) {
+ unsigned char address = (1 << 0) | (initiator << 1) | (s->dlci << 2);
+ return rfcomm_send_packet(_l2cap, address, BT_RFCOMM_DISC, 0, 0, 0);
+}
+
+//expect this to be called with socket type=SOCKET_RFCOM and addr->psm = channel and addr->bdaddr is the BT addr
+//of the device to connect to.
+//eg. Socket_Open(SOCKET_RFCOM, rfcommaddr(bdaddr, chan), receiver_func, appl_obj);
+int rfcomm::Open(SocketInternal* sock, SocketAddrHdr* addr) {
+ int ch = ((L2CAPAddr*)addr)->psm;//abused psm for channel ID
+ RFCOMMSocket *s = (RFCOMMSocket*)sock;
+ int slot = find_slot(ch);
+ if (slot < 0) return 0;
+ sckts[slot] = s->ID;
+ s->serdevice = this;
+ s->State = SocketState_Opening;
+
+ if (_l2cap == 0) { //no rfcomm -> l2cap connection yet
+ printf("Need to open L2CAP channel first before opening RFCOMM channel %d\n", s->dlci);
+ ((L2CAPAddr*)addr)->psm = L2CAP_PSM_RFCOMM;//open the l2cap channel and the rfcomm_ch channel
+ initiator = 1;
+ s->dlci = (ch<<1)|!initiator;
+ _l2cap = Socket_Open(SOCKET_L2CAP, addr, OnRfCommControl, this);//this is the socket between the RFCOMM and the L2CAP layer
+ if (_l2cap)
+ printf("Successfully opened L2CAP channel on socket %d\n", _l2cap);
+ else {
+ printf("Opening L2CAP channel failed\n");
+ sckts[slot] = 0;
+ s->State = SocketState_Closed;
+ return 0;
+ }
+ } else {//bypass the l2cap channel creation
+ s->dlci = (ch<<1)|!initiator;
+ _bt_rfcomm_send_uih_pn_command(_l2cap, initiator, s->dlci, maxframesize);
+ s->State = SocketState_L2CAP_Config_wait;
+ }
+ return s->ID; //return the application unique socket nr.
+}
+
+int rfcomm::set_remote_port_parameters(unsigned char dlci, port_settings *p) {
+ _bt_rfcomm_send_uih_rpn_cmd(_l2cap, initiator, dlci, p);
+ return 0;
+}
+
+//socket is an L2CAP socket and state is the state of the L2CAP socket, not the RFCOMM socket
+void rfcomm::OnRfCommControl(int socket, SocketState state, const u8* data, int len, void* userData) {
+ int packet_processed = 0;
+ rfcomm* self = (rfcomm*)userData;
+ const u8 initiator = self->initiator;
+ printf("\x1B[%dm", 32); //debug: set a different colour
+ printf("OnRfCommControl sock = %d, state = %d, length = %d\n", socket, state, len);
+
+ if (len == 0) {//client only
+ if (state==SocketState_Open) {//callback after change to 'open', the rfcomm->l2cap channel is now open
+ _bt_rfcomm_send_sabm(socket, initiator, 0); //setup the rfcomm control channel dlci==0
+ return;
+ }
+ return; //or other states to handle, e.g. Closing or Closed
+ }
+ //we have data, so parse the header
+ const u8 &addr = data[0];
+ u8 dlci = addr>>2;
+ const u8 &control = data[1];
+ u16 length = data[2]>>1;
+ const u8 *payload = data+3;
+ const u8 *pFCS = data+len-1; //expected position of the CRC
+ if (!(data[2]&1)) { //two byte length
+ length += ((unsigned)data[3])<<7;
+ payload++;
+ }
+ u8 credits = 0;
+ if (control == BT_RFCOMM_UIH_PF)//this packet carries credits
+ credits = *(payload++);
+ //sanity check
+ if (payload+length != pFCS)
+ printf("RFCOMM size mismatch, expected %d payload bytes, got %d\n", length, pFCS-payload);
+
+ if (DEBUG) {
+ printf("RFCOMM: EA=%d, C/R=%d, D=%d, ch=%d; control=%02X (P/F=%d); length=%d\n", addr&1, (addr>>1)&1, (addr>>2)&1, (addr>>3), control, (control>>4)&1, length);
+ printfBytes("payload:", payload, length);
+ }
+ if (dlci == 0) { //dlci==0 control channel
+ L2CAPSocket *s = (L2CAPSocket*)GetSocketInternal(socket);
+ switch (control) {
+ case BT_RFCOMM_UA:// received 1. message BT_RF_COMM_UA
+ packet_processed++;
+ if (s->si.State == SocketState_Closing || s->si.State==SocketState_L2CAP_WaitDisconnect) { //Confirmation of disconnect
+ printf("Remote side confirmed disconnect for socket %d\n", s->si.ID);
+ s->si.SetState(SocketState_Closed);
+ break;
+ }
+ printf("Received RFCOMM unnumbered acknowledgement for channel 0 - multiplexer working\n");
+ printf("Sending UIH Parameter Negotiation Command from OnRfCommControl\n");
+ self->initChannels(socket);
+ break;
+ case BT_RFCOMM_UIH:// received UIH Parameter Negotiation Response
+ switch (payload[0]) {
+ case BT_RFCOMM_PN_RSP: {//client
+ packet_processed++;
+ printf("UIH Parameter Negotiation Response\n");
+ printf("Sending SABM #%u\n", payload[2]);
+ _bt_rfcomm_send_sabm(socket, initiator, payload[2]);//was rfcomm_ch
+ RFCOMMSocket *r = self->find_socket(payload[2]);
+ if (r==0) break;
+ r->my_credits = payload[9]; //initial amount of credits
+ self->maxframesize = min(self->maxframesize, payload[6] + (payload[7]<<8));
+ printf("Max Frame Size = %d, got %d initial credits\n", self->maxframesize, payload[9]);
+ }
+ break;
+ case BT_RFCOMM_PN_CMD: { //remote side sent PN command, mtu and initial credits
+ packet_processed++;
+ printf("UIH Parameter Negotiation Indication\n");
+ self->maxframesize = min(self->maxframesize, payload[6] + (payload[7]<<8));
+ unsigned char cred = payload[9] & 7;
+ unsigned char _dlci = payload[2];
+
+ int skt = rfcomm_manager.find_socket(_dlci>>1);
+ if (skt == 0) { //No-one is listening
+ printf("No-one is Listening on channel %d\n", _dlci>>1);
+ rfcomm_send_packet(socket, (_dlci<<2)|1, BT_RFCOMM_DM, 0, 0, 0);
+ break;
+ }
+ RFCOMMSocket *r = (RFCOMMSocket*)GetSocketInternal(skt);
+ r->my_credits = cred;
+ r->peer_credits = INITIAL_CREDITS;
+ unsigned char reply[10];
+ memcpy(reply, payload, sizeof(reply));
+ reply[0] = BT_RFCOMM_PN_RSP;//[1]=len, [2]=dlci, [4]=priority, [5]=timer(must be 0), [8] retransmissions (must be 0)
+ reply[3] = payload[3]==0xF0 ? 0xE0 : 0; //support credit based flow control
+ reply[6] = self->maxframesize;
+ reply[7] = self->maxframesize>>8;
+ reply[9] = payload[3]==0xF0 ? r->peer_credits : 0;
+ printf("Max Frame Size = %d, give %d initial credits\n", self->maxframesize, reply[9]);
+ rfcomm_send_packet(socket, addr^2, BT_RFCOMM_UIH, 0, reply, sizeof(reply));
+ }
+ break;
+ case BT_RFCOMM_MSC_CMD:
+ packet_processed++;
+ {
+ printf("Received BT_RFCOMM_MSC_IND\n");
+ // fine with this, return the same status and ignore the value, there is no room in the socket to store the value, rfcomm could generate an event
+ RFCOMMSocket *r = self->find_socket(payload[2]>>2);
+ if (r==0) break;
+ unsigned char reply[5];
+ memcpy(reply, payload, 5); //keep length, dlci and value(s)
+ reply[0] = BT_RFCOMM_MSC_RSP; // change command into response
+ printf("Sending MSC_RSP (%d bytes)\n", length);
+ rfcomm_send_packet(socket, addr^2, BT_RFCOMM_UIH, 0, reply, (payload[1]>>1)+2); // reply is 4 or 5 bytes
+ switch (r->State) {
+ case SocketState_L2CAP_Config_wait:
+ r->State = SocketState_L2CAP_Config_wait_send;
+ printf("Sending MSC_CMD\n");
+ _bt_rfcomm_send_uih_msc_cmd(socket, initiator, payload[2]>>2, SIGNALS); // ea=1,fc=0,rtc(DSR/DTR)=1,rtr(RTS/CTs)=1,ic(RI)=0,dv(DCD)=1
+ r->State = SocketState_L2CAP_Config_wait_rsp;
+ break;
+ case SocketState_L2CAP_Config_wait_reqrsp:
+ r->State = SocketState_L2CAP_Config_wait_rsp;
+ break;
+ case SocketState_L2CAP_Config_wait_req:
+ r->SetState(SocketState_Open);
+ break;
+ case SocketState_Open:
+ //inform port adaptation layer
+ printf("Received MSC IND in state Open for dlci 0x%02x\n", payload[2]>>2);
+ break;
+ default:
+ printf("Received MSC IND in state %d for dlci 0x%02x\n", r->State, payload[2]>>2);
+ }
+ }
+ break;
+ case BT_RFCOMM_MSC_RSP:
+ packet_processed++;
+ {
+ RFCOMMSocket *r = self->find_socket(payload[2]>>2);
+ if (r==0) break;
+ if (r->State == SocketState_L2CAP_Config_wait_reqrsp)
+ r->State = SocketState_L2CAP_Config_wait_req;
+ else if (r->State == SocketState_L2CAP_Config_wait_rsp)
+ r->SetState(SocketState_Open);
+ else
+ printf("Received MSC confirmation in state %d for dlci 0x%02x\n", r->State, payload[2]>>2);
+ }
+ break;
+ case BT_RFCOMM_RPN_CMD:
+ packet_processed++;
+ //accept and ignore all settings
+ unsigned char reply[10];
+ memcpy(reply, payload, length); //keep length, dlci and value(s)
+ reply[0] = BT_RFCOMM_RPN_RSP; // change command into response
+ printf("Responding to RPN indication (%d bytes)\n", length);
+ rfcomm_send_packet(socket, addr^2, BT_RFCOMM_UIH, 0, reply, length);
+ break;
+ case BT_RFCOMM_RPN_RSP:
+ packet_processed++;
+ //ignore a response
+ printf("Received RPN confirmation\n");
+ break;
+ default:
+ printf("Unsupported multiplexer frame, type=%02XH\n", data[3]);
+ }
+ break;
+ case BT_RFCOMM_DISC:
+ printf("Remote site actively disconnected from DLCI0\n");
+ rfcomm_send_packet(socket, addr|2, BT_RFCOMM_UA, 0, 0, 0);//confirm disconnection
+ //intentional fall through
+ case BT_RFCOMM_DM:
+ packet_processed++;
+ printf("Remote side refused connection on DLCI0\n");
+ self->_l2cap = Socket_Close(socket);
+ break;
+ case BT_RFCOMM_SABM:
+ packet_processed++;
+ printf("Remote site is seeking connection on DLCI0\n"); //respond with UA
+ rfcomm_send_packet(socket, addr|2, BT_RFCOMM_UA, 0, 0, 0);//confirm connection
+ break;
+ default:
+ printf("Unexpected RFCOMM cmd %02XH for address %02XH, length=%d\n", control, addr, length);
+ }
+ } else { //data is for one of the serial sockets
+ RFCOMMSocket *s = 0;
+ if (control == BT_RFCOMM_SABM) { //req. for conn on this dlci
+ //cannot call self->rfcomm::find_socket because it has no socket yet
+ int skt = rfcomm_manager.find_socket(dlci>>1); //find the server socket
+ s = (RFCOMMSocket*)GetSocketInternal(skt);//the listening socket
+ if (s) {//move the listening socket to the appropriate rfcomm
+ int slot = self->find_slot(dlci>>1);
+ if (slot < 0) {
+ printf("RFCOMM Channel %d is not free on rfcomm with l2cap socket %d\n", dlci>>1, self->_l2cap);
+ return;
+ }
+ s->serdevice = self; //bind the socket to this refcomm entity
+ self->sckts[slot] = skt;
+ rfcomm_manager.remove_socket(skt);
+ } else {
+ printf("Couln't find a listening socket for dlci %d\n", dlci);
+ return;
+ }
+ } else
+ s = self->find_socket(dlci);
+ if (s==0){
+ printf("DLCI %d not found\n", dlci);
+ return;
+ }
+ switch (control) {
+ case BT_RFCOMM_SABM:
+ packet_processed++;
+ rfcomm_send_packet(socket, addr|2, BT_RFCOMM_UA, 0, 0, 0);//confirm connection
+ s->State = SocketState_L2CAP_Config_wait; //wait for msc cmd
+#ifdef TAKE_INITIATIVE
+ printf("Sending MSC_CMD\n");
+ _bt_rfcomm_send_uih_msc_cmd(socket, initiator, dlci, 0x8d); // ea=1,fc=0,rtc(DSR/DTR)=1,rtr(RTS/CTs)=1,ic(RI)=0,dv(DCD)=1
+ s->State = SocketState_L2CAP_Config_wait_reqrsp;
+#endif
+ break;
+ case BT_RFCOMM_UA:// received 2. message BT_RF_COMM_UA
+ packet_processed++;
+ if (s->State == SocketState_Closing) { //Confirmation of disconnect
+ printf("Remote side confirmed disconnect for socket %d\n", s->ID);
+ s->SetState(SocketState_Closed);
+ break;
+ }
+ printf("Received RFCOMM unnumbered acknowledgement for dlci %u - channel opened\n", dlci);
+ if (s->State == SocketState_L2CAP_Config_wait) {
+ printf("Sending MSC CMD\n");
+ _bt_rfcomm_send_uih_msc_cmd(socket, initiator, dlci, 0x8d); // ea=1,fc=0,rtc(DSR/DTR)=1,rtr(RTS/CTs)=1,ic(RI)=0,dv(DCD)=1
+ s->State = SocketState_L2CAP_Config_wait_reqrsp;
+ }
+ break;
+ case BT_RFCOMM_UIH_PF: //user data with credits
+ printf("Got %u credits\n", credits);
+ s->my_credits += credits;
+ //intentional fall-through
+ case BT_RFCOMM_UIH: //user data
+ packet_processed++;
+ if (DEBUG) {
+ printf("RX: address %02x, control %02x: ", addr, control);
+ printHex( payload, length);
+ }
+ if (length) {
+ s->peer_credits--;
+ s->Recv(payload, length);
+ } else
+ printf("Received empty packet\n");
+ if (length == 0 || s->peer_credits == 0) {//send credits when peer runs out
+ //char ini = !(dlci & 1);
+ unsigned char address = (1 << 0) | (initiator << 1) | (dlci << 2);
+ printf("send %d credits to dlci %d\n", NR_CREDITS, addr>>2);
+ rfcomm_send_packet(socket, address, BT_RFCOMM_UIH_PF, NR_CREDITS, NULL, 0);
+ s->peer_credits += NR_CREDITS;
+ }
+ break;
+ case BT_RFCOMM_DISC:
+ packet_processed++;
+ printf("Received DISC IND for dlci %d\n", dlci);
+ rfcomm_send_packet(socket, addr, BT_RFCOMM_UA, 0, 0, 0);//confirm disconnection
+ s->SetState(SocketState_Closed);
+ break;
+ case BT_RFCOMM_DM:
+ case BT_RFCOMM_DM_PF:
+ packet_processed++;
+ printf("Received DM IND (%02X) for dlci %d\n", control, dlci);
+ s->SetState(SocketState_Closed);
+ break;
+ default:
+ printf("Unexpected RFCOMM cmd %02XH for address %02XH, length=%d\n", control, addr, length);
+ }
+ }
+
+ if (!packet_processed) {
+ // just dump data for now
+ printf("??: address %02x, control %02x: ", data[0], data[1]);
+ printHex( data, len );
+ }
+ printf("\x1B[%dm", 0);//reset terminal colour
+}
+
+//should make the functions below member functions
+
+/**
+ * @param credits - only used for RFCOMM flow control in UIH wiht P/F = 1
+ */
+/* Questionable optimisation. When OFFSET==8 a buffer is (dynamically) allocated that provides space for the lower
+ layer headers, this reduces copying to, and allocation of, a lower layer buffer. However, all other layers using
+ HCI/L2CAP must do the same.
+*/
+#define OFFSET 8
+
+int rfcomm_send_packet(uint16_t source_cid, uint8_t address, uint8_t control, uint8_t credits, const uint8_t *data, uint16_t len) {
+
+ uint16_t pos = OFFSET;
+ uint8_t crc_fields = 3;
+
+#if OFFSET == 8
+ uint8_t* rfcomm_out_buffer = new uint8_t[OFFSET+len+6];
+#else
+ static uint8_t rfcomm_out_buffer[MAXFRAMESIZE+6];//seems a bit big as default max framesize is 127
+#endif
+ rfcomm_out_buffer[pos++] = address;
+ rfcomm_out_buffer[pos++] = control;
+
+ // length field can be 1 or 2 octets
+ if (len < 128) {
+ rfcomm_out_buffer[pos++] = (len << 1)| 1; // bits 0-6
+ } else {
+ rfcomm_out_buffer[pos++] = (len & 0x7f) << 1; // bits 0-6
+ rfcomm_out_buffer[pos++] = len >> 7; // bits 7-14
+ crc_fields++;
+ }
+
+ // add credits for UIH frames when PF bit is set
+ if (control == BT_RFCOMM_UIH_PF) {
+ rfcomm_out_buffer[pos++] = credits;
+ }
+
+ // copy actual data
+ memcpy(&rfcomm_out_buffer[pos], data, len);
+ pos += len;
+
+ // UIH frames only calc FCS over address + control (5.1.1)
+ if ((control & 0xef) == BT_RFCOMM_UIH) {
+ crc_fields = 2;
+ }
+ rfcomm_out_buffer[pos++] = crc8_calc(rfcomm_out_buffer+OFFSET, crc_fields); // calc fcs
+ int retval = Socket_Send( source_cid, rfcomm_out_buffer, pos);
+#if OFFSET == 8
+ delete[] rfcomm_out_buffer;
+#endif
+ if (retval <= 0)
+ return retval;
+ return len;//return the size of the payload
+}
+
+void _bt_rfcomm_send_sabm(uint16_t source_cid, uint8_t initiator, uint8_t dlci) {
+ uint8_t address = (1 << 0) | (initiator << 1) | (dlci << 2);
+ rfcomm_send_packet(source_cid, address, BT_RFCOMM_SABM, 0, NULL, 0);
+}
+
+void _bt_rfcomm_send_uih_pn_command(uint16_t source_cid, uint8_t initiator, uint8_t dlci, uint16_t max_frame_size) {
+ uint8_t payload[10];
+ uint8_t address = (1 << 0) | (initiator << 1); // EA and C/R bit set - always server channel 0
+ uint8_t pos = 0;
+ payload[pos++] = BT_RFCOMM_PN_CMD;
+ payload[pos++] = 8 << 1 | 1; // len
+ payload[pos++] = dlci;
+ payload[pos++] = 0xf0; // pre defined for Bluetooth, see 5.5.3 of TS 07.10 Adaption for RFCOMM
+ payload[pos++] = 0; // priority
+ payload[pos++] = 0; // max 60 seconds ack
+ payload[pos++] = max_frame_size & 0xff; // max framesize low
+ payload[pos++] = max_frame_size >> 8; // max framesize high
+ payload[pos++] = 0x00; // number of retransmissions
+ payload[pos++] = INITIAL_CREDITS; // unused error recovery window
+ rfcomm_send_packet(source_cid, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos);
+}
+
+void _bt_rfcomm_send_uih_data(uint16_t source_cid, uint8_t initiator, uint8_t channel, uint8_t *data, uint16_t len) {
+ uint8_t address = (1 << 0) | (initiator << 1) | (!initiator << 2) | (channel << 3);
+ rfcomm_send_packet(source_cid, address, BT_RFCOMM_UIH, 0, data, len);
+}
+
+void _bt_rfcomm_send_uih_msc_cmd(uint16_t source_cid, uint8_t initiator, uint8_t dlci, uint8_t signals) {
+ uint8_t address = (1 << 0) | (initiator << 1); // EA and C/R bit set - always server channel 0
+ uint8_t payload[5];
+ uint8_t pos = 0;
+ payload[pos++] = BT_RFCOMM_MSC_CMD;
+ payload[pos++] = 2 << 1 | 1; // len, should be adapted when adding break byte
+ payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2); //C/R is always 1
+ payload[pos++] = signals;
+ // payload[pos++] = brk;
+ rfcomm_send_packet(source_cid, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos);
+}
+
+void _bt_rfcomm_send_uih_rpn_cmd(uint16_t source_cid, uint8_t initiator, uint8_t dlci, port_settings *val) {
+ uint8_t address = (1 << 0) | (initiator << 1); // EA and C/R bit set - always server channel 0
+ uint8_t payload[sizeof(port_settings)+3];
+ uint8_t pos = 0;
+ payload[pos++] = BT_RFCOMM_RPN_CMD;//type
+ if (val) {
+ payload[pos++] = ((1+sizeof(port_settings)) << 1) | 1; // len
+ payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2);
+ memcpy(payload+pos, (char*)val, sizeof(port_settings));
+ pos += sizeof(port_settings);
+ } else {
+ payload[pos++] = (1 << 1) | 1; // len
+ payload[pos++] = (1 << 0) | (1 << 1) | (dlci << 2);
+ }
+ rfcomm_send_packet(source_cid, address, BT_RFCOMM_UIH, 0, (uint8_t *) payload, pos);
+}
+
+int set_remote_port_parameters(int socket, port_settings *p) {
+ RFCOMMSocket* si = (RFCOMMSocket*)GetSocketInternal(socket);//gets the RFCOMM socket
+ if (!si || si->ID != socket)
+ return ERR_SOCKET_NOT_FOUND;
+ return si->serdevice->set_remote_port_parameters(si->dlci, p);
+}
+
+
+/*
+ * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0.
+ */
+static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */
+ 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
+ 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
+ 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
+ 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
+ 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
+ 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
+ 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
+ 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
+ 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
+ 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
+ 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
+ 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
+ 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
+ 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
+ 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
+ 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF
+};
+
+#define CRC8_INIT 0xFF // Initial FCS value
+#define CRC8_OK 0xCF // Good final FCS value
+/*-----------------------------------------------------------------------------------*/
+uint8_t crc8(uint8_t *data, uint16_t len) {
+ uint16_t count;
+ uint8_t crc = CRC8_INIT;
+ for (count = 0; count < len; count++)
+ crc = crc8table[crc ^ data[count]];
+ return crc;
+}
+
+/*-----------------------------------------------------------------------------------*/
+uint8_t crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum) {
+ uint8_t crc;
+
+ crc = crc8(data, len);
+
+ crc = crc8table[crc ^ check_sum];
+ if (crc == CRC8_OK)
+ return 0; /* Valid */
+ else
+ return 1; /* Failed */
+
+}
+
+/*-----------------------------------------------------------------------------------*/
+uint8_t crc8_calc(uint8_t *data, uint16_t len) {
+ /* Ones complement */
+ return 0xFF - crc8(data, len);
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/rfcomm/RFCOMM.h Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,326 @@
+#ifndef RFCOMM_H
+#define RFCOMM_H
+#include "USBHost.h"
+#include "hci.h"
+#include "Utils.h"
+
+#define MAX_RFCOMM_SCKTS 4
+//#define MAX_FRAME_SIZE 350 //ACL buffer - some headroom
+#define MAX_FRAME_SIZE 127 //ft size
+
+/*
+template <class T>
+T min(T a, T b) {
+ return a<b ? a : b;
+}
+*/
+
+#define MASK_BITRATE 0X0001
+#define MASK_DATABITS 0X0002
+#define MASK_STOPBITS 0X0004
+#define MASK_PARITYBITS 0X0008
+#define MASK_PARITYTYPE 0X0010
+#define MASK_XONCHAR 0X0020
+#define MASK_XOFFCHAR 0X0040
+#define MASK_XONXOFFIN 0X0100
+#define MASK_XONXOFFOUT 0X0200
+#define MASK_RTRIN 0X0400
+#define MASK_RTROUT 0X0800
+#define MASK_RTCIN 0X1000
+#define MASK_RTCOUT 0X2000
+
+struct port_settings {
+ unsigned char baud;
+unsigned char bits:
+ 2;
+unsigned char stop:
+ 1;
+unsigned char par:
+ 1;
+unsigned char par_t:
+ 2;
+unsigned char :
+ 2;
+unsigned char flow:
+ 6;
+unsigned char :
+ 2;
+ unsigned char xon;
+ unsigned char xoff;
+ unsigned short mask;
+};
+
+class rfcomm;
+class RFCOMMManager;
+#define MAX_RFCOMM_DEVICES 8 //physical devices
+
+class RFCOMMSocket: public SocketInternal {//this object must not be larger than SocketInternalPad (socketinternal + 8 bytes)
+public:
+ rfcomm* serdevice;
+ u8 dlci; //channel + D bit, D bit is inverted initiator bit
+ u8 my_credits, peer_credits;
+};
+
+class rfcomm: public SocketHandler {
+ int _l2cap; //socket to the l2cap layer
+ int _devClass;
+ BD_ADDR _addr;
+ u8 initiator;
+ u8 _pad[0]; // Struct align
+ char sckts[MAX_RFCOMM_SCKTS];
+ //static
+ unsigned maxframesize;
+ int find_slot(unsigned ch);
+ RFCOMMSocket* find_socket(unsigned dlci);
+ void initChannels(int socket);
+ unsigned release_channel(unsigned dlci);
+ static void OnRfCommControl(int socket, SocketState state, const u8* data, int len, void* userData);//I guess this is called when data comes out of the socket
+ int Disconnect(RFCOMMSocket*);
+public:
+ rfcomm() : _l2cap(0), _devClass(0) {
+ for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) sckts[i] = 0;
+ maxframesize = MAX_FRAME_SIZE;
+ }
+
+ bool InUse() {
+ return _l2cap != 0;
+ }
+ virtual int Open(SocketInternal* sock, SocketAddrHdr* addr);
+ virtual int Send(SocketInternal* sock, const u8* data, int len);//wrap data in RFCOMM frame and dispatch
+ virtual int SetOpt(SocketInternal *sock, int so, int* data, int len);
+ virtual int GetOpt(SocketInternal *sock, int so, int* data, int len);
+ virtual int Close(SocketInternal* sock);
+ virtual char* Name() {
+ return "rfcomm SocketHandler";
+ }
+ void Recv(const u8* data, int len) {
+ printf("rfcomm::Recv was called\n");
+ }
+#if 0
+ int Listen(unsigned char ch=0) {//passive open, similar semantics to 'Open' but connection is only made at request of the peer
+ RFCOMMSocket *s = 0;//this entity may or may not be bound to a remote entity
+ if (ch>0) {//specific channel
+ s = find_socket(ch<<1);
+ if (s) { //socket for this channel already in use
+ printf("rfcomm::Listen: channel %d already in use\n", ch);
+ return 0;
+ } //else s==0, no socket for channel ch
+ }//else listen to any channel
+ int sn = find_slot(ch);
+ if (sn<0) {
+ printf("No socket could be found for channel %d\n", ch);
+ return 0;
+ } //else use slot 'sn' for the new rfcomm socket
+ int sock = Socket_Create(SOCKET_RFCOM, OnRfCommControl, this);//allocate an rfcomm socket
+ sckts[sn] = sock; //claim the socket
+ RFCOMMSocket *rs = (RFCOMMSocket*)GetSocketInternal(sock);
+ rs->serdevice = this;
+ rs->dlci = (ch<<1)|1;//server socket
+ //initiator = 0; what to do if already connected actively on different channel???
+ /*l2cap is already present or is created when accepting the connection
+ if (_l2cap == 0) { //no rfcomm -> l2cap connection yet
+ printf("Need to open L2CAP channel first before opening RFCOMM channel %d\n", rs->dlci);
+ ((L2CAPAddr*)addr)->psm = L2CAP_PSM_RFCOMM;//open the l2cap channel and the rfcomm_ch channel
+ initiator = 0;
+ _l2cap = Socket_Create(SOCKET_L2CAP, addr, OnRfCommControl, this);//this is the socket between the RFCOMM and the L2CAP layer
+ if (_l2cap)
+ printf("Successfully opened L2CAP channel on socket %d\n", _l2cap);
+ else {
+ printf("Opening L2CAP channel failed\n");
+ return 0;
+ }
+ }
+ */
+ return sock;
+ }
+#endif
+ //int Open(BD_ADDR* bdAddr, inquiry_info* info);
+ int set_remote_port_parameters(unsigned char dlci, port_settings *p);
+ friend RFCOMMManager;
+};
+
+class RFCOMMManager: public SocketHandler {
+ rfcomm _devs[MAX_RFCOMM_DEVICES];
+ int serverSock;
+ char sckts[MAX_RFCOMM_SCKTS];//listening sockets
+public:
+ virtual int Open(SocketInternal* sock, SocketAddrHdr* addr) {
+ L2CAPAddr* ad = (L2CAPAddr*)addr;
+ BD_ADDR* a = &ad->bdaddr;
+ rfcomm *r = FindRfCommDevice(a);
+ if (r==0)
+ r = NewRfCommDevice();
+ if (r==0)
+ return 0;
+ return r->Open(sock, addr);
+ }
+
+ int FindSocket(BTDevice* dev) {//finds the l2cap socket for a certain rfcomm-btdevice connection
+ for (int i = 0; i < MAX_RFCOMM_DEVICES; i++) {
+ rfcomm *r = _devs+i;
+ int l2cap = r->_l2cap;
+ if (l2cap) {
+ L2CAPSocket *p = (L2CAPSocket*)GetSocketInternal(l2cap);
+ if (p->btdevice == dev)
+ return l2cap;
+ }
+ }
+ return 0;
+ }
+
+ rfcomm* FindDev(BTDevice* dev) {//finds the rfcomm entity for a certain rfcomm-btdevice connection
+ for (int i = 0; i < MAX_RFCOMM_DEVICES; i++) {
+ rfcomm *r = _devs+i;
+ int l2cap = r->_l2cap;
+ if (l2cap) {
+ L2CAPSocket *p = (L2CAPSocket*)GetSocketInternal(l2cap);
+ if (p->btdevice == dev)
+ return r;
+ }
+ }
+ return 0;
+ }
+
+ int BindSocket(int s) {
+ L2CAPSocket *ls = (L2CAPSocket*)GetSocketInternal(s);
+ printf("Binding l2cap socket %d to a new rfcomm server entity\n", s);
+ rfcomm *r = NewRfCommDevice();
+ r->_l2cap = s;
+ r->initiator = 0;//we are server
+ ls->si.userData = r;//was BTDevice, make rfcomm
+ return 0;
+ }
+
+ int new_slot(unsigned ch) {
+ for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) {
+ if (sckts[i] != 0) { //socket is in use
+ RFCOMMSocket *s = (RFCOMMSocket*)GetSocketInternal(sckts[i]);
+ if (s==0) {
+ printf("find_slot: socket %d not found\n", sckts[i]);
+ continue;
+ }
+ if ((s->dlci >> 1) == ch) {
+ printf("Channel %d is already in use on socket %d\n", ch, sckts[i]);
+ return -1;
+ }
+ } else //slot is free
+ return i;
+ }
+ return -2; //no free slots
+ }
+
+ int find_socket(unsigned ch) {
+ for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) {
+ if (sckts[i] != 0) { //socket is in use
+ RFCOMMSocket *s = (RFCOMMSocket*)GetSocketInternal(sckts[i]);
+ if (s==0) {
+ printf("find_slot: socket %d not found\n", sckts[i]);
+ continue;
+ }
+ if ((s->dlci >> 1) == ch) {
+ printf("Found Channel %d on socket %d\n", ch, sckts[i]);
+ return sckts[i];
+ }
+ else
+ printf("slot %d has socket %d has dlci %d\n", i, sckts[i], s->dlci);
+ }
+ else
+ printf("Slot %d is free\n", i);
+ }
+ printf("channel %d not found\n", ch);
+ return 0; //channel not found
+ }
+
+ int remove_socket(int sock) {
+ for (int i = 0; i < MAX_RFCOMM_SCKTS; i++) {
+ if (sckts[i] == sock) {
+ sckts[i] = 0;
+ return 0;
+ }
+ }
+ return -1;
+ }
+
+ virtual int Listen(SocketInternal* sock, int ch) {//called by Socket_Listen(SOCKET_RFCOM, channel, callback, userData)
+ int slot = new_slot(ch);
+ switch (slot) {
+ case -1:
+ printf("There is already someone listening on ch %d\n", ch);
+ return ERR_SOCKET_CANT_LISTEN;//channel is occupied
+ case -2:
+ printf("All listener sockets are in use\n");
+ return ERR_SOCKET_NONE_LEFT;
+ }
+ RFCOMMSocket *rs = (RFCOMMSocket*)sock;
+ const char dir = 0;
+ rs->dlci = (ch<<1)|dir;
+ rs->State = SocketState_Listen;
+ rs->serdevice = 0;//don't know yet
+ sckts[slot] = rs->ID;
+ printf("RFCOMM listener socket %d for ch %d (dlci 0x%02X) is assigned to slot %d\n", rs->ID, ch, rs->dlci, slot);
+ return rs->ID;
+ }
+/*
+ virtual int Accept(SocketInternal *sock, int scid, int rxid) { //called indirectly from BTDevice::Control
+ //sock is registered as an RFCOMM sock but we use it as an L2CAP sock
+ //sock->type=RFCOM, meaning open/close/send/accept go to RFCOMMManager first
+ //sock->userData = BTDevice, necessary to make the call back to BTDevice::Accept
+ //Internal = L2CAPSocket, for scid, dcid
+ BTDevice *l2cap = (BTDevice*)sock->userData;
+ //sock->si.dcid = scid
+ //sock->si.scid = something based on sock->ID
+ serverSock = sock->ID;
+ printf("Invoking accept on %p (%s) for sock %d and scid=%d\n", l2cap, l2cap->Name(), sock->ID, scid);
+ return l2cap->Accept(sock, scid, rxid); //connect 'serverSock' to the remote RFCOMM client
+ }
+*/
+ virtual int Send(SocketInternal* sock, const u8* data, int len) {
+ RFCOMMSocket *s = (RFCOMMSocket*)sock;
+ return s->serdevice->Send(sock, data, len);
+ }
+
+ virtual int Close(SocketInternal* sock) {
+ RFCOMMSocket *s = (RFCOMMSocket*)sock;
+ return s->serdevice->Close(sock);
+ }
+
+ virtual int SetOpt(SocketInternal* sock, int so, int* data, int len) {
+ RFCOMMSocket *s = (RFCOMMSocket*)sock;
+ return s->serdevice->SetOpt(sock, so, data, len);
+ }
+
+ virtual int GetOpt(SocketInternal* sock, int so, int* data, int len) {
+ RFCOMMSocket *s = (RFCOMMSocket*)sock;
+ return s->serdevice->GetOpt(sock, so, data, len);
+ }
+
+ virtual char* Name() {
+ return "RFCOMMManager SocketHandler";
+ }
+
+ rfcomm* NewRfCommDevice() {//allocate a new RFCOMM device from the pool
+ for (int i = 0; i < MAX_RFCOMM_DEVICES; i++)
+ if (!_devs[i].InUse())
+ return _devs+i;
+ return 0;
+ }
+
+ rfcomm* FindRfCommDevice(BD_ADDR* ad) {//get a specific RFCOMM device from the pool
+ for (int i = 0; i < MAX_RFCOMM_DEVICES; i++)
+ if (_devs[i].InUse() && memcmp(ad, &_devs[i]._addr, 6)==0)
+ return _devs+i;
+ return 0;
+ }
+
+ static void SerServer(int socket, SocketState state, const u8* data, int len, void* userData) {
+ printfBytes("SerServer: ", data, len);
+ //userData is the rfcomm
+ rfcomm::OnRfCommControl(socket, state, data, len, userData);
+ }
+ //friend rfcomm;
+};
+
+int set_remote_port_parameters(int socket, port_settings *p);
+extern RFCOMMManager rfcomm_manager;
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/sdp/sdp.cpp Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,941 @@
+#include "mbed.h"
+#include "Utils.h"
+#include "hci.h"
+#include "sdp_data.h"
+#include "sdp.h"
+
+SDPManager SDP; //instance
+const unsigned char base_uuid[16] = { 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0, 0x07, 0x70, 0, 0x10, 0, 0};
+map<unsigned, serv_rec*> SDPManager::server;
+int SDPManager::serverSock = 0;
+
+void attribHandler(serv_rec *r) {
+ printf("Service 0x%08X\n", (*r)[0x0000]->asUnsigned());
+ map<unsigned short, sdp_data*>::iterator it = r->begin();
+ for (;it != r->end();it++) {
+ printf(" 0x%04X: %s\n", (*it).first, (*it).second->asString());
+ }
+}
+
+#define BROWSEGROUP 0x1001
+#define BROWSEROOT 0x1002
+#define SERIALSERV 0x1101
+
+SDPHandler::SDPHandler(): txid(1), tree(0) {
+ ErrorResponse=errorhandler;
+ ServiceSearchResponse=0;
+ ServiceAttributeResponse=attribHandler;
+ ServiceSearchAttributeResponse=0;
+ buf = l2cap_buf+OFFSET;
+ contBuf = 0;
+ byteCount = 0;
+ contState[0] = 0;
+ _state = 0;
+}
+
+void SDPHandler::Clear() {
+ for (index = services.begin(); index != services.end(); index++) {//for all services
+ for (serv_rec::iterator it = index->second->begin(); it != index->second->end(); it++)//for all attributes
+ delete it->second; //delete the attribute value tree
+ delete (*index).second; //delete the attribute list
+ }
+ services.clear();//and empty the services list
+}
+
+//Called as: Socket_Open(SOCKET_SDP, addr, callback, userdata(this)) from SDPManager
+//never called
+int SDPHandler::Open(SocketInternal* sock, SocketAddrHdr* addr) {
+ printf("Successfully opened SDP socket %d\n", sock->ID);
+ sock->SetState(SocketState_Open);
+ sdp_socket = sock->ID;
+ return sdp_socket;
+}
+
+int SDPHandler::Send(SocketInternal* sock, const u8* data, int len) {
+ printf("SDPHandler::Send should not be called directly\n");
+// return Socket_Send(_l2cap, data, len);
+ BTDevice *l2cap = (BTDevice*)sock->userData;
+ return l2cap->Send(sock, data, len);
+}
+
+int SDPHandler::Close(SocketInternal* sock) {
+ printf("SDPHandler::Close(%d)\n", sock->ID);
+ Clear();
+// printf("SDP socket %d and L2CAP socket %d closed, freemem=%d\n", sock->ID, _l2cap, AvailableMemory(1));
+ int retval = 0;//Socket_Close(_l2cap);//could also keep it open for the next connection
+ return retval;
+}
+
+//this function is called when the L2CAP layer receives SDP packets (see SDPHandler::Open), userdata is the sdpmanager instance
+void SDPHandler::OnSdpRsp(int socket, SocketState state, const u8* data, int len, void* userData) {
+ printf("\x1B[%dm", 35);
+// printf("OnSdpRsp(socket=%d, state=%d, len=%d)\n", socket, state, len);
+ printf("SDPHandler::OnSdpRsp(socket=%d, state=%d, len=%d, freemem=%d\n", socket, state, len, AvailableMemory(1));
+ SDPHandler *self = (SDPHandler*)userData;
+ if (state == SocketState_Open) {
+ self->sdp_socket = socket;
+ self->OnSdpRsp(data, len);
+ } else if (state == SocketState_Closed) {
+ SDP.Destroy(socket);
+ }
+ printf("\x1B[%dm", 0);
+}
+
+//this function is called when the L2CAP layer receives SDP packets (see SDPHandler::Open), userdata is the sdpmanager instance
+//void SDPHandler::OnSdpRsp(int socket, SocketState state, const u8* data, int len, void* userData) {
+void SDPHandler::OnSdpRsp(const u8* data, int len) {
+ static sdp_data list(sdp_data::SEQUENCE);
+ static sdp_data all(0x0000ffffU,4);
+ static sdp_data serviceID(0U, 2);
+ static sdp_data name(0x100U, 2);
+ static sdp_data root(sdp_data::UUID, BROWSEROOT);
+ static sdp_data req(sdp_data::SEQUENCE);
+ static bool once = true;
+// printf("_state=%d first=%d ", _state, once);
+ if (once) {
+ list.add_element(&all);
+ //list.add_element(&serviceID);
+ //list.add_element(&name);
+ req.add_element(&root);
+ once = false;
+ }
+ if (data) {
+ parseRsp(data, len);
+ }
+ switch (_state) {
+ case 0: //closed
+ if (len==0) { //socket just opened
+ //'Open' cleared the services list
+ ServiceSearchRequest(&req, 10);
+ _state = 1; //wait for service handles
+ }
+ break;
+ case 1: //service handles arriving
+ if (contState[0]) {//continuation, repeat request
+ ServiceSearchRequest(&req, 5);
+ } else {
+ if (data[0]==3) {
+ index = services.begin();
+ if (index != services.end()) {
+ unsigned handle = (*index).first;
+ //printf("req.: handle %#X\n", handle);
+ ServiceAttributeRequest(handle, 100, &list);//0x1001D
+ } else
+ printf(" - empty list - \n");//should not happen
+ _state = 2; //wait for attribute response
+ } else
+ printf("Expected a ServiceSearchResponse 0x03, got %#x\n", data[0]);
+ }
+ break;
+ case 2:
+ if (contState[0])//repeat request
+ ServiceAttributeRequest((*index).first, 100, &list);
+ else {
+ if (data[0]==5) {
+ index++; //move to next service
+ if (index != services.end()) {
+ //printf("req.: handle %#X\n", (*index).first);
+ ServiceAttributeRequest((*index).first, 100, &list);
+ } else {
+ printf(" - end of list - \n");
+ Socket_Close(sdp_socket); //Note: socket=L2CAP, sdp_socket=SDP !!!
+ _state = 0;
+ }
+ } else
+ printf("Expected a ServiceAttributeResponse 0x05, got %#x\n", data[0]);
+ }
+ break;
+ }
+}
+
+//this function is called when the SDP sockets receives data (see HCICallback in TestShell),
+//currently does not happen because not forwarded from OnSdpRsp, can be used to handle multiple connections
+void SDPHandler::OnSockCallback(int socket, SocketState state, const u8* data, int len, void* userData) {
+ printf("SDPHandler::OnSockCallback(socket=%d, state=%d, len=%d)\n", socket, state, len);
+ printfBytes("Got SDP Response from socket: ", data, len);
+}
+
+//void SDPHandler::errorhandler(unsigned err) {//default error handler
+void errorhandler(unsigned err) {//default error handler
+ switch (err) {
+ case 1:
+ printf("Unsupported version of SDP\n");
+ break;
+ case 2:
+ printf("Invalid SDP ServiceRecordHandle\n");
+ break;
+ case 3:
+ printf("SDP syntax error\n");
+ break;
+ case 4:
+ printf("PDU size was invalid\n");
+ break;
+ case 5:
+ printf("Continuation state was invalid\n");
+ break;
+ case 6:
+ printf("SDP server has insufficient resources\n");
+ break;
+ default:
+ printf("Unknown SDP error code\n");
+ break;
+ }
+}
+
+int SDPHandler::ServiceSearchRequest(sdp_data *sp, unsigned count, unsigned cs) {
+ int parlen = sp->Size() + contState[0] + 3;
+ buf[0] = 2; //pdu
+ buf[1] = txid>>8;
+ buf[2] = txid++;
+ buf[4] = parlen;
+ buf[3] = parlen>>8;
+ int p = sp->build(buf+5, 100-10);
+ buf[p+6] = count;
+ buf[p+5] = count>>8;
+ buf[p+7] = contState[0];
+ for (int j = 1; j <= contState[0]; j++)
+ buf[p+j+7] = contState[j];
+ //printfBytes("SDP Send: ", buf, parlen+5);
+ return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET);
+}
+
+int SDPHandler::ServiceAttributeRequest(unsigned handle, unsigned count, sdp_data* al, unsigned cs) {
+ int parlen = al->Size() + contState[0] + 7;
+ buf[0] = 4; //pdu
+ buf[1] = txid>>8;
+ buf[2] = txid++;
+ buf[4] = parlen;
+ buf[3] = parlen>>8;
+ for (int i = 0; i < 4; i++)
+ buf[i+5] = ((char*)&handle)[3-i];
+ buf[9] = count>>8;
+ buf[10] = count;
+ int p = al->build(buf+11, 100-26);
+ buf[p+11] = contState[0];
+ for (int j = 1; j <= contState[0]; j++)
+ buf[p+j+11] = contState[j];
+ //printfBytes("SDP Send: ", buf, parlen+5);
+ return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET);
+}
+
+int SDPHandler::ServiceSearchAttributeRequest(sdp_data *sp, unsigned count, sdp_data* al, unsigned cs) {
+ int parlen = sp->Size() + al->Size() + contState[0] + 3; // count (2 bytes) + at least 1 cont
+ buf[0] = 6; //pdu
+ buf[1] = txid>>8;
+ buf[2] = txid++;
+ buf[4] = parlen;
+ buf[3] = parlen>>8;
+ int p = sp->build(buf+5, 30);
+ buf[p+6] = count;
+ buf[p+5] = count>>8;
+ p += al->build(buf+11, 100-38);
+ buf[p+7] = contState[0];
+ for (int j = 1; j <= contState[0]; j++)
+ buf[p+j+7] = contState[j];
+ //printfBytes("SDP Send: ", buf, parlen+5);
+ return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET);
+}
+
+unsigned BE32(unsigned le) {
+ unsigned be=0;
+ for (int i = 0; i < 32; i+=8){
+ be |= ((le>>i)&0xFFU)<<(24-i);
+ }
+ return be;
+}
+
+int SDPManager::ServiceSearchReply(unsigned rxid, unsigned *handles, unsigned count, unsigned cs) {//'count' is number of matching handles and capped at the maximumservicerecordcount
+ unsigned size = count*4;
+ unsigned cont = 0;//outgoing continuation
+ unsigned char *_buf = new unsigned char[OFFSET+5+4+size+3];
+ unsigned char *buf = _buf+OFFSET;
+ unsigned current = count - cs; //remainder of data to send
+ unsigned parlen = 4 + 4*current + 1 ;// attributelistbytecount+payload+no_continuation
+ //never need a continuation unless other criterion like PDU size is used
+ if (current > count) {//still too large, need continuation
+ current = count; //limit at maximum
+ cont = cs + count; //start for next iteration
+ parlen = 4 + 4*current + 3; //adjusted for payload and continuation
+ printf("Need continuation, sending handles [%d, %d> of attributeList\n", cs, cont);
+ } else {
+ // printf("Final or only block, sending bytes [%d, %d> of attributeList\n", cs, cs+byteCount);
+ }
+ buf[0] = 3; //pdu
+ buf[1] = rxid>>8;
+ buf[2] = rxid;
+ buf[3] = parlen>>8;
+ buf[4] = parlen;
+ buf[5] = count>>8;
+ buf[6] = count;
+ buf[7] = current>>8;
+ buf[8] = current;
+ unsigned *p = (unsigned*)&buf[9];
+ for (int i = 0; i < current; i++)
+ p[i] = BE32(handles[i]);
+ //printf("'build' added %d bytes to the buffer\n", p);
+ if (cs == 0) { //this is not a continuation
+ buf[5+parlen-1] = 0;
+ } else { //this is a continuation
+ memcpy(buf+9, buf+9+cs*4, current*4);//move part of interrest to beginning of buffer
+ buf[5+parlen-3] = 2;
+ buf[5+parlen-2] = cont>>8;
+ buf[5+parlen-1] = cont;
+ }
+ //printfBytes("SDP Send: ", buf, parlen+5);
+ int retval = Socket_Send(serverSock, _buf, parlen + 5 + OFFSET);
+ delete[] _buf;
+ return retval;
+}
+
+int SDPManager::ServiceAttributeReply(unsigned rxid, sdp_data* al, unsigned count, unsigned cs) {
+ unsigned size = al->Size();
+ unsigned cont = 0;//outgoing continuation
+ unsigned char *_buf = new unsigned char[OFFSET+5+2+size+3];
+ unsigned char *buf = _buf+OFFSET;
+ unsigned byteCount = size - cs; //remainder of data to send
+ unsigned parlen = 2 + byteCount + 1 ;// attributelistbytecount+payload+no_continuation
+ if (byteCount > count) {//still too large, need continuation
+ byteCount = count; //limit at maximum
+ cont = cs + count; //start for next iteration
+ parlen = 2 + byteCount + 3; //adjusted for payload and continuation
+ printf("Need continuation, sending bytes [%d, %d> of attributeList\n", cs, cont);
+ } else {
+ // printf("Final or only block, sending bytes [%d, %d> of attributeList\n", cs, cs+byteCount);
+ }
+ buf[0] = 5; //pdu
+ buf[1] = rxid>>8;
+ buf[2] = rxid;
+ buf[3] = parlen>>8;
+ buf[4] = parlen;
+ buf[5] = byteCount>>8;
+ buf[6] = byteCount;
+ int p = al->build(buf+7, size);//limited only by buffersize
+ //printf("'build' added %d bytes to the buffer\n", p);
+ if (cs == 0) { //this is not a continuation
+ buf[byteCount+7] = 0;
+ } else { //this is a continuation
+ memcpy(buf+7, buf+7+cs, byteCount);//move part of interrest to beginning of buffer
+ buf[byteCount+7] = 2;
+ buf[byteCount+8] = cont>>8;
+ buf[byteCount+9] = cont;
+ }
+ //printfBytes("SDP Send: ", buf, parlen+5);
+ int retval = Socket_Send(serverSock, _buf, parlen + 5 + OFFSET);
+ delete[] _buf;
+ return retval;
+}
+
+int SDPManager::ServiceSearchAttributeReply(unsigned rxid, sdp_data* al, unsigned count, unsigned cs) {
+ unsigned size = al->Size();
+ unsigned cont = 0;//outgoing continuation
+ unsigned char *_buf = new unsigned char[OFFSET+5+2+size+3];
+ unsigned char *buf = _buf+OFFSET;
+ unsigned byteCount = size - cs; //remainder of data to send
+ unsigned parlen = 2 + byteCount + 1 ;// attributelistbytecount+payload+no_continuation
+ if (byteCount > count) {//still too large, need continuation
+ byteCount = count; //limit at maximum
+ cont = cs + count; //start for next iteration
+ parlen = 2 + byteCount + 3; //adjusted for payload and continuation
+ printf("Need continuation, sending bytes [%d, %d> of attributeList\n", cs, cont);
+ } else {
+ // printf("Final or only block, sending bytes [%d, %d> of attributeList\n", cs, cs+byteCount);
+ }
+ buf[0] = 7; //pdu
+ buf[1] = rxid>>8;
+ buf[2] = rxid;
+ buf[3] = parlen>>8;
+ buf[4] = parlen;
+ buf[5] = byteCount>>8;
+ buf[6] = byteCount;
+ int p = al->build(buf+7, size);//limited only by buffersize
+ //printf("'build' added %d bytes to the buffer\n", p);
+ if (cs == 0) { //this is not a continuation
+ buf[byteCount+7] = 0;
+ } else { //this is a continuation
+ memcpy(buf+7, buf+7+cs, byteCount);//move part of interrest to beginning of buffer
+ buf[byteCount+7] = 2;
+ buf[byteCount+8] = cont>>8;
+ buf[byteCount+9] = cont;
+ }
+ //printfBytes("SDP Send: ", buf, parlen+5);
+ int retval = Socket_Send(serverSock, _buf, parlen + 5 + OFFSET);
+ delete[] _buf;
+ return retval;
+}
+
+//unsigned SDPHandler::getval(const unsigned char *p, int n) {
+unsigned getval(const unsigned char *p, int n) {
+ unsigned ret = 0;
+ for (int i = 0; i < n; i++)
+ ret = (ret<<8) + (unsigned)p[i];
+ return ret;
+}
+
+//unsigned SDPHandler::length(const unsigned char *el, unsigned &p) {
+unsigned length(const unsigned char *el, unsigned &p) {
+ unsigned len = 0;
+ switch (el[p++] & 7) {//length
+ case 0:
+ len = 1;
+ break;
+ case 1:
+ len = 2;
+ break;
+ case 2:
+ len = 4;
+ break;
+ case 3:
+ len = 8;
+ break;
+ case 4:
+ len = 16;
+ break;
+ case 7://4bytes
+ len= el[p++]<<24;
+ len += el[p++]<<16;
+ case 6://2bytes
+ len += el[p++]<<8;
+ case 5://1byte
+ len += el[p++];
+ break;
+ }
+ return len;
+}
+
+extern "C" void HardFault_Handler() {
+ printf("Hard Fault! %d bytes left\n", AvailableMemory(1));
+ while (1);
+}
+
+unsigned SDPHandler::parseLight (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) {
+ unsigned p = 0;
+ unsigned len = length(el, p);
+ int end = p+len;//end is the index of the item just after the sequence
+ sdp_data *item = 0;
+ switch (el[0]>>3) {//type
+ case sdp_data::NULL_:
+ printf("NULL ");
+ break;
+ case sdp_data::UNSIGNED:
+ printf("UINT%d=%u ", len, (unsigned)getval(el+p, len));
+ break;
+ case sdp_data::SIGNED:
+ printf("INT%d=%d ", len, (unsigned)getval(el+p, len));
+ break;
+ case sdp_data::UUID:
+ if (len==16) {
+ printf("UUID16= ");
+ for (int i = 0; i < 16; i++)
+ printf("%02x ", el[p+i]);
+ } else
+ printf("UUID%d=%u ", len, (unsigned)getval(el+p, len));
+ break;
+ case sdp_data::STRING:
+ printf("STR%d='%s' ", len, (char*)el+p);
+ break;
+ case sdp_data::BOOL:
+ printf("BOOL%d=%d ", len, (unsigned)getval(el+p, len));
+ break;
+ case sdp_data::SEQUENCE:
+ goto skip;
+ case sdp_data::ALTERNATIVE:
+skip: {//p points just after the length indicator, hence at the first item IN the sequence
+ printf("SEQ%d{%p ", len, item);
+ int n = 0;
+ unsigned short key;
+ serv_rec *dummy = 0;
+ while (p < end) {
+ sdp_data *elem = 0;
+ p += parseLight(el + p, len-p, elem, dummy);//parse each element in the sequence, the second arg is as yet unused
+ if (record) {
+ if (n & 1) { //value
+ record->insert(pair<unsigned short, sdp_data*>(key, elem));
+ } else //key
+ key = n;
+ n++;
+ }
+ }
+ }
+ printf("}\n");
+ break;
+ case 8:
+ printf("URL%d='%s' ", len, (char*)el+p);
+ break;
+ default:
+ printf("Parse: Unknown type %d, len=%d (code=%#02X)\n", el[0]>>3, len, el[0]);
+ }
+ result = item;
+ return end;
+}
+
+unsigned SDPHandler::parse (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) {
+ unsigned p = 0;
+ unsigned len = length(el, p);
+ int end = p+len;//end is the index of the item just after the sequence
+ sdp_data *item = 0;
+ switch (el[0]>>3) {//type
+ case sdp_data::NULL_:
+ item = new sdp_data();
+ break;
+ case sdp_data::UNSIGNED:
+ item = new sdp_data((unsigned)getval(el+p, len), len);
+ break;
+ case sdp_data::SIGNED:
+ item = new sdp_data((int)getval(el+p, len), len);
+ break;
+ case sdp_data::UUID:
+ if (len==16) {
+ char rev[16];
+ for (int i = 0; i < 16; i++)
+ rev[i] = el[p+15-i];
+ item = new sdp_data(sdp_data::UUID, rev, len);
+ } else
+ item = new sdp_data(sdp_data::UUID, getval(el+p, len), len);
+ break;
+ case sdp_data::STRING:
+ item = new sdp_data((char*)el+p, len);
+ break;
+ case sdp_data::BOOL:
+ item = new sdp_data((bool)getval(el+p, len), len);
+ break;
+ case sdp_data::SEQUENCE:
+ item = new sdp_data(sdp_data::SEQUENCE);
+ goto skip;
+ case sdp_data::ALTERNATIVE:
+ item = new sdp_data(sdp_data::ALTERNATIVE);
+skip: {//p points just after the length indicator, hence at the first item IN the sequence
+ //printf("SEQ%d{%p ", len, item);
+ int n = 0;
+ unsigned short key;
+ serv_rec *dummy = 0;//means: there is no service record to fill in for deeper levels
+ while (p < end) {
+ sdp_data *elem = 0; //this becomes the tree with attribute values
+ p += parse(el + p, len-p, elem, dummy);//parse each element in the sequence, the second arg is as yet unused
+ if (record) { //if at the level of attribute list, add elem to record as key/value pair
+ if (n & 1) { //value
+ record->insert(pair<unsigned short, sdp_data*>(key, elem));
+ } else //key
+ key = elem->asUnsigned();
+ n++;
+ } else //just add the elements to the value tree
+ item->add_element(elem);
+ }
+ }
+ //printf("}\n");
+ break;
+ case 8:
+ item = new sdp_data(sdp_data::URL, (char*)el+p, len);
+ break;
+ default:
+ printf("Parse: Unknown type %d, len=%d (code=%#02X)\n", el[0]>>3, len, el[0]);
+ }
+ result = item;
+ return end;
+}
+
+void SDPHandler::append(const unsigned char *payload, int len) {
+ unsigned char *tmp = new unsigned char[byteCount + len];//append the payload to the previous continuation buffer
+ if (contBuf && byteCount) {
+ memcpy(tmp, contBuf, byteCount); //copy the existing part
+ delete[] contBuf;//delete the old buffer
+ }
+ memcpy(tmp+byteCount, payload, len); //append the new part
+ contBuf = tmp;
+ byteCount += len;
+}
+
+void SDPHandler::freeBuf() {
+ if (contBuf) {
+ delete[] contBuf;
+ contBuf = 0;
+ }
+ byteCount = 0;
+}
+
+
+//TODO: test case 7, add server support (cases 2, 4, 6)
+//3 cases: cont==0 && contBuf==0 -> use rsp; cont!=0 -> append; cont==0 && contBuf!=0 -> append and use contBuf
+int SDPHandler::parseRsp(const unsigned char*rsp, int len) {
+ //unsigned tid = rsp[2] + ((unsigned)rsp[1]<<8);
+ unsigned parlen = rsp[4] + ((unsigned)rsp[3]<<8);
+ //printf("ParseRsp: tid=%04X, parlen=%d ", tid, parlen);
+ unsigned cont = 0;
+ switch (rsp[0]) {
+ case 1: {//errorRsp
+ unsigned errorcode = rsp[6] + ((unsigned)rsp[5]<<8);
+ if (parlen > 2) {
+ printf("ErrorInfo (%d bytes) for error %d is available\n", parlen-2, errorcode);
+ }
+ if (ErrorResponse)
+ ErrorResponse(errorcode);
+ return errorcode;
+ }
+ //break;
+ case 3: { //servicesearchRsp
+ unsigned total = rsp[6] + ((unsigned)rsp[5]<<8);
+ unsigned current = rsp[8] + ((unsigned)rsp[7]<<8);
+ cont = rsp[9+4*current];
+ memcpy(contState, &rsp[9+4*current], cont+1);//copy the continuation state
+ //printf("total=%d, current=%d, cont=%d\n", total, current, cont);
+ if (cont) {
+ //no special handling here, just append the servicerecordhandles
+ }
+ //linear list of 32bit service-handles
+ for (int i = 0; i < current; i++) {
+ unsigned result = 0;
+ for (int j = 0; j< 4; j++)
+ result = (result<<8) + rsp[9 + 4*i + j];
+ printf("SDP Search handle %08X\n", result);
+ services.insert(pair<unsigned, serv_rec*>(result, 0));
+ }
+ if (ServiceSearchResponse)
+ ServiceSearchResponse();
+ }
+ break;
+ case 5: { //serviceattributeRsp
+ unsigned count = rsp[6] + ((unsigned)rsp[5]<<8);//bytes in this part of the attribute list
+// append(rsp+7, count);
+ cont = rsp[7+count];
+ memcpy(contState, &rsp[7+count], cont+1);//copy the continuation state
+ if (cont) {
+ append(rsp+7, count);
+ break;
+ }
+ //printf("count=%d parsing...\n", byteCount);
+ serv_rec *serv = new serv_rec;
+ if (contBuf) {
+ append(rsp+7, count);
+ parse(contBuf, byteCount, tree, serv);
+ } else
+ parse(rsp+7, count, tree, serv);
+ //printf("...parsing done, ");
+ //get the AttributeID, make sure attribId 0 is always included in the request
+ unsigned key = (*serv)[0]->asUnsigned();//AttributeID '0' always refers to the serviceID
+ //printf("Key=%#X\n", key); //key will be 0 when not requested
+ services[key] = serv; //Add the attribute list to the services
+ freeBuf();
+ if (ServiceAttributeResponse)
+ ServiceAttributeResponse(serv);
+ }
+ break;
+ //below is UNTESTED
+ case 7: { //servicesearchattributeRsp
+ unsigned count = rsp[6] + ((unsigned)rsp[5]<<8);
+ append(rsp+7, count);
+ cont = rsp[7+count];
+ memcpy(contState, &rsp[7+count], cont+1);
+ if (cont)
+ break;
+ unsigned pos = 0;
+ if (contBuf[pos]>>3 != sdp_data::SEQUENCE) {
+ printf("Expected a sequence of attribute lists\n");
+ break;
+ }
+ unsigned len = length(contBuf, pos);//get the length of the list of lists and advance pos to the first list
+ while (pos<len) {
+ printf("pos=%d, count=%d, parsing...\n", pos, len);
+ serv_rec *serv = new serv_rec;
+ pos = parse(contBuf+pos, len, tree, serv);
+ unsigned key = (*serv)[0]->asUnsigned();
+ services[key] = serv;
+ }
+ freeBuf();
+ printf("...parsing done, pos=%d\n", pos);
+ if (ServiceSearchAttributeResponse)
+ ServiceSearchAttributeResponse();
+ }
+ break;
+ default:
+ printf("Unknown SDP response type %02X\n", rsp[0]);
+ break;
+ }
+ return 0;
+}
+
+//************************ SERVER related *******************************************//
+
+//unsigned SDPHandler::parseUUID(const u8* data, int len, unsigned &p) {
+unsigned parseUUID(const u8* data, int len, unsigned &p) {
+ unsigned u = 0;
+ if ((data[p]>>3) != sdp_data::UUID) {
+ printf(" UUID expected, got %d ", data[p]>>3);
+ return (unsigned)-1;
+ }
+ switch (data[p++] & 7) {
+ case 1:
+ u = getval(data+p, 2);
+ p +=2;
+ break;
+ case 2:
+ u = getval(data+p, 4);
+ p += 4;
+ break;
+ case 4:
+ u = getval(data+p, 4);
+ p += 16;
+ break;
+ default:
+ printf(" UUID must be 2, 4 or 16 bytes, got type %d\n", data[p-1]);
+ }
+ return u;
+}
+
+#define SVC_HANDLE 0x0001001DU //serial service
+void SDPManager::buildServer() {
+ static sdp_data rfcomm(sdp_data::SEQUENCE);
+ static sdp_data l2cap(sdp_data::SEQUENCE);
+ static sdp_data protocol(sdp_data::SEQUENCE);
+ static sdp_data serviceclass(sdp_data::SEQUENCE);
+ static sdp_data browsegroup(sdp_data::SEQUENCE);
+ static sdp_data root(sdp_data::UUID, BROWSEROOT);
+ static sdp_data l2capuuid(sdp_data::UUID, 0x0100);
+ static sdp_data rfcommuuid(sdp_data::UUID, 0x003);
+ static sdp_data serial(sdp_data::UUID, 0x1101);
+ static sdp_data chan(1U,1);
+ static sdp_data handle(SVC_HANDLE,4);
+ static sdp_data serviceID(0U, 2);
+// static sdp_data name("MBED BlueUSB RFCOMM Serial");
+ static sdp_data name("Serial Port");
+ rfcomm.add_element(&rfcommuuid);
+ rfcomm.add_element(&chan);
+ l2cap.add_element(&l2capuuid);
+ protocol.add_element(&l2cap);
+ protocol.add_element(&rfcomm);
+ serviceclass.add_element(&serial);
+ browsegroup.add_element(&root);
+ static serv_rec attr_list;
+ attr_list[0] = &handle;
+ attr_list[1] = &serviceclass;
+ attr_list[4] = &protocol;
+ attr_list[5] = &browsegroup;
+ attr_list[0x100] = &name;
+ server[SVC_HANDLE] = &attr_list;//server is static and this statement crashes the program when invoked from the constructor which is also invoked statically
+}
+
+int SDPManager::findUUID(unsigned h, unsigned uuid) {
+ serv_rec *rec = server[h];
+ for ( serv_rec::iterator it = rec->begin(); it != rec->end(); it++) {
+ if (it->second->findUUID(uuid))
+ return it->first;
+ }
+ printf("rejected %08X because of %04Xx\n", h, uuid);
+ return -1;
+}
+
+void SDPManager::match(bool elig[], unsigned uuid) {
+ map<unsigned, serv_rec*>::iterator idx;
+ int i = 0;
+ for (idx = server.begin(); idx != server.end(); idx++, i++)
+ if (findUUID(idx->first, uuid) < 0)
+ elig[i] = false;
+}
+
+bool SDPManager::isInList(unsigned short id, const unsigned char* list, int end) {
+ int len;
+ for (unsigned pos = 0; pos < end; pos += len) {
+ len = length(list, pos);
+ switch (len) {
+ case 2: //single value
+ if (getval(list+pos, 2) == id)
+ return true;
+ break;
+ case 4: //range
+ if (getval(list+pos, 2) > id) break;
+ if (getval(list+pos+2, 2) < id) break;
+ return true;
+ default:
+ printf("Unexpected length %d\n", len);
+ }
+ }
+ return false;
+}
+
+void SDPManager::addToReply(sdp_data *svc, serv_rec *list, const unsigned char* att, int end) {
+ unsigned short len, low, high;
+ serv_rec::iterator from, to, idx;
+ if (list==0) {
+ printf("Invalid attribute list (NULL)\n");
+ return;
+ }
+ for (unsigned pos = 0; pos < end; pos += len) {
+ len = length(att, pos);
+ switch (len) {
+ case 2: //single value
+ low = getval(att+pos, 2);
+ svc->add_element(new sdp_data(low, 2));
+ svc->add_element((*list)[low]);
+ printf("Found attrib %d\n", low);
+ break;
+ case 4: //range
+ low = getval(att+pos, 2);
+ high = getval(att+pos+2, 2);
+ from = list->lower_bound(low);
+ to = list->upper_bound(high);
+ for (idx = from; idx != to; idx++) {
+ svc->add_element(new sdp_data(idx->first, 2));
+ svc->add_element(idx->second);
+ printf("Found attrib %d\n", idx->first);
+ }
+ break;
+ default:
+ printf("Unexpected length %d\n", len);
+ }
+ }
+}
+
+//for continuations, just generate the entire list, truncate to desired length and append position of the remainder as continuation
+//on the next iteration, generate the list again, use continuation to find remainder and reiterate
+void SDPManager::SDPServer(int socket, SocketState state, const u8* data, int len, void* userData) {
+ unsigned tid = data[2] + ((unsigned)data[1]<<8);
+ unsigned parlen = data[4] + ((unsigned)data[3]<<8);
+ //printf("ParseReq: PDU_ID=%d, tid=%04X, parlen=%d ", data[0], tid, parlen);
+ unsigned pos = 5;
+ switch (data[0]) {
+ case 1: {//errorRsp
+ unsigned errorcode = data[6] + ((unsigned)data[5]<<8);
+ if (parlen > 2) {
+ printf("ErrorInfo (%d bytes) for error %d is available\n", parlen-2, errorcode);
+ }
+ errorhandler(errorcode);
+ }
+ break;
+ case 2: { //servicesearchReq
+ printf("servicesearchReq almost implemented\n");
+ unsigned pat[12];//the received search pattern
+ int pn;//number of uuids in the pattern
+ if (data[pos]>>3 != sdp_data::SEQUENCE) {//the uuids are wrapped in a sequence
+ printf("Expected a sequence of UUIDs\n");
+ break;
+ }
+ unsigned end = pos + length(data, pos);//get the length of the list of lists and advance pos to the first list
+ bool *eligible = new bool[server.size()];//marks for the services identified by the search pattern
+ for (int i = 0; i < server.size(); i++) eligible[i] = true;
+ for (pn = 0; pn < 12 && pos < end; pn++) {
+ pat[pn] = parseUUID(data,end, pos);//store uuid from the sequence in the pattern
+ match(eligible, pat[pn]);//unmark a service when it does not contain the uuid
+ //printf("pos=%d, count=%d, uuid=%#X\n", pos, len, pat[pn]);
+ }
+
+ unsigned count = getval(data+pos, 2); //maximum length of attribute list to return
+ pos += 2;
+ unsigned tail = data[pos];
+ if (tail) {
+ tail = getval(data+pos+1, tail);
+ printf("requested continuation tailpos=%u\n", tail);
+ } else {
+ //printf("No continuation requested\n");
+ }
+ unsigned *handles = new unsigned[server.size()];
+ int total = 0, current = 0;
+ map<unsigned, serv_rec*>::iterator idx;
+ int i = 0;
+ for (idx = server.begin(); idx != server.end() && total < count; idx++, i++)
+ if (eligible[i])
+ handles[total++] = idx->first;
+ ServiceSearchReply(tid, handles, total, tail);
+ delete[] handles;
+ delete[] eligible;
+ }
+ break;
+ case 4: { //serviceattributeReq
+ printf("serviceattributeReq almost implemented\n");
+ sdp_data reply(sdp_data::SEQUENCE);//the attributelist of the reply
+ unsigned svcid = getval(data+pos, 4);
+ pos += 4;
+ unsigned count = getval(data+pos, 2); //maximum length of attribute list to return
+ pos += 2;
+ int len = length(data, pos); //get the length of the attributeID list
+ int cont = pos + len;
+ printf("svcid=%08X, count=%u, len=%u, pos=%u\n", svcid, count, len, pos);
+ addToReply(&reply, server[svcid], data+pos, len);
+ unsigned tail = data[cont];
+ printf("tail=%u, reply size=%d\n", tail, reply.Size());
+ if (tail) {
+ tail = getval(data+cont+1, tail);
+ printf("requested continuation tailpos=%u, size=%u\n", tail, reply.Size());
+ } else {
+ //printf("No continuation requested\n");
+ }
+
+ ServiceAttributeReply(tid, &reply, count, tail);
+ for (int k = 0; k < reply.items(); k++) {
+ if ((k & 1) == 0) //even hence ID
+ delete reply.item(k); //destroy the ID
+ reply.remove(k); //set all items to nil to prevent destruction of the DB
+ }
+ }
+ break;
+ case 6: { //servicesearchattributeReq
+ sdp_data reply(sdp_data::SEQUENCE);//the attributelist of the reply
+
+ unsigned pat[12];//the received search pattern
+ int pn;//number of uuids in the pattern
+ if (data[pos]>>3 != sdp_data::SEQUENCE) {//the uuids are wrapped in a sequence
+ printf("Expected a sequence of UUIDs\n");
+ break;
+ }
+ unsigned end = pos + length(data, pos);//get the length of the list of lists and advance pos to the first list
+ bool *eligible = new bool[server.size()];//marks for the services identified by the search pattern
+ for (int i = 0; i < server.size(); i++) eligible[i] = true;
+ for (pn = 0; pn < 12 && pos < end; pn++) {
+ pat[pn] = parseUUID(data,end, pos);//store uuid from the sequence in the pattern
+ match(eligible, pat[pn]);//unmark a service when it does not contain the uuid
+ //printf("pos=%d, count=%d, uuid=%#X\n", pos, len, pat[pn]);
+ }
+
+ unsigned count = getval(data+pos, 2); //maximum length of attribute list to return
+ pos += 2;
+
+ int len = length(data, pos); //get the length of the attributeID list
+ int cont = pos + len;
+ //printf("Count = %d pos=%d, data[pos]=%02x, len=%d, cont=%d\n", count, pos, data[pos], len, cont);
+ int i = 0;
+ for (map<unsigned, serv_rec*>::iterator idx = server.begin(); idx != server.end(); idx++, i++) {//foreach service
+ //printf("testing service handle %08X\n", idx->first);
+ if (!eligible[i]) continue; //skip services that don't match the pattern
+ sdp_data *svc = new sdp_data(sdp_data::SEQUENCE); //create a sequence for the attributes of the present service
+#if 0
+ for (serv_rec::iterator attr = idx->second->begin(); attr != idx->second->end(); attr++) {//foreach attrib in the service
+ //printf("testing attribID %u\n", attr->first);
+ if (isInList(attr->first, data+pos, len)) {//check if it is requested
+ printf("Found attribID %d\n", attr->first);
+ sdp_data *p = attr->second; //the attribute
+ svc->add_element(new sdp_data(attr->first, 2)); //add the attribute ID as an unsigned short
+ svc->add_element(p); //add the attribute
+ //printf("appending %d bytes\n", p->Size());
+ }
+ }
+#else
+//alternatively use map::lower/upper_bound and equal_range, needs only one pass over the range list for each service
+ addToReply(svc, idx->second, data+pos, len);
+#endif
+ reply.add_element(svc); //append the new attribute list to the reply list
+ }
+
+ unsigned tail = data[cont];
+ if (tail) {
+ tail = getval(data+cont+1, tail);
+ printf("requested continuation tailpos=%u, size=%u\n", tail, reply.Size());
+ } else {
+ //printf("No continuation requested\n");
+ }
+ ServiceSearchAttributeReply(tid, &reply, count, tail);
+
+ for (int j = 0; j < reply.items(); j++) {
+ sdp_data *al = reply.item(j);
+ for (int k = 0; k < al->items(); k++) {
+ if ((k & 1) == 0) //even hence ID
+ delete al->item(k); //destroy the ID
+ al->remove(k); //set all items to nil to prevent destruction of the DB
+ }
+ delete al; //destroy the list;
+ reply.remove(j);
+ }
+ delete[] eligible;
+ }
+ break;
+ default:
+ printf("Unknown SDP request type %02X\n", data[0]);
+ break;
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/sdp/sdp.h Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,186 @@
+#ifndef SDP_H
+#define SDP_H
+#include "AvailableMemory.h"
+#include "sdp_data.h"
+#include <map>
+#define OFFSET 8
+
+class SDPManager;
+extern SDPManager SDP;
+typedef map<unsigned short, sdp_data*> serv_rec;
+
+void attribHandler(serv_rec *r);
+unsigned parseUUID(const u8* data, int len, unsigned &p);
+unsigned length(const unsigned char *el, unsigned &p);
+unsigned getval(const unsigned char *p, int n) ;
+void errorhandler(unsigned err);//default error handler
+
+
+class SDPHandler: public SocketHandler {
+// int _l2cap;
+ int sdp_socket;
+ unsigned char l2cap_buf[100+OFFSET];
+ unsigned char* buf;
+ unsigned txid;
+ unsigned char contState[17];//maximum size, in practive it is 3
+ unsigned char *contBuf;
+ unsigned byteCount;
+ int _state;
+ sdp_data *tree;//root of the entire service tree
+ map<unsigned, serv_rec*> services;//the set of supported services <handle, service>
+ map<unsigned, serv_rec*>::iterator index;
+//server properties
+// static map<unsigned, serv_rec*> server;
+// static int serverSock;
+public:
+ SDPHandler();
+ void Clear();
+ virtual int Open(SocketInternal* sock, SocketAddrHdr* addr);
+// virtual int Accept(SocketInternal *sock, int scid, int rxid); //called indirectly from BTDevice::Control
+ virtual int Send(SocketInternal* sock, const u8* data, int len);
+ virtual int Close(SocketInternal* sock);
+ virtual char* Name() {
+ return "SDPHandler SocketHandler";
+ }
+ void OnSdpRsp(const u8* data, int len);
+ static void OnSdpRsp(int socket, SocketState state, const u8* data, int len, void* userData);
+
+ //this function is called when the SDP sockets receives data (see HCICallback in TestShell),
+ //currently does not happen because not forwarded from OnSdpRsp, can be used to handle multiple connections
+ static void OnSockCallback(int socket, SocketState state, const u8* data, int len, void* userData) ;
+ //The SDP server is stateless hence can be static
+// static void SDPServer(int socket, SocketState state, const u8* data, int len, void* userData) ;
+
+ void (*ErrorResponse)(unsigned) ;
+ void (*ServiceSearchResponse)() ;
+ void (*ServiceAttributeResponse)(serv_rec*) ;
+ void (*ServiceSearchAttributeResponse)() ;
+ int ServiceSearchRequest(sdp_data *sp, unsigned count, unsigned cs=0);
+ int ServiceAttributeRequest(unsigned handle, unsigned count, sdp_data* al, unsigned cs=0) ;
+ int ServiceSearchAttributeRequest(sdp_data *sp, unsigned count, sdp_data* al, unsigned cs=0);
+//server
+// static int ServiceSearchAttributeReply(unsigned rxid, sdp_data* al, unsigned count, unsigned cs=0);
+private:
+// static unsigned length(const unsigned char *el, unsigned &p);
+// static unsigned getval(const unsigned char *p, int n) ;
+// static unsigned parseUUID(const u8* data, int len, unsigned &p);
+ unsigned parse (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) ;
+ unsigned parseLight (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) ;
+ int parseRsp(const unsigned char*rsp, int len) ;
+ void append(const unsigned char*rsp, int len) ;
+ void freeBuf();
+};
+/*
+class SDPClient: public SDPHandler {
+};
+
+class SDPServer: public SDPHandler {
+};
+*/
+class SDPManager: public SocketHandler {
+ map<int, SDPHandler*> handlers;
+//server properties
+// SDPHandler *Server;
+ static map<unsigned, serv_rec*> server;
+ static int serverSock;
+ bool once;
+public:
+ SDPManager() {
+ once = true;
+ }
+ virtual int Open(SocketInternal* sock, SocketAddrHdr* addr) {
+ printf("SDPManager::Open(sock (ID=%d, type=%d), addr): should not be called\n", sock->ID, sock->Type);
+ return sock->ID;//((SDPHandler*)sock->userData)->Open(sock, addr);
+ }
+ int Open(SocketAddrHdr* addr) {
+ L2CAPAddr* ad = (L2CAPAddr*)addr;
+ ad->psm = L2CAP_PSM_SDP;//open the l2cap channel
+ SDPHandler *h = new SDPHandler;
+ int s = Socket_Open(SOCKET_L2CAP, addr, &SDPHandler::OnSdpRsp, h);
+ handlers[s] = h;
+ return s;
+ }
+ virtual int Accept(SocketInternal *sock, int scid, int rxid) { //called indirectly from BTDevice::Control
+ if (once) {
+ once = false;
+ buildServer();//build the DB on the first connection
+ }
+ //sock is registered as an SDP sock but we use it as an L2CAP sock
+ //type=SDP
+ //userData = BTDevice
+ //Internal = L2CAPSocket
+ BTDevice *l2cap = (BTDevice*)sock->userData;
+ //sock->dcid = scid
+ //sock->scid = something based on sock->ID
+ serverSock = sock->ID;
+ printf("Invoking accept on %p (%s) for sock %d and scid=%d\n", l2cap, l2cap->Name(), sock->ID, scid);
+ return l2cap->Accept(sock, scid, rxid);
+ }
+ virtual int Send(SocketInternal* sock, const u8* data, int len) {//called by the server
+ BTDevice *l2cap = (BTDevice*)sock->userData;
+ return l2cap->Send(sock, data, len);
+ }
+ virtual int Close(SocketInternal* sock) {
+ printf("SDPManager::Close() closing socket %d\n", sock->ID);
+ SDPHandler *h = handlers[sock->ID];
+ int retval = h->Close(sock);
+ delete h;
+ handlers[sock->ID] = 0;
+ return retval;
+ }
+ void Destroy(int s) {
+ printf("Deleting handler for socket %d\n", s);
+ delete handlers[s];
+ handlers[s] = 0;
+ }
+ virtual char* Name() {
+ return "SDPManager SocketHandler";
+ }
+ //void OnSdpRsp(const u8* data, int len);
+ static void OnSdpRsp(int socket, SocketState state, const u8* data, int len, void* userData) {
+ printf("SDPManager::OnSdpRsp(socket %d, state %d, len %d)\n", socket, state, len);
+ }
+ //The SDP server is (almost) stateless hence can be static
+ static void SDPServer(int socket, SocketState state, const u8* data, int len, void* userData) ;
+ static void match(bool elig[], unsigned uuid);
+ static bool isInList(unsigned short id, const unsigned char* list, int end);
+ static void addToReply(sdp_data *svc, serv_rec *list, const unsigned char* att, int end);
+ static int findUUID(unsigned h, unsigned uuid);
+ void buildServer();
+ static int ServiceSearchReply(unsigned rxid, unsigned *handles, unsigned count, unsigned cs=0);
+ static int ServiceAttributeReply(unsigned rxid, sdp_data* al, unsigned count, unsigned cs=0);
+ static int ServiceSearchAttributeReply(unsigned rxid, sdp_data* al, unsigned count, unsigned cs=0);
+ /*
+ //this function is called when the SDP sockets receives data (see HCICallback in TestShell),
+ //currently does not happen because not forwarded from OnSdpRsp, can be used to handle multiple connections
+ static void OnSockCallback(int socket, SocketState state, const u8* data, int len, void* userData) ;
+
+ static void errorhandler(unsigned err);
+
+ void (*ErrorResponse)(unsigned) ;
+ void (*ServiceSearchResponse)() ;
+ void (*ServiceAttributeResponse)(serv_rec*) ;
+ void (*ServiceSearchAttributeResponse)() ;
+ int ServiceSearchRequest(sdp_data *sp, unsigned count, unsigned cs=0);
+ int ServiceAttributeRequest(unsigned handle, unsigned count, sdp_data* al, unsigned cs=0) ;
+ int ServiceSearchAttributeRequest(sdp_data *sp, unsigned count, sdp_data* al, unsigned cs=0);
+ //server
+ private:
+ static unsigned length(const unsigned char *el, unsigned &p);
+ static unsigned getval(const unsigned char *p, int n) ;
+ static unsigned parseUUID(const u8* data, int len, unsigned &p);
+ static void addAttrib(unsigned h, unsigned short id, sdp_data *attrib);
+ static void addIndex(unsigned h, unsigned uuid);
+ static int findUUID(unsigned h, unsigned uuid);
+ static void match(bool elig[], unsigned uuid);
+ static bool isInList(unsigned short id, const unsigned char* list, int end);
+ void buildServer();
+ unsigned parse (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) ;
+ unsigned parseLight (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) ;
+ int parseRsp(const unsigned char*rsp, int len) ;
+ void append(const unsigned char*rsp, int len) ;
+ void freeBuf();
+ */
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/sdp/sdp_data.cpp Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,229 @@
+#include "mbed.h"
+#include "sdp_data.h"
+#include "Utils.h"
+
+char sdp_data::ret[12];
+
+unsigned sdp_data::asUnsigned() {
+ switch (type) {
+ case NULL_:
+ return 0;
+ case UNSIGNED:
+ case SIGNED:
+ case BOOL:
+ return data;
+ case UUID:
+#ifdef LONGUUID
+ return uuid[6] + uuid[7]<<16;
+#else
+ return data;
+#endif
+ default:
+ return 0;
+ }
+}
+
+const char* sdp_data::asString(bool alt) {
+ char sep = ',';
+ switch (type) {
+ case NULL_:
+ return "NULL";
+ case UNSIGNED:
+ if (alt) sprintf(ret, "0x%0*X", size*2, data);
+ else sprintf(ret, "%u", data);
+ return ret;
+ case SIGNED:
+ sprintf(ret, "%d", data);
+ return ret;
+ case BOOL:
+ return data ? "TRUE" : "FALSE";
+ case STRING:
+ case URL:
+ return str;
+ case ALTERNATIVE:
+ sep = '|';
+ case SEQUENCE: {
+ if (longstr) delete[] longstr;
+ int n = sprintf(ret, "SEQ %d { ", size) + 1;
+ longstr = new char[n];
+ strcpy(longstr, ret);
+ for (int i = 0; i < sequence.size(); i++) {
+ const char *s = sequence[i]->asString(alt);
+ n = strlen(longstr) + strlen(s) + 2;
+ char *t = new char[n];
+ strcpy(t, longstr);
+ strcat(t, s);
+ t[n-2] = sep;
+ t[n-1]='\0';
+ //printf("[%s]+[%s]+%c=[%s]\n", longstr, s, sep, t);
+ delete[] longstr;
+ longstr = t;
+ }
+ longstr[n-2] = '}';
+ }
+ return longstr;
+ case UUID:
+#ifdef LONGUUID
+ switch (size) {
+ case 2:
+ sprintf(ret, "0x%04X", uuid[6]);
+ return ret;
+ case 4:
+ sprintf(ret, "0x%04X%04X", uuid[7],uuid[6]);
+ return ret;
+ case 16:
+ longstr = new char[35];
+ sprintf(longstr, "%04X%04X-%04X-%04X-%04X-%04X%04X%04X", uuid[7],uuid[6],uuid[5],uuid[4],uuid[3],uuid[2],uuid[1],uuid[0]);
+ return longstr;
+ }
+#else
+ switch (size) {
+ case 2:
+ sprintf(ret, "0x%04X", data & 0xffff);
+ return ret;
+ case 4:
+ sprintf(ret, "0x%08X", data);
+ return ret;
+ case 16:
+ longstr = new char[35];
+ sprintf(longstr, "%08X-%04X-%04X-%04X-%04X%04X%04X", data,base_uuid[5],base_uuid[4],base_uuid[3],base_uuid[2],base_uuid[1],base_uuid[0]);
+ return longstr;
+ }
+#endif
+ }
+ return "Unsupported";
+}
+
+unsigned sdp_data::Size() {
+ if (size==0 && type==SEQUENCE)
+ return 2;
+ if (size<3 || size==4 || size==8 || size==16)
+ return size+1;//include descriptor
+ if (size < 256) return size+2; //1 extra byte
+ if (size < 65536) return size+3; //2 extra bytes
+ return size+5; //4 extra bytes
+}
+
+unsigned sdp_data::sizedesc(unsigned char *buf) {
+ int desc, extra=0;
+ switch (size) {
+ case 0:
+ /* should be:
+ if (type != NULL_) {
+ desc = 5;
+ extra = 1;
+ buf[1] = 0;
+ }
+ */
+ case 1:
+ desc = 0;
+ break;
+ case 2:
+ desc = 1;
+ break;
+ case 4:
+ desc = 2;
+ break;
+ case 8:
+ desc = 3;
+ break;
+ case 16:
+ desc = 4;
+ break;
+ default:
+ if (size < 256) {
+ desc = 5;
+ extra = 1;
+ buf[1] = size;
+ } else if (size < 65536) {
+ desc = 6;
+ extra = 2;
+ *(unsigned short*)&buf[1] = size;
+ } else {
+ desc = 7;
+ extra = 4;
+ *(unsigned*)&buf[1] = size;
+ }
+ }
+ buf[0] |= desc;
+ return extra+1;
+}
+
+void sdp_data::revcpy(unsigned char*d, const unsigned char*s, int n) {
+ for (int i = 0; i < n; i++)
+ d[i] = s[n-i-1];
+}
+
+unsigned sdp_data::build(unsigned char *buf, unsigned max) {//max is ignored
+ int p = 0;
+ buf[p] = type<<3;
+ switch (type) {
+ case NULL_:
+ p++;
+ break;
+ case UNSIGNED:
+ case SIGNED:
+ case BOOL:
+ p += sizedesc(buf+p);
+ revcpy(buf+p, (unsigned char*)&data, size);
+ break;
+ case UUID:
+ p += sizedesc(buf+p);
+#ifdef LONGUUID
+ switch (size) {
+ case 2:
+ case 4:
+ revcpy(buf+p, (unsigned char*)&uuid[6], size);
+ break;
+ case 16:
+ revcpy(buf+p, (unsigned char*)uuid, size);
+ break;
+ }
+#else
+ switch (size) {
+ case 2:
+ case 4:
+ revcpy(buf+p, (unsigned char*)&data, size);
+ break;
+ case 16:
+ revcpy(buf+p, (unsigned char*)&data, 4);
+ revcpy(buf+p+4, base_uuid, 12);
+ break;
+ }
+#endif
+ break;
+ case STRING:
+ case URL:
+ p += sizedesc(buf+p);
+ memcpy(buf+p, str, size);
+ break;
+ case SEQUENCE:
+ case ALTERNATIVE: {
+ if (sequence.size()==0) {//hack: should be solved in sizedesc
+ buf[p++] |= 5;
+ buf[p++] = 0;
+ break;
+ }
+ int n = 0;
+ p += sizedesc(buf+p);
+ for (int i = 0; i < sequence.size(); i++)
+ n += sequence.at(i)->build(buf+p+n, max-p);
+ }
+ break;
+ }
+ p += size;
+// printfBytes("Build:", buf, p);
+ return p;
+}
+
+bool sdp_data::findUUID(unsigned uuid) {
+ if (type == UUID)
+ return asUnsigned()==uuid;
+ if (type==SEQUENCE || type==ALTERNATIVE) {
+ for (int i = 0; i < sequence.size(); i++) {
+ if (sequence[i]->findUUID(uuid))
+ return true;
+ }
+ }
+ return false;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/sdp/sdp_data.h Fri Jul 01 09:16:00 2011 +0000
@@ -0,0 +1,116 @@
+#ifndef SDP_DATA_H
+#define SDP_DATA_H
+
+#include <vector>
+
+extern const unsigned char base_uuid[16];// = { 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0, 0x07, 0x70, 0, 0x10, 0, 0};
+
+class sdp_data {
+public:
+ enum elements { NULL_, UNSIGNED, SIGNED, UUID, STRING, BOOL, SEQUENCE, ALTERNATIVE, URL};
+private:
+ enum elements type;
+ char size;
+ union {
+ unsigned data;
+ char *str;
+#ifdef LONGUUID
+ unsigned short uuid[8];
+#endif
+ };
+ vector<sdp_data*> sequence; //not allowed to be in union
+ static char ret[12];
+ char *longstr;
+public:
+ sdp_data(): type(NULL_), size(0), longstr(0) {
+ //printf("NULL%d ", size);
+ }
+ sdp_data(unsigned d, unsigned sz=4): type(UNSIGNED), size(sz), longstr(0) {
+ data=d;
+ //printf("UINT%d=%u ", size, data);
+ }
+ sdp_data(unsigned short d, unsigned sz=2): type(UNSIGNED), size(sz), longstr(0) {
+ data=d;
+ //printf("UINT%d=%u ", size, data);
+ }
+ sdp_data(signed d, unsigned sz=4): type(SIGNED), size(sz), longstr(0) {
+ data=d;
+ //printf("INT%d=%d ", size, data);
+ }
+ sdp_data(bool d, unsigned sz=1): type(BOOL), size(sz), longstr(0) {
+ data=d;
+ //printf("BOOL%d=%u ", size, data);
+ }
+ sdp_data(char*s, unsigned sz=0): type(STRING), longstr(0) {
+ if (sz) size = sz+1;
+ else size = strlen(s)+1;
+ str = new char[size];
+ strncpy(str, s, size);
+ str[size-1] = '\0';
+ //printf("STR%d='%s' ", size, str);
+ }
+ sdp_data(enum elements t, unsigned d, unsigned sz=2): type(t), size(sz), longstr(0) {
+ if (t==UUID) {
+#ifdef LONGUUID
+ memcpy(uuid, base_uuid, 16);
+ uuid[6] = d;
+ uuid[7] = d>>16;
+ // printf("UUID%d=%04X%04X ", size, uuid[7], uuid[6]);
+#else
+ data = d;
+#endif
+ } else printf("Please use other constructor for type %d\n", t);
+ }
+ sdp_data(enum elements t, char *d=0, unsigned sz=0): type(t), size(sz), longstr(0) {
+ switch (t) {
+#ifdef LONGUUID
+ case UUID:
+ memcpy(uuid, d, size);
+ // printf("UUID%d=%08X ", size, uuid[6]);
+ break;
+#endif
+ case URL:
+ //size = strlen(d)+1;
+ str = new char[size+1];
+ strcpy(str, d);
+ // printf("URL%d='%u' ", size, str);
+ break;
+ case SEQUENCE:
+ case ALTERNATIVE:
+ break;
+ default:
+ printf("Please use other constructor for type %d\n", t);
+ }
+ }
+ ~sdp_data() {
+ switch (type) {
+ case STRING:
+ case URL:
+ delete[] str;
+ break;
+ case SEQUENCE:
+ case ALTERNATIVE:
+ for (int i = 0; i < sequence.size(); i++)
+ delete sequence.at(i);
+ break;
+ }
+ if (longstr)
+ delete[] longstr;
+ }
+ void add_element(sdp_data *el) {
+ sequence.push_back(el);
+ size += el->Size();
+ }
+ unsigned asUnsigned() ;
+ const char* asString(bool alt=false) ;
+ unsigned Size() ;
+ unsigned items() { return sequence.size();}
+ sdp_data* item(int i) { return sequence[i];}
+ void remove(int i) { sequence[i] = 0;}
+ unsigned sizedesc(unsigned char *buf) ;
+ void revcpy(unsigned char*d, const unsigned char*s, int n) ;
+ unsigned build(unsigned char *buf, unsigned max) ;
+ bool findUUID(unsigned uuid);
+};
+
+#endif
\ No newline at end of file