emulator/amp.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2011-2012  Intel Corporation
   6  *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This library is free software; you can redistribute it and/or
  10  *  modify it under the terms of the GNU Lesser General Public
  11  *  License as published by the Free Software Foundation; either
  12  *  version 2.1 of the License, or (at your option) any later version.
  13  *
  14  *  This library is distributed in the hope that it will be useful,
  15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17  *  Lesser General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU Lesser General Public
  20  *  License along with this library; if not, write to the Free Software
  21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  22  *
  23  */
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include <config.h>
  27 #endif
  28
  29 #include <fcntl.h>
  30 #include <unistd.h>
  31 #include <stdlib.h>
  32 #include <string.h>
  33 #include <sys/socket.h>
  34 #include <sys/un.h>
  35
  36 #include "lib/bluetooth.h"
  37 #include "lib/hci.h"
  38
  39 #include "src/shared/util.h"
  40 #include "src/shared/mainloop.h"
  41 #include "monitor/bt.h"
  42
  43 #include "amp.h"
  44
  45 #define PHY_MODE_IDLE           0x00
  46 #define PHY_MODE_INITIATOR      0x01
  47 #define PHY_MODE_ACCEPTOR       0x02
  48
  49 #define MAX_ASSOC_LEN   672
  50
  51 struct bt_amp {
  52         volatile int ref_count;
  53         int vhci_fd;
  54
  55         char phylink_path[32];
  56         int phylink_fd;
  57
  58         uint8_t  event_mask[16];
  59         uint16_t manufacturer;
  60         uint8_t  commands[64];
  61         uint8_t  features[8];
  62
  63         uint8_t  amp_status;
  64         uint8_t  amp_type;
  65         uint8_t  local_assoc[MAX_ASSOC_LEN];
  66         uint16_t local_assoc_len;
  67         uint8_t  remote_assoc[MAX_ASSOC_LEN];
  68         uint16_t remote_assoc_len;
  69
  70         uint8_t  phy_mode;
  71         uint8_t  phy_handle;
  72         uint16_t logic_handle;
  73 };
  74
  75 static void reset_defaults(struct bt_amp *amp)
  76 {
  77         memset(amp->event_mask, 0, sizeof(amp->event_mask));
  78         amp->event_mask[1] |= 0x20;     /* Command Complete */
  79         amp->event_mask[1] |= 0x40;     /* Command Status */
  80         amp->event_mask[1] |= 0x80;     /* Hardware Error */
  81         amp->event_mask[2] |= 0x01;     /* Flush Occurred */
  82         amp->event_mask[2] |= 0x04;     /* Number of Completed Packets */
  83         amp->event_mask[3] |= 0x02;     /* Data Buffer Overflow */
  84         amp->event_mask[3] |= 0x20;     /* QoS Violation */
  85         amp->event_mask[7] |= 0x01;     /* Enhanced Flush Complete */
  86
  87         amp->event_mask[8] |= 0x01;     /* Physical Link Complete */
  88         amp->event_mask[8] |= 0x02;     /* Channel Selected */
  89         amp->event_mask[8] |= 0x04;     /* Disconnection Physical Link Complete */
  90         amp->event_mask[8] |= 0x08;     /* Physical Link Loss Early Warning */
  91         amp->event_mask[8] |= 0x10;     /* Physical Link Recovery */
  92         amp->event_mask[8] |= 0x20;     /* Logical Link Complete */
  93         amp->event_mask[8] |= 0x40;     /* Disconection Logical Link Complete */
  94         amp->event_mask[8] |= 0x80;     /* Flow Specification Modify Complete */
  95         amp->event_mask[9] |= 0x01;     /* Number of Completed Data Blocks */
  96         amp->event_mask[9] |= 0x02;     /* AMP Start Test */
  97         amp->event_mask[9] |= 0x04;     /* AMP Test End */
  98         amp->event_mask[9] |= 0x08;     /* AMP Receiver Report */
  99         amp->event_mask[9] |= 0x10;     /* Short Range Mode Change Complete */
 100         amp->event_mask[9] |= 0x20;     /* AMP Status Change */
 101
 102         amp->manufacturer = 0x003f;     /* Bluetooth SIG (63) */
 103
 104         memset(amp->commands, 0, sizeof(amp->commands));
 105         amp->commands[5]  |= 0x40;      /* Set Event Mask */
 106         amp->commands[5]  |= 0x80;      /* Reset */
 107         //amp->commands[6]  |= 0x01;    /* Set Event Filter */
 108         //amp->commands[7]  |= 0x04;    /* Read Connection Accept Timeout */
 109         //amp->commands[7]  |= 0x08;    /* Write Connection Accept Timeout */
 110         //amp->commands[10] |= 0x80;    /* Host Number of Completed Packets */
 111         //amp->commands[11] |= 0x01;    /* Read Link Supervision Timeout */
 112         //amp->commands[11] |= 0x02;    /* Write Link Supervision Timeout */
 113         amp->commands[14] |= 0x08;      /* Read Local Version Information */
 114         amp->commands[14] |= 0x10;      /* Read Local Supported Commands */
 115         amp->commands[14] |= 0x20;      /* Read Local Supported Features */
 116         amp->commands[14] |= 0x80;      /* Read Buffer Size */
 117         //amp->commands[15] |= 0x04;    /* Read Failed Contact Counter */
 118         //amp->commands[15] |= 0x08;    /* Reset Failed Contact Counter */
 119         //amp->commands[15] |= 0x10;    /* Read Link Quality */
 120         //amp->commands[15] |= 0x20;    /* Read RSSI */
 121         //amp->commands[16] |= 0x04;    /* Enable Device Under Test Mode */
 122         //amp->commands[19] |= 0x40;    /* Enhanced Flush */
 123
 124         amp->commands[21] |= 0x01;      /* Create Physical Link */
 125         amp->commands[21] |= 0x02;      /* Accept Physical Link */
 126         amp->commands[21] |= 0x04;      /* Disconnect Phyiscal Link */
 127         amp->commands[21] |= 0x08;      /* Create Logical Link */
 128         amp->commands[21] |= 0x10;      /* Accept Logical Link */
 129         amp->commands[21] |= 0x20;      /* Disconnect Logical Link */
 130         amp->commands[21] |= 0x40;      /* Logical Link Cancel */
 131         //amp->commands[21] |= 0x80;    /* Flow Specification Modify */
 132         //amp->commands[22] |= 0x01;    /* Read Logical Link Accept Timeout */
 133         //amp->commands[22] |= 0x02;    /* Write Logical Link Accept Timeout */
 134         amp->commands[22] |= 0x04;      /* Set Event Mask Page 2 */
 135         amp->commands[22] |= 0x08;      /* Read Location Data */
 136         amp->commands[22] |= 0x10;      /* Write Location Data */
 137         amp->commands[22] |= 0x20;      /* Read Local AMP Info */
 138         amp->commands[22] |= 0x40;      /* Read Local AMP ASSOC */
 139         amp->commands[22] |= 0x80;      /* Write Remote AMP ASSOC */
 140         amp->commands[23] |= 0x01;      /* Read Flow Control Mode */
 141         amp->commands[23] |= 0x02;      /* Write Flow Control Mode */
 142         amp->commands[23] |= 0x04;      /* Read Data Block Size */
 143         //amp->commands[23] |= 0x20;    /* Enable AMP Receiver Reports */
 144         //amp->commands[23] |= 0x40;    /* AMP Test End */
 145         //amp->commands[23] |= 0x80;    /* AMP Test */
 146         //amp->commands[24] |= 0x04;    /* Read Best Effort Flush Timeout */
 147         //amp->commands[24] |= 0x08;    /* Write Best Effort Flush Timeout */
 148         //amp->commands[24] |= 0x10;    /* Short Range Mode */
 149
 150         memset(amp->features, 0, sizeof(amp->features));
 151
 152         amp->amp_status = 0x01;         /* Used for Bluetooth only */
 153         amp->amp_type = 0x42;           /* Fake virtual AMP type */
 154
 155         memset(amp->local_assoc, 0, sizeof(amp->local_assoc));
 156         amp->local_assoc_len = 0;
 157
 158         memset(amp->remote_assoc, 0, sizeof(amp->remote_assoc));
 159         amp->remote_assoc_len = 0;
 160
 161         amp->phy_mode = PHY_MODE_IDLE;
 162         amp->phy_handle = 0x00;         /* Invalid physical link handle */
 163         amp->logic_handle = 0x0000;
 164 }
 165
 166 static void send_packet(struct bt_amp *amp, const void *data, uint16_t len)
 167 {
 168         if (write(amp->vhci_fd, data, len) < 0)
 169                 fprintf(stderr, "Write to /dev/vhci failed\n");
 170 }
 171
 172 static void send_event(struct bt_amp *amp, uint8_t event,
 173                                                 const void *data, uint8_t len)
 174 {
 175         struct bt_hci_evt_hdr *hdr;
 176         uint16_t pkt_len;
 177         void *pkt_data;
 178
 179         pkt_len = 1 + sizeof(*hdr) + len;
 180
 181         pkt_data = alloca(pkt_len);
 182         if (!pkt_data)
 183                 return;
 184
 185         ((uint8_t *) pkt_data)[0] = BT_H4_EVT_PKT;
 186
 187         hdr = pkt_data + 1;
 188         hdr->evt = event;
 189         hdr->plen = len;
 190
 191         if (len > 0)
 192                 memcpy(pkt_data + 1 + sizeof(*hdr), data, len);
 193
 194         send_packet(amp, pkt_data, pkt_len);
 195 }
 196
 197 static void cmd_complete(struct bt_amp *amp, uint16_t opcode,
 198                                                 const void *data, uint8_t len)
 199 {
 200         struct bt_hci_evt_hdr *hdr;
 201         struct bt_hci_evt_cmd_complete *cc;
 202         uint16_t pkt_len;
 203         void *pkt_data;
 204
 205         pkt_len = 1 + sizeof(*hdr) + sizeof(*cc) + len;
 206
 207         pkt_data = alloca(pkt_len);
 208         if (!pkt_data)
 209                 return;
 210
 211         ((uint8_t *) pkt_data)[0] = BT_H4_EVT_PKT;
 212
 213         hdr = pkt_data + 1;
 214         hdr->evt = BT_HCI_EVT_CMD_COMPLETE;
 215         hdr->plen = sizeof(*cc) + len;
 216
 217         cc = pkt_data + 1 + sizeof(*hdr);
 218         cc->ncmd = 0x01;
 219         cc->opcode = cpu_to_le16(opcode);
 220
 221         if (len > 0)
 222                 memcpy(pkt_data + 1 + sizeof(*hdr) + sizeof(*cc), data, len);
 223
 224         send_packet(amp, pkt_data, pkt_len);
 225 }
 226
 227 static void cmd_status(struct bt_amp *amp, uint8_t status, uint16_t opcode)
 228 {
 229         struct bt_hci_evt_hdr *hdr;
 230         struct bt_hci_evt_cmd_status *cs;
 231         uint16_t pkt_len;
 232         void *pkt_data;
 233
 234         pkt_len = 1 + sizeof(*hdr) + sizeof(*cs);
 235
 236         pkt_data = alloca(pkt_len);
 237         if (!pkt_data)
 238                 return;
 239
 240         ((uint8_t *) pkt_data)[0] = BT_H4_EVT_PKT;
 241
 242         hdr = pkt_data + 1;
 243         hdr->evt = BT_HCI_EVT_CMD_STATUS;
 244         hdr->plen = sizeof(*cs);
 245
 246         cs = pkt_data + 1 + sizeof(*hdr);
 247         cs->status = status;
 248         cs->ncmd = 0x01;
 249         cs->opcode = cpu_to_le16(opcode);
 250
 251         send_packet(amp, pkt_data, pkt_len);
 252 }
 253
 254 static void cmd_set_event_mask(struct bt_amp *amp,
 255                                                 const void *data, uint8_t size)
 256 {
 257         const struct bt_hci_cmd_set_event_mask *cmd = data;
 258         uint8_t status;
 259
 260         memcpy(amp->event_mask, cmd->mask, 8);
 261
 262         status = BT_HCI_ERR_SUCCESS;
 263         cmd_complete(amp, BT_HCI_CMD_SET_EVENT_MASK, &status, sizeof(status));
 264 }
 265
 266 static void cmd_reset(struct bt_amp *amp, const void *data, uint8_t size)
 267 {
 268         uint8_t status;
 269
 270         reset_defaults(amp);
 271
 272         amp->local_assoc[0] = 0x00;
 273         amp->local_assoc_len = 1;
 274
 275         status = BT_HCI_ERR_SUCCESS;
 276         cmd_complete(amp, BT_HCI_CMD_RESET, &status, sizeof(status));
 277 }
 278
 279 static void cmd_read_local_version(struct bt_amp *amp,
 280                                                 const void *data, uint8_t size)
 281 {
 282         struct bt_hci_rsp_read_local_version rsp;
 283
 284         rsp.status = BT_HCI_ERR_SUCCESS;
 285         rsp.hci_ver = 0x05;
 286         rsp.hci_rev = cpu_to_le16(0x0000);
 287         rsp.lmp_ver = 0x01;
 288         rsp.manufacturer = cpu_to_le16(amp->manufacturer);
 289         rsp.lmp_subver = cpu_to_le16(0x0000);
 290
 291         cmd_complete(amp, BT_HCI_CMD_READ_LOCAL_VERSION, &rsp, sizeof(rsp));
 292 }
 293
 294 static void cmd_read_local_commands(struct bt_amp *amp,
 295                                                 const void *data, uint8_t size)
 296 {
 297         struct bt_hci_rsp_read_local_commands rsp;
 298
 299         rsp.status = BT_HCI_ERR_SUCCESS;
 300         memcpy(rsp.commands, amp->commands, 64);
 301
 302         cmd_complete(amp, BT_HCI_CMD_READ_LOCAL_COMMANDS, &rsp, sizeof(rsp));
 303 }
 304
 305 static void cmd_read_local_features(struct bt_amp *amp,
 306                                                 const void *data, uint8_t size)
 307 {
 308         struct bt_hci_rsp_read_local_features rsp;
 309
 310         rsp.status = BT_HCI_ERR_SUCCESS;
 311         memcpy(rsp.features, amp->features, 8);
 312
 313         cmd_complete(amp, BT_HCI_CMD_READ_LOCAL_FEATURES, &rsp, sizeof(rsp));
 314 }
 315
 316 static void cmd_read_buffer_size(struct bt_amp *amp,
 317                                                 const void *data, uint8_t size)
 318 {
 319         struct bt_hci_rsp_read_buffer_size rsp;
 320
 321         rsp.status = BT_HCI_ERR_SUCCESS;
 322         rsp.acl_mtu = cpu_to_le16(0x0000);
 323         rsp.sco_mtu = 0x00;
 324         rsp.acl_max_pkt = cpu_to_le16(0x0000);
 325         rsp.sco_max_pkt = cpu_to_le16(0x0000);
 326
 327         cmd_complete(amp, BT_HCI_CMD_READ_BUFFER_SIZE, &rsp, sizeof(rsp));
 328 }
 329
 330 static void evt_phy_link_complete(struct bt_amp *amp)
 331 {
 332         struct bt_hci_evt_phy_link_complete evt;
 333
 334         evt.status = BT_HCI_ERR_SUCCESS;
 335         evt.phy_handle = amp->phy_handle;
 336
 337         send_event(amp, BT_HCI_EVT_PHY_LINK_COMPLETE, &evt, sizeof(evt));
 338 }
 339
 340 static void evt_disconn_phy_link_complete(struct bt_amp *amp, uint8_t reason)
 341 {
 342         struct bt_hci_evt_disconn_phy_link_complete evt;
 343
 344         evt.status = BT_HCI_ERR_SUCCESS;
 345         evt.phy_handle = amp->phy_handle;
 346         evt.reason = reason;
 347
 348         send_event(amp, BT_HCI_EVT_DISCONN_PHY_LINK_COMPLETE,
 349                                                 &evt, sizeof(evt));
 350 }
 351
 352 static void link_callback(int fd, uint32_t events, void *user_data)
 353 {
 354         struct bt_amp *amp = user_data;
 355
 356         if (events & (EPOLLERR | EPOLLHUP)) {
 357                 close(fd);
 358                 mainloop_remove_fd(fd);
 359
 360                 evt_disconn_phy_link_complete(amp, 0x13);
 361
 362                 amp->phy_mode = PHY_MODE_IDLE;
 363                 amp->phy_handle = 0x00;
 364                 return;
 365         }
 366 }
 367
 368 static void cmd_create_phy_link(struct bt_amp *amp,
 369                                                 const void *data, uint8_t size)
 370 {
 371         const struct bt_hci_cmd_create_phy_link *cmd = data;
 372
 373         if (cmd->phy_handle == 0x00) {
 374                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 375                                         BT_HCI_CMD_CREATE_PHY_LINK);
 376                 return;
 377         }
 378
 379         if (amp->phy_mode != PHY_MODE_IDLE) {
 380                 cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
 381                                         BT_HCI_CMD_CREATE_PHY_LINK);
 382                 return;
 383         }
 384
 385         amp->phy_mode = PHY_MODE_INITIATOR;
 386         amp->phy_handle = cmd->phy_handle;
 387
 388         cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_CREATE_PHY_LINK);
 389 }
 390
 391 static void cmd_accept_phy_link(struct bt_amp *amp,
 392                                                 const void *data, uint8_t size)
 393 {
 394         const struct bt_hci_cmd_accept_phy_link *cmd = data;
 395
 396         if (cmd->phy_handle == 0x00) {
 397                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 398                                         BT_HCI_CMD_ACCEPT_PHY_LINK);
 399                 return;
 400         }
 401
 402         if (amp->phy_mode != PHY_MODE_IDLE) {
 403                 cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
 404                                         BT_HCI_CMD_ACCEPT_PHY_LINK);
 405                 return;
 406         }
 407
 408         amp->phy_mode = PHY_MODE_ACCEPTOR;
 409         amp->phy_handle = cmd->phy_handle;
 410
 411         cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_ACCEPT_PHY_LINK);
 412 }
 413
 414 static void cmd_disconn_phy_link(struct bt_amp *amp,
 415                                                 const void *data, uint8_t size)
 416 {
 417         const struct bt_hci_cmd_disconn_phy_link *cmd = data;
 418
 419         if (cmd->phy_handle == 0x00) {
 420                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 421                                         BT_HCI_CMD_DISCONN_PHY_LINK);
 422                 return;
 423         }
 424
 425         if (amp->phy_mode == PHY_MODE_IDLE) {
 426                 cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
 427                                         BT_HCI_CMD_DISCONN_PHY_LINK);
 428                 return;
 429         }
 430
 431         if (cmd->phy_handle != amp->phy_handle) {
 432                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 433                                         BT_HCI_CMD_DISCONN_PHY_LINK);
 434                 return;
 435         }
 436
 437         cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_DISCONN_PHY_LINK);
 438
 439         mainloop_remove_fd(amp->phylink_fd);
 440         close(amp->phylink_fd);
 441
 442         evt_disconn_phy_link_complete(amp, cmd->reason);
 443
 444         amp->phy_mode = PHY_MODE_IDLE;
 445         amp->phy_handle = 0x00;
 446 }
 447
 448 static void evt_logic_link_complete(struct bt_amp *amp)
 449 {
 450         struct bt_hci_evt_logic_link_complete evt;
 451
 452         evt.status = BT_HCI_ERR_SUCCESS;
 453         evt.handle = htobs(amp->logic_handle);
 454         evt.phy_handle = amp->phy_handle;
 455         evt.flow_spec = 0x00;
 456
 457         send_event(amp, BT_HCI_EVT_LOGIC_LINK_COMPLETE, &evt, sizeof(evt));
 458 }
 459
 460 static void evt_disconn_logic_link_complete(struct bt_amp *amp, uint8_t reason)
 461 {
 462         struct bt_hci_evt_disconn_logic_link_complete evt;
 463
 464         evt.status = BT_HCI_ERR_SUCCESS;
 465         evt.handle = htobs(amp->logic_handle);
 466         evt.reason = reason;
 467
 468         send_event(amp, BT_HCI_EVT_DISCONN_LOGIC_LINK_COMPLETE,
 469                                                 &evt, sizeof(evt));
 470 }
 471
 472 static void cmd_create_logic_link(struct bt_amp *amp,
 473                                                 const void *data, uint8_t size)
 474 {
 475         const struct bt_hci_cmd_create_logic_link *cmd = data;
 476
 477         if (cmd->phy_handle == 0x00) {
 478                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 479                                         BT_HCI_CMD_CREATE_LOGIC_LINK);
 480                 return;
 481         }
 482
 483         if (amp->phy_mode != PHY_MODE_IDLE) {
 484                 cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
 485                                         BT_HCI_CMD_CREATE_LOGIC_LINK);
 486                 return;
 487         }
 488
 489         if (amp->logic_handle != 0x00) {
 490                 cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
 491                                         BT_HCI_CMD_CREATE_LOGIC_LINK);
 492                 return;
 493         }
 494
 495         cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_CREATE_LOGIC_LINK);
 496
 497         amp->logic_handle = 0x0042;
 498
 499         evt_logic_link_complete(amp);
 500 }
 501
 502 static void cmd_accept_logic_link(struct bt_amp *amp,
 503                                                 const void *data, uint8_t size)
 504 {
 505         const struct bt_hci_cmd_accept_logic_link *cmd = data;
 506
 507         if (cmd->phy_handle == 0x00) {
 508                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 509                                         BT_HCI_CMD_ACCEPT_LOGIC_LINK);
 510                 return;
 511         }
 512
 513         if (amp->phy_mode != PHY_MODE_IDLE) {
 514                 cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
 515                                         BT_HCI_CMD_ACCEPT_LOGIC_LINK);
 516                 return;
 517         }
 518
 519         if (amp->logic_handle != 0x00) {
 520                 cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
 521                                         BT_HCI_CMD_ACCEPT_LOGIC_LINK);
 522                 return;
 523         }
 524
 525         cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_ACCEPT_LOGIC_LINK);
 526
 527         amp->logic_handle = 0x0023;
 528
 529         evt_logic_link_complete(amp);
 530 }
 531
 532 static void cmd_disconn_logic_link(struct bt_amp *amp,
 533                                                 const void *data, uint8_t size)
 534 {
 535         const struct bt_hci_cmd_disconn_logic_link *cmd = data;
 536
 537         if (cmd->handle == 0x00) {
 538                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 539                                         BT_HCI_CMD_DISCONN_LOGIC_LINK);
 540                 return;
 541         }
 542
 543         if (cmd->handle != amp->logic_handle) {
 544                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 545                                         BT_HCI_CMD_DISCONN_LOGIC_LINK);
 546                 return;
 547         }
 548
 549         cmd_status(amp, BT_HCI_ERR_SUCCESS, BT_HCI_CMD_DISCONN_LOGIC_LINK);
 550
 551         evt_disconn_logic_link_complete(amp, 0x13);
 552
 553         amp->logic_handle = 0x0000;
 554 }
 555
 556 static void cmd_logic_link_cancel(struct bt_amp *amp,
 557                                                 const void *data, uint8_t size)
 558 {
 559         const struct bt_hci_cmd_logic_link_cancel *cmd = data;
 560         struct bt_hci_rsp_logic_link_cancel rsp;
 561
 562         if (cmd->phy_handle == 0x00) {
 563                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 564                                         BT_HCI_CMD_LOGIC_LINK_CANCEL);
 565                 return;
 566         }
 567
 568         if (amp->phy_mode != PHY_MODE_IDLE) {
 569                 cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
 570                                         BT_HCI_CMD_LOGIC_LINK_CANCEL);
 571                 return;
 572         }
 573
 574         amp->logic_handle = 0x0000;
 575
 576         rsp.status = BT_HCI_ERR_SUCCESS;
 577         rsp.phy_handle = amp->phy_handle;
 578         rsp.flow_spec = 0x00;
 579
 580         cmd_complete(amp, BT_HCI_CMD_LOGIC_LINK_CANCEL, &rsp, sizeof(rsp));
 581 }
 582
 583 static void cmd_set_event_mask_page2(struct bt_amp *amp,
 584                                                 const void *data, uint8_t size)
 585 {
 586         const struct bt_hci_cmd_set_event_mask_page2 *cmd = data;
 587         uint8_t status;
 588
 589         memcpy(amp->event_mask + 8, cmd->mask, 8);
 590
 591         status = BT_HCI_ERR_SUCCESS;
 592         cmd_complete(amp, BT_HCI_CMD_SET_EVENT_MASK_PAGE2,
 593                                                 &status, sizeof(status));
 594 }
 595
 596 static void cmd_read_location_data(struct bt_amp *amp,
 597                                                 const void *data, uint8_t size)
 598 {
 599         struct bt_hci_rsp_read_location_data rsp;
 600
 601         rsp.status = BT_HCI_ERR_SUCCESS;
 602         rsp.domain_aware = 0x00;
 603         rsp.domain[0] = 0x58;
 604         rsp.domain[1] = 0x58;
 605         rsp.domain_options = 0x58;
 606         rsp.options = 0x00;
 607
 608         cmd_complete(amp, BT_HCI_CMD_READ_LOCATION_DATA, &rsp, sizeof(rsp));
 609 }
 610
 611 static void cmd_write_location_data(struct bt_amp *amp,
 612                                                 const void *data, uint8_t size)
 613 {
 614         const struct bt_hci_cmd_write_location_data *cmd = data;
 615         uint8_t status;
 616
 617         if (cmd->domain_aware > 0x01) {
 618                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 619                                         BT_HCI_CMD_WRITE_LOCATION_DATA);
 620                 return;
 621         }
 622
 623         status = BT_HCI_ERR_SUCCESS;
 624         cmd_complete(amp, BT_HCI_CMD_WRITE_LOCATION_DATA,
 625                                                 &status, sizeof(status));
 626 }
 627
 628 static void cmd_read_flow_control_mode(struct bt_amp *amp,
 629                                                 const void *data, uint8_t size)
 630 {
 631         struct bt_hci_rsp_read_flow_control_mode rsp;
 632
 633         rsp.status = BT_HCI_ERR_SUCCESS;
 634         rsp.mode = 0x01;
 635
 636         cmd_complete(amp, BT_HCI_CMD_READ_FLOW_CONTROL_MODE,
 637                                                 &rsp, sizeof(rsp));
 638 }
 639
 640 static void cmd_write_flow_control_mode(struct bt_amp *amp,
 641                                                 const void *data, uint8_t size)
 642 {
 643         const struct bt_hci_cmd_write_flow_control_mode *cmd = data;
 644         uint8_t status;
 645
 646         if (cmd->mode != 0x01) {
 647                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 648                                         BT_HCI_CMD_WRITE_FLOW_CONTROL_MODE);
 649                 return;
 650         }
 651
 652         status = BT_HCI_ERR_SUCCESS;
 653         cmd_complete(amp, BT_HCI_CMD_WRITE_FLOW_CONTROL_MODE,
 654                                                 &status, sizeof(status));
 655 }
 656
 657 static void cmd_read_data_block_size(struct bt_amp *amp,
 658                                                 const void *data, uint8_t size)
 659 {
 660         struct bt_hci_rsp_read_data_block_size rsp;
 661
 662         rsp.status = BT_HCI_ERR_SUCCESS;
 663         rsp.max_acl_len = cpu_to_le16(1492);
 664         rsp.block_len = cpu_to_le16(1492);
 665         rsp.num_blocks = cpu_to_le16(1);
 666
 667         cmd_complete(amp, BT_HCI_CMD_READ_DATA_BLOCK_SIZE, &rsp, sizeof(rsp));
 668 }
 669
 670 static void cmd_read_local_amp_info(struct bt_amp *amp,
 671                                                 const void *data, uint8_t size)
 672 {
 673         struct bt_hci_rsp_read_local_amp_info rsp;
 674
 675         rsp.status = BT_HCI_ERR_SUCCESS;
 676         rsp.amp_status = amp->amp_status;
 677         rsp.total_bw = cpu_to_le32(24000);
 678         rsp.max_bw = cpu_to_le32(24000);
 679         rsp.min_latency = cpu_to_le32(100);
 680         rsp.max_pdu = cpu_to_le32(1492);
 681         rsp.amp_type = amp->amp_type;
 682         rsp.pal_cap = cpu_to_le16(0x0001);
 683         rsp.max_assoc_len = cpu_to_le16(MAX_ASSOC_LEN);
 684         rsp.max_flush_to = cpu_to_le32(20000);
 685         rsp.be_flush_to = cpu_to_le32(20000);
 686
 687         cmd_complete(amp, BT_HCI_CMD_READ_LOCAL_AMP_INFO, &rsp, sizeof(rsp));
 688 }
 689
 690 static void cmd_read_local_amp_assoc(struct bt_amp *amp,
 691                                                 const void *data, uint8_t size)
 692 {
 693         const struct bt_hci_cmd_read_local_amp_assoc *cmd = data;
 694         struct bt_hci_rsp_read_local_amp_assoc rsp;
 695         uint16_t len_so_far, remain_assoc_len, fragment_len;
 696
 697         if (cmd->phy_handle != amp->phy_handle) {
 698                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 699                                         BT_HCI_CMD_READ_LOCAL_AMP_ASSOC);
 700                 return;
 701         }
 702
 703         len_so_far = le16_to_cpu(cmd->len_so_far);
 704         remain_assoc_len = amp->local_assoc_len - len_so_far;
 705         fragment_len = remain_assoc_len > 248 ? 248 : remain_assoc_len;
 706
 707         rsp.status = BT_HCI_ERR_SUCCESS;
 708         rsp.phy_handle = cmd->phy_handle;
 709         rsp.remain_assoc_len = cpu_to_le16(remain_assoc_len);
 710         memcpy(rsp.assoc_fragment, amp->local_assoc + len_so_far,
 711                                                         fragment_len);
 712
 713         cmd_complete(amp, BT_HCI_CMD_READ_LOCAL_AMP_ASSOC,
 714                                                 &rsp, 4 + fragment_len);
 715 }
 716
 717 static int create_unix_server(const char *path)
 718 {
 719         struct sockaddr_un addr;
 720         int fd;
 721
 722         fd = socket(PF_UNIX, SOCK_SEQPACKET, 0);
 723         if (fd < 0)
 724                 return -1;
 725
 726         memset(&addr, 0, sizeof(addr));
 727         addr.sun_family = AF_UNIX;
 728         addr.sun_path[0] = '\0';
 729         strcpy(addr.sun_path + 1, path);
 730
 731         if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 732                 close(fd);
 733                 return -1;
 734         }
 735
 736         if (listen(fd, 1) < 0) {
 737                 close(fd);
 738                 return -1;
 739         }
 740
 741         return fd;
 742 }
 743
 744 static int connect_unix_client(const char *path)
 745 {
 746         struct sockaddr_un addr;
 747         int fd;
 748
 749         fd = socket(PF_UNIX, SOCK_SEQPACKET | SOCK_CLOEXEC | SOCK_NONBLOCK, 0);
 750         if (fd < 0)
 751                 return -1;
 752
 753         memset(&addr, 0, sizeof(addr));
 754         addr.sun_family = AF_UNIX;
 755         addr.sun_path[0] = '\0';
 756         strcpy(addr.sun_path + 1, path);
 757
 758         if (connect(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 759                 close(fd);
 760                 return -1;
 761         }
 762
 763         return fd;
 764 }
 765
 766 static void accept_callback(int fd, uint32_t events, void *user_data)
 767 {
 768         struct bt_amp *amp = user_data;
 769         struct sockaddr_un addr;
 770         socklen_t len;
 771         int new_fd;
 772
 773         if (events & (EPOLLERR | EPOLLHUP)) {
 774                 mainloop_remove_fd(fd);
 775                 return;
 776         }
 777
 778         memset(&addr, 0, sizeof(addr));
 779         len = sizeof(addr);
 780
 781         new_fd = accept4(fd, (struct sockaddr *) &addr, &len,
 782                                                 SOCK_CLOEXEC | SOCK_NONBLOCK);
 783         if (new_fd < 0)
 784                 return;
 785
 786         mainloop_remove_fd(fd);
 787         close(fd);
 788
 789         amp->phylink_fd = new_fd;
 790
 791         evt_phy_link_complete(amp);
 792
 793         mainloop_add_fd(new_fd, EPOLLIN, link_callback, amp, NULL);
 794 }
 795
 796 static void connect_callback(int fd, uint32_t events, void *user_data)
 797 {
 798         struct bt_amp *amp = user_data;
 799
 800         if (events & (EPOLLERR | EPOLLHUP)) {
 801                 mainloop_remove_fd(fd);
 802                 return;
 803         }
 804
 805         mainloop_remove_fd(fd);
 806
 807         evt_phy_link_complete(amp);
 808
 809         mainloop_add_fd(fd, EPOLLIN, link_callback, amp, NULL);
 810 }
 811
 812 static void cmd_write_remote_amp_assoc(struct bt_amp *amp,
 813                                                 const void *data, uint8_t size)
 814 {
 815         const struct bt_hci_cmd_write_remote_amp_assoc *cmd = data;
 816         struct bt_hci_rsp_write_remote_amp_assoc rsp;
 817         int fd;
 818
 819         if (cmd->phy_handle == 0x00) {
 820                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 821                                         BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
 822                 return;
 823         }
 824
 825         if (cmd->phy_handle != amp->phy_handle) {
 826                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS,
 827                                         BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
 828                 return;
 829         }
 830
 831         switch (amp->phy_mode) {
 832         case PHY_MODE_INITIATOR:
 833                 strcpy(amp->phylink_path, "amp");
 834
 835                 fd = create_unix_server(amp->phylink_path);
 836                 if (fd < 0) {
 837                         cmd_status(amp, BT_HCI_ERR_UNSPECIFIED_ERROR,
 838                                         BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
 839                         return;
 840                 }
 841
 842                 amp->local_assoc[0] = 0x01;
 843                 memcpy(amp->local_assoc + 1, amp->phylink_path,
 844                                         strlen(amp->phylink_path) + 1);
 845                 amp->local_assoc_len = strlen(amp->phylink_path) + 2;
 846
 847                 mainloop_add_fd(fd, EPOLLIN, accept_callback, amp, NULL);
 848
 849                 amp->phylink_fd = fd;
 850                 break;
 851
 852         case PHY_MODE_ACCEPTOR:
 853                 if (cmd->assoc_fragment[0] != 0x01) {
 854                         cmd_status(amp, BT_HCI_ERR_UNSPECIFIED_ERROR,
 855                                         BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
 856                         return;
 857                 }
 858
 859                 memcpy(amp->phylink_path, cmd->assoc_fragment + 1,
 860                                                 cmd->remain_assoc_len - 1);
 861
 862                 fd = connect_unix_client(amp->phylink_path);
 863                 if (fd < 0) {
 864                         cmd_status(amp, BT_HCI_ERR_UNSPECIFIED_ERROR,
 865                                         BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
 866                         return;
 867                 }
 868
 869                 mainloop_add_fd(fd, EPOLLOUT, connect_callback, amp, NULL);
 870
 871                 amp->phylink_fd = fd;
 872                 break;
 873
 874         default:
 875                 cmd_status(amp, BT_HCI_ERR_COMMAND_DISALLOWED,
 876                                         BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC);
 877                 return;
 878         }
 879
 880         rsp.status = BT_HCI_ERR_SUCCESS;
 881         rsp.phy_handle = amp->phy_handle;
 882
 883         cmd_complete(amp, BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC, &rsp, sizeof(rsp));
 884
 885         if (amp->phy_mode == PHY_MODE_INITIATOR) {
 886                 struct bt_hci_evt_channel_selected evt;
 887
 888                 evt.phy_handle = amp->phy_handle;
 889
 890                 send_event(amp, BT_HCI_EVT_CHANNEL_SELECTED, &evt, sizeof(evt));
 891         }
 892 }
 893
 894 static const struct {
 895         uint16_t opcode;
 896         void (*func) (struct bt_amp *amp, const void *data, uint8_t size);
 897         uint8_t size;
 898         bool fixed;
 899 } cmd_table[] = {
 900         { BT_HCI_CMD_SET_EVENT_MASK,       cmd_set_event_mask,      8, true },
 901         { BT_HCI_CMD_RESET,                cmd_reset,               0, true },
 902         { BT_HCI_CMD_READ_LOCAL_VERSION,   cmd_read_local_version,  0, true },
 903         { BT_HCI_CMD_READ_LOCAL_COMMANDS,  cmd_read_local_commands, 0, true },
 904         { BT_HCI_CMD_READ_LOCAL_FEATURES,  cmd_read_local_features, 0, true },
 905         { BT_HCI_CMD_READ_BUFFER_SIZE,     cmd_read_buffer_size,    0, true },
 906
 907         { BT_HCI_CMD_CREATE_PHY_LINK,
 908                                 cmd_create_phy_link, 3, false },
 909         { BT_HCI_CMD_ACCEPT_PHY_LINK,
 910                                 cmd_accept_phy_link, 3, false },
 911         { BT_HCI_CMD_DISCONN_PHY_LINK,
 912                                 cmd_disconn_phy_link, 2, true },
 913         { BT_HCI_CMD_CREATE_LOGIC_LINK,
 914                                 cmd_create_logic_link, 33, true },
 915         { BT_HCI_CMD_ACCEPT_LOGIC_LINK,
 916                                 cmd_accept_logic_link, 33, true },
 917         { BT_HCI_CMD_DISCONN_LOGIC_LINK,
 918                                 cmd_disconn_logic_link, 2, true },
 919         { BT_HCI_CMD_LOGIC_LINK_CANCEL,
 920                                 cmd_logic_link_cancel, 2, true },
 921         { BT_HCI_CMD_SET_EVENT_MASK_PAGE2,
 922                                 cmd_set_event_mask_page2, 8, true },
 923         { BT_HCI_CMD_READ_LOCATION_DATA,
 924                                 cmd_read_location_data, 0, true },
 925         { BT_HCI_CMD_WRITE_LOCATION_DATA,
 926                                 cmd_write_location_data, 5, true },
 927         { BT_HCI_CMD_READ_FLOW_CONTROL_MODE,
 928                                 cmd_read_flow_control_mode, 0, true },
 929         { BT_HCI_CMD_WRITE_FLOW_CONTROL_MODE,
 930                                 cmd_write_flow_control_mode, 1, true },
 931         { BT_HCI_CMD_READ_DATA_BLOCK_SIZE,
 932                                 cmd_read_data_block_size, 0, true },
 933         { BT_HCI_CMD_READ_LOCAL_AMP_INFO,
 934                                 cmd_read_local_amp_info, 0, true },
 935         { BT_HCI_CMD_READ_LOCAL_AMP_ASSOC,
 936                                 cmd_read_local_amp_assoc, 5, true },
 937         { BT_HCI_CMD_WRITE_REMOTE_AMP_ASSOC,
 938                                 cmd_write_remote_amp_assoc, 6, false },
 939         { }
 940 };
 941
 942 static void process_command(struct bt_amp *amp, const void *data, size_t size)
 943 {
 944         const struct bt_hci_cmd_hdr *hdr = data;
 945         uint16_t opcode;
 946         unsigned int i;
 947
 948         if (size < sizeof(*hdr))
 949                 return;
 950
 951         data += sizeof(*hdr);
 952         size -= sizeof(*hdr);
 953
 954         opcode = le16_to_cpu(hdr->opcode);
 955
 956         if (hdr->plen != size) {
 957                 cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
 958                 return;
 959         }
 960
 961         for (i = 0; cmd_table[i].func; i++) {
 962                 if (cmd_table[i].opcode != opcode)
 963                         continue;
 964
 965                 if ((cmd_table[i].fixed && size != cmd_table[i].size) ||
 966                                                 size < cmd_table[i].size) {
 967                         cmd_status(amp, BT_HCI_ERR_INVALID_PARAMETERS, opcode);
 968                         return;
 969                 }
 970
 971                 cmd_table[i].func(amp, data, size);
 972                 return;
 973         }
 974
 975         cmd_status(amp, BT_HCI_ERR_UNKNOWN_COMMAND, opcode);
 976 }
 977
 978 static void vhci_read_callback(int fd, uint32_t events, void *user_data)
 979 {
 980         struct bt_amp *amp = user_data;
 981         unsigned char buf[4096];
 982         ssize_t len;
 983
 984         if (events & (EPOLLERR | EPOLLHUP))
 985                 return;
 986
 987         len = read(amp->vhci_fd, buf, sizeof(buf));
 988         if (len < 1)
 989                 return;
 990
 991         switch (buf[0]) {
 992         case BT_H4_CMD_PKT:
 993                 process_command(amp, buf + 1, len - 1);
 994                 break;
 995         }
 996 }
 997
 998 struct bt_amp *bt_amp_new(void)
 999 {
1000         unsigned char setup_cmd[2];
1001         struct bt_amp *amp;
1002
1003         amp = calloc(1, sizeof(*amp));
1004         if (!amp)
1005                 return NULL;
1006
1007         reset_defaults(amp);
1008
1009         amp->vhci_fd = open("/dev/vhci", O_RDWR);
1010         if (amp->vhci_fd < 0) {
1011                 free(amp);
1012                 return NULL;
1013         }
1014
1015         setup_cmd[0] = HCI_VENDOR_PKT;
1016         setup_cmd[1] = HCI_AMP;
1017
1018         if (write(amp->vhci_fd, setup_cmd, sizeof(setup_cmd)) < 0) {
1019                 close(amp->vhci_fd);
1020                 free(amp);
1021                 return NULL;
1022         }
1023
1024         mainloop_add_fd(amp->vhci_fd, EPOLLIN, vhci_read_callback, amp, NULL);
1025
1026         return bt_amp_ref(amp);
1027 }
1028
1029 struct bt_amp *bt_amp_ref(struct bt_amp *amp)
1030 {
1031         if (!amp)
1032                 return NULL;
1033
1034         __sync_fetch_and_add(&amp->ref_count, 1);
1035
1036         return amp;
1037 }
1038
1039 void bt_amp_unref(struct bt_amp *amp)
1040 {
1041         if (!amp)
1042                 return;
1043
1044         if (__sync_sub_and_fetch(&amp->ref_count, 1))
1045                 return;
1046
1047         mainloop_remove_fd(amp->vhci_fd);
1048
1049         close(amp->vhci_fd);
1050
1051         free(amp);
1052 }