/*
 * Copyright (C) 2016 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.
 */

#include <inttypes.h>
#include <stdint.h>
#include <sys/endian.h>
#include <string.h>
#include <alloca.h>

#include <variant/variant.h>
#include <eventnums.h>

#include <plat/pwr.h>

#include <nanohub/crc.h>

#include <platform.h>
#include <cpu.h>
#include <halIntf.h>
#include <hostIntf.h>
#include <hostIntf_priv.h>
#include <nanohubCommand.h>
#include <nanohubPacket.h>
#include <seos.h>
#include <seos_priv.h>
#include <util.h>
#include <atomicBitset.h>
#include <atomic.h>
#include <gpio.h>
#include <apInt.h>
#include <sensors.h>
#include <timer.h>
#include <heap.h>
#include <simpleQ.h>

#define HOSTINTF_MAX_ERR_MSG    8
#define MAX_NUM_BLOCKS          280         /* times 256 = 71680 bytes */
#define MIN_NUM_BLOCKS          10          /* times 256 = 2560 bytes */
#define SENSOR_INIT_DELAY       500000000   /* ns */
#define SENSOR_INIT_ERROR_MAX   4
#define CHECK_LATENCY_TIME      500000000   /* ns */
#define EVT_LATENCY_TIMER       EVT_NO_FIRST_USER_EVENT

static const uint32_t delta_time_multiplier_order = 9;
static const uint32_t delta_time_coarse_mask = ~1;
static const uint32_t delta_time_fine_mask = 1;
static const uint32_t delta_time_rounding = 0x200;      /* 1ul << delta_time_multiplier_order */
static const uint64_t delta_time_max = 0x1FFFFFFFE00;   /* UINT32_MAX << delta_time_multiplier_order */

enum ConfigCmds
{
    CONFIG_CMD_DISABLE      = 0,
    CONFIG_CMD_ENABLE       = 1,
    CONFIG_CMD_FLUSH        = 2,
    CONFIG_CMD_CFG_DATA     = 3,
    CONFIG_CMD_CALIBRATE    = 4,
    CONFIG_CMD_SELF_TEST    = 5,
};

struct ConfigCmd
{
    uint64_t latency;
    uint32_t rate;
    uint8_t sensType;
    uint8_t cmd;
    uint16_t flags;
} __attribute__((packed));

struct ActiveSensor
{
    uint64_t latency;
    uint64_t firstTime;
    uint64_t lastTime;
    struct HostIntfDataBuffer buffer;
    uint32_t rate;
    uint32_t sensorHandle;
    float rawScale;
    uint16_t minSamples;
    uint16_t curSamples;
    uint8_t numAxis;
    uint8_t interrupt;
    uint8_t numSamples;
    uint8_t packetSamples;
    // The sensorType used to report bias samples; normally the same as
    // buffer.sensorType, but in the case of raw, this gets set to the base
    // sensorType matching struct SensorInfo (because the sensor can have a
    // different rawType). Note that this is different than biasType in struct
    // SensorInfo.
    uint8_t biasReportType;
    uint8_t oneshot : 1;
    uint8_t discard : 1;
    uint8_t raw : 1;
    uint8_t reserved : 5;
} __attribute__((packed));

static uint8_t mSensorList[SENS_TYPE_LAST_USER];
static struct SimpleQueue *mOutputQ;
static struct ActiveSensor *mActiveSensorTable;
static uint8_t mNumSensors;
static uint8_t mLastSensor;

static const struct HostIntfComm *mComm;
static bool mBusy;
static uint64_t mRxTimestamp;
static uint8_t mRxBuf[NANOHUB_PACKET_SIZE_MAX];
static size_t mRxSize;
static struct
{
    const struct NanohubCommand *cmd;
    uint32_t seq;
    bool seqMatch;
} mTxRetrans;
static struct
{
    uint8_t pad; // packet header is 10 bytes. + 2 to word align
    uint8_t prePreamble;
    uint8_t buf[NANOHUB_PACKET_SIZE_MAX];
    uint8_t postPreamble;
} mTxBuf;
static struct
{
    uint8_t pad; // packet header is 10 bytes. + 2 to word align
    uint8_t prePreamble;
    uint8_t buf[NANOHUB_PACKET_SIZE_MIN];
    uint8_t postPreamble;
} mTxNakBuf;
static size_t mTxSize;
static uint8_t *mTxBufPtr;
static const struct NanohubCommand *mRxCmd;
ATOMIC_BITSET_DECL(mInterrupt, HOSTINTF_MAX_INTERRUPTS, static);
ATOMIC_BITSET_DECL(mInterruptMask, HOSTINTF_MAX_INTERRUPTS, static);
static uint32_t mInterruptCntWkup, mInterruptCntNonWkup;
static uint32_t mWakeupBlocks, mNonWakeupBlocks, mTotalBlocks;
static uint32_t mHostIntfTid;
static uint32_t mLatencyTimer;
static uint8_t mLatencyCnt;

static uint8_t mRxIdle;
static uint8_t mWakeActive;
static uint8_t mActiveWrite;
static uint8_t mRestartRx;
static uint8_t mIntErrMsgIdx;
static volatile uint32_t mIntErrMsgCnt;

enum hostIntfIntErrReason
{
    HOSTINTF_ERR_PKG_INCOMPELETE = 0,
    HOSTINTF_ERR_PGK_SIZE,
    HOSTINTF_ERR_PKG_PAYLOAD_SIZE,
    HOSTINTF_ERR_PKG_CRC,
    HOSTINTF_ERR_RECEIVE,
    HOSTINTF_ERR_SEND,
    HOSTINTF_ERR_ACK,
    HOSTINTF_ERR_NAK,
    HOSTINTF_ERR_UNKNOWN
};

struct hostIntfIntErrMsg
{
    enum LogLevel level;
    enum hostIntfIntErrReason reason;
    const char *func;
};
static struct hostIntfIntErrMsg mIntErrMsg[HOSTINTF_MAX_ERR_MSG];

static void hostIntfTxPacket(uint32_t reason, uint8_t len, uint32_t seq,
        HostIntfCommCallbackF callback);

static void hostIntfRxDone(size_t rx, int err);
static void hostIntfGenerateAck(void *cookie);

static void hostIntfTxAckDone(size_t tx, int err);
static void hostIntfGenerateResponse(void *cookie);

static void hostIntfTxPayloadDone(size_t tx, int err);

static inline void *hostIntfGetPayload(uint8_t *buf)
{
    struct NanohubPacket *packet = (struct NanohubPacket *)buf;
    return packet->data;
}

static inline uint8_t hostIntfGetPayloadLen(uint8_t *buf)
{
    struct NanohubPacket *packet = (struct NanohubPacket *)buf;
    return packet->len;
}

static inline struct NanohubPacketFooter *hostIntfGetFooter(uint8_t *buf)
{
    struct NanohubPacket *packet = (struct NanohubPacket *)buf;
    return (struct NanohubPacketFooter *)(buf + sizeof(*packet) + packet->len);
}

static inline __le32 hostIntfComputeCrc(uint8_t *buf)
{
    struct NanohubPacket *packet = (struct NanohubPacket *)buf;
    uint32_t crc = crc32(packet, packet->len + sizeof(*packet), CRC_INIT);
    return htole32(crc);
}

static void hostIntfPrintErrMsg(void *cookie)
{
    struct hostIntfIntErrMsg *msg = (struct hostIntfIntErrMsg *)cookie;
    osLog(msg->level, "%s failed with: %d\n", msg->func, msg->reason);
    atomicAdd32bits(&mIntErrMsgCnt, -1UL);
}

static void hostIntfDeferErrLog(enum LogLevel level, enum hostIntfIntErrReason reason, const char *func)
{
    // If the message buffer is full, we drop the newer messages.
    if (atomicRead32bits(&mIntErrMsgCnt) == HOSTINTF_MAX_ERR_MSG)
        return;

    mIntErrMsg[mIntErrMsgIdx].level = level;
    mIntErrMsg[mIntErrMsgIdx].reason = reason;
    mIntErrMsg[mIntErrMsgIdx].func = func;
    if (osDefer(hostIntfPrintErrMsg, &mIntErrMsg[mIntErrMsgIdx], false)) {
        atomicAdd32bits(&mIntErrMsgCnt, 1UL);
        mIntErrMsgIdx = (mIntErrMsgIdx + 1) % HOSTINTF_MAX_ERR_MSG;
    }
}

static inline const struct NanohubCommand *hostIntfFindHandler(uint8_t *buf, size_t size, uint32_t *seq)
{
    struct NanohubPacket *packet = (struct NanohubPacket *)buf;
    struct NanohubPacketFooter *footer;
    __le32 packetCrc;
    uint32_t packetReason;
    const struct NanohubCommand *cmd;

    if (size < NANOHUB_PACKET_SIZE(0)) {
        hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_INCOMPELETE, __func__);
        return NULL;
    }

    if (size != NANOHUB_PACKET_SIZE(packet->len)) {
        hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PGK_SIZE, __func__);
        return NULL;
    }

    footer = hostIntfGetFooter(buf);
    packetCrc = hostIntfComputeCrc(buf);
    if (footer->crc != packetCrc) {
        hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_CRC, __func__);
        return NULL;
    }

    if (mTxRetrans.seq == packet->seq) {
        mTxRetrans.seqMatch = true;
        return mTxRetrans.cmd;
    } else {
        mTxRetrans.seqMatch = false;
    }

    *seq = packet->seq;

    if (mBusy)
        return NULL;

    packetReason = le32toh(packet->reason);

    if ((cmd = nanohubFindCommand(packetReason)) != NULL) {
        if (packet->len < cmd->minDataLen || packet->len > cmd->maxDataLen) {
            hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_PKG_PAYLOAD_SIZE, __func__);
            return NULL;
        }

        return cmd;
    }

    hostIntfDeferErrLog(LOG_WARN, HOSTINTF_ERR_UNKNOWN, __func__);
    return NULL;
}

static void hostIntfTxBuf(int size, uint8_t *buf, HostIntfCommCallbackF callback)
{
    mTxSize = size;
    mTxBufPtr = buf;
    mComm->txPacket(mTxBufPtr, mTxSize, callback);
}

static void hostIntfTxPacket(__le32 reason, uint8_t len, uint32_t seq,
        HostIntfCommCallbackF callback)
{
    struct NanohubPacket *txPacket = (struct NanohubPacket *)(mTxBuf.buf);
    txPacket->reason = reason;
    txPacket->seq = seq;
    txPacket->sync = NANOHUB_SYNC_BYTE;
    txPacket->len = len;

    struct NanohubPacketFooter *txFooter = hostIntfGetFooter(mTxBuf.buf);
    txFooter->crc = hostIntfComputeCrc(mTxBuf.buf);

    // send starting with the prePremable byte
    hostIntfTxBuf(1+NANOHUB_PACKET_SIZE(len), &mTxBuf.prePreamble, callback);
}

static void hostIntfTxNakPacket(__le32 reason, uint32_t seq,
        HostIntfCommCallbackF callback)
{
    struct NanohubPacket *txPacket = (struct NanohubPacket *)(mTxNakBuf.buf);
    txPacket->reason = reason;
    txPacket->seq = seq;
    txPacket->sync = NANOHUB_SYNC_BYTE;
    txPacket->len = 0;

    struct NanohubPacketFooter *txFooter = hostIntfGetFooter(mTxNakBuf.buf);
    txFooter->crc = hostIntfComputeCrc(mTxNakBuf.buf);

    // send starting with the prePremable byte
    hostIntfTxBuf(1+NANOHUB_PACKET_SIZE_MIN, &mTxNakBuf.prePreamble, callback);
}

static inline bool hostIntfTxPacketDone(int err, size_t tx,
        HostIntfCommCallbackF callback)
{
    if (!err && tx < mTxSize) {
        mTxSize -= tx;
        mTxBufPtr += tx;

        mComm->txPacket(mTxBufPtr, mTxSize, callback);
        return false;
    }

    return true;
}

static bool hostIntfRequest(uint32_t tid)
{
    mHostIntfTid = tid;
    atomicBitsetInit(mInterrupt, HOSTINTF_MAX_INTERRUPTS);
    atomicBitsetInit(mInterruptMask, HOSTINTF_MAX_INTERRUPTS);
#ifdef AP_INT_NONWAKEUP
    hostIntfSetInterruptMask(NANOHUB_INT_NONWAKEUP);
#endif
    mTxBuf.prePreamble = NANOHUB_PREAMBLE_BYTE;
    mTxBuf.postPreamble = NANOHUB_PREAMBLE_BYTE;
    mTxNakBuf.prePreamble = NANOHUB_PREAMBLE_BYTE;
    mTxNakBuf.postPreamble = NANOHUB_PREAMBLE_BYTE;

    mComm = platHostIntfInit();
    if (mComm) {
        int err = mComm->request();
        if (!err) {
            nanohubInitCommand();
            mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone);
            osEventSubscribe(mHostIntfTid, EVT_APP_START);
            return true;
        }
    }

    return false;
}

void hostIntfRxPacket(bool wakeupActive)
{
    if (mWakeActive) {
        if (atomicXchgByte(&mRxIdle, false)) {
            if (!wakeupActive)
                hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE);
            mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone);
            if (wakeupActive)
                hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
        } else if (atomicReadByte(&mActiveWrite)) {
            atomicWriteByte(&mRestartRx, true);
        } else {
            if (!wakeupActive)
                hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE);
            else
                hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
        }
    } else if (wakeupActive && !atomicReadByte(&mActiveWrite))
        hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);

    mWakeActive = wakeupActive;
}

static void hostIntfRxDone(size_t rx, int err)
{
    mRxTimestamp = sensorGetTime();
    mRxSize = rx;

    if (err != 0) {
        hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_RECEIVE, __func__);
        return;
    }

    hostIntfGenerateAck(NULL);
}

static void hostIntfTxSendAck(uint32_t resp)
{
    void *txPayload = hostIntfGetPayload(mTxBuf.buf);

    if (resp == NANOHUB_FAST_UNHANDLED_ACK) {
        hostIntfCopyInterrupts(txPayload, HOSTINTF_MAX_INTERRUPTS);
        hostIntfTxPacket(NANOHUB_REASON_ACK, 32, mTxRetrans.seq, hostIntfTxAckDone);
    } else if (resp == NANOHUB_FAST_DONT_ACK) {
        // do nothing. something else will do the ack
    } else {
        hostIntfTxPacket(mRxCmd->reason, resp, mTxRetrans.seq, hostIntfTxPayloadDone);
    }
}

void hostIntfTxAck(void *buffer, uint8_t len)
{
    void *txPayload = hostIntfGetPayload(mTxBuf.buf);

    memcpy(txPayload, buffer, len);

    hostIntfTxSendAck(len);
}

static void hostIntfGenerateAck(void *cookie)
{
    uint32_t seq = 0;
    void *txPayload = hostIntfGetPayload(mTxBuf.buf);
    void *rxPayload = hostIntfGetPayload(mRxBuf);
    uint8_t rx_len = hostIntfGetPayloadLen(mRxBuf);
    uint32_t resp = NANOHUB_FAST_UNHANDLED_ACK;

    atomicWriteByte(&mActiveWrite, true);
    hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
    mRxCmd = hostIntfFindHandler(mRxBuf, mRxSize, &seq);

    if (mRxCmd) {
        if (mTxRetrans.seqMatch) {
            hostIntfTxBuf(mTxSize, &mTxBuf.prePreamble, hostIntfTxPayloadDone);
        } else {
            mTxRetrans.seq = seq;
            mTxRetrans.cmd = mRxCmd;
            if (mRxCmd->fastHandler)
                resp = mRxCmd->fastHandler(rxPayload, rx_len, txPayload, mRxTimestamp);

            hostIntfTxSendAck(resp);
        }
    } else {
        if (mBusy)
            hostIntfTxNakPacket(NANOHUB_REASON_NAK_BUSY, seq, hostIntfTxAckDone);
        else
            hostIntfTxNakPacket(NANOHUB_REASON_NAK, seq, hostIntfTxAckDone);
    }
}


static void hostIntfTxComplete(bool clearInt, bool restartRx)
{
    if (restartRx || clearInt || !mWakeActive)
        hostIntfClearInterrupt(NANOHUB_INT_WAKE_COMPLETE);
    atomicWriteByte(&mActiveWrite, false);
    atomicWriteByte(&mRestartRx, false);
    if (restartRx) {
        mComm->rxPacket(mRxBuf, sizeof(mRxBuf), hostIntfRxDone);
        hostIntfSetInterrupt(NANOHUB_INT_WAKE_COMPLETE);
    } else {
        atomicWriteByte(&mRxIdle, true);
    }
}

static void hostIntfTxAckDone(size_t tx, int err)
{
    hostIntfTxPacketDone(err, tx, hostIntfTxAckDone);

    if (err) {
        hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_ACK, __func__);
        hostIntfTxComplete(false, false);
        return;
    }

    if (!mRxCmd) {
        if (!mBusy)
            hostIntfDeferErrLog(LOG_DEBUG, HOSTINTF_ERR_NAK, __func__);
        if (atomicReadByte(&mRestartRx))
            hostIntfTxComplete(false, true);
        else
            hostIntfTxComplete(false, false);
        return;
    } else if (atomicReadByte(&mRestartRx)) {
        mTxRetrans.seq = 0;
        mTxRetrans.cmd = NULL;
        hostIntfTxComplete(false, true);
    } else {
        if (!osDefer(hostIntfGenerateResponse, NULL, true)) {
            mTxRetrans.seq = 0;
            mTxRetrans.cmd = NULL;
            hostIntfTxComplete(false, false);
        }
    }
}

static void hostIntfGenerateResponse(void *cookie)
{
    void *rxPayload = hostIntfGetPayload(mRxBuf);
    uint8_t rx_len = hostIntfGetPayloadLen(mRxBuf);
    void *txPayload = hostIntfGetPayload(mTxBuf.buf);
    uint8_t respLen = mRxCmd->handler(rxPayload, rx_len, txPayload, mRxTimestamp);

    hostIntfTxPacket(mRxCmd->reason, respLen, mTxRetrans.seq, hostIntfTxPayloadDone);
}

static void hostIntfTxPayloadDone(size_t tx, int err)
{
    bool done = hostIntfTxPacketDone(err, tx, hostIntfTxPayloadDone);

    if (err)
        hostIntfDeferErrLog(LOG_ERROR, HOSTINTF_ERR_SEND, __func__);

    if (done) {
        if (atomicReadByte(&mRestartRx))
            hostIntfTxComplete(true, true);
        else
            hostIntfTxComplete(true, false);
    }
}

static void hostIntfRelease()
{
    mComm->release();
}

static void resetBuffer(struct ActiveSensor *sensor)
{
    sensor->discard = true;
    sensor->buffer.length = 0;
    memset(&sensor->buffer.firstSample, 0x00, sizeof(struct SensorFirstSample));
}

void hostIntfSetBusy(bool busy)
{
    mBusy = busy;
}

static inline struct ActiveSensor *getActiveSensorByType(uint32_t sensorType)
{
    struct ActiveSensor *sensor = NULL;

    if (sensorType > SENS_TYPE_INVALID && sensorType <= SENS_TYPE_LAST_USER &&
        mSensorList[sensorType - 1] < MAX_REGISTERED_SENSORS)
        sensor = mActiveSensorTable + mSensorList[sensorType - 1];

    return sensor;
}

bool hostIntfPacketDequeue(void *data, uint32_t *wakeup, uint32_t *nonwakeup)
{
    struct HostIntfDataBuffer *buffer = data;
    bool ret;
    struct ActiveSensor *sensor;
    uint32_t i;

    ret = simpleQueueDequeue(mOutputQ, buffer);
    while (ret) {
        sensor = getActiveSensorByType(buffer->sensType);
        if (sensor) {
            // do not sent sensor data if sensor is not requested; only maintain stats
            if (sensor->sensorHandle == 0 && !buffer->firstSample.biasPresent && !buffer->firstSample.numFlushes) {
                if (sensor->interrupt == NANOHUB_INT_WAKEUP)
                    mWakeupBlocks--;
                else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
                    mNonWakeupBlocks--;
                sensor->curSamples -= buffer->firstSample.numSamples;
                ret = simpleQueueDequeue(mOutputQ, buffer);
            } else {
                break;
            }
        } else {
            break;
        }
    }

    if (!ret) {
        // nothing in queue. look for partial buffers to flush
        for (i = 0; i < mNumSensors; i++, mLastSensor = (mLastSensor + 1) % mNumSensors) {
            sensor = mActiveSensorTable + mLastSensor;

            if (sensor->curSamples != sensor->buffer.firstSample.numSamples) {
                osLog(LOG_ERROR, "hostIntfPacketDequeue: sensor(%d)->curSamples=%d != buffer->numSamples=%d\n", sensor->buffer.sensType, sensor->curSamples, sensor->buffer.firstSample.numSamples);
                sensor->curSamples = sensor->buffer.firstSample.numSamples;
            }

            if (sensor->buffer.length > 0) {
                memcpy(buffer, &sensor->buffer, sizeof(struct HostIntfDataBuffer));
                resetBuffer(sensor);
                ret = true;
                mLastSensor = (mLastSensor + 1) % mNumSensors;
                break;
            }
        }
    }

    if (ret) {
        sensor = getActiveSensorByType(buffer->sensType);
        if (sensor) {
            if (sensor->interrupt == NANOHUB_INT_WAKEUP)
                mWakeupBlocks--;
            else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
                mNonWakeupBlocks--;
            sensor->curSamples -= buffer->firstSample.numSamples;
            sensor->firstTime = 0ull;
        } else {
            if (buffer->interrupt == NANOHUB_INT_WAKEUP)
                mWakeupBlocks--;
            else if (buffer->interrupt == NANOHUB_INT_NONWAKEUP)
                mNonWakeupBlocks--;
        }
    }

    *wakeup = mWakeupBlocks;
    *nonwakeup = mNonWakeupBlocks;

    return ret;
}

static void initCompleteCallback(uint32_t timerId, void *data)
{
    osEnqueuePrivateEvt(EVT_APP_START, NULL, NULL, mHostIntfTid);
}

static bool queueDiscard(void *data, bool onDelete)
{
    struct HostIntfDataBuffer *buffer = data;
    struct ActiveSensor *sensor = getActiveSensorByType(buffer->sensType);

    if (sensor) {
        if (sensor->curSamples - buffer->firstSample.numSamples >= sensor->minSamples || onDelete) {
            if (sensor->interrupt == NANOHUB_INT_WAKEUP)
                mWakeupBlocks--;
            else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
                mNonWakeupBlocks--;
            sensor->curSamples -= buffer->firstSample.numSamples;

            return true;
        } else {
            return false;
        }
    } else {
        if (buffer->interrupt == NANOHUB_INT_WAKEUP)
            mWakeupBlocks--;
        else if (buffer->interrupt == NANOHUB_INT_NONWAKEUP)
            mNonWakeupBlocks--;
        return true;
    }
}

static void latencyTimerCallback(uint32_t timerId, void* data)
{
    osEnqueuePrivateEvt(EVT_LATENCY_TIMER, data, NULL, mHostIntfTid);
}

static bool initSensors()
{
    uint32_t i, j, blocks, maxBlocks, numAxis, packetSamples;
    bool present, error;
    const struct SensorInfo *si;
    uint32_t handle;
    static uint8_t errorCnt = 0;
    uint32_t totalBlocks = 0;
    uint8_t numSensors = 0;
    ATOMIC_BITSET_DECL(sensorPresent, SENS_TYPE_LAST_USER - SENS_TYPE_INVALID,);

    atomicBitsetInit(sensorPresent, SENS_TYPE_LAST_USER - SENS_TYPE_INVALID);

    for (i = SENS_TYPE_INVALID + 1; i <= SENS_TYPE_LAST_USER; i++) {
        for (j = 0, present = 0, error = 0; (si = sensorFind(i, j, &handle)) != NULL; j++) {
            if (!sensorGetInitComplete(handle)) {
                if (errorCnt >= SENSOR_INIT_ERROR_MAX) {
                    osLog(LOG_ERROR, "initSensors: %s not ready - skipping!\n", si->sensorName);
                    continue;
                } else {
                    osLog(LOG_INFO, "initSensors: %s not ready!\n", si->sensorName);
                    timTimerSet(SENSOR_INIT_DELAY, 0, 50, initCompleteCallback, NULL, true);
                    errorCnt ++;
                    return false;
                }
            } else if (!(si->flags1 & SENSOR_INFO_FLAGS1_LOCAL_ONLY)) {
                if (!present) {
                    present = 1;
                    numAxis = si->numAxis;
                    switch (si->numAxis) {
                    case NUM_AXIS_EMBEDDED:
                    case NUM_AXIS_ONE:
                        packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct SingleAxisDataPoint);
                        break;
                    case NUM_AXIS_THREE:
                        if (si->flags1 & SENSOR_INFO_FLAGS1_RAW)
                            packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct RawTripleAxisDataPoint);
                        else
                            packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct TripleAxisDataPoint);
                        break;
                    default:
                        packetSamples = 1;
                        error = true;
                    }
                    if (si->minSamples > MAX_MIN_SAMPLES)
                        maxBlocks = (MAX_MIN_SAMPLES + packetSamples - 1) / packetSamples;
                    else
                        maxBlocks = (si->minSamples + packetSamples - 1) / packetSamples;
                } else {
                    if (si->numAxis != numAxis) {
                        error = true;
                    } else {
                        if (si->minSamples > MAX_MIN_SAMPLES)
                            blocks = (MAX_MIN_SAMPLES + packetSamples - 1) / packetSamples;
                        else
                            blocks = (si->minSamples + packetSamples - 1) / packetSamples;

                        maxBlocks = maxBlocks > blocks ? maxBlocks : blocks;
                    }
                }
            }
        }

        if (present && !error) {
            atomicBitsetSetBit(sensorPresent, i - 1);
            numSensors++;
            totalBlocks += maxBlocks;
        }
    }

    if (totalBlocks > MAX_NUM_BLOCKS) {
        osLog(LOG_INFO, "initSensors: totalBlocks of %ld exceeds maximum of %d\n", totalBlocks, MAX_NUM_BLOCKS);
        totalBlocks = MAX_NUM_BLOCKS;
    } else if (totalBlocks < MIN_NUM_BLOCKS) {
        totalBlocks = MIN_NUM_BLOCKS;
    }

    mOutputQ = simpleQueueAlloc(totalBlocks, sizeof(struct HostIntfDataBuffer), queueDiscard);
    mActiveSensorTable = heapAlloc(numSensors * sizeof(struct ActiveSensor));
    memset(mActiveSensorTable, 0x00, numSensors * sizeof(struct ActiveSensor));

    for (i = SENS_TYPE_INVALID; i < SENS_TYPE_LAST_USER; i++) {
        mSensorList[i] = MAX_REGISTERED_SENSORS;
    }

    for (i = SENS_TYPE_INVALID + 1, j = 0; i <= SENS_TYPE_LAST_USER && j < numSensors; i++) {
        if (atomicBitsetGetBit(sensorPresent, i - 1)
            && (si = sensorFind(i, 0, &handle)) != NULL
            && !(si->flags1 & SENSOR_INFO_FLAGS1_LOCAL_ONLY)) {
            mSensorList[i - 1] = j;
            resetBuffer(mActiveSensorTable + j);
            mActiveSensorTable[j].buffer.sensType = i;
            mActiveSensorTable[j].biasReportType = 0;
            mActiveSensorTable[j].rate = 0;
            mActiveSensorTable[j].latency = 0;
            mActiveSensorTable[j].numAxis = si->numAxis;
            mActiveSensorTable[j].interrupt = si->interrupt;
            if (si->flags1 & SENSOR_INFO_FLAGS1_RAW) {
                mSensorList[si->rawType - 1] = j;
                mActiveSensorTable[j].buffer.sensType = si->rawType;
                mActiveSensorTable[j].raw = true;
                mActiveSensorTable[j].rawScale = si->rawScale;
            }
            if (si->flags1 & SENSOR_INFO_FLAGS1_BIAS) {
                mSensorList[si->biasType - 1] = j;
                mActiveSensorTable[j].biasReportType = i;
                osEventSubscribe(mHostIntfTid, sensorGetMyEventType(si->biasType));
            }
            if (si->minSamples > MAX_MIN_SAMPLES) {
                mActiveSensorTable[j].minSamples = MAX_MIN_SAMPLES;
                osLog(LOG_INFO, "initSensors: %s: minSamples of %d exceeded max of %d\n", si->sensorName, si->minSamples, MAX_MIN_SAMPLES);
            } else {
                mActiveSensorTable[j].minSamples = si->minSamples;
            }
            mActiveSensorTable[j].curSamples = 0;
            mActiveSensorTable[j].oneshot = false;
            mActiveSensorTable[j].firstTime = 0ull;
            switch (si->numAxis) {
            case NUM_AXIS_EMBEDDED:
            case NUM_AXIS_ONE:
                mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct SingleAxisDataPoint);
                break;
            case NUM_AXIS_THREE:
                if (mActiveSensorTable[j].raw)
                    mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct RawTripleAxisDataPoint);
                else
                    mActiveSensorTable[j].packetSamples = HOSTINTF_SENSOR_DATA_MAX / sizeof(struct TripleAxisDataPoint);
                break;
            }
            j++;
        }
    }

    mTotalBlocks = totalBlocks;
    mNumSensors = numSensors;

    return true;
}

static inline int16_t floatToInt16(float val)
{
    if (val < (INT16_MIN + 0.5f))
        return INT16_MIN;
    else if (val > (INT16_MAX - 0.5f))
        return INT16_MAX;
    else if (val >= 0.0f)
        return val + 0.5f;
    else
        return val - 0.5f;
}

static uint32_t encodeDeltaTime(uint64_t time)
{
    uint32_t deltaTime;

    if (time <= UINT32_MAX) {
        deltaTime = time | delta_time_fine_mask;
    } else {
        deltaTime = ((time + delta_time_rounding) >> delta_time_multiplier_order) & delta_time_coarse_mask;
    }
    return deltaTime;
}

static bool enqueueSensorBuffer(struct ActiveSensor *sensor)
{
    bool queued = simpleQueueEnqueue(mOutputQ, &sensor->buffer,
                                     sizeof(uint32_t) + sensor->buffer.length, sensor->discard);

    if (!queued) {
        // undo counters if failed to add buffer
        if (sensor->interrupt == NANOHUB_INT_WAKEUP)
            mWakeupBlocks--;
        else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
            mNonWakeupBlocks--;
        sensor->curSamples -= sensor->buffer.firstSample.numSamples;
    }
    resetBuffer(sensor);
    return queued;
}

static void copySingleSamples(struct ActiveSensor *sensor, const struct SingleAxisDataEvent *single)
{
    int i;
    uint32_t deltaTime;
    uint8_t numSamples;
    uint8_t evtNumSamples = single->samples[0].firstSample.numSamples;

    for (i = 0; i < evtNumSamples; i++) {
        if (sensor->buffer.firstSample.numSamples == sensor->packetSamples)
            enqueueSensorBuffer(sensor);

        if (sensor->buffer.firstSample.numSamples == 0) {
            if (i == 0) {
                sensor->lastTime = sensor->buffer.referenceTime = single->referenceTime;
            } else {
                sensor->lastTime += single->samples[i].deltaTime;
                sensor->buffer.referenceTime = sensor->lastTime;
            }
            sensor->buffer.length = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint);
            sensor->buffer.single[0].idata = single->samples[i].idata;
            if (sensor->interrupt == NANOHUB_INT_WAKEUP)
                mWakeupBlocks++;
            else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
                mNonWakeupBlocks++;
            sensor->buffer.firstSample.numSamples = 1;
            sensor->buffer.firstSample.interrupt = sensor->interrupt;
            if (sensor->curSamples++ == 0)
                sensor->firstTime = sensor->buffer.referenceTime;
        } else {
            if (i == 0) {
                if (sensor->lastTime > single->referenceTime) {
                    // shouldn't happen. flush current packet
                    enqueueSensorBuffer(sensor);
                    i--;
                } else if (single->referenceTime - sensor->lastTime >= delta_time_max) {
                    enqueueSensorBuffer(sensor);
                    i--;
                } else {
                    deltaTime = encodeDeltaTime(single->referenceTime - sensor->lastTime);
                    numSamples = sensor->buffer.firstSample.numSamples;

                    sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
                    sensor->buffer.single[numSamples].deltaTime = deltaTime;
                    sensor->buffer.single[numSamples].idata = single->samples[0].idata;
                    sensor->lastTime = single->referenceTime;
                    sensor->buffer.firstSample.numSamples++;
                    sensor->curSamples++;
                }
            } else {
                deltaTime = single->samples[i].deltaTime;
                numSamples = sensor->buffer.firstSample.numSamples;

                sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
                sensor->buffer.single[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
                sensor->buffer.single[numSamples].idata = single->samples[i].idata;
                sensor->lastTime += deltaTime;
                sensor->buffer.firstSample.numSamples++;
                sensor->curSamples++;
            }
        }
    }
}

static void copyTripleSamples(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple)
{
    int i;
    uint32_t deltaTime;
    uint8_t numSamples;

    for (i = 0; i < triple->samples[0].firstSample.numSamples; i++) {
        if (sensor->buffer.firstSample.numSamples == sensor->packetSamples)
            enqueueSensorBuffer(sensor);

        if (sensor->buffer.firstSample.numSamples == 0) {
            if (i == 0) {
                sensor->lastTime = sensor->buffer.referenceTime = triple->referenceTime;
            } else {
                sensor->lastTime += triple->samples[i].deltaTime;
                sensor->buffer.referenceTime = sensor->lastTime;
            }
            sensor->buffer.length = sizeof(struct TripleAxisDataEvent) + sizeof(struct TripleAxisDataPoint);
            sensor->buffer.triple[0].ix = triple->samples[i].ix;
            sensor->buffer.triple[0].iy = triple->samples[i].iy;
            sensor->buffer.triple[0].iz = triple->samples[i].iz;
            if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == i) {
                sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
                sensor->buffer.firstSample.biasPresent = 1;
                sensor->buffer.firstSample.biasSample = 0;
                sensor->discard = false;
            }
            if (sensor->interrupt == NANOHUB_INT_WAKEUP)
                mWakeupBlocks++;
            else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
                mNonWakeupBlocks++;
            sensor->buffer.firstSample.numSamples = 1;
            sensor->buffer.firstSample.interrupt = sensor->interrupt;
            if (sensor->curSamples++ == 0)
                sensor->firstTime = sensor->buffer.referenceTime;
        } else {
            if (i == 0) {
                if (sensor->lastTime > triple->referenceTime) {
                    // shouldn't happen. flush current packet
                    enqueueSensorBuffer(sensor);
                    i--;
                } else if (triple->referenceTime - sensor->lastTime >= delta_time_max) {
                    enqueueSensorBuffer(sensor);
                    i--;
                } else {
                    deltaTime = encodeDeltaTime(triple->referenceTime - sensor->lastTime);
                    numSamples = sensor->buffer.firstSample.numSamples;

                    sensor->buffer.length += sizeof(struct TripleAxisDataPoint);
                    sensor->buffer.triple[numSamples].deltaTime = deltaTime;
                    sensor->buffer.triple[numSamples].ix = triple->samples[0].ix;
                    sensor->buffer.triple[numSamples].iy = triple->samples[0].iy;
                    sensor->buffer.triple[numSamples].iz = triple->samples[0].iz;
                    sensor->lastTime = triple->referenceTime;
                    if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == 0) {
                        sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
                        sensor->buffer.firstSample.biasPresent = 1;
                        sensor->buffer.firstSample.biasSample = numSamples;
                        sensor->discard = false;
                    }
                    sensor->buffer.firstSample.numSamples++;
                    sensor->curSamples++;
                }
            } else {
                deltaTime = triple->samples[i].deltaTime;
                numSamples = sensor->buffer.firstSample.numSamples;

                sensor->buffer.length += sizeof(struct TripleAxisDataPoint);
                sensor->buffer.triple[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
                sensor->buffer.triple[numSamples].ix = triple->samples[i].ix;
                sensor->buffer.triple[numSamples].iy = triple->samples[i].iy;
                sensor->buffer.triple[numSamples].iz = triple->samples[i].iz;
                sensor->lastTime += deltaTime;
                if (triple->samples[0].firstSample.biasPresent && triple->samples[0].firstSample.biasSample == i) {
                    sensor->buffer.firstSample.biasCurrent = triple->samples[0].firstSample.biasCurrent;
                    sensor->buffer.firstSample.biasPresent = 1;
                    sensor->buffer.firstSample.biasSample = numSamples;
                    sensor->discard = false;
                }
                sensor->buffer.firstSample.numSamples++;
                sensor->curSamples++;
            }
        }
    }
}

static void copyTripleSamplesBias(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple)
{
    uint8_t sensType = sensor->buffer.sensType;

    if (sensType == sensor->biasReportType) {
        copyTripleSamples(sensor, triple);
    } else {
        // Bias needs to be sent with a different sensType, so enqueue any pending buffer, enqueue
        // bias with a different sensor type, then restore the sensType
        if (sensor->buffer.firstSample.numSamples > 0)
            enqueueSensorBuffer(sensor);
        sensor->buffer.sensType = sensor->biasReportType;
        copyTripleSamples(sensor, triple);
        if (sensor->buffer.firstSample.numSamples > 0)
            enqueueSensorBuffer(sensor);
        sensor->buffer.sensType = sensType;
    }
}

static void copyTripleSamplesRaw(struct ActiveSensor *sensor, const struct TripleAxisDataEvent *triple)
{
    int i;
    uint32_t deltaTime;
    uint8_t numSamples;

    // Bias not supported in raw format; treat as regular format triple samples (potentially
    // handling alternate bias report type)
    if (triple->samples[0].firstSample.biasPresent) {
        copyTripleSamplesBias(sensor, triple);
        return;
    }

    for (i = 0; i < triple->samples[0].firstSample.numSamples; i++) {
        if (sensor->buffer.firstSample.numSamples == sensor->packetSamples)
            enqueueSensorBuffer(sensor);

        if (sensor->buffer.firstSample.numSamples == 0) {
            if (i == 0) {
                sensor->lastTime = sensor->buffer.referenceTime = triple->referenceTime;
            } else {
                sensor->lastTime += triple->samples[i].deltaTime;
                sensor->buffer.referenceTime = sensor->lastTime;
            }
            sensor->buffer.length = sizeof(struct RawTripleAxisDataEvent) + sizeof(struct RawTripleAxisDataPoint);
            sensor->buffer.rawTriple[0].ix = floatToInt16(triple->samples[i].x * sensor->rawScale);
            sensor->buffer.rawTriple[0].iy = floatToInt16(triple->samples[i].y * sensor->rawScale);
            sensor->buffer.rawTriple[0].iz = floatToInt16(triple->samples[i].z * sensor->rawScale);
            if (sensor->interrupt == NANOHUB_INT_WAKEUP)
                mWakeupBlocks++;
            else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
                mNonWakeupBlocks++;
            sensor->buffer.firstSample.numSamples = 1;
            sensor->buffer.firstSample.interrupt = sensor->interrupt;
            if (sensor->curSamples++ == 0)
                sensor->firstTime = sensor->buffer.referenceTime;
        } else {
            if (i == 0) {
                if (sensor->lastTime > triple->referenceTime) {
                    // shouldn't happen. flush current packet
                    enqueueSensorBuffer(sensor);
                    i--;
                } else if (triple->referenceTime - sensor->lastTime >= delta_time_max) {
                    enqueueSensorBuffer(sensor);
                    i--;
                } else {
                    deltaTime = encodeDeltaTime(triple->referenceTime - sensor->lastTime);
                    numSamples = sensor->buffer.firstSample.numSamples;

                    sensor->buffer.length += sizeof(struct RawTripleAxisDataPoint);
                    sensor->buffer.rawTriple[numSamples].deltaTime = deltaTime;
                    sensor->buffer.rawTriple[numSamples].ix = floatToInt16(triple->samples[0].x * sensor->rawScale);
                    sensor->buffer.rawTriple[numSamples].iy = floatToInt16(triple->samples[0].y * sensor->rawScale);
                    sensor->buffer.rawTriple[numSamples].iz = floatToInt16(triple->samples[0].z * sensor->rawScale);
                    sensor->lastTime = triple->referenceTime;
                    sensor->buffer.firstSample.numSamples++;
                    sensor->curSamples++;
                }
            } else {
                deltaTime = triple->samples[i].deltaTime;
                numSamples = sensor->buffer.firstSample.numSamples;

                sensor->buffer.length += sizeof(struct RawTripleAxisDataPoint);
                sensor->buffer.rawTriple[numSamples].deltaTime = deltaTime | delta_time_fine_mask;
                sensor->buffer.rawTriple[numSamples].ix = floatToInt16(triple->samples[i].x * sensor->rawScale);
                sensor->buffer.rawTriple[numSamples].iy = floatToInt16(triple->samples[i].y * sensor->rawScale);
                sensor->buffer.rawTriple[numSamples].iz = floatToInt16(triple->samples[i].z * sensor->rawScale);
                sensor->lastTime += deltaTime;
                sensor->buffer.firstSample.numSamples++;
                sensor->curSamples++;
            }
        }
    }
}

static void hostIntfAddBlock(struct HostIntfDataBuffer *data, bool discardable, bool interrupt)
{
    if (!simpleQueueEnqueue(mOutputQ, data, sizeof(uint32_t) + data->length, discardable))
        return;

    if (data->interrupt == NANOHUB_INT_WAKEUP)
        mWakeupBlocks++;
    else if (data->interrupt == NANOHUB_INT_NONWAKEUP)
        mNonWakeupBlocks++;
    nanohubPrefetchTx(interrupt ? data->interrupt : HOSTINTF_MAX_INTERRUPTS, mWakeupBlocks, mNonWakeupBlocks);
}

static void hostIntfNotifyReboot(uint32_t reason)
{
    __le32 raw_reason = htole32(reason);

    struct NanohubHalSysMgmtTx *resp;
    resp = heapAlloc(sizeof(*resp));
    if (resp) {
        resp->hdr = (struct NanohubHalHdr) {
            .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
            .len = sizeof(*resp) - sizeof(resp->hdr),
        };
        resp->ret = (struct NanohubHalRet) {
            .msg = NANOHUB_HAL_SYS_MGMT,
            .status = raw_reason,
        };
        resp->cmd = NANOHUB_HAL_SYS_MGMT_REBOOT;
        osEnqueueEvtOrFree(EVT_APP_TO_HOST_CHRE, resp, heapFree);
    }

#ifdef LEGACY_HAL_ENABLED
    struct NanohubHalLegacyRebootTx *respLegacy;
    respLegacy = heapAlloc(sizeof(*respLegacy));
    if (respLegacy) {
        respLegacy->hdr = (struct NanohubHalLegacyHdr) {
            .appId = APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0),
            .len = sizeof(*respLegacy) - sizeof(respLegacy->hdr) + sizeof(respLegacy->hdr.msg),
            .msg = NANOHUB_HAL_LEGACY_REBOOT,
        };
        memcpy(&respLegacy->reason, &raw_reason, sizeof(respLegacy->reason));
        osEnqueueEvtOrFree(EVT_APP_TO_HOST, respLegacy, heapFree);
    }
#endif
}

static void queueFlush(struct ActiveSensor *sensor)
{
    if (sensor->buffer.length == 0) {
        sensor->buffer.length = sizeof(sensor->buffer.referenceTime) + sizeof(struct SensorFirstSample);
        sensor->buffer.referenceTime = 0ull;
        if (sensor->interrupt == NANOHUB_INT_WAKEUP)
            mWakeupBlocks++;
        else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
            mNonWakeupBlocks++;
        sensor->buffer.firstSample.numFlushes = 1;
    } else {
        sensor->buffer.firstSample.numFlushes++;
    }
    sensor->discard = false;
    hostIntfSetInterrupt(sensor->interrupt);
}

static void fakeFlush(struct ConfigCmd *cmd)
{
    struct HostIntfDataBuffer *buffer;
    uint8_t size = sizeof(buffer->evtType) + sizeof(buffer->referenceTime) + sizeof(struct SensorFirstSample);
    buffer = alloca(size);
    memset(buffer, 0x00, size);

    buffer->sensType = cmd->sensType;
    buffer->length = sizeof(buffer->referenceTime) + sizeof(struct SensorFirstSample);
    buffer->interrupt = NANOHUB_INT_WAKEUP;
    mWakeupBlocks++;
    buffer->firstSample.numFlushes = 1;
    if (!simpleQueueEnqueue(mOutputQ, buffer, size, false))
        mWakeupBlocks--;
}

static void onEvtAppStart(const void *evtData)
{
    if (initSensors()) {
        uint32_t reason;
        struct HostIntfDataBuffer *data;

        osEventUnsubscribe(mHostIntfTid, EVT_APP_START);
        osEventsSubscribe(4, EVT_NO_SENSOR_CONFIG_EVENT,
                             EVT_APP_TO_SENSOR_HAL_DATA,
                             EVT_APP_TO_HOST,
                             EVT_APP_TO_HOST_CHRE);
#ifdef DEBUG_LOG_EVT
        osEventSubscribe(mHostIntfTid, EVT_DEBUG_LOG);
        platEarlyLogFlush();
#endif
        reason = pwrResetReason();
        data = alloca(sizeof(uint32_t) + sizeof(reason));
        data->sensType = SENS_TYPE_INVALID;
        data->length = sizeof(reason);
        data->dataType = HOSTINTF_DATA_TYPE_RESET_REASON;
        data->interrupt = NANOHUB_INT_WAKEUP;
        memcpy(data->buffer, &reason, sizeof(reason));
        hostIntfAddBlock(data, false, true);
        hostIntfNotifyReboot(reason);
    }
}

static void onEvtAppToHost(const void *evtData)
{
    const struct HostHubRawPacket *hostMsg = evtData;

    if (hostMsg->dataLen <= HOST_HUB_RAW_PACKET_MAX_LEN) {
        struct HostIntfDataBuffer *data = alloca(sizeof(uint32_t) + sizeof(*hostMsg) + hostMsg->dataLen);

        data->sensType = SENS_TYPE_INVALID;
        data->length = sizeof(*hostMsg) + hostMsg->dataLen;
        data->dataType = HOSTINTF_DATA_TYPE_APP_TO_HOST;
        data->interrupt = NANOHUB_INT_WAKEUP;
        memcpy(data->buffer, evtData, data->length);
        hostIntfAddBlock(data, false, true);
    }
}

static void onEvtAppToHostChre(const void *evtData)
{
    const struct HostHubChrePacket *hostMsg = evtData;

    if (hostMsg->messageSize <= HOST_HUB_CHRE_PACKET_MAX_LEN) {
        struct HostIntfDataBuffer *data = alloca(sizeof(uint32_t) + sizeof(*hostMsg) + hostMsg->messageSize);

        data->sensType = SENS_TYPE_INVALID;
        data->length = sizeof(*hostMsg) + hostMsg->messageSize;
        data->dataType = HOSTINTF_DATA_TYPE_APP_TO_HOST;
        data->interrupt = NANOHUB_INT_WAKEUP;
        memcpy(data->buffer, evtData, data->length);
        hostIntfAddBlock(data, false, true);
    }
}

#ifdef LEGACY_HAL_ENABLED
static void handleLegacyHalCmd(const uint8_t *halData, uint8_t size)
{
    const struct NanohubHalLegacyCommand *halCmd = nanohubHalLegacyFindCommand(halData[0]);
    if (halCmd)
        halCmd->handler((void *)&halData[1], size - 1);
}

static void onEvtAppFromHost(const void *evtData)
{
    const uint8_t *halMsg = evtData;
    handleLegacyHalCmd(&halMsg[1], halMsg[0]);
}
#endif

static void onEvtAppFromHostChre(const void *evtData)
{
    const struct NanohubMsgChreHdr *halMsg = (const struct NanohubMsgChreHdr *)evtData;
    const struct NanohubHalCommand *halCmd;
    const uint8_t *halData = (const uint8_t *)(halMsg+1);
    uint8_t len;
    uint32_t transactionId;

    memcpy(&transactionId, &halMsg->appEvent, sizeof(halMsg->appEvent));

    if (halMsg->size >= 1) {
        len = halMsg->size - 1;
        halCmd = nanohubHalFindCommand(halData[0]);
        if (halCmd) {
            if (len >= halCmd->minDataLen && len <= halCmd->maxDataLen)
                halCmd->handler((void *)&halData[1], len, transactionId);
            return;
        }
    }
#ifdef LEGACY_HAL_ENABLED
    handleLegacyHalCmd(halData, halMsg->size);
#endif
}

#ifdef DEBUG_LOG_EVT
static void onEvtDebugLog(const void *evtData)
{
    struct HostIntfDataBuffer *data = (struct HostIntfDataBuffer *)evtData;

    if (data->sensType == SENS_TYPE_INVALID && data->dataType == HOSTINTF_DATA_TYPE_LOG)
        hostIntfAddBlock(data, true, true);
}
#endif

static void onEvtLatencyTimer(const void *evtData)
{
    uint64_t sensorTime = sensorGetTime();
    uint32_t i, cnt;

    for (i = 0, cnt = 0; i < mNumSensors && cnt < mLatencyCnt; i++) {
        if (mActiveSensorTable[i].latency > 0) {
            cnt++;
            if (mActiveSensorTable[i].firstTime &&
                sensorTime >= mActiveSensorTable[i].firstTime + mActiveSensorTable[i].latency) {
                hostIntfSetInterrupt(mActiveSensorTable[i].interrupt);
            }
        }
    }
}

static void onConfigCmdFlushOne(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
{
    sensorFlush(sensor->sensorHandle);
}

static void onConfigCmdEnableOne(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
{
    if (sensorRequestRateChange(mHostIntfTid, sensor->sensorHandle, cmd->rate, cmd->latency)) {
        sensor->rate = cmd->rate;
        if (sensor->latency != cmd->latency) {
            if (!sensor->latency) {
                if (mLatencyCnt++ == 0)
                    mLatencyTimer = timTimerSet(CHECK_LATENCY_TIME, 100, 100, latencyTimerCallback, NULL, false);
            } else if (!cmd->latency) {
                if (--mLatencyCnt == 0) {
                    timTimerCancel(mLatencyTimer);
                    mLatencyTimer = 0;
                }
            }
            sensor->latency = cmd->latency;
        }
    }
}

static void onConfigCmdEnableAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
{
    for (uint32_t i = 0; sensorFind(cmd->sensType, i, &sensor->sensorHandle) != NULL; i++) {
        if (cmd->rate == SENSOR_RATE_ONESHOT) {
            cmd->rate = SENSOR_RATE_ONCHANGE;
            sensor->oneshot = true;
        } else {
            sensor->oneshot = false;
        }

        if (sensorRequest(mHostIntfTid, sensor->sensorHandle, cmd->rate, cmd->latency)) {
            if (cmd->latency) {
                if (mLatencyCnt++ == 0)
                    mLatencyTimer = timTimerSet(CHECK_LATENCY_TIME, 100, 100, latencyTimerCallback, NULL, false);
            }
            sensor->rate = cmd->rate;
            sensor->latency = cmd->latency;
            osEventSubscribe(mHostIntfTid, sensorGetMyEventType(cmd->sensType));
            break;
        } else {
            sensor->sensorHandle = 0;
        }
    }
}

static void onConfigCmdDisableOne(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
{
    sensorRelease(mHostIntfTid, sensor->sensorHandle);
    osEventUnsubscribe(mHostIntfTid, sensorGetMyEventType(cmd->sensType));
    if (sensor->latency) {
        if (--mLatencyCnt == 0) {
            timTimerCancel(mLatencyTimer);
            mLatencyTimer = 0;
        }
    }
    sensor->rate = 0;
    sensor->latency = 0;
    sensor->oneshot = false;
    sensor->sensorHandle = 0;
    if (sensor->buffer.length) {
        enqueueSensorBuffer(sensor);
        hostIntfSetInterrupt(sensor->interrupt);
    }
}

static void onConfigCmdCalibrateAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
{
    uint32_t tempSensorHandle;
    for (uint32_t i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++)
        sensorCalibrate(tempSensorHandle);
}

static void onConfigCmdSelfTestAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
{
    uint32_t tempSensorHandle;
    for (uint32_t i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++)
        sensorSelfTest(tempSensorHandle);
}

static void onConfigCmdCfgDataAll(struct ActiveSensor *sensor, struct ConfigCmd *cmd)
{
    uint32_t tempSensorHandle;
    for (uint32_t i = 0; sensorFind(cmd->sensType, i, &tempSensorHandle) != NULL; i++)
        sensorCfgData(tempSensorHandle, (void *)(cmd+1));
}

static void onEvtNoSensorConfigEvent(const void *evtData)
{
    struct ConfigCmd *cmd = (struct ConfigCmd *)evtData;
    struct ActiveSensor *sensor = getActiveSensorByType(cmd->sensType);
    if (sensor) {
        if (sensor->sensorHandle) {
            switch (cmd->cmd) {
            case CONFIG_CMD_FLUSH:
                onConfigCmdFlushOne(sensor, cmd);
                break;
            case CONFIG_CMD_ENABLE:
                onConfigCmdEnableOne(sensor, cmd);
                break;
            case CONFIG_CMD_DISABLE:
                onConfigCmdDisableOne(sensor, cmd);
                break;
            case CONFIG_CMD_CFG_DATA:
                onConfigCmdCfgDataAll(sensor, cmd);
                break;
            }
        } else {
            switch (cmd->cmd) {
            case CONFIG_CMD_ENABLE:
                onConfigCmdEnableAll(sensor, cmd);
                break;
            case CONFIG_CMD_CALIBRATE:
                onConfigCmdCalibrateAll(sensor, cmd);
                break;
            case CONFIG_CMD_SELF_TEST:
                onConfigCmdSelfTestAll(sensor, cmd);
                break;
            case CONFIG_CMD_CFG_DATA:
                onConfigCmdCfgDataAll(sensor, cmd);
                break;
            case CONFIG_CMD_FLUSH:
                queueFlush(sensor);
                break;
            }
        }
    } else if (cmd->cmd == CONFIG_CMD_FLUSH && cmd->sensType > SENS_TYPE_INVALID) {
        // if a flush event is for an unknown sensor, we just return a fake flush event.
        osLog(LOG_INFO, "Flush request from unrecognized sensor, returning a fake flush\n");
        fakeFlush(cmd);
    }
}

static void onEvtAppToSensorHalData(const void *evtData)
{
    struct HostIntfDataBuffer *data = (struct HostIntfDataBuffer *)evtData;
    if (data->sensType == SENS_TYPE_INVALID
            && data->dataType == HOSTINTF_DATA_TYPE_APP_TO_SENSOR_HAL) {
        struct AppToSensorHalDataBuffer *buffer = (struct AppToSensorHalDataBuffer *)data;
        hostIntfAddBlock(data, (buffer->payload.type & EVENT_TYPE_BIT_DISCARDABLE) != 0, false);
    }
}

static void copyEmbeddedSamples(struct ActiveSensor *sensor, const void* evtData)
{
    uint64_t sensorTime = sensorGetTime();

    if (sensor->buffer.length > 0 && sensorTime - sensor->lastTime >= delta_time_max)
        enqueueSensorBuffer(sensor);

    if (sensor->buffer.length == 0) {
        sensor->buffer.length = sizeof(struct SingleAxisDataEvent) + sizeof(struct SingleAxisDataPoint);
        sensor->lastTime = sensor->buffer.referenceTime = sensorTime;
        if (sensor->interrupt == NANOHUB_INT_WAKEUP)
            mWakeupBlocks++;
        else if (sensor->interrupt == NANOHUB_INT_NONWAKEUP)
            mNonWakeupBlocks++;
        sensor->buffer.firstSample.numSamples = 1;
        sensor->buffer.firstSample.interrupt = sensor->interrupt;
        sensor->buffer.single[0].idata = (uint32_t)evtData;
    } else {
        sensor->buffer.length += sizeof(struct SingleAxisDataPoint);
        sensor->buffer.single[sensor->buffer.firstSample.numSamples].deltaTime =
                encodeDeltaTime(sensorTime - sensor->lastTime);
        sensor->lastTime = sensorTime;
        sensor->buffer.single[sensor->buffer.firstSample.numSamples].idata = (uint32_t)evtData;
        sensor->buffer.firstSample.numSamples++;
    }
    if (sensor->curSamples++ == 0)
        sensor->firstTime = sensor->buffer.referenceTime;
}

static uint32_t getSensorInterrupt(struct ActiveSensor *sensor)
{
    uint32_t interrupt = HOSTINTF_MAX_INTERRUPTS;
    uint64_t sensorTime = sensorGetTime();

    if (sensor->firstTime &&
        ((sensorTime >= sensor->firstTime + sensor->latency) ||
         ((sensor->latency > sensorGetCurLatency(sensor->sensorHandle)) &&
          (sensorTime + sensorGetCurLatency(sensor->sensorHandle) > sensor->firstTime + sensor->latency)))) {
        interrupt = sensor->interrupt;
    } else if (mWakeupBlocks + mNonWakeupBlocks >= mTotalBlocks) {
        interrupt = sensor->interrupt;
    }

    return interrupt;
}

static void onEvtSensorDataActive(struct ActiveSensor *sensor, uint32_t evtType, const void* evtData)
{
    if (evtData == SENSOR_DATA_EVENT_FLUSH) {
        queueFlush(sensor);
    } else {
        bool haveFlush = sensor->buffer.firstSample.numFlushes > 0;
        if (sensor->buffer.length > 0 &&
            (haveFlush || sensor->buffer.firstSample.numSamples == sensor->packetSamples)) {
                // processing will be aborted if we have pending flush and are not able to send
                // in this case, send eventually will be retried, otherwise data will be lost
                if (!enqueueSensorBuffer(sensor) && haveFlush)
                    return;
        }

        switch (sensor->numAxis) {
        case NUM_AXIS_EMBEDDED:
            copyEmbeddedSamples(sensor, evtData);
            break;
        case NUM_AXIS_ONE:
            copySingleSamples(sensor, evtData);
            break;
        case NUM_AXIS_THREE:
            if (sensor->raw)
                copyTripleSamplesRaw(sensor, evtData);
            else
                copyTripleSamples(sensor, evtData);
            break;
        default:
            return;
        }
    }

    nanohubPrefetchTx(getSensorInterrupt(sensor), mWakeupBlocks, mNonWakeupBlocks);

    if (sensor->oneshot) {
        sensorRelease(mHostIntfTid, sensor->sensorHandle);
        osEventUnsubscribe(mHostIntfTid, evtType);
        sensor->sensorHandle = 0;
        sensor->oneshot = false;
    }
}

static void onEvtSensorDataInactive(struct ActiveSensor *sensor, uint32_t evtType, const void* evtData)
{
    if (evtData != SENSOR_DATA_EVENT_FLUSH) {
        // handle bias data which can be generated for sensors that are
        // not currently requested by the AP
        switch (sensor->numAxis) {
        case NUM_AXIS_THREE:
            if (((const struct TripleAxisDataEvent *)evtData)->samples[0].firstSample.biasPresent) {
                copyTripleSamplesBias(sensor, evtData);
                nanohubPrefetchTx(HOSTINTF_MAX_INTERRUPTS, mWakeupBlocks, mNonWakeupBlocks);
            }
            break;
        }
    }
}

static void onEvtSensorData(uint32_t evtType, const void* evtData)
{
    if (evtType > EVT_NO_FIRST_SENSOR_EVENT && evtType < EVT_NO_SENSOR_CONFIG_EVENT) {
        struct ActiveSensor *sensor = getActiveSensorByType(evtType & 0xFF);
        if (sensor) {
            if (sensor->sensorHandle)
                onEvtSensorDataActive(sensor, evtType, evtData);
            else
                onEvtSensorDataInactive(sensor, evtType, evtData);
        }
    }
}

static void hostIntfHandleEvent(uint32_t evtType, const void* evtData)
{
    switch (EVENT_GET_EVENT(evtType)) {
    case EVT_APP_START:
        onEvtAppStart(evtData);
        break;
    case EVT_APP_TO_HOST:
        onEvtAppToHost(evtData);
        break;
    case EVT_APP_TO_HOST_CHRE:
        onEvtAppToHostChre(evtData);
        break;
#ifdef LEGACY_HAL_ENABLED
    case EVT_APP_FROM_HOST:
        onEvtAppFromHost(evtData);
        break;
#endif
    case EVT_APP_FROM_HOST_CHRE:
        onEvtAppFromHostChre(evtData);
        break;
#ifdef DEBUG_LOG_EVT
    case EVT_DEBUG_LOG:
        onEvtDebugLog(evtData);
        break;
#endif
    case EVT_LATENCY_TIMER:
        onEvtLatencyTimer(evtData);
        break;
    case EVT_NO_SENSOR_CONFIG_EVENT:
        onEvtNoSensorConfigEvent(evtData);
        break;
    case EVT_APP_TO_SENSOR_HAL_DATA:
        onEvtAppToSensorHalData(evtData);
        break;
    default:
        onEvtSensorData(EVENT_GET_EVENT(evtType), evtData);
        break;
    }
}

void hostIntfCopyInterrupts(void *dst, uint32_t numBits)
{
    if (mInterrupt->numBits != numBits)
        return;

    atomicBitsetBulkRead(mInterrupt, dst, numBits);
}

void hostIntfClearInterrupts()
{
    uint32_t i;

    for (i = 0; i < HOSTINTF_MAX_INTERRUPTS; i++) {
        if (atomicBitsetGetBit(mInterrupt, i))
            hostIntfClearInterrupt(i);
    }
}

void hostIntfSetInterrupt(uint32_t bit)
{
    uint64_t state = cpuIntsOff();
    if (mHostIntfTid) {
        if (!atomicBitsetGetBit(mInterrupt, bit)) {
            atomicBitsetSetBit(mInterrupt, bit);
            if (!atomicBitsetGetBit(mInterruptMask, bit)) {
                if (mInterruptCntWkup++ == 0)
                    apIntSet(true);
            } else {
                if (mInterruptCntNonWkup++ == 0)
                    apIntSet(false);
            }
        }
    }
    cpuIntsRestore(state);
}

bool hostIntfGetInterrupt(uint32_t bit)
{
    return atomicBitsetGetBit(mInterrupt, bit);
}

void hostIntfClearInterrupt(uint32_t bit)
{
    uint64_t state = cpuIntsOff();
    if (mHostIntfTid) {
        if (atomicBitsetGetBit(mInterrupt, bit)) {
            atomicBitsetClearBit(mInterrupt, bit);
            if (!atomicBitsetGetBit(mInterruptMask, bit)) {
                if (--mInterruptCntWkup == 0)
                    apIntClear(true);
            } else {
                if (--mInterruptCntNonWkup == 0)
                    apIntClear(false);
            }
        }
    }
    cpuIntsRestore(state);
}

void hostIntfSetInterruptMask(uint32_t bit)
{
    uint64_t state = cpuIntsOff();
    if (mHostIntfTid) {
        if (!atomicBitsetGetBit(mInterruptMask, bit)) {
            atomicBitsetSetBit(mInterruptMask, bit);
            if (atomicBitsetGetBit(mInterrupt, bit)) {
                if (--mInterruptCntWkup == 0)
                    apIntClear(true);
                if (mInterruptCntNonWkup++ == 0)
                    apIntSet(false);
            }
        }
    }
    cpuIntsRestore(state);
}

bool hostIntfGetInterruptMask(uint32_t bit)
{
    return atomicBitsetGetBit(mInterruptMask, bit);
}

void hostIntfClearInterruptMask(uint32_t bit)
{
    uint64_t state = cpuIntsOff();
    if (mHostIntfTid) {
        if (atomicBitsetGetBit(mInterruptMask, bit)) {
            atomicBitsetClearBit(mInterruptMask, bit);
            if (atomicBitsetGetBit(mInterrupt, bit)) {
                if (mInterruptCntWkup++ == 0)
                    apIntSet(true);
                if (--mInterruptCntNonWkup == 0)
                    apIntClear(false);
            }
        }
    }
    cpuIntsRestore(state);
}

INTERNAL_CHRE_APP_INIT(APP_ID_MAKE(NANOHUB_VENDOR_GOOGLE, 0), 0, hostIntfRequest, hostIntfRelease, hostIntfHandleEvent);