tests/oslo_data_injection_test.c

/*
 * Copyright (C) 2019 The Android Open Source Project
 *
 * 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.
 */

#define LOG_TAG "oslo_data_injection_test"

#include <errno.h>
#include <getopt.h>
#include <inttypes.h>
#include <limits.h>
#include <log/log.h>
#include <math.h>
#include <pthread.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

#include "oslo_iaxxx_sensor_control.h"

#define FRAME_SIZE_MAX (16 * 1024)
#define FRAME_PERIOD_MS_MAX (1000)
#define INJECT_BYTES_PER_SEC_MAX (400 * 1024)  // Reach: 12672 Bytes * 30 Hz = 380160 Bytes/Sec
#define CHANNEL_NUM 3
#define BYTES_PER_SAMPLE 2

enum oslo_header_index {
    OSLO_HEADER_INDEX_SYNC_0 = 0,
    OSLO_HEADER_INDEX_SYNC_1,
    OSLO_HEADER_INDEX_FRAME_COUNT,
    OSLO_HEADER_INDEX_SHAPE_GROUP_COUNT,
    OSLO_HEADER_INDEX_CHIRP_LENGTH,
    OSLO_HEADER_INDEX_SADC_VAL,
    OSLO_HEADER_INDEX_NUM,
};

const uint16_t SYNC_WORD = 0x0000;

static struct frame_sync {
    pthread_mutex_t mutex;
    pthread_cond_t condition;
    bool ready;
} f_sync = {PTHREAD_MUTEX_INITIALIZER, PTHREAD_COND_INITIALIZER, false};

static void frame_sync_signal(struct frame_sync *fs) {
    pthread_mutex_lock(&fs->mutex);
    fs->ready = true;
    pthread_cond_signal(&fs->condition);
    pthread_mutex_unlock(&fs->mutex);
}

static void frame_sync_wait(struct frame_sync *fs) {
    int ret;
    pthread_mutex_lock(&fs->mutex);
    while (!fs->ready) {
        ret = pthread_cond_wait(&fs->condition, &fs->mutex);
    }
    fs->ready = false;
    pthread_mutex_unlock(&fs->mutex);
}

static void frame_sync_timer_handler(int signum __unused) {
    ALOGV("%s", __func__);
    frame_sync_signal(&f_sync);
}

static void frame_sync_timer_enable(bool en, uint32_t period_ms) {
    struct itimerval tv;
    struct sigaction sa;

    memset(&sa, 0, sizeof(sa));
    memset(&tv, 0, sizeof(tv));

    if (en) {
        sa.sa_handler = frame_sync_timer_handler;
        sigaction(SIGALRM, &sa, NULL);

        tv.it_interval.tv_sec = period_ms / 1000;
        tv.it_interval.tv_usec = period_ms * 1000;
        tv.it_value.tv_sec = period_ms / 1000;
        tv.it_value.tv_usec = period_ms * 1000;
        setitimer(ITIMER_REAL, &tv, NULL);
    } else {
        setitimer(ITIMER_REAL, &tv, NULL);

        sa.sa_handler = SIG_DFL;
        sigaction(SIGALRM, &sa, NULL);
    }
}

static bool frame_data_validate(const uint8_t *data, size_t frame_size) {
    uint16_t *header = (uint16_t *)data;
    uint16_t frame_cnt;
    uint16_t shape_group_count;
    uint16_t chirp_len;
    size_t chirp_size;
    size_t num_chirps_per_frame;

    // Validate 1st header of frame
    if (header[OSLO_HEADER_INDEX_SYNC_0] != SYNC_WORD ||
        header[OSLO_HEADER_INDEX_SYNC_1] != SYNC_WORD ||
        header[OSLO_HEADER_INDEX_SHAPE_GROUP_COUNT] != 0) {
        ALOGE("%s: Invalid frame header: %04x, %04x, %04x, %04x, %04x, %04x\n", __func__,
              header[0], header[1], header[2], header[3], header[4], header[5]);
        fprintf(stderr, "%s: Invalid frame header: %04x, %04x, %04x, %04x, %04x, %04x\n", __func__,
                header[0], header[1], header[2], header[3], header[4], header[5]);
        return false;
    } else {
        frame_cnt = header[OSLO_HEADER_INDEX_FRAME_COUNT];
        shape_group_count = header[OSLO_HEADER_INDEX_SHAPE_GROUP_COUNT];
        chirp_len = header[OSLO_HEADER_INDEX_CHIRP_LENGTH];
        chirp_size = chirp_len * CHANNEL_NUM * BYTES_PER_SAMPLE;
        chirp_size += OSLO_HEADER_INDEX_NUM * BYTES_PER_SAMPLE;
        num_chirps_per_frame = frame_size / chirp_size;
        ALOGD("%s: frame_cnt: 0x%04x, chirp_len: %d, chirp_size: %d, num_chirps_per_frame: %d\n",
              __func__, frame_cnt, chirp_len, chirp_size, num_chirps_per_frame);
    }

    if (frame_size % chirp_size != 0) {
        ALOGE("frame_size (%d) is not a multiple of the chirp_size (%d)!!!\n", frame_size,
              chirp_size);
        fprintf(stderr, "frame_size (%d) is not a multiple of the chirp_size (%d)!!!\n",
                frame_size, chirp_size);
        return false;
    }

    // Validate header of each chirp
    for (int i = 0; i < num_chirps_per_frame; i++) {
        if (header[OSLO_HEADER_INDEX_SYNC_0] != SYNC_WORD ||
            header[OSLO_HEADER_INDEX_SYNC_1] != SYNC_WORD ||
            header[OSLO_HEADER_INDEX_FRAME_COUNT] != frame_cnt ||
            header[OSLO_HEADER_INDEX_SHAPE_GROUP_COUNT] != i ||
            header[OSLO_HEADER_INDEX_CHIRP_LENGTH] != chirp_len) {
            ALOGE("%s: Invalid %dth header: %04x, %04x, %04x, %04x, %04x, %04x\n", __func__, i,
                  header[0], header[1], header[2], header[3], header[4], header[5]);
            fprintf(stderr, "%s: Invalid %dth chirp header: %04x, %04x, %04x, %04x, %04x, %04x\n",
                    __func__, i, header[0], header[1], header[2], header[3], header[4], header[5]);
            return false;
        }
        data += chirp_size;
        header = (uint16_t *)data;
    }

    return true;
}

static int show_help() {
    fprintf(stdout,
            "usage: oslo_data_injection_test <file_name> <frame_period_ms> <frame_size> [-s n] [-c n]\n"
            "\n"
            "-s n  Skip n input frames\n"
            "-c n  Inject only n input frames\n");
    exit(EXIT_FAILURE);
}

int main(int argc, char *argv[]) {
    int ret = 0;
    int frame_size = 0;
    int frame_period_ms = 0;
    char *file_path = NULL;
    uint8_t frame_data_buf[FRAME_SIZE_MAX] = {0};
    uint8_t *data_ptr;
    uint16_t *header = (uint16_t *)frame_data_buf;
    uint32_t frames_injected = 0;
    uint32_t frames_processed_pre_inject = 0;
    uint32_t frames_processed_post_inject = 0;
    size_t bytes_written;
    size_t frame_data_bytes_remaining;
    struct ia_sensor_mgr *smd;
    FILE *fid = NULL;
    struct stat file_stat;
    uint32_t file_size;
    uint32_t inject_bytes_per_sec;
    uint32_t skip_frames = 0;
    uint32_t max_inject_frames = UINT32_MAX;
    int c;

    const struct option long_options[] = {{"skip", required_argument, NULL, 's'},
                                          {"count", required_argument, NULL, 'c'},
                                          {NULL, 0, NULL, 0}};

    if (argc < 4) {
        show_help();
    } else {
        file_path = argv[1];
        frame_period_ms = strtol(argv[2], NULL, 0);
        frame_size = strtol(argv[3], NULL, 0);
    }

    while ((c = getopt_long(argc, argv, "s:c:", long_options, NULL)) != -1) {
        switch (c) {
            case 's':
                if (NULL == optarg) {
                    show_help();
                } else {
                    skip_frames = strtoul(optarg, NULL, 0);
                }
                break;
            case 'c':
                if (NULL == optarg) {
                    show_help();
                } else {
                    max_inject_frames = strtoul(optarg, NULL, 0);
                }
                break;
            default:
                show_help();
                break;
        }
    }

    if (frame_period_ms < 0 || frame_period_ms > FRAME_PERIOD_MS_MAX) {
        fprintf(stderr, "Invalid frame_period_ms:%d\n", frame_period_ms);
        ret = -EINVAL;
        goto out;
    }

    if (frame_size < 0 || frame_size > FRAME_SIZE_MAX) {
        fprintf(stderr, "Invalid frame_size:%d\n", frame_size);
        ret = -EINVAL;
        goto out;
    }

    inject_bytes_per_sec = frame_size * 1000 / frame_period_ms;
    if (inject_bytes_per_sec > INJECT_BYTES_PER_SEC_MAX) {
        fprintf(stderr, "Invalid bytes_per_sec:%d\n", inject_bytes_per_sec);
        ret = -EINVAL;
        goto out;
    }

    if (stat(file_path, &file_stat) == -1) {
        fprintf(stderr, "Could not stat file: %s - %s\n", file_path, strerror(errno));
        ret = -errno;
        goto out;
    }
    file_size = file_stat.st_size;

    smd = iaxxx_sensor_mgr_init();
    if (NULL == smd) {
        ALOGE("%s: ERROR Failed to init ia_sensor_mgr", __func__);
        ret = -ENOMEM;
        goto out;
    }

    frames_processed_pre_inject = oslo_driver_get_param(smd, SENSOR_PARAM_FRAMES_PROCESSED);

    fid = fopen(file_path, "r");
    if (fid == NULL) {
        ALOGE("Cannot open '%s' (%s)\n", file_path, strerror(errno));
        fprintf(stdout, "Cannot open '%s' (%s)\n", file_path, strerror(errno));
        ret = -errno;
        goto out_err_fopen;
    }

    frame_sync_timer_enable(true, frame_period_ms);

    while (!feof(fid)) {
        frame_data_bytes_remaining = fread(frame_data_buf, 1, frame_size, fid);
        if (!frame_data_bytes_remaining) {
            ALOGE("Zero bytes read\n");
            break;
        }
        if (frame_data_bytes_remaining != frame_size ||
            !frame_data_validate(frame_data_buf, frame_size)) {
            ALOGE("Drop invalid frame, header: %04x, %04x, %04x, %04x, %04x, %04x\n", header[0],
                  header[1], header[2], header[3], header[4], header[5]);
            fprintf(stdout, "Drop invalid frame, header: %04x, %04x, %04x, %04x, %04x, %04x\n",
                    header[0], header[1], header[2], header[3], header[4], header[5]);
            continue;
        }
        if (skip_frames > 0) {
            ALOGD("Num of frames to skip %u\n", skip_frames);
            skip_frames--;
            continue;
        }
        if (frames_injected >= max_inject_frames) {
            ALOGD("Already injected %u frames, STOP\n", frames_injected);
            break;
        }
        data_ptr = frame_data_buf;
        while (frame_data_bytes_remaining) {
            bytes_written = oslo_driver_set_param_blk(smd, PARAM_BLK_ID_FRAME_DATA_INJECTION,
                                                      data_ptr, frame_data_bytes_remaining);
            if (bytes_written == 0) {
                ALOGE("Failed to inject data!!!\n");
                break;
            }
            frame_data_bytes_remaining -= bytes_written;
            data_ptr += bytes_written;
        }
        frame_sync_wait(&f_sync);
        if (frames_injected % 10 == 0)
            ALOGD("Process injected frame %d, header: %04x, %04x, %04x, %04x, %04x, %04x\n",
                  frames_injected, header[0], header[1], header[2], header[3], header[4],
                  header[5]);
        oslo_driver_set_param(smd, OSLO_CONTROL_INJECT_FRAME_READY, 1);
        frames_injected++;
    }

    frame_sync_timer_enable(false, 0);

    if (fid)
        fclose(fid);

    frames_processed_post_inject = oslo_driver_get_param(smd, SENSOR_PARAM_FRAMES_PROCESSED);

    ALOGD("A total of %d frames were injected\n", frames_injected);
    ALOGD("Frames processed pre:%d, post:%d\n", frames_processed_pre_inject,
          frames_processed_post_inject);

out_err_fopen:
    iaxxx_sensor_mgr_deinit(smd);
out:

    return ret;
}