Sami Alshorman / Mbed 2 deprecated BluetoothNONIN

Pulse Oximeter (NONIN) communicates with mbed via Bluetooth dongle and sends Heart Rate and Oxygen Saturation via GPRS module

Dependencies:   C12832 GPS GSM mbed

Fork of myBlueUSB_localfix by Nobuaki Aoki

sdp/sdp.cpp@3:55a622e3dbb5, 2015-04-14 (annotated)

Committer:
samialshorman
Date:
Tue Apr 14 21:48:07 2015 +0000
Revision:
3:55a622e3dbb5
Parent:
0:003889bc474f 
Nonin (Pulse Oximeter) connected to mbed lpc 1768 by Bluetooth dongle and sends SMS including Heart Rate and Oxygen saturation by GPRS module

Who changed what in which revision?

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