profiles/health/mcap.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  *
   4  *  BlueZ - Bluetooth protocol stack for Linux
   5  *
   6  *  Copyright (C) 2010 GSyC/LibreSoft, Universidad Rey Juan Carlos.
   7  *  Copyright (C) 2010 Signove
   8  *  Copyright (C) 2014 Intel Corporation. All rights reserved.
   9  *
  10  */
  11
  12 #ifdef HAVE_CONFIG_H
  13 #include "config.h"
  14 #endif
  15
  16 #include <netinet/in.h>
  17 #include <stdlib.h>
  18 #include <errno.h>
  19 #include <unistd.h>
  20 #include <sys/random.h>
  21
  22 #include <glib.h>
  23
  24 #include "lib/bluetooth.h"
  25 #include "bluetooth/l2cap.h"
  26 #include "btio/btio.h"
  27 #include "src/log.h"
  28 #include "src/shared/timeout.h"
  29
  30 #include "mcap.h"
  31
  32 #define MCAP_BTCLOCK_HALF (MCAP_BTCLOCK_FIELD / 2)
  33 #define CLK CLOCK_MONOTONIC
  34
  35 #define MCAP_CSP_ERROR g_quark_from_static_string("mcap-csp-error-quark")
  36 #define MAX_RETRIES     10
  37 #define SAMPLE_COUNT    20
  38
  39 #define RESPONSE_TIMER  6       /* seconds */
  40 #define MAX_CACHED      10      /* 10 devices */
  41
  42 #define MCAP_ERROR g_quark_from_static_string("mcap-error-quark")
  43
  44 #define RELEASE_TIMER(__mcl) do {               \
  45         if (__mcl->tid) {                       \
  46                 timeout_remove(__mcl->tid);     \
  47                 __mcl->tid = 0;                 \
  48         }                                       \
  49 } while(0)
  50
  51 struct mcap_csp {
  52         uint64_t        base_tmstamp;   /* CSP base timestamp */
  53         struct timespec base_time;      /* CSP base time when timestamp set */
  54         guint           local_caps;     /* CSP-Cent.: have got remote caps */
  55         guint           remote_caps;    /* CSP-Perip: remote central got caps */
  56         guint           rem_req_acc;    /* CSP-Perip: accuracy req by central */
  57         guint           ind_expected;   /* CSP-Cent.: indication expected */
  58         uint8_t         csp_req;        /* CSP-Cent.: Request control flag */
  59         guint           ind_timer;      /* CSP-Perip: indication timer */
  60         guint           set_timer;      /* CSP-Perip: delayed set timer */
  61         void            *set_data;      /* CSP-Perip: delayed set data */
  62         void            *csp_priv_data; /* CSP-Cent.: In-flight request data */
  63 };
  64
  65 struct mcap_sync_cap_cbdata {
  66         mcap_sync_cap_cb        cb;
  67         gpointer                user_data;
  68 };
  69
  70 struct mcap_sync_set_cbdata {
  71         mcap_sync_set_cb        cb;
  72         gpointer                user_data;
  73 };
  74
  75 struct csp_caps {
  76         int ts_acc;             /* timestamp accuracy */
  77         int ts_res;             /* timestamp resolution */
  78         int latency;            /* Read BT clock latency */
  79         int preempt_thresh;     /* Preemption threshold for latency */
  80         int syncleadtime_ms;    /* SyncLeadTime in ms */
  81 };
  82
  83 struct sync_set_data {
  84         uint8_t update;
  85         uint32_t sched_btclock;
  86         uint64_t timestamp;
  87         int ind_freq;
  88         gboolean role;
  89 };
  90
  91 struct connect_mcl {
  92         struct mcap_mcl         *mcl;           /* MCL for this operation */
  93         mcap_mcl_connect_cb     connect_cb;     /* Connect callback */
  94         GDestroyNotify          destroy;        /* Destroy callback */
  95         gpointer                user_data;      /* Callback user data */
  96 };
  97
  98 typedef union {
  99         mcap_mdl_operation_cb           op;
 100         mcap_mdl_operation_conf_cb      op_conf;
 101         mcap_mdl_notify_cb              notify;
 102 } mcap_cb_type;
 103
 104 struct mcap_mdl_op_cb {
 105         struct mcap_mdl         *mdl;           /* MDL for this operation */
 106         mcap_cb_type            cb;             /* Operation callback */
 107         GDestroyNotify          destroy;        /* Destroy callback */
 108         gpointer                user_data;      /* Callback user data */
 109 };
 110
 111 /* MCAP finite state machine functions */
 112 static void proc_req_connected(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t l);
 113 static void proc_req_pending(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t l);
 114 static void proc_req_active(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t l);
 115
 116 static void (*proc_req[])(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len) = {
 117         proc_req_connected,
 118         proc_req_pending,
 119         proc_req_active
 120 };
 121
 122 static gboolean csp_caps_initialized = FALSE;
 123 struct csp_caps _caps;
 124
 125 static void mcap_cache_mcl(struct mcap_mcl *mcl);
 126
 127 static void default_mdl_connected_cb(struct mcap_mdl *mdl, gpointer data)
 128 {
 129         DBG("MCAP Unmanaged mdl connection");
 130 }
 131
 132 static void default_mdl_closed_cb(struct mcap_mdl *mdl, gpointer data)
 133 {
 134         DBG("MCAP Unmanaged mdl closed");
 135 }
 136
 137 static void default_mdl_deleted_cb(struct mcap_mdl *mdl, gpointer data)
 138 {
 139         DBG("MCAP Unmanaged mdl deleted");
 140 }
 141
 142 static void default_mdl_aborted_cb(struct mcap_mdl *mdl, gpointer data)
 143 {
 144         DBG("MCAP Unmanaged mdl aborted");
 145 }
 146
 147 static uint8_t default_mdl_conn_req_cb(struct mcap_mcl *mcl,
 148                                                 uint8_t mdepid, uint16_t mdlid,
 149                                                 uint8_t *conf, gpointer data)
 150 {
 151         DBG("MCAP mdl remote connection aborted");
 152         /* Due to this callback isn't managed this request won't be supported */
 153         return MCAP_REQUEST_NOT_SUPPORTED;
 154 }
 155
 156 static uint8_t default_mdl_reconn_req_cb(struct mcap_mdl *mdl,
 157                                                 gpointer data)
 158 {
 159         DBG("MCAP mdl remote reconnection aborted");
 160         /* Due to this callback isn't managed this request won't be supported */
 161         return MCAP_REQUEST_NOT_SUPPORTED;
 162 }
 163
 164 static void set_default_cb(struct mcap_mcl *mcl)
 165 {
 166         if (!mcl->cb)
 167                 mcl->cb = g_new0(struct mcap_mdl_cb, 1);
 168
 169         mcl->cb->mdl_connected = default_mdl_connected_cb;
 170         mcl->cb->mdl_closed = default_mdl_closed_cb;
 171         mcl->cb->mdl_deleted = default_mdl_deleted_cb;
 172         mcl->cb->mdl_aborted = default_mdl_aborted_cb;
 173         mcl->cb->mdl_conn_req = default_mdl_conn_req_cb;
 174         mcl->cb->mdl_reconn_req = default_mdl_reconn_req_cb;
 175 }
 176
 177 static char *error2str(uint8_t rc)
 178 {
 179         switch (rc) {
 180         case MCAP_SUCCESS:
 181                 return "Success";
 182         case MCAP_INVALID_OP_CODE:
 183                 return "Invalid Op Code";
 184         case MCAP_INVALID_PARAM_VALUE:
 185                 return "Invalid Parameter Value";
 186         case MCAP_INVALID_MDEP:
 187                 return "Invalid MDEP";
 188         case MCAP_MDEP_BUSY:
 189                 return "MDEP Busy";
 190         case MCAP_INVALID_MDL:
 191                 return "Invalid MDL";
 192         case MCAP_MDL_BUSY:
 193                 return "MDL Busy";
 194         case MCAP_INVALID_OPERATION:
 195                 return "Invalid Operation";
 196         case MCAP_RESOURCE_UNAVAILABLE:
 197                 return "Resource Unavailable";
 198         case MCAP_UNSPECIFIED_ERROR:
 199                 return "Unspecified Error";
 200         case MCAP_REQUEST_NOT_SUPPORTED:
 201                 return "Request Not Supported";
 202         case MCAP_CONFIGURATION_REJECTED:
 203                 return "Configuration Rejected";
 204         default:
 205                 return "Unknown Response Code";
 206         }
 207 }
 208
 209 static gboolean mcap_send_std_opcode(struct mcap_mcl *mcl, void *cmd,
 210                                                 uint32_t size, GError **err)
 211 {
 212         if (mcl->state == MCL_IDLE) {
 213                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
 214                                                         "MCL is not connected");
 215                 return FALSE;
 216         }
 217
 218         if (mcl->req != MCL_AVAILABLE) {
 219                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_RESOURCE_UNAVAILABLE,
 220                                                         "Pending request");
 221                 return FALSE;
 222         }
 223
 224         if (!(mcl->ctrl & MCAP_CTRL_STD_OP)) {
 225                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_REQUEST_NOT_SUPPORTED,
 226                                 "Remote does not support standard opcodes");
 227                 return FALSE;
 228         }
 229
 230         if (mcl->state == MCL_PENDING) {
 231                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_OPERATION,
 232                         "Not Std Op. Codes can be sent in PENDING State");
 233                 return FALSE;
 234         }
 235
 236         if (mcap_send_data(g_io_channel_unix_get_fd(mcl->cc), cmd, size) < 0) {
 237                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
 238                                         "Command can't be sent, write error");
 239                 return FALSE;
 240         }
 241
 242         mcl->lcmd = cmd;
 243         mcl->req = MCL_WAITING_RSP;
 244
 245         return TRUE;
 246 }
 247
 248 static void update_mcl_state(struct mcap_mcl *mcl)
 249 {
 250         GSList *l;
 251         struct mcap_mdl *mdl;
 252
 253         if (mcl->state == MCL_PENDING)
 254                 return;
 255
 256         for (l = mcl->mdls; l; l = l->next) {
 257                 mdl = l->data;
 258
 259                 if (mdl->state == MDL_CONNECTED) {
 260                         mcl->state = MCL_ACTIVE;
 261                         return;
 262                 }
 263         }
 264
 265         mcl->state = MCL_CONNECTED;
 266 }
 267
 268 static void shutdown_mdl(struct mcap_mdl *mdl)
 269 {
 270         mdl->state = MDL_CLOSED;
 271
 272         if (mdl->wid) {
 273                 g_source_remove(mdl->wid);
 274                 mdl->wid = 0;
 275         }
 276
 277         if (mdl->dc) {
 278                 g_io_channel_shutdown(mdl->dc, TRUE, NULL);
 279                 g_io_channel_unref(mdl->dc);
 280                 mdl->dc = NULL;
 281         }
 282 }
 283
 284 static void free_mdl(struct mcap_mdl *mdl)
 285 {
 286         if (!mdl)
 287                 return;
 288
 289         mcap_mcl_unref(mdl->mcl);
 290         g_free(mdl);
 291 }
 292
 293 static int cmp_mdl_state(gconstpointer a, gconstpointer b)
 294 {
 295         const struct mcap_mdl *mdl = a;
 296         const MDLState *st = b;
 297
 298         if (mdl->state == *st)
 299                 return 0;
 300         else if (mdl->state < *st)
 301                 return -1;
 302         else
 303                 return 1;
 304 }
 305
 306 static void free_mcap_mdl_op(struct mcap_mdl_op_cb *op)
 307 {
 308         if (op->destroy)
 309                 op->destroy(op->user_data);
 310
 311         if (op->mdl)
 312                 mcap_mdl_unref(op->mdl);
 313
 314         g_free(op);
 315 }
 316
 317 static void free_mcl_priv_data(struct mcap_mcl *mcl)
 318 {
 319         free_mcap_mdl_op(mcl->priv_data);
 320         mcl->priv_data = NULL;
 321 }
 322
 323 static void mcap_notify_error(struct mcap_mcl *mcl, GError *err)
 324 {
 325         struct mcap_mdl_op_cb *con = mcl->priv_data;
 326         struct mcap_mdl *mdl;
 327         MDLState st;
 328         GSList *l;
 329
 330         if (!con || !mcl->lcmd)
 331                 return;
 332
 333         switch (mcl->lcmd[0]) {
 334         case MCAP_MD_CREATE_MDL_REQ:
 335                 st = MDL_WAITING;
 336                 l = g_slist_find_custom(mcl->mdls, &st, cmp_mdl_state);
 337                 mdl = l->data;
 338                 mcl->mdls = g_slist_remove(mcl->mdls, mdl);
 339                 mcap_mdl_unref(mdl);
 340                 update_mcl_state(mcl);
 341                 con->cb.op_conf(NULL, 0, err, con->user_data);
 342                 break;
 343         case MCAP_MD_ABORT_MDL_REQ:
 344                 st = MDL_WAITING;
 345                 l = g_slist_find_custom(mcl->mdls, &st, cmp_mdl_state);
 346                 shutdown_mdl(l->data);
 347                 update_mcl_state(mcl);
 348                 con->cb.notify(err, con->user_data);
 349                 break;
 350         case MCAP_MD_DELETE_MDL_REQ:
 351                 for (l = mcl->mdls; l; l = l->next) {
 352                         mdl = l->data;
 353                         if (mdl->state == MDL_DELETING)
 354                                 mdl->state = (mdl->dc) ? MDL_CONNECTED :
 355                                                                 MDL_CLOSED;
 356                 }
 357                 update_mcl_state(mcl);
 358                 con->cb.notify(err, con->user_data);
 359                 break;
 360         case MCAP_MD_RECONNECT_MDL_REQ:
 361                 st = MDL_WAITING;
 362                 l = g_slist_find_custom(mcl->mdls, &st, cmp_mdl_state);
 363                 shutdown_mdl(l->data);
 364                 update_mcl_state(mcl);
 365                 con->cb.op(NULL, err, con->user_data);
 366                 break;
 367         }
 368
 369         free_mcl_priv_data(mcl);
 370         g_free(mcl->lcmd);
 371         mcl->lcmd = NULL;
 372 }
 373
 374 int mcap_send_data(int sock, const void *buf, uint32_t size)
 375 {
 376         const uint8_t *buf_b = buf;
 377         uint32_t sent = 0;
 378
 379         while (sent < size) {
 380                 int n = write(sock, buf_b + sent, size - sent);
 381                 if (n < 0)
 382                         return -1;
 383                 sent += n;
 384         }
 385
 386         return 0;
 387 }
 388
 389 static int mcap_send_cmd(struct mcap_mcl *mcl, uint8_t oc, uint8_t rc,
 390                                         uint16_t mdl, uint8_t *data, size_t len)
 391 {
 392         mcap_rsp *cmd;
 393         int sock, sent;
 394
 395         if (mcl->cc == NULL)
 396                 return -1;
 397
 398         sock = g_io_channel_unix_get_fd(mcl->cc);
 399
 400         cmd = g_malloc(sizeof(mcap_rsp) + len);
 401         cmd->op = oc;
 402         cmd->rc = rc;
 403         cmd->mdl = htons(mdl);
 404
 405         if (data && len > 0)
 406                 memcpy(cmd->data, data, len);
 407
 408         sent = mcap_send_data(sock, cmd, sizeof(mcap_rsp) + len);
 409         g_free(cmd);
 410
 411         return sent;
 412 }
 413
 414 static struct mcap_mdl *get_mdl(struct mcap_mcl *mcl, uint16_t mdlid)
 415 {
 416         GSList *l;
 417         struct mcap_mdl *mdl;
 418
 419         for (l = mcl->mdls; l; l = l->next) {
 420                 mdl = l->data;
 421                 if (mdlid == mdl->mdlid)
 422                         return mdl;
 423         }
 424
 425         return NULL;
 426 }
 427
 428 static uint16_t generate_mdlid(struct mcap_mcl *mcl)
 429 {
 430         uint16_t mdlid = mcl->next_mdl;
 431         struct mcap_mdl *mdl;
 432
 433         do {
 434                 mdl = get_mdl(mcl, mdlid);
 435                 if (!mdl) {
 436                         mcl->next_mdl = (mdlid % MCAP_MDLID_FINAL) + 1;
 437                         return mdlid;
 438                 } else
 439                         mdlid = (mdlid % MCAP_MDLID_FINAL) + 1;
 440         } while (mdlid != mcl->next_mdl);
 441
 442         /* No more mdlids availables */
 443         return 0;
 444 }
 445
 446 static mcap_md_req *create_req(uint8_t op, uint16_t mdl_id)
 447 {
 448         mcap_md_req *req_cmd;
 449
 450         req_cmd = g_new0(mcap_md_req, 1);
 451
 452         req_cmd->op = op;
 453         req_cmd->mdl = htons(mdl_id);
 454
 455         return req_cmd;
 456 }
 457
 458 static mcap_md_create_mdl_req *create_mdl_req(uint16_t mdl_id, uint8_t mdep,
 459                                                                 uint8_t conf)
 460 {
 461         mcap_md_create_mdl_req *req_mdl;
 462
 463         req_mdl = g_new0(mcap_md_create_mdl_req, 1);
 464
 465         req_mdl->op = MCAP_MD_CREATE_MDL_REQ;
 466         req_mdl->mdl = htons(mdl_id);
 467         req_mdl->mdep = mdep;
 468         req_mdl->conf = conf;
 469
 470         return req_mdl;
 471 }
 472
 473 static int compare_mdl(gconstpointer a, gconstpointer b)
 474 {
 475         const struct mcap_mdl *mdla = a;
 476         const struct mcap_mdl *mdlb = b;
 477
 478         if (mdla->mdlid == mdlb->mdlid)
 479                 return 0;
 480         else if (mdla->mdlid < mdlb->mdlid)
 481                 return -1;
 482         else
 483                 return 1;
 484 }
 485
 486 static bool wait_response_timer(gpointer data)
 487 {
 488         struct mcap_mcl *mcl = data;
 489
 490         GError *gerr = NULL;
 491
 492         RELEASE_TIMER(mcl);
 493
 494         g_set_error(&gerr, MCAP_ERROR, MCAP_ERROR_FAILED,
 495                                         "Timeout waiting response");
 496
 497         mcap_notify_error(mcl, gerr);
 498
 499         g_error_free(gerr);
 500         mcl->mi->mcl_disconnected_cb(mcl, mcl->mi->user_data);
 501         mcap_cache_mcl(mcl);
 502
 503         return FALSE;
 504 }
 505
 506 gboolean mcap_create_mdl(struct mcap_mcl *mcl,
 507                                 uint8_t mdepid,
 508                                 uint8_t conf,
 509                                 mcap_mdl_operation_conf_cb connect_cb,
 510                                 gpointer user_data,
 511                                 GDestroyNotify destroy,
 512                                 GError **err)
 513 {
 514         struct mcap_mdl *mdl;
 515         struct mcap_mdl_op_cb *con;
 516         mcap_md_create_mdl_req *cmd;
 517         uint16_t id;
 518
 519         id = generate_mdlid(mcl);
 520         if (!id) {
 521                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
 522                                         "Not more mdlids available");
 523                 return FALSE;
 524         }
 525
 526         mdl = g_new0(struct mcap_mdl, 1);
 527         mdl->mcl = mcap_mcl_ref(mcl);
 528         mdl->mdlid = id;
 529         mdl->mdep_id = mdepid;
 530         mdl->state = MDL_WAITING;
 531
 532         con = g_new0(struct mcap_mdl_op_cb, 1);
 533         con->mdl = mcap_mdl_ref(mdl);
 534         con->cb.op_conf = connect_cb;
 535         con->destroy = destroy;
 536         con->user_data = user_data;
 537
 538         cmd = create_mdl_req(id, mdepid, conf);
 539         if (!mcap_send_std_opcode(mcl, cmd, sizeof(mcap_md_create_mdl_req),
 540                                                                         err)) {
 541                 mcap_mdl_unref(con->mdl);
 542                 g_free(con);
 543                 g_free(cmd);
 544                 return FALSE;
 545         }
 546
 547         mcl->state = MCL_ACTIVE;
 548         mcl->priv_data = con;
 549
 550         mcl->mdls = g_slist_insert_sorted(mcl->mdls, mcap_mdl_ref(mdl),
 551                                                                 compare_mdl);
 552         mcl->tid = timeout_add_seconds(RESPONSE_TIMER, wait_response_timer,
 553                                                         mcl, NULL);
 554         return TRUE;
 555 }
 556
 557 gboolean mcap_reconnect_mdl(struct mcap_mdl *mdl,
 558                                 mcap_mdl_operation_cb reconnect_cb,
 559                                 gpointer user_data,
 560                                 GDestroyNotify destroy,
 561                                 GError **err)
 562 {
 563         struct mcap_mdl_op_cb *con;
 564         struct mcap_mcl *mcl = mdl->mcl;
 565         mcap_md_req *cmd;
 566
 567         if (mdl->state != MDL_CLOSED) {
 568                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
 569                                         "MDL is not closed");
 570                 return FALSE;
 571         }
 572
 573         cmd = create_req(MCAP_MD_RECONNECT_MDL_REQ, mdl->mdlid);
 574         if (!mcap_send_std_opcode(mcl, cmd, sizeof(mcap_md_req), err)) {
 575                 g_free(cmd);
 576                 return FALSE;
 577         }
 578
 579         mdl->state = MDL_WAITING;
 580
 581         con = g_new0(struct mcap_mdl_op_cb, 1);
 582         con->mdl = mcap_mdl_ref(mdl);
 583         con->cb.op = reconnect_cb;
 584         con->destroy = destroy;
 585         con->user_data = user_data;
 586
 587         mcl->state = MCL_ACTIVE;
 588         mcl->priv_data = con;
 589
 590         mcl->tid = timeout_add_seconds(RESPONSE_TIMER, wait_response_timer,
 591                                                                 mcl, NULL);
 592         return TRUE;
 593 }
 594
 595 static gboolean send_delete_req(struct mcap_mcl *mcl,
 596                                                 struct mcap_mdl_op_cb *con,
 597                                                 uint16_t mdlid,
 598                                                 GError **err)
 599 {
 600         mcap_md_req *cmd;
 601
 602         cmd = create_req(MCAP_MD_DELETE_MDL_REQ, mdlid);
 603         if (!mcap_send_std_opcode(mcl, cmd, sizeof(mcap_md_req), err)) {
 604                 g_free(cmd);
 605                 return FALSE;
 606         }
 607
 608         mcl->priv_data = con;
 609
 610         mcl->tid = timeout_add_seconds(RESPONSE_TIMER, wait_response_timer,
 611                                                         mcl, NULL);
 612         return TRUE;
 613 }
 614
 615 gboolean mcap_delete_all_mdls(struct mcap_mcl *mcl,
 616                                         mcap_mdl_notify_cb delete_cb,
 617                                         gpointer user_data,
 618                                         GDestroyNotify destroy,
 619                                         GError **err)
 620 {
 621         GSList *l;
 622         struct mcap_mdl *mdl;
 623         struct mcap_mdl_op_cb *con;
 624
 625         DBG("MCL in state: %d", mcl->state);
 626         if (!mcl->mdls) {
 627                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
 628                                 "There are not MDLs created");
 629                 return FALSE;
 630         }
 631
 632         for (l = mcl->mdls; l; l = l->next) {
 633                 mdl = l->data;
 634                 if (mdl->state != MDL_WAITING)
 635                         mdl->state = MDL_DELETING;
 636         }
 637
 638         con = g_new0(struct mcap_mdl_op_cb, 1);
 639         con->mdl = NULL;
 640         con->cb.notify = delete_cb;
 641         con->destroy = destroy;
 642         con->user_data = user_data;
 643
 644
 645         if (!send_delete_req(mcl, con, MCAP_ALL_MDLIDS, err)) {
 646                 g_free(con);
 647                 return FALSE;
 648         }
 649
 650         return TRUE;
 651 }
 652
 653 gboolean mcap_delete_mdl(struct mcap_mdl *mdl, mcap_mdl_notify_cb delete_cb,
 654                                                         gpointer user_data,
 655                                                         GDestroyNotify destroy,
 656                                                         GError **err)
 657 {
 658         struct mcap_mcl *mcl= mdl->mcl;
 659         struct mcap_mdl_op_cb *con;
 660         GSList *l;
 661
 662         l = g_slist_find(mcl->mdls, mdl);
 663
 664         if (!l) {
 665                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_MDL,
 666                                         "%s" , error2str(MCAP_INVALID_MDEP));
 667                 return FALSE;
 668         }
 669
 670         if (mdl->state == MDL_WAITING) {
 671                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
 672                                                         "Mdl is not created");
 673                 return FALSE;
 674         }
 675
 676         mdl->state = MDL_DELETING;
 677
 678         con = g_new0(struct mcap_mdl_op_cb, 1);
 679         con->mdl = mcap_mdl_ref(mdl);
 680         con->cb.notify = delete_cb;
 681         con->destroy = destroy;
 682         con->user_data = user_data;
 683
 684         if (!send_delete_req(mcl, con, mdl->mdlid, err)) {
 685                 mcap_mdl_unref(con->mdl);
 686                 g_free(con);
 687                 return FALSE;
 688         }
 689
 690         return TRUE;
 691 }
 692
 693 gboolean mcap_mdl_abort(struct mcap_mdl *mdl, mcap_mdl_notify_cb abort_cb,
 694                                                         gpointer user_data,
 695                                                         GDestroyNotify destroy,
 696                                                         GError **err)
 697 {
 698         struct mcap_mdl_op_cb *con;
 699         struct mcap_mcl *mcl = mdl->mcl;
 700         mcap_md_req *cmd;
 701
 702         if (mdl->state != MDL_WAITING) {
 703                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_FAILED,
 704                                                         "Mdl in invalid state");
 705                 return FALSE;
 706         }
 707
 708         cmd = create_req(MCAP_MD_ABORT_MDL_REQ, mdl->mdlid);
 709         if (!mcap_send_std_opcode(mcl, cmd, sizeof(mcap_md_req), err)) {
 710                 g_free(cmd);
 711                 return FALSE;
 712         }
 713
 714         con = g_new0(struct mcap_mdl_op_cb, 1);
 715         con->mdl = mcap_mdl_ref(mdl);
 716         con->cb.notify = abort_cb;
 717         con->destroy = destroy;
 718         con->user_data = user_data;
 719
 720         mcl->priv_data = con;
 721         mcl->tid = timeout_add_seconds(RESPONSE_TIMER, wait_response_timer,
 722                                                                 mcl, NULL);
 723         return TRUE;
 724 }
 725
 726 static struct mcap_mcl *find_mcl(GSList *list, const bdaddr_t *addr)
 727 {
 728         struct mcap_mcl *mcl;
 729
 730         for (; list; list = list->next) {
 731                 mcl = list->data;
 732
 733                 if (!bacmp(&mcl->addr, addr))
 734                         return mcl;
 735         }
 736
 737         return NULL;
 738 }
 739
 740 int mcap_mdl_get_fd(struct mcap_mdl *mdl)
 741 {
 742         if (!mdl || mdl->state != MDL_CONNECTED)
 743                 return -ENOTCONN;
 744
 745         return g_io_channel_unix_get_fd(mdl->dc);
 746 }
 747
 748 uint16_t mcap_mdl_get_mdlid(struct mcap_mdl *mdl)
 749 {
 750         if (!mdl)
 751                 return MCAP_MDLID_RESERVED;
 752
 753         return mdl->mdlid;
 754 }
 755
 756 static void shutdown_mdl_cb(void *data, void *user_data)
 757 {
 758         shutdown_mdl(data);
 759 }
 760
 761 static void mdl_unref_cb(void *data, void *user_data)
 762 {
 763         mcap_mdl_unref(data);
 764 }
 765
 766 static void close_mcl(struct mcap_mcl *mcl, gboolean cache_requested)
 767 {
 768         gboolean save = ((!(mcl->ctrl & MCAP_CTRL_FREE)) && cache_requested);
 769
 770         RELEASE_TIMER(mcl);
 771
 772         if (mcl->cc) {
 773                 g_io_channel_shutdown(mcl->cc, TRUE, NULL);
 774                 g_io_channel_unref(mcl->cc);
 775                 mcl->cc = NULL;
 776         }
 777
 778         if (mcl->wid) {
 779                 g_source_remove(mcl->wid);
 780                 mcl->wid = 0;
 781         }
 782
 783         if (mcl->lcmd) {
 784                 g_free(mcl->lcmd);
 785                 mcl->lcmd = NULL;
 786         }
 787
 788         if (mcl->priv_data)
 789                 free_mcl_priv_data(mcl);
 790
 791         g_slist_foreach(mcl->mdls, shutdown_mdl_cb, NULL);
 792
 793         mcap_sync_stop(mcl);
 794
 795         mcl->state = MCL_IDLE;
 796
 797         if (save)
 798                 return;
 799
 800         g_slist_foreach(mcl->mdls, mdl_unref_cb, NULL);
 801         g_slist_free(mcl->mdls);
 802         mcl->mdls = NULL;
 803 }
 804
 805 static void mcap_mcl_shutdown(struct mcap_mcl *mcl)
 806 {
 807         close_mcl(mcl, TRUE);
 808 }
 809
 810 static void mcap_mcl_release(struct mcap_mcl *mcl)
 811 {
 812         close_mcl(mcl, FALSE);
 813 }
 814
 815 static void mcap_cache_mcl(struct mcap_mcl *mcl)
 816 {
 817         GSList *l;
 818         struct mcap_mcl *last;
 819         int len;
 820
 821         if (mcl->ctrl & MCAP_CTRL_CACHED)
 822                 return;
 823
 824         mcl->mi->mcls = g_slist_remove(mcl->mi->mcls, mcl);
 825
 826         if (mcl->ctrl & MCAP_CTRL_NOCACHE) {
 827                 mcl->mi->cached = g_slist_remove(mcl->mi->cached, mcl);
 828                 mcap_mcl_release(mcl);
 829                 mcap_mcl_unref(mcl);
 830                 return;
 831         }
 832
 833         DBG("Caching MCL");
 834
 835         len = g_slist_length(mcl->mi->cached);
 836         if (len == MAX_CACHED) {
 837                 /* Remove the latest cached mcl */
 838                 l = g_slist_last(mcl->mi->cached);
 839                 last = l->data;
 840                 mcl->mi->cached = g_slist_remove(mcl->mi->cached, last);
 841                 last->ctrl &= ~MCAP_CTRL_CACHED;
 842                 if (last->ctrl & MCAP_CTRL_CONN) {
 843                         /*
 844                          * We have to release this MCL if connection is not
 845                          * successful
 846                          */
 847                         last->ctrl |= MCAP_CTRL_FREE;
 848                 } else {
 849                         mcap_mcl_release(last);
 850                         last->mi->mcl_uncached_cb(last, last->mi->user_data);
 851                 }
 852                 mcap_mcl_unref(last);
 853         }
 854
 855         mcl->mi->cached = g_slist_prepend(mcl->mi->cached, mcl);
 856         mcl->ctrl |= MCAP_CTRL_CACHED;
 857         mcap_mcl_shutdown(mcl);
 858 }
 859
 860 static void mcap_uncache_mcl(struct mcap_mcl *mcl)
 861 {
 862         if (!(mcl->ctrl & MCAP_CTRL_CACHED))
 863                 return;
 864
 865         DBG("Got MCL from cache");
 866
 867         mcl->mi->cached = g_slist_remove(mcl->mi->cached, mcl);
 868         mcl->mi->mcls = g_slist_prepend(mcl->mi->mcls, mcl);
 869         mcl->ctrl &= ~MCAP_CTRL_CACHED;
 870         mcl->ctrl &= ~MCAP_CTRL_FREE;
 871 }
 872
 873 void mcap_close_mcl(struct mcap_mcl *mcl, gboolean cache)
 874 {
 875         if (!mcl)
 876                 return;
 877
 878         if (mcl->ctrl & MCAP_CTRL_FREE) {
 879                 mcap_mcl_release(mcl);
 880                 return;
 881         }
 882
 883         if (!cache)
 884                 mcl->ctrl |= MCAP_CTRL_NOCACHE;
 885
 886         if (mcl->cc) {
 887                 g_io_channel_shutdown(mcl->cc, TRUE, NULL);
 888                 g_io_channel_unref(mcl->cc);
 889                 mcl->cc = NULL;
 890                 mcl->state = MCL_IDLE;
 891         } else if ((mcl->ctrl & MCAP_CTRL_CACHED) &&
 892                                         (mcl->ctrl & MCAP_CTRL_NOCACHE)) {
 893                 mcl->ctrl &= ~MCAP_CTRL_CACHED;
 894                 mcl->mi->cached = g_slist_remove(mcl->mi->cached, mcl);
 895                 mcap_mcl_release(mcl);
 896                 mcap_mcl_unref(mcl);
 897         }
 898 }
 899
 900 struct mcap_mcl *mcap_mcl_ref(struct mcap_mcl *mcl)
 901 {
 902         mcl->ref++;
 903
 904         DBG("mcap_mcl_ref(%p): ref=%d", mcl, mcl->ref);
 905
 906         return mcl;
 907 }
 908
 909 void mcap_mcl_unref(struct mcap_mcl *mcl)
 910 {
 911         mcl->ref--;
 912
 913         DBG("mcap_mcl_unref(%p): ref=%d", mcl, mcl->ref);
 914
 915         if (mcl->ref > 0)
 916                 return;
 917
 918         mcap_mcl_release(mcl);
 919         mcap_instance_unref(mcl->mi);
 920         g_free(mcl->cb);
 921         g_free(mcl);
 922 }
 923
 924 static gboolean parse_set_opts(struct mcap_mdl_cb *mdl_cb, GError **err,
 925                                                 McapMclCb cb1, va_list args)
 926 {
 927         McapMclCb cb = cb1;
 928         struct mcap_mdl_cb *c;
 929
 930         c = g_new0(struct mcap_mdl_cb, 1);
 931
 932         while (cb != MCAP_MDL_CB_INVALID) {
 933                 switch (cb) {
 934                 case MCAP_MDL_CB_CONNECTED:
 935                         c->mdl_connected = va_arg(args, mcap_mdl_event_cb);
 936                         break;
 937                 case MCAP_MDL_CB_CLOSED:
 938                         c->mdl_closed = va_arg(args, mcap_mdl_event_cb);
 939                         break;
 940                 case MCAP_MDL_CB_DELETED:
 941                         c->mdl_deleted = va_arg(args, mcap_mdl_event_cb);
 942                         break;
 943                 case MCAP_MDL_CB_ABORTED:
 944                         c->mdl_aborted = va_arg(args, mcap_mdl_event_cb);
 945                         break;
 946                 case MCAP_MDL_CB_REMOTE_CONN_REQ:
 947                         c->mdl_conn_req = va_arg(args,
 948                                                 mcap_remote_mdl_conn_req_cb);
 949                         break;
 950                 case MCAP_MDL_CB_REMOTE_RECONN_REQ:
 951                         c->mdl_reconn_req = va_arg(args,
 952                                                 mcap_remote_mdl_reconn_req_cb);
 953                         break;
 954                 case MCAP_MDL_CB_INVALID:
 955                 default:
 956                         g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
 957                                                 "Unknown option %d", cb);
 958                         g_free(c);
 959                         return FALSE;
 960                 }
 961                 cb = va_arg(args, int);
 962         }
 963
 964         /* Set new callbacks */
 965         if (c->mdl_connected)
 966                 mdl_cb->mdl_connected = c->mdl_connected;
 967         if (c->mdl_closed)
 968                 mdl_cb->mdl_closed = c->mdl_closed;
 969         if (c->mdl_deleted)
 970                 mdl_cb->mdl_deleted = c->mdl_deleted;
 971         if (c->mdl_aborted)
 972                 mdl_cb->mdl_aborted = c->mdl_aborted;
 973         if (c->mdl_conn_req)
 974                 mdl_cb->mdl_conn_req = c->mdl_conn_req;
 975         if (c->mdl_reconn_req)
 976                 mdl_cb->mdl_reconn_req = c->mdl_reconn_req;
 977
 978         g_free(c);
 979
 980         return TRUE;
 981 }
 982
 983 gboolean mcap_mcl_set_cb(struct mcap_mcl *mcl, gpointer user_data,
 984                                         GError **gerr, McapMclCb cb1, ...)
 985 {
 986         va_list args;
 987         gboolean ret;
 988
 989         va_start(args, cb1);
 990         ret = parse_set_opts(mcl->cb, gerr, cb1, args);
 991         va_end(args);
 992
 993         if (!ret)
 994                 return FALSE;
 995
 996         mcl->cb->user_data = user_data;
 997         return TRUE;
 998 }
 999
1000 void mcap_mcl_get_addr(struct mcap_mcl *mcl, bdaddr_t *addr)
1001 {
1002         bacpy(addr, &mcl->addr);
1003 }
1004
1005 static void mcap_del_mdl(gpointer elem, gpointer user_data)
1006 {
1007         struct mcap_mdl *mdl = elem;
1008         gboolean notify = *(gboolean *) user_data;
1009
1010         if (notify)
1011                 mdl->mcl->cb->mdl_deleted(mdl, mdl->mcl->cb->user_data);
1012
1013         shutdown_mdl(mdl);
1014         mcap_mdl_unref(mdl);
1015 }
1016
1017 static gboolean check_cmd_req_length(struct mcap_mcl *mcl, void *cmd,
1018                                 uint32_t rlen, uint32_t explen, uint8_t rspcod)
1019 {
1020         mcap_md_req *req;
1021         uint16_t mdl_id;
1022
1023         if (rlen != explen) {
1024                 if (rlen >= sizeof(mcap_md_req)) {
1025                         req = cmd;
1026                         mdl_id = ntohs(req->mdl);
1027                 } else {
1028                         /* We can't get mdlid */
1029                         mdl_id = MCAP_MDLID_RESERVED;
1030                 }
1031                 mcap_send_cmd(mcl, rspcod, MCAP_INVALID_PARAM_VALUE, mdl_id,
1032                                                                 NULL, 0);
1033                 return FALSE;
1034         }
1035         return TRUE;
1036 }
1037
1038 static void process_md_create_mdl_req(struct mcap_mcl *mcl, void *cmd,
1039                                                                 uint32_t len)
1040 {
1041         mcap_md_create_mdl_req *req;
1042         struct mcap_mdl *mdl;
1043         uint16_t mdl_id;
1044         uint8_t mdep_id;
1045         uint8_t cfga, conf;
1046         uint8_t rsp;
1047
1048         if (!check_cmd_req_length(mcl, cmd, len, sizeof(mcap_md_create_mdl_req),
1049                                                         MCAP_MD_CREATE_MDL_RSP))
1050                 return;
1051
1052         req = cmd;
1053         mdl_id = ntohs(req->mdl);
1054         if (mdl_id < MCAP_MDLID_INITIAL || mdl_id > MCAP_MDLID_FINAL) {
1055                 mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_INVALID_MDL,
1056                                                         mdl_id, NULL, 0);
1057                 return;
1058         }
1059
1060         mdep_id = req->mdep;
1061         if (mdep_id > MCAP_MDEPID_FINAL) {
1062                 mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_INVALID_MDEP,
1063                                                         mdl_id, NULL, 0);
1064                 return;
1065         }
1066
1067         mdl = get_mdl(mcl, mdl_id);
1068         if (mdl && (mdl->state == MDL_WAITING || mdl->state == MDL_DELETING )) {
1069                 /*
1070                  *  Creation request arrives for a MDL that is being managed
1071                  * at current moment
1072                  */
1073                 mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_MDL_BUSY,
1074                                                         mdl_id, NULL, 0);
1075                 return;
1076         }
1077
1078         cfga = conf = req->conf;
1079         /* Callback to upper layer */
1080         rsp = mcl->cb->mdl_conn_req(mcl, mdep_id, mdl_id, &conf,
1081                                                         mcl->cb->user_data);
1082         if (mcl->state == MCL_IDLE) {
1083                 /* MCL has been closed int the callback */
1084                 return;
1085         }
1086
1087         if (cfga != 0 && cfga != conf) {
1088                 /*
1089                  * Remote device set default configuration but upper profile
1090                  * has changed it. Protocol Error: force closing the MCL by
1091                  * remote device using UNSPECIFIED_ERROR response
1092                  */
1093                 mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP,
1094                                 MCAP_UNSPECIFIED_ERROR, mdl_id, NULL, 0);
1095                 return;
1096         }
1097         if (rsp != MCAP_SUCCESS) {
1098                 mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, rsp, mdl_id,
1099                                                                 NULL, 0);
1100                 return;
1101         }
1102
1103         if (!mdl) {
1104                 mdl = g_new0(struct mcap_mdl, 1);
1105                 mdl->mcl = mcap_mcl_ref(mcl);
1106                 mdl->mdlid = mdl_id;
1107                 mcl->mdls = g_slist_insert_sorted(mcl->mdls, mcap_mdl_ref(mdl),
1108                                                                 compare_mdl);
1109         } else if (mdl->state == MDL_CONNECTED) {
1110                 /*
1111                  * MCAP specification says that we should close the MCL if
1112                  * it is open when we receive a MD_CREATE_MDL_REQ
1113                  */
1114                 shutdown_mdl(mdl);
1115         }
1116
1117         mdl->mdep_id = mdep_id;
1118         mdl->state = MDL_WAITING;
1119
1120         mcl->state = MCL_PENDING;
1121         mcap_send_cmd(mcl, MCAP_MD_CREATE_MDL_RSP, MCAP_SUCCESS, mdl_id,
1122                                                                 &conf, 1);
1123 }
1124
1125 static void process_md_reconnect_mdl_req(struct mcap_mcl *mcl, void *cmd,
1126                                                                 uint32_t len)
1127 {
1128         mcap_md_req *req;
1129         struct mcap_mdl *mdl;
1130         uint16_t mdl_id;
1131         uint8_t rsp;
1132
1133         if (!check_cmd_req_length(mcl, cmd, len, sizeof(mcap_md_req),
1134                                                 MCAP_MD_RECONNECT_MDL_RSP))
1135                 return;
1136
1137         req = cmd;
1138         mdl_id = ntohs(req->mdl);
1139
1140         mdl = get_mdl(mcl, mdl_id);
1141         if (!mdl) {
1142                 mcap_send_cmd(mcl, MCAP_MD_RECONNECT_MDL_RSP, MCAP_INVALID_MDL,
1143                                                         mdl_id, NULL, 0);
1144                 return;
1145         } else if (mdl->state == MDL_WAITING || mdl->state == MDL_DELETING ) {
1146                 /*
1147                  * Creation request arrives for a MDL that is being managed
1148                  * at current moment
1149                  */
1150                 mcap_send_cmd(mcl, MCAP_MD_RECONNECT_MDL_RSP, MCAP_MDL_BUSY,
1151                                                         mdl_id, NULL, 0);
1152                 return;
1153         }
1154
1155         /* Callback to upper layer */
1156         rsp = mcl->cb->mdl_reconn_req(mdl, mcl->cb->user_data);
1157         if (mcl->state == MCL_IDLE)
1158                 return;
1159
1160         if (rsp != MCAP_SUCCESS) {
1161                 mcap_send_cmd(mcl, MCAP_MD_RECONNECT_MDL_RSP, rsp, mdl_id,
1162                                                                 NULL, 0);
1163                 return;
1164         }
1165
1166         if (mdl->state == MDL_CONNECTED)
1167                 shutdown_mdl(mdl);
1168
1169         mdl->state = MDL_WAITING;
1170         mcl->state = MCL_PENDING;
1171         mcap_send_cmd(mcl, MCAP_MD_RECONNECT_MDL_RSP, MCAP_SUCCESS, mdl_id,
1172                                                                 NULL, 0);
1173 }
1174
1175 static void process_md_abort_mdl_req(struct mcap_mcl *mcl, void *cmd,
1176                                                                 uint32_t len)
1177 {
1178         mcap_md_req *req;
1179         GSList *l;
1180         struct mcap_mdl *mdl, *abrt;
1181         uint16_t mdl_id;
1182
1183         if (!check_cmd_req_length(mcl, cmd, len, sizeof(mcap_md_req),
1184                                                         MCAP_MD_ABORT_MDL_RSP))
1185                 return;
1186
1187         req = cmd;
1188         mdl_id = ntohs(req->mdl);
1189         mcl->state = MCL_CONNECTED;
1190         abrt = NULL;
1191         for (l = mcl->mdls; l; l = l->next) {
1192                 mdl = l->data;
1193                 if (mdl_id == mdl->mdlid && mdl->state == MDL_WAITING) {
1194                         abrt = mdl;
1195                         if (mcl->state != MCL_CONNECTED)
1196                                 break;
1197                         continue;
1198                 }
1199                 if (mdl->state == MDL_CONNECTED && mcl->state != MCL_ACTIVE)
1200                         mcl->state = MCL_ACTIVE;
1201
1202                 if (abrt && mcl->state == MCL_ACTIVE)
1203                         break;
1204         }
1205
1206         if (!abrt) {
1207                 mcap_send_cmd(mcl, MCAP_MD_ABORT_MDL_RSP, MCAP_INVALID_MDL,
1208                                                         mdl_id, NULL, 0);
1209                 return;
1210         }
1211
1212         mcl->cb->mdl_aborted(abrt, mcl->cb->user_data);
1213         abrt->state = MDL_CLOSED;
1214         mcap_send_cmd(mcl, MCAP_MD_ABORT_MDL_RSP, MCAP_SUCCESS, mdl_id,
1215                                                                 NULL, 0);
1216 }
1217
1218 static void process_md_delete_mdl_req(struct mcap_mcl *mcl, void *cmd,
1219                                                                 uint32_t len)
1220 {
1221         mcap_md_req *req;
1222         struct mcap_mdl *mdl, *aux;
1223         uint16_t mdlid;
1224         gboolean notify;
1225         GSList *l;
1226
1227         if (!check_cmd_req_length(mcl, cmd, len, sizeof(mcap_md_req),
1228                                                         MCAP_MD_DELETE_MDL_RSP))
1229                 return;
1230
1231         req = cmd;
1232         mdlid = ntohs(req->mdl);
1233         if (mdlid == MCAP_ALL_MDLIDS) {
1234                 notify = TRUE;
1235                 g_slist_foreach(mcl->mdls, mcap_del_mdl, &notify);
1236                 g_slist_free(mcl->mdls);
1237                 mcl->mdls = NULL;
1238                 mcl->state = MCL_CONNECTED;
1239                 goto resp;
1240         }
1241
1242         if (mdlid < MCAP_MDLID_INITIAL || mdlid > MCAP_MDLID_FINAL) {
1243                 mcap_send_cmd(mcl, MCAP_MD_DELETE_MDL_RSP, MCAP_INVALID_MDL,
1244                                                                 mdlid, NULL, 0);
1245                 return;
1246         }
1247
1248         for (l = mcl->mdls, mdl = NULL; l; l = l->next) {
1249                 aux = l->data;
1250                 if (aux->mdlid == mdlid) {
1251                         mdl = aux;
1252                         break;
1253                 }
1254         }
1255
1256         if (!mdl || mdl->state == MDL_WAITING) {
1257                 mcap_send_cmd(mcl, MCAP_MD_DELETE_MDL_RSP, MCAP_INVALID_MDL,
1258                                                                 mdlid, NULL, 0);
1259                 return;
1260         }
1261
1262         mcl->mdls = g_slist_remove(mcl->mdls, mdl);
1263         update_mcl_state(mcl);
1264         notify = TRUE;
1265         mcap_del_mdl(mdl, &notify);
1266
1267 resp:
1268         mcap_send_cmd(mcl, MCAP_MD_DELETE_MDL_RSP, MCAP_SUCCESS, mdlid,
1269                                                                 NULL, 0);
1270 }
1271
1272 static void invalid_req_state(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
1273 {
1274         uint16_t mdlr;
1275
1276         error("Invalid cmd received (op code = %d) in state %d", cmd[0],
1277                                                                 mcl->state);
1278         /*
1279          * Get previously mdlid sent to generate an appropriate
1280          * response if it is possible
1281          */
1282         mdlr = len < sizeof(mcap_md_req) ? MCAP_MDLID_RESERVED :
1283                                         ntohs(((mcap_md_req *) cmd)->mdl);
1284         mcap_send_cmd(mcl, cmd[0]+1, MCAP_INVALID_OPERATION, mdlr, NULL, 0);
1285 }
1286
1287 /* Function used to process commands depending of MCL state */
1288 static void proc_req_connected(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
1289 {
1290         switch (cmd[0]) {
1291         case MCAP_MD_CREATE_MDL_REQ:
1292                 process_md_create_mdl_req(mcl, cmd, len);
1293                 break;
1294         case MCAP_MD_RECONNECT_MDL_REQ:
1295                 process_md_reconnect_mdl_req(mcl, cmd, len);
1296                 break;
1297         case MCAP_MD_DELETE_MDL_REQ:
1298                 process_md_delete_mdl_req(mcl, cmd, len);
1299                 break;
1300         default:
1301                 invalid_req_state(mcl, cmd, len);
1302         }
1303 }
1304
1305 static void proc_req_pending(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
1306 {
1307         if (cmd[0] == MCAP_MD_ABORT_MDL_REQ)
1308                 process_md_abort_mdl_req(mcl, cmd, len);
1309         else
1310                 invalid_req_state(mcl, cmd, len);
1311 }
1312
1313 static void proc_req_active(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
1314 {
1315         switch (cmd[0]) {
1316         case MCAP_MD_CREATE_MDL_REQ:
1317                 process_md_create_mdl_req(mcl, cmd, len);
1318                 break;
1319         case MCAP_MD_RECONNECT_MDL_REQ:
1320                 process_md_reconnect_mdl_req(mcl, cmd, len);
1321                 break;
1322         case MCAP_MD_DELETE_MDL_REQ:
1323                 process_md_delete_mdl_req(mcl, cmd, len);
1324                 break;
1325         default:
1326                 invalid_req_state(mcl, cmd, len);
1327         }
1328 }
1329
1330 /* Function used to process replies */
1331 static gboolean check_err_rsp(struct mcap_mcl *mcl, mcap_rsp *rsp,
1332                                 uint32_t rlen, uint32_t len, GError **gerr)
1333 {
1334         mcap_md_req *cmdlast = (mcap_md_req *) mcl->lcmd;
1335         int err = MCAP_ERROR_FAILED;
1336         gboolean close = FALSE;
1337         char *msg;
1338
1339         if (rsp->op == MCAP_ERROR_RSP) {
1340                 msg = "MCAP_ERROR_RSP received";
1341                 close = FALSE;
1342                 goto fail;
1343         }
1344
1345         /* Check if the response matches with the last request */
1346         if (rlen < sizeof(mcap_rsp) || (mcl->lcmd[0] + 1) != rsp->op) {
1347                 msg = "Protocol error";
1348                 close = FALSE;
1349                 goto fail;
1350         }
1351
1352         if (rlen < len) {
1353                 msg = "Protocol error";
1354                 close = FALSE;
1355                 goto fail;
1356         }
1357
1358         if (rsp->mdl != cmdlast->mdl) {
1359                 msg = "MDLID received doesn't match with MDLID sent";
1360                 close = TRUE;
1361                 goto fail;
1362         }
1363
1364         if (rsp->rc == MCAP_REQUEST_NOT_SUPPORTED) {
1365                 msg = "Remote does not support opcodes";
1366                 mcl->ctrl &= ~MCAP_CTRL_STD_OP;
1367                 goto fail;
1368         }
1369
1370         if (rsp->rc == MCAP_UNSPECIFIED_ERROR) {
1371                 msg = "Unspecified error";
1372                 close = TRUE;
1373                 goto fail;
1374         }
1375
1376         if (rsp->rc != MCAP_SUCCESS) {
1377                 msg = error2str(rsp->rc);
1378                 err = rsp->rc;
1379                 goto fail;
1380         }
1381
1382         return FALSE;
1383
1384 fail:
1385         g_set_error(gerr, MCAP_ERROR, err, "%s", msg);
1386         return close;
1387 }
1388
1389 static gboolean process_md_create_mdl_rsp(struct mcap_mcl *mcl,
1390                                                 mcap_rsp *rsp, uint32_t len)
1391 {
1392         mcap_md_create_mdl_req *cmdlast = (mcap_md_create_mdl_req *) mcl->lcmd;
1393         struct mcap_mdl_op_cb *conn = mcl->priv_data;
1394         mcap_mdl_operation_conf_cb connect_cb = conn->cb.op_conf;
1395         gpointer user_data = conn->user_data;
1396         struct mcap_mdl *mdl = conn->mdl;
1397         uint8_t conf = cmdlast->conf;
1398         gboolean close;
1399         GError *gerr = NULL;
1400
1401         close = check_err_rsp(mcl, rsp, len, sizeof(mcap_rsp) + 1, &gerr);
1402         g_free(mcl->lcmd);
1403         mcl->lcmd = NULL;
1404         mcl->req = MCL_AVAILABLE;
1405
1406         if (gerr)
1407                 goto fail;
1408
1409         /* Check if preferences changed */
1410         if (conf != 0x00 && rsp->data[0] != conf) {
1411                 g_set_error(&gerr, MCAP_ERROR, MCAP_ERROR_FAILED,
1412                                                 "Configuration changed");
1413                 close = TRUE;
1414                 goto fail;
1415         }
1416
1417         connect_cb(mdl, rsp->data[0], gerr, user_data);
1418         return close;
1419
1420 fail:
1421         connect_cb(NULL, 0, gerr, user_data);
1422         mcl->mdls = g_slist_remove(mcl->mdls, mdl);
1423         mcap_mdl_unref(mdl);
1424         g_error_free(gerr);
1425         update_mcl_state(mcl);
1426         return close;
1427 }
1428
1429 static gboolean process_md_reconnect_mdl_rsp(struct mcap_mcl *mcl,
1430                                                 mcap_rsp *rsp, uint32_t len)
1431 {
1432         struct mcap_mdl_op_cb *reconn = mcl->priv_data;
1433         mcap_mdl_operation_cb reconn_cb = reconn->cb.op;
1434         gpointer user_data = reconn->user_data;
1435         struct mcap_mdl *mdl = reconn->mdl;
1436         GError *gerr = NULL;
1437         gboolean close;
1438
1439         close = check_err_rsp(mcl, rsp, len, sizeof(mcap_rsp), &gerr);
1440
1441         g_free(mcl->lcmd);
1442         mcl->lcmd = NULL;
1443         mcl->req = MCL_AVAILABLE;
1444
1445         reconn_cb(mdl, gerr, user_data);
1446         if (!gerr)
1447                 return close;
1448
1449         g_error_free(gerr);
1450         shutdown_mdl(mdl);
1451         update_mcl_state(mcl);
1452
1453         if (rsp->rc != MCAP_INVALID_MDL)
1454                 return close;
1455
1456         /* Remove cached mdlid */
1457         mcl->mdls = g_slist_remove(mcl->mdls, mdl);
1458         mcl->cb->mdl_deleted(mdl, mcl->cb->user_data);
1459         mcap_mdl_unref(mdl);
1460
1461         return close;
1462 }
1463
1464 static gboolean process_md_abort_mdl_rsp(struct mcap_mcl *mcl,
1465                                                 mcap_rsp *rsp, uint32_t len)
1466 {
1467         struct mcap_mdl_op_cb *abrt = mcl->priv_data;
1468         mcap_mdl_notify_cb abrt_cb = abrt->cb.notify;
1469         gpointer user_data = abrt->user_data;
1470         struct mcap_mdl *mdl = abrt->mdl;
1471         GError *gerr = NULL;
1472         gboolean close;
1473
1474         close = check_err_rsp(mcl, rsp, len, sizeof(mcap_rsp), &gerr);
1475
1476         g_free(mcl->lcmd);
1477         mcl->lcmd = NULL;
1478         mcl->req = MCL_AVAILABLE;
1479
1480         abrt_cb(gerr, user_data);
1481         shutdown_mdl(mdl);
1482
1483         if (len >= sizeof(mcap_rsp) && rsp->rc == MCAP_INVALID_MDL) {
1484                 mcl->mdls = g_slist_remove(mcl->mdls, mdl);
1485                 mcl->cb->mdl_deleted(mdl, mcl->cb->user_data);
1486                 mcap_mdl_unref(mdl);
1487         }
1488
1489         if (gerr)
1490                 g_error_free(gerr);
1491
1492         update_mcl_state(mcl);
1493
1494         return close;
1495 }
1496
1497 static void restore_mdl(gpointer elem, gpointer data)
1498 {
1499         struct mcap_mdl *mdl = elem;
1500
1501         if (mdl->state == MDL_DELETING) {
1502                 if (mdl->dc)
1503                         mdl->state = MDL_CONNECTED;
1504                 else
1505                         mdl->state = MDL_CLOSED;
1506         } else if (mdl->state == MDL_CLOSED)
1507                 mdl->mcl->cb->mdl_closed(mdl, mdl->mcl->cb->user_data);
1508 }
1509
1510 static void check_mdl_del_err(struct mcap_mdl *mdl, mcap_rsp *rsp)
1511 {
1512         if (rsp->rc != MCAP_ERROR_INVALID_MDL) {
1513                 restore_mdl(mdl, NULL);
1514                 return;
1515         }
1516
1517         /* MDL does not exist in remote side, we can delete it */
1518         mdl->mcl->mdls = g_slist_remove(mdl->mcl->mdls, mdl);
1519         mcap_mdl_unref(mdl);
1520 }
1521
1522 static gboolean process_md_delete_mdl_rsp(struct mcap_mcl *mcl, mcap_rsp *rsp,
1523                                                                 uint32_t len)
1524 {
1525         struct mcap_mdl_op_cb *del = mcl->priv_data;
1526         struct mcap_mdl *mdl = del->mdl;
1527         mcap_mdl_notify_cb deleted_cb = del->cb.notify;
1528         gpointer user_data = del->user_data;
1529         mcap_md_req *cmdlast = (mcap_md_req *) mcl->lcmd;
1530         uint16_t mdlid = ntohs(cmdlast->mdl);
1531         GError *gerr = NULL;
1532         gboolean close;
1533         gboolean notify = FALSE;
1534
1535         close = check_err_rsp(mcl, rsp, len, sizeof(mcap_rsp), &gerr);
1536
1537         g_free(mcl->lcmd);
1538         mcl->lcmd = NULL;
1539         mcl->req = MCL_AVAILABLE;
1540
1541         if (gerr) {
1542                 if (mdl)
1543                         check_mdl_del_err(mdl, rsp);
1544                 else
1545                         g_slist_foreach(mcl->mdls, restore_mdl, NULL);
1546                 deleted_cb(gerr, user_data);
1547                 g_error_free(gerr);
1548                 return close;
1549         }
1550
1551         if (mdlid == MCAP_ALL_MDLIDS) {
1552                 g_slist_foreach(mcl->mdls, mcap_del_mdl, &notify);
1553                 g_slist_free(mcl->mdls);
1554                 mcl->mdls = NULL;
1555                 mcl->state = MCL_CONNECTED;
1556         } else {
1557                 mcl->mdls = g_slist_remove(mcl->mdls, mdl);
1558                 update_mcl_state(mcl);
1559                 mcap_del_mdl(mdl, &notify);
1560         }
1561
1562         deleted_cb(gerr, user_data);
1563
1564         return close;
1565 }
1566
1567 static void post_process_rsp(struct mcap_mcl *mcl, struct mcap_mdl_op_cb *op)
1568 {
1569         if (mcl->priv_data != op) {
1570                 /*
1571                  * Queued MCAP request in some callback.
1572                  * We should not delete the mcl private data
1573                  */
1574                 free_mcap_mdl_op(op);
1575         } else {
1576                 /*
1577                  * This is not a queued request. It's safe
1578                  * delete the mcl private data here.
1579                  */
1580                 free_mcl_priv_data(mcl);
1581         }
1582 }
1583
1584 static void proc_response(struct mcap_mcl *mcl, void *buf, uint32_t len)
1585 {
1586         struct mcap_mdl_op_cb *op = mcl->priv_data;
1587         mcap_rsp *rsp = buf;
1588         gboolean close;
1589
1590         RELEASE_TIMER(mcl);
1591
1592         switch (mcl->lcmd[0] + 1) {
1593         case MCAP_MD_CREATE_MDL_RSP:
1594                 close = process_md_create_mdl_rsp(mcl, rsp, len);
1595                 post_process_rsp(mcl, op);
1596                 break;
1597         case MCAP_MD_RECONNECT_MDL_RSP:
1598                 close = process_md_reconnect_mdl_rsp(mcl, rsp, len);
1599                 post_process_rsp(mcl, op);
1600                 break;
1601         case MCAP_MD_ABORT_MDL_RSP:
1602                 close = process_md_abort_mdl_rsp(mcl, rsp, len);
1603                 post_process_rsp(mcl, op);
1604                 break;
1605         case MCAP_MD_DELETE_MDL_RSP:
1606                 close = process_md_delete_mdl_rsp(mcl, rsp, len);
1607                 post_process_rsp(mcl, op);
1608                 break;
1609         default:
1610                 DBG("Unknown cmd response received (op code = %d)", rsp->op);
1611                 close = TRUE;
1612                 break;
1613         }
1614
1615         if (close) {
1616                 mcl->mi->mcl_disconnected_cb(mcl, mcl->mi->user_data);
1617                 mcap_cache_mcl(mcl);
1618         }
1619 }
1620
1621 static void proc_cmd(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
1622 {
1623         GError *gerr = NULL;
1624
1625         if (cmd[0] > MCAP_MD_SYNC_INFO_IND ||
1626                                         (cmd[0] > MCAP_MD_DELETE_MDL_RSP &&
1627                                         cmd[0] < MCAP_MD_SYNC_CAP_REQ)) {
1628                 error("Unknown cmd received (op code = %d)", cmd[0]);
1629                 mcap_send_cmd(mcl, MCAP_ERROR_RSP, MCAP_INVALID_OP_CODE,
1630                                                 MCAP_MDLID_RESERVED, NULL, 0);
1631                 return;
1632         }
1633
1634         if (cmd[0] >= MCAP_MD_SYNC_CAP_REQ &&
1635                                         cmd[0] <= MCAP_MD_SYNC_INFO_IND) {
1636                 proc_sync_cmd(mcl, cmd, len);
1637                 return;
1638         }
1639
1640         if (!(mcl->ctrl & MCAP_CTRL_STD_OP)) {
1641                 /* In case the remote device doesn't work correctly */
1642                 error("Remote device does not support opcodes, cmd ignored");
1643                 return;
1644         }
1645
1646         if (mcl->req == MCL_WAITING_RSP) {
1647                 if (cmd[0] & 0x01) {
1648                         /* Request arrived when a response is expected */
1649                         if (mcl->role == MCL_INITIATOR)
1650                                 /* ignore */
1651                                 return;
1652                         /* Initiator will ignore our last request */
1653                         RELEASE_TIMER(mcl);
1654                         mcl->req = MCL_AVAILABLE;
1655                         g_set_error(&gerr, MCAP_ERROR, MCAP_ERROR_REQ_IGNORED,
1656                                 "Initiator sent a request with more priority");
1657                         mcap_notify_error(mcl, gerr);
1658                         proc_req[mcl->state](mcl, cmd, len);
1659                         return;
1660                 }
1661                 proc_response(mcl, cmd, len);
1662         } else if (cmd[0] & 0x01)
1663                 proc_req[mcl->state](mcl, cmd, len);
1664 }
1665
1666 static gboolean mdl_event_cb(GIOChannel *chan, GIOCondition cond, gpointer data)
1667 {
1668
1669         struct mcap_mdl *mdl = data;
1670         gboolean notify;
1671
1672         DBG("Close MDL %d", mdl->mdlid);
1673
1674         notify = (mdl->state == MDL_CONNECTED);
1675         shutdown_mdl(mdl);
1676
1677         update_mcl_state(mdl->mcl);
1678
1679         if (notify) {
1680                 /*Callback to upper layer */
1681                 mdl->mcl->cb->mdl_closed(mdl, mdl->mcl->cb->user_data);
1682         }
1683
1684         return FALSE;
1685 }
1686
1687 static void mcap_connect_mdl_cb(GIOChannel *chan, GError *conn_err,
1688                                                                 gpointer data)
1689 {
1690         struct mcap_mdl_op_cb *con = data;
1691         struct mcap_mdl *mdl = con->mdl;
1692         mcap_mdl_operation_cb cb = con->cb.op;
1693         gpointer user_data = con->user_data;
1694
1695         DBG("mdl connect callback");
1696
1697         if (conn_err) {
1698                 DBG("ERROR: mdl connect callback");
1699                 mdl->state = MDL_CLOSED;
1700                 g_io_channel_unref(mdl->dc);
1701                 mdl->dc = NULL;
1702                 cb(mdl, conn_err, user_data);
1703                 return;
1704         }
1705
1706         mdl->state = MDL_CONNECTED;
1707         mdl->wid = g_io_add_watch_full(mdl->dc, G_PRIORITY_DEFAULT,
1708                                         G_IO_ERR | G_IO_HUP | G_IO_NVAL,
1709                                         (GIOFunc) mdl_event_cb,
1710                                         mcap_mdl_ref(mdl),
1711                                         (GDestroyNotify) mcap_mdl_unref);
1712
1713         cb(mdl, conn_err, user_data);
1714 }
1715
1716 gboolean mcap_connect_mdl(struct mcap_mdl *mdl, uint8_t mode,
1717                                         uint16_t dcpsm,
1718                                         mcap_mdl_operation_cb connect_cb,
1719                                         gpointer user_data,
1720                                         GDestroyNotify destroy,
1721                                         GError **err)
1722 {
1723         struct mcap_mdl_op_cb *con;
1724
1725         if (mdl->state != MDL_WAITING) {
1726                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_MDL,
1727                                         "%s", error2str(MCAP_INVALID_MDL));
1728                 return FALSE;
1729         }
1730
1731         if ((mode != BT_IO_MODE_ERTM) && (mode != BT_IO_MODE_STREAMING)) {
1732                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
1733                                                 "Invalid MDL configuration");
1734                 return FALSE;
1735         }
1736
1737         con = g_new0(struct mcap_mdl_op_cb, 1);
1738         con->mdl = mcap_mdl_ref(mdl);
1739         con->cb.op = connect_cb;
1740         con->destroy = destroy;
1741         con->user_data = user_data;
1742
1743         mdl->dc = bt_io_connect(mcap_connect_mdl_cb, con,
1744                                 (GDestroyNotify) free_mcap_mdl_op, err,
1745                                 BT_IO_OPT_SOURCE_BDADDR, &mdl->mcl->mi->src,
1746                                 BT_IO_OPT_DEST_BDADDR, &mdl->mcl->addr,
1747                                 BT_IO_OPT_PSM, dcpsm,
1748                                 BT_IO_OPT_MTU, MCAP_DC_MTU,
1749                                 BT_IO_OPT_SEC_LEVEL, mdl->mcl->mi->sec,
1750                                 BT_IO_OPT_MODE, mode,
1751                                 BT_IO_OPT_INVALID);
1752         if (!mdl->dc) {
1753                 DBG("MDL Connection error");
1754                 mdl->state = MDL_CLOSED;
1755                 mcap_mdl_unref(con->mdl);
1756                 g_free(con);
1757                 return FALSE;
1758         }
1759
1760         return TRUE;
1761 }
1762
1763 static gboolean mcl_control_cb(GIOChannel *chan, GIOCondition cond,
1764                                                                 gpointer data)
1765 {
1766         GError *gerr = NULL;
1767         struct mcap_mcl *mcl = data;
1768         int sk, len;
1769         uint8_t buf[MCAP_CC_MTU];
1770
1771         if (cond & (G_IO_ERR | G_IO_HUP | G_IO_NVAL))
1772                 goto fail;
1773
1774         sk = g_io_channel_unix_get_fd(chan);
1775         len = read(sk, buf, sizeof(buf));
1776         if (len < 0)
1777                 goto fail;
1778
1779         proc_cmd(mcl, buf, (uint32_t) len);
1780         return TRUE;
1781
1782 fail:
1783         if (mcl->state != MCL_IDLE) {
1784                 if (mcl->req == MCL_WAITING_RSP) {
1785                         /* notify error in pending callback */
1786                         g_set_error(&gerr, MCAP_ERROR, MCAP_ERROR_MCL_CLOSED,
1787                                                                 "MCL closed");
1788                         mcap_notify_error(mcl, gerr);
1789                         g_error_free(gerr);
1790                 }
1791                 mcl->mi->mcl_disconnected_cb(mcl, mcl->mi->user_data);
1792         }
1793         mcap_cache_mcl(mcl);
1794         return FALSE;
1795 }
1796
1797 static void mcap_connect_mcl_cb(GIOChannel *chan, GError *conn_err,
1798                                                         gpointer user_data)
1799 {
1800         char dstaddr[18];
1801         struct connect_mcl *con = user_data;
1802         struct mcap_mcl *aux, *mcl = con->mcl;
1803         mcap_mcl_connect_cb connect_cb = con->connect_cb;
1804         gpointer data = con->user_data;
1805         GError *gerr = NULL;
1806
1807         mcl->ctrl &= ~MCAP_CTRL_CONN;
1808
1809         if (conn_err) {
1810                 if (mcl->ctrl & MCAP_CTRL_FREE) {
1811                         mcap_mcl_release(mcl);
1812                         mcl->mi->mcl_uncached_cb(mcl, mcl->mi->user_data);
1813                 }
1814                 connect_cb(NULL, conn_err, data);
1815                 return;
1816         }
1817
1818         ba2str(&mcl->addr, dstaddr);
1819
1820         aux = find_mcl(mcl->mi->mcls, &mcl->addr);
1821         if (aux) {
1822                 /* Double MCL connection case */
1823                 error("MCL error: Device %s is already connected", dstaddr);
1824                 g_set_error(&gerr, MCAP_ERROR, MCAP_ERROR_ALREADY_EXISTS,
1825                                         "MCL %s is already connected", dstaddr);
1826                 connect_cb(NULL, gerr, data);
1827                 g_error_free(gerr);
1828                 return;
1829         }
1830
1831         mcl->state = MCL_CONNECTED;
1832         mcl->role = MCL_INITIATOR;
1833         mcl->req = MCL_AVAILABLE;
1834         mcl->ctrl |= MCAP_CTRL_STD_OP;
1835
1836         mcap_sync_init(mcl);
1837
1838         if (mcl->ctrl & MCAP_CTRL_CACHED)
1839                 mcap_uncache_mcl(mcl);
1840         else {
1841                 mcl->ctrl &= ~MCAP_CTRL_FREE;
1842                 mcl->mi->mcls = g_slist_prepend(mcl->mi->mcls,
1843                                                         mcap_mcl_ref(mcl));
1844         }
1845
1846         mcl->wid = g_io_add_watch_full(mcl->cc, G_PRIORITY_DEFAULT,
1847                                 G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
1848                                 (GIOFunc) mcl_control_cb,
1849                                 mcap_mcl_ref(mcl),
1850                                 (GDestroyNotify) mcap_mcl_unref);
1851         connect_cb(mcl, gerr, data);
1852 }
1853
1854 static void set_mdl_properties(GIOChannel *chan, struct mcap_mdl *mdl)
1855 {
1856         struct mcap_mcl *mcl = mdl->mcl;
1857
1858         mdl->state = MDL_CONNECTED;
1859         mdl->dc = g_io_channel_ref(chan);
1860         mdl->wid = g_io_add_watch_full(mdl->dc, G_PRIORITY_DEFAULT,
1861                                         G_IO_ERR | G_IO_HUP | G_IO_NVAL,
1862                                         (GIOFunc) mdl_event_cb,
1863                                         mcap_mdl_ref(mdl),
1864                                         (GDestroyNotify) mcap_mdl_unref);
1865
1866         mcl->state = MCL_ACTIVE;
1867         mcl->cb->mdl_connected(mdl, mcl->cb->user_data);
1868 }
1869
1870 static void mcl_io_destroy(gpointer data)
1871 {
1872         struct connect_mcl *con = data;
1873
1874         mcap_mcl_unref(con->mcl);
1875         if (con->destroy)
1876                 con->destroy(con->user_data);
1877         g_free(con);
1878 }
1879
1880 gboolean mcap_create_mcl(struct mcap_instance *mi,
1881                                 const bdaddr_t *addr,
1882                                 uint16_t ccpsm,
1883                                 mcap_mcl_connect_cb connect_cb,
1884                                 gpointer user_data,
1885                                 GDestroyNotify destroy,
1886                                 GError **err)
1887 {
1888         struct mcap_mcl *mcl;
1889         struct connect_mcl *con;
1890         uint16_t val;
1891
1892         mcl = find_mcl(mi->mcls, addr);
1893         if (mcl) {
1894                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_ALREADY_EXISTS,
1895                                         "MCL is already connected.");
1896                 return FALSE;
1897         }
1898
1899         mcl = find_mcl(mi->cached, addr);
1900         if (!mcl) {
1901                 mcl = g_new0(struct mcap_mcl, 1);
1902                 mcl->mi = mcap_instance_ref(mi);
1903                 mcl->state = MCL_IDLE;
1904                 bacpy(&mcl->addr, addr);
1905                 set_default_cb(mcl);
1906                 if (getrandom(&val, sizeof(val), 0) < 0) {
1907                         mcap_instance_unref(mcl->mi);
1908                         g_free(mcl);
1909                         return FALSE;
1910                 }
1911                 mcl->next_mdl = (val % MCAP_MDLID_FINAL) + 1;
1912         }
1913
1914         mcl->ctrl |= MCAP_CTRL_CONN;
1915
1916         con = g_new0(struct connect_mcl, 1);
1917         con->mcl = mcap_mcl_ref(mcl);
1918         con->connect_cb = connect_cb;
1919         con->destroy = destroy;
1920         con->user_data = user_data;
1921
1922         mcl->cc = bt_io_connect(mcap_connect_mcl_cb, con,
1923                                 mcl_io_destroy, err,
1924                                 BT_IO_OPT_SOURCE_BDADDR, &mi->src,
1925                                 BT_IO_OPT_DEST_BDADDR, addr,
1926                                 BT_IO_OPT_PSM, ccpsm,
1927                                 BT_IO_OPT_MTU, MCAP_CC_MTU,
1928                                 BT_IO_OPT_SEC_LEVEL, mi->sec,
1929                                 BT_IO_OPT_MODE, BT_IO_MODE_ERTM,
1930                                 BT_IO_OPT_INVALID);
1931         if (!mcl->cc) {
1932                 mcl->ctrl &= ~MCAP_CTRL_CONN;
1933                 if (mcl->ctrl & MCAP_CTRL_FREE) {
1934                         mcap_mcl_release(mcl);
1935                         mcl->mi->mcl_uncached_cb(mcl, mcl->mi->user_data);
1936                 }
1937                 mcap_mcl_unref(con->mcl);
1938                 g_free(con);
1939                 return FALSE;
1940         }
1941
1942         return TRUE;
1943 }
1944
1945 static void connect_dc_event_cb(GIOChannel *chan, GError *gerr,
1946                                                         gpointer user_data)
1947 {
1948         struct mcap_instance *mi = user_data;
1949         struct mcap_mcl *mcl;
1950         struct mcap_mdl *mdl;
1951         GError *err = NULL;
1952         bdaddr_t dst;
1953         GSList *l;
1954
1955         if (gerr)
1956                 return;
1957
1958         bt_io_get(chan, &err, BT_IO_OPT_DEST_BDADDR, &dst, BT_IO_OPT_INVALID);
1959         if (err) {
1960                 error("%s", err->message);
1961                 g_error_free(err);
1962                 goto drop;
1963         }
1964
1965         mcl = find_mcl(mi->mcls, &dst);
1966         if (!mcl || mcl->state != MCL_PENDING)
1967                 goto drop;
1968
1969         for (l = mcl->mdls; l; l = l->next) {
1970                 mdl = l->data;
1971                 if (mdl->state == MDL_WAITING) {
1972                         set_mdl_properties(chan, mdl);
1973                         return;
1974                 }
1975         }
1976
1977 drop:
1978         g_io_channel_shutdown(chan, TRUE, NULL);
1979 }
1980
1981 static void set_mcl_conf(GIOChannel *chan, struct mcap_mcl *mcl)
1982 {
1983         gboolean reconn;
1984
1985         mcl->state = MCL_CONNECTED;
1986         mcl->role = MCL_ACCEPTOR;
1987         mcl->req = MCL_AVAILABLE;
1988         mcl->cc = g_io_channel_ref(chan);
1989         mcl->ctrl |= MCAP_CTRL_STD_OP;
1990
1991         mcap_sync_init(mcl);
1992
1993         reconn = (mcl->ctrl & MCAP_CTRL_CACHED);
1994         if (reconn)
1995                 mcap_uncache_mcl(mcl);
1996         else
1997                 mcl->mi->mcls = g_slist_prepend(mcl->mi->mcls,
1998                                                         mcap_mcl_ref(mcl));
1999
2000         mcl->wid = g_io_add_watch_full(mcl->cc, G_PRIORITY_DEFAULT,
2001                                 G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL,
2002                                 (GIOFunc) mcl_control_cb,
2003                                 mcap_mcl_ref(mcl),
2004                                 (GDestroyNotify) mcap_mcl_unref);
2005
2006         /* Callback to report new MCL */
2007         if (reconn)
2008                 mcl->mi->mcl_reconnected_cb(mcl, mcl->mi->user_data);
2009         else
2010                 mcl->mi->mcl_connected_cb(mcl, mcl->mi->user_data);
2011 }
2012
2013 static void connect_mcl_event_cb(GIOChannel *chan, GError *gerr,
2014                                                         gpointer user_data)
2015 {
2016         struct mcap_instance *mi = user_data;
2017         struct mcap_mcl *mcl;
2018         bdaddr_t dst;
2019         char address[18], srcstr[18];
2020         GError *err = NULL;
2021         uint16_t val;
2022
2023         if (gerr)
2024                 return;
2025
2026         bt_io_get(chan, &err,
2027                         BT_IO_OPT_DEST_BDADDR, &dst,
2028                         BT_IO_OPT_DEST, address,
2029                         BT_IO_OPT_INVALID);
2030         if (err) {
2031                 error("%s", err->message);
2032                 g_error_free(err);
2033                 goto drop;
2034         }
2035
2036         ba2str(&mi->src, srcstr);
2037         mcl = find_mcl(mi->mcls, &dst);
2038         if (mcl) {
2039                 error("Control channel already created with %s on adapter %s",
2040                                 address, srcstr);
2041                 goto drop;
2042         }
2043
2044         mcl = find_mcl(mi->cached, &dst);
2045         if (!mcl) {
2046                 mcl = g_new0(struct mcap_mcl, 1);
2047                 mcl->mi = mcap_instance_ref(mi);
2048                 bacpy(&mcl->addr, &dst);
2049                 set_default_cb(mcl);
2050                 if (getrandom(&val, sizeof(val), 0) < 0) {
2051                         mcap_instance_unref(mcl->mi);
2052                         g_free(mcl);
2053                         goto drop;
2054                 }
2055                 mcl->next_mdl = (val % MCAP_MDLID_FINAL) + 1;
2056         }
2057
2058         set_mcl_conf(chan, mcl);
2059
2060         return;
2061 drop:
2062         g_io_channel_shutdown(chan, TRUE, NULL);
2063 }
2064
2065 struct mcap_instance *mcap_create_instance(const bdaddr_t *src,
2066                                         BtIOSecLevel sec,
2067                                         uint16_t ccpsm,
2068                                         uint16_t dcpsm,
2069                                         mcap_mcl_event_cb mcl_connected,
2070                                         mcap_mcl_event_cb mcl_reconnected,
2071                                         mcap_mcl_event_cb mcl_disconnected,
2072                                         mcap_mcl_event_cb mcl_uncached,
2073                                         mcap_info_ind_event_cb mcl_sync_info_ind,
2074                                         gpointer user_data,
2075                                         GError **gerr)
2076 {
2077         struct mcap_instance *mi;
2078
2079         if (sec < BT_IO_SEC_MEDIUM) {
2080                 g_set_error(gerr, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
2081                                 "Security level can't be minor of %d",
2082                                 BT_IO_SEC_MEDIUM);
2083                 return NULL;
2084         }
2085
2086         if (!(mcl_connected && mcl_reconnected &&
2087                         mcl_disconnected && mcl_uncached)) {
2088                 g_set_error(gerr, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
2089                                 "The callbacks can't be null");
2090                 return NULL;
2091         }
2092
2093         mi = g_new0(struct mcap_instance, 1);
2094
2095         bacpy(&mi->src, src);
2096
2097         mi->sec = sec;
2098         mi->mcl_connected_cb = mcl_connected;
2099         mi->mcl_reconnected_cb = mcl_reconnected;
2100         mi->mcl_disconnected_cb = mcl_disconnected;
2101         mi->mcl_uncached_cb = mcl_uncached;
2102         mi->mcl_sync_infoind_cb = mcl_sync_info_ind;
2103         mi->user_data = user_data;
2104         mi->csp_enabled = FALSE;
2105
2106         /* Listen incoming connections in control channel */
2107         mi->ccio = bt_io_listen(connect_mcl_event_cb, NULL, mi,
2108                                 NULL, gerr,
2109                                 BT_IO_OPT_SOURCE_BDADDR, &mi->src,
2110                                 BT_IO_OPT_PSM, ccpsm,
2111                                 BT_IO_OPT_MTU, MCAP_CC_MTU,
2112                                 BT_IO_OPT_SEC_LEVEL, sec,
2113                                 BT_IO_OPT_MODE, BT_IO_MODE_ERTM,
2114                                 BT_IO_OPT_INVALID);
2115         if (!mi->ccio) {
2116                 error("%s", (*gerr)->message);
2117                 g_free(mi);
2118                 return NULL;
2119         }
2120
2121         /* Listen incoming connections in data channels */
2122         mi->dcio = bt_io_listen(connect_dc_event_cb, NULL, mi,
2123                                 NULL, gerr,
2124                                 BT_IO_OPT_SOURCE_BDADDR, &mi->src,
2125                                 BT_IO_OPT_PSM, dcpsm,
2126                                 BT_IO_OPT_MTU, MCAP_DC_MTU,
2127                                 BT_IO_OPT_SEC_LEVEL, sec,
2128                                 BT_IO_OPT_INVALID);
2129         if (!mi->dcio) {
2130                 g_io_channel_shutdown(mi->ccio, TRUE, NULL);
2131                 g_io_channel_unref(mi->ccio);
2132                 mi->ccio = NULL;
2133                 error("%s", (*gerr)->message);
2134                 g_free(mi);
2135                 return NULL;
2136         }
2137
2138         /* Initialize random seed to generate mdlids for this instance */
2139         srand(time(NULL));
2140
2141         return mcap_instance_ref(mi);
2142 }
2143
2144 void mcap_release_instance(struct mcap_instance *mi)
2145 {
2146         GSList *l;
2147
2148         if (!mi)
2149                 return;
2150
2151         if (mi->ccio) {
2152                 g_io_channel_shutdown(mi->ccio, TRUE, NULL);
2153                 g_io_channel_unref(mi->ccio);
2154                 mi->ccio = NULL;
2155         }
2156
2157         if (mi->dcio) {
2158                 g_io_channel_shutdown(mi->dcio, TRUE, NULL);
2159                 g_io_channel_unref(mi->dcio);
2160                 mi->dcio = NULL;
2161         }
2162
2163         for (l = mi->mcls; l; l = l->next) {
2164                 mcap_mcl_release(l->data);
2165                 mcap_mcl_unref(l->data);
2166         }
2167
2168         g_slist_free(mi->mcls);
2169         mi->mcls = NULL;
2170
2171         for (l = mi->cached; l; l = l->next) {
2172                 mcap_mcl_release(l->data);
2173                 mcap_mcl_unref(l->data);
2174         }
2175
2176         g_slist_free(mi->cached);
2177         mi->cached = NULL;
2178 }
2179
2180 struct mcap_instance *mcap_instance_ref(struct mcap_instance *mi)
2181 {
2182         mi->ref++;
2183
2184         DBG("mcap_instance_ref(%p): ref=%d", mi, mi->ref);
2185
2186         return mi;
2187 }
2188
2189 void mcap_instance_unref(struct mcap_instance *mi)
2190 {
2191         mi->ref--;
2192
2193         DBG("mcap_instance_unref(%p): ref=%d", mi, mi->ref);
2194
2195         if (mi->ref > 0)
2196                 return;
2197
2198         mcap_release_instance(mi);
2199         g_free(mi);
2200 }
2201
2202 uint16_t mcap_get_ctrl_psm(struct mcap_instance *mi, GError **err)
2203 {
2204         uint16_t lpsm;
2205
2206         if (!(mi && mi->ccio)) {
2207                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
2208                         "Invalid MCAP instance");
2209                 return 0;
2210         }
2211
2212         if (!bt_io_get(mi->ccio, err, BT_IO_OPT_PSM, &lpsm, BT_IO_OPT_INVALID))
2213                 return 0;
2214
2215         return lpsm;
2216 }
2217
2218 uint16_t mcap_get_data_psm(struct mcap_instance *mi, GError **err)
2219 {
2220         uint16_t lpsm;
2221
2222         if (!(mi && mi->dcio)) {
2223                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
2224                         "Invalid MCAP instance");
2225                 return 0;
2226         }
2227
2228         if (!bt_io_get(mi->dcio, err, BT_IO_OPT_PSM, &lpsm, BT_IO_OPT_INVALID))
2229                 return 0;
2230
2231         return lpsm;
2232 }
2233
2234 gboolean mcap_set_data_chan_mode(struct mcap_instance *mi, uint8_t mode,
2235                                                                 GError **err)
2236 {
2237         if (!(mi && mi->dcio)) {
2238                 g_set_error(err, MCAP_ERROR, MCAP_ERROR_INVALID_ARGS,
2239                                                 "Invalid MCAP instance");
2240                 return FALSE;
2241         }
2242
2243         return bt_io_set(mi->dcio, err, BT_IO_OPT_MODE, mode,
2244                                                         BT_IO_OPT_INVALID);
2245 }
2246
2247 struct mcap_mdl *mcap_mdl_ref(struct mcap_mdl *mdl)
2248 {
2249         mdl->ref++;
2250
2251         DBG("mcap_mdl_ref(%p): ref=%d", mdl, mdl->ref);
2252
2253         return mdl;
2254 }
2255
2256 void mcap_mdl_unref(struct mcap_mdl *mdl)
2257 {
2258         mdl->ref--;
2259
2260         DBG("mcap_mdl_unref(%p): ref=%d", mdl, mdl->ref);
2261
2262         if (mdl->ref > 0)
2263                 return;
2264
2265         free_mdl(mdl);
2266 }
2267
2268
2269 static int send_sync_cmd(struct mcap_mcl *mcl, const void *buf, uint32_t size)
2270 {
2271         int sock;
2272
2273         if (mcl->cc == NULL)
2274                 return -1;
2275
2276         sock = g_io_channel_unix_get_fd(mcl->cc);
2277         return mcap_send_data(sock, buf, size);
2278 }
2279
2280 static int send_unsupported_cap_req(struct mcap_mcl *mcl)
2281 {
2282         mcap_md_sync_cap_rsp *cmd;
2283         int sent;
2284
2285         cmd = g_new0(mcap_md_sync_cap_rsp, 1);
2286         cmd->op = MCAP_MD_SYNC_CAP_RSP;
2287         cmd->rc = MCAP_REQUEST_NOT_SUPPORTED;
2288
2289         sent = send_sync_cmd(mcl, cmd, sizeof(*cmd));
2290         g_free(cmd);
2291
2292         return sent;
2293 }
2294
2295 static int send_unsupported_set_req(struct mcap_mcl *mcl)
2296 {
2297         mcap_md_sync_set_rsp *cmd;
2298         int sent;
2299
2300         cmd = g_new0(mcap_md_sync_set_rsp, 1);
2301         cmd->op = MCAP_MD_SYNC_SET_RSP;
2302         cmd->rc = MCAP_REQUEST_NOT_SUPPORTED;
2303
2304         sent = send_sync_cmd(mcl, cmd, sizeof(*cmd));
2305         g_free(cmd);
2306
2307         return sent;
2308 }
2309
2310 static void reset_tmstamp(struct mcap_csp *csp, struct timespec *base_time,
2311                                 uint64_t new_tmstamp)
2312 {
2313         csp->base_tmstamp = new_tmstamp;
2314         if (base_time)
2315                 csp->base_time = *base_time;
2316         else
2317                 clock_gettime(CLK, &csp->base_time);
2318 }
2319
2320 void mcap_sync_init(struct mcap_mcl *mcl)
2321 {
2322         if (!mcl->mi->csp_enabled) {
2323                 mcl->csp = NULL;
2324                 return;
2325         }
2326
2327         mcl->csp = g_new0(struct mcap_csp, 1);
2328
2329         mcl->csp->rem_req_acc = 10000; /* safe divisor */
2330         mcl->csp->set_data = NULL;
2331         mcl->csp->csp_priv_data = NULL;
2332
2333         reset_tmstamp(mcl->csp, NULL, 0);
2334 }
2335
2336 void mcap_sync_stop(struct mcap_mcl *mcl)
2337 {
2338         if (!mcl->csp)
2339                 return;
2340
2341         if (mcl->csp->ind_timer)
2342                 g_source_remove(mcl->csp->ind_timer);
2343
2344         if (mcl->csp->set_timer)
2345                 g_source_remove(mcl->csp->set_timer);
2346
2347         if (mcl->csp->set_data)
2348                 g_free(mcl->csp->set_data);
2349
2350         if (mcl->csp->csp_priv_data)
2351                 g_free(mcl->csp->csp_priv_data);
2352
2353         mcl->csp->ind_timer = 0;
2354         mcl->csp->set_timer = 0;
2355         mcl->csp->set_data = NULL;
2356         mcl->csp->csp_priv_data = NULL;
2357
2358         g_free(mcl->csp);
2359         mcl->csp = NULL;
2360 }
2361
2362 static uint64_t time_us(struct timespec *tv)
2363 {
2364         return tv->tv_sec * 1000000ll + tv->tv_nsec / 1000ll;
2365 }
2366
2367 static int64_t bt2us(int bt)
2368 {
2369         return bt * 312.5;
2370 }
2371
2372 static int bt2ms(int bt)
2373 {
2374         return bt * 312.5 / 1000;
2375 }
2376
2377 static int btoffset(uint32_t btclk1, uint32_t btclk2)
2378 {
2379         int offset = btclk2 - btclk1;
2380
2381         if (offset <= -MCAP_BTCLOCK_HALF)
2382                 offset += MCAP_BTCLOCK_FIELD;
2383         else if (offset > MCAP_BTCLOCK_HALF)
2384                 offset -= MCAP_BTCLOCK_FIELD;
2385
2386         return offset;
2387 }
2388
2389 static int btdiff(uint32_t btclk1, uint32_t btclk2)
2390 {
2391         return btoffset(btclk1, btclk2);
2392 }
2393
2394 static gboolean valid_btclock(uint32_t btclk)
2395 {
2396         return btclk <= MCAP_BTCLOCK_MAX;
2397 }
2398
2399 /* This call may fail; either deal with retry or use read_btclock_retry */
2400 static gboolean read_btclock(struct mcap_mcl *mcl, uint32_t *btclock,
2401                                                         uint16_t *btaccuracy)
2402 {
2403         /*
2404          * FIXME: btd_adapter_read_clock(...) always return FALSE, current
2405          * code doesn't support CSP (Clock Synchronization Protocol). To avoid
2406          * build dependancy on struct 'btd_adapter', removing this code.
2407          */
2408
2409         return FALSE;
2410 }
2411
2412 static gboolean read_btclock_retry(struct mcap_mcl *mcl, uint32_t *btclock,
2413                                                         uint16_t *btaccuracy)
2414 {
2415         int retries = 5;
2416
2417         while (--retries >= 0) {
2418                 if (read_btclock(mcl, btclock, btaccuracy))
2419                         return TRUE;
2420                 DBG("CSP: retrying to read bt clock...");
2421         }
2422
2423         return FALSE;
2424 }
2425
2426 static gboolean get_btrole(struct mcap_mcl *mcl)
2427 {
2428         int sock, flags;
2429         socklen_t len;
2430
2431         if (mcl->cc == NULL)
2432                 return -1;
2433
2434         sock = g_io_channel_unix_get_fd(mcl->cc);
2435         len = sizeof(flags);
2436
2437         if (getsockopt(sock, SOL_L2CAP, L2CAP_LM, &flags, &len))
2438                 DBG("CSP: could not read role");
2439
2440         return flags & L2CAP_LM_MASTER;
2441 }
2442
2443 uint64_t mcap_get_timestamp(struct mcap_mcl *mcl,
2444                                 struct timespec *given_time)
2445 {
2446         struct timespec now;
2447         uint64_t tmstamp;
2448
2449         if (!mcl->csp)
2450                 return MCAP_TMSTAMP_DONTSET;
2451
2452         if (given_time)
2453                 now = *given_time;
2454         else
2455                 if (clock_gettime(CLK, &now) < 0)
2456                         return MCAP_TMSTAMP_DONTSET;
2457
2458         tmstamp = time_us(&now) - time_us(&mcl->csp->base_time)
2459                 + mcl->csp->base_tmstamp;
2460
2461         return tmstamp;
2462 }
2463
2464 uint32_t mcap_get_btclock(struct mcap_mcl *mcl)
2465 {
2466         uint32_t btclock;
2467         uint16_t accuracy;
2468
2469         if (!mcl->csp)
2470                 return MCAP_BTCLOCK_IMMEDIATE;
2471
2472         if (!read_btclock_retry(mcl, &btclock, &accuracy))
2473                 btclock = 0xffffffff;
2474
2475         return btclock;
2476 }
2477
2478 static gboolean initialize_caps(struct mcap_mcl *mcl)
2479 {
2480         struct timespec t1, t2;
2481         int latencies[SAMPLE_COUNT];
2482         int latency, avg, dev;
2483         uint32_t btclock;
2484         uint16_t btaccuracy;
2485         int i;
2486         int retries;
2487
2488         clock_getres(CLK, &t1);
2489
2490         _caps.ts_res = time_us(&t1);
2491         if (_caps.ts_res < 1)
2492                 _caps.ts_res = 1;
2493
2494         _caps.ts_acc = 20; /* ppm, estimated */
2495
2496         /* A little exercise before measuing latency */
2497         clock_gettime(CLK, &t1);
2498         read_btclock_retry(mcl, &btclock, &btaccuracy);
2499
2500         /* Read clock a number of times and measure latency */
2501         avg = 0;
2502         i = 0;
2503         retries = MAX_RETRIES;
2504         while (i < SAMPLE_COUNT && retries > 0) {
2505                 clock_gettime(CLK, &t1);
2506                 if (!read_btclock(mcl, &btclock, &btaccuracy)) {
2507                         retries--;
2508                         continue;
2509                 }
2510                 clock_gettime(CLK, &t2);
2511
2512                 latency = time_us(&t2) - time_us(&t1);
2513                 latencies[i] = latency;
2514                 avg += latency;
2515                 i++;
2516         }
2517
2518         if (retries <= 0)
2519                 return FALSE;
2520
2521         /* Calculate average and deviation */
2522         avg /= SAMPLE_COUNT;
2523         dev = 0;
2524         for (i = 0; i < SAMPLE_COUNT; ++i)
2525                 dev += abs(latencies[i] - avg);
2526         dev /= SAMPLE_COUNT;
2527
2528         /* Calculate corrected average, without 'freak' latencies */
2529         latency = 0;
2530         for (i = 0; i < SAMPLE_COUNT; ++i) {
2531                 if (latencies[i] > (avg + dev * 6))
2532                         latency += avg;
2533                 else
2534                         latency += latencies[i];
2535         }
2536         latency /= SAMPLE_COUNT;
2537
2538         _caps.latency = latency;
2539         _caps.preempt_thresh = latency * 4;
2540         _caps.syncleadtime_ms = latency * 50 / 1000;
2541
2542         csp_caps_initialized = TRUE;
2543         return TRUE;
2544 }
2545
2546 static struct csp_caps *caps(struct mcap_mcl *mcl)
2547 {
2548         if (!csp_caps_initialized)
2549                 if (!initialize_caps(mcl)) {
2550                         /* Temporary failure in reading BT clock */
2551                         return NULL;
2552                 }
2553
2554         return &_caps;
2555 }
2556
2557 static int send_sync_cap_rsp(struct mcap_mcl *mcl, uint8_t rspcode,
2558                         uint8_t btclockres, uint16_t synclead,
2559                         uint16_t tmstampres, uint16_t tmstampacc)
2560 {
2561         mcap_md_sync_cap_rsp *rsp;
2562         int sent;
2563
2564         rsp = g_new0(mcap_md_sync_cap_rsp, 1);
2565
2566         rsp->op = MCAP_MD_SYNC_CAP_RSP;
2567         rsp->rc = rspcode;
2568
2569         rsp->btclock = btclockres;
2570         rsp->sltime = htons(synclead);
2571         rsp->timestnr = htons(tmstampres);
2572         rsp->timestna = htons(tmstampacc);
2573
2574         sent = send_sync_cmd(mcl, rsp, sizeof(*rsp));
2575         g_free(rsp);
2576
2577         return sent;
2578 }
2579
2580 static void proc_sync_cap_req(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
2581 {
2582         mcap_md_sync_cap_req *req;
2583         uint16_t required_accuracy;
2584         uint16_t our_accuracy;
2585         uint32_t btclock;
2586         uint16_t btres;
2587
2588         if (len != sizeof(mcap_md_sync_cap_req)) {
2589                 send_sync_cap_rsp(mcl, MCAP_INVALID_PARAM_VALUE,
2590                                         0, 0, 0, 0);
2591                 return;
2592         }
2593
2594         if (!caps(mcl)) {
2595                 send_sync_cap_rsp(mcl, MCAP_RESOURCE_UNAVAILABLE,
2596                                         0, 0, 0, 0);
2597                 return;
2598         }
2599
2600         req = (mcap_md_sync_cap_req *) cmd;
2601         required_accuracy = ntohs(req->timest);
2602         our_accuracy = caps(mcl)->ts_acc;
2603         btres = 0;
2604
2605         if (required_accuracy < our_accuracy || required_accuracy < 1) {
2606                 send_sync_cap_rsp(mcl, MCAP_RESOURCE_UNAVAILABLE,
2607                                         0, 0, 0, 0);
2608                 return;
2609         }
2610
2611         if (!read_btclock_retry(mcl, &btclock, &btres)) {
2612                 send_sync_cap_rsp(mcl, MCAP_RESOURCE_UNAVAILABLE,
2613                                         0, 0, 0, 0);
2614                 return;
2615         }
2616
2617         mcl->csp->remote_caps = 1;
2618         mcl->csp->rem_req_acc = required_accuracy;
2619
2620         send_sync_cap_rsp(mcl, MCAP_SUCCESS, btres,
2621                                 caps(mcl)->syncleadtime_ms,
2622                                 caps(mcl)->ts_res, our_accuracy);
2623 }
2624
2625 static int send_sync_set_rsp(struct mcap_mcl *mcl, uint8_t rspcode,
2626                         uint32_t btclock, uint64_t timestamp,
2627                         uint16_t tmstampres)
2628 {
2629         mcap_md_sync_set_rsp *rsp;
2630         int sent;
2631
2632         rsp = g_new0(mcap_md_sync_set_rsp, 1);
2633
2634         rsp->op = MCAP_MD_SYNC_SET_RSP;
2635         rsp->rc = rspcode;
2636         rsp->btclock = htonl(btclock);
2637         rsp->timestst = hton64(timestamp);
2638         rsp->timestsa = htons(tmstampres);
2639
2640         sent = send_sync_cmd(mcl, rsp, sizeof(*rsp));
2641         g_free(rsp);
2642
2643         return sent;
2644 }
2645
2646 static gboolean get_all_clocks(struct mcap_mcl *mcl, uint32_t *btclock,
2647                                 struct timespec *base_time,
2648                                 uint64_t *timestamp)
2649 {
2650         int latency;
2651         int retry = 5;
2652         uint16_t btres;
2653         struct timespec t0;
2654
2655         if (!caps(mcl))
2656                 return FALSE;
2657
2658         latency = caps(mcl)->preempt_thresh + 1;
2659
2660         while (latency > caps(mcl)->preempt_thresh && --retry >= 0) {
2661
2662                 if (clock_gettime(CLK, &t0) < 0)
2663                         return FALSE;
2664
2665                 if (!read_btclock(mcl, btclock, &btres))
2666                         continue;
2667
2668                 if (clock_gettime(CLK, base_time) < 0)
2669                         return FALSE;
2670
2671                 /*
2672                  * Tries to detect preemption between clock_gettime
2673                  * and read_btclock by measuring transaction time
2674                  */
2675                 latency = time_us(base_time) - time_us(&t0);
2676         }
2677
2678         if (retry < 0)
2679                 return FALSE;
2680
2681         *timestamp = mcap_get_timestamp(mcl, base_time);
2682
2683         return TRUE;
2684 }
2685
2686 static gboolean sync_send_indication(gpointer user_data)
2687 {
2688         struct mcap_mcl *mcl;
2689         mcap_md_sync_info_ind *cmd;
2690         uint32_t btclock;
2691         uint64_t tmstamp;
2692         struct timespec base_time;
2693         int sent;
2694
2695         if (!user_data)
2696                 return FALSE;
2697
2698         btclock = 0;
2699         mcl = user_data;
2700
2701         if (!caps(mcl))
2702                 return FALSE;
2703
2704         if (!get_all_clocks(mcl, &btclock, &base_time, &tmstamp))
2705                 return FALSE;
2706
2707         cmd = g_new0(mcap_md_sync_info_ind, 1);
2708
2709         cmd->op = MCAP_MD_SYNC_INFO_IND;
2710         cmd->btclock = htonl(btclock);
2711         cmd->timestst = hton64(tmstamp);
2712         cmd->timestsa = htons(caps(mcl)->latency);
2713
2714         sent = send_sync_cmd(mcl, cmd, sizeof(*cmd));
2715         g_free(cmd);
2716
2717         return !sent;
2718 }
2719
2720 static gboolean proc_sync_set_req_phase2(gpointer user_data)
2721 {
2722         struct mcap_mcl *mcl;
2723         struct sync_set_data *data;
2724         uint8_t update;
2725         uint32_t sched_btclock;
2726         uint64_t new_tmstamp;
2727         int ind_freq;
2728         int role;
2729         uint32_t btclock;
2730         uint64_t tmstamp;
2731         struct timespec base_time;
2732         uint16_t tmstampacc;
2733         gboolean reset;
2734         int delay;
2735
2736         if (!user_data)
2737                 return FALSE;
2738
2739         mcl = user_data;
2740
2741         if (!mcl->csp->set_data)
2742                 return FALSE;
2743
2744         btclock = 0;
2745         data = mcl->csp->set_data;
2746         update = data->update;
2747         sched_btclock = data->sched_btclock;
2748         new_tmstamp = data->timestamp;
2749         ind_freq = data->ind_freq;
2750         role = data->role;
2751
2752         if (!caps(mcl)) {
2753                 send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0);
2754                 return FALSE;
2755         }
2756
2757         if (!get_all_clocks(mcl, &btclock, &base_time, &tmstamp)) {
2758                 send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0);
2759                 return FALSE;
2760         }
2761
2762         if (get_btrole(mcl) != role) {
2763                 send_sync_set_rsp(mcl, MCAP_INVALID_OPERATION, 0, 0, 0);
2764                 return FALSE;
2765         }
2766
2767         reset = (new_tmstamp != MCAP_TMSTAMP_DONTSET);
2768
2769         if (reset) {
2770                 if (sched_btclock != MCAP_BTCLOCK_IMMEDIATE) {
2771                         delay = bt2us(btdiff(sched_btclock, btclock));
2772                         if (delay >= 0 || ((new_tmstamp - delay) > 0)) {
2773                                 new_tmstamp += delay;
2774                                 DBG("CSP: reset w/ delay %dus, compensated",
2775                                                                         delay);
2776                         } else
2777                                 DBG("CSP: reset w/ delay %dus, uncompensated",
2778                                                                         delay);
2779                 }
2780
2781                 reset_tmstamp(mcl->csp, &base_time, new_tmstamp);
2782                 tmstamp = new_tmstamp;
2783         }
2784
2785         tmstampacc = caps(mcl)->latency + caps(mcl)->ts_acc;
2786
2787         if (mcl->csp->ind_timer) {
2788                 g_source_remove(mcl->csp->ind_timer);
2789                 mcl->csp->ind_timer = 0;
2790         }
2791
2792         if (update) {
2793                 int when = ind_freq + caps(mcl)->syncleadtime_ms;
2794                 mcl->csp->ind_timer = g_timeout_add(when,
2795                                                 sync_send_indication,
2796                                                 mcl);
2797         }
2798
2799         send_sync_set_rsp(mcl, MCAP_SUCCESS, btclock, tmstamp, tmstampacc);
2800
2801         /* First indication after set is immediate */
2802         if (update)
2803                 sync_send_indication(mcl);
2804
2805         return FALSE;
2806 }
2807
2808 static void proc_sync_set_req(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
2809 {
2810         mcap_md_sync_set_req *req;
2811         uint32_t sched_btclock, cur_btclock;
2812         uint16_t btres;
2813         uint8_t update;
2814         uint64_t timestamp;
2815         struct sync_set_data *set_data;
2816         int phase2_delay, ind_freq, when;
2817
2818         if (len != sizeof(mcap_md_sync_set_req)) {
2819                 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0);
2820                 return;
2821         }
2822
2823         req = (mcap_md_sync_set_req *) cmd;
2824         sched_btclock = ntohl(req->btclock);
2825         update = req->timestui;
2826         timestamp = ntoh64(req->timestst);
2827         cur_btclock = 0;
2828
2829         if (sched_btclock != MCAP_BTCLOCK_IMMEDIATE &&
2830                         !valid_btclock(sched_btclock)) {
2831                 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0);
2832                 return;
2833         }
2834
2835         if (update > 1) {
2836                 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0);
2837                 return;
2838         }
2839
2840         if (!mcl->csp->remote_caps) {
2841                 /* Remote side did not ask our capabilities yet */
2842                 send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE, 0, 0, 0);
2843                 return;
2844         }
2845
2846         if (!caps(mcl)) {
2847                 send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0);
2848                 return;
2849         }
2850
2851         if (!read_btclock_retry(mcl, &cur_btclock, &btres)) {
2852                 send_sync_set_rsp(mcl, MCAP_UNSPECIFIED_ERROR, 0, 0, 0);
2853                 return;
2854         }
2855
2856         if (sched_btclock == MCAP_BTCLOCK_IMMEDIATE)
2857                 phase2_delay = 0;
2858         else {
2859                 phase2_delay = btdiff(cur_btclock, sched_btclock);
2860
2861                 if (phase2_delay < 0) {
2862                         /* can not reset in the past tense */
2863                         send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE,
2864                                                 0, 0, 0);
2865                         return;
2866                 }
2867
2868                 /* Convert to miliseconds */
2869                 phase2_delay = bt2ms(phase2_delay);
2870
2871                 if (phase2_delay > 61*1000) {
2872                         /* More than 60 seconds in the future */
2873                         send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE,
2874                                                 0, 0, 0);
2875                         return;
2876                 } else if (phase2_delay < caps(mcl)->latency / 1000) {
2877                         /* Too fast for us to do in time */
2878                         send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE,
2879                                                 0, 0, 0);
2880                         return;
2881                 }
2882         }
2883
2884         if (update) {
2885                 /*
2886                  * Indication frequency: required accuracy divided by ours
2887                  * Converted to milisseconds
2888                  */
2889                 ind_freq = (1000 * mcl->csp->rem_req_acc) / caps(mcl)->ts_acc;
2890
2891                 if (ind_freq < MAX(caps(mcl)->latency * 2 / 1000, 100)) {
2892                         /* Too frequent, we can't handle */
2893                         send_sync_set_rsp(mcl, MCAP_INVALID_PARAM_VALUE,
2894                                                 0, 0, 0);
2895                         return;
2896                 }
2897
2898                 DBG("CSP: indication every %dms", ind_freq);
2899         } else
2900                 ind_freq = 0;
2901
2902         if (mcl->csp->ind_timer) {
2903                 /* Old indications are no longer sent */
2904                 g_source_remove(mcl->csp->ind_timer);
2905                 mcl->csp->ind_timer = 0;
2906         }
2907
2908         if (!mcl->csp->set_data)
2909                 mcl->csp->set_data = g_new0(struct sync_set_data, 1);
2910
2911         set_data = (struct sync_set_data *) mcl->csp->set_data;
2912
2913         set_data->update = update;
2914         set_data->sched_btclock = sched_btclock;
2915         set_data->timestamp = timestamp;
2916         set_data->ind_freq = ind_freq;
2917         set_data->role = get_btrole(mcl);
2918
2919         /*
2920          * TODO is there some way to schedule a call based directly on
2921          * a BT clock value, instead of this estimation that uses
2922          * the SO clock?
2923          */
2924
2925         if (phase2_delay > 0) {
2926                 when = phase2_delay + caps(mcl)->syncleadtime_ms;
2927                 mcl->csp->set_timer = g_timeout_add(when,
2928                                                 proc_sync_set_req_phase2,
2929                                                 mcl);
2930         } else
2931                 proc_sync_set_req_phase2(mcl);
2932
2933         /* First indication is immediate */
2934         if (update)
2935                 sync_send_indication(mcl);
2936 }
2937
2938 static void proc_sync_cap_rsp(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
2939 {
2940         mcap_md_sync_cap_rsp *rsp;
2941         uint8_t mcap_err;
2942         uint8_t btclockres;
2943         uint16_t synclead;
2944         uint16_t tmstampres;
2945         uint16_t tmstampacc;
2946         struct mcap_sync_cap_cbdata *cbdata;
2947         mcap_sync_cap_cb cb;
2948         gpointer user_data;
2949
2950         if (mcl->csp->csp_req != MCAP_MD_SYNC_CAP_REQ) {
2951                 DBG("CSP: got unexpected cap respose");
2952                 return;
2953         }
2954
2955         if (!mcl->csp->csp_priv_data) {
2956                 DBG("CSP: no priv data for cap respose");
2957                 return;
2958         }
2959
2960         cbdata = mcl->csp->csp_priv_data;
2961         cb = cbdata->cb;
2962         user_data = cbdata->user_data;
2963         g_free(cbdata);
2964
2965         mcl->csp->csp_priv_data = NULL;
2966         mcl->csp->csp_req = 0;
2967
2968         if (len != sizeof(mcap_md_sync_cap_rsp)) {
2969                 DBG("CSP: got corrupted cap respose");
2970                 return;
2971         }
2972
2973         rsp = (mcap_md_sync_cap_rsp *) cmd;
2974         mcap_err = rsp->rc;
2975         btclockres = rsp->btclock;
2976         synclead = ntohs(rsp->sltime);
2977         tmstampres = ntohs(rsp->timestnr);
2978         tmstampacc = ntohs(rsp->timestna);
2979
2980         if (!mcap_err)
2981                 mcl->csp->local_caps = TRUE;
2982
2983         cb(mcl, mcap_err, btclockres, synclead, tmstampres, tmstampacc, NULL,
2984                                                                 user_data);
2985 }
2986
2987 static void proc_sync_set_rsp(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
2988 {
2989         mcap_md_sync_set_rsp *rsp;
2990         uint8_t mcap_err;
2991         uint32_t btclock;
2992         uint64_t timestamp;
2993         uint16_t accuracy;
2994         struct mcap_sync_set_cbdata *cbdata;
2995         mcap_sync_set_cb cb;
2996         gpointer user_data;
2997
2998         if (mcl->csp->csp_req != MCAP_MD_SYNC_SET_REQ) {
2999                 DBG("CSP: got unexpected set respose");
3000                 return;
3001         }
3002
3003         if (!mcl->csp->csp_priv_data) {
3004                 DBG("CSP: no priv data for set respose");
3005                 return;
3006         }
3007
3008         cbdata = mcl->csp->csp_priv_data;
3009         cb = cbdata->cb;
3010         user_data = cbdata->user_data;
3011         g_free(cbdata);
3012
3013         mcl->csp->csp_priv_data = NULL;
3014         mcl->csp->csp_req = 0;
3015
3016         if (len != sizeof(mcap_md_sync_set_rsp)) {
3017                 DBG("CSP: got corrupted set respose");
3018                 return;
3019         }
3020
3021         rsp = (mcap_md_sync_set_rsp *) cmd;
3022         mcap_err = rsp->rc;
3023         btclock = ntohl(rsp->btclock);
3024         timestamp = ntoh64(rsp->timestst);
3025         accuracy = ntohs(rsp->timestsa);
3026
3027         if (!mcap_err && !valid_btclock(btclock))
3028                 mcap_err = MCAP_ERROR_INVALID_ARGS;
3029
3030         cb(mcl, mcap_err, btclock, timestamp, accuracy, NULL, user_data);
3031 }
3032
3033 static void proc_sync_info_ind(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
3034 {
3035         mcap_md_sync_info_ind *req;
3036         struct sync_info_ind_data data;
3037         uint32_t btclock;
3038
3039         if (!mcl->csp->ind_expected) {
3040                 DBG("CSP: received unexpected info indication");
3041                 return;
3042         }
3043
3044         if (len != sizeof(mcap_md_sync_info_ind))
3045                 return;
3046
3047         req = (mcap_md_sync_info_ind *) cmd;
3048
3049         btclock = ntohl(req->btclock);
3050
3051         if (!valid_btclock(btclock))
3052                 return;
3053
3054         data.btclock = btclock;
3055         data.timestamp = ntoh64(req->timestst);
3056         data.accuracy = ntohs(req->timestsa);
3057
3058         if (mcl->mi->mcl_sync_infoind_cb)
3059                 mcl->mi->mcl_sync_infoind_cb(mcl, &data);
3060 }
3061
3062 void proc_sync_cmd(struct mcap_mcl *mcl, uint8_t *cmd, uint32_t len)
3063 {
3064         if (!mcl->mi->csp_enabled || !mcl->csp) {
3065                 switch (cmd[0]) {
3066                 case MCAP_MD_SYNC_CAP_REQ:
3067                         send_unsupported_cap_req(mcl);
3068                         break;
3069                 case MCAP_MD_SYNC_SET_REQ:
3070                         send_unsupported_set_req(mcl);
3071                         break;
3072                 }
3073                 return;
3074         }
3075
3076         switch (cmd[0]) {
3077         case MCAP_MD_SYNC_CAP_REQ:
3078                 proc_sync_cap_req(mcl, cmd, len);
3079                 break;
3080         case MCAP_MD_SYNC_CAP_RSP:
3081                 proc_sync_cap_rsp(mcl, cmd, len);
3082                 break;
3083         case MCAP_MD_SYNC_SET_REQ:
3084                 proc_sync_set_req(mcl, cmd, len);
3085                 break;
3086         case MCAP_MD_SYNC_SET_RSP:
3087                 proc_sync_set_rsp(mcl, cmd, len);
3088                 break;
3089         case MCAP_MD_SYNC_INFO_IND:
3090                 proc_sync_info_ind(mcl, cmd, len);
3091                 break;
3092         }
3093 }
3094
3095 void mcap_sync_cap_req(struct mcap_mcl *mcl, uint16_t reqacc,
3096                         mcap_sync_cap_cb cb, gpointer user_data,
3097                         GError **err)
3098 {
3099         struct mcap_sync_cap_cbdata *cbdata;
3100         mcap_md_sync_cap_req *cmd;
3101
3102         if (!mcl->mi->csp_enabled || !mcl->csp) {
3103                 g_set_error(err,
3104                         MCAP_CSP_ERROR,
3105                         MCAP_ERROR_RESOURCE_UNAVAILABLE,
3106                         "CSP not enabled for the instance");
3107                 return;
3108         }
3109
3110         if (mcl->csp->csp_req) {
3111                 g_set_error(err,
3112                         MCAP_CSP_ERROR,
3113                         MCAP_ERROR_RESOURCE_UNAVAILABLE,
3114                         "Pending CSP request");
3115                 return;
3116         }
3117
3118         mcl->csp->csp_req = MCAP_MD_SYNC_CAP_REQ;
3119         cmd = g_new0(mcap_md_sync_cap_req, 1);
3120
3121         cmd->op = MCAP_MD_SYNC_CAP_REQ;
3122         cmd->timest = htons(reqacc);
3123
3124         cbdata = g_new0(struct mcap_sync_cap_cbdata, 1);
3125         cbdata->cb = cb;
3126         cbdata->user_data = user_data;
3127         mcl->csp->csp_priv_data = cbdata;
3128
3129         send_sync_cmd(mcl, cmd, sizeof(*cmd));
3130
3131         g_free(cmd);
3132 }
3133
3134 void mcap_sync_set_req(struct mcap_mcl *mcl, uint8_t update, uint32_t btclock,
3135                         uint64_t timestamp, mcap_sync_set_cb cb,
3136                         gpointer user_data, GError **err)
3137 {
3138         mcap_md_sync_set_req *cmd;
3139         struct mcap_sync_set_cbdata *cbdata;
3140
3141         if (!mcl->mi->csp_enabled || !mcl->csp) {
3142                 g_set_error(err,
3143                         MCAP_CSP_ERROR,
3144                         MCAP_ERROR_RESOURCE_UNAVAILABLE,
3145                         "CSP not enabled for the instance");
3146                 return;
3147         }
3148
3149         if (!mcl->csp->local_caps) {
3150                 g_set_error(err,
3151                         MCAP_CSP_ERROR,
3152                         MCAP_ERROR_RESOURCE_UNAVAILABLE,
3153                         "Did not get CSP caps from peripheral yet");
3154                 return;
3155         }
3156
3157         if (mcl->csp->csp_req) {
3158                 g_set_error(err,
3159                         MCAP_CSP_ERROR,
3160                         MCAP_ERROR_RESOURCE_UNAVAILABLE,
3161                         "Pending CSP request");
3162                 return;
3163         }
3164
3165         mcl->csp->csp_req = MCAP_MD_SYNC_SET_REQ;
3166         cmd = g_new0(mcap_md_sync_set_req, 1);
3167
3168         cmd->op = MCAP_MD_SYNC_SET_REQ;
3169         cmd->timestui = update;
3170         cmd->btclock = htonl(btclock);
3171         cmd->timestst = hton64(timestamp);
3172
3173         mcl->csp->ind_expected = update;
3174
3175         cbdata = g_new0(struct mcap_sync_set_cbdata, 1);
3176         cbdata->cb = cb;
3177         cbdata->user_data = user_data;
3178         mcl->csp->csp_priv_data = cbdata;
3179
3180         send_sync_cmd(mcl, cmd, sizeof(*cmd));
3181
3182         g_free(cmd);
3183 }
3184
3185 void mcap_enable_csp(struct mcap_instance *mi)
3186 {
3187         mi->csp_enabled = TRUE;
3188 }
3189
3190 void mcap_disable_csp(struct mcap_instance *mi)
3191 {
3192         mi->csp_enabled = FALSE;
3193 }