Ad van der Weiden / sdp

SDP client for myBlueUSB

Dependents:   mbed_TANK_Kinect ftusbClass

sdp.cpp@2:d5a27b2d2e08, 2011-04-07 (annotated)

Committer:
networker
Date:
Thu Apr 07 21:01:39 2011 +0000
Revision:
2:d5a27b2d2e08
Parent:
1:70ee392bcfd4
Child:
3:e8d2ebb7392e
fixed some memory problems; able to read my iPhone services

Who changed what in which revision?

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