Ad van der Weiden / sdp

SDP client for myBlueUSB

Dependents:   mbed_TANK_Kinect ftusbClass

sdp.cpp@3:e8d2ebb7392e, 2011-05-04 (annotated)

Committer:
networker
Date:
Wed May 04 09:27:32 2011 +0000
Revision:
3:e8d2ebb7392e
Parent:
2:d5a27b2d2e08
Child:
4:d5c3e499603d
SDP server partially implemented. Provides SPP compatible with Windows BT stack.

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 3:e8d2ebb7392e 9 map<unsigned, serv_rec*> SDPManager::server;
networker 3:e8d2ebb7392e 10 int SDPManager::serverSock = 0;
networker 0:7493bf6bb1b9 11
networker 0:7493bf6bb1b9 12 void attribHandler(serv_rec *r) {
networker 0:7493bf6bb1b9 13 printf("Service 0x%08X\n", (*r)[0x0000]->asUnsigned());
networker 0:7493bf6bb1b9 14 map<unsigned short, sdp_data*>::iterator it = r->begin();
networker 0:7493bf6bb1b9 15 for (;it != r->end();it++) {
networker 0:7493bf6bb1b9 16 printf(" 0x%04X: %s\n", (*it).first, (*it).second->asString());
networker 0:7493bf6bb1b9 17 }
networker 0:7493bf6bb1b9 18 }
networker 0:7493bf6bb1b9 19
networker 1:70ee392bcfd4 20 #define BROWSEROOT 0x1002
networker 1:70ee392bcfd4 21 #define SERIALSERV 0x1101
networker 1:70ee392bcfd4 22
networker 3:e8d2ebb7392e 23 SDPHandler::SDPHandler(): txid(1), tree(0) {
networker 3:e8d2ebb7392e 24 ErrorResponse=errorhandler;
networker 3:e8d2ebb7392e 25 ServiceSearchResponse=0;
networker 3:e8d2ebb7392e 26 ServiceAttributeResponse=attribHandler;
networker 3:e8d2ebb7392e 27 ServiceSearchAttributeResponse=0;
networker 3:e8d2ebb7392e 28 buf = l2cap_buf+OFFSET;
networker 3:e8d2ebb7392e 29 contBuf = 0;
networker 3:e8d2ebb7392e 30 byteCount = 0;
networker 3:e8d2ebb7392e 31 contState[0] = 0;
networker 3:e8d2ebb7392e 32 _state = 0;
networker 3:e8d2ebb7392e 33 }
networker 3:e8d2ebb7392e 34
networker 3:e8d2ebb7392e 35 void SDPHandler::Clear() {
networker 3:e8d2ebb7392e 36 for (index = services.begin(); index != services.end(); index++) {//for all services
networker 3:e8d2ebb7392e 37 for (serv_rec::iterator it = index->second->begin(); it != index->second->end(); it++)//for all attributes
networker 3:e8d2ebb7392e 38 delete it->second; //delete the attribute value tree
networker 3:e8d2ebb7392e 39 delete (*index).second; //delete the attribute list
networker 3:e8d2ebb7392e 40 }
networker 3:e8d2ebb7392e 41 services.clear();//and empty the services list
networker 3:e8d2ebb7392e 42 }
networker 3:e8d2ebb7392e 43
networker 3:e8d2ebb7392e 44 //Called as: Socket_Open(SOCKET_SDP, addr, callback, userdata(this)) from SDPManager
networker 3:e8d2ebb7392e 45 //never called
networker 3:e8d2ebb7392e 46 int SDPHandler::Open(SocketInternal* sock, SocketAddrHdr* addr) {
networker 3:e8d2ebb7392e 47 printf("Successfully opened SDP socket %d\n", sock->ID);
networker 3:e8d2ebb7392e 48 sock->SetState(SocketState_Open);
networker 3:e8d2ebb7392e 49 sdp_socket = sock->ID;
networker 3:e8d2ebb7392e 50 return sdp_socket;
networker 3:e8d2ebb7392e 51 }
networker 3:e8d2ebb7392e 52
networker 3:e8d2ebb7392e 53 int SDPHandler::Send(SocketInternal* sock, const u8* data, int len) {
networker 3:e8d2ebb7392e 54 printf("SDPHandler::Send should not be called directly\n");
networker 3:e8d2ebb7392e 55 // return Socket_Send(_l2cap, data, len);
networker 3:e8d2ebb7392e 56 BTDevice *l2cap = (BTDevice*)sock->userData;
networker 3:e8d2ebb7392e 57 return l2cap->Send(sock, data, len);
networker 3:e8d2ebb7392e 58 }
networker 3:e8d2ebb7392e 59
networker 3:e8d2ebb7392e 60 int SDPHandler::Close(SocketInternal* sock) {
networker 3:e8d2ebb7392e 61 printf("SDPHandler::Close(%d)\n", sock->ID);
networker 3:e8d2ebb7392e 62 Clear();
networker 3:e8d2ebb7392e 63 // printf("SDP socket %d and L2CAP socket %d closed, freemem=%d\n", sock->ID, _l2cap, AvailableMemory(1));
networker 3:e8d2ebb7392e 64 int retval = 0;//Socket_Close(_l2cap);//could also keep it open for the next connection
networker 3:e8d2ebb7392e 65 return retval;
networker 3:e8d2ebb7392e 66 }
networker 3:e8d2ebb7392e 67
networker 3:e8d2ebb7392e 68 //this function is called when the L2CAP layer receives SDP packets (see SDPHandler::Open), userdata is the sdpmanager instance
networker 3:e8d2ebb7392e 69 void SDPHandler::OnSdpRsp(int socket, SocketState state, const u8* data, int len, void* userData) {
networker 3:e8d2ebb7392e 70 printf("\x1B[%dm", 35);
networker 3:e8d2ebb7392e 71 // printf("OnSdpRsp(socket=%d, state=%d, len=%d)\n", socket, state, len);
networker 3:e8d2ebb7392e 72 printf("SDPHandler::OnSdpRsp(socket=%d, state=%d, len=%d, freemem=%d\n", socket, state, len, AvailableMemory(1));
networker 3:e8d2ebb7392e 73 SDPHandler *self = (SDPHandler*)userData;
networker 3:e8d2ebb7392e 74 if (state == SocketState_Open) {
networker 3:e8d2ebb7392e 75 self->sdp_socket = socket;
networker 3:e8d2ebb7392e 76 self->OnSdpRsp(data, len);
networker 3:e8d2ebb7392e 77 } else if (state == SocketState_Closed) {
networker 3:e8d2ebb7392e 78 SDP.Destroy(socket);
networker 3:e8d2ebb7392e 79 }
networker 3:e8d2ebb7392e 80 printf("\x1B[%dm", 0);
networker 3:e8d2ebb7392e 81 }
networker 3:e8d2ebb7392e 82
networker 3:e8d2ebb7392e 83 //this function is called when the L2CAP layer receives SDP packets (see SDPHandler::Open), userdata is the sdpmanager instance
networker 3:e8d2ebb7392e 84 //void SDPHandler::OnSdpRsp(int socket, SocketState state, const u8* data, int len, void* userData) {
networker 3:e8d2ebb7392e 85 void SDPHandler::OnSdpRsp(const u8* data, int len) {
networker 3:e8d2ebb7392e 86 static sdp_data list(sdp_data::SEQUENCE);
networker 1:70ee392bcfd4 87 static sdp_data all(0x0000ffffU,4);
networker 2:d5a27b2d2e08 88 static sdp_data serviceID(0U, 2);
networker 2:d5a27b2d2e08 89 static sdp_data name(0x100U, 2);
networker 1:70ee392bcfd4 90 static sdp_data root(sdp_data::UUID, BROWSEROOT);
networker 1:70ee392bcfd4 91 static sdp_data req(sdp_data::SEQUENCE);
networker 1:70ee392bcfd4 92 static bool once = true;
networker 3:e8d2ebb7392e 93 // printf("_state=%d first=%d ", _state, once);
networker 1:70ee392bcfd4 94 if (once) {
networker 3:e8d2ebb7392e 95 list.add_element(&all);
networker 3:e8d2ebb7392e 96 //list.add_element(&serviceID);
networker 3:e8d2ebb7392e 97 //list.add_element(&name);
networker 3:e8d2ebb7392e 98 req.add_element(&root);
networker 3:e8d2ebb7392e 99 once = false;
networker 0:7493bf6bb1b9 100 }
networker 3:e8d2ebb7392e 101 if (data) {
networker 1:70ee392bcfd4 102 parseRsp(data, len);
networker 3:e8d2ebb7392e 103 }
networker 1:70ee392bcfd4 104 switch (_state) {
networker 1:70ee392bcfd4 105 case 0: //closed
networker 1:70ee392bcfd4 106 if (len==0) { //socket just opened
networker 1:70ee392bcfd4 107 //'Open' cleared the services list
networker 2:d5a27b2d2e08 108 ServiceSearchRequest(&req, 10);
networker 1:70ee392bcfd4 109 _state = 1; //wait for service handles
networker 1:70ee392bcfd4 110 }
networker 1:70ee392bcfd4 111 break;
networker 1:70ee392bcfd4 112 case 1: //service handles arriving
networker 1:70ee392bcfd4 113 if (contState[0]) {//continuation, repeat request
networker 1:70ee392bcfd4 114 ServiceSearchRequest(&req, 5);
networker 1:70ee392bcfd4 115 } else {
networker 1:70ee392bcfd4 116 if (data[0]==3) {
networker 1:70ee392bcfd4 117 index = services.begin();
networker 1:70ee392bcfd4 118 if (index != services.end()) {
networker 1:70ee392bcfd4 119 unsigned handle = (*index).first;
networker 2:d5a27b2d2e08 120 //printf("req.: handle %#X\n", handle);
networker 1:70ee392bcfd4 121 ServiceAttributeRequest(handle, 100, &list);//0x1001D
networker 3:e8d2ebb7392e 122 } else
networker 1:70ee392bcfd4 123 printf(" - empty list - \n");//should not happen
networker 1:70ee392bcfd4 124 _state = 2; //wait for attribute response
networker 3:e8d2ebb7392e 125 } else
networker 1:70ee392bcfd4 126 printf("Expected a ServiceSearchResponse 0x03, got %#x\n", data[0]);
networker 1:70ee392bcfd4 127 }
networker 1:70ee392bcfd4 128 break;
networker 1:70ee392bcfd4 129 case 2:
networker 1:70ee392bcfd4 130 if (contState[0])//repeat request
networker 1:70ee392bcfd4 131 ServiceAttributeRequest((*index).first, 100, &list);
networker 1:70ee392bcfd4 132 else {
networker 1:70ee392bcfd4 133 if (data[0]==5) {
networker 1:70ee392bcfd4 134 index++; //move to next service
networker 1:70ee392bcfd4 135 if (index != services.end()) {
networker 2:d5a27b2d2e08 136 //printf("req.: handle %#X\n", (*index).first);
networker 1:70ee392bcfd4 137 ServiceAttributeRequest((*index).first, 100, &list);
networker 1:70ee392bcfd4 138 } else {
networker 1:70ee392bcfd4 139 printf(" - end of list - \n");
networker 1:70ee392bcfd4 140 Socket_Close(sdp_socket); //Note: socket=L2CAP, sdp_socket=SDP !!!
networker 1:70ee392bcfd4 141 _state = 0;
networker 1:70ee392bcfd4 142 }
networker 3:e8d2ebb7392e 143 } else
networker 1:70ee392bcfd4 144 printf("Expected a ServiceAttributeResponse 0x05, got %#x\n", data[0]);
networker 1:70ee392bcfd4 145 }
networker 1:70ee392bcfd4 146 break;
networker 1:70ee392bcfd4 147 }
networker 0:7493bf6bb1b9 148 }
networker 0:7493bf6bb1b9 149
networker 0:7493bf6bb1b9 150 //this function is called when the SDP sockets receives data (see HCICallback in TestShell),
networker 0:7493bf6bb1b9 151 //currently does not happen because not forwarded from OnSdpRsp, can be used to handle multiple connections
networker 3:e8d2ebb7392e 152 void SDPHandler::OnSockCallback(int socket, SocketState state, const u8* data, int len, void* userData) {
networker 3:e8d2ebb7392e 153 printf("SDPHandler::OnSockCallback(socket=%d, state=%d, len=%d)\n", socket, state, len);
networker 0:7493bf6bb1b9 154 printfBytes("Got SDP Response from socket: ", data, len);
networker 0:7493bf6bb1b9 155 }
networker 0:7493bf6bb1b9 156
networker 3:e8d2ebb7392e 157 //void SDPHandler::errorhandler(unsigned err) {//default error handler
networker 3:e8d2ebb7392e 158 void errorhandler(unsigned err) {//default error handler
networker 0:7493bf6bb1b9 159 switch (err) {
networker 0:7493bf6bb1b9 160 case 1:
networker 0:7493bf6bb1b9 161 printf("Unsupported version of SDP\n");
networker 0:7493bf6bb1b9 162 break;
networker 0:7493bf6bb1b9 163 case 2:
networker 0:7493bf6bb1b9 164 printf("Invalid SDP ServiceRecordHandle\n");
networker 0:7493bf6bb1b9 165 break;
networker 0:7493bf6bb1b9 166 case 3:
networker 0:7493bf6bb1b9 167 printf("SDP syntax error\n");
networker 0:7493bf6bb1b9 168 break;
networker 0:7493bf6bb1b9 169 case 4:
networker 0:7493bf6bb1b9 170 printf("PDU size was invalid\n");
networker 0:7493bf6bb1b9 171 break;
networker 0:7493bf6bb1b9 172 case 5:
networker 0:7493bf6bb1b9 173 printf("Continuation state was invalid\n");
networker 0:7493bf6bb1b9 174 break;
networker 0:7493bf6bb1b9 175 case 6:
networker 0:7493bf6bb1b9 176 printf("SDP server has insufficient resources\n");
networker 0:7493bf6bb1b9 177 break;
networker 0:7493bf6bb1b9 178 default:
networker 0:7493bf6bb1b9 179 printf("Unknown SDP error code\n");
networker 0:7493bf6bb1b9 180 break;
networker 0:7493bf6bb1b9 181 }
networker 0:7493bf6bb1b9 182 }
networker 0:7493bf6bb1b9 183
networker 3:e8d2ebb7392e 184 int SDPHandler::ServiceSearchRequest(sdp_data *sp, unsigned count, unsigned cs) {
networker 1:70ee392bcfd4 185 int parlen = sp->Size() + contState[0] + 3;
networker 0:7493bf6bb1b9 186 buf[0] = 2; //pdu
networker 0:7493bf6bb1b9 187 buf[1] = txid>>8;
networker 0:7493bf6bb1b9 188 buf[2] = txid++;
networker 0:7493bf6bb1b9 189 buf[4] = parlen;
networker 0:7493bf6bb1b9 190 buf[3] = parlen>>8;
networker 0:7493bf6bb1b9 191 int p = sp->build(buf+5, 100-10);
networker 0:7493bf6bb1b9 192 buf[p+6] = count;
networker 0:7493bf6bb1b9 193 buf[p+5] = count>>8;
networker 1:70ee392bcfd4 194 buf[p+7] = contState[0];
networker 1:70ee392bcfd4 195 for (int j = 1; j <= contState[0]; j++)
networker 1:70ee392bcfd4 196 buf[p+j+7] = contState[j];
networker 0:7493bf6bb1b9 197 //printfBytes("SDP Send: ", buf, parlen+5);
networker 3:e8d2ebb7392e 198 return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET);
networker 0:7493bf6bb1b9 199 }
networker 0:7493bf6bb1b9 200
networker 3:e8d2ebb7392e 201 int SDPHandler::ServiceAttributeRequest(unsigned handle, unsigned count, sdp_data* al, unsigned cs) {
networker 1:70ee392bcfd4 202 int parlen = al->Size() + contState[0] + 7;
networker 0:7493bf6bb1b9 203 buf[0] = 4; //pdu
networker 0:7493bf6bb1b9 204 buf[1] = txid>>8;
networker 0:7493bf6bb1b9 205 buf[2] = txid++;
networker 0:7493bf6bb1b9 206 buf[4] = parlen;
networker 0:7493bf6bb1b9 207 buf[3] = parlen>>8;
networker 0:7493bf6bb1b9 208 for (int i = 0; i < 4; i++)
networker 0:7493bf6bb1b9 209 buf[i+5] = ((char*)&handle)[3-i];
networker 0:7493bf6bb1b9 210 buf[9] = count>>8;
networker 0:7493bf6bb1b9 211 buf[10] = count;
networker 0:7493bf6bb1b9 212 int p = al->build(buf+11, 100-26);
networker 1:70ee392bcfd4 213 buf[p+11] = contState[0];
networker 1:70ee392bcfd4 214 for (int j = 1; j <= contState[0]; j++)
networker 1:70ee392bcfd4 215 buf[p+j+11] = contState[j];
networker 0:7493bf6bb1b9 216 //printfBytes("SDP Send: ", buf, parlen+5);
networker 3:e8d2ebb7392e 217 return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET);
networker 0:7493bf6bb1b9 218 }
networker 0:7493bf6bb1b9 219
networker 3:e8d2ebb7392e 220 int SDPHandler::ServiceSearchAttributeRequest(sdp_data *sp, unsigned count, sdp_data* al, unsigned cs) {
networker 1:70ee392bcfd4 221 int parlen = sp->Size() + al->Size() + contState[0] + 3; // count (2 bytes) + at least 1 cont
networker 0:7493bf6bb1b9 222 buf[0] = 6; //pdu
networker 0:7493bf6bb1b9 223 buf[1] = txid>>8;
networker 0:7493bf6bb1b9 224 buf[2] = txid++;
networker 0:7493bf6bb1b9 225 buf[4] = parlen;
networker 0:7493bf6bb1b9 226 buf[3] = parlen>>8;
networker 0:7493bf6bb1b9 227 int p = sp->build(buf+5, 30);
networker 0:7493bf6bb1b9 228 buf[p+6] = count;
networker 0:7493bf6bb1b9 229 buf[p+5] = count>>8;
networker 0:7493bf6bb1b9 230 p += al->build(buf+11, 100-38);
networker 1:70ee392bcfd4 231 buf[p+7] = contState[0];
networker 1:70ee392bcfd4 232 for (int j = 1; j <= contState[0]; j++)
networker 1:70ee392bcfd4 233 buf[p+j+7] = contState[j];
networker 0:7493bf6bb1b9 234 //printfBytes("SDP Send: ", buf, parlen+5);
networker 3:e8d2ebb7392e 235 return Socket_Send(sdp_socket, l2cap_buf, parlen + 5 + OFFSET);
networker 0:7493bf6bb1b9 236 }
networker 0:7493bf6bb1b9 237
networker 3:e8d2ebb7392e 238 int SDPManager::ServiceSearchAttributeReply(unsigned rxid, sdp_data* al, unsigned count, unsigned cs) {
networker 3:e8d2ebb7392e 239 unsigned size = al->Size();
networker 3:e8d2ebb7392e 240 unsigned cont = 0;//outgoing continuation
networker 3:e8d2ebb7392e 241 unsigned char *_buf = new unsigned char[OFFSET+5+2+size+3];
networker 3:e8d2ebb7392e 242 unsigned char *buf = _buf+OFFSET;
networker 3:e8d2ebb7392e 243 unsigned byteCount = size - cs; //remainder of data to send
networker 3:e8d2ebb7392e 244 unsigned parlen = 2 + byteCount + 1 ;// attributelistbytecount+payload+no_continuation
networker 3:e8d2ebb7392e 245 if (byteCount > count) {//still too large, need continuation
networker 3:e8d2ebb7392e 246 byteCount = count; //limit at maximum
networker 3:e8d2ebb7392e 247 cont = cs + count; //start for next iteration
networker 3:e8d2ebb7392e 248 parlen = 2 + byteCount + 3; //adjusted for payload and continuation
networker 3:e8d2ebb7392e 249 printf("Need continuation, sending bytes [%d, %d> of attributeList\n", cs, cont);
networker 3:e8d2ebb7392e 250 } else {
networker 3:e8d2ebb7392e 251 // printf("Final or only block, sending bytes [%d, %d> of attributeList\n", cs, cs+byteCount);
networker 3:e8d2ebb7392e 252 }
networker 3:e8d2ebb7392e 253 buf[0] = 7; //pdu
networker 3:e8d2ebb7392e 254 buf[1] = rxid>>8;
networker 3:e8d2ebb7392e 255 buf[2] = rxid;
networker 3:e8d2ebb7392e 256 buf[3] = parlen>>8;
networker 3:e8d2ebb7392e 257 buf[4] = parlen;
networker 3:e8d2ebb7392e 258 buf[5] = byteCount>>8;
networker 3:e8d2ebb7392e 259 buf[6] = byteCount;
networker 3:e8d2ebb7392e 260 int p = al->build(buf+7, size);//limited only by buffersize
networker 3:e8d2ebb7392e 261 //printf("'build' added %d bytes to the buffer\n", p);
networker 3:e8d2ebb7392e 262 if (cs == 0) { //this is not a continuation
networker 3:e8d2ebb7392e 263 buf[byteCount+7] = 0;
networker 3:e8d2ebb7392e 264 } else { //this is a continuation
networker 3:e8d2ebb7392e 265 memcpy(buf+7, buf+7+cs, byteCount);//move part of interrest to beginning of buffer
networker 3:e8d2ebb7392e 266 buf[byteCount+7] = 2;
networker 3:e8d2ebb7392e 267 buf[byteCount+8] = cont>>8;
networker 3:e8d2ebb7392e 268 buf[byteCount+9] = cont;
networker 3:e8d2ebb7392e 269 }
networker 3:e8d2ebb7392e 270 //printfBytes("SDP Send: ", buf, parlen+5);
networker 3:e8d2ebb7392e 271 int retval = Socket_Send(serverSock, _buf, parlen + 5 + OFFSET);
networker 3:e8d2ebb7392e 272 delete[] _buf;
networker 3:e8d2ebb7392e 273 return retval;
networker 3:e8d2ebb7392e 274 }
networker 3:e8d2ebb7392e 275
networker 3:e8d2ebb7392e 276 //unsigned SDPHandler::getval(const unsigned char *p, int n) {
networker 3:e8d2ebb7392e 277 unsigned getval(const unsigned char *p, int n) {
networker 0:7493bf6bb1b9 278 unsigned ret = 0;
networker 0:7493bf6bb1b9 279 for (int i = 0; i < n; i++)
networker 0:7493bf6bb1b9 280 ret = (ret<<8) + (unsigned)p[i];
networker 0:7493bf6bb1b9 281 return ret;
networker 0:7493bf6bb1b9 282 }
networker 0:7493bf6bb1b9 283
networker 3:e8d2ebb7392e 284 //unsigned SDPHandler::length(const unsigned char *el, unsigned &p) {
networker 3:e8d2ebb7392e 285 unsigned length(const unsigned char *el, unsigned &p) {
networker 0:7493bf6bb1b9 286 unsigned len = 0;
networker 0:7493bf6bb1b9 287 switch (el[p++] & 7) {//length
networker 0:7493bf6bb1b9 288 case 0:
networker 0:7493bf6bb1b9 289 len = 1;
networker 0:7493bf6bb1b9 290 break;
networker 0:7493bf6bb1b9 291 case 1:
networker 0:7493bf6bb1b9 292 len = 2;
networker 0:7493bf6bb1b9 293 break;
networker 0:7493bf6bb1b9 294 case 2:
networker 0:7493bf6bb1b9 295 len = 4;
networker 0:7493bf6bb1b9 296 break;
networker 0:7493bf6bb1b9 297 case 3:
networker 0:7493bf6bb1b9 298 len = 8;
networker 0:7493bf6bb1b9 299 break;
networker 0:7493bf6bb1b9 300 case 4:
networker 0:7493bf6bb1b9 301 len = 16;
networker 0:7493bf6bb1b9 302 break;
networker 0:7493bf6bb1b9 303 case 7://4bytes
networker 0:7493bf6bb1b9 304 len= el[p++]<<24;
networker 0:7493bf6bb1b9 305 len += el[p++]<<16;
networker 0:7493bf6bb1b9 306 case 6://2bytes
networker 0:7493bf6bb1b9 307 len += el[p++]<<8;
networker 0:7493bf6bb1b9 308 case 5://1byte
networker 0:7493bf6bb1b9 309 len += el[p++];
networker 0:7493bf6bb1b9 310 break;
networker 0:7493bf6bb1b9 311 }
networker 0:7493bf6bb1b9 312 return len;
networker 0:7493bf6bb1b9 313 }
networker 0:7493bf6bb1b9 314
networker 1:70ee392bcfd4 315 extern "C" void HardFault_Handler() {
networker 1:70ee392bcfd4 316 printf("Hard Fault! %d bytes left\n", AvailableMemory(1));
networker 1:70ee392bcfd4 317 while (1);
networker 1:70ee392bcfd4 318 }
networker 0:7493bf6bb1b9 319
networker 3:e8d2ebb7392e 320 unsigned SDPHandler::parseLight (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) {
networker 1:70ee392bcfd4 321 unsigned p = 0;
networker 1:70ee392bcfd4 322 unsigned len = length(el, p);
networker 1:70ee392bcfd4 323 int end = p+len;//end is the index of the item just after the sequence
networker 1:70ee392bcfd4 324 sdp_data *item = 0;
networker 1:70ee392bcfd4 325 switch (el[0]>>3) {//type
networker 1:70ee392bcfd4 326 case sdp_data::NULL_:
networker 1:70ee392bcfd4 327 printf("NULL ");
networker 1:70ee392bcfd4 328 break;
networker 1:70ee392bcfd4 329 case sdp_data::UNSIGNED:
networker 1:70ee392bcfd4 330 printf("UINT%d=%u ", len, (unsigned)getval(el+p, len));
networker 1:70ee392bcfd4 331 break;
networker 1:70ee392bcfd4 332 case sdp_data::SIGNED:
networker 1:70ee392bcfd4 333 printf("INT%d=%d ", len, (unsigned)getval(el+p, len));
networker 1:70ee392bcfd4 334 break;
networker 1:70ee392bcfd4 335 case sdp_data::UUID:
networker 1:70ee392bcfd4 336 if (len==16) {
networker 1:70ee392bcfd4 337 printf("UUID16= ");
networker 1:70ee392bcfd4 338 for (int i = 0; i < 16; i++)
networker 1:70ee392bcfd4 339 printf("%02x ", el[p+i]);
networker 1:70ee392bcfd4 340 } else
networker 1:70ee392bcfd4 341 printf("UUID%d=%u ", len, (unsigned)getval(el+p, len));
networker 1:70ee392bcfd4 342 break;
networker 1:70ee392bcfd4 343 case sdp_data::STRING:
networker 1:70ee392bcfd4 344 printf("STR%d='%s' ", len, (char*)el+p);
networker 1:70ee392bcfd4 345 break;
networker 1:70ee392bcfd4 346 case sdp_data::BOOL:
networker 1:70ee392bcfd4 347 printf("BOOL%d=%d ", len, (unsigned)getval(el+p, len));
networker 1:70ee392bcfd4 348 break;
networker 1:70ee392bcfd4 349 case sdp_data::SEQUENCE:
networker 1:70ee392bcfd4 350 goto skip;
networker 1:70ee392bcfd4 351 case sdp_data::ALTERNATIVE:
networker 1:70ee392bcfd4 352 skip: {//p points just after the length indicator, hence at the first item IN the sequence
networker 3:e8d2ebb7392e 353 printf("SEQ%d{%p ", len, item);
networker 1:70ee392bcfd4 354 int n = 0;
networker 1:70ee392bcfd4 355 unsigned short key;
networker 1:70ee392bcfd4 356 serv_rec *dummy = 0;
networker 1:70ee392bcfd4 357 while (p < end) {
networker 1:70ee392bcfd4 358 sdp_data *elem = 0;
networker 1:70ee392bcfd4 359 p += parseLight(el + p, len-p, elem, dummy);//parse each element in the sequence, the second arg is as yet unused
networker 1:70ee392bcfd4 360 if (record) {
networker 1:70ee392bcfd4 361 if (n & 1) { //value
networker 1:70ee392bcfd4 362 record->insert(pair<unsigned short, sdp_data*>(key, elem));
networker 1:70ee392bcfd4 363 } else //key
networker 1:70ee392bcfd4 364 key = n;
networker 1:70ee392bcfd4 365 n++;
networker 1:70ee392bcfd4 366 }
networker 1:70ee392bcfd4 367 }
networker 1:70ee392bcfd4 368 }
networker 1:70ee392bcfd4 369 printf("}\n");
networker 1:70ee392bcfd4 370 break;
networker 1:70ee392bcfd4 371 case 8:
networker 1:70ee392bcfd4 372 printf("URL%d='%s' ", len, (char*)el+p);
networker 1:70ee392bcfd4 373 break;
networker 1:70ee392bcfd4 374 default:
networker 1:70ee392bcfd4 375 printf("Parse: Unknown type %d, len=%d (code=%#02X)\n", el[0]>>3, len, el[0]);
networker 1:70ee392bcfd4 376 }
networker 1:70ee392bcfd4 377 result = item;
networker 1:70ee392bcfd4 378 return end;
networker 1:70ee392bcfd4 379 }
networker 0:7493bf6bb1b9 380
networker 3:e8d2ebb7392e 381 unsigned SDPHandler::parse (const unsigned char *el, unsigned count, sdp_data* &result, serv_rec* &record) {
networker 0:7493bf6bb1b9 382 unsigned p = 0;
networker 0:7493bf6bb1b9 383 unsigned len = length(el, p);
networker 0:7493bf6bb1b9 384 int end = p+len;//end is the index of the item just after the sequence
networker 0:7493bf6bb1b9 385 sdp_data *item = 0;
networker 0:7493bf6bb1b9 386 switch (el[0]>>3) {//type
networker 0:7493bf6bb1b9 387 case sdp_data::NULL_:
networker 0:7493bf6bb1b9 388 item = new sdp_data();
networker 0:7493bf6bb1b9 389 break;
networker 0:7493bf6bb1b9 390 case sdp_data::UNSIGNED:
networker 0:7493bf6bb1b9 391 item = new sdp_data((unsigned)getval(el+p, len), len);
networker 0:7493bf6bb1b9 392 break;
networker 0:7493bf6bb1b9 393 case sdp_data::SIGNED:
networker 0:7493bf6bb1b9 394 item = new sdp_data((int)getval(el+p, len), len);
networker 0:7493bf6bb1b9 395 break;
networker 0:7493bf6bb1b9 396 case sdp_data::UUID:
networker 0:7493bf6bb1b9 397 if (len==16) {
networker 0:7493bf6bb1b9 398 char rev[16];
networker 0:7493bf6bb1b9 399 for (int i = 0; i < 16; i++)
networker 0:7493bf6bb1b9 400 rev[i] = el[p+15-i];
networker 0:7493bf6bb1b9 401 item = new sdp_data(sdp_data::UUID, rev, len);
networker 0:7493bf6bb1b9 402 } else
networker 0:7493bf6bb1b9 403 item = new sdp_data(sdp_data::UUID, getval(el+p, len), len);
networker 0:7493bf6bb1b9 404 break;
networker 0:7493bf6bb1b9 405 case sdp_data::STRING:
networker 0:7493bf6bb1b9 406 item = new sdp_data((char*)el+p, len);
networker 0:7493bf6bb1b9 407 break;
networker 0:7493bf6bb1b9 408 case sdp_data::BOOL:
networker 0:7493bf6bb1b9 409 item = new sdp_data((bool)getval(el+p, len), len);
networker 0:7493bf6bb1b9 410 break;
networker 0:7493bf6bb1b9 411 case sdp_data::SEQUENCE:
networker 0:7493bf6bb1b9 412 item = new sdp_data(sdp_data::SEQUENCE);
networker 0:7493bf6bb1b9 413 goto skip;
networker 0:7493bf6bb1b9 414 case sdp_data::ALTERNATIVE:
networker 0:7493bf6bb1b9 415 item = new sdp_data(sdp_data::ALTERNATIVE);
networker 0:7493bf6bb1b9 416 skip: {//p points just after the length indicator, hence at the first item IN the sequence
networker 2:d5a27b2d2e08 417 //printf("SEQ%d{%p ", len, item);
networker 0:7493bf6bb1b9 418 int n = 0;
networker 0:7493bf6bb1b9 419 unsigned short key;
networker 2:d5a27b2d2e08 420 serv_rec *dummy = 0;//means: there is no service record to fill in for deeper levels
networker 0:7493bf6bb1b9 421 while (p < end) {
networker 2:d5a27b2d2e08 422 sdp_data *elem = 0; //this becomes the tree with attribute values
networker 0:7493bf6bb1b9 423 p += parse(el + p, len-p, elem, dummy);//parse each element in the sequence, the second arg is as yet unused
networker 2:d5a27b2d2e08 424 if (record) { //if at the level of attribute list, add elem to record as key/value pair
networker 0:7493bf6bb1b9 425 if (n & 1) { //value
networker 0:7493bf6bb1b9 426 record->insert(pair<unsigned short, sdp_data*>(key, elem));
networker 0:7493bf6bb1b9 427 } else //key
networker 0:7493bf6bb1b9 428 key = elem->asUnsigned();
networker 0:7493bf6bb1b9 429 n++;
networker 2:d5a27b2d2e08 430 } else //just add the elements to the value tree
networker 2:d5a27b2d2e08 431 item->add_element(elem);
networker 0:7493bf6bb1b9 432 }
networker 0:7493bf6bb1b9 433 }
networker 1:70ee392bcfd4 434 //printf("}\n");
networker 0:7493bf6bb1b9 435 break;
networker 0:7493bf6bb1b9 436 case 8:
networker 0:7493bf6bb1b9 437 item = new sdp_data(sdp_data::URL, (char*)el+p, len);
networker 0:7493bf6bb1b9 438 break;
networker 0:7493bf6bb1b9 439 default:
networker 0:7493bf6bb1b9 440 printf("Parse: Unknown type %d, len=%d (code=%#02X)\n", el[0]>>3, len, el[0]);
networker 0:7493bf6bb1b9 441 }
networker 0:7493bf6bb1b9 442 result = item;
networker 0:7493bf6bb1b9 443 return end;
networker 0:7493bf6bb1b9 444 }
networker 0:7493bf6bb1b9 445
networker 3:e8d2ebb7392e 446 void SDPHandler::append(const unsigned char *payload, int len) {
networker 1:70ee392bcfd4 447 unsigned char *tmp = new unsigned char[byteCount + len];//append the payload to the previous continuation buffer
networker 1:70ee392bcfd4 448 if (contBuf && byteCount) {
networker 3:e8d2ebb7392e 449 memcpy(tmp, contBuf, byteCount); //copy the existing part
networker 3:e8d2ebb7392e 450 delete[] contBuf;//delete the old buffer
networker 1:70ee392bcfd4 451 }
networker 1:70ee392bcfd4 452 memcpy(tmp+byteCount, payload, len); //append the new part
networker 1:70ee392bcfd4 453 contBuf = tmp;
networker 1:70ee392bcfd4 454 byteCount += len;
networker 1:70ee392bcfd4 455 }
networker 1:70ee392bcfd4 456
networker 3:e8d2ebb7392e 457 void SDPHandler::freeBuf() {
networker 1:70ee392bcfd4 458 if (contBuf) {
networker 3:e8d2ebb7392e 459 delete[] contBuf;
networker 3:e8d2ebb7392e 460 contBuf = 0;
networker 1:70ee392bcfd4 461 }
networker 1:70ee392bcfd4 462 byteCount = 0;
networker 1:70ee392bcfd4 463 }
networker 1:70ee392bcfd4 464
networker 2:d5a27b2d2e08 465
networker 2:d5a27b2d2e08 466 //TODO: test case 7, add server support (cases 2, 4, 6)
networker 2:d5a27b2d2e08 467 //3 cases: cont==0 && contBuf==0 -> use rsp; cont!=0 -> append; cont==0 && contBuf!=0 -> append and use contBuf
networker 3:e8d2ebb7392e 468 int SDPHandler::parseRsp(const unsigned char*rsp, int len) {
networker 3:e8d2ebb7392e 469 //unsigned tid = rsp[2] + ((unsigned)rsp[1]<<8);
networker 0:7493bf6bb1b9 470 unsigned parlen = rsp[4] + ((unsigned)rsp[3]<<8);
networker 2:d5a27b2d2e08 471 //printf("ParseRsp: tid=%04X, parlen=%d ", tid, parlen);
networker 0:7493bf6bb1b9 472 unsigned cont = 0;
networker 0:7493bf6bb1b9 473 switch (rsp[0]) {
networker 0:7493bf6bb1b9 474 case 1: {//errorRsp
networker 0:7493bf6bb1b9 475 unsigned errorcode = rsp[6] + ((unsigned)rsp[5]<<8);
networker 0:7493bf6bb1b9 476 if (parlen > 2) {
networker 0:7493bf6bb1b9 477 printf("ErrorInfo (%d bytes) for error %d is available\n", parlen-2, errorcode);
networker 0:7493bf6bb1b9 478 }
networker 0:7493bf6bb1b9 479 if (ErrorResponse)
networker 0:7493bf6bb1b9 480 ErrorResponse(errorcode);
networker 0:7493bf6bb1b9 481 return errorcode;
networker 0:7493bf6bb1b9 482 }
networker 0:7493bf6bb1b9 483 //break;
networker 0:7493bf6bb1b9 484 case 3: { //servicesearchRsp
networker 1:70ee392bcfd4 485 unsigned total = rsp[6] + ((unsigned)rsp[5]<<8);
networker 0:7493bf6bb1b9 486 unsigned current = rsp[8] + ((unsigned)rsp[7]<<8);
networker 0:7493bf6bb1b9 487 cont = rsp[9+4*current];
networker 1:70ee392bcfd4 488 memcpy(contState, &rsp[9+4*current], cont+1);//copy the continuation state
networker 3:e8d2ebb7392e 489 //printf("total=%d, current=%d, cont=%d\n", total, current, cont);
networker 1:70ee392bcfd4 490 if (cont) {
networker 1:70ee392bcfd4 491 //no special handling here, just append the servicerecordhandles
networker 1:70ee392bcfd4 492 }
networker 0:7493bf6bb1b9 493 //linear list of 32bit service-handles
networker 0:7493bf6bb1b9 494 for (int i = 0; i < current; i++) {
networker 0:7493bf6bb1b9 495 unsigned result = 0;
networker 0:7493bf6bb1b9 496 for (int j = 0; j< 4; j++)
networker 0:7493bf6bb1b9 497 result = (result<<8) + rsp[9 + 4*i + j];
networker 1:70ee392bcfd4 498 printf("SDP Search handle %08X\n", result);
networker 0:7493bf6bb1b9 499 services.insert(pair<unsigned, serv_rec*>(result, 0));
networker 0:7493bf6bb1b9 500 }
networker 0:7493bf6bb1b9 501 if (ServiceSearchResponse)
networker 0:7493bf6bb1b9 502 ServiceSearchResponse();
networker 0:7493bf6bb1b9 503 }
networker 0:7493bf6bb1b9 504 break;
networker 0:7493bf6bb1b9 505 case 5: { //serviceattributeRsp
networker 1:70ee392bcfd4 506 unsigned count = rsp[6] + ((unsigned)rsp[5]<<8);//bytes in this part of the attribute list
networker 2:d5a27b2d2e08 507 // append(rsp+7, count);
networker 0:7493bf6bb1b9 508 cont = rsp[7+count];
networker 1:70ee392bcfd4 509 memcpy(contState, &rsp[7+count], cont+1);//copy the continuation state
networker 2:d5a27b2d2e08 510 if (cont) {
networker 3:e8d2ebb7392e 511 append(rsp+7, count);
networker 3:e8d2ebb7392e 512 break;
networker 2:d5a27b2d2e08 513 }
networker 2:d5a27b2d2e08 514 //printf("count=%d parsing...\n", byteCount);
networker 0:7493bf6bb1b9 515 serv_rec *serv = new serv_rec;
networker 3:e8d2ebb7392e 516 if (contBuf) {
networker 3:e8d2ebb7392e 517 append(rsp+7, count);
networker 3:e8d2ebb7392e 518 parse(contBuf, byteCount, tree, serv);
networker 2:d5a27b2d2e08 519 } else
networker 3:e8d2ebb7392e 520 parse(rsp+7, count, tree, serv);
networker 2:d5a27b2d2e08 521 //printf("...parsing done, ");
networker 2:d5a27b2d2e08 522 //get the AttributeID, make sure attribId 0 is always included in the request
networker 2:d5a27b2d2e08 523 unsigned key = (*serv)[0]->asUnsigned();//AttributeID '0' always refers to the serviceID
networker 2:d5a27b2d2e08 524 //printf("Key=%#X\n", key); //key will be 0 when not requested
networker 2:d5a27b2d2e08 525 services[key] = serv; //Add the attribute list to the services
networker 1:70ee392bcfd4 526 freeBuf();
networker 0:7493bf6bb1b9 527 if (ServiceAttributeResponse)
networker 0:7493bf6bb1b9 528 ServiceAttributeResponse(serv);
networker 0:7493bf6bb1b9 529 }
networker 0:7493bf6bb1b9 530 break;
networker 1:70ee392bcfd4 531 //below is UNTESTED
networker 0:7493bf6bb1b9 532 case 7: { //servicesearchattributeRsp
networker 0:7493bf6bb1b9 533 unsigned count = rsp[6] + ((unsigned)rsp[5]<<8);
networker 1:70ee392bcfd4 534 append(rsp+7, count);
networker 0:7493bf6bb1b9 535 cont = rsp[7+count];
networker 1:70ee392bcfd4 536 memcpy(contState, &rsp[7+count], cont+1);
networker 0:7493bf6bb1b9 537 if (cont)
networker 0:7493bf6bb1b9 538 break;
networker 1:70ee392bcfd4 539 unsigned pos = 0;
networker 1:70ee392bcfd4 540 if (contBuf[pos]>>3 != sdp_data::SEQUENCE) {
networker 1:70ee392bcfd4 541 printf("Expected a sequence of attribute lists\n");
networker 1:70ee392bcfd4 542 break;
networker 1:70ee392bcfd4 543 }
networker 1:70ee392bcfd4 544 unsigned len = length(contBuf, pos);//get the length of the list of lists and advance pos to the first list
networker 1:70ee392bcfd4 545 while (pos<len) {
networker 0:7493bf6bb1b9 546 printf("pos=%d, count=%d, parsing...\n", pos, len);
networker 0:7493bf6bb1b9 547 serv_rec *serv = new serv_rec;
networker 1:70ee392bcfd4 548 pos = parse(contBuf+pos, len, tree, serv);
networker 0:7493bf6bb1b9 549 unsigned key = (*serv)[0]->asUnsigned();
networker 0:7493bf6bb1b9 550 services[key] = serv;
networker 0:7493bf6bb1b9 551 }
networker 1:70ee392bcfd4 552 freeBuf();
networker 0:7493bf6bb1b9 553 printf("...parsing done, pos=%d\n", pos);
networker 0:7493bf6bb1b9 554 if (ServiceSearchAttributeResponse)
networker 0:7493bf6bb1b9 555 ServiceSearchAttributeResponse();
networker 0:7493bf6bb1b9 556 }
networker 0:7493bf6bb1b9 557 break;
networker 0:7493bf6bb1b9 558 default:
networker 0:7493bf6bb1b9 559 printf("Unknown SDP response type %02X\n", rsp[0]);
networker 3:e8d2ebb7392e 560 break;
networker 0:7493bf6bb1b9 561 }
networker 0:7493bf6bb1b9 562 return 0;
networker 0:7493bf6bb1b9 563 }
networker 3:e8d2ebb7392e 564
networker 3:e8d2ebb7392e 565 //************************ SERVER related *******************************************//
networker 3:e8d2ebb7392e 566
networker 3:e8d2ebb7392e 567 //unsigned SDPHandler::parseUUID(const u8* data, int len, unsigned &p) {
networker 3:e8d2ebb7392e 568 unsigned parseUUID(const u8* data, int len, unsigned &p) {
networker 3:e8d2ebb7392e 569 unsigned u = 0;
networker 3:e8d2ebb7392e 570 if ((data[p]>>3) != sdp_data::UUID) {
networker 3:e8d2ebb7392e 571 printf(" UUID expected, got %d ", data[p]>>3);
networker 3:e8d2ebb7392e 572 return (unsigned)-1;
networker 3:e8d2ebb7392e 573 }
networker 3:e8d2ebb7392e 574 switch (data[p++] & 7) {
networker 3:e8d2ebb7392e 575 case 1:
networker 3:e8d2ebb7392e 576 u = getval(data+p, 2);
networker 3:e8d2ebb7392e 577 p +=2;
networker 3:e8d2ebb7392e 578 break;
networker 3:e8d2ebb7392e 579 case 2:
networker 3:e8d2ebb7392e 580 u = getval(data+p, 4);
networker 3:e8d2ebb7392e 581 p += 4;
networker 3:e8d2ebb7392e 582 break;
networker 3:e8d2ebb7392e 583 case 4:
networker 3:e8d2ebb7392e 584 u = getval(data+p, 4);
networker 3:e8d2ebb7392e 585 p += 16;
networker 3:e8d2ebb7392e 586 break;
networker 3:e8d2ebb7392e 587 default:
networker 3:e8d2ebb7392e 588 printf(" UUID must be 2, 4 or 16 bytes, got type %d\n", data[p-1]);
networker 3:e8d2ebb7392e 589 }
networker 3:e8d2ebb7392e 590 return u;
networker 3:e8d2ebb7392e 591 }
networker 3:e8d2ebb7392e 592
networker 3:e8d2ebb7392e 593 #define SVC_HANDLE 0x0001001DU //serial service
networker 3:e8d2ebb7392e 594 void SDPManager::buildServer() {
networker 3:e8d2ebb7392e 595 static sdp_data rfcomm(sdp_data::SEQUENCE);
networker 3:e8d2ebb7392e 596 static sdp_data l2cap(sdp_data::SEQUENCE);
networker 3:e8d2ebb7392e 597 static sdp_data protocol(sdp_data::SEQUENCE);
networker 3:e8d2ebb7392e 598 static sdp_data serviceclass(sdp_data::SEQUENCE);
networker 3:e8d2ebb7392e 599 static sdp_data browsegroup(sdp_data::SEQUENCE);
networker 3:e8d2ebb7392e 600 static sdp_data root(sdp_data::UUID, BROWSEROOT);
networker 3:e8d2ebb7392e 601 static sdp_data l2capuuid(sdp_data::UUID, 0x0100);
networker 3:e8d2ebb7392e 602 static sdp_data rfcommuuid(sdp_data::UUID, 0x003);
networker 3:e8d2ebb7392e 603 static sdp_data serial(sdp_data::UUID, 0x1101);
networker 3:e8d2ebb7392e 604 static sdp_data chan(1U,1);
networker 3:e8d2ebb7392e 605 static sdp_data handle(SVC_HANDLE,4);
networker 3:e8d2ebb7392e 606 static sdp_data serviceID(0U, 2);
networker 3:e8d2ebb7392e 607 static sdp_data name("MBED BlueUSB RFCOMM Serial");
networker 3:e8d2ebb7392e 608 rfcomm.add_element(&rfcommuuid);
networker 3:e8d2ebb7392e 609 rfcomm.add_element(&chan);
networker 3:e8d2ebb7392e 610 l2cap.add_element(&l2capuuid);
networker 3:e8d2ebb7392e 611 protocol.add_element(&l2cap);
networker 3:e8d2ebb7392e 612 protocol.add_element(&rfcomm);
networker 3:e8d2ebb7392e 613 serviceclass.add_element(&serial);
networker 3:e8d2ebb7392e 614 browsegroup.add_element(&root);
networker 3:e8d2ebb7392e 615 static serv_rec attr_list;
networker 3:e8d2ebb7392e 616 attr_list[0] = &handle;
networker 3:e8d2ebb7392e 617 attr_list[1] = &serviceclass;
networker 3:e8d2ebb7392e 618 attr_list[4] = &protocol;
networker 3:e8d2ebb7392e 619 attr_list[5] = &browsegroup;
networker 3:e8d2ebb7392e 620 attr_list[0x100] = &name;
networker 3:e8d2ebb7392e 621 server[SVC_HANDLE] = &attr_list;//server is static and this statement crashes the program when invoked from the constructor which is also invoked statically
networker 3:e8d2ebb7392e 622 }
networker 3:e8d2ebb7392e 623
networker 3:e8d2ebb7392e 624 int SDPManager::findUUID(unsigned h, unsigned uuid) {
networker 3:e8d2ebb7392e 625 serv_rec *rec = server[h];
networker 3:e8d2ebb7392e 626 for ( serv_rec::iterator it = rec->begin(); it != rec->end(); it++) {
networker 3:e8d2ebb7392e 627 if (it->second->findUUID(uuid))
networker 3:e8d2ebb7392e 628 return it->first;
networker 3:e8d2ebb7392e 629 }
networker 3:e8d2ebb7392e 630 printf("rejected %08X because of %04Xx\n", h, uuid);
networker 3:e8d2ebb7392e 631 return -1;
networker 3:e8d2ebb7392e 632 }
networker 3:e8d2ebb7392e 633
networker 3:e8d2ebb7392e 634 void SDPManager::match(bool elig[], unsigned uuid) {
networker 3:e8d2ebb7392e 635 map<unsigned, serv_rec*>::iterator idx;
networker 3:e8d2ebb7392e 636 int i = 0;
networker 3:e8d2ebb7392e 637 for (idx = server.begin(); idx != server.end(); idx++, i++)
networker 3:e8d2ebb7392e 638 if (findUUID(idx->first, uuid) < 0)
networker 3:e8d2ebb7392e 639 elig[i] = false;
networker 3:e8d2ebb7392e 640 }
networker 3:e8d2ebb7392e 641
networker 3:e8d2ebb7392e 642 bool SDPManager::isInList(unsigned short id, const unsigned char* list, int end) {
networker 3:e8d2ebb7392e 643 int len;
networker 3:e8d2ebb7392e 644 for (unsigned pos = 0; pos < end; pos += len) {
networker 3:e8d2ebb7392e 645 len = length(list, pos);
networker 3:e8d2ebb7392e 646 switch (len) {
networker 3:e8d2ebb7392e 647 case 2: //single value
networker 3:e8d2ebb7392e 648 if (getval(list+pos, 2) == id)
networker 3:e8d2ebb7392e 649 return true;
networker 3:e8d2ebb7392e 650 break;
networker 3:e8d2ebb7392e 651 case 4: //range
networker 3:e8d2ebb7392e 652 if (getval(list+pos, 2) > id) break;
networker 3:e8d2ebb7392e 653 if (getval(list+pos+2, 2) < id) break;
networker 3:e8d2ebb7392e 654 return true;
networker 3:e8d2ebb7392e 655 default:
networker 3:e8d2ebb7392e 656 printf("Unexpected length %d\n", len);
networker 3:e8d2ebb7392e 657 }
networker 3:e8d2ebb7392e 658 }
networker 3:e8d2ebb7392e 659 return false;
networker 3:e8d2ebb7392e 660 }
networker 3:e8d2ebb7392e 661
networker 3:e8d2ebb7392e 662 void SDPManager::addToReply(sdp_data *svc, serv_rec *list, const unsigned char* att, int end) {
networker 3:e8d2ebb7392e 663 unsigned short len, low, high;
networker 3:e8d2ebb7392e 664 serv_rec::iterator from, to, idx;
networker 3:e8d2ebb7392e 665 for (unsigned pos = 0; pos < end; pos += len) {
networker 3:e8d2ebb7392e 666 len = length(att, pos);
networker 3:e8d2ebb7392e 667 switch (len) {
networker 3:e8d2ebb7392e 668 case 2: //single value
networker 3:e8d2ebb7392e 669 low = getval(att+pos, 2);
networker 3:e8d2ebb7392e 670 svc->add_element(new sdp_data(low, 2));
networker 3:e8d2ebb7392e 671 svc->add_element((*list)[low]);
networker 3:e8d2ebb7392e 672 printf("Found attrib %d\n", low);
networker 3:e8d2ebb7392e 673 break;
networker 3:e8d2ebb7392e 674 case 4: //range
networker 3:e8d2ebb7392e 675 low = getval(att+pos, 2);
networker 3:e8d2ebb7392e 676 high = getval(att+pos+2, 2);
networker 3:e8d2ebb7392e 677 from = list->lower_bound(low);
networker 3:e8d2ebb7392e 678 to = list->upper_bound(high);
networker 3:e8d2ebb7392e 679 for (idx = from; idx != to; idx++) {
networker 3:e8d2ebb7392e 680 svc->add_element(new sdp_data(idx->first, 2));
networker 3:e8d2ebb7392e 681 svc->add_element(idx->second);
networker 3:e8d2ebb7392e 682 printf("Found attrib %d\n", idx->first);
networker 3:e8d2ebb7392e 683 }
networker 3:e8d2ebb7392e 684 break;
networker 3:e8d2ebb7392e 685 default:
networker 3:e8d2ebb7392e 686 printf("Unexpected length %d\n", len);
networker 3:e8d2ebb7392e 687 }
networker 3:e8d2ebb7392e 688 }
networker 3:e8d2ebb7392e 689 }
networker 3:e8d2ebb7392e 690
networker 3:e8d2ebb7392e 691 //for continuations, just generate the entire list, truncate to desired length and append position of the remainder as continuation
networker 3:e8d2ebb7392e 692 //on the next iteration, generate the list again, use continuation to find remainder and reiterate
networker 3:e8d2ebb7392e 693 void SDPManager::SDPServer(int socket, SocketState state, const u8* data, int len, void* userData) {
networker 3:e8d2ebb7392e 694 unsigned tid = data[2] + ((unsigned)data[1]<<8);
networker 3:e8d2ebb7392e 695 unsigned parlen = data[4] + ((unsigned)data[3]<<8);
networker 3:e8d2ebb7392e 696 //printf("ParseReq: PDU_ID=%d, tid=%04X, parlen=%d ", data[0], tid, parlen);
networker 3:e8d2ebb7392e 697 unsigned pos = 5;
networker 3:e8d2ebb7392e 698 switch (data[0]) {
networker 3:e8d2ebb7392e 699 case 1: {//errorRsp
networker 3:e8d2ebb7392e 700 unsigned errorcode = data[6] + ((unsigned)data[5]<<8);
networker 3:e8d2ebb7392e 701 if (parlen > 2) {
networker 3:e8d2ebb7392e 702 printf("ErrorInfo (%d bytes) for error %d is available\n", parlen-2, errorcode);
networker 3:e8d2ebb7392e 703 }
networker 3:e8d2ebb7392e 704 errorhandler(errorcode);
networker 3:e8d2ebb7392e 705 }
networker 3:e8d2ebb7392e 706 break;
networker 3:e8d2ebb7392e 707 case 2: { //servicesearchReq
networker 3:e8d2ebb7392e 708 }
networker 3:e8d2ebb7392e 709 break;
networker 3:e8d2ebb7392e 710 case 4: { //serviceattributeReq
networker 3:e8d2ebb7392e 711 }
networker 3:e8d2ebb7392e 712 break;
networker 3:e8d2ebb7392e 713 case 6: { //servicesearchattributeReq
networker 3:e8d2ebb7392e 714 sdp_data reply(sdp_data::SEQUENCE);//the attributelist of the reply
networker 3:e8d2ebb7392e 715
networker 3:e8d2ebb7392e 716 unsigned pat[12];//the received search pattern
networker 3:e8d2ebb7392e 717 int pn;//number of uuids in the pattern
networker 3:e8d2ebb7392e 718 if (data[pos]>>3 != sdp_data::SEQUENCE) {//the uuids are wrapped in a sequence
networker 3:e8d2ebb7392e 719 printf("Expected a sequence of UUIDs\n");
networker 3:e8d2ebb7392e 720 break;
networker 3:e8d2ebb7392e 721 }
networker 3:e8d2ebb7392e 722 unsigned end = pos + length(data, pos);//get the length of the list of lists and advance pos to the first list
networker 3:e8d2ebb7392e 723 bool *eligible = new bool[server.size()];//marks for the services identified by the search pattern
networker 3:e8d2ebb7392e 724 for (int i = 0; i < server.size(); i++) eligible[i] = true;
networker 3:e8d2ebb7392e 725 for (pn = 0; pn < 12 && pos < end; pn++) {
networker 3:e8d2ebb7392e 726 pat[pn] = parseUUID(data,end, pos);//store uuid from the sequence in the pattern
networker 3:e8d2ebb7392e 727 match(eligible, pat[pn]);//unmark a service when it does not contain the uuid
networker 3:e8d2ebb7392e 728 //printf("pos=%d, count=%d, uuid=%#X\n", pos, len, pat[pn]);
networker 3:e8d2ebb7392e 729 }
networker 3:e8d2ebb7392e 730
networker 3:e8d2ebb7392e 731 unsigned count = getval(data+pos, 2); //maximum length of attribute list to return
networker 3:e8d2ebb7392e 732 pos += 2;
networker 3:e8d2ebb7392e 733
networker 3:e8d2ebb7392e 734 int len = length(data, pos); //get the length of the attributeID list
networker 3:e8d2ebb7392e 735 int cont = pos + len;
networker 3:e8d2ebb7392e 736 //printf("Count = %d pos=%d, data[pos]=%02x, len=%d, cont=%d\n", count, pos, data[pos], len, cont);
networker 3:e8d2ebb7392e 737 int i = 0;
networker 3:e8d2ebb7392e 738 for (map<unsigned, serv_rec*>::iterator idx = server.begin(); idx != server.end(); idx++, i++) {//foreach service
networker 3:e8d2ebb7392e 739 //printf("testing service handle %08X\n", idx->first);
networker 3:e8d2ebb7392e 740 if (!eligible[i]) continue; //skip services that don't match the pattern
networker 3:e8d2ebb7392e 741 sdp_data *svc = new sdp_data(sdp_data::SEQUENCE); //create a sequence for the attributes of the present service
networker 3:e8d2ebb7392e 742 #if 0
networker 3:e8d2ebb7392e 743 for (serv_rec::iterator attr = idx->second->begin(); attr != idx->second->end(); attr++) {//foreach attrib in the service
networker 3:e8d2ebb7392e 744 //printf("testing attribID %u\n", attr->first);
networker 3:e8d2ebb7392e 745 if (isInList(attr->first, data+pos, len)) {//check if it is requested
networker 3:e8d2ebb7392e 746 printf("Found attribID %d\n", attr->first);
networker 3:e8d2ebb7392e 747 sdp_data *p = attr->second; //the attribute
networker 3:e8d2ebb7392e 748 svc->add_element(new sdp_data(attr->first, 2)); //add the attribute ID as an unsigned short
networker 3:e8d2ebb7392e 749 svc->add_element(p); //add the attribute
networker 3:e8d2ebb7392e 750 //printf("appending %d bytes\n", p->Size());
networker 3:e8d2ebb7392e 751 }
networker 3:e8d2ebb7392e 752 }
networker 3:e8d2ebb7392e 753 #else
networker 3:e8d2ebb7392e 754 //alternatively use map::lower/upper_bound and equal_range, needs only one pass over the range list for each service
networker 3:e8d2ebb7392e 755 addToReply(svc, idx->second, data+pos, len);
networker 3:e8d2ebb7392e 756 #endif
networker 3:e8d2ebb7392e 757 reply.add_element(svc); //append the new attribute list to the reply list
networker 3:e8d2ebb7392e 758 }
networker 3:e8d2ebb7392e 759
networker 3:e8d2ebb7392e 760 unsigned tail = data[cont];
networker 3:e8d2ebb7392e 761 if (tail) {
networker 3:e8d2ebb7392e 762 tail = getval(data+cont+1, tail);
networker 3:e8d2ebb7392e 763 printf("requested continuation tailpos=%u, size=%u\n", tail, reply.Size());
networker 3:e8d2ebb7392e 764 }
networker 3:e8d2ebb7392e 765 else {
networker 3:e8d2ebb7392e 766 //printf("No continuation requested\n");
networker 3:e8d2ebb7392e 767 }
networker 3:e8d2ebb7392e 768 ServiceSearchAttributeReply(tid, &reply, count, tail);
networker 3:e8d2ebb7392e 769
networker 3:e8d2ebb7392e 770 for (int j = 0; j < reply.items(); j++) {
networker 3:e8d2ebb7392e 771 sdp_data *al = reply.item(j);
networker 3:e8d2ebb7392e 772 for (int k = 0; k < al->items(); k++) {
networker 3:e8d2ebb7392e 773 if ((k & 1) == 0) //even hence ID
networker 3:e8d2ebb7392e 774 delete al->item(k); //destroy the ID
networker 3:e8d2ebb7392e 775 al->remove(k); //set all items to nil to prevent destruction of the DB
networker 3:e8d2ebb7392e 776 }
networker 3:e8d2ebb7392e 777 delete al; //destroy the list;
networker 3:e8d2ebb7392e 778 reply.remove(j);
networker 3:e8d2ebb7392e 779 }
networker 3:e8d2ebb7392e 780 delete[] eligible;
networker 3:e8d2ebb7392e 781 }
networker 3:e8d2ebb7392e 782 break;
networker 3:e8d2ebb7392e 783 default:
networker 3:e8d2ebb7392e 784 printf("Unknown SDP request type %02X\n", data[0]);
networker 3:e8d2ebb7392e 785 break;
networker 3:e8d2ebb7392e 786 }
networker 3:e8d2ebb7392e 787 }