src/attrib-server.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2010  Nokia Corporation
   6  *  Copyright (C) 2010  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This program is free software; you can redistribute it and/or modify
  10  *  it under the terms of the GNU General Public License as published by
  11  *  the Free Software Foundation; either version 2 of the License, or
  12  *  (at your option) any later version.
  13  *
  14  *  This program is distributed in the hope that it will be useful,
  15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17  *  GNU General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU General Public License
  20  *  along with this program; if not, write to the Free Software
  21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  22  *
  23  */
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include <config.h>
  27 #endif
  28
  29 #include <errno.h>
  30 #include <stdint.h>
  31 #include <string.h>
  32 #include <unistd.h>
  33 #include <glib.h>
  34
  35 #include <bluetooth/bluetooth.h>
  36 #include <bluetooth/uuid.h>
  37 #include <bluetooth/sdp.h>
  38 #include <bluetooth/sdp_lib.h>
  39
  40 #include "log.h"
  41 #include "gdbus.h"
  42 #include "btio.h"
  43 #include "sdpd.h"
  44 #include "hcid.h"
  45 #include "adapter.h"
  46 #include "device.h"
  47 #include "manager.h"
  48 #include "gattrib.h"
  49 #include "att.h"
  50 #include "gatt.h"
  51 #include "att-database.h"
  52 #include "storage.h"
  53
  54 #include "attrib-server.h"
  55
  56 static GSList *servers = NULL;
  57
  58 struct gatt_server {
  59         struct btd_adapter *adapter;
  60         GIOChannel *l2cap_io;
  61         GIOChannel *le_io;
  62         uint32_t gatt_sdp_handle;
  63         uint32_t gap_sdp_handle;
  64         GList *database;
  65         GSList *clients;
  66         uint16_t name_handle;
  67         uint16_t appearance_handle;
  68 };
  69
  70 struct gatt_channel {
  71         bdaddr_t src;
  72         bdaddr_t dst;
  73         GAttrib *attrib;
  74         guint mtu;
  75         gboolean le;
  76         guint id;
  77         gboolean encrypted;
  78         struct gatt_server *server;
  79         guint cleanup_id;
  80         struct btd_device *device;
  81 };
  82
  83 struct group_elem {
  84         uint16_t handle;
  85         uint16_t end;
  86         uint8_t *data;
  87         uint16_t len;
  88 };
  89
  90 static bt_uuid_t prim_uuid = {
  91                         .type = BT_UUID16,
  92                         .value.u16 = GATT_PRIM_SVC_UUID
  93 };
  94 static bt_uuid_t snd_uuid = {
  95                         .type = BT_UUID16,
  96                         .value.u16 = GATT_SND_SVC_UUID
  97 };
  98 static bt_uuid_t ccc_uuid = {
  99                         .type = BT_UUID16,
 100                         .value.u16 = GATT_CLIENT_CHARAC_CFG_UUID
 101 };
 102
 103 static void attrib_free(void *data)
 104 {
 105         struct attribute *a = data;
 106
 107         g_free(a->data);
 108         g_free(a);
 109 }
 110
 111 static void channel_free(struct gatt_channel *channel)
 112 {
 113
 114         if (channel->cleanup_id)
 115                 g_source_remove(channel->cleanup_id);
 116
 117         if (channel->device)
 118                 btd_device_unref(channel->device);
 119
 120         g_attrib_unref(channel->attrib);
 121         g_free(channel);
 122 }
 123
 124 static void gatt_server_free(struct gatt_server *server)
 125 {
 126         g_list_free_full(server->database, attrib_free);
 127
 128         if (server->l2cap_io != NULL) {
 129                 g_io_channel_unref(server->l2cap_io);
 130                 g_io_channel_shutdown(server->l2cap_io, FALSE, NULL);
 131         }
 132
 133         if (server->le_io != NULL) {
 134                 g_io_channel_unref(server->le_io);
 135                 g_io_channel_shutdown(server->le_io, FALSE, NULL);
 136         }
 137
 138         g_slist_free_full(server->clients, (GDestroyNotify) channel_free);
 139
 140         if (server->gatt_sdp_handle > 0)
 141                 remove_record_from_server(server->gatt_sdp_handle);
 142
 143         if (server->gap_sdp_handle > 0)
 144                 remove_record_from_server(server->gap_sdp_handle);
 145
 146         if (server->adapter != NULL)
 147                 btd_adapter_unref(server->adapter);
 148
 149         g_free(server);
 150 }
 151
 152 static gint adapter_cmp_addr(gconstpointer a, gconstpointer b)
 153 {
 154         const struct gatt_server *server = a;
 155         const bdaddr_t *bdaddr = b;
 156         bdaddr_t src;
 157
 158         adapter_get_address(server->adapter, &src);
 159
 160         return bacmp(&src, bdaddr);
 161 }
 162
 163 static gint adapter_cmp(gconstpointer a, gconstpointer b)
 164 {
 165         const struct gatt_server *server = a;
 166         const struct btd_adapter *adapter = b;
 167
 168         if (server->adapter == adapter)
 169                 return 0;
 170
 171         return -1;
 172 }
 173
 174 static struct gatt_server *find_gatt_server(const bdaddr_t *bdaddr)
 175 {
 176         GSList *l;
 177
 178         l = g_slist_find_custom(servers, bdaddr, adapter_cmp_addr);
 179         if (l == NULL) {
 180                 char addr[18];
 181
 182                 ba2str(bdaddr, addr);
 183                 error("No GATT server found in %s", addr);
 184                 return NULL;
 185         }
 186
 187         return l->data;
 188 }
 189
 190 static sdp_record_t *server_record_new(uuid_t *uuid, uint16_t start, uint16_t end)
 191 {
 192         sdp_list_t *svclass_id, *apseq, *proto[2], *root, *aproto;
 193         uuid_t root_uuid, proto_uuid, l2cap;
 194         sdp_record_t *record;
 195         sdp_data_t *psm, *sh, *eh;
 196         uint16_t lp = ATT_PSM;
 197
 198         if (uuid == NULL)
 199                 return NULL;
 200
 201         if (start > end)
 202                 return NULL;
 203
 204         record = sdp_record_alloc();
 205         if (record == NULL)
 206                 return NULL;
 207
 208         sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP);
 209         root = sdp_list_append(NULL, &root_uuid);
 210         sdp_set_browse_groups(record, root);
 211         sdp_list_free(root, NULL);
 212
 213         svclass_id = sdp_list_append(NULL, uuid);
 214         sdp_set_service_classes(record, svclass_id);
 215         sdp_list_free(svclass_id, NULL);
 216
 217         sdp_uuid16_create(&l2cap, L2CAP_UUID);
 218         proto[0] = sdp_list_append(NULL, &l2cap);
 219         psm = sdp_data_alloc(SDP_UINT16, &lp);
 220         proto[0] = sdp_list_append(proto[0], psm);
 221         apseq = sdp_list_append(NULL, proto[0]);
 222
 223         sdp_uuid16_create(&proto_uuid, ATT_UUID);
 224         proto[1] = sdp_list_append(NULL, &proto_uuid);
 225         sh = sdp_data_alloc(SDP_UINT16, &start);
 226         proto[1] = sdp_list_append(proto[1], sh);
 227         eh = sdp_data_alloc(SDP_UINT16, &end);
 228         proto[1] = sdp_list_append(proto[1], eh);
 229         apseq = sdp_list_append(apseq, proto[1]);
 230
 231         aproto = sdp_list_append(NULL, apseq);
 232         sdp_set_access_protos(record, aproto);
 233
 234         sdp_data_free(psm);
 235         sdp_data_free(sh);
 236         sdp_data_free(eh);
 237         sdp_list_free(proto[0], NULL);
 238         sdp_list_free(proto[1], NULL);
 239         sdp_list_free(apseq, NULL);
 240         sdp_list_free(aproto, NULL);
 241
 242         return record;
 243 }
 244
 245 static int handle_cmp(gconstpointer a, gconstpointer b)
 246 {
 247         const struct attribute *attrib = a;
 248         uint16_t handle = GPOINTER_TO_UINT(b);
 249
 250         return attrib->handle - handle;
 251 }
 252
 253 static int attribute_cmp(gconstpointer a1, gconstpointer a2)
 254 {
 255         const struct attribute *attrib1 = a1;
 256         const struct attribute *attrib2 = a2;
 257
 258         return attrib1->handle - attrib2->handle;
 259 }
 260
 261 static struct attribute *find_svc_range(struct gatt_server *server,
 262                                         uint16_t start, uint16_t *end)
 263 {
 264         struct attribute *attrib;
 265         guint h = start;
 266         GList *l;
 267
 268         if (end == NULL)
 269                 return NULL;
 270
 271         l = g_list_find_custom(server->database, GUINT_TO_POINTER(h),
 272                                                                 handle_cmp);
 273         if (!l)
 274                 return NULL;
 275
 276         attrib = l->data;
 277
 278         if (bt_uuid_cmp(&attrib->uuid, &prim_uuid) != 0 &&
 279                         bt_uuid_cmp(&attrib->uuid, &snd_uuid) != 0)
 280                 return NULL;
 281
 282         *end = start;
 283
 284         for (l = l->next; l; l = l->next) {
 285                 struct attribute *a = l->data;
 286
 287                 if (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
 288                                 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0)
 289                         break;
 290
 291                 *end = a->handle;
 292         }
 293
 294         return attrib;
 295 }
 296
 297 static uint32_t attrib_create_sdp_new(struct gatt_server *server,
 298                                         uint16_t handle, const char *name)
 299 {
 300         sdp_record_t *record;
 301         struct attribute *a;
 302         uint16_t end = 0;
 303         uuid_t svc, gap_uuid;
 304         bdaddr_t addr;
 305
 306         a = find_svc_range(server, handle, &end);
 307
 308         if (a == NULL)
 309                 return 0;
 310
 311         if (a->len == 2)
 312                 sdp_uuid16_create(&svc, att_get_u16(a->data));
 313         else if (a->len == 16)
 314                 sdp_uuid128_create(&svc, a->data);
 315         else
 316                 return 0;
 317
 318         record = server_record_new(&svc, handle, end);
 319         if (record == NULL)
 320                 return 0;
 321
 322         if (name != NULL)
 323                 sdp_set_info_attr(record, name, "BlueZ", NULL);
 324
 325         sdp_uuid16_create(&gap_uuid, GENERIC_ACCESS_PROFILE_ID);
 326         if (sdp_uuid_cmp(&svc, &gap_uuid) == 0) {
 327                 sdp_set_url_attr(record, "http://www.bluez.org/",
 328                                 "http://www.bluez.org/",
 329                                 "http://www.bluez.org/");
 330         }
 331
 332         adapter_get_address(server->adapter, &addr);
 333         if (add_record_to_server(&addr, record) == 0)
 334                 return record->handle;
 335
 336         sdp_record_free(record);
 337         return 0;
 338 }
 339
 340 static struct attribute *attrib_db_add_new(struct gatt_server *server,
 341                                 uint16_t handle, bt_uuid_t *uuid, int read_reqs,
 342                                 int write_reqs, const uint8_t *value, int len)
 343 {
 344         struct attribute *a;
 345         guint h = handle;
 346
 347         DBG("handle=0x%04x", handle);
 348
 349         if (g_list_find_custom(server->database, GUINT_TO_POINTER(h),
 350                                                                 handle_cmp))
 351                 return NULL;
 352
 353         a = g_new0(struct attribute, 1);
 354         a->len = len;
 355         a->data = g_memdup(value, len);
 356         a->handle = handle;
 357         a->uuid = *uuid;
 358         a->read_reqs = read_reqs;
 359         a->write_reqs = write_reqs;
 360
 361         server->database = g_list_insert_sorted(server->database, a,
 362                                                                 attribute_cmp);
 363
 364         return a;
 365 }
 366
 367 static uint8_t att_check_reqs(struct gatt_channel *channel, uint8_t opcode,
 368                                                                 int reqs)
 369 {
 370         /* FIXME: currently, it is assumed an encrypted link is enough for
 371          * authentication. This will allow to enable the SMP negotiation once
 372          * it is on upstream kernel. High security level should be mapped
 373          * to authentication and medium to encryption permission. */
 374         if (!channel->encrypted)
 375                 channel->encrypted = g_attrib_is_encrypted(channel->attrib);
 376         if (reqs == ATT_AUTHENTICATION && !channel->encrypted)
 377                 return ATT_ECODE_AUTHENTICATION;
 378         else if (reqs == ATT_AUTHORIZATION)
 379                 return ATT_ECODE_AUTHORIZATION;
 380
 381         switch (opcode) {
 382         case ATT_OP_READ_BY_GROUP_REQ:
 383         case ATT_OP_READ_BY_TYPE_REQ:
 384         case ATT_OP_READ_REQ:
 385         case ATT_OP_READ_BLOB_REQ:
 386         case ATT_OP_READ_MULTI_REQ:
 387                 if (reqs == ATT_NOT_PERMITTED)
 388                         return ATT_ECODE_READ_NOT_PERM;
 389                 break;
 390         case ATT_OP_PREP_WRITE_REQ:
 391         case ATT_OP_WRITE_REQ:
 392         case ATT_OP_WRITE_CMD:
 393                 if (reqs == ATT_NOT_PERMITTED)
 394                         return ATT_ECODE_WRITE_NOT_PERM;
 395                 break;
 396         }
 397
 398         return 0;
 399 }
 400
 401 static uint16_t read_by_group(struct gatt_channel *channel, uint16_t start,
 402                                                 uint16_t end, bt_uuid_t *uuid,
 403                                                 uint8_t *pdu, int len)
 404 {
 405         struct att_data_list *adl;
 406         struct attribute *a;
 407         struct group_elem *cur, *old = NULL;
 408         GSList *l, *groups;
 409         GList *dl, *database;
 410         uint16_t length, last_handle, last_size = 0;
 411         uint8_t status;
 412         int i;
 413
 414         if (start > end || start == 0x0000)
 415                 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, start,
 416                                         ATT_ECODE_INVALID_HANDLE, pdu, len);
 417
 418         /*
 419          * Only <<Primary Service>> and <<Secondary Service>> grouping
 420          * types may be used in the Read By Group Type Request.
 421          */
 422
 423         if (bt_uuid_cmp(uuid, &prim_uuid) != 0 &&
 424                 bt_uuid_cmp(uuid, &snd_uuid) != 0)
 425                 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, 0x0000,
 426                                         ATT_ECODE_UNSUPP_GRP_TYPE, pdu, len);
 427
 428         last_handle = end;
 429         database = channel->server->database;
 430         for (dl = database, groups = NULL, cur = NULL; dl; dl = dl->next) {
 431
 432                 a = dl->data;
 433
 434                 if (a->handle < start)
 435                         continue;
 436
 437                 if (a->handle >= end)
 438                         break;
 439
 440                 /* The old group ends when a new one starts */
 441                 if (old && (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
 442                                 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0)) {
 443                         old->end = last_handle;
 444                         old = NULL;
 445                 }
 446
 447                 if (bt_uuid_cmp(&a->uuid, uuid) != 0) {
 448                         /* Still inside a service, update its last handle */
 449                         if (old)
 450                                 last_handle = a->handle;
 451                         continue;
 452                 }
 453
 454                 if (last_size && (last_size != a->len))
 455                         break;
 456
 457                 status = att_check_reqs(channel, ATT_OP_READ_BY_GROUP_REQ,
 458                                                                 a->read_reqs);
 459
 460                 if (status == 0x00 && a->read_cb)
 461                         status = a->read_cb(a, channel->device,
 462                                                         a->cb_user_data);
 463
 464                 if (status) {
 465                         g_slist_free_full(groups, g_free);
 466                         return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ,
 467                                                 a->handle, status, pdu, len);
 468                 }
 469
 470                 cur = g_new0(struct group_elem, 1);
 471                 cur->handle = a->handle;
 472                 cur->data = a->data;
 473                 cur->len = a->len;
 474
 475                 /* Attribute Grouping Type found */
 476                 groups = g_slist_append(groups, cur);
 477
 478                 last_size = a->len;
 479                 old = cur;
 480                 last_handle = cur->handle;
 481         }
 482
 483         if (groups == NULL)
 484                 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, start,
 485                                         ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
 486
 487         if (dl == NULL)
 488                 cur->end = a->handle;
 489         else
 490                 cur->end = last_handle;
 491
 492         length = g_slist_length(groups);
 493
 494         adl = att_data_list_alloc(length, last_size + 4);
 495
 496         for (i = 0, l = groups; l; l = l->next, i++) {
 497                 uint8_t *value;
 498
 499                 cur = l->data;
 500
 501                 value = (void *) adl->data[i];
 502
 503                 att_put_u16(cur->handle, value);
 504                 att_put_u16(cur->end, &value[2]);
 505                 /* Attribute Value */
 506                 memcpy(&value[4], cur->data, cur->len);
 507         }
 508
 509         length = enc_read_by_grp_resp(adl, pdu, len);
 510
 511         att_data_list_free(adl);
 512         g_slist_free_full(groups, g_free);
 513
 514         return length;
 515 }
 516
 517 static uint16_t read_by_type(struct gatt_channel *channel, uint16_t start,
 518                                                 uint16_t end, bt_uuid_t *uuid,
 519                                                 uint8_t *pdu, int len)
 520 {
 521         struct att_data_list *adl;
 522         GSList *l, *types;
 523         GList *dl, *database;
 524         struct attribute *a;
 525         uint16_t num, length;
 526         uint8_t status;
 527         int i;
 528
 529         if (start > end || start == 0x0000)
 530                 return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ, start,
 531                                         ATT_ECODE_INVALID_HANDLE, pdu, len);
 532
 533         database = channel->server->database;
 534         for (dl = database, length = 0, types = NULL; dl; dl = dl->next) {
 535
 536                 a = dl->data;
 537
 538                 if (a->handle < start)
 539                         continue;
 540
 541                 if (a->handle > end)
 542                         break;
 543
 544                 if (bt_uuid_cmp(&a->uuid, uuid)  != 0)
 545                         continue;
 546
 547                 status = att_check_reqs(channel, ATT_OP_READ_BY_TYPE_REQ,
 548                                                                 a->read_reqs);
 549
 550                 if (status == 0x00 && a->read_cb)
 551                         status = a->read_cb(a, channel->device,
 552                                                         a->cb_user_data);
 553
 554                 if (status) {
 555                         g_slist_free(types);
 556                         return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ,
 557                                                 a->handle, status, pdu, len);
 558                 }
 559
 560                 /* All elements must have the same length */
 561                 if (length == 0)
 562                         length = a->len;
 563                 else if (a->len != length)
 564                         break;
 565
 566                 types = g_slist_append(types, a);
 567         }
 568
 569         if (types == NULL)
 570                 return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ, start,
 571                                         ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
 572
 573         num = g_slist_length(types);
 574
 575         /* Handle length plus attribute value length */
 576         length += 2;
 577
 578         adl = att_data_list_alloc(num, length);
 579
 580         for (i = 0, l = types; l; i++, l = l->next) {
 581                 uint8_t *value;
 582
 583                 a = l->data;
 584
 585                 value = (void *) adl->data[i];
 586
 587                 att_put_u16(a->handle, value);
 588
 589                 /* Attribute Value */
 590                 memcpy(&value[2], a->data, a->len);
 591         }
 592
 593         length = enc_read_by_type_resp(adl, pdu, len);
 594
 595         att_data_list_free(adl);
 596         g_slist_free(types);
 597
 598         return length;
 599 }
 600
 601 static int find_info(struct gatt_channel *channel, uint16_t start, uint16_t end,
 602                                                         uint8_t *pdu, int len)
 603 {
 604         struct attribute *a;
 605         struct att_data_list *adl;
 606         GSList *l, *info;
 607         GList *dl, *database;
 608         uint8_t format, last_type = BT_UUID_UNSPEC;
 609         uint16_t length, num;
 610         int i;
 611
 612         if (start > end || start == 0x0000)
 613                 return enc_error_resp(ATT_OP_FIND_INFO_REQ, start,
 614                                         ATT_ECODE_INVALID_HANDLE, pdu, len);
 615
 616         database = channel->server->database;
 617         for (dl = database, info = NULL, num = 0; dl; dl = dl->next) {
 618                 a = dl->data;
 619
 620                 if (a->handle < start)
 621                         continue;
 622
 623                 if (a->handle > end)
 624                         break;
 625
 626                 if (last_type == BT_UUID_UNSPEC)
 627                         last_type = a->uuid.type;
 628
 629                 if (a->uuid.type != last_type)
 630                         break;
 631
 632                 info = g_slist_append(info, a);
 633                 num++;
 634
 635                 last_type = a->uuid.type;
 636         }
 637
 638         if (info == NULL)
 639                 return enc_error_resp(ATT_OP_FIND_INFO_REQ, start,
 640                                         ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
 641
 642         if (last_type == BT_UUID16) {
 643                 length = 2;
 644                 format = 0x01;
 645         } else if (last_type == BT_UUID128) {
 646                 length = 16;
 647                 format = 0x02;
 648         } else {
 649                 g_slist_free(info);
 650                 return 0;
 651         }
 652
 653         adl = att_data_list_alloc(num, length + 2);
 654
 655         for (i = 0, l = info; l; i++, l = l->next) {
 656                 uint8_t *value;
 657
 658                 a = l->data;
 659
 660                 value = (void *) adl->data[i];
 661
 662                 att_put_u16(a->handle, value);
 663
 664                 /* Attribute Value */
 665                 att_put_uuid(a->uuid, &value[2]);
 666         }
 667
 668         length = enc_find_info_resp(format, adl, pdu, len);
 669
 670         att_data_list_free(adl);
 671         g_slist_free(info);
 672
 673         return length;
 674 }
 675
 676 static int find_by_type(struct gatt_channel *channel, uint16_t start,
 677                         uint16_t end, bt_uuid_t *uuid, const uint8_t *value,
 678                                         int vlen, uint8_t *opdu, int mtu)
 679 {
 680         struct attribute *a;
 681         struct att_range *range;
 682         GSList *matches;
 683         GList *dl, *database;
 684         int len;
 685
 686         if (start > end || start == 0x0000)
 687                 return enc_error_resp(ATT_OP_FIND_BY_TYPE_REQ, start,
 688                                         ATT_ECODE_INVALID_HANDLE, opdu, mtu);
 689
 690         /* Searching first requested handle number */
 691         database = channel->server->database;
 692         for (dl = database, matches = NULL, range = NULL; dl; dl = dl->next) {
 693                 a = dl->data;
 694
 695                 if (a->handle < start)
 696                         continue;
 697
 698                 if (a->handle > end)
 699                         break;
 700
 701                 /* Primary service? Attribute value matches? */
 702                 if ((bt_uuid_cmp(&a->uuid, uuid) == 0) && (a->len == vlen) &&
 703                                         (memcmp(a->data, value, vlen) == 0)) {
 704
 705                         range = g_new0(struct att_range, 1);
 706                         range->start = a->handle;
 707                         /* It is allowed to have end group handle the same as
 708                          * start handle, for groups with only one attribute. */
 709                         range->end = a->handle;
 710
 711                         matches = g_slist_append(matches, range);
 712                 } else if (range) {
 713                         /* Update the last found handle or reset the pointer
 714                          * to track that a new group started: Primary or
 715                          * Secondary service. */
 716                         if (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
 717                                         bt_uuid_cmp(&a->uuid, &snd_uuid) == 0)
 718                                 range = NULL;
 719                         else
 720                                 range->end = a->handle;
 721                 }
 722         }
 723
 724         if (matches == NULL)
 725                 return enc_error_resp(ATT_OP_FIND_BY_TYPE_REQ, start,
 726                                 ATT_ECODE_ATTR_NOT_FOUND, opdu, mtu);
 727
 728         len = enc_find_by_type_resp(matches, opdu, mtu);
 729
 730         g_slist_free_full(matches, g_free);
 731
 732         return len;
 733 }
 734
 735 static uint16_t read_value(struct gatt_channel *channel, uint16_t handle,
 736                                                         uint8_t *pdu, int len)
 737 {
 738         struct attribute *a;
 739         uint8_t status;
 740         GList *l;
 741         uint16_t cccval;
 742         uint8_t bdaddr_type;
 743         guint h = handle;
 744
 745         l = g_list_find_custom(channel->server->database,
 746                                         GUINT_TO_POINTER(h), handle_cmp);
 747         if (!l)
 748                 return enc_error_resp(ATT_OP_READ_REQ, handle,
 749                                         ATT_ECODE_INVALID_HANDLE, pdu, len);
 750
 751         a = l->data;
 752
 753         bdaddr_type = device_get_addr_type(channel->device);
 754
 755         if (bt_uuid_cmp(&ccc_uuid, &a->uuid) == 0 &&
 756                 read_device_ccc(&channel->src, &channel->dst, bdaddr_type,
 757                                                         handle, &cccval) == 0) {
 758                 uint8_t config[2];
 759
 760                 att_put_u16(cccval, config);
 761                 return enc_read_resp(config, sizeof(config), pdu, len);
 762         }
 763
 764         status = att_check_reqs(channel, ATT_OP_READ_REQ, a->read_reqs);
 765
 766         if (status == 0x00 && a->read_cb)
 767                 status = a->read_cb(a, channel->device, a->cb_user_data);
 768
 769         if (status)
 770                 return enc_error_resp(ATT_OP_READ_REQ, handle, status, pdu,
 771                                                                         len);
 772
 773         return enc_read_resp(a->data, a->len, pdu, len);
 774 }
 775
 776 static uint16_t read_blob(struct gatt_channel *channel, uint16_t handle,
 777                                         uint16_t offset, uint8_t *pdu, int len)
 778 {
 779         struct attribute *a;
 780         uint8_t status;
 781         GList *l;
 782         uint16_t cccval;
 783         uint8_t bdaddr_type;
 784         guint h = handle;
 785
 786         l = g_list_find_custom(channel->server->database,
 787                                         GUINT_TO_POINTER(h), handle_cmp);
 788         if (!l)
 789                 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle,
 790                                         ATT_ECODE_INVALID_HANDLE, pdu, len);
 791
 792         a = l->data;
 793
 794         if (a->len <= offset)
 795                 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle,
 796                                         ATT_ECODE_INVALID_OFFSET, pdu, len);
 797
 798         bdaddr_type = device_get_addr_type(channel->device);
 799
 800         if (bt_uuid_cmp(&ccc_uuid, &a->uuid) == 0 &&
 801                 read_device_ccc(&channel->src, &channel->dst, bdaddr_type,
 802                                                         handle, &cccval) == 0) {
 803                 uint8_t config[2];
 804
 805                 att_put_u16(cccval, config);
 806                 return enc_read_blob_resp(config, sizeof(config), offset,
 807                                                                 pdu, len);
 808         }
 809
 810         status = att_check_reqs(channel, ATT_OP_READ_BLOB_REQ, a->read_reqs);
 811
 812         if (status == 0x00 && a->read_cb)
 813                 status = a->read_cb(a, channel->device, a->cb_user_data);
 814
 815         if (status)
 816                 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle, status,
 817                                                                 pdu, len);
 818
 819         return enc_read_blob_resp(a->data, a->len, offset, pdu, len);
 820 }
 821
 822 static uint16_t write_value(struct gatt_channel *channel, uint16_t handle,
 823                                                 const uint8_t *value, int vlen,
 824                                                 uint8_t *pdu, int len)
 825 {
 826         struct attribute *a;
 827         uint8_t status;
 828         GList *l;
 829         guint h = handle;
 830
 831         l = g_list_find_custom(channel->server->database,
 832                                         GUINT_TO_POINTER(h), handle_cmp);
 833         if (!l)
 834                 return enc_error_resp(ATT_OP_WRITE_REQ, handle,
 835                                 ATT_ECODE_INVALID_HANDLE, pdu, len);
 836
 837         a = l->data;
 838
 839         status = att_check_reqs(channel, ATT_OP_WRITE_REQ, a->write_reqs);
 840         if (status)
 841                 return enc_error_resp(ATT_OP_WRITE_REQ, handle, status, pdu,
 842                                                                         len);
 843
 844         if (bt_uuid_cmp(&ccc_uuid, &a->uuid) != 0) {
 845
 846                 attrib_db_update(channel->server->adapter, handle, NULL,
 847                                                         value, vlen, NULL);
 848
 849                 if (a->write_cb) {
 850                         status = a->write_cb(a, channel->device,
 851                                                         a->cb_user_data);
 852                         if (status)
 853                                 return enc_error_resp(ATT_OP_WRITE_REQ, handle,
 854                                                         status, pdu, len);
 855                 }
 856         } else {
 857                 uint16_t cccval = att_get_u16(value);
 858                 uint8_t bdaddr_type = device_get_addr_type(channel->device);
 859
 860                 write_device_ccc(&channel->src, &channel->dst, bdaddr_type,
 861                                                                 handle, cccval);
 862         }
 863
 864         return enc_write_resp(pdu, len);
 865 }
 866
 867 static uint16_t mtu_exchange(struct gatt_channel *channel, uint16_t mtu,
 868                 uint8_t *pdu, int len)
 869 {
 870         GError *gerr = NULL;
 871         GIOChannel *io;
 872         uint16_t imtu;
 873
 874         if (mtu < ATT_DEFAULT_LE_MTU)
 875                 return enc_error_resp(ATT_OP_MTU_REQ, 0,
 876                                         ATT_ECODE_REQ_NOT_SUPP, pdu, len);
 877
 878         io = g_attrib_get_channel(channel->attrib);
 879
 880         bt_io_get(io, BT_IO_L2CAP, &gerr,
 881                         BT_IO_OPT_IMTU, &imtu,
 882                         BT_IO_OPT_INVALID);
 883
 884         if (gerr)
 885                 return enc_error_resp(ATT_OP_MTU_REQ, 0,
 886                                         ATT_ECODE_UNLIKELY, pdu, len);
 887
 888         channel->mtu = MIN(mtu, imtu);
 889         g_attrib_set_mtu(channel->attrib, channel->mtu);
 890
 891         return enc_mtu_resp(imtu, pdu, len);
 892 }
 893
 894 static void channel_remove(struct gatt_channel *channel)
 895 {
 896         channel->server->clients = g_slist_remove(channel->server->clients,
 897                                                                 channel);
 898         channel_free(channel);
 899 }
 900
 901 static gboolean channel_watch_cb(GIOChannel *io, GIOCondition cond,
 902                                                 gpointer user_data)
 903 {
 904         channel_remove(user_data);
 905
 906         return FALSE;
 907 }
 908
 909 static void channel_handler(const uint8_t *ipdu, uint16_t len,
 910                                                         gpointer user_data)
 911 {
 912         struct gatt_channel *channel = user_data;
 913         uint8_t opdu[ATT_MAX_MTU], value[ATT_MAX_MTU];
 914         uint16_t length, start, end, mtu, offset;
 915         bt_uuid_t uuid;
 916         uint8_t status = 0;
 917         int vlen;
 918
 919         DBG("op 0x%02x", ipdu[0]);
 920
 921         switch (ipdu[0]) {
 922         case ATT_OP_READ_BY_GROUP_REQ:
 923                 length = dec_read_by_grp_req(ipdu, len, &start, &end, &uuid);
 924                 if (length == 0) {
 925                         status = ATT_ECODE_INVALID_PDU;
 926                         goto done;
 927                 }
 928
 929                 length = read_by_group(channel, start, end, &uuid, opdu,
 930                                                                 channel->mtu);
 931                 break;
 932         case ATT_OP_READ_BY_TYPE_REQ:
 933                 length = dec_read_by_type_req(ipdu, len, &start, &end, &uuid);
 934                 if (length == 0) {
 935                         status = ATT_ECODE_INVALID_PDU;
 936                         goto done;
 937                 }
 938
 939                 length = read_by_type(channel, start, end, &uuid, opdu,
 940                                                                 channel->mtu);
 941                 break;
 942         case ATT_OP_READ_REQ:
 943                 length = dec_read_req(ipdu, len, &start);
 944                 if (length == 0) {
 945                         status = ATT_ECODE_INVALID_PDU;
 946                         goto done;
 947                 }
 948
 949                 length = read_value(channel, start, opdu, channel->mtu);
 950                 break;
 951         case ATT_OP_READ_BLOB_REQ:
 952                 length = dec_read_blob_req(ipdu, len, &start, &offset);
 953                 if (length == 0) {
 954                         status = ATT_ECODE_INVALID_PDU;
 955                         goto done;
 956                 }
 957
 958                 length = read_blob(channel, start, offset, opdu, channel->mtu);
 959                 break;
 960         case ATT_OP_MTU_REQ:
 961                 if (!channel->le) {
 962                         status = ATT_ECODE_REQ_NOT_SUPP;
 963                         goto done;
 964                 }
 965
 966                 length = dec_mtu_req(ipdu, len, &mtu);
 967                 if (length == 0) {
 968                         status = ATT_ECODE_INVALID_PDU;
 969                         goto done;
 970                 }
 971
 972                 length = mtu_exchange(channel, mtu, opdu, channel->mtu);
 973                 break;
 974         case ATT_OP_FIND_INFO_REQ:
 975                 length = dec_find_info_req(ipdu, len, &start, &end);
 976                 if (length == 0) {
 977                         status = ATT_ECODE_INVALID_PDU;
 978                         goto done;
 979                 }
 980
 981                 length = find_info(channel, start, end, opdu, channel->mtu);
 982                 break;
 983         case ATT_OP_WRITE_REQ:
 984                 length = dec_write_req(ipdu, len, &start, value, &vlen);
 985                 if (length == 0) {
 986                         status = ATT_ECODE_INVALID_PDU;
 987                         goto done;
 988                 }
 989
 990                 length = write_value(channel, start, value, vlen, opdu,
 991                                                                 channel->mtu);
 992                 break;
 993         case ATT_OP_WRITE_CMD:
 994                 length = dec_write_cmd(ipdu, len, &start, value, &vlen);
 995                 if (length > 0)
 996                         write_value(channel, start, value, vlen, opdu,
 997                                                                 channel->mtu);
 998                 return;
 999         case ATT_OP_FIND_BY_TYPE_REQ:
1000                 length = dec_find_by_type_req(ipdu, len, &start, &end,
1001                                                         &uuid, value, &vlen);
1002                 if (length == 0) {
1003                         status = ATT_ECODE_INVALID_PDU;
1004                         goto done;
1005                 }
1006
1007                 length = find_by_type(channel, start, end, &uuid, value, vlen,
1008                                                         opdu, channel->mtu);
1009                 break;
1010         case ATT_OP_HANDLE_CNF:
1011                 return;
1012         case ATT_OP_HANDLE_IND:
1013         case ATT_OP_HANDLE_NOTIFY:
1014                 /* The attribute client is already handling these */
1015                 return;
1016         case ATT_OP_READ_MULTI_REQ:
1017         case ATT_OP_PREP_WRITE_REQ:
1018         case ATT_OP_EXEC_WRITE_REQ:
1019         default:
1020                 DBG("Unsupported request 0x%02x", ipdu[0]);
1021                 status = ATT_ECODE_REQ_NOT_SUPP;
1022                 goto done;
1023         }
1024
1025         if (length == 0)
1026                 status = ATT_ECODE_IO;
1027
1028 done:
1029         if (status)
1030                 length = enc_error_resp(ipdu[0], 0x0000, status, opdu,
1031                                                                 channel->mtu);
1032
1033         g_attrib_send(channel->attrib, 0, opdu[0], opdu, length,
1034                                                         NULL, NULL, NULL);
1035 }
1036
1037 guint attrib_channel_attach(GAttrib *attrib)
1038 {
1039         struct gatt_server *server;
1040         struct btd_device *device;
1041         struct gatt_channel *channel;
1042         GIOChannel *io;
1043         GError *gerr = NULL;
1044         char addr[18];
1045         uint16_t cid;
1046         guint mtu = 0;
1047
1048         io = g_attrib_get_channel(attrib);
1049
1050         channel = g_new0(struct gatt_channel, 1);
1051
1052         bt_io_get(io, BT_IO_L2CAP, &gerr,
1053                         BT_IO_OPT_SOURCE_BDADDR, &channel->src,
1054                         BT_IO_OPT_DEST_BDADDR, &channel->dst,
1055                         BT_IO_OPT_CID, &cid,
1056                         BT_IO_OPT_IMTU, &mtu,
1057                         BT_IO_OPT_INVALID);
1058         if (gerr) {
1059                 error("bt_io_get: %s", gerr->message);
1060                 g_error_free(gerr);
1061                 g_free(channel);
1062                 return 0;
1063         }
1064
1065         server = find_gatt_server(&channel->src);
1066         if (server == NULL) {
1067                 char src[18];
1068
1069                 ba2str(&channel->src, src);
1070                 error("No GATT server found in %s", src);
1071                 g_free(channel);
1072                 return 0;
1073         }
1074
1075         channel->server = server;
1076
1077         ba2str(&channel->dst, addr);
1078
1079         device = adapter_find_device(server->adapter, addr);
1080         if (device == NULL || device_is_bonded(device) == FALSE)
1081                 delete_device_ccc(&channel->src, &channel->dst);
1082
1083         if (cid != ATT_CID) {
1084                 channel->le = FALSE;
1085                 channel->mtu = mtu;
1086         } else {
1087                 channel->le = TRUE;
1088                 channel->mtu = ATT_DEFAULT_LE_MTU;
1089         }
1090
1091         channel->attrib = g_attrib_ref(attrib);
1092         channel->id = g_attrib_register(channel->attrib, GATTRIB_ALL_REQS,
1093                                         channel_handler, channel, NULL);
1094
1095         channel->cleanup_id = g_io_add_watch(io, G_IO_HUP, channel_watch_cb,
1096                                                                 channel);
1097
1098         channel->device = btd_device_ref(device);
1099
1100         server->clients = g_slist_append(server->clients, channel);
1101
1102         return channel->id;
1103 }
1104
1105 static gint channel_id_cmp(gconstpointer data, gconstpointer user_data)
1106 {
1107         const struct gatt_channel *channel = data;
1108         guint id = GPOINTER_TO_UINT(user_data);
1109
1110         return channel->id - id;
1111 }
1112
1113 gboolean attrib_channel_detach(GAttrib *attrib, guint id)
1114 {
1115         struct gatt_server *server;
1116         struct gatt_channel *channel;
1117         GError *gerr = NULL;
1118         GIOChannel *io;
1119         bdaddr_t src;
1120         GSList *l;
1121
1122         io = g_attrib_get_channel(attrib);
1123
1124         bt_io_get(io, BT_IO_L2CAP, &gerr, BT_IO_OPT_SOURCE_BDADDR, &src,
1125                                                         BT_IO_OPT_INVALID);
1126
1127         if (gerr != NULL) {
1128                 error("bt_io_get: %s", gerr->message);
1129                 g_error_free(gerr);
1130                 return FALSE;
1131         }
1132
1133         server = find_gatt_server(&src);
1134         if (server == NULL)
1135                 return FALSE;
1136
1137         l = g_slist_find_custom(server->clients, GUINT_TO_POINTER(id),
1138                                                                 channel_id_cmp);
1139         if (!l)
1140                 return FALSE;
1141
1142         channel = l->data;
1143
1144         g_attrib_unregister(channel->attrib, channel->id);
1145         channel_remove(channel);
1146
1147         return TRUE;
1148 }
1149
1150 static void connect_event(GIOChannel *io, GError *gerr, void *user_data)
1151 {
1152         GAttrib *attrib;
1153
1154         if (gerr) {
1155                 error("%s", gerr->message);
1156                 return;
1157         }
1158
1159         attrib = g_attrib_new(io);
1160         attrib_channel_attach(attrib);
1161         g_attrib_unref(attrib);
1162 }
1163
1164 static void confirm_event(GIOChannel *io, void *user_data)
1165 {
1166         GError *gerr = NULL;
1167
1168         if (bt_io_accept(io, connect_event, user_data, NULL, &gerr) == FALSE) {
1169                 error("bt_io_accept: %s", gerr->message);
1170                 g_error_free(gerr);
1171                 g_io_channel_unref(io);
1172         }
1173
1174         return;
1175 }
1176
1177 static gboolean register_core_services(struct gatt_server *server)
1178 {
1179         uint8_t atval[256];
1180         bt_uuid_t uuid;
1181         uint16_t appearance = 0x0000;
1182
1183         /* GAP service: primary service definition */
1184         bt_uuid16_create(&uuid, GATT_PRIM_SVC_UUID);
1185         att_put_u16(GENERIC_ACCESS_PROFILE_ID, &atval[0]);
1186         attrib_db_add_new(server, 0x0001, &uuid, ATT_NONE, ATT_NOT_PERMITTED,
1187                                                                 atval, 2);
1188
1189         /* GAP service: device name characteristic */
1190         server->name_handle = 0x0006;
1191         bt_uuid16_create(&uuid, GATT_CHARAC_UUID);
1192         atval[0] = ATT_CHAR_PROPER_READ;
1193         att_put_u16(server->name_handle, &atval[1]);
1194         att_put_u16(GATT_CHARAC_DEVICE_NAME, &atval[3]);
1195         attrib_db_add_new(server, 0x0004, &uuid, ATT_NONE, ATT_NOT_PERMITTED,
1196                                                                 atval, 5);
1197
1198         /* GAP service: device name attribute */
1199         bt_uuid16_create(&uuid, GATT_CHARAC_DEVICE_NAME);
1200         attrib_db_add_new(server, server->name_handle, &uuid, ATT_NONE,
1201                                                 ATT_NOT_PERMITTED, NULL, 0);
1202
1203         /* GAP service: device appearance characteristic */
1204         server->appearance_handle = 0x0008;
1205         bt_uuid16_create(&uuid, GATT_CHARAC_UUID);
1206         atval[0] = ATT_CHAR_PROPER_READ;
1207         att_put_u16(server->appearance_handle, &atval[1]);
1208         att_put_u16(GATT_CHARAC_APPEARANCE, &atval[3]);
1209         attrib_db_add_new(server, 0x0007, &uuid, ATT_NONE, ATT_NOT_PERMITTED,
1210                                                                 atval, 5);
1211
1212         /* GAP service: device appearance attribute */
1213         bt_uuid16_create(&uuid, GATT_CHARAC_APPEARANCE);
1214         att_put_u16(appearance, &atval[0]);
1215         attrib_db_add_new(server, server->appearance_handle, &uuid, ATT_NONE,
1216                                                 ATT_NOT_PERMITTED, atval, 2);
1217         server->gap_sdp_handle = attrib_create_sdp_new(server, 0x0001,
1218                                                 "Generic Access Profile");
1219         if (server->gap_sdp_handle == 0) {
1220                 error("Failed to register GAP service record");
1221                 return FALSE;
1222         }
1223
1224         /* GATT service: primary service definition */
1225         bt_uuid16_create(&uuid, GATT_PRIM_SVC_UUID);
1226         att_put_u16(GENERIC_ATTRIB_PROFILE_ID, &atval[0]);
1227         attrib_db_add_new(server, 0x0010, &uuid, ATT_NONE, ATT_NOT_PERMITTED,
1228                                                                 atval, 2);
1229
1230         server->gatt_sdp_handle = attrib_create_sdp_new(server, 0x0010,
1231                                                 "Generic Attribute Profile");
1232         if (server->gatt_sdp_handle == 0) {
1233                 error("Failed to register GATT service record");
1234                 return FALSE;
1235         }
1236
1237         return TRUE;
1238 }
1239
1240 int btd_adapter_gatt_server_start(struct btd_adapter *adapter)
1241 {
1242         struct gatt_server *server;
1243         GError *gerr = NULL;
1244         bdaddr_t addr;
1245
1246         DBG("Start GATT server in hci%d", adapter_get_dev_id(adapter));
1247
1248         server = g_new0(struct gatt_server, 1);
1249         server->adapter = btd_adapter_ref(adapter);
1250
1251         adapter_get_address(server->adapter, &addr);
1252
1253         /* BR/EDR socket */
1254         server->l2cap_io = bt_io_listen(BT_IO_L2CAP, NULL, confirm_event,
1255                                         NULL, NULL, &gerr,
1256                                         BT_IO_OPT_SOURCE_BDADDR, &addr,
1257                                         BT_IO_OPT_PSM, ATT_PSM,
1258                                         BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
1259                                         BT_IO_OPT_INVALID);
1260
1261         if (server->l2cap_io == NULL) {
1262                 error("%s", gerr->message);
1263                 g_error_free(gerr);
1264                 gatt_server_free(server);
1265                 return -1;
1266         }
1267
1268         if (!register_core_services(server)) {
1269                 gatt_server_free(server);
1270                 return -1;
1271         }
1272
1273         /* LE socket */
1274         server->le_io = bt_io_listen(BT_IO_L2CAP, NULL, confirm_event,
1275                                         &server->le_io, NULL, &gerr,
1276                                         BT_IO_OPT_SOURCE_BDADDR, &addr,
1277                                         BT_IO_OPT_CID, ATT_CID,
1278                                         BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
1279                                         BT_IO_OPT_INVALID);
1280
1281         if (server->le_io == NULL) {
1282                 error("%s", gerr->message);
1283                 g_error_free(gerr);
1284                 /* Doesn't have LE support, continue */
1285         }
1286
1287         servers = g_slist_prepend(servers, server);
1288         return 0;
1289 }
1290
1291 void btd_adapter_gatt_server_stop(struct btd_adapter *adapter)
1292 {
1293         struct gatt_server *server;
1294         GSList *l;
1295
1296         l = g_slist_find_custom(servers, adapter, adapter_cmp);
1297         if (l == NULL)
1298                 return;
1299
1300         DBG("Stop GATT server in hci%d", adapter_get_dev_id(adapter));
1301
1302         server = l->data;
1303         servers = g_slist_remove(servers, server);
1304         gatt_server_free(server);
1305 }
1306
1307 uint32_t attrib_create_sdp(struct btd_adapter *adapter, uint16_t handle,
1308                                                         const char *name)
1309 {
1310         GSList *l;
1311
1312         l = g_slist_find_custom(servers, adapter, adapter_cmp);
1313         if (l == NULL)
1314                 return 0;
1315
1316         return attrib_create_sdp_new(l->data, handle, name);
1317 }
1318
1319 void attrib_free_sdp(uint32_t sdp_handle)
1320 {
1321         remove_record_from_server(sdp_handle);
1322 }
1323
1324 static uint16_t find_uuid16_avail(struct btd_adapter *adapter, uint16_t nitems)
1325 {
1326         struct gatt_server *server;
1327         uint16_t handle;
1328         GSList *l;
1329         GList *dl;
1330
1331         l = g_slist_find_custom(servers, adapter, adapter_cmp);
1332         if (l == NULL)
1333                 return 0;
1334
1335         server = l->data;
1336         if (server->database == NULL)
1337                 return 0x0001;
1338
1339         for (dl = server->database, handle = 0x0001; dl; dl = dl->next) {
1340                 struct attribute *a = dl->data;
1341
1342                 if ((bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
1343                                 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0) &&
1344                                 a->handle - handle >= nitems)
1345                         /* Note: the range above excludes the current handle */
1346                         return handle;
1347
1348                 if (a->len == 16 && (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
1349                                 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0)) {
1350                         /* 128 bit UUID service definition */
1351                         return 0;
1352                 }
1353
1354                 if (a->handle == 0xffff)
1355                         return 0;
1356
1357                 handle = a->handle + 1;
1358         }
1359
1360         if (0xffff - handle + 1 >= nitems)
1361                 return handle;
1362
1363         return 0;
1364 }
1365
1366 static uint16_t find_uuid128_avail(struct btd_adapter *adapter, uint16_t nitems)
1367 {
1368         uint16_t handle = 0, end = 0xffff;
1369         struct gatt_server *server;
1370         GList *dl;
1371         GSList *l;
1372
1373         l = g_slist_find_custom(servers, adapter, adapter_cmp);
1374         if (l == NULL)
1375                 return 0;
1376
1377         server = l->data;
1378         if (server->database == NULL)
1379                 return 0xffff - nitems + 1;
1380
1381         for (dl = g_list_last(server->database); dl; dl = dl->prev) {
1382                 struct attribute *a = dl->data;
1383
1384                 if (handle == 0)
1385                         handle = a->handle;
1386
1387                 if (bt_uuid_cmp(&a->uuid, &prim_uuid) != 0 &&
1388                                 bt_uuid_cmp(&a->uuid, &snd_uuid) != 0)
1389                         continue;
1390
1391                 if (end - handle >= nitems)
1392                         return end - nitems + 1;
1393
1394                 if (a->len == 2) {
1395                         /* 16 bit UUID service definition */
1396                         return 0;
1397                 }
1398
1399                 if (a->handle == 0x0001)
1400                         return 0;
1401
1402                 end = a->handle - 1;
1403                 handle = 0;
1404         }
1405
1406         if (end - 0x0001 >= nitems)
1407                 return end - nitems + 1;
1408
1409         return 0;
1410 }
1411
1412 uint16_t attrib_db_find_avail(struct btd_adapter *adapter, bt_uuid_t *svc_uuid,
1413                                                                 uint16_t nitems)
1414 {
1415         g_assert(nitems > 0);
1416
1417         if (svc_uuid->type == BT_UUID16)
1418                 return find_uuid16_avail(adapter, nitems);
1419         else if (svc_uuid->type == BT_UUID128)
1420                 return find_uuid128_avail(adapter, nitems);
1421         else {
1422                 char uuidstr[MAX_LEN_UUID_STR];
1423
1424                 bt_uuid_to_string(svc_uuid, uuidstr, MAX_LEN_UUID_STR);
1425                 error("Service uuid: %s is neither a 16-bit nor a 128-bit uuid",
1426                                                                 uuidstr);
1427                 return 0;
1428         }
1429 }
1430
1431 struct attribute *attrib_db_add(struct btd_adapter *adapter, uint16_t handle,
1432                                 bt_uuid_t *uuid, int read_reqs, int write_reqs,
1433                                                 const uint8_t *value, int len)
1434 {
1435         GSList *l;
1436
1437         l = g_slist_find_custom(servers, adapter, adapter_cmp);
1438         if (l == NULL)
1439                 return NULL;
1440
1441         return attrib_db_add_new(l->data, handle, uuid, read_reqs, write_reqs,
1442                                                                 value, len);
1443 }
1444
1445 int attrib_db_update(struct btd_adapter *adapter, uint16_t handle,
1446                                         bt_uuid_t *uuid, const uint8_t *value,
1447                                         int len, struct attribute **attr)
1448 {
1449         struct gatt_server *server;
1450         struct attribute *a;
1451         GSList *l;
1452         GList *dl;
1453         guint h = handle;
1454
1455         l = g_slist_find_custom(servers, adapter, adapter_cmp);
1456         if (l == NULL)
1457                 return -ENOENT;
1458
1459         server = l->data;
1460
1461         DBG("handle=0x%04x", handle);
1462
1463         dl = g_list_find_custom(server->database, GUINT_TO_POINTER(h),
1464                                                                 handle_cmp);
1465         if (dl == NULL)
1466                 return -ENOENT;
1467
1468         a = dl->data;
1469
1470         a->data = g_try_realloc(a->data, len);
1471         if (len && a->data == NULL)
1472                 return -ENOMEM;
1473
1474         a->len = len;
1475         memcpy(a->data, value, len);
1476
1477         if (uuid != NULL)
1478                 a->uuid = *uuid;
1479
1480         if (attr)
1481                 *attr = a;
1482
1483         return 0;
1484 }
1485
1486 int attrib_db_del(struct btd_adapter *adapter, uint16_t handle)
1487 {
1488         struct gatt_server *server;
1489         struct attribute *a;
1490         GSList *l;
1491         GList *dl;
1492         guint h = handle;
1493
1494         l = g_slist_find_custom(servers, adapter, adapter_cmp);
1495         if (l == NULL)
1496                 return -ENOENT;
1497
1498         server = l->data;
1499
1500         DBG("handle=0x%04x", handle);
1501
1502         dl = g_list_find_custom(server->database, GUINT_TO_POINTER(h),
1503                                                                 handle_cmp);
1504         if (dl == NULL)
1505                 return -ENOENT;
1506
1507         a = dl->data;
1508         server->database = g_list_remove(server->database, a);
1509         g_free(a->data);
1510         g_free(a);
1511
1512         return 0;
1513 }
1514
1515 int attrib_gap_set(struct btd_adapter *adapter, uint16_t uuid,
1516                                                 const uint8_t *value, int len)
1517 {
1518         struct gatt_server *server;
1519         uint16_t handle;
1520         GSList *l;
1521
1522         l = g_slist_find_custom(servers, adapter, adapter_cmp);
1523         if (l == NULL)
1524                 return -ENOENT;
1525
1526         server = l->data;
1527
1528         /* FIXME: Missing Privacy and Reconnection Address */
1529
1530         switch (uuid) {
1531         case GATT_CHARAC_DEVICE_NAME:
1532                 handle = server->name_handle;
1533                 break;
1534         case GATT_CHARAC_APPEARANCE:
1535                 handle = server->appearance_handle;
1536                 break;
1537         default:
1538                 return -ENOSYS;
1539         }
1540
1541         return attrib_db_update(adapter, handle, NULL, value, len, NULL);
1542 }