xref: /device/google/contexthub/firmware/os/drivers/invensense_icm40600/invensense_icm40600.c

/*
 * Copyright (C) 2018 InvenSense Inc.
 *
 * 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 <algos/time_sync.h>
#include <stdlib.h>
#include <string.h>
#include <timer.h>
#include <sensors.h>
#include <heap.h>
#include <spi.h>
#include <slab.h>
#include <limits.h>
#include <atomic.h>
#include <plat/rtc.h>
#include <plat/gpio.h>
#include <plat/exti.h>
#include <plat/syscfg.h>
#include <seos.h>
#include <gpio.h>
#include <isr.h>
#include <halIntf.h>
#include <hostIntf.h>
#include <nanohubPacket.h>
#include <cpu/cpuMath.h>
#include <nanohub_math.h>
#include <variant/sensType.h>
#include <variant/variant.h>
#include <common/math/macros.h>

#ifdef ACCEL_CAL_ENABLED
#include <calibration/accelerometer/accel_cal.h>
#endif
#ifdef GYRO_CAL_ENABLED
#include <calibration/gyroscope/gyro_cal.h>
#endif

#define INFO_PRINT(fmt, ...) do { \
        osLog(LOG_INFO, "%s " fmt, "[ICM40600]", ##__VA_ARGS__); \
    } while (0);

#define ERROR_PRINT(fmt, ...) do { \
        osLog(LOG_ERROR, "%s " fmt, "[ICM40600] ERROR:", ##__VA_ARGS__); \
    } while (0);

#define DEBUG_PRINT(fmt, ...) do { \
        if (DBG_ENABLE) {  \
            INFO_PRINT(fmt,  ##__VA_ARGS__); \
        } \
    } while (0);

#define DEBUG_PRINT_IF(cond, fmt, ...) do { \
        if ((cond) && DBG_ENABLE) {  \
            INFO_PRINT(fmt,  ##__VA_ARGS__); \
        } \
    } while (0);

#define DBG_ENABLE                  0
#define DBG_STATE                   0
#define DBG_TIMESTAMP               0

#define ICM40600_APP_ID             APP_ID_MAKE(NANOHUB_VENDOR_INVENSENSE, 0x2)
#define ICM40600_APP_VERSION        1

#define ICM40600_SPI_WRITE          0x00
#define ICM40600_SPI_READ           0x80

// SPI speeds officialy supported: 5MHz (low speed), 17MHz (high speed)
#define ICM40600_SPI_LOW_SPEED_HZ       5000000
#define ICM40600_SPI_HIGH_SPEED_HZ      17000000
#define ICM40600_SPI_DEFAULT_SPEED_HZ   ICM40600_SPI_LOW_SPEED_HZ
#define ICM40600_SPI_MODE               3

#ifndef ICM40600_SPI_BUS_ID
#define ICM40600_SPI_BUS_ID         1
#endif
#ifndef ICM40600_INT1_PIN
#define ICM40600_INT1_PIN           GPIO_PB(6)
#endif
#ifndef ICM40600_INT1_IRQ
#define ICM40600_INT1_IRQ           EXTI9_5_IRQn
#endif
#define ICM40600_SPI_SPEED_HZ       ICM40600_SPI_HIGH_SPEED_HZ

// set SPI speed register value
#if ICM40600_SPI_SPEED_HZ == ICM40600_SPI_LOW_SPEED_HZ
#define ICM40600_SPI_SPEED_REG_VALUE    BIT_SPI_SPEED_5MHZ
#elif ICM40600_SPI_SPEED_HZ == ICM40600_SPI_HIGH_SPEED_HZ
#define ICM40600_SPI_SPEED_REG_VALUE    BIT_SPI_SPEED_17MHZ
#else
#error "Invalid ICM40600_SPI_SPEED_HZ setting: valid values are 5MHz or 17MHz"
#endif

/*
 **********************
 * Chip configuration
 **********************
 */
/* Full-scale range */
// Default accel range is 8g
#ifndef ICM40600_ACC_RANGE_G
#define ICM40600_ACC_RANGE_G        8
#endif
// Default gyro range is 2000dps
#ifndef ICM40600_GYR_RANGE_DPS
#define ICM40600_GYR_RANGE_DPS      2000
#endif
// 0 -> +-16g, 1 -> +-8g, 2 -> +-4g, 3 -> +-2g, 4 -> +-1g
#if ICM40600_ACC_RANGE_G == 16
#define ICM40600_ACC_FS     0
#elif ICM40600_ACC_RANGE_G == 8
#define ICM40600_ACC_FS     1
#elif ICM40600_ACC_RANGE_G == 4
#define ICM40600_ACC_FS     2
#else
#error "Invalid ICM40600_ACC_RANGE_G setting: valid values are 16, 8, 4 (no HiFi)"
#endif
// 0 -> +-2000dps, 1 -> +-1000dps, 2 -> +-500dps, 3 -> +-250dps, 4 -> +-125dps, 5 -> +-62.5dps, 6 -> +-31.25dps, 7 -> +-15.6225dps
#if ICM40600_GYR_RANGE_DPS == 2000
#define ICM40600_GYR_FS     0
#elif ICM40600_GYR_RANGE_DPS == 1000
#define ICM40600_GYR_FS     1
#else
#error "Invalid ICM40600_GYR_RANGE_DPS setting: valid values are 2000, 1000"
#endif
// Bandwidth for low-pass filters, using ODR/2 FIR
#define ICM40600_ACC_BW_IND     BIT_ACCEL_UI_BW_2_FIR
#define ICM40600_GYR_BW_IND     BIT_GYRO_UI_BW_2_FIR

/* INT1 configuration */
// Polarity: 0 -> Active Low, 1 -> Active High
#define INT1_POLARITY           1
// Drive circuit: 0 -> Open Drain, 1 -> Push-Pull, fixed for INT1 do not change!
#define INT1_DRIVE_CIRCUIT      1
// Mode: 0 -> Pulse, 1 -> Latch
#define INT1_MODE               0

/* Wake-on-Motion/No-Motion */
#define ICM40600_WOM_THRESHOLD_MG       160     // 160mg @ 25Hz
#define ICM40600_WOM_COMPUTE(val_mg)    ((256 * val_mg) / 1000)
// No-Motion duration: 5s
#define ICM40600_NOM_DURATION_NS        (5 * 1000000000ULL)

/*
 **********************
 * Factory calibration
 **********************
 */
#define CALIBRATION_ODR                 7       // 200Hz (support both LPM and LNM)
#define CALIBRATION_ACC_FS              0       // +-16g (= resolution of offset register)
#define CALIBRATION_GYR_FS              1       // +-1000dps (= resolution of offset register)
#define CALIBRATION_ACC_1G              2048    // LSB/g @+-16g
#define CALIBRATION_ACC_BW_IND          BIT_ACCEL_UI_BW_4_IIR
#define CALIBRATION_GYR_BW_IND          BIT_GYRO_UI_BW_4_IIR

#define CALIBRATION_READ_INTERVAL_US    (200 * 1000)    // 200ms (200/5ms=40 packets -> 40 * 16 = 640 bytes)
#define CALIBRATION_SAMPLE_NB           200             // 200/40packets = 5 loops
#define RETRY_CNT_CALIBRATION           10              // > 5 loops

/*
 **********************
 * Selftest
 **********************
 */
#define SELF_TEST_ODR                   6   // 1000Hz
#define SELF_TEST_ACC_FS                3   // +-2g
#define SELF_TEST_GYR_FS                3   // +-250dps
#define SELF_TEST_ACC_BW_IND            BIT_ACCEL_UI_BW_10_IIR
#define SELF_TEST_GYR_BW_IND            BIT_GYRO_UI_BW_10_IIR
#define SELF_TEST_MIN_ACC_MG            225 // mg
#define SELF_TEST_MAX_ACC_MG            675 // mg
#define SELF_TEST_MIN_GYR_DPS           60  // dps
#define SELF_TEST_MAX_GYR_OFF_DPS       20  // dps
#define SELF_TEST_ACC_SHIFT_DELTA       500 // = 0.5
#define SELF_TEST_GYR_SHIFT_DELTA       500 // = 0.5

#define SELF_TEST_ACC_SCALE             32768 / (16 >> SELF_TEST_ACC_FS) / 1000
#define SELF_TEST_GYR_SCALE             32768 / (2000 >> SELF_TEST_GYR_FS)

#define SELF_TEST_READ_INTERVAL_US      (20 * 1000) // 20ms (20/1ms=20 packets -> 20 * 16 = 320 bytes)
#define SELF_TEST_SAMPLE_NB             200 // 200/20packets = 10 loops
#define RETRY_CNT_SELF_TEST             20  // > 10 loops
#define SELF_TEST_PRECISION             1000
#define SELF_TEST_SETTLE_TIME_MS        25

#define SELF_TEST_MIN_ACC               (SELF_TEST_MIN_ACC_MG * SELF_TEST_ACC_SCALE * SELF_TEST_PRECISION)
#define SELF_TEST_MAX_ACC               (SELF_TEST_MAX_ACC_MG * SELF_TEST_ACC_SCALE * SELF_TEST_PRECISION)
#define SELF_TEST_MIN_GYR               (SELF_TEST_MIN_GYR_DPS * SELF_TEST_GYR_SCALE * SELF_TEST_PRECISION)
#define SELF_TEST_MAX_GYR               (SELF_TEST_MAX_GYR_DPS * SELF_TEST_GYR_SCALE * SELF_TEST_PRECISION)
#define SELF_TEST_MAX_GYR_OFFSET        (SELF_TEST_MAX_GYR_OFF_DPS * SELF_TEST_GYR_SCALE * SELF_TEST_PRECISION)

/*
 ****************
 * register map *
 ****************
 */
/* Bank 0 */
#define REG_DEVICE_CONFIG           0x11
#define REG_SPI_SPEED               0x13
#define REG_INT_CONFIG              0x14
#define REG_FIFO_CONFIG             0x16
#define REG_INT_STATUS              0x2D
#define REG_FIFO_BYTE_COUNT1        0x2E
#define REG_FIFO_BYTE_COUNT2        0x2F
#define REG_FIFO_DATA               0x30
#define REG_SIGNAL_PATH_RESET       0x4B
#define REG_INTF_CONFIG0            0x4C
#define REG_PWR_MGMT_0              0x4E
#define REG_GYRO_CONFIG0            0x4F
#define REG_ACCEL_CONFIG0           0x50
#define REG_GYRO_CONFIG1            0x51
#define REG_GYRO_ACCEL_CONFIG0      0x52
#define REG_ACCEL_CONFIG1           0x53
#define REG_ACCEL_WOM_X_THR         0x54
#define REG_ACCEL_WOM_Y_THR         0x55
#define REG_ACCEL_WOM_Z_THR         0x56
#define REG_SMD_CONFIG              0x57
#define REG_INT_STATUS2             0x59
#define REG_TMST_CONFIG             0x5A
#define REG_FIFO_CONFIG1            0x5F
#define REG_FIFO_CONFIG2            0x60
#define REG_FIFO_CONFIG3            0x61
#define REG_INT_CONFIG0             0x63
#define REG_INT_CONFIG1             0x64
#define REG_INT_SOURCE0             0x65
#define REG_INT_SOURCE1             0x66
#define REG_SELF_TEST_CONFIG        0x70
#define REG_WHO_AM_I                0x75
#define REG_REG_BANK_SEL            0x76
#define REG_GOS_USER0               0x77
#define REG_GOS_USER1               0x78
#define REG_GOS_USER2               0x79
#define REG_GOS_USER3               0x7A
#define REG_GOS_USER4               0x7B
#define REG_GOS_USER5               0x7C
#define REG_GOS_USER6               0x7D
#define REG_GOS_USER7               0x7E
#define REG_GOS_USER8               0x7F
/* Bank 1 */
#define REG_XG_ST_DATA              0x5F
#define REG_YG_ST_DATA              0x60
#define REG_ZG_ST_DATA              0x61
/* Bank 2 */
#define REG_XA_ST_DATA              0x3B
#define REG_YA_ST_DATA              0x3C
#define REG_ZA_ST_DATA              0x3D

/* REG_WHO_AM_I */
#define WHO_AM_I_ICM40604           0x32
#define WHO_AM_I_ICM40605           0x33
/* REG_REG_BANK_SEL */
#define BIT_BANK_SEL_0              0x00
#define BIT_BANK_SEL_1              0x01
#define BIT_BANK_SEL_2              0x02
/* REG_DEVICE_CONFIG */
#define BIT_SOFT_RESET              0x01
/* REG_SPI_SPEED */
#define BIT_SPI_SPEED_5MHZ          0x03
#define BIT_SPI_SPEED_17MHZ         0x05
/* REG_GYRO_CONFIG0/REG_ACCEL_CONFIG0 */
#define SHIFT_GYRO_FS_SEL           5
#define SHIFT_ACCEL_FS_SEL          5
#define SHIFT_ODR_CONF              0
/* REG_INT_CONFIG */
#define SHIFT_INT1_POLARITY         0
#define SHIFT_INT1_DRIVE_CIRCUIT    1
#define SHIFT_INT1_MODE             2
/* REG_PWR_MGMT_0 */
#define BIT_TEMP_DIS                0x20
#define BIT_IDLE                    0x10
#define BIT_GYRO_MODE_OFF           0x00
#define BIT_GYRO_MODE_STBY          0x04
#define BIT_GYRO_MODE_LN            0x0C
#define BIT_ACCEL_MODE_OFF          0x00
#define BIT_ACCEL_MODE_LN           0x03
/* REG_SIGNAL_PATH_RESET */
#define BIT_FIFO_FLUSH              0x02
/* REG_INTF_CONFIG0 */
#define BIT_FIFO_COUNT_REC          0x40
#define BIT_COUNT_BIG_ENDIAN        0x20
#define BIT_SENS_DATA_BIG_ENDIAN    0x10
#define BIT_UI_SIFS_DISABLE_SPI     0x02
#define BIT_UI_SIFS_DISABLE_I2C     0x03
/* REG_FIFO_CONFIG1 */
#define BIT_FIFO_ACCEL_EN           0x01
#define BIT_FIFO_GYRO_EN            0x02
#define BIT_FIFO_TEMP_EN            0x04
#define BIT_FIFO_TMST_FSYNC_EN      0x08
#define BIT_FIFO_HIRES_EN           0x10
#define BIT_FIFO_WM_TH              0x20
#define BIT_FIFO_RESUME_PART_RD     0x40
/* REG_INT_CONFIG1 */
#define BIT_INT_ASY_RST_DISABLE     0x10
/* REG_INT_SOURCE0 */
#define BIT_INT_UI_AGC_RDY_INT1_EN  0x01
#define BIT_INT_FIFO_FULL_INT1_EN   0x02
#define BIT_INT_FIFO_THS_INT1_EN    0x04
#define BIT_INT_UI_DRDY_INT1_EN     0x08
#define BIT_INT_RESET_DONE_INT1_EN  0x10
#define BIT_INT_PLL_RDY_INT1_EN     0x20
#define BIT_INT_UI_FSYNC_INT1_EN    0x40
/* REG_INT_SOURCE1 */
#define BIT_INT_WOM_X_INT1_EN       0x01
#define BIT_INT_WOM_Y_INT1_EN       0x02
#define BIT_INT_WOM_Z_INT1_EN       0x04
#define BIT_INT_SMD_INT1_EN         0x08
#define BIT_INT_WOM_XYZ_INT1_EN     (BIT_INT_WOM_X_INT1_EN | BIT_INT_WOM_Y_INT1_EN | BIT_INT_WOM_Z_INT1_EN)
/* REG_SELF_TEST_CONFIG */
#define BIT_SELF_TEST_REGULATOR_EN  0x40
#define BIT_TEST_AZ_EN              0x20
#define BIT_TEST_AY_EN              0x10
#define BIT_TEST_AX_EN              0x08
#define BIT_TEST_GZ_EN              0x04
#define BIT_TEST_GY_EN              0x02
#define BIT_TEST_GX_EN              0x01
/* REG_INT_STATUS */
#define BIT_INT_STATUS_AGC_RDY      0x01
#define BIT_INT_STATUS_FIFO_FULL    0x02
#define BIT_INT_STATUS_FIFO_THS     0x04
#define BIT_INT_STATUS_DRDY         0x08
#define BIT_INT_STATUS_RESET_DONE   0x10
#define BIT_INT_STATUS_PLL_DRY      0x20
#define BIT_INT_STATUS_UI_FSYNC     0x40
/* REG_INT_STATUS2 */
#define BIT_INT_STATUS_WOM_X        0x01
#define BIT_INT_STATUS_WOM_Y        0x02
#define BIT_INT_STATUS_WOM_Z        0x04
#define BIT_INT_STATUS_SMD          0x08
#define BIT_INT_STATUS_WOM_XYZ      (BIT_INT_STATUS_WOM_X | BIT_INT_STATUS_WOM_Y | BIT_INT_STATUS_WOM_Z)
/* REG_FIFO_CONFIG */
#define BIT_FIFO_MODE_BYPASS        0x00
#define BIT_FIFO_MODE_STREAM        0x40
#define BIT_FIFO_MODE_STOP_FULL     0x80
/* REG_GYRO_ACCEL_CONFIG0 */
#define BIT_ACCEL_UI_BW_2_FIR       0x00
#define BIT_ACCEL_UI_BW_4_IIR       0x10
#define BIT_ACCEL_UI_BW_5_IIR       0x20
#define BIT_ACCEL_UI_BW_8_IIR       0x30
#define BIT_ACCEL_UI_BW_10_IIR      0x40
#define BIT_ACCEL_UI_BW_16_IIR      0x50
#define BIT_ACCEL_UI_BW_20_IIR      0x60
#define BIT_ACCEL_UI_BW_40_IIR      0x70
#define BIT_GYRO_UI_BW_2_FIR        0x00
#define BIT_GYRO_UI_BW_4_IIR        0x01
#define BIT_GYRO_UI_BW_5_IIR        0x02
#define BIT_GYRO_UI_BW_8_IIR        0x03
#define BIT_GYRO_UI_BW_10_IIR       0x04
#define BIT_GYRO_UI_BW_16_IIR       0x05
#define BIT_GYRO_UI_BW_20_IIR       0x06
#define BIT_GYRO_UI_BW_40_IIR       0x07
/* fifo data packet header */
#define BIT_FIFO_HEAD_MSG           0x80
#define BIT_FIFO_HEAD_ACCEL         0x40
#define BIT_FIFO_HEAD_GYRO          0x20
#define BIT_FIFO_HEAD_TMSP_ODR      0x08
#define BIT_FIFO_HEAD_TMSP_NO_ODR   0x04
#define BIT_FIFO_HEAD_TMSP_FSYNC    0x0C
#define BIT_FIFO_HEAD_ODR_ACCEL     0x02
#define BIT_FIFO_HEAD_ODR_GYRO      0x01
/* REG_SMD_CONFIG */
#define BIT_WOM_INT_MODE_OR         0x00
#define BIT_WOM_INT_MODE_AND        0x08
#define BIT_WOM_MODE_INITIAL        0x00
#define BIT_WOM_MODE_PREV           0x04
#define BIT_SMD_MODE_OFF            0x00
#define BIT_SMD_MODE_OLD            0x01
#define BIT_SMD_MODE_SHORT          0x02
#define BIT_SMD_MODE_LONG           0x03
/* REG_TMST_CONFIG */
#define BIT_EN_DREG_FIFO_D2A        0x20
#define BIT_TMST_TO_REGS_EN         0x10
#define BIT_TMST_RESOL              0x08
#define BIT_TMST_DELTA_EN           0x04
#define BIT_TMST_FSYNC_EN           0x02
#define BIT_TMST_EN                 0x01

/* FIFO data definitions */
#define FIFO_PACKET_SIZE            16
#define FIFO_MAX_PACKET_NB          65
#define FIFO_MIN_PACKET_NB          62

/* sensor startup time */
#define ICM40600_GYRO_START_TIME_MS     40
#define ICM40600_ACCEL_START_TIME_MS    10

/* temperature sensor */
#define TEMP_SCALE                  (128.0f / 115.49f)      // scale, #9447
#define TEMP_OFFSET                 25                      // 25 degC offset

#define SPI_WRITE_0(addr, data) spiQueueWrite(addr, data, 2)
#define SPI_WRITE_1(addr, data, delay) spiQueueWrite(addr, data, delay)
#define GET_SPI_WRITE_MACRO(_1,_2,_3,NAME,...) NAME
#define SPI_WRITE(...) GET_SPI_WRITE_MACRO(__VA_ARGS__, SPI_WRITE_1, SPI_WRITE_0)(__VA_ARGS__)

#define SPI_READ_0(addr, size, buf) spiQueueRead(addr, size, buf, 0)
#define SPI_READ_1(addr, size, buf, delay) spiQueueRead(addr, size, buf, delay)
#define GET_SPI_READ_MACRO(_1,_2,_3,_4,NAME,...) NAME
#define SPI_READ(...) GET_SPI_READ_MACRO(__VA_ARGS__, SPI_READ_1, SPI_READ_0)(__VA_ARGS__)

#define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG)
#define EVT_SENSOR_NO_MOTION sensorGetMyEventType(SENS_TYPE_NO_MOTION)
#define EVT_SENSOR_ANY_MOTION sensorGetMyEventType(SENS_TYPE_ANY_MOTION)

#define MAX_NUM_COMMS_EVENT_SAMPLES 15

#define SPI_PACKET_SIZE     30
#define FIFO_READ_SIZE      (FIFO_MAX_PACKET_NB * FIFO_PACKET_SIZE)
#define BUF_MARGIN          32   // some extra buffer for additional reg RW when a FIFO read happens
#define SPI_BUF_SIZE        (FIFO_READ_SIZE + BUF_MARGIN)

#define ACC_FS_RANGE        (16 >> ICM40600_ACC_FS)
#define GYR_FS_RANGE        (2000 >> ICM40600_GYR_FS)
#define GRAVITY_NORM        9.80665f
#define kScale_acc          (GRAVITY_NORM * ACC_FS_RANGE / 32768.0f)
#define kScale_gyr          (NANO_PI / 180.0f * GYR_FS_RANGE / 32768.0f)

#define RATE_TO_HZ              SENSOR_HZ(1.0f)
#define DECIMATION_HIGH_RATE    SENSOR_HZ(1000.0f)
#define DECIMATION_LOW_RATE     SENSOR_HZ(25.0f)
#define NO_DECIMATION_MAX_RATE  SENSOR_HZ(200.0f)   // will set ODR to DECIMATION_HIGH_RATE if more than this rate
#define NO_DECIMATION_MIN_RATE  SENSOR_HZ(25.0f)    // will set ODR to DECIMATION_LOW_RATE if less than this rate
#define MAX_BATCH_SIZE          (((FIFO_MIN_PACKET_NB * 90) / 100) * FIFO_PACKET_SIZE)      // 90% of FIFO size

/* skip first some samples */
#define ACC_SKIP_SAMPLE_NB          0
#define GYR_SKIP_SAMPLE_NB          3

#define CHIP_TIME_RES_US        1
#define CHIP_TIME_OFFSET_US     64000000ULL // 64sec
#define MIN_INCREMENT_TIME_NS   1000000ULL  // 1ms for 1000Hz

enum SensorIndex {
    FIRST_CONT_SENSOR = 0,
    ACC = FIRST_CONT_SENSOR,
    GYR,
    NUM_CONT_SENSOR,
    FIRST_ONESHOT_SENSOR = NUM_CONT_SENSOR,
    WOM = FIRST_ONESHOT_SENSOR,
    NOMO,
    NUM_OF_SENSOR,
};

enum SensorEvents {
    NO_EVT = -1,
    EVT_SPI_DONE = EVT_APP_START + 1,
    EVT_SENSOR_INTERRUPT_1,
};

enum IntState {
    INT_READ_STATUS,
    INT_PROCESS_STATUS,
    INT_READ_FIFO_DATA,
    INT_DONE,
};

enum InitState {
    RESET_ICM40600,
    INIT_ICM40600,
    INIT_DONE,
};

enum CalibrationState {
    CALIBRATION_START,
    CALIBRATION_READ_STATUS,
    CALIBRATION_READ_DATA,
    CALIBRATION_SET_OFFSET,
    CALIBRATION_DONE
};

enum SelfTestState {
    TEST_START,
    TEST_READ_STATUS,
    TEST_READ_DATA,
    TEST_READ_OTP,
    TEST_REPORT,
    TEST_DONE
};

enum SensorState {
    // keep this in sync with getStateName
    SENSOR_BOOT,
    SENSOR_VERIFY_ID,
    SENSOR_INITIALIZING,
    SENSOR_IDLE,
    SENSOR_POWERING_UP,
    SENSOR_POWERING_DOWN,
    SENSOR_CONFIG_CHANGING,
    SENSOR_INT_1_HANDLING,
    SENSOR_CALIBRATING,
    SENSOR_TESTING,
    SENSOR_SAVE_CALIBRATION,
    SENSOR_NUM_OF_STATE
};
#if DBG_STATE
#define PRI_STATE "s"
static const char * getStateName(int32_t s) {
    // keep this in sync with SensorState
    static const char* const l[] = {"BOOT", "VERIFY_ID", "INIT", "IDLE", "PWR_UP",
            "PWR_DN", "CFG_CHANGE", "INT1", "CALIB", "TEST", "SAVE_CALIB"};
    if (s >= 0 && s < SENSOR_NUM_OF_STATE) {
        return l[s];
    }
    return "???";
#else
#define PRI_STATE PRIi32
static int32_t getStateName(int32_t s) {
    return s;
#endif
}

#if DBG_TIMESTAMP
struct StatisticsSet {
    uint32_t sync_reset_count;
    uint32_t sync_count;
    uint32_t sync_adjust_plus;
    uint32_t sync_adjust_minus;
    uint32_t sync_truncate;
};
#endif

struct ConfigStat {
    uint64_t latency;
    uint32_t rate;
    bool enable;
};

struct CalibrationData {
    struct HostHubRawPacket header;
    struct SensorAppEventHeader data_header;
    int32_t xBias;
    int32_t yBias;
    int32_t zBias;
} __attribute__((packed));

struct TestResultData {
    struct HostHubRawPacket header;
    struct SensorAppEventHeader data_header;
} __attribute__((packed));

struct ICM40600Sensor {
    struct ConfigStat pConfig; // pending config status request
    struct TripleAxisDataEvent *data_evt;
    uint32_t handle;
    uint32_t rate;
    uint64_t latency;
    uint64_t prev_rtc_time;
    int16_t offset[3];
    int16_t data[3];
    bool updated;
    bool powered; // activate status
    bool configed; // configure status
    bool wait_for_odr;
    enum SensorIndex idx;
    uint8_t flush;
    uint8_t decimator;
    uint8_t data_cnt;
    uint8_t skip_sample_cnt;
};

struct FifoPacketData {
    int16_t accel[3];
    int16_t gyro[3];
    uint16_t timestamp;
    bool odr_accel;
    bool odr_gyro;
    bool valid_accel;
    bool valid_gyro;
    int8_t temp;
};

struct ICM40600FactoryCal {
    int32_t data[3];
    int32_t data_count;
};

struct ICM40600SelfTest {
    int32_t data[3];
    int32_t data_st_on[3];
    int32_t data_st_off[3];
    int32_t data_count;
    bool st_mode;
    uint8_t otp_st_data_gyro[3];
    uint8_t otp_st_data_accel[3];
};

struct ICM40600Config {
    uint32_t accel_rate;
    uint32_t gyro_rate;
    uint32_t wom_threshold;
    uint32_t fifo_rate;
    uint16_t fifo_watermark;
    uint8_t fifo_sample_size;
    bool accel_on;
    bool gyro_on;
    bool wom_on;
};

struct ICM40600Task {
    uint32_t tid;
    struct ICM40600Sensor sensors[NUM_OF_SENSOR];

    // spi and interrupt
    spi_cs_t cs;
    struct SpiMode mode;
    struct SpiPacket packets[SPI_PACKET_SIZE];
    struct SpiDevice *spiDev;
    struct Gpio *Int1;
    IRQn_Type Irq1;
    struct ChainedIsr Isr1;
    bool Int1_EN;

    // spi buffers
    uint8_t *dataBuffer[3];
    uint8_t txrxBuffer[SPI_BUF_SIZE];

    // states
    volatile uint8_t state;  //task state, type enum SensorState, do NOT change this directly
    enum InitState init_state;
    enum IntState int_state;
    enum CalibrationState calibration_state;
    enum SelfTestState selftest_state;

    // pending config
    bool pending_int[1];
    bool pending_flush;
    bool pending_config[NUM_OF_SENSOR];
    bool pending_calibration_save;

    struct ICM40600Config config;
    uint32_t noMotionTimerHandle;
    uint16_t fifo_count;
    bool powered;
    bool flush;

    // calibration
#ifdef ACCEL_CAL_ENABLED
    struct AccelCal accel_cal;
#endif
#ifdef GYRO_CAL_ENABLED
    struct GyroCal gyro_cal;
#endif

    // timestamping
    time_sync_t gSensorTime2RTC;
    uint64_t data_timestamp;
    uint64_t chip_time_us;
    uint64_t last_sync_time;
    uint64_t last_sync_data_ts;
    uint16_t chip_timestamp;
    bool fifo_start_sync;

    // temperature data from chip in degC
    float chip_temperature;

    // spi rw
    struct SlabAllocator *mDataSlab;
    uint16_t mWbufCnt;
    uint8_t mRegCnt;
    uint8_t mRetryLeft;
    bool spiInUse;

    struct ICM40600FactoryCal factory_cal;

    struct ICM40600SelfTest self_test;

#if DBG_TIMESTAMP
    struct StatisticsSet statistics_set;
#endif
};

static uint32_t AccRates[] = {
    SENSOR_HZ(25.0f/8.0f),
    SENSOR_HZ(25.0f/4.0f),
    SENSOR_HZ(25.0f/2.0f),
    SENSOR_HZ(25.0f),
    SENSOR_HZ(50.0f),
    SENSOR_HZ(100.0f),
    SENSOR_HZ(200.0f),
    SENSOR_HZ(500.0f),
    0,
};

static uint32_t GyrRates[] = {
    SENSOR_HZ(25.0f/8.0f),
    SENSOR_HZ(25.0f/4.0f),
    SENSOR_HZ(25.0f/2.0f),
    SENSOR_HZ(25.0f),
    SENSOR_HZ(50.0f),
    SENSOR_HZ(100.0f),
    SENSOR_HZ(200.0f),
    SENSOR_HZ(500.0f),
    0,
};

static struct ICM40600Task mTask;

#define DEC_INFO(name, type, axis, inter, samples) \
    .sensorName = name, \
    .sensorType = type, \
    .numAxis = axis, \
    .interrupt = inter, \
    .minSamples = samples

#define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \
    DEC_INFO(name, type, axis, inter, samples), \
    .supportedRates = rates

#define DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale) \
    DEC_INFO(name, type, axis, inter, samples), \
    .supportedRates = rates, \
    .flags1 = SENSOR_INFO_FLAGS1_RAW, \
    .rawType = raw, \
    .rawScale = scale

#define DEC_INFO_RATE_BIAS(name, rates, type, axis, inter, samples, bias) \
    DEC_INFO(name, type, axis, inter, samples), \
    .supportedRates = rates, \
    .flags1 = SENSOR_INFO_FLAGS1_BIAS, \
    .biasType = bias

#define DEC_INFO_RATE_RAW_BIAS(name, rates, type, axis, inter, samples, raw, scale, bias) \
    DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale), \
    .flags1 = SENSOR_INFO_FLAGS1_RAW | SENSOR_INFO_FLAGS1_BIAS, \
    .biasType = bias

typedef struct ICM40600Task _Task;
#define TASK  _Task* const _task

// To get rid of static variables all task functions should have a task structure pointer input.
// This is an intermediate step.
#define TDECL()  TASK = &mTask; (void)_task

// Access task variables without explicitly specify the task structure pointer.
#define T(v)  (_task->v)

// Atomic get state
#define GET_STATE() (atomicReadByte(&(_task->state)))

// Atomic set state, this set the state to arbitrary value, use with caution
#define SET_STATE(s) do{\
        DEBUG_PRINT_IF(DBG_STATE, "set state %" PRI_STATE "\n", getStateName(s));\
        atomicWriteByte(&(_task->state), (s));\
    }while(0)

// Atomic switch state from IDLE to desired state.
static bool trySwitchState_(TASK, enum SensorState newState) {
#if DBG_STATE
    bool ret = atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState);
    uint8_t prevState = ret ? SENSOR_IDLE : GET_STATE();
    DEBUG_PRINT("switch state %" PRI_STATE "->%" PRI_STATE ", %s\n",
            getStateName(prevState), getStateName(newState), ret ? "ok" : "failed");
    return ret;
#else
    return atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState);
#endif
}
// Short-hand
#define trySwitchState(s) trySwitchState_(_task, (s))

static const struct SensorInfo mSensorInfo[NUM_OF_SENSOR] =
{
#ifdef ACCEL_CAL_ENABLED
    { DEC_INFO_RATE_RAW_BIAS("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
            NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0 / kScale_acc, SENS_TYPE_ACCEL_BIAS) },
#else
    { DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
            NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0 / kScale_acc) },
#endif
#ifdef GYRO_CAL_ENABLED
    { DEC_INFO_RATE_BIAS("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
            NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS) },
#else
    { DEC_INFO_RATE("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
            NANOHUB_INT_NONWAKEUP, 20) },
#endif
    { DEC_INFO("Wake-on-Motion", SENS_TYPE_ANY_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
    { DEC_INFO("No-Motion", SENS_TYPE_NO_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
};

static void time_init(TASK)
{
    time_sync_init(&T(gSensorTime2RTC));
}

static bool sensortime_to_rtc_time(TASK, uint64_t sensor_time, uint64_t *rtc_time_ns)
{
    return time_sync_estimate_time1(&T(gSensorTime2RTC), sensor_time, rtc_time_ns);
}

static void map_sensortime_to_rtc_time(TASK, uint64_t sensor_time, uint64_t rtc_time_ns)
{
#if DBG_TIMESTAMP
    T(statistics_set).sync_count++;
#endif
    time_sync_add(&T(gSensorTime2RTC), rtc_time_ns, sensor_time);
}

static void invalidate_sensortime_to_rtc_time(TASK)
{
#if DBG_TIMESTAMP
    T(statistics_set).sync_reset_count++;
#endif
    time_sync_reset(&T(gSensorTime2RTC));
}

static void dataEvtFree(void *ptr)
{
    TDECL();
    struct TripleAxisDataEvent *ev = (struct TripleAxisDataEvent *)ptr;

    slabAllocatorFree(T(mDataSlab), ev);
}

static void spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay)
{
    TDECL();

    if (T(spiInUse)) {
        ERROR_PRINT("SPI in use, cannot queue write\n");
        return;
    }
    T(packets[T(mRegCnt)]).size = 2;
    T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]);
    T(packets[T(mRegCnt)]).rxBuf = &T(txrxBuffer[T(mWbufCnt)]);
    T(packets[T(mRegCnt)]).delay = delay * 1000;
    T(txrxBuffer[T(mWbufCnt++)]) = ICM40600_SPI_WRITE | addr;
    T(txrxBuffer[T(mWbufCnt++)]) = data;
    T(mRegCnt)++;
}

/*
 * need to be sure size of buf is larger than read size
 */
static void spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay)
{
    TDECL();

    if (T(spiInUse)) {
        ERROR_PRINT("SPI in use, cannot queue read %d %d\n", (int)addr, (int)size);
        return;
    }

    *buf = &T(txrxBuffer[T(mWbufCnt)]);
    T(packets[T(mRegCnt)]).size = size + 1; // first byte will not contain valid data
    T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]);
    T(packets[T(mRegCnt)]).rxBuf = *buf;
    T(packets[T(mRegCnt)]).delay = delay * 1000;
    T(txrxBuffer[T(mWbufCnt)++]) = ICM40600_SPI_READ | addr;
    T(mWbufCnt) += size;
    T(mRegCnt)++;
}

static void spiBatchTxRx(struct SpiMode *mode,
        SpiCbkF callback, void *cookie, const char * src)
{
    TDECL();

    if (T(mRegCnt) == 0) {
        callback(cookie, 0);
        return;
    }

    if (T(mWbufCnt) > SPI_BUF_SIZE) {
        ERROR_PRINT("NO enough SPI buffer space, dropping transaction.\n");
        return;
    }
    if (T(mRegCnt) > SPI_PACKET_SIZE) {
        ERROR_PRINT("spiBatchTxRx too many packets!\n");
        return;
    }

    T(spiInUse) = true;
    T(mWbufCnt) = 0;

    // Reset variables before issuing SPI transaction.
    // SPI may finish before spiMasterRxTx finish
    uint8_t regCount = T(mRegCnt);
    T(mRegCnt) = 0;

    if (spiMasterRxTx(T(spiDev), T(cs), T(packets), regCount, mode, callback, cookie) < 0) {
        ERROR_PRINT("spiMasterRxTx failed!\n");
    }
}

static bool icm40600Isr1(struct ChainedIsr *isr)
{
    TASK = container_of(isr, struct ICM40600Task, Isr1);

    if (!extiIsPendingGpio(T(Int1))) {
        return false;
    }

    /* better to use atomic read for the pending flag but not serious even not atomic */
    if (!T(pending_int[0])) {
        osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, _task, NULL, T(tid));
    }
    extiClearPendingGpio(T(Int1));
    return true;
}

static void sensorSpiCallback(void *cookie, int err)
{
    TDECL();

    T(spiInUse) = false;

    if (!osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, T(tid)))
        ERROR_PRINT("sensorSpiCallback: osEnqueuePrivateEvt() failed\n");
}

static void noMotionCallback(uint32_t timerId, void *data)
{
    const struct ICM40600Sensor *sensor = (const struct ICM40600Sensor *)data;

    if (!sensor->configed)
        return;

    osEnqueueEvt(EVT_SENSOR_NO_MOTION, NULL, NULL);
}

static void setOffsetReg(TASK)
{
    uint8_t val;
    const int16_t * const acc_offset = T(sensors[ACC]).offset;
    const int16_t * const gyr_offset = T(sensors[GYR]).offset;

    DEBUG_PRINT("Set ACC hw offset %d %d %d\n",
                -acc_offset[0], -acc_offset[1], -acc_offset[2]);
    DEBUG_PRINT("Set GYR hw offset %d %d %d\n",
                -gyr_offset[0], -gyr_offset[1], -gyr_offset[2]);

    // GX_L
    val = (-gyr_offset[0]) & 0xff;
    SPI_WRITE(REG_GOS_USER0, val);
    // GY_H / GX_H
    val = (-gyr_offset[1] >> 4) & 0xf0;
    val |= ((-gyr_offset[0] >> 8) & 0x0f);
    SPI_WRITE(REG_GOS_USER1, val);
    // GY_L
    val = (-gyr_offset[1]) & 0xff;
    SPI_WRITE(REG_GOS_USER2, val);
    // GZ_L
    val = (-gyr_offset[2]) & 0xff;
    SPI_WRITE(REG_GOS_USER3, val);
    // AX_H / GZ_H
    val = (-acc_offset[0] >> 4) & 0xf0;
    val |= ((-gyr_offset[2] >> 8) & 0x0f);
    SPI_WRITE(REG_GOS_USER4, val);
    // AX_H / GZ_H
    val = (-acc_offset[0] >> 4) & 0xf0;
    val |= ((-gyr_offset[2] >> 8) & 0x0f);
    SPI_WRITE(REG_GOS_USER4, val);
    // AX_L
    val = (-acc_offset[0]) & 0xff;
    SPI_WRITE(REG_GOS_USER5, val);
    // AY_L
    val = (-acc_offset[1]) & 0xff;
    SPI_WRITE(REG_GOS_USER6, val);
    // AZ_H / AY_H
    val = (-acc_offset[2] >> 4) & 0xf0;
    val |= ((-acc_offset[1] >> 8) & 0x0f);
    SPI_WRITE(REG_GOS_USER7, val);
    // AZ_L
    val = (-acc_offset[2]) & 0xff;
    SPI_WRITE(REG_GOS_USER8, val);
}

static void resetOffsetReg(TASK, enum SensorIndex idx)
{
    uint8_t val;

    DEBUG_PRINT("Reset offset registers sensor=%d\n", idx);

    if (idx == ACC) {
        val = (-T(sensors[GYR]).offset[2] >> 8) & 0x0f;
        SPI_WRITE(REG_GOS_USER4, val);  // AX_H / GZ_H
        SPI_WRITE(REG_GOS_USER5, 0x00); // AX_L
        SPI_WRITE(REG_GOS_USER6, 0x00); // AY_L
        SPI_WRITE(REG_GOS_USER7, 0x00); // AZ_H / AY_H
        SPI_WRITE(REG_GOS_USER8, 0x00); // AZ_L
    } else if (idx == GYR) {
        SPI_WRITE(REG_GOS_USER0, 0x00); // GX_L
        SPI_WRITE(REG_GOS_USER1, 0x00); // GY_H / GX_H
        SPI_WRITE(REG_GOS_USER2, 0x00); // GY_L
        SPI_WRITE(REG_GOS_USER3, 0x00); // GZ_L
        val = (-T(sensors[ACC]).offset[0] >> 4) & 0xf0;
        SPI_WRITE(REG_GOS_USER4, val);  // AX_H / GZ_H
    }
}

static bool saveCalibration(TASK)
{
    if (trySwitchState(SENSOR_SAVE_CALIBRATION)) {
        setOffsetReg(_task);
        spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
        return true;
    } else {
        return false;
    }
}

static bool accCfgData(void *data, void *cookie)
{
    TDECL();
    struct ICM40600Sensor *sensor = &T(sensors[ACC]);
    struct CfgData {
        int32_t hw[3];  // chip frame (hw unit @FSR=factory calibration)
        float sw[3];    // body frame
    };
    struct CfgData *values = data;
    int i;

    for (i = 0; i < 3; i++) {
        // offset register is 12bit
        if (values->hw[i] > 2047) {
            sensor->offset[i] = 2047;
        } else if (values->hw[i] < -2048) {
            sensor->offset[i] = -2048;
        } else {
            sensor->offset[i] = values->hw[i];
        }
    }
#ifdef ACCEL_CAL_ENABLED
    accelCalBiasSet(&T(accel_cal), values->sw[0], values->sw[1], values->sw[2]);
#endif
    if (!saveCalibration(_task)) {
        T(pending_calibration_save) = true;
    }
    INFO_PRINT("accCfgData hw bias: data=%d, %d, %d\n",
            sensor->offset[0], sensor->offset[1], sensor->offset[2]);

    return true;
}

static bool gyrCfgData(void *data, void *cookie)
{
    TDECL();
    struct ICM40600Sensor *sensor = &T(sensors[GYR]);
    const struct AppToSensorHalDataPayload *p = data;
    int i;

    if (p->type == HALINTF_TYPE_GYRO_CAL_BIAS && p->size == sizeof(struct GyroCalBias)) {
        const struct GyroCalBias *bias = p->gyroCalBias;
        for (i = 0; i < 3; i++) {
            // offset register is 12bit
            if (bias->hardwareBias[i] > 2047) {
                sensor->offset[i] = 2047;
            } else if (bias->hardwareBias[i] < -2048) {
                sensor->offset[i] = -2048;
            } else {
                sensor->offset[i] = bias->hardwareBias[i];
            }
        }
#ifdef GYRO_CAL_ENABLED
        gyroCalSetBias(&T(gyro_cal), bias->softwareBias[0], bias->softwareBias[1],
                       bias->softwareBias[2], sensorGetTime());
#endif
        if (!saveCalibration(_task)) {
            T(pending_calibration_save) = true;
        }
        INFO_PRINT("gyrCfgData hw bias: data=%d, %d, %d\n",
                sensor->offset[0], sensor->offset[1], sensor->offset[2]);
    } else if (p->type == HALINTF_TYPE_GYRO_OTC_DATA && p->size == sizeof(struct GyroOtcData)) {
        // Over-temperature gyro calibration not supported
    } else {
        ERROR_PRINT("Unknown gyro config data type 0x%04x, size %d\n", p->type, p->size);
    }

    return true;
}

static bool validateFifoData(const int16_t data[3])
{
    bool ret = true;

    if ((data[0] == -32768) && (data[1] == -32768) && (data[2] == -32768))
        ret = false;

    return ret;
}

static void getFifoData(const uint8_t *data, int idx, struct FifoPacketData *out)
{
    /* invalidate all flags */
    out->valid_accel = false;
    out->valid_gyro = false;

    /* Header        : 1 byte
     * Accel         : 6 byte
     * Gyro          : 6 byte
     * Temp          : 1 byte
     * Timestamp     : 2 byte */
    if (data[idx] & BIT_FIFO_HEAD_ACCEL) {
        out->accel[0] = (data[idx + 2] << 8) | data[idx + 1];
        out->accel[1] = (data[idx + 4] << 8) | data[idx + 3];
        out->accel[2] = (data[idx + 6] << 8) | data[idx + 5];
        out->valid_accel = validateFifoData(out->accel);
    }
    out->odr_accel = (data[idx] & BIT_FIFO_HEAD_ODR_ACCEL) ? true : false;

    if (data[idx] & BIT_FIFO_HEAD_GYRO) {
        out->gyro[0] = (data[idx + 8] << 8) | data[idx + 7];
        out->gyro[1] = (data[idx + 10] << 8) | data[idx + 9];
        out->gyro[2] = (data[idx + 12] << 8) | data[idx + 11];
        out->valid_gyro = validateFifoData(out->gyro);
    }
    out->odr_gyro = (data[idx] & BIT_FIFO_HEAD_ODR_GYRO) ? true : false;

    out->temp = (int8_t)data[idx + 13];
    out->timestamp = (data[idx + 15] << 8) | data[idx + 14];
}

static bool calSelftestGetOneData(enum SensorIndex sensor_idx, const uint8_t *data, int idx, int16_t raw_data[3])
{
    struct FifoPacketData fifo_data;
    bool ret = false;

    getFifoData(data, idx, &fifo_data);

    switch (sensor_idx) {
    case ACC:
        if (fifo_data.valid_accel) {
            raw_data[0] = fifo_data.accel[0];
            raw_data[1] = fifo_data.accel[1];
            raw_data[2] = fifo_data.accel[2];
            ret = true;
        }
        break;
    case GYR:
        if (fifo_data.valid_gyro) {
            raw_data[0] = fifo_data.gyro[0];
            raw_data[1] = fifo_data.gyro[1];
            raw_data[2] = fifo_data.gyro[2];
            ret = true;
        }
        break;
    default:
        break;
    }

    return ret;
}

static void calSelftestFifoEnable(TASK, enum SensorIndex idx, bool en, uint32_t delay)
{
    uint8_t val;

    if (en) {
        // enable FIFO output
        T(config).fifo_sample_size = FIFO_PACKET_SIZE;
        val = BIT_FIFO_ACCEL_EN | BIT_FIFO_GYRO_EN |
              BIT_FIFO_TEMP_EN | BIT_FIFO_TMST_FSYNC_EN;
        SPI_WRITE(REG_FIFO_CONFIG, BIT_FIFO_MODE_STREAM);
        SPI_WRITE(REG_FIFO_CONFIG1, val, delay); // with wait
    } else {
        // disable FIFO output
        T(config).fifo_sample_size = 0;
        SPI_WRITE(REG_FIFO_CONFIG, BIT_FIFO_MODE_BYPASS);
        SPI_WRITE(REG_FIFO_CONFIG1, 0);
    }
}

/*
 * Factory calibration
 */
static void sendCalibrationResult(uint8_t status, uint8_t sensorType,
        int32_t xBias, int32_t yBias, int32_t zBias)
{
    struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData));
    if (!data) {
        ERROR_PRINT("Couldn't alloc cal result pkt");
        return;
    }

    data->header.appId = ICM40600_APP_ID;
    data->header.dataLen = (sizeof(struct CalibrationData) - sizeof(struct HostHubRawPacket));
    data->data_header.msgId = SENSOR_APP_MSG_ID_CAL_RESULT;
    data->data_header.sensorType = sensorType;
    data->data_header.status = status;

    data->xBias = xBias;
    data->yBias = yBias;
    data->zBias = zBias;

    if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
        ERROR_PRINT("Couldn't send cal result evt");
}

static void calibrationInit(TASK, enum SensorIndex idx)
{
    uint8_t val;

    // Disable Interrupt
    SPI_WRITE(REG_INT_SOURCE0, 0);
    SPI_WRITE(REG_INT_SOURCE1, 0);

    // reset offset registers
    resetOffsetReg(_task, idx);

    // stop FIFO
    calSelftestFifoEnable(_task, idx, false, 0);

    // set rate
    SPI_WRITE(REG_GYRO_CONFIG0, (CALIBRATION_GYR_FS << SHIFT_GYRO_FS_SEL) |
                                (CALIBRATION_ODR << SHIFT_ODR_CONF));
    SPI_WRITE(REG_ACCEL_CONFIG0, (CALIBRATION_ACC_FS << SHIFT_ACCEL_FS_SEL) |
                                 (CALIBRATION_ODR << SHIFT_ODR_CONF));

    // set filter
    SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, CALIBRATION_ACC_BW_IND | CALIBRATION_GYR_BW_IND);

    // turn on sensors
    switch (idx) {
    case ACC:
        val = BIT_ACCEL_MODE_LN;
        break;
    case GYR:
        val = BIT_GYRO_MODE_LN;
        break;
    default:
        val = 0;
        break;
    }
    SPI_WRITE(REG_PWR_MGMT_0, val, 200 * 1000);

    calSelftestFifoEnable(_task, idx, true, CALIBRATION_READ_INTERVAL_US);
}

static void calibrationDeinit(TASK, enum SensorIndex idx)
{
    calSelftestFifoEnable(_task, idx, false, 0);

    SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, ICM40600_ACC_BW_IND | ICM40600_GYR_BW_IND);
    SPI_WRITE(REG_PWR_MGMT_0, 0, 200); // 9136

    // make register accesses happen when sensor is enabled next time
    T(config).gyro_rate = 0;
    T(config).accel_rate = 0;
    T(config).fifo_rate = 0;
    T(config).fifo_watermark = 0;
    T(config).accel_on = false;
    T(config).gyro_on = false;
    T(config).wom_on = false;
}

static void calibrationHandling(TASK, enum SensorIndex idx)
{
    const uint8_t *buf, *data;
    uint8_t int_status;
    uint16_t fifo_count;
    int i, t;
    bool r;
    int16_t raw_data[3] = { 0, 0, 0 };

    if (idx != ACC && idx != GYR) {
        ERROR_PRINT("Invalid sensor index\n");
        return;
    }

    switch (T(calibration_state)) {
        case CALIBRATION_START:
            T(mRetryLeft) = RETRY_CNT_CALIBRATION;
            calibrationInit(_task, idx);
            for (i = 0; i < 3; i++) {
                T(factory_cal).data[i] = 0;
            }
            SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
            SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1]));
            T(calibration_state) = CALIBRATION_READ_STATUS;
            T(factory_cal).data_count = CALIBRATION_SAMPLE_NB;
            spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
            break;
        case CALIBRATION_READ_STATUS:
            buf = T(dataBuffer[0]);
            int_status = buf[1];
            buf = T(dataBuffer[1]);
            fifo_count = buf[2] << 8 | buf[1];
            fifo_count = fifo_count - fifo_count % T(config).fifo_sample_size;
            T(fifo_count) = fifo_count;
            DEBUG_PRINT("fifo_count = %d\n", fifo_count);
            if (int_status & BIT_INT_STATUS_FIFO_FULL) {
                ERROR_PRINT("fifo overflow\n");
                calibrationDeinit(_task, idx);
                T(calibration_state) = CALIBRATION_DONE;
                sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType, 0, 0, 0);
            } else {
                T(calibration_state) = CALIBRATION_READ_DATA;
                SPI_READ(REG_FIFO_DATA, fifo_count, &T(dataBuffer[0]));
            }
            spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
            break;
        case CALIBRATION_READ_DATA:
            buf = T(dataBuffer[0]);
            data = &buf[1];
            for (i = 0; i < T(fifo_count); i += T(config).fifo_sample_size) {
                r = calSelftestGetOneData(idx, data, i, raw_data);
                if (r == false) {
                    ERROR_PRINT("invalid data packet\n");
                    calibrationDeinit(_task, idx);
                    T(calibration_state) = CALIBRATION_DONE;
                    sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType, 0, 0, 0);
                    break;
                }
                for (t = 0; t < 3; t++) {
                    T(factory_cal).data[t] += raw_data[t];
                }
                T(factory_cal).data_count--;
                if (T(factory_cal).data_count == 0) {
                    break;
                }
            }
            if (T(factory_cal).data_count > 0) {
                if (--T(mRetryLeft) == 0) {
                    ERROR_PRINT("calibration timeout\n");
                    calibrationDeinit(_task, idx);
                    T(calibration_state) = CALIBRATION_DONE;
                    sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType, 0, 0, 0);
                } else {
                    calSelftestFifoEnable(_task, idx, false, 0); // toggle FIFO to reset
                    calSelftestFifoEnable(_task, idx, true, CALIBRATION_READ_INTERVAL_US);
                    SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
                    SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1]));
                    T(calibration_state) = CALIBRATION_READ_STATUS;
                }
                spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
                break;
            }
            T(calibration_state) = CALIBRATION_SET_OFFSET;
            // fall-through
        case CALIBRATION_SET_OFFSET:
            DEBUG_PRINT("calibration total: %ld, %ld, %ld, data count=%d\n",
                    T(factory_cal).data[0],
                    T(factory_cal).data[1],
                    T(factory_cal).data[2],
                    CALIBRATION_SAMPLE_NB);
            for (i = 0; i < 3; i++) {
                T(factory_cal).data[i] /= CALIBRATION_SAMPLE_NB;
            }
            DEBUG_PRINT("average: %ld, %ld, %ld\n",
                    T(factory_cal).data[0],
                    T(factory_cal).data[1],
                    T(factory_cal).data[2]);
            if (idx == ACC) {
                // assume the largest data axis shows +1 or -1 gee
                t = 0;
                for (i = 0; i < 3; i++) {
                    if (abs(T(factory_cal).data[i]) > abs(T(factory_cal).data[t]))
                        t = i;
                }
                if (T(factory_cal).data[t] > 0) {
                    DEBUG_PRINT("assume axis %d is %d\n", t, CALIBRATION_ACC_1G);
                    T(factory_cal).data[t] -= CALIBRATION_ACC_1G;
                } else {
                    DEBUG_PRINT("assume axis %d is %d\n", t, -CALIBRATION_ACC_1G);
                    T(factory_cal).data[t] += CALIBRATION_ACC_1G;
                }
            }
            // set bias to offset registers
            for (i = 0; i < 3; i++) {
                T(sensors[idx]).offset[i] = T(factory_cal).data[i];
            }
            setOffsetReg(_task);
            calibrationDeinit(_task, idx);

            sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS,
                                  mSensorInfo[idx].sensorType,
                                  T(factory_cal).data[0],
                                  T(factory_cal).data[1],
                                  T(factory_cal).data[2]);
            INFO_PRINT("reported: %ld, %ld, %ld\n",
                        T(factory_cal).data[0],
                        T(factory_cal).data[1],
                        T(factory_cal).data[2]);
            T(calibration_state) = CALIBRATION_DONE;
            spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
            break;
        default:
            break;
    }
}

static bool accCalibration(void *cookie)
{
    TDECL();

    INFO_PRINT("Accel Calibration\n");

    if (!T(powered) && trySwitchState(SENSOR_CALIBRATING)) {
        T(calibration_state) = CALIBRATION_START;
        calibrationHandling(_task, ACC);
        return true;
    } else {
        ERROR_PRINT("Cannot run calibration because sensor is busy\n");
        sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL, 0, 0, 0);
        return false;
    }
}

static bool gyrCalibration(void *cookie)
{
    TDECL();

    INFO_PRINT("Gyro Calibration\n");

    if (!T(powered) && trySwitchState(SENSOR_CALIBRATING)) {
        T(calibration_state) = CALIBRATION_START;
        calibrationHandling(_task, GYR);
        return true;
    } else {
        ERROR_PRINT("Cannot run calibration because sensor is busy\n");
        sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO, 0, 0, 0);
        return false;
    }
}

/*
 * Selftest
 */
static void sendTestResult(uint8_t status, uint8_t sensorType)
{
    struct TestResultData *data = heapAlloc(sizeof(struct TestResultData));
    if (!data) {
        ERROR_PRINT("Couldn't alloc test result packet");
        return;
    }

    data->header.appId = ICM40600_APP_ID;
    data->header.dataLen = (sizeof(struct TestResultData) - sizeof(struct HostHubRawPacket));
    data->data_header.msgId = SENSOR_APP_MSG_ID_TEST_RESULT;
    data->data_header.sensorType = sensorType;
    data->data_header.status = status;

    if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
        ERROR_PRINT("Couldn't send test result packet");
}

static const uint16_t SelfTestEquation[256] = {
    2620, 2646, 2672, 2699, 2726, 2753, 2781, 2808,
    2837, 2865, 2894, 2923, 2952, 2981, 3011, 3041,
    3072, 3102, 3133, 3165, 3196, 3228, 3261, 3293,
    3326, 3359, 3393, 3427, 3461, 3496, 3531, 3566,
    3602, 3638, 3674, 3711, 3748, 3786, 3823, 3862,
    3900, 3939, 3979, 4019, 4059, 4099, 4140, 4182,
    4224, 4266, 4308, 4352, 4395, 4439, 4483, 4528,
    4574, 4619, 4665, 4712, 4759, 4807, 4855, 4903,
    4953, 5002, 5052, 5103, 5154, 5205, 5257, 5310,
    5363, 5417, 5471, 5525, 5581, 5636, 5693, 5750,
    5807, 5865, 5924, 5983, 6043, 6104, 6165, 6226,
    6289, 6351, 6415, 6479, 6544, 6609, 6675, 6742,
    6810, 6878, 6946, 7016, 7086, 7157, 7229, 7301,
    7374, 7448, 7522, 7597, 7673, 7750, 7828, 7906,
    7985, 8065, 8145, 8227, 8309, 8392, 8476, 8561,
    8647, 8733, 8820, 8909, 8998, 9088, 9178, 9270,
    9363, 9457, 9551, 9647, 9743, 9841, 9939, 10038,
    10139, 10240, 10343, 10446, 10550, 10656, 10763, 10870,
    10979, 11089, 11200, 11312, 11425, 11539, 11654, 11771,
    11889, 12008, 12128, 12249, 12371, 12495, 12620, 12746,
    12874, 13002, 13132, 13264, 13396, 13530, 13666, 13802,
    13940, 14080, 14221, 14363, 14506, 14652, 14798, 14946,
    15096, 15247, 15399, 15553, 15709, 15866, 16024, 16184,
    16346, 16510, 16675, 16842, 17010, 17180, 17352, 17526,
    17701, 17878, 18057, 18237, 18420, 18604, 18790, 18978,
    19167, 19359, 19553, 19748, 19946, 20145, 20347, 20550,
    20756, 20963, 21173, 21385, 21598, 21814, 22033, 22253,
    22475, 22700, 22927, 23156, 23388, 23622, 23858, 24097,
    24338, 24581, 24827, 25075, 25326, 25579, 25835, 26093,
    26354, 26618, 26884, 27153, 27424, 27699, 27976, 28255,
    28538, 28823, 29112, 29403, 29697, 29994, 30294, 30597,
    30903, 31212, 31524, 31839, 32157, 32479, 32804
};

static void selfTestInit(TASK, enum SensorIndex idx)
{
    uint8_t val;

    // Disable Interrupt
    SPI_WRITE(REG_INT_SOURCE0, 0);
    SPI_WRITE(REG_INT_SOURCE1, 0);

    // reset offset registers
    resetOffsetReg(_task, idx);

    // stop FIFO
    calSelftestFifoEnable(_task, idx, false, 0);

    // self-test mode set
    val = 0;
    if (T(self_test).st_mode) {
        if (idx == ACC) {
            val |= (BIT_TEST_AX_EN | BIT_TEST_AY_EN | BIT_TEST_AZ_EN);
            val |= BIT_SELF_TEST_REGULATOR_EN;
        } else if (idx == GYR) {
            val |= (BIT_TEST_GX_EN | BIT_TEST_GY_EN | BIT_TEST_GZ_EN);
        }
    }
    SPI_WRITE(REG_SELF_TEST_CONFIG, val);

    // set rate
    SPI_WRITE(REG_GYRO_CONFIG0, (SELF_TEST_GYR_FS << SHIFT_GYRO_FS_SEL) |
                                (SELF_TEST_ODR << SHIFT_ODR_CONF));
    SPI_WRITE(REG_ACCEL_CONFIG0, (SELF_TEST_ACC_FS << SHIFT_ACCEL_FS_SEL) |
                                 (SELF_TEST_ODR << SHIFT_ODR_CONF));

    // set filter
    val = 0;
    if (idx == ACC) {
        val |= SELF_TEST_ACC_BW_IND;
    } else if (idx == GYR) {
        val |= SELF_TEST_GYR_BW_IND;
    }
    SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, val);

    // turn on sensors
    val = 0;
    if (idx == ACC) {
        val |= BIT_ACCEL_MODE_LN;
    } else if (idx == GYR) {
        val |= BIT_GYRO_MODE_LN;
    }
    SPI_WRITE(REG_PWR_MGMT_0, val, 200 * 1000);

    calSelftestFifoEnable(_task, idx, true, SELF_TEST_READ_INTERVAL_US);
}

static void selfTestDeinit(TASK, enum SensorIndex idx)
{
    calSelftestFifoEnable(_task, idx, false, 0);

    SPI_WRITE(REG_SELF_TEST_CONFIG, 0);
    SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, ICM40600_ACC_BW_IND | ICM40600_GYR_BW_IND);
    SPI_WRITE(REG_PWR_MGMT_0, 0, 200); // 9136

    // make register accesses happen when sensor is enabled next time
    T(config).gyro_rate = 0;
    T(config).accel_rate = 0;
    T(config).fifo_rate = 0;
    T(config).fifo_watermark = 0;
    T(config).accel_on = false;
    T(config).gyro_on = false;
    T(config).wom_on = false;
}

static bool checkAccelSelftest(TASK)
{
    int32_t st_res;
    uint32_t st_otp[3];
    int i;
    int32_t ratio;
    bool pass = true;
    bool otp_value_zero = false;

    /* calculate ST_OTP */
    for (i = 0; i < 3; i++) {
        if (T(self_test.otp_st_data_accel[i] != 0))
            st_otp[i] = SelfTestEquation[T(self_test).otp_st_data_accel[i] - 1];
        else
            otp_value_zero = true;
    }

    if (!otp_value_zero) {
        /* criteria a */
        for (i = 0; i < 3; i++) {
            st_res = T(self_test).data_st_on[i] - T(self_test).data_st_off[i];
            ratio = abs(st_res / st_otp[i] - SELF_TEST_PRECISION);
            if (ratio >= SELF_TEST_ACC_SHIFT_DELTA) {
                INFO_PRINT("error accel[%d] : st_res = %ld, st_otp = %ld\n", i, st_res, st_otp[i]);
                pass = false;
            }
        }
    } else {
        /* criteria b */
        for (i = 0; i < 3; i++) {
            st_res = abs(T(self_test).data_st_on[i] - T(self_test).data_st_off[i]);
            if (st_res < SELF_TEST_MIN_ACC || st_res > SELF_TEST_MAX_ACC) {
                INFO_PRINT("error accel[%d] : st_res = %ld, min = %d, max = %d\n", i, st_res, SELF_TEST_MIN_ACC, SELF_TEST_MAX_ACC);
                pass = false;
            }
        }
    }

    return pass;
}

static bool checkGyroSelftest(TASK)
{
    int32_t st_res;
    uint32_t st_otp[3];
    int i;
    bool pass = true;
    bool otp_value_zero = false;

    /* calculate ST_OTP */
    for (i = 0; i < 3; i++) {
        if (T(self_test).otp_st_data_gyro[i] != 0)
            st_otp[i] = SelfTestEquation[T(self_test).otp_st_data_gyro[i] - 1];
        else
            otp_value_zero = true;
    }

    if (!otp_value_zero) {
        /* criteria a */
        for (i = 0; i < 3; i++) {
            st_res = T(self_test).data_st_on[i] - T(self_test).data_st_off[i];
            if (st_res <= st_otp[i] * SELF_TEST_GYR_SHIFT_DELTA) {
                INFO_PRINT("error gyro[%d] : st_res = %ld, st_otp = %ld\n", i, st_res, st_otp[i]);
                pass = false;
            }
        }
    } else {
        /* criteria b */
        for (i = 0; i < 3; i++) {
            st_res = abs(T(self_test).data_st_on[i] - T(self_test).data_st_off[i]);
            if (st_res < SELF_TEST_MIN_GYR) {
                INFO_PRINT("error gyro[%d] : st_res = %ld, min = %d\n", i, st_res, SELF_TEST_MIN_GYR);
                pass = false;
            }
        }
    }

    if (pass) {
        /* criteria c */
        for (i = 0; i < 3; i++) {
            if (abs(T(self_test).data_st_off[i]) > SELF_TEST_MAX_GYR_OFFSET) {
                INFO_PRINT("error gyro[%d] = %d, max = %d\n", i, abs(T(self_test).data_st_off[i]), SELF_TEST_MAX_GYR_OFFSET);
                pass = false;
            }
        }
    }

    return pass;
}

static void selfTestHandling(TASK, enum SensorIndex idx)
{
    const uint8_t *buf, *data;
    uint8_t int_status;
    uint16_t fifo_count;
    int i, t;
    bool r;
    int16_t raw_data[3] = { 0, 0, 0 };
    bool pass;

    if (idx != ACC && idx != GYR) {
        ERROR_PRINT("Invalid sensor index\n");
        return;
    }

    switch (T(selftest_state)) {
    case TEST_START:
        T(mRetryLeft) = RETRY_CNT_SELF_TEST;
        selfTestInit(_task, idx);
        for (i = 0; i < 3; i++) {
            T(self_test).data[i] = 0;
        }
        SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
        SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1]));
        T(selftest_state) = TEST_READ_STATUS;
        T(self_test).data_count = SELF_TEST_SAMPLE_NB;
        spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
        break;
    case TEST_READ_STATUS:
        buf = T(dataBuffer[0]);
        int_status = buf[1];
        buf = T(dataBuffer[1]);
        fifo_count = buf[2] << 8 | buf[1];
        fifo_count = fifo_count - fifo_count % T(config).fifo_sample_size;
        T(fifo_count) = fifo_count;
        DEBUG_PRINT("fifo_count = %d\n", fifo_count);
        if (int_status & BIT_INT_STATUS_FIFO_FULL) {
            ERROR_PRINT("fifo overflow\n");
            selfTestDeinit(_task, idx);
            T(selftest_state) = TEST_DONE;
            sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType);
        } else {
            T(selftest_state) = TEST_READ_DATA;
            SPI_READ(REG_FIFO_DATA, fifo_count, &T(dataBuffer[0]));
        }
        spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
        break;
    case TEST_READ_DATA:
        buf = T(dataBuffer[0]);
        data = &buf[1];
        for (i = 0; i < T(fifo_count); i += T(config).fifo_sample_size) {
            r = calSelftestGetOneData(idx, data, i, raw_data);
            if (r == false) {
                ERROR_PRINT("invalid data packet\n");
                selfTestDeinit(_task, idx);
                T(selftest_state) = TEST_DONE;
                sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType);
                break;
            }
            for (t = 0; t < 3; t++) {
                T(self_test).data[t] += raw_data[t];
            }
            T(self_test).data_count--;
            if (T(self_test).data_count == 0) {
                break;
            }
        }
        if (T(self_test).data_count > 0) {
            if (--T(mRetryLeft) == 0) {
                ERROR_PRINT("selftest timeout\n");
                selfTestDeinit(_task, idx);
                T(selftest_state) = TEST_DONE;
                sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, mSensorInfo[idx].sensorType);
            } else {
                calSelftestFifoEnable(_task, idx, false, 0); // toggle FIFO to reset
                calSelftestFifoEnable(_task, idx, true, SELF_TEST_READ_INTERVAL_US);
                SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
                SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[1]));
                T(selftest_state) = TEST_READ_STATUS;
            }
            spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
            break;
        }
        T(selftest_state) = TEST_READ_OTP;
        // fall-through
    case TEST_READ_OTP:
        for (i = 0; i < 3; i++) {
            T(self_test).data[i] = T(self_test).data[i] / SELF_TEST_SAMPLE_NB * SELF_TEST_PRECISION;
        }
        INFO_PRINT("st_mode=%d average (scaled) : %ld, %ld, %ld\n",
                    T(self_test).st_mode,
                    T(self_test).data[0],
                    T(self_test).data[1],
                    T(self_test).data[2]);

        selfTestDeinit(_task, idx);

        for (i = 0; i < 3; i++) {
            if (T(self_test).st_mode) {
                T(self_test).data_st_on[i] = T(self_test).data[i];
            } else {
                T(self_test).data_st_off[i] = T(self_test).data[i];
            }
        }

        // Run again with self-test mode on
        if (!T(self_test).st_mode) {
            T(self_test).st_mode = true;
            T(selftest_state) = TEST_START;
        } else {
            INFO_PRINT("st mode on  : %ld %ld %ld\n",
                    T(self_test).data_st_on[0],
                    T(self_test).data_st_on[1],
                    T(self_test).data_st_on[2]);
            INFO_PRINT("st mode off : %ld %ld %ld\n",
                    T(self_test).data_st_off[0],
                    T(self_test).data_st_off[1],
                    T(self_test).data_st_off[2]);

            // read OTP
            if (idx == ACC) {
                SPI_WRITE(REG_REG_BANK_SEL, BIT_BANK_SEL_2);
                SPI_READ(REG_XA_ST_DATA, 3, &T(dataBuffer[0]));
            } else if (idx == GYR) {
                SPI_WRITE(REG_REG_BANK_SEL, BIT_BANK_SEL_1);
                SPI_READ(REG_XG_ST_DATA, 3, &T(dataBuffer[0]));
            }
            SPI_WRITE(REG_REG_BANK_SEL, BIT_BANK_SEL_0);

            T(selftest_state) = TEST_REPORT;
        }
        spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
        break;
    case TEST_REPORT:
        buf = T(dataBuffer[0]);
        if (idx == ACC) {
            T(self_test).otp_st_data_accel[0] = buf[1];
            T(self_test).otp_st_data_accel[1] = buf[2];
            T(self_test).otp_st_data_accel[2] = buf[3];
            INFO_PRINT("otp accel : %d %d %d\n",
                    T(self_test).otp_st_data_accel[0],
                    T(self_test).otp_st_data_accel[1],
                    T(self_test).otp_st_data_accel[2]);
        } else if (idx == GYR) {
            T(self_test).otp_st_data_gyro[0] = buf[1];
            T(self_test).otp_st_data_gyro[1] = buf[2];
            T(self_test).otp_st_data_gyro[2] = buf[3];
            INFO_PRINT("otp gyro  : %d %d %d\n",
                    T(self_test).otp_st_data_gyro[0],
                    T(self_test).otp_st_data_gyro[1],
                    T(self_test).otp_st_data_gyro[2]);
        }

        pass = false;
        if (idx == ACC) {
            pass = checkAccelSelftest(_task);
        } else if (idx == GYR) {
            pass = checkGyroSelftest(_task);
        }
        if (pass) {
            sendTestResult(SENSOR_APP_EVT_STATUS_SUCCESS,
                                  mSensorInfo[idx].sensorType);
        } else {
            sendTestResult(SENSOR_APP_EVT_STATUS_ERROR,
                                  mSensorInfo[idx].sensorType);
        }

        T(selftest_state) = TEST_DONE;
        selfTestDeinit(_task, idx);
        spiBatchTxRx(&T(mode), sensorSpiCallback, &T(sensors[idx]), __FUNCTION__);
        break;
    default:
        break;
    }
}

static bool accSelfTest(void *cookie)
{
    TDECL();

    INFO_PRINT("Accel Selftest\n");

   if (!T(powered) && trySwitchState(SENSOR_TESTING)) {
        T(self_test).st_mode = false;
        T(selftest_state) = TEST_START;
        selfTestHandling(_task, ACC);
        return true;
    } else {
        ERROR_PRINT("cannot test accel because sensor is busy\n");
        sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL);
        return false;
    }
}

static bool gyrSelfTest(void *cookie)
{
    TDECL();

    INFO_PRINT("Gyro Selftest\n");

    if (!T(powered) && trySwitchState(SENSOR_TESTING)) {
        T(self_test).st_mode = false;
        T(selftest_state) = TEST_START;
        selfTestHandling(_task, GYR);
        return true;
    } else {
        ERROR_PRINT("cannot test accel because sensor is busy\n");
        sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO);
        return false;
    }
}

static bool sensorFirmwareUpload(void *cookie)
{
    TDECL();
    int i = (long int)cookie;

    sensorSignalInternalEvt(T(sensors[i]).handle, SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);

    return true;
}

static bool enableInterrupt(struct Gpio *pin, IRQn_Type irq, struct ChainedIsr *isr)
{
    gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
    syscfgSetExtiPort(pin);
    extiEnableIntGpio(pin, EXTI_TRIGGER_RISING);
    extiChainIsr(irq, isr);
    return true;
}

static bool disableInterrupt(struct Gpio *pin, IRQn_Type irq, struct ChainedIsr *isr)
{
    extiUnchainIsr(irq, isr);
    extiDisableIntGpio(pin);
    return true;
}

static void configInt1(TASK, bool on)
{
    enum SensorIndex i;
    bool powered = false;

    if (on && !T(Int1_EN)) {
        enableInterrupt(T(Int1), T(Irq1), &T(Isr1));
        T(Int1_EN) = true;
    } else if (!on && T(Int1_EN)) {
        for (i = 0; i < NUM_OF_SENSOR; i++) {
            if (T(sensors[i]).powered) {
                powered = true;
                break;
            }
        }
        if (!powered) {
            disableInterrupt(T(Int1), T(Irq1), &T(Isr1));
            T(Int1_EN) = false;
        }
    }
}

static uint8_t computeOdrConf(uint32_t rate)
{
    switch (rate) {
    case SENSOR_HZ(1000.0f):
        return 6;
    case SENSOR_HZ(200.0f):
        return 7;
    case SENSOR_HZ(100.0f):
        return 8;
    case SENSOR_HZ(50.0f):
        return 9;
    case SENSOR_HZ(25.0f):
    default:
        return 10;
    }
}

static void computeConfig(TASK, struct ICM40600Config *config)
{
    uint64_t latency;
    uint64_t val;
    uint32_t latency_ms;
    int i;

    // copy current parameters
    *config = T(config);

    // compute sensors on
    if (T(sensors[ACC]).configed || T(sensors[WOM]).configed || T(sensors[NOMO]).configed) {
        config->accel_on = true;
    } else {
        config->accel_on = false;
    }
    config->gyro_on = T(sensors[GYR]).configed;
    if (T(sensors[WOM]).configed || T(sensors[NOMO]).configed) {
        config->wom_on = true;
    } else {
        config->wom_on = false;
    }

    // compute accel and gyro rates
    if (config->accel_on) {
        if (T(sensors[ACC]).configed) {
            if (T(sensors[ACC]).rate > NO_DECIMATION_MAX_RATE) {
                config->accel_rate = DECIMATION_HIGH_RATE;
            } else if (T(sensors[ACC]).rate < NO_DECIMATION_MIN_RATE) {
                config->accel_rate = DECIMATION_LOW_RATE;
            } else {
                config->accel_rate = T(sensors[ACC]).rate;
            }
        } else {
            config->accel_rate = NO_DECIMATION_MIN_RATE;
        }
    }
    if (config->gyro_on) {
        if (T(sensors[GYR]).configed) {
            if (T(sensors[GYR]).rate > NO_DECIMATION_MAX_RATE) {
                config->gyro_rate = DECIMATION_HIGH_RATE;
            } else if (T(sensors[GYR]).rate < NO_DECIMATION_MIN_RATE) {
                config->gyro_rate = DECIMATION_LOW_RATE;
            } else {
                config->gyro_rate = T(sensors[GYR]).rate;
            }
        } else {
            config->gyro_rate = NO_DECIMATION_MIN_RATE;
        }
    }

    // compute wom threshold
    if (config->wom_on) {
        config->wom_threshold = ICM40600_WOM_THRESHOLD_MG / (config->accel_rate / NO_DECIMATION_MIN_RATE);
    }

    // compute fifo configuration
    if (T(sensors[ACC]).configed || T(sensors[GYR]).configed) {
        config->fifo_sample_size = FIFO_PACKET_SIZE;
    } else {
        config->fifo_sample_size = 0;
    }

    // compute fifo rate and latency/watermark
    config->fifo_rate = 0;
    latency = SENSOR_LATENCY_NODATA;
    for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) {
        if (T(sensors[i]).configed) {
            // look for the highest rate
            if (T(sensors[i]).rate > config->fifo_rate) {
                config->fifo_rate = T(sensors[i]).rate;
            }
            // look for the shortest latency
            if (T(sensors[i]).latency < latency) {
                latency = T(sensors[i]).latency;
            }
        }
    }
    if (config->fifo_rate > NO_DECIMATION_MAX_RATE) {
        config->fifo_rate = DECIMATION_HIGH_RATE;
    } else if (config->fifo_rate < NO_DECIMATION_MIN_RATE) {
        config->fifo_rate = DECIMATION_LOW_RATE;
    }

    // add 0.5ms for rounding
    latency_ms = cpuMathU64DivByU16(cpuMathU64DivByU16(latency + 500000, 1000), 1000);
    val = (uint64_t)latency_ms * (uint64_t)config->fifo_rate;
    config->fifo_watermark = cpuMathU64DivByU16(cpuMathU64DivByU16(val, 1000), RATE_TO_HZ);
    config->fifo_watermark *= config->fifo_sample_size;
    if (config->fifo_watermark < config->fifo_sample_size) {
        config->fifo_watermark = config->fifo_sample_size;
    }
    if (config->fifo_watermark > MAX_BATCH_SIZE) {
        config->fifo_watermark = MAX_BATCH_SIZE - (MAX_BATCH_SIZE % config->fifo_sample_size);
    }
}

static void updateConfig(TASK, const struct ICM40600Config *config)
{
    uint32_t delay_us;
    uint8_t val, val2;

    // Disable Interrupt
    SPI_WRITE(REG_INT_SOURCE0, 0);
    SPI_WRITE(REG_INT_SOURCE1, 0);

    // set rate and WoM threshold
    if (config->gyro_rate != T(config).gyro_rate) {
        val = computeOdrConf(config->gyro_rate);
        SPI_WRITE(REG_GYRO_CONFIG0, (ICM40600_GYR_FS << SHIFT_GYRO_FS_SEL) |
                                    (val << SHIFT_ODR_CONF));
    }
    if (config->accel_rate != T(config).accel_rate) {
        val = computeOdrConf(config->accel_rate);
        SPI_WRITE(REG_ACCEL_CONFIG0, (ICM40600_ACC_FS << SHIFT_ACCEL_FS_SEL) |
                                     (val << SHIFT_ODR_CONF));
    }
    if (config->wom_threshold != T(config).wom_threshold) {
        val = ICM40600_WOM_COMPUTE(config->wom_threshold);
        SPI_WRITE(REG_ACCEL_WOM_X_THR, val);
        SPI_WRITE(REG_ACCEL_WOM_Y_THR, val);
        SPI_WRITE(REG_ACCEL_WOM_Z_THR, val);
    }

    // set WM
    if (config->fifo_watermark != T(config).fifo_watermark) {
        SPI_WRITE(REG_FIFO_CONFIG2, config->fifo_watermark & 0xFF);
        SPI_WRITE(REG_FIFO_CONFIG3, (config->fifo_watermark >> 8) & 0xFF);
    }

    // turn on/off accel and gyro if any change
    if (config->gyro_on != T(config).gyro_on || config->accel_on != T(config).accel_on) {
        val = 0;
        if (config->gyro_on) {
            val |= BIT_GYRO_MODE_LN;
        }
        if (config->accel_on) {
            val |= BIT_ACCEL_MODE_LN;
        }
        // delay needed if gyro or accel turning on
        if ((config->gyro_on && !T(config).gyro_on) || (config->accel_on && !T(config).accel_on)) {
            delay_us = 200; // 9136
        } else {
            delay_us = 0;
        }
        SPI_WRITE(REG_PWR_MGMT_0, val, delay_us);
    }

    // turn on/off WOM
    if (config->wom_on != T(config).wom_on) {
        if (config->wom_on) {
            val = BIT_WOM_INT_MODE_OR | BIT_WOM_MODE_PREV | BIT_SMD_MODE_OLD;
        } else {
            val = 0;
        }
        SPI_WRITE(REG_SMD_CONFIG, val);
    }

    // turn on/off FIFO
    if (config->fifo_sample_size != T(config).fifo_sample_size) {
        if (config->fifo_sample_size != 0) {
            // enabling FIFO
            val = BIT_FIFO_MODE_STREAM;
            val2 = BIT_FIFO_ACCEL_EN | BIT_FIFO_GYRO_EN | BIT_FIFO_TEMP_EN |
                    BIT_FIFO_TMST_FSYNC_EN | BIT_FIFO_WM_TH;
            // reset chip time
            T(chip_time_us) = 0;
            T(chip_timestamp) = 0;
            T(fifo_start_sync) = true;
            invalidate_sensortime_to_rtc_time(_task);
        } else {
            // disabling FIFO
            val = BIT_FIFO_MODE_BYPASS;
            val2 = 0;
        }
        SPI_WRITE(REG_FIFO_CONFIG, val);
        SPI_WRITE(REG_FIFO_CONFIG1, val2);
        if (val == BIT_FIFO_MODE_BYPASS) {
            SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[0])); // 9052
        }
    }

    // enable/disable FIFO data interrupt
    if (config->fifo_sample_size != 0) {
        val = BIT_INT_FIFO_THS_INT1_EN;
    } else {
        val = 0;
    }
    SPI_WRITE(REG_INT_SOURCE0, val);

    // enable/disable WOM interrupt (only when FIFO disabled or batch mode)
    if (config->wom_on && (config->fifo_sample_size == 0 || config->fifo_watermark > config->fifo_sample_size)) {
        val = BIT_INT_WOM_XYZ_INT1_EN;
    } else {
        val = 0;
    }
    SPI_WRITE(REG_INT_SOURCE1, val);
}

static void applyConfig(TASK, const struct ICM40600Config *config)
{
    uint32_t duration_us;
    uint32_t decimator;
    int i;

    // setup wait for odr change
    if (config->accel_rate != T(config).accel_rate) {
        T(sensors[ACC]).wait_for_odr = true;
    }
    if (config->gyro_rate != T(config).gyro_rate) {
        T(sensors[GYR]).wait_for_odr = true;
    }

    // setup first samples skip
    if (config->gyro_on && !T(config).gyro_on) {
        // gyro turning on
        duration_us = (1000000 * RATE_TO_HZ) / config->gyro_rate;
        if (duration_us * GYR_SKIP_SAMPLE_NB > ICM40600_GYRO_START_TIME_MS * 1000) {
            T(sensors[GYR]).skip_sample_cnt = GYR_SKIP_SAMPLE_NB;
        } else {
            T(sensors[GYR]).skip_sample_cnt = (ICM40600_GYRO_START_TIME_MS * 1000) / duration_us;
            if ((ICM40600_GYRO_START_TIME_MS * 1000) % duration_us) {
                T(sensors[GYR]).skip_sample_cnt++;
            }
        }
    }
    if (config->accel_on && !T(config).accel_on) {
        // accel turning on
        duration_us = (1000000 * RATE_TO_HZ) / config->accel_rate;
        if (duration_us * ACC_SKIP_SAMPLE_NB > ICM40600_ACCEL_START_TIME_MS * 1000) {
            T(sensors[ACC]).skip_sample_cnt = ACC_SKIP_SAMPLE_NB;
        } else {
            T(sensors[ACC]).skip_sample_cnt = (ICM40600_ACCEL_START_TIME_MS * 1000) / duration_us;
            if ((ICM40600_ACCEL_START_TIME_MS * 1000) % duration_us) {
                T(sensors[ACC]).skip_sample_cnt++;
            }
        }
    }

    // update all sensors decimators
    for (i = 0; i <= GYR; i++) {
        if (!T(sensors[i]).configed || T(sensors[i]).rate == 0) {
            decimator = 1;
        } else {
            if (i == ACC) {
                decimator = config->accel_rate / T(sensors[i]).rate;
            } else if (i == GYR) {
                decimator = config->gyro_rate / T(sensors[i]).rate;
            }
        }
        if (decimator != T(sensors[i]).decimator) {
            T(sensors[i]).decimator = decimator;
            T(sensors[i]).data_cnt = 0;
        }
    }

    // setup NOMO timer
    if (T(sensors[NOMO]).configed && !T(noMotionTimerHandle)) {
        T(noMotionTimerHandle) = timTimerSet(ICM40600_NOM_DURATION_NS, 0, 100, noMotionCallback, &T(sensors[NOMO]), false);
    } else if (!T(sensors[NOMO]).configed && T(noMotionTimerHandle)) {
        timTimerCancel(T(noMotionTimerHandle));
        T(noMotionTimerHandle) = 0;
    }

    // update config
    T(config) = *config;

    DEBUG_PRINT("config: accel(%d, %luHz/%u), gyro(%d, %luHz/%u), wom(%d, %lu), "
            "fifo(%u/%u, %luHz)\n",
            T(config).accel_on, T(config).accel_rate / RATE_TO_HZ, T(sensors[ACC]).decimator,
            T(config).gyro_on, T(config).gyro_rate / RATE_TO_HZ, T(sensors[GYR]).decimator,
            T(config).wom_on, T(config).wom_threshold,
            T(config).fifo_sample_size, T(config).fifo_watermark, T(config).fifo_rate / RATE_TO_HZ);
}

static void configSensor(TASK)
{
    struct ICM40600Config config;

    computeConfig(_task, &config);
    updateConfig(_task, &config);
    applyConfig(_task, &config);
}

static void sensorTurnOn(TASK)
{
    /* enable chip timestamp */
    SPI_WRITE(REG_TMST_CONFIG, BIT_EN_DREG_FIFO_D2A | BIT_TMST_EN);

    T(powered) = true;
#if DBG_TIMESTAMP
    memset(&T(statistics_set), 0, sizeof(struct StatisticsSet));
#endif
    DEBUG_PRINT("chip on\n");
}

static void sensorTurnOff(TASK)
{
    /* disable chip timestamp */
    SPI_WRITE(REG_TMST_CONFIG, BIT_EN_DREG_FIFO_D2A);

    T(powered) = false;
#if DBG_TIMESTAMP
    DEBUG_PRINT("time sync stats: reset: %ld, sync: %ld, adjust+: %ld, adjust-: %ld, truncate: %ld\n",
            T(statistics_set).sync_reset_count,
            T(statistics_set).sync_count,
            T(statistics_set).sync_adjust_plus,
            T(statistics_set).sync_adjust_minus,
            T(statistics_set).sync_truncate);
#endif
    DEBUG_PRINT("chip off\n");
}

static void sensorPower(TASK, int sensorType, bool on)
{
    bool chip_on;
    int i;

    if (on) {
        // turn on chip if needed
        if (!T(powered)) {
            sensorTurnOn(_task);
        }
    } else {
        // change chip configuration
        configSensor(_task);
        // turn chip off if needed
        chip_on = false;
        for (i = 0; i < NUM_OF_SENSOR; ++i) {
            if (T(sensors[i]).powered) {
                chip_on = true;
                break;
            }
        }
        if (!chip_on) {
            sensorTurnOff(_task);
        }
    }
}

static bool sensorSetPower(bool on, void *cookie)
{
    TDECL();
    int sensor_type = (int)cookie;
    struct ICM40600Sensor *sensor = &T(sensors[sensor_type]);

    DEBUG_PRINT("%s: on=%d, state=%" PRI_STATE "\n", mSensorInfo[sensor_type].sensorName, on, getStateName(GET_STATE()));

    if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
        sensor->powered = on;
        if (!on) {
            sensor->configed = false;
        }
        configInt1(_task, on);
        sensorPower(_task, sensor_type, on);
        spiBatchTxRx(&T(mode), sensorSpiCallback, sensor, __FUNCTION__);
    } else {
        T(pending_config[sensor_type]) = true;
        sensor->pConfig.enable = on;
    }

    return true;
}

static bool sensorSetRate(uint32_t rate, uint64_t latency, void *cookie)
{
    TDECL();
    int sensor_type = (int)cookie;
    struct ICM40600Sensor *sensor = &T(sensors[sensor_type]);

    DEBUG_PRINT("%s: rate=%lu, latency=%llu, state=%" PRI_STATE "\n",
            mSensorInfo[sensor_type].sensorName, rate, latency, getStateName(GET_STATE()));

    if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
        sensor->rate = rate;
        sensor->latency = latency;
        sensor->configed = true;
        configSensor(_task);
        spiBatchTxRx(&T(mode), sensorSpiCallback, sensor, __FUNCTION__);
    } else {
        T(pending_config[sensor_type]) = true;
        sensor->pConfig.enable = sensor->powered;
        sensor->pConfig.rate = rate;
        sensor->pConfig.latency = latency;
    }

    return true;
}

static void sendFlushEvt(void)
{
    TDECL();
    uint32_t evtType = 0;
    int i;

    for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; i++) {
        while (T(sensors[i]).flush > 0) {
            evtType = sensorGetMyEventType(mSensorInfo[i].sensorType);
            osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL);
            T(sensors[i]).flush--;
        }
    }
}

static void int1Evt(TASK, bool flush);

static bool flushSensor(void *cookie)
{
    TDECL();
    int sensor_idx = (int)cookie;
    uint32_t evtType;

    DEBUG_PRINT("%s flush\n", mSensorInfo[sensor_idx].sensorName);

    if (sensor_idx >= FIRST_CONT_SENSOR && sensor_idx < NUM_CONT_SENSOR) {
        T(sensors[sensor_idx]).flush++;
        int1Evt(_task, true);
        return true;
    }
    if (sensor_idx >= FIRST_ONESHOT_SENSOR && sensor_idx < NUM_OF_SENSOR) {
        evtType = sensorGetMyEventType(mSensorInfo[sensor_idx].sensorType);
        osEnqueueEvt(evtType, SENSOR_DATA_EVENT_FLUSH, NULL);
        return true;
    }

    return false;
}

static bool flushData(struct ICM40600Sensor *sensor, uint32_t eventId)
{
    bool success = false;

    if (sensor->data_evt) {
        success = osEnqueueEvtOrFree(eventId, sensor->data_evt, dataEvtFree);
        sensor->data_evt = NULL;
    }

    return success;
}

static void flushAllData(void)
{
    TDECL();
    int i;

    for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; i++) {
        flushData(&T(sensors[i]),
                EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[i].sensorType));
    }
}

static bool allocateDataEvt(struct ICM40600Sensor *mSensor, uint64_t rtc_time)
{
    TDECL();

    mSensor->data_evt = slabAllocatorAlloc(T(mDataSlab));
    if (mSensor->data_evt == NULL) {
        // slab allocation failed
        ERROR_PRINT("slabAllocatorAlloc() failed\n");
        return false;
    }

    // delta time for the first sample is sample count
    memset(&mSensor->data_evt->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
    mSensor->data_evt->referenceTime = rtc_time;
    mSensor->prev_rtc_time = rtc_time;

    return true;
}

static void parseOnePacket(TASK, const uint8_t *data, int idx)
{
    struct FifoPacketData fifo_data;
    uint16_t diff;

    getFifoData(data, idx, &fifo_data);

    /* wait for odr */
    if (T(sensors[ACC]).wait_for_odr) {
        if (fifo_data.odr_accel) {
            T(sensors[ACC]).wait_for_odr = false;
        } else {
            fifo_data.valid_accel = false;
        }
    }
    if (T(sensors[GYR]).wait_for_odr) {
        if (fifo_data.odr_gyro) {
            T(sensors[GYR]).wait_for_odr = false;
        } else {
            fifo_data.valid_gyro = false;
        }
    }

    /* drop first some samples */
    if (fifo_data.valid_accel) {
        if (T(sensors[ACC]).skip_sample_cnt > 0) {
            fifo_data.valid_accel = false;
            T(sensors[ACC]).skip_sample_cnt--;
        }
    }
    if (fifo_data.valid_gyro) {
        if (T(sensors[GYR]).skip_sample_cnt > 0) {
            fifo_data.valid_gyro = false;
            T(sensors[GYR]).skip_sample_cnt--;
        }
    }

    /* update sensors data */
    if (fifo_data.valid_accel) {
        ICM40600_TO_ANDROID_COORDINATE(fifo_data.accel[0], fifo_data.accel[1], fifo_data.accel[2]);
        if (T(sensors[ACC]).configed) {
            T(sensors[ACC]).data[0] = fifo_data.accel[0];
            T(sensors[ACC]).data[1] = fifo_data.accel[1];
            T(sensors[ACC]).data[2] = fifo_data.accel[2];
            T(sensors[ACC]).updated = true;
            T(sensors[ACC]).data_cnt++;
        }
    }
    if (fifo_data.valid_gyro) {
        ICM40600_TO_ANDROID_COORDINATE(fifo_data.gyro[0], fifo_data.gyro[1], fifo_data.gyro[2]);
        if (T(sensors[GYR]).configed) {
            T(sensors[GYR]).data[0] = fifo_data.gyro[0];
            T(sensors[GYR]).data[1] = fifo_data.gyro[1];
            T(sensors[GYR]).data[2] = fifo_data.gyro[2];
            T(sensors[GYR]).updated = true;
            T(sensors[GYR]).data_cnt++;
        }
    }

    // update temperature
    T(chip_temperature) = fifo_data.temp * TEMP_SCALE + TEMP_OFFSET;

    // count up chip time
    if (T(chip_time_us) == 0) {
        T(chip_time_us) = (uint64_t)fifo_data.timestamp * CHIP_TIME_RES_US + CHIP_TIME_OFFSET_US;
    } else {
        // unsigned difference handle counter roll-up
        diff = fifo_data.timestamp - T(chip_timestamp);
        T(chip_time_us) += diff * CHIP_TIME_RES_US;
    }
    T(chip_timestamp) = fifo_data.timestamp;
}

static void pushSensorData(TASK, struct ICM40600Sensor *mSensor, uint64_t rtc_time)
{
    float x, y, z;
    float offset[3];
    bool new_offset_update = false;
    struct TripleAxisDataPoint *sample;
    uint32_t delta_time;

    switch (mSensor->idx) {
    case ACC:
        // scale data to android units
        x = mSensor->data[0] * kScale_acc;
        y = mSensor->data[1] * kScale_acc;
        z = mSensor->data[2] * kScale_acc;
        // run and apply calibration on sensor data
#ifdef ACCEL_CAL_ENABLED
        accelCalRun(&T(accel_cal), rtc_time, x, y, z, T(chip_temperature));
        accelCalBiasRemove(&T(accel_cal), &x, &y, &z);
#  ifdef ACCEL_CAL_DBG_ENABLED
        accelCalDebPrint(&T(accel_cal), T(chip_temperature));
#  endif
#endif
#ifdef GYRO_CAL_ENABLED
        gyroCalUpdateAccel(&T(gyro_cal), rtc_time, x, y, z);
#endif
        break;
    case GYR:
        // scale data to android units
        x = mSensor->data[0] * kScale_gyr;
        y = mSensor->data[1] * kScale_gyr;
        z = mSensor->data[2] * kScale_gyr;
        // run and apply calibration on sensor data
#ifdef GYRO_CAL_ENABLED
        gyroCalUpdateGyro(&T(gyro_cal), rtc_time, x, y, z, T(chip_temperature));
        gyroCalRemoveBias(&T(gyro_cal), x, y, z, &x, &y, &z);
        new_offset_update = gyroCalNewBiasAvailable(&T(gyro_cal));
        if (new_offset_update) {
            float gyro_offset_temperature_celsius;
            gyroCalGetBias(&T(gyro_cal), &offset[0], &offset[1], &offset[2],
                    &gyro_offset_temperature_celsius);
        }
#endif
        break;
    default:
        return;
    }

    if (mSensor->data_evt == NULL) {
        if (!allocateDataEvt(mSensor, rtc_time))
            return;
    }

    if (mSensor->data_evt->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) {
        ERROR_PRINT("samples bad index\n");
        return;
    }

    // handle bias sending
    if (new_offset_update) {
        if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
            // flush existing samples so the bias appears after them.
            flushData(mSensor, EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[mSensor->idx].sensorType));
            if (!allocateDataEvt(mSensor, rtc_time)) {
                return;
            }
        }
        mSensor->data_evt->samples[0].firstSample.biasCurrent = true;
        mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
        mSensor->data_evt->samples[0].firstSample.biasSample = mSensor->data_evt->samples[0].firstSample.numSamples;
        sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
        // Updates the offset in HAL.
        sample->x = offset[0];
        sample->y = offset[1];
        sample->z = offset[2];
        flushData(mSensor, sensorGetMyEventType(mSensorInfo[mSensor->idx].biasType));
        DEBUG_PRINT("send new gyro bias\n");
        if (!allocateDataEvt(mSensor, rtc_time)) {
            return;
        }
    }

    sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];

    // the first deltatime is for sample size
    if (mSensor->data_evt->samples[0].firstSample.numSamples > 1) {
        delta_time = rtc_time - mSensor->prev_rtc_time;
        delta_time = delta_time < 0 ? 0 : delta_time;
        sample->deltaTime = delta_time;
        mSensor->prev_rtc_time = rtc_time;
    }

    sample->x = x;
    sample->y = y;
    sample->z = z;

    if (mSensor->data_evt->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) {
        flushAllData();
    }
}

static void computeTimeSync(TASK, const uint8_t *data, uint32_t count, uint64_t data_timestamp)
{
    const uint64_t now = sensorGetTime();
    uint64_t chip_time_us;
    uint32_t sample_index;
    uint32_t duration_us;
    struct FifoPacketData packet;
    uint16_t sample_nb;
    uint16_t chip_timestamp;
    uint16_t diff;
    bool first_sync = false;
    bool sync = false;

    /* scan fifo data for the latest chip timestamp */
    chip_time_us = T(chip_time_us);
    chip_timestamp = T(chip_timestamp);
    sample_index = 0;
    sample_nb = 0;
    while (count >= T(config).fifo_sample_size) {
        sample_nb++;
        getFifoData(data, sample_index, &packet);
        sample_index += T(config).fifo_sample_size;
        count -= T(config).fifo_sample_size;
        // count up chip time
        if (chip_time_us == 0) {
            // first chip timestamp
            chip_time_us = (uint64_t)packet.timestamp * CHIP_TIME_RES_US + CHIP_TIME_OFFSET_US;
        } else {
            // unsigned difference handle counter roll-up
            diff = packet.timestamp - chip_timestamp;
            chip_time_us += diff * CHIP_TIME_RES_US;
        }
        chip_timestamp = packet.timestamp;
    }

    /* first time sync after FIFO enable */
    if (T(fifo_start_sync)) {
        if (T(config).fifo_rate != 0) {
            duration_us = (1000000 * RATE_TO_HZ) / T(config).fifo_rate;
        } else {
            duration_us = 0;
        }
        // use estimated duration
        map_sensortime_to_rtc_time(_task,
                chip_time_us - duration_us * sample_nb,
                data_timestamp - (uint64_t)duration_us * 1000ULL * sample_nb);
        T(fifo_start_sync) = false;
        T(last_sync_time) = now;
        first_sync = true;
    }

    /* periodical time sync */
    if ((now - T(last_sync_time)) > MSEC_TO_NANOS(100)) {
        // every 100ms
        uint64_t estimated_rtc;
        uint64_t min_rtc, max_rtc;
        uint64_t limit_ns, adjust_ns;
        uint64_t updated_data_timestamp = data_timestamp;
        bool valid = sensortime_to_rtc_time(_task, chip_time_us, &estimated_rtc);
        // check if appropriate to inject to time sync algorithm
        // adjust rtc time if necessary not to make large jitters
        if (valid) {
            limit_ns = U64_DIV_BY_CONST_U16((data_timestamp - T(last_sync_data_ts)) * 10, 1000);  // 1% (100ms * 1% = 1ms)
            adjust_ns = U64_DIV_BY_CONST_U16((data_timestamp - T(last_sync_data_ts)) * 1, 10000); // 0.01% (100ms * 0.01% = 10us)
            min_rtc = data_timestamp - limit_ns; // actual ts - x
            max_rtc = data_timestamp + limit_ns; // actual ts + x
            if ((estimated_rtc >= min_rtc) && (estimated_rtc <= max_rtc)) {
                sync = true;
            } else if (estimated_rtc < min_rtc) {
                sync = true;
                updated_data_timestamp = updated_data_timestamp - adjust_ns;
#if DBG_TIMESTAMP
                T(statistics_set).sync_adjust_minus++;
#endif
            } else if (estimated_rtc > max_rtc) {
                sync = true;
                updated_data_timestamp = updated_data_timestamp + adjust_ns;
#if DBG_TIMESTAMP
                T(statistics_set).sync_adjust_plus++;
#endif
            }
            // limit the adjustment
            if (sync) {
                if (updated_data_timestamp > (data_timestamp + MSEC_TO_NANOS(1))) {
                    updated_data_timestamp = data_timestamp + MSEC_TO_NANOS(1);
                }
            }
        }
        if (sync) {
            data_timestamp = updated_data_timestamp;
        }
    }

    /* do time sync */
    if (first_sync || sync) {
        map_sensortime_to_rtc_time(_task, chip_time_us, data_timestamp);
        T(last_sync_time) = now;
        T(last_sync_data_ts) = data_timestamp;
    }
}

static void dispatchData(TASK, const uint8_t *data, uint32_t count)
{
    uint64_t ts = 0;
    uint32_t sample_index = 0;
    int i;

    if (count == 0) {
        return;
    }

    /* do time sync if no flush or fist time starting FIFO */
    if (T(fifo_start_sync) || !T(flush)) {
        computeTimeSync(_task, data, count, T(data_timestamp));
    }

    /* process FIFO data */
    while (count >= T(config).fifo_sample_size) {
        for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) {
            T(sensors[i]).updated = false;
        }
        parseOnePacket(_task, data, sample_index);
        sample_index += T(config).fifo_sample_size;
        count -= T(config).fifo_sample_size;
        // compute timestamp
        if (!sensortime_to_rtc_time(_task, T(chip_time_us), &ts)) {
            continue;
        }
        // add data
        for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) {
            if (!T(sensors[i]).configed || !T(sensors[i]).updated) {
                continue;
            }
            if (T(sensors[i]).data_cnt % T(sensors[i]).decimator == 0) {
                if (ts <= T(sensors[i]).prev_rtc_time) {
                    ts = T(sensors[i]).prev_rtc_time + MIN_INCREMENT_TIME_NS;
#if DBG_TIMESTAMP
                    T(statistics_set).sync_truncate++;
#endif
                }
                pushSensorData(_task, &T(sensors[i]), ts);
            }
        }
    }

    flushAllData();
}

static void int1Handling(TASK)
{
    uint8_t int_status;
    uint8_t int_status2;
    uint16_t fifo_count = 0;
    union EmbeddedDataPoint trigger_axies;
    const uint8_t *buf;

    switch (T(int_state)) {
    case INT_READ_STATUS:
        T(int_state) = INT_PROCESS_STATUS;
        // read INT status1, status2 and fifo_count registers
        SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
        SPI_READ(REG_INT_STATUS2, 1, &T(dataBuffer[1]));
        SPI_READ(REG_FIFO_BYTE_COUNT1, 2, &T(dataBuffer[2]));
        T(data_timestamp) = sensorGetTime();
        spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
        break;
    case INT_PROCESS_STATUS:
        buf = T(dataBuffer[0]);
        int_status = buf[1];
        buf = T(dataBuffer[1]);
        int_status2 = buf[1];
        buf = T(dataBuffer[2]);
        fifo_count = buf[2] << 8 | buf[1];
        // INT status handling
        if (int_status2 & BIT_INT_STATUS_WOM_XYZ) {
            if (T(sensors[WOM]).configed) {
                trigger_axies.idata = ((int_status2 & BIT_INT_STATUS_WOM_X) >> 0) | ((int_status2 & BIT_INT_STATUS_WOM_Y) >> 1) | ((int_status2 & BIT_INT_STATUS_WOM_Z) >> 2);
                osEnqueueEvt(EVT_SENSOR_ANY_MOTION, trigger_axies.vptr, NULL);
            }
            if (T(sensors[NOMO]).configed && T(noMotionTimerHandle)) {
                timTimerCancel(T(noMotionTimerHandle));
                T(noMotionTimerHandle) = timTimerSet(ICM40600_NOM_DURATION_NS, 0, 100, noMotionCallback, (void *)&T(sensors[NOMO]), false);
            }
        }
        // FIFO overflow
        if (int_status & BIT_INT_STATUS_FIFO_FULL) {
            ERROR_PRINT("fifo overflow\n");
        }
        // FIFO WM status handling
        if ((int_status & BIT_INT_STATUS_FIFO_THS) || T(flush)) {
            T(int_state) = INT_READ_FIFO_DATA;
        } else {
            T(int_state) = INT_DONE;
            T(fifo_count) = 0;
            sensorSpiCallback(_task, 0);
            break;
        }
        // fall-through
    case INT_READ_FIFO_DATA:
        T(int_state) = INT_DONE;
        // compute fifo count and delete partial sample
        fifo_count -= fifo_count % T(config).fifo_sample_size;
        // read FIFO data
        T(fifo_count) = fifo_count;
        if (fifo_count > 0) {
            SPI_READ(REG_FIFO_DATA, fifo_count, &T(dataBuffer[0]));
            spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
        } else {
            sensorSpiCallback(_task, 0);
        }
        break;
    default:
        T(int_state) = INT_DONE;
        T(fifo_count) = 0;
        sensorSpiCallback(_task, 0);
        break;
    }
}

static void int1Evt(TASK, bool flush)
{
    if (trySwitchState(SENSOR_INT_1_HANDLING)) {
        T(flush) = flush;
        T(int_state) = INT_READ_STATUS;
        int1Handling(_task);
    } else {
        if (flush) {
            T(pending_flush) = true;
        } else {
            T(pending_int[0]) = true;
        }
    }
}

#define DEC_OPS(power, firmware, rate, flush) \
    .sensorPower = power, \
    .sensorFirmwareUpload = firmware, \
    .sensorSetRate = rate, \
    .sensorFlush = flush

#define DEC_OPS_CAL_CFG_TEST(power, firmware, rate, flush, cal, cfg, test) \
    DEC_OPS(power, firmware, rate, flush), \
    .sensorCalibrate = cal, \
    .sensorCfgData = cfg, \
    .sensorSelfTest = test,

#define DEC_OPS_CFG(power, firmware, rate, flush, cfg) \
    DEC_OPS(power, firmware, rate, flush), \
    .sensorCfgData = cfg

static const struct SensorOps mSensorOps[NUM_OF_SENSOR] =
{
    { DEC_OPS_CAL_CFG_TEST(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor, accCalibration,
            accCfgData, accSelfTest) },
    { DEC_OPS_CAL_CFG_TEST(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor, gyrCalibration,
            gyrCfgData, gyrSelfTest) },
    { DEC_OPS(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor) },
    { DEC_OPS(sensorSetPower, sensorFirmwareUpload, sensorSetRate, flushSensor) },
};

static void configEvent(struct ICM40600Sensor *mSensor, struct ConfigStat *ConfigData)
{
    TDECL();
    int i;

    for (i = 0; &T(sensors[i]) != mSensor; i++) ;

    if (ConfigData->enable == 0 && mSensor->powered) {
        mSensorOps[i].sensorPower(false, (void *)i);
    } else if (ConfigData->enable == 1 && !mSensor->powered) {
        mSensorOps[i].sensorPower(true, (void *)i);
    } else {
        mSensorOps[i].sensorSetRate(ConfigData->rate, ConfigData->latency, (void *)i);
    }
}

static void processPendingEvt(TASK)
{
    enum SensorIndex i;

    if (T(pending_int[0])) {
        T(pending_int[0]) = false;
        int1Evt(_task, false);
        return;
    }

    if (T(pending_flush)) {
        T(pending_flush) = false;
        int1Evt(_task, true);
        return;
    }

    for (i = 0; i < NUM_OF_SENSOR; i++) {
        if (T(pending_config[i])) {
            T(pending_config[i]) = false;
            configEvent(&T(sensors[i]), &T(sensors[i]).pConfig);
            return;
        }
    }

    if (T(pending_calibration_save)) {
        T(pending_calibration_save) = !saveCalibration(_task);
        return;
    }
}

static void sensorInit(TASK)
{
    const uint8_t *buf;

    switch (T(init_state)) {
    case RESET_ICM40600:
        // reset chip and turn on
        SPI_WRITE(REG_DEVICE_CONFIG, BIT_SOFT_RESET, 100 * 1000);
        SPI_READ(REG_INT_STATUS, 1, &T(dataBuffer[0]));
        T(powered) = false;
        sensorTurnOn(_task);
        // set SPI speed register
        SPI_WRITE(REG_SPI_SPEED, ICM40600_SPI_SPEED_REG_VALUE);
        T(init_state) = INIT_ICM40600;
        spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
        break;
    case INIT_ICM40600:
        buf = T(dataBuffer[0]);
        if (!(buf[1] & BIT_INT_STATUS_RESET_DONE)) {
            ERROR_PRINT("chip reset failed!\n");
        }
        // reconfigure SPI speed
        T(mode).speed = ICM40600_SPI_SPEED_HZ;
        // configure interface
        // i2c disabled
        // fifo_count: little endian
        // sensor data: little endian
        SPI_WRITE(REG_INTF_CONFIG0, BIT_UI_SIFS_DISABLE_I2C);
        // configure interrupt
        SPI_WRITE(REG_INT_CONFIG, (INT1_POLARITY << SHIFT_INT1_POLARITY) |
                                  (INT1_DRIVE_CIRCUIT << SHIFT_INT1_DRIVE_CIRCUIT) |
                                  (INT1_MODE << SHIFT_INT1_MODE));
        SPI_WRITE(REG_INT_CONFIG1, BIT_INT_ASY_RST_DISABLE); // 9139
        // static config of sensors
        SPI_WRITE(REG_GYRO_ACCEL_CONFIG0, ICM40600_ACC_BW_IND | ICM40600_GYR_BW_IND);
        SPI_WRITE(REG_GYRO_CONFIG1, 0x1A);  // default reset value
        SPI_WRITE(REG_ACCEL_CONFIG1, 0x15); // default reset value
        // turn off FIFO and sensors
        SPI_WRITE(REG_FIFO_CONFIG, BIT_FIFO_MODE_BYPASS);
        SPI_WRITE(REG_FIFO_CONFIG1, 0);
        SPI_WRITE(REG_FIFO_CONFIG2, 0);
        SPI_WRITE(REG_FIFO_CONFIG3, 0);
        SPI_WRITE(REG_SMD_CONFIG, BIT_SMD_MODE_OFF);
        SPI_WRITE(REG_PWR_MGMT_0, BIT_GYRO_MODE_OFF | BIT_ACCEL_MODE_OFF, 200); // 9136
        sensorTurnOff(_task);
        T(init_state) = INIT_DONE;
        spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
        break;
    default:
        break;
    }
}

static void handleSpiDoneEvt(const void* evtData)
{
    TDECL();
    struct ICM40600Sensor *mSensor;
    bool returnIdle = false;
    uint32_t i;
    const uint8_t *buf;
#ifdef ACCEL_CAL_ENABLED
    bool accelCalNewBiasAvailable;
    struct TripleAxisDataPoint *sample;
    float accelCalBiasX, accelCalBiasY, accelCalBiasZ;
    bool fallThrough;
#endif

    switch (GET_STATE()) {
    case SENSOR_BOOT:
        SET_STATE(SENSOR_VERIFY_ID);
        SPI_READ(REG_WHO_AM_I, 1, &T(dataBuffer[0]));
        spiBatchTxRx(&T(mode), sensorSpiCallback, _task, __FUNCTION__);
        break;
    case SENSOR_VERIFY_ID:
        buf = T(dataBuffer[0]);
        if (buf[1] != WHO_AM_I_ICM40604 && buf[1] != WHO_AM_I_ICM40605) {
            ERROR_PRINT("chip not found (id=0x%x)\n", buf[1]);
            break;
        }
        INFO_PRINT("chip found (id=0x%x)\n", buf[1]);
        SET_STATE(SENSOR_INITIALIZING);
        T(init_state) = RESET_ICM40600;
        sensorInit(_task);
        break;
    case SENSOR_INITIALIZING:
        if (T(init_state) == INIT_DONE) {
            for (i = 0; i < NUM_OF_SENSOR; i++) {
                sensorRegisterInitComplete(T(sensors[i]).handle);
            }
            returnIdle = true;
        } else {
            sensorInit(_task);
        }
        break;
    case SENSOR_POWERING_UP:
        mSensor = (struct ICM40600Sensor *)evtData;
        sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0);
        returnIdle = true;
        break;
    case SENSOR_POWERING_DOWN:
        mSensor = (struct ICM40600Sensor *)evtData;
#ifdef ACCEL_CAL_ENABLED
        if (mSensor->idx == ACC) {
            // https://source.android.com/devices/sensors/sensor-types.html
            // "The bias and scale calibration must only be updated while the sensor is deactivated,
            // so as to avoid causing jumps in values during streaming."
            accelCalNewBiasAvailable = accelCalUpdateBias(&T(accel_cal), &accelCalBiasX, &accelCalBiasY, &accelCalBiasZ);
            // notify HAL about new accel bias calibration
            if (accelCalNewBiasAvailable) {
                fallThrough = true;
                if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
                    // flush existing samples so the bias appears after them
                    flushData(mSensor, EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[ACC].sensorType));
                    // try to allocate another data event and break if unsuccessful
                    if (!allocateDataEvt(mSensor, sensorGetTime())) {
                        fallThrough = false;
                    }
                }
                if (fallThrough) {
                    mSensor->data_evt->samples[0].firstSample.biasCurrent = true;
                    mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
                    mSensor->data_evt->samples[0].firstSample.biasSample =
                            mSensor->data_evt->samples[0].firstSample.numSamples;
                    sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
                    sample->x = accelCalBiasX;
                    sample->y = accelCalBiasY;
                    sample->z = accelCalBiasZ;
                    flushData(mSensor, sensorGetMyEventType(mSensorInfo[ACC].biasType));
                    DEBUG_PRINT("send new accel bias\n");
                    allocateDataEvt(mSensor, sensorGetTime());
                }
            }
        }
#endif
        sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0);
        returnIdle = true;
        break;
    case SENSOR_INT_1_HANDLING:
        if (T(int_state) == INT_DONE) {
            buf = T(dataBuffer[0]);
            dispatchData(_task, &buf[1], T(fifo_count));
            if (T(flush)) {
                sendFlushEvt();
            }
            returnIdle = true;
        } else {
            int1Handling(_task);
        }
        break;
    case SENSOR_CONFIG_CHANGING:
        mSensor = (struct ICM40600Sensor *)evtData;
        sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate, mSensor->latency);
        returnIdle = true;
        break;
    case SENSOR_CALIBRATING:
        mSensor = (struct ICM40600Sensor *)evtData;
        if (mSensor->idx == ACC) {
            if (T(calibration_state) == CALIBRATION_DONE) {
                INFO_PRINT("Accel Calibration done\n");
                returnIdle = true;
            } else {
                calibrationHandling(_task, ACC);
            }
        } else if (mSensor->idx == GYR) {
            if (T(calibration_state) == CALIBRATION_DONE) {
                INFO_PRINT("Gyro Calibration done\n");
                returnIdle = true;
            } else {
                calibrationHandling(_task, GYR);
            }
        }
        break;
    case SENSOR_TESTING:
        mSensor = (struct ICM40600Sensor *)evtData;
        if (mSensor->idx == ACC) {
            if (T(selftest_state) == TEST_DONE) {
                INFO_PRINT("Accel Selftest done\n");
                returnIdle = true;
            } else {
                selfTestHandling(_task, ACC);
            }
        } else if (mSensor->idx == GYR) {
            if (T(selftest_state) == TEST_DONE) {
                INFO_PRINT("Gyro Selftest done\n");
                returnIdle = true;
            } else {
                selfTestHandling(_task, GYR);
            }
        }
        break;
    case SENSOR_SAVE_CALIBRATION:
        returnIdle = true;
        break;
    default:
        break;
    }

    if (returnIdle) {
        SET_STATE(SENSOR_IDLE);
        processPendingEvt(_task);
    }
}

#ifdef GYRO_CAL_ENABLED
static void processMagEvents(TASK, const struct TripleAxisDataEvent *evtData)
{
    const struct SensorFirstSample * const first = &evtData->samples[0].firstSample;
    const struct TripleAxisDataPoint *sample;
    uint64_t ts = evtData->referenceTime;
    unsigned int i;

    for (i = 0; i < first->numSamples; ++i) {
        sample = &evtData->samples[i];
        if (i > 0) {
            ts += sample->deltaTime;
        }
        gyroCalUpdateMag(&T(gyro_cal), ts, sample->x, sample->y, sample->z);
    }
}
#endif

static void handleEvent(uint32_t evtType, const void* evtData)
{
    TDECL();

    switch (evtType) {
    case EVT_APP_START:
        SET_STATE(SENSOR_BOOT);
        osEventUnsubscribe(T(tid), EVT_APP_START);
#ifdef GYRO_CAL_ENABLED
        osEventSubscribe(T(tid), EVT_SENSOR_MAG_DATA_RDY);
#endif
        // fall through
    case EVT_SPI_DONE:
        handleSpiDoneEvt(evtData);
        break;
    case EVT_SENSOR_INTERRUPT_1:
        int1Evt(_task, false);
        break;
#ifdef GYRO_CAL_ENABLED
    case EVT_SENSOR_MAG_DATA_RDY:
        if (evtData != SENSOR_DATA_EVENT_FLUSH) {
            processMagEvents(_task, (const struct TripleAxisDataEvent *)evtData);
        }
        break;
#endif
    default:
        break;
    }
}

static void initSensorStruct(struct ICM40600Sensor *sensor, enum SensorIndex idx)
{
    sensor->data_evt = NULL;
    sensor->rate = 0;
    sensor->latency = 0;
    sensor->decimator = 1;
    sensor->data_cnt = 0;
    sensor->prev_rtc_time = 0;
    sensor->skip_sample_cnt = 0;
    sensor->data[0] = 0;
    sensor->data[1] = 0;
    sensor->data[2] = 0;
    sensor->offset[0] = 0;
    sensor->offset[1] = 0;
    sensor->offset[2] = 0;
    sensor->updated = false;
    sensor->powered = false;
    sensor->configed = false;
    sensor->wait_for_odr = false;
    sensor->idx = idx;
    sensor->flush = 0;
}

static bool startTask(uint32_t task_id)
{
    TDECL();
    enum SensorIndex i;
    size_t slabSize;

    time_init(_task);

    T(tid) = task_id;

    T(Int1) = gpioRequest(ICM40600_INT1_PIN);
    T(Irq1) = ICM40600_INT1_IRQ;
    T(Isr1).func = icm40600Isr1;
    T(Int1_EN) = false;
    T(pending_int[0]) = false;
    T(pending_flush) = false;
    T(pending_calibration_save) = false;

    T(mode).speed = ICM40600_SPI_DEFAULT_SPEED_HZ;
    T(mode).bitsPerWord = 8;
    T(mode).cpol = SPI_CPOL_IDLE_HI;
    T(mode).cpha = SPI_CPHA_TRAILING_EDGE;
    T(mode).nssChange = true;
    T(mode).format = SPI_FORMAT_MSB_FIRST;
    T(cs) = GPIO_PB(12);
    T(config).accel_rate = 0;
    T(config).gyro_rate = 0;
    T(config).fifo_rate = 0;
    T(config).wom_threshold = 0;
    T(config).fifo_watermark = 0;
    T(config).fifo_sample_size = 0;
    T(config).accel_on = false;
    T(config).gyro_on = false;
    T(config).wom_on = false;
    T(noMotionTimerHandle) = 0;
    T(fifo_count) = 0;
    T(powered) = false;
    T(flush) = false;
    T(data_timestamp) = 0;
    T(chip_time_us) = 0;
    T(last_sync_time) = 0;
    T(last_sync_data_ts) = 0;
    T(chip_timestamp) = 0;
    T(fifo_start_sync) = false;
    T(chip_temperature) = TEMP_OFFSET;

    spiMasterRequest(ICM40600_SPI_BUS_ID, &T(spiDev));

    for (i = 0; i < NUM_OF_SENSOR; i++) {
        initSensorStruct(&T(sensors[i]), i);
        T(sensors[i]).handle = sensorRegister(&mSensorInfo[i], &mSensorOps[i], (void *)i, false);
        T(pending_config[i]) = false;
    }

    osEventSubscribe(T(tid), EVT_APP_START);

#ifdef ACCEL_CAL_ENABLED
    // Init Accel Cal
    accelCalInit(&T(accel_cal),
                 800000000, // Stillness Time in ns (0.8s)
                 5,         // Minimum Sample Number
                 0.00025,   // Threshold
                 15,        // nx bucket count
                 15,        // nxb bucket count
                 15,        // ny bucket count
                 15,        // nyb bucket count
                 15,        // nz bucket count
                 15,        // nzb bucket count
                 15);       // nle bucket count
#endif
#ifdef GYRO_CAL_ENABLED
    // Init Gyro Cal
    gyroCalInit(&T(gyro_cal),
                SEC_TO_NANOS(5.0f),   // Min stillness period = 5.0 seconds
                SEC_TO_NANOS(5.9f),   // Max stillness period = 6.0 seconds (NOTE 1)
                0, 0, 0,              // Initial bias offset calibration
                0,                    // Time stamp of initial bias calibration
                SEC_TO_NANOS(1.5f),   // Analysis window length = 1.5 seconds
                7.5e-5f,              // Gyroscope variance threshold [rad/sec]^2
                1.5e-5f,              // Gyroscope confidence delta [rad/sec]^2
                4.5e-3f,              // Accelerometer variance threshold [m/sec^2]^2
                9.0e-4f,              // Accelerometer confidence delta [m/sec^2]^2
                5.0f,                 // Magnetometer variance threshold [uT]^2
                1.0f,                 // Magnetometer confidence delta [uT]^2
                0.95f,                // Stillness threshold [0,1]
                40.0f * MDEG_TO_RAD,  // Stillness mean variation limit [rad/sec]
                1.5f,                 // Max temperature delta during stillness [C]
                true);                // Gyro calibration enable
    // NOTE 1: This parameter is set to 5.9 seconds to achieve a max stillness
    // period of 6.0 seconds and avoid buffer boundary conditions that could push
    // the max stillness to the next multiple of the analysis window length
    // (i.e., 7.5 seconds).
#endif

    slabSize = sizeof(struct TripleAxisDataEvent) +
               MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint);
    // TODO: need to investigate slab items number
    T(mDataSlab) = slabAllocatorNew(slabSize, 4, 20);
    if (!T(mDataSlab)) {
        ERROR_PRINT("slabAllocatorNew() failed\n");
        return false;
    }
    T(mWbufCnt) = 0;
    T(mRegCnt) = 0;
    T(spiInUse) = false;

    return true;
}

static void endTask(void)
{
    TDECL();

#ifdef ACCEL_CAL_ENABLED
    accelCalDestroy(&T(accel_cal));
#endif
#ifdef GYRO_CAL_ENABLED
    gyroCalDestroy(&T(gyro_cal));
#endif
    slabAllocatorDestroy(T(mDataSlab));
    spiMasterRelease(T(spiDev));

    // disable and release interrupt.
    disableInterrupt(T(Int1), T(Irq1), &T(Isr1));
    gpioRelease(T(Int1));
}

INTERNAL_APP_INIT(ICM40600_APP_ID, ICM40600_APP_VERSION, startTask, endTask, handleEvent);