Diff: sdp.cpp

Revision:

1:70ee392bcfd4

Parent:

0:7493bf6bb1b9

Child:

2:d5a27b2d2e08

--- a/sdp.cpp	Mon Apr 04 16:45:20 2011 +0000
+++ b/sdp.cpp	Wed Apr 06 18:09:20 2011 +0000
@@ -15,43 +15,70 @@
     }
 }
 
+#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) {
-printf("\x1B[%dm", 34);
-    printf("OnSdpRsp(socket=%d, state=%d, len=%d)\n", socket, state, len);
-    //printfBytes("Got SDP Response from L2CAP: ", data, len);
-    SDPManager *self = (SDPManager*)userData;
-    if (state != SocketState_Open)
-      return;
-    sdp_data list(sdp_data::SEQUENCE);
-    if (data)
-        self->parseRsp(data, len);
-    if (len==0) {
-        sdp_data root(sdp_data::UUID, 0x1101);
-        sdp_data req(sdp_data::SEQUENCE);
-        req.add_element(&root);
-        self->ServiceSearchRequest(&req, 5);
-    } else if (data[0]==3) {
-        self->index = self->services.begin();
-        if (self->index != self->services.end()) {
-            unsigned handle = (*self->index).first;
-            //printf("req.: handle %#X\n", handle);
-            sdp_data all(0xffffU,4);
-            list.add_element(&all);
-            self->ServiceAttributeRequest(handle, 100, &list);//0x1001D
-            self->index++;
-        } else printf(" - empty list - \n");//should not happen
-    } else if (data[0]==5) {
-        if (self->index != self->services.end()) {
-            //printf("req.: handle %#X\n", (*self->index).first);
-            self->ServiceAttributeRequest((*self->index).first, 100, &list);
-            self->index++;
-        } else {
-            printf(" - end of list - \n");
-            Socket_Close(self->sdp_socket); //Note: socket=L2CAP, sdp_socket=SDP !!!
-        }
+//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); 
+    static sdp_data all(0x0000ffffU,4);
+    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);
+       req.add_element(&root);
+       once = false;
     }
-printf("\x1B[%dm", 0);
+    if (data){
+        parseRsp(data, len);
+    }    
+    switch (_state) {
+        case 0: //closed
+            if (len==0) { //socket just opened
+                //'Open' cleared the services list
+                ServiceSearchRequest(&req, 1);
+                _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),
@@ -88,7 +115,7 @@
 }
 
 int SDPManager::ServiceSearchRequest(sdp_data *sp, unsigned count, unsigned cs) {
-    int parlen = sp->Size() + 3; //no continuation
+    int parlen = sp->Size() + contState[0] + 3;
     buf[0] = 2; //pdu
     buf[1] = txid>>8;
     buf[2] = txid++;
@@ -97,16 +124,15 @@
     int p = sp->build(buf+5, 100-10);
     buf[p+6] = count;
     buf[p+5] = count>>8;
-    if (cs==0)
-        buf[p+7] = 0;
-    else
-        printf("Continuation not supported\n");
+    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(_l2cap, l2cap_buf, parlen + 5 + OFFSET);
 }
 
 int SDPManager::ServiceAttributeRequest(unsigned handle, unsigned count, sdp_data* al, unsigned cs) {
-    int parlen = al->Size() + 7; //no continuation
+    int parlen = al->Size() + contState[0] + 7;
     buf[0] = 4; //pdu
     buf[1] = txid>>8;
     buf[2] = txid++;
@@ -117,16 +143,15 @@
     buf[9] = count>>8;
     buf[10] = count;
     int p = al->build(buf+11, 100-26);
-    if (cs==0)
-        buf[p+11] = 0;
-    else
-        printf("Continuation not supported\n");
+    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(_l2cap, l2cap_buf, parlen + 5 + OFFSET);
 }
 
 int SDPManager::ServiceSearchAttributeRequest(sdp_data *sp, unsigned count, sdp_data* al, unsigned cs) {
-    int parlen = sp->Size() + al->Size() + 3; //no continuation (1 byte), count (2 bytes)
+    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++;
@@ -136,10 +161,9 @@
     buf[p+6] = count;
     buf[p+5] = count>>8;
     p += al->build(buf+11, 100-38);
-    if (cs==0)
-        buf[p+7] = 0;
-    else
-        printf("Continuation not supported\n");
+    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(_l2cap, l2cap_buf, parlen + 5 + OFFSET);
 }
@@ -181,8 +205,72 @@
     return len;
 }
 
-extern "C" void HardFault_Handler() { printf("Hard Fault!\n"); while(1); }
+extern "C" void HardFault_Handler() {
+    printf("Hard Fault! %d bytes left\n", AvailableMemory(1));
+    while (1);
+}
 
+unsigned SDPManager::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) {
+                char rev[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 SDPManager::parse (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) {
     unsigned p = 0;
@@ -220,7 +308,7 @@
         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);
+               //printf("SEQ%d{%p ", len, item);
                 int n = 0;
                 unsigned short key;
                 serv_rec *dummy = 0;
@@ -237,7 +325,7 @@
                     }
                 }
             }
-//            printf("}\n");
+            //printf("}\n");
             break;
         case 8:
             item = new sdp_data(sdp_data::URL, (char*)el+p, len);
@@ -249,10 +337,29 @@
     return end;
 }
 
+void SDPManager::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 SDPManager::freeBuf() {
+    if (contBuf) {
+       delete[] contBuf;
+       contBuf = 0;
+    }
+    byteCount = 0;
+}
+
 int SDPManager::parseRsp(const unsigned char*rsp, int len) {
-    //unsigned tid = rsp[2] + ((unsigned)rsp[1]<<8);
+    unsigned tid = rsp[2] + ((unsigned)rsp[1]<<8);
     unsigned parlen = rsp[4] + ((unsigned)rsp[3]<<8);
-//    printf("ParseRsp: tid=%04X, parlen=%d ", tid, parlen);
+    printf("ParseRsp: tid=%04X, parlen=%d ", tid, parlen);
     unsigned cont = 0;
     switch (rsp[0]) {
         case 1: {//errorRsp
@@ -266,17 +373,20 @@
         }
         //break;
         case 3: { //servicesearchRsp
-            //unsigned total = rsp[6] + ((unsigned)rsp[5]<<8);
+            unsigned total = rsp[6] + ((unsigned)rsp[5]<<8);
             unsigned current = rsp[8] + ((unsigned)rsp[7]<<8);
             cont = rsp[9+4*current];
-//            printf("total=%d, current=%d, cont=%d\n", total, current, cont);
-            if (cont) break;
+            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);
+                printf("SDP Search handle %08X\n", result);
                 services.insert(pair<unsigned, serv_rec*>(result, 0));
             }
             if (ServiceSearchResponse)
@@ -285,35 +395,46 @@
         break;
         case 5: { //serviceattributeRsp
             if (tree) delete tree;
-            unsigned count = rsp[6] + ((unsigned)rsp[5]<<8);
+            unsigned count = rsp[6] + ((unsigned)rsp[5]<<8);//bytes in this part of the attribute list
+            append(rsp+7, count);
             cont = rsp[7+count];
-            if (cont)
-                break;
-//            printf("pos=%d, count=%d parsing...\n", 7, count);
+            memcpy(contState, &rsp[7+count], cont+1);//copy the continuation state
+            if (cont) break;
+            printf("count=%d parsing...\n", byteCount);
             serv_rec *serv = new serv_rec;
-            unsigned p = parse(rsp+7, count, tree, serv);
-            unsigned key = (*serv)[0]->asUnsigned();
+            parse(contBuf, byteCount, tree, serv);
+            printf("...parsing done,  ");
+            unsigned key = (*serv)[0]->asUnsigned();//'0' is always the serviceID
+            printf("Key=%#X\n", key);
             services[key] = serv;
-//            printf("...parsing done, pos=%d, cont=%d\n", p, cont);
+            freeBuf();
             if (ServiceAttributeResponse)
                 ServiceAttributeResponse(serv);
         }
         break;
+        //below is UNTESTED
         case 7: { //servicesearchattributeRsp
             if (tree) delete tree;
             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 = 7;
-            while (rsp[pos]>>3 == sdp_data::SEQUENCE) {
-                unsigned len = length(rsp, pos);
+            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(rsp+pos, len, tree, serv);
+                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();
@@ -321,9 +442,7 @@
         break;
         default:
             printf("Unknown SDP response type %02X\n", rsp[0]);
-            break;
+        break;
     }
-    if (cont)
-        printf("Continuation not supported (yet)\n");
     return 0;
 }