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