sdp.cpp

00001 #include "mbed.h"
00002 #include "Utils.h"
00003 #include "hci.h"
00004 #include "sdp_data.h"
00005 #include "sdp.h"
00006 
00007 SDPManager SDP; //instance
00008 const unsigned char base_uuid[16] = { 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0, 0x07, 0x70, 0, 0x10, 0, 0};
00009 map<unsigned, serv_rec*> SDPManager::server;
00010 int SDPManager::serverSock = 0;
00011 
00012 void attribHandler(serv_rec *r) {
00013     printf("Service 0x%08X\n", (*r)[0x0000]->asUnsigned());
00014     map<unsigned short, sdp_data*>::iterator it = r->begin();
00015     for (;it != r->end();it++) {
00016         printf("   0x%04X: %s\n", (*it).first, (*it).second->asString());
00017     }
00018 }
00019 
00020 #define BROWSEGROUP 0x1001
00021 #define BROWSEROOT  0x1002
00022 #define SERIALSERV  0x1101
00023 
00024 SDPHandler::SDPHandler(): txid(1), tree(0) {
00025     ErrorResponse=errorhandler;
00026     ServiceSearchResponse=0;
00027     ServiceAttributeResponse=attribHandler;
00028     ServiceSearchAttributeResponse=0;
00029     buf = l2cap_buf+OFFSET;
00030     contBuf = 0;
00031     byteCount = 0;
00032     contState[0] = 0;
00033     _state = 0;
00034 }
00035 
00036 void SDPHandler::Clear() {
00037     for (index = services.begin(); index != services.end(); index++) {//for all services
00038         for (serv_rec::iterator it = index->second->begin(); it != index->second->end(); it++)//for all attributes
00039             delete it->second; //delete the attribute value tree
00040         delete (*index).second; //delete the attribute list
00041     }
00042     services.clear();//and empty the services list
00043 }
00044 
00045 //Called as: Socket_Open(SOCKET_SDP, addr, callback, userdata(this)) from SDPManager
00046 //never called
00047 int SDPHandler::Open(SocketInternal* sock, SocketAddrHdr* addr) {
00048     printf("Successfully opened SDP socket %d\n", sock->ID);
00049     sock->SetState(SocketState_Open);
00050     sdp_socket = sock->ID;
00051     return sdp_socket;
00052 }
00053 
00054 int SDPHandler::Send(SocketInternal* sock, const u8* data, int len) {
00055     printf("SDPHandler::Send should not be called directly\n");
00056 //        return Socket_Send(_l2cap, data, len);
00057     BTDevice *l2cap = (BTDevice*)sock->userData;
00058     return l2cap->Send(sock, data, len);
00059 }
00060 
00061 int SDPHandler::Close(SocketInternal* sock) {
00062     printf("SDPHandler::Close(%d)\n", sock->ID);
00063     Clear();
00064 //    printf("SDP socket %d and L2CAP socket %d closed, freemem=%d\n", sock->ID, _l2cap, AvailableMemory(1));
00065     int retval = 0;//Socket_Close(_l2cap);//could also keep it open for the next connection
00066     return retval;
00067 }
00068 
00069 //this function is called when the L2CAP layer receives SDP packets (see SDPHandler::Open), userdata is the sdpmanager instance
00070 void SDPHandler::OnSdpRsp(int socket, SocketState state, const u8* data, int len, void* userData) {
00071     printf("\x1B[%dm", 35);
00072 //        printf("OnSdpRsp(socket=%d, state=%d, len=%d)\n", socket, state, len);
00073     printf("SDPHandler::OnSdpRsp(socket=%d, state=%d, len=%d, freemem=%d\n", socket, state, len, AvailableMemory(1));
00074     SDPHandler *self = (SDPHandler*)userData;
00075     if (state == SocketState_Open) {
00076         self->sdp_socket = socket;
00077         self->OnSdpRsp(data, len);
00078     } else if (state == SocketState_Closed) {
00079         SDP.Destroy(socket);
00080     }
00081     printf("\x1B[%dm", 0);
00082 }
00083 
00084 //this function is called when the L2CAP layer receives SDP packets (see SDPHandler::Open), userdata is the sdpmanager instance
00085 //void SDPHandler::OnSdpRsp(int socket, SocketState state, const u8* data, int len, void* userData) {
00086 void SDPHandler::OnSdpRsp(const u8* data, int len) {
00087     static sdp_data list(sdp_data::SEQUENCE);
00088     static sdp_data all(0x0000ffffU,4);
00089     static sdp_data serviceID(0U, 2);
00090     static sdp_data name(0x100U, 2);
00091     static sdp_data root(sdp_data::UUID, BROWSEROOT);
00092     static sdp_data req(sdp_data::SEQUENCE);
00093     static bool once = true;
00094 //    printf("_state=%d first=%d   ", _state, once);
00095     if (once) {
00096         list.add_element(&all);
00097         //list.add_element(&serviceID);
00098         //list.add_element(&name);
00099         req.add_element(&root);
00100         once = false;
00101     }
00102     if (data) {
00103         parseRsp(data, len);
00104     }
00105     switch (_state) {
00106         case 0: //closed
00107             if (len==0) { //socket just opened
00108                 //'Open' cleared the services list
00109                 ServiceSearchRequest(&req, 10);
00110                 _state = 1; //wait for service handles
00111             }
00112             break;
00113         case 1: //service handles arriving
00114             if (contState[0]) {//continuation, repeat request
00115                 ServiceSearchRequest(&req, 5);
00116             } else {
00117                 if (data[0]==3) {
00118                     index = services.begin();
00119                     if (index != services.end()) {
00120                         unsigned handle = (*index).first;
00121                         //printf("req.: handle %#X\n", handle);
00122                         ServiceAttributeRequest(handle, 100, &list);//0x1001D
00123                     } else
00124                         printf(" - empty list - \n");//should not happen
00125                     _state = 2; //wait for attribute response
00126                 } else
00127                     printf("Expected a ServiceSearchResponse 0x03, got %#x\n", data[0]);
00128             }
00129             break;
00130         case 2:
00131             if (contState[0])//repeat request
00132                 ServiceAttributeRequest((*index).first, 100, &list);
00133             else {
00134                 if (data[0]==5) {
00135                     index++; //move to next service
00136                     if (index != services.end()) {
00137                         //printf("req.: handle %#X\n", (*index).first);
00138                         ServiceAttributeRequest((*index).first, 100, &list);
00139                     } else {
00140                         printf(" - end of list - \n");
00141                         Socket_Close(sdp_socket); //Note: socket=L2CAP, sdp_socket=SDP !!!
00142                         _state = 0;
00143                     }
00144                 } else
00145                     printf("Expected a ServiceAttributeResponse 0x05, got %#x\n", data[0]);
00146             }
00147             break;
00148     }
00149 }
00150 
00151 //this function is called when the SDP sockets receives data (see HCICallback in TestShell),
00152 //currently does not happen because not forwarded from OnSdpRsp, can be used to handle multiple connections
00153 void SDPHandler::OnSockCallback(int socket, SocketState state, const u8* data, int len, void* userData) {
00154     printf("SDPHandler::OnSockCallback(socket=%d, state=%d, len=%d)\n", socket, state, len);
00155     printfBytes("Got SDP Response from socket: ", data, len);
00156 }
00157 
00158 //void SDPHandler::errorhandler(unsigned err) {//default error handler
00159 void errorhandler(unsigned err) {//default error handler
00160     switch (err) {
00161         case 1:
00162             printf("Unsupported version of SDP\n");
00163             break;
00164         case 2:
00165             printf("Invalid SDP ServiceRecordHandle\n");
00166             break;
00167         case 3:
00168             printf("SDP syntax error\n");
00169             break;
00170         case 4:
00171             printf("PDU size was invalid\n");
00172             break;
00173         case 5:
00174             printf("Continuation state was invalid\n");
00175             break;
00176         case 6:
00177             printf("SDP server has insufficient resources\n");
00178             break;
00179         default:
00180             printf("Unknown SDP error code\n");
00181             break;
00182     }
00183 }
00184 
00185 int SDPHandler::ServiceSearchRequest(sdp_data *sp, unsigned count, unsigned cs) {
00186     int parlen = sp->Size() + contState[0] + 3;
00187     buf[0] = 2; //pdu
00188     buf[1] = txid>>8;
00189     buf[2] = txid++;
00190     buf[4] = parlen;
00191     buf[3] = parlen>>8;
00192     int p = sp->build(buf+5, 100-10);
00193     buf[p+6] = count;
00194     buf[p+5] = count>>8;
00195     buf[p+7] = contState[0];
00196     for (int j = 1; j <= contState[0]; j++)
00197         buf[p+j+7] = contState[j];
00198     //printfBytes("SDP Send: ", buf, parlen+5);
00199     return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET);
00200 }
00201 
00202 int SDPHandler::ServiceAttributeRequest(unsigned handle, unsigned count, sdp_data* al, unsigned cs) {
00203     int parlen = al->Size() + contState[0] + 7;
00204     buf[0] = 4; //pdu
00205     buf[1] = txid>>8;
00206     buf[2] = txid++;
00207     buf[4] = parlen;
00208     buf[3] = parlen>>8;
00209     for (int i = 0; i < 4; i++)
00210         buf[i+5] = ((char*)&handle)[3-i];
00211     buf[9] = count>>8;
00212     buf[10] = count;
00213     int p = al->build(buf+11, 100-26);
00214     buf[p+11] = contState[0];
00215     for (int j = 1; j <= contState[0]; j++)
00216         buf[p+j+11] = contState[j];
00217     //printfBytes("SDP Send: ", buf, parlen+5);
00218     return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET);
00219 }
00220 
00221 int SDPHandler::ServiceSearchAttributeRequest(sdp_data *sp, unsigned count, sdp_data* al, unsigned cs) {
00222     int parlen = sp->Size() + al->Size() + contState[0] + 3; // count (2 bytes) + at least 1 cont
00223     buf[0] = 6; //pdu
00224     buf[1] = txid>>8;
00225     buf[2] = txid++;
00226     buf[4] = parlen;
00227     buf[3] = parlen>>8;
00228     int p = sp->build(buf+5, 30);
00229     buf[p+6] = count;
00230     buf[p+5] = count>>8;
00231     p += al->build(buf+11, 100-38);
00232     buf[p+7] = contState[0];
00233     for (int j = 1; j <= contState[0]; j++)
00234         buf[p+j+7] = contState[j];
00235     //printfBytes("SDP Send: ", buf, parlen+5);
00236     return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET);
00237 }
00238 
00239 unsigned BE32(unsigned le) {
00240     unsigned be=0;
00241     for (int i = 0; i < 32; i+=8){
00242       be |= ((le>>i)&0xFFU)<<(24-i);
00243     }   
00244     return be;
00245 }
00246 
00247 int SDPManager::ServiceSearchReply(unsigned rxid, unsigned *handles, unsigned count, unsigned cs) {//'count' is number of matching handles and capped at the maximumservicerecordcount
00248     unsigned size = count*4;
00249     unsigned cont = 0;//outgoing continuation
00250     unsigned char *_buf = new unsigned char[OFFSET+5+4+size+3];
00251     unsigned char *buf = _buf+OFFSET;
00252     unsigned current = count - cs; //remainder of data to send
00253     unsigned parlen = 4 + 4*current + 1 ;// attributelistbytecount+payload+no_continuation
00254     //never need a continuation unless other criterion like PDU size is used
00255     if (current > count) {//still too large, need continuation
00256         current = count; //limit at maximum
00257         cont = cs + count; //start for next iteration
00258         parlen = 4 + 4*current + 3; //adjusted for payload and continuation
00259         printf("Need continuation, sending handles [%d, %d> of attributeList\n", cs, cont);
00260     } else {
00261         //   printf("Final or only block, sending bytes [%d, %d> of attributeList\n", cs, cs+byteCount);
00262     }
00263     buf[0] = 3; //pdu
00264     buf[1] = rxid>>8;
00265     buf[2] = rxid;
00266     buf[3] = parlen>>8;
00267     buf[4] = parlen;
00268     buf[5] = count>>8;
00269     buf[6] = count;
00270     buf[7] = current>>8;
00271     buf[8] = current;
00272     unsigned *p = (unsigned*)&buf[9];
00273     for (int i = 0; i < current; i++)
00274         p[i] = BE32(handles[i]);
00275     //printf("'build' added %d bytes to the buffer\n", p);
00276     if (cs == 0) { //this is not a continuation
00277         buf[5+parlen-1] = 0;
00278     } else { //this is a continuation
00279         memcpy(buf+9, buf+9+cs*4, current*4);//move part of interrest to beginning of buffer
00280         buf[5+parlen-3] = 2;
00281         buf[5+parlen-2] = cont>>8;
00282         buf[5+parlen-1] = cont;
00283     }
00284     //printfBytes("SDP Send: ", buf, parlen+5);
00285     int retval = Socket_Send(serverSock, _buf, parlen + 5 + OFFSET);
00286     delete[] _buf;
00287     return retval;
00288 }
00289 
00290 int SDPManager::ServiceAttributeReply(unsigned rxid, sdp_data* al, unsigned count, unsigned cs) {
00291     unsigned size = al->Size();
00292     unsigned cont = 0;//outgoing continuation
00293     unsigned char *_buf = new unsigned char[OFFSET+5+2+size+3];
00294     unsigned char *buf = _buf+OFFSET;
00295     unsigned byteCount = size - cs; //remainder of data to send
00296     unsigned parlen = 2 + byteCount + 1 ;// attributelistbytecount+payload+no_continuation
00297     if (byteCount > count) {//still too large, need continuation
00298         byteCount = count; //limit at maximum
00299         cont = cs + count; //start for next iteration
00300         parlen = 2 + byteCount + 3; //adjusted for payload and continuation
00301         printf("Need continuation, sending bytes [%d, %d> of attributeList\n", cs, cont);
00302     } else {
00303         //   printf("Final or only block, sending bytes [%d, %d> of attributeList\n", cs, cs+byteCount);
00304     }
00305     buf[0] = 5; //pdu
00306     buf[1] = rxid>>8;
00307     buf[2] = rxid;
00308     buf[3] = parlen>>8;
00309     buf[4] = parlen;
00310     buf[5] = byteCount>>8;
00311     buf[6] = byteCount;
00312     int p = al->build(buf+7, size);//limited only by buffersize
00313     //printf("'build' added %d bytes to the buffer\n", p);
00314     if (cs == 0) { //this is not a continuation
00315         buf[byteCount+7] = 0;
00316     } else { //this is a continuation
00317         memcpy(buf+7, buf+7+cs, byteCount);//move part of interrest to beginning of buffer
00318         buf[byteCount+7] = 2;
00319         buf[byteCount+8] = cont>>8;
00320         buf[byteCount+9] = cont;
00321     }
00322     //printfBytes("SDP Send: ", buf, parlen+5);
00323     int retval = Socket_Send(serverSock, _buf, parlen + 5 + OFFSET);
00324     delete[] _buf;
00325     return retval;
00326 }
00327 
00328 int SDPManager::ServiceSearchAttributeReply(unsigned rxid, sdp_data* al, unsigned count, unsigned cs) {
00329     unsigned size = al->Size();
00330     unsigned cont = 0;//outgoing continuation
00331     unsigned char *_buf = new unsigned char[OFFSET+5+2+size+3];
00332     unsigned char *buf = _buf+OFFSET;
00333     unsigned byteCount = size - cs; //remainder of data to send
00334     unsigned parlen = 2 + byteCount + 1 ;// attributelistbytecount+payload+no_continuation
00335     if (byteCount > count) {//still too large, need continuation
00336         byteCount = count; //limit at maximum
00337         cont = cs + count; //start for next iteration
00338         parlen = 2 + byteCount + 3; //adjusted for payload and continuation
00339         printf("Need continuation, sending bytes [%d, %d> of attributeList\n", cs, cont);
00340     } else {
00341         //   printf("Final or only block, sending bytes [%d, %d> of attributeList\n", cs, cs+byteCount);
00342     }
00343     buf[0] = 7; //pdu
00344     buf[1] = rxid>>8;
00345     buf[2] = rxid;
00346     buf[3] = parlen>>8;
00347     buf[4] = parlen;
00348     buf[5] = byteCount>>8;
00349     buf[6] = byteCount;
00350     int p = al->build(buf+7, size);//limited only by buffersize
00351     //printf("'build' added %d bytes to the buffer\n", p);
00352     if (cs == 0) { //this is not a continuation
00353         buf[byteCount+7] = 0;
00354     } else { //this is a continuation
00355         memcpy(buf+7, buf+7+cs, byteCount);//move part of interrest to beginning of buffer
00356         buf[byteCount+7] = 2;
00357         buf[byteCount+8] = cont>>8;
00358         buf[byteCount+9] = cont;
00359     }
00360     //printfBytes("SDP Send: ", buf, parlen+5);
00361     int retval = Socket_Send(serverSock, _buf, parlen + 5 + OFFSET);
00362     delete[] _buf;
00363     return retval;
00364 }
00365 
00366 //unsigned SDPHandler::getval(const unsigned char *p, int n) {
00367 unsigned getval(const unsigned char *p, int n) {
00368     unsigned ret = 0;
00369     for (int i = 0; i < n; i++)
00370         ret = (ret<<8) + (unsigned)p[i];
00371     return ret;
00372 }
00373 
00374 //unsigned SDPHandler::length(const unsigned char *el, unsigned &p) {
00375 unsigned length(const unsigned char *el, unsigned &p) {
00376     unsigned len = 0;
00377     switch (el[p++] & 7) {//length
00378         case 0:
00379             len = 1;
00380             break;
00381         case 1:
00382             len = 2;
00383             break;
00384         case 2:
00385             len = 4;
00386             break;
00387         case 3:
00388             len = 8;
00389             break;
00390         case 4:
00391             len = 16;
00392             break;
00393         case 7://4bytes
00394             len= el[p++]<<24;
00395             len += el[p++]<<16;
00396         case 6://2bytes
00397             len += el[p++]<<8;
00398         case 5://1byte
00399             len += el[p++];
00400             break;
00401     }
00402     return len;
00403 }
00404 
00405 extern "C" void HardFault_Handler() {
00406     printf("Hard Fault! %d bytes left\n", AvailableMemory(1));
00407     while (1);
00408 }
00409 
00410 unsigned SDPHandler::parseLight (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) {
00411     unsigned p = 0;
00412     unsigned len = length(el, p);
00413     int end = p+len;//end is the index of the item just after the sequence
00414     sdp_data *item = 0;
00415     switch (el[0]>>3) {//type
00416         case sdp_data::NULL_:
00417             printf("NULL ");
00418             break;
00419         case sdp_data::UNSIGNED:
00420             printf("UINT%d=%u ", len, (unsigned)getval(el+p, len));
00421             break;
00422         case sdp_data::SIGNED:
00423             printf("INT%d=%d ", len, (unsigned)getval(el+p, len));
00424             break;
00425         case sdp_data::UUID:
00426             if (len==16) {
00427                 printf("UUID16= ");
00428                 for (int i = 0; i < 16; i++)
00429                     printf("%02x ", el[p+i]);
00430             } else
00431                 printf("UUID%d=%u ", len, (unsigned)getval(el+p, len));
00432             break;
00433         case sdp_data::STRING:
00434             printf("STR%d='%s' ", len, (char*)el+p);
00435             break;
00436         case sdp_data::BOOL:
00437             printf("BOOL%d=%d ", len, (unsigned)getval(el+p, len));
00438             break;
00439         case sdp_data::SEQUENCE:
00440             goto skip;
00441         case sdp_data::ALTERNATIVE:
00442 skip: {//p points just after the length indicator, hence at the first item IN the sequence
00443                 printf("SEQ%d{%p ", len, item);
00444                 int n = 0;
00445                 unsigned short key;
00446                 serv_rec *dummy = 0;
00447                 while (p < end) {
00448                     sdp_data *elem = 0;
00449                     p += parseLight(el + p, len-p, elem, dummy);//parse each element in the sequence, the second arg is as yet unused
00450                     if (record) {
00451                         if (n & 1) { //value
00452                             record->insert(pair<unsigned short, sdp_data*>(key, elem));
00453                         } else //key
00454                             key = n;
00455                         n++;
00456                     }
00457                 }
00458             }
00459             printf("}\n");
00460             break;
00461         case 8:
00462             printf("URL%d='%s' ", len, (char*)el+p);
00463             break;
00464         default:
00465             printf("Parse: Unknown type %d, len=%d (code=%#02X)\n", el[0]>>3, len, el[0]);
00466     }
00467     result = item;
00468     return end;
00469 }
00470 
00471 unsigned SDPHandler::parse (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) {
00472     unsigned p = 0;
00473     unsigned len = length(el, p);
00474     int end = p+len;//end is the index of the item just after the sequence
00475     sdp_data *item = 0;
00476     switch (el[0]>>3) {//type
00477         case sdp_data::NULL_:
00478             item = new sdp_data();
00479             break;
00480         case sdp_data::UNSIGNED:
00481             item = new sdp_data((unsigned)getval(el+p, len), len);
00482             break;
00483         case sdp_data::SIGNED:
00484             item = new sdp_data((int)getval(el+p, len), len);
00485             break;
00486         case sdp_data::UUID:
00487             if (len==16) {
00488                 char rev[16];
00489                 for (int i = 0; i < 16; i++)
00490                     rev[i] = el[p+15-i];
00491                 item = new sdp_data(sdp_data::UUID, rev, len);
00492             } else
00493                 item = new sdp_data(sdp_data::UUID, getval(el+p, len), len);
00494             break;
00495         case sdp_data::STRING:
00496             item = new sdp_data((char*)el+p, len);
00497             break;
00498         case sdp_data::BOOL:
00499             item = new sdp_data((bool)getval(el+p, len), len);
00500             break;
00501         case sdp_data::SEQUENCE:
00502             item = new sdp_data(sdp_data::SEQUENCE);
00503             goto skip;
00504         case sdp_data::ALTERNATIVE:
00505             item = new sdp_data(sdp_data::ALTERNATIVE);
00506 skip: {//p points just after the length indicator, hence at the first item IN the sequence
00507                 //printf("SEQ%d{%p ", len, item);
00508                 int n = 0;
00509                 unsigned short key;
00510                 serv_rec *dummy = 0;//means: there is no service record to fill in for deeper levels
00511                 while (p < end) {
00512                     sdp_data *elem = 0; //this becomes the tree with attribute values
00513                     p += parse(el + p, len-p, elem, dummy);//parse each element in the sequence, the second arg is as yet unused
00514                     if (record) { //if at the level of attribute list, add elem to record as key/value pair
00515                         if (n & 1) { //value
00516                             record->insert(pair<unsigned short, sdp_data*>(key, elem));
00517                         } else //key
00518                             key = elem->asUnsigned();
00519                         n++;
00520                     } else //just add the elements to the value tree
00521                         item->add_element(elem);
00522                 }
00523             }
00524             //printf("}\n");
00525             break;
00526         case 8:
00527             item = new sdp_data(sdp_data::URL, (char*)el+p, len);
00528             break;
00529         default:
00530             printf("Parse: Unknown type %d, len=%d (code=%#02X)\n", el[0]>>3, len, el[0]);
00531     }
00532     result = item;
00533     return end;
00534 }
00535 
00536 void SDPHandler::append(const unsigned char *payload, int len) {
00537     unsigned char *tmp = new unsigned char[byteCount + len];//append the payload to the previous continuation buffer
00538     if (contBuf && byteCount) {
00539         memcpy(tmp, contBuf, byteCount); //copy the existing part
00540         delete[] contBuf;//delete the old buffer
00541     }
00542     memcpy(tmp+byteCount, payload, len); //append the new part
00543     contBuf = tmp;
00544     byteCount += len;
00545 }
00546 
00547 void SDPHandler::freeBuf() {
00548     if (contBuf) {
00549         delete[] contBuf;
00550         contBuf = 0;
00551     }
00552     byteCount = 0;
00553 }
00554 
00555 
00556 //TODO: test case 7, add server support (cases 2, 4, 6)
00557 //3 cases: cont==0 && contBuf==0 -> use rsp; cont!=0 -> append; cont==0 && contBuf!=0 -> append and use contBuf
00558 int SDPHandler::parseRsp(const unsigned char*rsp, int len) {
00559     //unsigned tid = rsp[2] + ((unsigned)rsp[1]<<8);
00560     unsigned parlen = rsp[4] + ((unsigned)rsp[3]<<8);
00561     //printf("ParseRsp: tid=%04X, parlen=%d ", tid, parlen);
00562     unsigned cont = 0;
00563     switch (rsp[0]) {
00564         case 1: {//errorRsp
00565             unsigned errorcode = rsp[6] + ((unsigned)rsp[5]<<8);
00566             if (parlen > 2) {
00567                 printf("ErrorInfo (%d bytes) for error %d is available\n", parlen-2, errorcode);
00568             }
00569             if (ErrorResponse)
00570                 ErrorResponse(errorcode);
00571             return errorcode;
00572         }
00573         //break;
00574         case 3: { //servicesearchRsp
00575             unsigned total = rsp[6] + ((unsigned)rsp[5]<<8);
00576             unsigned current = rsp[8] + ((unsigned)rsp[7]<<8);
00577             cont = rsp[9+4*current];
00578             memcpy(contState, &rsp[9+4*current], cont+1);//copy the continuation state
00579             //printf("total=%d, current=%d, cont=%d\n", total, current, cont);
00580             if (cont) {
00581                 //no special handling here, just append the servicerecordhandles
00582             }
00583             //linear list of 32bit service-handles
00584             for (int i = 0; i < current; i++) {
00585                 unsigned result = 0;
00586                 for (int j = 0; j< 4; j++)
00587                     result = (result<<8) + rsp[9 + 4*i + j];
00588                 printf("SDP Search handle %08X\n", result);
00589                 services.insert(pair<unsigned, serv_rec*>(result, 0));
00590             }
00591             if (ServiceSearchResponse)
00592                 ServiceSearchResponse();
00593         }
00594         break;
00595         case 5: { //serviceattributeRsp
00596             unsigned count = rsp[6] + ((unsigned)rsp[5]<<8);//bytes in this part of the attribute list
00597 //            append(rsp+7, count);
00598             cont = rsp[7+count];
00599             memcpy(contState, &rsp[7+count], cont+1);//copy the continuation state
00600             if (cont) {
00601                 append(rsp+7, count);
00602                 break;
00603             }
00604             //printf("count=%d parsing...\n", byteCount);
00605             serv_rec *serv = new serv_rec;
00606             if (contBuf) {
00607                 append(rsp+7, count);
00608                 parse(contBuf, byteCount, tree, serv);
00609             } else
00610                 parse(rsp+7, count, tree, serv);
00611             //printf("...parsing done,  ");
00612             //get the AttributeID, make sure attribId 0 is always included in the request
00613             unsigned key = (*serv)[0]->asUnsigned();//AttributeID '0' always refers to the serviceID
00614             //printf("Key=%#X\n", key); //key will be 0 when not requested
00615             services[key] = serv; //Add the attribute list to the services
00616             freeBuf();
00617             if (ServiceAttributeResponse)
00618                 ServiceAttributeResponse(serv);
00619         }
00620         break;
00621         //below is UNTESTED
00622         case 7: { //servicesearchattributeRsp
00623             unsigned count = rsp[6] + ((unsigned)rsp[5]<<8);
00624             append(rsp+7, count);
00625             cont = rsp[7+count];
00626             memcpy(contState, &rsp[7+count], cont+1);
00627             if (cont)
00628                 break;
00629             unsigned pos = 0;
00630             if (contBuf[pos]>>3 != sdp_data::SEQUENCE) {
00631                 printf("Expected a sequence of attribute lists\n");
00632                 break;
00633             }
00634             unsigned len = length(contBuf, pos);//get the length of the list of lists and advance pos to the first list
00635             while (pos<len) {
00636                 printf("pos=%d, count=%d, parsing...\n", pos, len);
00637                 serv_rec *serv = new serv_rec;
00638                 pos = parse(contBuf+pos, len, tree, serv);
00639                 unsigned key = (*serv)[0]->asUnsigned();
00640                 services[key] = serv;
00641             }
00642             freeBuf();
00643             printf("...parsing done, pos=%d\n", pos);
00644             if (ServiceSearchAttributeResponse)
00645                 ServiceSearchAttributeResponse();
00646         }
00647         break;
00648         default:
00649             printf("Unknown SDP response type %02X\n", rsp[0]);
00650             break;
00651     }
00652     return 0;
00653 }
00654 
00655 //************************ SERVER related *******************************************//
00656 
00657 //unsigned SDPHandler::parseUUID(const u8* data, int len, unsigned &p) {
00658 unsigned parseUUID(const u8* data, int len, unsigned &p) {
00659     unsigned u = 0;
00660     if ((data[p]>>3) != sdp_data::UUID) {
00661         printf(" UUID expected, got %d ", data[p]>>3);
00662         return (unsigned)-1;
00663     }
00664     switch (data[p++] & 7) {
00665         case 1:
00666             u = getval(data+p, 2);
00667             p +=2;
00668             break;
00669         case 2:
00670             u = getval(data+p, 4);
00671             p += 4;
00672             break;
00673         case 4:
00674             u = getval(data+p, 4);
00675             p += 16;
00676             break;
00677         default:
00678             printf(" UUID must be 2, 4 or 16 bytes, got type %d\n", data[p-1]);
00679     }
00680     return u;
00681 }
00682 
00683 #define SVC_HANDLE  0x0001001DU  //serial service
00684 void SDPManager::buildServer() {
00685     static sdp_data rfcomm(sdp_data::SEQUENCE);
00686     static sdp_data l2cap(sdp_data::SEQUENCE);
00687     static sdp_data protocol(sdp_data::SEQUENCE);
00688     static sdp_data serviceclass(sdp_data::SEQUENCE);
00689     static sdp_data browsegroup(sdp_data::SEQUENCE);
00690     static sdp_data root(sdp_data::UUID, BROWSEROOT);
00691     static sdp_data l2capuuid(sdp_data::UUID, 0x0100);
00692     static sdp_data rfcommuuid(sdp_data::UUID, 0x003);
00693     static sdp_data serial(sdp_data::UUID, 0x1101);
00694     static sdp_data chan(1U,1);
00695     static sdp_data handle(SVC_HANDLE,4);
00696     static sdp_data serviceID(0U, 2);
00697 //    static sdp_data name("MBED BlueUSB RFCOMM Serial");
00698     static sdp_data name("Serial Port");
00699     rfcomm.add_element(&rfcommuuid);
00700     rfcomm.add_element(&chan);
00701     l2cap.add_element(&l2capuuid);
00702     protocol.add_element(&l2cap);
00703     protocol.add_element(&rfcomm);
00704     serviceclass.add_element(&serial);
00705     browsegroup.add_element(&root);
00706     static serv_rec attr_list;
00707     attr_list[0] = &handle;
00708     attr_list[1] = &serviceclass;
00709     attr_list[4] = &protocol;
00710     attr_list[5] = &browsegroup;
00711     attr_list[0x100] = &name;
00712     server[SVC_HANDLE] = &attr_list;//server is static and this statement crashes the program when invoked from the constructor which is also invoked statically
00713 }
00714 
00715 int SDPManager::findUUID(unsigned h, unsigned uuid) {
00716     serv_rec *rec = server[h];
00717     for ( serv_rec::iterator it = rec->begin(); it != rec->end(); it++) {
00718         if (it->second->findUUID(uuid))
00719             return it->first;
00720     }
00721     printf("rejected %08X because of %04Xx\n", h, uuid);
00722     return -1;
00723 }
00724 
00725 void SDPManager::match(bool elig[], unsigned uuid) {
00726     map<unsigned, serv_rec*>::iterator idx;
00727     int i = 0;
00728     for (idx = server.begin(); idx != server.end(); idx++, i++)
00729         if (findUUID(idx->first, uuid) < 0)
00730             elig[i] = false;
00731 }
00732 
00733 bool SDPManager::isInList(unsigned short id, const unsigned char* list, int end) {
00734     int len;
00735     for (unsigned pos = 0; pos < end; pos += len) {
00736         len = length(list, pos);
00737         switch (len) {
00738             case 2: //single value
00739                 if (getval(list+pos, 2) == id)
00740                     return true;
00741                 break;
00742             case 4: //range
00743                 if (getval(list+pos, 2) > id) break;
00744                 if (getval(list+pos+2, 2) < id) break;
00745                 return true;
00746             default:
00747                 printf("Unexpected length %d\n", len);
00748         }
00749     }
00750     return false;
00751 }
00752 
00753 void SDPManager::addToReply(sdp_data *svc, serv_rec *list, const unsigned char* att, int end) {
00754     unsigned short len, low, high;
00755     serv_rec::iterator from, to, idx;
00756     if (list==0) {
00757         printf("Invalid attribute list (NULL)\n");
00758         return;
00759     }
00760     for (unsigned pos = 0; pos < end; pos += len) {
00761         len = length(att, pos);
00762         switch (len) {
00763             case 2: //single value
00764                 low = getval(att+pos, 2);
00765                 svc->add_element(new sdp_data(low, 2));
00766                 svc->add_element((*list)[low]);
00767                 printf("Found attrib %d\n", low);
00768                 break;
00769             case 4: //range
00770                 low = getval(att+pos, 2);
00771                 high = getval(att+pos+2, 2);
00772                 from = list->lower_bound(low);
00773                 to = list->upper_bound(high);
00774                 for (idx = from; idx != to; idx++) {
00775                     svc->add_element(new sdp_data(idx->first, 2));
00776                     svc->add_element(idx->second);
00777                     printf("Found attrib %d\n", idx->first);
00778                 }
00779                 break;
00780             default:
00781                 printf("Unexpected length %d\n", len);
00782         }
00783     }
00784 }
00785 
00786 //for continuations, just generate the entire list, truncate to desired length and append position of the remainder as continuation
00787 //on the next iteration, generate the list again, use continuation to find remainder and reiterate
00788 void SDPManager::SDPServer(int socket, SocketState state, const u8* data, int len, void* userData) {
00789     unsigned tid = data[2] + ((unsigned)data[1]<<8);
00790     unsigned parlen = data[4] + ((unsigned)data[3]<<8);
00791     //printf("ParseReq: PDU_ID=%d, tid=%04X, parlen=%d ", data[0], tid, parlen);
00792     unsigned pos = 5;
00793     switch (data[0]) {
00794         case 1: {//errorRsp
00795             unsigned errorcode = data[6] + ((unsigned)data[5]<<8);
00796             if (parlen > 2) {
00797                 printf("ErrorInfo (%d bytes) for error %d is available\n", parlen-2, errorcode);
00798             }
00799             errorhandler(errorcode);
00800         }
00801         break;
00802         case 2: { //servicesearchReq
00803             printf("servicesearchReq almost implemented\n");
00804             unsigned pat[12];//the received search pattern
00805             int pn;//number of uuids in the pattern
00806             if (data[pos]>>3 != sdp_data::SEQUENCE) {//the uuids are wrapped in a sequence
00807                 printf("Expected a sequence of UUIDs\n");
00808                 break;
00809             }
00810             unsigned end = pos + length(data, pos);//get the length of the list of lists and advance pos to the first list
00811             bool *eligible = new bool[server.size()];//marks for the services identified by the search pattern
00812             for (int i = 0; i < server.size(); i++) eligible[i] = true;
00813             for (pn = 0; pn < 12 && pos < end; pn++) {
00814                 pat[pn] = parseUUID(data,end, pos);//store uuid from the sequence in the pattern
00815                 match(eligible, pat[pn]);//unmark a service when it does not contain the uuid
00816                 //printf("pos=%d, count=%d, uuid=%#X\n", pos, len, pat[pn]);
00817             }
00818 
00819             unsigned count = getval(data+pos, 2); //maximum length of attribute list to return
00820             pos += 2;
00821             unsigned tail = data[pos];
00822             if (tail) {
00823                 tail = getval(data+pos+1, tail);
00824                 printf("requested continuation tailpos=%u\n", tail);
00825             } else {
00826                 //printf("No continuation requested\n");
00827             }
00828             unsigned *handles = new unsigned[server.size()];
00829             int total = 0, current = 0;
00830             map<unsigned, serv_rec*>::iterator idx;
00831             int i = 0;
00832             for (idx = server.begin(); idx != server.end() && total < count; idx++, i++)
00833                 if (eligible[i])
00834                     handles[total++] = idx->first;
00835             ServiceSearchReply(tid, handles, total, tail);
00836             delete[] handles;
00837             delete[] eligible;
00838         }
00839         break;
00840         case 4: { //serviceattributeReq
00841             printf("serviceattributeReq almost implemented\n");
00842             sdp_data reply(sdp_data::SEQUENCE);//the attributelist of the reply
00843             unsigned svcid = getval(data+pos, 4);
00844             pos += 4;
00845             unsigned count = getval(data+pos, 2); //maximum length of attribute list to return
00846             pos += 2;
00847             int len = length(data, pos); //get the length of the attributeID list
00848             int cont = pos + len;
00849             printf("svcid=%08X, count=%u, len=%u, pos=%u\n", svcid, count, len, pos);
00850             addToReply(&reply, server[svcid], data+pos, len);
00851             unsigned tail = data[cont];
00852             printf("tail=%u, reply size=%d\n", tail, reply.Size());
00853             if (tail) {
00854                 tail = getval(data+cont+1, tail);
00855                 printf("requested continuation tailpos=%u, size=%u\n", tail, reply.Size());
00856             } else {
00857                 //printf("No continuation requested\n");
00858             }
00859 
00860             ServiceAttributeReply(tid, &reply, count, tail);
00861             for (int k = 0; k < reply.items(); k++) {
00862                 if ((k & 1) == 0) //even hence ID
00863                     delete reply.item(k); //destroy the ID
00864                 reply.remove(k);  //set all items to nil to prevent destruction of the DB
00865             }
00866         }
00867         break;
00868         case 6: { //servicesearchattributeReq
00869             sdp_data reply(sdp_data::SEQUENCE);//the attributelist of the reply
00870 
00871             unsigned pat[12];//the received search pattern
00872             int pn;//number of uuids in the pattern
00873             if (data[pos]>>3 != sdp_data::SEQUENCE) {//the uuids are wrapped in a sequence
00874                 printf("Expected a sequence of UUIDs\n");
00875                 break;
00876             }
00877             unsigned end = pos + length(data, pos);//get the length of the list of lists and advance pos to the first list
00878             bool *eligible = new bool[server.size()];//marks for the services identified by the search pattern
00879             for (int i = 0; i < server.size(); i++) eligible[i] = true;
00880             for (pn = 0; pn < 12 && pos < end; pn++) {
00881                 pat[pn] = parseUUID(data,end, pos);//store uuid from the sequence in the pattern
00882                 match(eligible, pat[pn]);//unmark a service when it does not contain the uuid
00883                 //printf("pos=%d, count=%d, uuid=%#X\n", pos, len, pat[pn]);
00884             }
00885 
00886             unsigned count = getval(data+pos, 2); //maximum length of attribute list to return
00887             pos += 2;
00888 
00889             int len = length(data, pos); //get the length of the attributeID list
00890             int cont = pos + len;
00891             //printf("Count = %d pos=%d, data[pos]=%02x, len=%d, cont=%d\n", count, pos, data[pos], len, cont);
00892             int i = 0;
00893             for (map<unsigned, serv_rec*>::iterator idx = server.begin(); idx != server.end(); idx++, i++) {//foreach service
00894                 //printf("testing service handle %08X\n", idx->first);
00895                 if (!eligible[i]) continue; //skip services that don't match the pattern
00896                 sdp_data *svc = new sdp_data(sdp_data::SEQUENCE); //create a sequence for the attributes of the present service
00897 #if 0
00898                 for (serv_rec::iterator attr = idx->second->begin(); attr != idx->second->end(); attr++) {//foreach attrib in the service
00899                     //printf("testing attribID %u\n", attr->first);
00900                     if (isInList(attr->first, data+pos, len)) {//check if it is requested
00901                         printf("Found attribID %d\n", attr->first);
00902                         sdp_data *p = attr->second; //the attribute
00903                         svc->add_element(new sdp_data(attr->first, 2)); //add the attribute ID as an unsigned short
00904                         svc->add_element(p);                            //add the attribute
00905                         //printf("appending %d bytes\n", p->Size());
00906                     }
00907                 }
00908 #else
00909 //alternatively use map::lower/upper_bound and equal_range, needs only one pass over the range list for each service
00910                 addToReply(svc, idx->second, data+pos, len);
00911 #endif
00912                 reply.add_element(svc); //append the new attribute list to the reply list
00913             }
00914 
00915             unsigned tail = data[cont];
00916             if (tail) {
00917                 tail = getval(data+cont+1, tail);
00918                 printf("requested continuation tailpos=%u, size=%u\n", tail, reply.Size());
00919             } else {
00920                 //printf("No continuation requested\n");
00921             }
00922             ServiceSearchAttributeReply(tid, &reply, count, tail);
00923 
00924             for (int j = 0; j < reply.items(); j++) {
00925                 sdp_data *al = reply.item(j);
00926                 for (int k = 0; k < al->items(); k++) {
00927                     if ((k & 1) == 0) //even hence ID
00928                         delete al->item(k); //destroy the ID
00929                     al->remove(k);  //set all items to nil to prevent destruction of the DB
00930                 }
00931                 delete al; //destroy the list;
00932                 reply.remove(j);
00933             }
00934             delete[] eligible;
00935         }
00936         break;
00937         default:
00938             printf("Unknown SDP request type %02X\n", data[0]);
00939             break;
00940     }
00941 }