tests/tunnel_test_sensor.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 <stdlib.h>
#include <string.h>
#include <signal.h>
#include <stdbool.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <unistd.h>

#define LOG_TAG "ia_tunneling_sensor_test"

#include <cutils/log.h>
#include <linux/mfd/adnc/iaxxx-system-identifiers.h>
#include <linux/mfd/adnc/iaxxx-tunnel-intf.h>
#include <linux/mfd/adnc/iaxxx-sensor-tunnel.h>

#define MAX_TUNNELS                 32
#define BUF_SIZE                    8192
#define OUTPUT_FILE                 "/data/data/tnl_op"
#define UNPARSED_OUTPUT_FILE        "/data/data/unparsed_output"

#define TUNNEL_DEVICE               "/dev/sensor_tunnel"
#define SENSOR_TUNNEL_SOURCE_ID     IAXXX_SYSID_CHANNEL_RX_15_EP_0
#define SENSOR_TUNNEL_MODE          TNL_MODE_ASYNC
#define SENSOR_TUNNEL_ENCODE        TNL_ENC_Q15
#define VSYNC_SENSOR_SOURCE_ID      IAXXX_SYSID_SENSOR_OUTPUT_1
#define VSYNC_SENSOR_MODE           TNL_MODE_ASYNC
#define VSYNC_SENSOR_ENCODE         TNL_ENC_OPAQUE

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

volatile int capturing = 1;

void sigint_handler(int sig __unused) {
    ALOGE("Interrupted, setting the exit condition");
    capturing = 0;
}

int main(int argc, char *argv[]) {
    int err = 0;
    FILE *tun_dev = NULL;
    FILE *out_fp[MAX_TUNNELS] = { NULL };
    FILE *unp_out_fp = NULL;
    int bytes_avail = 0, bytes_rem = 0;
    int bytes_read = 0;
    void *buf = NULL;
    // 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};
    int i = 0;
    bool valid_frame = true;
    int timer_signal = 0;
    int lastSeqNum[MAX_TUNNELS] = { 0 };
    int notFirstFrame[MAX_TUNNELS] = { 0 };
    int frameDropCount[MAX_TUNNELS] = { 0 };
    uint64_t tunnel_time_stamps[MAX_TUNNELS] = { 0 };
    unsigned char *frame_start, *buf_itr;
    // Minimum bytes required is the magic number + tunnel id + reserved and
    // crc + raf struct
    int min_bytes_req = 4 + 2 + 6 + sizeof(struct raf_frame_type);
    struct tunlMsg tm;

    if (argc < 2) {
        ALOGE("USAGE: %s <Time in seconds> ", argv[0]);
        return -EINVAL;
    }

    timer_signal= strtol(argv[1], NULL, 0);
    ALOGD("tunnel out timer based req %d", timer_signal);

    tun_dev = fopen(TUNNEL_DEVICE, "r");
    if (NULL == tun_dev) {
        ALOGE("Failed to open the tunnel device node");
        goto exit;
    }

    err = ioctl(fileno(tun_dev), FLICKER_ROUTE_SETUP, NULL);
    if (err == -1) {
        ALOGE("%s: ERROR Tunnel setup failed for VSYNC sensor %s", __func__, strerror(errno));
        goto exit;
    }

    tm.tunlSrc = VSYNC_SENSOR_SOURCE_ID;
    tm.tunlMode = VSYNC_SENSOR_MODE;
    tm.tunlEncode = VSYNC_SENSOR_ENCODE;
    err = ioctl(fileno(tun_dev), FLICKER_TUNNEL_SETUP, &tm);
    if (err == -1) {
        ALOGE("%s: ERROR Tunnel setup failed for VSYNC sensor %s", __func__, strerror(errno));
        goto exit;
    }

    tm.tunlSrc = SENSOR_TUNNEL_SOURCE_ID;
    tm.tunlMode = SENSOR_TUNNEL_MODE;
    tm.tunlEncode = SENSOR_TUNNEL_ENCODE;
    err = ioctl(fileno(tun_dev), FLICKER_TUNNEL_SETUP, &tm);
    if (err == -1) {
        ALOGE("%s: ERROR Tunnel setup failed %s", __func__, strerror(errno));
        goto exit;
    }

    buf = malloc(BUF_SIZE * 2);
    if (NULL == buf) {
        ALOGE("Failed to allocate memory to read buffer");
        goto exit;
    }

    unp_out_fp = fopen(UNPARSED_OUTPUT_FILE, "wb");
    if (NULL == unp_out_fp) {
        ALOGE("Failed to open the file %s", UNPARSED_OUTPUT_FILE);
        goto exit;
    }

    signal(SIGINT, sigint_handler);

    if (timer_signal) {
        signal(SIGALRM, sigint_handler);
        alarm(timer_signal);
    }

    unsigned short int tunnel_id;
    unsigned short int tunl_src;
    while (1) {
read_again:
        if (0 == capturing) {
            ALOGE("Time to bail from here");
            break;
        }

        if (0 != bytes_avail) {
            if (bytes_avail < 0) {
                bytes_rem = 0;
            } else {
                bytes_rem = bytes_avail;
                ALOGD("bytes_avail is %d", bytes_rem);
                memcpy(buf, buf_itr, bytes_rem);
            }
        } else {
            bytes_rem = 0;
        }

        // Ensure that we read BUF_SIZE always otherwise kernel read will hang
        bytes_avail = fread((void *)((unsigned char *)buf + bytes_rem),
                             1, BUF_SIZE, tun_dev);
        if (bytes_avail <= 0) {
            ALOGE("Failed to read data from the tunnel");
            break;
        }

        fwrite((void *)((unsigned char *)buf + bytes_rem),
                bytes_avail, 1, unp_out_fp);
        fflush(unp_out_fp);

        // update the available bytes with the previous reminder if any
        bytes_avail += bytes_rem;
        ALOGD("bytes_avail is after read %d", bytes_avail);
        buf_itr = (unsigned char *)buf;

        do {
            // Check for MagicNumber 0x454D4F52
            if (buf_itr[0] != magic_num[0] || buf_itr[1] != magic_num[1] ||
                    buf_itr[2] != magic_num[2] || buf_itr[3] != magic_num[3]) {
                  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]);
                  goto exit;
            }
            ALOGD("bytes_avail is after magic %d: prev :%d",
                    bytes_avail, bytes_avail + 540);
            // Bookmark the start of the frame
            frame_start = 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;

            tunl_src = ((unsigned char) (buf_itr[0]) |
                         (unsigned char) (buf_itr[1]) << 8);

            // Skip src id field and CRC - 6 bytes in total
            buf_itr += 6;
            bytes_avail -= 6;

            valid_frame = true;
            if (tunnel_id > MAX_TUNNELS) {
                ALOGE("Invalid tunnel id %d", tunnel_id);
                valid_frame = false;
            }

            struct raf_frame_type rft;
            memcpy(&rft, buf_itr, sizeof(struct raf_frame_type));
            if (true == valid_frame) {
                if (NULL == out_fp[tunnel_id]) {
                    char filename[256];
                    snprintf(filename, 256,
                             "%sid%d-src0x%x-enc0x%x.raw",
                             OUTPUT_FILE, tunnel_id,
                             tunl_src, rft.format.encoding);
                    // Open the file to dump
                    out_fp[tunnel_id] = fopen(filename, "wb");
                    if (NULL == out_fp[tunnel_id]) {
                        ALOGE("ERROR: Failed to open the file %s", filename);
                        goto exit;
                    }
                }
            }

            ALOGD("Tunnel id %d timestamp %llu", tunnel_id, rft.timeStamp);
            tunnel_time_stamps[tunnel_id] = rft.timeStamp;

            // 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);
                buf_itr = frame_start;
                bytes_avail += min_bytes_req;
                goto read_again;
            }

            if (true == valid_frame) {
                ALOGD("@@@Tunnel id %d encoding %d",
                        tunnel_id, rft.format.encoding);
                fwrite(buf_itr, rft.format.frameSizeInBytes, 1, out_fp[tunnel_id]);
            }

            /* Calculate the frame drop count */
            if (notFirstFrame[tunnel_id]) {
                   frameDropCount[tunnel_id] +=
                            (rft.seqNo - lastSeqNum[tunnel_id] - 1);
            }
            lastSeqNum[tunnel_id] = rft.seqNo;
            notFirstFrame[tunnel_id] = 1;
            // Skip the data
            buf_itr += rft.format.frameSizeInBytes;
            bytes_avail -= rft.format.frameSizeInBytes;
            bytes_read += rft.format.frameSizeInBytes + min_bytes_req;
        } while (bytes_avail > min_bytes_req);
    }

exit:
    for (i = 0; i < MAX_TUNNELS; i++) {
        if (notFirstFrame[i]) {
            ALOGD("drop count tunnel id %u: %u", i, frameDropCount[i]);
        }
    }
    ALOGD("bytes_read so far %d", bytes_read);
    if (buf) {
        free(buf);
        buf = NULL;
    }

    if (unp_out_fp) {
        fflush(unp_out_fp);
        fclose(unp_out_fp);
    }

    for (i = 0; i < MAX_TUNNELS; i++) {
        if (out_fp[i]) {
            fflush(out_fp[i]);
            fclose(out_fp[i]);
        }
    }

    tm.tunlSrc = SENSOR_TUNNEL_SOURCE_ID;
    tm.tunlMode = SENSOR_TUNNEL_MODE;
    tm.tunlEncode = SENSOR_TUNNEL_ENCODE;
    err = ioctl(fileno(tun_dev), FLICKER_TUNNEL_TERMINATE, &tm);
    if (err == -1) {
        ALOGE("%s: ERROR Tunnel terminate failed %s",
                __func__, strerror(errno));
    }

    tm.tunlSrc = VSYNC_SENSOR_SOURCE_ID;
    tm.tunlMode = VSYNC_SENSOR_MODE;
    tm.tunlEncode = VSYNC_SENSOR_ENCODE;
    err = ioctl(fileno(tun_dev), FLICKER_TUNNEL_TERMINATE, &tm);
    if (err == -1) {
        ALOGE("%s: ERROR Tunnel terminate failed %s",
                __func__, strerror(errno));
    }

    err = ioctl(fileno(tun_dev), FLICKER_ROUTE_TERMINATE, NULL);
    if (err == -1) {
        ALOGE("%s: ERROR Tunnel terminate failed %s",
                __func__, strerror(errno));
    }

    if (tun_dev)
        fclose(tun_dev);

    return 0;
}