profiles/audioOverBle/btvoice.c

/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2011  Nokia Corporation
 *  Copyright (C) 2011  Marcel Holtmann <marcel@holtmann.org>
 *
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <pthread.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
#include <stdbool.h>

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/stat.h>

#include <glib.h>

#include "lib/bluetooth.h"
#include "lib/sdp.h"
#include "lib/sdp_lib.h"
#include "lib/uuid.h"

#include <gdbus/gdbus.h>

#include "attrib/gattrib.h"
#include "attrib/att.h"
#include "attrib/gatt.h"
#include "src/adapter.h"
#include "src/attio.h"
#include "src/dbus-common.h"
#include "src/device.h"
#include "src/error.h"
#include "src/log.h"
#include "src/shared/util.h"
#include "src/textfile.h"

#include "btvoice.h"

#include "RAS_lib.h"

#define AUD_START_UUID 0xFFF1
#define AUD_CONFIG_UUID 0xFFF2
#define AUD_STREAM_UUID 0xFFF4

static pthread_mutex_t audio_mutex = PTHREAD_MUTEX_INITIALIZER;

struct btvoice_device {
  uint16_t id;
  struct btd_device *device;
  GAttrib *attrib;
  guint attioid;
  struct gatt_primary *audioOverBle;
  struct btvoiceenable voiceenable;
  GSList *reports;
  uint16_t aud_start_handle;
  uint16_t aud_config_handle;
  uint16_t aud_stream_handle;
  uint16_t aud_cust1_handle;
  uint16_t aud_cust2_handle;
  uint16_t aud_cust4_handle;
  uint16_t aud_cust7_handle;
  uint16_t aud_cust8_handle;
};

struct audiostream {
  uint8_t id;
  uint8_t type;
  uint16_t ccc_handle;
  guint notifyid;
  struct gatt_char *decl;
  struct btvoice_device *btvoicedev;
};

static GSList *btvoicedevices = NULL;

static struct btvoice_device *find_btvoicedev(struct btd_device *device) {
  GSList *l;

  for (l = btvoicedevices; l; l = l->next) {
    struct btvoice_device *btvoicedev = l->data;

    if (btvoicedev->device == device) return btvoicedev;
  }

  return NULL;
}

static void aud_cfg_written_cb(guint8 status, const guint8 *pdu, guint16 plen,
                               gpointer user_data) {
  if (status != 0) {
    error("Write output report failed: %s", att_ecode2str(status));
    return;
  }
}

static void aud_cust1a_written_cb(guint8 status, const guint8 *pdu, guint16 plen,
                               gpointer user_data) {
  if (status != 0) {
    error("cust1a failed: %s", att_ecode2str(status));
    return;
  }
}

static void aud_cust1_written_cb(guint8 status, const guint8 *pdu, guint16 plen,
                               gpointer user_data) {
  struct btvoice_device *btvoicedev = user_data;
  uint8_t data[] = { 0x0a, 0x01 };

  if (status != 0) {
    error("cust1 failed: %s", att_ecode2str(status));
    return;
  }

  gatt_write_char(btvoicedev->attrib, btvoicedev->aud_cust1_handle, data,
    sizeof(data), aud_cust1a_written_cb, btvoicedev);
}

static void aud_cust2b_written_cb(guint8 status, const guint8 *pdu, guint16 plen,
                               gpointer user_data) {
  if (status != 0) {
    error("cust2b failed: %s", att_ecode2str(status));
    return;
  }
}

static void aud_cust2a_written_cb(guint8 status, const guint8 *pdu, guint16 plen,
                               gpointer user_data) {
  struct btvoice_device *btvoicedev = user_data;
  uint8_t data[] = { 0x05, 0x00, 0x01, 0x05 };

  if (status != 0) {
    error("cust2a failed: %s", att_ecode2str(status));
    return;
  }

  gatt_write_char(btvoicedev->attrib, btvoicedev->aud_cust2_handle, data,
    sizeof(data), aud_cust2b_written_cb, btvoicedev);
}

static void aud_cust2_written_cb(guint8 status, const guint8 *pdu, guint16 plen,
                               gpointer user_data) {
  struct btvoice_device *btvoicedev = user_data;
  uint8_t data[] = { 0x05, 0x00, 0x01, 0x01 };

  if (status != 0) {
    error("cust2 failed: %s", att_ecode2str(status));
    return;
  }

  gatt_write_char(btvoicedev->attrib, btvoicedev->aud_cust2_handle, data,
    sizeof(data), aud_cust2a_written_cb, btvoicedev);
}

static void aud_cust4_written_cb(guint8 status, const guint8 *pdu, guint16 plen,
                               gpointer user_data) {
  if (status != 0) {
    error("Write output report failed: %s", att_ecode2str(status));
    return;
  }
}

static void aud_cust7_written_cb(guint8 status, const guint8 *pdu, guint16 plen,
                               gpointer user_data) {
  if (status != 0) {
    error("Write output report failed: %s", att_ecode2str(status));
    return;
  }
}

static void aud_cust8_written_cb(guint8 status, const guint8 *pdu, guint16 plen,
                               gpointer user_data) {
  if (status != 0) {
    error("Write output report failed: %s", att_ecode2str(status));
    return;
  }
}

#define RAS_START_CMD    0x04
#define RAS_DATA_TIC1_CMD  0x01
#define RAS_STOP_CMD    0x02
#define RAS_DATA_RAW_CMD  0x03
#define RAS_END_CMD      0x00

static void audio_value_cb(const uint8_t *pdu, uint16_t len,
                           gpointer user_data)
{
  static int PrevSeqNum = 0;
  static int nbFrameLost, nbFrameReceived;
  static int starting_sync = 0;
  static uint8_t SeqNum;
  uint8_t frameType;
  static char state;
  int16_t writebuf[1024];
  uint16 to_write;
  uint16_t libVersion;
  int wrote;
  int fp;
  const struct sigaction ignore = { .sa_handler = SIG_IGN, };

  if (sigaction(SIGPIPE, &ignore, NULL) != 0) {
    perror("sigaction:");
  }

  pthread_mutex_lock(&audio_mutex);
  fp = open(FIFO_BTD2AUDIO_FILE, O_WRONLY);

  if (fp != -1) {
    switch (len) {
      case 4:
        switch (pdu[3]) {
          case RAS_DATA_TIC1_CMD:
            break;

          case RAS_START_CMD:
            state = 0;
            PrevSeqNum = 0;
            starting_sync = 1;

            libVersion = RAS_GetVersion();
            DBG("Library version: %d.%d\n", (libVersion & 0xFF00) >> 8, libVersion & 0x00FF);
            RAS_Init(RAS_PEC_MODE1);
            break;

          case RAS_END_CMD:
            break;
        }
        break;

      case 23:
        switch (state) {
          case 0:
            SeqNum = ((pdu[3] >> 3) & 0x1f);
            DBG("SeqNum %d PrevSeqNum %d", SeqNum, PrevSeqNum);

            if (SeqNum != (0x1f & (PrevSeqNum + 1))) {
              DBG("missed %d SeqNum", SeqNum - PrevSeqNum - 1);

              if (starting_sync) {
                DBG("throwing it away");
                break;
              }
            } else {
              starting_sync = 0;
            }

            PrevSeqNum = SeqNum;
            // nobreak;

          case 1:
          case 2:
          case 3:
            RAS_Decode(RAS_DECODE_TI_TYPE1, (uint8 *)&pdu[4], len - 4, writebuf, &to_write);
            wrote = write(fp, writebuf, to_write);
            state++;
            break;

          case 4:
            RAS_Decode(RAS_DECODE_TI_TYPE1, (uint8 *)&pdu[4], len - 4, writebuf, &to_write);
            wrote = write(fp, writebuf, to_write);
            state = 0;
            break;
        }
        break;

      case 103:
        SeqNum = ((pdu[3] >> 3) & 0x1f);
        DBG("SeqNum %d PrevSeqNum %d", SeqNum, PrevSeqNum);

        if (SeqNum != (0x1f & (PrevSeqNum + 1))) {
          DBG("missed %d SeqNum", SeqNum - PrevSeqNum - 1);

          if (starting_sync) {
            DBG("throwing it away");
            break;
          }
        } else {
          starting_sync = 0;
        }

        PrevSeqNum = SeqNum;
        RAS_Decode(RAS_DECODE_TI_TYPE1, (uint8 *)&pdu[4],
            len - 4, writebuf, &to_write);
        wrote = write(fp, writebuf, to_write);
        break;
    }

    close(fp);
  } else {
    DBG("can't open the pipe: %s", FIFO_BTD2AUDIO_FILE);
  }

  pthread_mutex_unlock(&audio_mutex);
}

static void audio_ccc_written_cb(guint8 status, const guint8 *pdu, guint16 plen,
                                 gpointer user_data) {
  struct audiostream *audiostream = user_data;

  if (status != 0) {
    error("Write audio characteristic descriptor failed: %s",
          att_ecode2str(status));
    return;
  }
}

static void audio_write_ccc(uint16_t handle, gpointer user_data) {
  struct audiostream *audiostream = user_data;
  struct btvoice_device *btvoicedev = audiostream->btvoicedev;
  uint8_t value[] = {0x01, 0x00};

  audiostream->notifyid = g_attrib_register(
      btvoicedev->attrib, ATT_OP_HANDLE_NOTIFY, audiostream->decl->value_handle,
      audio_value_cb, audiostream, NULL);

  gatt_write_char(btvoicedev->attrib, handle, value, sizeof(value),
                  audio_ccc_written_cb, audiostream);
}

static void report_reference_cb(guint8 status, const guint8 *pdu, guint16 plen,
                                gpointer user_data) {
  struct audiostream *report = user_data;

  if (status != 0) {
    error("Read Report Reference descriptor failed: %s", att_ecode2str(status));
    return;
  }

  if (plen != 3) {
    error("Malformed ATT read response");
    return;
  }

  report->id = pdu[1];
  report->type = pdu[2];
}

static void discover_descriptor_cb(uint8_t status, GSList *descs,
                                   void *user_data) {
  struct audiostream *report;
  GAttrib *attrib = NULL;

  if (status != 0) {
    error("Discover all characteristic descriptors failed: %s",
          att_ecode2str(status));
    return;
  }

  for (; descs; descs = descs->next) {
    struct gatt_desc *desc = descs->data;

    switch (desc->uuid16) {
      case GATT_CLIENT_CHARAC_CFG_UUID:
        report = user_data;
        report->ccc_handle = desc->handle;
        attrib = report->btvoicedev->attrib;
        audio_write_ccc(desc->handle, report);
        DBG("Audio Notifications Enabled\n");
        break;
      case GATT_REPORT_REFERENCE:
        report = user_data;
        attrib = report->btvoicedev->attrib;
        gatt_read_char(attrib, desc->handle, report_reference_cb, report);
        DBG("Audio Stream Report Reference Characterstic Read\n");
        break;
    }
  }
}

static void discover_descriptor(GAttrib *attrib, uint16_t start, uint16_t end,
                                gpointer user_data) {
  if (start > end) {
    DBG("Start before End\n");
    return;
  }

  gatt_discover_desc(attrib, start, end, NULL, discover_descriptor_cb,
                     user_data);
}

static void char_discovered_cb(guint8 status, GSList *chars, void *user_data) {
  struct btvoice_device *btvoicedev = user_data;
  bt_uuid_t audstart_uuid, audconfig_uuid, audstream_uuid;
  bt_uuid_t audcust1_uuid, audcust2_uuid, audcust4_uuid,
    audcust7_uuid, audcust8_uuid;

  uint128_t aud_custom1_uuid = {
      .data = {  0xf0, 0x00, 0xfe, 0xf1, 0x04, 0x51, 0x40, 0x00,
      0xb0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
  };

  uint128_t aud_custom2_uuid = {
      .data = {  0xf0, 0x00, 0xfe, 0xf2, 0x04, 0x51, 0x40, 0x00,
      0xb0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
  };

  uint128_t aud_custom4_uuid = {
      .data = {  0xf0, 0x00, 0xfe, 0xf4, 0x04, 0x51, 0x40, 0x00,
      0xb0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
  };

  uint128_t aud_custom7_uuid = {
      .data = {  0xf0, 0x00, 0xfe, 0xf7, 0x04, 0x51, 0x40, 0x00,
      0xb0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
  };

  uint128_t aud_custom8_uuid = {
      .data = {  0xf0, 0x00, 0xfe, 0xf8, 0x04, 0x51, 0x40, 0x00,
      0xb0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }
  };

  struct audiostream *audiostream;
  GSList *l;

  if (status != 0) {
    const char *str = att_ecode2str(status);
    return;
  }

  bt_uuid16_create(&audstart_uuid, AUD_START_UUID);
  bt_uuid16_create(&audconfig_uuid, AUD_CONFIG_UUID);
  bt_uuid16_create(&audstream_uuid, AUD_STREAM_UUID);
  bt_uuid128_create(&audcust1_uuid, aud_custom1_uuid);
  bt_uuid128_create(&audcust2_uuid, aud_custom2_uuid);
  bt_uuid128_create(&audcust4_uuid, aud_custom4_uuid);
  bt_uuid128_create(&audcust7_uuid, aud_custom7_uuid);
  bt_uuid128_create(&audcust8_uuid, aud_custom8_uuid);

  for (l = chars; l; l = g_slist_next(l)) {
    struct gatt_char *chr;
    bt_uuid_t uuid;

    chr = l->data;
    bt_string_to_uuid(&uuid, chr->uuid);

    if (bt_uuid_cmp(&uuid, &audstart_uuid) == 0) {
      btvoicedev->aud_start_handle = chr->value_handle;
    } else if (bt_uuid_cmp(&uuid, &audconfig_uuid) == 0) {
      btvoicedev->aud_config_handle = chr->value_handle;
    } else if (bt_uuid_cmp(&uuid, &audstream_uuid) == 0) {
      audiostream = g_new0(struct audiostream, 1);
      audiostream->btvoicedev = btvoicedev;
      audiostream->decl = g_memdup(chr, sizeof(*chr));
      btvoicedev->reports = g_slist_append(btvoicedev->reports, audiostream);

      btvoicedev->aud_stream_handle = chr->value_handle;
      discover_descriptor(btvoicedev->attrib, chr->value_handle,
                          btvoicedev->audioOverBle->range.end, audiostream);
    } else if (bt_uuid_cmp(&uuid, &audcust1_uuid) == 0) {
      uint8_t data[] = { 0x0a, 0x00 };
      btvoicedev->aud_cust1_handle = chr->value_handle;
      gatt_write_char(btvoicedev->attrib, btvoicedev->aud_cust1_handle, data,
          sizeof(data), aud_cust1_written_cb, btvoicedev);
    } else if (bt_uuid_cmp(&uuid, &audcust2_uuid) == 0) {
      uint8_t data[] = { 0x00, 0x05, 0x00, 0x00, 0x01 };
      btvoicedev->aud_cust2_handle = chr->value_handle;
      gatt_write_char(btvoicedev->attrib, btvoicedev->aud_cust2_handle, data,
          sizeof(data), aud_cust2_written_cb, btvoicedev);
    } else if (bt_uuid_cmp(&uuid, &audcust4_uuid) == 0) {
      uint8_t data[] = { 0x0f };
      btvoicedev->aud_cust4_handle = chr->value_handle;
      gatt_write_char(btvoicedev->attrib, btvoicedev->aud_cust4_handle, data,
          sizeof(data), aud_cust4_written_cb, btvoicedev);
    } else if (bt_uuid_cmp(&uuid, &audcust7_uuid) == 0) {
      uint8_t data[] = { 0x05 };
      btvoicedev->aud_cust7_handle = chr->value_handle;
      gatt_write_char(btvoicedev->attrib, btvoicedev->aud_cust7_handle, data,
          sizeof(data), aud_cust7_written_cb, btvoicedev);
    } else if (bt_uuid_cmp(&uuid, &audcust8_uuid) == 0) {
      uint8_t data[] = { 0x02 };
      btvoicedev->aud_cust8_handle = chr->value_handle;
      gatt_write_char(btvoicedev->attrib, btvoicedev->aud_cust8_handle, data,
          sizeof(data), aud_cust8_written_cb, btvoicedev);
    }
  }

  if (btvoicedev->aud_config_handle) {
    uint8_t data[] = {0x03, 0x00, 0x00};
    gatt_write_char(btvoicedev->attrib, btvoicedev->aud_config_handle, data,
                    sizeof(data), aud_cfg_written_cb, btvoicedev);
  }
}


static void attio_connected_cb(GAttrib *attrib, gpointer user_data) {
  struct btvoice_device *btvoicedev = user_data;
  struct gatt_primary *prim = btvoicedev->audioOverBle;

  btvoicedev->attrib = g_attrib_ref(attrib);

  if (btvoicedev->voiceenable.audioOverBle) {
    gatt_discover_char(btvoicedev->attrib, prim->range.start, prim->range.end,
                       NULL, char_discovered_cb, btvoicedev);
  }
}

static void attio_disconnected_cb(gpointer user_data) {
  struct btvoice_device *btvoicedev = user_data;
  GSList *l;

  for (l = btvoicedev->reports; l; l = l->next) {
    struct audiostream *r = l->data;

    g_attrib_unregister(btvoicedev->attrib, r->notifyid);
  }

  g_attrib_unref(btvoicedev->attrib);
  btvoicedev->attrib = NULL;
}

static struct btvoice_device *register_btvoicedevice(
    struct btd_device *device)
{
  struct btvoice_device *btvoicedev;

  btvoicedev = find_btvoicedev(device);
  if (btvoicedev != NULL) return btvoicedev;

  btvoicedev = g_try_new0(struct btvoice_device, 1);
  if (!btvoicedev) return NULL;

  btvoicedev->device = btd_device_ref(device);

  btvoicedevices = g_slist_append(btvoicedevices, btvoicedev);

  return btvoicedev;
}

static void update_btvoicedevice(struct btvoice_device *btvoicedev)
{
  if (!btvoicedev->voiceenable.audioOverBle) return;

  if (btvoicedev->attioid != 0) return;

  btvoicedev->attioid =
      btd_device_add_attio_callback(btvoicedev->device, attio_connected_cb,
                                    attio_disconnected_cb, btvoicedev);
}

int btvoice_register_audioOverBle(struct btd_device *device,
                                  struct btvoiceenable *voiceenable,
                                  struct gatt_primary *prim) {
  struct btvoice_device *btvoicedev;

  if (!voiceenable->audioOverBle) return 0;

  btvoicedev = register_btvoicedevice(device);
  if (btvoicedev == NULL) return -1;

  btvoicedev->id = prim->range.start;
  btvoicedev->audioOverBle = g_memdup(prim, sizeof(*prim));
  btvoicedev->voiceenable.audioOverBle = TRUE;

  update_btvoicedevice(btvoicedev);

  // Create Pipe for audio data exchange
  if (access(FIFO_BTD2AUDIO_FILE, F_OK) == -1) {
    int res = mkfifo(FIFO_BTD2AUDIO_FILE, 0666);
    if (res != 0) {
      DBG("Could not create fifo %s\n", FIFO_BTD2AUDIO_FILE);
      exit(1);
    }
  }

  return 0;
}

static void cleanup_btdevice(struct btvoice_device *btvoicedev)
{
  struct btd_device *device = btvoicedev->device;

  if (btvoicedev->audioOverBle != NULL) return;

  if (btvoicedev->attioid != 0) {
    btd_device_remove_attio_callback(device, btvoicedev->attioid);
    btvoicedev->attioid = 0;
  }

  if (btvoicedev->attrib != NULL) {
    g_attrib_unref(btvoicedev->attrib);
    btvoicedev->attrib = NULL;
  }

  if (btvoicedev->device != NULL) {
    btd_device_unref(btvoicedev->device);
    btvoicedev->device = NULL;
  }
}

void btvoice_unregister_audioOverBle(struct btd_device *device)
{
  struct btvoice_device *btvoicedev;

  btvoicedev = find_btvoicedev(device);
  if (btvoicedev == NULL) return;

  g_free(btvoicedev->audioOverBle);
  btvoicedev->audioOverBle = NULL;
  btvoicedev->voiceenable.audioOverBle = FALSE;

  cleanup_btdevice(btvoicedev);

  g_free(btvoicedev);
}