local fix version of myBlueUSB (http://mbed.org/users/networker/code/myBlueUSB/). - merge deleted files which are required to compile. - enable echo back of received data via RFCOMM.
Dependencies: AvailableMemory FatFileSystem mbed myUSBHost
sdp/sdp.cpp
- Committer:
- nobukuma
- Date:
- 2013-12-08
- Revision:
- 2:9f25a7fa1a54
- Parent:
- 0:003889bc474f
File content as of revision 2:9f25a7fa1a54:
#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; } }