android/gatt.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2014  Intel Corporation. All rights reserved.
   6  *
   7  *
   8  *  This library is free software; you can redistribute it and/or
   9  *  modify it under the terms of the GNU Lesser General Public
  10  *  License as published by the Free Software Foundation; either
  11  *  version 2.1 of the License, or (at your option) any later version.
  12  *
  13  *  This library is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  *  Lesser General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU Lesser General Public
  19  *  License along with this library; if not, write to the Free Software
  20  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  21  *
  22  */
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <stdbool.h>
  29 #include <stdlib.h>
  30 #include <stdint.h>
  31 #include <glib.h>
  32 #include <errno.h>
  33 #include <sys/socket.h>
  34
  35 #include "ipc.h"
  36 #include "ipc-common.h"
  37 #include "lib/sdp.h"
  38 #include "lib/sdp_lib.h"
  39 #include "lib/uuid.h"
  40 #include "bluetooth.h"
  41 #include "gatt.h"
  42 #include "src/log.h"
  43 #include "hal-msg.h"
  44 #include "utils.h"
  45 #include "src/shared/util.h"
  46 #include "src/shared/queue.h"
  47 #include "src/shared/att.h"
  48 #include "src/shared/gatt-db.h"
  49 #include "attrib/gattrib.h"
  50 #include "attrib/att.h"
  51 #include "attrib/gatt.h"
  52 #include "btio/btio.h"
  53
  54 /* set according to Android bt_gatt_client.h */
  55 #define GATT_MAX_ATTR_LEN 600
  56
  57 #define GATT_SUCCESS    0x00000000
  58 #define GATT_FAILURE    0x00000101
  59
  60 #define BASE_UUID16_OFFSET     12
  61
  62 #define GATT_PERM_READ                  0x00000001
  63 #define GATT_PERM_READ_ENCRYPTED        0x00000002
  64 #define GATT_PERM_READ_MITM             0x00000004
  65 #define GATT_PERM_READ_AUTHORIZATION    0x00000008
  66 #define GATT_PERM_WRITE                 0x00000100
  67 #define GATT_PERM_WRITE_ENCRYPTED       0x00000200
  68 #define GATT_PERM_WRITE_MITM            0x00000400
  69 #define GATT_PERM_WRITE_AUTHORIZATION   0x00000800
  70 #define GATT_PERM_WRITE_SIGNED          0x00010000
  71 #define GATT_PERM_WRITE_SIGNED_MITM     0x00020000
  72 #define GATT_PERM_NONE                  0x10000000
  73
  74 #define GATT_PAIR_CONN_TIMEOUT 30
  75 #define GATT_CONN_TIMEOUT 2
  76
  77 static const uint8_t BLUETOOTH_UUID[] = {
  78         0xfb, 0x34, 0x9b, 0x5f, 0x80, 0x00, 0x00, 0x80,
  79         0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
  80 };
  81
  82 typedef enum {
  83         DEVICE_DISCONNECTED = 0,
  84         DEVICE_CONNECT_INIT,            /* connection procedure initiated */
  85         DEVICE_CONNECT_READY,           /* dev found during LE scan */
  86         DEVICE_CONNECTED,               /* connection has been established */
  87 } gatt_device_state_t;
  88
  89 static const char *device_state_str[] = {
  90         "DISCONNECTED",
  91         "CONNECT INIT",
  92         "CONNECT READY",
  93         "CONNECTED",
  94 };
  95
  96 struct pending_trans_data {
  97         unsigned int id;
  98         uint8_t opcode;
  99         struct gatt_db_attribute *attrib;
 100         unsigned int serial_id;
 101 };
 102
 103 struct gatt_app {
 104         int32_t id;
 105         uint8_t uuid[16];
 106
 107         gatt_type_t type;
 108
 109         /* Valid for client applications */
 110         struct queue *notifications;
 111
 112         gatt_conn_cb_t func;
 113 };
 114
 115 struct element_id {
 116         bt_uuid_t uuid;
 117         uint8_t instance;
 118 };
 119
 120 struct descriptor {
 121         struct element_id id;
 122         uint16_t handle;
 123 };
 124
 125 struct characteristic {
 126         struct element_id id;
 127         struct gatt_char ch;
 128         uint16_t end_handle;
 129
 130         struct queue *descriptors;
 131 };
 132
 133 struct service {
 134         struct element_id id;
 135         struct gatt_primary prim;
 136         struct gatt_included incl;
 137
 138         bool primary;
 139
 140         struct queue *chars;
 141         struct queue *included; /* Valid only for primary services */
 142         bool incl_search_done;
 143 };
 144
 145 struct notification_data {
 146         struct hal_gatt_srvc_id service;
 147         struct hal_gatt_gatt_id ch;
 148         struct app_connection *conn;
 149         guint notif_id;
 150         guint ind_id;
 151         int ref;
 152 };
 153
 154 struct gatt_device {
 155         bdaddr_t bdaddr;
 156
 157         gatt_device_state_t state;
 158
 159         GAttrib *attrib;
 160         GIOChannel *att_io;
 161         struct queue *services;
 162         bool partial_srvc_search;
 163
 164         guint watch_id;
 165         guint server_id;
 166         guint ind_id;
 167
 168         int ref;
 169
 170         struct queue *autoconnect_apps;
 171
 172         struct queue *pending_requests;
 173 };
 174
 175 struct app_connection {
 176         struct gatt_device *device;
 177         struct gatt_app *app;
 178         struct queue *transactions;
 179         int32_t id;
 180
 181         guint timeout_id;
 182
 183         bool wait_execute_write;
 184 };
 185
 186 struct service_sdp {
 187         int32_t service_handle;
 188         uint32_t sdp_handle;
 189 };
 190
 191 static struct ipc *hal_ipc = NULL;
 192 static bdaddr_t adapter_addr;
 193 static bool scanning = false;
 194 static unsigned int advertising_cnt = 0;
 195
 196 static struct queue *gatt_apps = NULL;
 197 static struct queue *gatt_devices = NULL;
 198 static struct queue *app_connections = NULL;
 199
 200 static struct queue *services_sdp = NULL;
 201
 202 static struct queue *listen_apps = NULL;
 203 static struct gatt_db *gatt_db = NULL;
 204
 205 static struct gatt_db_attribute *service_changed_attrib = NULL;
 206
 207 static GIOChannel *le_io = NULL;
 208 static GIOChannel *bredr_io = NULL;
 209
 210 static uint32_t gatt_sdp_handle = 0;
 211 static uint32_t gap_sdp_handle = 0;
 212 static uint32_t dis_sdp_handle = 0;
 213
 214 static struct bt_crypto *crypto = NULL;
 215
 216 static int test_client_if = 0;
 217 static const uint8_t TEST_UUID[] = {
 218         0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
 219         0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x03, 0x04
 220 };
 221
 222 static bool is_bluetooth_uuid(const uint8_t *uuid)
 223 {
 224         int i;
 225
 226         for (i = 0; i < 16; i++) {
 227                 /* ignore minimal uuid (16) value */
 228                 if (i == 12 || i == 13)
 229                         continue;
 230
 231                 if (uuid[i] != BLUETOOTH_UUID[i])
 232                         return false;
 233         }
 234
 235         return true;
 236 }
 237
 238 static void android2uuid(const uint8_t *uuid, bt_uuid_t *dst)
 239 {
 240         if (is_bluetooth_uuid(uuid)) {
 241                 /* copy 16 bit uuid value from full android 128bit uuid */
 242                 dst->type = BT_UUID16;
 243                 dst->value.u16 = (uuid[13] << 8) + uuid[12];
 244         } else {
 245                 int i;
 246
 247                 dst->type = BT_UUID128;
 248                 for (i = 0; i < 16; i++)
 249                         dst->value.u128.data[i] = uuid[15 - i];
 250         }
 251 }
 252
 253 static void uuid2android(const bt_uuid_t *src, uint8_t *uuid)
 254 {
 255         bt_uuid_t uu128;
 256         uint8_t i;
 257
 258         if (src->type != BT_UUID128) {
 259                 bt_uuid_to_uuid128(src, &uu128);
 260                 src = &uu128;
 261         }
 262
 263         for (i = 0; i < 16; i++)
 264                 uuid[15 - i] = src->value.u128.data[i];
 265 }
 266
 267 static void hal_srvc_id_to_element_id(const struct hal_gatt_srvc_id *from,
 268                                                         struct element_id *to)
 269 {
 270         to->instance = from->inst_id;
 271         android2uuid(from->uuid, &to->uuid);
 272 }
 273
 274 static void element_id_to_hal_srvc_id(const struct element_id *from,
 275                                                 uint8_t primary,
 276                                                 struct hal_gatt_srvc_id *to)
 277 {
 278         to->is_primary = primary;
 279         to->inst_id = from->instance;
 280         uuid2android(&from->uuid, to->uuid);
 281 }
 282
 283 static void hal_gatt_id_to_element_id(const struct hal_gatt_gatt_id *from,
 284                                                         struct element_id *to)
 285 {
 286         to->instance = from->inst_id;
 287         android2uuid(from->uuid, &to->uuid);
 288 }
 289
 290 static void element_id_to_hal_gatt_id(const struct element_id *from,
 291                                                 struct hal_gatt_gatt_id *to)
 292 {
 293         to->inst_id = from->instance;
 294         uuid2android(&from->uuid, to->uuid);
 295 }
 296
 297 static void destroy_characteristic(void *data)
 298 {
 299         struct characteristic *chars = data;
 300
 301         if (!chars)
 302                 return;
 303
 304         queue_destroy(chars->descriptors, free);
 305         free(chars);
 306 }
 307
 308 static void destroy_service(void *data)
 309 {
 310         struct service *srvc = data;
 311
 312         if (!srvc)
 313                 return;
 314
 315         queue_destroy(srvc->chars, destroy_characteristic);
 316
 317         /*
 318          * Included services we keep on two queues.
 319          * 1. On the same queue with primary services.
 320          * 2. On the queue inside primary service.
 321          * So we need to free service memory only once but we need to destroy
 322          * two queues
 323          */
 324         queue_destroy(srvc->included, NULL);
 325
 326         free(srvc);
 327 }
 328
 329 static bool match_app_by_uuid(const void *data, const void *user_data)
 330 {
 331         const uint8_t *exp_uuid = user_data;
 332         const struct gatt_app *client = data;
 333
 334         return !memcmp(exp_uuid, client->uuid, sizeof(client->uuid));
 335 }
 336
 337 static bool match_app_by_id(const void *data, const void *user_data)
 338 {
 339         int32_t exp_id = PTR_TO_INT(user_data);
 340         const struct gatt_app *client = data;
 341
 342         return client->id == exp_id;
 343 }
 344
 345 static struct gatt_app *find_app_by_id(int32_t id)
 346 {
 347         return queue_find(gatt_apps, match_app_by_id, INT_TO_PTR(id));
 348 }
 349
 350 static bool match_device_by_bdaddr(const void *data, const void *user_data)
 351 {
 352         const struct gatt_device *dev = data;
 353         const bdaddr_t *addr = user_data;
 354
 355         return !bacmp(&dev->bdaddr, addr);
 356 }
 357
 358 static bool match_device_by_state(const void *data, const void *user_data)
 359 {
 360         const struct gatt_device *dev = data;
 361
 362         if (dev->state != PTR_TO_UINT(user_data))
 363                 return false;
 364
 365         return true;
 366 }
 367
 368 static bool match_pending_device(const void *data, const void *user_data)
 369 {
 370         const struct gatt_device *dev = data;
 371
 372         if ((dev->state == DEVICE_CONNECT_INIT) ||
 373                                         (dev->state == DEVICE_CONNECT_READY))
 374                 return true;
 375
 376         return false;
 377 }
 378
 379 static bool match_connection_by_id(const void *data, const void *user_data)
 380 {
 381         const struct app_connection *conn = data;
 382         const int32_t id = PTR_TO_INT(user_data);
 383
 384         return conn->id == id;
 385 }
 386
 387 static bool match_connection_by_device_and_app(const void *data,
 388                                                         const void *user_data)
 389 {
 390         const struct app_connection *conn = data;
 391         const struct app_connection *match = user_data;
 392
 393         return conn->device == match->device && conn->app == match->app;
 394 }
 395
 396 static struct app_connection *find_connection_by_id(int32_t conn_id)
 397 {
 398         struct app_connection *conn;
 399
 400         conn = queue_find(app_connections, match_connection_by_id,
 401                                                         INT_TO_PTR(conn_id));
 402         if (conn && conn->device->state == DEVICE_CONNECTED)
 403                 return conn;
 404
 405         return NULL;
 406 }
 407
 408 static bool match_connection_by_device(const void *data, const void *user_data)
 409 {
 410         const struct app_connection *conn = data;
 411         const struct gatt_device *dev = user_data;
 412
 413         return conn->device == dev;
 414 }
 415
 416 static bool match_connection_by_app(const void *data, const void *user_data)
 417 {
 418         const struct app_connection *conn = data;
 419         const struct gatt_app *app = user_data;
 420
 421         return conn->app == app;
 422 }
 423
 424 static struct gatt_device *find_device_by_addr(const bdaddr_t *addr)
 425 {
 426         return queue_find(gatt_devices, match_device_by_bdaddr, addr);
 427 }
 428
 429 static struct gatt_device *find_pending_device(void)
 430 {
 431         return queue_find(gatt_devices, match_pending_device, NULL);
 432 }
 433
 434 static struct gatt_device *find_device_by_state(uint32_t state)
 435 {
 436         return queue_find(gatt_devices, match_device_by_state,
 437                                                         UINT_TO_PTR(state));
 438 }
 439
 440 static bool match_srvc_by_element_id(const void *data, const void *user_data)
 441 {
 442         const struct element_id *exp_id = user_data;
 443         const struct service *service = data;
 444
 445         if (service->id.instance == exp_id->instance)
 446                 return !bt_uuid_cmp(&service->id.uuid, &exp_id->uuid);
 447
 448         return false;
 449 }
 450
 451 static bool match_srvc_by_higher_inst_id(const void *data,
 452                                                         const void *user_data)
 453 {
 454         const struct service *s = data;
 455         uint8_t inst_id = PTR_TO_INT(user_data);
 456
 457         /* For now we match inst_id as it is unique */
 458         return inst_id < s->id.instance;
 459 }
 460
 461 static bool match_srvc_by_bt_uuid(const void *data, const void *user_data)
 462 {
 463         const bt_uuid_t *exp_uuid = user_data;
 464         const struct service *service = data;
 465
 466         return !bt_uuid_cmp(exp_uuid, &service->id.uuid);
 467 }
 468
 469 static bool match_srvc_by_range(const void *data, const void *user_data)
 470 {
 471         const struct service *srvc = data;
 472         const struct att_range *range = user_data;
 473
 474         return !memcmp(&srvc->prim.range, range, sizeof(srvc->prim.range));
 475 }
 476
 477 static bool match_char_by_higher_inst_id(const void *data,
 478                                                         const void *user_data)
 479 {
 480         const struct characteristic *ch = data;
 481         uint8_t inst_id = PTR_TO_INT(user_data);
 482
 483         /* For now we match inst_id as it is unique, we'll match uuids later */
 484         return inst_id < ch->id.instance;
 485 }
 486
 487 static bool match_descr_by_element_id(const void *data, const void *user_data)
 488 {
 489         const struct element_id *exp_id = user_data;
 490         const struct descriptor *descr = data;
 491
 492         if (exp_id->instance == descr->id.instance)
 493                 return !bt_uuid_cmp(&descr->id.uuid, &exp_id->uuid);
 494
 495         return false;
 496 }
 497
 498 static bool match_descr_by_higher_inst_id(const void *data,
 499                                                         const void *user_data)
 500 {
 501         const struct descriptor *descr = data;
 502         uint8_t instance = PTR_TO_INT(user_data);
 503
 504         /* For now we match instance as it is unique */
 505         return instance < descr->id.instance;
 506 }
 507
 508 static bool match_notification(const void *a, const void *b)
 509 {
 510         const struct notification_data *a1 = a;
 511         const struct notification_data *b1 = b;
 512
 513         if (a1->conn != b1->conn)
 514                 return false;
 515
 516         if (memcmp(&a1->ch, &b1->ch, sizeof(a1->ch)))
 517                 return false;
 518
 519         if (memcmp(&a1->service, &b1->service, sizeof(a1->service)))
 520                 return false;
 521
 522         return true;
 523 }
 524
 525 static bool match_char_by_element_id(const void *data, const void *user_data)
 526 {
 527         const struct element_id *exp_id = user_data;
 528         const struct characteristic *chars = data;
 529
 530         if (exp_id->instance == chars->id.instance)
 531                 return !bt_uuid_cmp(&chars->id.uuid, &exp_id->uuid);
 532
 533         return false;
 534 }
 535
 536 static void destroy_notification(void *data)
 537 {
 538         struct notification_data *notification = data;
 539         struct gatt_app *app;
 540
 541         if (!notification)
 542                 return;
 543
 544         if (--notification->ref)
 545                 return;
 546
 547         app = notification->conn->app;
 548         queue_remove_if(app->notifications, match_notification, notification);
 549         free(notification);
 550 }
 551
 552 static void unregister_notification(void *data)
 553 {
 554         struct notification_data *notification = data;
 555         struct gatt_device *dev = notification->conn->device;
 556
 557         /*
 558          * No device means it was already disconnected and client cleanup was
 559          * triggered afterwards, but once client unregisters, device stays if
 560          * used by others. Then just unregister single handle.
 561          */
 562         if (!queue_find(gatt_devices, NULL, dev))
 563                 return;
 564
 565         if (notification->notif_id && dev)
 566                 g_attrib_unregister(dev->attrib, notification->notif_id);
 567
 568         if (notification->ind_id && dev)
 569                 g_attrib_unregister(dev->attrib, notification->ind_id);
 570 }
 571
 572 static void device_set_state(struct gatt_device *dev, uint32_t state)
 573 {
 574         char bda[18];
 575
 576         if (dev->state == state)
 577                 return;
 578
 579         ba2str(&dev->bdaddr, bda);
 580         DBG("gatt: Device %s state changed %s -> %s", bda,
 581                         device_state_str[dev->state], device_state_str[state]);
 582
 583         dev->state = state;
 584 }
 585
 586 static bool auto_connect_le(struct gatt_device *dev)
 587 {
 588         /*  For LE devices use auto connect feature if possible */
 589         if (bt_kernel_conn_control()) {
 590                 if (!bt_auto_connect_add(bt_get_id_addr(&dev->bdaddr, NULL)))
 591                         return false;
 592         } else {
 593                 /* Trigger discovery if not already started */
 594                 if (!scanning && !bt_le_discovery_start()) {
 595                         error("gatt: Could not start scan");
 596                         return false;
 597                 }
 598         }
 599
 600         device_set_state(dev, DEVICE_CONNECT_INIT);
 601         return true;
 602 }
 603
 604 static void connection_cleanup(struct gatt_device *device)
 605 {
 606         if (device->watch_id) {
 607                 g_source_remove(device->watch_id);
 608                 device->watch_id = 0;
 609         }
 610
 611         if (device->att_io) {
 612                 g_io_channel_shutdown(device->att_io, FALSE, NULL);
 613                 g_io_channel_unref(device->att_io);
 614                 device->att_io = NULL;
 615         }
 616
 617         if (device->attrib) {
 618                 GAttrib *attrib = device->attrib;
 619
 620                 if (device->server_id > 0)
 621                         g_attrib_unregister(device->attrib, device->server_id);
 622
 623                 if (device->ind_id > 0)
 624                         g_attrib_unregister(device->attrib, device->ind_id);
 625
 626                 device->attrib = NULL;
 627                 g_attrib_cancel_all(attrib);
 628                 g_attrib_unref(attrib);
 629         }
 630
 631         /*
 632          * If device was in connection_pending or connectable state we
 633          * search device list if we should stop the scan.
 634          */
 635         if (!scanning && (device->state == DEVICE_CONNECT_INIT ||
 636                                 device->state == DEVICE_CONNECT_READY)) {
 637                 if (!find_pending_device())
 638                         bt_le_discovery_stop(NULL);
 639         }
 640
 641         /* If device is not bonded service cache should be refreshed */
 642         if (!bt_device_is_bonded(&device->bdaddr))
 643                 queue_remove_all(device->services, NULL, NULL, destroy_service);
 644
 645         device_set_state(device, DEVICE_DISCONNECTED);
 646
 647         if (!queue_isempty(device->autoconnect_apps))
 648                 auto_connect_le(device);
 649         else
 650                 bt_auto_connect_remove(&device->bdaddr);
 651 }
 652
 653 static void destroy_gatt_app(void *data)
 654 {
 655         struct gatt_app *app = data;
 656
 657         if (!app)
 658                 return;
 659
 660         /*
 661          * First we want to get all notifications and unregister them.
 662          * We don't pass unregister_notification to queue_destroy,
 663          * because destroy notification performs operations on queue
 664          * too. So remove all elements and then destroy queue.
 665          */
 666
 667         if (app->type == GATT_CLIENT)
 668                 while (queue_peek_head(app->notifications)) {
 669                         struct notification_data *notification;
 670
 671                         notification = queue_pop_head(app->notifications);
 672                         unregister_notification(notification);
 673                 }
 674
 675         queue_destroy(app->notifications, free);
 676
 677         free(app);
 678 }
 679
 680 struct pending_request {
 681         struct gatt_db_attribute *attrib;
 682         int length;
 683         uint8_t *value;
 684         uint16_t offset;
 685
 686         uint8_t *filter_value;
 687         uint16_t filter_vlen;
 688
 689         bool completed;
 690         uint8_t error;
 691 };
 692
 693 static void destroy_pending_request(void *data)
 694 {
 695         struct pending_request *entry = data;
 696
 697         if (!entry)
 698                 return;
 699
 700         free(entry->value);
 701         free(entry->filter_value);
 702         free(entry);
 703 }
 704
 705 static void destroy_device(void *data)
 706 {
 707         struct gatt_device *dev = data;
 708
 709         if (!dev)
 710                 return;
 711
 712         queue_destroy(dev->services, destroy_service);
 713         queue_destroy(dev->pending_requests, destroy_pending_request);
 714         queue_destroy(dev->autoconnect_apps, NULL);
 715
 716         bt_auto_connect_remove(&dev->bdaddr);
 717
 718         free(dev);
 719 }
 720
 721 static struct gatt_device *device_ref(struct gatt_device *device)
 722 {
 723         if (!device)
 724                 return NULL;
 725
 726         device->ref++;
 727
 728         return device;
 729 }
 730
 731 static void device_unref(struct gatt_device *device)
 732 {
 733         if (!device)
 734                 return;
 735
 736         if (--device->ref)
 737                 return;
 738
 739         destroy_device(device);
 740 }
 741
 742 static struct gatt_device *create_device(const bdaddr_t *addr)
 743 {
 744         struct gatt_device *dev;
 745
 746         dev = new0(struct gatt_device, 1);
 747
 748         bacpy(&dev->bdaddr, addr);
 749
 750         dev->services = queue_new();
 751         dev->autoconnect_apps = queue_new();
 752         dev->pending_requests = queue_new();
 753
 754         queue_push_head(gatt_devices, dev);
 755
 756         return device_ref(dev);
 757 }
 758
 759 static void send_client_connect_status_notify(struct app_connection *conn,
 760                                                                 int32_t status)
 761 {
 762         struct hal_ev_gatt_client_connect ev;
 763
 764         if (conn->app->func) {
 765                 conn->app->func(&conn->device->bdaddr,
 766                                         status == GATT_SUCCESS ? 0 : -ENOTCONN,
 767                                         conn->device->attrib);
 768                 return;
 769         }
 770
 771         ev.client_if = conn->app->id;
 772         ev.conn_id = conn->id;
 773         ev.status = status;
 774
 775         bdaddr2android(&conn->device->bdaddr, &ev.bda);
 776
 777         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT, HAL_EV_GATT_CLIENT_CONNECT,
 778                                                         sizeof(ev), &ev);
 779 }
 780
 781 static void send_server_connection_state_notify(struct app_connection *conn,
 782                                                                 bool connected)
 783 {
 784         struct hal_ev_gatt_server_connection ev;
 785
 786         if (conn->app->func) {
 787                 conn->app->func(&conn->device->bdaddr,
 788                                         connected ? 0 : -ENOTCONN,
 789                                         conn->device->attrib);
 790                 return;
 791         }
 792
 793         ev.server_if = conn->app->id;
 794         ev.conn_id = conn->id;
 795         ev.connected = connected;
 796
 797         bdaddr2android(&conn->device->bdaddr, &ev.bdaddr);
 798
 799         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
 800                                 HAL_EV_GATT_SERVER_CONNECTION, sizeof(ev), &ev);
 801 }
 802
 803 static void send_client_disconnect_status_notify(struct app_connection *conn,
 804                                                                 int32_t status)
 805 {
 806         struct hal_ev_gatt_client_disconnect ev;
 807
 808         if (conn->app->func) {
 809                 conn->app->func(&conn->device->bdaddr, -ENOTCONN,
 810                                                 conn->device->attrib);
 811                 return;
 812         }
 813
 814         ev.client_if = conn->app->id;
 815         ev.conn_id = conn->id;
 816         ev.status = status;
 817
 818         bdaddr2android(&conn->device->bdaddr, &ev.bda);
 819
 820         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
 821                                 HAL_EV_GATT_CLIENT_DISCONNECT, sizeof(ev), &ev);
 822
 823 }
 824
 825 static void notify_app_disconnect_status(struct app_connection *conn,
 826                                                                 int32_t status)
 827 {
 828         if (!conn->app)
 829                 return;
 830
 831         if (conn->app->type == GATT_CLIENT)
 832                 send_client_disconnect_status_notify(conn, status);
 833         else
 834                 send_server_connection_state_notify(conn, !!status);
 835 }
 836
 837 static void notify_app_connect_status(struct app_connection *conn,
 838                                                                 int32_t status)
 839 {
 840         if (!conn->app)
 841                 return;
 842
 843         if (conn->app->type == GATT_CLIENT)
 844                 send_client_connect_status_notify(conn, status);
 845         else
 846                 send_server_connection_state_notify(conn, !status);
 847 }
 848
 849 static void destroy_connection(void *data)
 850 {
 851         struct app_connection *conn = data;
 852
 853         if (!conn)
 854                 return;
 855
 856         if (conn->timeout_id > 0)
 857                 g_source_remove(conn->timeout_id);
 858
 859         switch (conn->device->state) {
 860         case DEVICE_CONNECTED:
 861                 notify_app_disconnect_status(conn, GATT_SUCCESS);
 862                 break;
 863         case DEVICE_CONNECT_INIT:
 864         case DEVICE_CONNECT_READY:
 865                 notify_app_connect_status(conn, GATT_FAILURE);
 866                 break;
 867         case DEVICE_DISCONNECTED:
 868                 break;
 869         }
 870
 871         if (!queue_find(app_connections, match_connection_by_device,
 872                                                         conn->device))
 873                 connection_cleanup(conn->device);
 874
 875         queue_destroy(conn->transactions, free);
 876         device_unref(conn->device);
 877         free(conn);
 878 }
 879
 880 static gboolean disconnected_cb(GIOChannel *io, GIOCondition cond,
 881                                                         gpointer user_data)
 882 {
 883         struct gatt_device *dev = user_data;
 884         int sock, err = 0;
 885         socklen_t len;
 886
 887         sock = g_io_channel_unix_get_fd(io);
 888         len = sizeof(err);
 889         if (!getsockopt(sock, SOL_SOCKET, SO_ERROR, &err, &len))
 890                 DBG("%s (%d)", strerror(err), err);
 891
 892         queue_remove_all(app_connections, match_connection_by_device, dev,
 893                                                         destroy_connection);
 894
 895         return FALSE;
 896 }
 897
 898 static bool get_local_mtu(struct gatt_device *dev, uint16_t *mtu)
 899 {
 900         GIOChannel *io;
 901         uint16_t imtu, omtu;
 902
 903         io = g_attrib_get_channel(dev->attrib);
 904
 905         if (!bt_io_get(io, NULL, BT_IO_OPT_IMTU, &imtu, BT_IO_OPT_OMTU, &omtu,
 906                                                         BT_IO_OPT_INVALID)) {
 907                 error("gatt: Failed to get local MTU");
 908                 return false;
 909         }
 910
 911         /*
 912          * Limit MTU to  MIN(IMTU, OMTU). This is to avoid situation where
 913          * local OMTU < MIN(remote MTU, IMTU)
 914          */
 915         if (mtu)
 916                 *mtu = MIN(imtu, omtu);
 917
 918         return true;
 919 }
 920
 921 static void notify_client_mtu_change(struct app_connection *conn, bool success)
 922 {
 923         struct hal_ev_gatt_client_configure_mtu ev;
 924         size_t mtu;
 925
 926         g_attrib_get_buffer(conn->device->attrib, &mtu);
 927
 928         ev.conn_id = conn->id;
 929         ev.status = success ? GATT_SUCCESS : GATT_FAILURE;
 930         ev.mtu = mtu;
 931
 932         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
 933                         HAL_EV_GATT_CLIENT_CONFIGURE_MTU, sizeof(ev), &ev);
 934 }
 935
 936 static void notify_server_mtu(struct app_connection *conn)
 937 {
 938         struct hal_ev_gatt_server_mtu_changed ev;
 939         size_t mtu;
 940
 941         g_attrib_get_buffer(conn->device->attrib, &mtu);
 942
 943         ev.conn_id = conn->id;
 944         ev.mtu = mtu;
 945
 946         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
 947                         HAL_EV_GATT_SERVER_MTU_CHANGED, sizeof(ev), &ev);
 948 }
 949
 950 static void notify_mtu_change(void *data, void *user_data)
 951 {
 952         struct gatt_device *device = user_data;
 953         struct app_connection *conn = data;
 954
 955         if (conn->device != device)
 956                 return;
 957
 958         if (!conn->app) {
 959                 error("gatt: can't notify mtu - no app registered for conn");
 960                 return;
 961         }
 962
 963         switch (conn->app->type) {
 964         case GATT_CLIENT:
 965                 notify_client_mtu_change(conn, true);
 966                 break;
 967         case GATT_SERVER:
 968                 notify_server_mtu(conn);
 969                 break;
 970         default:
 971                 break;
 972         }
 973 }
 974
 975 static bool update_mtu(struct gatt_device *device, uint16_t rmtu)
 976 {
 977         uint16_t mtu, lmtu;
 978
 979         if (!get_local_mtu(device, &lmtu))
 980                 return false;
 981
 982         DBG("remote_mtu:%d local_mtu:%d", rmtu, lmtu);
 983
 984         if (rmtu < ATT_DEFAULT_LE_MTU) {
 985                 error("gatt: remote MTU invalid (%u bytes)", rmtu);
 986                 return false;
 987         }
 988
 989         mtu = MIN(lmtu, rmtu);
 990
 991         if (mtu == ATT_DEFAULT_LE_MTU)
 992                 return true;
 993
 994         if (!g_attrib_set_mtu(device->attrib, mtu)) {
 995                 error("gatt: Failed to set MTU");
 996                 return false;
 997         }
 998
 999         queue_foreach(app_connections, notify_mtu_change, device);
1000
1001         return true;
1002 }
1003
1004 static void att_handler(const uint8_t *ipdu, uint16_t len, gpointer user_data);
1005
1006 static void exchange_mtu_cb(guint8 status, const guint8 *pdu, guint16 plen,
1007                                                         gpointer user_data)
1008 {
1009         struct gatt_device *device = user_data;
1010         uint16_t rmtu;
1011
1012         DBG("");
1013
1014         if (status) {
1015                 error("gatt: MTU exchange: %s", att_ecode2str(status));
1016                 goto failed;
1017         }
1018
1019         if (!dec_mtu_resp(pdu, plen, &rmtu)) {
1020                 error("gatt: MTU exchange: protocol error");
1021                 goto failed;
1022         }
1023
1024         update_mtu(device, rmtu);
1025
1026 failed:
1027         device_unref(device);
1028 }
1029
1030 static void send_exchange_mtu_request(struct gatt_device *device)
1031 {
1032         uint16_t mtu;
1033
1034         if (!get_local_mtu(device, &mtu))
1035                 return;
1036
1037         DBG("mtu %u", mtu);
1038
1039         if (!gatt_exchange_mtu(device->attrib, mtu, exchange_mtu_cb,
1040                                                         device_ref(device)))
1041                 device_unref(device);
1042 }
1043
1044 static void ignore_confirmation_cb(guint8 status, const guint8 *pdu,
1045                                         guint16 len, gpointer user_data)
1046 {
1047         /* Ignored. */
1048 }
1049
1050 static void notify_att_range_change(struct gatt_device *dev,
1051                                                         struct att_range *range)
1052 {
1053         uint16_t handle;
1054         uint16_t length = 0;
1055         uint16_t ccc;
1056         uint8_t *pdu;
1057         size_t mtu;
1058         GAttribResultFunc confirmation_cb = NULL;
1059
1060         handle = gatt_db_attribute_get_handle(service_changed_attrib);
1061         if (!handle)
1062                 return;
1063
1064         ccc = bt_get_gatt_ccc(&dev->bdaddr);
1065         if (!ccc)
1066                 return;
1067
1068         pdu = g_attrib_get_buffer(dev->attrib, &mtu);
1069
1070         switch (ccc) {
1071         case 0x0001:
1072                 length = enc_notification(handle, (uint8_t *) range,
1073                                                 sizeof(*range), pdu, mtu);
1074                 break;
1075         case 0x0002:
1076                 length = enc_indication(handle, (uint8_t *) range,
1077                                                 sizeof(*range), pdu, mtu);
1078                 confirmation_cb = ignore_confirmation_cb;
1079                 break;
1080         default:
1081                 /* 0xfff4 reserved for future use */
1082                 break;
1083         }
1084
1085         g_attrib_send(dev->attrib, 0, pdu, length, confirmation_cb, NULL, NULL);
1086 }
1087
1088 static struct app_connection *create_connection(struct gatt_device *device,
1089                                                 struct gatt_app *app)
1090 {
1091         struct app_connection *new_conn;
1092         static int32_t last_conn_id = 1;
1093
1094         /* Check if already connected */
1095         new_conn = new0(struct app_connection, 1);
1096
1097         /* Make connection id unique to connection record (app, device) pair */
1098         new_conn->app = app;
1099         new_conn->id = last_conn_id++;
1100         new_conn->transactions = queue_new();
1101
1102         queue_push_head(app_connections, new_conn);
1103
1104         new_conn->device = device_ref(device);
1105
1106         return new_conn;
1107 }
1108
1109 static struct service *create_service(uint8_t id, bool primary, char *uuid,
1110                                                                 void *data)
1111 {
1112         struct service *s;
1113
1114         s = new0(struct service, 1);
1115
1116         if (bt_string_to_uuid(&s->id.uuid, uuid) < 0) {
1117                 error("gatt: Cannot convert string to uuid");
1118                 free(s);
1119                 return NULL;
1120         }
1121
1122         s->chars = queue_new();
1123         s->included = queue_new();
1124         s->id.instance = id;
1125
1126         /* Put primary service to our local list */
1127         s->primary = primary;
1128         if (s->primary)
1129                 memcpy(&s->prim, data, sizeof(s->prim));
1130         else
1131                 memcpy(&s->incl, data, sizeof(s->incl));
1132
1133         return s;
1134 }
1135
1136 static void send_client_primary_notify(void *data, void *user_data)
1137 {
1138         struct hal_ev_gatt_client_search_result ev;
1139         struct service *p = data;
1140         int32_t conn_id = PTR_TO_INT(user_data);
1141
1142         /* In service queue we will have also included services */
1143         if (!p->primary)
1144                 return;
1145
1146         ev.conn_id  = conn_id;
1147         element_id_to_hal_srvc_id(&p->id, 1, &ev.srvc_id);
1148
1149         uuid2android(&p->id.uuid, ev.srvc_id.uuid);
1150
1151         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
1152                         HAL_EV_GATT_CLIENT_SEARCH_RESULT, sizeof(ev), &ev);
1153 }
1154
1155 static void send_client_search_complete_notify(int32_t status, int32_t conn_id)
1156 {
1157         struct hal_ev_gatt_client_search_complete ev;
1158
1159         ev.status = status;
1160         ev.conn_id = conn_id;
1161         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
1162                         HAL_EV_GATT_CLIENT_SEARCH_COMPLETE, sizeof(ev), &ev);
1163 }
1164
1165 struct discover_srvc_data {
1166         bt_uuid_t uuid;
1167         struct app_connection *conn;
1168 };
1169
1170 static void discover_srvc_by_uuid_cb(uint8_t status, GSList *ranges,
1171                                                                 void *user_data)
1172 {
1173         struct discover_srvc_data *cb_data = user_data;
1174         struct gatt_primary prim;
1175         struct service *s;
1176         int32_t gatt_status;
1177         struct gatt_device *dev = cb_data->conn->device;
1178         uint8_t instance_id = queue_length(dev->services);
1179
1180         DBG("Status %d", status);
1181
1182         if (status) {
1183                 error("gatt: Discover pri srvc filtered by uuid failed: %s",
1184                                                         att_ecode2str(status));
1185                 gatt_status = GATT_FAILURE;
1186                 goto reply;
1187         }
1188
1189         if (!ranges) {
1190                 info("gatt: No primary services searched by uuid found");
1191                 gatt_status = GATT_SUCCESS;
1192                 goto reply;
1193         }
1194
1195         bt_uuid_to_string(&cb_data->uuid, prim.uuid, sizeof(prim.uuid));
1196
1197         for (; ranges; ranges = ranges->next) {
1198                 memcpy(&prim.range, ranges->data, sizeof(prim.range));
1199
1200                 s = create_service(instance_id++, true, prim.uuid, &prim);
1201                 if (!s) {
1202                         gatt_status = GATT_FAILURE;
1203                         goto reply;
1204                 }
1205
1206                 queue_push_tail(dev->services, s);
1207
1208                 send_client_primary_notify(s, INT_TO_PTR(cb_data->conn->id));
1209
1210                 DBG("attr handle = 0x%04x, end grp handle = 0x%04x uuid: %s",
1211                                 prim.range.start, prim.range.end, prim.uuid);
1212         }
1213
1214         /* Partial search service scanning was performed */
1215         dev->partial_srvc_search = true;
1216         gatt_status = GATT_SUCCESS;
1217
1218 reply:
1219         send_client_search_complete_notify(gatt_status, cb_data->conn->id);
1220         free(cb_data);
1221 }
1222
1223 static void discover_srvc_all_cb(uint8_t status, GSList *services,
1224                                                                 void *user_data)
1225 {
1226         struct discover_srvc_data *cb_data = user_data;
1227         struct gatt_device *dev = cb_data->conn->device;
1228         int32_t gatt_status;
1229         GSList *l;
1230         /*
1231          * There might be multiply services with same uuid. Therefore make sure
1232          * each primary service one has unique instance_id
1233          */
1234         uint8_t instance_id = queue_length(dev->services);
1235
1236         DBG("Status %d", status);
1237
1238         if (status) {
1239                 error("gatt: Discover all primary services failed: %s",
1240                                                         att_ecode2str(status));
1241                 gatt_status = GATT_FAILURE;
1242                 goto reply;
1243         }
1244
1245         if (!services) {
1246                 info("gatt: No primary services found");
1247                 gatt_status = GATT_SUCCESS;
1248                 goto reply;
1249         }
1250
1251         for (l = services; l; l = l->next) {
1252                 struct gatt_primary *prim = l->data;
1253                 struct service *p;
1254
1255                 if (queue_find(dev->services, match_srvc_by_range,
1256                                                                 &prim->range))
1257                         continue;
1258
1259                 p = create_service(instance_id++, true, prim->uuid, prim);
1260                 if (!p)
1261                         continue;
1262
1263                 queue_push_tail(dev->services, p);
1264
1265                 DBG("attr handle = 0x%04x, end grp handle = 0x%04x uuid: %s",
1266                         prim->range.start, prim->range.end, prim->uuid);
1267         }
1268
1269         /*
1270          * Send all found services notifications - first cache,
1271          * then send notifies
1272          */
1273         queue_foreach(dev->services, send_client_primary_notify,
1274                                                 INT_TO_PTR(cb_data->conn->id));
1275
1276         /* Full search service scanning was performed */
1277         dev->partial_srvc_search = false;
1278         gatt_status = GATT_SUCCESS;
1279
1280 reply:
1281         send_client_search_complete_notify(gatt_status, cb_data->conn->id);
1282         free(cb_data);
1283 }
1284
1285 static gboolean connection_timeout(void *user_data)
1286 {
1287         struct app_connection *conn = user_data;
1288
1289         conn->timeout_id = 0;
1290
1291         queue_remove(app_connections, conn);
1292         destroy_connection(conn);
1293
1294         return FALSE;
1295 }
1296
1297 static void discover_primary_cb(uint8_t status, GSList *services,
1298                                                                 void *user_data)
1299 {
1300         struct discover_srvc_data *cb_data = user_data;
1301         struct app_connection *conn = cb_data->conn;
1302         struct gatt_device *dev = conn->device;
1303         GSList *l, *uuids = NULL;
1304
1305         DBG("Status %d", status);
1306
1307         if (status) {
1308                 error("gatt: Discover all primary services failed: %s",
1309                                                         att_ecode2str(status));
1310                 free(cb_data);
1311
1312                 return;
1313         }
1314
1315         if (!services) {
1316                 info("gatt: No primary services found");
1317                 free(cb_data);
1318
1319                 return;
1320         }
1321
1322         for (l = services; l; l = l->next) {
1323                 struct gatt_primary *prim = l->data;
1324                 uint8_t *new_uuid;
1325                 bt_uuid_t uuid, u128;
1326
1327                 DBG("uuid: %s", prim->uuid);
1328
1329                 if (bt_string_to_uuid(&uuid, prim->uuid) < 0) {
1330                         error("gatt: Cannot convert string to uuid");
1331                         continue;
1332                 }
1333
1334                 bt_uuid_to_uuid128(&uuid, &u128);
1335                 new_uuid = g_memdup(&u128.value.u128, sizeof(u128.value.u128));
1336
1337                 uuids = g_slist_prepend(uuids, new_uuid);
1338         }
1339
1340         bt_device_set_uuids(&dev->bdaddr, uuids);
1341
1342         free(cb_data);
1343
1344         conn->timeout_id = g_timeout_add_seconds(GATT_CONN_TIMEOUT,
1345                                                 connection_timeout, conn);
1346 }
1347
1348 static guint search_dev_for_srvc(struct app_connection *conn, bt_uuid_t *uuid)
1349 {
1350         struct discover_srvc_data *cb_data;
1351
1352         cb_data = new0(struct discover_srvc_data, 1);
1353         cb_data->conn = conn;
1354
1355         if (uuid) {
1356                 memcpy(&cb_data->uuid, uuid, sizeof(cb_data->uuid));
1357                 return gatt_discover_primary(conn->device->attrib, uuid,
1358                                         discover_srvc_by_uuid_cb, cb_data);
1359         }
1360
1361         if (conn->app)
1362                 return gatt_discover_primary(conn->device->attrib, NULL,
1363                                                 discover_srvc_all_cb, cb_data);
1364
1365         return gatt_discover_primary(conn->device->attrib, NULL,
1366                                                 discover_primary_cb, cb_data);
1367 }
1368
1369 struct connect_data {
1370         struct gatt_device *dev;
1371         int32_t status;
1372 };
1373
1374 static void notify_app_connect_status_by_device(void *data, void *user_data)
1375 {
1376         struct app_connection *conn = data;
1377         struct connect_data *con_data = user_data;
1378
1379         if (conn->device == con_data->dev)
1380                 notify_app_connect_status(conn, con_data->status);
1381 }
1382
1383 static struct app_connection *find_conn_without_app(struct gatt_device *dev)
1384 {
1385         struct app_connection conn_match;
1386
1387         conn_match.device = dev;
1388         conn_match.app = NULL;
1389
1390         return queue_find(app_connections, match_connection_by_device_and_app,
1391                                                                 &conn_match);
1392 }
1393
1394 static struct app_connection *find_conn(const bdaddr_t *addr, int32_t app_id)
1395 {
1396         struct app_connection conn_match;
1397         struct gatt_device *dev;
1398         struct gatt_app *app;
1399
1400         /* Check if app is registered */
1401         app = find_app_by_id(app_id);
1402         if (!app) {
1403                 error("gatt: Client id %d not found", app_id);
1404                 return NULL;
1405         }
1406
1407         /* Check if device is known */
1408         dev = find_device_by_addr(addr);
1409         if (!dev) {
1410                 error("gatt: Client id %d not found", app_id);
1411                 return NULL;
1412         }
1413
1414         conn_match.device = dev;
1415         conn_match.app = app;
1416
1417         return queue_find(app_connections, match_connection_by_device_and_app,
1418                                                                 &conn_match);
1419 }
1420
1421 static void create_app_connection(void *data, void *user_data)
1422 {
1423         struct gatt_device *dev = user_data;
1424         struct gatt_app *app;
1425
1426         app = find_app_by_id(PTR_TO_INT(data));
1427         if (!app)
1428                 return;
1429
1430         DBG("Autoconnect application id=%d", app->id);
1431
1432         if (!find_conn(&dev->bdaddr, PTR_TO_INT(data)))
1433                 create_connection(dev, app);
1434 }
1435
1436 static void ind_handler(const uint8_t *cmd, uint16_t cmd_len,
1437                                                         gpointer user_data)
1438 {
1439         struct gatt_device *dev = user_data;
1440         uint16_t resp_length = 0;
1441         size_t length;
1442         uint8_t *opdu = g_attrib_get_buffer(dev->attrib, &length);
1443
1444         /*
1445          * We have to send confirmation here. If some client is
1446          * registered for this indication, event will be send in
1447          * handle_notification
1448          */
1449
1450         resp_length = enc_confirmation(opdu, length);
1451         g_attrib_send(dev->attrib, 0, opdu, resp_length, NULL, NULL, NULL);
1452 }
1453
1454 static void connect_cb(GIOChannel *io, GError *gerr, gpointer user_data)
1455 {
1456         struct gatt_device *dev = user_data;
1457         struct connect_data data;
1458         struct att_range range;
1459         uint32_t status;
1460         GError *err = NULL;
1461         GAttrib *attrib;
1462         uint16_t mtu, cid;
1463
1464         if (dev->state != DEVICE_CONNECT_READY) {
1465                 error("gatt: Device not in a connecting state!?");
1466                 g_io_channel_shutdown(io, TRUE, NULL);
1467                 return;
1468         }
1469
1470         if (dev->att_io) {
1471                 g_io_channel_unref(dev->att_io);
1472                 dev->att_io = NULL;
1473         }
1474
1475         if (gerr) {
1476                 error("gatt: connection failed %s", gerr->message);
1477                 device_set_state(dev, DEVICE_DISCONNECTED);
1478                 status = GATT_FAILURE;
1479                 goto reply;
1480         }
1481
1482         if (!bt_io_get(io, &err, BT_IO_OPT_IMTU, &mtu, BT_IO_OPT_CID, &cid,
1483                                                         BT_IO_OPT_INVALID)) {
1484                 error("gatt: Could not get imtu or cid: %s", err->message);
1485                 device_set_state(dev, DEVICE_DISCONNECTED);
1486                 status = GATT_FAILURE;
1487                 g_error_free(err);
1488                 goto reply;
1489         }
1490
1491         /* on BR/EDR MTU must not be less then minimal allowed MTU */
1492         if (cid != ATT_CID && mtu < ATT_DEFAULT_L2CAP_MTU) {
1493                 error("gatt: MTU too small (%u bytes)", mtu);
1494                 device_set_state(dev, DEVICE_DISCONNECTED);
1495                 status = GATT_FAILURE;
1496                 goto reply;
1497         }
1498
1499         DBG("mtu %u cid %u", mtu, cid);
1500
1501         /* on LE we always start with default MTU */
1502         if (cid == ATT_CID)
1503                 mtu = ATT_DEFAULT_LE_MTU;
1504
1505         attrib = g_attrib_new(io, mtu, true);
1506         if (!attrib) {
1507                 error("gatt: unable to create new GAttrib instance");
1508                 device_set_state(dev, DEVICE_DISCONNECTED);
1509                 status = GATT_FAILURE;
1510                 goto reply;
1511         }
1512
1513         dev->attrib = attrib;
1514         dev->watch_id = g_io_add_watch(io, G_IO_HUP | G_IO_ERR | G_IO_NVAL,
1515                                                         disconnected_cb, dev);
1516
1517         dev->server_id = g_attrib_register(attrib, GATTRIB_ALL_REQS,
1518                                                 GATTRIB_ALL_HANDLES,
1519                                                 att_handler, dev, NULL);
1520         dev->ind_id = g_attrib_register(attrib, ATT_OP_HANDLE_IND,
1521                                                 GATTRIB_ALL_HANDLES,
1522                                                 ind_handler, dev, NULL);
1523         if ((dev->server_id && dev->ind_id) == 0)
1524                 error("gatt: Could not attach to server");
1525
1526         device_set_state(dev, DEVICE_CONNECTED);
1527
1528         /* Send exchange mtu request as we assume being client and server */
1529         /* TODO: Dont exchange mtu if no client apps */
1530
1531         /* MTU exchange shall not be used on BR/EDR - Vol 3. Part G. 4.3.1 */
1532         if (cid == ATT_CID)
1533                 send_exchange_mtu_request(dev);
1534
1535         /*
1536          * Service Changed Characteristic and CCC Descriptor handles
1537          * should not change if there are bonded devices. We have them
1538          * constant all the time, thus they should be excluded from
1539          * range indicating changes.
1540          */
1541         range.start = gatt_db_attribute_get_handle(service_changed_attrib) + 2;
1542         range.end = 0xffff;
1543
1544         /*
1545          * If there is ccc stored for that device we were acting as server for
1546          * it, and as we dont have last connect and last services (de)activation
1547          * timestamps we should always assume something has changed.
1548          */
1549         notify_att_range_change(dev, &range);
1550
1551         status = GATT_SUCCESS;
1552
1553 reply:
1554         /*
1555          * Make sure there are app_connections for all apps interested in auto
1556          * connect to that device
1557          */
1558         queue_foreach(dev->autoconnect_apps, create_app_connection, dev);
1559
1560         if (!queue_find(app_connections, match_connection_by_device, dev)) {
1561                 struct app_connection *conn;
1562
1563                 if (!dev->attrib)
1564                         return;
1565
1566                 conn = create_connection(dev, NULL);
1567                 if (!conn)
1568                         return;
1569
1570                 if (bt_is_pairing(&dev->bdaddr))
1571                         /*
1572                          * If there is bonding ongoing lets wait for paired
1573                          * callback. Once we get that we can start search
1574                          * services
1575                          */
1576                         conn->timeout_id = g_timeout_add_seconds(
1577                                                 GATT_PAIR_CONN_TIMEOUT,
1578                                                 connection_timeout, conn);
1579                 else
1580                         /*
1581                          * There is no ongoing bonding, lets search for primary
1582                          * services
1583                          */
1584                         search_dev_for_srvc(conn, NULL);
1585         }
1586
1587         data.dev = dev;
1588         data.status = status;
1589         queue_foreach(app_connections, notify_app_connect_status_by_device,
1590                                                                         &data);
1591
1592         /* For BR/EDR notify about MTU since it is not negotiable*/
1593         if (cid != ATT_CID && status == GATT_SUCCESS)
1594                 queue_foreach(app_connections, notify_mtu_change, dev);
1595
1596         device_unref(dev);
1597
1598         /* Check if we should restart scan */
1599         if (scanning)
1600                 bt_le_discovery_start();
1601
1602         /* FIXME: What to do if discovery won't start here. */
1603 }
1604
1605 static int connect_le(struct gatt_device *dev)
1606 {
1607         GIOChannel *io;
1608         GError *gerr = NULL;
1609         char addr[18];
1610         const bdaddr_t *bdaddr;
1611         uint8_t bdaddr_type;
1612
1613         ba2str(&dev->bdaddr, addr);
1614
1615         /* There is one connection attempt going on */
1616         if (dev->att_io) {
1617                 info("gatt: connection to dev %s is ongoing", addr);
1618                 return -EALREADY;
1619         }
1620
1621         DBG("Connection attempt to: %s", addr);
1622
1623         bdaddr = bt_get_id_addr(&dev->bdaddr, &bdaddr_type);
1624
1625         /*
1626          * This connection will help us catch any PDUs that comes before
1627          * pairing finishes
1628          */
1629         io = bt_io_connect(connect_cb, device_ref(dev), NULL, &gerr,
1630                                         BT_IO_OPT_SOURCE_BDADDR, &adapter_addr,
1631                                         BT_IO_OPT_SOURCE_TYPE, BDADDR_LE_PUBLIC,
1632                                         BT_IO_OPT_DEST_BDADDR, bdaddr,
1633                                         BT_IO_OPT_DEST_TYPE, bdaddr_type,
1634                                         BT_IO_OPT_CID, ATT_CID,
1635                                         BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
1636                                         BT_IO_OPT_INVALID);
1637         if (!io) {
1638                 error("gatt: Failed bt_io_connect(%s): %s", addr,
1639                                                         gerr->message);
1640                 g_error_free(gerr);
1641                 return -EIO;
1642         }
1643
1644         /* Keep this, so we can cancel the connection */
1645         dev->att_io = io;
1646
1647         device_set_state(dev, DEVICE_CONNECT_READY);
1648
1649         return 0;
1650 }
1651
1652 static int connect_next_dev(void)
1653 {
1654         struct gatt_device *dev;
1655
1656         DBG("");
1657
1658         dev = find_device_by_state(DEVICE_CONNECT_READY);
1659         if (!dev)
1660                 return -ENODEV;
1661
1662         return connect_le(dev);
1663 }
1664
1665 static void bt_le_discovery_stop_cb(void)
1666 {
1667         DBG("");
1668
1669         /* Check now if there is any device ready to connect */
1670         if (connect_next_dev() < 0)
1671                 bt_le_discovery_start();
1672 }
1673
1674 static void le_device_found_handler(const bdaddr_t *addr, int rssi,
1675                                         uint16_t eir_len, const void *eir,
1676                                         bool connectable, bool bonded)
1677 {
1678         uint8_t buf[IPC_MTU];
1679         struct hal_ev_gatt_client_scan_result *ev = (void *) buf;
1680         struct gatt_device *dev;
1681         char bda[18];
1682
1683         if (!scanning)
1684                 goto done;
1685
1686         ba2str(addr, bda);
1687         DBG("LE Device found: %s, rssi: %d, adv_data: %d", bda, rssi, !!eir);
1688
1689         bdaddr2android(addr, ev->bda);
1690         ev->rssi = rssi;
1691         ev->len = eir_len;
1692
1693         memcpy(ev->adv_data, eir, ev->len);
1694
1695         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
1696                                                 HAL_EV_GATT_CLIENT_SCAN_RESULT,
1697                                                 sizeof(*ev) + ev->len, ev);
1698
1699 done:
1700         if (!connectable)
1701                 return;
1702
1703         /* We use auto connect feature from kernel if possible */
1704         if (bt_kernel_conn_control())
1705                 return;
1706
1707         dev = find_device_by_addr(addr);
1708         if (!dev) {
1709                 if (!bonded)
1710                         return;
1711
1712                 dev = create_device(addr);
1713         }
1714
1715         if (dev->state != DEVICE_CONNECT_INIT)
1716                 return;
1717
1718         device_set_state(dev, DEVICE_CONNECT_READY);
1719
1720         /*
1721          * We are ok to perform connect now. Stop discovery
1722          * and once it is stopped continue with creating ACL
1723          */
1724         bt_le_discovery_stop(bt_le_discovery_stop_cb);
1725 }
1726
1727 static struct gatt_app *register_app(const uint8_t *uuid, gatt_type_t type)
1728 {
1729         static int32_t application_id = 1;
1730         struct gatt_app *app;
1731
1732         if (queue_find(gatt_apps, match_app_by_uuid, uuid)) {
1733                 error("gatt: app uuid is already on list");
1734                 return NULL;
1735         }
1736
1737         app = new0(struct gatt_app, 1);
1738
1739         app->type = type;
1740
1741         if (app->type == GATT_CLIENT)
1742                 app->notifications = queue_new();
1743
1744         memcpy(app->uuid, uuid, sizeof(app->uuid));
1745
1746         app->id = application_id++;
1747
1748         queue_push_head(gatt_apps, app);
1749
1750         if (app->type == GATT_SERVER)
1751                 queue_push_tail(listen_apps, INT_TO_PTR(app->id));
1752
1753         return app;
1754 }
1755
1756 static void handle_client_register(const void *buf, uint16_t len)
1757 {
1758         const struct hal_cmd_gatt_client_register *cmd = buf;
1759         struct hal_ev_gatt_client_register_client ev;
1760         struct gatt_app *app;
1761
1762         DBG("");
1763
1764         memset(&ev, 0, sizeof(ev));
1765
1766         app = register_app(cmd->uuid, GATT_CLIENT);
1767
1768         if (app) {
1769                 ev.client_if = app->id;
1770                 ev.status = GATT_SUCCESS;
1771         } else {
1772                 ev.status = GATT_FAILURE;
1773         }
1774
1775         /* We should send notification with given in cmd UUID */
1776         memcpy(ev.app_uuid, cmd->uuid, sizeof(ev.app_uuid));
1777
1778         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
1779                         HAL_EV_GATT_CLIENT_REGISTER_CLIENT, sizeof(ev), &ev);
1780
1781         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_REGISTER,
1782                                                         HAL_STATUS_SUCCESS);
1783 }
1784
1785 static void handle_client_scan(const void *buf, uint16_t len)
1786 {
1787         const struct hal_cmd_gatt_client_scan *cmd = buf;
1788         uint8_t status;
1789
1790         DBG("new state %d", cmd->start);
1791
1792         if (cmd->client_if != 0) {
1793                 void *registered = find_app_by_id(cmd->client_if);
1794
1795                 if (!registered) {
1796                         error("gatt: Client not registered");
1797                         status = HAL_STATUS_FAILED;
1798                         goto reply;
1799                 }
1800         }
1801
1802         /* Turn off scan */
1803         if (!cmd->start) {
1804                 DBG("Stopping LE SCAN");
1805
1806                 if (scanning) {
1807                         bt_le_discovery_stop(NULL);
1808                         scanning = false;
1809                 }
1810
1811                 status = HAL_STATUS_SUCCESS;
1812                 goto reply;
1813         }
1814
1815         /* Reply success if we already do scan */
1816         if (scanning) {
1817                 status = HAL_STATUS_SUCCESS;
1818                 goto reply;
1819         }
1820
1821         /* Turn on scan */
1822         if (!bt_le_discovery_start()) {
1823                 error("gatt: LE scan switch failed");
1824                 status = HAL_STATUS_FAILED;
1825                 goto reply;
1826         }
1827
1828         scanning = true;
1829         status = HAL_STATUS_SUCCESS;
1830
1831 reply:
1832         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_SCAN,
1833                                                                         status);
1834 }
1835
1836 static int connect_bredr(struct gatt_device *dev)
1837 {
1838         BtIOSecLevel sec_level;
1839         GIOChannel *io;
1840         GError *gerr = NULL;
1841         char addr[18];
1842
1843         ba2str(&dev->bdaddr, addr);
1844
1845         /* There is one connection attempt going on */
1846         if (dev->att_io) {
1847                 info("gatt: connection to dev %s is ongoing", addr);
1848                 return -EALREADY;
1849         }
1850
1851         DBG("Connection attempt to: %s", addr);
1852
1853         sec_level = bt_device_is_bonded(&dev->bdaddr) ? BT_IO_SEC_MEDIUM :
1854                                                                 BT_IO_SEC_LOW;
1855
1856         io = bt_io_connect(connect_cb, device_ref(dev), NULL, &gerr,
1857                                         BT_IO_OPT_SOURCE_BDADDR, &adapter_addr,
1858                                         BT_IO_OPT_SOURCE_TYPE, BDADDR_BREDR,
1859                                         BT_IO_OPT_DEST_BDADDR, &dev->bdaddr,
1860                                         BT_IO_OPT_DEST_TYPE, BDADDR_BREDR,
1861                                         BT_IO_OPT_PSM, ATT_PSM,
1862                                         BT_IO_OPT_SEC_LEVEL, sec_level,
1863                                         BT_IO_OPT_INVALID);
1864         if (!io) {
1865                 error("gatt: Failed bt_io_connect(%s): %s", addr,
1866                                                         gerr->message);
1867                 g_error_free(gerr);
1868                 return -EIO;
1869         }
1870
1871         device_set_state(dev, DEVICE_CONNECT_READY);
1872
1873         /* Keep this, so we can cancel the connection */
1874         dev->att_io = io;
1875
1876         return 0;
1877 }
1878
1879 static bool trigger_connection(struct app_connection *conn, bool direct)
1880 {
1881         switch (conn->device->state) {
1882         case DEVICE_DISCONNECTED:
1883                 /*
1884                  *  If device was last seen over BR/EDR connect over it.
1885                  *  Note: Connection state is handled in connect_bredr() func
1886                  */
1887                 if (bt_device_last_seen_bearer(&conn->device->bdaddr) ==
1888                                                                 BDADDR_BREDR)
1889                         return connect_bredr(conn->device) == 0;
1890
1891                 if (direct)
1892                         return connect_le(conn->device) == 0;
1893
1894                 bt_gatt_add_autoconnect(conn->app->id, &conn->device->bdaddr);
1895                 return auto_connect_le(conn->device);
1896         case DEVICE_CONNECTED:
1897                 notify_app_connect_status(conn, GATT_SUCCESS);
1898                 return true;
1899         case DEVICE_CONNECT_READY:
1900         case DEVICE_CONNECT_INIT:
1901         default:
1902                 /* In those cases connection is already triggered. */
1903                 return true;
1904         }
1905 }
1906
1907 static void remove_autoconnect_device(struct gatt_device *dev)
1908 {
1909         bt_auto_connect_remove(&dev->bdaddr);
1910
1911         if (dev->state == DEVICE_CONNECT_INIT)
1912                 device_set_state(dev, DEVICE_DISCONNECTED);
1913
1914         device_unref(dev);
1915 }
1916
1917 static void clear_autoconnect_devices(void *data, void *user_data)
1918 {
1919         struct gatt_device *dev = data;
1920
1921         if (queue_remove(dev->autoconnect_apps, user_data))
1922                 if (queue_isempty(dev->autoconnect_apps))
1923                         remove_autoconnect_device(dev);
1924 }
1925
1926 static uint8_t unregister_app(int client_if)
1927 {
1928         struct gatt_app *cl;
1929
1930         /*
1931          * Make sure that there is no devices in auto connect list for this
1932          * application
1933          */
1934         queue_foreach(gatt_devices, clear_autoconnect_devices,
1935                                                         INT_TO_PTR(client_if));
1936
1937         cl = queue_remove_if(gatt_apps, match_app_by_id, INT_TO_PTR(client_if));
1938         if (!cl) {
1939                 error("gatt: client_if=%d not found", client_if);
1940
1941                 return HAL_STATUS_FAILED;
1942         }
1943
1944         /* Destroy app connections with proper notifications for this app. */
1945         queue_remove_all(app_connections, match_connection_by_app, cl,
1946                                                         destroy_connection);
1947         destroy_gatt_app(cl);
1948
1949         return HAL_STATUS_SUCCESS;
1950 }
1951
1952 static void send_client_listen_notify(int32_t id, int32_t status)
1953 {
1954         struct hal_ev_gatt_client_listen ev;
1955
1956         /* Server if because of typo in android headers */
1957         ev.server_if = id;
1958         ev.status = status;
1959
1960         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT, HAL_EV_GATT_CLIENT_LISTEN,
1961                                                         sizeof(ev), &ev);
1962 }
1963
1964 struct listen_data {
1965         int32_t client_id;
1966         bool start;
1967 };
1968
1969 static struct listen_data *create_listen_data(int32_t client_id, bool start)
1970 {
1971         struct listen_data *d;
1972
1973         d = new0(struct listen_data, 1);
1974         d->client_id = client_id;
1975         d->start = start;
1976
1977         return d;
1978 }
1979
1980 static void set_advertising_cb(uint8_t status, void *user_data)
1981 {
1982         struct listen_data *l = user_data;
1983
1984         send_client_listen_notify(l->client_id, status);
1985
1986         /* In case of success update advertising state*/
1987         if (!status)
1988                 advertising_cnt = l->start ? 1 : 0;
1989
1990         /*
1991          * Let's remove client from the list in two cases
1992          * 1. Start failed
1993          * 2. Stop succeed
1994          */
1995         if ((l->start && status) || (!l->start && !status))
1996                 queue_remove(listen_apps, INT_TO_PTR(l->client_id));
1997
1998         free(l);
1999 }
2000
2001 static void handle_client_unregister(const void *buf, uint16_t len)
2002 {
2003         const struct hal_cmd_gatt_client_unregister *cmd = buf;
2004         uint8_t status;
2005         void *listening_client;
2006         struct listen_data *data;
2007
2008         DBG("");
2009
2010         listening_client = queue_find(listen_apps, NULL,
2011                                                 INT_TO_PTR(cmd->client_if));
2012
2013         if (listening_client) {
2014                 advertising_cnt--;
2015                 queue_remove(listen_apps, INT_TO_PTR(cmd->client_if));
2016         } else {
2017                 status = unregister_app(cmd->client_if);
2018                 goto reply;
2019         }
2020
2021         if (!advertising_cnt) {
2022                 data = create_listen_data(cmd->client_if, false);
2023
2024                 if (!bt_le_set_advertising(data->start, set_advertising_cb,
2025                                                                 data)) {
2026                         error("gatt: Could not set advertising");
2027                         status = HAL_STATUS_FAILED;
2028                         free(data);
2029                         goto reply;
2030                 }
2031         }
2032
2033         status = unregister_app(cmd->client_if);
2034
2035 reply:
2036         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
2037                                         HAL_OP_GATT_CLIENT_UNREGISTER, status);
2038 }
2039
2040 static uint8_t handle_connect(int32_t app_id, const bdaddr_t *addr, bool direct)
2041 {
2042         struct app_connection conn_match;
2043         struct app_connection *conn;
2044         struct gatt_device *device;
2045         struct gatt_app *app;
2046
2047         DBG("");
2048
2049         app = find_app_by_id(app_id);
2050         if (!app)
2051                 return HAL_STATUS_FAILED;
2052
2053         device = find_device_by_addr(addr);
2054         if (!device)
2055                 device = create_device(addr);
2056
2057         conn_match.device = device;
2058         conn_match.app = app;
2059
2060         conn = queue_find(app_connections, match_connection_by_device_and_app,
2061                                                                 &conn_match);
2062         if (!conn) {
2063                 conn = create_connection(device, app);
2064                 if (!conn)
2065                         return HAL_STATUS_NOMEM;
2066         }
2067
2068         if (!trigger_connection(conn, direct))
2069                 return HAL_STATUS_FAILED;
2070
2071         return HAL_STATUS_SUCCESS;
2072 }
2073
2074 static void handle_client_connect(const void *buf, uint16_t len)
2075 {
2076         const struct hal_cmd_gatt_client_connect *cmd = buf;
2077         uint8_t status;
2078         bdaddr_t addr;
2079
2080         DBG("is_direct:%u transport:%u", cmd->is_direct, cmd->transport);
2081
2082         android2bdaddr(&cmd->bdaddr, &addr);
2083
2084         /* TODO handle transport flag */
2085
2086         status = handle_connect(cmd->client_if, &addr, cmd->is_direct);
2087
2088         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_CONNECT,
2089                                                                 status);
2090 }
2091
2092 static void handle_client_disconnect(const void *buf, uint16_t len)
2093 {
2094         const struct hal_cmd_gatt_client_disconnect *cmd = buf;
2095         struct app_connection *conn;
2096         uint8_t status;
2097
2098         DBG("");
2099
2100         /* TODO: should we care to match also bdaddr when conn_id is unique? */
2101         conn = queue_remove_if(app_connections, match_connection_by_id,
2102                                                 INT_TO_PTR(cmd->conn_id));
2103         destroy_connection(conn);
2104
2105         status = HAL_STATUS_SUCCESS;
2106
2107         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
2108                                         HAL_OP_GATT_CLIENT_DISCONNECT, status);
2109 }
2110
2111 static void handle_client_listen(const void *buf, uint16_t len)
2112 {
2113         const struct hal_cmd_gatt_client_listen *cmd = buf;
2114         uint8_t status;
2115         struct listen_data *data;
2116         bool req_sent = false;
2117         void *listening_client;
2118
2119         DBG("");
2120
2121         if (!find_app_by_id(cmd->client_if)) {
2122                 error("gatt: Client not registered");
2123                 status = HAL_STATUS_FAILED;
2124                 goto reply;
2125         }
2126
2127         listening_client = queue_find(listen_apps, NULL,
2128                                                 INT_TO_PTR(cmd->client_if));
2129         /* Start listening */
2130         if (cmd->start) {
2131                 if (listening_client) {
2132                         status = HAL_STATUS_SUCCESS;
2133                         goto reply;
2134                 }
2135
2136                 queue_push_tail(listen_apps, INT_TO_PTR(cmd->client_if));
2137
2138                 /* If listen is already on just return success*/
2139                 if (advertising_cnt > 0) {
2140                         advertising_cnt++;
2141                         status = HAL_STATUS_SUCCESS;
2142                         goto reply;
2143                 }
2144         } else {
2145                 /* Stop listening. Check if client was listening */
2146                 if (!listening_client) {
2147                         error("gatt: This client %d does not listen",
2148                                                         cmd->client_if);
2149                         status = HAL_STATUS_FAILED;
2150                         goto reply;
2151                 }
2152
2153                 /*
2154                  * In case there is more listening clients don't stop
2155                  * advertising
2156                  */
2157                 if (advertising_cnt > 1) {
2158                         advertising_cnt--;
2159                         queue_remove(listen_apps, INT_TO_PTR(cmd->client_if));
2160                         status = HAL_STATUS_SUCCESS;
2161                         goto reply;
2162                 }
2163         }
2164
2165         data = create_listen_data(cmd->client_if, cmd->start);
2166
2167         if (!bt_le_set_advertising(cmd->start, set_advertising_cb, data)) {
2168                 error("gatt: Could not set advertising");
2169                 status = HAL_STATUS_FAILED;
2170                 free(data);
2171                 goto reply;
2172         }
2173
2174         /*
2175          * Use this flag to keep in mind that we are waiting for callback with
2176          * result
2177          */
2178         req_sent = true;
2179
2180         status = HAL_STATUS_SUCCESS;
2181
2182 reply:
2183         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_LISTEN,
2184                                                         status);
2185
2186         /* In case of early success or error, just send notification up */
2187         if (!req_sent) {
2188                 int32_t gatt_status = status == HAL_STATUS_SUCCESS ?
2189                                                 GATT_SUCCESS : GATT_FAILURE;
2190                 send_client_listen_notify(cmd->client_if, gatt_status);
2191         }
2192 }
2193
2194 static void handle_client_refresh(const void *buf, uint16_t len)
2195 {
2196         const struct hal_cmd_gatt_client_refresh *cmd = buf;
2197         struct gatt_device *dev;
2198         uint8_t status;
2199         bdaddr_t bda;
2200
2201         /*
2202          * This is Android's framework hidden API call. It seams that no
2203          * notification is expected and Bluedroid silently updates device's
2204          * cache under the hood. As we use lazy caching ,we can just clear the
2205          * cache and we're done.
2206          */
2207
2208         DBG("");
2209
2210         android2bdaddr(&cmd->bdaddr, &bda);
2211         dev = find_device_by_addr(&bda);
2212         if (!dev) {
2213                 status = HAL_STATUS_FAILED;
2214                 goto done;
2215         }
2216
2217         queue_remove_all(dev->services, NULL, NULL, destroy_service);
2218
2219         status = HAL_STATUS_SUCCESS;
2220
2221 done:
2222         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_CLIENT_REFRESH,
2223                                                                         status);
2224 }
2225
2226 static void handle_client_search_service(const void *buf, uint16_t len)
2227 {
2228         const struct hal_cmd_gatt_client_search_service *cmd = buf;
2229         struct app_connection *conn;
2230         uint8_t status;
2231         struct service *s;
2232         bt_uuid_t uuid;
2233         guint srvc_search_success;
2234
2235         DBG("");
2236
2237         if (len != sizeof(*cmd) + (cmd->filtered ? 16 : 0)) {
2238                 error("Invalid search service size (%u bytes), terminating",
2239                                                                         len);
2240                 raise(SIGTERM);
2241                 return;
2242         }
2243
2244         conn = find_connection_by_id(cmd->conn_id);
2245         if (!conn) {
2246                 error("gatt: dev with conn_id=%d not found", cmd->conn_id);
2247
2248                 status = HAL_STATUS_FAILED;
2249                 goto reply;
2250         }
2251
2252         if (conn->device->state != DEVICE_CONNECTED) {
2253                 char bda[18];
2254
2255                 ba2str(&conn->device->bdaddr, bda);
2256                 error("gatt: device %s not connected", bda);
2257
2258                 status = HAL_STATUS_FAILED;
2259                 goto reply;
2260         }
2261
2262         if (cmd->filtered)
2263                 android2uuid(cmd->filter_uuid, &uuid);
2264
2265         /* Services not cached yet */
2266         if (queue_isempty(conn->device->services)) {
2267                 if (cmd->filtered)
2268                         srvc_search_success = search_dev_for_srvc(conn, &uuid);
2269                 else
2270                         srvc_search_success = search_dev_for_srvc(conn, NULL);
2271
2272                 if (!srvc_search_success) {
2273                         status = HAL_STATUS_FAILED;
2274                         goto reply;
2275                 }
2276
2277                 status = HAL_STATUS_SUCCESS;
2278                 goto reply;
2279         }
2280
2281         /* Search in cached services for given service */
2282         if (cmd->filtered) {
2283                 /* Search in cache for service by uuid */
2284                 s = queue_find(conn->device->services, match_srvc_by_bt_uuid,
2285                                                                         &uuid);
2286
2287                 if (s) {
2288                         send_client_primary_notify(s, INT_TO_PTR(conn->id));
2289                 } else {
2290                         if (!search_dev_for_srvc(conn, &uuid)) {
2291                                 status = HAL_STATUS_FAILED;
2292                                 goto reply;
2293                         }
2294
2295                         status = HAL_STATUS_SUCCESS;
2296                         goto reply;
2297                 }
2298         } else {
2299                 /* Refresh service cache if only partial search was performed */
2300                 if (conn->device->partial_srvc_search) {
2301                         srvc_search_success = search_dev_for_srvc(conn, NULL);
2302                         if (!srvc_search_success) {
2303                                 status = HAL_STATUS_FAILED;
2304                                 goto reply;
2305                         }
2306                 } else
2307                         queue_foreach(conn->device->services,
2308                                                 send_client_primary_notify,
2309                                                 INT_TO_PTR(cmd->conn_id));
2310         }
2311
2312         send_client_search_complete_notify(GATT_SUCCESS, conn->id);
2313
2314         status = HAL_STATUS_SUCCESS;
2315
2316 reply:
2317         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
2318                         HAL_OP_GATT_CLIENT_SEARCH_SERVICE, status);
2319 }
2320
2321 static void send_client_incl_service_notify(const struct element_id *srvc_id,
2322                                                 const struct service *incl,
2323                                                 int32_t conn_id)
2324 {
2325         struct hal_ev_gatt_client_get_inc_service ev;
2326
2327         memset(&ev, 0, sizeof(ev));
2328
2329         ev.conn_id = conn_id;
2330
2331         element_id_to_hal_srvc_id(srvc_id, 1, &ev.srvc_id);
2332
2333         if (incl) {
2334                 element_id_to_hal_srvc_id(&incl->id, 0, &ev.incl_srvc_id);
2335                 ev.status = GATT_SUCCESS;
2336         } else {
2337                 ev.status = GATT_FAILURE;
2338         }
2339
2340         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT ,
2341                                         HAL_EV_GATT_CLIENT_GET_INC_SERVICE,
2342                                         sizeof(ev), &ev);
2343 }
2344
2345 struct get_included_data {
2346         struct service *prim;
2347         struct app_connection *conn;
2348 };
2349
2350 static int get_inst_id_of_prim_services(const struct gatt_device *dev)
2351 {
2352         struct service *s = queue_peek_tail(dev->services);
2353
2354         if (s)
2355                 return s->id.instance;
2356
2357         return -1;
2358 }
2359
2360 static void get_included_cb(uint8_t status, GSList *included, void *user_data)
2361 {
2362         struct get_included_data *data = user_data;
2363         struct app_connection *conn = data->conn;
2364         struct service *service = data->prim;
2365         struct service *incl = NULL;
2366         int instance_id;
2367
2368         DBG("");
2369
2370         free(data);
2371
2372         if (status) {
2373                 error("gatt: no included services found");
2374                 goto failed;
2375         }
2376
2377         /* Remember that we already search included services.*/
2378         service->incl_search_done = true;
2379
2380         /*
2381          * There might be multiply services with same uuid. Therefore make sure
2382          * each service has unique instance id. Let's take the latest instance
2383          * id of primary service and start iterate included services from this
2384          * point.
2385          */
2386         instance_id = get_inst_id_of_prim_services(conn->device);
2387         if (instance_id < 0)
2388                 goto failed;
2389
2390         for (; included; included = included->next) {
2391                 struct gatt_included *included_service = included->data;
2392
2393                 incl = create_service(++instance_id, false,
2394                                                         included_service->uuid,
2395                                                         included_service);
2396                 if (!incl)
2397                         continue;
2398
2399                 /*
2400                  * Lets keep included service on two queues.
2401                  * 1. on services queue together with primary service
2402                  * 2. on special queue inside primary service
2403                  */
2404                 queue_push_tail(service->included, incl);
2405                 queue_push_tail(conn->device->services, incl);
2406         }
2407
2408         /*
2409          * Notify upper layer about first included service.
2410          * Android framework will iterate for next one.
2411          */
2412         incl = queue_peek_head(service->included);
2413
2414 failed:
2415         send_client_incl_service_notify(&service->id, incl, conn->id);
2416 }
2417
2418 static void search_included_services(struct app_connection *conn,
2419                                                         struct service *service)
2420 {
2421         struct get_included_data *data;
2422         uint16_t start, end;
2423
2424         data = new0(struct get_included_data, 1);
2425         data->prim = service;
2426         data->conn = conn;
2427
2428         if (service->primary) {
2429                 start = service->prim.range.start;
2430                 end = service->prim.range.end;
2431         } else {
2432                 start = service->incl.range.start;
2433                 end = service->incl.range.end;
2434         }
2435
2436         gatt_find_included(conn->device->attrib, start, end, get_included_cb,
2437                                                                         data);
2438 }
2439
2440 static bool find_service(int32_t conn_id, struct element_id *service_id,
2441                                         struct app_connection **connection,
2442                                         struct service **service)
2443 {
2444         struct service *srvc;
2445         struct app_connection *conn;
2446
2447         conn = find_connection_by_id(conn_id);
2448         if (!conn) {
2449                 error("gatt: conn_id=%d not found", conn_id);
2450                 return false;
2451         }
2452
2453         srvc = queue_find(conn->device->services, match_srvc_by_element_id,
2454                                                                 service_id);
2455         if (!srvc) {
2456                 error("gatt: Service with inst_id: %d not found",
2457                                                         service_id->instance);
2458                 return false;
2459         }
2460
2461         *connection = conn;
2462         *service = srvc;
2463
2464         return true;
2465 }
2466
2467 static void handle_client_get_included_service(const void *buf, uint16_t len)
2468 {
2469         const struct hal_cmd_gatt_client_get_included_service *cmd = buf;
2470         struct app_connection *conn;
2471         struct service *prim_service;
2472         struct service *incl_service = NULL;
2473         struct element_id match_id;
2474         struct element_id srvc_id;
2475         uint8_t status;
2476
2477         DBG("");
2478
2479         hal_srvc_id_to_element_id(&cmd->srvc_id, &srvc_id);
2480
2481         if (len != sizeof(*cmd) +
2482                         (cmd->continuation ? sizeof(cmd->incl_srvc_id[0]) : 0)) {
2483                 error("Invalid get incl services size (%u bytes), terminating",
2484                                                                         len);
2485                 raise(SIGTERM);
2486                 return;
2487         }
2488
2489         hal_srvc_id_to_element_id(&cmd->srvc_id, &match_id);
2490         if (!find_service(cmd->conn_id, &match_id, &conn, &prim_service)) {
2491                 status = HAL_STATUS_FAILED;
2492                 goto notify;
2493         }
2494
2495         if (!prim_service->incl_search_done) {
2496                 search_included_services(conn, prim_service);
2497                 status = HAL_STATUS_SUCCESS;
2498                 goto reply;
2499         }
2500
2501         /* Try to use cache here */
2502         if (!cmd->continuation) {
2503                 incl_service = queue_peek_head(prim_service->included);
2504         } else {
2505                 uint8_t inst_id = cmd->incl_srvc_id[0].inst_id;
2506
2507                 incl_service = queue_find(prim_service->included,
2508                                                 match_srvc_by_higher_inst_id,
2509                                                 INT_TO_PTR(inst_id));
2510         }
2511
2512         status = HAL_STATUS_SUCCESS;
2513
2514 notify:
2515         /*
2516          * In case of error in handling request we need to send event with
2517          * service id of cmd and gatt failure status.
2518          */
2519         send_client_incl_service_notify(&srvc_id, incl_service, cmd->conn_id);
2520
2521 reply:
2522         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
2523                         HAL_OP_GATT_CLIENT_GET_INCLUDED_SERVICE, status);
2524 }
2525
2526 static void send_client_char_notify(const struct hal_gatt_srvc_id *service,
2527                                         const struct hal_gatt_gatt_id *charac,
2528                                         int32_t char_prop, int32_t conn_id)
2529 {
2530         struct hal_ev_gatt_client_get_characteristic ev;
2531
2532         ev.conn_id = conn_id;
2533
2534         if (charac) {
2535                 memcpy(&ev.char_id, charac, sizeof(struct hal_gatt_gatt_id));
2536                 ev.char_prop = char_prop;
2537                 ev.status = GATT_SUCCESS;
2538         } else {
2539                 memset(&ev.char_id, 0, sizeof(struct hal_gatt_gatt_id));
2540                 ev.char_prop = 0;
2541                 ev.status = GATT_FAILURE;
2542         }
2543
2544         memcpy(&ev.srvc_id, service, sizeof(struct hal_gatt_srvc_id));
2545
2546         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
2547                                         HAL_EV_GATT_CLIENT_GET_CHARACTERISTIC,
2548                                         sizeof(ev), &ev);
2549 }
2550
2551 static void convert_send_client_char_notify(const struct characteristic *ch,
2552                                                 int32_t conn_id,
2553                                                 const struct service *service)
2554 {
2555         struct hal_gatt_srvc_id srvc;
2556         struct hal_gatt_gatt_id charac;
2557
2558         element_id_to_hal_srvc_id(&service->id, service->primary, &srvc);
2559
2560         if (ch) {
2561                 element_id_to_hal_gatt_id(&ch->id, &charac);
2562                 send_client_char_notify(&srvc, &charac, ch->ch.properties,
2563                                                                 conn_id);
2564         } else {
2565                 send_client_char_notify(&srvc, NULL, 0, conn_id);
2566         }
2567 }
2568
2569 static void cache_all_srvc_chars(struct service *srvc, GSList *characteristics)
2570 {
2571         uint16_t inst_id = 0;
2572         bt_uuid_t uuid;
2573
2574         for (; characteristics; characteristics = characteristics->next) {
2575                 struct characteristic *ch;
2576
2577                 ch = new0(struct characteristic, 1);
2578                 ch->descriptors = queue_new();
2579
2580                 memcpy(&ch->ch, characteristics->data, sizeof(ch->ch));
2581
2582                 bt_string_to_uuid(&uuid, ch->ch.uuid);
2583                 bt_uuid_to_uuid128(&uuid, &ch->id.uuid);
2584
2585                 /*
2586                  * For now we increment inst_id and use it as characteristic
2587                  * handle
2588                  */
2589                 ch->id.instance = ++inst_id;
2590
2591                 /* Store end handle to use later for descriptors discovery */
2592                 if (characteristics->next) {
2593                         struct gatt_char *next = characteristics->next->data;
2594
2595                         ch->end_handle = next->handle - 1;
2596                 } else {
2597                         ch->end_handle = srvc->primary ? srvc->prim.range.end :
2598                                                         srvc->incl.range.end;
2599                 }
2600
2601                 DBG("attr handle = 0x%04x, end handle = 0x%04x uuid: %s",
2602                                 ch->ch.handle, ch->end_handle, ch->ch.uuid);
2603
2604                 queue_push_tail(srvc->chars, ch);
2605         }
2606 }
2607
2608 struct discover_char_data {
2609         int32_t conn_id;
2610         struct service *service;
2611 };
2612
2613 static void discover_char_cb(uint8_t status, GSList *characteristics,
2614                                                                 void *user_data)
2615 {
2616         struct discover_char_data *data = user_data;
2617         struct service *srvc = data->service;
2618
2619         if (status) {
2620                 error("gatt: Failed to get characteristics: %s",
2621                                                         att_ecode2str(status));
2622                 convert_send_client_char_notify(NULL, data->conn_id, srvc);
2623                 goto done;
2624         }
2625
2626         if (queue_isempty(srvc->chars))
2627                 cache_all_srvc_chars(srvc, characteristics);
2628
2629         convert_send_client_char_notify(queue_peek_head(srvc->chars),
2630                                                         data->conn_id, srvc);
2631
2632 done:
2633         free(data);
2634 }
2635
2636 static void handle_client_get_characteristic(const void *buf, uint16_t len)
2637 {
2638         const struct hal_cmd_gatt_client_get_characteristic *cmd = buf;
2639         struct characteristic *ch;
2640         struct element_id match_id;
2641         struct app_connection *conn;
2642         struct service *srvc;
2643         uint8_t status;
2644
2645         DBG("");
2646
2647         if (len != sizeof(*cmd) + (cmd->continuation ? sizeof(cmd->char_id[0]) : 0)) {
2648                 error("Invalid get characteristic size (%u bytes), terminating",
2649                                                                         len);
2650                 raise(SIGTERM);
2651                 return;
2652         }
2653
2654         hal_srvc_id_to_element_id(&cmd->srvc_id, &match_id);
2655         if (!find_service(cmd->conn_id, &match_id, &conn, &srvc)) {
2656                 status = HAL_STATUS_FAILED;
2657                 goto done;
2658         }
2659
2660         /* Discover all characteristics for services if not cached yet */
2661         if (queue_isempty(srvc->chars)) {
2662                 struct discover_char_data *cb_data;
2663                 struct att_range range;
2664
2665                 cb_data = new0(struct discover_char_data, 1);
2666                 cb_data->service = srvc;
2667                 cb_data->conn_id = conn->id;
2668
2669                 range = srvc->primary ? srvc->prim.range : srvc->incl.range;
2670
2671                 if (!gatt_discover_char(conn->device->attrib, range.start,
2672                                                 range.end, NULL,
2673                                                 discover_char_cb, cb_data)) {
2674                         free(cb_data);
2675
2676                         status = HAL_STATUS_FAILED;
2677                         goto done;
2678                 }
2679
2680                 status = HAL_STATUS_SUCCESS;
2681                 goto done;
2682         }
2683
2684         if (cmd->continuation)
2685                 ch = queue_find(srvc->chars, match_char_by_higher_inst_id,
2686                                         INT_TO_PTR(cmd->char_id[0].inst_id));
2687         else
2688                 ch = queue_peek_head(srvc->chars);
2689
2690         convert_send_client_char_notify(ch, conn->id, srvc);
2691
2692         status = HAL_STATUS_SUCCESS;
2693
2694 done:
2695         if (status != HAL_STATUS_SUCCESS)
2696                 send_client_char_notify(&cmd->srvc_id, NULL, 0, cmd->conn_id);
2697
2698         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
2699                                 HAL_OP_GATT_CLIENT_GET_CHARACTERISTIC, status);
2700 }
2701
2702 static void send_client_descr_notify(int32_t status, int32_t conn_id,
2703                                         bool primary,
2704                                         const struct element_id *srvc,
2705                                         const struct element_id *ch,
2706                                         const struct element_id *opt_descr)
2707 {
2708         struct hal_ev_gatt_client_get_descriptor ev;
2709
2710         memset(&ev, 0, sizeof(ev));
2711
2712         ev.status = status;
2713         ev.conn_id = conn_id;
2714
2715         element_id_to_hal_srvc_id(srvc, primary, &ev.srvc_id);
2716         element_id_to_hal_gatt_id(ch, &ev.char_id);
2717
2718         if (opt_descr)
2719                 element_id_to_hal_gatt_id(opt_descr, &ev.descr_id);
2720
2721         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
2722                         HAL_EV_GATT_CLIENT_GET_DESCRIPTOR, sizeof(ev), &ev);
2723 }
2724
2725 struct discover_desc_data {
2726         struct app_connection *conn;
2727         struct service *srvc;
2728         struct characteristic *ch;
2729 };
2730
2731 static void gatt_discover_desc_cb(guint8 status, GSList *descs,
2732                                                         gpointer user_data)
2733 {
2734         struct discover_desc_data *data = user_data;
2735         struct app_connection *conn = data->conn;
2736         struct service *srvc = data->srvc;
2737         struct characteristic *ch = data->ch;
2738         struct descriptor *descr;
2739         int i = 0;
2740
2741         if (status != 0) {
2742                 error("Discover all characteristic descriptors failed [%s]: %s",
2743                                         ch->ch.uuid, att_ecode2str(status));
2744                 goto reply;
2745         }
2746
2747         for ( ; descs; descs = descs->next) {
2748                 struct gatt_desc *desc = descs->data;
2749                 bt_uuid_t uuid;
2750
2751                 descr = new0(struct descriptor, 1);
2752
2753                 bt_string_to_uuid(&uuid, desc->uuid);
2754                 bt_uuid_to_uuid128(&uuid, &descr->id.uuid);
2755
2756                 descr->id.instance = ++i;
2757                 descr->handle = desc->handle;
2758
2759                 DBG("attr handle = 0x%04x, uuid: %s", desc->handle, desc->uuid);
2760
2761                 queue_push_tail(ch->descriptors, descr);
2762         }
2763
2764 reply:
2765         descr = queue_peek_head(ch->descriptors);
2766
2767         send_client_descr_notify(status ? GATT_FAILURE : GATT_SUCCESS, conn->id,
2768                                         srvc->primary, &srvc->id, &ch->id,
2769                                         descr ? &descr->id : NULL);
2770
2771         free(data);
2772 }
2773
2774 static bool build_descr_cache(struct app_connection *conn, struct service *srvc,
2775                                                 struct characteristic *ch)
2776 {
2777         struct discover_desc_data *cb_data;
2778         uint16_t start, end;
2779
2780         /* Clip range to given characteristic */
2781         start = ch->ch.value_handle + 1;
2782         end = ch->end_handle;
2783
2784         /* If there are no descriptors, notify with fail status. */
2785         if (start > end)
2786                 return false;
2787
2788         cb_data = new0(struct discover_desc_data, 1);
2789         cb_data->conn = conn;
2790         cb_data->srvc = srvc;
2791         cb_data->ch = ch;
2792
2793         if (!gatt_discover_desc(conn->device->attrib, start, end, NULL,
2794                                         gatt_discover_desc_cb, cb_data)) {
2795                 free(cb_data);
2796                 return false;
2797         }
2798
2799         return true;
2800 }
2801
2802 static void handle_client_get_descriptor(const void *buf, uint16_t len)
2803 {
2804         const struct hal_cmd_gatt_client_get_descriptor *cmd = buf;
2805         struct descriptor *descr = NULL;
2806         struct characteristic *ch;
2807         struct service *srvc;
2808         struct element_id srvc_id;
2809         struct element_id char_id;
2810         struct app_connection *conn;
2811         int32_t conn_id;
2812         uint8_t primary;
2813         uint8_t status;
2814
2815         DBG("");
2816
2817         if (len != sizeof(*cmd) +
2818                         (cmd->continuation ? sizeof(cmd->descr_id[0]) : 0)) {
2819                 error("gatt: Invalid get descr command (%u bytes), terminating",
2820                                                                         len);
2821
2822                 raise(SIGTERM);
2823                 return;
2824         }
2825
2826         conn_id = cmd->conn_id;
2827         primary = cmd->srvc_id.is_primary;
2828
2829         hal_srvc_id_to_element_id(&cmd->srvc_id, &srvc_id);
2830         hal_gatt_id_to_element_id(&cmd->char_id, &char_id);
2831
2832         if (!find_service(conn_id, &srvc_id, &conn, &srvc)) {
2833                 error("gatt: Get descr. could not find service");
2834
2835                 status = HAL_STATUS_FAILED;
2836                 goto failed;
2837         }
2838
2839         ch = queue_find(srvc->chars, match_char_by_element_id, &char_id);
2840         if (!ch) {
2841                 error("gatt: Get descr. could not find characteristic");
2842
2843                 status = HAL_STATUS_FAILED;
2844                 goto failed;
2845         }
2846
2847         if (queue_isempty(ch->descriptors)) {
2848                 if (build_descr_cache(conn, srvc, ch)) {
2849                         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
2850                                         HAL_OP_GATT_CLIENT_GET_DESCRIPTOR,
2851                                         HAL_STATUS_SUCCESS);
2852                         return;
2853                 }
2854         }
2855
2856         status = HAL_STATUS_SUCCESS;
2857
2858         /* Send from cache */
2859         if (cmd->continuation)
2860                 descr = queue_find(ch->descriptors,
2861                                         match_descr_by_higher_inst_id,
2862                                         INT_TO_PTR(cmd->descr_id[0].inst_id));
2863         else
2864                 descr = queue_peek_head(ch->descriptors);
2865
2866 failed:
2867         send_client_descr_notify(descr ? GATT_SUCCESS : GATT_FAILURE, conn_id,
2868                                                 primary, &srvc_id, &char_id,
2869                                                 &descr->id);
2870
2871         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
2872                                 HAL_OP_GATT_CLIENT_GET_DESCRIPTOR, status);
2873 }
2874
2875 struct char_op_data {
2876         int32_t conn_id;
2877         const struct element_id *srvc_id;
2878         const struct element_id *char_id;
2879         uint8_t primary;
2880 };
2881
2882 static struct char_op_data *create_char_op_data(int32_t conn_id,
2883                                                 const struct element_id *s_id,
2884                                                 const struct element_id *ch_id,
2885                                                 bool primary)
2886 {
2887         struct char_op_data *d;
2888
2889         d = new0(struct char_op_data, 1);
2890         d->conn_id = conn_id;
2891         d->srvc_id = s_id;
2892         d->char_id = ch_id;
2893         d->primary = primary;
2894
2895         return d;
2896 }
2897
2898 static void send_client_read_char_notify(int32_t status, const uint8_t *pdu,
2899                                                 uint16_t len, int32_t conn_id,
2900                                                 const struct element_id *s_id,
2901                                                 const struct element_id *ch_id,
2902                                                 uint8_t primary)
2903 {
2904         uint8_t buf[IPC_MTU];
2905         struct hal_ev_gatt_client_read_characteristic *ev = (void *) buf;
2906         ssize_t vlen;
2907
2908         memset(buf, 0, sizeof(buf));
2909
2910         ev->conn_id = conn_id;
2911         ev->status = status;
2912         ev->data.status = status;
2913
2914         element_id_to_hal_srvc_id(s_id, primary, &ev->data.srvc_id);
2915         element_id_to_hal_gatt_id(ch_id, &ev->data.char_id);
2916
2917         if (status == 0 && pdu) {
2918                 vlen = dec_read_resp(pdu, len, ev->data.value, sizeof(buf));
2919                 if (vlen < 0) {
2920                         error("gatt: Protocol error");
2921                         ev->status = GATT_FAILURE;
2922                 } else {
2923                         ev->data.len = vlen;
2924                 }
2925         }
2926
2927         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
2928                                         HAL_EV_GATT_CLIENT_READ_CHARACTERISTIC,
2929                                         sizeof(*ev) + ev->data.len, ev);
2930 }
2931
2932 static void read_char_cb(guint8 status, const guint8 *pdu, guint16 len,
2933                                                         gpointer user_data)
2934 {
2935         struct char_op_data *data = user_data;
2936
2937         send_client_read_char_notify(status, pdu, len, data->conn_id,
2938                                                 data->srvc_id, data->char_id,
2939                                                 data->primary);
2940
2941         free(data);
2942 }
2943
2944 static int get_cid(struct gatt_device *dev)
2945 {
2946         GIOChannel *io;
2947         uint16_t cid;
2948
2949         io = g_attrib_get_channel(dev->attrib);
2950
2951         if (!bt_io_get(io, NULL, BT_IO_OPT_CID, &cid, BT_IO_OPT_INVALID)) {
2952                 error("gatt: Failed to get CID");
2953                 return -1;
2954         }
2955
2956         return cid;
2957 }
2958
2959 static int get_sec_level(struct gatt_device *dev)
2960 {
2961         GIOChannel *io;
2962         int sec_level;
2963
2964         io = g_attrib_get_channel(dev->attrib);
2965
2966         if (!bt_io_get(io, NULL, BT_IO_OPT_SEC_LEVEL, &sec_level,
2967                                                         BT_IO_OPT_INVALID)) {
2968                 error("gatt: Failed to get sec_level");
2969                 return -1;
2970         }
2971
2972         return sec_level;
2973 }
2974
2975 static bool set_security(struct gatt_device *device, int req_sec_level)
2976 {
2977         int sec_level;
2978         GError *gerr = NULL;
2979         GIOChannel *io;
2980
2981         sec_level = get_sec_level(device);
2982         if (sec_level < 0)
2983                 return false;
2984
2985         if (req_sec_level <= sec_level)
2986                 return true;
2987
2988         io = g_attrib_get_channel(device->attrib);
2989         if (!io)
2990                 return false;
2991
2992         bt_io_set(io, &gerr, BT_IO_OPT_SEC_LEVEL, req_sec_level,
2993                                                         BT_IO_OPT_INVALID);
2994         if (gerr) {
2995                 error("gatt: Failed to set security level: %s", gerr->message);
2996                 g_error_free(gerr);
2997                 return false;
2998         }
2999
3000         return true;
3001 }
3002
3003 bool bt_gatt_set_security(const bdaddr_t *bdaddr, int sec_level)
3004 {
3005         struct gatt_device *device;
3006
3007         device = find_device_by_addr(bdaddr);
3008         if (!device)
3009                 return false;
3010
3011         return set_security(device, sec_level);
3012 }
3013
3014 static bool set_auth_type(struct gatt_device *device, int auth_type)
3015 {
3016         int sec_level;
3017
3018         switch (auth_type) {
3019         case HAL_GATT_AUTHENTICATION_MITM:
3020                 sec_level = BT_SECURITY_HIGH;
3021                 break;
3022         case HAL_GATT_AUTHENTICATION_NO_MITM:
3023                 sec_level = BT_SECURITY_MEDIUM;
3024                 break;
3025         case HAL_GATT_AUTHENTICATION_NONE:
3026                 sec_level = BT_SECURITY_LOW;
3027                 break;
3028         default:
3029                 error("gatt: Invalid auth_type value: %d", auth_type);
3030                 return false;
3031         }
3032
3033         return set_security(device, sec_level);
3034 }
3035
3036 static void handle_client_read_characteristic(const void *buf, uint16_t len)
3037 {
3038         const struct hal_cmd_gatt_client_read_characteristic *cmd = buf;
3039         struct char_op_data *cb_data;
3040         struct characteristic *ch;
3041         struct app_connection *conn;
3042         struct service *srvc;
3043         struct element_id srvc_id;
3044         struct element_id char_id;
3045         uint8_t status;
3046
3047         DBG("");
3048
3049         /* TODO authorization needs to be handled */
3050
3051         hal_srvc_id_to_element_id(&cmd->srvc_id, &srvc_id);
3052         hal_gatt_id_to_element_id(&cmd->char_id, &char_id);
3053
3054         if (!find_service(cmd->conn_id, &srvc_id, &conn, &srvc)) {
3055                 status = HAL_STATUS_FAILED;
3056                 goto failed;
3057         }
3058
3059         /* search characteristics by element id */
3060         ch = queue_find(srvc->chars, match_char_by_element_id, &char_id);
3061         if (!ch) {
3062                 error("gatt: Characteristic with inst_id: %d not found",
3063                                                         cmd->char_id.inst_id);
3064                 status = HAL_STATUS_FAILED;
3065                 goto failed;
3066         }
3067
3068         cb_data = create_char_op_data(cmd->conn_id, &srvc->id, &ch->id,
3069                                                 cmd->srvc_id.is_primary);
3070
3071         if (!set_auth_type(conn->device, cmd->auth_req)) {
3072                 error("gatt: Failed to set security %d", cmd->auth_req);
3073                 status = HAL_STATUS_FAILED;
3074                 free(cb_data);
3075                 goto failed;
3076         }
3077
3078         if (!gatt_read_char(conn->device->attrib, ch->ch.value_handle,
3079                                                 read_char_cb, cb_data)) {
3080                 error("gatt: Cannot read characteristic with inst_id: %d",
3081                                                         cmd->char_id.inst_id);
3082                 status = HAL_STATUS_FAILED;
3083                 free(cb_data);
3084                 goto failed;
3085         }
3086
3087         status = HAL_STATUS_SUCCESS;
3088
3089 failed:
3090         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
3091                                 HAL_OP_GATT_CLIENT_READ_CHARACTERISTIC, status);
3092
3093         /*
3094          * We should send notification with service, characteristic id in case
3095          * of errors.
3096          */
3097         if (status != HAL_STATUS_SUCCESS)
3098                 send_client_read_char_notify(GATT_FAILURE, NULL, 0,
3099                                                 cmd->conn_id, &srvc_id,
3100                                                 &char_id,
3101                                                 cmd->srvc_id.is_primary);
3102 }
3103
3104 static void send_client_write_char_notify(int32_t status, int32_t conn_id,
3105                                         const struct element_id *srvc_id,
3106                                         const struct element_id *char_id,
3107                                         uint8_t primary)
3108 {
3109         struct hal_ev_gatt_client_write_characteristic ev;
3110
3111         memset(&ev, 0, sizeof(ev));
3112
3113         ev.conn_id = conn_id;
3114         ev.status = status;
3115         ev.data.status = status;
3116
3117         element_id_to_hal_srvc_id(srvc_id, primary, &ev.data.srvc_id);
3118         element_id_to_hal_gatt_id(char_id, &ev.data.char_id);
3119
3120         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
3121                                         HAL_EV_GATT_CLIENT_WRITE_CHARACTERISTIC,
3122                                         sizeof(ev), &ev);
3123 }
3124
3125 static void write_char_cb(guint8 status, const guint8 *pdu, guint16 len,
3126                                                         gpointer user_data)
3127 {
3128         struct char_op_data *data = user_data;
3129
3130         send_client_write_char_notify(status, data->conn_id, data->srvc_id,
3131                                                 data->char_id, data->primary);
3132
3133         free(data);
3134 }
3135
3136 static guint signed_write_cmd(struct gatt_device *dev, uint16_t handle,
3137                                         const uint8_t *value, uint16_t vlen)
3138 {
3139         uint8_t csrk[16];
3140         uint32_t sign_cnt;
3141         guint res;
3142
3143         memset(csrk, 0, 16);
3144
3145         if (!bt_get_csrk(&dev->bdaddr, true, csrk, &sign_cnt, NULL)) {
3146                 error("gatt: Could not get csrk key");
3147                 return 0;
3148         }
3149
3150         res = gatt_signed_write_cmd(dev->attrib, handle, value, vlen, crypto,
3151                                                 csrk, sign_cnt, NULL, NULL);
3152         if (!res) {
3153                 error("gatt: Signed write command failed");
3154                 return 0;
3155         }
3156
3157         bt_update_sign_counter(&dev->bdaddr, true, ++sign_cnt);
3158
3159         return res;
3160 }
3161
3162 static void handle_client_write_characteristic(const void *buf, uint16_t len)
3163 {
3164         const struct hal_cmd_gatt_client_write_characteristic *cmd = buf;
3165         struct char_op_data *cb_data = NULL;
3166         struct characteristic *ch;
3167         struct app_connection *conn;
3168         struct service *srvc;
3169         struct element_id srvc_id;
3170         struct element_id char_id;
3171         uint8_t status;
3172         guint res;
3173
3174         DBG("");
3175
3176         if (len != sizeof(*cmd) + cmd->len) {
3177                 error("Invalid write char size (%u bytes), terminating", len);
3178                 raise(SIGTERM);
3179                 return;
3180         }
3181
3182         hal_srvc_id_to_element_id(&cmd->srvc_id, &srvc_id);
3183         hal_gatt_id_to_element_id(&cmd->char_id, &char_id);
3184
3185         if (!find_service(cmd->conn_id, &srvc_id, &conn, &srvc)) {
3186                 status = HAL_STATUS_FAILED;
3187                 goto failed;
3188         }
3189
3190         /* search characteristics by instance id */
3191         ch = queue_find(srvc->chars, match_char_by_element_id, &char_id);
3192         if (!ch) {
3193                 error("gatt: Characteristic with inst_id: %d not found",
3194                                                         cmd->char_id.inst_id);
3195                 status = HAL_STATUS_FAILED;
3196                 goto failed;
3197         }
3198
3199         if (cmd->write_type == GATT_WRITE_TYPE_PREPARE ||
3200                                 cmd->write_type == GATT_WRITE_TYPE_DEFAULT) {
3201                 cb_data = create_char_op_data(cmd->conn_id, &srvc->id, &ch->id,
3202                                                 cmd->srvc_id.is_primary);
3203         }
3204
3205         if (!set_auth_type(conn->device, cmd->auth_req)) {
3206                 error("gatt: Failed to set security %d", cmd->auth_req);
3207                 status = HAL_STATUS_FAILED;
3208                 goto failed;
3209         }
3210
3211         switch (cmd->write_type) {
3212         case GATT_WRITE_TYPE_NO_RESPONSE:
3213                 res = gatt_write_cmd(conn->device->attrib, ch->ch.value_handle,
3214                                                         cmd->value, cmd->len,
3215                                                         NULL, NULL);
3216                 break;
3217         case GATT_WRITE_TYPE_PREPARE:
3218                 res = gatt_reliable_write_char(conn->device->attrib,
3219                                                         ch->ch.value_handle,
3220                                                         cmd->value, cmd->len,
3221                                                         write_char_cb, cb_data);
3222                 break;
3223         case GATT_WRITE_TYPE_DEFAULT:
3224                 res = gatt_write_char(conn->device->attrib, ch->ch.value_handle,
3225                                                         cmd->value, cmd->len,
3226                                                         write_char_cb, cb_data);
3227                 break;
3228         case GATT_WRITE_TYPE_SIGNED:
3229                 if (get_cid(conn->device) != ATT_CID) {
3230                         error("gatt: Cannot write signed on BR/EDR bearer");
3231                         status = HAL_STATUS_FAILED;
3232                         goto failed;
3233                 }
3234
3235                 if (get_sec_level(conn->device) > BT_SECURITY_LOW)
3236                         res = gatt_write_cmd(conn->device->attrib,
3237                                                 ch->ch.value_handle, cmd->value,
3238                                                 cmd->len, NULL, NULL);
3239                 else
3240                         res = signed_write_cmd(conn->device,
3241                                                 ch->ch.value_handle, cmd->value,
3242                                                 cmd->len);
3243                 break;
3244         default:
3245                 error("gatt: Write type %d unsupported", cmd->write_type);
3246                 status = HAL_STATUS_UNSUPPORTED;
3247                 goto failed;
3248         }
3249
3250         if (!res) {
3251                 error("gatt: Cannot write char. with inst_id: %d",
3252                                                         cmd->char_id.inst_id);
3253                 status = HAL_STATUS_FAILED;
3254                 goto failed;
3255         }
3256
3257         status = HAL_STATUS_SUCCESS;
3258
3259 failed:
3260         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
3261                         HAL_OP_GATT_CLIENT_WRITE_CHARACTERISTIC, status);
3262
3263         /*
3264          * We should send notification with service, characteristic id in case
3265          * of error and write with no response
3266          */
3267         if (status != HAL_STATUS_SUCCESS ||
3268                         cmd->write_type == GATT_WRITE_TYPE_NO_RESPONSE ||
3269                         cmd->write_type == GATT_WRITE_TYPE_SIGNED) {
3270                 int32_t gatt_status = (status == HAL_STATUS_SUCCESS) ?
3271                                                 GATT_SUCCESS : GATT_FAILURE;
3272
3273                 send_client_write_char_notify(gatt_status, cmd->conn_id,
3274                                                 &srvc_id, &char_id,
3275                                                 cmd->srvc_id.is_primary);
3276                 free(cb_data);
3277         }
3278 }
3279
3280 static void send_client_descr_read_notify(int32_t status, const uint8_t *pdu,
3281                                                 guint16 len, int32_t conn_id,
3282                                                 const struct element_id *srvc,
3283                                                 const struct element_id *ch,
3284                                                 const struct element_id *descr,
3285                                                 uint8_t primary)
3286 {
3287         uint8_t buf[IPC_MTU];
3288         struct hal_ev_gatt_client_read_descriptor *ev = (void *) buf;
3289
3290         memset(buf, 0, sizeof(buf));
3291
3292         ev->status = status;
3293         ev->conn_id = conn_id;
3294         ev->data.status = ev->status;
3295
3296         element_id_to_hal_srvc_id(srvc, primary, &ev->data.srvc_id);
3297         element_id_to_hal_gatt_id(ch, &ev->data.char_id);
3298         element_id_to_hal_gatt_id(descr, &ev->data.descr_id);
3299
3300         if (status == 0 && pdu) {
3301                 ssize_t ret;
3302
3303                 ret = dec_read_resp(pdu, len, ev->data.value,
3304                                                         GATT_MAX_ATTR_LEN);
3305                 if (ret < 0) {
3306                         error("gatt: Protocol error");
3307                         ev->status = GATT_FAILURE;
3308                 } else {
3309                         ev->data.len = ret;
3310                 }
3311         }
3312
3313         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
3314                                         HAL_EV_GATT_CLIENT_READ_DESCRIPTOR,
3315                                         sizeof(*ev) + ev->data.len, ev);
3316 }
3317
3318 struct desc_data {
3319         int32_t conn_id;
3320         const struct element_id *srvc_id;
3321         const struct element_id *char_id;
3322         const struct element_id *descr_id;
3323         uint8_t primary;
3324 };
3325
3326 static void read_desc_cb(guint8 status, const guint8 *pdu, guint16 len,
3327                                                         gpointer user_data)
3328 {
3329         struct desc_data *cb_data = user_data;
3330
3331         if (status != 0)
3332                 error("gatt: Discover all char descriptors failed: %s",
3333                                                         att_ecode2str(status));
3334
3335         send_client_descr_read_notify(status, pdu, len, cb_data->conn_id,
3336                                         cb_data->srvc_id, cb_data->char_id,
3337                                         cb_data->descr_id, cb_data->primary);
3338
3339         free(cb_data);
3340 }
3341
3342 static struct desc_data *create_desc_data(int32_t conn_id,
3343                                                 const struct element_id *s_id,
3344                                                 const struct element_id *ch_id,
3345                                                 const struct element_id *d_id,
3346                                                 uint8_t primary)
3347 {
3348         struct desc_data *d;
3349
3350         d = new0(struct desc_data, 1);
3351         d->conn_id = conn_id;
3352         d->srvc_id = s_id;
3353         d->char_id = ch_id;
3354         d->descr_id = d_id;
3355         d->primary = primary;
3356
3357         return d;
3358 }
3359
3360 static void handle_client_read_descriptor(const void *buf, uint16_t len)
3361 {
3362         const struct hal_cmd_gatt_client_read_descriptor *cmd = buf;
3363         struct desc_data *cb_data;
3364         struct characteristic *ch;
3365         struct descriptor *descr;
3366         struct service *srvc;
3367         struct element_id char_id;
3368         struct element_id descr_id;
3369         struct element_id srvc_id;
3370         struct app_connection *conn;
3371         int32_t conn_id = 0;
3372         uint8_t primary;
3373         uint8_t status;
3374
3375         DBG("");
3376
3377         conn_id = cmd->conn_id;
3378         primary = cmd->srvc_id.is_primary;
3379
3380         hal_srvc_id_to_element_id(&cmd->srvc_id, &srvc_id);
3381         hal_gatt_id_to_element_id(&cmd->char_id, &char_id);
3382         hal_gatt_id_to_element_id(&cmd->descr_id, &descr_id);
3383
3384         if (!find_service(conn_id, &srvc_id, &conn, &srvc)) {
3385                 error("gatt: Read descr. could not find service");
3386
3387                 status = HAL_STATUS_FAILED;
3388                 goto failed;
3389         }
3390
3391         ch = queue_find(srvc->chars, match_char_by_element_id, &char_id);
3392         if (!ch) {
3393                 error("gatt: Read descr. could not find characteristic");
3394
3395                 status = HAL_STATUS_FAILED;
3396                 goto failed;
3397         }
3398
3399         descr = queue_find(ch->descriptors, match_descr_by_element_id,
3400                                                                 &descr_id);
3401         if (!descr) {
3402                 error("gatt: Read descr. could not find descriptor");
3403
3404                 status = HAL_STATUS_FAILED;
3405                 goto failed;
3406         }
3407
3408         cb_data = create_desc_data(conn_id, &srvc->id, &ch->id, &descr->id,
3409                                                                 primary);
3410
3411         if (!set_auth_type(conn->device, cmd->auth_req)) {
3412                 error("gatt: Failed to set security %d", cmd->auth_req);
3413                 status = HAL_STATUS_FAILED;
3414                 free(cb_data);
3415                 goto failed;
3416         }
3417
3418         if (!gatt_read_char(conn->device->attrib, descr->handle, read_desc_cb,
3419                                                                 cb_data)) {
3420                 free(cb_data);
3421
3422                 status = HAL_STATUS_FAILED;
3423                 goto failed;
3424         }
3425
3426         status = HAL_STATUS_SUCCESS;
3427
3428 failed:
3429         if (status != HAL_STATUS_SUCCESS)
3430                 send_client_descr_read_notify(GATT_FAILURE, NULL, 0, conn_id,
3431                                                 &srvc_id, &char_id, &descr_id,
3432                                                 primary);
3433
3434         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
3435                         HAL_OP_GATT_CLIENT_READ_DESCRIPTOR, status);
3436 }
3437
3438 static void send_client_descr_write_notify(int32_t status, int32_t conn_id,
3439                                                 const struct element_id *srvc,
3440                                                 const struct element_id *ch,
3441                                                 const struct element_id *descr,
3442                                                 uint8_t primary) {
3443         uint8_t buf[IPC_MTU];
3444         struct hal_ev_gatt_client_write_descriptor *ev = (void *) buf;
3445
3446         memset(buf, 0, sizeof(buf));
3447
3448         ev->status = status;
3449         ev->conn_id = conn_id;
3450
3451         element_id_to_hal_srvc_id(srvc, primary, &ev->data.srvc_id);
3452         element_id_to_hal_gatt_id(ch, &ev->data.char_id);
3453         element_id_to_hal_gatt_id(descr, &ev->data.descr_id);
3454         ev->data.status = status;
3455
3456         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
3457                                         HAL_EV_GATT_CLIENT_WRITE_DESCRIPTOR,
3458                                         sizeof(*ev), ev);
3459 }
3460
3461 static void write_descr_cb(guint8 status, const guint8 *pdu, guint16 len,
3462                                                         gpointer user_data)
3463 {
3464         struct desc_data *cb_data = user_data;
3465
3466         if (status)
3467                 error("gatt: Write descriptors failed: %s",
3468                                                         att_ecode2str(status));
3469
3470         send_client_descr_write_notify(status, cb_data->conn_id,
3471                                         cb_data->srvc_id, cb_data->char_id,
3472                                         cb_data->descr_id, cb_data->primary);
3473
3474         free(cb_data);
3475 }
3476
3477 static void handle_client_write_descriptor(const void *buf, uint16_t len)
3478 {
3479         const struct hal_cmd_gatt_client_write_descriptor *cmd = buf;
3480         struct desc_data *cb_data = NULL;
3481         struct characteristic *ch;
3482         struct descriptor *descr;
3483         struct service *srvc;
3484         struct element_id srvc_id;
3485         struct element_id char_id;
3486         struct element_id descr_id;
3487         struct app_connection *conn;
3488         int32_t conn_id;
3489         uint8_t primary;
3490         uint8_t status;
3491         guint res;
3492
3493         DBG("");
3494
3495         if (len != sizeof(*cmd) + cmd->len) {
3496                 error("Invalid write desriptor command (%u bytes), terminating",
3497                                                                         len);
3498                 raise(SIGTERM);
3499                 return;
3500         }
3501
3502         primary = cmd->srvc_id.is_primary;
3503         conn_id = cmd->conn_id;
3504
3505         hal_srvc_id_to_element_id(&cmd->srvc_id, &srvc_id);
3506         hal_gatt_id_to_element_id(&cmd->char_id, &char_id);
3507         hal_gatt_id_to_element_id(&cmd->descr_id, &descr_id);
3508
3509         if (!find_service(cmd->conn_id, &srvc_id, &conn, &srvc)) {
3510                 error("gatt: Write descr. could not find service");
3511
3512                 status = HAL_STATUS_FAILED;
3513                 goto failed;
3514         }
3515
3516         ch = queue_find(srvc->chars, match_char_by_element_id, &char_id);
3517         if (!ch) {
3518                 error("gatt: Write descr. could not find characteristic");
3519
3520                 status = HAL_STATUS_FAILED;
3521                 goto failed;
3522         }
3523
3524         descr = queue_find(ch->descriptors, match_descr_by_element_id,
3525                                                                 &descr_id);
3526         if (!descr) {
3527                 error("gatt: Write descr. could not find descriptor");
3528
3529                 status = HAL_STATUS_FAILED;
3530                 goto failed;
3531         }
3532
3533         if (cmd->write_type != GATT_WRITE_TYPE_NO_RESPONSE)
3534                 cb_data = create_desc_data(conn_id, &srvc->id, &ch->id,
3535                                                         &descr->id, primary);
3536
3537         if (!set_auth_type(conn->device, cmd->auth_req)) {
3538                 error("gatt: Failed to set security %d", cmd->auth_req);
3539                 status = HAL_STATUS_FAILED;
3540                 goto failed;
3541         }
3542
3543         switch (cmd->write_type) {
3544         case GATT_WRITE_TYPE_NO_RESPONSE:
3545                 res = gatt_write_cmd(conn->device->attrib, descr->handle,
3546                                         cmd->value, cmd->len, NULL , NULL);
3547                 break;
3548         case GATT_WRITE_TYPE_PREPARE:
3549                 res = gatt_reliable_write_char(conn->device->attrib,
3550                                                 descr->handle, cmd->value,
3551                                                 cmd->len, write_descr_cb,
3552                                                 cb_data);
3553                 break;
3554         case GATT_WRITE_TYPE_DEFAULT:
3555                 res = gatt_write_char(conn->device->attrib, descr->handle,
3556                                                 cmd->value, cmd->len,
3557                                                 write_descr_cb, cb_data);
3558                 break;
3559         default:
3560                 error("gatt: Write type %d unsupported", cmd->write_type);
3561                 status = HAL_STATUS_UNSUPPORTED;
3562                 goto failed;
3563         }
3564
3565         if (!res) {
3566                 error("gatt: Write desc, could not write desc");
3567                 status = HAL_STATUS_FAILED;
3568                 goto failed;
3569         }
3570
3571         status = HAL_STATUS_SUCCESS;
3572
3573 failed:
3574         if (status != HAL_STATUS_SUCCESS ||
3575                         cmd->write_type == GATT_WRITE_TYPE_NO_RESPONSE) {
3576                 int32_t gatt_status = (status == HAL_STATUS_SUCCESS) ?
3577                                                 GATT_SUCCESS : GATT_FAILURE;
3578
3579                 send_client_descr_write_notify(gatt_status, conn_id, &srvc_id,
3580                                                 &char_id, &descr_id, primary);
3581                 free(cb_data);
3582         }
3583
3584         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
3585                                 HAL_OP_GATT_CLIENT_WRITE_DESCRIPTOR, status);
3586 }
3587
3588 static void send_client_write_execute_notify(int32_t id, int32_t status)
3589 {
3590         struct hal_ev_gatt_client_exec_write ev;
3591
3592         ev.conn_id = id;
3593         ev.status = status;
3594
3595         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
3596                                         HAL_EV_GATT_CLIENT_EXEC_WRITE,
3597                                         sizeof(ev), &ev);
3598 }
3599
3600 static void write_execute_cb(guint8 status, const guint8 *pdu, guint16 len,
3601                                                         gpointer user_data)
3602 {
3603         send_client_write_execute_notify(PTR_TO_INT(user_data), status);
3604 }
3605
3606 static void handle_client_execute_write(const void *buf, uint16_t len)
3607 {
3608         const struct hal_cmd_gatt_client_execute_write *cmd = buf;
3609         struct app_connection *conn;
3610         uint8_t status;
3611         uint8_t flags;
3612
3613         DBG("");
3614
3615         conn = find_connection_by_id(cmd->conn_id);
3616         if (!conn) {
3617                 status = HAL_STATUS_FAILED;
3618                 goto reply;
3619         }
3620
3621         flags = cmd->execute ? ATT_WRITE_ALL_PREP_WRITES :
3622                                                 ATT_CANCEL_ALL_PREP_WRITES;
3623
3624         if (!gatt_execute_write(conn->device->attrib, flags, write_execute_cb,
3625                                                 INT_TO_PTR(cmd->conn_id))) {
3626                 error("gatt: Could not send execute write");
3627                 status = HAL_STATUS_FAILED;
3628                 goto reply;
3629         }
3630
3631         status = HAL_STATUS_SUCCESS;
3632 reply:
3633         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
3634                         HAL_OP_GATT_CLIENT_EXECUTE_WRITE, status);
3635
3636         /* In case of early error send also notification.*/
3637         if (status != HAL_STATUS_SUCCESS)
3638                 send_client_write_execute_notify(cmd->conn_id, GATT_FAILURE);
3639 }
3640
3641 static void handle_notification(const uint8_t *pdu, uint16_t len,
3642                                                         gpointer user_data)
3643 {
3644         uint8_t buf[IPC_MTU];
3645         struct hal_ev_gatt_client_notify *ev = (void *) buf;
3646         struct notification_data *notification = user_data;
3647         uint8_t data_offset = sizeof(uint8_t) + sizeof(uint16_t);
3648
3649         if (len < data_offset)
3650                 return;
3651
3652         memcpy(&ev->char_id, &notification->ch, sizeof(ev->char_id));
3653         memcpy(&ev->srvc_id, &notification->service, sizeof(ev->srvc_id));
3654         bdaddr2android(&notification->conn->device->bdaddr, &ev->bda);
3655         ev->conn_id = notification->conn->id;
3656         ev->is_notify = pdu[0] == ATT_OP_HANDLE_NOTIFY;
3657
3658         /* We have to cut opcode and handle from data */
3659         ev->len = len - data_offset;
3660         memcpy(ev->value, pdu + data_offset, len - data_offset);
3661
3662         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT, HAL_EV_GATT_CLIENT_NOTIFY,
3663                                                 sizeof(*ev) + ev->len, ev);
3664 }
3665
3666 static void send_register_for_notification_ev(int32_t id, int32_t registered,
3667                                         int32_t status,
3668                                         const struct hal_gatt_srvc_id *srvc,
3669                                         const struct hal_gatt_gatt_id *ch)
3670 {
3671         struct hal_ev_gatt_client_reg_for_notif ev;
3672
3673         ev.conn_id = id;
3674         ev.status = status;
3675         ev.registered = registered;
3676         memcpy(&ev.srvc_id, srvc, sizeof(ev.srvc_id));
3677         memcpy(&ev.char_id, ch, sizeof(ev.char_id));
3678
3679         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
3680                         HAL_EV_GATT_CLIENT_REGISTER_FOR_NOTIF, sizeof(ev), &ev);
3681 }
3682
3683 static void handle_client_register_for_notification(const void *buf,
3684                                                                 uint16_t len)
3685 {
3686         const struct hal_cmd_gatt_client_register_for_notification *cmd = buf;
3687         struct notification_data *notification;
3688         struct characteristic *c;
3689         struct element_id match_id;
3690         struct app_connection *conn;
3691         int32_t conn_id = 0;
3692         struct service *service;
3693         uint8_t status;
3694         int32_t gatt_status;
3695         bdaddr_t addr;
3696
3697         DBG("");
3698
3699         android2bdaddr(&cmd->bdaddr, &addr);
3700
3701         conn = find_conn(&addr, cmd->client_if);
3702         if (!conn) {
3703                 status = HAL_STATUS_FAILED;
3704                 goto failed;
3705         }
3706
3707         conn_id = conn->id;
3708
3709         hal_srvc_id_to_element_id(&cmd->srvc_id, &match_id);
3710         service = queue_find(conn->device->services, match_srvc_by_element_id,
3711                                                                 &match_id);
3712         if (!service) {
3713                 status = HAL_STATUS_FAILED;
3714                 goto failed;
3715         }
3716
3717         hal_gatt_id_to_element_id(&cmd->char_id, &match_id);
3718         c = queue_find(service->chars, match_char_by_element_id, &match_id);
3719         if (!c) {
3720                 status = HAL_STATUS_FAILED;
3721                 goto failed;
3722         }
3723
3724         notification = new0(struct notification_data, 1);
3725
3726         memcpy(&notification->ch, &cmd->char_id, sizeof(notification->ch));
3727         memcpy(&notification->service, &cmd->srvc_id,
3728                                                 sizeof(notification->service));
3729         notification->conn = conn;
3730
3731         if (queue_find(conn->app->notifications, match_notification,
3732                                                                 notification)) {
3733                 free(notification);
3734                 status = HAL_STATUS_SUCCESS;
3735                 goto failed;
3736         }
3737
3738         notification->notif_id = g_attrib_register(conn->device->attrib,
3739                                                         ATT_OP_HANDLE_NOTIFY,
3740                                                         c->ch.value_handle,
3741                                                         handle_notification,
3742                                                         notification,
3743                                                         destroy_notification);
3744         if (!notification->notif_id) {
3745                 free(notification);
3746                 status = HAL_STATUS_FAILED;
3747                 goto failed;
3748         }
3749
3750         notification->ind_id = g_attrib_register(conn->device->attrib,
3751                                                         ATT_OP_HANDLE_IND,
3752                                                         c->ch.value_handle,
3753                                                         handle_notification,
3754                                                         notification,
3755                                                         destroy_notification);
3756         if (!notification->ind_id) {
3757                 g_attrib_unregister(conn->device->attrib,
3758                                                         notification->notif_id);
3759                 free(notification);
3760                 status = HAL_STATUS_FAILED;
3761                 goto failed;
3762         }
3763
3764         /*
3765          * Because same data - notification - is shared by two handlers, we
3766          * introduce ref counter to be sure that data can be freed with no risk.
3767          * Counter is decremented in destroy_notification.
3768          */
3769         notification->ref = 2;
3770
3771         queue_push_tail(conn->app->notifications, notification);
3772
3773         status = HAL_STATUS_SUCCESS;
3774
3775 failed:
3776         gatt_status = status ? GATT_FAILURE : GATT_SUCCESS;
3777         send_register_for_notification_ev(conn_id, 1, gatt_status,
3778                                                 &cmd->srvc_id, &cmd->char_id);
3779         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
3780                         HAL_OP_GATT_CLIENT_REGISTER_FOR_NOTIFICATION, status);
3781 }
3782
3783 static void handle_client_deregister_for_notification(const void *buf,
3784                                                                 uint16_t len)
3785 {
3786         const struct hal_cmd_gatt_client_deregister_for_notification *cmd = buf;
3787         struct notification_data *notification, notif;
3788         struct app_connection *conn;
3789         int32_t conn_id = 0;
3790         uint8_t status;
3791         int32_t gatt_status;
3792         bdaddr_t addr;
3793
3794         DBG("");
3795
3796         android2bdaddr(&cmd->bdaddr, &addr);
3797
3798         conn = find_conn(&addr, cmd->client_if);
3799         if (!conn) {
3800                 status = HAL_STATUS_FAILED;
3801                 goto failed;
3802         }
3803
3804         conn_id = conn->id;
3805
3806         memcpy(&notif.ch, &cmd->char_id, sizeof(notif.ch));
3807         memcpy(&notif.service, &cmd->srvc_id, sizeof(notif.service));
3808         notif.conn = conn;
3809
3810         notification = queue_find(conn->app->notifications,
3811                                                 match_notification, &notif);
3812         if (!notification) {
3813                 status = HAL_STATUS_FAILED;
3814                 goto failed;
3815         }
3816
3817         unregister_notification(notification);
3818
3819         status = HAL_STATUS_SUCCESS;
3820
3821 failed:
3822         gatt_status = status ? GATT_FAILURE : GATT_SUCCESS;
3823         send_register_for_notification_ev(conn_id, 0, gatt_status,
3824                                                 &cmd->srvc_id, &cmd->char_id);
3825
3826         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
3827                         HAL_OP_GATT_CLIENT_DEREGISTER_FOR_NOTIFICATION, status);
3828 }
3829
3830 static void send_client_remote_rssi_notify(int32_t client_if,
3831                                                 const bdaddr_t *addr,
3832                                                 int32_t rssi, int32_t status)
3833 {
3834         struct hal_ev_gatt_client_read_remote_rssi ev;
3835
3836         ev.client_if = client_if;
3837         bdaddr2android(addr, &ev.address);
3838         ev.rssi = rssi;
3839         ev.status = status;
3840
3841         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
3842                         HAL_EV_GATT_CLIENT_READ_REMOTE_RSSI, sizeof(ev), &ev);
3843 }
3844
3845 static void read_remote_rssi_cb(uint8_t status, const bdaddr_t *addr,
3846                                                 int8_t rssi, void *user_data)
3847 {
3848         int32_t client_if = PTR_TO_INT(user_data);
3849         int32_t gatt_status = status ? GATT_FAILURE : GATT_SUCCESS;
3850
3851         send_client_remote_rssi_notify(client_if, addr, rssi, gatt_status);
3852 }
3853
3854 static void handle_client_read_remote_rssi(const void *buf, uint16_t len)
3855 {
3856         const struct hal_cmd_gatt_client_read_remote_rssi *cmd = buf;
3857         uint8_t status;
3858         bdaddr_t bdaddr;
3859
3860         DBG("");
3861
3862         if (!find_app_by_id(cmd->client_if)) {
3863                 status = HAL_STATUS_FAILED;
3864                 goto failed;
3865         }
3866
3867         android2bdaddr(cmd->bdaddr, &bdaddr);
3868         if (!bt_read_device_rssi(&bdaddr, read_remote_rssi_cb,
3869                                                 INT_TO_PTR(cmd->client_if))) {
3870                 error("gatt: Could not read RSSI");
3871                 status = HAL_STATUS_FAILED;
3872                 goto failed;
3873         }
3874
3875         status = HAL_STATUS_SUCCESS;
3876
3877 failed:
3878         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
3879                         HAL_OP_GATT_CLIENT_READ_REMOTE_RSSI, status);
3880
3881         if (status != HAL_STATUS_SUCCESS)
3882                 send_client_remote_rssi_notify(cmd->client_if, &bdaddr, 0,
3883                                                                 GATT_FAILURE);
3884 }
3885
3886 static void handle_client_get_device_type(const void *buf, uint16_t len)
3887 {
3888         const struct hal_cmd_gatt_client_get_device_type *cmd = buf;
3889         struct hal_rsp_gatt_client_get_device_type rsp;
3890         bdaddr_t bdaddr;
3891
3892         DBG("");
3893
3894         android2bdaddr(cmd->bdaddr, &bdaddr);
3895
3896         rsp.type = bt_get_device_android_type(&bdaddr);
3897
3898         ipc_send_rsp_full(hal_ipc, HAL_SERVICE_ID_GATT,
3899                                         HAL_OP_GATT_CLIENT_GET_DEVICE_TYPE,
3900                                         sizeof(rsp), &rsp, -1);
3901 }
3902
3903 static void handle_client_set_adv_data(const void *buf, uint16_t len)
3904 {
3905         const struct hal_cmd_gatt_client_set_adv_data *cmd = buf;
3906         uint8_t status;
3907
3908         if (len != sizeof(*cmd) + cmd->manufacturer_len) {
3909                 error("Invalid set adv data command (%u bytes), terminating",
3910                                                                         len);
3911                 raise(SIGTERM);
3912                 return;
3913         }
3914
3915         DBG("scan_rsp=%u name=%u tx=%u min=%d max=%d app=%d",
3916                 cmd->set_scan_rsp, cmd->include_name, cmd->include_txpower,
3917                 cmd->min_interval, cmd->max_interval, cmd->appearance);
3918
3919         DBG("manufacturer=%u service_data=%u service_uuid=%u",
3920                                 cmd->manufacturer_len, cmd->service_data_len,
3921                                 cmd->service_uuid_len);
3922
3923         /* TODO This should be implemented when kernel supports it */
3924         if (cmd->manufacturer_len || cmd->service_data_len ||
3925                                                         cmd->service_uuid_len) {
3926                 error("gatt: Extra advertising data not supported");
3927                 status = HAL_STATUS_FAILED;
3928                 goto failed;
3929         }
3930
3931         status = HAL_STATUS_SUCCESS;
3932
3933 failed:
3934         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
3935                                 HAL_OP_GATT_CLIENT_SET_ADV_DATA, status);
3936 }
3937
3938 static void test_command_result(guint8 status, const guint8 *pdu,
3939                                         guint16 len, gpointer user_data)
3940 {
3941         DBG("status: %d", status);
3942 }
3943
3944 static uint8_t test_read_write(bdaddr_t *bdaddr, bt_uuid_t *uuid, uint16_t op,
3945                                                 uint16_t u2, uint16_t u3,
3946                                                 uint16_t u4, uint16_t u5)
3947 {
3948         guint16 length = 0;
3949         struct gatt_device *dev;
3950         uint8_t *pdu;
3951         size_t mtu;
3952
3953         dev = find_device_by_addr(bdaddr);
3954         if (!dev || dev->state != DEVICE_CONNECTED)
3955                 return HAL_STATUS_FAILED;
3956
3957         pdu = g_attrib_get_buffer(dev->attrib, &mtu);
3958         if (!pdu)
3959                 return HAL_STATUS_FAILED;
3960
3961         switch (op) {
3962         case ATT_OP_READ_REQ:
3963                 length = enc_read_req(u2, pdu, mtu);
3964                 break;
3965         case ATT_OP_READ_BY_TYPE_REQ:
3966                 length = enc_read_by_type_req(u2, u3, uuid, pdu, mtu);
3967                 break;
3968         case ATT_OP_READ_BLOB_REQ:
3969                 length = enc_read_blob_req(u2, u3, pdu, mtu);
3970                 break;
3971         case ATT_OP_READ_BY_GROUP_REQ:
3972                 length = enc_read_by_grp_req(u2, u3, uuid, pdu, mtu);
3973                 break;
3974         case ATT_OP_READ_MULTI_REQ:
3975                 return HAL_STATUS_UNSUPPORTED;
3976         case ATT_OP_WRITE_REQ:
3977                 length = enc_write_req(u2, (uint8_t *) &u3, sizeof(u3), pdu,
3978                                                                         mtu);
3979                 break;
3980         case ATT_OP_WRITE_CMD:
3981                 length = enc_write_cmd(u2, (uint8_t *) &u3, sizeof(u3), pdu,
3982                                                                         mtu);
3983                 break;
3984         case ATT_OP_PREP_WRITE_REQ:
3985                 length = enc_prep_write_req(u2, u3, (uint8_t *) &u4, sizeof(u4),
3986                                                                 pdu, mtu);
3987                 break;
3988         case ATT_OP_EXEC_WRITE_REQ:
3989                 length = enc_exec_write_req(u2, pdu, mtu);
3990                 break;
3991         case ATT_OP_SIGNED_WRITE_CMD:
3992                 if (signed_write_cmd(dev, u2, (uint8_t *) &u3, sizeof(u3)))
3993                         return HAL_STATUS_SUCCESS;
3994                 else
3995                         return HAL_STATUS_FAILED;
3996         default:
3997                 error("gatt: Unknown operation type");
3998
3999                 return HAL_STATUS_UNSUPPORTED;
4000         }
4001
4002         if (!g_attrib_send(dev->attrib, 0, pdu, length, test_command_result,
4003                                                                 NULL, NULL))
4004                 return HAL_STATUS_FAILED;
4005
4006         return HAL_STATUS_SUCCESS;
4007 }
4008
4009 static uint8_t test_increase_security(bdaddr_t *bdaddr, uint16_t u1)
4010 {
4011         struct gatt_device *device;
4012
4013         device = find_device_by_addr(bdaddr);
4014         if (!device)
4015                 return HAL_STATUS_FAILED;
4016
4017         if (!set_auth_type(device, u1))
4018                 return HAL_STATUS_FAILED;
4019
4020         return HAL_STATUS_SUCCESS;
4021 }
4022
4023 static void handle_client_test_command(const void *buf, uint16_t len)
4024 {
4025         const struct hal_cmd_gatt_client_test_command *cmd = buf;
4026         struct gatt_app *app;
4027         bdaddr_t bdaddr;
4028         bt_uuid_t uuid;
4029         uint8_t status;
4030
4031         DBG("");
4032
4033         android2bdaddr(cmd->bda1, &bdaddr);
4034         android2uuid(cmd->uuid1, &uuid);
4035
4036         switch (cmd->command) {
4037         case GATT_CLIENT_TEST_CMD_ENABLE:
4038                 if (cmd->u1) {
4039                         if (!test_client_if) {
4040                                 app = register_app(TEST_UUID, GATT_CLIENT);
4041                                 if (app)
4042                                         test_client_if = app->id;
4043                         }
4044
4045                         if (test_client_if)
4046                                 status = HAL_STATUS_SUCCESS;
4047                         else
4048                                 status = HAL_STATUS_FAILED;
4049                 } else {
4050                         status = unregister_app(test_client_if);
4051                         test_client_if = 0;
4052                 }
4053                 break;
4054         case GATT_CLIENT_TEST_CMD_CONNECT:
4055                 /* TODO u1 holds device type, for now assume BLE */
4056                 status = handle_connect(test_client_if, &bdaddr, false);
4057                 break;
4058         case GATT_CLIENT_TEST_CMD_DISCONNECT:
4059                 app = queue_find(gatt_apps, match_app_by_id,
4060                                                 INT_TO_PTR(test_client_if));
4061                 queue_remove_all(app_connections, match_connection_by_app, app,
4062                                                         destroy_connection);
4063
4064                 status = HAL_STATUS_SUCCESS;
4065                 break;
4066         case GATT_CLIENT_TEST_CMD_DISCOVER:
4067                 status = HAL_STATUS_FAILED;
4068                 break;
4069         case GATT_CLIENT_TEST_CMD_READ:
4070         case GATT_CLIENT_TEST_CMD_WRITE:
4071                 status = test_read_write(&bdaddr, &uuid, cmd->u1, cmd->u2,
4072                                                 cmd->u3, cmd->u4, cmd->u5);
4073                 break;
4074         case GATT_CLIENT_TEST_CMD_INCREASE_SECURITY:
4075                 status = test_increase_security(&bdaddr, cmd->u1);
4076                 break;
4077         case GATT_CLIENT_TEST_CMD_PAIRING_CONFIG:
4078         default:
4079                 status = HAL_STATUS_FAILED;
4080                 break;
4081         }
4082
4083         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
4084                                 HAL_OP_GATT_CLIENT_TEST_COMMAND, status);
4085 }
4086
4087 static void handle_server_register(const void *buf, uint16_t len)
4088 {
4089         const struct hal_cmd_gatt_server_register *cmd = buf;
4090         struct hal_ev_gatt_server_register ev;
4091         struct gatt_app *app;
4092
4093         DBG("");
4094
4095         memset(&ev, 0, sizeof(ev));
4096
4097         app = register_app(cmd->uuid, GATT_SERVER);
4098
4099         if (app) {
4100                 ev.server_if = app->id;
4101                 ev.status = GATT_SUCCESS;
4102         } else {
4103                 ev.status = GATT_FAILURE;
4104         }
4105
4106         memcpy(ev.uuid, cmd->uuid, sizeof(ev.uuid));
4107
4108         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
4109                                 HAL_EV_GATT_SERVER_REGISTER, sizeof(ev), &ev);
4110
4111         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_SERVER_REGISTER,
4112                                                         HAL_STATUS_SUCCESS);
4113 }
4114
4115 static void handle_server_unregister(const void *buf, uint16_t len)
4116 {
4117         const struct hal_cmd_gatt_server_unregister *cmd = buf;
4118         uint8_t status;
4119
4120         DBG("");
4121
4122         status = unregister_app(cmd->server_if);
4123
4124         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
4125                                         HAL_OP_GATT_SERVER_UNREGISTER, status);
4126 }
4127
4128 static void handle_server_connect(const void *buf, uint16_t len)
4129 {
4130         const struct hal_cmd_gatt_server_connect *cmd = buf;
4131         uint8_t status;
4132         bdaddr_t addr;
4133
4134         DBG("");
4135
4136         android2bdaddr(&cmd->bdaddr, &addr);
4137
4138         /* TODO: Handle transport flag */
4139
4140         status = handle_connect(cmd->server_if, &addr, cmd->is_direct);
4141
4142         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT, HAL_OP_GATT_SERVER_CONNECT,
4143                                                                 status);
4144 }
4145
4146 static void handle_server_disconnect(const void *buf, uint16_t len)
4147 {
4148         const struct hal_cmd_gatt_server_disconnect *cmd = buf;
4149         struct app_connection *conn;
4150         uint8_t status;
4151
4152         DBG("");
4153
4154         /* TODO: should we care to match also bdaddr when conn_id is unique? */
4155         conn = queue_remove_if(app_connections, match_connection_by_id,
4156                                                 INT_TO_PTR(cmd->conn_id));
4157         destroy_connection(conn);
4158
4159         status = HAL_STATUS_SUCCESS;
4160
4161         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
4162                                         HAL_OP_GATT_SERVER_DISCONNECT, status);
4163 }
4164
4165 static void handle_server_add_service(const void *buf, uint16_t len)
4166 {
4167         const struct hal_cmd_gatt_server_add_service *cmd = buf;
4168         struct hal_ev_gatt_server_service_added ev;
4169         struct gatt_app *server;
4170         struct gatt_db_attribute *service;
4171         uint8_t status;
4172         bt_uuid_t uuid;
4173
4174         DBG("");
4175
4176         memset(&ev, 0, sizeof(ev));
4177
4178         server = find_app_by_id(cmd->server_if);
4179         if (!server) {
4180                 status = HAL_STATUS_FAILED;
4181                 goto failed;
4182         }
4183
4184         android2uuid(cmd->srvc_id.uuid, &uuid);
4185
4186         service = gatt_db_add_service(gatt_db, &uuid, cmd->srvc_id.is_primary,
4187                                                         cmd->num_handles);
4188         if (!service) {
4189                 status = HAL_STATUS_FAILED;
4190                 goto failed;
4191         }
4192
4193         ev.srvc_handle = gatt_db_attribute_get_handle(service);
4194         if (!ev.srvc_handle) {
4195                 status = HAL_STATUS_FAILED;
4196                 goto failed;
4197         }
4198
4199         status = HAL_STATUS_SUCCESS;
4200
4201 failed:
4202         ev.status = status == HAL_STATUS_SUCCESS ? GATT_SUCCESS : GATT_FAILURE;
4203         ev.srvc_id = cmd->srvc_id;
4204         ev.server_if = cmd->server_if;
4205
4206         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
4207                         HAL_EV_GATT_SERVER_SERVICE_ADDED, sizeof(ev), &ev);
4208
4209         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
4210                                         HAL_OP_GATT_SERVER_ADD_SERVICE, status);
4211 }
4212
4213 static void handle_server_add_included_service(const void *buf, uint16_t len)
4214 {
4215         const struct hal_cmd_gatt_server_add_inc_service *cmd = buf;
4216         struct hal_ev_gatt_server_inc_srvc_added ev;
4217         struct gatt_app *server;
4218         struct gatt_db_attribute *service, *include;
4219         uint8_t status;
4220
4221         DBG("");
4222
4223         memset(&ev, 0, sizeof(ev));
4224
4225         server = find_app_by_id(cmd->server_if);
4226         if (!server) {
4227                 status = HAL_STATUS_FAILED;
4228                 goto failed;
4229         }
4230
4231         service = gatt_db_get_attribute(gatt_db, cmd->service_handle);
4232         if (!service) {
4233                 status = HAL_STATUS_FAILED;
4234                 goto failed;
4235         }
4236
4237         include = gatt_db_get_attribute(gatt_db, cmd->included_handle);
4238         if (!include) {
4239                 status = HAL_STATUS_FAILED;
4240                 goto failed;
4241         }
4242
4243         service = gatt_db_service_add_included(service, include);
4244         if (!service) {
4245                 status = HAL_STATUS_FAILED;
4246                 goto failed;
4247         }
4248
4249         ev.incl_srvc_handle = gatt_db_attribute_get_handle(service);
4250         status = HAL_STATUS_SUCCESS;
4251 failed:
4252         ev.srvc_handle = cmd->service_handle;
4253         ev.status = status;
4254         ev.server_if = cmd->server_if;
4255         ev.status = status == HAL_STATUS_SUCCESS ? GATT_SUCCESS : GATT_FAILURE;
4256
4257         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
4258                         HAL_EV_GATT_SERVER_INC_SRVC_ADDED, sizeof(ev), &ev);
4259
4260         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
4261                                 HAL_OP_GATT_SERVER_ADD_INC_SERVICE, status);
4262 }
4263
4264 static bool is_service(const bt_uuid_t *type)
4265 {
4266         bt_uuid_t uuid;
4267
4268         bt_uuid16_create(&uuid, GATT_PRIM_SVC_UUID);
4269         if (!bt_uuid_cmp(&uuid, type))
4270                 return true;
4271
4272         bt_uuid16_create(&uuid, GATT_SND_SVC_UUID);
4273         if (!bt_uuid_cmp(&uuid, type))
4274                 return true;
4275
4276         return false;
4277 }
4278
4279 static bool match_pending_dev_request(const void *data, const void *user_data)
4280 {
4281         const struct pending_request *pending_request = data;
4282
4283         return !pending_request->completed;
4284 }
4285
4286 static void send_dev_complete_response(struct gatt_device *device,
4287                                                                 uint8_t opcode)
4288 {
4289         size_t mtu;
4290         uint8_t *rsp = g_attrib_get_buffer(device->attrib, &mtu);
4291         struct pending_request *val;
4292         uint16_t len = 0;
4293         uint8_t error = 0;
4294
4295         if (queue_isempty(device->pending_requests))
4296                 return;
4297
4298         if (queue_find(device->pending_requests, match_pending_dev_request,
4299                                                                         NULL)) {
4300                 DBG("Still pending requests");
4301                 return;
4302         }
4303
4304         val = queue_peek_head(device->pending_requests);
4305         if (!val) {
4306                 error = ATT_ECODE_ATTR_NOT_FOUND;
4307                 goto done;
4308         }
4309
4310         if (val->error) {
4311                 error = val->error;
4312                 goto done;
4313         }
4314
4315         switch (opcode) {
4316         case ATT_OP_READ_BY_TYPE_REQ: {
4317                 struct att_data_list *adl;
4318                 int iterator = 0;
4319                 int length;
4320                 struct queue *temp;
4321
4322                 temp = queue_new();
4323
4324                 val = queue_pop_head(device->pending_requests);
4325                 if (!val) {
4326                         queue_destroy(temp, NULL);
4327                         error = ATT_ECODE_ATTR_NOT_FOUND;
4328                         goto done;
4329                 }
4330
4331                 if (val->error) {
4332                         queue_destroy(temp, NULL);
4333                         error = val->error;
4334                         destroy_pending_request(val);
4335                         goto done;
4336                 }
4337
4338                 length = val->length;
4339
4340                 while (val && val->length == length && val->error == 0) {
4341                         queue_push_tail(temp, val);
4342                         val = queue_pop_head(device->pending_requests);
4343                 }
4344
4345                 adl = att_data_list_alloc(queue_length(temp),
4346                                                 sizeof(uint16_t) + length);
4347
4348                 destroy_pending_request(val);
4349
4350                 val = queue_pop_head(temp);
4351                 while (val) {
4352                         uint8_t *value = adl->data[iterator++];
4353                         uint16_t handle;
4354
4355                         handle = gatt_db_attribute_get_handle(val->attrib);
4356
4357                         put_le16(handle, value);
4358                         memcpy(&value[2], val->value, val->length);
4359
4360                         destroy_pending_request(val);
4361                         val = queue_pop_head(temp);
4362                 }
4363
4364                 len = enc_read_by_type_resp(adl, rsp, mtu);
4365
4366                 att_data_list_free(adl);
4367                 queue_destroy(temp, destroy_pending_request);
4368
4369                 break;
4370         }
4371         case ATT_OP_READ_BLOB_REQ:
4372                 len = enc_read_blob_resp(val->value, val->length, val->offset,
4373                                                                 rsp, mtu);
4374                 break;
4375         case ATT_OP_READ_REQ:
4376                 len = enc_read_resp(val->value, val->length, rsp, mtu);
4377                 break;
4378         case ATT_OP_READ_BY_GROUP_REQ: {
4379                 struct att_data_list *adl;
4380                 int iterator = 0;
4381                 int length;
4382                 struct queue *temp;
4383
4384                 temp = queue_new();
4385
4386                 val = queue_pop_head(device->pending_requests);
4387                 if (!val) {
4388                         queue_destroy(temp, NULL);
4389                         error = ATT_ECODE_ATTR_NOT_FOUND;
4390                         goto done;
4391                 }
4392
4393                 length = val->length;
4394
4395                 while (val && val->length == length) {
4396                         queue_push_tail(temp, val);
4397                         val = queue_pop_head(device->pending_requests);
4398                 }
4399
4400                 adl = att_data_list_alloc(queue_length(temp),
4401                                                 2 * sizeof(uint16_t) + length);
4402
4403                 val = queue_pop_head(temp);
4404                 while (val) {
4405                         uint8_t *value = adl->data[iterator++];
4406                         uint16_t start_handle, end_handle;
4407
4408                         gatt_db_attribute_get_service_handles(val->attrib,
4409                                                                 &start_handle,
4410                                                                 &end_handle);
4411
4412                         put_le16(start_handle, value);
4413                         put_le16(end_handle, &value[2]);
4414                         memcpy(&value[4], val->value, val->length);
4415
4416                         destroy_pending_request(val);
4417                         val = queue_pop_head(temp);
4418                 }
4419
4420                 len = enc_read_by_grp_resp(adl, rsp, mtu);
4421
4422                 att_data_list_free(adl);
4423                 queue_destroy(temp, destroy_pending_request);
4424
4425                 break;
4426         }
4427         case ATT_OP_FIND_BY_TYPE_REQ: {
4428                 GSList *list = NULL;
4429
4430                 val = queue_pop_head(device->pending_requests);
4431                 while (val) {
4432                         struct att_range *range;
4433                         const bt_uuid_t *type;
4434
4435                         /* Its find by type and value - filter by value here */
4436                         if ((val->length != val->filter_vlen) ||
4437                                 memcmp(val->value, val->filter_value,
4438                                                                 val->length)) {
4439
4440                                 destroy_pending_request(val);
4441                                 val = queue_pop_head(device->pending_requests);
4442                                 continue;
4443                         }
4444
4445                         range = new0(struct att_range, 1);
4446                         range->start = gatt_db_attribute_get_handle(
4447                                                                 val->attrib);
4448
4449                         type = gatt_db_attribute_get_type(val->attrib);
4450                         if (is_service(type))
4451                                 gatt_db_attribute_get_service_handles(
4452                                                                 val->attrib,
4453                                                                 NULL,
4454                                                                 &range->end);
4455                         else
4456                                 range->end = range->start;
4457
4458                         list = g_slist_append(list, range);
4459
4460                         destroy_pending_request(val);
4461                         val = queue_pop_head(device->pending_requests);
4462                 }
4463
4464                 if (list && !error)
4465                         len = enc_find_by_type_resp(list, rsp, mtu);
4466                 else
4467                         error = ATT_ECODE_ATTR_NOT_FOUND;
4468
4469                 g_slist_free_full(list, free);
4470
4471                 break;
4472         }
4473         case ATT_OP_EXEC_WRITE_REQ:
4474                 len = enc_exec_write_resp(rsp);
4475                 break;
4476         case ATT_OP_WRITE_REQ:
4477                 len = enc_write_resp(rsp);
4478                 break;
4479         case ATT_OP_PREP_WRITE_REQ: {
4480                 uint16_t handle;
4481
4482                 handle = gatt_db_attribute_get_handle(val->attrib);
4483                 len = enc_prep_write_resp(handle, val->offset, val->value,
4484                                                         val->length, rsp, mtu);
4485                 break;
4486         }
4487         default:
4488                 break;
4489         }
4490
4491 done:
4492         if (!len)
4493                 len = enc_error_resp(opcode, 0x0000, error, rsp, mtu);
4494
4495         g_attrib_send(device->attrib, 0, rsp, len, NULL, NULL, NULL);
4496
4497         queue_remove_all(device->pending_requests, NULL, NULL,
4498                                                 destroy_pending_request);
4499 }
4500
4501 struct request_processing_data {
4502         uint8_t opcode;
4503         struct gatt_device *device;
4504 };
4505
4506 static uint8_t check_device_permissions(struct gatt_device *device,
4507                                         uint8_t opcode, uint32_t permissions)
4508 {
4509         GIOChannel *io;
4510         int sec_level;
4511
4512         io = g_attrib_get_channel(device->attrib);
4513
4514         if (!bt_io_get(io, NULL, BT_IO_OPT_SEC_LEVEL, &sec_level,
4515                                                         BT_IO_OPT_INVALID))
4516                 return ATT_ECODE_UNLIKELY;
4517
4518         DBG("opcode 0x%02x permissions %u sec_level %u", opcode, permissions,
4519                                                                 sec_level);
4520
4521         switch (opcode) {
4522         case ATT_OP_SIGNED_WRITE_CMD:
4523                 if (!(permissions & GATT_PERM_WRITE_SIGNED))
4524                                 return ATT_ECODE_WRITE_NOT_PERM;
4525
4526                 if (permissions & GATT_PERM_WRITE_SIGNED_MITM) {
4527                         bool auth;
4528
4529                         if (bt_get_csrk(&device->bdaddr, true, NULL, NULL,
4530                                         &auth) && auth)
4531                                 break;
4532
4533                         return ATT_ECODE_AUTHENTICATION;
4534                 }
4535                 break;
4536         case ATT_OP_READ_BY_TYPE_REQ:
4537         case ATT_OP_READ_REQ:
4538         case ATT_OP_READ_BLOB_REQ:
4539         case ATT_OP_READ_MULTI_REQ:
4540         case ATT_OP_READ_BY_GROUP_REQ:
4541         case ATT_OP_FIND_BY_TYPE_REQ:
4542         case ATT_OP_FIND_INFO_REQ:
4543                 if (!(permissions & GATT_PERM_READ))
4544                         return ATT_ECODE_READ_NOT_PERM;
4545
4546                 if ((permissions & GATT_PERM_READ_MITM) &&
4547                                                 sec_level < BT_SECURITY_HIGH)
4548                         return ATT_ECODE_AUTHENTICATION;
4549
4550                 if ((permissions & GATT_PERM_READ_ENCRYPTED) &&
4551                                                 sec_level < BT_SECURITY_MEDIUM)
4552                         return ATT_ECODE_INSUFF_ENC;
4553
4554                 if (permissions & GATT_PERM_READ_AUTHORIZATION)
4555                         return ATT_ECODE_AUTHORIZATION;
4556                 break;
4557         case ATT_OP_WRITE_REQ:
4558         case ATT_OP_WRITE_CMD:
4559         case ATT_OP_PREP_WRITE_REQ:
4560         case ATT_OP_EXEC_WRITE_REQ:
4561                 if (!(permissions & GATT_PERM_WRITE))
4562                         return ATT_ECODE_WRITE_NOT_PERM;
4563
4564                 if ((permissions & GATT_PERM_WRITE_MITM) &&
4565                                                 sec_level < BT_SECURITY_HIGH)
4566                         return ATT_ECODE_AUTHENTICATION;
4567
4568                 if ((permissions & GATT_PERM_WRITE_ENCRYPTED) &&
4569                                                 sec_level < BT_SECURITY_MEDIUM)
4570                         return ATT_ECODE_INSUFF_ENC;
4571
4572                 if (permissions & GATT_PERM_WRITE_AUTHORIZATION)
4573                         return ATT_ECODE_AUTHORIZATION;
4574                 break;
4575         default:
4576                 return ATT_ECODE_UNLIKELY;
4577         }
4578
4579         return 0;
4580 }
4581
4582 static uint8_t err_to_att(int err)
4583 {
4584         if (!err || (err > 0 && err < UINT8_MAX))
4585                 return err;
4586
4587         switch (err) {
4588         case -ENOENT:
4589                 return ATT_ECODE_INVALID_HANDLE;
4590         case -ENOMEM:
4591                 return ATT_ECODE_INSUFF_RESOURCES;
4592         default:
4593                 return ATT_ECODE_UNLIKELY;
4594         }
4595 }
4596
4597 static void attribute_read_cb(struct gatt_db_attribute *attrib, int err,
4598                                         const uint8_t *value, size_t length,
4599                                         void *user_data)
4600 {
4601         struct pending_request *resp_data = user_data;
4602         uint8_t error = err_to_att(err);
4603
4604         resp_data->attrib = attrib;
4605         resp_data->length = length;
4606         resp_data->error = error;
4607
4608         resp_data->completed = true;
4609
4610         if (!length)
4611                 return;
4612
4613         resp_data->value = malloc0(length);
4614         if (!resp_data->value) {
4615                 resp_data->error = ATT_ECODE_INSUFF_RESOURCES;
4616
4617                 return;
4618         }
4619
4620         memcpy(resp_data->value, value, length);
4621 }
4622
4623 static void read_requested_attributes(void *data, void *user_data)
4624 {
4625         struct pending_request *resp_data = data;
4626         struct request_processing_data *process_data = user_data;
4627         struct bt_att *att = g_attrib_get_att(process_data->device->attrib);
4628         struct gatt_db_attribute *attrib;
4629         uint32_t permissions;
4630         uint8_t error;
4631
4632         attrib = resp_data->attrib;
4633         if (!attrib) {
4634                 resp_data->error = ATT_ECODE_ATTR_NOT_FOUND;
4635                 resp_data->completed = true;
4636                 return;
4637         }
4638
4639         permissions = gatt_db_attribute_get_permissions(attrib);
4640
4641         /*
4642          * Check if it is attribute we didn't declare permissions, like service
4643          * declaration or included service. Set permissions to read only
4644          */
4645         if (permissions == 0)
4646                 permissions = GATT_PERM_READ;
4647
4648         error = check_device_permissions(process_data->device,
4649                                                         process_data->opcode,
4650                                                         permissions);
4651         if (error != 0) {
4652                 resp_data->error = error;
4653                 resp_data->completed = true;
4654                 return;
4655         }
4656
4657         gatt_db_attribute_read(attrib, resp_data->offset, process_data->opcode,
4658                                         att, attribute_read_cb, resp_data);
4659 }
4660
4661 static void process_dev_pending_requests(struct gatt_device *device,
4662                                                         uint8_t att_opcode)
4663 {
4664         struct request_processing_data process_data;
4665
4666         if (queue_isempty(device->pending_requests))
4667                 return;
4668
4669         process_data.device = device;
4670         process_data.opcode = att_opcode;
4671
4672         /* Process pending requests and prepare response */
4673         queue_foreach(device->pending_requests, read_requested_attributes,
4674                                                                 &process_data);
4675
4676         send_dev_complete_response(device, att_opcode);
4677 }
4678
4679 static struct pending_trans_data *conn_add_transact(struct app_connection *conn,
4680                                         uint8_t opcode,
4681                                         struct gatt_db_attribute *attrib,
4682                                         unsigned int serial_id)
4683 {
4684         struct pending_trans_data *transaction;
4685         static int32_t trans_id = 1;
4686
4687         transaction = new0(struct pending_trans_data, 1);
4688         transaction->id = trans_id++;
4689         transaction->opcode = opcode;
4690         transaction->attrib = attrib;
4691         transaction->serial_id = serial_id;
4692
4693         queue_push_tail(conn->transactions, transaction);
4694
4695         return transaction;
4696 }
4697
4698 static bool get_dst_addr(struct bt_att *att, bdaddr_t *dst)
4699 {
4700         GIOChannel *io = NULL;
4701         GError *gerr = NULL;
4702
4703         io = g_io_channel_unix_new(bt_att_get_fd(att));
4704         if (!io)
4705                 return false;
4706
4707         bt_io_get(io, &gerr, BT_IO_OPT_DEST_BDADDR, dst, BT_IO_OPT_INVALID);
4708         if (gerr) {
4709                 error("gatt: bt_io_get: %s", gerr->message);
4710                 g_error_free(gerr);
4711                 g_io_channel_unref(io);
4712                 return false;
4713         }
4714
4715         g_io_channel_unref(io);
4716         return true;
4717 }
4718
4719 static void read_cb(struct gatt_db_attribute *attrib, unsigned int id,
4720                         uint16_t offset, uint8_t opcode, struct bt_att *att,
4721                         void *user_data)
4722 {
4723         struct pending_trans_data *transaction;
4724         struct hal_ev_gatt_server_request_read ev;
4725         struct gatt_app *app;
4726         struct app_connection *conn;
4727         int32_t app_id = PTR_TO_INT(user_data);
4728         bdaddr_t bdaddr;
4729
4730         DBG("id %u", id);
4731
4732         app = find_app_by_id(app_id);
4733         if (!app) {
4734                 error("gatt: read_cb, cound not found app id");
4735                 goto failed;
4736         }
4737
4738         if (!get_dst_addr(att, &bdaddr)) {
4739                 error("gatt: read_cb, could not obtain dst BDADDR");
4740                 goto failed;
4741         }
4742
4743         conn = find_conn(&bdaddr, app->id);
4744         if (!conn) {
4745                 error("gatt: read_cb, cound not found connection");
4746                 goto failed;
4747         }
4748
4749         memset(&ev, 0, sizeof(ev));
4750
4751         /* Store the request data, complete callback and transaction id */
4752         transaction = conn_add_transact(conn, opcode, attrib, id);
4753
4754         bdaddr2android(&bdaddr, ev.bdaddr);
4755         ev.conn_id = conn->id;
4756         ev.attr_handle = gatt_db_attribute_get_handle(attrib);
4757         ev.offset = offset;
4758         ev.is_long = opcode == ATT_OP_READ_BLOB_REQ;
4759         ev.trans_id = transaction->id;
4760
4761         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
4762                                         HAL_EV_GATT_SERVER_REQUEST_READ,
4763                                         sizeof(ev), &ev);
4764
4765         return;
4766
4767 failed:
4768         gatt_db_attribute_read_result(attrib, id, -ENOENT, NULL, 0);
4769 }
4770
4771 static void write_cb(struct gatt_db_attribute *attrib, unsigned int id,
4772                         uint16_t offset, const uint8_t *value, size_t len,
4773                         uint8_t opcode, struct bt_att *att, void *user_data)
4774 {
4775         uint8_t buf[IPC_MTU];
4776         struct hal_ev_gatt_server_request_write *ev = (void *) buf;
4777         struct pending_trans_data *transaction;
4778         struct gatt_app *app;
4779         int32_t app_id = PTR_TO_INT(user_data);
4780         struct app_connection *conn;
4781         bdaddr_t bdaddr;
4782
4783         DBG("id %u", id);
4784
4785         app = find_app_by_id(app_id);
4786         if (!app) {
4787                 error("gatt: write_cb could not found app id");
4788                 goto failed;
4789         }
4790
4791         if (!get_dst_addr(att, &bdaddr)) {
4792                 error("gatt: write_cb, could not obtain dst BDADDR");
4793                 goto failed;
4794         }
4795
4796         conn = find_conn(&bdaddr, app->id);
4797         if (!conn) {
4798                 error("gatt: write_cb could not found connection");
4799                 goto failed;
4800         }
4801
4802         /*
4803          * Remember that this application has ongoing prep write
4804          * Need it later to find out where to send execute write
4805          */
4806         if (opcode == ATT_OP_PREP_WRITE_REQ)
4807                 conn->wait_execute_write = true;
4808
4809         /* Store the request data, complete callback and transaction id */
4810         transaction = conn_add_transact(conn, opcode, attrib, id);
4811
4812         memset(ev, 0, sizeof(*ev));
4813
4814         bdaddr2android(&bdaddr, &ev->bdaddr);
4815         ev->attr_handle = gatt_db_attribute_get_handle(attrib);
4816         ev->offset = offset;
4817
4818         ev->conn_id = conn->id;
4819         ev->trans_id = transaction->id;
4820
4821         ev->is_prep = opcode == ATT_OP_PREP_WRITE_REQ;
4822
4823         if (opcode == ATT_OP_WRITE_REQ || opcode == ATT_OP_PREP_WRITE_REQ)
4824                 ev->need_rsp = 0x01;
4825         else
4826                 gatt_db_attribute_write_result(attrib, id, 0);
4827
4828         ev->length = len;
4829         memcpy(ev->value, value, len);
4830
4831         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
4832                         HAL_EV_GATT_SERVER_REQUEST_WRITE,
4833                                                 sizeof(*ev) + ev->length , ev);
4834         return;
4835
4836 failed:
4837         gatt_db_attribute_write_result(attrib, id, ATT_ECODE_UNLIKELY);
4838 }
4839
4840 static uint32_t android_to_gatt_permissions(int32_t hal_permissions)
4841 {
4842         uint32_t permissions = 0;
4843
4844         if (hal_permissions & HAL_GATT_PERMISSION_READ)
4845                 permissions |= GATT_PERM_READ;
4846
4847         if (hal_permissions & HAL_GATT_PERMISSION_READ_ENCRYPTED)
4848                 permissions |= GATT_PERM_READ_ENCRYPTED | GATT_PERM_READ;
4849
4850         if (hal_permissions & HAL_GATT_PERMISSION_READ_ENCRYPTED_MITM)
4851                 permissions |= GATT_PERM_READ_MITM | GATT_PERM_READ_ENCRYPTED |
4852                                                                 GATT_PERM_READ;
4853
4854         if (hal_permissions & HAL_GATT_PERMISSION_WRITE)
4855                 permissions |= GATT_PERM_WRITE;
4856
4857         if (hal_permissions & HAL_GATT_PERMISSION_WRITE_ENCRYPTED)
4858                 permissions |= GATT_PERM_WRITE_ENCRYPTED | GATT_PERM_WRITE;
4859
4860         if (hal_permissions & HAL_GATT_PERMISSION_WRITE_ENCRYPTED_MITM)
4861                 permissions |= GATT_PERM_WRITE_MITM |
4862                                 GATT_PERM_WRITE_ENCRYPTED | GATT_PERM_WRITE;
4863
4864         if (hal_permissions & HAL_GATT_PERMISSION_WRITE_SIGNED)
4865                 permissions |= GATT_PERM_WRITE_SIGNED;
4866
4867         if (hal_permissions & HAL_GATT_PERMISSION_WRITE_SIGNED_MITM)
4868                 permissions |= GATT_PERM_WRITE_SIGNED_MITM |
4869                                                         GATT_PERM_WRITE_SIGNED;
4870
4871         return permissions;
4872 }
4873
4874 static void handle_server_add_characteristic(const void *buf, uint16_t len)
4875 {
4876         const struct hal_cmd_gatt_server_add_characteristic *cmd = buf;
4877         struct hal_ev_gatt_server_characteristic_added ev;
4878         struct gatt_app *server;
4879         struct gatt_db_attribute *attrib;
4880         bt_uuid_t uuid;
4881         uint8_t status;
4882         uint32_t permissions;
4883         int32_t app_id = cmd->server_if;
4884
4885         DBG("");
4886
4887         memset(&ev, 0, sizeof(ev));
4888
4889         server = find_app_by_id(app_id);
4890         if (!server) {
4891                 status = HAL_STATUS_FAILED;
4892                 goto failed;
4893         }
4894
4895         attrib = gatt_db_get_attribute(gatt_db, cmd->service_handle);
4896         if (!attrib) {
4897                 status = HAL_STATUS_FAILED;
4898                 goto failed;
4899         }
4900
4901         android2uuid(cmd->uuid, &uuid);
4902         permissions = android_to_gatt_permissions(cmd->permissions);
4903
4904         attrib = gatt_db_service_add_characteristic(attrib,
4905                                                         &uuid, permissions,
4906                                                         cmd->properties,
4907                                                         read_cb, write_cb,
4908                                                         INT_TO_PTR(app_id));
4909         if (!attrib) {
4910                 status = HAL_STATUS_FAILED;
4911                 goto failed;
4912         }
4913
4914         ev.char_handle = gatt_db_attribute_get_handle(attrib);
4915         status = HAL_STATUS_SUCCESS;
4916
4917 failed:
4918         ev.srvc_handle = cmd->service_handle;
4919         ev.status = status;
4920         ev.server_if = app_id;
4921         ev.status = status == HAL_STATUS_SUCCESS ? GATT_SUCCESS : GATT_FAILURE;
4922         memcpy(ev.uuid, cmd->uuid, sizeof(cmd->uuid));
4923
4924         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
4925                                 HAL_EV_GATT_SERVER_CHAR_ADDED, sizeof(ev), &ev);
4926
4927         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
4928                                 HAL_OP_GATT_SERVER_ADD_CHARACTERISTIC, status);
4929 }
4930
4931 static void handle_server_add_descriptor(const void *buf, uint16_t len)
4932 {
4933         const struct hal_cmd_gatt_server_add_descriptor *cmd = buf;
4934         struct hal_ev_gatt_server_descriptor_added ev;
4935         struct gatt_app *server;
4936         struct gatt_db_attribute *attrib;
4937         bt_uuid_t uuid;
4938         uint8_t status;
4939         uint32_t permissions;
4940         int32_t app_id = cmd->server_if;
4941
4942         DBG("");
4943
4944         memset(&ev, 0, sizeof(ev));
4945
4946         server = find_app_by_id(app_id);
4947         if (!server) {
4948                 status = HAL_STATUS_FAILED;
4949                 goto failed;
4950         }
4951
4952         android2uuid(cmd->uuid, &uuid);
4953         permissions = android_to_gatt_permissions(cmd->permissions);
4954
4955         attrib = gatt_db_get_attribute(gatt_db, cmd->service_handle);
4956         if (!attrib) {
4957                 status = HAL_STATUS_FAILED;
4958                 goto failed;
4959         }
4960
4961         attrib = gatt_db_service_add_descriptor(attrib, &uuid, permissions,
4962                                                         read_cb, write_cb,
4963                                                         INT_TO_PTR(app_id));
4964         if (!attrib) {
4965                 status = HAL_STATUS_FAILED;
4966                 goto failed;
4967         }
4968
4969         ev.descr_handle = gatt_db_attribute_get_handle(attrib);
4970         status = HAL_STATUS_SUCCESS;
4971
4972 failed:
4973         ev.server_if = app_id;
4974         ev.srvc_handle = cmd->service_handle;
4975         memcpy(ev.uuid, cmd->uuid, sizeof(cmd->uuid));
4976         ev.status = status == HAL_STATUS_SUCCESS ? GATT_SUCCESS : GATT_FAILURE;
4977
4978         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
4979                         HAL_EV_GATT_SERVER_DESCRIPTOR_ADDED, sizeof(ev), &ev);
4980
4981         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
4982                                 HAL_OP_GATT_SERVER_ADD_DESCRIPTOR, status);
4983 }
4984
4985 static void notify_service_change(void *data, void *user_data)
4986 {
4987         struct att_range range;
4988         struct gatt_db_attribute *attrib = user_data;
4989
4990         gatt_db_attribute_get_service_handles(attrib, &range.start, &range.end);
4991
4992         /* In case of db error */
4993         if (!range.end)
4994                 return;
4995
4996         notify_att_range_change(data, &range);
4997 }
4998
4999 static sdp_record_t *get_sdp_record(uuid_t *uuid, uint16_t start, uint16_t end,
5000                                                         const char *name)
5001 {
5002         sdp_list_t *svclass_id, *apseq, *proto[2], *root, *aproto;
5003         uuid_t root_uuid, proto_uuid, l2cap;
5004         sdp_record_t *record;
5005         sdp_data_t *psm, *sh, *eh;
5006         uint16_t lp = ATT_PSM;
5007
5008         record = sdp_record_alloc();
5009         if (!record)
5010                 return NULL;
5011
5012         sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP);
5013         root = sdp_list_append(NULL, &root_uuid);
5014         sdp_set_browse_groups(record, root);
5015         sdp_list_free(root, NULL);
5016
5017         svclass_id = sdp_list_append(NULL, uuid);
5018         sdp_set_service_classes(record, svclass_id);
5019         sdp_list_free(svclass_id, NULL);
5020
5021         sdp_uuid16_create(&l2cap, L2CAP_UUID);
5022         proto[0] = sdp_list_append(NULL, &l2cap);
5023         psm = sdp_data_alloc(SDP_UINT16, &lp);
5024         proto[0] = sdp_list_append(proto[0], psm);
5025         apseq = sdp_list_append(NULL, proto[0]);
5026
5027         sdp_uuid16_create(&proto_uuid, ATT_UUID);
5028         proto[1] = sdp_list_append(NULL, &proto_uuid);
5029         sh = sdp_data_alloc(SDP_UINT16, &start);
5030         proto[1] = sdp_list_append(proto[1], sh);
5031         eh = sdp_data_alloc(SDP_UINT16, &end);
5032         proto[1] = sdp_list_append(proto[1], eh);
5033         apseq = sdp_list_append(apseq, proto[1]);
5034
5035         aproto = sdp_list_append(NULL, apseq);
5036         sdp_set_access_protos(record, aproto);
5037
5038         if (name)
5039                 sdp_set_info_attr(record, name, "BlueZ for Android", NULL);
5040
5041         sdp_data_free(psm);
5042         sdp_data_free(sh);
5043         sdp_data_free(eh);
5044         sdp_list_free(proto[0], NULL);
5045         sdp_list_free(proto[1], NULL);
5046         sdp_list_free(apseq, NULL);
5047         sdp_list_free(aproto, NULL);
5048
5049         return record;
5050 }
5051
5052 static uint32_t add_sdp_record(const bt_uuid_t *uuid, uint16_t start,
5053                                                 uint16_t end, const char *name)
5054 {
5055         sdp_record_t *rec;
5056         uuid_t u, u32;
5057
5058         switch (uuid->type) {
5059         case BT_UUID16:
5060                 sdp_uuid16_create(&u, uuid->value.u16);
5061                 break;
5062         case BT_UUID32:
5063                 sdp_uuid32_create(&u32, uuid->value.u32);
5064                 sdp_uuid32_to_uuid128(&u, &u32);
5065                 break;
5066         case BT_UUID128:
5067                 sdp_uuid128_create(&u, &uuid->value.u128);
5068                 break;
5069         case BT_UUID_UNSPEC:
5070         default:
5071                 return 0;
5072         }
5073
5074         rec = get_sdp_record(&u, start, end, name);
5075         if (!rec)
5076                 return 0;
5077
5078         if (bt_adapter_add_record(rec, 0) < 0) {
5079                 error("gatt: Failed to register SDP record");
5080                 sdp_record_free(rec);
5081                 return 0;
5082         }
5083
5084         return rec->handle;
5085 }
5086
5087 static bool match_service_sdp(const void *data, const void *user_data)
5088 {
5089         const struct service_sdp *s = data;
5090
5091         return s->service_handle == PTR_TO_INT(user_data);
5092 }
5093
5094 static struct service_sdp *new_service_sdp_record(int32_t service_handle)
5095 {
5096         bt_uuid_t uuid;
5097         struct service_sdp *s;
5098         struct gatt_db_attribute *attrib;
5099         uint16_t end_handle;
5100
5101         attrib = gatt_db_get_attribute(gatt_db, service_handle);
5102         if (!attrib)
5103                 return NULL;
5104
5105         gatt_db_attribute_get_service_handles(attrib, NULL, &end_handle);
5106         if (!end_handle)
5107                 return NULL;
5108
5109         if (!gatt_db_attribute_get_service_uuid(attrib, &uuid))
5110                 return NULL;
5111
5112         s = new0(struct service_sdp, 1);
5113         s->service_handle = service_handle;
5114         s->sdp_handle = add_sdp_record(&uuid, service_handle, end_handle, NULL);
5115         if (!s->sdp_handle) {
5116                 free(s);
5117                 return NULL;
5118         }
5119
5120         return s;
5121 }
5122
5123 static void free_service_sdp_record(void *data)
5124 {
5125         struct service_sdp *s = data;
5126
5127         if (!s)
5128                 return;
5129
5130         bt_adapter_remove_record(s->sdp_handle);
5131         free(s);
5132 }
5133
5134 static bool add_service_sdp_record(int32_t service_handle)
5135 {
5136         struct service_sdp *s;
5137
5138         s = queue_find(services_sdp, match_service_sdp,
5139                                                 INT_TO_PTR(service_handle));
5140         if (s)
5141                 return true;
5142
5143         s = new_service_sdp_record(service_handle);
5144         if (!s)
5145                 return false;
5146
5147         queue_push_tail(services_sdp, s);
5148
5149         return true;
5150 }
5151
5152 static void remove_service_sdp_record(int32_t service_handle)
5153 {
5154         struct service_sdp *s;
5155
5156         s = queue_remove_if(services_sdp, match_service_sdp,
5157                                                 INT_TO_PTR(service_handle));
5158         if (!s)
5159                 return;
5160
5161         free_service_sdp_record(s);
5162 }
5163
5164 static void handle_server_start_service(const void *buf, uint16_t len)
5165 {
5166         const struct hal_cmd_gatt_server_start_service *cmd = buf;
5167         struct hal_ev_gatt_server_service_started ev;
5168         struct gatt_app *server;
5169         struct gatt_db_attribute *attrib;
5170         uint8_t status;
5171
5172         DBG("transport 0x%02x", cmd->transport);
5173
5174         memset(&ev, 0, sizeof(ev));
5175
5176         if (cmd->transport == 0) {
5177                 status = HAL_STATUS_FAILED;
5178                 goto failed;
5179         }
5180
5181         server = find_app_by_id(cmd->server_if);
5182         if (!server) {
5183                 status = HAL_STATUS_FAILED;
5184                 goto failed;
5185         }
5186
5187         if (cmd->transport & GATT_SERVER_TRANSPORT_BREDR_BIT) {
5188                 if (!add_service_sdp_record(cmd->service_handle)) {
5189                         status = HAL_STATUS_FAILED;
5190                         goto failed;
5191                 }
5192         }
5193         /* TODO: Handle BREDR only */
5194
5195         attrib = gatt_db_get_attribute(gatt_db, cmd->service_handle);
5196         if (!attrib) {
5197                 status = HAL_STATUS_FAILED;
5198                 goto failed;
5199         }
5200
5201         if (!gatt_db_service_set_active(attrib, true)) {
5202                 /*
5203                  * no need to clean SDP since this can fail only if service
5204                  * handle is invalid in which case add_sdp_record() also fails
5205                  */
5206                 status = HAL_STATUS_FAILED;
5207                 goto failed;
5208         }
5209
5210         queue_foreach(gatt_devices, notify_service_change, attrib);
5211
5212         status = HAL_STATUS_SUCCESS;
5213
5214 failed:
5215         ev.status = status == HAL_STATUS_SUCCESS ? GATT_SUCCESS : GATT_FAILURE;
5216         ev.server_if = cmd->server_if;
5217         ev.srvc_handle = cmd->service_handle;
5218
5219         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
5220                         HAL_EV_GATT_SERVER_SERVICE_STARTED, sizeof(ev), &ev);
5221
5222         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5223                                 HAL_OP_GATT_SERVER_START_SERVICE, status);
5224 }
5225
5226 static void handle_server_stop_service(const void *buf, uint16_t len)
5227 {
5228         const struct hal_cmd_gatt_server_stop_service *cmd = buf;
5229         struct hal_ev_gatt_server_service_stopped ev;
5230         struct gatt_app *server;
5231         struct gatt_db_attribute *attrib;
5232         uint8_t status;
5233
5234         DBG("");
5235
5236         memset(&ev, 0, sizeof(ev));
5237
5238         server = find_app_by_id(cmd->server_if);
5239         if (!server) {
5240                 status = HAL_STATUS_FAILED;
5241                 goto failed;
5242         }
5243
5244         attrib = gatt_db_get_attribute(gatt_db, cmd->service_handle);
5245         if (!attrib) {
5246                 status = HAL_STATUS_FAILED;
5247                 goto failed;
5248         }
5249
5250         if (!gatt_db_service_set_active(attrib, false)) {
5251                 status = HAL_STATUS_FAILED;
5252                 goto failed;
5253         }
5254
5255         remove_service_sdp_record(cmd->service_handle);
5256
5257         status = HAL_STATUS_SUCCESS;
5258
5259         queue_foreach(gatt_devices, notify_service_change, attrib);
5260
5261 failed:
5262         ev.status = status == HAL_STATUS_SUCCESS ? GATT_SUCCESS : GATT_FAILURE;
5263         ev.server_if = cmd->server_if;
5264         ev.srvc_handle = cmd->service_handle;
5265
5266         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
5267                         HAL_EV_GATT_SERVER_SERVICE_STOPPED, sizeof(ev), &ev);
5268
5269         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5270                                 HAL_OP_GATT_SERVER_STOP_SERVICE, status);
5271 }
5272
5273 static void handle_server_delete_service(const void *buf, uint16_t len)
5274 {
5275         const struct hal_cmd_gatt_server_delete_service *cmd = buf;
5276         struct hal_ev_gatt_server_service_deleted ev;
5277         struct gatt_app *server;
5278         struct gatt_db_attribute *attrib;
5279         uint8_t status;
5280
5281         DBG("");
5282
5283         memset(&ev, 0, sizeof(ev));
5284
5285         server = find_app_by_id(cmd->server_if);
5286         if (!server) {
5287                 status = HAL_STATUS_FAILED;
5288                 goto failed;
5289         }
5290
5291         attrib = gatt_db_get_attribute(gatt_db, cmd->service_handle);
5292         if (!attrib) {
5293                 status = HAL_STATUS_FAILED;
5294                 goto failed;
5295         }
5296
5297         if (!gatt_db_remove_service(gatt_db, attrib)) {
5298                 status = HAL_STATUS_FAILED;
5299                 goto failed;
5300         }
5301
5302         remove_service_sdp_record(cmd->service_handle);
5303
5304         status = HAL_STATUS_SUCCESS;
5305
5306 failed:
5307         ev.status = status == HAL_STATUS_SUCCESS ? GATT_SUCCESS : GATT_FAILURE;
5308         ev.srvc_handle = cmd->service_handle;
5309         ev.server_if = cmd->server_if;
5310
5311         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
5312                         HAL_EV_GATT_SERVER_SERVICE_DELETED, sizeof(ev), &ev);
5313
5314         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5315                                 HAL_OP_GATT_SERVER_DELETE_SERVICE, status);
5316 }
5317
5318 static void indication_confirmation_cb(guint8 status, const guint8 *pdu,
5319                                                 guint16 len, gpointer user_data)
5320 {
5321         struct hal_ev_gatt_server_indication_sent ev;
5322
5323         ev.status = status;
5324         ev.conn_id = PTR_TO_UINT(user_data);
5325
5326         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
5327                         HAL_EV_GATT_SERVER_INDICATION_SENT, sizeof(ev), &ev);
5328 }
5329
5330 static void handle_server_send_indication(const void *buf, uint16_t len)
5331 {
5332         const struct hal_cmd_gatt_server_send_indication *cmd = buf;
5333         struct app_connection *conn;
5334         uint8_t status;
5335         uint16_t length;
5336         uint8_t *pdu;
5337         size_t mtu;
5338         GAttribResultFunc confirmation_cb = NULL;
5339
5340         DBG("");
5341
5342         conn = find_connection_by_id(cmd->conn_id);
5343         if (!conn) {
5344                 error("gatt: Could not find connection");
5345                 status = HAL_STATUS_FAILED;
5346                 goto reply;
5347         }
5348
5349         pdu = g_attrib_get_buffer(conn->device->attrib, &mtu);
5350
5351         if (cmd->confirm) {
5352                 length = enc_indication(cmd->attribute_handle,
5353                                         (uint8_t *) cmd->value, cmd->len, pdu,
5354                                         mtu);
5355                 confirmation_cb = indication_confirmation_cb;
5356         } else {
5357                 length = enc_notification(cmd->attribute_handle,
5358                                                 (uint8_t *) cmd->value,
5359                                                 cmd->len, pdu, mtu);
5360         }
5361
5362         if (!g_attrib_send(conn->device->attrib, 0, pdu, length,
5363                                 confirmation_cb, UINT_TO_PTR(conn->id), NULL)) {
5364                 error("gatt: Failed to send indication");
5365                 status = HAL_STATUS_FAILED;
5366         } else {
5367                 status = HAL_STATUS_SUCCESS;
5368         }
5369
5370         /* Here we confirm failed indications and all notifications */
5371         if (status || !confirmation_cb)
5372                 indication_confirmation_cb(status, NULL, 0,
5373                                                         UINT_TO_PTR(conn->id));
5374
5375 reply:
5376         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5377                                 HAL_OP_GATT_SERVER_SEND_INDICATION, status);
5378 }
5379
5380 static bool match_trans_id(const void *data, const void *user_data)
5381 {
5382         const struct pending_trans_data *transaction = data;
5383
5384         return transaction->id == PTR_TO_UINT(user_data);
5385 }
5386
5387 static bool find_conn_waiting_exec_write(const void *data,
5388                                                         const void *user_data)
5389 {
5390         const struct app_connection *conn = data;
5391
5392         return conn->wait_execute_write;
5393 }
5394
5395 static bool pending_execute_write(void)
5396 {
5397         return queue_find(app_connections, find_conn_waiting_exec_write, NULL);
5398 }
5399
5400 static void handle_server_send_response(const void *buf, uint16_t len)
5401 {
5402         const struct hal_cmd_gatt_server_send_response *cmd = buf;
5403         struct pending_trans_data *transaction;
5404         struct app_connection *conn;
5405         uint8_t status;
5406
5407         DBG("");
5408
5409         conn = find_connection_by_id(cmd->conn_id);
5410         if (!conn) {
5411                 error("gatt: could not found connection");
5412                 status = HAL_STATUS_FAILED;
5413                 goto reply;
5414         }
5415
5416         transaction = queue_remove_if(conn->transactions, match_trans_id,
5417                                                 UINT_TO_PTR(cmd->trans_id));
5418         if (!transaction) {
5419                 error("gatt: transaction ID = %d not found", cmd->trans_id);
5420                 status = HAL_STATUS_FAILED;
5421                 goto reply;
5422         }
5423
5424         if (transaction->opcode == ATT_OP_EXEC_WRITE_REQ) {
5425                 struct pending_request *req;
5426
5427                 conn->wait_execute_write = false;
5428
5429                 /* Check for execute response from all server applications */
5430                 if (pending_execute_write())
5431                         goto done;
5432
5433                 /*
5434                  * This is usually done through db write callback but for
5435                  * execute write we dont have the attribute or handle to call
5436                  * gatt_db_attribute_write().
5437                  */
5438                 req = queue_peek_head(conn->device->pending_requests);
5439                 if (!req)
5440                         goto done;
5441
5442                 /* Cast status to uint8_t, due to (byte) cast in java layer. */
5443                 req->error = err_to_att((uint8_t) cmd->status);
5444                 req->completed = true;
5445
5446                 /*
5447                  * FIXME: Handle situation when not all server applications
5448                  * respond with a success.
5449                  */
5450         }
5451
5452         /* Cast status to uint8_t, due to (byte) cast in java layer. */
5453         if (transaction->opcode < ATT_OP_WRITE_REQ)
5454                 gatt_db_attribute_read_result(transaction->attrib,
5455                                         transaction->serial_id,
5456                                         err_to_att((uint8_t) cmd->status),
5457                                         cmd->data, cmd->len);
5458         else
5459                 gatt_db_attribute_write_result(transaction->attrib,
5460                                         transaction->serial_id,
5461                                         err_to_att((uint8_t) cmd->status));
5462
5463         send_dev_complete_response(conn->device, transaction->opcode);
5464
5465 done:
5466         /* Clean request data */
5467         free(transaction);
5468
5469         status = HAL_STATUS_SUCCESS;
5470
5471 reply:
5472         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5473                         HAL_OP_GATT_SERVER_SEND_RESPONSE, status);
5474 }
5475
5476 static void handle_client_scan_filter_setup(const void *buf, uint16_t len)
5477 {
5478         const struct hal_cmd_gatt_client_scan_filter_setup *cmd = buf;
5479
5480         DBG("client_if %u", cmd->client_if);
5481
5482         /* TODO */
5483
5484         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5485                                         HAL_OP_GATT_CLIENT_SCAN_FILTER_SETUP,
5486                                         HAL_STATUS_UNSUPPORTED);
5487 }
5488
5489 static void handle_client_scan_filter_add_remove(const void *buf, uint16_t len)
5490 {
5491         const struct hal_cmd_gatt_client_scan_filter_add_remove *cmd = buf;
5492
5493         DBG("client_if %u", cmd->client_if);
5494
5495         /* TODO */
5496
5497         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5498                                 HAL_OP_GATT_CLIENT_SCAN_FILTER_ADD_REMOVE,
5499                                 HAL_STATUS_UNSUPPORTED);
5500 }
5501
5502 static void handle_client_scan_filter_clear(const void *buf, uint16_t len)
5503 {
5504         const struct hal_cmd_gatt_client_scan_filter_clear *cmd = buf;
5505
5506         DBG("client_if %u", cmd->client_if);
5507
5508         /* TODO */
5509
5510         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5511                                         HAL_OP_GATT_CLIENT_SCAN_FILTER_CLEAR,
5512                                         HAL_STATUS_UNSUPPORTED);
5513 }
5514
5515 static void handle_client_scan_filter_enable(const void *buf, uint16_t len)
5516 {
5517         const struct hal_cmd_gatt_client_scan_filter_enable *cmd = buf;
5518
5519         DBG("client_if %u", cmd->client_if);
5520
5521         /* TODO */
5522
5523         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5524                                         HAL_OP_GATT_CLIENT_SCAN_FILTER_ENABLE,
5525                                         HAL_STATUS_UNSUPPORTED);
5526 }
5527
5528 static void handle_client_configure_mtu(const void *buf, uint16_t len)
5529 {
5530         const struct hal_cmd_gatt_client_configure_mtu *cmd = buf;
5531         static struct app_connection *conn;
5532         uint8_t status;
5533
5534         DBG("conn_id %u mtu %d", cmd->conn_id, cmd->mtu);
5535
5536         conn = find_connection_by_id(cmd->conn_id);
5537         if (!conn) {
5538                 status = HAL_STATUS_FAILED;
5539                 goto failed;
5540         }
5541
5542         /*
5543          * currently MTU is always exchanged on connection, just report current
5544          * value
5545          *
5546          * TODO figure out when send failed status in notification
5547          * TODO should we fail for BR/EDR?
5548          */
5549         notify_client_mtu_change(conn, false);
5550         status = HAL_STATUS_SUCCESS;
5551
5552 failed:
5553         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5554                                         HAL_OP_GATT_CLIENT_CONFIGURE_MTU,
5555                                         status);
5556 }
5557
5558 static void handle_client_conn_param_update(const void *buf, uint16_t len)
5559 {
5560         const struct hal_cmd_gatt_client_conn_param_update *cmd = buf;
5561         char address[18];
5562         bdaddr_t bdaddr;
5563
5564         android2bdaddr(cmd->address, &bdaddr);
5565         ba2str(&bdaddr, address);
5566
5567         DBG("%s", address);
5568
5569         /* TODO */
5570
5571         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5572                                         HAL_OP_GATT_CLIENT_CONN_PARAM_UPDATE,
5573                                         HAL_STATUS_UNSUPPORTED);
5574 }
5575
5576 static void handle_client_set_scan_param(const void *buf, uint16_t len)
5577 {
5578         const struct hal_cmd_gatt_client_set_scan_param *cmd = buf;
5579
5580         DBG("interval %d window %d", cmd->interval, cmd->window);
5581
5582         /* TODO */
5583
5584         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5585                                         HAL_OP_GATT_CLIENT_SET_SCAN_PARAM,
5586                                         HAL_STATUS_UNSUPPORTED);
5587 }
5588
5589 static void handle_client_setup_multi_adv(const void *buf, uint16_t len)
5590 {
5591         const struct hal_cmd_gatt_client_setup_multi_adv *cmd = buf;
5592
5593         DBG("client_if %d", cmd->client_if);
5594
5595         /* TODO */
5596
5597         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5598                                         HAL_OP_GATT_CLIENT_SETUP_MULTI_ADV,
5599                                         HAL_STATUS_UNSUPPORTED);
5600 }
5601
5602 static void handle_client_update_multi_adv(const void *buf, uint16_t len)
5603 {
5604         const struct hal_cmd_gatt_client_update_multi_adv *cmd = buf;
5605
5606         DBG("client_if %d", cmd->client_if);
5607
5608         /* TODO */
5609
5610         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5611                                         HAL_OP_GATT_CLIENT_UPDATE_MULTI_ADV,
5612                                         HAL_STATUS_UNSUPPORTED);
5613 }
5614
5615 static void handle_client_setup_multi_adv_inst(const void *buf, uint16_t len)
5616 {
5617         const struct hal_cmd_gatt_client_setup_multi_adv_inst *cmd = buf;
5618
5619         DBG("client_if %d", cmd->client_if);
5620
5621         /* TODO */
5622
5623         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5624                                         HAL_OP_GATT_CLIENT_SETUP_MULTI_ADV_INST,
5625                                         HAL_STATUS_UNSUPPORTED);
5626 }
5627
5628 static void handle_client_disable_multi_adv_inst(const void *buf, uint16_t len)
5629 {
5630         const struct hal_cmd_gatt_client_disable_multi_adv_inst *cmd = buf;
5631
5632         DBG("client_if %d", cmd->client_if);
5633
5634         /* TODO */
5635
5636         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5637                                 HAL_OP_GATT_CLIENT_DISABLE_MULTI_ADV_INST,
5638                                 HAL_STATUS_UNSUPPORTED);
5639 }
5640
5641 static void handle_client_configure_batchscan(const void *buf, uint16_t len)
5642 {
5643         const struct hal_cmd_gatt_client_configure_batchscan *cmd = buf;
5644
5645         DBG("client_if %d", cmd->client_if);
5646
5647         /* TODO */
5648
5649         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5650                                         HAL_OP_GATT_CLIENT_CONFIGURE_BATCHSCAN,
5651                                         HAL_STATUS_UNSUPPORTED);
5652 }
5653
5654 static void handle_client_enable_batchscan(const void *buf, uint16_t len)
5655 {
5656         const struct hal_cmd_gatt_client_enable_batchscan *cmd = buf;
5657
5658         DBG("client_if %d", cmd->client_if);
5659
5660         /* TODO */
5661
5662         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5663                                         HAL_OP_GATT_CLIENT_ENABLE_BATCHSCAN,
5664                                         HAL_STATUS_UNSUPPORTED);
5665 }
5666
5667 static void handle_client_disable_batchscan(const void *buf, uint16_t len)
5668 {
5669         const struct hal_cmd_gatt_client_disable_batchscan *cmd = buf;
5670
5671         DBG("client_if %d", cmd->client_if);
5672
5673         /* TODO */
5674
5675         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5676                                         HAL_OP_GATT_CLIENT_DISABLE_BATCHSCAN,
5677                                         HAL_STATUS_UNSUPPORTED);
5678 }
5679
5680 static void handle_client_read_batchscan_reports(const void *buf, uint16_t len)
5681 {
5682         const struct hal_cmd_gatt_client_read_batchscan_reports *cmd = buf;
5683
5684         DBG("client_if %d", cmd->client_if);
5685
5686         /* TODO */
5687
5688         ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
5689                                 HAL_OP_GATT_CLIENT_READ_BATCHSCAN_REPORTS,
5690                                 HAL_STATUS_UNSUPPORTED);
5691 }
5692
5693 static const struct ipc_handler cmd_handlers[] = {
5694         /* HAL_OP_GATT_CLIENT_REGISTER */
5695         { handle_client_register, false,
5696                 sizeof(struct hal_cmd_gatt_client_register) },
5697         /* HAL_OP_GATT_CLIENT_UNREGISTER */
5698         { handle_client_unregister, false,
5699                 sizeof(struct hal_cmd_gatt_client_unregister) },
5700         /* HAL_OP_GATT_CLIENT_SCAN */
5701         { handle_client_scan, false,
5702                 sizeof(struct hal_cmd_gatt_client_scan) },
5703         /* HAL_OP_GATT_CLIENT_CONNECT */
5704         { handle_client_connect, false,
5705                 sizeof(struct hal_cmd_gatt_client_connect) },
5706         /* HAL_OP_GATT_CLIENT_DISCONNECT */
5707         { handle_client_disconnect, false,
5708                 sizeof(struct hal_cmd_gatt_client_disconnect) },
5709         /* HAL_OP_GATT_CLIENT_LISTEN */
5710         { handle_client_listen, false,
5711                 sizeof(struct hal_cmd_gatt_client_listen) },
5712         /* HAL_OP_GATT_CLIENT_REFRESH */
5713         { handle_client_refresh, false,
5714                 sizeof(struct hal_cmd_gatt_client_refresh) },
5715         /* HAL_OP_GATT_CLIENT_SEARCH_SERVICE */
5716         { handle_client_search_service, true,
5717                 sizeof(struct hal_cmd_gatt_client_search_service) },
5718         /* HAL_OP_GATT_CLIENT_GET_INCLUDED_SERVICE */
5719         { handle_client_get_included_service, true,
5720                 sizeof(struct hal_cmd_gatt_client_get_included_service) },
5721         /* HAL_OP_GATT_CLIENT_GET_CHARACTERISTIC */
5722         { handle_client_get_characteristic, true,
5723                 sizeof(struct hal_cmd_gatt_client_get_characteristic) },
5724         /* HAL_OP_GATT_CLIENT_GET_DESCRIPTOR */
5725         { handle_client_get_descriptor, true,
5726                 sizeof(struct hal_cmd_gatt_client_get_descriptor) },
5727         /* HAL_OP_GATT_CLIENT_READ_CHARACTERISTIC */
5728         { handle_client_read_characteristic, false,
5729                 sizeof(struct hal_cmd_gatt_client_read_characteristic) },
5730         /* HAL_OP_GATT_CLIENT_WRITE_CHARACTERISTIC */
5731         { handle_client_write_characteristic, true,
5732                 sizeof(struct hal_cmd_gatt_client_write_characteristic) },
5733         /* HAL_OP_GATT_CLIENT_READ_DESCRIPTOR */
5734         { handle_client_read_descriptor, false,
5735                 sizeof(struct hal_cmd_gatt_client_read_descriptor) },
5736         /* HAL_OP_GATT_CLIENT_WRITE_DESCRIPTOR */
5737         { handle_client_write_descriptor, true,
5738                 sizeof(struct hal_cmd_gatt_client_write_descriptor) },
5739         /* HAL_OP_GATT_CLIENT_EXECUTE_WRITE */
5740         { handle_client_execute_write, false,
5741                 sizeof(struct hal_cmd_gatt_client_execute_write)},
5742         /* HAL_OP_GATT_CLIENT_REGISTER_FOR_NOTIFICATION */
5743         { handle_client_register_for_notification, false,
5744                 sizeof(struct hal_cmd_gatt_client_register_for_notification) },
5745         /* HAL_OP_GATT_CLIENT_DEREGISTER_FOR_NOTIFICATION */
5746         { handle_client_deregister_for_notification, false,
5747                 sizeof(struct hal_cmd_gatt_client_deregister_for_notification) },
5748         /* HAL_OP_GATT_CLIENT_READ_REMOTE_RSSI */
5749         { handle_client_read_remote_rssi, false,
5750                 sizeof(struct hal_cmd_gatt_client_read_remote_rssi) },
5751         /* HAL_OP_GATT_CLIENT_GET_DEVICE_TYPE */
5752         { handle_client_get_device_type, false,
5753                 sizeof(struct hal_cmd_gatt_client_get_device_type) },
5754         /* HAL_OP_GATT_CLIENT_SET_ADV_DATA */
5755         { handle_client_set_adv_data, true,
5756                 sizeof(struct hal_cmd_gatt_client_set_adv_data) },
5757         /* HAL_OP_GATT_CLIENT_TEST_COMMAND */
5758         { handle_client_test_command, false,
5759                 sizeof(struct hal_cmd_gatt_client_test_command) },
5760         /* HAL_OP_GATT_SERVER_REGISTER */
5761         { handle_server_register, false,
5762                 sizeof(struct hal_cmd_gatt_server_register) },
5763         /* HAL_OP_GATT_SERVER_UNREGISTER */
5764         { handle_server_unregister, false,
5765                 sizeof(struct hal_cmd_gatt_server_unregister) },
5766         /* HAL_OP_GATT_SERVER_CONNECT */
5767         { handle_server_connect, false,
5768                 sizeof(struct hal_cmd_gatt_server_connect) },
5769         /* HAL_OP_GATT_SERVER_DISCONNECT */
5770         { handle_server_disconnect, false,
5771                 sizeof(struct hal_cmd_gatt_server_disconnect) },
5772         /* HAL_OP_GATT_SERVER_ADD_SERVICE */
5773         { handle_server_add_service, false,
5774                 sizeof(struct hal_cmd_gatt_server_add_service) },
5775         /* HAL_OP_GATT_SERVER_ADD_INC_SERVICE */
5776         { handle_server_add_included_service, false,
5777                 sizeof(struct hal_cmd_gatt_server_add_inc_service) },
5778         /* HAL_OP_GATT_SERVER_ADD_CHARACTERISTIC */
5779         { handle_server_add_characteristic, false,
5780                 sizeof(struct hal_cmd_gatt_server_add_characteristic) },
5781         /* HAL_OP_GATT_SERVER_ADD_DESCRIPTOR */
5782         { handle_server_add_descriptor, false,
5783                 sizeof(struct hal_cmd_gatt_server_add_descriptor) },
5784         /* HAL_OP_GATT_SERVER_START_SERVICE */
5785         { handle_server_start_service, false,
5786                 sizeof(struct hal_cmd_gatt_server_start_service) },
5787         /* HAL_OP_GATT_SERVER_STOP_SERVICE */
5788         { handle_server_stop_service, false,
5789                 sizeof(struct hal_cmd_gatt_server_stop_service) },
5790         /* HAL_OP_GATT_SERVER_DELETE_SERVICE */
5791         { handle_server_delete_service, false,
5792                 sizeof(struct hal_cmd_gatt_server_delete_service) },
5793         /* HAL_OP_GATT_SERVER_SEND_INDICATION */
5794         { handle_server_send_indication, true,
5795                 sizeof(struct hal_cmd_gatt_server_send_indication) },
5796         /* HAL_OP_GATT_SERVER_SEND_RESPONSE */
5797         { handle_server_send_response, true,
5798                 sizeof(struct hal_cmd_gatt_server_send_response) },
5799         /* HAL_OP_GATT_CLIENT_SCAN_FILTER_SETUP */
5800         { handle_client_scan_filter_setup, false,
5801                 sizeof(struct hal_cmd_gatt_client_scan_filter_setup) },
5802         /* HAL_OP_GATT_CLIENT_SCAN_FILTER_ADD_REMOVE */
5803         { handle_client_scan_filter_add_remove, true,
5804                 sizeof(struct hal_cmd_gatt_client_scan_filter_add_remove) },
5805         /* HAL_OP_GATT_CLIENT_SCAN_FILTER_CLEAR */
5806         { handle_client_scan_filter_clear, false,
5807                 sizeof(struct hal_cmd_gatt_client_scan_filter_clear) },
5808         /* HAL_OP_GATT_CLIENT_SCAN_FILTER_ENABLE */
5809         { handle_client_scan_filter_enable, false,
5810                 sizeof(struct hal_cmd_gatt_client_scan_filter_enable) },
5811         /* HAL_OP_GATT_CLIENT_CONFIGURE_MTU */
5812         { handle_client_configure_mtu, false,
5813                 sizeof(struct hal_cmd_gatt_client_configure_mtu) },
5814         /* HAL_OP_GATT_CLIENT_CONN_PARAM_UPDATE */
5815         { handle_client_conn_param_update, false,
5816                 sizeof(struct hal_cmd_gatt_client_conn_param_update) },
5817         /* HAL_OP_GATT_CLIENT_SET_SCAN_PARAM */
5818         { handle_client_set_scan_param, false,
5819                 sizeof(struct hal_cmd_gatt_client_set_scan_param) },
5820         /* HAL_OP_GATT_CLIENT_SETUP_MULTI_ADV */
5821         { handle_client_setup_multi_adv, false,
5822                 sizeof(struct hal_cmd_gatt_client_setup_multi_adv) },
5823         /* HAL_OP_GATT_CLIENT_UPDATE_MULTI_ADV */
5824         { handle_client_update_multi_adv, false,
5825                 sizeof(struct hal_cmd_gatt_client_update_multi_adv) },
5826         /* HAL_OP_GATT_CLIENT_SETUP_MULTI_ADV_INST */
5827         { handle_client_setup_multi_adv_inst, false,
5828                 sizeof(struct hal_cmd_gatt_client_setup_multi_adv_inst) },
5829         /* HAL_OP_GATT_CLIENT_DISABLE_MULTI_ADV_INST */
5830         { handle_client_disable_multi_adv_inst, false,
5831                 sizeof(struct hal_cmd_gatt_client_disable_multi_adv_inst) },
5832         /* HAL_OP_GATT_CLIENT_CONFIGURE_BATCHSCAN */
5833         { handle_client_configure_batchscan, false,
5834                 sizeof(struct hal_cmd_gatt_client_configure_batchscan) },
5835         /* HAL_OP_GATT_CLIENT_ENABLE_BATCHSCAN */
5836         { handle_client_enable_batchscan, false,
5837                 sizeof(struct hal_cmd_gatt_client_enable_batchscan) },
5838         /* HAL_OP_GATT_CLIENT_DISABLE_BATCHSCAN */
5839         { handle_client_disable_batchscan, false,
5840                 sizeof(struct hal_cmd_gatt_client_disable_batchscan) },
5841         /* HAL_OP_GATT_CLIENT_READ_BATCHSCAN_REPORTS */
5842         { handle_client_read_batchscan_reports, false,
5843                 sizeof(struct hal_cmd_gatt_client_read_batchscan_reports) },
5844 };
5845
5846 static uint8_t read_by_type(const uint8_t *cmd, uint16_t cmd_len,
5847                                                 struct gatt_device *device)
5848 {
5849         uint16_t start, end;
5850         uint16_t len = 0;
5851         bt_uuid_t uuid;
5852         struct queue *q;
5853
5854         DBG("");
5855
5856         switch (cmd[0]) {
5857         case ATT_OP_READ_BY_TYPE_REQ:
5858                 len = dec_read_by_type_req(cmd, cmd_len, &start, &end, &uuid);
5859                 break;
5860         case ATT_OP_READ_BY_GROUP_REQ:
5861                 len = dec_read_by_grp_req(cmd, cmd_len, &start, &end, &uuid);
5862                 break;
5863         default:
5864                 break;
5865         }
5866
5867         if (!len)
5868                 return ATT_ECODE_INVALID_PDU;
5869
5870         if (start > end || start == 0)
5871                 return ATT_ECODE_INVALID_HANDLE;
5872
5873         q = queue_new();
5874
5875         switch (cmd[0]) {
5876         case ATT_OP_READ_BY_TYPE_REQ:
5877                 gatt_db_read_by_type(gatt_db, start, end, uuid, q);
5878                 break;
5879         case ATT_OP_READ_BY_GROUP_REQ:
5880                 gatt_db_read_by_group_type(gatt_db, start, end, uuid, q);
5881                 break;
5882         default:
5883                 break;
5884         }
5885
5886         if (queue_isempty(q)) {
5887                 queue_destroy(q, NULL);
5888                 return ATT_ECODE_ATTR_NOT_FOUND;
5889         }
5890
5891         while (queue_peek_head(q)) {
5892                 struct pending_request *data;
5893                 struct gatt_db_attribute *attrib = queue_pop_head(q);
5894
5895                 data = new0(struct pending_request, 1);
5896                 data->attrib = attrib;
5897                 queue_push_tail(device->pending_requests, data);
5898         }
5899
5900         queue_destroy(q, NULL);
5901         process_dev_pending_requests(device, cmd[0]);
5902
5903         return 0;
5904 }
5905
5906 static uint8_t read_request(const uint8_t *cmd, uint16_t cmd_len,
5907                                                         struct gatt_device *dev)
5908 {
5909         struct gatt_db_attribute *attrib;
5910         uint16_t handle;
5911         uint16_t len;
5912         uint16_t offset;
5913         struct pending_request *data;
5914
5915         DBG("");
5916
5917         switch (cmd[0]) {
5918         case ATT_OP_READ_BLOB_REQ:
5919                 len = dec_read_blob_req(cmd, cmd_len, &handle, &offset);
5920                 if (!len)
5921                         return ATT_ECODE_INVALID_PDU;
5922                 break;
5923         case ATT_OP_READ_REQ:
5924                 len = dec_read_req(cmd, cmd_len, &handle);
5925                 if (!len)
5926                         return ATT_ECODE_INVALID_PDU;
5927                 offset = 0;
5928                 break;
5929         default:
5930                 error("gatt: Unexpected read type 0x%02x", cmd[0]);
5931                 return ATT_ECODE_REQ_NOT_SUPP;
5932         }
5933
5934         attrib = gatt_db_get_attribute(gatt_db, handle);
5935         if (attrib == 0)
5936                 return ATT_ECODE_INVALID_HANDLE;
5937
5938         data = new0(struct pending_request, 1);
5939         data->offset = offset;
5940         data->attrib = attrib;
5941         queue_push_tail(dev->pending_requests, data);
5942
5943         process_dev_pending_requests(dev, cmd[0]);
5944
5945         return 0;
5946 }
5947
5948 static uint8_t mtu_att_handle(const uint8_t *cmd, uint16_t cmd_len,
5949                                                         struct gatt_device *dev)
5950 {
5951         uint16_t rmtu, mtu, len;
5952         size_t length;
5953         uint8_t *rsp;
5954
5955         DBG("");
5956
5957         len = dec_mtu_req(cmd, cmd_len, &rmtu);
5958         if (!len)
5959                 return ATT_ECODE_INVALID_PDU;
5960
5961         /* MTU exchange shall not be used on BR/EDR - Vol 3. Part G. 4.3.1 */
5962         if (get_cid(dev) != ATT_CID)
5963                 return ATT_ECODE_UNLIKELY;
5964
5965         if (!get_local_mtu(dev, &mtu))
5966                 return ATT_ECODE_UNLIKELY;
5967
5968         if (!update_mtu(dev, rmtu))
5969                 return ATT_ECODE_UNLIKELY;
5970
5971         rsp = g_attrib_get_buffer(dev->attrib, &length);
5972
5973         /* Respond with our MTU */
5974         len = enc_mtu_resp(mtu, rsp, length);
5975         if (!g_attrib_send(dev->attrib, 0, rsp, len, NULL, NULL, NULL))
5976                 return ATT_ECODE_UNLIKELY;
5977
5978         return 0;
5979 }
5980
5981 static uint8_t find_info_handle(const uint8_t *cmd, uint16_t cmd_len,
5982                                 uint8_t *rsp, size_t rsp_size, uint16_t *length)
5983 {
5984         struct gatt_db_attribute *attrib;
5985         struct queue *q, *temp;
5986         struct att_data_list *adl;
5987         int iterator = 0;
5988         uint16_t start, end;
5989         uint16_t len, queue_len;
5990         uint8_t format;
5991         uint8_t ret = 0;
5992
5993         DBG("");
5994
5995         len = dec_find_info_req(cmd, cmd_len, &start, &end);
5996         if (!len)
5997                 return ATT_ECODE_INVALID_PDU;
5998
5999         if (start > end || start == 0)
6000                 return ATT_ECODE_INVALID_HANDLE;
6001
6002         q = queue_new();
6003
6004         gatt_db_find_information(gatt_db, start, end, q);
6005
6006         if (queue_isempty(q)) {
6007                 queue_destroy(q, NULL);
6008                 return ATT_ECODE_ATTR_NOT_FOUND;
6009         }
6010
6011         temp = queue_new();
6012
6013         attrib = queue_peek_head(q);
6014         /* UUIDS can be only 128 bit and 16 bit */
6015         len = bt_uuid_len(gatt_db_attribute_get_type(attrib));
6016         if (len != 2 && len != 16) {
6017                 queue_destroy(q, NULL);
6018                 queue_destroy(temp, NULL);
6019                 return ATT_ECODE_UNLIKELY;
6020         }
6021
6022         while (attrib) {
6023                 const bt_uuid_t *type;
6024
6025                 type = gatt_db_attribute_get_type(attrib);
6026                 if (bt_uuid_len(type) != len)
6027                         break;
6028
6029                 queue_push_tail(temp, queue_pop_head(q));
6030                 attrib = queue_peek_head(q);
6031         }
6032
6033         queue_destroy(q, NULL);
6034
6035         queue_len = queue_length(temp);
6036         adl = att_data_list_alloc(queue_len, len + sizeof(uint16_t));
6037         if (!adl) {
6038                 queue_destroy(temp, NULL);
6039                 return ATT_ECODE_INSUFF_RESOURCES;
6040         }
6041
6042         while (queue_peek_head(temp)) {
6043                 uint8_t *value;
6044                 const bt_uuid_t *type;
6045                 struct gatt_db_attribute *attrib = queue_pop_head(temp);
6046                 uint16_t handle;
6047
6048                 type = gatt_db_attribute_get_type(attrib);
6049                 if (!type)
6050                         break;
6051
6052                 value = adl->data[iterator++];
6053
6054                 handle = gatt_db_attribute_get_handle(attrib);
6055                 put_le16(handle, value);
6056                 memcpy(&value[2], &type->value, len);
6057         }
6058
6059         if (len == 2)
6060                 format = ATT_FIND_INFO_RESP_FMT_16BIT;
6061         else
6062                 format = ATT_FIND_INFO_RESP_FMT_128BIT;
6063
6064         len = enc_find_info_resp(format, adl, rsp, rsp_size);
6065         if (!len)
6066                 ret = ATT_ECODE_UNLIKELY;
6067
6068         *length = len;
6069         att_data_list_free(adl);
6070         queue_destroy(temp, NULL);
6071
6072         return ret;
6073 }
6074
6075 struct find_by_type_request_data {
6076         struct gatt_device *device;
6077         uint8_t *search_value;
6078         size_t search_vlen;
6079         uint8_t error;
6080 };
6081
6082 static void find_by_type_request_cb(struct gatt_db_attribute *attrib,
6083                                                                 void *user_data)
6084 {
6085         struct find_by_type_request_data *find_data = user_data;
6086         struct pending_request *request_data;
6087
6088         if (find_data->error)
6089                 return;
6090
6091         request_data = new0(struct pending_request, 1);
6092         request_data->filter_value = malloc0(find_data->search_vlen);
6093         if (!request_data->filter_value) {
6094                 destroy_pending_request(request_data);
6095                 find_data->error = ATT_ECODE_INSUFF_RESOURCES;
6096                 return;
6097         }
6098
6099         request_data->attrib = attrib;
6100         request_data->filter_vlen = find_data->search_vlen;
6101         memcpy(request_data->filter_value, find_data->search_value,
6102                                                         find_data->search_vlen);
6103
6104         queue_push_tail(find_data->device->pending_requests, request_data);
6105 }
6106
6107 static uint8_t find_by_type_request(const uint8_t *cmd, uint16_t cmd_len,
6108                                                 struct gatt_device *device)
6109 {
6110         uint8_t search_value[cmd_len];
6111         size_t search_vlen;
6112         uint16_t start, end;
6113         bt_uuid_t uuid;
6114         uint16_t len;
6115         struct find_by_type_request_data data;
6116
6117         DBG("");
6118
6119         len = dec_find_by_type_req(cmd, cmd_len, &start, &end, &uuid,
6120                                                 search_value, &search_vlen);
6121         if (!len)
6122                 return ATT_ECODE_INVALID_PDU;
6123
6124         if (start > end || start == 0)
6125                 return ATT_ECODE_INVALID_HANDLE;
6126
6127         data.error = 0;
6128         data.search_vlen = search_vlen;
6129         data.search_value = search_value;
6130         data.device = device;
6131
6132         if (gatt_db_find_by_type(gatt_db, start, end, &uuid,
6133                                         find_by_type_request_cb, &data) == 0) {
6134                 size_t mtu;
6135                 uint8_t *rsp = g_attrib_get_buffer(device->attrib, &mtu);
6136
6137                 len = enc_error_resp(ATT_OP_FIND_BY_TYPE_REQ, start,
6138                                         ATT_ECODE_ATTR_NOT_FOUND, rsp, mtu);
6139                 g_attrib_send(device->attrib, 0, rsp, len, NULL, NULL, NULL);
6140                 return 0;
6141         }
6142
6143         if (!data.error)
6144                 process_dev_pending_requests(device, ATT_OP_FIND_BY_TYPE_REQ);
6145
6146         return data.error;
6147 }
6148
6149 static void write_confirm(struct gatt_db_attribute *attrib,
6150                                                 int err, void *user_data)
6151 {
6152         if (!err)
6153                 return;
6154
6155         error("Error writting attribute %p", attrib);
6156 }
6157
6158 static void write_cmd_request(const uint8_t *cmd, uint16_t cmd_len,
6159                                                 struct gatt_device *dev)
6160 {
6161         uint8_t value[cmd_len];
6162         struct gatt_db_attribute *attrib;
6163         uint32_t permissions;
6164         uint16_t handle;
6165         uint16_t len;
6166         size_t vlen;
6167
6168         len = dec_write_cmd(cmd, cmd_len, &handle, value, &vlen);
6169         if (!len)
6170                 return;
6171
6172         if (handle == 0)
6173                 return;
6174
6175         attrib = gatt_db_get_attribute(gatt_db, handle);
6176         if (!attrib)
6177                 return;
6178
6179         permissions = gatt_db_attribute_get_permissions(attrib);
6180
6181         if (check_device_permissions(dev, cmd[0], permissions))
6182                 return;
6183
6184         gatt_db_attribute_write(attrib, 0, value, vlen, cmd[0],
6185                                                 g_attrib_get_att(dev->attrib),
6186                                                 write_confirm, NULL);
6187 }
6188
6189 static void write_signed_cmd_request(const uint8_t *cmd, uint16_t cmd_len,
6190                                                 struct gatt_device *dev)
6191 {
6192         uint8_t value[cmd_len];
6193         uint8_t s[ATT_SIGNATURE_LEN];
6194         struct gatt_db_attribute *attrib;
6195         uint32_t permissions;
6196         uint16_t handle;
6197         uint16_t len;
6198         size_t vlen;
6199         uint8_t csrk[16];
6200         uint32_t sign_cnt;
6201
6202         if (get_cid(dev) != ATT_CID) {
6203                 error("gatt: Remote tries write signed on BR/EDR bearer");
6204                 connection_cleanup(dev);
6205                 return;
6206         }
6207
6208         if (get_sec_level(dev) != BT_SECURITY_LOW) {
6209                 error("gatt: Remote tries write signed on encrypted link");
6210                 connection_cleanup(dev);
6211                 return;
6212         }
6213
6214         if (!bt_get_csrk(&dev->bdaddr, false, csrk, &sign_cnt, NULL)) {
6215                 error("gatt: No valid csrk from remote device");
6216                 return;
6217         }
6218
6219         len = dec_signed_write_cmd(cmd, cmd_len, &handle, value, &vlen, s);
6220
6221         if (handle == 0)
6222                 return;
6223
6224         attrib = gatt_db_get_attribute(gatt_db, handle);
6225         if (!attrib)
6226                 return;
6227
6228         permissions = gatt_db_attribute_get_permissions(attrib);
6229
6230         if (check_device_permissions(dev, cmd[0], permissions))
6231                 return;
6232
6233         if (len) {
6234                 uint8_t t[ATT_SIGNATURE_LEN];
6235                 uint32_t r_sign_cnt = get_le32(s);
6236
6237                 if (r_sign_cnt < sign_cnt) {
6238                         error("gatt: Invalid sign counter (%d<%d)",
6239                                                         r_sign_cnt, sign_cnt);
6240                         return;
6241                 }
6242
6243                 /* Generate signature and verify it */
6244                 if (!bt_crypto_sign_att(crypto, csrk, cmd,
6245                                                 cmd_len - ATT_SIGNATURE_LEN,
6246                                                 r_sign_cnt, t)) {
6247                         error("gatt: Error when generating att signature");
6248                         return;
6249                 }
6250
6251                 if (memcmp(t, s, ATT_SIGNATURE_LEN)) {
6252                         error("gatt: signature does not match");
6253                         return;
6254                 }
6255                 /* Signature OK, proceed with write */
6256                 bt_update_sign_counter(&dev->bdaddr, false, r_sign_cnt);
6257                 gatt_db_attribute_write(attrib, 0, value, vlen, cmd[0],
6258                                                 g_attrib_get_att(dev->attrib),
6259                                                 write_confirm, NULL);
6260         }
6261 }
6262
6263 static void attribute_write_cb(struct gatt_db_attribute *attrib, int err,
6264                                                                 void *user_data)
6265 {
6266         struct pending_request *data = user_data;
6267         uint8_t error = err_to_att(err);
6268
6269         DBG("");
6270
6271         data->attrib = attrib;
6272         data->error = error;
6273         data->completed = true;
6274 }
6275
6276 static uint8_t write_req_request(const uint8_t *cmd, uint16_t cmd_len,
6277                                                 struct gatt_device *dev)
6278 {
6279         uint8_t value[cmd_len];
6280         struct pending_request *data;
6281         struct gatt_db_attribute *attrib;
6282         uint32_t permissions;
6283         uint16_t handle;
6284         uint16_t len;
6285         uint8_t error;
6286         size_t vlen;
6287
6288         len = dec_write_req(cmd, cmd_len, &handle, value, &vlen);
6289         if (!len)
6290                 return ATT_ECODE_INVALID_PDU;
6291
6292         if (handle == 0)
6293                 return ATT_ECODE_INVALID_HANDLE;
6294
6295         attrib = gatt_db_get_attribute(gatt_db, handle);
6296         if (!attrib)
6297                 return ATT_ECODE_ATTR_NOT_FOUND;
6298
6299         permissions = gatt_db_attribute_get_permissions(attrib);
6300
6301         error = check_device_permissions(dev, cmd[0], permissions);
6302         if (error)
6303                 return error;
6304
6305         data = new0(struct pending_request, 1);
6306         data->attrib = attrib;
6307
6308         queue_push_tail(dev->pending_requests, data);
6309
6310         if (!gatt_db_attribute_write(attrib, 0, value, vlen, cmd[0],
6311                                                 g_attrib_get_att(dev->attrib),
6312                                                 attribute_write_cb, data)) {
6313                 queue_remove(dev->pending_requests, data);
6314                 free(data);
6315                 return ATT_ECODE_UNLIKELY;
6316         }
6317
6318         send_dev_complete_response(dev, cmd[0]);
6319
6320         return 0;
6321 }
6322
6323 static uint8_t write_prep_request(const uint8_t *cmd, uint16_t cmd_len,
6324                                                 struct gatt_device *dev)
6325 {
6326         uint8_t value[cmd_len];
6327         struct pending_request *data;
6328         struct gatt_db_attribute *attrib;
6329         uint32_t permissions;
6330         uint16_t handle;
6331         uint16_t offset;
6332         uint8_t error;
6333         uint16_t len;
6334         size_t vlen;
6335
6336         len = dec_prep_write_req(cmd, cmd_len, &handle, &offset,
6337                                                 value, &vlen);
6338         if (!len)
6339                 return ATT_ECODE_INVALID_PDU;
6340
6341         if (handle == 0)
6342                 return ATT_ECODE_INVALID_HANDLE;
6343
6344         attrib = gatt_db_get_attribute(gatt_db, handle);
6345         if (!attrib)
6346                 return ATT_ECODE_ATTR_NOT_FOUND;
6347
6348         permissions = gatt_db_attribute_get_permissions(attrib);
6349
6350         error = check_device_permissions(dev, cmd[0], permissions);
6351         if (error)
6352                 return error;
6353
6354         data = new0(struct pending_request, 1);
6355         data->attrib = attrib;
6356         data->offset = offset;
6357
6358         queue_push_tail(dev->pending_requests, data);
6359
6360         data->value = g_memdup(value, vlen);
6361         data->length = vlen;
6362
6363         if (!gatt_db_attribute_write(attrib, offset, value, vlen, cmd[0],
6364                                                 g_attrib_get_att(dev->attrib),
6365                                                 attribute_write_cb, data)) {
6366                 queue_remove(dev->pending_requests, data);
6367                 g_free(data->value);
6368                 free(data);
6369
6370                 return ATT_ECODE_UNLIKELY;
6371         }
6372
6373         send_dev_complete_response(dev, cmd[0]);
6374
6375         return 0;
6376 }
6377
6378 static void send_server_write_execute_notify(void *data, void *user_data)
6379 {
6380         struct hal_ev_gatt_server_request_exec_write *ev = user_data;
6381         struct pending_trans_data *transaction;
6382         struct app_connection *conn = data;
6383
6384         if (!conn->wait_execute_write)
6385                 return;
6386
6387         ev->conn_id = conn->id;
6388
6389         transaction = conn_add_transact(conn, ATT_OP_EXEC_WRITE_REQ, NULL, 0);
6390
6391         ev->trans_id = transaction->id;
6392
6393         ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
6394                         HAL_EV_GATT_SERVER_REQUEST_EXEC_WRITE, sizeof(*ev), ev);
6395 }
6396
6397 static uint8_t write_execute_request(const uint8_t *cmd, uint16_t cmd_len,
6398                                                 struct gatt_device *dev)
6399 {
6400         struct hal_ev_gatt_server_request_exec_write ev;
6401         uint8_t value;
6402         struct pending_request *data;
6403
6404         /*
6405          * Check if there was any write prep before.
6406          * TODO: Try to find better error code if possible
6407          */
6408         if (!pending_execute_write())
6409                 return ATT_ECODE_UNLIKELY;
6410
6411         if (!dec_exec_write_req(cmd, cmd_len, &value))
6412                 return ATT_ECODE_INVALID_PDU;
6413
6414         memset(&ev, 0, sizeof(ev));
6415         bdaddr2android(&dev->bdaddr, &ev.bdaddr);
6416         ev.exec_write = value;
6417
6418         data = new0(struct pending_request, 1);
6419
6420         queue_push_tail(dev->pending_requests, data);
6421
6422         queue_foreach(app_connections, send_server_write_execute_notify, &ev);
6423         send_dev_complete_response(dev, cmd[0]);
6424
6425         return 0;
6426 }
6427
6428 static void att_handler(const uint8_t *ipdu, uint16_t len, gpointer user_data)
6429 {
6430         struct gatt_device *dev = user_data;
6431         uint8_t status;
6432         uint16_t resp_length = 0;
6433         size_t length;
6434         uint8_t *opdu = g_attrib_get_buffer(dev->attrib, &length);
6435
6436         DBG("op 0x%02x", ipdu[0]);
6437
6438         if (len > length) {
6439                 error("gatt: Too much data on ATT socket %p", opdu);
6440                 status = ATT_ECODE_INVALID_PDU;
6441                 goto done;
6442         }
6443
6444         switch (ipdu[0]) {
6445         case ATT_OP_READ_BY_GROUP_REQ:
6446         case ATT_OP_READ_BY_TYPE_REQ:
6447                 status = read_by_type(ipdu, len, dev);
6448                 break;
6449         case ATT_OP_READ_REQ:
6450         case ATT_OP_READ_BLOB_REQ:
6451                 status = read_request(ipdu, len, dev);
6452                 break;
6453         case ATT_OP_MTU_REQ:
6454                 status = mtu_att_handle(ipdu, len, dev);
6455                 break;
6456         case ATT_OP_FIND_INFO_REQ:
6457                 status = find_info_handle(ipdu, len, opdu, length,
6458                                                                 &resp_length);
6459                 break;
6460         case ATT_OP_WRITE_REQ:
6461                 status = write_req_request(ipdu, len, dev);
6462                 break;
6463         case ATT_OP_WRITE_CMD:
6464                 write_cmd_request(ipdu, len, dev);
6465                 /* No response on write cmd */
6466                 return;
6467         case ATT_OP_SIGNED_WRITE_CMD:
6468                 write_signed_cmd_request(ipdu, len, dev);
6469                 /* No response on write signed cmd */
6470                 return;
6471         case ATT_OP_PREP_WRITE_REQ:
6472                 status = write_prep_request(ipdu, len, dev);
6473                 break;
6474         case ATT_OP_FIND_BY_TYPE_REQ:
6475                 status = find_by_type_request(ipdu, len, dev);
6476                 break;
6477         case ATT_OP_EXEC_WRITE_REQ:
6478                 status = write_execute_request(ipdu, len, dev);
6479                 break;
6480         case ATT_OP_READ_MULTI_REQ:
6481         default:
6482                 DBG("Unsupported request 0x%02x", ipdu[0]);
6483                 status = ATT_ECODE_REQ_NOT_SUPP;
6484                 break;
6485         }
6486
6487 done:
6488         if (status)
6489                 resp_length = enc_error_resp(ipdu[0], 0x0000, status, opdu,
6490                                                                         length);
6491
6492         g_attrib_send(dev->attrib, 0, opdu, resp_length, NULL, NULL, NULL);
6493 }
6494
6495 static void connect_confirm(GIOChannel *io, void *user_data)
6496 {
6497         struct gatt_device *dev;
6498         bdaddr_t dst;
6499         GError *gerr = NULL;
6500
6501         DBG("");
6502
6503         bt_io_get(io, &gerr, BT_IO_OPT_DEST_BDADDR, &dst, BT_IO_OPT_INVALID);
6504         if (gerr) {
6505                 error("gatt: bt_io_get: %s", gerr->message);
6506                 g_error_free(gerr);
6507                 return;
6508         }
6509
6510         /* TODO Handle collision */
6511         dev = find_device_by_addr(&dst);
6512         if (!dev) {
6513                 dev = create_device(&dst);
6514         } else {
6515                 if ((dev->state != DEVICE_DISCONNECTED) &&
6516                                         !(dev->state == DEVICE_CONNECT_INIT &&
6517                                         bt_kernel_conn_control())) {
6518                         char addr[18];
6519
6520                         ba2str(&dst, addr);
6521                         info("gatt: Rejecting incoming connection from %s",
6522                                                                         addr);
6523                         goto drop;
6524                 }
6525         }
6526
6527         if (!bt_io_accept(io, connect_cb, device_ref(dev), NULL, NULL)) {
6528                 error("gatt: failed to accept connection");
6529                 device_unref(dev);
6530                 goto drop;
6531         }
6532
6533         queue_foreach(listen_apps, create_app_connection, dev);
6534         device_set_state(dev, DEVICE_CONNECT_READY);
6535
6536         return;
6537
6538 drop:
6539         g_io_channel_shutdown(io, TRUE, NULL);
6540 }
6541
6542 struct gap_srvc_handles {
6543         struct gatt_db_attribute *srvc;
6544
6545         /* Characteristics */
6546         struct gatt_db_attribute *dev_name;
6547         struct gatt_db_attribute *appear;
6548         struct gatt_db_attribute *priv;
6549 };
6550
6551 static struct gap_srvc_handles gap_srvc_data;
6552
6553 #define APPEARANCE_GENERIC_PHONE 0x0040
6554 #define PERIPHERAL_PRIVACY_DISABLE 0x00
6555
6556 static void device_name_read_cb(struct gatt_db_attribute *attrib,
6557                                         unsigned int id, uint16_t offset,
6558                                         uint8_t opcode, struct bt_att *att,
6559                                         void *user_data)
6560 {
6561         const char *name = bt_get_adapter_name();
6562
6563         gatt_db_attribute_read_result(attrib, id, 0, (void *) name,
6564                                                                 strlen(name));
6565 }
6566
6567 static void register_gap_service(void)
6568 {
6569         uint16_t start, end;
6570         bt_uuid_t uuid;
6571
6572         /* GAP UUID */
6573         bt_uuid16_create(&uuid, 0x1800);
6574         gap_srvc_data.srvc = gatt_db_add_service(gatt_db, &uuid, true, 7);
6575
6576         /* Device name characteristic */
6577         bt_uuid16_create(&uuid, GATT_CHARAC_DEVICE_NAME);
6578         gap_srvc_data.dev_name =
6579                         gatt_db_service_add_characteristic(gap_srvc_data.srvc,
6580                                                         &uuid, GATT_PERM_READ,
6581                                                         GATT_CHR_PROP_READ,
6582                                                         device_name_read_cb,
6583                                                         NULL, NULL);
6584
6585         /* Appearance */
6586         bt_uuid16_create(&uuid, GATT_CHARAC_APPEARANCE);
6587
6588         gap_srvc_data.appear =
6589                         gatt_db_service_add_characteristic(gap_srvc_data.srvc,
6590                                                         &uuid, GATT_PERM_READ,
6591                                                         GATT_CHR_PROP_READ,
6592                                                         NULL, NULL, NULL);
6593         if (gap_srvc_data.appear) {
6594                 uint16_t value;
6595                 /* Store appearance into db */
6596                 value = cpu_to_le16(APPEARANCE_GENERIC_PHONE);
6597                 gatt_db_attribute_write(gap_srvc_data.appear, 0,
6598                                                 (void *) &value, sizeof(value),
6599                                                 ATT_OP_WRITE_REQ, NULL,
6600                                                 write_confirm, NULL);
6601         }
6602
6603         /* Pripheral privacy flag */
6604         bt_uuid16_create(&uuid, GATT_CHARAC_PERIPHERAL_PRIV_FLAG);
6605         gap_srvc_data.priv =
6606                         gatt_db_service_add_characteristic(gap_srvc_data.srvc,
6607                                                         &uuid, GATT_PERM_READ,
6608                                                         GATT_CHR_PROP_READ,
6609                                                         NULL, NULL, NULL);
6610         if (gap_srvc_data.priv) {
6611                 uint8_t value;
6612
6613                 /* Store privacy into db */
6614                 value = PERIPHERAL_PRIVACY_DISABLE;
6615                 gatt_db_attribute_write(gap_srvc_data.priv, 0,
6616                                                 &value, sizeof(value),
6617                                                 ATT_OP_WRITE_REQ, NULL,
6618                                                 write_confirm, NULL);
6619         }
6620
6621         gatt_db_service_set_active(gap_srvc_data.srvc , true);
6622
6623         /* SDP */
6624         bt_uuid16_create(&uuid, 0x1800);
6625         gatt_db_attribute_get_service_handles(gap_srvc_data.srvc, &start, &end);
6626         gap_sdp_handle = add_sdp_record(&uuid, start, end,
6627                                                 "Generic Access Profile");
6628         if (!gap_sdp_handle)
6629                 error("gatt: Failed to register GAP SDP record");
6630 }
6631
6632 static void device_info_read_cb(struct gatt_db_attribute *attrib,
6633                                         unsigned int id, uint16_t offset,
6634                                         uint8_t opcode, struct bt_att *att,
6635                                         void *user_data)
6636 {
6637         char *buf = user_data;
6638
6639         gatt_db_attribute_read_result(attrib, id, 0, user_data, strlen(buf));
6640 }
6641
6642 static void device_info_read_system_id_cb(struct gatt_db_attribute *attrib,
6643                                         unsigned int id, uint16_t offset,
6644                                         uint8_t opcode, struct bt_att *att,
6645                                         void *user_data)
6646 {
6647         uint8_t pdu[8];
6648
6649         put_le64(bt_config_get_system_id(), pdu);
6650
6651         gatt_db_attribute_read_result(attrib, id, 0, pdu, sizeof(pdu));
6652 }
6653
6654 static void device_info_read_pnp_id_cb(struct gatt_db_attribute *attrib,
6655                                         unsigned int id, uint16_t offset,
6656                                         uint8_t opcode, struct bt_att *att,
6657                                         void *user_data)
6658 {
6659         uint8_t pdu[7];
6660
6661         pdu[0] = bt_config_get_pnp_source();
6662         put_le16(bt_config_get_pnp_vendor(), &pdu[1]);
6663         put_le16(bt_config_get_pnp_product(), &pdu[3]);
6664         put_le16(bt_config_get_pnp_version(), &pdu[5]);
6665
6666         gatt_db_attribute_read_result(attrib, id, 0, pdu, sizeof(pdu));
6667 }
6668
6669 static void register_device_info_service(void)
6670 {
6671         bt_uuid_t uuid;
6672         struct gatt_db_attribute *service;
6673         uint16_t start_handle, end_handle;
6674         const char *data;
6675         uint32_t enc_perm = GATT_PERM_READ | GATT_PERM_READ_ENCRYPTED;
6676
6677         DBG("");
6678
6679         /* Device Information Service */
6680         bt_uuid16_create(&uuid, 0x180a);
6681         service = gatt_db_add_service(gatt_db, &uuid, true, 17);
6682
6683         /* User data are not const hence (void *) cast is used */
6684         data = bt_config_get_name();
6685         if (data) {
6686                 bt_uuid16_create(&uuid, GATT_CHARAC_MODEL_NUMBER_STRING);
6687                 gatt_db_service_add_characteristic(service, &uuid,
6688                                                 GATT_PERM_READ,
6689                                                 GATT_CHR_PROP_READ,
6690                                                 device_info_read_cb, NULL,
6691                                                 (void *) data);
6692         }
6693
6694         data = bt_config_get_serial();
6695         if (data) {
6696                 bt_uuid16_create(&uuid, GATT_CHARAC_SERIAL_NUMBER_STRING);
6697                 gatt_db_service_add_characteristic(service, &uuid,
6698                                                 enc_perm, GATT_CHR_PROP_READ,
6699                                                 device_info_read_cb, NULL,
6700                                                 (void *) data);
6701         }
6702
6703         if (bt_config_get_system_id()) {
6704                 bt_uuid16_create(&uuid, GATT_CHARAC_SYSTEM_ID);
6705                 gatt_db_service_add_characteristic(service, &uuid,
6706                                                 enc_perm, GATT_CHR_PROP_READ,
6707                                                 device_info_read_system_id_cb,
6708                                                 NULL, NULL);
6709         }
6710
6711         data = bt_config_get_fw_rev();
6712         if (data) {
6713                 bt_uuid16_create(&uuid, GATT_CHARAC_FIRMWARE_REVISION_STRING);
6714                 gatt_db_service_add_characteristic(service, &uuid,
6715                                                 GATT_PERM_READ,
6716                                                 GATT_CHR_PROP_READ,
6717                                                 device_info_read_cb, NULL,
6718                                                 (void *) data);
6719         }
6720
6721         data = bt_config_get_hw_rev();
6722         if (data) {
6723                 bt_uuid16_create(&uuid, GATT_CHARAC_HARDWARE_REVISION_STRING);
6724                 gatt_db_service_add_characteristic(service, &uuid,
6725                                                 GATT_PERM_READ,
6726                                                 GATT_CHR_PROP_READ,
6727                                                 device_info_read_cb, NULL,
6728                                                 (void *) data);
6729         }
6730
6731         bt_uuid16_create(&uuid, GATT_CHARAC_SOFTWARE_REVISION_STRING);
6732         gatt_db_service_add_characteristic(service, &uuid, GATT_PERM_READ,
6733                                         GATT_CHR_PROP_READ, device_info_read_cb,
6734                                         NULL, VERSION);
6735
6736         data = bt_config_get_vendor();
6737         if (data) {
6738                 bt_uuid16_create(&uuid, GATT_CHARAC_MANUFACTURER_NAME_STRING);
6739                 gatt_db_service_add_characteristic(service, &uuid,
6740                                                 GATT_PERM_READ,
6741                                                 GATT_CHR_PROP_READ,
6742                                                 device_info_read_cb, NULL,
6743                                                 (void *) data);
6744         }
6745
6746         if (bt_config_get_pnp_source()) {
6747                 bt_uuid16_create(&uuid, GATT_CHARAC_PNP_ID);
6748                 gatt_db_service_add_characteristic(service, &uuid,
6749                                                 GATT_PERM_READ,
6750                                                 GATT_CHR_PROP_READ,
6751                                                 device_info_read_pnp_id_cb,
6752                                                 NULL, NULL);
6753         }
6754
6755         gatt_db_service_set_active(service, true);
6756
6757         /* SDP */
6758         bt_uuid16_create(&uuid, 0x180a);
6759         gatt_db_attribute_get_service_handles(service, &start_handle,
6760                                                                 &end_handle);
6761         dis_sdp_handle = add_sdp_record(&uuid, start_handle, end_handle,
6762                                                 "Device Information Service");
6763         if (!dis_sdp_handle)
6764                 error("gatt: Failed to register DIS SDP record");
6765 }
6766
6767 static void gatt_srvc_change_write_cb(struct gatt_db_attribute *attrib,
6768                                         unsigned int id, uint16_t offset,
6769                                         const uint8_t *value, size_t len,
6770                                         uint8_t opcode, struct bt_att *att,
6771                                         void *user_data)
6772 {
6773         struct gatt_device *dev;
6774         bdaddr_t bdaddr;
6775
6776         if (!get_dst_addr(att, &bdaddr)) {
6777                 error("gatt: srvc_change_write_cb, could not obtain BDADDR");
6778                 return;
6779         }
6780
6781         dev = find_device_by_addr(&bdaddr);
6782         if (!dev) {
6783                 error("gatt: Could not find device ?!");
6784                 return;
6785         }
6786
6787         if (!bt_device_is_bonded(&bdaddr)) {
6788                 gatt_db_attribute_write_result(attrib, id,
6789                                                 ATT_ECODE_AUTHORIZATION);
6790                 return;
6791         }
6792
6793         /* 2 octets are expected as CCC value */
6794         if (len != 2) {
6795                 gatt_db_attribute_write_result(attrib, id,
6796                                                 ATT_ECODE_INVAL_ATTR_VALUE_LEN);
6797                 return;
6798         }
6799
6800         /* Set services changed indication value */
6801         bt_store_gatt_ccc(&bdaddr, get_le16(value));
6802
6803         gatt_db_attribute_write_result(attrib, id, 0);
6804 }
6805
6806 static void gatt_srvc_change_read_cb(struct gatt_db_attribute *attrib,
6807                                         unsigned int id, uint16_t offset,
6808                                         uint8_t opcode, struct bt_att *att,
6809                                         void *user_data)
6810 {
6811         struct gatt_device *dev;
6812         uint8_t pdu[2];
6813         bdaddr_t bdaddr;
6814
6815         if (!get_dst_addr(att, &bdaddr)) {
6816                 error("gatt: srvc_change_read_cb, could not obtain BDADDR");
6817                 return;
6818         }
6819
6820         dev = find_device_by_addr(&bdaddr);
6821         if (!dev) {
6822                 error("gatt: Could not find device ?!");
6823                 return;
6824         }
6825
6826         put_le16(bt_get_gatt_ccc(&dev->bdaddr), pdu);
6827
6828         gatt_db_attribute_read_result(attrib, id, 0, pdu, sizeof(pdu));
6829 }
6830
6831 static void register_gatt_service(void)
6832 {
6833         struct gatt_db_attribute *service;
6834         uint16_t start_handle, end_handle;
6835         bt_uuid_t uuid;
6836
6837         DBG("");
6838
6839         bt_uuid16_create(&uuid, 0x1801);
6840         service = gatt_db_add_service(gatt_db, &uuid, true, 4);
6841
6842         bt_uuid16_create(&uuid, GATT_CHARAC_SERVICE_CHANGED);
6843         service_changed_attrib = gatt_db_service_add_characteristic(service,
6844                                                         &uuid, GATT_PERM_NONE,
6845                                                         GATT_CHR_PROP_INDICATE,
6846                                                         NULL, NULL, NULL);
6847
6848         bt_uuid16_create(&uuid, GATT_CLIENT_CHARAC_CFG_UUID);
6849         gatt_db_service_add_descriptor(service, &uuid,
6850                                         GATT_PERM_READ | GATT_PERM_WRITE,
6851                                         gatt_srvc_change_read_cb,
6852                                         gatt_srvc_change_write_cb, NULL);
6853
6854         gatt_db_service_set_active(service, true);
6855
6856         /* SDP */
6857         bt_uuid16_create(&uuid, 0x1801);
6858         gatt_db_attribute_get_service_handles(service, &start_handle,
6859                                                                 &end_handle);
6860         gatt_sdp_handle = add_sdp_record(&uuid, start_handle, end_handle,
6861                                                 "Generic Attribute Profile");
6862
6863         if (!gatt_sdp_handle)
6864                 error("gatt: Failed to register GATT SDP record");
6865 }
6866
6867 static bool start_listening(void)
6868 {
6869         /* BR/EDR socket */
6870         bredr_io = bt_io_listen(NULL, connect_confirm, NULL, NULL, NULL,
6871                                         BT_IO_OPT_SOURCE_TYPE, BDADDR_BREDR,
6872                                         BT_IO_OPT_PSM, ATT_PSM,
6873                                         BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
6874                                         BT_IO_OPT_INVALID);
6875
6876         /* LE socket */
6877         le_io = bt_io_listen(NULL, connect_confirm, NULL, NULL, NULL,
6878                                         BT_IO_OPT_SOURCE_TYPE, BDADDR_LE_PUBLIC,
6879                                         BT_IO_OPT_CID, ATT_CID,
6880                                         BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_LOW,
6881                                         BT_IO_OPT_INVALID);
6882
6883         if (!le_io && !bredr_io) {
6884                 error("gatt: Failed to start listening IO");
6885                 return false;
6886         }
6887
6888         return true;
6889 }
6890
6891 static void gatt_paired_cb(const bdaddr_t *addr)
6892 {
6893         struct gatt_device *dev;
6894         char address[18];
6895         struct app_connection *conn;
6896
6897         dev = find_device_by_addr(addr);
6898         if (!dev)
6899                 return;
6900
6901         ba2str(addr, address);
6902         DBG("Paired device %s", address);
6903
6904         /* conn without app is internal one used for search primary services */
6905         conn = find_conn_without_app(dev);
6906         if (!conn)
6907                 return;
6908
6909         if (conn->timeout_id > 0) {
6910                 g_source_remove(conn->timeout_id);
6911                 conn->timeout_id = 0;
6912         }
6913
6914         search_dev_for_srvc(conn, NULL);
6915 }
6916
6917 static void gatt_unpaired_cb(const bdaddr_t *addr)
6918 {
6919         struct gatt_device *dev;
6920         char address[18];
6921
6922         dev = find_device_by_addr(addr);
6923         if (!dev)
6924                 return;
6925
6926         ba2str(addr, address);
6927         DBG("Unpaired device %s", address);
6928
6929         queue_remove(gatt_devices, dev);
6930         destroy_device(dev);
6931 }
6932
6933 bool bt_gatt_register(struct ipc *ipc, const bdaddr_t *addr)
6934 {
6935         DBG("");
6936
6937         if (!bt_paired_register(gatt_paired_cb)) {
6938                 error("gatt: Could not register paired callback");
6939                 return false;
6940         }
6941
6942         if (!bt_unpaired_register(gatt_unpaired_cb)) {
6943                 error("gatt: Could not register unpaired callback");
6944                 return false;
6945         }
6946
6947         if (!start_listening())
6948                 return false;
6949
6950         crypto = bt_crypto_new();
6951         if (!crypto) {
6952                 error("gatt: Failed to setup crypto");
6953                 goto failed;
6954         }
6955
6956         gatt_devices = queue_new();
6957         gatt_apps = queue_new();
6958         app_connections = queue_new();
6959         listen_apps = queue_new();
6960         services_sdp = queue_new();
6961         gatt_db = gatt_db_new();
6962
6963         if (!gatt_db) {
6964                 error("gatt: Failed to allocate memory for database");
6965                 goto failed;
6966         }
6967
6968         if (!bt_le_register(le_device_found_handler)) {
6969                 error("gatt: bt_le_register failed");
6970                 goto failed;
6971         }
6972
6973         bacpy(&adapter_addr, addr);
6974
6975         hal_ipc = ipc;
6976
6977         ipc_register(hal_ipc, HAL_SERVICE_ID_GATT, cmd_handlers,
6978                                                 G_N_ELEMENTS(cmd_handlers));
6979
6980         register_gap_service();
6981         register_device_info_service();
6982         register_gatt_service();
6983
6984         info("gatt: LE: %s BR/EDR: %s", le_io ? "enabled" : "disabled",
6985                                         bredr_io ? "enabled" : "disabled");
6986
6987         return true;
6988
6989 failed:
6990
6991         bt_paired_unregister(gatt_paired_cb);
6992         bt_unpaired_unregister(gatt_unpaired_cb);
6993
6994         queue_destroy(gatt_apps, NULL);
6995         gatt_apps = NULL;
6996
6997         queue_destroy(gatt_devices, NULL);
6998         gatt_devices = NULL;
6999
7000         queue_destroy(app_connections, NULL);
7001         app_connections = NULL;
7002
7003         queue_destroy(listen_apps, NULL);
7004         listen_apps = NULL;
7005
7006         queue_destroy(services_sdp, NULL);
7007         services_sdp = NULL;
7008
7009         gatt_db_unref(gatt_db);
7010         gatt_db = NULL;
7011
7012         bt_crypto_unref(crypto);
7013         crypto = NULL;
7014
7015         if (le_io) {
7016                 g_io_channel_unref(le_io);
7017                 le_io = NULL;
7018         }
7019
7020         if (bredr_io) {
7021                 g_io_channel_unref(bredr_io);
7022                 bredr_io = NULL;
7023         }
7024
7025         return false;
7026 }
7027
7028 void bt_gatt_unregister(void)
7029 {
7030         DBG("");
7031
7032         ipc_unregister(hal_ipc, HAL_SERVICE_ID_GATT);
7033         hal_ipc = NULL;
7034
7035         queue_destroy(app_connections, destroy_connection);
7036         app_connections = NULL;
7037
7038         queue_destroy(gatt_apps, destroy_gatt_app);
7039         gatt_apps = NULL;
7040
7041         queue_destroy(gatt_devices, destroy_device);
7042         gatt_devices = NULL;
7043
7044         queue_destroy(services_sdp, free_service_sdp_record);
7045         services_sdp = NULL;
7046
7047         queue_destroy(listen_apps, NULL);
7048         listen_apps = NULL;
7049
7050         gatt_db_unref(gatt_db);
7051         gatt_db = NULL;
7052
7053         if (le_io) {
7054                 g_io_channel_unref(le_io);
7055                 le_io = NULL;
7056         }
7057
7058         if (bredr_io) {
7059                 g_io_channel_unref(bredr_io);
7060                 bredr_io = NULL;
7061         }
7062
7063         if (gap_sdp_handle) {
7064                 bt_adapter_remove_record(gap_sdp_handle);
7065                 gap_sdp_handle = 0;
7066         }
7067
7068         if (gatt_sdp_handle) {
7069                 bt_adapter_remove_record(gatt_sdp_handle);
7070                 gatt_sdp_handle = 0;
7071         }
7072
7073         if (dis_sdp_handle) {
7074                 bt_adapter_remove_record(dis_sdp_handle);
7075                 dis_sdp_handle = 0;
7076         }
7077
7078         bt_crypto_unref(crypto);
7079         crypto = NULL;
7080
7081         bt_le_unregister();
7082         bt_unpaired_unregister(gatt_unpaired_cb);
7083 }
7084
7085 unsigned int bt_gatt_register_app(const char *uuid, gatt_type_t type,
7086                                                         gatt_conn_cb_t func)
7087 {
7088         struct gatt_app *app;
7089         bt_uuid_t u, u128;
7090
7091         bt_string_to_uuid(&u, uuid);
7092         bt_uuid_to_uuid128(&u, &u128);
7093         app = register_app((void *) &u128.value.u128, type);
7094         if (!app)
7095                 return 0;
7096
7097         app->func = func;
7098
7099         return app->id;
7100 }
7101
7102 bool bt_gatt_unregister_app(unsigned int id)
7103 {
7104         uint8_t status;
7105
7106         status = unregister_app(id);
7107
7108         return status != HAL_STATUS_FAILED;
7109 }
7110
7111 bool bt_gatt_connect_app(unsigned int id, const bdaddr_t *addr)
7112 {
7113         uint8_t status;
7114
7115         status = handle_connect(id, addr, false);
7116
7117         return status != HAL_STATUS_FAILED;
7118 }
7119
7120 bool bt_gatt_disconnect_app(unsigned int id, const bdaddr_t *addr)
7121 {
7122         struct app_connection match;
7123         struct app_connection *conn;
7124         struct gatt_device *device;
7125         struct gatt_app *app;
7126
7127         app = find_app_by_id(id);
7128         if (!app)
7129                 return false;
7130
7131         device = find_device_by_addr(addr);
7132         if (!device)
7133                 return false;
7134
7135         match.device = device;
7136         match.app = app;
7137
7138         conn = queue_remove_if(app_connections,
7139                                 match_connection_by_device_and_app, &match);
7140         if (!conn)
7141                 return false;
7142
7143         destroy_connection(conn);
7144
7145         return true;
7146 }
7147
7148 bool bt_gatt_add_autoconnect(unsigned int id, const bdaddr_t *addr)
7149 {
7150         struct gatt_device *dev;
7151         struct gatt_app *app;
7152
7153         DBG("");
7154
7155         app = find_app_by_id(id);
7156         if (!app) {
7157                 error("gatt: App ID=%d not found", id);
7158                 return false;
7159         }
7160
7161         dev = find_device_by_addr(addr);
7162         if (!dev) {
7163                 error("gatt: Device not found");
7164                 return false;
7165         }
7166
7167         /* Take reference of device for auto connect purpose */
7168         if (queue_isempty(dev->autoconnect_apps))
7169                 device_ref(dev);
7170
7171         if (!queue_find(dev->autoconnect_apps, NULL, INT_TO_PTR(id)))
7172                 return queue_push_head(dev->autoconnect_apps, INT_TO_PTR(id));
7173
7174         return true;
7175 }
7176
7177 void bt_gatt_remove_autoconnect(unsigned int id, const bdaddr_t *addr)
7178 {
7179         struct gatt_device *dev;
7180
7181         DBG("");
7182
7183         dev = find_device_by_addr(addr);
7184         if (!dev) {
7185                 error("gatt: Device not found");
7186                 return;
7187         }
7188
7189         queue_remove(dev->autoconnect_apps, INT_TO_PTR(id));
7190
7191         if (queue_isempty(dev->autoconnect_apps))
7192                 remove_autoconnect_device(dev);
7193 }