Sami Alshorman
Pulse Oximeter (NONIN) communicates with mbed via Bluetooth dongle and sends Heart Rate and Oxygen Saturation via GPRS module
Dependencies: C12832 GPS GSM mbed
Fork of
myBlueUSB_localfix
by 
Nobuaki Aoki
Diff: sdp/sdp.cpp
- Revision:
- 0:003889bc474f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/sdp/sdp.cpp Sat Dec 07 14:19:00 2013 +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;
+ }
+}