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 "glib-helper.h"
  42 #include "btio.h"
  43 #include "sdpd.h"
  44 #include "hcid.h"
  45 #include "att.h"
  46 #include "gattrib.h"
  47
  48 #include "attrib-server.h"
  49
  50 #define GATT_PSM 0x1f
  51 #define GATT_CID 4
  52
  53 static GSList *database = NULL;
  54
  55 struct gatt_channel {
  56         bdaddr_t src;
  57         bdaddr_t dst;
  58         GSList *configs;
  59         GSList *notify;
  60         GSList *indicate;
  61         GAttrib *attrib;
  62         guint mtu;
  63         gboolean le;
  64         guint id;
  65         gboolean encrypted;
  66 };
  67
  68 struct group_elem {
  69         uint16_t handle;
  70         uint16_t end;
  71         uint8_t *data;
  72         uint16_t len;
  73 };
  74
  75 static GIOChannel *l2cap_io = NULL;
  76 static GIOChannel *le_io = NULL;
  77 static GSList *clients = NULL;
  78 static uint32_t gatt_sdp_handle = 0;
  79 static uint32_t gap_sdp_handle = 0;
  80
  81 /* GAP attribute handles */
  82 static uint16_t name_handle = 0x0000;
  83 static uint16_t appearance_handle = 0x0000;
  84
  85 static bt_uuid_t prim_uuid = {
  86                         .type = BT_UUID16,
  87                         .value.u16 = GATT_PRIM_SVC_UUID
  88 };
  89 static bt_uuid_t snd_uuid = {
  90                         .type = BT_UUID16,
  91                         .value.u16 = GATT_SND_SVC_UUID
  92 };
  93
  94 static sdp_record_t *server_record_new(uuid_t *uuid, uint16_t start, uint16_t end)
  95 {
  96         sdp_list_t *svclass_id, *apseq, *proto[2], *root, *aproto;
  97         uuid_t root_uuid, proto_uuid, l2cap;
  98         sdp_record_t *record;
  99         sdp_data_t *psm, *sh, *eh;
 100         uint16_t lp = GATT_PSM;
 101
 102         if (uuid == NULL)
 103                 return NULL;
 104
 105         if (start > end)
 106                 return NULL;
 107
 108         record = sdp_record_alloc();
 109         if (record == NULL)
 110                 return NULL;
 111
 112         sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP);
 113         root = sdp_list_append(NULL, &root_uuid);
 114         sdp_set_browse_groups(record, root);
 115         sdp_list_free(root, NULL);
 116
 117         svclass_id = sdp_list_append(NULL, uuid);
 118         sdp_set_service_classes(record, svclass_id);
 119         sdp_list_free(svclass_id, NULL);
 120
 121         sdp_uuid16_create(&l2cap, L2CAP_UUID);
 122         proto[0] = sdp_list_append(NULL, &l2cap);
 123         psm = sdp_data_alloc(SDP_UINT16, &lp);
 124         proto[0] = sdp_list_append(proto[0], psm);
 125         apseq = sdp_list_append(NULL, proto[0]);
 126
 127         sdp_uuid16_create(&proto_uuid, ATT_UUID);
 128         proto[1] = sdp_list_append(NULL, &proto_uuid);
 129         sh = sdp_data_alloc(SDP_UINT16, &start);
 130         proto[1] = sdp_list_append(proto[1], sh);
 131         eh = sdp_data_alloc(SDP_UINT16, &end);
 132         proto[1] = sdp_list_append(proto[1], eh);
 133         apseq = sdp_list_append(apseq, proto[1]);
 134
 135         aproto = sdp_list_append(NULL, apseq);
 136         sdp_set_access_protos(record, aproto);
 137
 138         sdp_data_free(psm);
 139         sdp_data_free(sh);
 140         sdp_data_free(eh);
 141         sdp_list_free(proto[0], NULL);
 142         sdp_list_free(proto[1], NULL);
 143         sdp_list_free(apseq, NULL);
 144         sdp_list_free(aproto, NULL);
 145
 146         return record;
 147 }
 148
 149 static int handle_cmp(gconstpointer a, gconstpointer b)
 150 {
 151         const struct attribute *attrib = a;
 152         uint16_t handle = GPOINTER_TO_UINT(b);
 153
 154         return attrib->handle - handle;
 155 }
 156
 157 static int attribute_cmp(gconstpointer a1, gconstpointer a2)
 158 {
 159         const struct attribute *attrib1 = a1;
 160         const struct attribute *attrib2 = a2;
 161
 162         return attrib1->handle - attrib2->handle;
 163 }
 164
 165 static uint8_t att_check_reqs(struct gatt_channel *channel, uint8_t opcode,
 166                                                                 int reqs)
 167 {
 168         /* FIXME: currently, it is assumed an encrypted link is enough for
 169          * authentication. This will allow to enable the SMP negotiation once
 170          * it is on upstream kernel. */
 171         if (!channel->encrypted)
 172                 channel->encrypted = g_attrib_is_encrypted(channel->attrib);
 173         if (reqs == ATT_AUTHENTICATION && !channel->encrypted)
 174                 return ATT_ECODE_INSUFF_ENC;
 175
 176         switch (opcode) {
 177         case ATT_OP_READ_BY_GROUP_REQ:
 178         case ATT_OP_READ_BY_TYPE_REQ:
 179         case ATT_OP_READ_REQ:
 180         case ATT_OP_READ_BLOB_REQ:
 181         case ATT_OP_READ_MULTI_REQ:
 182                 if (reqs == ATT_NOT_PERMITTED)
 183                         return ATT_ECODE_READ_NOT_PERM;
 184                 break;
 185         case ATT_OP_PREP_WRITE_REQ:
 186         case ATT_OP_WRITE_REQ:
 187         case ATT_OP_WRITE_CMD:
 188                 if (reqs == ATT_NOT_PERMITTED)
 189                         return ATT_ECODE_WRITE_NOT_PERM;
 190                 break;
 191         }
 192
 193         return 0;
 194 }
 195
 196 static uint8_t client_set_notifications(struct attribute *attr,
 197                                                         gpointer user_data)
 198 {
 199         struct gatt_channel *channel = user_data;
 200         struct attribute *last_chr_val = NULL;
 201         uint16_t cfg_val;
 202         uint8_t props;
 203         bt_uuid_t uuid;
 204         GSList *l;
 205
 206         cfg_val = att_get_u16(attr->data);
 207
 208         bt_uuid16_create(&uuid, GATT_CHARAC_UUID);
 209         for (l = database, props = 0; l != NULL; l = l->next) {
 210                 struct attribute *a = l->data;
 211                 static uint16_t handle = 0;
 212
 213                 if (a->handle >= attr->handle)
 214                         break;
 215
 216                 if (bt_uuid_cmp(&a->uuid, &uuid) == 0) {
 217                         props = att_get_u8(&a->data[0]);
 218                         handle = att_get_u16(&a->data[1]);
 219                         continue;
 220                 }
 221
 222                 if (handle && a->handle == handle)
 223                         last_chr_val = a;
 224         }
 225
 226         if (last_chr_val == NULL)
 227                 return 0;
 228
 229         if ((cfg_val & 0x0001) && !(props & ATT_CHAR_PROPER_NOTIFY))
 230                 return ATT_ECODE_WRITE_NOT_PERM;
 231
 232         if ((cfg_val & 0x0002) && !(props & ATT_CHAR_PROPER_INDICATE))
 233                 return ATT_ECODE_WRITE_NOT_PERM;
 234
 235         if (cfg_val & 0x0001)
 236                 channel->notify = g_slist_append(channel->notify, last_chr_val);
 237         else
 238                 channel->notify = g_slist_remove(channel->notify, last_chr_val);
 239
 240         if (cfg_val & 0x0002)
 241                 channel->indicate = g_slist_append(channel->indicate,
 242                                                                 last_chr_val);
 243         else
 244                 channel->indicate = g_slist_remove(channel->indicate,
 245                                                                 last_chr_val);
 246
 247         return 0;
 248 }
 249
 250 static struct attribute *client_cfg_attribute(struct gatt_channel *channel,
 251                                                 struct attribute *orig_attr,
 252                                                 const uint8_t *value, int vlen)
 253 {
 254         guint handle = orig_attr->handle;
 255         bt_uuid_t uuid;
 256         GSList *l;
 257
 258         bt_uuid16_create(&uuid, GATT_CLIENT_CHARAC_CFG_UUID);
 259         if (bt_uuid_cmp(&orig_attr->uuid, &uuid) != 0)
 260                 return NULL;
 261
 262         /* Value is unchanged, not need to create a private copy yet */
 263         if (vlen == orig_attr->len && memcmp(orig_attr->data, value, vlen) == 0)
 264                 return orig_attr;
 265
 266         l = g_slist_find_custom(channel->configs, GUINT_TO_POINTER(handle),
 267                                                                 handle_cmp);
 268         if (!l) {
 269                 struct attribute *a;
 270
 271                 /* Create a private copy of the Client Characteristic
 272                  * Configuration attribute */
 273                 a = g_malloc0(sizeof(*a) + vlen);
 274                 memcpy(a, orig_attr, sizeof(*a));
 275                 memcpy(a->data, value, vlen);
 276                 a->write_cb = client_set_notifications;
 277                 a->cb_user_data = channel;
 278
 279                 channel->configs = g_slist_insert_sorted(channel->configs, a,
 280                                                                 attribute_cmp);
 281
 282                 return a;
 283         }
 284
 285         return l->data;
 286 }
 287
 288 static uint16_t read_by_group(struct gatt_channel *channel, uint16_t start,
 289                                                 uint16_t end, bt_uuid_t *uuid,
 290                                                 uint8_t *pdu, int len)
 291 {
 292         struct att_data_list *adl;
 293         struct attribute *a;
 294         struct group_elem *cur, *old = NULL;
 295         GSList *l, *groups;
 296         uint16_t length, last_handle, last_size = 0;
 297         uint8_t status;
 298         int i;
 299
 300         if (start > end || start == 0x0000)
 301                 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, start,
 302                                         ATT_ECODE_INVALID_HANDLE, pdu, len);
 303
 304         /*
 305          * Only <<Primary Service>> and <<Secondary Service>> grouping
 306          * types may be used in the Read By Group Type Request.
 307          */
 308
 309         if (bt_uuid_cmp(uuid, &prim_uuid) != 0 &&
 310                 bt_uuid_cmp(uuid, &snd_uuid) != 0)
 311                 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, 0x0000,
 312                                         ATT_ECODE_UNSUPP_GRP_TYPE, pdu, len);
 313
 314         last_handle = end;
 315         for (l = database, groups = NULL; l; l = l->next) {
 316                 struct attribute *client_attr;
 317
 318                 a = l->data;
 319
 320                 if (a->handle < start)
 321                         continue;
 322
 323                 if (a->handle >= end)
 324                         break;
 325
 326                 /* The old group ends when a new one starts */
 327                 if (old && (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
 328                                 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0)) {
 329                         old->end = last_handle;
 330                         old = NULL;
 331                 }
 332
 333                 if (bt_uuid_cmp(&a->uuid, uuid) != 0) {
 334                         /* Still inside a service, update its last handle */
 335                         if (old)
 336                                 last_handle = a->handle;
 337                         continue;
 338                 }
 339
 340                 if (last_size && (last_size != a->len))
 341                         break;
 342
 343                 status = att_check_reqs(channel, ATT_OP_READ_BY_GROUP_REQ,
 344                                                                 a->read_reqs);
 345
 346                 client_attr = client_cfg_attribute(channel, a, a->data, a->len);
 347                 if (client_attr)
 348                         a = client_attr;
 349
 350                 if (status == 0x00 && a->read_cb)
 351                         status = a->read_cb(a, a->cb_user_data);
 352
 353                 if (status) {
 354                         g_slist_foreach(groups, (GFunc) g_free, NULL);
 355                         g_slist_free(groups);
 356                         return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ,
 357                                                 a->handle, status, pdu, len);
 358                 }
 359
 360                 cur = g_new0(struct group_elem, 1);
 361                 cur->handle = a->handle;
 362                 cur->data = a->data;
 363                 cur->len = a->len;
 364
 365                 /* Attribute Grouping Type found */
 366                 groups = g_slist_append(groups, cur);
 367
 368                 last_size = a->len;
 369                 old = cur;
 370                 last_handle = cur->handle;
 371         }
 372
 373         if (groups == NULL)
 374                 return enc_error_resp(ATT_OP_READ_BY_GROUP_REQ, start,
 375                                         ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
 376
 377         if (l == NULL)
 378                 cur->end = a->handle;
 379         else
 380                 cur->end = last_handle;
 381
 382         length = g_slist_length(groups);
 383
 384         adl = att_data_list_alloc(length, last_size + 4);
 385
 386         for (i = 0, l = groups; l; l = l->next, i++) {
 387                 uint8_t *value;
 388
 389                 cur = l->data;
 390
 391                 value = (void *) adl->data[i];
 392
 393                 att_put_u16(cur->handle, value);
 394                 att_put_u16(cur->end, &value[2]);
 395                 /* Attribute Value */
 396                 memcpy(&value[4], cur->data, cur->len);
 397         }
 398
 399         length = enc_read_by_grp_resp(adl, pdu, len);
 400
 401         att_data_list_free(adl);
 402         g_slist_foreach(groups, (GFunc) g_free, NULL);
 403         g_slist_free(groups);
 404
 405         return length;
 406 }
 407
 408 static uint16_t read_by_type(struct gatt_channel *channel, uint16_t start,
 409                                                 uint16_t end, bt_uuid_t *uuid,
 410                                                 uint8_t *pdu, int len)
 411 {
 412         struct att_data_list *adl;
 413         GSList *l, *types;
 414         struct attribute *a;
 415         uint16_t num, length;
 416         uint8_t status;
 417         int i;
 418
 419         if (start > end || start == 0x0000)
 420                 return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ, start,
 421                                         ATT_ECODE_INVALID_HANDLE, pdu, len);
 422
 423         for (l = database, length = 0, types = NULL; l; l = l->next) {
 424                 struct attribute *client_attr;
 425
 426                 a = l->data;
 427
 428                 if (a->handle < start)
 429                         continue;
 430
 431                 if (a->handle >= end)
 432                         break;
 433
 434                 if (bt_uuid_cmp(&a->uuid, uuid)  != 0)
 435                         continue;
 436
 437                 status = att_check_reqs(channel, ATT_OP_READ_BY_TYPE_REQ,
 438                                                                 a->read_reqs);
 439
 440                 client_attr = client_cfg_attribute(channel, a, a->data, a->len);
 441                 if (client_attr)
 442                         a = client_attr;
 443
 444                 if (status == 0x00 && a->read_cb)
 445                         status = a->read_cb(a, a->cb_user_data);
 446
 447                 if (status) {
 448                         g_slist_free(types);
 449                         return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ,
 450                                                 a->handle, status, pdu, len);
 451                 }
 452
 453                 /* All elements must have the same length */
 454                 if (length == 0)
 455                         length = a->len;
 456                 else if (a->len != length)
 457                         break;
 458
 459                 types = g_slist_append(types, a);
 460         }
 461
 462         if (types == NULL)
 463                 return enc_error_resp(ATT_OP_READ_BY_TYPE_REQ, start,
 464                                         ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
 465
 466         num = g_slist_length(types);
 467
 468         /* Handle length plus attribute value length */
 469         length += 2;
 470
 471         adl = att_data_list_alloc(num, length);
 472
 473         for (i = 0, l = types; l; i++, l = l->next) {
 474                 uint8_t *value;
 475
 476                 a = l->data;
 477
 478                 value = (void *) adl->data[i];
 479
 480                 att_put_u16(a->handle, value);
 481
 482                 /* Attribute Value */
 483                 memcpy(&value[2], a->data, a->len);
 484         }
 485
 486         length = enc_read_by_type_resp(adl, pdu, len);
 487
 488         att_data_list_free(adl);
 489         g_slist_free(types);
 490
 491         return length;
 492 }
 493
 494 static int find_info(uint16_t start, uint16_t end, uint8_t *pdu, int len)
 495 {
 496         struct attribute *a;
 497         struct att_data_list *adl;
 498         GSList *l, *info;
 499         uint8_t format, last_type = BT_UUID_UNSPEC;
 500         uint16_t length, num;
 501         int i;
 502
 503         if (start > end || start == 0x0000)
 504                 return enc_error_resp(ATT_OP_FIND_INFO_REQ, start,
 505                                         ATT_ECODE_INVALID_HANDLE, pdu, len);
 506
 507         for (l = database, info = NULL, num = 0; l; l = l->next) {
 508                 a = l->data;
 509
 510                 if (a->handle < start)
 511                         continue;
 512
 513                 if (a->handle > end)
 514                         break;
 515
 516                 if (last_type == BT_UUID_UNSPEC)
 517                         last_type = a->uuid.type;
 518
 519                 if (a->uuid.type != last_type)
 520                         break;
 521
 522                 info = g_slist_append(info, a);
 523                 num++;
 524
 525                 last_type = a->uuid.type;
 526         }
 527
 528         if (info == NULL)
 529                 return enc_error_resp(ATT_OP_FIND_INFO_REQ, start,
 530                                         ATT_ECODE_ATTR_NOT_FOUND, pdu, len);
 531
 532         if (last_type == BT_UUID16) {
 533                 length = 2;
 534                 format = 0x01;
 535         } else if (last_type == BT_UUID128) {
 536                 length = 16;
 537                 format = 0x02;
 538         }
 539
 540         adl = att_data_list_alloc(num, length + 2);
 541
 542         for (i = 0, l = info; l; i++, l = l->next) {
 543                 uint8_t *value;
 544
 545                 a = l->data;
 546
 547                 value = (void *) adl->data[i];
 548
 549                 att_put_u16(a->handle, value);
 550
 551                 /* Attribute Value */
 552                 att_put_uuid(a->uuid, &value[2]);
 553         }
 554
 555         length = enc_find_info_resp(format, adl, pdu, len);
 556
 557         att_data_list_free(adl);
 558         g_slist_free(info);
 559
 560         return length;
 561 }
 562
 563 static int find_by_type(uint16_t start, uint16_t end, bt_uuid_t *uuid,
 564                         const uint8_t *value, int vlen, uint8_t *opdu, int mtu)
 565 {
 566         struct attribute *a;
 567         struct att_range *range;
 568         GSList *l, *matches;
 569         int len;
 570
 571         if (start > end || start == 0x0000)
 572                 return enc_error_resp(ATT_OP_FIND_BY_TYPE_REQ, start,
 573                                         ATT_ECODE_INVALID_HANDLE, opdu, mtu);
 574
 575         /* Searching first requested handle number */
 576         for (l = database, matches = NULL, range = NULL; l; l = l->next) {
 577                 a = l->data;
 578
 579                 if (a->handle < start)
 580                         continue;
 581
 582                 if (a->handle > end)
 583                         break;
 584
 585                 /* Primary service? Attribute value matches? */
 586                 if ((bt_uuid_cmp(&a->uuid, uuid) == 0) && (a->len == vlen) &&
 587                                         (memcmp(a->data, value, vlen) == 0)) {
 588
 589                         range = g_new0(struct att_range, 1);
 590                         range->start = a->handle;
 591                         /* It is allowed to have end group handle the same as
 592                          * start handle, for groups with only one attribute. */
 593                         range->end = a->handle;
 594
 595                         matches = g_slist_append(matches, range);
 596                 } else if (range) {
 597                         /* Update the last found handle or reset the pointer
 598                          * to track that a new group started: Primary or
 599                          * Secondary service. */
 600                         if (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
 601                                         bt_uuid_cmp(&a->uuid, &snd_uuid) == 0)
 602                                 range = NULL;
 603                         else
 604                                 range->end = a->handle;
 605                 }
 606         }
 607
 608         if (matches == NULL)
 609                 return enc_error_resp(ATT_OP_FIND_BY_TYPE_REQ, start,
 610                                 ATT_ECODE_ATTR_NOT_FOUND, opdu, mtu);
 611
 612         len = enc_find_by_type_resp(matches, opdu, mtu);
 613
 614         g_slist_foreach(matches, (GFunc) g_free, NULL);
 615         g_slist_free(matches);
 616
 617         return len;
 618 }
 619
 620 static struct attribute *find_primary_range(uint16_t start, uint16_t *end)
 621 {
 622         struct attribute *attrib;
 623         guint h = start;
 624         GSList *l;
 625
 626         if (end == NULL)
 627                 return NULL;
 628
 629         l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
 630         if (!l)
 631                 return NULL;
 632
 633         attrib = l->data;
 634
 635         if (bt_uuid_cmp(&attrib->uuid, &prim_uuid) != 0)
 636                 return NULL;
 637
 638         *end = start;
 639
 640         for (l = l->next; l; l = l->next) {
 641                 struct attribute *a = l->data;
 642
 643                 if (bt_uuid_cmp(&a->uuid, &prim_uuid) == 0 ||
 644                                 bt_uuid_cmp(&a->uuid, &snd_uuid) == 0)
 645                         break;
 646
 647                 *end = a->handle;
 648         }
 649
 650         return attrib;
 651 }
 652
 653 static uint16_t read_value(struct gatt_channel *channel, uint16_t handle,
 654                                                         uint8_t *pdu, int len)
 655 {
 656         struct attribute *a, *client_attr;
 657         uint8_t status;
 658         GSList *l;
 659         guint h = handle;
 660
 661         l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
 662         if (!l)
 663                 return enc_error_resp(ATT_OP_READ_REQ, handle,
 664                                         ATT_ECODE_INVALID_HANDLE, pdu, len);
 665
 666         a = l->data;
 667
 668         status = att_check_reqs(channel, ATT_OP_READ_REQ, a->read_reqs);
 669
 670         client_attr = client_cfg_attribute(channel, a, a->data, a->len);
 671         if (client_attr)
 672                 a = client_attr;
 673
 674         if (status == 0x00 && a->read_cb)
 675                 status = a->read_cb(a, a->cb_user_data);
 676
 677         if (status)
 678                 return enc_error_resp(ATT_OP_READ_REQ, handle, status, pdu,
 679                                                                         len);
 680
 681         return enc_read_resp(a->data, a->len, pdu, len);
 682 }
 683
 684 static uint16_t read_blob(struct gatt_channel *channel, uint16_t handle,
 685                                         uint16_t offset, uint8_t *pdu, int len)
 686 {
 687         struct attribute *a, *client_attr;
 688         uint8_t status;
 689         GSList *l;
 690         guint h = handle;
 691
 692         l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
 693         if (!l)
 694                 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle,
 695                                         ATT_ECODE_INVALID_HANDLE, pdu, len);
 696
 697         a = l->data;
 698
 699         if (a->len <= offset)
 700                 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle,
 701                                         ATT_ECODE_INVALID_OFFSET, pdu, len);
 702
 703         status = att_check_reqs(channel, ATT_OP_READ_BLOB_REQ, a->read_reqs);
 704
 705         client_attr = client_cfg_attribute(channel, a, a->data, a->len);
 706         if (client_attr)
 707                 a = client_attr;
 708
 709         if (status == 0x00 && a->read_cb)
 710                 status = a->read_cb(a, a->cb_user_data);
 711
 712         if (status)
 713                 return enc_error_resp(ATT_OP_READ_BLOB_REQ, handle, status,
 714                                                                 pdu, len);
 715
 716         return enc_read_blob_resp(a->data, a->len, offset, pdu, len);
 717 }
 718
 719 static uint16_t write_value(struct gatt_channel *channel, uint16_t handle,
 720                                                 const uint8_t *value, int vlen,
 721                                                 uint8_t *pdu, int len)
 722 {
 723         struct attribute *a, *client_attr;
 724         uint8_t status;
 725         GSList *l;
 726         guint h = handle;
 727
 728         l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
 729         if (!l)
 730                 return enc_error_resp(ATT_OP_WRITE_REQ, handle,
 731                                 ATT_ECODE_INVALID_HANDLE, pdu, len);
 732
 733         a = l->data;
 734
 735         status = att_check_reqs(channel, ATT_OP_WRITE_REQ, a->write_reqs);
 736         if (status)
 737                 return enc_error_resp(ATT_OP_WRITE_REQ, handle, status, pdu,
 738                                                                         len);
 739
 740         client_attr = client_cfg_attribute(channel, a, value, vlen);
 741         if (client_attr)
 742                 a = client_attr;
 743         else
 744                 attrib_db_update(a->handle, &a->uuid, value, vlen);
 745
 746         if (a->write_cb) {
 747                 status = a->write_cb(a, a->cb_user_data);
 748                 if (status)
 749                         return enc_error_resp(ATT_OP_WRITE_REQ, handle, status,
 750                                                                 pdu, len);
 751         }
 752
 753         DBG("Notifications: %d, indications: %d",
 754                                         g_slist_length(channel->notify),
 755                                         g_slist_length(channel->indicate));
 756
 757         return enc_write_resp(pdu, len);
 758 }
 759
 760 static uint16_t mtu_exchange(struct gatt_channel *channel, uint16_t mtu,
 761                 uint8_t *pdu, int len)
 762 {
 763         guint old_mtu = channel->mtu;
 764
 765         if (mtu < ATT_DEFAULT_LE_MTU)
 766                 channel->mtu = ATT_DEFAULT_LE_MTU;
 767         else
 768                 channel->mtu = MIN(mtu, channel->mtu);
 769
 770         bt_io_set(le_io, BT_IO_L2CAP, NULL,
 771                         BT_IO_OPT_OMTU, channel->mtu,
 772                         BT_IO_OPT_INVALID);
 773
 774         return enc_mtu_resp(old_mtu, pdu, len);
 775 }
 776
 777 static void channel_disconnect(void *user_data)
 778 {
 779         struct gatt_channel *channel = user_data;
 780
 781         g_attrib_unref(channel->attrib);
 782         clients = g_slist_remove(clients, channel);
 783
 784         g_slist_free(channel->notify);
 785         g_slist_free(channel->indicate);
 786         g_slist_foreach(channel->configs, (GFunc) g_free, NULL);
 787         g_slist_free(channel->configs);
 788
 789         g_free(channel);
 790 }
 791
 792 static void channel_handler(const uint8_t *ipdu, uint16_t len,
 793                                                         gpointer user_data)
 794 {
 795         struct gatt_channel *channel = user_data;
 796         uint8_t opdu[ATT_MAX_MTU], value[ATT_MAX_MTU];
 797         uint16_t length, start, end, mtu, offset;
 798         bt_uuid_t uuid;
 799         uint8_t status = 0;
 800         int vlen;
 801
 802         DBG("op 0x%02x", ipdu[0]);
 803
 804         switch (ipdu[0]) {
 805         case ATT_OP_READ_BY_GROUP_REQ:
 806                 length = dec_read_by_grp_req(ipdu, len, &start, &end, &uuid);
 807                 if (length == 0) {
 808                         status = ATT_ECODE_INVALID_PDU;
 809                         goto done;
 810                 }
 811
 812                 length = read_by_group(channel, start, end, &uuid, opdu,
 813                                                                 channel->mtu);
 814                 break;
 815         case ATT_OP_READ_BY_TYPE_REQ:
 816                 length = dec_read_by_type_req(ipdu, len, &start, &end, &uuid);
 817                 if (length == 0) {
 818                         status = ATT_ECODE_INVALID_PDU;
 819                         goto done;
 820                 }
 821
 822                 length = read_by_type(channel, start, end, &uuid, opdu,
 823                                                                 channel->mtu);
 824                 break;
 825         case ATT_OP_READ_REQ:
 826                 length = dec_read_req(ipdu, len, &start);
 827                 if (length == 0) {
 828                         status = ATT_ECODE_INVALID_PDU;
 829                         goto done;
 830                 }
 831
 832                 length = read_value(channel, start, opdu, channel->mtu);
 833                 break;
 834         case ATT_OP_READ_BLOB_REQ:
 835                 length = dec_read_blob_req(ipdu, len, &start, &offset);
 836                 if (length == 0) {
 837                         status = ATT_ECODE_INVALID_PDU;
 838                         goto done;
 839                 }
 840
 841                 length = read_blob(channel, start, offset, opdu, channel->mtu);
 842                 break;
 843         case ATT_OP_MTU_REQ:
 844                 if (!channel->le) {
 845                         status = ATT_ECODE_REQ_NOT_SUPP;
 846                         goto done;
 847                 }
 848
 849                 length = dec_mtu_req(ipdu, len, &mtu);
 850                 if (length == 0) {
 851                         status = ATT_ECODE_INVALID_PDU;
 852                         goto done;
 853                 }
 854
 855                 length = mtu_exchange(channel, mtu, opdu, channel->mtu);
 856                 break;
 857         case ATT_OP_FIND_INFO_REQ:
 858                 length = dec_find_info_req(ipdu, len, &start, &end);
 859                 if (length == 0) {
 860                         status = ATT_ECODE_INVALID_PDU;
 861                         goto done;
 862                 }
 863
 864                 length = find_info(start, end, opdu, channel->mtu);
 865                 break;
 866         case ATT_OP_WRITE_REQ:
 867                 length = dec_write_req(ipdu, len, &start, value, &vlen);
 868                 if (length == 0) {
 869                         status = ATT_ECODE_INVALID_PDU;
 870                         goto done;
 871                 }
 872
 873                 length = write_value(channel, start, value, vlen, opdu,
 874                                                                 channel->mtu);
 875                 break;
 876         case ATT_OP_WRITE_CMD:
 877                 length = dec_write_cmd(ipdu, len, &start, value, &vlen);
 878                 if (length > 0)
 879                         write_value(channel, start, value, vlen, opdu,
 880                                                                 channel->mtu);
 881                 return;
 882         case ATT_OP_FIND_BY_TYPE_REQ:
 883                 length = dec_find_by_type_req(ipdu, len, &start, &end,
 884                                                         &uuid, value, &vlen);
 885                 if (length == 0) {
 886                         status = ATT_ECODE_INVALID_PDU;
 887                         goto done;
 888                 }
 889
 890                 length = find_by_type(start, end, &uuid, value, vlen,
 891                                                         opdu, channel->mtu);
 892                 break;
 893         case ATT_OP_HANDLE_CNF:
 894                 return;
 895         case ATT_OP_READ_MULTI_REQ:
 896         case ATT_OP_PREP_WRITE_REQ:
 897         case ATT_OP_EXEC_WRITE_REQ:
 898         default:
 899                 DBG("Unsupported request 0x%02x", ipdu[0]);
 900                 status = ATT_ECODE_REQ_NOT_SUPP;
 901                 goto done;
 902         }
 903
 904         if (length == 0)
 905                 status = ATT_ECODE_IO;
 906
 907 done:
 908         if (status)
 909                 length = enc_error_resp(ipdu[0], 0x0000, status, opdu,
 910                                                                 channel->mtu);
 911
 912         g_attrib_send(channel->attrib, 0, opdu[0], opdu, length,
 913                                                         NULL, NULL, NULL);
 914 }
 915
 916 static void connect_event(GIOChannel *io, GError *err, void *user_data)
 917 {
 918         struct gatt_channel *channel;
 919         uint16_t cid;
 920         GError *gerr = NULL;
 921
 922         if (err) {
 923                 error("%s", err->message);
 924                 return;
 925         }
 926
 927         channel = g_new0(struct gatt_channel, 1);
 928
 929         bt_io_get(io, BT_IO_L2CAP, &gerr,
 930                         BT_IO_OPT_SOURCE_BDADDR, &channel->src,
 931                         BT_IO_OPT_DEST_BDADDR, &channel->dst,
 932                         BT_IO_OPT_CID, &cid,
 933                         BT_IO_OPT_OMTU, &channel->mtu,
 934                         BT_IO_OPT_INVALID);
 935         if (gerr) {
 936                 error("bt_io_get: %s", gerr->message);
 937                 g_error_free(gerr);
 938                 g_free(channel);
 939                 g_io_channel_shutdown(io, TRUE, NULL);
 940                 return;
 941         }
 942
 943         if (channel->mtu > ATT_MAX_MTU)
 944                 channel->mtu = ATT_MAX_MTU;
 945
 946         if (cid != GATT_CID)
 947                 channel->le = FALSE;
 948         else
 949                 channel->le = TRUE;
 950
 951         channel->attrib = g_attrib_new(io);
 952         g_io_channel_unref(io);
 953
 954         channel->id = g_attrib_register(channel->attrib, GATTRIB_ALL_EVENTS,
 955                                 channel_handler, channel, NULL);
 956
 957         g_attrib_set_disconnect_function(channel->attrib, channel_disconnect,
 958                                                                 channel);
 959
 960         clients = g_slist_append(clients, channel);
 961 }
 962
 963 static void confirm_event(GIOChannel *io, void *user_data)
 964 {
 965         GError *gerr = NULL;
 966
 967         if (bt_io_accept(io, connect_event, user_data, NULL, &gerr) == FALSE) {
 968                 error("bt_io_accept: %s", gerr->message);
 969                 g_error_free(gerr);
 970                 g_io_channel_unref(io);
 971         }
 972
 973         return;
 974 }
 975
 976 static void attrib_notify_clients(struct attribute *attr)
 977 {
 978         guint handle = attr->handle;
 979         GSList *l;
 980
 981         for (l = clients; l; l = l->next) {
 982                 struct gatt_channel *channel = l->data;
 983
 984                 /* Notification */
 985                 if (g_slist_find_custom(channel->notify,
 986                                         GUINT_TO_POINTER(handle), handle_cmp)) {
 987                         uint8_t pdu[ATT_MAX_MTU];
 988                         uint16_t len;
 989
 990                         len = enc_notification(attr, pdu, channel->mtu);
 991                         if (len == 0)
 992                                 continue;
 993
 994                         g_attrib_send(channel->attrib, 0, pdu[0], pdu, len,
 995                                                         NULL, NULL, NULL);
 996                 }
 997
 998                 /* Indication */
 999                 if (g_slist_find_custom(channel->indicate,
1000                                         GUINT_TO_POINTER(handle), handle_cmp)) {
1001                         uint8_t pdu[ATT_MAX_MTU];
1002                         uint16_t len;
1003
1004                         len = enc_indication(attr, pdu, channel->mtu);
1005                         if (len == 0)
1006                                 return;
1007
1008                         g_attrib_send(channel->attrib, 0, pdu[0], pdu, len,
1009                                                         NULL, NULL, NULL);
1010                 }
1011         }
1012 }
1013
1014 static gboolean register_core_services(void)
1015 {
1016         uint8_t atval[256];
1017         bt_uuid_t uuid;
1018         uint16_t appearance = 0x0000;
1019
1020         /* GAP service: primary service definition */
1021         bt_uuid16_create(&uuid, GATT_PRIM_SVC_UUID);
1022         att_put_u16(GENERIC_ACCESS_PROFILE_ID, &atval[0]);
1023         attrib_db_add(0x0001, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 2);
1024
1025         /* GAP service: device name characteristic */
1026         name_handle = 0x0006;
1027         bt_uuid16_create(&uuid, GATT_CHARAC_UUID);
1028         atval[0] = ATT_CHAR_PROPER_READ;
1029         att_put_u16(name_handle, &atval[1]);
1030         att_put_u16(GATT_CHARAC_DEVICE_NAME, &atval[3]);
1031         attrib_db_add(0x0004, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 5);
1032
1033         /* GAP service: device name attribute */
1034         bt_uuid16_create(&uuid, GATT_CHARAC_DEVICE_NAME);
1035         attrib_db_add(name_handle, &uuid, ATT_NONE, ATT_NOT_PERMITTED,
1036                                                                 NULL, 0);
1037
1038         /* GAP service: device appearance characteristic */
1039         appearance_handle = 0x0008;
1040         bt_uuid16_create(&uuid, GATT_CHARAC_UUID);
1041         atval[0] = ATT_CHAR_PROPER_READ;
1042         att_put_u16(appearance_handle, &atval[1]);
1043         att_put_u16(GATT_CHARAC_APPEARANCE, &atval[3]);
1044         attrib_db_add(0x0007, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 5);
1045
1046         /* GAP service: device appearance attribute */
1047         bt_uuid16_create(&uuid, GATT_CHARAC_APPEARANCE);
1048         att_put_u16(appearance, &atval[0]);
1049         attrib_db_add(appearance_handle, &uuid, ATT_NONE, ATT_NOT_PERMITTED,
1050                                                                 atval, 2);
1051         gap_sdp_handle = attrib_create_sdp(0x0001, "Generic Access Profile");
1052         if (gap_sdp_handle == 0) {
1053                 error("Failed to register GAP service record");
1054                 return FALSE;
1055         }
1056
1057         /* GATT service: primary service definition */
1058         bt_uuid16_create(&uuid, GATT_PRIM_SVC_UUID);
1059         att_put_u16(GENERIC_ATTRIB_PROFILE_ID, &atval[0]);
1060         attrib_db_add(0x0010, &uuid, ATT_NONE, ATT_NOT_PERMITTED, atval, 2);
1061
1062         gatt_sdp_handle = attrib_create_sdp(0x0010,
1063                                                 "Generic Attribute Profile");
1064         if (gatt_sdp_handle == 0) {
1065                 error("Failed to register GATT service record");
1066                 goto failed;
1067         }
1068
1069         return TRUE;
1070
1071 failed:
1072         remove_record_from_server(gap_sdp_handle);
1073
1074         return FALSE;
1075 }
1076
1077 int attrib_server_init(void)
1078 {
1079         GError *gerr = NULL;
1080
1081         /* BR/EDR socket */
1082         l2cap_io = bt_io_listen(BT_IO_L2CAP, NULL, confirm_event,
1083                                         NULL, NULL, &gerr,
1084                                         BT_IO_OPT_SOURCE_BDADDR, BDADDR_ANY,
1085                                         BT_IO_OPT_PSM, GATT_PSM,
1086                                         BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
1087                                         BT_IO_OPT_INVALID);
1088         if (l2cap_io == NULL) {
1089                 error("%s", gerr->message);
1090                 g_error_free(gerr);
1091                 return -1;
1092         }
1093
1094         if (!register_core_services())
1095                 goto failed;
1096
1097         if (!main_opts.le)
1098                 return 0;
1099
1100         /* LE socket */
1101         le_io = bt_io_listen(BT_IO_L2CAP, NULL, confirm_event,
1102                                         &le_io, NULL, &gerr,
1103                                         BT_IO_OPT_SOURCE_BDADDR, BDADDR_ANY,
1104                                         BT_IO_OPT_CID, GATT_CID,
1105                                         BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
1106                                         BT_IO_OPT_INVALID);
1107         if (le_io == NULL) {
1108                 error("%s", gerr->message);
1109                 g_error_free(gerr);
1110                 /* Doesn't have LE support, continue */
1111         }
1112
1113         return 0;
1114
1115 failed:
1116         g_io_channel_unref(l2cap_io);
1117         l2cap_io = NULL;
1118
1119         if (le_io) {
1120                 g_io_channel_unref(le_io);
1121                 le_io = NULL;
1122         }
1123
1124         return -1;
1125 }
1126
1127 void attrib_server_exit(void)
1128 {
1129         GSList *l;
1130
1131         g_slist_foreach(database, (GFunc) g_free, NULL);
1132         g_slist_free(database);
1133
1134         if (l2cap_io) {
1135                 g_io_channel_unref(l2cap_io);
1136                 g_io_channel_shutdown(l2cap_io, FALSE, NULL);
1137         }
1138
1139         if (le_io) {
1140                 g_io_channel_unref(le_io);
1141                 g_io_channel_shutdown(le_io, FALSE, NULL);
1142         }
1143
1144         for (l = clients; l; l = l->next) {
1145                 struct gatt_channel *channel = l->data;
1146
1147                 g_slist_free(channel->notify);
1148                 g_slist_free(channel->indicate);
1149                 g_slist_foreach(channel->configs, (GFunc) g_free, NULL);
1150                 g_slist_free(channel->configs);
1151
1152                 g_attrib_unref(channel->attrib);
1153                 g_free(channel);
1154         }
1155
1156         g_slist_free(clients);
1157
1158         if (gatt_sdp_handle)
1159                 remove_record_from_server(gatt_sdp_handle);
1160
1161         if (gap_sdp_handle)
1162                 remove_record_from_server(gap_sdp_handle);
1163 }
1164
1165 uint32_t attrib_create_sdp(uint16_t handle, const char *name)
1166 {
1167         sdp_record_t *record;
1168         struct attribute *a;
1169         uint16_t end = 0;
1170         uuid_t svc, gap_uuid;
1171
1172         a = find_primary_range(handle, &end);
1173
1174         if (a == NULL)
1175                 return 0;
1176
1177         if (a->len == 2)
1178                 sdp_uuid16_create(&svc, att_get_u16(a->data));
1179         else if (a->len == 16)
1180                 sdp_uuid128_create(&svc, a->data);
1181         else
1182                 return 0;
1183
1184         record = server_record_new(&svc, handle, end);
1185         if (record == NULL)
1186                 return 0;
1187
1188         if (name)
1189                 sdp_set_info_attr(record, name, "BlueZ", NULL);
1190
1191         sdp_uuid16_create(&gap_uuid, GENERIC_ACCESS_PROFILE_ID);
1192         if (sdp_uuid_cmp(&svc, &gap_uuid) == 0) {
1193                 sdp_set_url_attr(record, "http://www.bluez.org/",
1194                                 "http://www.bluez.org/",
1195                                 "http://www.bluez.org/");
1196         }
1197
1198         if (add_record_to_server(BDADDR_ANY, record) < 0)
1199                 sdp_record_free(record);
1200         else
1201                 return record->handle;
1202
1203         return 0;
1204 }
1205
1206 void attrib_free_sdp(uint32_t sdp_handle)
1207 {
1208         remove_record_from_server(sdp_handle);
1209 }
1210
1211 struct attribute *attrib_db_add(uint16_t handle, bt_uuid_t *uuid, int read_reqs,
1212                                 int write_reqs, const uint8_t *value, int len)
1213 {
1214         struct attribute *a;
1215
1216         /* FIXME: handle conflicts */
1217
1218         a = g_malloc0(sizeof(struct attribute) + len);
1219         a->handle = handle;
1220         memcpy(&a->uuid, uuid, sizeof(bt_uuid_t));
1221         a->read_reqs = read_reqs;
1222         a->write_reqs = write_reqs;
1223         a->len = len;
1224         memcpy(a->data, value, len);
1225
1226         database = g_slist_insert_sorted(database, a, attribute_cmp);
1227
1228         return a;
1229 }
1230
1231 int attrib_db_update(uint16_t handle, bt_uuid_t *uuid, const uint8_t *value,
1232                                                                 int len)
1233 {
1234         struct attribute *a;
1235         GSList *l;
1236         guint h = handle;
1237
1238         l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
1239         if (!l)
1240                 return -ENOENT;
1241
1242         a = g_try_realloc(l->data, sizeof(struct attribute) + len);
1243         if (a == NULL)
1244                 return -ENOMEM;
1245
1246         l->data = a;
1247         a->handle = handle;
1248         if (uuid != &a->uuid)
1249                 memcpy(&a->uuid, uuid, sizeof(bt_uuid_t));
1250         a->len = len;
1251         memcpy(a->data, value, len);
1252
1253         attrib_notify_clients(a);
1254
1255         return 0;
1256 }
1257
1258 int attrib_db_del(uint16_t handle)
1259 {
1260         struct attribute *a;
1261         GSList *l;
1262         guint h = handle;
1263
1264         l = g_slist_find_custom(database, GUINT_TO_POINTER(h), handle_cmp);
1265         if (!l)
1266                 return -ENOENT;
1267
1268         a = l->data;
1269         database = g_slist_remove(database, a);
1270         g_free(a);
1271
1272         return 0;
1273 }
1274
1275 int attrib_gap_set(uint16_t uuid, const uint8_t *value, int len)
1276 {
1277         bt_uuid_t u16;
1278         uint16_t handle;
1279
1280         /* FIXME: Missing Privacy and Reconnection Address */
1281
1282         bt_uuid16_create(&u16, uuid);
1283
1284         switch (uuid) {
1285         case GATT_CHARAC_DEVICE_NAME:
1286                 handle = name_handle;
1287                 break;
1288         case GATT_CHARAC_APPEARANCE:
1289                 handle = appearance_handle;
1290                 break;
1291         default:
1292                 return -ENOSYS;
1293         }
1294
1295         return attrib_db_update(handle, &u16, value, len);
1296 }