adk-stack/btSDP.c

   1 /*
   2  * Copyright (C) 2012 The Android Open Source Project
   3  *
   4  * Licensed under the Apache License, Version 2.0 (the "License");
   5  * you may not use this file except in compliance with the License.
   6  * You may obtain a copy of the License at
   7  *
   8  *      http://www.apache.org/licenses/LICENSE-2.0
   9  *
  10  * Unless required by applicable law or agreed to in writing, software
  11  * distributed under the License is distributed on an "AS IS" BASIS,
  12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  13  * See the License for the specific language governing permissions and
  14  * limitations under the License.
  15  */
  16 #define ADK_INTERNAL
  17 #include "fwk.h"
  18 #include "btSDP.h"
  19 #include <string.h>
  20 #include "sgBuf.h"
  21 #include "btL2CAP.h"
  22 #include "dbg.h"  // dprintf
  23
  24
  25 #define SDP_PDU_Error_Response                          1
  26 #define SDP_PDU_Service_Search_Request                  2
  27 #define SDP_PDU_Service_Search_Response                 3
  28 #define SDP_PDU_Service_Attribute_Request               4
  29 #define SDP_PDU_Service_Attribute_Response              5
  30 #define SDP_PDU_Service_Search_Attribute_Request        6
  31 #define SDP_PDU_Service_Search_Attribute_Response       7
  32
  33 #define SDP_ERR_Invalid_SDP_Version                     0x0001
  34 #define SDP_ERR_Invalid_Service_Record_Handle           0x0002
  35 #define SDP_ERR_Invalid_Request_Syntax                  0x0003
  36 #define SDP_ERR_Invalid_PDU_Size                        0x0004
  37 #define SDP_ERR_Invalid_Continuation_State              0x0005
  38 #define SDP_ERR_Insufficient_Resources                  0x0006
  39
  40
  41 typedef struct{
  42
  43     uint64_t hi, lo;
  44
  45 }uuid;
  46
  47 #define MAX_UUIDS_IN_SEARCH             12      //as per spec
  48 #define MAX_ATTRS_IN_SEARCH_STRING      8       //as per my opinion
  49 #define MAX_SEARCH_RESULTS              16      //no more than this will ever be returned
  50
  51 typedef struct SdpService{
  52
  53     struct SdpService* next;
  54
  55     uint32_t handle;
  56     const uint8_t* descriptor;
  57     uint16_t descrLen;
  58
  59 }SdpService;
  60
  61 typedef struct{
  62
  63     uint16_t aclConn;
  64     uint16_t remChan;
  65
  66     uint32_t contDescr; //which continuation descriptor we expect
  67     uint8_t* result;
  68     uint32_t resultSz;
  69     uint16_t numMatches; //for servicesearch
  70
  71 }SdpInstance;
  72
  73
  74 static SdpService* knownServices = NULL;
  75 static uint32_t sdpContVal = 0x12345678;
  76 static uint32_t sdpNextHandle = 0;
  77
  78 static const uint8_t sdpDescrSdp[] =
  79 {
  80     //define the SDP service itself
  81         //service class ID list
  82         SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x00, 0x01, SDP_ITEM_DESC(SDP_TYPE_ARRAY, SDP_SZ_u8), 3,
  83             SDP_ITEM_DESC(SDP_TYPE_UUID, SDP_SZ_2), 0x10, 0x00, // ServiceDiscoveryServerServiceClassID
  84         //ProtocolDescriptorList
  85         SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x00, 0x04, SDP_ITEM_DESC(SDP_TYPE_ARRAY, SDP_SZ_u8), 8,
  86             SDP_ITEM_DESC(SDP_TYPE_ARRAY, SDP_SZ_u8), 6,
  87                 SDP_ITEM_DESC(SDP_TYPE_UUID, SDP_SZ_2), 0x01, 0x00, // L2CAP
  88                 SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), L2CAP_PSM_SDP >> 8, L2CAP_PSM_SDP & 0xFF, // L2CAP PSM
  89
  90         //browse group list
  91         SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x00, 0x05, SDP_ITEM_DESC(SDP_TYPE_ARRAY, SDP_SZ_u8), 3,
  92             SDP_ITEM_DESC(SDP_TYPE_UUID, SDP_SZ_2), 0x10, 0x02, // Public Browse Group
  93
  94         //magic data #1
  95         SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0xDD, 0xDD, SDP_ITEM_DESC(SDP_TYPE_TEXT, SDP_SZ_u8), 19, 0x53, 0x57, 0x3A, 0x20, 0x44, 0x6D, 0x69, 0x74, 0x72, 0x79, 0x20, 0x47, 0x72, 0x69, 0x6e, 0x62, 0x65, 0x72, 0x67,
  96         //magic data #2
  97         SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0xDD, 0xDE, SDP_ITEM_DESC(SDP_TYPE_TEXT, SDP_SZ_u8), 19, 0x48, 0x57, 0x3A, 0x20, 0x45, 0x72, 0x69, 0x63, 0x20, 0x53, 0x63, 0x68, 0x6c, 0x61, 0x65, 0x70, 0x66, 0x65, 0x72
  98 };
  99
 100 static const uuid bt_base_uuid = {0x0000000000001000ULL, 0x800000805F9B34FBULL};
 101
 102 static void sdpIntToUUID(uuid* dst, uint32_t val){
 103
 104     *dst = bt_base_uuid;
 105     dst->hi += ((uint64_t)val) << 32;
 106 }
 107
 108 static char sdpUuidEqual(const uuid* a, const uuid* b){
 109
 110     return a->lo == b->lo && a->hi == b->hi;
 111 }
 112
 113 static uint32_t btSdpGetElemSz(const uint8_t** descr){
 114
 115     const uint8_t* ptr = *descr;
 116     uint8_t item = *ptr++;
 117     uint32_t sz = 0;
 118
 119     if((item >> 3) != SDP_TYPE_NIL){
 120
 121         switch(item & 7){
 122
 123             case SDP_SZ_1:
 124             case SDP_SZ_2:
 125             case SDP_SZ_4:
 126             case SDP_SZ_8:
 127             case SDP_SZ_16:
 128
 129                 sz = 1 << (item & 7);
 130                 break;
 131
 132             case SDP_SZ_u8:
 133
 134                 sz = *ptr++;
 135                 break;
 136
 137             case SDP_SZ_u16:
 138
 139                 sz = ptr[0];
 140                 sz = (sz << 8) | ptr[1];
 141                 ptr += 2;
 142                 break;
 143
 144             case SDP_SZ_u32:
 145
 146                 sz = ptr[0];
 147                 sz = (sz << 8) | ptr[1];
 148                 sz = (sz << 8) | ptr[2];
 149                 sz = (sz << 8) | ptr[3];
 150                 ptr += 4;
 151                 break;
 152         }
 153     }
 154     *descr = ptr;
 155     return sz;
 156 }
 157
 158 static uint8_t btSdpGetUUID(const uint8_t** descr, uuid* dst){ //return num bytes consumed
 159
 160     uint32_t sz, i;
 161     const uint8_t* orig = *descr;
 162
 163     if(((**descr) >> 3) != SDP_TYPE_UUID) return 0; //not valid UUID type
 164     sz = btSdpGetElemSz(descr);
 165
 166     switch(sz){
 167
 168         case 2:
 169
 170             sdpIntToUUID(dst, (((uint32_t)((*descr)[0])) << 8) | ((*descr)[1]));
 171             break;
 172
 173         case 4:
 174
 175             sdpIntToUUID(dst, (((uint32_t)((*descr)[0])) << 24) | (((uint32_t)((*descr)[1])) << 16) | (((uint32_t)((*descr)[2])) << 8) | ((*descr)[3]));
 176             break;
 177
 178         case 16:
 179
 180             dst->lo = 0;
 181             dst->hi = 0;
 182
 183             for(i = 0; i < 8; i++){
 184
 185                 dst->lo = (dst->lo << 8) | (*descr)[i];
 186                 dst->hi = (dst->lo << 8) | (*descr)[i + 8];
 187             }
 188             break;
 189
 190         default:
 191
 192             return 0;
 193     }
 194     *descr += sz;
 195
 196     return (*descr) - orig;
 197 }
 198
 199 //static int recursive =0;
 200 static void btStdRecursiveWalk(void* itemList, uint8_t* listSzP, sg_buf** walkResultP, const uint8_t** ptr, uint32_t len){
 201
 202     uint32_t sz;
 203     uint8_t typ, numWantedIDs;
 204     const uint8_t* end = (*ptr) + len;
 205     uuid id;
 206     sg_buf* result = NULL;
 207     char isID = 1, skipNext = 0;
 208     uuid* wantedIDs;
 209     uint8_t* numWantedIDsP;
 210     uint32_t* wantedRanges;
 211     uint8_t wantedRangesListSz;
 212
 213     //printf("r%d\n",++recursive);
 214     if(walkResultP){    //copy-traversal
 215
 216         result = sg_alloc();
 217         if(!result) {/*printf("r%d\n",--recursive );*/ return;}
 218
 219         wantedIDs = NULL;
 220         numWantedIDsP = NULL;
 221         wantedRanges = itemList;
 222         wantedRangesListSz = *listSzP;
 223     }
 224     else{               //search for UUIDs
 225
 226         wantedIDs = itemList;
 227         numWantedIDsP = listSzP;
 228         numWantedIDs = *numWantedIDsP;
 229         wantedRanges = NULL;
 230         wantedRangesListSz = 0;
 231     }
 232
 233     while((*ptr) < end){
 234
 235         typ = (**ptr) >> 3;
 236
 237         if(wantedIDs && typ == SDP_TYPE_UUID){
 238
 239             sz = btSdpGetUUID(ptr, &id);
 240             if(end < (*ptr)){
 241
 242                 dbgPrintf("SDP: UUID size > allowed size (%d, %d)\n", sz, end - (*ptr));
 243                 goto out;
 244             }
 245
 246             for(sz = 0; sz < numWantedIDs; sz++){
 247
 248                 if(sdpUuidEqual(wantedIDs + sz, &id)){
 249
 250                     wantedIDs[sz] = wantedIDs[numWantedIDs - 1];
 251                     numWantedIDs--;
 252                     sz--;
 253                 }
 254             }
 255         }
 256         else{
 257
 258             const uint8_t* itemStart = *ptr;
 259
 260             sz = btSdpGetElemSz(ptr);
 261
 262             if(sz > (unsigned)(end - (*ptr))){
 263
 264                 dbgPrintf("SDP: element size > allowed size (%d, %d)\n", sz, end - (*ptr));
 265                 goto out;
 266             }
 267
 268             if(typ == SDP_TYPE_ARRAY || typ == SDP_TYPE_OR_LIST){
 269
 270                 btStdRecursiveWalk(wantedIDs, &numWantedIDs, NULL, ptr, sz);
 271             }
 272             else{
 273
 274                 (*ptr) += sz;
 275             }
 276             if(walkResultP){
 277
 278                 if(isID){
 279
 280                     uint16_t attrID;
 281                     uint8_t i;
 282
 283                     if(sz != 2) dbgPrintf("SDP: attrib ID not 16 bits!\n");
 284
 285                     attrID = (*ptr)[-2];
 286                     attrID = (attrID << 8) | (*ptr)[-1];
 287
 288                     skipNext = 2;
 289                     for(i = 0; i < wantedRangesListSz && skipNext; i++){
 290
 291                         if(attrID >= (wantedRanges[i] >> 16) && attrID <= (wantedRanges[i] & 0xFFFF)) skipNext = 0; //in range
 292                     }
 293                 }
 294                 isID ^= 1;
 295
 296                 if(skipNext){
 297
 298                     skipNext--;
 299                 }
 300                 else{
 301
 302                     if(!sg_add_back(result, itemStart, (*ptr) - itemStart, SG_FLAG_MAKE_A_COPY)){
 303
 304                         sg_free(result);
 305                         //printf("r%d\n",--recursive );
 306                         return;
 307                     }
 308                 }
 309             }
 310         }
 311     }
 312
 313 out:
 314     //printf("r%d\n",--recursive );
 315     if(walkResultP) *walkResultP = result;
 316     if(numWantedIDsP) *numWantedIDsP = numWantedIDs;
 317 }
 318
 319 static char btSdpPutIntoGroup(sg_buf* buf){
 320
 321     uint8_t i, sizeFieldSz, sizeFieldName;
 322     uint32_t sz = sg_length(buf);
 323     uint8_t res[5];
 324
 325     //figure out needed header size field
 326     if(sz < 0x100){
 327
 328         sizeFieldSz = 1;
 329         sizeFieldName = SDP_SZ_u8;
 330         sz <<= 24;
 331     }
 332     else if(sz < 0x10000){
 333
 334         sizeFieldSz = 2;
 335         sizeFieldName = SDP_SZ_u16;
 336         sz <<= 16;
 337     }
 338     else{
 339
 340         sizeFieldSz = 4;
 341         sizeFieldName = SDP_SZ_u32;
 342     }
 343
 344     //add the header
 345     res[0] = SDP_ITEM_DESC(SDP_TYPE_ARRAY, sizeFieldName);
 346     for(i = 0; i < sizeFieldSz; i++, sz <<= 8) res[1 + i] = sz >> 24;
 347     return sg_add_front(buf, res, 1 + sizeFieldSz, SG_FLAG_MAKE_A_COPY);
 348 }
 349
 350 static sg_buf* btSdpError(const uint8_t* trans, uint16_t errNum){
 351
 352     sg_buf* buf;
 353     uint8_t data[] = {SDP_PDU_Error_Response, trans[0], trans[1], errNum >> 8, errNum, 0, 0};
 354
 355     buf = sg_alloc();
 356     if(buf){
 357
 358         if(!sg_add_front(buf, data, sizeof(data), SG_FLAG_MAKE_A_COPY)){
 359
 360             sg_free(buf);
 361             buf = NULL;
 362         }
 363     }
 364     return NULL;
 365 }
 366
 367 static sg_buf* btSdpProcessRequest(SdpInstance* inst, const uint8_t* req, uint16_t reqSz){
 368     uint8_t trans[2] ,cmd, contStateSz, numIDs = 0, numAttrs = 0;
 369     uint32_t maxReplSz = 0, wantedHandle = 0, sz;
 370     uint32_t attrs[MAX_ATTRS_IN_SEARCH_STRING];
 371     SdpService* results[MAX_SEARCH_RESULTS];
 372     uuid ids[MAX_UUIDS_IN_SEARCH];
 373     const uint8_t* end;
 374     sg_buf* result;
 375     unsigned i, j;
 376
 377     cmd = *req++;
 378     trans[0] = *req++;
 379     trans[1] = *req++;
 380
 381     reqSz -= 5;
 382     if(reqSz != (((uint16_t)req[0]) << 8) + req[1]) return btSdpError(trans, SDP_ERR_Invalid_PDU_Size);
 383     req += 2;
 384
 385     // dbgPrintf("SDP request cmd %d (session %02X%02X) with %d bytes of data\n", cmd, trans[0], trans[1], reqSz);
 386
 387     if(cmd == SDP_PDU_Service_Search_Request || cmd == SDP_PDU_Service_Search_Attribute_Request){
 388
 389         if((*req) >> 3 != SDP_TYPE_ARRAY) return btSdpError(trans, SDP_ERR_Invalid_Request_Syntax);
 390         sz = btSdpGetElemSz(&req);
 391         end = req + sz;
 392
 393         while(req < end){
 394
 395             if(numIDs == MAX_UUIDS_IN_SEARCH) return btSdpError(trans, SDP_ERR_Invalid_Request_Syntax); //too many requests
 396             if(!btSdpGetUUID(&req, &ids[numIDs++])) return btSdpError(trans, SDP_ERR_Invalid_Request_Syntax);   //malformed UUID
 397         }
 398     }
 399     else if(cmd == SDP_PDU_Service_Attribute_Request){
 400
 401         for(i = 0; i < 4; i++) wantedHandle = (wantedHandle << 8) | *req++;
 402     }
 403     else{
 404
 405         dbgPrintf("SDP: invalid request: %d\n", cmd);
 406         return btSdpError(trans, SDP_ERR_Invalid_Request_Syntax);
 407     }
 408
 409     for(i = 0; i < 2; i++) maxReplSz = (maxReplSz << 8) | *req++;
 410
 411     if(cmd == SDP_PDU_Service_Attribute_Request || cmd == SDP_PDU_Service_Search_Attribute_Request){
 412
 413         if((*req) >> 3 != SDP_TYPE_ARRAY) return btSdpError(trans, SDP_ERR_Invalid_Request_Syntax);
 414         sz = btSdpGetElemSz(&req);
 415         end = req + sz;
 416
 417         while(req < end){
 418
 419             if(numAttrs == MAX_UUIDS_IN_SEARCH) return btSdpError(trans, SDP_ERR_Insufficient_Resources);       //too many -> unsupported request -> fail
 420             sz = btSdpGetElemSz(&req);
 421             if(sz == 2){
 422
 423                 sz = 0;
 424                 for(i =0; i < 2; i++) sz = (sz << 8) | *req++;
 425                 sz |= sz << 16;
 426             }
 427             else if(sz == 4){
 428
 429                 sz = 0;
 430                 for(i =0; i < 4; i++) sz = (sz << 8) | *req++;
 431             }
 432             else return btSdpError(trans, SDP_ERR_Invalid_Request_Syntax);      //fail -> invalid number format
 433             attrs[numAttrs++] = sz;
 434         }
 435     }
 436
 437     contStateSz = *req++;
 438
 439     if(contStateSz){            // verify continuation is valid or fail
 440         uint32_t contState = 0;
 441
 442         if(contStateSz != sizeof(uint32_t)) return btSdpError(trans, SDP_ERR_Invalid_Continuation_State);
 443         for(i = 0; i < 4; i++) contState = (contState << 8) | *req++;
 444
 445         if(contState != inst->contDescr || !inst->result){
 446
 447             dbgPrintf("SDP: invalid continuation state. Wanted %08X, got %08X\n", inst->contDescr, contState);
 448             if(inst->result){
 449
 450                 free(inst->result);
 451                 inst->result = NULL;
 452             }
 453             return btSdpError(trans, SDP_ERR_Invalid_Continuation_State);
 454         }
 455     }
 456     else{                       //perform the actual search
 457
 458         SdpService* curSvc = knownServices;
 459         uint8_t numFound = 0;
 460
 461         //cleanup first
 462         if(inst->result){
 463
 464             free(inst->result);
 465             inst->result = NULL;
 466         }
 467
 468         //perform the search
 469         if(cmd == SDP_PDU_Service_Search_Request || cmd == SDP_PDU_Service_Search_Attribute_Request){
 470
 471             for(curSvc = knownServices; curSvc && numFound < MAX_SEARCH_RESULTS; curSvc = curSvc->next){
 472
 473                 const uint8_t* ptr = curSvc->descriptor;
 474                 uuid uuids_copy[MAX_UUIDS_IN_SEARCH];
 475                 uint8_t num;
 476
 477                 for(num = 0; num < numIDs; num++) uuids_copy[num] = ids[num];
 478
 479                 btStdRecursiveWalk(uuids_copy, &num, NULL, &ptr, curSvc->descrLen);
 480                 if(!num) results[numFound++] = curSvc;
 481             }
 482         }
 483         else if(cmd == SDP_PDU_Service_Attribute_Request){
 484
 485             for(curSvc = knownServices; curSvc && !numFound; curSvc = curSvc->next){
 486
 487                 if(curSvc->handle == wantedHandle) results[numFound++] = curSvc;
 488             }
 489             if(!numFound) return btSdpError(trans, SDP_ERR_Invalid_Service_Record_Handle);
 490         }
 491
 492         //gather & prepare results
 493         if(cmd == SDP_PDU_Service_Attribute_Request || cmd == SDP_PDU_Service_Search_Attribute_Request){
 494
 495             //we'll assemble the whole result in this buffer
 496             sg_buf* resultSoFar = sg_alloc();
 497             if(!resultSoFar) return btSdpError(trans, SDP_ERR_Insufficient_Resources);
 498
 499             //process each match
 500             for(i = 0; i < numFound; i++){
 501
 502                 const uint8_t* ptr = results[i]->descriptor;
 503                 sg_buf* res;
 504
 505                 //collect wanted attributes
 506                 btStdRecursiveWalk(attrs, &numAttrs, &res, &ptr, results[i]->descrLen);
 507                 if(!res) continue;
 508
 509                 //if requested, add the handle attribute
 510                 for(j = 0; j < numAttrs; j++) if(SDP_ATTR_HANDLE >= (attrs[j] >> 16) && SDP_ATTR_HANDLE <= (attrs[j] & 0xFFFF)) break;
 511                 if(j != numAttrs){
 512                     uint8_t buf[8] = {SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_2), 0x00, 0x00, SDP_ITEM_DESC(SDP_TYPE_UINT, SDP_SZ_4)};
 513
 514                     buf[4] = results[i]->handle >> 24;
 515                     buf[5] = results[i]->handle >> 16;
 516                     buf[6] = results[i]->handle >> 8;
 517                     buf[7] = results[i]->handle;
 518
 519                     if(!sg_add_back(res, buf, sizeof(buf), SG_FLAG_MAKE_A_COPY)){
 520
 521                         sg_free(res);
 522                         free(res);
 523                         continue;
 524                     }
 525                 }
 526
 527                 //wrap and append to the full results list
 528                 if(btSdpPutIntoGroup(res)) sg_concat_back(resultSoFar, res);
 529                 sg_free(res);
 530                 free(res);
 531             }
 532
 533             //wrap the whole thing if required
 534             if((cmd == SDP_PDU_Service_Search_Attribute_Request) && !btSdpPutIntoGroup(resultSoFar)){
 535                 dbgPrintf("SDP: Failed to put results into a group\n");
 536                 sg_free(resultSoFar);
 537                 free(resultSoFar);
 538                 return btSdpError(trans, SDP_ERR_Insufficient_Resources);
 539             }
 540
 541             //flatten to a buffer
 542             uint8_t* buf = malloc(sg_length(resultSoFar));
 543             if(!buf){
 544
 545                 dbgPrintf("SDP: Failed to allocate flattened result array (%ub)\n", sg_length(resultSoFar));
 546                 sg_free(resultSoFar);
 547                 free(resultSoFar);
 548                 return btSdpError(trans, SDP_ERR_Insufficient_Resources);
 549             }
 550             inst->resultSz = sg_length(resultSoFar);
 551             inst->result = buf;
 552             inst->contDescr = sdpContVal;
 553             sg_copyto(resultSoFar, buf);
 554             sg_free(resultSoFar);
 555             free(resultSoFar);
 556         }
 557         else if(cmd == SDP_PDU_Service_Search_Request){
 558
 559             //allocate the array
 560             //inst->resultSz = sizeof(uint32_t[numFound]);
 561             inst->resultSz = sizeof(uint32_t) * numFound;
 562             uint8_t* buf = malloc(inst->resultSz);
 563
 564             if(!buf){
 565
 566                 //dbgPrintf("SDP: Failed to allocate flattened result array (%ub)\n", sizeof(uint32_t[numFound]));
 567                 dbgPrintf("SDP: Failed to allocate flattened result array (%ub)\n", sizeof(uint32_t) * numFound);
 568                 return btSdpError(trans, SDP_ERR_Insufficient_Resources);
 569             }
 570
 571             //process each match
 572             for(i = 0; i < numFound; i++){
 573
 574                 buf[i * 4 + 0] = results[i]->handle >> 24;
 575                 buf[i * 4 + 1] = results[i]->handle >> 16;
 576                 buf[i * 4 + 2] = results[i]->handle >> 8;
 577                 buf[i * 4 + 3] = results[i]->handle;
 578             }
 579
 580             //put everything in the right place
 581             inst->result = buf;
 582             inst->contDescr = sdpContVal;
 583             inst->numMatches = numFound;
 584         }
 585     }
 586     if(++sdpContVal == 0) sdpContVal = 0x01234567; //update continuation state to the next value
 587
 588     //produce the packet to send
 589     uint8_t bufPrepend[9], bufPostpend[5] = {0, }, preSz = 5, postSz = 1;
 590     uint32_t sendSz = 0;
 591     result = sg_alloc();
 592     if(!result) return btSdpError(trans, SDP_ERR_Insufficient_Resources);
 593
 594     if(cmd == SDP_PDU_Service_Attribute_Request || cmd == SDP_PDU_Service_Search_Attribute_Request){
 595
 596         if(maxReplSz > 256) maxReplSz = 256;    //no harm in fragmenting - keep the packets small
 597
 598         sendSz = inst->resultSz;
 599         if(sendSz > maxReplSz) sendSz = maxReplSz;
 600
 601         bufPrepend[preSz++] = sendSz >> 8;
 602         bufPrepend[preSz++] = sendSz & 0xFF;
 603     }
 604     else if(cmd == SDP_PDU_Service_Search_Request){
 605
 606         if(maxReplSz > 64) maxReplSz = 64;      //no harm in fragmenting - keep the packets small
 607
 608         sendSz = inst->resultSz;
 609         if(sendSz > maxReplSz * sizeof(uint32_t)) sendSz = maxReplSz * sizeof(uint32_t);
 610
 611         bufPrepend[preSz++] = inst->numMatches >> 8;
 612         bufPrepend[preSz++] = inst->numMatches & 0xFF;
 613         bufPrepend[preSz++] = (sendSz / sizeof(uint32_t)) >> 8;
 614         bufPrepend[preSz++] = (sendSz / sizeof(uint32_t)) & 0xFF;
 615     }
 616
 617     if(!sg_add_back(result, inst->result, sendSz, SG_FLAG_MAKE_A_COPY)){
 618
 619         dbgPrintf("SDP: Failed to attach reply. Droping");
 620         free(inst->result);
 621         inst->result = NULL;
 622         sg_free(result);
 623         free(result);
 624         return btSdpError(trans, SDP_ERR_Insufficient_Resources);
 625     }
 626     else{
 627
 628         inst->resultSz -= sendSz;
 629         if(inst->resultSz){
 630
 631             memcpy(inst->result, inst->result + sendSz, inst->resultSz);
 632             inst->result = realloc(inst->result, inst->resultSz);
 633         }
 634         else{
 635             free(inst->result);
 636             inst->result = NULL;
 637         }
 638     }
 639
 640     if(inst->result){ //have more
 641
 642         bufPostpend[0] = 4;
 643         for(i = 0; i < 4; i++) bufPostpend[i + 1] = inst->contDescr >> ((3 - i) << 3);
 644         postSz = 5;
 645     }
 646
 647     bufPrepend[0] = cmd + 1; //response to this request
 648     bufPrepend[1] = trans[0];
 649     bufPrepend[2] = trans[1];
 650     bufPrepend[3] = (sendSz + preSz + postSz - 5) >> 8;
 651     bufPrepend[4] = (sendSz + preSz + postSz - 5) & 0xFF;
 652
 653     if(sg_add_front(result, bufPrepend, preSz, SG_FLAG_MAKE_A_COPY) && sg_add_back(result, bufPostpend, postSz, SG_FLAG_MAKE_A_COPY)){
 654
 655         return result;
 656     }
 657     sg_free(result);
 658     free(result);
 659     return btSdpError(trans, SDP_ERR_Insufficient_Resources);
 660 }
 661
 662 static void* sdpServiceAlloc(uint16_t conn, uint16_t chan, uint16_t remChan){
 663
 664     SdpInstance* inst = malloc(sizeof(SdpInstance));
 665     if(inst){
 666
 667         inst->result = NULL;
 668         inst->aclConn = conn;
 669         inst->remChan = remChan;
 670     }
 671     return inst;
 672 }
 673
 674 static void sdpServiceFree(void* service){
 675
 676     SdpInstance* inst = (SdpInstance*)service;
 677
 678     if(inst->result) free(inst->result);
 679     free(inst);
 680 }
 681
 682 static void sdpServiceDataRx(void* service, const uint8_t* data, uint16_t size){
 683
 684     SdpInstance* inst = (SdpInstance*)service;
 685     uint16_t conn = inst->aclConn;
 686     uint16_t remChan = inst->remChan;
 687
 688     sg_buf* reply = btSdpProcessRequest(inst, data, size);
 689     if(reply){
 690
 691 /*//  -- ugly debugging code --
 692         unsigned i;
 693         uint8_t buf[256];
 694         sg_copyto(reply, buf);
 695
 696         dbgPrintf("SDP req got (0x%x): ", size);
 697         for(i = 0; i < size; i++) dbgPrintf(" %02X", data[i]);
 698         dbgPrintf("\n");
 699
 700         dbgPrintf("SDP reply sent (0x%x): ", sg_length(reply));
 701         for(i = 0; i < sg_length(reply); i++) dbgPrintf(" %02X", buf[i]);
 702         dbgPrintf("\n");
 703 */
 704         l2capServiceTx(conn, remChan, reply);
 705     }
 706 }
 707
 708 void btSdpRegisterL2capService(){
 709
 710     const L2capService sdp = {L2CAP_FLAG_SUPPORT_CONNECTIONS, sdpServiceAlloc, sdpServiceFree, sdpServiceDataRx};
 711     if(!l2capServiceRegister(L2CAP_PSM_SDP, &sdp)) dbgPrintf("SDP L2CAP registration failed\n");
 712
 713     btSdpServiceDescriptorAdd(sdpDescrSdp, sizeof(sdpDescrSdp));
 714 }
 715
 716 void btSdpUnregisterL2capService(char sendDiscPacket){
 717     if (!l2capServiceUnregister(L2CAP_PSM_SDP,sendDiscPacket)) dbgPrintf("SDP L2CAP unregistration failed\n");
 718
 719     btSdpServiceDescriptorDel(sdpDescrSdp);
 720 }
 721
 722 void btSdpServiceDescriptorAdd(const uint8_t* descriptor, uint16_t descrLen) {
 723
 724     SdpService *t, *s = malloc(sizeof(SdpService));
 725     if(s){
 726
 727         s->handle = sdpNextHandle;
 728
 729         if(sdpNextHandle) sdpNextHandle++;
 730         else sdpNextHandle = SDP_FIRST_USER_HANDLE; //first add is special - it adds the SDP service itself
 731
 732         s->descriptor = descriptor;
 733         s->descrLen = descrLen;
 734         s->next = NULL;
 735
 736         t = knownServices;      //add at end
 737         while(t && t->next) t = t->next;
 738         if(t) t->next = s;
 739         else knownServices = s;
 740     }
 741 }
 742
 743 void btSdpServiceDescriptorDel(const uint8_t* descriptor){
 744
 745     SdpService *s = knownServices, *p = NULL;
 746
 747     while(s && s->descriptor != descriptor){
 748
 749         p = s;
 750         s = s->next;
 751     }
 752     if(p) p->next = s->next;
 753     else knownServices = s->next;
 754
 755     free(s);
 756 }
 757
 758