v811_spc009/hardware/knowles/athletico/sound_trigger_hal/adnc_strm.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;
}