Diff: sdp.cpp

Revision:

4:d5c3e499603d

Parent:

3:e8d2ebb7392e

--- a/sdp.cpp	Wed May 04 09:27:32 2011 +0000
+++ b/sdp.cpp	Sat Jun 11 19:43:36 2011 +0000
@@ -17,6 +17,7 @@
     }
 }
 
+#define BROWSEGROUP 0x1001
 #define BROWSEROOT  0x1002
 #define SERIALSERV  0x1101
 
@@ -235,6 +236,95 @@
     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
@@ -248,7 +338,7 @@
         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);
+        //   printf("Final or only block, sending bytes [%d, %d> of attributeList\n", cs, cs+byteCount);
     }
     buf[0] = 7; //pdu
     buf[1] = rxid>>8;
@@ -604,7 +694,8 @@
     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("MBED BlueUSB RFCOMM Serial");
+    static sdp_data name("Serial Port");
     rfcomm.add_element(&rfcommuuid);
     rfcomm.add_element(&chan);
     l2cap.add_element(&l2capuuid);
@@ -662,6 +753,10 @@
 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) {
@@ -677,9 +772,9 @@
                 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);
+                    svc->add_element(new sdp_data(idx->first, 2));
+                    svc->add_element(idx->second);
+                    printf("Found attrib %d\n", idx->first);
                 }
                 break;
             default:
@@ -705,9 +800,69 @@
         }
         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
@@ -756,17 +911,16 @@
 #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 {
+            } 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++) {