sh2.c

/*
 * Copyright 2015-2022 CEVA, Inc.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License and 
 * any applicable agreements you may have with CEVA, Inc.
 * 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.
 */

/**
 * @file sh2.c
 * @author David Wheeler
 * @date 22 Sept 2015
 * @brief API Definition for SH-2 Sensor Hub.
 *
 * The sh2 API provides functions for opening a session with
 * the sensor hub and performing all supported operations with it.
 * This includes enabling sensors and reading events as well as
 * other housekeeping functions.
 *
 */

//Suppress warning about fopen and strcpy safety under MSVC
#ifdef _MSC_VER
#   ifndef _CRT_SECURE_NO_WARNINGS
#       define _CRT_SECURE_NO_WARNINGS
#   endif
#endif

#include "sh2.h"
#include "sh2_err.h"
#include "shtp.h"
#include "sh2_util.h"

#include <string.h>
#include <stdio.h>

// ------------------------------------------------------------------------
// Private type definitions

#define CHAN_EXECUTABLE_DEVICE    (1)
#define CHAN_SENSORHUB_CONTROL    (2)
#define CHAN_SENSORHUB_INPUT      (3)
#define CHAN_SENSORHUB_INPUT_WAKE (4)
#define CHAN_SENSORHUB_INPUT_GIRV (5)

// executable/device channel responses
#define EXECUTABLE_DEVICE_CMD_RESET (1)
#define EXECUTABLE_DEVICE_CMD_ON    (2)
#define EXECUTABLE_DEVICE_CMD_SLEEP (3)

// executable/device channel responses
#define EXECUTABLE_DEVICE_RESP_RESET_COMPLETE (1)

// Tags for sensorhub app advertisements.
#define TAG_SH2_VERSION (0x80)
#define TAG_SH2_REPORT_LENGTHS (0x81)

// Max length of sensorhub version string.
#define MAX_VER_LEN (16)

// Max number of report ids supported
#define SH2_MAX_REPORT_IDS (64)

#if defined(_MSC_VER)
#define PACKED_STRUCT struct
#pragma pack(push, 1)
#elif defined(__GNUC__)
#define PACKED_STRUCT struct __attribute__((packed))
#else 
#define PACKED_STRUCT __packed struct
#endif

#define ADVERT_TIMEOUT_US (200000)

// Command and Subcommand values
#define SH2_CMD_ERRORS                 1
#define SH2_CMD_COUNTS                 2
#define     SH2_COUNTS_GET_COUNTS          0
#define     SH2_COUNTS_CLEAR_COUNTS        1
#define SH2_CMD_TARE                   3
#define     SH2_TARE_TARE_NOW              0
#define     SH2_TARE_PERSIST_TARE          1
#define     SH2_TARE_SET_REORIENTATION     2
#define SH2_CMD_INITIALIZE             4
#define     SH2_INIT_SYSTEM                1
#define     SH2_INIT_UNSOLICITED           0x80
// #define SH2_CMD_FRS                    5 /* Depreciated */
#define SH2_CMD_DCD                    6
#define SH2_CMD_ME_CAL                 7
#define SH2_CMD_DCD_SAVE               9
#define SH2_CMD_GET_OSC_TYPE           0x0A
#define SH2_CMD_CLEAR_DCD_AND_RESET    0x0B
#define SH2_CMD_CAL                    0x0C
#define     SH2_CAL_START                   0
#define     SH2_CAL_FINISH                  1
#define SH2_CMD_BOOTLOADER             0x0D     /* SH-2 Reference Manual 6.4.12 */
#define     SH2_BL_MODE_REQ                 0
#define     SH2_BL_STATUS_REQ               1
#define SH2_CMD_INTERACTIVE_ZRO        0x0E     /* SH-2 Reference Manual 6.4.13 */
#define SH2_CMD_WHEEL_REQ              0x0F
#define SH2_CMD_DR_CAL_SAVE            0x10

// SENSORHUB_COMMAND_REQ
#define SENSORHUB_COMMAND_REQ        (0xF2)
#define COMMAND_PARAMS (9)
typedef PACKED_STRUCT {
    uint8_t reportId;
    uint8_t seq;
    uint8_t command;
    uint8_t p[COMMAND_PARAMS];
} CommandReq_t;

// SENSORHUB_COMMAND_RESP
#define SENSORHUB_COMMAND_RESP       (0xF1)
#define RESPONSE_VALUES (11)
typedef PACKED_STRUCT {
    uint8_t reportId;
    uint8_t seq;
    uint8_t command;
    uint8_t commandSeq;
    uint8_t respSeq;
    uint8_t r[RESPONSE_VALUES];
} CommandResp_t;

// SENSORHUB_PROD_ID_REQ
#define SENSORHUB_PROD_ID_REQ        (0xF9)
typedef PACKED_STRUCT {
    uint8_t reportId;  
    uint8_t reserved;
} ProdIdReq_t;

// SENSORHUB_PROD_ID_RESP
#define SENSORHUB_PROD_ID_RESP       (0xF8)
typedef PACKED_STRUCT {
    uint8_t reportId;
    uint8_t resetCause;
    uint8_t swVerMajor;
    uint8_t swVerMinor;
    uint32_t swPartNumber;
    uint32_t swBuildNumber;
    uint16_t swVerPatch;
    uint8_t reserved0;
    uint8_t reserved1;
} ProdIdResp_t;

// Report definitions
// Bit fields for Feature Report flags
#define FEAT_CHANGE_SENSITIVITY_MODE_BIT   0
#define FEAT_CHANGE_SENSITIVITY_ENABLE_BIT 1
#define FEAT_WAKEUP_ENABLE_BIT             2
#define FEAT_ALWAYS_ON_ENABLE_BIT          3
#define FEAT_SNIFF_ENABLE_BIT              4
#define FEAT_CHANGE_SENSITIVITY_RELATIVE   (1 << FEAT_CHANGE_SENSITIVITY_MODE_BIT)
#define FEAT_CHANGE_SENSITIVITY_ABSOLUTE   (0 << FEAT_CHANGE_SENSITIVITY_MODE_BIT)
#define FEAT_CHANGE_SENSITIVITY_ENABLED    (1 << FEAT_CHANGE_SENSITIVITY_ENABLE_BIT)
#define FEAT_CHANGE_SENSITIVITY_DISABLED   (0 << FEAT_CHANGE_SENSITIVITY_ENABLE_BIT)
#define FEAT_WAKE_ENABLED                  (1 << FEAT_WAKEUP_ENABLE_BIT)
#define FEAT_WAKE_DISABLED                 (0 << FEAT_WAKEUP_ENABLE_BIT)
#define FEAT_ALWAYS_ON_ENABLED             (1 << FEAT_ALWAYS_ON_ENABLE_BIT)
#define FEAT_ALWAYS_ON_DISABLED            (0 << FEAT_ALWAYS_ON_ENABLE_BIT)
#define FEAT_SNIFF_ENABLED                 (1 << FEAT_SNIFF_ENABLE_BIT)
#define FEAT_SNIFF_DISABLED                (0 << FEAT_SNIFF_ENABLE_BIT)


// GET_FEATURE_REQ
#define SENSORHUB_GET_FEATURE_REQ    (0xFE)
typedef PACKED_STRUCT{
    uint8_t reportId;
    uint8_t featureReportId;
} GetFeatureReq_t;

// SENSORHUB_GET_FEATURE_RESP
#define SENSORHUB_GET_FEATURE_RESP   (0xFC)
typedef PACKED_STRUCT{
    uint8_t reportId;
    uint8_t featureReportId;      // sensor id
    uint8_t flags;                // FEAT_... values
    uint16_t changeSensitivity;
    uint32_t reportInterval_uS;
    uint32_t batchInterval_uS;
    uint32_t sensorSpecific;
} GetFeatureResp_t;


typedef struct sh2_s sh2_t;

typedef int (sh2_OpStart_t)(sh2_t *pSh2);
typedef void (sh2_OpRx_t)(sh2_t *pSh2, const uint8_t *payload, uint16_t len);
typedef void (sh2_OpReset_t)(sh2_t *pSh2);

typedef struct sh2_Op_s {
    uint32_t timeout_us;
    sh2_OpStart_t *start;
    sh2_OpRx_t *rx;
    sh2_OpReset_t *onReset;
} sh2_Op_t;

// Parameters and state information for the operation in progress
typedef union {
    struct {
        CommandReq_t req;
    } sendCmd;
    struct {
        sh2_ProductIds_t *pProdIds;
        uint8_t nextEntry;
        uint8_t expectedEntries;
    } getProdIds;
    struct {
        sh2_SensorConfig_t *pConfig;
        sh2_SensorId_t sensorId;
    } getSensorConfig;
    struct {
        const sh2_SensorConfig_t *pConfig;
        sh2_SensorId_t sensorId;
    } setSensorConfig;
    struct {
        uint16_t frsType;
        uint32_t *pData;
        uint16_t *pWords;
        uint16_t nextOffset;
    } getFrs;
    struct {
        uint16_t frsType;
        uint32_t *pData;
        uint16_t words;
        uint16_t offset;
    } setFrs;
    struct {
        uint8_t severity;
        sh2_ErrorRecord_t *pErrors;
        uint16_t *pNumErrors;
        uint16_t errsRead;
    } getErrors;
    struct {
        sh2_SensorId_t sensorId;
        sh2_Counts_t *pCounts;
    } getCounts;
    struct {
        uint8_t sensors;
    } calConfig;
    struct {
        uint8_t *pSensors;
    } getCalConfig;
    struct {
        sh2_SensorId_t sensorId;
    } forceFlush;
    struct {
        sh2_OscType_t *pOscType;
    } getOscType;
    struct {
        uint32_t interval_us;
    } startCal;
    struct {
        sh2_CalStatus_t status;
    } finishCal;
    struct {
        uint8_t wheelIndex;
        uint32_t timestamp;
        int16_t wheelData;
        uint8_t dataType;
    } wheelRequest;
} sh2_OpData_t;

// Max length of an FRS record, words.
#define MAX_FRS_WORDS (72)

struct sh2_s {
    // Pointer to the SHTP HAL
    sh2_Hal_t *pHal;

    // associated SHTP instance
    void *pShtp;
    
    volatile bool resetComplete;
    char version[MAX_VER_LEN+1];

    // Multi-step operation support
    const sh2_Op_t *pOp;
    int opStatus;
    sh2_OpData_t opData;
    uint8_t lastCmdId;
    uint8_t cmdSeq;
    uint8_t nextCmdSeq;
    
    // Event callback and it's cookie
    sh2_EventCallback_t *eventCallback;
    void * eventCookie;

    // Sensor callback and it's cookie
    sh2_SensorCallback_t *sensorCallback;
    void * sensorCookie;

    // Storage space for reading sensor metadata
    uint32_t frsData[MAX_FRS_WORDS];
    uint16_t frsDataLen;

    // Stats
    uint32_t execBadPayload;
    uint32_t emptyPayloads;
    uint32_t unknownReportIds;

};

#define SENSORHUB_BASE_TIMESTAMP_REF (0xFB)
typedef PACKED_STRUCT {
    uint8_t reportId;
    int32_t timebase;
} BaseTimestampRef_t;

#define SENSORHUB_TIMESTAMP_REBASE   (0xFA)
typedef PACKED_STRUCT {
    uint8_t reportId;
    int32_t timebase;
} TimestampRebase_t;

// SENSORHUB_FORCE_SENSOR_FLUSH
#define SENSORHUB_FORCE_SENSOR_FLUSH (0xF0)
typedef PACKED_STRUCT {
    uint8_t reportId;
    uint8_t sensorId;
} ForceFlushReq_t;

// SENSORHUB_FLUSH_COMPLETED    
#define SENSORHUB_FLUSH_COMPLETED    (0xEF)
typedef PACKED_STRUCT {
    uint8_t reportId;
    uint8_t sensorId;
} ForceFlushResp_t;

typedef struct sh2_ReportLen_s {
    uint8_t id;
    uint8_t len;
} sh2_ReportLen_t;

// SENSORHUB_FRS_WRITE_REQ
#define SENSORHUB_FRS_WRITE_REQ      (0xF7)
typedef PACKED_STRUCT {
    uint8_t reportId;
    uint8_t reserved;
    uint16_t length;
    uint16_t frsType;
} FrsWriteReq_t;

// SENSORHUB_FRS_WRITE_DATA_REQ
#define SENSORHUB_FRS_WRITE_DATA_REQ (0xF6)
typedef PACKED_STRUCT {
    uint8_t reportId;
    uint8_t reserved;
    uint16_t offset;
    uint32_t data0;
    uint32_t data1;
} FrsWriteDataReq_t;

// FRS write status values
#define FRS_WRITE_STATUS_RECEIVED (0)
#define FRS_WRITE_STATUS_UNRECOGNIZED_FRS_TYPE (1)
#define FRS_WRITE_STATUS_BUSY (2)
#define FRS_WRITE_STATUS_WRITE_COMPLETED (3)
#define FRS_WRITE_STATUS_READY (4)
#define FRS_WRITE_STATUS_FAILED (5)
#define FRS_WRITE_STATUS_NOT_READY (6) // data received when not in write mode
#define FRS_WRITE_STATUS_INVALID_LENGTH (7)
#define FRS_WRITE_STATUS_RECORD_VALID (8)
#define FRS_WRITE_STATUS_INVALID_RECORD (9)
#define FRS_WRITE_STATUS_DEVICE_ERROR (10)
#define FRS_WRITE_STATUS_READ_ONLY (11)

// SENSORHUB_FRS_WRITE_RESP
#define SENSORHUB_FRS_WRITE_RESP     (0xF5)
typedef PACKED_STRUCT {
    uint8_t reportId;
    uint8_t status;
    uint16_t wordOffset;
} FrsWriteResp_t;

// RESP_FRS_READ_REQ
#define SENSORHUB_FRS_READ_REQ       (0xF4)
typedef PACKED_STRUCT {
    uint8_t reportId;
    uint8_t reserved;
    uint16_t readOffset;
    uint16_t frsType;
    uint16_t blockSize;
} FrsReadReq_t;

// Get Datalen portion of len_status field
#define FRS_READ_DATALEN(x) (((x) >> 4) & 0x0F)

// Get status portion of len_status field
#define FRS_READ_STATUS(x) ((x) & 0x0F)

// Status values
#define FRS_READ_STATUS_NO_ERROR                        0
#define FRS_READ_STATUS_UNRECOGNIZED_FRS_TYPE           1
#define FRS_READ_STATUS_BUSY                            2
#define FRS_READ_STATUS_READ_RECORD_COMPLETED           3
#define FRS_READ_STATUS_OFFSET_OUT_OF_RANGE             4
#define FRS_READ_STATUS_RECORD_EMPTY                    5
#define FRS_READ_STATUS_READ_BLOCK_COMPLETED            6
#define FRS_READ_STATUS_READ_BLOCK_AND_RECORD_COMPLETED 7
#define FRS_READ_STATUS_DEVICE_ERROR                    8

// SENSORHUB_FRS_READ_RESP
#define SENSORHUB_FRS_READ_RESP      (0xF3)
typedef PACKED_STRUCT {
    uint8_t reportId;
    uint8_t len_status;  // See FRS_READ... macros above
    uint16_t wordOffset;
    uint32_t data0;
    uint32_t data1;
    uint16_t frsType;
    uint8_t reserved0;
    uint8_t reserved1;
} FrsReadResp_t;

// ------------------------------------------------------------------------
// Private data

// SH2 state
sh2_t _sh2;

// SH2 Async Event Message
static sh2_AsyncEvent_t sh2AsyncEvent;

// Lengths of reports by report id.
static const sh2_ReportLen_t sh2ReportLens[] = {
    // Sensor reports
    {.id = SH2_ACCELEROMETER,                .len = 10},  
    {.id = SH2_GYROSCOPE_CALIBRATED,         .len = 10},
    {.id = SH2_MAGNETIC_FIELD_CALIBRATED,    .len = 10},
    {.id = SH2_LINEAR_ACCELERATION,          .len = 10},
    {.id = SH2_ROTATION_VECTOR,              .len = 14},
    {.id = SH2_GRAVITY,                      .len = 10},
    {.id = SH2_GYROSCOPE_UNCALIBRATED,       .len = 16},
    {.id = SH2_GAME_ROTATION_VECTOR,         .len = 12},
    {.id = SH2_GEOMAGNETIC_ROTATION_VECTOR,  .len = 14},
    {.id = SH2_PRESSURE,                     .len =  8},
    {.id = SH2_AMBIENT_LIGHT,                .len =  8},
    {.id = SH2_HUMIDITY,                     .len =  6},
    {.id = SH2_PROXIMITY,                    .len =  6},
    {.id = SH2_TEMPERATURE,                  .len =  6},
    {.id = SH2_MAGNETIC_FIELD_UNCALIBRATED,  .len = 16},
    {.id = SH2_TAP_DETECTOR,                 .len =  5},
    {.id = SH2_STEP_COUNTER,                 .len = 12},
    {.id = SH2_SIGNIFICANT_MOTION,           .len =  6},
    {.id = SH2_STABILITY_CLASSIFIER,         .len =  6},
    {.id = SH2_RAW_ACCELEROMETER,            .len = 16},
    {.id = SH2_RAW_GYROSCOPE,                .len = 16},
    {.id = SH2_RAW_MAGNETOMETER,             .len = 16},
    {.id = SH2_STEP_DETECTOR,                .len =  8},
    {.id = SH2_SHAKE_DETECTOR,               .len =  6},
    {.id = SH2_FLIP_DETECTOR,                .len =  6},
    {.id = SH2_PICKUP_DETECTOR,              .len =  8},
    {.id = SH2_STABILITY_DETECTOR,           .len =  6},
    {.id = SH2_PERSONAL_ACTIVITY_CLASSIFIER, .len = 16},
    {.id = SH2_SLEEP_DETECTOR,               .len =  6},
    {.id = SH2_TILT_DETECTOR,                .len =  6},
    {.id = SH2_POCKET_DETECTOR,              .len =  6},
    {.id = SH2_CIRCLE_DETECTOR,              .len =  6},
    {.id = SH2_HEART_RATE_MONITOR,           .len =  6},
    {.id = SH2_ARVR_STABILIZED_RV,           .len = 14},
    {.id = SH2_ARVR_STABILIZED_GRV,          .len = 12},
    {.id = SH2_GYRO_INTEGRATED_RV,           .len = 14},
    {.id = SH2_IZRO_MOTION_REQUEST,          .len =  6},
    {.id = SH2_RAW_OPTICAL_FLOW,             .len = 24},
    {.id = SH2_DEAD_RECKONING_POSE,          .len = 60},
    {.id = SH2_WHEEL_ENCODER,                .len = 12},

    // Other response types
    {.id = SENSORHUB_FLUSH_COMPLETED,        .len =  2},
    {.id = SENSORHUB_COMMAND_RESP,           .len = 16},
    {.id = SENSORHUB_FRS_READ_RESP,          .len = 16},
    {.id = SENSORHUB_FRS_WRITE_RESP,         .len =  4},
    {.id = SENSORHUB_PROD_ID_RESP,           .len = 16},
    {.id = SENSORHUB_TIMESTAMP_REBASE,       .len =  5},
    {.id = SENSORHUB_BASE_TIMESTAMP_REF,     .len =  5},
    {.id = SENSORHUB_GET_FEATURE_RESP,       .len = 17},
};

// ------------------------------------------------------------------------
// Private functions

// SH-2 transaction phases
static int opStart(sh2_t *pSh2, const sh2_Op_t *pOp)
{
    // return error if another operation already in progress
    if (pSh2->pOp) return SH2_ERR_OP_IN_PROGRESS;

    // Establish this operation as the new operation in progress
    pSh2->pOp = pOp;
    pSh2->opStatus = SH2_OK;
    int rc = pOp->start(pSh2);  // Call start method
    if (rc != SH2_OK) {
        // Unregister this operation
        pSh2->opStatus = rc;
        pSh2->pOp = 0;
    }

    return rc;
}

static void opRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{ 
    if ((pSh2->pOp != 0) &&                      // An operation is in progress
        (pSh2->pOp->rx != 0)) {                  // and it has an rx method
        pSh2->pOp->rx(pSh2, payload, len);  // Call receive method
    }
}

static int opCompleted(sh2_t *pSh2, int status)
{
    // Record status
    pSh2->opStatus = status;

    // Signal that op is done.
    pSh2->pOp = 0;

    return SH2_OK;
}

static void opOnReset(sh2_t *pSh2)
{
    if (pSh2->pOp != 0) {
        if (pSh2->pOp->onReset != 0) {
            // This operation has its own reset handler so use it.
            pSh2->pOp->onReset(pSh2);
        }
        else {
            // No reset handler : abort the operation with SH2_ERR code
            opCompleted(pSh2, SH2_ERR);
        }
    }
}

static uint8_t getReportLen(uint8_t reportId)
{
    for (unsigned n = 0; n < ARRAY_LEN(sh2ReportLens); n++) {
        if (sh2ReportLens[n].id == reportId) {
            return sh2ReportLens[n].len;
        }
    }

    return 0;
}

static void sensorhubControlHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
    (void)timestamp;  // unused.
    
    sh2_t *pSh2 = (sh2_t *)cookie;

    uint16_t cursor = 0;
    uint32_t count = 0;
    CommandResp_t * pResp = 0;

    if (len == 0) {
        pSh2->emptyPayloads++;
        return;
    }

    while (cursor < len) {
        // Get next report id
        count++;
        uint8_t reportId = payload[cursor];

        // Determine report length
        uint8_t reportLen = getReportLen(reportId);
        if (reportLen == 0) {
            // An unrecognized report id
            pSh2->unknownReportIds++;
            return;
        }
        else {
            // Check for unsolicited initialize response
            if (reportId == SENSORHUB_COMMAND_RESP) {
                pResp = (CommandResp_t *)(payload+cursor);
                if ((pResp->command == (SH2_CMD_INITIALIZE | SH2_INIT_UNSOLICITED)) &&
                    (pResp->r[1] == SH2_INIT_SYSTEM)) {
                    // This is an unsolicited INIT message.
                    // Ignore this.  EXECUTABLE_DEVICE_RESP_RESET_COMPLETE makes it redundant.
                }

            } // Check for Get Feature Response
            else if (reportId == SENSORHUB_GET_FEATURE_RESP) {
                if (pSh2->eventCallback) {
                    GetFeatureResp_t * pGetFeatureResp;
                    pGetFeatureResp = (GetFeatureResp_t *)(payload + cursor);

                    sh2AsyncEvent.eventId = SH2_GET_FEATURE_RESP;
                    sh2AsyncEvent.sh2SensorConfigResp.sensorId = pGetFeatureResp->featureReportId;
                    sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.changeSensitivityEnabled =
                        ((pGetFeatureResp->flags & FEAT_CHANGE_SENSITIVITY_ENABLED) != 0);
                    sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.changeSensitivityRelative =
                        ((pGetFeatureResp->flags & FEAT_CHANGE_SENSITIVITY_RELATIVE) != 0);
                    sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.wakeupEnabled =
                        ((pGetFeatureResp->flags & FEAT_WAKE_ENABLED) != 0);
                    sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.alwaysOnEnabled =
                        ((pGetFeatureResp->flags & FEAT_ALWAYS_ON_ENABLED) != 0);
                    sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.changeSensitivity =
                        pGetFeatureResp->changeSensitivity;
                    sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.reportInterval_us =
                        pGetFeatureResp->reportInterval_uS;
                    sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.batchInterval_us =
                        pGetFeatureResp->batchInterval_uS;
                    sh2AsyncEvent.sh2SensorConfigResp.sensorConfig.sensorSpecific =
                        pGetFeatureResp->sensorSpecific;

                    pSh2->eventCallback(pSh2->eventCookie, &sh2AsyncEvent);
                }
            }

            // Hand off to operation in progress, if any
            opRx(pSh2, payload+cursor, reportLen);
            cursor += reportLen;
        }
    }
}


static int opProcess(sh2_t *pSh2, const sh2_Op_t *pOp)
{
    int status = SH2_OK;
    uint32_t start_us = 0;

    start_us = pSh2->pHal->getTimeUs(pSh2->pHal);
    
    status = opStart(pSh2, pOp);
    if (status != SH2_OK) {
        return status;
    }

    uint32_t now_us = start_us;
    // While op not complete and not timed out.
    while ((pSh2->pOp != 0) &&
           ((pOp->timeout_us == 0) ||
            ((now_us-start_us) < pOp->timeout_us))) {

        if (pSh2->pShtp == 0) {
            // Was SH2 interface closed unexpectedly?
            pSh2->opStatus = SH2_ERR;
            break;
        }
            
        // Service SHTP to poll the device.
        shtp_service(pSh2->pShtp);

        // Update the time
        now_us = pSh2->pHal->getTimeUs(pSh2->pHal);
    }

    if (pSh2->pOp != 0) {
        // Operation has timed out.  Clean up.
        pSh2->pOp = 0;
        pSh2->opStatus = SH2_ERR_TIMEOUT;
    }

    return pSh2->opStatus;
}

// Produce 64-bit microsecond timestamp for a sensor event
static uint64_t touSTimestamp(uint32_t hostInt, int32_t referenceDelta, uint16_t delay)
{
    static uint32_t lastHostInt = 0;
    static uint32_t rollovers = 0;
    uint64_t timestamp;

    // Count times hostInt timestamps rolled over to produce upper bits
    if (hostInt < lastHostInt) {
        rollovers++;
    }
    lastHostInt = hostInt;
    
    timestamp = ((uint64_t)rollovers << 32);
    timestamp += hostInt + (referenceDelta + delay) * 100;

    return timestamp;
}

static void sensorhubInputHdlr(sh2_t *pSh2, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
    sh2_SensorEvent_t event;
    uint16_t cursor = 0;

    int32_t referenceDelta = 0;

    while (cursor < len) {
        // Get next report id
        uint8_t reportId = payload[cursor];

        // Determine report length
        uint8_t reportLen = getReportLen(reportId);
        if (reportLen == 0) {
            // An unrecognized report id
            pSh2->unknownReportIds++;
            return;
        }
        else {
            if (reportId == SENSORHUB_BASE_TIMESTAMP_REF) {
                const BaseTimestampRef_t *rpt = (const BaseTimestampRef_t *)(payload+cursor);
                
                // store base timestamp reference
                referenceDelta = -rpt->timebase;
            }
            else if (reportId == SENSORHUB_TIMESTAMP_REBASE) {
                const TimestampRebase_t *rpt = (const TimestampRebase_t *)(payload+cursor);

                referenceDelta += rpt->timebase;
            }
            else if (reportId == SENSORHUB_FLUSH_COMPLETED) {
                // Route this as if it arrived on command channel.
                opRx(pSh2, payload+cursor, reportLen);
            }
            else {
                // Sensor event.  Call callback
                uint8_t *pReport = payload+cursor;
                uint16_t delay = ((pReport[2] & 0xFC) << 6) + pReport[3];
                event.timestamp_uS = touSTimestamp(timestamp, referenceDelta, delay);
                event.delay_uS = (referenceDelta + delay) * 100;
                event.reportId = reportId;
                memcpy(event.report, pReport, reportLen);
                event.len = reportLen;
                if (pSh2->sensorCallback != 0) {
                    pSh2->sensorCallback(pSh2->sensorCookie, &event);
                }
            }
            
            // Move to next report in the payload
            cursor += reportLen;
        }
    }
}

static void sensorhubInputNormalHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
    sh2_t *pSh2 = (sh2_t *)cookie;

    sensorhubInputHdlr(pSh2, payload, len, timestamp);
}

static void sensorhubInputWakeHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
    sh2_t *pSh2 = (sh2_t *)cookie;
    
    sensorhubInputHdlr(pSh2, payload, len, timestamp);
}

static void sensorhubInputGyroRvHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
    sh2_t *pSh2 = (sh2_t *)cookie;
    sh2_SensorEvent_t event;
    uint16_t cursor = 0;

    uint8_t reportId = SH2_GYRO_INTEGRATED_RV;
    uint8_t reportLen = getReportLen(reportId);

    while (cursor < len) {
        event.timestamp_uS = timestamp;
        event.reportId = reportId;
        memcpy(event.report, payload+cursor, reportLen);
        event.len = reportLen;

        if (pSh2->sensorCallback != 0) {
            pSh2->sensorCallback(pSh2->sensorCookie, &event);
        }

        cursor += reportLen;
    }
}

static void executableDeviceHdlr(void *cookie, uint8_t *payload, uint16_t len, uint32_t timestamp)
{
    (void)timestamp;  // unused
    
    sh2_t *pSh2 = (sh2_t *)cookie;

    // Discard if length is bad
    if (len != 1) {
        pSh2->execBadPayload++;
        return;
    }
    
    switch (payload[0]) {
        case EXECUTABLE_DEVICE_RESP_RESET_COMPLETE:
            // reset process is now done.
            pSh2->resetComplete = true;
            
            // Send reset event to SH2 operation processor.
            // Some commands may handle themselves.  Most will be aborted with SH2_ERR.
            opOnReset(pSh2);

            // Notify client that reset is complete.
            sh2AsyncEvent.eventId = SH2_RESET;
            if (pSh2->eventCallback) {
                pSh2->eventCallback(pSh2->eventCookie, &sh2AsyncEvent);
            }
            break;
        default:
            pSh2->execBadPayload++;
            break;
    }
}

static int sendExecutable(sh2_t *pSh2, uint8_t cmd)
{
    return shtp_send(pSh2->pShtp, CHAN_EXECUTABLE_DEVICE, &cmd, 1);
}

static int sendCtrl(sh2_t *pSh2, const uint8_t *data, uint16_t len)
{
    return shtp_send(pSh2->pShtp, CHAN_SENSORHUB_CONTROL, data, len);
}

static int16_t toQ14(double x)
{
    int16_t retval = (int16_t)(x * (1<<14));
    
    return retval;
}

// ------------------------------------------------------------------------
// Get Product ID support

// Get Product ID Op handler
static int getProdIdStart(sh2_t *pSh2)
{
    int rc = SH2_OK;
    ProdIdReq_t req;
    
    pSh2->opData.getProdIds.nextEntry = 0;
    pSh2->opData.getProdIds.expectedEntries = 4;  // Most products supply 4 product ids.
                                                // When the first arrives, we'll know if
                                                // we need to adjust this.
    
    // Set up request to issue
    memset(&req, 0, sizeof(req));
    req.reportId = SENSORHUB_PROD_ID_REQ;
    rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));

    return rc;
}

static void getProdIdRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len;  // unused
    
    ProdIdResp_t *resp = (ProdIdResp_t *)payload;
    
    // skip this if it isn't the product id response.
    if (resp->reportId != SENSORHUB_PROD_ID_RESP) return;

    // Store this product id, if we can
    sh2_ProductIds_t *pProdIds = pSh2->opData.getProdIds.pProdIds;
    
    if (pProdIds) {
        // Store the product id response
        if (pSh2->opData.getProdIds.nextEntry < pSh2->opData.getProdIds.expectedEntries) {
            sh2_ProductId_t *pProdId = &pProdIds->entry[pSh2->opData.getProdIds.nextEntry];
            
            pProdId->resetCause = resp->resetCause;
            pProdId->swVersionMajor = resp->swVerMajor;
            pProdId->swVersionMinor = resp->swVerMinor;
            pProdId->swPartNumber = resp->swPartNumber;
            pProdId->swBuildNumber = resp->swBuildNumber;
            pProdId->swVersionPatch = resp->swVerPatch;
            pProdId->reserved0 = resp->reserved0;
            pProdId->reserved1 = resp->reserved1;

            if ((pProdId->swPartNumber == 10004095) ||
                (pProdId->swPartNumber == 10004818) ||
                (pProdId->swPartNumber == 10005028)) {
                // FSP200 has 5 product id entries
                pSh2->opData.getProdIds.expectedEntries = 5;
            }


            pSh2->opData.getProdIds.nextEntry++;
        }
    }

    // Complete this operation if there is no storage for more product ids
    if ((pSh2->opData.getProdIds.pProdIds == 0) ||
        (pSh2->opData.getProdIds.nextEntry >= pSh2->opData.getProdIds.expectedEntries)) {
        
        pSh2->opData.getProdIds.pProdIds->numEntries = pSh2->opData.getProdIds.nextEntry;
        opCompleted(pSh2, SH2_OK);
    }

    return;
}

const sh2_Op_t getProdIdOp = {
    .start = getProdIdStart,
    .rx = getProdIdRx,
};

// ------------------------------------------------------------------------
// Set Sensor Config

static int getSensorConfigStart(sh2_t *pSh2)
{
    int rc = SH2_OK;
    GetFeatureReq_t req;
    
    // set up request to issue
    memset(&req, 0, sizeof(req));
    req.reportId = SENSORHUB_GET_FEATURE_REQ;
    req.featureReportId = pSh2->opData.getSensorConfig.sensorId;
    rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));

    return rc;
}

static void getSensorConfigRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    
    GetFeatureResp_t *resp = (GetFeatureResp_t *)payload;
    sh2_SensorConfig_t *pConfig;
    
    // skip this if it isn't the response we're waiting for.
    if (resp->reportId != SENSORHUB_GET_FEATURE_RESP) return;
    if (resp->featureReportId != pSh2->opData.getSensorConfig.sensorId) return;

    // Copy out data
    pConfig = pSh2->opData.getSensorConfig.pConfig;
    
    pConfig->changeSensitivityEnabled = ((resp->flags & FEAT_CHANGE_SENSITIVITY_ENABLED) != 0);
    pConfig->changeSensitivityRelative = ((resp->flags & FEAT_CHANGE_SENSITIVITY_RELATIVE) != 0);
    pConfig->wakeupEnabled = ((resp->flags & FEAT_WAKE_ENABLED) != 0);
    pConfig->alwaysOnEnabled = ((resp->flags & FEAT_ALWAYS_ON_ENABLED) != 0);
    pConfig->sniffEnabled = ((resp->flags & FEAT_SNIFF_ENABLED) !=0);
    pConfig->changeSensitivity = resp->changeSensitivity;
    pConfig->reportInterval_us = resp->reportInterval_uS;
    pConfig->batchInterval_us = resp->batchInterval_uS;
    pConfig->sensorSpecific = resp->sensorSpecific;

    // Complete this operation
    opCompleted(pSh2, SH2_OK);

    return;
}

const sh2_Op_t getSensorConfigOp = {
    .start = getSensorConfigStart,
    .rx = getSensorConfigRx,
};

// ------------------------------------------------------------------------
// Set Sensor Config

// SENSORHUB_SET_FEATURE_CMD
#define SENSORHUB_SET_FEATURE_CMD    (0xFD)
typedef PACKED_STRUCT {
    uint8_t reportId;             // 0xFD
    uint8_t featureReportId;      // sensor id
    uint8_t flags;                // FEAT_... values
    uint16_t changeSensitivity;
    uint32_t reportInterval_uS;
    uint32_t batchInterval_uS;
    uint32_t sensorSpecific;
} SetFeatureReport_t;

static int setSensorConfigStart(sh2_t *pSh2)
{
    SetFeatureReport_t req;
    uint8_t flags = 0;
    int rc;
    sh2_SensorConfig_t *pConfig = pSh2->opData.getSensorConfig.pConfig;
    
    if (pConfig->changeSensitivityEnabled)  flags |= FEAT_CHANGE_SENSITIVITY_ENABLED;
    if (pConfig->changeSensitivityRelative) flags |= FEAT_CHANGE_SENSITIVITY_RELATIVE;
    if (pConfig->wakeupEnabled)             flags |= FEAT_WAKE_ENABLED;
    if (pConfig->alwaysOnEnabled)           flags |= FEAT_ALWAYS_ON_ENABLED;
    if (pConfig->sniffEnabled)              flags |= FEAT_SNIFF_ENABLED;

    memset(&req, 0, sizeof(req));
    req.reportId = SENSORHUB_SET_FEATURE_CMD;
    req.featureReportId = pSh2->opData.setSensorConfig.sensorId;
    req.flags = flags;
    req.changeSensitivity = pConfig->changeSensitivity;
    req.reportInterval_uS = pConfig->reportInterval_us;
    req.batchInterval_uS = pConfig->batchInterval_us;
    req.sensorSpecific = pConfig->sensorSpecific;

    rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
    opCompleted(pSh2, rc);

    return rc;
}

const sh2_Op_t setSensorConfigOp = {
    .start = setSensorConfigStart,
};

// ------------------------------------------------------------------------
// Get FRS.


static int getFrsStart(sh2_t *pSh2)
{
    int rc = SH2_OK;
    FrsReadReq_t req;

    pSh2->opData.getFrs.nextOffset = 0;
    
    // set up request to issue
    memset(&req, 0, sizeof(req));
    req.reportId = SENSORHUB_FRS_READ_REQ;
    req.reserved = 0;
    req.readOffset = 0; // read from start
    req.frsType = pSh2->opData.getFrs.frsType;
    req.blockSize = 0;  // read all avail data

    rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));

    return rc;
}

static void getFrsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    
    FrsReadResp_t *resp = (FrsReadResp_t *)payload;
    uint8_t status;

    // skip this if it isn't the response we're looking for
    if (resp->reportId != SENSORHUB_FRS_READ_RESP) return;

    // Check for errors: Unrecognized FRS type, Busy, Out of range, Device error
    status = FRS_READ_STATUS(resp->len_status);
    if ((status == FRS_READ_STATUS_UNRECOGNIZED_FRS_TYPE) ||
        (status == FRS_READ_STATUS_BUSY) ||
        (status == FRS_READ_STATUS_OFFSET_OUT_OF_RANGE) ||
        (status == FRS_READ_STATUS_DEVICE_ERROR)
        ) {
        // Operation failed
        opCompleted(pSh2, SH2_ERR_HUB);
        return;
    }

    if (status == FRS_READ_STATUS_RECORD_EMPTY) {
        // Empty record, return zero length.
        *(pSh2->opData.getFrs.pWords) = 0;
        opCompleted(pSh2, SH2_OK);
    }

    // Store the contents from this response
    uint16_t offset = resp->wordOffset;

    // check for missed offsets, resulting in error.
    if (offset != pSh2->opData.getFrs.nextOffset) {
        // Some data was dropped.
        *(pSh2->opData.getFrs.pWords) = 0;
        opCompleted(pSh2, SH2_ERR_IO);
    }
    
    // store first word, if we have room
    if ((*(pSh2->opData.getFrs.pWords) == 0) ||
        (offset <= *(pSh2->opData.getFrs.pWords))) {
        pSh2->opData.getFrs.pData[offset] = resp->data0;
        pSh2->opData.getFrs.nextOffset = offset+1;
    }

    // store second word if there is one and we have room
    if ((FRS_READ_DATALEN(resp->len_status) == 2)  &&
        ((*(pSh2->opData.getFrs.pWords) == 0) ||
         (offset <= *(pSh2->opData.getFrs.pWords)))) {
        pSh2->opData.getFrs.pData[offset+1] = resp->data1;
        pSh2->opData.getFrs.nextOffset = offset+2;
    }

    // If read is done, complete the operation
    if ((status == FRS_READ_STATUS_READ_RECORD_COMPLETED) ||
        (status == FRS_READ_STATUS_READ_BLOCK_COMPLETED) ||
        (status == FRS_READ_STATUS_READ_BLOCK_AND_RECORD_COMPLETED)) {
        *(pSh2->opData.getFrs.pWords) = pSh2->opData.getFrs.nextOffset;

        opCompleted(pSh2, SH2_OK);
    }

    return;
}

const sh2_Op_t getFrsOp = {
    .start = getFrsStart,
    .rx = getFrsRx,
};

// ------------------------------------------------------------------------
// Support for sh2_getMetadata

static const struct {
    sh2_SensorId_t sensorId;
    uint16_t recordId;
} sensorToRecordMap[] = {
    { SH2_RAW_ACCELEROMETER,            FRS_ID_META_RAW_ACCELEROMETER },
    { SH2_ACCELEROMETER,                FRS_ID_META_ACCELEROMETER },
    { SH2_LINEAR_ACCELERATION,          FRS_ID_META_LINEAR_ACCELERATION },
    { SH2_GRAVITY,                      FRS_ID_META_GRAVITY },
    { SH2_RAW_GYROSCOPE,                FRS_ID_META_RAW_GYROSCOPE },
    { SH2_GYROSCOPE_CALIBRATED,         FRS_ID_META_GYROSCOPE_CALIBRATED },
    { SH2_GYROSCOPE_UNCALIBRATED,       FRS_ID_META_GYROSCOPE_UNCALIBRATED },
    { SH2_RAW_MAGNETOMETER,             FRS_ID_META_RAW_MAGNETOMETER },
    { SH2_MAGNETIC_FIELD_CALIBRATED,    FRS_ID_META_MAGNETIC_FIELD_CALIBRATED },
    { SH2_MAGNETIC_FIELD_UNCALIBRATED,  FRS_ID_META_MAGNETIC_FIELD_UNCALIBRATED },
    { SH2_ROTATION_VECTOR,              FRS_ID_META_ROTATION_VECTOR },
    { SH2_GAME_ROTATION_VECTOR,         FRS_ID_META_GAME_ROTATION_VECTOR },
    { SH2_GEOMAGNETIC_ROTATION_VECTOR,  FRS_ID_META_GEOMAGNETIC_ROTATION_VECTOR },
    { SH2_PRESSURE,                     FRS_ID_META_PRESSURE },
    { SH2_AMBIENT_LIGHT,                FRS_ID_META_AMBIENT_LIGHT },
    { SH2_HUMIDITY,                     FRS_ID_META_HUMIDITY },
    { SH2_PROXIMITY,                    FRS_ID_META_PROXIMITY },
    { SH2_TEMPERATURE,                  FRS_ID_META_TEMPERATURE },
    { SH2_TAP_DETECTOR,                 FRS_ID_META_TAP_DETECTOR },
    { SH2_STEP_DETECTOR,                FRS_ID_META_STEP_DETECTOR },
    { SH2_STEP_COUNTER,                 FRS_ID_META_STEP_COUNTER },
    { SH2_SIGNIFICANT_MOTION,           FRS_ID_META_SIGNIFICANT_MOTION },
    { SH2_STABILITY_CLASSIFIER,         FRS_ID_META_STABILITY_CLASSIFIER },
    { SH2_SHAKE_DETECTOR,               FRS_ID_META_SHAKE_DETECTOR },
    { SH2_FLIP_DETECTOR,                FRS_ID_META_FLIP_DETECTOR },
    { SH2_PICKUP_DETECTOR,              FRS_ID_META_PICKUP_DETECTOR },
    { SH2_STABILITY_DETECTOR,           FRS_ID_META_STABILITY_DETECTOR },
    { SH2_PERSONAL_ACTIVITY_CLASSIFIER, FRS_ID_META_PERSONAL_ACTIVITY_CLASSIFIER },
    { SH2_SLEEP_DETECTOR,               FRS_ID_META_SLEEP_DETECTOR },
    { SH2_TILT_DETECTOR,                FRS_ID_META_TILT_DETECTOR },
    { SH2_POCKET_DETECTOR,              FRS_ID_META_POCKET_DETECTOR },
    { SH2_CIRCLE_DETECTOR,              FRS_ID_META_CIRCLE_DETECTOR },
};

static void stuffMetadata(sh2_SensorMetadata_t *pData, uint32_t *frsData)
{
    // Populate the sensorMetadata structure with results
    pData->meVersion        = (frsData[0] >> 0) & 0xFF;
    pData->mhVersion        = (frsData[0] >> 8) & 0xFF;
    pData->shVersion        = (frsData[0] >> 16) & 0xFF;
    pData->range            = frsData[1];
    pData->resolution       = frsData[2];
    pData->power_mA         = (frsData[3] >> 0) & 0xFFFF;    // 16.10 format
    pData->revision         = (frsData[3] >> 16) & 0xFFFF;
    pData->minPeriod_uS     = frsData[4];
    pData->maxPeriod_uS     = 0;  // ...unless reading format 4 metadata below
    pData->fifoMax          = (frsData[5] >> 0) & 0xFFFF;
    pData->fifoReserved     = (frsData[5] >> 16) & 0xFFFF;
    pData->batchBufferBytes = (frsData[6] >> 0) & 0xFFFF;;
    pData->vendorIdLen      = (frsData[6] >> 16) & 0xFFFF;

    // Init fields that may not be present, depending on metadata revision
    pData->qPoint1           = 0;
    pData->qPoint2           = 0;
    pData->qPoint3           = 0;
    pData->sensorSpecificLen = 0;
    strcpy(pData->vendorId, ""); // init with empty string in case vendorIdLen == 0

    int vendorIdOffset = 8;
    // Get revision-specific fields
    if (pData->revision == 0) {
        // No fixed fields, vendor id copied after if-else block
    }
    else if (pData->revision == 1) {
        pData->qPoint1        = (frsData[7] >> 0) & 0xFFFF;
        pData->qPoint2        = (frsData[7] >> 16) & 0xFFFF;
    }
    else if (pData->revision == 2) {
        pData->qPoint1        = (frsData[7] >> 0) & 0xFFFF;
        pData->qPoint2        = (frsData[7] >> 16) & 0xFFFF;
        pData->sensorSpecificLen = (frsData[8] >> 0) & 0xFFFF;
        memcpy(pData->sensorSpecific, (uint8_t *)&frsData[9], pData->sensorSpecificLen);
        vendorIdOffset = 9 + ((pData->sensorSpecificLen+3)/4); // 9 + one word for every 4 bytes of SS data
    }
    else if (pData->revision == 3) {
        pData->qPoint1        = (frsData[7] >> 0) & 0xFFFF;
        pData->qPoint2        = (frsData[7] >> 16) & 0xFFFF;
        pData->sensorSpecificLen = (frsData[8] >> 0) & 0xFFFF;
        pData->qPoint3        = (frsData[8] >> 16) & 0xFFFF;
        memcpy(pData->sensorSpecific, (uint8_t *)&frsData[9], pData->sensorSpecificLen);
        vendorIdOffset = 9 + ((pData->sensorSpecificLen+3)/4); // 9 + one word for every 4 bytes of SS data
    }
    else if (pData->revision == 4) {
        pData->qPoint1        = (frsData[7] >> 0) & 0xFFFF;
        pData->qPoint2        = (frsData[7] >> 16) & 0xFFFF;
        pData->sensorSpecificLen = (frsData[8] >> 0) & 0xFFFF;
        pData->qPoint3        = (frsData[8] >> 16) & 0xFFFF;
        pData->maxPeriod_uS   = frsData[9];
        memcpy(pData->sensorSpecific, (uint8_t *)&frsData[10], pData->sensorSpecificLen);
        vendorIdOffset = 10 + ((pData->sensorSpecificLen+3)/4); // 9 + one word for every 4 bytes of SS data
    }
    else {
        // Unrecognized revision!
    }

    // Copy vendor id
    memcpy(pData->vendorId, (uint8_t *)&frsData[vendorIdOffset],
           pData->vendorIdLen);
}

// ------------------------------------------------------------------------
// Set FRS.

static int setFrsStart(sh2_t *pSh2)
{
    int rc = SH2_OK;
    FrsWriteReq_t req;

    pSh2->opData.setFrs.offset = 0;
    
    // set up request to issue
    memset(&req, 0, sizeof(req));
    req.reportId = SENSORHUB_FRS_WRITE_REQ;
    req.reserved = 0;
    req.length = pSh2->opData.setFrs.words;
    req.frsType = pSh2->opData.getFrs.frsType;

    rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));

    return rc;
}

static void setFrsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    
    FrsWriteResp_t *resp = (FrsWriteResp_t *)payload;
    FrsWriteDataReq_t req;
    uint8_t status;
    bool sendMoreData = false;
    bool completed = false;
    int rc = SH2_OK;

    // skip this if it isn't the response we're looking for.
    if (resp->reportId != SENSORHUB_FRS_WRITE_RESP) return;

    // Check for errors: Unrecognized FRS type, Busy, Out of range, Device error
    status = resp->status;
    switch(status) {
        case FRS_WRITE_STATUS_RECEIVED:
        case FRS_WRITE_STATUS_READY:
            sendMoreData = true;
            break;
        case FRS_WRITE_STATUS_UNRECOGNIZED_FRS_TYPE:
        case FRS_WRITE_STATUS_BUSY:
        case FRS_WRITE_STATUS_FAILED:
        case FRS_WRITE_STATUS_NOT_READY:
        case FRS_WRITE_STATUS_INVALID_LENGTH:
        case FRS_WRITE_STATUS_INVALID_RECORD:
        case FRS_WRITE_STATUS_DEVICE_ERROR:
        case FRS_WRITE_STATUS_READ_ONLY:
            rc = SH2_ERR_HUB;
            completed = true;
            break;
        case FRS_WRITE_STATUS_WRITE_COMPLETED:
            // Successful completion
            rc = SH2_OK;
            completed = true;
            break;
        case FRS_WRITE_STATUS_RECORD_VALID:
            // That's nice, keep waiting
            break;
    }

    // if we should send more data, do it.
    if (sendMoreData &&
        (pSh2->opData.setFrs.offset < pSh2->opData.setFrs.words)) {
        uint16_t offset = pSh2->opData.setFrs.offset;
        
        memset(&req, 0, sizeof(req));
        req.reportId = SENSORHUB_FRS_WRITE_DATA_REQ;
        req.reserved = 0;
        req.offset = offset;
        req.data0 = pSh2->opData.setFrs.pData[offset++];
        if (offset < pSh2->opData.setFrs.words) {
            req.data1 = pSh2->opData.setFrs.pData[offset++];
        } else {
            req.data1 = 0;
        }
        pSh2->opData.setFrs.offset = offset;
        
        rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
    }

    // if the operation is done or has to be aborted, complete it
    if (completed) {
        opCompleted(pSh2, rc);
    }

    return;
}

const sh2_Op_t setFrsOp = {
    .start = setFrsStart,
    .rx = setFrsRx,
};

// ------------------------------------------------------------------------
// Support for sending commands

static int sendCmd(sh2_t *pSh2, uint8_t cmd, uint8_t p[COMMAND_PARAMS])
{
    int rc = SH2_OK;
    CommandReq_t req;

    // Clear request structure
    memset(&req, 0, sizeof(req));
    
    // Create a command sequence number for this command
    pSh2->lastCmdId = cmd;
    pSh2->cmdSeq = pSh2->nextCmdSeq++;
    
    // set up request to issue
    req.reportId = SENSORHUB_COMMAND_REQ;
    req.seq = pSh2->cmdSeq;
    req.command = cmd;
    for (int n = 0; n < COMMAND_PARAMS; n++) {
        req.p[n] = p[n];
    }
    rc = sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
    
    return rc;
}

// Send a command with 0 parameters
static int sendCmd0(sh2_t *pSh2, uint8_t cmd)
{
    uint8_t p[COMMAND_PARAMS];

    memset(p, 0, COMMAND_PARAMS);

    return sendCmd(pSh2, cmd, p);
}

// Send a command with 1 parameter
static int sendCmd1(sh2_t *pSh2, uint8_t cmd, uint8_t p0)
{
    uint8_t p[COMMAND_PARAMS];

    memset(p, 0, COMMAND_PARAMS);

    p[0] = p0;
    return sendCmd(pSh2, cmd, p);
}

// Send a command with 2 parameters
static int sendCmd2(sh2_t *pSh2, uint8_t cmd, uint8_t p0, uint8_t p1)
{
    uint8_t p[COMMAND_PARAMS];

    memset(p, 0, COMMAND_PARAMS);
    
    p[0] = p0;
    p[1] = p1;
    return sendCmd(pSh2, cmd, p);
}

static bool wrongResponse(sh2_t *pSh2, CommandResp_t *resp)
{
    if (resp->reportId != SENSORHUB_COMMAND_RESP) return true;
    if (resp->command != pSh2->lastCmdId) return true;
    if (resp->commandSeq != pSh2->cmdSeq) return true;

    return false;
}

// ------------------------------------------------------------------------
// Get Errors

static int getErrorsStart(sh2_t *pSh2)
{
    // Initialize state
    pSh2->opData.getErrors.errsRead = 0;

    return sendCmd1(pSh2, SH2_CMD_ERRORS, pSh2->opData.getErrors.severity);
}

static void getErrorsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    
    CommandResp_t *resp = (CommandResp_t *)payload;
    
    // skip this if it isn't the right response
    if (wrongResponse(pSh2, resp)) return;

    if (resp->r[2] == 255) {
        // No error to report, operation is complete
        *(pSh2->opData.getErrors.pNumErrors) = pSh2->opData.getErrors.errsRead;
        opCompleted(pSh2, SH2_OK);
    } else {
        // Copy data for invoker.
        unsigned int index = pSh2->opData.getErrors.errsRead;
        if (index < *(pSh2->opData.getErrors.pNumErrors)) {
            // We have room for this one.
            pSh2->opData.getErrors.pErrors[index].severity = resp->r[0];
            pSh2->opData.getErrors.pErrors[index].sequence = resp->r[1];
            pSh2->opData.getErrors.pErrors[index].source = resp->r[2];
            pSh2->opData.getErrors.pErrors[index].error = resp->r[3];
            pSh2->opData.getErrors.pErrors[index].module = resp->r[4];
            pSh2->opData.getErrors.pErrors[index].code = resp->r[5];

            pSh2->opData.getErrors.errsRead++;
        }
    }

    return;
}

const sh2_Op_t getErrorsOp = {
    .start = getErrorsStart,
    .rx = getErrorsRx,
};

// ------------------------------------------------------------------------
// Get Counts

static int getCountsStart(sh2_t *pSh2)
{
    return sendCmd2(pSh2, SH2_CMD_COUNTS, SH2_COUNTS_GET_COUNTS, pSh2->opData.getCounts.sensorId);
}

static void getCountsRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    
    CommandResp_t *resp = (CommandResp_t *)payload;

    if (wrongResponse(pSh2, resp)) return;
    
    // Store results
    if (resp->respSeq == 0) {
        pSh2->opData.getCounts.pCounts->offered = readu32(&resp->r[3]);
        pSh2->opData.getCounts.pCounts->accepted = readu32(&resp->r[7]);
    }
    else {
        pSh2->opData.getCounts.pCounts->on = readu32(&resp->r[3]);
        pSh2->opData.getCounts.pCounts->attempted = readu32(&resp->r[7]);
    }
    
    // Complete this operation if we've received last response
    if (resp->respSeq == 1) {
        opCompleted(pSh2, SH2_OK);
    }

    return;
}

const sh2_Op_t getCountsOp = {
    .start = getCountsStart,
    .rx = getCountsRx,
};

// ------------------------------------------------------------------------
// Generic Send Command

static int sendCmdStart(sh2_t *pSh2)
{
    int status = sendCmd(pSh2, pSh2->opData.sendCmd.req.command,
                     pSh2->opData.sendCmd.req.p);

    opCompleted(pSh2, status);

    return status;
}

const sh2_Op_t sendCmdOp = {
    .start = sendCmdStart,
};

// ------------------------------------------------------------------------
// Reinit

static int reinitStart(sh2_t *pSh2)
{
    int status = sendCmd1(pSh2, SH2_CMD_INITIALIZE, SH2_INIT_SYSTEM);

    return status;
}

static void reinitRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    CommandResp_t *resp = (CommandResp_t *)payload;

    // Ignore message if it doesn't pertain to this operation
    if (wrongResponse(pSh2, resp)) return;

    // Get return status
    if (resp->r[0] != 0) {
        pSh2->opStatus = SH2_ERR_HUB;
        opCompleted(pSh2, pSh2->opStatus);
    }
    else {
        pSh2->opStatus = SH2_OK;
        opCompleted(pSh2, pSh2->opStatus);
    }
}

const sh2_Op_t reinitOp = {
    .start = reinitStart,
    .rx = reinitRx,
};

// ------------------------------------------------------------------------
// Save DCD Now

static int saveDcdNowStart(sh2_t *pSh2)
{
    int status = sendCmd0(pSh2, SH2_CMD_DCD);

    return status;
}

static void saveDcdNowRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    CommandResp_t *resp = (CommandResp_t *)payload;

    // Ignore message if it doesn't pertain to this operation
    if (wrongResponse(pSh2, resp)) return;

    // Get return status
    int status = SH2_OK;
    if (resp->r[0] != 0) {
        status = SH2_ERR_HUB;
    }

    opCompleted(pSh2, status);
}

const sh2_Op_t saveDcdNowOp = {
    .start = saveDcdNowStart,
    .rx = saveDcdNowRx,
};

// ------------------------------------------------------------------------
// Get Osc Type

static int getOscTypeStart(sh2_t *pSh2)
{
    return sendCmd0(pSh2, SH2_CMD_GET_OSC_TYPE);
}

static void getOscTypeRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    CommandResp_t *resp = (CommandResp_t *)payload;
    sh2_OscType_t *pOscType;
    
    // Ignore message if it doesn't pertain to this operation
    if (wrongResponse(pSh2, resp)) return;

    // Read out data
    pOscType = pSh2->opData.getOscType.pOscType;
    *pOscType = (sh2_OscType_t)resp->r[0];

    // Complete this operation
    opCompleted(pSh2, SH2_OK);
}

const sh2_Op_t getOscTypeOp = {
    .start = getOscTypeStart,
    .rx = getOscTypeRx,
};

// ------------------------------------------------------------------------
// Set Cal Config

static int setCalConfigStart(sh2_t *pSh2)
{
    uint8_t p[COMMAND_PARAMS];

    // Clear p.  (Importantly, set subcommand in p[3] to 0, CONFIGURE)
    memset(p, 0, COMMAND_PARAMS);
    
    // Which cal processes to enable/disable
    p[0] = (pSh2->opData.calConfig.sensors & SH2_CAL_ACCEL) ? 1 : 0; // accel cal
    p[1] = (pSh2->opData.calConfig.sensors & SH2_CAL_GYRO)  ? 1 : 0; // gyro cal
    p[2] = (pSh2->opData.calConfig.sensors & SH2_CAL_MAG)   ? 1 : 0; // mag cal
    p[4] = (pSh2->opData.calConfig.sensors & SH2_CAL_PLANAR) ? 1 : 0; // planar cal
    p[5] = (pSh2->opData.calConfig.sensors & SH2_CAL_ON_TABLE) ? 1 : 0; // on-table cal

    p[6] = (pSh2->opData.calConfig.sensors & SH2_CAL_ZERO_GYRO_CONTROL_MASK) >> 5;
    
    return sendCmd(pSh2, SH2_CMD_ME_CAL, p);
}

static void setCalConfigRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    
    CommandResp_t *resp = (CommandResp_t *)payload;
    
    // Ignore message if it doesn't pertain to this operation
    if (wrongResponse(pSh2, resp)) return;

    // Read out data
    int status = SH2_OK;
    if (resp->r[0] != 0) {
        status = SH2_ERR_HUB;
    }

    // Complete this operation
    opCompleted(pSh2, status);
}

const sh2_Op_t setCalConfigOp = {
    .start = setCalConfigStart,
    .rx = setCalConfigRx,
};

// ------------------------------------------------------------------------
// Get Cal Config

static int getCalConfigStart(sh2_t *pSh2)
{
    uint8_t p[COMMAND_PARAMS];

    // Clear p.  (Importantly, set subcommand in p[3] to 0, CONFIGURE)
    memset(p, 0, COMMAND_PARAMS);
    
    // Subcommand: Get ME Calibration
    p[3] = 0x01;
    
    return sendCmd(pSh2, SH2_CMD_ME_CAL, p);
}

static void getCalConfigRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    CommandResp_t *resp = (CommandResp_t *)payload;
    
    // Ignore message if it doesn't pertain to this operation
    if (wrongResponse(pSh2, resp)) return;

    // Read out data
    int status = SH2_OK;
    if (resp->r[0] != 0) {
        status = SH2_ERR_HUB;
    }
    else {
        // Unload results into pSensors
        uint8_t sensors = 0;
        if (resp->r[1]) sensors |= SH2_CAL_ACCEL;
        if (resp->r[2]) sensors |= SH2_CAL_GYRO;
        if (resp->r[3]) sensors |= SH2_CAL_MAG;
        if (resp->r[4]) sensors |= SH2_CAL_PLANAR;
        *(pSh2->opData.getCalConfig.pSensors) = sensors;
    }
    
    // Complete this operation
    opCompleted(pSh2, status);
}


const sh2_Op_t getCalConfigOp = {
    .start = getCalConfigStart,
    .rx = getCalConfigRx,
};

// ------------------------------------------------------------------------
// Force Flush

static int forceFlushStart(sh2_t *pSh2)
{
    ForceFlushReq_t req;

    memset(&req, 0, sizeof(req));
    req.reportId = SENSORHUB_FORCE_SENSOR_FLUSH;
    req.sensorId = pSh2->opData.forceFlush.sensorId;
    
    return sendCtrl(pSh2, (uint8_t *)&req, sizeof(req));
}

static void forceFlushRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    
    ForceFlushResp_t *resp = (ForceFlushResp_t *)payload;
    
    // Ignore message if it doesn't pertain to this operation
    if (resp->reportId != SENSORHUB_FLUSH_COMPLETED) return;
    if (resp->sensorId != pSh2->opData.forceFlush.sensorId) return;

    // Complete this operation
    opCompleted(pSh2, SH2_OK);
}

const sh2_Op_t forceFlushOp = {
    .start = forceFlushStart,
    .rx = forceFlushRx,
};

// ------------------------------------------------------------------------
// Clear DCD And Reset

static int clearDcdAndResetStart(sh2_t *pSh2)
{
    pSh2->resetComplete = false;
    int status = sendCmd0(pSh2, SH2_CMD_CLEAR_DCD_AND_RESET);

    return status;
}

static void clearDcdAndResetOnReset(sh2_t *pSh2)
{
    // When reset is detected, this op is complete.
    opCompleted(pSh2, SH2_OK);
}

const sh2_Op_t clearDcdAndResetOp = {
    .start = clearDcdAndResetStart,
    .onReset = clearDcdAndResetOnReset,
};

// ------------------------------------------------------------------------
// Start Cal

static int startCalStart(sh2_t *pSh2)
{
    uint8_t p[COMMAND_PARAMS];

    // Clear p.  (Importantly, set subcommand in p[3] to 0, CONFIGURE)
    memset(p, 0, COMMAND_PARAMS);
    
    // Subcommand: Get ME Calibration
    p[0] = SH2_CAL_START;
    p[1] = pSh2->opData.startCal.interval_us & 0xFF;          // LSB
    p[2] = (pSh2->opData.startCal.interval_us >> 8) & 0xFF;
    p[3] = (pSh2->opData.startCal.interval_us >> 16) & 0xFF;
    p[4] = (pSh2->opData.startCal.interval_us >> 24) & 0xFF;  // MSB
    
    return sendCmd(pSh2, SH2_CMD_CAL, p);
}

static void startCalRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    
    CommandResp_t *resp = (CommandResp_t *)payload;
    
    // Ignore message if it doesn't pertain to this operation
    if (wrongResponse(pSh2, resp)) return;

    // Complete this operation
    opCompleted(pSh2, SH2_OK);
}

const sh2_Op_t startCalOp = {
    .start = startCalStart,
    .rx = startCalRx,
};

// ------------------------------------------------------------------------
// Start Cal

static int finishCalStart(sh2_t *pSh2)
{
    return sendCmd1(pSh2, SH2_CMD_CAL, SH2_CAL_FINISH);
}

static void finishCalRx(sh2_t *pSh2, const uint8_t *payload, uint16_t len)
{
    (void)len; // unused
    
    CommandResp_t *resp = (CommandResp_t *)payload;
    
    // Ignore message if it doesn't pertain to this operation
    if (wrongResponse(pSh2, resp)) return;

    pSh2->opData.finishCal.status = (sh2_CalStatus_t)resp->r[1];

    // Complete this operation
    if (pSh2->opData.finishCal.status == SH2_CAL_SUCCESS) {
        opCompleted(pSh2, SH2_OK);
    }
    else {
        opCompleted(pSh2, SH2_ERR_HUB);
    }
}

const sh2_Op_t finishCalOp = {
    .start = finishCalStart,
    .rx = finishCalRx,
};

// -----------------------------------------------------------------------
static int sendWheelOpStart(sh2_t *pSh2)
{
    uint8_t p[COMMAND_PARAMS];
    memset(p, 0, COMMAND_PARAMS);
    p[0] = pSh2->opData.wheelRequest.wheelIndex;
    p[1] = (pSh2->opData.wheelRequest.timestamp >> 0) & 0xFF; 
    p[2] = (pSh2->opData.wheelRequest.timestamp >> 8) & 0xFF;
    p[3] = (pSh2->opData.wheelRequest.timestamp >> 16) & 0xFF;
    p[4] = (pSh2->opData.wheelRequest.timestamp >> 24) & 0xFF;
    p[5] = (pSh2->opData.wheelRequest.wheelData >> 0) & 0xFF;
    p[6] = (pSh2->opData.wheelRequest.wheelData >> 8) & 0xFF;
    p[7] = pSh2->opData.wheelRequest.dataType;
    int status = sendCmd(pSh2, SH2_CMD_WHEEL_REQ, p);
    opCompleted(pSh2, status);
    return status;
}


const sh2_Op_t sendWheelOp = {
    .start = sendWheelOpStart,
    .timeout_us = 5000000,
};


// ------------------------------------------------------------------------
// SHTP Event Callback

static void shtpEventCallback(void *cookie, shtp_Event_t shtpEvent) {
    (void)cookie; // unused
    
    sh2_t *pSh2 = &_sh2;

    sh2AsyncEvent.eventId = SH2_SHTP_EVENT;
    sh2AsyncEvent.shtpEvent = shtpEvent;
    if (pSh2->eventCallback) {
        pSh2->eventCallback(pSh2->eventCookie, &sh2AsyncEvent);
    }
}

// ------------------------------------------------------------------------
// Public functions

/**
 * @brief Open a session with a sensor hub.
 *
 * This function should be called before others in this API.
 * An instance of an SH2 HAL should be passed in.
 * This call will result in the open() function of the HAL being called.
 *
 * As part of the initialization process, a callback function is registered that will
 * be invoked when the device generates certain events.  (See sh2_AsyncEventId)
 *
 * @param pHal Pointer to an SH2 HAL instance, provided by the target system.
 * @param  eventCallback Will be called when events, such as reset complete, occur.
 * @param  eventCookie Will be passed to eventCallback.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_open(sh2_Hal_t *pHal,
             sh2_EventCallback_t *eventCallback, void *eventCookie)
{
    sh2_t *pSh2 = &_sh2;
    
    // Validate parameters
    if (pHal == 0) return SH2_ERR_BAD_PARAM;

    // Clear everything in sh2 structure.
    memset(pSh2, 0, sizeof(sh2_t));

    // will go true after reset response from SH.
    pSh2->resetComplete = false;
    
    // Store reference to HAL for future use.
    pSh2->pHal = pHal;
    pSh2->eventCallback = eventCallback;
    pSh2->eventCookie = eventCookie;
    pSh2->sensorCallback = 0;
    pSh2->sensorCookie = 0;

    // Open SHTP layer
    pSh2->pShtp = shtp_open(pSh2->pHal);
    if (pSh2->pShtp == 0) {
        // Error opening SHTP
        return SH2_ERR;
    }

    // Register SHTP event callback
    shtp_setEventCallback(pSh2->pShtp, shtpEventCallback, pSh2);

    // Register with SHTP
    // Register SH2 handlers
    shtp_listenChan(pSh2->pShtp, CHAN_SENSORHUB_CONTROL, sensorhubControlHdlr, pSh2);
    shtp_listenChan(pSh2->pShtp, CHAN_SENSORHUB_INPUT, sensorhubInputNormalHdlr, pSh2);
    shtp_listenChan(pSh2->pShtp, CHAN_SENSORHUB_INPUT_WAKE, sensorhubInputWakeHdlr, pSh2);
    shtp_listenChan(pSh2->pShtp, CHAN_SENSORHUB_INPUT_GIRV, sensorhubInputGyroRvHdlr, pSh2);

    // Register EXECUTABLE handlers
    shtp_listenChan(pSh2->pShtp, CHAN_EXECUTABLE_DEVICE, executableDeviceHdlr, pSh2);

    // Wait for reset notifications to arrive.
    // The client can't talk to the sensor hub until that happens.
    uint32_t start_us = pSh2->pHal->getTimeUs(pSh2->pHal);
    uint32_t now_us = start_us;
    while (((now_us - start_us) < ADVERT_TIMEOUT_US) &&
           (!pSh2->resetComplete))
    {
        shtp_service(pSh2->pShtp);
        now_us = pSh2->pHal->getTimeUs(pSh2->pHal);
    }
    
    // No errors.
    return SH2_OK;
}

/**
 * @brief Close a session with a sensor hub.
 *
 * This should be called at the end of a sensor hub session.  
 * The underlying SHTP and HAL instances will be closed.
 */
void sh2_close(void)
{
    sh2_t *pSh2 = &_sh2;
    
    if (pSh2->pShtp != 0) {
        shtp_close(pSh2->pShtp);
    }

    // Clear everything in sh2 structure.
    memset(pSh2, 0, sizeof(sh2_t));
}

/**
 * @brief Service the SH2 device, reading any data that is available and dispatching callbacks.
 *
 * This function should be called periodically by the host system to service an open sensor hub.
 */
void sh2_service(void)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp != 0) {
        shtp_service(pSh2->pShtp);
    }
}

/**
 * @brief Register a function to receive sensor events.
 *
 * @param  callback A function that will be called each time a sensor event is received.
 * @param  cookie  A value that will be passed to the sensor callback function.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_setSensorCallback(sh2_SensorCallback_t *callback, void *cookie)
{
    sh2_t *pSh2 = &_sh2;
    
    pSh2->sensorCallback = callback;
    pSh2->sensorCookie = cookie;

    return SH2_OK;
}

/**
 * @brief Reset the sensor hub device by sending RESET (1) command on "device" channel.
 *
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_devReset(void)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    return sendExecutable(pSh2, EXECUTABLE_DEVICE_CMD_RESET);
}

/**
 * @brief Turn sensor hub on by sending ON (2) command on "device" channel.
 *
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_devOn(void)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    return sendExecutable(pSh2, EXECUTABLE_DEVICE_CMD_ON);
}

/**
 * @brief Put sensor hub in sleep state by sending SLEEP (3) command on "device" channel.
 *
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_devSleep(void)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    return sendExecutable(pSh2, EXECUTABLE_DEVICE_CMD_SLEEP);
}

/**
 * @brief Get Product ID information from Sensorhub.
 *
 * @param  prodIds Pointer to structure that will receive results.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_getProdIds(sh2_ProductIds_t *prodIds)
{
    sh2_t *pSh2 = &_sh2;
    
    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    pSh2->opData.getProdIds.pProdIds = prodIds;

    return opProcess(pSh2, &getProdIdOp);
}

/**
 * @brief Get sensor configuration.
 *
 * @param  sensorId Which sensor to query.
 * @param  config SensorConfig structure to store results.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_getSensorConfig(sh2_SensorId_t sensorId, sh2_SensorConfig_t *pConfig)
{
    sh2_t *pSh2 = &_sh2;
    
    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    // Set up operation
    pSh2->opData.getSensorConfig.sensorId = sensorId;
    pSh2->opData.getSensorConfig.pConfig = pConfig;

    return opProcess(pSh2, &getSensorConfigOp);
}

/**
 * @brief Set sensor configuration. (e.g enable a sensor at a particular rate.)
 *
 * @param  sensorId Which sensor to configure.
 * @param  pConfig Pointer to structure holding sensor configuration.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_setSensorConfig(sh2_SensorId_t sensorId, const sh2_SensorConfig_t *pConfig)
{
    sh2_t *pSh2 = &_sh2;
    
    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }
 
    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    // Set up operation
    pSh2->opData.setSensorConfig.sensorId = sensorId;
    pSh2->opData.setSensorConfig.pConfig = pConfig;

    return opProcess(pSh2, &setSensorConfigOp);
}

/**
 * @brief Get metadata related to a sensor.
 *
 * @param  sensorId Which sensor to query.
 * @param  pData Pointer to structure to receive the results.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_getMetadata(sh2_SensorId_t sensorId, sh2_SensorMetadata_t *pData)
{
    sh2_t *pSh2 = &_sh2;
    
    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // pData must be non-null
    if (pData == 0) return SH2_ERR_BAD_PARAM;
  
    // Convert sensorId to metadata recordId
    unsigned i;
    for (i = 0; i < ARRAY_LEN(sensorToRecordMap); i++) {
        if (sensorToRecordMap[i].sensorId == sensorId) {
            break;
        }
    }
    if (i >= ARRAY_LEN(sensorToRecordMap)) {
        // no match was found
        return SH2_ERR_BAD_PARAM;
    }
    uint16_t recordId = sensorToRecordMap[i].recordId;
    
    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    // Set up an FRS read operation
    pSh2->opData.getFrs.frsType = recordId;
    pSh2->opData.getFrs.pData = pSh2->frsData;
    pSh2->frsDataLen = ARRAY_LEN(pSh2->frsData);
    pSh2->opData.getFrs.pWords = &pSh2->frsDataLen;

    // Read an FRS record
    int status = opProcess(pSh2, &getFrsOp);
    
    // Copy the results into pData
    if (status == SH2_OK) {
        stuffMetadata(pData, pSh2->frsData);
    }

    return status;
}

/**
 * @brief Get an FRS record.
 *
 * @param  recordId Which FRS Record to retrieve.
 * @param  pData pointer to buffer to receive the results
 * @param[in] words Size of pData buffer, in 32-bit words.
 * @param[out] words Number of 32-bit words retrieved.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_getFrs(uint16_t recordId, uint32_t *pData, uint16_t *words)
{
    sh2_t *pSh2 = &_sh2;
    
    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    if ((pData == 0) || (words == 0)) {
        return SH2_ERR_BAD_PARAM;
    }
    
    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    // Store params for this op
    pSh2->opData.getFrs.frsType = recordId;
    pSh2->opData.getFrs.pData = pData;
    pSh2->opData.getFrs.pWords = words;

    return opProcess(pSh2, &getFrsOp);
}

/**
 * @brief Set an FRS record
 *
 * @param  recordId Which FRS Record to set.
 * @param  pData pointer to buffer containing the new data.
 * @param  words number of 32-bit words to write.  (0 to delete record.)
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_setFrs(uint16_t recordId, uint32_t *pData, uint16_t words)
{
    sh2_t *pSh2 = &_sh2;
    
    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    if ((pData == 0) && (words != 0)) {
        return SH2_ERR_BAD_PARAM;
    }
    
    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    pSh2->opData.setFrs.frsType = recordId;
    pSh2->opData.setFrs.pData = pData;
    pSh2->opData.setFrs.words = words;

    return opProcess(pSh2, &setFrsOp);
}

/**
 * @brief Get error counts.
 *
 * @param  severity Only errors of this severity or greater are returned.
 * @param  pErrors Buffer to receive error codes.
 * @param  numErrors size of pErrors array
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_getErrors(uint8_t severity, sh2_ErrorRecord_t *pErrors, uint16_t *numErrors)
{
    sh2_t *pSh2 = &_sh2;
    
    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    pSh2->opData.getErrors.severity = severity;
    pSh2->opData.getErrors.pErrors = pErrors;
    pSh2->opData.getErrors.pNumErrors = numErrors;
    
    return opProcess(pSh2, &getErrorsOp);
}

/**
 * @brief Read counters related to a sensor.
 *
 * @param  sensorId Which sensor to operate on.
 * @param  pCounts Pointer to Counts structure that will receive data.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_getCounts(sh2_SensorId_t sensorId, sh2_Counts_t *pCounts)
{
    sh2_t *pSh2 = &_sh2;
    
    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    pSh2->opData.getCounts.sensorId = sensorId;
    pSh2->opData.getCounts.pCounts = pCounts;
    
    return opProcess(pSh2, &getCountsOp);
}

/**
 * @brief Clear counters related to a sensor.
 *
 * @param  sensorId which sensor to operate on.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_clearCounts(sh2_SensorId_t sensorId)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    
    pSh2->opData.sendCmd.req.command = SH2_CMD_COUNTS;
    pSh2->opData.sendCmd.req.p[0] = SH2_COUNTS_CLEAR_COUNTS;
    pSh2->opData.sendCmd.req.p[1] = sensorId;

    return opProcess(pSh2, &sendCmdOp);
}

/**
 * @brief Perform a tare operation on one or more axes.
 *
 * @param  axes Bit mask specifying which axes should be tared.
 * @param  basis Which rotation vector to use as the basis for Tare adjustment.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_setTareNow(uint8_t axes,    // SH2_TARE_X | SH2_TARE_Y | SH2_TARE_Z
                   sh2_TareBasis_t basis)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    
    pSh2->opData.sendCmd.req.command = SH2_CMD_TARE;
    pSh2->opData.sendCmd.req.p[0] = SH2_TARE_TARE_NOW;
    pSh2->opData.sendCmd.req.p[1] = axes;
    pSh2->opData.sendCmd.req.p[2] = basis;

    return opProcess(pSh2, &sendCmdOp);
}

/**
 * @brief Clears the previously applied tare operation.
 *
 * @return SH2_OK \n");
 */
int sh2_clearTare(void)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    
    pSh2->opData.sendCmd.req.command = SH2_CMD_TARE;
    pSh2->opData.sendCmd.req.p[0] = SH2_TARE_SET_REORIENTATION;

    return opProcess(pSh2, &sendCmdOp);
}

/**
 * @brief Persist the results of last tare operation to flash.
 *
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_persistTare(void)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    
    pSh2->opData.sendCmd.req.command = SH2_CMD_TARE;
    pSh2->opData.sendCmd.req.p[0] = SH2_TARE_PERSIST_TARE;

    return opProcess(pSh2, &sendCmdOp);
}

/**
 * @brief Set the current run-time sensor reorientation. (Set to zero to clear tare.)
 *
 * @param  orientation Quaternion rotation vector to apply as new tare.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_setReorientation(sh2_Quaternion_t *orientation)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    
    pSh2->opData.sendCmd.req.command = SH2_CMD_TARE;
    pSh2->opData.sendCmd.req.p[0] = SH2_TARE_SET_REORIENTATION;

    // save me a lot of typing and you a lot of reading
    uint8_t *p = pSh2->opData.sendCmd.req.p;

    // Put new orientation in command parameters
    writeu16(&p[1], toQ14(orientation->x));
    writeu16(&p[3], toQ14(orientation->y));
    writeu16(&p[5], toQ14(orientation->z));
    writeu16(&p[7], toQ14(orientation->w));

    return opProcess(pSh2, &sendCmdOp);
}

/**
 * @brief Command the sensorhub to reset.
 *
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_reinitialize(void)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    return opProcess(pSh2, &reinitOp);
}

/**
 * @brief Save Dynamic Calibration Data to flash.
 *
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_saveDcdNow(void)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    return opProcess(pSh2, &saveDcdNowOp);
}

/**
 * @brief Get Oscillator type.
 *
 * @param  pOscType pointer to data structure to receive results.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_getOscType(sh2_OscType_t *pOscType)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    pSh2->opData.getOscType.pOscType = pOscType;

    return opProcess(pSh2, &getOscTypeOp);
}

/**
 * @brief Enable/Disable dynamic calibration for certain sensors
 *
 * @param  sensors Bit mask to configure which sensors are affected.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_setCalConfig(uint8_t sensors)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    pSh2->opData.calConfig.sensors = sensors;

    return opProcess(pSh2, &setCalConfigOp);
}

/**
 * @brief Get dynamic calibration configuration settings.
 *
 * @param  pSensors pointer to Bit mask, set on return.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_getCalConfig(uint8_t *pSensors)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    pSh2->opData.getCalConfig.pSensors = pSensors;

    return opProcess(pSh2, &getCalConfigOp);
}

/**
 * @brief Configure automatic saving of dynamic calibration data.
 *
 * @param  enabled Enable or Disable DCD auto-save.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_setDcdAutoSave(bool enabled)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    pSh2->opData.sendCmd.req.command = SH2_CMD_DCD_SAVE;
    pSh2->opData.sendCmd.req.p[0] = enabled ? 0 : 1;

    return opProcess(pSh2, &sendCmdOp);
}

/**
 * @brief Immediately issue all buffered sensor reports from a given sensor.
 *
 * @param  sensorId Which sensor reports to flush.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_flush(sh2_SensorId_t sensorId)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    pSh2->opData.forceFlush.sensorId = sensorId;

    return opProcess(pSh2, &forceFlushOp);
}

/**
 * @brief Command clear DCD in RAM, then reset sensor hub.
 *
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_clearDcdAndReset(void)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    return opProcess(pSh2, &clearDcdAndResetOp);
}

/**
 * @brief Start simple self-calibration procedure.
 *
 * @parameter interval_us sensor report interval, uS.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_startCal(uint32_t interval_us)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    pSh2->opData.startCal.interval_us = interval_us;

    return opProcess(pSh2, &startCalOp);
}

/**
 * @brief Finish simple self-calibration procedure.
 *
 * @parameter status contains calibration status code on return.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_finishCal(sh2_CalStatus_t *status)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    
    int retval = opProcess(pSh2, &finishCalOp);
    if (status != NULL) {
        *status = pSh2->opData.finishCal.status;
    }

    return retval;
}

/**
 * @brief send Interactive ZRO Request.
 *
 * @parameter intent Inform the sensor hub what sort of motion should be in progress.
 * @return SH2_OK (0), on success.  Negative value from sh2_err.h on error.
 */
int sh2_setIZro(sh2_IZroMotionIntent_t intent)
{
    sh2_t *pSh2 = &_sh2;

    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    // clear opData
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));

    // set up opData for iZRO request
    pSh2->opData.sendCmd.req.command = SH2_CMD_INTERACTIVE_ZRO;
    pSh2->opData.sendCmd.req.p[0] = intent;

    // Send command
    return opProcess(pSh2, &sendCmdOp);
}


int sh2_reportWheelEncoder(uint8_t wheelIndex, uint32_t timestamp, int16_t wheelData, uint8_t dataType){
    sh2_t *pSh2 = &_sh2;
    
    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    //No callback (am i doing this right?)
    pSh2->pOp = 0;
    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    pSh2->opData.wheelRequest.wheelIndex = wheelIndex;
    pSh2->opData.wheelRequest.timestamp = timestamp;
    pSh2->opData.wheelRequest.wheelData = wheelData;
    pSh2->opData.wheelRequest.dataType = dataType;
    int rc = opProcess(pSh2, &sendWheelOp);
    return rc;
}

int sh2_saveDeadReckoningCalNow(void){
    sh2_t *pSh2 = &_sh2;
    
    if (pSh2->pShtp == 0) {
        return SH2_ERR;  // sh2 API isn't open
    }

    memset(&pSh2->opData, 0, sizeof(sh2_OpData_t));
    pSh2->opData.sendCmd.req.command = SH2_CMD_DR_CAL_SAVE;

    return opProcess(pSh2, &sendCmdOp);
}