Ad van der Weiden / sdp

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?

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 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 }