SDP client for myBlueUSB
Dependents: mbed_TANK_Kinect ftusbClass
sdp.cpp@1:70ee392bcfd4, 2011-04-06 (annotated)
- Committer:
- networker
- Date:
- Wed Apr 06 18:09:20 2011 +0000
- Revision:
- 1:70ee392bcfd4
- Parent:
- 0:7493bf6bb1b9
- Child:
- 2:d5a27b2d2e08
Added support for continuation PDUs. Still some issues with handling multiple services.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
networker | 0:7493bf6bb1b9 | 1 | #include "mbed.h" |
networker | 0:7493bf6bb1b9 | 2 | #include "Utils.h" |
networker | 0:7493bf6bb1b9 | 3 | #include "hci.h" |
networker | 0:7493bf6bb1b9 | 4 | #include "sdp_data.h" |
networker | 0:7493bf6bb1b9 | 5 | #include "sdp.h" |
networker | 0:7493bf6bb1b9 | 6 | |
networker | 0:7493bf6bb1b9 | 7 | SDPManager SDP; //instance |
networker | 0:7493bf6bb1b9 | 8 | const unsigned char base_uuid[16] = { 0xfb, 0x34, 0x9b, 0x5f, 0x80, 0, 0x07, 0x70, 0, 0x10, 0, 0}; |
networker | 0:7493bf6bb1b9 | 9 | |
networker | 0:7493bf6bb1b9 | 10 | void attribHandler(serv_rec *r) { |
networker | 0:7493bf6bb1b9 | 11 | printf("Service 0x%08X\n", (*r)[0x0000]->asUnsigned()); |
networker | 0:7493bf6bb1b9 | 12 | map<unsigned short, sdp_data*>::iterator it = r->begin(); |
networker | 0:7493bf6bb1b9 | 13 | for (;it != r->end();it++) { |
networker | 0:7493bf6bb1b9 | 14 | printf(" 0x%04X: %s\n", (*it).first, (*it).second->asString()); |
networker | 0:7493bf6bb1b9 | 15 | } |
networker | 0:7493bf6bb1b9 | 16 | } |
networker | 0:7493bf6bb1b9 | 17 | |
networker | 1:70ee392bcfd4 | 18 | #define BROWSEROOT 0x1002 |
networker | 1:70ee392bcfd4 | 19 | #define SERIALSERV 0x1101 |
networker | 1:70ee392bcfd4 | 20 | |
networker | 0:7493bf6bb1b9 | 21 | //this function is called when the L2CAP layer receives SDP packets (see SDPManager::Open), userdata is the sdpmanager instance |
networker | 1:70ee392bcfd4 | 22 | //void SDPManager::OnSdpRsp(int socket, SocketState state, const u8* data, int len, void* userData) { |
networker | 1:70ee392bcfd4 | 23 | void SDPManager::OnSdpRsp(const u8* data, int len) { |
networker | 1:70ee392bcfd4 | 24 | static sdp_data list(sdp_data::SEQUENCE); |
networker | 1:70ee392bcfd4 | 25 | static sdp_data all(0x0000ffffU,4); |
networker | 1:70ee392bcfd4 | 26 | static sdp_data root(sdp_data::UUID, BROWSEROOT); |
networker | 1:70ee392bcfd4 | 27 | static sdp_data req(sdp_data::SEQUENCE); |
networker | 1:70ee392bcfd4 | 28 | static bool once = true; |
networker | 1:70ee392bcfd4 | 29 | printf("_state=%d first=%d ", _state, once); |
networker | 1:70ee392bcfd4 | 30 | if (once) { |
networker | 1:70ee392bcfd4 | 31 | list.add_element(&all); |
networker | 1:70ee392bcfd4 | 32 | req.add_element(&root); |
networker | 1:70ee392bcfd4 | 33 | once = false; |
networker | 0:7493bf6bb1b9 | 34 | } |
networker | 1:70ee392bcfd4 | 35 | if (data){ |
networker | 1:70ee392bcfd4 | 36 | parseRsp(data, len); |
networker | 1:70ee392bcfd4 | 37 | } |
networker | 1:70ee392bcfd4 | 38 | switch (_state) { |
networker | 1:70ee392bcfd4 | 39 | case 0: //closed |
networker | 1:70ee392bcfd4 | 40 | if (len==0) { //socket just opened |
networker | 1:70ee392bcfd4 | 41 | //'Open' cleared the services list |
networker | 1:70ee392bcfd4 | 42 | ServiceSearchRequest(&req, 1); |
networker | 1:70ee392bcfd4 | 43 | _state = 1; //wait for service handles |
networker | 1:70ee392bcfd4 | 44 | } |
networker | 1:70ee392bcfd4 | 45 | break; |
networker | 1:70ee392bcfd4 | 46 | case 1: //service handles arriving |
networker | 1:70ee392bcfd4 | 47 | if (contState[0]) {//continuation, repeat request |
networker | 1:70ee392bcfd4 | 48 | ServiceSearchRequest(&req, 5); |
networker | 1:70ee392bcfd4 | 49 | } else { |
networker | 1:70ee392bcfd4 | 50 | if (data[0]==3) { |
networker | 1:70ee392bcfd4 | 51 | index = services.begin(); |
networker | 1:70ee392bcfd4 | 52 | if (index != services.end()) { |
networker | 1:70ee392bcfd4 | 53 | unsigned handle = (*index).first; |
networker | 1:70ee392bcfd4 | 54 | printf("req.: handle %#X\n", handle); |
networker | 1:70ee392bcfd4 | 55 | ServiceAttributeRequest(handle, 100, &list);//0x1001D |
networker | 1:70ee392bcfd4 | 56 | } else |
networker | 1:70ee392bcfd4 | 57 | printf(" - empty list - \n");//should not happen |
networker | 1:70ee392bcfd4 | 58 | _state = 2; //wait for attribute response |
networker | 1:70ee392bcfd4 | 59 | } else |
networker | 1:70ee392bcfd4 | 60 | printf("Expected a ServiceSearchResponse 0x03, got %#x\n", data[0]); |
networker | 1:70ee392bcfd4 | 61 | } |
networker | 1:70ee392bcfd4 | 62 | break; |
networker | 1:70ee392bcfd4 | 63 | case 2: |
networker | 1:70ee392bcfd4 | 64 | if (contState[0])//repeat request |
networker | 1:70ee392bcfd4 | 65 | ServiceAttributeRequest((*index).first, 100, &list); |
networker | 1:70ee392bcfd4 | 66 | else { |
networker | 1:70ee392bcfd4 | 67 | if (data[0]==5) { |
networker | 1:70ee392bcfd4 | 68 | index++; //move to next service |
networker | 1:70ee392bcfd4 | 69 | if (index != services.end()) { |
networker | 1:70ee392bcfd4 | 70 | printf("req.: handle %#X\n", (*index).first); |
networker | 1:70ee392bcfd4 | 71 | ServiceAttributeRequest((*index).first, 100, &list); |
networker | 1:70ee392bcfd4 | 72 | } else { |
networker | 1:70ee392bcfd4 | 73 | printf(" - end of list - \n"); |
networker | 1:70ee392bcfd4 | 74 | Socket_Close(sdp_socket); //Note: socket=L2CAP, sdp_socket=SDP !!! |
networker | 1:70ee392bcfd4 | 75 | _state = 0; |
networker | 1:70ee392bcfd4 | 76 | } |
networker | 1:70ee392bcfd4 | 77 | } else |
networker | 1:70ee392bcfd4 | 78 | printf("Expected a ServiceAttributeResponse 0x05, got %#x\n", data[0]); |
networker | 1:70ee392bcfd4 | 79 | } |
networker | 1:70ee392bcfd4 | 80 | break; |
networker | 1:70ee392bcfd4 | 81 | } |
networker | 0:7493bf6bb1b9 | 82 | } |
networker | 0:7493bf6bb1b9 | 83 | |
networker | 0:7493bf6bb1b9 | 84 | //this function is called when the SDP sockets receives data (see HCICallback in TestShell), |
networker | 0:7493bf6bb1b9 | 85 | //currently does not happen because not forwarded from OnSdpRsp, can be used to handle multiple connections |
networker | 0:7493bf6bb1b9 | 86 | void SDPManager::OnSockCallback(int socket, SocketState state, const u8* data, int len, void* userData) { |
networker | 0:7493bf6bb1b9 | 87 | printf("OnSockCallback(socket=%d, state=%d, len=%d)\n", socket, state, len); |
networker | 0:7493bf6bb1b9 | 88 | printfBytes("Got SDP Response from socket: ", data, len); |
networker | 0:7493bf6bb1b9 | 89 | } |
networker | 0:7493bf6bb1b9 | 90 | |
networker | 0:7493bf6bb1b9 | 91 | void SDPManager::errorhandler(unsigned err) {//default error handler |
networker | 0:7493bf6bb1b9 | 92 | switch (err) { |
networker | 0:7493bf6bb1b9 | 93 | case 1: |
networker | 0:7493bf6bb1b9 | 94 | printf("Unsupported version of SDP\n"); |
networker | 0:7493bf6bb1b9 | 95 | break; |
networker | 0:7493bf6bb1b9 | 96 | case 2: |
networker | 0:7493bf6bb1b9 | 97 | printf("Invalid SDP ServiceRecordHandle\n"); |
networker | 0:7493bf6bb1b9 | 98 | break; |
networker | 0:7493bf6bb1b9 | 99 | case 3: |
networker | 0:7493bf6bb1b9 | 100 | printf("SDP syntax error\n"); |
networker | 0:7493bf6bb1b9 | 101 | break; |
networker | 0:7493bf6bb1b9 | 102 | case 4: |
networker | 0:7493bf6bb1b9 | 103 | printf("PDU size was invalid\n"); |
networker | 0:7493bf6bb1b9 | 104 | break; |
networker | 0:7493bf6bb1b9 | 105 | case 5: |
networker | 0:7493bf6bb1b9 | 106 | printf("Continuation state was invalid\n"); |
networker | 0:7493bf6bb1b9 | 107 | break; |
networker | 0:7493bf6bb1b9 | 108 | case 6: |
networker | 0:7493bf6bb1b9 | 109 | printf("SDP server has insufficient resources\n"); |
networker | 0:7493bf6bb1b9 | 110 | break; |
networker | 0:7493bf6bb1b9 | 111 | default: |
networker | 0:7493bf6bb1b9 | 112 | printf("Unknown SDP error code\n"); |
networker | 0:7493bf6bb1b9 | 113 | break; |
networker | 0:7493bf6bb1b9 | 114 | } |
networker | 0:7493bf6bb1b9 | 115 | } |
networker | 0:7493bf6bb1b9 | 116 | |
networker | 0:7493bf6bb1b9 | 117 | int SDPManager::ServiceSearchRequest(sdp_data *sp, unsigned count, unsigned cs) { |
networker | 1:70ee392bcfd4 | 118 | int parlen = sp->Size() + contState[0] + 3; |
networker | 0:7493bf6bb1b9 | 119 | buf[0] = 2; //pdu |
networker | 0:7493bf6bb1b9 | 120 | buf[1] = txid>>8; |
networker | 0:7493bf6bb1b9 | 121 | buf[2] = txid++; |
networker | 0:7493bf6bb1b9 | 122 | buf[4] = parlen; |
networker | 0:7493bf6bb1b9 | 123 | buf[3] = parlen>>8; |
networker | 0:7493bf6bb1b9 | 124 | int p = sp->build(buf+5, 100-10); |
networker | 0:7493bf6bb1b9 | 125 | buf[p+6] = count; |
networker | 0:7493bf6bb1b9 | 126 | buf[p+5] = count>>8; |
networker | 1:70ee392bcfd4 | 127 | buf[p+7] = contState[0]; |
networker | 1:70ee392bcfd4 | 128 | for (int j = 1; j <= contState[0]; j++) |
networker | 1:70ee392bcfd4 | 129 | buf[p+j+7] = contState[j]; |
networker | 0:7493bf6bb1b9 | 130 | //printfBytes("SDP Send: ", buf, parlen+5); |
networker | 0:7493bf6bb1b9 | 131 | return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET); |
networker | 0:7493bf6bb1b9 | 132 | } |
networker | 0:7493bf6bb1b9 | 133 | |
networker | 0:7493bf6bb1b9 | 134 | int SDPManager::ServiceAttributeRequest(unsigned handle, unsigned count, sdp_data* al, unsigned cs) { |
networker | 1:70ee392bcfd4 | 135 | int parlen = al->Size() + contState[0] + 7; |
networker | 0:7493bf6bb1b9 | 136 | buf[0] = 4; //pdu |
networker | 0:7493bf6bb1b9 | 137 | buf[1] = txid>>8; |
networker | 0:7493bf6bb1b9 | 138 | buf[2] = txid++; |
networker | 0:7493bf6bb1b9 | 139 | buf[4] = parlen; |
networker | 0:7493bf6bb1b9 | 140 | buf[3] = parlen>>8; |
networker | 0:7493bf6bb1b9 | 141 | for (int i = 0; i < 4; i++) |
networker | 0:7493bf6bb1b9 | 142 | buf[i+5] = ((char*)&handle)[3-i]; |
networker | 0:7493bf6bb1b9 | 143 | buf[9] = count>>8; |
networker | 0:7493bf6bb1b9 | 144 | buf[10] = count; |
networker | 0:7493bf6bb1b9 | 145 | int p = al->build(buf+11, 100-26); |
networker | 1:70ee392bcfd4 | 146 | buf[p+11] = contState[0]; |
networker | 1:70ee392bcfd4 | 147 | for (int j = 1; j <= contState[0]; j++) |
networker | 1:70ee392bcfd4 | 148 | buf[p+j+11] = contState[j]; |
networker | 0:7493bf6bb1b9 | 149 | //printfBytes("SDP Send: ", buf, parlen+5); |
networker | 0:7493bf6bb1b9 | 150 | return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET); |
networker | 0:7493bf6bb1b9 | 151 | } |
networker | 0:7493bf6bb1b9 | 152 | |
networker | 0:7493bf6bb1b9 | 153 | int SDPManager::ServiceSearchAttributeRequest(sdp_data *sp, unsigned count, sdp_data* al, unsigned cs) { |
networker | 1:70ee392bcfd4 | 154 | int parlen = sp->Size() + al->Size() + contState[0] + 3; // count (2 bytes) + at least 1 cont |
networker | 0:7493bf6bb1b9 | 155 | buf[0] = 6; //pdu |
networker | 0:7493bf6bb1b9 | 156 | buf[1] = txid>>8; |
networker | 0:7493bf6bb1b9 | 157 | buf[2] = txid++; |
networker | 0:7493bf6bb1b9 | 158 | buf[4] = parlen; |
networker | 0:7493bf6bb1b9 | 159 | buf[3] = parlen>>8; |
networker | 0:7493bf6bb1b9 | 160 | int p = sp->build(buf+5, 30); |
networker | 0:7493bf6bb1b9 | 161 | buf[p+6] = count; |
networker | 0:7493bf6bb1b9 | 162 | buf[p+5] = count>>8; |
networker | 0:7493bf6bb1b9 | 163 | p += al->build(buf+11, 100-38); |
networker | 1:70ee392bcfd4 | 164 | buf[p+7] = contState[0]; |
networker | 1:70ee392bcfd4 | 165 | for (int j = 1; j <= contState[0]; j++) |
networker | 1:70ee392bcfd4 | 166 | buf[p+j+7] = contState[j]; |
networker | 0:7493bf6bb1b9 | 167 | //printfBytes("SDP Send: ", buf, parlen+5); |
networker | 0:7493bf6bb1b9 | 168 | return Socket_Send(_l2cap, l2cap_buf, parlen + 5 + OFFSET); |
networker | 0:7493bf6bb1b9 | 169 | } |
networker | 0:7493bf6bb1b9 | 170 | |
networker | 0:7493bf6bb1b9 | 171 | unsigned SDPManager::getval(const unsigned char *p, int n) { |
networker | 0:7493bf6bb1b9 | 172 | unsigned ret = 0; |
networker | 0:7493bf6bb1b9 | 173 | for (int i = 0; i < n; i++) |
networker | 0:7493bf6bb1b9 | 174 | ret = (ret<<8) + (unsigned)p[i]; |
networker | 0:7493bf6bb1b9 | 175 | return ret; |
networker | 0:7493bf6bb1b9 | 176 | } |
networker | 0:7493bf6bb1b9 | 177 | |
networker | 0:7493bf6bb1b9 | 178 | unsigned SDPManager::length(const unsigned char *el, unsigned &p) { |
networker | 0:7493bf6bb1b9 | 179 | unsigned len = 0; |
networker | 0:7493bf6bb1b9 | 180 | switch (el[p++] & 7) {//length |
networker | 0:7493bf6bb1b9 | 181 | case 0: |
networker | 0:7493bf6bb1b9 | 182 | len = 1; |
networker | 0:7493bf6bb1b9 | 183 | break; |
networker | 0:7493bf6bb1b9 | 184 | case 1: |
networker | 0:7493bf6bb1b9 | 185 | len = 2; |
networker | 0:7493bf6bb1b9 | 186 | break; |
networker | 0:7493bf6bb1b9 | 187 | case 2: |
networker | 0:7493bf6bb1b9 | 188 | len = 4; |
networker | 0:7493bf6bb1b9 | 189 | break; |
networker | 0:7493bf6bb1b9 | 190 | case 3: |
networker | 0:7493bf6bb1b9 | 191 | len = 8; |
networker | 0:7493bf6bb1b9 | 192 | break; |
networker | 0:7493bf6bb1b9 | 193 | case 4: |
networker | 0:7493bf6bb1b9 | 194 | len = 16; |
networker | 0:7493bf6bb1b9 | 195 | break; |
networker | 0:7493bf6bb1b9 | 196 | case 7://4bytes |
networker | 0:7493bf6bb1b9 | 197 | len= el[p++]<<24; |
networker | 0:7493bf6bb1b9 | 198 | len += el[p++]<<16; |
networker | 0:7493bf6bb1b9 | 199 | case 6://2bytes |
networker | 0:7493bf6bb1b9 | 200 | len += el[p++]<<8; |
networker | 0:7493bf6bb1b9 | 201 | case 5://1byte |
networker | 0:7493bf6bb1b9 | 202 | len += el[p++]; |
networker | 0:7493bf6bb1b9 | 203 | break; |
networker | 0:7493bf6bb1b9 | 204 | } |
networker | 0:7493bf6bb1b9 | 205 | return len; |
networker | 0:7493bf6bb1b9 | 206 | } |
networker | 0:7493bf6bb1b9 | 207 | |
networker | 1:70ee392bcfd4 | 208 | extern "C" void HardFault_Handler() { |
networker | 1:70ee392bcfd4 | 209 | printf("Hard Fault! %d bytes left\n", AvailableMemory(1)); |
networker | 1:70ee392bcfd4 | 210 | while (1); |
networker | 1:70ee392bcfd4 | 211 | } |
networker | 0:7493bf6bb1b9 | 212 | |
networker | 1:70ee392bcfd4 | 213 | unsigned SDPManager::parseLight (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) { |
networker | 1:70ee392bcfd4 | 214 | unsigned p = 0; |
networker | 1:70ee392bcfd4 | 215 | unsigned len = length(el, p); |
networker | 1:70ee392bcfd4 | 216 | int end = p+len;//end is the index of the item just after the sequence |
networker | 1:70ee392bcfd4 | 217 | sdp_data *item = 0; |
networker | 1:70ee392bcfd4 | 218 | switch (el[0]>>3) {//type |
networker | 1:70ee392bcfd4 | 219 | case sdp_data::NULL_: |
networker | 1:70ee392bcfd4 | 220 | printf("NULL "); |
networker | 1:70ee392bcfd4 | 221 | break; |
networker | 1:70ee392bcfd4 | 222 | case sdp_data::UNSIGNED: |
networker | 1:70ee392bcfd4 | 223 | printf("UINT%d=%u ", len, (unsigned)getval(el+p, len)); |
networker | 1:70ee392bcfd4 | 224 | break; |
networker | 1:70ee392bcfd4 | 225 | case sdp_data::SIGNED: |
networker | 1:70ee392bcfd4 | 226 | printf("INT%d=%d ", len, (unsigned)getval(el+p, len)); |
networker | 1:70ee392bcfd4 | 227 | break; |
networker | 1:70ee392bcfd4 | 228 | case sdp_data::UUID: |
networker | 1:70ee392bcfd4 | 229 | if (len==16) { |
networker | 1:70ee392bcfd4 | 230 | char rev[16]; |
networker | 1:70ee392bcfd4 | 231 | printf("UUID16= "); |
networker | 1:70ee392bcfd4 | 232 | for (int i = 0; i < 16; i++) |
networker | 1:70ee392bcfd4 | 233 | printf("%02x ", el[p+i]); |
networker | 1:70ee392bcfd4 | 234 | } else |
networker | 1:70ee392bcfd4 | 235 | printf("UUID%d=%u ", len, (unsigned)getval(el+p, len)); |
networker | 1:70ee392bcfd4 | 236 | break; |
networker | 1:70ee392bcfd4 | 237 | case sdp_data::STRING: |
networker | 1:70ee392bcfd4 | 238 | printf("STR%d='%s' ", len, (char*)el+p); |
networker | 1:70ee392bcfd4 | 239 | break; |
networker | 1:70ee392bcfd4 | 240 | case sdp_data::BOOL: |
networker | 1:70ee392bcfd4 | 241 | printf("BOOL%d=%d ", len, (unsigned)getval(el+p, len)); |
networker | 1:70ee392bcfd4 | 242 | break; |
networker | 1:70ee392bcfd4 | 243 | case sdp_data::SEQUENCE: |
networker | 1:70ee392bcfd4 | 244 | goto skip; |
networker | 1:70ee392bcfd4 | 245 | case sdp_data::ALTERNATIVE: |
networker | 1:70ee392bcfd4 | 246 | skip: {//p points just after the length indicator, hence at the first item IN the sequence |
networker | 1:70ee392bcfd4 | 247 | printf("SEQ%d{%p ", len, item); |
networker | 1:70ee392bcfd4 | 248 | int n = 0; |
networker | 1:70ee392bcfd4 | 249 | unsigned short key; |
networker | 1:70ee392bcfd4 | 250 | serv_rec *dummy = 0; |
networker | 1:70ee392bcfd4 | 251 | while (p < end) { |
networker | 1:70ee392bcfd4 | 252 | sdp_data *elem = 0; |
networker | 1:70ee392bcfd4 | 253 | p += parseLight(el + p, len-p, elem, dummy);//parse each element in the sequence, the second arg is as yet unused |
networker | 1:70ee392bcfd4 | 254 | if (record) { |
networker | 1:70ee392bcfd4 | 255 | if (n & 1) { //value |
networker | 1:70ee392bcfd4 | 256 | record->insert(pair<unsigned short, sdp_data*>(key, elem)); |
networker | 1:70ee392bcfd4 | 257 | } else //key |
networker | 1:70ee392bcfd4 | 258 | key = n; |
networker | 1:70ee392bcfd4 | 259 | n++; |
networker | 1:70ee392bcfd4 | 260 | } |
networker | 1:70ee392bcfd4 | 261 | } |
networker | 1:70ee392bcfd4 | 262 | } |
networker | 1:70ee392bcfd4 | 263 | printf("}\n"); |
networker | 1:70ee392bcfd4 | 264 | break; |
networker | 1:70ee392bcfd4 | 265 | case 8: |
networker | 1:70ee392bcfd4 | 266 | printf("URL%d='%s' ", len, (char*)el+p); |
networker | 1:70ee392bcfd4 | 267 | break; |
networker | 1:70ee392bcfd4 | 268 | default: |
networker | 1:70ee392bcfd4 | 269 | printf("Parse: Unknown type %d, len=%d (code=%#02X)\n", el[0]>>3, len, el[0]); |
networker | 1:70ee392bcfd4 | 270 | } |
networker | 1:70ee392bcfd4 | 271 | result = item; |
networker | 1:70ee392bcfd4 | 272 | return end; |
networker | 1:70ee392bcfd4 | 273 | } |
networker | 0:7493bf6bb1b9 | 274 | |
networker | 0:7493bf6bb1b9 | 275 | unsigned SDPManager::parse (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) { |
networker | 0:7493bf6bb1b9 | 276 | unsigned p = 0; |
networker | 0:7493bf6bb1b9 | 277 | unsigned len = length(el, p); |
networker | 0:7493bf6bb1b9 | 278 | int end = p+len;//end is the index of the item just after the sequence |
networker | 0:7493bf6bb1b9 | 279 | sdp_data *item = 0; |
networker | 0:7493bf6bb1b9 | 280 | switch (el[0]>>3) {//type |
networker | 0:7493bf6bb1b9 | 281 | case sdp_data::NULL_: |
networker | 0:7493bf6bb1b9 | 282 | item = new sdp_data(); |
networker | 0:7493bf6bb1b9 | 283 | break; |
networker | 0:7493bf6bb1b9 | 284 | case sdp_data::UNSIGNED: |
networker | 0:7493bf6bb1b9 | 285 | item = new sdp_data((unsigned)getval(el+p, len), len); |
networker | 0:7493bf6bb1b9 | 286 | break; |
networker | 0:7493bf6bb1b9 | 287 | case sdp_data::SIGNED: |
networker | 0:7493bf6bb1b9 | 288 | item = new sdp_data((int)getval(el+p, len), len); |
networker | 0:7493bf6bb1b9 | 289 | break; |
networker | 0:7493bf6bb1b9 | 290 | case sdp_data::UUID: |
networker | 0:7493bf6bb1b9 | 291 | if (len==16) { |
networker | 0:7493bf6bb1b9 | 292 | char rev[16]; |
networker | 0:7493bf6bb1b9 | 293 | for (int i = 0; i < 16; i++) |
networker | 0:7493bf6bb1b9 | 294 | rev[i] = el[p+15-i]; |
networker | 0:7493bf6bb1b9 | 295 | item = new sdp_data(sdp_data::UUID, rev, len); |
networker | 0:7493bf6bb1b9 | 296 | } else |
networker | 0:7493bf6bb1b9 | 297 | item = new sdp_data(sdp_data::UUID, getval(el+p, len), len); |
networker | 0:7493bf6bb1b9 | 298 | break; |
networker | 0:7493bf6bb1b9 | 299 | case sdp_data::STRING: |
networker | 0:7493bf6bb1b9 | 300 | item = new sdp_data((char*)el+p, len); |
networker | 0:7493bf6bb1b9 | 301 | break; |
networker | 0:7493bf6bb1b9 | 302 | case sdp_data::BOOL: |
networker | 0:7493bf6bb1b9 | 303 | item = new sdp_data((bool)getval(el+p, len), len); |
networker | 0:7493bf6bb1b9 | 304 | break; |
networker | 0:7493bf6bb1b9 | 305 | case sdp_data::SEQUENCE: |
networker | 0:7493bf6bb1b9 | 306 | item = new sdp_data(sdp_data::SEQUENCE); |
networker | 0:7493bf6bb1b9 | 307 | goto skip; |
networker | 0:7493bf6bb1b9 | 308 | case sdp_data::ALTERNATIVE: |
networker | 0:7493bf6bb1b9 | 309 | item = new sdp_data(sdp_data::ALTERNATIVE); |
networker | 0:7493bf6bb1b9 | 310 | skip: {//p points just after the length indicator, hence at the first item IN the sequence |
networker | 1:70ee392bcfd4 | 311 | //printf("SEQ%d{%p ", len, item); |
networker | 0:7493bf6bb1b9 | 312 | int n = 0; |
networker | 0:7493bf6bb1b9 | 313 | unsigned short key; |
networker | 0:7493bf6bb1b9 | 314 | serv_rec *dummy = 0; |
networker | 0:7493bf6bb1b9 | 315 | while (p < end) { |
networker | 0:7493bf6bb1b9 | 316 | sdp_data *elem = 0; |
networker | 0:7493bf6bb1b9 | 317 | p += parse(el + p, len-p, elem, dummy);//parse each element in the sequence, the second arg is as yet unused |
networker | 0:7493bf6bb1b9 | 318 | item->add_element(elem); |
networker | 0:7493bf6bb1b9 | 319 | if (record) { |
networker | 0:7493bf6bb1b9 | 320 | if (n & 1) { //value |
networker | 0:7493bf6bb1b9 | 321 | record->insert(pair<unsigned short, sdp_data*>(key, elem)); |
networker | 0:7493bf6bb1b9 | 322 | } else //key |
networker | 0:7493bf6bb1b9 | 323 | key = elem->asUnsigned(); |
networker | 0:7493bf6bb1b9 | 324 | n++; |
networker | 0:7493bf6bb1b9 | 325 | } |
networker | 0:7493bf6bb1b9 | 326 | } |
networker | 0:7493bf6bb1b9 | 327 | } |
networker | 1:70ee392bcfd4 | 328 | //printf("}\n"); |
networker | 0:7493bf6bb1b9 | 329 | break; |
networker | 0:7493bf6bb1b9 | 330 | case 8: |
networker | 0:7493bf6bb1b9 | 331 | item = new sdp_data(sdp_data::URL, (char*)el+p, len); |
networker | 0:7493bf6bb1b9 | 332 | break; |
networker | 0:7493bf6bb1b9 | 333 | default: |
networker | 0:7493bf6bb1b9 | 334 | printf("Parse: Unknown type %d, len=%d (code=%#02X)\n", el[0]>>3, len, el[0]); |
networker | 0:7493bf6bb1b9 | 335 | } |
networker | 0:7493bf6bb1b9 | 336 | result = item; |
networker | 0:7493bf6bb1b9 | 337 | return end; |
networker | 0:7493bf6bb1b9 | 338 | } |
networker | 0:7493bf6bb1b9 | 339 | |
networker | 1:70ee392bcfd4 | 340 | void SDPManager::append(const unsigned char *payload, int len) { |
networker | 1:70ee392bcfd4 | 341 | unsigned char *tmp = new unsigned char[byteCount + len];//append the payload to the previous continuation buffer |
networker | 1:70ee392bcfd4 | 342 | if (contBuf && byteCount) { |
networker | 1:70ee392bcfd4 | 343 | memcpy(tmp, contBuf, byteCount); //copy the existing part |
networker | 1:70ee392bcfd4 | 344 | delete[] contBuf;//delete the old buffer |
networker | 1:70ee392bcfd4 | 345 | } |
networker | 1:70ee392bcfd4 | 346 | memcpy(tmp+byteCount, payload, len); //append the new part |
networker | 1:70ee392bcfd4 | 347 | contBuf = tmp; |
networker | 1:70ee392bcfd4 | 348 | byteCount += len; |
networker | 1:70ee392bcfd4 | 349 | } |
networker | 1:70ee392bcfd4 | 350 | |
networker | 1:70ee392bcfd4 | 351 | void SDPManager::freeBuf() { |
networker | 1:70ee392bcfd4 | 352 | if (contBuf) { |
networker | 1:70ee392bcfd4 | 353 | delete[] contBuf; |
networker | 1:70ee392bcfd4 | 354 | contBuf = 0; |
networker | 1:70ee392bcfd4 | 355 | } |
networker | 1:70ee392bcfd4 | 356 | byteCount = 0; |
networker | 1:70ee392bcfd4 | 357 | } |
networker | 1:70ee392bcfd4 | 358 | |
networker | 0:7493bf6bb1b9 | 359 | int SDPManager::parseRsp(const unsigned char*rsp, int len) { |
networker | 1:70ee392bcfd4 | 360 | unsigned tid = rsp[2] + ((unsigned)rsp[1]<<8); |
networker | 0:7493bf6bb1b9 | 361 | unsigned parlen = rsp[4] + ((unsigned)rsp[3]<<8); |
networker | 1:70ee392bcfd4 | 362 | printf("ParseRsp: tid=%04X, parlen=%d ", tid, parlen); |
networker | 0:7493bf6bb1b9 | 363 | unsigned cont = 0; |
networker | 0:7493bf6bb1b9 | 364 | switch (rsp[0]) { |
networker | 0:7493bf6bb1b9 | 365 | case 1: {//errorRsp |
networker | 0:7493bf6bb1b9 | 366 | unsigned errorcode = rsp[6] + ((unsigned)rsp[5]<<8); |
networker | 0:7493bf6bb1b9 | 367 | if (parlen > 2) { |
networker | 0:7493bf6bb1b9 | 368 | printf("ErrorInfo (%d bytes) for error %d is available\n", parlen-2, errorcode); |
networker | 0:7493bf6bb1b9 | 369 | } |
networker | 0:7493bf6bb1b9 | 370 | if (ErrorResponse) |
networker | 0:7493bf6bb1b9 | 371 | ErrorResponse(errorcode); |
networker | 0:7493bf6bb1b9 | 372 | return errorcode; |
networker | 0:7493bf6bb1b9 | 373 | } |
networker | 0:7493bf6bb1b9 | 374 | //break; |
networker | 0:7493bf6bb1b9 | 375 | case 3: { //servicesearchRsp |
networker | 1:70ee392bcfd4 | 376 | unsigned total = rsp[6] + ((unsigned)rsp[5]<<8); |
networker | 0:7493bf6bb1b9 | 377 | unsigned current = rsp[8] + ((unsigned)rsp[7]<<8); |
networker | 0:7493bf6bb1b9 | 378 | cont = rsp[9+4*current]; |
networker | 1:70ee392bcfd4 | 379 | memcpy(contState, &rsp[9+4*current], cont+1);//copy the continuation state |
networker | 1:70ee392bcfd4 | 380 | printf("total=%d, current=%d, cont=%d\n", total, current, cont); |
networker | 1:70ee392bcfd4 | 381 | if (cont) { |
networker | 1:70ee392bcfd4 | 382 | //no special handling here, just append the servicerecordhandles |
networker | 1:70ee392bcfd4 | 383 | } |
networker | 0:7493bf6bb1b9 | 384 | //linear list of 32bit service-handles |
networker | 0:7493bf6bb1b9 | 385 | for (int i = 0; i < current; i++) { |
networker | 0:7493bf6bb1b9 | 386 | unsigned result = 0; |
networker | 0:7493bf6bb1b9 | 387 | for (int j = 0; j< 4; j++) |
networker | 0:7493bf6bb1b9 | 388 | result = (result<<8) + rsp[9 + 4*i + j]; |
networker | 1:70ee392bcfd4 | 389 | printf("SDP Search handle %08X\n", result); |
networker | 0:7493bf6bb1b9 | 390 | services.insert(pair<unsigned, serv_rec*>(result, 0)); |
networker | 0:7493bf6bb1b9 | 391 | } |
networker | 0:7493bf6bb1b9 | 392 | if (ServiceSearchResponse) |
networker | 0:7493bf6bb1b9 | 393 | ServiceSearchResponse(); |
networker | 0:7493bf6bb1b9 | 394 | } |
networker | 0:7493bf6bb1b9 | 395 | break; |
networker | 0:7493bf6bb1b9 | 396 | case 5: { //serviceattributeRsp |
networker | 0:7493bf6bb1b9 | 397 | if (tree) delete tree; |
networker | 1:70ee392bcfd4 | 398 | unsigned count = rsp[6] + ((unsigned)rsp[5]<<8);//bytes in this part of the attribute list |
networker | 1:70ee392bcfd4 | 399 | append(rsp+7, count); |
networker | 0:7493bf6bb1b9 | 400 | cont = rsp[7+count]; |
networker | 1:70ee392bcfd4 | 401 | memcpy(contState, &rsp[7+count], cont+1);//copy the continuation state |
networker | 1:70ee392bcfd4 | 402 | if (cont) break; |
networker | 1:70ee392bcfd4 | 403 | printf("count=%d parsing...\n", byteCount); |
networker | 0:7493bf6bb1b9 | 404 | serv_rec *serv = new serv_rec; |
networker | 1:70ee392bcfd4 | 405 | parse(contBuf, byteCount, tree, serv); |
networker | 1:70ee392bcfd4 | 406 | printf("...parsing done, "); |
networker | 1:70ee392bcfd4 | 407 | unsigned key = (*serv)[0]->asUnsigned();//'0' is always the serviceID |
networker | 1:70ee392bcfd4 | 408 | printf("Key=%#X\n", key); |
networker | 0:7493bf6bb1b9 | 409 | services[key] = serv; |
networker | 1:70ee392bcfd4 | 410 | freeBuf(); |
networker | 0:7493bf6bb1b9 | 411 | if (ServiceAttributeResponse) |
networker | 0:7493bf6bb1b9 | 412 | ServiceAttributeResponse(serv); |
networker | 0:7493bf6bb1b9 | 413 | } |
networker | 0:7493bf6bb1b9 | 414 | break; |
networker | 1:70ee392bcfd4 | 415 | //below is UNTESTED |
networker | 0:7493bf6bb1b9 | 416 | case 7: { //servicesearchattributeRsp |
networker | 0:7493bf6bb1b9 | 417 | if (tree) delete tree; |
networker | 0:7493bf6bb1b9 | 418 | unsigned count = rsp[6] + ((unsigned)rsp[5]<<8); |
networker | 1:70ee392bcfd4 | 419 | append(rsp+7, count); |
networker | 0:7493bf6bb1b9 | 420 | cont = rsp[7+count]; |
networker | 1:70ee392bcfd4 | 421 | memcpy(contState, &rsp[7+count], cont+1); |
networker | 0:7493bf6bb1b9 | 422 | if (cont) |
networker | 0:7493bf6bb1b9 | 423 | break; |
networker | 1:70ee392bcfd4 | 424 | unsigned pos = 0; |
networker | 1:70ee392bcfd4 | 425 | if (contBuf[pos]>>3 != sdp_data::SEQUENCE) { |
networker | 1:70ee392bcfd4 | 426 | printf("Expected a sequence of attribute lists\n"); |
networker | 1:70ee392bcfd4 | 427 | break; |
networker | 1:70ee392bcfd4 | 428 | } |
networker | 1:70ee392bcfd4 | 429 | unsigned len = length(contBuf, pos);//get the length of the list of lists and advance pos to the first list |
networker | 1:70ee392bcfd4 | 430 | while (pos<len) { |
networker | 0:7493bf6bb1b9 | 431 | printf("pos=%d, count=%d, parsing...\n", pos, len); |
networker | 0:7493bf6bb1b9 | 432 | serv_rec *serv = new serv_rec; |
networker | 1:70ee392bcfd4 | 433 | pos = parse(contBuf+pos, len, tree, serv); |
networker | 0:7493bf6bb1b9 | 434 | unsigned key = (*serv)[0]->asUnsigned(); |
networker | 0:7493bf6bb1b9 | 435 | services[key] = serv; |
networker | 0:7493bf6bb1b9 | 436 | } |
networker | 1:70ee392bcfd4 | 437 | freeBuf(); |
networker | 0:7493bf6bb1b9 | 438 | printf("...parsing done, pos=%d\n", pos); |
networker | 0:7493bf6bb1b9 | 439 | if (ServiceSearchAttributeResponse) |
networker | 0:7493bf6bb1b9 | 440 | ServiceSearchAttributeResponse(); |
networker | 0:7493bf6bb1b9 | 441 | } |
networker | 0:7493bf6bb1b9 | 442 | break; |
networker | 0:7493bf6bb1b9 | 443 | default: |
networker | 0:7493bf6bb1b9 | 444 | printf("Unknown SDP response type %02X\n", rsp[0]); |
networker | 1:70ee392bcfd4 | 445 | break; |
networker | 0:7493bf6bb1b9 | 446 | } |
networker | 0:7493bf6bb1b9 | 447 | return 0; |
networker | 0:7493bf6bb1b9 | 448 | } |