SDP client for myBlueUSB
Dependents: mbed_TANK_Kinect ftusbClass
Diff: sdp.cpp
- Revision:
- 3:e8d2ebb7392e
- Parent:
- 2:d5a27b2d2e08
- Child:
- 4:d5c3e499603d
--- a/sdp.cpp Thu Apr 07 21:01:39 2011 +0000 +++ b/sdp.cpp Wed May 04 09:27:32 2011 +0000 @@ -6,6 +6,8 @@ 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()); @@ -18,27 +20,87 @@ #define BROWSEROOT 0x1002 #define SERIALSERV 0x1101 -//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) { -void SDPManager::OnSdpRsp(const u8* data, int len) { - static sdp_data list(sdp_data::SEQUENCE); +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); +// 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; + list.add_element(&all); + //list.add_element(&serviceID); + //list.add_element(&name); + req.add_element(&root); + once = false; } - if (data){ + if (data) { parseRsp(data, len); - } + } switch (_state) { case 0: //closed if (len==0) { //socket just opened @@ -57,10 +119,10 @@ unsigned handle = (*index).first; //printf("req.: handle %#X\n", handle); ServiceAttributeRequest(handle, 100, &list);//0x1001D - } else + } else printf(" - empty list - \n");//should not happen _state = 2; //wait for attribute response - } else + } else printf("Expected a ServiceSearchResponse 0x03, got %#x\n", data[0]); } break; @@ -78,7 +140,7 @@ Socket_Close(sdp_socket); //Note: socket=L2CAP, sdp_socket=SDP !!! _state = 0; } - } else + } else printf("Expected a ServiceAttributeResponse 0x05, got %#x\n", data[0]); } break; @@ -87,12 +149,13 @@ //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); +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 SDPManager::errorhandler(unsigned err) {//default error handler +//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"); @@ -118,7 +181,7 @@ } } -int SDPManager::ServiceSearchRequest(sdp_data *sp, unsigned count, unsigned cs) { +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; @@ -132,10 +195,10 @@ for (int j = 1; j <= contState[0]; j++) buf[p+j+7] = contState[j]; //printfBytes("SDP Send: ", buf, parlen+5); - return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET); + return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET); } -int SDPManager::ServiceAttributeRequest(unsigned handle, unsigned count, sdp_data* al, unsigned cs) { +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; @@ -151,10 +214,10 @@ for (int j = 1; j <= contState[0]; j++) buf[p+j+11] = contState[j]; //printfBytes("SDP Send: ", buf, parlen+5); - return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET); + return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET); } -int SDPManager::ServiceSearchAttributeRequest(sdp_data *sp, unsigned count, sdp_data* al, unsigned cs) { +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; @@ -169,17 +232,57 @@ for (int j = 1; j <= contState[0]; j++) buf[p+j+7] = contState[j]; //printfBytes("SDP Send: ", buf, parlen+5); - return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET); + return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET); } -unsigned SDPManager::getval(const unsigned char *p, int n) { +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 SDPManager::length(const unsigned char *el, unsigned &p) { +//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: @@ -214,7 +317,7 @@ while (1); } -unsigned SDPManager::parseLight (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) { +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 @@ -231,7 +334,6 @@ break; case sdp_data::UUID: if (len==16) { - char rev[16]; printf("UUID16= "); for (int i = 0; i < 16; i++) printf("%02x ", el[p+i]); @@ -248,7 +350,7 @@ 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); + printf("SEQ%d{%p ", len, item); int n = 0; unsigned short key; serv_rec *dummy = 0; @@ -276,7 +378,7 @@ return end; } -unsigned SDPManager::parse (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) { +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 @@ -341,21 +443,21 @@ return end; } -void SDPManager::append(const unsigned char *payload, int len) { +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, 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 SDPManager::freeBuf() { +void SDPHandler::freeBuf() { if (contBuf) { - delete[] contBuf; - contBuf = 0; + delete[] contBuf; + contBuf = 0; } byteCount = 0; } @@ -363,8 +465,8 @@ //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 SDPManager::parseRsp(const unsigned char*rsp, int len) { - unsigned tid = rsp[2] + ((unsigned)rsp[1]<<8); +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; @@ -384,7 +486,7 @@ 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); + //printf("total=%d, current=%d, cont=%d\n", total, current, cont); if (cont) { //no special handling here, just append the servicerecordhandles } @@ -406,16 +508,16 @@ cont = rsp[7+count]; memcpy(contState, &rsp[7+count], cont+1);//copy the continuation state if (cont) { - append(rsp+7, count); - break; + 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); + if (contBuf) { + append(rsp+7, count); + parse(contBuf, byteCount, tree, serv); } else - parse(rsp+7, count, tree, serv); + 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 @@ -455,7 +557,231 @@ break; default: printf("Unknown SDP response type %02X\n", rsp[0]); - break; + 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"); + 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; + 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 + } + break; + case 4: { //serviceattributeReq + } + 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; + } +}