SDP client for myBlueUSB
Dependents: mbed_TANK_Kinect ftusbClass
Diff: sdp.cpp
- Revision:
- 0:7493bf6bb1b9
- Child:
- 1:70ee392bcfd4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/sdp.cpp Mon Apr 04 16:45:20 2011 +0000 @@ -0,0 +1,329 @@ +#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}; + +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()); + } +} + +//this function is called when the L2CAP layer receives SDP packets (see SDPManager::Open), userdata is the sdpmanager instance +void SDPManager::OnSdpRsp(int socket, SocketState state, const u8* data, int len, void* userData) { +printf("\x1B[%dm", 34); + printf("OnSdpRsp(socket=%d, state=%d, len=%d)\n", socket, state, len); + //printfBytes("Got SDP Response from L2CAP: ", data, len); + SDPManager *self = (SDPManager*)userData; + if (state != SocketState_Open) + return; + sdp_data list(sdp_data::SEQUENCE); + if (data) + self->parseRsp(data, len); + if (len==0) { + sdp_data root(sdp_data::UUID, 0x1101); + sdp_data req(sdp_data::SEQUENCE); + req.add_element(&root); + self->ServiceSearchRequest(&req, 5); + } else if (data[0]==3) { + self->index = self->services.begin(); + if (self->index != self->services.end()) { + unsigned handle = (*self->index).first; + //printf("req.: handle %#X\n", handle); + sdp_data all(0xffffU,4); + list.add_element(&all); + self->ServiceAttributeRequest(handle, 100, &list);//0x1001D + self->index++; + } else printf(" - empty list - \n");//should not happen + } else if (data[0]==5) { + if (self->index != self->services.end()) { + //printf("req.: handle %#X\n", (*self->index).first); + self->ServiceAttributeRequest((*self->index).first, 100, &list); + self->index++; + } else { + printf(" - end of list - \n"); + Socket_Close(self->sdp_socket); //Note: socket=L2CAP, sdp_socket=SDP !!! + } + } +printf("\x1B[%dm", 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 +void SDPManager::OnSockCallback(int socket, SocketState state, const u8* data, int len, void* userData) { + printf("OnSockCallback(socket=%d, state=%d, len=%d)\n", socket, state, len); + printfBytes("Got SDP Response from socket: ", data, len); +} + +void SDPManager::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 SDPManager::ServiceSearchRequest(sdp_data *sp, unsigned count, unsigned cs) { + int parlen = sp->Size() + 3; //no continuation + 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; + if (cs==0) + buf[p+7] = 0; + else + printf("Continuation not supported\n"); + //printfBytes("SDP Send: ", buf, parlen+5); + return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET); +} + +int SDPManager::ServiceAttributeRequest(unsigned handle, unsigned count, sdp_data* al, unsigned cs) { + int parlen = al->Size() + 7; //no continuation + 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); + if (cs==0) + buf[p+11] = 0; + else + printf("Continuation not supported\n"); + //printfBytes("SDP Send: ", buf, parlen+5); + return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET); +} + +int SDPManager::ServiceSearchAttributeRequest(sdp_data *sp, unsigned count, sdp_data* al, unsigned cs) { + int parlen = sp->Size() + al->Size() + 3; //no continuation (1 byte), count (2 bytes) + 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); + if (cs==0) + buf[p+7] = 0; + else + printf("Continuation not supported\n"); + //printfBytes("SDP Send: ", buf, parlen+5); + return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET); +} + +unsigned SDPManager::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 SDPManager::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!\n"); while(1); } + + +unsigned SDPManager::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; + while (p < end) { + sdp_data *elem = 0; + p += parse(el + p, len-p, elem, dummy);//parse each element in the sequence, the second arg is as yet unused + item->add_element(elem); + if (record) { + if (n & 1) { //value + record->insert(pair<unsigned short, sdp_data*>(key, elem)); + } else //key + key = elem->asUnsigned(); + n++; + } + } + } +// 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; +} + +int SDPManager::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]; +// printf("total=%d, current=%d, cont=%d\n", total, current, cont); + if (cont) break; + //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 + if (tree) delete tree; + unsigned count = rsp[6] + ((unsigned)rsp[5]<<8); + cont = rsp[7+count]; + if (cont) + break; +// printf("pos=%d, count=%d parsing...\n", 7, count); + serv_rec *serv = new serv_rec; + unsigned p = parse(rsp+7, count, tree, serv); + unsigned key = (*serv)[0]->asUnsigned(); + services[key] = serv; +// printf("...parsing done, pos=%d, cont=%d\n", p, cont); + if (ServiceAttributeResponse) + ServiceAttributeResponse(serv); + } + break; + case 7: { //servicesearchattributeRsp + if (tree) delete tree; + unsigned count = rsp[6] + ((unsigned)rsp[5]<<8); + cont = rsp[7+count]; + if (cont) + break; + unsigned pos = 7; + while (rsp[pos]>>3 == sdp_data::SEQUENCE) { + unsigned len = length(rsp, pos); + printf("pos=%d, count=%d, parsing...\n", pos, len); + serv_rec *serv = new serv_rec; + pos = parse(rsp+pos, len, tree, serv); + unsigned key = (*serv)[0]->asUnsigned(); + services[key] = serv; + } + printf("...parsing done, pos=%d\n", pos); + if (ServiceSearchAttributeResponse) + ServiceSearchAttributeResponse(); + } + break; + default: + printf("Unknown SDP response type %02X\n", rsp[0]); + break; + } + if (cont) + printf("Continuation not supported (yet)\n"); + return 0; +}