cras/src/server/cras_hfp_info.c

/* Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */

#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <syslog.h>

#include "audio_thread.h"
#include "bluetooth.h"
#include "byte_buffer.h"
#include "cras_hfp_info.h"
#include "cras_hfp_slc.h"
#include "cras_iodev_list.h"
#include "cras_plc.h"
#include "cras_sbc_codec.h"
#include "cras_server_metrics.h"
#include "utlist.h"
#include "packet_status_logger.h"

/* The max buffer size. Note that the actual used size must set to multiple
 * of SCO packet size, and the packet size does not necessarily be equal to
 * MTU. We should keep this as common multiple of possible packet sizes, for
 * example: 48, 60, 64, 128.
 */
#define MAX_HFP_BUF_SIZE_BYTES 28800

/* rate(8kHz) * sample_size(2 bytes) * channels(1) */
#define HFP_BYTE_RATE 16000

/* Per Bluetooth Core v5.0 and HFP 1.7 specification. */
#define MSBC_H2_HEADER_LEN 2
#define MSBC_FRAME_LEN 57
#define MSBC_FRAME_SIZE 59
#define MSBC_CODE_SIZE 240
#define MSBC_SYNC_WORD 0xAD

/* For one mSBC 1 compressed wideband audio channel the HCI packets will
 * be 3 octets of HCI header + 60 octets of data. */
#define MSBC_PKT_SIZE 60

#define H2_HEADER_0 0x01

/* Supported HCI SCO packet sizes. The wideband speech mSBC frame parsing
 * code ties to limited packet size values. Specifically list them out
 * to check against when setting packet size.
 *
 * Temp buffer size should be set to least common multiple of HCI SCO packet
 * size and MSBC_PKT_SIZE for optimizing buffer copy.
 * To add a new supported packet size value, add corresponding entry to the
 * lists, test the read/write msbc code, and fix the code if needed.
 */
static const size_t wbs_supported_packet_size[] = { 60, 24, 0 };
static const size_t wbs_hci_sco_buffer_size[] = { 60, 120, 0 };

/* Second octet of H2 header is composed by 4 bits fixed 0x8 and 4 bits
 * sequence number 0000, 0011, 1100, 1111. */
static const uint8_t h2_header_frames_count[] = { 0x08, 0x38, 0xc8, 0xf8 };

/* Structure to hold variables for a HFP connection. Since HFP supports
 * bi-direction audio, two iodevs should share one hfp_info if they
 * represent two directions of the same HFP headset
 * Members:
 *     fd - The file descriptor for SCO socket.
 *     started - If the hfp_info has started to read/write SCO data.
 *     mtu - The max transmit unit reported from BT adapter.
 *     packet_size - The size of SCO packet to read/write preferred by
 *         adapter, could be different than mtu.
 *     capture_buf - The buffer to hold samples read from SCO socket.
 *     playback_buf - The buffer to hold samples about to write to SCO socket.
 *     msbc_read - mSBC codec to decode input audio in wideband speech mode.
 *     msbc_write - mSBC codec to encode output audio in wideband speech mode.
 *     msbc_plc - PLC component to handle the packet loss of input audio in
 *         wideband speech mode.
 *     msbc_num_out_frames - Number of total written mSBC frames.
 *     msbc_num_in_frames - Number of total read mSBC frames.
 *     msbc_num_lost_frames - Number of total lost mSBC frames.
 *     read_cb - Callback to call when SCO socket can read. It returns the
 *         number of PCM bytes read.
 *     write_cb - Callback to call when SCO socket can write.
 *     write_buf - Temp buffer for writeing HCI SCO packet in wideband.
 *     read_buf - Temp buffer for reading HCI SCO packet in wideband.
 *     input_format_bytes - The audio format bytes for input device. 0 means
 *         there is no input device for the hfp_info.
 *     output_format_bytes - The audio format bytes for output device. 0 means
 *         there is no output device for the hfp_info.
 *     write_wp - Write pointer of write_buf.
 *     write_rp - Read pointer of write_buf.
 *     read_wp - Write pointer of read_buf.
 *     read_rp - Read pointer of read_buf.
 *     read_align_cb - Callback used to align mSBC frame reading with read buf.
 *     msbc_read_current_corrupted - Flag to mark if the current mSBC frame
 *         read is corrupted.
 *     wbs_logger - The logger for packet status in WBS.
 */
struct hfp_info {
	int fd;
	int started;
	unsigned int mtu;
	unsigned int packet_size;
	struct byte_buffer *capture_buf;
	struct byte_buffer *playback_buf;
	struct cras_audio_codec *msbc_read;
	struct cras_audio_codec *msbc_write;
	struct cras_msbc_plc *msbc_plc;
	unsigned int msbc_num_out_frames;
	unsigned int msbc_num_in_frames;
	unsigned int msbc_num_lost_frames;
	int (*read_cb)(struct hfp_info *info);
	int (*write_cb)(struct hfp_info *info);
	uint8_t *write_buf;
	uint8_t *read_buf;
	size_t input_format_bytes;
	size_t output_format_bytes;
	size_t write_wp;
	size_t write_rp;
	size_t read_wp;
	size_t read_rp;
	int (*read_align_cb)(uint8_t *buf);
	bool msbc_read_current_corrupted;
	struct packet_status_logger *wbs_logger;
};

int hfp_info_add_iodev(struct hfp_info *info,
		       enum CRAS_STREAM_DIRECTION direction,
		       struct cras_audio_format *format)
{
	if (direction == CRAS_STREAM_OUTPUT) {
		if (info->output_format_bytes)
			goto invalid;
		info->output_format_bytes = cras_get_format_bytes(format);

		buf_reset(info->playback_buf);
	} else if (direction == CRAS_STREAM_INPUT) {
		if (info->input_format_bytes)
			goto invalid;
		info->input_format_bytes = cras_get_format_bytes(format);

		buf_reset(info->capture_buf);
	}

	return 0;

invalid:
	return -EINVAL;
}

int hfp_info_rm_iodev(struct hfp_info *info,
		      enum CRAS_STREAM_DIRECTION direction)
{
	if (direction == CRAS_STREAM_OUTPUT && info->output_format_bytes) {
		memset(info->playback_buf->bytes, 0,
		       info->playback_buf->used_size);
		info->output_format_bytes = 0;
	} else if (direction == CRAS_STREAM_INPUT && info->input_format_bytes) {
		info->input_format_bytes = 0;
	} else {
		return -EINVAL;
	}

	return 0;
}

int hfp_info_has_iodev(struct hfp_info *info)
{
	return info->output_format_bytes || info->input_format_bytes;
}

void hfp_buf_acquire(struct hfp_info *info,
		     enum CRAS_STREAM_DIRECTION direction, uint8_t **buf,
		     unsigned *count)
{
	size_t format_bytes;
	unsigned int buf_avail;

	if (direction == CRAS_STREAM_OUTPUT && info->output_format_bytes) {
		*buf = buf_write_pointer_size(info->playback_buf, &buf_avail);
		format_bytes = info->output_format_bytes;
	} else if (direction == CRAS_STREAM_INPUT && info->input_format_bytes) {
		*buf = buf_read_pointer_size(info->capture_buf, &buf_avail);
		format_bytes = info->input_format_bytes;
	} else {
		*count = 0;
		return;
	}

	if (*count * format_bytes > buf_avail)
		*count = buf_avail / format_bytes;
}

int hfp_buf_size(struct hfp_info *info, enum CRAS_STREAM_DIRECTION direction)
{
	if (direction == CRAS_STREAM_OUTPUT && info->output_format_bytes)
		return info->playback_buf->used_size /
		       info->output_format_bytes;
	else if (direction == CRAS_STREAM_INPUT && info->input_format_bytes)
		return info->capture_buf->used_size / info->input_format_bytes;
	return 0;
}

void hfp_buf_release(struct hfp_info *info,
		     enum CRAS_STREAM_DIRECTION direction,
		     unsigned written_frames)
{
	if (direction == CRAS_STREAM_OUTPUT && info->output_format_bytes)
		buf_increment_write(info->playback_buf,
				    written_frames * info->output_format_bytes);
	else if (direction == CRAS_STREAM_INPUT && info->input_format_bytes)
		buf_increment_read(info->capture_buf,
				   written_frames * info->input_format_bytes);
	else
		written_frames = 0;
}

int hfp_buf_queued(struct hfp_info *info, enum CRAS_STREAM_DIRECTION direction)
{
	if (direction == CRAS_STREAM_OUTPUT && info->output_format_bytes)
		return buf_queued(info->playback_buf) /
		       info->output_format_bytes;
	else if (direction == CRAS_STREAM_INPUT && info->input_format_bytes)
		return buf_queued(info->capture_buf) / info->input_format_bytes;
	else
		return 0;
}

int hfp_fill_output_with_zeros(struct hfp_info *info, unsigned int nframes)
{
	unsigned int buf_avail;
	unsigned int nbytes;
	uint8_t *buf;
	int i;
	int ret = 0;

	if (info->output_format_bytes) {
		nbytes = nframes * info->output_format_bytes;
		/* Loop twice to make sure ring buffer is filled. */
		for (i = 0; i < 2; i++) {
			buf = buf_write_pointer_size(info->playback_buf,
						     &buf_avail);
			if (buf_avail == 0)
				break;
			buf_avail = MIN(nbytes, buf_avail);
			memset(buf, 0, buf_avail);
			buf_increment_write(info->playback_buf, buf_avail);
			nbytes -= buf_avail;
			ret += buf_avail / info->output_format_bytes;
		}
	}
	return ret;
}

void hfp_force_output_level(struct hfp_info *info, unsigned int level)
{
	if (info->output_format_bytes) {
		level *= info->output_format_bytes;
		level = MIN(level, MAX_HFP_BUF_SIZE_BYTES);
		buf_adjust_readable(info->playback_buf, level);
	}
}

int hfp_write_msbc(struct hfp_info *info)
{
	size_t encoded;
	int err;
	int pcm_encoded;
	unsigned int pcm_avail, to_write;
	uint8_t *samples;
	uint8_t *wp;

	if (info->write_rp + info->packet_size <= info->write_wp)
		goto msbc_send_again;

	/* Make sure there are MSBC_CODE_SIZE bytes to encode. */
	samples = buf_read_pointer_size(info->playback_buf, &pcm_avail);
	if (pcm_avail < MSBC_CODE_SIZE) {
		to_write = MSBC_CODE_SIZE - pcm_avail;
		/*
		 * Size of playback_buf is multiple of MSBC_CODE_SIZE so we
		 * are safe to prepare the buffer by appending some zero bytes.
		 */
		wp = buf_write_pointer_size(info->playback_buf, &pcm_avail);
		memset(wp, 0, to_write);
		buf_increment_write(info->playback_buf, to_write);

		samples = buf_read_pointer_size(info->playback_buf, &pcm_avail);
		if (pcm_avail < MSBC_CODE_SIZE)
			return -EINVAL;
	}

	/* Encode the next MSBC_CODE_SIZE of bytes. */
	wp = info->write_buf + info->write_wp;
	wp[0] = H2_HEADER_0;
	wp[1] = h2_header_frames_count[info->msbc_num_out_frames % 4];
	pcm_encoded = info->msbc_write->encode(
		info->msbc_write, samples, pcm_avail, wp + MSBC_H2_HEADER_LEN,
		MSBC_PKT_SIZE - MSBC_H2_HEADER_LEN, &encoded);
	if (pcm_encoded < 0) {
		syslog(LOG_ERR, "msbc encoding err: %s", strerror(pcm_encoded));
		return pcm_encoded;
	}
	buf_increment_read(info->playback_buf, pcm_encoded);
	pcm_avail -= pcm_encoded;
	info->write_wp += MSBC_PKT_SIZE;
	info->msbc_num_out_frames++;

	if (info->write_rp + info->packet_size > info->write_wp)
		return 0;

msbc_send_again:
	err = send(info->fd, info->write_buf + info->write_rp,
		   info->packet_size, 0);
	if (err < 0) {
		if (errno == EINTR)
			goto msbc_send_again;
		return err;
	}
	if (err != (int)info->packet_size) {
		syslog(LOG_ERR, "Partially write %d bytes for mSBC", err);
		return -1;
	}
	info->write_rp += info->packet_size;
	if (info->write_rp == info->write_wp) {
		info->write_rp = 0;
		info->write_wp = 0;
	}

	return err;
}

int hfp_write(struct hfp_info *info)
{
	int err = 0;
	unsigned to_send;
	uint8_t *samples;

	/* Write something */
	samples = buf_read_pointer_size(info->playback_buf, &to_send);
	if (to_send < info->packet_size)
		return 0;
	to_send = info->packet_size;

send_sample:
	err = send(info->fd, samples, to_send, 0);
	if (err < 0) {
		if (errno == EINTR)
			goto send_sample;

		return err;
	}

	if (err != (int)info->packet_size) {
		syslog(LOG_ERR,
		       "Partially write %d bytes for SCO packet size %u", err,
		       info->packet_size);
		return -1;
	}

	buf_increment_read(info->playback_buf, to_send);

	return err;
}

static int h2_header_get_seq(const uint8_t *p)
{
	int i;
	for (i = 0; i < 4; i++) {
		if (*p == h2_header_frames_count[i])
			return i;
	}
	return -1;
}

/*
 * Extract mSBC frame from SCO socket input bytes, given that the mSBC frame
 * could be lost or corrupted.
 * Args:
 *    input - Pointer to input bytes read from SCO socket.
 *    len - Length of input bytes.
 *    seq_out - To be filled by the sequence number of mSBC packet.
 * Returns:
 *    The starting position of mSBC frame if found.
 */
static const uint8_t *extract_msbc_frame(const uint8_t *input, int len,
					 unsigned int *seq_out)
{
	int rp = 0;
	int seq = -1;
	while (len - rp >= MSBC_FRAME_SIZE) {
		if ((input[rp] != H2_HEADER_0) ||
		    (input[rp + 2] != MSBC_SYNC_WORD)) {
			rp++;
			continue;
		}
		seq = h2_header_get_seq(input + rp + 1);
		if (seq < 0) {
			rp++;
			continue;
		}
		// `seq` is guaranteed to be positive now.
		*seq_out = (unsigned int)seq;
		return input + rp;
	}
	return NULL;
}

/* Log value 0 when packet is received. */
static void log_wbs_packet_received(struct hfp_info *info)
{
	if (info->wbs_logger)
		packet_status_logger_update(info->wbs_logger, 0);
}

/* Log value 1 when packet is lost. */
static void log_wbs_packet_lost(struct hfp_info *info)
{
	if (info->wbs_logger)
		packet_status_logger_update(info->wbs_logger, 1);
}

/*
 * Handle the case when mSBC frame is considered lost.
 * Args:
 *    info - The hfp_info instance holding mSBC codec and PLC objects.
 */
static int handle_packet_loss(struct hfp_info *info)
{
	int decoded;
	unsigned int pcm_avail;
	uint8_t *in_bytes;

	/* It's possible client doesn't consume data causing overrun. In that
	 * case we treat it as one mSBC frame read but dropped. */
	info->msbc_num_in_frames++;
	info->msbc_num_lost_frames++;

	log_wbs_packet_lost(info);

	in_bytes = buf_write_pointer_size(info->capture_buf, &pcm_avail);
	if (pcm_avail < MSBC_CODE_SIZE)
		return 0;

	decoded = cras_msbc_plc_handle_bad_frames(info->msbc_plc,
						  info->msbc_read, in_bytes);
	if (decoded < 0)
		return decoded;

	buf_increment_write(info->capture_buf, decoded);

	return decoded;
}

/* Checks if mSBC frame header aligns with the beginning of buffer. */
static int msbc_frame_align(uint8_t *buf)
{
	if ((buf[0] != H2_HEADER_0) || (buf[2] != MSBC_SYNC_WORD)) {
		syslog(LOG_DEBUG, "Waiting for valid mSBC frame head");
		return 0;
	}
	return 1;
}

int hfp_read_msbc(struct hfp_info *info)
{
	int err = 0;
	unsigned int pcm_avail = 0;
	int decoded;
	size_t pcm_decoded = 0;
	size_t pcm_read = 0;
	uint8_t *capture_buf;
	const uint8_t *frame_head = NULL;
	unsigned int seq;

	struct msghdr msg = { 0 };
	struct iovec iov;
	struct cmsghdr *cmsg;
	const unsigned int control_size = CMSG_SPACE(sizeof(int));
	char control[control_size];
	uint8_t pkt_status;

	memset(control, 0, sizeof(control));
recv_msbc_bytes:
	msg.msg_iov = &iov;
	msg.msg_iovlen = 1;
	iov.iov_base = info->read_buf + info->read_wp;
	iov.iov_len = info->packet_size;
	msg.msg_control = control;
	msg.msg_controllen = control_size;

	err = recvmsg(info->fd, &msg, 0);
	if (err < 0) {
		syslog(LOG_ERR, "HCI SCO packet read err %s", strerror(errno));
		if (errno == EINTR)
			goto recv_msbc_bytes;
		return err;
	}
	/*
	 * Treat return code 0 (socket shutdown) as error here. BT stack
	 * shall send signal to main thread for device disconnection.
	 */
	if (err != (int)info->packet_size) {
		syslog(LOG_ERR, "Partially read %d bytes for mSBC packet", err);
		return -1;
	}

	/* Offset in input data breaks mSBC frame parsing. Discard this packet
	 * until read alignment succeed. */
	if (info->read_align_cb) {
		if (!info->read_align_cb(info->read_buf))
			return 0;
		else
			info->read_align_cb = NULL;
	}
	info->read_wp += err;

	pkt_status = 0;
	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
	     cmsg = CMSG_NXTHDR(&msg, cmsg)) {
		if (cmsg->cmsg_level == SOL_BLUETOOTH &&
		    cmsg->cmsg_type == BT_SCM_PKT_STATUS) {
			size_t len = cmsg->cmsg_len - sizeof(*cmsg);
			memcpy(&pkt_status, CMSG_DATA(cmsg), len);
		}
	}

	/*
	 * HCI SCO packet status flag:
	 * 0x00 - correctly received data.
	 * 0x01 - possibly invalid data.
	 * 0x10 - No data received.
	 * 0x11 - Data partially lost.
	 *
	 * If the latest SCO packet read doesn't cross the boundary of a mSBC
	 * frame, the packet status flag can be used to derive if the current
	 * mSBC frame is corrupted.
	 */
	if (info->read_rp + MSBC_PKT_SIZE >= info->read_wp)
		info->msbc_read_current_corrupted |= (pkt_status > 0);

	/* Read buffer not enough to parse another mSBC frame. */
	if (info->read_rp + MSBC_PKT_SIZE > info->read_wp)
		return 0;

	if (info->msbc_read_current_corrupted) {
		syslog(LOG_DEBUG, "mSBC frame corrputed from packet status");
		info->msbc_read_current_corrupted = 0;
		frame_head = NULL;
	} else {
		frame_head =
			extract_msbc_frame(info->read_buf + info->read_rp,
					   info->read_wp - info->read_rp, &seq);
		if (!frame_head)
			syslog(LOG_DEBUG, "Failed to extract msbc frame");
	}

	/*
	 * Done with parsing the raw bytes just read. If mSBC frame head not
	 * found, we shall handle it as packet loss.
	 */
	info->read_rp += MSBC_PKT_SIZE;
	if (info->read_rp == info->read_wp) {
		info->read_rp = 0;
		info->read_wp = 0;
	}
	if (!frame_head)
		return handle_packet_loss(info);

	/*
	 * Consider packet loss when found discontinuity in sequence number.
	 */
	while (seq != (info->msbc_num_in_frames % 4)) {
		syslog(LOG_DEBUG, "SCO packet seq unmatch");
		err = handle_packet_loss(info);
		if (err < 0)
			return err;
		pcm_read += err;
	}

	/* Check if there's room for more PCM. */
	capture_buf = buf_write_pointer_size(info->capture_buf, &pcm_avail);
	if (pcm_avail < MSBC_CODE_SIZE)
		return pcm_read;

	decoded = info->msbc_read->decode(info->msbc_read,
					  frame_head + MSBC_H2_HEADER_LEN,
					  MSBC_FRAME_LEN, capture_buf,
					  pcm_avail, &pcm_decoded);
	if (decoded < 0) {
		/*
		 * If mSBC frame cannot be decoded, consider this packet is
		 * corrupted and lost.
		 */
		syslog(LOG_ERR, "mSBC decode failed");
		err = handle_packet_loss(info);
		if (err < 0)
			return err;
		pcm_read += err;
	} else {
		/* Good mSBC frame decoded. */
		log_wbs_packet_received(info);
		buf_increment_write(info->capture_buf, pcm_decoded);
		info->msbc_num_in_frames++;
		cras_msbc_plc_handle_good_frames(info->msbc_plc, capture_buf,
						 capture_buf);
		pcm_read += pcm_decoded;
	}
	return pcm_read;
}

int hfp_read(struct hfp_info *info)
{
	int err = 0;
	unsigned to_read;
	uint8_t *capture_buf;

	capture_buf = buf_write_pointer_size(info->capture_buf, &to_read);

	if (to_read < info->packet_size)
		return 0;
	to_read = info->packet_size;

recv_sample:
	err = recv(info->fd, capture_buf, to_read, 0);
	if (err < 0) {
		syslog(LOG_ERR, "Read error %s", strerror(errno));
		if (errno == EINTR)
			goto recv_sample;

		return err;
	}

	if (err != (int)info->packet_size) {
		/* Allow the SCO packet size be modified from the default MTU
		 * value to the size of SCO data we first read. This is for
		 * some adapters who prefers a different value than MTU for
		 * transmitting SCO packet.
		 */
		if (err && (info->packet_size == info->mtu)) {
			info->packet_size = err;
		} else {
			syslog(LOG_ERR,
			       "Partially read %d bytes for %u size SCO packet",
			       err, info->packet_size);
			return -1;
		}
	}

	buf_increment_write(info->capture_buf, err);

	return err;
}

/* Callback function to handle sample read and write.
 * Note that we poll the SCO socket for read sample, since it reflects
 * there is actual some sample to read while the socket always reports
 * writable even when device buffer is full.
 * The strategy is to synchronize read & write operations:
 * 1. Read one chunk of MTU bytes of data.
 * 2. When input device not attached, ignore the data just read.
 * 3. When output device attached, write one chunk of MTU bytes of data.
 */
static int hfp_info_callback(void *arg, int revents)
{
	struct hfp_info *info = (struct hfp_info *)arg;
	int err = 0;

	if (!info->started)
		return 0;

	/* Allow last read before handling error or hang-up events. */
	if (revents & POLLIN) {
		err = info->read_cb(info);
		if (err < 0) {
			syslog(LOG_ERR, "Read error");
			goto read_write_error;
		}
	}
	/* Ignore the bytes just read if input dev not in present */
	if (!info->input_format_bytes)
		buf_increment_read(info->capture_buf, err);

	if (revents & (POLLERR | POLLHUP)) {
		syslog(LOG_ERR, "Error polling SCO socket, revent %d", revents);
		goto read_write_error;
	}

	/* Without output stream's presence, we shall still send zero packets
	 * to HF. This is required for some HF devices to start sending non-zero
	 * data to AG.
	 */
	if (!info->output_format_bytes)
		buf_increment_write(info->playback_buf,
				    info->msbc_write ? err : info->packet_size);

	err = info->write_cb(info);
	if (err < 0) {
		syslog(LOG_ERR, "Write error");
		goto read_write_error;
	}

	return 0;

read_write_error:
	/*
	 * This callback is executing in audio thread, so it's safe to
	 * unregister itself by audio_thread_rm_callback().
	 */
	audio_thread_rm_callback(info->fd);
	close(info->fd);
	info->fd = 0;
	info->started = 0;

	return 0;
}

struct hfp_info *hfp_info_create()
{
	struct hfp_info *info;
	info = (struct hfp_info *)calloc(1, sizeof(*info));
	if (!info)
		goto error;

	info->capture_buf = byte_buffer_create(MAX_HFP_BUF_SIZE_BYTES);
	if (!info->capture_buf)
		goto error;

	info->playback_buf = byte_buffer_create(MAX_HFP_BUF_SIZE_BYTES);
	if (!info->playback_buf)
		goto error;

	return info;

error:
	if (info) {
		if (info->capture_buf)
			byte_buffer_destroy(&info->capture_buf);
		if (info->playback_buf)
			byte_buffer_destroy(&info->playback_buf);
		free(info);
	}
	return NULL;
}

void hfp_info_set_wbs_logger(struct hfp_info *info,
			     struct packet_status_logger *wbs_logger)
{
	info->wbs_logger = wbs_logger;
}

int hfp_info_running(struct hfp_info *info)
{
	return info->started;
}

int hfp_info_start(int fd, unsigned int mtu, int codec, struct hfp_info *info)
{
	info->fd = fd;
	info->mtu = mtu;

	/* Initialize to MTU, it may change when actually read the socket. */
	info->packet_size = mtu;
	buf_reset(info->playback_buf);
	buf_reset(info->capture_buf);

	if (codec == HFP_CODEC_ID_MSBC) {
		int i;
		for (i = 0; wbs_supported_packet_size[i] != 0; i++) {
			if (info->packet_size == wbs_supported_packet_size[i])
				break;
		}
		/* In case of unsupported value, error log and fallback to
		 * MSBC_PKT_SIZE(60). */
		if (wbs_supported_packet_size[i] == 0) {
			syslog(LOG_ERR, "Unsupported packet size %u",
			       info->packet_size);
			i = 0;
		}
		info->packet_size = wbs_supported_packet_size[i];
		info->write_buf = (uint8_t *)malloc(wbs_hci_sco_buffer_size[i]);
		info->read_buf = (uint8_t *)malloc(wbs_hci_sco_buffer_size[i]);

		info->write_cb = hfp_write_msbc;
		info->read_cb = hfp_read_msbc;
		info->msbc_read = cras_msbc_codec_create();
		info->msbc_write = cras_msbc_codec_create();
		info->msbc_plc = cras_msbc_plc_create();

		packet_status_logger_init(info->wbs_logger);
	} else {
		info->write_cb = hfp_write;
		info->read_cb = hfp_read;
	}

	audio_thread_add_events_callback(info->fd, hfp_info_callback, info,
					 POLLIN | POLLERR | POLLHUP);

	info->started = 1;
	info->msbc_num_out_frames = 0;
	info->msbc_num_in_frames = 0;
	info->msbc_num_lost_frames = 0;
	info->write_rp = 0;
	info->write_wp = 0;
	info->read_rp = 0;
	info->read_wp = 0;

	/* Mark as aligned if packet size equals to MSBC_PKT_SIZE. */
	info->read_align_cb =
		(info->packet_size == MSBC_PKT_SIZE) ? NULL : msbc_frame_align;
	info->msbc_read_current_corrupted = 0;

	return 0;
}

int hfp_info_stop(struct hfp_info *info)
{
	if (!info->started)
		return 0;

	audio_thread_rm_callback_sync(cras_iodev_list_get_audio_thread(),
				      info->fd);

	close(info->fd);
	info->fd = 0;
	info->started = 0;

	/* Unset the write/read callbacks. */
	info->write_cb = NULL;
	info->read_cb = NULL;

	if (info->write_buf)
		free(info->write_buf);
	if (info->read_buf)
		free(info->read_buf);

	if (info->msbc_read) {
		cras_sbc_codec_destroy(info->msbc_read);
		info->msbc_read = NULL;
	}
	if (info->msbc_write) {
		cras_sbc_codec_destroy(info->msbc_write);
		info->msbc_write = NULL;
	}
	if (info->msbc_plc) {
		cras_msbc_plc_destroy(info->msbc_plc);
		info->msbc_plc = NULL;
	}

	if (info->msbc_num_in_frames) {
		cras_server_metrics_hfp_packet_loss(
			(float)info->msbc_num_lost_frames /
			info->msbc_num_in_frames);
	}

	return 0;
}

void hfp_info_destroy(struct hfp_info *info)
{
	if (info->capture_buf)
		byte_buffer_destroy(&info->capture_buf);

	if (info->playback_buf)
		byte_buffer_destroy(&info->playback_buf);

	free(info);
}