Takashi Ogura
Based on myBlueUSB and rosserial_mbed
Dependencies: mbed myUSBHost AvailableMemory myBlueUSB
Revision 0:7684b95768c7, committed 2011-09-17
- Comitter:
- OTL
- Date:
- Sat Sep 17 14:21:35 2011 +0000
- Child:
- 1:18139954944b
- Commit message:
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/AutoEvents.cpp Sat Sep 17 14:21:35 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 Sat Sep 17 14:21:35 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 Sat Sep 17 14:21:35 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_unsupported/code/fatfilesystem/ \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MbedBT.h Sat Sep 17 14:21:35 2011 +0000
@@ -0,0 +1,67 @@
+/*
+ * MbedBT
+ *
+ * 2011/09/14 by OTL
+ */
+
+#ifndef MBEDBT_H_
+#define MBEDBT_H_
+
+#include"mbed.h"
+#include "btserial.h"
+#include "ftclasslibusbdevbt.h"
+
+extern int state;
+
+void test_tick() {
+ USBLoop();
+ printf("tick\n");
+ }
+
+class MbedBT {
+public:
+ MbedBT(){
+ timer_.start();
+ }
+ void init() {
+ USBInit();
+
+ while(state != 1) {
+ USBLoop();
+ }
+
+ 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);
+ }
+ bluetooth_ = new btserial(1);
+// bluetooth_->baud(57600);
+ }
+
+ int read() {
+ return bluetooth_->getc();
+ }
+ void write(uint8_t* data, int length) {
+ bluetooth_->write(data, length);
+ }
+
+ unsigned long time() {
+ return timer_.read_ms();
+ }
+ ~MbedBT() {
+ delete bluetooth_;
+// ticker_.dettach();
+ }
+protected:
+ btserial* bluetooth_;
+// Ticker ticker_;
+ Timer timer_;
+};
+
+
+#endif /* MBEDBT_H_ */
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/TestShell.cpp Sat Sep 17 14:21:35 2011 +0000
@@ -0,0 +1,329 @@
+
+/*
+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
+ if (devs[i]->_handle == 0) {
+ 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);
+ //incoming->baud(3);
+ }
+ break;
+ case 2://main loop
+ if (n%10000==0) {printf("*");}
+ if (incoming->readable()>0){
+ int c= incoming->getc();
+ printf("!!!!!!!!!!!!!!!!!!!!!!!!GOT%d!!!!!!!!!!!!!!!!!!!!!!!!!!", c);
+ if (c > 0) {
+ printf("sending");
+ //Socket_Send(SOCKET_RFCOM, &data , 1);
+ if ( incoming->putc(c) == -1) {
+ printf("FAIL");
+ }
+ // if (incoming->putc('\n')==-1) {
+ // printf("FAIL2");
+ // }
+ }
+ }
+ 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) {
+ u8 hoge = 1;
+ Socket_Send(SOCKET_RFCOM, &hoge, 1);
+
+ n=0;
+ led = !led;
+ }
+ }
+ //printf("Dropped out of main loop!\n");
+}
+
+//********************************************************************************************************************************
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/btserial.cpp Sat Sep 17 14:21:35 2011 +0000
@@ -0,0 +1,120 @@
+#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;
+}
+
+int btserial::write(u8* data, int len) {
+ Socket_Send(sock, data, len);
+}
+
+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 Sat Sep 17 14:21:35 2011 +0000
@@ -0,0 +1,33 @@
+#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 btserial::write(u8* data, int len);
+ 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 Sat Sep 17 14:21:35 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 Sat Sep 17 14:21:35 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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/m3pi.lib Sat Sep 17 14:21:35 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/chris/code/m3pi/#4b7d6ea9b35b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Sat Sep 17 14:21:35 2011 +0000
@@ -0,0 +1,147 @@
+/*
+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();
+
+#ifdef ROS_MAIN
+#include "ros_mbedbt.h"
+#include <std_msgs/String.h>
+
+ros::NodeHandle nh;
+
+std_msgs::String str_msg;
+ros::Publisher chatter("chatter", &str_msg);
+
+char hello[13] = "hello world!";
+
+int main() {
+
+ pc.baud(9600);
+
+ nh.initNode();
+ nh.advertise(chatter);
+ while (1) {
+ USBLoop();
+ wait_ms(100);
+ str_msg.data = hello;
+ chatter.publish( &str_msg );
+ nh.spinOnce();
+ }
+
+}
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Sep 17 14:21:35 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/myBlueUSB.lib Sat Sep 17 14:21:35 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/networker/code/myBlueUSB/#57f7679dd651
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/myUSBHost.lib Sat Sep 17 14:21:35 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 Sat Sep 17 14:21:35 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 Sat Sep 17 14:21:35 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 Sat Sep 17 14:21:35 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 Sat Sep 17 14:21:35 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/ros_mbedbt.h Sat Sep 17 14:21:35 2011 +0000
@@ -0,0 +1,12 @@
+#ifndef ROS_MBEDBT_H
+#define ROS_MBEDBT_H
+
+#include "ros/ros_impl.h"
+#include "MbedBT.h"
+
+namespace ros
+{
+ typedef NodeHandle_<MbedBT, 25, 25, 350, 350> NodeHandle;
+}
+
+#endif // ROS_MBEDBT_H
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/rosserial_mbed_lib.lib Sat Sep 17 14:21:35 2011 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/nucho/code/rosserial_mbed_lib/#684f39d0c346
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/sdp/sdp.cpp Sat Sep 17 14:21:35 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 Sat Sep 17 14:21:35 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 Sat Sep 17 14:21:35 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 Sat Sep 17 14:21:35 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