/* 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 #include #include #include #include #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); }