563 lines
17 KiB
C
563 lines
17 KiB
C
/*
|
|
* Copyright (C) 2018 Knowles Electronics
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <stdbool.h>
|
|
#include <string.h>
|
|
#include <math.h>
|
|
#include <errno.h>
|
|
|
|
#define LOG_TAG "SoundTriggerHALAdnc"
|
|
//#define LOG_NDEBUG 0
|
|
//#define LOG_NDDEBUG 0
|
|
|
|
#include <log/log.h>
|
|
#include <linux/mfd/adnc/iaxxx-system-identifiers.h>
|
|
#include "adnc_strm.h"
|
|
#include "tunnel.h"
|
|
|
|
#define MAX_TUNNELS (32)
|
|
#define BUF_SIZE (8192)
|
|
|
|
#define CVQ_TUNNEL_ID (1)
|
|
#define TNL_Q15 (0xF)
|
|
|
|
#define UNPARSED_OUTPUT_FILE "/data/data/unparsed_output"
|
|
// By defining this macros, dumps will be enabled at key points to help in debugging
|
|
//#define ENABLE_DEBUG_DUMPS
|
|
#define HOTWORD_MODEL (0)
|
|
#define AMBIENT_MODEL (1)
|
|
|
|
struct raf_format_type {
|
|
uint16_t frameSizeInBytes; // Frame length in bytes
|
|
uint8_t encoding; // Encoding
|
|
uint8_t sampleRate; // Sample rate
|
|
};
|
|
|
|
struct raf_frame_type {
|
|
uint64_t timeStamp; // Timestamp of the frame
|
|
uint32_t seqNo; // Optional sequence number of the frame
|
|
struct raf_format_type format; // Format information for the frame
|
|
uint32_t data[0]; /* Start of the variable size payload.
|
|
It must start at 128 bit aligned
|
|
address for all the frames */
|
|
};
|
|
|
|
struct adnc_strm_device
|
|
{
|
|
struct ia_tunneling_hal *tun_hdl;
|
|
int end_point;
|
|
int idx;
|
|
int mode;
|
|
int encode;
|
|
|
|
bool enable_stripping;
|
|
unsigned int kw_start_frame;
|
|
|
|
void *pcm_buf;
|
|
size_t pcm_buf_size;
|
|
size_t pcm_avail_size;
|
|
size_t pcm_read_offset;
|
|
|
|
void *unparsed_buf;
|
|
size_t unparsed_buf_size;
|
|
size_t unparsed_avail_size;
|
|
|
|
#ifdef DUMP_UNPARSED_OUTPUT
|
|
FILE *dump_file;
|
|
#endif
|
|
|
|
pthread_mutex_t lock;
|
|
};
|
|
|
|
static void kst_split_aft(uint32_t *pAfloat, int32_t *exp,
|
|
int64_t *mant, int32_t *sign)
|
|
{
|
|
uint32_t uAft = *pAfloat;
|
|
|
|
*exp = (uAft >> 25) & 0x3F;
|
|
*mant = uAft & 0x1FFFFFF;
|
|
*sign = uAft >> 31;
|
|
if (*exp || *mant) {
|
|
*mant |= 1 << 25;
|
|
}
|
|
}
|
|
|
|
static void kst_aft_to_dbl(void *pDouble, void *pAfloat)
|
|
{
|
|
uint64_t uDbl;
|
|
int32_t exp;
|
|
int32_t sign;
|
|
int64_t mant;
|
|
|
|
kst_split_aft((uint32_t *)pAfloat, &exp, &mant, &sign);
|
|
if (exp || mant) {
|
|
uDbl = ((uint64_t)sign << 63) |
|
|
((uint64_t)(exp + (1023 - (1 << 5))) << 52) |
|
|
((uint64_t)(mant & ((1 << 25) - 1)) << (52 - 25));
|
|
} else {
|
|
uDbl = (uint64_t)sign << 63;
|
|
}
|
|
*((uint64_t *)pDouble) = uDbl;
|
|
}
|
|
|
|
void kst_float_to_q15_vector(void *pDst, void *pSrc, uint32_t elCnt)
|
|
{
|
|
uint32_t *pSrcT;
|
|
int16_t *pDstT;
|
|
uint32_t idx;
|
|
double smp;
|
|
|
|
pSrcT = (uint32_t *)pSrc;
|
|
pDstT = (int16_t *)pDst;
|
|
for (idx = 0; idx < elCnt; idx++) {
|
|
kst_aft_to_dbl(&smp, &(pSrcT[idx]));
|
|
smp = smp * 32768.0;
|
|
pDstT[idx] = ((smp < 32767.0) ?
|
|
((smp > -32768.0) ?
|
|
((int16_t)smp) : -32768) : 32767);
|
|
}
|
|
}
|
|
|
|
void parse_audio_tunnel_data(unsigned char *buf_itr,
|
|
unsigned char *pcm_buf_itr,
|
|
int frame_sz_in_bytes,
|
|
bool is_q15_conversion_required)
|
|
{
|
|
//char q16_buf[BUF_SIZE]; // This can be smaller but by how much?
|
|
int frameSizeInWords = (frame_sz_in_bytes + 3) >> 2;
|
|
|
|
if (buf_itr == NULL || pcm_buf_itr == NULL) {
|
|
ALOGE("%s: Buffer is NULL", __func__);
|
|
return;
|
|
}
|
|
|
|
if (is_q15_conversion_required == true) {
|
|
kst_float_to_q15_vector(pcm_buf_itr, buf_itr, frameSizeInWords);
|
|
} else {
|
|
memcpy(pcm_buf_itr, buf_itr, frame_sz_in_bytes);
|
|
}
|
|
#ifdef ENABLE_DEBUG_DUMPS
|
|
out_fp = fopen("/data/data/pcm_dump", "ab");
|
|
if (out_fp) {
|
|
ALOGE("Dumping to pcm_dump");
|
|
fwrite(pcm_buf_itr, (frameSizeInWords * 2), 1, out_fp);
|
|
fflush(out_fp);
|
|
fclose(out_fp);
|
|
} else {
|
|
ALOGE("Failed to open the out_fp file %s", strerror(errno));
|
|
ALOGE("out_fp is NULL");
|
|
}
|
|
#endif
|
|
}
|
|
|
|
static int parse_tunnel_buf(struct adnc_strm_device *adnc_strm_dev)
|
|
{
|
|
/*
|
|
* The magic number is ROME in ASCII reversed.
|
|
* So we are looking for EMOR in the byte stream
|
|
*/
|
|
const unsigned char magic_num[4] = {0x45, 0x4D, 0x4F, 0x52};
|
|
unsigned short int tunnel_id;
|
|
unsigned char *start_frame = NULL;
|
|
bool valid_frame = true;
|
|
unsigned char *buf_itr = adnc_strm_dev->unparsed_buf;
|
|
/*
|
|
* Minimum bytes required is
|
|
* magic number + tunnel id + reserved and crc + raf struct
|
|
*/
|
|
int min_bytes_req = 4 + 2 + 6 + sizeof(struct raf_frame_type);
|
|
int bytes_avail = adnc_strm_dev->unparsed_avail_size;
|
|
unsigned char *pcm_buf_itr = NULL;
|
|
int curr_pcm_frame_size;
|
|
bool is_q15_conversion_required = false;
|
|
|
|
if (buf_itr == NULL) {
|
|
ALOGE("Invalid input sent to parse_tunnel_buf");
|
|
return 0;
|
|
}
|
|
|
|
do {
|
|
// Check for MagicNumber 0x454D4F52
|
|
while (buf_itr[0] != magic_num[0] || buf_itr[1] != magic_num[1] ||
|
|
buf_itr[2] != magic_num[2] || buf_itr[3] != magic_num[3]) {
|
|
buf_itr++;
|
|
bytes_avail--;
|
|
if (bytes_avail <= 0) {
|
|
ALOGE("Could not find the magic number, reading again");
|
|
ALOGE("buf_itr[0] %x buf_itr[1] %x buf_itr[2] %x buf_itr[3] %x",
|
|
buf_itr[0], buf_itr[1], buf_itr[2], buf_itr[3]);
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
start_frame = buf_itr;
|
|
|
|
// Skip the magic number
|
|
buf_itr += 4;
|
|
bytes_avail -= 4;
|
|
|
|
// Read the tunnelID
|
|
tunnel_id = ((unsigned char) (buf_itr[0]) |
|
|
(unsigned char) (buf_itr[1]) << 8);
|
|
|
|
// Skip tunnelID
|
|
buf_itr += 2;
|
|
bytes_avail -= 2;
|
|
|
|
// Skip Reserved field and CRC - 6 bytes in total
|
|
buf_itr += 6;
|
|
bytes_avail -= 6;
|
|
|
|
valid_frame = true;
|
|
// There is only one tunnel data we are looking
|
|
if (tunnel_id > MAX_TUNNELS) {
|
|
ALOGE("Invalid tunnel id %d\n", tunnel_id);
|
|
valid_frame = false;
|
|
}
|
|
|
|
struct raf_frame_type rft;
|
|
memcpy(&rft, buf_itr, sizeof(struct raf_frame_type));
|
|
|
|
bool skip_extra_data = false;
|
|
if ((adnc_strm_dev->enable_stripping == true) &&
|
|
(rft.seqNo < adnc_strm_dev->kw_start_frame)) {
|
|
skip_extra_data = true;
|
|
}
|
|
|
|
/*
|
|
* 1 indicates that it is afloat encoding and
|
|
* F indicates it is in q15 encoding
|
|
*/
|
|
if (rft.format.encoding == 1) {
|
|
is_q15_conversion_required = true;
|
|
curr_pcm_frame_size = rft.format.frameSizeInBytes / 2;
|
|
} else {
|
|
is_q15_conversion_required = false;
|
|
curr_pcm_frame_size = rft.format.frameSizeInBytes;
|
|
}
|
|
|
|
// Skip the raf_frame_type
|
|
buf_itr += sizeof(struct raf_frame_type);
|
|
bytes_avail -= sizeof(struct raf_frame_type);
|
|
|
|
if (bytes_avail < rft.format.frameSizeInBytes) {
|
|
ALOGD("Incomplete frame received bytes_avail %d framesize %d",
|
|
bytes_avail, rft.format.frameSizeInBytes);
|
|
bytes_avail += min_bytes_req;
|
|
break;
|
|
}
|
|
|
|
if (valid_frame == true && skip_extra_data == false) {
|
|
if ((adnc_strm_dev->pcm_avail_size + curr_pcm_frame_size) <
|
|
adnc_strm_dev->pcm_buf_size) {
|
|
pcm_buf_itr = (unsigned char *)adnc_strm_dev->pcm_buf +
|
|
adnc_strm_dev->pcm_avail_size;
|
|
parse_audio_tunnel_data(buf_itr, pcm_buf_itr,
|
|
rft.format.frameSizeInBytes,
|
|
is_q15_conversion_required);
|
|
adnc_strm_dev->pcm_avail_size += curr_pcm_frame_size;
|
|
} else {
|
|
ALOGD("Not enough PCM buffer available break now");
|
|
bytes_avail += min_bytes_req;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Skip the data
|
|
buf_itr += rft.format.frameSizeInBytes;
|
|
bytes_avail -= rft.format.frameSizeInBytes;
|
|
} while (bytes_avail > min_bytes_req);
|
|
|
|
return bytes_avail;
|
|
}
|
|
|
|
|
|
__attribute__ ((visibility ("default")))
|
|
size_t adnc_strm_read(long handle, void *buffer, size_t bytes)
|
|
{
|
|
int ret = 0;
|
|
struct adnc_strm_device *adnc_strm_dev = (struct adnc_strm_device *) handle;
|
|
int bytes_read, bytes_rem;
|
|
|
|
if (adnc_strm_dev == NULL) {
|
|
ALOGE("Invalid handle");
|
|
ret = 0;
|
|
goto exit;
|
|
}
|
|
|
|
pthread_mutex_lock(&adnc_strm_dev->lock);
|
|
|
|
if (bytes > adnc_strm_dev->pcm_avail_size) {
|
|
/*
|
|
* We don't have enough PCM data, read more from the device.
|
|
* First copy the remainder of the PCM buffer to the front
|
|
* of the PCM buffer
|
|
*/
|
|
if (adnc_strm_dev->pcm_avail_size != 0) {
|
|
ALOGD("Copying to the front of the buffer pcm_avail_size %zu"
|
|
" pcm_read_offset %zu", adnc_strm_dev->pcm_avail_size,
|
|
adnc_strm_dev->pcm_read_offset);
|
|
memcpy(adnc_strm_dev->pcm_buf,
|
|
((unsigned char *)adnc_strm_dev->pcm_buf +
|
|
adnc_strm_dev->pcm_read_offset),
|
|
adnc_strm_dev->pcm_avail_size);
|
|
}
|
|
// Always read from the start of the PCM buffer at this point of time
|
|
adnc_strm_dev->pcm_read_offset = 0;
|
|
|
|
read_again:
|
|
// Read data from the kernel, account for the leftover
|
|
// data from previous run
|
|
bytes_read = ia_read_tunnel_data(adnc_strm_dev->tun_hdl,
|
|
(void *)((unsigned char *)
|
|
adnc_strm_dev->unparsed_buf +
|
|
adnc_strm_dev->unparsed_avail_size),
|
|
BUF_SIZE);
|
|
if (bytes_read <= 0) {
|
|
ALOGE("Failed to read data from tunnel");
|
|
ret = 0; // TODO should we try to read a couple of times?
|
|
pthread_mutex_unlock(&adnc_strm_dev->lock);
|
|
goto exit;
|
|
}
|
|
|
|
// Parse the data to get PCM data
|
|
adnc_strm_dev->unparsed_avail_size += bytes_read;
|
|
bytes_rem = parse_tunnel_buf(adnc_strm_dev);
|
|
|
|
#ifdef ENABLE_DEBUG_DUMPS
|
|
if (adnc_strm_dev->pcm_avail_size != 0) {
|
|
FILE *out_fp = fopen("/data/data/pcm_dump2", "ab");
|
|
if (out_fp) {
|
|
ALOGE("Dumping to pcm_dump2");
|
|
fwrite(((unsigned char *)adnc_strm_dev->pcm_buf +
|
|
adnc_strm_dev->pcm_avail_size),
|
|
adnc_strm_dev->pcm_avail_size, 1, out_fp);
|
|
fflush(out_fp);
|
|
fclose(out_fp);
|
|
} else {
|
|
ALOGE("Failed to open the pcm_dump2 file %s", strerror(errno));
|
|
}
|
|
}
|
|
#endif
|
|
|
|
// Copy the left over unparsed data to the front of the buffer
|
|
if (bytes_rem != 0) {
|
|
int offset = adnc_strm_dev->unparsed_avail_size - bytes_rem;
|
|
memcpy(adnc_strm_dev->unparsed_buf,
|
|
((unsigned char *)adnc_strm_dev->unparsed_buf + offset),
|
|
bytes_rem);
|
|
}
|
|
adnc_strm_dev->unparsed_avail_size = bytes_rem;
|
|
|
|
/*
|
|
* If stripping is enabled then we didn't read anything to the pcm
|
|
* bufferso read again or if we still don't have enough bytes then
|
|
* read data again.
|
|
*/
|
|
if (adnc_strm_dev->pcm_avail_size == 0 ||
|
|
adnc_strm_dev->pcm_avail_size < bytes) {
|
|
goto read_again;
|
|
}
|
|
}
|
|
|
|
// Copy the PCM data to output buffer and return
|
|
memcpy(buffer,
|
|
((unsigned char *)adnc_strm_dev->pcm_buf +
|
|
adnc_strm_dev->pcm_read_offset),
|
|
bytes);
|
|
|
|
#ifdef ENABLE_DEBUG_DUMPS
|
|
char l_buffer[64];
|
|
int cx;
|
|
FILE *out_fp = NULL;
|
|
cx = snprintf(l_buffer, sizeof(l_buffer), "/data/data/adnc_dump_%x",
|
|
adnc_strm_dev->end_point);
|
|
if (cx >= 0 && cx < 64)
|
|
out_fp = fopen(l_buffer, "ab");
|
|
if (out_fp) {
|
|
ALOGD("Dumping to adnc_dump:%s", l_buffer);
|
|
fwrite(buffer, bytes, 1, out_fp);
|
|
fflush(out_fp);
|
|
fclose(out_fp);
|
|
} else {
|
|
ALOGE("Failed to open the adnc_dump file %s", strerror(errno));
|
|
}
|
|
#endif
|
|
|
|
adnc_strm_dev->pcm_avail_size -= bytes;
|
|
adnc_strm_dev->pcm_read_offset += bytes;
|
|
|
|
pthread_mutex_unlock(&adnc_strm_dev->lock);
|
|
|
|
exit:
|
|
|
|
return bytes;
|
|
}
|
|
|
|
|
|
__attribute__ ((visibility ("default")))
|
|
long adnc_strm_open(bool enable_stripping,
|
|
unsigned int kw_start_frame,
|
|
int stream_end_point)
|
|
{
|
|
int ret = 0, err;
|
|
struct adnc_strm_device *adnc_strm_dev = NULL;
|
|
|
|
adnc_strm_dev = (struct adnc_strm_device *)
|
|
calloc(1, sizeof(struct adnc_strm_device));
|
|
if (adnc_strm_dev == NULL) {
|
|
ALOGE("Failed to allocate memory for adnc_strm_dev");
|
|
ret = 0;
|
|
goto exit_no_memory;
|
|
}
|
|
|
|
pthread_mutex_init(&adnc_strm_dev->lock, (const pthread_mutexattr_t *) NULL);
|
|
|
|
pthread_mutex_lock(&adnc_strm_dev->lock);
|
|
|
|
adnc_strm_dev->end_point = stream_end_point;
|
|
adnc_strm_dev->idx = 0;
|
|
adnc_strm_dev->mode = 0;
|
|
adnc_strm_dev->encode = TNL_Q15;
|
|
adnc_strm_dev->enable_stripping = enable_stripping;
|
|
adnc_strm_dev->kw_start_frame = kw_start_frame;
|
|
adnc_strm_dev->tun_hdl = NULL;
|
|
adnc_strm_dev->pcm_buf = NULL;
|
|
adnc_strm_dev->unparsed_buf = NULL;
|
|
|
|
adnc_strm_dev->tun_hdl = ia_start_tunneling(640);
|
|
if (adnc_strm_dev->tun_hdl == NULL) {
|
|
ALOGE("Failed to start tunneling");
|
|
ret = 0;
|
|
goto exit_on_error;
|
|
}
|
|
|
|
ret = ia_enable_tunneling_source(adnc_strm_dev->tun_hdl,
|
|
adnc_strm_dev->end_point,
|
|
adnc_strm_dev->mode,
|
|
adnc_strm_dev->encode);
|
|
if (ret != 0) {
|
|
ALOGE("Failed to enable tunneling for CVQ tunl_id %u src_id %u mode %u",
|
|
adnc_strm_dev->idx, adnc_strm_dev->end_point, adnc_strm_dev->mode);
|
|
ret = 0;
|
|
goto exit_on_error;
|
|
}
|
|
|
|
adnc_strm_dev->unparsed_buf_size = BUF_SIZE * 2;
|
|
adnc_strm_dev->unparsed_avail_size = 0;
|
|
adnc_strm_dev->unparsed_buf = malloc(adnc_strm_dev->unparsed_buf_size);
|
|
if (adnc_strm_dev->unparsed_buf == NULL) {
|
|
ret = 0;
|
|
ALOGE("Failed to allocate memory for unparsed buffer");
|
|
goto exit_on_error;
|
|
}
|
|
|
|
adnc_strm_dev->pcm_buf_size = BUF_SIZE * 2;
|
|
adnc_strm_dev->pcm_avail_size = 0;
|
|
adnc_strm_dev->pcm_read_offset = 0;
|
|
adnc_strm_dev->pcm_buf = malloc(adnc_strm_dev->pcm_buf_size);
|
|
if (adnc_strm_dev->pcm_buf == NULL) {
|
|
ret = 0;
|
|
ALOGE("Failed to allocate memory for pcm buffer");
|
|
goto exit_on_error;
|
|
}
|
|
|
|
pthread_mutex_unlock(&adnc_strm_dev->lock);
|
|
|
|
return (long)adnc_strm_dev;
|
|
|
|
exit_on_error:
|
|
if (adnc_strm_dev->pcm_buf) {
|
|
free(adnc_strm_dev->pcm_buf);
|
|
}
|
|
|
|
if (adnc_strm_dev->unparsed_buf) {
|
|
free(adnc_strm_dev->unparsed_buf);
|
|
}
|
|
|
|
err = ia_disable_tunneling_source(adnc_strm_dev->tun_hdl,
|
|
adnc_strm_dev->end_point,
|
|
adnc_strm_dev->mode,
|
|
adnc_strm_dev->encode);
|
|
if (err != 0) {
|
|
ALOGE("Failed to disable the tunneling source");
|
|
}
|
|
|
|
err = ia_stop_tunneling(adnc_strm_dev->tun_hdl);
|
|
if (err != 0) {
|
|
ALOGE("Failed to stop tunneling");
|
|
}
|
|
|
|
pthread_mutex_unlock(&adnc_strm_dev->lock);
|
|
|
|
if (adnc_strm_dev) {
|
|
free(adnc_strm_dev);
|
|
}
|
|
|
|
exit_no_memory:
|
|
|
|
return ret;
|
|
}
|
|
|
|
__attribute__ ((visibility ("default")))
|
|
int adnc_strm_close(long handle)
|
|
{
|
|
int ret = 0;
|
|
struct adnc_strm_device *adnc_strm_dev = (struct adnc_strm_device *) handle;
|
|
|
|
if (adnc_strm_dev == NULL) {
|
|
ALOGE("Invalid handle");
|
|
ret = -1;
|
|
goto exit;
|
|
}
|
|
|
|
pthread_mutex_lock(&adnc_strm_dev->lock);
|
|
|
|
if (adnc_strm_dev->pcm_buf) {
|
|
free(adnc_strm_dev->pcm_buf);
|
|
}
|
|
|
|
if (adnc_strm_dev->unparsed_buf) {
|
|
free(adnc_strm_dev->unparsed_buf);
|
|
}
|
|
|
|
ret = ia_disable_tunneling_source(adnc_strm_dev->tun_hdl,
|
|
adnc_strm_dev->end_point,
|
|
adnc_strm_dev->mode,
|
|
adnc_strm_dev->encode);
|
|
if (ret != 0) {
|
|
ALOGE("Failed to disable the tunneling source");
|
|
}
|
|
|
|
ret = ia_stop_tunneling(adnc_strm_dev->tun_hdl);
|
|
if (ret != 0) {
|
|
ALOGE("Failed to stop tunneling");
|
|
}
|
|
|
|
pthread_mutex_unlock(&adnc_strm_dev->lock);
|
|
|
|
if (adnc_strm_dev) {
|
|
free(adnc_strm_dev);
|
|
}
|
|
|
|
exit:
|
|
return ret;
|
|
}
|
|
|