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