Home | History | Annotate | Download | only in bosch_bmi160
      1 /*
      2  * Copyright (C) 2016 The Android Open Source Project
      3  *
      4  * Licensed under the Apache License, Version 2.0 (the "License");
      5  * you may not use this file except in compliance with the License.
      6  * You may obtain a copy of the License at
      7  *
      8  *      http://www.apache.org/licenses/LICENSE-2.0
      9  *
     10  * Unless required by applicable law or agreed to in writing, software
     11  * distributed under the License is distributed on an "AS IS" BASIS,
     12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
     13  * See the License for the specific language governing permissions and
     14  * limitations under the License.
     15  */
     16 
     17 #include <algos/time_sync.h>
     18 #include <atomic.h>
     19 #include <cpu/inc/cpuMath.h>
     20 #include <gpio.h>
     21 #include <heap.h>
     22 #include <hostIntf.h>
     23 #include <isr.h>
     24 #include <nanohub_math.h>
     25 #include <nanohubPacket.h>
     26 #include <plat/inc/exti.h>
     27 #include <plat/inc/gpio.h>
     28 #include <plat/inc/syscfg.h>
     29 #include <plat/inc/rtc.h>
     30 #include <printf.h>
     31 #include <sensors.h>
     32 #include <seos.h>
     33 #include <slab.h>
     34 #include <spi.h>
     35 #include <timer.h>
     36 #include <variant/inc/sensType.h>
     37 #include <variant/inc/variant.h>
     38 
     39 #ifdef MAG_SLAVE_PRESENT
     40 #include <algos/mag_cal.h>
     41 #endif
     42 
     43 #ifdef ACCEL_CAL_ENABLED
     44 #include <algos/accel_cal.h>
     45 #endif
     46 
     47 #ifdef GYRO_CAL_ENABLED
     48 // Gyro Cal -- Header.
     49 #include <algos/gyro_cal.h>
     50 #endif
     51 
     52 #include <limits.h>
     53 #include <stdlib.h>
     54 #include <string.h>
     55 
     56 #define INFO_PRINT(fmt, ...) do { \
     57         osLog(LOG_INFO, "%s " fmt, "[BMI160]", ##__VA_ARGS__); \
     58     } while (0);
     59 
     60 #define ERROR_PRINT(fmt, ...) do { \
     61         osLog(LOG_ERROR, "%s " fmt, "[BMI160] ERROR:", ##__VA_ARGS__); \
     62     } while (0);
     63 
     64 #define DEBUG_PRINT(fmt, ...) do { \
     65         if (DBG_ENABLE) {  \
     66             INFO_PRINT(fmt,  ##__VA_ARGS__); \
     67         } \
     68     } while (0);
     69 
     70 #define DEBUG_PRINT_IF(cond, fmt, ...) do { \
     71         if ((cond) && DBG_ENABLE) {  \
     72             INFO_PRINT(fmt,  ##__VA_ARGS__); \
     73         } \
     74     } while (0);
     75 
     76 #define DBG_ENABLE                0
     77 #define DBG_CHUNKED               0
     78 #define DBG_INT                   0
     79 #define DBG_SHALLOW_PARSE         0
     80 #define DBG_STATE                 0
     81 #define DBG_WM_CALC               0
     82 #define TIMESTAMP_DBG             0
     83 
     84 #define BMI160_APP_VERSION 11
     85 
     86 // fixme: to list required definitions for a slave mag
     87 #ifdef USE_BMM150
     88 #include "bosch_bmm150_slave.h"
     89 #elif USE_AK09915
     90 #include "akm_ak09915_slave.h"
     91 #endif
     92 
     93 #define BMI160_APP_ID APP_ID_MAKE(APP_ID_VENDOR_GOOGLE, 2)
     94 
     95 #define BMI160_SPI_WRITE          0x00
     96 #define BMI160_SPI_READ           0x80
     97 
     98 #define BMI160_SPI_BUS_ID         1
     99 #define BMI160_SPI_SPEED_HZ       8000000
    100 #define BMI160_SPI_MODE           3
    101 
    102 #define BMI160_INT_IRQ            EXTI9_5_IRQn
    103 #define BMI160_INT1_PIN           GPIO_PB(6)
    104 #define BMI160_INT2_PIN           GPIO_PB(7)
    105 
    106 #define BMI160_ID                 0xd1
    107 
    108 #define BMI160_REG_ID             0x00
    109 #define BMI160_REG_ERR            0x02
    110 #define BMI160_REG_PMU_STATUS     0x03
    111 #define BMI160_REG_DATA_0         0x04
    112 #define BMI160_REG_DATA_1         0x05
    113 #define BMI160_REG_DATA_14        0x12
    114 #define BMI160_REG_SENSORTIME_0   0x18
    115 #define BMI160_REG_STATUS         0x1b
    116 #define BMI160_REG_INT_STATUS_0   0x1c
    117 #define BMI160_REG_INT_STATUS_1   0x1d
    118 #define BMI160_REG_TEMPERATURE_0  0x20
    119 #define BMI160_REG_TEMPERATURE_1  0x21
    120 #define BMI160_REG_FIFO_LENGTH_0  0x22
    121 #define BMI160_REG_FIFO_DATA      0x24
    122 #define BMI160_REG_ACC_CONF       0x40
    123 #define BMI160_REG_ACC_RANGE      0x41
    124 #define BMI160_REG_GYR_CONF       0x42
    125 #define BMI160_REG_GYR_RANGE      0x43
    126 #define BMI160_REG_MAG_CONF       0x44
    127 #define BMI160_REG_FIFO_DOWNS     0x45
    128 #define BMI160_REG_FIFO_CONFIG_0  0x46
    129 #define BMI160_REG_FIFO_CONFIG_1  0x47
    130 #define BMI160_REG_MAG_IF_0       0x4b
    131 #define BMI160_REG_MAG_IF_1       0x4c
    132 #define BMI160_REG_MAG_IF_2       0x4d
    133 #define BMI160_REG_MAG_IF_3       0x4e
    134 #define BMI160_REG_MAG_IF_4       0x4f
    135 #define BMI160_REG_INT_EN_0       0x50
    136 #define BMI160_REG_INT_EN_1       0x51
    137 #define BMI160_REG_INT_EN_2       0x52
    138 #define BMI160_REG_INT_OUT_CTRL   0x53
    139 #define BMI160_REG_INT_LATCH      0x54
    140 #define BMI160_REG_INT_MAP_0      0x55
    141 #define BMI160_REG_INT_MAP_1      0x56
    142 #define BMI160_REG_INT_MAP_2      0x57
    143 #define BMI160_REG_INT_DATA_0     0x58
    144 #define BMI160_REG_INT_MOTION_0   0x5f
    145 #define BMI160_REG_INT_MOTION_1   0x60
    146 #define BMI160_REG_INT_MOTION_2   0x61
    147 #define BMI160_REG_INT_MOTION_3   0x62
    148 #define BMI160_REG_INT_TAP_0      0x63
    149 #define BMI160_REG_INT_TAP_1      0x64
    150 #define BMI160_REG_INT_FLAT_0     0x67
    151 #define BMI160_REG_INT_FLAT_1     0x68
    152 #define BMI160_REG_PMU_TRIGGER    0x6C
    153 #define BMI160_REG_FOC_CONF       0x69
    154 #define BMI160_REG_CONF           0x6a
    155 #define BMI160_REG_IF_CONF        0x6b
    156 #define BMI160_REG_SELF_TEST      0x6d
    157 #define BMI160_REG_OFFSET_0       0x71
    158 #define BMI160_REG_OFFSET_3       0x74
    159 #define BMI160_REG_OFFSET_6       0x77
    160 #define BMI160_REG_STEP_CNT_0     0x78
    161 #define BMI160_REG_STEP_CONF_0    0x7a
    162 #define BMI160_REG_STEP_CONF_1    0x7b
    163 #define BMI160_REG_CMD            0x7e
    164 #define BMI160_REG_MAGIC          0x7f
    165 
    166 #define INT_STEP        0x01
    167 #define INT_ANY_MOTION  0x04
    168 #define INT_DOUBLE_TAP  0x10
    169 #define INT_SINGLE_TAP  0x20
    170 #define INT_ORIENT      0x40
    171 #define INT_FLAT        0x80
    172 #define INT_HIGH_G_Z    0x04
    173 #define INT_LOW_G       0x08
    174 #define INT_DATA_RDY    0x10
    175 #define INT_FIFO_FULL   0x20
    176 #define INT_FIFO_WM     0x40
    177 #define INT_NO_MOTION   0x80
    178 
    179 #define BMI160_FRAME_HEADER_INVALID  0x80   // mark the end of valid data
    180 #define BMI160_FRAME_HEADER_SKIP     0x81   // not defined by hw, used for skip a byte in buffer
    181 
    182 #define WATERMARK_MIN                1
    183 #define WATERMARK_MAX                200    // must <= 255 (0xff)
    184 
    185 #define WATERMARK_MAX_SENSOR_RATE    400    // Accel and gyro are 400 Hz max
    186 #define WATERMARK_TIME_UNIT_NS       (1000000000ULL/(WATERMARK_MAX_SENSOR_RATE))
    187 
    188 #define gSPI    BMI160_SPI_BUS_ID
    189 
    190 #define ACCL_INT_LINE EXTI_LINE_P6
    191 #define GYR_INT_LINE EXTI_LINE_P7
    192 
    193 #define SPI_WRITE_0(addr, data) spiQueueWrite(addr, data, 2)
    194 #define SPI_WRITE_1(addr, data, delay) spiQueueWrite(addr, data, delay)
    195 #define GET_SPI_WRITE_MACRO(_1,_2,_3,NAME,...) NAME
    196 #define SPI_WRITE(...) GET_SPI_WRITE_MACRO(__VA_ARGS__, SPI_WRITE_1, SPI_WRITE_0)(__VA_ARGS__)
    197 
    198 #define SPI_READ_0(addr, size, buf) spiQueueRead(addr, size, buf, 0)
    199 #define SPI_READ_1(addr, size, buf, delay) spiQueueRead(addr, size, buf, delay)
    200 #define GET_SPI_READ_MACRO(_1,_2,_3,_4,NAME,...) NAME
    201 #define SPI_READ(...) GET_SPI_READ_MACRO(__VA_ARGS__, SPI_READ_1, SPI_READ_0)(__VA_ARGS__)
    202 
    203 #define EVT_SENSOR_ACC_DATA_RDY sensorGetMyEventType(SENS_TYPE_ACCEL)
    204 #define EVT_SENSOR_GYR_DATA_RDY sensorGetMyEventType(SENS_TYPE_GYRO)
    205 #define EVT_SENSOR_MAG_DATA_RDY sensorGetMyEventType(SENS_TYPE_MAG)
    206 #define EVT_SENSOR_STEP sensorGetMyEventType(SENS_TYPE_STEP_DETECT)
    207 #define EVT_SENSOR_NO_MOTION sensorGetMyEventType(SENS_TYPE_NO_MOTION)
    208 #define EVT_SENSOR_ANY_MOTION sensorGetMyEventType(SENS_TYPE_ANY_MOTION)
    209 #define EVT_SENSOR_FLAT sensorGetMyEventType(SENS_TYPE_FLAT)
    210 #define EVT_SENSOR_DOUBLE_TAP sensorGetMyEventType(SENS_TYPE_DOUBLE_TAP)
    211 #define EVT_SENSOR_STEP_COUNTER sensorGetMyEventType(SENS_TYPE_STEP_COUNT)
    212 
    213 #define MAX_NUM_COMMS_EVENT_SAMPLES 15
    214 
    215 #define kScale_acc    0.00239501953f  // ACC_range * 9.81f / 32768.0f;
    216 #define kScale_gyr    0.00053263221f  // GYR_range * M_PI / (180.0f * 32768.0f);
    217 #define kScale_temp   0.001953125f    // temperature in deg C
    218 #define kTempInvalid  -1000.0f
    219 
    220 #define kTimeSyncPeriodNs        100000000ull // sync sensor and RTC time every 100ms
    221 #define kSensorTimerIntervalUs   39ull        // bmi160 clock increaments every 39000ns
    222 
    223 #define kMinRTCTimeIncrementNs   1250000ull // forced min rtc time increment, 1.25ms for 400Hz
    224 #define kMinSensorTimeIncrement  64         // forced min sensortime increment,
    225                                             // 64 = 2.5 msec for 400Hz
    226 
    227 #define ACC_MIN_RATE    5
    228 #define GYR_MIN_RATE    6
    229 #define ACC_MAX_RATE    12
    230 #define GYR_MAX_RATE    13
    231 #define MAG_MAX_RATE    11
    232 #define ACC_MAX_OSR     3
    233 #define GYR_MAX_OSR     4
    234 #define OSR_THRESHOLD   8
    235 
    236 #define MOTION_ODR         7
    237 
    238 #define RETRY_CNT_CALIBRATION 10
    239 #define RETRY_CNT_ID 5
    240 #define RETRY_CNT_MAG 30
    241 
    242 #define SPI_PACKET_SIZE 30
    243 #define FIFO_READ_SIZE  (1024+4)
    244 #define CHUNKED_READ_SIZE (64)
    245 #define BUF_MARGIN 32   // some extra buffer for additional reg RW when a FIFO read happens
    246 #define SPI_BUF_SIZE (FIFO_READ_SIZE + CHUNKED_READ_SIZE + BUF_MARGIN)
    247 
    248 #define ABS(x) (((x) > 0) ? (x) : -(x))
    249 
    250 enum SensorIndex {
    251     FIRST_CONT_SENSOR = 0,
    252     ACC = FIRST_CONT_SENSOR,
    253     GYR,
    254 #ifdef MAG_SLAVE_PRESENT
    255     MAG,
    256 #endif
    257     NUM_CONT_SENSOR,
    258     FIRST_ONESHOT_SENSOR = NUM_CONT_SENSOR,
    259     STEP = FIRST_ONESHOT_SENSOR,
    260     DTAP,
    261     FLAT,
    262     ANYMO,
    263     NOMO,
    264     STEPCNT,
    265     NUM_OF_SENSOR,
    266 };
    267 
    268 enum SensorEvents {
    269     NO_EVT = -1,
    270     EVT_SPI_DONE = EVT_APP_START + 1,
    271     EVT_SENSOR_INTERRUPT_1,
    272     EVT_SENSOR_INTERRUPT_2,
    273     EVT_TIME_SYNC,
    274 };
    275 
    276 enum InitState {
    277     RESET_BMI160,
    278     INIT_BMI160,
    279     INIT_MAG,
    280     INIT_ON_CHANGE_SENSORS,
    281     INIT_DONE,
    282 };
    283 
    284 enum CalibrationState {
    285     CALIBRATION_START,
    286     CALIBRATION_FOC,
    287     CALIBRATION_WAIT_FOC_DONE,
    288     CALIBRATION_SET_OFFSET,
    289     CALIBRATION_DONE,
    290     CALIBRATION_TIMEOUT,
    291 };
    292 
    293 enum AccTestState {
    294     ACC_TEST_START,
    295     ACC_TEST_CONFIG,
    296     ACC_TEST_RUN_0,
    297     ACC_TEST_RUN_1,
    298     ACC_TEST_VERIFY,
    299     ACC_TEST_DONE
    300 };
    301 
    302 enum GyroTestState {
    303     GYRO_TEST_START,
    304     GYRO_TEST_RUN,
    305     GYRO_TEST_VERIFY,
    306     GYRO_TEST_DONE
    307 };
    308 
    309 enum SensorState {
    310     // keep this in sync with getStateName
    311     SENSOR_BOOT,
    312     SENSOR_VERIFY_ID,
    313     SENSOR_INITIALIZING,
    314     SENSOR_IDLE,
    315     SENSOR_POWERING_UP,
    316     SENSOR_POWERING_DOWN,
    317     SENSOR_CONFIG_CHANGING,
    318     SENSOR_INT_1_HANDLING,
    319     SENSOR_INT_2_HANDLING,
    320     SENSOR_CALIBRATING,
    321     SENSOR_TESTING,
    322     SENSOR_STEP_CNT,
    323     SENSOR_TIME_SYNC,
    324     SENSOR_SAVE_CALIBRATION,
    325     SENSOR_NUM_OF_STATE
    326 };
    327 #if DBG_STATE
    328 #define PRI_STATE "s"
    329 static const char * getStateName(int32_t s) {
    330     // keep this in sync with SensorState
    331     static const char* const l[] = {"BOOT", "VERIFY_ID", "INIT", "IDLE", "PWR_UP",
    332             "PWR-DN", "CFG_CHANGE", "INT1", "INT2", "CALIB", "STEP_CNT", "SYNC", "SAVE_CALIB"};
    333     if (s >= 0 && s < SENSOR_NUM_OF_STATE) {
    334         return l[s];
    335     }
    336     return "???";
    337 #else
    338 #define PRI_STATE PRIi32
    339 static int32_t getStateName(int32_t s) {
    340     return s;
    341 #endif
    342 }
    343 
    344 enum MagConfigState {
    345     MAG_SET_START,
    346     MAG_SET_IF,
    347 
    348     // BMM150 only
    349     MAG_SET_REPXY,
    350     MAG_SET_REPZ,
    351     MAG_GET_DIG_X,
    352     MAG_GET_DIG_Y,
    353     MAG_GET_DIG_Z,
    354     MAG_SET_SAVE_DIG,
    355 
    356     MAG_SET_FORCE,
    357     MAG_SET_ADDR,
    358     MAG_SET_DATA,
    359     MAG_SET_DONE,
    360 
    361     MAG_INIT_FAILED
    362 };
    363 
    364 struct ConfigStat {
    365     uint64_t latency;
    366     uint32_t rate;
    367     bool enable;
    368 };
    369 
    370 struct CalibrationData {
    371     struct HostHubRawPacket header;
    372     struct SensorAppEventHeader data_header;
    373     int32_t xBias;
    374     int32_t yBias;
    375     int32_t zBias;
    376 } __attribute__((packed));
    377 
    378 struct TestResultData {
    379     struct HostHubRawPacket header;
    380     struct SensorAppEventHeader data_header;
    381 } __attribute__((packed));
    382 
    383 struct BMI160Sensor {
    384     struct ConfigStat pConfig; // pending config status request
    385     struct TripleAxisDataEvent *data_evt;
    386     uint32_t handle;
    387     uint32_t rate;
    388     uint64_t latency;
    389     uint64_t prev_rtc_time;
    390     uint32_t offset[3];
    391     bool powered; // activate status
    392     bool configed; // configure status
    393     bool offset_enable;
    394     uint8_t flush;
    395     enum SensorIndex idx;
    396 };
    397 
    398 struct BMI160Task {
    399     uint32_t tid;
    400     struct BMI160Sensor sensors[NUM_OF_SENSOR];
    401 
    402 #ifdef GYRO_CAL_ENABLED
    403     // Gyro Cal -- Declaration.
    404     struct gyroCal_t gyro_cal;
    405   #ifdef GYRO_CAL_DBG_ENABLED
    406     // Gyro Cal -- Read out Debug data.
    407     int gyro_debug_state;
    408   #endif
    409 #endif
    410 
    411     // time keeping.
    412     uint64_t last_sensortime;
    413     uint64_t frame_sensortime;
    414     uint64_t prev_frame_time[NUM_CONT_SENSOR];
    415     uint64_t time_delta[NUM_CONT_SENSOR];
    416     uint64_t next_delta[NUM_CONT_SENSOR];
    417     uint64_t tempTime;
    418     uint64_t timesync_rtc_time;
    419 
    420     // spi and interrupt
    421     spi_cs_t cs;
    422     struct SpiMode mode;
    423     struct SpiPacket packets[SPI_PACKET_SIZE];
    424     struct SpiDevice *spiDev;
    425     struct Gpio *Int1;
    426     struct Gpio *Int2;
    427     struct ChainedIsr Isr1;
    428     struct ChainedIsr Isr2;
    429 #ifdef ACCEL_CAL_ENABLED
    430     struct accelCal_t acc;
    431 #endif
    432 #ifdef MAG_SLAVE_PRESENT
    433     struct MagCal moc;
    434 #endif
    435     time_sync_t gSensorTime2RTC;
    436 
    437     float tempCelsius;
    438     float last_charging_bias_x;
    439     uint32_t total_step_cnt;
    440     uint32_t last_step_cnt;
    441     uint32_t poll_generation;
    442     uint32_t active_poll_generation;
    443     uint8_t active_oneshot_sensor_cnt;
    444     uint8_t interrupt_enable_0;
    445     uint8_t interrupt_enable_2;
    446     uint8_t acc_downsample;
    447     uint8_t gyr_downsample;
    448     bool magBiasPosted;
    449     bool magBiasCurrent;
    450     bool fifo_enabled[NUM_CONT_SENSOR];
    451 
    452     // for step count
    453     uint32_t stepCntSamplingTimerHandle;
    454     bool step_cnt_changed;
    455 
    456     // spi buffers
    457     int xferCnt;
    458     uint8_t *dataBuffer;
    459     uint8_t *statusBuffer;
    460     uint8_t *sensorTimeBuffer;
    461     uint8_t *temperatureBuffer;
    462     uint8_t txrxBuffer[SPI_BUF_SIZE];
    463 
    464     // states
    465     volatile uint8_t state;  //task state, type enum SensorState, do NOT change this directly
    466     enum InitState init_state;
    467     enum MagConfigState mag_state;
    468     enum CalibrationState calibration_state;
    469     enum AccTestState acc_test_state;
    470     enum GyroTestState gyro_test_state;
    471 
    472     // for self-test
    473     int16_t accTestX, accTestY, accTestZ;
    474 
    475     // pending configs
    476     bool pending_int[2];
    477     bool pending_step_cnt;
    478     bool pending_config[NUM_OF_SENSOR];
    479     bool pending_calibration_save;
    480     bool pending_time_sync;
    481     bool pending_delta[NUM_CONT_SENSOR];
    482     bool pending_dispatch;
    483     bool frame_sensortime_valid;
    484 
    485     // FIFO setting
    486     uint16_t chunkReadSize;
    487     uint8_t  watermark;
    488 
    489     // spi rw
    490     struct SlabAllocator *mDataSlab;
    491     uint16_t mWbufCnt;
    492     uint8_t mRegCnt;
    493     uint8_t mRetryLeft;
    494     bool spiInUse;
    495 };
    496 
    497 static uint32_t AccRates[] = {
    498     SENSOR_HZ(25.0f/8.0f),
    499     SENSOR_HZ(25.0f/4.0f),
    500     SENSOR_HZ(25.0f/2.0f),
    501     SENSOR_HZ(25.0f),
    502     SENSOR_HZ(50.0f),
    503     SENSOR_HZ(100.0f),
    504     SENSOR_HZ(200.0f),
    505     SENSOR_HZ(400.0f),
    506     0,
    507 };
    508 
    509 static uint32_t GyrRates[] = {
    510     SENSOR_HZ(25.0f/8.0f),
    511     SENSOR_HZ(25.0f/4.0f),
    512     SENSOR_HZ(25.0f/2.0f),
    513     SENSOR_HZ(25.0f),
    514     SENSOR_HZ(50.0f),
    515     SENSOR_HZ(100.0f),
    516     SENSOR_HZ(200.0f),
    517     SENSOR_HZ(400.0f),
    518     0,
    519 };
    520 
    521 #ifdef MAG_SLAVE_PRESENT
    522 static uint32_t MagRates[] = {
    523     SENSOR_HZ(25.0f/8.0f),
    524     SENSOR_HZ(25.0f/4.0f),
    525     SENSOR_HZ(25.0f/2.0f),
    526     SENSOR_HZ(25.0f),
    527     SENSOR_HZ(50.0f),
    528     SENSOR_HZ(100.0f),
    529     0,
    530 };
    531 #endif
    532 
    533 static uint32_t StepCntRates[] = {
    534     SENSOR_HZ(1.0f/300.0f),
    535     SENSOR_HZ(1.0f/240.0f),
    536     SENSOR_HZ(1.0f/180.0f),
    537     SENSOR_HZ(1.0f/120.0f),
    538     SENSOR_HZ(1.0f/90.0f),
    539     SENSOR_HZ(1.0f/60.0f),
    540     SENSOR_HZ(1.0f/45.0f),
    541     SENSOR_HZ(1.0f/30.0f),
    542     SENSOR_HZ(1.0f/15.0f),
    543     SENSOR_HZ(1.0f/10.0f),
    544     SENSOR_HZ(1.0f/5.0f),
    545     SENSOR_RATE_ONCHANGE,
    546     0
    547 };
    548 
    549 static const uint64_t stepCntRateTimerVals[] = // should match StepCntRates and be the timer length for that rate in nanosecs
    550 {
    551     300 * 1000000000ULL,
    552     240 * 1000000000ULL,
    553     180 * 1000000000ULL,
    554     120 * 1000000000ULL,
    555     90 * 1000000000ULL,
    556     60 * 1000000000ULL,
    557     45 * 1000000000ULL,
    558     30 * 1000000000ULL,
    559     15 * 1000000000ULL,
    560     10 * 1000000000ULL,
    561     5 * 1000000000ULL,
    562 };
    563 
    564 static struct BMI160Task mTask;
    565 
    566 #ifdef MAG_SLAVE_PRESENT
    567 static struct MagTask magTask;
    568 #endif
    569 
    570 #define MAG_WRITE(addr, data)                                   \
    571     do {                                                        \
    572         SPI_WRITE(BMI160_REG_MAG_IF_4, data);                   \
    573         SPI_WRITE(BMI160_REG_MAG_IF_3, addr);                   \
    574     } while (0)
    575 
    576 #define MAG_READ(addr, size)                                    \
    577     do {                                                        \
    578         SPI_WRITE(BMI160_REG_MAG_IF_2, addr, 5000);             \
    579         SPI_READ(BMI160_REG_DATA_0, size, &mTask.dataBuffer);   \
    580     } while (0)
    581 
    582 #define DEC_INFO(name, type, axis, inter, samples) \
    583     .sensorName = name, \
    584     .sensorType = type, \
    585     .numAxis = axis, \
    586     .interrupt = inter, \
    587     .minSamples = samples
    588 
    589 #define DEC_INFO_RATE(name, rates, type, axis, inter, samples) \
    590     DEC_INFO(name, type, axis, inter, samples), \
    591     .supportedRates = rates
    592 
    593 #define DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale) \
    594     DEC_INFO(name, type, axis, inter, samples), \
    595     .supportedRates = rates, \
    596     .flags1 = SENSOR_INFO_FLAGS1_RAW, \
    597     .rawType = raw, \
    598     .rawScale = scale
    599 
    600 #define DEC_INFO_RATE_BIAS(name, rates, type, axis, inter, samples, bias) \
    601     DEC_INFO(name, type, axis, inter, samples), \
    602     .supportedRates = rates, \
    603     .flags1 = SENSOR_INFO_FLAGS1_BIAS, \
    604     .biasType = bias
    605 
    606 #define DEC_INFO_RATE_RAW_BIAS(name, rates, type, axis, inter, samples, raw, scale, bias) \
    607     DEC_INFO_RATE_RAW(name, rates, type, axis, inter, samples, raw, scale), \
    608     .flags1 = SENSOR_INFO_FLAGS1_RAW | SENSOR_INFO_FLAGS1_BIAS, \
    609     .biasType = bias
    610 
    611 typedef struct BMI160Task _Task;
    612 #define TASK  _Task* const _task
    613 
    614 // To get rid of static variables all task functions should have a task structure pointer input.
    615 // This is an intermediate step.
    616 #define TDECL()  TASK = &mTask; (void)_task
    617 
    618 // Access task variables without explicitly specify the task structure pointer.
    619 #define T(v)  (_task->v)
    620 
    621 // Atomic get state
    622 #define GET_STATE() (atomicReadByte(&(_task->state)))
    623 
    624 // Atomic set state, this set the state to arbitrary value, use with caution
    625 #define SET_STATE(s) do{\
    626         DEBUG_PRINT_IF(DBG_STATE, "set state %" PRI_STATE "\n", getStateName(s));\
    627         atomicWriteByte(&(_task->state), (s));\
    628     }while(0)
    629 
    630 // Atomic switch state from IDLE to desired state.
    631 static bool trySwitchState_(TASK, enum SensorState newState) {
    632 #if DBG_STATE
    633     bool ret = atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState);
    634     uint8_t prevState = ret ? SENSOR_IDLE : GET_STATE();
    635     DEBUG_PRINT("switch state %" PRI_STATE "->%" PRI_STATE ", %s\n",
    636             getStateName(prevState), getStateName(newState), ret ? "ok" : "failed");
    637     return ret;
    638 #else
    639     return atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState);
    640 #endif
    641 }
    642 // Short-hand
    643 #define trySwitchState(s) trySwitchState_(_task, (s))
    644 
    645 // Chunked FIFO read functions
    646 static void chunkedReadInit_(TASK, int index, int size);
    647 #define chunkedReadInit(a,b) chunkedReadInit_(_task, (a), (b))
    648 static void chunkedReadSpiCallback(void *cookie, int error);
    649 static void initiateFifoRead_(TASK, bool isInterruptContext);
    650 #define initiateFifoRead(a) initiateFifoRead_(_task, (a))
    651 static uint8_t* shallowParseFrame(uint8_t * buf, int size);
    652 
    653 // Binary dump to osLog
    654 static void dumpBinary(void* buf, unsigned int address, size_t size);
    655 
    656 // Watermark calculation
    657 static uint8_t calcWatermark2_(TASK);
    658 #define calcWatermark2() calcWatermark2_(_task)
    659 
    660 static const struct SensorInfo mSensorInfo[NUM_OF_SENSOR] =
    661 {
    662 #ifdef ACCEL_CAL_ENABLED
    663     { DEC_INFO_RATE_RAW_BIAS("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
    664             NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc,
    665             SENS_TYPE_ACCEL_BIAS) },
    666 #else
    667     { DEC_INFO_RATE_RAW("Accelerometer", AccRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE,
    668             NANOHUB_INT_NONWAKEUP, 3000, SENS_TYPE_ACCEL_RAW, 1.0/kScale_acc) },
    669 #endif
    670     { DEC_INFO_RATE_BIAS("Gyroscope", GyrRates, SENS_TYPE_GYRO, NUM_AXIS_THREE,
    671             NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS) },
    672 #ifdef MAG_SLAVE_PRESENT
    673     { DEC_INFO_RATE_BIAS("Magnetometer", MagRates, SENS_TYPE_MAG, NUM_AXIS_THREE,
    674             NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_BIAS) },
    675 #endif
    676     { DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED,
    677             NANOHUB_INT_NONWAKEUP, 100) },
    678     { DEC_INFO("Double Tap", SENS_TYPE_DOUBLE_TAP, NUM_AXIS_EMBEDDED,
    679             NANOHUB_INT_NONWAKEUP, 20) },
    680     { DEC_INFO("Flat", SENS_TYPE_FLAT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
    681     { DEC_INFO("Any Motion", SENS_TYPE_ANY_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
    682     { DEC_INFO("No Motion", SENS_TYPE_NO_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20) },
    683     { DEC_INFO_RATE("Step Counter", StepCntRates, SENS_TYPE_STEP_COUNT, NUM_AXIS_EMBEDDED,
    684             NANOHUB_INT_NONWAKEUP, 20) },
    685 };
    686 
    687 static void time_init(void) {
    688     time_sync_init(&mTask.gSensorTime2RTC);
    689 }
    690 
    691 static bool sensortime_to_rtc_time(uint64_t sensor_time, uint64_t *rtc_time_ns) {
    692 // fixme: nsec?
    693     return time_sync_estimate_time1(
    694             &mTask.gSensorTime2RTC, sensor_time * 39ull, rtc_time_ns);
    695 }
    696 
    697 static void map_sensortime_to_rtc_time(uint64_t sensor_time, uint64_t rtc_time_ns) {
    698 // fixme: nsec?
    699     time_sync_add(&mTask.gSensorTime2RTC, rtc_time_ns, sensor_time * 39ull);
    700 }
    701 
    702 static void invalidate_sensortime_to_rtc_time(void) {
    703     time_sync_reset(&mTask.gSensorTime2RTC);
    704 }
    705 
    706 static void minimize_sensortime_history(void) {
    707     // truncate datapoints to the latest two to maintain valid sensortime to rtc
    708     // mapping and minimize the inflence of the past mapping
    709     time_sync_truncate(&mTask.gSensorTime2RTC, 2);
    710 
    711     // drop the oldest datapoint when a new one arrives for two times to
    712     // completely shift out the influence of the past mapping
    713     time_sync_hold(&mTask.gSensorTime2RTC, 2);
    714 }
    715 
    716 static void dataEvtFree(void *ptr)
    717 {
    718     TDECL();
    719     struct TripleAxisDataEvent *ev = (struct TripleAxisDataEvent *)ptr;
    720     slabAllocatorFree(T(mDataSlab), ev);
    721 }
    722 
    723 static void spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay)
    724 {
    725     TDECL();
    726     if (T(spiInUse)) {
    727         ERROR_PRINT("SPI in use, cannot queue write\n");
    728         return;
    729     }
    730     T(packets[T(mRegCnt)]).size = 2;
    731     T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]);
    732     T(packets[T(mRegCnt)]).rxBuf = &T(txrxBuffer[T(mWbufCnt)]);
    733     T(packets[T(mRegCnt)]).delay = delay * 1000;
    734     T(txrxBuffer[T(mWbufCnt++)]) = BMI160_SPI_WRITE | addr;
    735     T(txrxBuffer[T(mWbufCnt++)]) = data;
    736     T(mRegCnt)++;
    737 }
    738 
    739 /*
    740  * need to be sure size of buf is larger than read size
    741  */
    742 static void spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay)
    743 {
    744     TDECL();
    745     if (T(spiInUse)) {
    746         ERROR_PRINT("SPI in use, cannot queue read %d %d\n", (int)addr, (int)size);
    747         return;
    748     }
    749 
    750     *buf = &T(txrxBuffer[T(mWbufCnt)]);
    751     T(packets[T(mRegCnt)]).size = size + 1; // first byte will not contain valid data
    752     T(packets[T(mRegCnt)]).txBuf = &T(txrxBuffer[T(mWbufCnt)]);
    753     T(packets[T(mRegCnt)]).rxBuf = *buf;
    754     T(packets[T(mRegCnt)]).delay = delay * 1000;
    755     T(txrxBuffer[T(mWbufCnt)++]) = BMI160_SPI_READ | addr;
    756     T(mWbufCnt) += size;
    757     T(mRegCnt)++;
    758 }
    759 
    760 static void spiBatchTxRx(struct SpiMode *mode,
    761         SpiCbkF callback, void *cookie, const char * src)
    762 {
    763     TDECL();
    764     if (T(mWbufCnt) > SPI_BUF_SIZE) {
    765         ERROR_PRINT("NO enough SPI buffer space, dropping transaction.\n");
    766         return;
    767     }
    768     if (T(mRegCnt) > SPI_PACKET_SIZE) {
    769         ERROR_PRINT("spiBatchTxRx too many packets!\n");
    770         return;
    771     }
    772 
    773     T(spiInUse) = true;
    774 
    775     // Reset variables before issuing SPI transaction.
    776     // SPI may finish before spiMasterRxTx finish
    777     uint8_t regCount = T(mRegCnt);
    778     T(mRegCnt) = 0;
    779     T(mWbufCnt) = 0;
    780 
    781     if (spiMasterRxTx(T(spiDev), T(cs), T(packets), regCount, mode, callback, cookie) < 0) {
    782         ERROR_PRINT("spiMasterRxTx failed!\n");
    783     }
    784 }
    785 
    786 
    787 static bool bmi160Isr1(struct ChainedIsr *isr)
    788 {
    789     TASK = container_of(isr, struct BMI160Task, Isr1);
    790 
    791     if (!extiIsPendingGpio(T(Int1))) {
    792         return false;
    793     }
    794     DEBUG_PRINT_IF(DBG_INT, "i1\n");
    795     initiateFifoRead(true /*isInterruptContext*/);
    796     extiClearPendingGpio(T(Int1));
    797     return true;
    798 }
    799 
    800 
    801 static bool bmi160Isr2(struct ChainedIsr *isr)
    802 {
    803     TASK = container_of(isr, struct BMI160Task, Isr2);
    804 
    805     if (!extiIsPendingGpio(T(Int2)))
    806         return false;
    807 
    808     DEBUG_PRINT_IF(DBG_INT, "i2\n");
    809     if (!osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_2, _task, NULL, T(tid)))
    810         ERROR_PRINT("bmi160Isr2: osEnqueuePrivateEvt() failed\n");
    811     extiClearPendingGpio(T(Int2));
    812     return true;
    813 }
    814 
    815 static void sensorSpiCallback(void *cookie, int err)
    816 {
    817     mTask.spiInUse = false;
    818 
    819     if (!osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, mTask.tid))
    820         ERROR_PRINT("sensorSpiCallback: osEnqueuePrivateEvt() failed\n");
    821 }
    822 
    823 static void sensorTimerCallback(uint32_t timerId, void *data)
    824 {
    825     if (!osEnqueuePrivateEvt(EVT_SPI_DONE, data, NULL, mTask.tid))
    826         ERROR_PRINT("sensorTimerCallback: osEnqueuePrivateEvt() failed\n")
    827 }
    828 
    829 static void timeSyncCallback(uint32_t timerId, void *data)
    830 {
    831     if (!osEnqueuePrivateEvt(EVT_TIME_SYNC, data, NULL, mTask.tid))
    832         ERROR_PRINT("timeSyncCallback: osEnqueuePrivateEvt() failed\n");
    833 }
    834 
    835 static void stepCntSamplingCallback(uint32_t timerId, void *data)
    836 {
    837     union EmbeddedDataPoint step_cnt;
    838 
    839     if (mTask.sensors[STEPCNT].powered && mTask.step_cnt_changed) {
    840         mTask.step_cnt_changed = false;
    841         step_cnt.idata = mTask.total_step_cnt;
    842         osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, step_cnt.vptr, NULL);
    843     }
    844 }
    845 
    846 static bool accFirmwareUpload(void *cookie)
    847 {
    848     sensorSignalInternalEvt(mTask.sensors[ACC].handle,
    849             SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
    850     return true;
    851 }
    852 
    853 static bool gyrFirmwareUpload(void *cookie)
    854 {
    855     sensorSignalInternalEvt(mTask.sensors[GYR].handle,
    856             SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
    857     return true;
    858 }
    859 
    860 #ifdef MAG_SLAVE_PRESENT
    861 static bool magFirmwareUpload(void *cookie)
    862 {
    863     sensorSignalInternalEvt(mTask.sensors[MAG].handle,
    864             SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
    865     return true;
    866 }
    867 #endif
    868 
    869 static bool stepFirmwareUpload(void *cookie)
    870 {
    871     sensorSignalInternalEvt(mTask.sensors[STEP].handle,
    872             SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
    873     return true;
    874 }
    875 
    876 static bool doubleTapFirmwareUpload(void *cookie)
    877 {
    878     sensorSignalInternalEvt(mTask.sensors[DTAP].handle,
    879             SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
    880     return true;
    881 }
    882 
    883 static bool noMotionFirmwareUpload(void *cookie)
    884 {
    885     sensorSignalInternalEvt(mTask.sensors[NOMO].handle,
    886             SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
    887     return true;
    888 }
    889 
    890 static bool anyMotionFirmwareUpload(void *cookie)
    891 {
    892     sensorSignalInternalEvt(mTask.sensors[ANYMO].handle,
    893             SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
    894     return true;
    895 }
    896 
    897 static bool flatFirmwareUpload(void *cookie)
    898 {
    899     sensorSignalInternalEvt(mTask.sensors[FLAT].handle,
    900             SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
    901     return true;
    902 }
    903 
    904 static bool stepCntFirmwareUpload(void *cookie)
    905 {
    906     sensorSignalInternalEvt(mTask.sensors[STEPCNT].handle,
    907             SENSOR_INTERNAL_EVT_FW_STATE_CHG, 1, 0);
    908     return true;
    909 }
    910 
    911 static bool enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
    912 {
    913     gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
    914     syscfgSetExtiPort(pin);
    915     extiEnableIntGpio(pin, EXTI_TRIGGER_RISING);
    916     extiChainIsr(BMI160_INT_IRQ, isr);
    917     return true;
    918 }
    919 
    920 static bool disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
    921 {
    922     extiUnchainIsr(BMI160_INT_IRQ, isr);
    923     extiDisableIntGpio(pin);
    924     return true;
    925 }
    926 
    927 static void magConfigMagic(void)
    928 {
    929     // set the MAG power to NORMAL mode
    930     SPI_WRITE(BMI160_REG_CMD, 0x19, 10000);
    931 
    932     // Magic register sequence to shift register page table to access hidden
    933     // register
    934     SPI_WRITE(BMI160_REG_CMD, 0x37);
    935     SPI_WRITE(BMI160_REG_CMD, 0x9a);
    936     SPI_WRITE(BMI160_REG_CMD, 0xc0);
    937     SPI_WRITE(BMI160_REG_MAGIC, 0x90);
    938     SPI_READ(BMI160_REG_DATA_1, 1, &mTask.dataBuffer);
    939 }
    940 
    941 static void magConfigIf(void)
    942 {
    943     // Set the on-chip I2C pull-up register settings and shift the register
    944     // table back down (magic)
    945     SPI_WRITE(BMI160_REG_DATA_1, mTask.dataBuffer[1] | 0x30);
    946     SPI_WRITE(BMI160_REG_MAGIC, 0x80);
    947 
    948     // Config the MAG I2C device address
    949 #ifdef MAG_SLAVE_PRESENT
    950     SPI_WRITE(BMI160_REG_MAG_IF_0, (MAG_I2C_ADDR << 1));
    951 #endif
    952 
    953     // set mag_manual_enable, mag_offset=0, mag_rd_burst='8 bytes'
    954     SPI_WRITE(BMI160_REG_MAG_IF_1, 0x83);
    955 
    956     // primary interface: autoconfig, secondary: magnetometer.
    957     SPI_WRITE(BMI160_REG_IF_CONF, 0x20);
    958 
    959     // fixme: move to mag-specific function
    960 #ifdef USE_BMM150
    961     // set mag to SLEEP mode
    962     MAG_WRITE(BMM150_REG_CTRL_1, 0x01);
    963 #elif USE_AK09915
    964     // set "low" Noise Suppression Filter (NSF) settings
    965     MAG_WRITE(AKM_AK09915_REG_CNTL1, 0x20);
    966 #endif
    967 }
    968 
    969 // fixme: break this up to master/slave-specific, so it'll be eventually slave-agnostic,
    970 // and slave provides its own stateless config function
    971 // fixme: not all async_elem_t is supported
    972 static void magConfig(void)
    973 {
    974     switch (mTask.mag_state) {
    975     case MAG_SET_START:
    976         magConfigMagic();
    977         mTask.mag_state = MAG_SET_IF;
    978         break;
    979     case MAG_SET_IF:
    980         magConfigIf();
    981 #ifdef USE_AK09915
    982         mTask.mag_state = MAG_SET_FORCE;
    983 #elif USE_BMM150
    984         mTask.mag_state = MAG_SET_REPXY;
    985 #endif
    986         break;
    987 
    988 #ifdef USE_BMM150
    989     case MAG_SET_REPXY:
    990         // MAG_SET_REPXY and MAG_SET_REPZ case set:
    991         // regular preset, f_max,ODR ~ 102 Hz
    992         MAG_WRITE(BMM150_REG_REPXY, 9);
    993         mTask.mag_state = MAG_SET_REPZ;
    994         break;
    995     case MAG_SET_REPZ:
    996         MAG_WRITE(BMM150_REG_REPZ, 15);
    997         mTask.mag_state = MAG_GET_DIG_X;
    998         break;
    999     case MAG_GET_DIG_X:
   1000         // MAG_GET_DIG_X, MAG_GET_DIG_Y and MAG_GET_DIG_Z cases:
   1001         // save parameters for temperature compensation.
   1002         MAG_READ(BMM150_REG_DIG_X1, 8);
   1003         mTask.mag_state = MAG_GET_DIG_Y;
   1004         break;
   1005     case MAG_GET_DIG_Y:
   1006         bmm150SaveDigData(&magTask, &mTask.dataBuffer[1], 0);
   1007         MAG_READ(BMM150_REG_DIG_X1 + 8, 8);
   1008         mTask.mag_state = MAG_GET_DIG_Z;
   1009         break;
   1010     case MAG_GET_DIG_Z:
   1011         bmm150SaveDigData(&magTask, &mTask.dataBuffer[1], 8);
   1012         MAG_READ(BMM150_REG_DIG_X1 + 16, 8);
   1013         mTask.mag_state = MAG_SET_SAVE_DIG;
   1014         break;
   1015     case MAG_SET_SAVE_DIG:
   1016         bmm150SaveDigData(&magTask, &mTask.dataBuffer[1], 16);
   1017         // fall through, no break;
   1018         mTask.mag_state = MAG_SET_FORCE;
   1019 #endif
   1020 
   1021     case MAG_SET_FORCE:
   1022         // set MAG mode to "forced". ready to pull data
   1023 #ifdef USE_AK09915
   1024         MAG_WRITE(AKM_AK09915_REG_CNTL2, 0x01);
   1025 #elif USE_BMM150
   1026         MAG_WRITE(BMM150_REG_CTRL_2, 0x02);
   1027 #endif
   1028         mTask.mag_state = MAG_SET_ADDR;
   1029         break;
   1030     case MAG_SET_ADDR:
   1031         // config MAG read data address to the first data register
   1032 #ifdef MAG_SLAVE_PRESENT
   1033         SPI_WRITE(BMI160_REG_MAG_IF_2, MAG_REG_DATA);
   1034 #endif
   1035         mTask.mag_state = MAG_SET_DATA;
   1036         break;
   1037     case MAG_SET_DATA:
   1038         // clear mag_manual_en.
   1039         SPI_WRITE(BMI160_REG_MAG_IF_1, 0x03, 1000);
   1040         // set the MAG power to SUSPEND mode
   1041         SPI_WRITE(BMI160_REG_CMD, 0x18, 10000);
   1042         mTask.mag_state = MAG_SET_DONE;
   1043         mTask.init_state = INIT_ON_CHANGE_SENSORS;
   1044         break;
   1045     default:
   1046         break;
   1047     }
   1048     SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 1000);
   1049 }
   1050 
   1051 static bool flushData(struct BMI160Sensor *sensor, uint32_t eventId)
   1052 {
   1053     bool success = false;
   1054 
   1055     if (sensor->data_evt) {
   1056         success = osEnqueueEvtOrFree(eventId, sensor->data_evt, dataEvtFree);
   1057         sensor->data_evt = NULL;
   1058     }
   1059 
   1060     return success;
   1061 }
   1062 
   1063 static void flushAllData(void)
   1064 {
   1065     int i;
   1066     for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; i++) {
   1067         flushData(&mTask.sensors[i],
   1068                 EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[i].sensorType));
   1069     }
   1070 }
   1071 
   1072 static bool allocateDataEvt(struct BMI160Sensor *mSensor, uint64_t rtc_time)
   1073 {
   1074     TDECL();
   1075     mSensor->data_evt = slabAllocatorAlloc(T(mDataSlab));
   1076     if (mSensor->data_evt == NULL) {
   1077         // slab allocation failed
   1078         ERROR_PRINT("slabAllocatorAlloc() failed\n");
   1079         return false;
   1080     }
   1081 
   1082     // delta time for the first sample is sample count
   1083     memset(&mSensor->data_evt->samples[0].firstSample, 0x00, sizeof(struct SensorFirstSample));
   1084     mSensor->data_evt->referenceTime = rtc_time;
   1085     mSensor->prev_rtc_time = rtc_time;
   1086 
   1087     return true;
   1088 }
   1089 
   1090 static inline bool anyFifoEnabled(void)
   1091 {
   1092     bool anyFifoEnabled = mTask.fifo_enabled[ACC] || mTask.fifo_enabled[GYR];
   1093 #ifdef MAG_SLAVE_PRESENT
   1094     anyFifoEnabled = anyFifoEnabled || mTask.fifo_enabled[MAG];
   1095 #endif
   1096     return anyFifoEnabled;
   1097 }
   1098 
   1099 static void configFifo(void)
   1100 {
   1101     TDECL();
   1102     int i;
   1103     uint8_t val = 0x12;
   1104     bool any_fifo_enabled_prev = anyFifoEnabled();
   1105 #ifdef ACCEL_CAL_ENABLED
   1106     struct BMI160Sensor *mSensorAcc;
   1107     bool accelCalNewBiasAvailable;
   1108     struct TripleAxisDataPoint *sample;
   1109     float accelCalBiasX, accelCalBiasY, accelCalBiasZ;
   1110     bool fallThrough;
   1111 #endif
   1112 
   1113     // if ACC is configed, enable ACC bit in fifo_config reg.
   1114     if (mTask.sensors[ACC].configed && mTask.sensors[ACC].latency != SENSOR_LATENCY_NODATA) {
   1115         val |= 0x40;
   1116         mTask.fifo_enabled[ACC] = true;
   1117     } else {
   1118         mTask.fifo_enabled[ACC] = false;
   1119 #ifdef ACCEL_CAL_ENABLED
   1120         // https://source.android.com/devices/sensors/sensor-types.html
   1121         // "The bias and scale calibration must only be updated while the sensor is deactivated,
   1122         // so as to avoid causing jumps in values during streaming."
   1123         accelCalNewBiasAvailable = accelCalUpdateBias(&mTask.acc, &accelCalBiasX, &accelCalBiasY, &accelCalBiasZ);
   1124 
   1125         mSensorAcc = &mTask.sensors[ACC];
   1126         // notify HAL about new accel bias calibration
   1127         if (accelCalNewBiasAvailable) {
   1128             fallThrough = true;
   1129             if (mSensorAcc->data_evt->samples[0].firstSample.numSamples > 0) {
   1130                 // flush existing samples so the bias appears after them
   1131                 flushData(mSensorAcc,
   1132                         EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[ACC].sensorType));
   1133 
   1134                 // try to allocate another data event and break if unsuccessful
   1135                 if (!allocateDataEvt(mSensorAcc, sensorGetTime())) {
   1136                     fallThrough = false;
   1137                 }
   1138             }
   1139 
   1140             if (fallThrough) {
   1141                 mSensorAcc->data_evt->samples[0].firstSample.biasCurrent = true;
   1142                 mSensorAcc->data_evt->samples[0].firstSample.biasPresent = 1;
   1143                 mSensorAcc->data_evt->samples[0].firstSample.biasSample =
   1144                         mSensorAcc->data_evt->samples[0].firstSample.numSamples;
   1145                 sample = &mSensorAcc->data_evt->samples[mSensorAcc->data_evt->samples[0].firstSample.numSamples++];
   1146                 sample->x = accelCalBiasX;
   1147                 sample->y = accelCalBiasY;
   1148                 sample->z = accelCalBiasZ;
   1149                 flushData(mSensorAcc, sensorGetMyEventType(mSensorInfo[ACC].biasType));
   1150 
   1151                 allocateDataEvt(mSensorAcc, sensorGetTime());
   1152             }
   1153         }
   1154 #endif
   1155     }
   1156 
   1157     // if GYR is configed, enable GYR bit in fifo_config reg.
   1158     if (mTask.sensors[GYR].configed && mTask.sensors[GYR].latency != SENSOR_LATENCY_NODATA) {
   1159         val |= 0x80;
   1160         mTask.fifo_enabled[GYR] = true;
   1161     } else {
   1162         mTask.fifo_enabled[GYR] = false;
   1163     }
   1164 
   1165 #ifdef MAG_SLAVE_PRESENT
   1166     // if MAG is configed, enable MAG bit in fifo_config reg.
   1167     if (mTask.sensors[MAG].configed && mTask.sensors[MAG].latency != SENSOR_LATENCY_NODATA) {
   1168         val |= 0x20;
   1169         mTask.fifo_enabled[MAG] = true;
   1170     } else {
   1171         mTask.fifo_enabled[MAG] = false;
   1172     }
   1173 #endif
   1174 
   1175     // if this is the first data sensor fifo to enable, start to
   1176     // sync the sensor time and rtc time
   1177     if (!any_fifo_enabled_prev && anyFifoEnabled()) {
   1178         invalidate_sensortime_to_rtc_time();
   1179 
   1180         // start a new poll generation and attach the generation number to event
   1181         if (!osEnqueuePrivateEvt(EVT_TIME_SYNC, (void *)mTask.poll_generation, NULL, mTask.tid))
   1182             ERROR_PRINT("configFifo: osEnqueuePrivateEvt() failed\n");
   1183     }
   1184 
   1185     // cancel current poll generation
   1186     if (any_fifo_enabled_prev && !anyFifoEnabled()) {
   1187         ++mTask.poll_generation;
   1188     }
   1189 
   1190     // if this is not the first fifo enabled or last fifo disabled, flush all fifo data;
   1191     if (any_fifo_enabled_prev && anyFifoEnabled()) {
   1192         mTask.pending_dispatch = true;
   1193         mTask.xferCnt = FIFO_READ_SIZE;
   1194         SPI_READ(BMI160_REG_FIFO_DATA, mTask.xferCnt, &mTask.dataBuffer);
   1195     }
   1196 
   1197     // calculate the new watermark level
   1198     if (anyFifoEnabled()) {
   1199         mTask.watermark = calcWatermark2_(_task);
   1200         DEBUG_PRINT("wm=%d", mTask.watermark);
   1201         SPI_WRITE(BMI160_REG_FIFO_CONFIG_0, mTask.watermark);
   1202     }
   1203 
   1204     // config the fifo register
   1205     SPI_WRITE(BMI160_REG_FIFO_CONFIG_1, val);
   1206 
   1207     // if no more fifo enabled, we need to cleanup the fifo and invalidate time
   1208     if (!anyFifoEnabled()) {
   1209         SPI_WRITE(BMI160_REG_CMD, 0xb0);
   1210         mTask.frame_sensortime_valid = false;
   1211         for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; i++) {
   1212             mTask.pending_delta[i] = false;
   1213             mTask.prev_frame_time[i] = ULONG_LONG_MAX;
   1214         }
   1215     }
   1216 }
   1217 
   1218 static bool accPower(bool on, void *cookie)
   1219 {
   1220     TDECL();
   1221 
   1222     INFO_PRINT("accPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
   1223     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
   1224         if (on) {
   1225             // set ACC power mode to NORMAL
   1226             SPI_WRITE(BMI160_REG_CMD, 0x11, 50000);
   1227         } else {
   1228             // set ACC power mode to SUSPEND
   1229             mTask.sensors[ACC].configed = false;
   1230             configFifo();
   1231             SPI_WRITE(BMI160_REG_CMD, 0x10, 5000);
   1232         }
   1233         mTask.sensors[ACC].powered = on;
   1234         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
   1235     } else {
   1236         mTask.pending_config[ACC] = true;
   1237         mTask.sensors[ACC].pConfig.enable = on;
   1238     }
   1239     return true;
   1240 }
   1241 
   1242 static bool gyrPower(bool on, void *cookie)
   1243 {
   1244     TDECL();
   1245     INFO_PRINT("gyrPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
   1246 
   1247     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
   1248         if (on) {
   1249             // set GYR power mode to NORMAL
   1250             SPI_WRITE(BMI160_REG_CMD, 0x15, 50000);
   1251         } else {
   1252             // set GYR power mode to SUSPEND
   1253             mTask.sensors[GYR].configed = false;
   1254             configFifo();
   1255             SPI_WRITE(BMI160_REG_CMD, 0x14, 5000);
   1256         }
   1257 
   1258         if (anyFifoEnabled() && on != mTask.sensors[GYR].powered) {
   1259 #if TIMESTAMP_DBG
   1260             DEBUG_PRINT("minimize_sensortime_history()\n");
   1261 #endif
   1262             minimize_sensortime_history();
   1263         }
   1264 
   1265         mTask.sensors[GYR].powered = on;
   1266         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
   1267     } else {
   1268         mTask.pending_config[GYR] = true;
   1269         mTask.sensors[GYR].pConfig.enable = on;
   1270     }
   1271     return true;
   1272 }
   1273 
   1274 #ifdef MAG_SLAVE_PRESENT
   1275 static bool magPower(bool on, void *cookie)
   1276 {
   1277     TDECL();
   1278     INFO_PRINT("magPower: on=%d, state=%" PRI_STATE "\n", on, getStateName(GET_STATE()));
   1279     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
   1280         if (on) {
   1281             // set MAG power mode to NORMAL
   1282             SPI_WRITE(BMI160_REG_CMD, 0x19, 10000);
   1283         } else {
   1284             // set MAG power mode to SUSPEND
   1285             mTask.sensors[MAG].configed = false;
   1286             configFifo();
   1287             SPI_WRITE(BMI160_REG_CMD, 0x18, 5000);
   1288         }
   1289         mTask.sensors[MAG].powered = on;
   1290         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG], __FUNCTION__);
   1291     } else {
   1292         mTask.pending_config[MAG] = true;
   1293         mTask.sensors[MAG].pConfig.enable = on;
   1294     }
   1295     return true;
   1296 }
   1297 #endif
   1298 
   1299 static bool stepPower(bool on, void *cookie)
   1300 {
   1301     TDECL();
   1302     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
   1303         // if step counter is powered, no need to change actual config of step
   1304         // detector.
   1305         // But we choose to perform one SPI_WRITE anyway to go down the code path
   1306         // to state SENSOR_POWERING_UP/DOWN to update sensor manager.
   1307         if (on) {
   1308             mTask.interrupt_enable_2 |= 0x08;
   1309         } else {
   1310             if (!mTask.sensors[STEPCNT].powered)
   1311                 mTask.interrupt_enable_2 &= ~0x08;
   1312             mTask.sensors[STEP].configed = false;
   1313         }
   1314         mTask.sensors[STEP].powered = on;
   1315         SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
   1316         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEP], __FUNCTION__);
   1317     } else {
   1318         mTask.pending_config[STEP] = true;
   1319         mTask.sensors[STEP].pConfig.enable = on;
   1320     }
   1321     return true;
   1322 }
   1323 
   1324 static bool flatPower(bool on, void *cookie)
   1325 {
   1326     TDECL();
   1327     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
   1328         if (on) {
   1329             mTask.interrupt_enable_0 |= 0x80;
   1330         } else {
   1331             mTask.interrupt_enable_0 &= ~0x80;
   1332             mTask.sensors[FLAT].configed = false;
   1333         }
   1334         mTask.sensors[FLAT].powered = on;
   1335         SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
   1336         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[FLAT], __FUNCTION__);
   1337     } else {
   1338         mTask.pending_config[FLAT] = true;
   1339         mTask.sensors[FLAT].pConfig.enable = on;
   1340     }
   1341     return true;
   1342 }
   1343 
   1344 static bool doubleTapPower(bool on, void *cookie)
   1345 {
   1346     TDECL();
   1347     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
   1348         if (on) {
   1349             mTask.interrupt_enable_0 |= 0x10;
   1350         } else {
   1351             mTask.interrupt_enable_0 &= ~0x10;
   1352             mTask.sensors[DTAP].configed = false;
   1353         }
   1354         mTask.sensors[DTAP].powered = on;
   1355         SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
   1356         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[DTAP], __FUNCTION__);
   1357     } else {
   1358         mTask.pending_config[DTAP] = true;
   1359         mTask.sensors[DTAP].pConfig.enable = on;
   1360     }
   1361     return true;
   1362 }
   1363 
   1364 static bool anyMotionPower(bool on, void *cookie)
   1365 {
   1366     TDECL();
   1367     DEBUG_PRINT("anyMotionPower: on=%d, oneshot_cnt %d, state=%" PRI_STATE "\n",
   1368             on, mTask.active_oneshot_sensor_cnt, getStateName(GET_STATE()));
   1369 
   1370     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
   1371         if (on) {
   1372             mTask.interrupt_enable_0 |= 0x07;
   1373         } else {
   1374             mTask.interrupt_enable_0 &= ~0x07;
   1375             mTask.sensors[ANYMO].configed = false;
   1376         }
   1377         mTask.sensors[ANYMO].powered = on;
   1378         SPI_WRITE(BMI160_REG_INT_EN_0, mTask.interrupt_enable_0, 450);
   1379         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ANYMO], __FUNCTION__);
   1380     } else {
   1381         mTask.pending_config[ANYMO] = true;
   1382         mTask.sensors[ANYMO].pConfig.enable = on;
   1383     }
   1384     return true;
   1385 }
   1386 
   1387 static bool noMotionPower(bool on, void *cookie)
   1388 {
   1389     TDECL();
   1390     DEBUG_PRINT("noMotionPower: on=%d, oneshot_cnt %d, state=%" PRI_STATE "\n",
   1391             on, mTask.active_oneshot_sensor_cnt, getStateName(GET_STATE()));
   1392     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
   1393         if (on) {
   1394             mTask.interrupt_enable_2 |= 0x07;
   1395         } else {
   1396             mTask.interrupt_enable_2 &= ~0x07;
   1397             mTask.sensors[NOMO].configed = false;
   1398         }
   1399         mTask.sensors[NOMO].powered = on;
   1400         SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
   1401         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[NOMO], __FUNCTION__);
   1402     } else {
   1403         mTask.pending_config[NOMO] = true;
   1404         mTask.sensors[NOMO].pConfig.enable = on;
   1405     }
   1406     return true;
   1407 }
   1408 
   1409 static bool stepCntPower(bool on, void *cookie)
   1410 {
   1411     TDECL();
   1412     if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
   1413         if (on) {
   1414             if (!mTask.sensors[STEP].powered) {
   1415                 mTask.interrupt_enable_2 |= 0x08;
   1416                 SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2, 450);
   1417             }
   1418             // set step_cnt_en bit
   1419             SPI_WRITE(BMI160_REG_STEP_CONF_1, 0x08 | 0x03, 1000);
   1420         } else {
   1421             if (mTask.stepCntSamplingTimerHandle) {
   1422                 timTimerCancel(mTask.stepCntSamplingTimerHandle);
   1423                 mTask.stepCntSamplingTimerHandle = 0;
   1424             }
   1425             if (!mTask.sensors[STEP].powered) {
   1426                 mTask.interrupt_enable_2 &= ~0x08;
   1427                 SPI_WRITE(BMI160_REG_INT_EN_2, mTask.interrupt_enable_2);
   1428             }
   1429             // unset step_cnt_en bit
   1430             SPI_WRITE(BMI160_REG_STEP_CONF_1, 0x03);
   1431             mTask.last_step_cnt = 0;
   1432             mTask.sensors[STEPCNT].configed = false;
   1433         }
   1434         mTask.sensors[STEPCNT].powered = on;
   1435         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT], __FUNCTION__);
   1436     } else {
   1437         mTask.pending_config[STEPCNT] = true;
   1438         mTask.sensors[STEPCNT].pConfig.enable = on;
   1439     }
   1440     return true;
   1441 }
   1442 
   1443 static void updateTimeDelta(uint8_t idx, uint8_t odr)
   1444 {
   1445     if (mTask.fifo_enabled[idx]) {
   1446         // wait till control frame to update, if not disabled
   1447         mTask.next_delta[idx] = 1ull << (16 - odr);
   1448         mTask.pending_delta[idx] = true;
   1449     } else {
   1450         mTask.time_delta[idx] = 1ull << (16 - odr);
   1451     }
   1452 }
   1453 
   1454 // compute the register value from sensor rate.
   1455 static uint8_t computeOdr(uint32_t rate)
   1456 {
   1457     uint8_t odr = 0x00;
   1458     switch (rate) {
   1459     // fall through intended to get the correct register value
   1460     case SENSOR_HZ(3200): odr ++;
   1461     case SENSOR_HZ(1600): odr ++;
   1462     case SENSOR_HZ(800): odr ++;
   1463     case SENSOR_HZ(400): odr ++;
   1464     case SENSOR_HZ(200): odr ++;
   1465     case SENSOR_HZ(100): odr ++;
   1466     case SENSOR_HZ(50): odr ++;
   1467     case SENSOR_HZ(25): odr ++;
   1468     case SENSOR_HZ(25.0f/2.0f): odr ++;
   1469     case SENSOR_HZ(25.0f/4.0f): odr ++;
   1470     case SENSOR_HZ(25.0f/8.0f): odr ++;
   1471     case SENSOR_HZ(25.0f/16.0f): odr ++;
   1472     case SENSOR_HZ(25.0f/32.0f): odr ++;
   1473     default:
   1474         return odr;
   1475     }
   1476 }
   1477 
   1478 static void configMotion(uint8_t odr) {
   1479     // motion threshold is element * 15.63mg (for 8g range)
   1480     static const uint8_t motion_thresholds[ACC_MAX_RATE+1] =
   1481         {5, 5, 5, 5, 5, 5, 5, 5, 4, 3, 2, 2, 2};
   1482 
   1483     // set any_motion duration to 1 point
   1484     // set no_motion duration to (3+1)*1.28sec=5.12sec
   1485     SPI_WRITE(BMI160_REG_INT_MOTION_0, 0x03 << 2, 450);
   1486 
   1487     // set any_motion threshold
   1488     SPI_WRITE(BMI160_REG_INT_MOTION_1, motion_thresholds[odr], 450);
   1489 
   1490     // set no_motion threshold
   1491     SPI_WRITE(BMI160_REG_INT_MOTION_2, motion_thresholds[odr], 450);
   1492 }
   1493 
   1494 static bool accSetRate(uint32_t rate, uint64_t latency, void *cookie)
   1495 {
   1496     TDECL();
   1497     int odr, osr = 0;
   1498 
   1499     INFO_PRINT("accSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n", rate, latency,
   1500             getStateName(GET_STATE()));
   1501 
   1502     if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
   1503         odr = computeOdr(rate);
   1504         if (!odr) {
   1505             ERROR_PRINT("invalid acc rate\n");
   1506             return false;
   1507         }
   1508 
   1509         updateTimeDelta(ACC, odr);
   1510 
   1511         // minimum supported rate for ACCEL is 12.5Hz.
   1512         // Anything lower than that shall be acheived by downsampling.
   1513         if (odr < ACC_MIN_RATE) {
   1514             osr = ACC_MIN_RATE - odr;
   1515             odr = ACC_MIN_RATE;
   1516         }
   1517 
   1518         // for high odrs, oversample to reduce hw latency and downsample
   1519         // to get desired odr
   1520         if (odr > OSR_THRESHOLD) {
   1521             osr = (ACC_MAX_OSR + odr) > ACC_MAX_RATE ? (ACC_MAX_RATE - odr) : ACC_MAX_OSR;
   1522             odr += osr;
   1523         }
   1524 
   1525         mTask.sensors[ACC].rate = rate;
   1526         mTask.sensors[ACC].latency = latency;
   1527         mTask.sensors[ACC].configed = true;
   1528         mTask.acc_downsample = osr;
   1529 
   1530         // configure ANY_MOTION and NO_MOTION based on odr
   1531         configMotion(odr);
   1532 
   1533         // set ACC bandwidth parameter to 2 (bits[4:6])
   1534         // set the rate (bits[0:3])
   1535         SPI_WRITE(BMI160_REG_ACC_CONF, 0x20 | odr);
   1536 
   1537         // configure down sampling ratio, 0x88 is to specify we are using
   1538         // filtered samples
   1539         SPI_WRITE(BMI160_REG_FIFO_DOWNS, (mTask.acc_downsample << 4) | mTask.gyr_downsample | 0x88);
   1540 
   1541         // flush the data and configure the fifo
   1542         configFifo();
   1543 
   1544         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
   1545     } else {
   1546         mTask.pending_config[ACC] = true;
   1547         mTask.sensors[ACC].pConfig.enable = 1;
   1548         mTask.sensors[ACC].pConfig.rate = rate;
   1549         mTask.sensors[ACC].pConfig.latency = latency;
   1550     }
   1551     return true;
   1552 }
   1553 
   1554 static bool gyrSetRate(uint32_t rate, uint64_t latency, void *cookie)
   1555 {
   1556     TDECL();
   1557     int odr, osr = 0;
   1558     INFO_PRINT("gyrSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n", rate, latency,
   1559             getStateName(GET_STATE()));
   1560 
   1561     if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
   1562         odr = computeOdr(rate);
   1563         if (!odr) {
   1564             ERROR_PRINT("invalid gyr rate\n");
   1565             return false;
   1566         }
   1567 
   1568         updateTimeDelta(GYR, odr);
   1569 
   1570         // minimum supported rate for GYRO is 25.0Hz.
   1571         // Anything lower than that shall be acheived by downsampling.
   1572         if (odr < GYR_MIN_RATE) {
   1573             osr = GYR_MIN_RATE - odr;
   1574             odr = GYR_MIN_RATE;
   1575         }
   1576 
   1577         // for high odrs, oversample to reduce hw latency and downsample
   1578         // to get desired odr
   1579         if (odr > OSR_THRESHOLD) {
   1580             osr = (GYR_MAX_OSR + odr) > GYR_MAX_RATE ? (GYR_MAX_RATE - odr) : GYR_MAX_OSR;
   1581             odr += osr;
   1582         }
   1583 
   1584         mTask.sensors[GYR].rate = rate;
   1585         mTask.sensors[GYR].latency = latency;
   1586         mTask.sensors[GYR].configed = true;
   1587         mTask.gyr_downsample = osr;
   1588 
   1589         // set GYR bandwidth parameter to 2 (bits[4:6])
   1590         // set the rate (bits[0:3])
   1591         SPI_WRITE(BMI160_REG_GYR_CONF, 0x20 | odr);
   1592 
   1593         // configure down sampling ratio, 0x88 is to specify we are using
   1594         // filtered samples
   1595         SPI_WRITE(BMI160_REG_FIFO_DOWNS, (mTask.acc_downsample << 4) | mTask.gyr_downsample | 0x88);
   1596 
   1597         // flush the data and configure the fifo
   1598         configFifo();
   1599 
   1600         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
   1601     } else {
   1602         mTask.pending_config[GYR] = true;
   1603         mTask.sensors[GYR].pConfig.enable = 1;
   1604         mTask.sensors[GYR].pConfig.rate = rate;
   1605         mTask.sensors[GYR].pConfig.latency = latency;
   1606     }
   1607     return true;
   1608 }
   1609 
   1610 #ifdef MAG_SLAVE_PRESENT
   1611 static bool magSetRate(uint32_t rate, uint64_t latency, void *cookie)
   1612 {
   1613     TDECL();
   1614     int odr;
   1615 
   1616     if (rate == SENSOR_RATE_ONCHANGE)
   1617         rate = SENSOR_HZ(100);
   1618 
   1619     INFO_PRINT("magSetRate: rate=%ld, latency=%lld, state=%" PRI_STATE "\n", rate, latency,
   1620             getStateName(GET_STATE()));
   1621 
   1622     if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
   1623         mTask.sensors[MAG].rate = rate;
   1624         mTask.sensors[MAG].latency = latency;
   1625         mTask.sensors[MAG].configed = true;
   1626 
   1627         odr = computeOdr(rate);
   1628         if (!odr) {
   1629             ERROR_PRINT("invalid mag rate\n");
   1630             return false;
   1631         }
   1632 
   1633         updateTimeDelta(MAG, odr);
   1634 
   1635         odr = odr > MAG_MAX_RATE ? MAG_MAX_RATE : odr;
   1636 
   1637         // set the rate for MAG
   1638         SPI_WRITE(BMI160_REG_MAG_CONF, odr);
   1639 
   1640         // flush the data and configure the fifo
   1641         configFifo();
   1642 
   1643         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[MAG], __FUNCTION__);
   1644     } else {
   1645         mTask.pending_config[MAG] = true;
   1646         mTask.sensors[MAG].pConfig.enable = 1;
   1647         mTask.sensors[MAG].pConfig.rate = rate;
   1648         mTask.sensors[MAG].pConfig.latency = latency;
   1649     }
   1650     return true;
   1651 }
   1652 #endif
   1653 
   1654 static bool stepSetRate(uint32_t rate, uint64_t latency, void *cookie)
   1655 {
   1656     mTask.sensors[STEP].rate = rate;
   1657     mTask.sensors[STEP].latency = latency;
   1658     mTask.sensors[STEP].configed = true;
   1659 
   1660     sensorSignalInternalEvt(mTask.sensors[STEP].handle,
   1661             SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
   1662     return true;
   1663 }
   1664 
   1665 static bool flatSetRate(uint32_t rate, uint64_t latency, void *cookie)
   1666 {
   1667     mTask.sensors[FLAT].rate = rate;
   1668     mTask.sensors[FLAT].latency = latency;
   1669     mTask.sensors[FLAT].configed = true;
   1670 
   1671     sensorSignalInternalEvt(mTask.sensors[FLAT].handle,
   1672             SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
   1673     return true;
   1674 }
   1675 
   1676 static bool doubleTapSetRate(uint32_t rate, uint64_t latency, void *cookie)
   1677 {
   1678     mTask.sensors[DTAP].rate = rate;
   1679     mTask.sensors[DTAP].latency = latency;
   1680     mTask.sensors[DTAP].configed = true;
   1681 
   1682     sensorSignalInternalEvt(mTask.sensors[DTAP].handle,
   1683             SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
   1684     return true;
   1685 }
   1686 
   1687 static bool anyMotionSetRate(uint32_t rate, uint64_t latency, void *cookie)
   1688 {
   1689     mTask.sensors[ANYMO].rate = rate;
   1690     mTask.sensors[ANYMO].latency = latency;
   1691     mTask.sensors[ANYMO].configed = true;
   1692 
   1693     sensorSignalInternalEvt(mTask.sensors[ANYMO].handle,
   1694             SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
   1695 
   1696     return true;
   1697 }
   1698 
   1699 static bool noMotionSetRate(uint32_t rate, uint64_t latency, void *cookie)
   1700 {
   1701     mTask.sensors[NOMO].rate = rate;
   1702     mTask.sensors[NOMO].latency = latency;
   1703     mTask.sensors[NOMO].configed = true;
   1704 
   1705     sensorSignalInternalEvt(mTask.sensors[NOMO].handle,
   1706             SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
   1707     return true;
   1708 }
   1709 
   1710 static bool stepCntSetRate(uint32_t rate, uint64_t latency, void *cookie)
   1711 {
   1712     mTask.sensors[STEPCNT].rate = rate;
   1713     mTask.sensors[STEPCNT].latency = latency;
   1714     mTask.sensors[STEPCNT].configed = true;
   1715 
   1716     if (rate == SENSOR_RATE_ONCHANGE && mTask.stepCntSamplingTimerHandle) {
   1717         timTimerCancel(mTask.stepCntSamplingTimerHandle);
   1718         mTask.stepCntSamplingTimerHandle = 0;
   1719     } else if (rate != SENSOR_RATE_ONCHANGE) {
   1720         if (mTask.stepCntSamplingTimerHandle) {
   1721             timTimerCancel(mTask.stepCntSamplingTimerHandle);
   1722         }
   1723         mTask.stepCntSamplingTimerHandle = timTimerSet(sensorTimerLookupCommon(StepCntRates, stepCntRateTimerVals, rate),
   1724                                                        0, 50, stepCntSamplingCallback, NULL, false);
   1725         if (!mTask.stepCntSamplingTimerHandle)
   1726             ERROR_PRINT("Couldn't get a timer for step counter\n");
   1727 
   1728     }
   1729 
   1730     sensorSignalInternalEvt(mTask.sensors[STEPCNT].handle,
   1731             SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);
   1732     return true;
   1733 }
   1734 
   1735 static void sendFlushEvt(void)
   1736 {
   1737     while (mTask.sensors[ACC].flush > 0) {
   1738         osEnqueueEvt(EVT_SENSOR_ACC_DATA_RDY, SENSOR_DATA_EVENT_FLUSH, NULL);
   1739         mTask.sensors[ACC].flush--;
   1740     }
   1741     while (mTask.sensors[GYR].flush > 0) {
   1742         osEnqueueEvt(EVT_SENSOR_GYR_DATA_RDY, SENSOR_DATA_EVENT_FLUSH, NULL);
   1743         mTask.sensors[GYR].flush--;
   1744     }
   1745 #ifdef MAG_SLAVE_PRESENT
   1746     while (mTask.sensors[MAG].flush > 0) {
   1747         osEnqueueEvt(EVT_SENSOR_MAG_DATA_RDY, SENSOR_DATA_EVENT_FLUSH, NULL);
   1748         mTask.sensors[MAG].flush--;
   1749     }
   1750 #endif
   1751 }
   1752 
   1753 static bool accFlush(void *cookie)
   1754 {
   1755     TDECL();
   1756     mTask.sensors[ACC].flush++;
   1757     initiateFifoRead(false /*isInterruptContext*/);
   1758     return true;
   1759 }
   1760 
   1761 static bool gyrFlush(void *cookie)
   1762 {
   1763     TDECL();
   1764     mTask.sensors[GYR].flush++;
   1765     initiateFifoRead(false /*isInterruptContext*/);
   1766     return true;
   1767 }
   1768 
   1769 #ifdef MAG_SLAVE_PRESENT
   1770 static bool magFlush(void *cookie)
   1771 {
   1772     TDECL();
   1773     mTask.sensors[MAG].flush++;
   1774     initiateFifoRead(false /*isInterruptContext*/);
   1775     return true;
   1776 }
   1777 #endif
   1778 
   1779 static bool stepFlush(void *cookie)
   1780 {
   1781     return osEnqueueEvt(EVT_SENSOR_STEP, SENSOR_DATA_EVENT_FLUSH, NULL);
   1782 }
   1783 
   1784 static bool flatFlush(void *cookie)
   1785 {
   1786     return osEnqueueEvt(EVT_SENSOR_FLAT, SENSOR_DATA_EVENT_FLUSH, NULL);
   1787 }
   1788 
   1789 static bool doubleTapFlush(void *cookie)
   1790 {
   1791     return osEnqueueEvt(EVT_SENSOR_DOUBLE_TAP, SENSOR_DATA_EVENT_FLUSH, NULL);
   1792 }
   1793 
   1794 static bool anyMotionFlush(void *cookie)
   1795 {
   1796     return osEnqueueEvt(EVT_SENSOR_ANY_MOTION, SENSOR_DATA_EVENT_FLUSH, NULL);
   1797 }
   1798 
   1799 static bool noMotionFlush(void *cookie)
   1800 {
   1801     return osEnqueueEvt(EVT_SENSOR_NO_MOTION, SENSOR_DATA_EVENT_FLUSH, NULL);
   1802 }
   1803 
   1804 static bool stepCntFlushGetData()
   1805 {
   1806     TDECL();
   1807     if (trySwitchState(SENSOR_STEP_CNT)) {
   1808         SPI_READ(BMI160_REG_STEP_CNT_0, 2, &mTask.dataBuffer);
   1809         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[STEPCNT], __FUNCTION__);
   1810         return true;
   1811     }
   1812     return false;
   1813 }
   1814 
   1815 static bool stepCntFlush(void *cookie)
   1816 {
   1817     mTask.sensors[STEPCNT].flush++;
   1818     stepCntFlushGetData();
   1819     return true;
   1820 }
   1821 
   1822 static void sendStepCnt()
   1823 {
   1824     union EmbeddedDataPoint step_cnt;
   1825     uint32_t cur_step_cnt;
   1826     cur_step_cnt = (int)(mTask.dataBuffer[1] | (mTask.dataBuffer[2] << 8));
   1827 
   1828     if (cur_step_cnt != mTask.last_step_cnt) {
   1829         // Check for possible overflow
   1830         if (cur_step_cnt < mTask.last_step_cnt) {
   1831             mTask.total_step_cnt += cur_step_cnt + (0xFFFF - mTask.last_step_cnt);
   1832         } else {
   1833             mTask.total_step_cnt += (cur_step_cnt - mTask.last_step_cnt);
   1834         }
   1835         mTask.last_step_cnt = cur_step_cnt;
   1836 
   1837         // Send the event if the current rate is ONCHANGE or we need to flush;
   1838         // otherwise, wait until step count sampling timer expires
   1839         if (mTask.sensors[STEPCNT].rate == SENSOR_RATE_ONCHANGE || mTask.sensors[STEPCNT].flush) {
   1840             step_cnt.idata = mTask.total_step_cnt;
   1841             osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, step_cnt.vptr, NULL);
   1842         } else {
   1843             mTask.step_cnt_changed = true;
   1844         }
   1845     }
   1846 
   1847     while (mTask.sensors[STEPCNT].flush) {
   1848         osEnqueueEvt(EVT_SENSOR_STEP_COUNTER, SENSOR_DATA_EVENT_FLUSH, NULL);
   1849         mTask.sensors[STEPCNT].flush--;
   1850     }
   1851 }
   1852 
   1853 static bool stepCntSendLastData(void *cookie, uint32_t tid)
   1854 {
   1855     // If this comes in and we don't have data yet, there's no harm in reporting step_cnt = 0
   1856     if (!osEnqueuePrivateEvt(EVT_SENSOR_STEP_COUNTER, (void *) mTask.total_step_cnt, NULL, tid)) {
   1857         ERROR_PRINT("stepCntSendLastData: osEnqueuePrivateEvt() failed\n");
   1858         return false;
   1859     }
   1860 
   1861     return true;
   1862 }
   1863 
   1864 static uint64_t parseSensortime(uint32_t sensor_time24)
   1865 {
   1866     uint32_t prev_time24;
   1867     uint32_t kHalf = 1ul << 23;
   1868     uint64_t full;
   1869 
   1870     prev_time24 = (uint32_t)mTask.last_sensortime & 0xffffff;
   1871 
   1872     if (mTask.last_sensortime == 0) {
   1873         mTask.last_sensortime = (uint64_t)sensor_time24;
   1874         return (uint64_t)(sensor_time24);
   1875     }
   1876 
   1877     if (sensor_time24 == prev_time24) {
   1878         return (uint64_t)(mTask.last_sensortime);
   1879     }
   1880 
   1881     full = (mTask.last_sensortime & ~0xffffffull) | sensor_time24;
   1882 
   1883     if (((prev_time24 < sensor_time24) && (sensor_time24 - prev_time24) < kHalf)
   1884             || ((prev_time24 > sensor_time24) && (prev_time24 - sensor_time24) > kHalf)) {
   1885         if (full < mTask.last_sensortime) {
   1886             full += 0x1000000ull;
   1887         }
   1888         mTask.last_sensortime = full;
   1889         return mTask.last_sensortime;
   1890     }
   1891 
   1892     if (full < mTask.last_sensortime) {
   1893         return full;
   1894     }
   1895 
   1896     return (full -  0x1000000ull);
   1897 }
   1898 
   1899 static void parseRawData(struct BMI160Sensor *mSensor, uint8_t *buf, float kScale, uint64_t sensorTime)
   1900 {
   1901     float x, y, z;
   1902     int16_t raw_x, raw_y, raw_z;
   1903     struct TripleAxisDataPoint *sample;
   1904     uint32_t delta_time;
   1905     uint64_t rtc_time;
   1906 #ifdef MAG_SLAVE_PRESENT
   1907     bool newMagBias = false;
   1908 #endif
   1909 
   1910     if (!sensortime_to_rtc_time(sensorTime, &rtc_time)) {
   1911         return;
   1912     }
   1913 
   1914     if (rtc_time < mSensor->prev_rtc_time + kMinRTCTimeIncrementNs) {
   1915 #if TIMESTAMP_DBG
   1916         DEBUG_PRINT("%s prev rtc 0x%08x %08x, curr 0x%08x %08x, delta %d usec\n",
   1917                 mSensorInfo[mSensor->idx].sensorName,
   1918                 (unsigned int)((mSensor->prev_rtc_time >> 32) & 0xffffffff),
   1919                 (unsigned int)(mSensor->prev_rtc_time & 0xffffffff),
   1920                 (unsigned int)((rtc_time >> 32) & 0xffffffff),
   1921                 (unsigned int)(rtc_time & 0xffffffff),
   1922                 (int)(rtc_time - mSensor->prev_rtc_time) / 1000);
   1923 #endif
   1924         rtc_time = mSensor->prev_rtc_time + kMinRTCTimeIncrementNs;
   1925     }
   1926 
   1927 #ifdef MAG_SLAVE_PRESENT
   1928     if (mSensor->idx == MAG) {
   1929         parseMagData(&magTask, &buf[0], &x, &y, &z);
   1930         BMM150_TO_ANDROID_COORDINATE(x, y, z);
   1931 
   1932         float xi, yi, zi;
   1933         magCalRemoveSoftiron(&mTask.moc, x, y, z, &xi, &yi, &zi);
   1934 
   1935         newMagBias |= magCalUpdate(&mTask.moc, sensorTime * kSensorTimerIntervalUs, xi, yi, zi);
   1936 
   1937         magCalRemoveBias(&mTask.moc, xi, yi, zi, &x, &y, &z);
   1938 
   1939 #ifdef GYRO_CAL_ENABLED
   1940         // Gyro Cal -- Add magnetometer sample.
   1941         gyroCalUpdateMag(&mTask.gyro_cal,
   1942                          rtc_time,  // nsec
   1943                          x, y, z);
   1944 #endif
   1945     } else
   1946 #endif
   1947     {
   1948         raw_x = (buf[0] | buf[1] << 8);
   1949         raw_y = (buf[2] | buf[3] << 8);
   1950         raw_z = (buf[4] | buf[5] << 8);
   1951 
   1952         x = (float)raw_x * kScale;
   1953         y = (float)raw_y * kScale;
   1954         z = (float)raw_z * kScale;
   1955 
   1956         BMI160_TO_ANDROID_COORDINATE(x, y, z);
   1957 
   1958         if (mSensor->idx == ACC) {
   1959 
   1960 #ifdef ACCEL_CAL_ENABLED
   1961           accelCalRun(&mTask.acc, rtc_time,
   1962                       x, y, z, mTask.tempCelsius);
   1963 
   1964           accelCalBiasRemove(&mTask.acc, &x, &y, &z);
   1965 #endif
   1966 
   1967 #ifdef GYRO_CAL_ENABLED
   1968           // Gyro Cal -- Add accelerometer sample.
   1969           gyroCalUpdateAccel(&mTask.gyro_cal,
   1970                              rtc_time,  // nsec
   1971                              x, y, z);
   1972 #endif
   1973         } else if (mSensor->idx == GYR) {
   1974 
   1975 #ifdef GYRO_CAL_ENABLED
   1976             // Gyro Cal -- Add gyroscope and temperature sample.
   1977             gyroCalUpdateGyro(&mTask.gyro_cal,
   1978                               rtc_time,  // nsec
   1979                               x, y, z,
   1980                               mTask.tempCelsius);
   1981 
   1982             // Gyro Cal -- Apply calibration correction.
   1983             gyroCalRemoveBias(&mTask.gyro_cal,
   1984                               x, y, z,  /* input values */
   1985                               &x, &y, &z  /* calibrated output */);
   1986 #endif
   1987 
   1988         }
   1989     }
   1990 
   1991     if (mSensor->data_evt == NULL) {
   1992         if (!allocateDataEvt(mSensor, rtc_time)) {
   1993             return;
   1994         }
   1995     }
   1996 
   1997     if (mSensor->data_evt->samples[0].firstSample.numSamples >= MAX_NUM_COMMS_EVENT_SAMPLES) {
   1998         ERROR_PRINT("BAD INDEX\n");
   1999         return;
   2000     }
   2001 
   2002 #ifdef MAG_SLAVE_PRESENT
   2003     if (mSensor->idx == MAG && (newMagBias || !mTask.magBiasPosted)) {
   2004         if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
   2005             // flush existing samples so the bias appears after them
   2006             flushData(mSensor,
   2007                     EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[MAG].sensorType));
   2008             if (!allocateDataEvt(mSensor, rtc_time)) {
   2009                 return;
   2010             }
   2011         }
   2012         if (newMagBias) {
   2013             mTask.magBiasCurrent = true;
   2014         }
   2015         mSensor->data_evt->samples[0].firstSample.biasCurrent = mTask.magBiasCurrent;
   2016         mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
   2017         mSensor->data_evt->samples[0].firstSample.biasSample =
   2018                 mSensor->data_evt->samples[0].firstSample.numSamples;
   2019         sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
   2020         magCalGetBias(&mTask.moc, &sample->x, &sample->y, &sample->z);
   2021         // bias is non-discardable, if we fail to enqueue, don't clear new_mag_bias
   2022         if (flushData(mSensor, sensorGetMyEventType(mSensorInfo[MAG].biasType))) {
   2023             mTask.magBiasPosted = true;
   2024         }
   2025 
   2026         if (!allocateDataEvt(mSensor, rtc_time)) {
   2027             return;
   2028         }
   2029     }
   2030 #endif
   2031 #ifdef GYRO_CAL_ENABLED
   2032     // Gyro Cal -- Notify HAL about new gyro bias calibration
   2033     if (mSensor->idx == GYR && gyroCalNewBiasAvailable(&mTask.gyro_cal)) {
   2034         if (mSensor->data_evt->samples[0].firstSample.numSamples > 0) {
   2035             // flush existing samples so the bias appears after them
   2036             flushData(mSensor,
   2037                     EVENT_TYPE_BIT_DISCARDABLE | sensorGetMyEventType(mSensorInfo[GYR].sensorType));
   2038             if (!allocateDataEvt(mSensor, rtc_time)) {
   2039                 return;
   2040             }
   2041         }
   2042         mSensor->data_evt->samples[0].firstSample.biasCurrent = true;
   2043         mSensor->data_evt->samples[0].firstSample.biasPresent = 1;
   2044         mSensor->data_evt->samples[0].firstSample.biasSample =
   2045                 mSensor->data_evt->samples[0].firstSample.numSamples;
   2046         sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
   2047         gyroCalGetBias(&mTask.gyro_cal, &sample->x, &sample->y, &sample->z);
   2048         flushData(mSensor, sensorGetMyEventType(mSensorInfo[GYR].biasType));
   2049 
   2050         if (!allocateDataEvt(mSensor, rtc_time)) {
   2051             return;
   2052         }
   2053     }
   2054 #endif
   2055 
   2056     sample = &mSensor->data_evt->samples[mSensor->data_evt->samples[0].firstSample.numSamples++];
   2057 
   2058     // the first deltatime is for sample size
   2059     if (mSensor->data_evt->samples[0].firstSample.numSamples > 1) {
   2060         delta_time = rtc_time - mSensor->prev_rtc_time;
   2061         delta_time = delta_time < 0 ? 0 : delta_time;
   2062         sample->deltaTime = delta_time;
   2063         mSensor->prev_rtc_time = rtc_time;
   2064     }
   2065 
   2066     sample->x = x;
   2067     sample->y = y;
   2068     sample->z = z;
   2069 
   2070     //DEBUG_PRINT("bmi160: x: %d, y: %d, z: %d\n", (int)(1000*x), (int)(1000*y), (int)(1000*z));
   2071 
   2072     //TODO: This was added to prevent to much data of the same type accumulate in internal buffer.
   2073     //      It might no longer be necessary and can be removed.
   2074     if (mSensor->data_evt->samples[0].firstSample.numSamples == MAX_NUM_COMMS_EVENT_SAMPLES) {
   2075         flushAllData();
   2076     }
   2077 }
   2078 
   2079 static void dispatchData(void)
   2080 {
   2081     size_t i = 1, j;
   2082     size_t size = mTask.xferCnt;
   2083     int fh_mode, fh_param;
   2084     uint8_t *buf = mTask.dataBuffer;
   2085 
   2086     uint64_t min_delta = ULONG_LONG_MAX;
   2087     uint32_t sensor_time24;
   2088     uint64_t full_sensor_time;
   2089     uint64_t frame_sensor_time = mTask.frame_sensortime;
   2090     bool observed[NUM_CONT_SENSOR];
   2091     uint64_t tmp_frame_time, tmp_time[NUM_CONT_SENSOR];
   2092     bool frame_sensor_time_valid = mTask.frame_sensortime_valid;
   2093     bool saved_pending_delta[NUM_CONT_SENSOR];
   2094     uint64_t saved_time_delta[NUM_CONT_SENSOR];
   2095 #if TIMESTAMP_DBG
   2096     int frame_num = -1;
   2097 #endif
   2098 
   2099     for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++)
   2100         observed[j] = false;
   2101 
   2102     if (!mTask.frame_sensortime_valid) {
   2103         // This is the first FIFO delivery after any sensor is enabled in
   2104         // bmi160. Sensor time reference is not establised until end of this
   2105         // FIFO frame. Assume time start from zero and do a dry run to estimate
   2106         // the time and then go through this FIFO again.
   2107         frame_sensor_time = 0ull;
   2108 
   2109         // Save these states for future recovery by the end of dry run.
   2110         for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
   2111             saved_pending_delta[j] = mTask.pending_delta[j];
   2112             saved_time_delta[j] = mTask.time_delta[j];
   2113         }
   2114     }
   2115 
   2116     while (size > 0) {
   2117         if (buf[i] == BMI160_FRAME_HEADER_INVALID) {
   2118             // reaching invalid header means no more data
   2119             break;
   2120         } else if (buf[i] == BMI160_FRAME_HEADER_SKIP) {
   2121             // manually injected skip header
   2122             DEBUG_PRINT_IF(DBG_CHUNKED, "skip nop header");
   2123             i++;
   2124             size--;
   2125             continue;
   2126         }
   2127 
   2128         fh_mode = buf[i] >> 6;
   2129         fh_param = (buf[i] >> 2) & 0xf;
   2130 
   2131         i++;
   2132         size--;
   2133 #if TIMESTAMP_DBG
   2134         ++frame_num;
   2135 #endif
   2136 
   2137         if (fh_mode == 1) {
   2138             // control frame.
   2139             if (fh_param == 0) {
   2140                 // skip frame, we skip it
   2141                 if (size >= 1) {
   2142                     i++;
   2143                     size--;
   2144                 } else {
   2145                     size = 0;
   2146                 }
   2147             } else if (fh_param == 1) {
   2148                 // sensortime frame
   2149                 if (size >= 3) {
   2150                     // The active sensor with the highest odr/lowest delta is the one that
   2151                     // determines the sensor time increments.
   2152                     for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
   2153                         if (mTask.sensors[j].configed &&
   2154                                 mTask.sensors[j].latency != SENSOR_LATENCY_NODATA) {
   2155                             min_delta = min_delta < mTask.time_delta[j] ? min_delta :
   2156                                     mTask.time_delta[j];
   2157                         }
   2158                     }
   2159                     sensor_time24 = buf[i + 2] << 16 | buf[i + 1] << 8 | buf[i];
   2160 
   2161                     // clear lower bits that measure time from taking the sample to reading the
   2162                     // FIFO, something we're not interested in.
   2163                     sensor_time24 &= ~(min_delta - 1);
   2164 
   2165                     full_sensor_time = parseSensortime(sensor_time24);
   2166 
   2167 #if TIMESTAMP_DBG
   2168                     if (frame_sensor_time == full_sensor_time) {
   2169                         //DEBUG_PRINT("frame %d FrameTime 0x%08x\n",
   2170                         //        frame_num - 1,
   2171                         //        (unsigned int)frame_sensor_time);
   2172                     } else if (frame_sensor_time_valid) {
   2173                         DEBUG_PRINT("frame %d FrameTime 0x%08x != SensorTime 0x%08x, jumped %d msec\n",
   2174                                 frame_num - 1,
   2175                                 (unsigned int)frame_sensor_time,
   2176                                 (unsigned int)full_sensor_time,
   2177                                 (int)(5 * ((int64_t)(full_sensor_time - frame_sensor_time) >> 7)));
   2178                     }
   2179 #endif
   2180 
   2181 
   2182                     if (frame_sensor_time_valid) {
   2183                         mTask.frame_sensortime = full_sensor_time;
   2184                     } else {
   2185                         // Dry run if frame_sensortime_valid == false,
   2186                         // no sample is added this round.
   2187                         // So let's time travel back to beginning of frame.
   2188                         mTask.frame_sensortime_valid = true;
   2189                         mTask.frame_sensortime = full_sensor_time - frame_sensor_time;
   2190 
   2191                         // recover states
   2192                         for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
   2193                             // reset all prev_frame_time to invalid values
   2194                             // they should be so anyway at the first FIFO
   2195                             mTask.prev_frame_time[j] = ULONG_LONG_MAX;
   2196 
   2197                             // recover saved time_delta and pending_delta values
   2198                             mTask.pending_delta[j] = saved_pending_delta[j];
   2199                             mTask.time_delta[j] = saved_time_delta[j];
   2200                         }
   2201 
   2202                         DEBUG_PRINT_IF(TIMESTAMP_DBG,
   2203                                 "sensortime invalid: full, frame, task = %llu, %llu, %llu\n",
   2204                                 full_sensor_time,
   2205                                 frame_sensor_time,
   2206                                 mTask.frame_sensortime);
   2207 
   2208                         // Parse again with known valid timing.
   2209                         // This time the sensor events will be committed into event buffer.
   2210                         return dispatchData();
   2211                     }
   2212 
   2213                     // Invalidate sensor timestamp that didn't get corrected by full_sensor_time,
   2214                     // so it can't be used as a reference at next FIFO read.
   2215                     // Use (ULONG_LONG_MAX - 1) to indicate this.
   2216                     for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
   2217                         mTask.prev_frame_time[j] = observed[j] ? full_sensor_time : (ULONG_LONG_MAX - 1);
   2218 
   2219                         // sensor can be disabled in the middle of the FIFO, but wait till the FIFO
   2220                         // end to invalidate prev_frame_time since it's still needed for parsing.
   2221                         // Also invalidate pending delta just to be safe.
   2222                         if (!mTask.sensors[j].configed ||
   2223                                 mTask.sensors[j].latency == SENSOR_LATENCY_NODATA) {
   2224                             mTask.prev_frame_time[j] = ULONG_LONG_MAX;
   2225                             mTask.pending_delta[j] = false;
   2226                         }
   2227                     }
   2228                     i += 3;
   2229                     size -= 3;
   2230                 } else {
   2231                     size = 0;
   2232                 }
   2233             } else if (fh_param == 2) {
   2234                 // fifo_input config frame
   2235 #if TIMESTAMP_DBG
   2236                 DEBUG_PRINT("frame %d config change 0x%02x\n", frame_num, buf[i]);
   2237 #endif
   2238                 if (size >= 1) {
   2239                     for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
   2240                         if (buf[i] & (0x01 << (j << 1)) && mTask.pending_delta[j]) {
   2241                             mTask.pending_delta[j] = false;
   2242                             mTask.time_delta[j] = mTask.next_delta[j];
   2243 #if TIMESTAMP_DBG
   2244                             DEBUG_PRINT("%s new delta %u\n", mSensorInfo[j].sensorName,
   2245                                     (unsigned int)mTask.time_delta[j]);
   2246 #endif
   2247                         }
   2248                     }
   2249                     i++;
   2250                     size--;
   2251                 } else {
   2252                     size = 0;
   2253                 }
   2254             } else {
   2255                 size = 0; // drop this batch
   2256                 ERROR_PRINT("Invalid fh_param in control frame\n");
   2257             }
   2258         } else if (fh_mode == 2) {
   2259             // Calcutate candidate frame time (tmp_frame_time):
   2260             // 1) When sensor is first enabled, reference from other sensors if possible.
   2261             // Otherwise, add the smallest increment to the previous data frame time.
   2262             // 2) The newly enabled sensor could only underestimate its
   2263             // frame time without reference from other sensors.
   2264             // 3) The underestimated frame time of a newly enabled sensor will be corrected
   2265             // as soon as it shows up in the same frame with another sensor.
   2266             // 4) (prev_frame_time == ULONG_LONG_MAX) means the sensor wasn't enabled.
   2267             // 5) (prev_frame_time == ULONG_LONG_MAX -1) means the sensor didn't appear in the last
   2268             // data frame of the previous fifo read.  So it won't be used as a frame time reference.
   2269 
   2270             tmp_frame_time = 0;
   2271             for (j = FIRST_CONT_SENSOR; j < NUM_CONT_SENSOR; j++) {
   2272                 observed[j] = false; // reset at each data frame
   2273                 tmp_time[j] = 0;
   2274                 if ((mTask.prev_frame_time[j] < ULONG_LONG_MAX - 1) && (fh_param & (1 << j))) {
   2275                     tmp_time[j] = mTask.prev_frame_time[j] + mTask.time_delta[j];
   2276                     tmp_frame_time = (tmp_time[j] > tmp_frame_time) ? tmp_time[j] : tmp_frame_time;
   2277                 }
   2278             }
   2279             tmp_frame_time = (frame_sensor_time + kMinSensorTimeIncrement > tmp_frame_time)
   2280                 ? (frame_sensor_time + kMinSensorTimeIncrement) : tmp_frame_time;
   2281 
   2282             // regular frame, dispatch data to each sensor's own fifo
   2283 #ifdef MAG_SLAVE_PRESENT
   2284             if (fh_param & 4) { // have mag data
   2285                 if (size >= 8) {
   2286                     if (frame_sensor_time_valid) {
   2287                         // scale not used
   2288                         parseRawData(&mTask.sensors[MAG], &buf[i], 0, tmp_frame_time);
   2289 #if TIMESTAMP_DBG
   2290                         if (mTask.prev_frame_time[MAG] == ULONG_LONG_MAX) {
   2291                             DEBUG_PRINT("mag enabled: frame %d time 0x%08x\n",
   2292                                     frame_num, (unsigned int)tmp_frame_time);
   2293                         } else if ((tmp_frame_time != tmp_time[MAG]) && (tmp_time[MAG] != 0)) {
   2294                             DEBUG_PRINT("frame %d mag time: 0x%08x -> 0x%08x, jumped %d msec\n",
   2295                                     frame_num,
   2296                                     (unsigned int)tmp_time[MAG],
   2297                                     (unsigned int)tmp_frame_time,
   2298                                     (int)(5 * ((int64_t)(tmp_frame_time - tmp_time[MAG]) >> 7)));
   2299                         }
   2300 #endif
   2301                     }
   2302                     mTask.prev_frame_time[MAG] = tmp_frame_time;
   2303                     i += 8;
   2304                     size -= 8;
   2305                     observed[MAG] = true;
   2306                 } else {
   2307                     size = 0;
   2308                 }
   2309             }
   2310 #endif
   2311             if (fh_param & 2) { // have gyro data
   2312                 if (size >= 6) {
   2313                     if (frame_sensor_time_valid) {
   2314                         parseRawData(&mTask.sensors[GYR], &buf[i], kScale_gyr, tmp_frame_time);
   2315 #if TIMESTAMP_DBG
   2316                         if (mTask.prev_frame_time[GYR] == ULONG_LONG_MAX) {
   2317                             DEBUG_PRINT("gyr enabled: frame %d time 0x%08x\n",
   2318                                     frame_num, (unsigned int)tmp_frame_time);
   2319                         } else if ((tmp_frame_time != tmp_time[GYR]) && (tmp_time[GYR] != 0)) {
   2320                             DEBUG_PRINT("frame %d gyr time: 0x%08x -> 0x%08x, jumped %d msec\n",
   2321                                     frame_num,
   2322                                     (unsigned int)tmp_time[GYR],
   2323                                     (unsigned int)tmp_frame_time,
   2324                                     (int)(5 * ((int64_t)(tmp_frame_time - tmp_time[GYR]) >> 7)));
   2325                         }
   2326 #endif
   2327                     }
   2328                     mTask.prev_frame_time[GYR] = tmp_frame_time;
   2329                     i += 6;
   2330                     size -= 6;
   2331                     observed[GYR] = true;
   2332                 } else {
   2333                     size = 0;
   2334                 }
   2335             }
   2336             if (fh_param & 1) { // have accel data
   2337                 if (size >= 6) {
   2338                     if (frame_sensor_time_valid) {
   2339                         parseRawData(&mTask.sensors[ACC], &buf[i], kScale_acc, tmp_frame_time);
   2340 #if TIMESTAMP_DBG
   2341                         if (mTask.prev_frame_time[ACC] == ULONG_LONG_MAX) {
   2342                             DEBUG_PRINT("acc enabled: frame %d time 0x%08x\n",
   2343                                     frame_num, (unsigned int)tmp_frame_time);
   2344                         } else if ((tmp_frame_time != tmp_time[ACC]) && (tmp_time[ACC] != 0)) {
   2345                             DEBUG_PRINT("frame %d gyr time: 0x%08x -> 0x%08x, jumped %d msec\n",
   2346                                     frame_num,
   2347                                     (unsigned int)tmp_time[ACC],
   2348                                     (unsigned int)tmp_frame_time,
   2349                                     (int)(5 * ((int64_t)(tmp_frame_time - tmp_time[ACC]) >> 7)));
   2350                         }
   2351 #endif
   2352                     }
   2353                     mTask.prev_frame_time[ACC] = tmp_frame_time;
   2354                     i += 6;
   2355                     size -= 6;
   2356                     observed[ACC] = true;
   2357                 } else {
   2358                     size = 0;
   2359                 }
   2360             }
   2361 
   2362             if (observed[ACC] || observed[GYR])
   2363                 frame_sensor_time = tmp_frame_time;
   2364 #ifdef MAG_SLAVE_PRESENT
   2365             else if (observed[MAG])
   2366                 frame_sensor_time = tmp_frame_time;
   2367 #endif
   2368         } else {
   2369             size = 0; // drop this batch
   2370             ERROR_PRINT("Invalid fh_mode %d at 0x%x, data dump:\n", fh_mode, i);
   2371             // dump (a) bytes back and (b) bytes forward.
   2372             int a = i < 0x80 ? 0 : (i - 0x80) & ~0x0F;
   2373             int b = ((i + 0x80 > mTask.xferCnt ? mTask.xferCnt : i + 0x80) + 0x0F) & ~0x0F;
   2374             dumpBinary(mTask.dataBuffer, a, b - a);
   2375         }
   2376     }
   2377 
   2378     //flush data events.
   2379     flushAllData();
   2380 }
   2381 
   2382 /*
   2383  * Read the interrupt type and send corresponding event
   2384  * If it's anymo or double tap, also send a single uint32 to indicate which axies
   2385  * is this interrupt triggered.
   2386  * If it's flat, also send a bit to indicate flat/non-flat position.
   2387  * If it's step detector, check if we need to send the total step count.
   2388  */
   2389 static void int2Handling(void)
   2390 {
   2391     TDECL();
   2392     union EmbeddedDataPoint trigger_axies;
   2393     uint8_t int_status_0 = mTask.statusBuffer[1];
   2394     uint8_t int_status_1 = mTask.statusBuffer[2];
   2395     if (int_status_0 & INT_STEP) {
   2396         if (mTask.sensors[STEP].powered) {
   2397             DEBUG_PRINT("Detected step\n");
   2398             osEnqueueEvt(EVT_SENSOR_STEP, NULL, NULL);
   2399         }
   2400         if (mTask.sensors[STEPCNT].powered) {
   2401             T(pending_step_cnt) = true;
   2402         }
   2403     }
   2404     if ((int_status_0 & INT_ANY_MOTION) && mTask.sensors[ANYMO].powered) {
   2405         // bit [0:2] of INT_STATUS[2] is set when anymo is triggered by x, y or
   2406         // z axies respectively. bit [3] indicates the slope.
   2407         trigger_axies.idata = (mTask.statusBuffer[3] & 0x0f);
   2408         DEBUG_PRINT("Detected any motion\n");
   2409         osEnqueueEvt(EVT_SENSOR_ANY_MOTION, trigger_axies.vptr, NULL);
   2410     }
   2411     if ((int_status_0 & INT_DOUBLE_TAP) && mTask.sensors[DTAP].powered) {
   2412         // bit [4:6] of INT_STATUS[2] is set when double tap is triggered by
   2413         // x, y or z axies respectively. bit [7] indicates the slope.
   2414         trigger_axies.idata = ((mTask.statusBuffer[3] & 0xf0) >> 4);
   2415         DEBUG_PRINT("Detected double tap\n");
   2416         osEnqueueEvt(EVT_SENSOR_DOUBLE_TAP, trigger_axies.vptr, NULL);
   2417     }
   2418     if ((int_status_0 & INT_FLAT) && mTask.sensors[FLAT].powered) {
   2419         // bit [7] of INT_STATUS[3] indicates flat/non-flat position
   2420         trigger_axies.idata = ((mTask.statusBuffer[4] & 0x80) >> 7);
   2421         DEBUG_PRINT("Detected flat\n");
   2422         osEnqueueEvt(EVT_SENSOR_FLAT, trigger_axies.vptr, NULL);
   2423     }
   2424     if ((int_status_1 & INT_NO_MOTION) && mTask.sensors[NOMO].powered) {
   2425         DEBUG_PRINT("Detected no motion\n");
   2426         osEnqueueEvt(EVT_SENSOR_NO_MOTION, NULL, NULL);
   2427     }
   2428     return;
   2429 }
   2430 
   2431 static void int2Evt(void)
   2432 {
   2433     TDECL();
   2434     if (trySwitchState(SENSOR_INT_2_HANDLING)) {
   2435         // Read the interrupt reg value to determine what interrupts
   2436         SPI_READ(BMI160_REG_INT_STATUS_0, 4, &mTask.statusBuffer);
   2437         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, __FUNCTION__);
   2438     } else {
   2439         // even if we are still in SENSOR_INT_2_HANDLING, the SPI may already finished and we need
   2440         // to issue another SPI read to get the latest status
   2441         mTask.pending_int[1] = true;
   2442     }
   2443 }
   2444 
   2445 // bits[6:7] in OFFSET[6] to enable/disable gyro/accel offset.
   2446 // bits[0:5] in OFFSET[6] stores the most significant 2 bits of gyro offset at
   2447 // its x, y, z axies.
   2448 // Calculate the stored gyro offset and compose it with the intended
   2449 // enable/disable mode for gyro/accel offset to determine the value for
   2450 // OFFSET[6].
   2451 static uint8_t offset6Mode(void)
   2452 {
   2453     uint8_t mode = 0;
   2454     if (mTask.sensors[GYR].offset_enable)
   2455         mode |= 0x01 << 7;
   2456     if (mTask.sensors[ACC].offset_enable)
   2457         mode |= 0x01 << 6;
   2458     mode |= (mTask.sensors[GYR].offset[2] & 0x0300) >> 4;
   2459     mode |= (mTask.sensors[GYR].offset[1] & 0x0300) >> 6;
   2460     mode |= (mTask.sensors[GYR].offset[0] & 0x0300) >> 8;
   2461     DEBUG_PRINT("OFFSET_6_MODE is: %02x\n", mode);
   2462     return mode;
   2463 }
   2464 
   2465 static bool saveCalibration()
   2466 {
   2467     TDECL();
   2468     if (trySwitchState(SENSOR_SAVE_CALIBRATION)) {
   2469         if (mTask.sensors[ACC].offset_enable) {
   2470             SPI_WRITE(BMI160_REG_OFFSET_0, mTask.sensors[ACC].offset[0] & 0xFF, 450);
   2471             SPI_WRITE(BMI160_REG_OFFSET_0 + 1, mTask.sensors[ACC].offset[1] & 0xFF, 450);
   2472             SPI_WRITE(BMI160_REG_OFFSET_0 + 2, mTask.sensors[ACC].offset[2] & 0xFF, 450);
   2473         }
   2474         if (mTask.sensors[GYR].offset_enable) {
   2475             SPI_WRITE(BMI160_REG_OFFSET_3, mTask.sensors[GYR].offset[0] & 0xFF, 450);
   2476             SPI_WRITE(BMI160_REG_OFFSET_3 + 1, mTask.sensors[GYR].offset[1] & 0xFF, 450);
   2477             SPI_WRITE(BMI160_REG_OFFSET_3 + 2, mTask.sensors[GYR].offset[2] & 0xFF, 450);
   2478         }
   2479         SPI_WRITE(BMI160_REG_OFFSET_6, offset6Mode(), 450);
   2480         SPI_READ(BMI160_REG_OFFSET_0, 7, &mTask.dataBuffer);
   2481         spiBatchTxRx(&mTask.mode, sensorSpiCallback, NULL, __FUNCTION__);
   2482         return true;
   2483     } else {
   2484         DEBUG_PRINT("%s, state != IDLE", __FUNCTION__);
   2485         return false;
   2486     }
   2487 }
   2488 
   2489 static void sendCalibrationResult(uint8_t status, uint8_t sensorType,
   2490         int32_t xBias, int32_t yBias, int32_t zBias) {
   2491     struct CalibrationData *data = heapAlloc(sizeof(struct CalibrationData));
   2492     if (!data) {
   2493         osLog(LOG_WARN, "Couldn't alloc cal result pkt");
   2494         return;
   2495     }
   2496 
   2497     data->header.appId = BMI160_APP_ID;
   2498     data->header.dataLen = (sizeof(struct CalibrationData) - sizeof(struct HostHubRawPacket));
   2499     data->data_header.msgId = SENSOR_APP_MSG_ID_CAL_RESULT;
   2500     data->data_header.sensorType = sensorType;
   2501     data->data_header.status = status;
   2502 
   2503     data->xBias = xBias;
   2504     data->yBias = yBias;
   2505     data->zBias = zBias;
   2506 
   2507     if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
   2508         osLog(LOG_WARN, "Couldn't send cal result evt");
   2509 }
   2510 
   2511 static void accCalibrationHandling(void)
   2512 {
   2513     TDECL();
   2514     switch (mTask.calibration_state) {
   2515     case CALIBRATION_START:
   2516         T(mRetryLeft) = RETRY_CNT_CALIBRATION;
   2517 
   2518         // turn ACC to NORMAL mode
   2519         SPI_WRITE(BMI160_REG_CMD, 0x11, 50000);
   2520 
   2521         mTask.calibration_state = CALIBRATION_FOC;
   2522         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
   2523         break;
   2524     case CALIBRATION_FOC:
   2525 
   2526         // set accel range to +-8g
   2527         SPI_WRITE(BMI160_REG_ACC_RANGE, 0x08);
   2528 
   2529         // enable accel fast offset compensation,
   2530         // x: 0g, y: 0g, z: 1g
   2531         SPI_WRITE(BMI160_REG_FOC_CONF, ACC_FOC_CONFIG);
   2532 
   2533         // start calibration
   2534         SPI_WRITE(BMI160_REG_CMD, 0x03, 100000);
   2535 
   2536         // poll the status reg until the calibration finishes.
   2537         SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
   2538 
   2539         mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
   2540         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
   2541         break;
   2542     case CALIBRATION_WAIT_FOC_DONE:
   2543         // if the STATUS REG has bit 3 set, it means calbration is done.
   2544         // otherwise, check back in 50ms later.
   2545         if (mTask.statusBuffer[1] & 0x08) {
   2546 
   2547             //disable FOC
   2548             SPI_WRITE(BMI160_REG_FOC_CONF, 0x00);
   2549 
   2550             //read the offset value for accel
   2551             SPI_READ(BMI160_REG_OFFSET_0, 3, &mTask.dataBuffer);
   2552             mTask.calibration_state = CALIBRATION_SET_OFFSET;
   2553             DEBUG_PRINT("FOC set FINISHED!\n");
   2554         } else {
   2555 
   2556             // calibration hasn't finished yet, go back to wait for 50ms.
   2557             SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
   2558             mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
   2559             T(mRetryLeft)--;
   2560         }
   2561         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
   2562 
   2563         // if calbration hasn't finished after 10 polling on the STATUS reg,
   2564         // declare timeout.
   2565         if (T(mRetryLeft) == 0) {
   2566             mTask.calibration_state = CALIBRATION_TIMEOUT;
   2567         }
   2568         break;
   2569     case CALIBRATION_SET_OFFSET:
   2570         mTask.sensors[ACC].offset[0] = mTask.dataBuffer[1];
   2571         mTask.sensors[ACC].offset[1] = mTask.dataBuffer[2];
   2572         mTask.sensors[ACC].offset[2] = mTask.dataBuffer[3];
   2573         // sign extend values
   2574         if (mTask.sensors[ACC].offset[0] & 0x80)
   2575             mTask.sensors[ACC].offset[0] |= 0xFFFFFF00;
   2576         if (mTask.sensors[ACC].offset[1] & 0x80)
   2577             mTask.sensors[ACC].offset[1] |= 0xFFFFFF00;
   2578         if (mTask.sensors[ACC].offset[2] & 0x80)
   2579             mTask.sensors[ACC].offset[2] |= 0xFFFFFF00;
   2580 
   2581         mTask.sensors[ACC].offset_enable = true;
   2582         DEBUG_PRINT("ACCELERATION OFFSET is %02x  %02x  %02x\n",
   2583                 (unsigned int)mTask.sensors[ACC].offset[0],
   2584                 (unsigned int)mTask.sensors[ACC].offset[1],
   2585                 (unsigned int)mTask.sensors[ACC].offset[2]);
   2586 
   2587         sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_ACCEL,
   2588                 mTask.sensors[ACC].offset[0], mTask.sensors[ACC].offset[1],
   2589                 mTask.sensors[ACC].offset[2]);
   2590 
   2591         // Enable offset compensation for accel
   2592         uint8_t mode = offset6Mode();
   2593         SPI_WRITE(BMI160_REG_OFFSET_6, mode);
   2594 
   2595         // turn ACC to SUSPEND mode
   2596         SPI_WRITE(BMI160_REG_CMD, 0x10, 5000);
   2597 
   2598         mTask.calibration_state = CALIBRATION_DONE;
   2599         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
   2600         break;
   2601     default:
   2602         ERROR_PRINT("Invalid calibration state\n");
   2603         break;
   2604     }
   2605 }
   2606 
   2607 static bool accCalibration(void *cookie)
   2608 {
   2609     TDECL();
   2610     if (!mTask.sensors[ACC].powered && trySwitchState(SENSOR_CALIBRATING)) {
   2611         mTask.calibration_state = CALIBRATION_START;
   2612         accCalibrationHandling();
   2613         return true;
   2614     } else {
   2615         ERROR_PRINT("cannot calibrate accel because sensor is busy\n");
   2616         sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL, 0, 0, 0);
   2617         return false;
   2618     }
   2619 }
   2620 
   2621 static bool accCfgData(void *data, void *cookie)
   2622 {
   2623     struct CfgData {
   2624         int32_t hw[3];
   2625         float sw[3];
   2626     };
   2627     struct CfgData *values = data;
   2628 
   2629     mTask.sensors[ACC].offset[0] = values->hw[0];
   2630     mTask.sensors[ACC].offset[1] = values->hw[1];
   2631     mTask.sensors[ACC].offset[2] = values->hw[2];
   2632     mTask.sensors[ACC].offset_enable = true;
   2633 
   2634 #ifdef ACCEL_CAL_ENABLED
   2635     accelCalBiasSet(&mTask.acc, values->sw[0], values->sw[1], values->sw[2]);
   2636 #endif
   2637 
   2638     INFO_PRINT("accCfgData: data=%02lx, %02lx, %02lx\n",
   2639             values->hw[0] & 0xFF, values->hw[1] & 0xFF, values->hw[2] & 0xFF);
   2640 
   2641     if (!saveCalibration()) {
   2642         mTask.pending_calibration_save = true;
   2643     }
   2644 
   2645     return true;
   2646 }
   2647 
   2648 static void sendTestResult(uint8_t status, uint8_t sensorType) {
   2649     struct TestResultData *data = heapAlloc(sizeof(struct TestResultData));
   2650     if (!data) {
   2651         osLog(LOG_WARN, "Couldn't alloc test result packet");
   2652         return;
   2653     }
   2654 
   2655     data->header.appId = BMI160_APP_ID;
   2656     data->header.dataLen = (sizeof(struct TestResultData) - sizeof(struct HostHubRawPacket));
   2657     data->data_header.msgId = SENSOR_APP_MSG_ID_TEST_RESULT;
   2658     data->data_header.sensorType = sensorType;
   2659     data->data_header.status = status;
   2660 
   2661     if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree))
   2662         osLog(LOG_WARN, "Couldn't send test result packet");
   2663 }
   2664 
   2665 static void accTestHandling(void)
   2666 {
   2667     // the minimum absolute differences, according to BMI160 datasheet section
   2668     // 2.8.1, are 800 mg for the x and y axes and 400 mg for the z axis
   2669     static const int32_t kMinDifferenceXY = (800 * 32767) / 8000;
   2670     static const int32_t kMinDifferenceZ = (400 * 32767) / 8000;
   2671 
   2672     int32_t tempTestX, tempTestY, tempTestZ;
   2673     int32_t absDiffX, absDiffY, absDiffZ;
   2674 
   2675     TDECL();
   2676 
   2677     switch (mTask.acc_test_state) {
   2678     case ACC_TEST_START:
   2679         // turn ACC to NORMAL mode
   2680         SPI_WRITE(BMI160_REG_CMD, 0x11, 50000);
   2681 
   2682         mTask.acc_test_state = ACC_TEST_CONFIG;
   2683         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
   2684         break;
   2685 
   2686     case ACC_TEST_CONFIG:
   2687         // set accel conf
   2688         SPI_WRITE(BMI160_REG_ACC_CONF, 0x2c);
   2689 
   2690         // set accel range to +-8g
   2691         SPI_WRITE(BMI160_REG_ACC_RANGE, 0x08);
   2692 
   2693         // read stale accel data
   2694         SPI_READ(BMI160_REG_DATA_14, 6, &mTask.dataBuffer);
   2695 
   2696         mTask.acc_test_state = ACC_TEST_RUN_0;
   2697         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
   2698         break;
   2699 
   2700     case ACC_TEST_RUN_0:
   2701         // configure acc_self_test_amp=1, acc_self_test_sign=0, acc_self_test_enable=b01
   2702         // wait 50ms for data to be available
   2703         SPI_WRITE(BMI160_REG_SELF_TEST, 0x09, 50000);
   2704 
   2705         // read accel data
   2706         SPI_READ(BMI160_REG_DATA_14, 6, &mTask.dataBuffer);
   2707 
   2708         mTask.acc_test_state = ACC_TEST_RUN_1;
   2709         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
   2710         break;
   2711 
   2712     case ACC_TEST_RUN_1:
   2713         // save accel data
   2714         mTask.accTestX = *(int16_t*)(mTask.dataBuffer+1);
   2715         mTask.accTestY = *(int16_t*)(mTask.dataBuffer+3);
   2716         mTask.accTestZ = *(int16_t*)(mTask.dataBuffer+5);
   2717 
   2718         // configure acc_self_test_amp=1, acc_self_test_sign=1, acc_self_test_enable=b01
   2719         // wait 50ms for data to be available
   2720         SPI_WRITE(BMI160_REG_SELF_TEST, 0x0d, 50000);
   2721 
   2722         // read accel data
   2723         SPI_READ(BMI160_REG_DATA_14, 6, &mTask.dataBuffer);
   2724 
   2725         mTask.acc_test_state = ACC_TEST_VERIFY;
   2726         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
   2727         break;
   2728 
   2729     case ACC_TEST_VERIFY:
   2730         // save accel data
   2731         tempTestX = *(int16_t*)(mTask.dataBuffer+1);
   2732         tempTestY = *(int16_t*)(mTask.dataBuffer+3);
   2733         tempTestZ = *(int16_t*)(mTask.dataBuffer+5);
   2734 
   2735         // calculate the differences between run 0 and run 1
   2736         absDiffX = ABS((int32_t)mTask.accTestX - tempTestX);
   2737         absDiffY = ABS((int32_t)mTask.accTestY - tempTestY);
   2738         absDiffZ = ABS((int32_t)mTask.accTestZ - tempTestZ);
   2739 
   2740         DEBUG_PRINT("accSelfTest diffs: X %d, Y %d, Z %d\n", (int)absDiffX, (int)absDiffY, (int)absDiffZ);
   2741 
   2742         // verify that the differences between run 0 and run 1 are within spec
   2743         if (absDiffX >= kMinDifferenceXY && absDiffY >= kMinDifferenceXY && absDiffZ >= kMinDifferenceZ) {
   2744             sendTestResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_ACCEL);
   2745         } else {
   2746             sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, SENS_TYPE_ACCEL);
   2747         }
   2748 
   2749         // turn ACC to SUSPEND mode
   2750         SPI_WRITE(BMI160_REG_CMD, 0x10, 5000);
   2751 
   2752         mTask.acc_test_state = ACC_TEST_DONE;
   2753         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[ACC], __FUNCTION__);
   2754         break;
   2755 
   2756     default:
   2757         ERROR_PRINT("Invalid accel test state\n");
   2758         break;
   2759     }
   2760 }
   2761 
   2762 static bool accSelfTest(void *cookie)
   2763 {
   2764     TDECL();
   2765     INFO_PRINT("accSelfTest\n");
   2766 
   2767     if (!mTask.sensors[ACC].powered && trySwitchState(SENSOR_TESTING)) {
   2768         mTask.acc_test_state = ACC_TEST_START;
   2769         accTestHandling();
   2770         return true;
   2771     } else {
   2772         ERROR_PRINT("cannot test accel because sensor is busy\n");
   2773         sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_ACCEL);
   2774         return false;
   2775     }
   2776 }
   2777 
   2778 static void gyrCalibrationHandling(void)
   2779 {
   2780     TDECL();
   2781     switch (mTask.calibration_state) {
   2782     case CALIBRATION_START:
   2783         T(mRetryLeft) = RETRY_CNT_CALIBRATION;
   2784 
   2785         // turn GYR to NORMAL mode
   2786         SPI_WRITE(BMI160_REG_CMD, 0x15, 50000);
   2787 
   2788         mTask.calibration_state = CALIBRATION_FOC;
   2789         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
   2790         break;
   2791     case CALIBRATION_FOC:
   2792 
   2793         // set gyro range to +-1000 deg/sec
   2794         SPI_WRITE(BMI160_REG_GYR_RANGE, 0x01);
   2795 
   2796         // enable gyro fast offset compensation
   2797         SPI_WRITE(BMI160_REG_FOC_CONF, 0x40);
   2798 
   2799         // start FOC
   2800         SPI_WRITE(BMI160_REG_CMD, 0x03, 100000);
   2801 
   2802         // poll the status reg until the calibration finishes.
   2803         SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
   2804 
   2805         mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
   2806         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
   2807         break;
   2808     case CALIBRATION_WAIT_FOC_DONE:
   2809 
   2810         // if the STATUS REG has bit 3 set, it means calbration is done.
   2811         // otherwise, check back in 50ms later.
   2812         if (mTask.statusBuffer[1] & 0x08) {
   2813 
   2814             // disable gyro fast offset compensation
   2815             SPI_WRITE(BMI160_REG_FOC_CONF, 0x00);
   2816 
   2817             //read the offset value for gyro
   2818             SPI_READ(BMI160_REG_OFFSET_3, 4, &mTask.dataBuffer);
   2819             mTask.calibration_state = CALIBRATION_SET_OFFSET;
   2820             DEBUG_PRINT("FOC set FINISHED!\n");
   2821         } else {
   2822 
   2823             // calibration hasn't finished yet, go back to wait for 50ms.
   2824             SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 50000);
   2825             mTask.calibration_state = CALIBRATION_WAIT_FOC_DONE;
   2826             T(mRetryLeft)--;
   2827         }
   2828         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
   2829 
   2830         // if calbration hasn't finished after 10 polling on the STATUS reg,
   2831         // declare timeout.
   2832         if (T(mRetryLeft) == 0) {
   2833             mTask.calibration_state = CALIBRATION_TIMEOUT;
   2834         }
   2835         break;
   2836     case CALIBRATION_SET_OFFSET:
   2837         mTask.sensors[GYR].offset[0] = ((mTask.dataBuffer[4] & 0x03) << 8) | mTask.dataBuffer[1];
   2838         mTask.sensors[GYR].offset[1] = ((mTask.dataBuffer[4] & 0x0C) << 6) | mTask.dataBuffer[2];
   2839         mTask.sensors[GYR].offset[2] = ((mTask.dataBuffer[4] & 0x30) << 4) | mTask.dataBuffer[3];
   2840         // sign extend values
   2841         if (mTask.sensors[GYR].offset[0] & 0x200)
   2842             mTask.sensors[GYR].offset[0] |= 0xFFFFFC00;
   2843         if (mTask.sensors[GYR].offset[1] & 0x200)
   2844             mTask.sensors[GYR].offset[1] |= 0xFFFFFC00;
   2845         if (mTask.sensors[GYR].offset[2] & 0x200)
   2846             mTask.sensors[GYR].offset[2] |= 0xFFFFFC00;
   2847 
   2848         mTask.sensors[GYR].offset_enable = true;
   2849         DEBUG_PRINT("GYRO OFFSET is %02x  %02x  %02x\n",
   2850                 (unsigned int)mTask.sensors[GYR].offset[0],
   2851                 (unsigned int)mTask.sensors[GYR].offset[1],
   2852                 (unsigned int)mTask.sensors[GYR].offset[2]);
   2853 
   2854         sendCalibrationResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_GYRO,
   2855                 mTask.sensors[GYR].offset[0], mTask.sensors[GYR].offset[1],
   2856                 mTask.sensors[GYR].offset[2]);
   2857 
   2858         // Enable offset compensation for gyro
   2859         uint8_t mode = offset6Mode();
   2860         SPI_WRITE(BMI160_REG_OFFSET_6, mode);
   2861 
   2862         // turn GYR to SUSPEND mode
   2863         SPI_WRITE(BMI160_REG_CMD, 0x14, 1000);
   2864 
   2865         mTask.calibration_state = CALIBRATION_DONE;
   2866         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
   2867         break;
   2868     default:
   2869         ERROR_PRINT("Invalid calibration state\n");
   2870         break;
   2871     }
   2872 }
   2873 
   2874 static bool gyrCalibration(void *cookie)
   2875 {
   2876     TDECL();
   2877     if (!mTask.sensors[GYR].powered && trySwitchState(SENSOR_CALIBRATING)) {
   2878         mTask.calibration_state = CALIBRATION_START;
   2879         gyrCalibrationHandling();
   2880         return true;
   2881     } else {
   2882         ERROR_PRINT("cannot calibrate gyro because sensor is busy\n");
   2883         sendCalibrationResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO, 0, 0, 0);
   2884         return false;
   2885     }
   2886 }
   2887 
   2888 static bool gyrCfgData(void *data, void *cookie)
   2889 {
   2890     struct CfgData {
   2891         int32_t hw[3];
   2892         float sw[3];
   2893     };
   2894     struct CfgData *values = data;
   2895 
   2896     mTask.sensors[GYR].offset[0] = values->hw[0];
   2897     mTask.sensors[GYR].offset[1] = values->hw[1];
   2898     mTask.sensors[GYR].offset[2] = values->hw[2];
   2899     mTask.sensors[GYR].offset_enable = true;
   2900 
   2901 #ifdef GYRO_CAL_ENABLED
   2902     gyroCalSetBias(&mTask.gyro_cal, values->sw[0], values->sw[1], values->sw[2], sensorGetTime());
   2903 #endif
   2904 
   2905     INFO_PRINT("gyrCfgData: data=%02lx, %02lx, %02lx\n",
   2906             values->hw[0] & 0xFF, values->hw[1] & 0xFF, values->hw[2] & 0xFF);
   2907 
   2908     if (!saveCalibration()) {
   2909         mTask.pending_calibration_save = true;
   2910     }
   2911 
   2912     return true;
   2913 }
   2914 
   2915 static void gyroTestHandling(void)
   2916 {
   2917     TDECL();
   2918 
   2919     switch (mTask.gyro_test_state) {
   2920     case GYRO_TEST_START:
   2921         // turn GYR to NORMAL mode
   2922         SPI_WRITE(BMI160_REG_CMD, 0x15, 50000);
   2923 
   2924         mTask.gyro_test_state = GYRO_TEST_RUN;
   2925         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
   2926         break;
   2927 
   2928     case GYRO_TEST_RUN:
   2929         // set gyr_self_test_enable
   2930         // wait 50ms to check test status
   2931         SPI_WRITE(BMI160_REG_SELF_TEST, 0x10, 50000);
   2932 
   2933         // check gyro self-test result in status register
   2934         SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer);
   2935 
   2936         mTask.gyro_test_state = GYRO_TEST_VERIFY;
   2937         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
   2938         break;
   2939 
   2940     case GYRO_TEST_VERIFY:
   2941         // gyr_self_test_ok is bit 1
   2942         if (mTask.statusBuffer[1] & 0x2) {
   2943             sendTestResult(SENSOR_APP_EVT_STATUS_SUCCESS, SENS_TYPE_GYRO);
   2944         } else {
   2945             sendTestResult(SENSOR_APP_EVT_STATUS_ERROR, SENS_TYPE_GYRO);
   2946         }
   2947 
   2948         // turn GYR to SUSPEND mode
   2949         SPI_WRITE(BMI160_REG_CMD, 0x14, 1000);
   2950 
   2951         mTask.gyro_test_state = GYRO_TEST_DONE;
   2952         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask.sensors[GYR], __FUNCTION__);
   2953         break;
   2954 
   2955     default:
   2956         ERROR_PRINT("Invalid gyro test state\n");
   2957         break;
   2958     }
   2959 }
   2960 
   2961 static bool gyrSelfTest(void *cookie)
   2962 {
   2963     TDECL();
   2964     INFO_PRINT("gyrSelfTest\n");
   2965 
   2966     if (!mTask.sensors[GYR].powered && trySwitchState(SENSOR_TESTING)) {
   2967         mTask.gyro_test_state = GYRO_TEST_START;
   2968         gyroTestHandling();
   2969         return true;
   2970     } else {
   2971         ERROR_PRINT("cannot test gyro because sensor is busy\n");
   2972         sendTestResult(SENSOR_APP_EVT_STATUS_BUSY, SENS_TYPE_GYRO);
   2973         return false;
   2974     }
   2975 }
   2976 
   2977 #ifdef MAG_SLAVE_PRESENT
   2978 static bool magCfgData(void *data, void *cookie)
   2979 {
   2980     float *values = data;
   2981 
   2982     INFO_PRINT("magCfgData: %ld, %ld, %ld\n",
   2983             (int32_t)(values[0] * 1000), (int32_t)(values[1] * 1000), (int32_t)(values[2] * 1000));
   2984 
   2985     mTask.moc.x_bias = values[0];
   2986     mTask.moc.y_bias = values[1];
   2987     mTask.moc.z_bias = values[2];
   2988 
   2989     mTask.magBiasPosted = false;
   2990 
   2991     return true;
   2992 }
   2993 #endif
   2994 
   2995 #define DEC_OPS(power, firmware, rate, flush) \
   2996     .sensorPower = power, \
   2997     .sensorFirmwareUpload = firmware, \
   2998     .sensorSetRate = rate, \
   2999     .sensorFlush = flush
   3000 
   3001 #define DEC_OPS_SEND(power, firmware, rate, flush, send) \
   3002     DEC_OPS(power, firmware, rate, flush), \
   3003     .sensorSendOneDirectEvt = send
   3004 
   3005 #define DEC_OPS_CAL_CFG_TEST(power, firmware, rate, flush, cal, cfg, test) \
   3006     DEC_OPS(power, firmware, rate, flush), \
   3007     .sensorCalibrate = cal, \
   3008     .sensorCfgData = cfg, \
   3009     .sensorSelfTest = test,
   3010 
   3011 #define DEC_OPS_CFG(power, firmware, rate, flush, cfg) \
   3012     DEC_OPS(power, firmware, rate, flush), \
   3013     .sensorCfgData = cfg
   3014 
   3015 static const struct SensorOps mSensorOps[NUM_OF_SENSOR] =
   3016 {
   3017     { DEC_OPS_CAL_CFG_TEST(accPower, accFirmwareUpload, accSetRate, accFlush, accCalibration,
   3018             accCfgData, accSelfTest) },
   3019     { DEC_OPS_CAL_CFG_TEST(gyrPower, gyrFirmwareUpload, gyrSetRate, gyrFlush, gyrCalibration,
   3020             gyrCfgData, gyrSelfTest) },
   3021 #ifdef MAG_SLAVE_PRESENT
   3022     { DEC_OPS_CFG(magPower, magFirmwareUpload, magSetRate, magFlush, magCfgData) },
   3023 #endif
   3024     { DEC_OPS(stepPower, stepFirmwareUpload, stepSetRate, stepFlush) },
   3025     { DEC_OPS(doubleTapPower, doubleTapFirmwareUpload, doubleTapSetRate, doubleTapFlush) },
   3026     { DEC_OPS(flatPower, flatFirmwareUpload, flatSetRate, flatFlush) },
   3027     { DEC_OPS(anyMotionPower, anyMotionFirmwareUpload, anyMotionSetRate, anyMotionFlush) },
   3028     { DEC_OPS(noMotionPower, noMotionFirmwareUpload, noMotionSetRate, noMotionFlush) },
   3029     { DEC_OPS_SEND(stepCntPower, stepCntFirmwareUpload, stepCntSetRate, stepCntFlush,
   3030             stepCntSendLastData) },
   3031 };
   3032 
   3033 static void configEvent(struct BMI160Sensor *mSensor, struct ConfigStat *ConfigData)
   3034 {
   3035     int i;
   3036 
   3037     for (i = 0; &mTask.sensors[i] != mSensor; i++) ;
   3038 
   3039     if (ConfigData->enable == 0 && mSensor->powered)
   3040         mSensorOps[i].sensorPower(false, (void *)i);
   3041     else if (ConfigData->enable == 1 && !mSensor->powered)
   3042         mSensorOps[i].sensorPower(true, (void *)i);
   3043     else
   3044         mSensorOps[i].sensorSetRate(ConfigData->rate, ConfigData->latency, (void *)i);
   3045 }
   3046 
   3047 static void timeSyncEvt(uint32_t evtGeneration, bool evtDataValid)
   3048 {
   3049     TDECL();
   3050     // not processing pending events
   3051     if (evtDataValid) {
   3052         // stale event
   3053         if (evtGeneration != mTask.poll_generation)
   3054             return;
   3055 
   3056         mTask.active_poll_generation = mTask.poll_generation;
   3057     }
   3058 
   3059     if (trySwitchState(SENSOR_TIME_SYNC)) {
   3060         SPI_READ(BMI160_REG_SENSORTIME_0, 3, &mTask.sensorTimeBuffer);
   3061         SPI_READ(BMI160_REG_TEMPERATURE_0, 2, &mTask.temperatureBuffer);
   3062         // sensorSpiCallback schedules a private event, which can be delayed
   3063         // by other long-running tasks.
   3064         // Take the rtc time now so it matches the current sensorTime register
   3065         // reading.
   3066         mTask.timesync_rtc_time = sensorGetTime();
   3067         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, __FUNCTION__);
   3068     } else {
   3069         mTask.pending_time_sync = true;
   3070     }
   3071 }
   3072 
   3073 static void processPendingEvt(void)
   3074 {
   3075     TDECL();
   3076     enum SensorIndex i;
   3077     if (mTask.pending_int[0]) {
   3078         mTask.pending_int[0] = false;
   3079         initiateFifoRead(false /*isInterruptContext*/);
   3080         return;
   3081     }
   3082     if (mTask.pending_int[1]) {
   3083         mTask.pending_int[1] = false;
   3084         int2Evt();
   3085         return;
   3086     }
   3087     if (mTask.pending_time_sync) {
   3088         mTask.pending_time_sync = false;
   3089         timeSyncEvt(0, false);
   3090         return;
   3091     }
   3092     for (i = FIRST_CONT_SENSOR; i < NUM_OF_SENSOR; i++) {
   3093         if (mTask.pending_config[i]) {
   3094             mTask.pending_config[i] = false;
   3095             configEvent(&mTask.sensors[i], &mTask.sensors[i].pConfig);
   3096             return;
   3097         }
   3098     }
   3099     if (mTask.sensors[STEPCNT].flush > 0 || T(pending_step_cnt)) {
   3100         T(pending_step_cnt) = T(pending_step_cnt) && !stepCntFlushGetData();
   3101         return;
   3102     }
   3103     if (mTask.pending_calibration_save) {
   3104         mTask.pending_calibration_save = !saveCalibration();
   3105         return;
   3106     }
   3107 }
   3108 
   3109 static void sensorInit(void)
   3110 {
   3111     TDECL();
   3112     switch (mTask.init_state) {
   3113     case RESET_BMI160:
   3114         DEBUG_PRINT("Performing soft reset\n");
   3115         // perform soft reset and wait for 100ms
   3116         SPI_WRITE(BMI160_REG_CMD, 0xb6, 100000);
   3117         // dummy reads after soft reset, wait 100us
   3118         SPI_READ(BMI160_REG_MAGIC, 1, &mTask.dataBuffer, 100);
   3119 
   3120         mTask.init_state = INIT_BMI160;
   3121         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit RESET" );
   3122         break;
   3123 
   3124     case INIT_BMI160:
   3125         // Read any pending interrupts to reset them
   3126         SPI_READ(BMI160_REG_INT_STATUS_0, 4, &mTask.statusBuffer);
   3127 
   3128         // disable accel, gyro and mag data in FIFO, enable header, enable time.
   3129         SPI_WRITE(BMI160_REG_FIFO_CONFIG_1, 0x12, 450);
   3130 
   3131         // set the watermark to 24 byte
   3132         SPI_WRITE(BMI160_REG_FIFO_CONFIG_0, 0x06, 450);
   3133 
   3134         // FIFO watermark and fifo_full interrupt enabled
   3135         SPI_WRITE(BMI160_REG_INT_EN_0, 0x00, 450);
   3136         SPI_WRITE(BMI160_REG_INT_EN_1, 0x60, 450);
   3137         SPI_WRITE(BMI160_REG_INT_EN_2, 0x00, 450);
   3138 
   3139         // INT1, INT2 enabled, high-edge (push-pull) triggered.
   3140         SPI_WRITE(BMI160_REG_INT_OUT_CTRL, 0xbb, 450);
   3141 
   3142         // INT1, INT2 input disabled, interrupt mode: non-latched
   3143         SPI_WRITE(BMI160_REG_INT_LATCH, 0x00, 450);
   3144 
   3145         // Map data interrupts (e.g., FIFO) to INT1 and physical
   3146         // interrupts (e.g., any motion) to INT2
   3147         SPI_WRITE(BMI160_REG_INT_MAP_0, 0x00, 450);
   3148         SPI_WRITE(BMI160_REG_INT_MAP_1, 0xE1, 450);
   3149         SPI_WRITE(BMI160_REG_INT_MAP_2, 0xFF, 450);
   3150 
   3151         // Use pre-filtered data for tap interrupt
   3152         SPI_WRITE(BMI160_REG_INT_DATA_0, 0x08);
   3153 
   3154         // Disable PMU_TRIGGER
   3155         SPI_WRITE(BMI160_REG_PMU_TRIGGER, 0x00, 450);
   3156 
   3157         // tell gyro and accel to NOT use the FOC offset.
   3158         mTask.sensors[ACC].offset_enable = false;
   3159         mTask.sensors[GYR].offset_enable = false;
   3160         SPI_WRITE(BMI160_REG_OFFSET_6, offset6Mode(), 450);
   3161 
   3162         // initial range for accel (+-8g) and gyro (+-1000 degree).
   3163         SPI_WRITE(BMI160_REG_ACC_RANGE, 0x08, 450);
   3164         SPI_WRITE(BMI160_REG_GYR_RANGE, 0x01, 450);
   3165 
   3166         // Reset step counter
   3167         SPI_WRITE(BMI160_REG_CMD, 0xB2, 10000);
   3168         // Reset interrupt
   3169         SPI_WRITE(BMI160_REG_CMD, 0xB1, 10000);
   3170         // Reset fifo
   3171         SPI_WRITE(BMI160_REG_CMD, 0xB0, 10000);
   3172 
   3173 #ifdef MAG_SLAVE_PRESENT
   3174         mTask.init_state = INIT_MAG;
   3175         mTask.mag_state = MAG_SET_START;
   3176 #else
   3177         // no mag connected to secondary interface
   3178         mTask.init_state = INIT_ON_CHANGE_SENSORS;
   3179 #endif
   3180         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT");
   3181         break;
   3182 
   3183     case INIT_MAG:
   3184         // Don't check statusBuffer if we are just starting mag config
   3185         if (mTask.mag_state == MAG_SET_START) {
   3186             T(mRetryLeft) = RETRY_CNT_MAG;
   3187             magConfig();
   3188         } else if (mTask.mag_state < MAG_SET_DATA && mTask.statusBuffer[1] & 0x04) {
   3189             // fixme: poll_until to reduce states
   3190             // fixme: check should be done before SPI_READ in MAG_READ
   3191             SPI_READ(BMI160_REG_STATUS, 1, &mTask.statusBuffer, 1000);
   3192             if (--T(mRetryLeft) == 0) {
   3193                 ERROR_PRINT("INIT_MAG failed\n");
   3194                 // fixme: duplicate suspend mag here
   3195                 mTask.mag_state = MAG_INIT_FAILED;
   3196                 mTask.init_state = INIT_ON_CHANGE_SENSORS;
   3197             }
   3198         } else {
   3199             T(mRetryLeft) = RETRY_CNT_MAG;
   3200             magConfig();
   3201         }
   3202 
   3203         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT_MAG");
   3204         break;
   3205 
   3206     case INIT_ON_CHANGE_SENSORS:
   3207         // configure any_motion and no_motion for 50Hz accel samples
   3208         configMotion(MOTION_ODR);
   3209 
   3210         // select no_motion over slow_motion
   3211         // select any_motion over significant motion
   3212         SPI_WRITE(BMI160_REG_INT_MOTION_3, 0x15, 450);
   3213 
   3214         // int_tap_quiet=30ms, int_tap_shock=75ms, int_tap_dur=150ms
   3215         SPI_WRITE(BMI160_REG_INT_TAP_0, 0x42, 450);
   3216 
   3217         // int_tap_th = 7 * 250 mg (8-g range)
   3218         SPI_WRITE(BMI160_REG_INT_TAP_1, TAP_THRESHOLD, 450);
   3219 
   3220         // config step detector
   3221         SPI_WRITE(BMI160_REG_STEP_CONF_0, 0x15, 450);
   3222         SPI_WRITE(BMI160_REG_STEP_CONF_1, 0x03, 450);
   3223 
   3224         // int_flat_theta = 44.8 deg * (16/64) = 11.2 deg
   3225         SPI_WRITE(BMI160_REG_INT_FLAT_0, 0x10, 450);
   3226 
   3227         // int_flat_hold_time = (640 msec)
   3228         // int_flat_hy = 44.8 * 4 / 64 = 2.8 deg
   3229         SPI_WRITE(BMI160_REG_INT_FLAT_1, 0x14, 450);
   3230 
   3231         mTask.init_state = INIT_DONE;
   3232         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "sensorInit INIT_ONC");
   3233         break;
   3234 
   3235     default:
   3236         INFO_PRINT("Invalid init_state.\n");
   3237     }
   3238 }
   3239 
   3240 static void handleSpiDoneEvt(const void* evtData)
   3241 {
   3242     TDECL();
   3243     struct BMI160Sensor *mSensor;
   3244     uint64_t SensorTime;
   3245     int16_t temperature16;
   3246     int i;
   3247     bool returnIdle = false;
   3248 
   3249     switch (GET_STATE()) {
   3250     case SENSOR_BOOT:
   3251         SET_STATE(SENSOR_VERIFY_ID);
   3252         // dummy reads after boot, wait 100us
   3253         SPI_READ(BMI160_REG_MAGIC, 1, &mTask.statusBuffer, 100);
   3254         // read the device ID for bmi160
   3255         SPI_READ(BMI160_REG_ID, 1, &mTask.dataBuffer);
   3256         spiBatchTxRx(&mTask.mode, sensorSpiCallback, &mTask, "spiDone SENSOR_BOOT");
   3257         break;
   3258     case SENSOR_VERIFY_ID:
   3259         if (mTask.dataBuffer[1] != BMI160_ID) {
   3260             T(mRetryLeft) --;
   3261             ERROR_PRINT("failed id match: %02x\n", mTask.dataBuffer[1]);
   3262             if (T(mRetryLeft) == 0)
   3263                 break;
   3264             // For some reason the first ID read will fail to get the
   3265             // correct value. need to retry a few times.
   3266             SET_STATE(SENSOR_BOOT);
   3267             if (timTimerSet(100000000, 100, 100, sensorTimerCallback, NULL, true) == 0)
   3268                 ERROR_PRINT("Couldn't get a timer to verify ID\n");
   3269             break;
   3270         } else {
   3271             SET_STATE(SENSOR_INITIALIZING);
   3272             mTask.init_state = RESET_BMI160;
   3273             sensorInit();
   3274             break;
   3275         }
   3276     case SENSOR_INITIALIZING:
   3277         if (mTask.init_state == INIT_DONE) {
   3278             DEBUG_PRINT("Done initialzing, system IDLE\n");
   3279             for (i=0; i<NUM_OF_SENSOR; i++)
   3280                 sensorRegisterInitComplete(mTask.sensors[i].handle);
   3281             // In case other tasks have already requested us before we finish booting up.
   3282             returnIdle = true;
   3283         } else {
   3284             sensorInit();
   3285         }
   3286         break;
   3287     case SENSOR_POWERING_UP:
   3288         mSensor = (struct BMI160Sensor *)evtData;
   3289         if (mSensor->idx >= FIRST_ONESHOT_SENSOR && ++mTask.active_oneshot_sensor_cnt == 1) {
   3290             // if this is the first one-shot sensor to enable, we need
   3291             // to request the accel at 50Hz.
   3292             sensorRequest(mTask.tid, mTask.sensors[ACC].handle, SENSOR_HZ(50), SENSOR_LATENCY_NODATA);
   3293             //DEBUG_PRINT("oneshot on\n");
   3294         }
   3295         sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0);
   3296         returnIdle = true;
   3297         break;
   3298     case SENSOR_POWERING_DOWN:
   3299         mSensor = (struct BMI160Sensor *)evtData;
   3300         if (mSensor->idx >= FIRST_ONESHOT_SENSOR && --mTask.active_oneshot_sensor_cnt == 0) {
   3301             // if this is the last one-shot sensor to disable, we need to
   3302             // release the accel.
   3303             sensorRelease(mTask.tid, mTask.sensors[ACC].handle);
   3304             //DEBUG_PRINT("oneshot off\n");
   3305         }
   3306         sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0);
   3307 
   3308         if (mTask.pending_dispatch) {
   3309             mTask.pending_dispatch = false;
   3310             dispatchData();
   3311         }
   3312         returnIdle = true;
   3313         break;
   3314     case SENSOR_INT_1_HANDLING:
   3315         dispatchData();
   3316         sendFlushEvt();
   3317         returnIdle = true;
   3318         break;
   3319     case SENSOR_INT_2_HANDLING:
   3320         int2Handling();
   3321         returnIdle = true;
   3322         break;
   3323     case SENSOR_CONFIG_CHANGING:
   3324         mSensor = (struct BMI160Sensor *)evtData;
   3325         sensorSignalInternalEvt(mSensor->handle,
   3326                 SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate, mSensor->latency);
   3327 
   3328         if (mTask.pending_dispatch) {
   3329             mTask.pending_dispatch = false;
   3330             dispatchData();
   3331         }
   3332 
   3333         returnIdle = true;
   3334         break;
   3335     case SENSOR_CALIBRATING:
   3336         mSensor = (struct BMI160Sensor *)evtData;
   3337         if (mTask.calibration_state == CALIBRATION_DONE) {
   3338             DEBUG_PRINT("DONE calibration\n");
   3339             returnIdle = true;
   3340         } else if (mTask.calibration_state == CALIBRATION_TIMEOUT) {
   3341             DEBUG_PRINT("Calibration TIMED OUT\n");
   3342             sendCalibrationResult(SENSOR_APP_EVT_STATUS_ERROR,
   3343                     (mSensor->idx == ACC) ? SENS_TYPE_ACCEL : SENS_TYPE_GYRO, 0, 0, 0);
   3344             returnIdle = true;
   3345         } else if (mSensor->idx == ACC) {
   3346             accCalibrationHandling();
   3347         } else if (mSensor->idx == GYR) {
   3348             gyrCalibrationHandling();
   3349         }
   3350         break;
   3351     case SENSOR_TESTING:
   3352         mSensor = (struct BMI160Sensor *)evtData;
   3353         if (mSensor->idx == ACC) {
   3354             if (mTask.acc_test_state == ACC_TEST_DONE) {
   3355                 returnIdle = true;
   3356             } else {
   3357                 accTestHandling();
   3358             }
   3359         } else if (mSensor->idx == GYR) {
   3360             if (mTask.gyro_test_state == GYRO_TEST_DONE) {
   3361                 returnIdle = true;
   3362             } else {
   3363                 gyroTestHandling();
   3364             }
   3365         }
   3366         break;
   3367     case SENSOR_STEP_CNT:
   3368         sendStepCnt();
   3369         returnIdle = true;
   3370         break;
   3371     case SENSOR_TIME_SYNC:
   3372         SensorTime = parseSensortime(mTask.sensorTimeBuffer[1] |
   3373                 (mTask.sensorTimeBuffer[2] << 8) | (mTask.sensorTimeBuffer[3] << 16));
   3374         map_sensortime_to_rtc_time(SensorTime, mTask.timesync_rtc_time);
   3375 
   3376         temperature16 = (mTask.temperatureBuffer[1] | (mTask.temperatureBuffer[2] << 8));
   3377         if (temperature16 == 0x8000) {
   3378             mTask.tempCelsius = kTempInvalid;
   3379         } else {
   3380             mTask.tempCelsius = 23.0f + temperature16 * kScale_temp;
   3381             mTask.tempTime = sensorGetTime();
   3382         }
   3383 
   3384         if (mTask.active_poll_generation == mTask.poll_generation) {
   3385             // attach the generation number to event
   3386             if (timTimerSet(kTimeSyncPeriodNs, 100, 100, timeSyncCallback,
   3387                     (void *)mTask.poll_generation, true) == 0)
   3388                 ERROR_PRINT("Couldn't get a timer for time sync\n");
   3389         }
   3390 
   3391         returnIdle = true;
   3392         break;
   3393     case SENSOR_SAVE_CALIBRATION:
   3394         DEBUG_PRINT("SENSOR_SAVE_CALIBRATION: %02x %02x %02x %02x %02x %02x %02x\n",
   3395                 mTask.dataBuffer[1], mTask.dataBuffer[2], mTask.dataBuffer[3], mTask.dataBuffer[4],
   3396                 mTask.dataBuffer[5], mTask.dataBuffer[6], mTask.dataBuffer[7]);
   3397         returnIdle = true;
   3398         break;
   3399     default:
   3400         break;
   3401     }
   3402 
   3403     if (returnIdle) {
   3404         SET_STATE(SENSOR_IDLE);
   3405         processPendingEvt();
   3406     }
   3407 }
   3408 
   3409 static void handleEvent(uint32_t evtType, const void* evtData)
   3410 {
   3411     TDECL();
   3412     uint64_t currTime;
   3413     uint8_t *packet;
   3414     float newMagBias;
   3415 
   3416     switch (evtType) {
   3417     case EVT_APP_START:
   3418         SET_STATE(SENSOR_BOOT);
   3419         T(mRetryLeft) = RETRY_CNT_ID;
   3420         osEventUnsubscribe(mTask.tid, EVT_APP_START);
   3421 
   3422         // wait 100ms for sensor to boot
   3423         currTime = timGetTime();
   3424         if (currTime < 100000000ULL) {
   3425             if (timTimerSet(100000000 - currTime, 100, 100, sensorTimerCallback, NULL, true) == 0)
   3426                 ERROR_PRINT("Couldn't get a timer for boot delay\n");
   3427             break;
   3428         }
   3429         /* We have already been powered on long enough - fall through */
   3430     case EVT_SPI_DONE:
   3431         handleSpiDoneEvt(evtData);
   3432         break;
   3433 
   3434     case EVT_APP_FROM_HOST:
   3435         packet = (uint8_t*)evtData;
   3436         if (packet[0] == sizeof(float)) {
   3437             memcpy(&newMagBias, packet+1, sizeof(float));
   3438 #ifdef MAG_SLAVE_PRESENT
   3439             magCalAddBias(&mTask.moc, (mTask.last_charging_bias_x - newMagBias), 0.0, 0.0);
   3440 #endif
   3441             mTask.last_charging_bias_x = newMagBias;
   3442             mTask.magBiasPosted = false;
   3443         }
   3444         break;
   3445 
   3446     case EVT_SENSOR_INTERRUPT_1:
   3447         initiateFifoRead(false /*isInterruptContext*/);
   3448         break;
   3449     case EVT_SENSOR_INTERRUPT_2:
   3450         int2Evt();
   3451         break;
   3452     case EVT_TIME_SYNC:
   3453         timeSyncEvt((uint32_t)evtData, true);
   3454     default:
   3455         break;
   3456     }
   3457 }
   3458 
   3459 static void initSensorStruct(struct BMI160Sensor *sensor, enum SensorIndex idx)
   3460 {
   3461     sensor->idx = idx;
   3462     sensor->powered = false;
   3463     sensor->configed = false;
   3464     sensor->rate = 0;
   3465     sensor->offset[0] = 0;
   3466     sensor->offset[1] = 0;
   3467     sensor->offset[2] = 0;
   3468     sensor->latency = 0;
   3469     sensor->data_evt = NULL;
   3470     sensor->flush = 0;
   3471     sensor->prev_rtc_time = 0;
   3472 }
   3473 
   3474 static bool startTask(uint32_t task_id)
   3475 {
   3476     TDECL();
   3477     enum SensorIndex i;
   3478     size_t slabSize;
   3479 
   3480     time_init();
   3481 
   3482     T(tid) = task_id;
   3483 
   3484     T(Int1) = gpioRequest(BMI160_INT1_PIN);
   3485     T(Isr1).func = bmi160Isr1;
   3486     T(Int2) = gpioRequest(BMI160_INT2_PIN);
   3487     T(Isr2).func = bmi160Isr2;
   3488     T(pending_int[0]) = false;
   3489     T(pending_int[1]) = false;
   3490     T(pending_step_cnt) = false;
   3491     T(pending_dispatch) = false;
   3492     T(frame_sensortime_valid) = false;
   3493     T(poll_generation) = 0;
   3494     T(tempCelsius) = kTempInvalid;
   3495     T(tempTime) = 0;
   3496 
   3497     T(mode).speed = BMI160_SPI_SPEED_HZ;
   3498     T(mode).bitsPerWord = 8;
   3499     T(mode).cpol = SPI_CPOL_IDLE_HI;
   3500     T(mode).cpha = SPI_CPHA_TRAILING_EDGE;
   3501     T(mode).nssChange = true;
   3502     T(mode).format = SPI_FORMAT_MSB_FIRST;
   3503     T(cs) = GPIO_PB(12);
   3504 
   3505     T(watermark) = 0;
   3506 
   3507     spiMasterRequest(BMI160_SPI_BUS_ID, &T(spiDev));
   3508 
   3509     for (i = FIRST_CONT_SENSOR; i < NUM_OF_SENSOR; i++) {
   3510         initSensorStruct(&T(sensors[i]), i);
   3511         T(sensors[i]).handle = sensorRegister(&mSensorInfo[i], &mSensorOps[i], NULL, false);
   3512         T(pending_config[i]) = false;
   3513     }
   3514 
   3515     osEventSubscribe(mTask.tid, EVT_APP_START);
   3516 
   3517 #ifdef ACCEL_CAL_ENABLED
   3518     // Init Accel Cal
   3519     accelCalInit(&mTask.acc,
   3520                  800000000, /* Stillness Time in ns (0.8s) */
   3521                  5,         /* Minimum Sample Number */
   3522                  0.00025,   /* Threshold */
   3523                  15,        /* nx bucket count */
   3524                  15,        /* nxb bucket count */
   3525                  15,        /* ny bucket count */
   3526                  15,        /* nyb bucket count */
   3527                  15,        /* nz bucket count */
   3528                  15,        /* nzb bucket count */
   3529                  15);       /* nle bucket count */
   3530 #endif
   3531 
   3532 #ifdef GYRO_CAL_ENABLED
   3533     // Gyro Cal -- Initialization.
   3534     gyroCalInit(&mTask.gyro_cal,
   3535                 5e9,      // min stillness period = 5 seconds
   3536                 6e9,      // max stillness period = 6 seconds
   3537                 0, 0, 0,  // initial bias offset calibration
   3538                 0,        // time stamp of initial bias calibration
   3539                 1.5e9,    // analysis window length = 1.5 seconds
   3540                 5e-5f,    // gyroscope variance threshold [rad/sec]^2
   3541                 1e-5f,    // gyroscope confidence delta [rad/sec]^2
   3542                 8e-3f,    // accelerometer variance threshold [m/sec^2]^2
   3543                 1.6e-3f,  // accelerometer confidence delta [m/sec^2]^2
   3544                 1.4f,     // magnetometer variance threshold [uT]^2
   3545                 0.25,     // magnetometer confidence delta [uT]^2
   3546                 0.95f,    // stillness threshold [0,1]
   3547                 1);       // 1=gyro calibrations will be applied
   3548 #endif
   3549 
   3550 #ifdef MAG_SLAVE_PRESENT
   3551     initMagCal(&mTask.moc,
   3552             0.0f, 0.0f, 0.0f,      // bias x, y, z
   3553             1.0f, 0.0f, 0.0f,      // c00, c01, c02
   3554             0.0f, 1.0f, 0.0f,      // c10, c11, c12
   3555             0.0f, 0.0f, 1.0f);     // c20, c21, c22
   3556 #endif
   3557 
   3558     slabSize = sizeof(struct TripleAxisDataEvent) +
   3559         MAX_NUM_COMMS_EVENT_SAMPLES * sizeof(struct TripleAxisDataPoint);
   3560 
   3561     // each event has 15 samples, with 7 bytes per sample from the fifo.
   3562     // the fifo size is 1K.
   3563     // 20 slabs because some slabs may only hold 1-2 samples.
   3564     // XXX: this consumes too much memeory, need to optimize
   3565     T(mDataSlab) = slabAllocatorNew(slabSize, 4, 20);
   3566     if (!T(mDataSlab)) {
   3567         INFO_PRINT("slabAllocatorNew() failed\n");
   3568         return false;
   3569     }
   3570     T(mWbufCnt) = 0;
   3571     T(mRegCnt) = 0;
   3572     T(spiInUse) = false;
   3573 
   3574     T(interrupt_enable_0) = 0x00;
   3575     T(interrupt_enable_2) = 0x00;
   3576 
   3577     // initialize the last bmi160 time to be ULONG_MAX, so that we know it's
   3578     // not valid yet.
   3579     T(last_sensortime) = 0;
   3580     T(frame_sensortime) = ULONG_LONG_MAX;
   3581 
   3582     // it's ok to leave interrupt open all the time.
   3583     enableInterrupt(T(Int1), &T(Isr1));
   3584     enableInterrupt(T(Int2), &T(Isr2));
   3585 
   3586     return true;
   3587 }
   3588 
   3589 static void endTask(void)
   3590 {
   3591     TDECL();
   3592 #ifdef MAG_SLAVE_PRESENT
   3593     destroy_mag_cal(&mTask.moc);
   3594 #endif
   3595 #ifdef ACCEL_CAL_ENABLED
   3596     accelCalDestroy(&mTask.acc);
   3597 #endif
   3598     slabAllocatorDestroy(T(mDataSlab));
   3599     spiMasterRelease(mTask.spiDev);
   3600 
   3601     // disable and release interrupt.
   3602     disableInterrupt(mTask.Int1, &mTask.Isr1);
   3603     disableInterrupt(mTask.Int2, &mTask.Isr2);
   3604     gpioRelease(mTask.Int1);
   3605     gpioRelease(mTask.Int2);
   3606 }
   3607 
   3608 /**
   3609  * Parse BMI160 FIFO frame without side effect.
   3610  *
   3611  * The major purpose of this function is to determine if FIFO content is received completely (start
   3612  * to see invalid headers). If not, return the pointer to the beginning last incomplete frame so
   3613  * additional read can use this pointer as start of read buffer.
   3614  *
   3615  * @param buf  buffer location
   3616  * @param size size of data to be parsed
   3617  *
   3618  * @return NULL if the FIFO is received completely; or pointer to the beginning of last incomplete
   3619  * frame for additional read.
   3620  */
   3621 static uint8_t* shallowParseFrame(uint8_t * buf, int size) {
   3622     int i = 0;
   3623     int iLastFrame = 0; // last valid frame header index
   3624 
   3625     DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf start %p: %x %x %x\n", buf, buf[0], buf[1], buf[2]);
   3626     while (size > 0) {
   3627         int fh_mode, fh_param;
   3628         iLastFrame = i;
   3629 
   3630         if (buf[i] == BMI160_FRAME_HEADER_INVALID) {
   3631             // no more data
   3632             DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf:at%d=0x80\n", iLastFrame);
   3633             return NULL;
   3634         } else if (buf[i] == BMI160_FRAME_HEADER_SKIP) {
   3635             // artifically added nop frame header, skip
   3636             DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, skip header\n", i);
   3637             i++;
   3638             size--;
   3639             continue;
   3640         }
   3641 
   3642         //++frame_num;
   3643 
   3644         fh_mode = buf[i] >> 6;
   3645         fh_param = (buf[i] >> 2) & 0xf;
   3646 
   3647         i++;
   3648         size--;
   3649 
   3650         if (fh_mode == 1) {
   3651             // control frame.
   3652             if (fh_param == 0) {
   3653                 // skip frame, we skip it (1 byte)
   3654                 i++;
   3655                 size--;
   3656                 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a skip frame\n", iLastFrame);
   3657             } else if (fh_param == 1) {
   3658                 // sensortime frame  (3 bytes)
   3659                 i += 3;
   3660                 size -= 3;
   3661                 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a sensor_time frame\n", iLastFrame);
   3662             } else if (fh_param == 2) {
   3663                 // fifo_input config frame (1byte)
   3664                 i++;
   3665                 size--;
   3666                 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a fifo cfg frame\n", iLastFrame);
   3667             } else {
   3668                 size = 0; // drop this batch
   3669                 DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "Invalid fh_param in control frame!!\n");
   3670                 // mark invalid
   3671                 buf[iLastFrame] = BMI160_FRAME_HEADER_INVALID;
   3672                 return NULL;
   3673             }
   3674         } else if (fh_mode == 2) {
   3675             // regular frame, dispatch data to each sensor's own fifo
   3676             if (fh_param & 4) { // have mag data
   3677                 i += 8;
   3678                 size -= 8;
   3679             }
   3680             if (fh_param & 2) { // have gyro data
   3681                 i += 6;
   3682                 size -= 6;
   3683             }
   3684             if (fh_param & 1) { // have accel data
   3685                 i += 6;
   3686                 size -= 6;
   3687             }
   3688             DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "at %d, a reg frame acc %d, gyro %d, mag %d\n",
   3689                        iLastFrame, fh_param &1 ? 1:0, fh_param&2?1:0, fh_param&4?1:0);
   3690         } else {
   3691             size = 0; // drop the rest of batch
   3692             DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "spf: Invalid fh_mode %d!!\n", fh_mode);
   3693             //mark invalid
   3694             buf[iLastFrame] = BMI160_FRAME_HEADER_INVALID;
   3695             return NULL;
   3696         }
   3697     }
   3698 
   3699     // there is a partial frame, return where to write next chunck of data
   3700     DEBUG_PRINT_IF(DBG_SHALLOW_PARSE, "partial frame ends %p\n", buf + iLastFrame);
   3701     return buf + iLastFrame;
   3702 }
   3703 
   3704 /**
   3705  * Intialize the first read of chunked SPI read sequence.
   3706  *
   3707  * @param index starting index of the txrxBuffer in which the data will be write into.
   3708  */
   3709 static void chunkedReadInit_(TASK, int index, int size) {
   3710 
   3711     if (GET_STATE() != SENSOR_INT_1_HANDLING) {
   3712         ERROR_PRINT("chunkedReadInit in wrong mode");
   3713         return;
   3714     }
   3715 
   3716     if (T(mRegCnt)) {
   3717         //chunked read are always executed as a single command. This should never happen.
   3718         ERROR_PRINT("SPI queue not empty at chunkedReadInit, regcnt = %d", T(mRegCnt));
   3719         // In case it did happen, we do not want to write crap to BMI160.
   3720         T(mRegCnt) = 0;
   3721     }
   3722 
   3723     T(mWbufCnt) = index;
   3724     if (T(mWbufCnt) > FIFO_READ_SIZE) {
   3725         // drop data to prevent bigger issue
   3726         T(mWbufCnt) = 0;
   3727     }
   3728     T(chunkReadSize) = size > CHUNKED_READ_SIZE ? size : CHUNKED_READ_SIZE;
   3729 
   3730     DEBUG_PRINT_IF(DBG_CHUNKED, "crd %d>>%d\n", T(chunkReadSize), index);
   3731     SPI_READ(BMI160_REG_FIFO_DATA, T(chunkReadSize), &T(dataBuffer));
   3732     spiBatchTxRx(&T(mode), chunkedReadSpiCallback, _task, __FUNCTION__);
   3733 }
   3734 
   3735 /**
   3736  * Chunked SPI read callback.
   3737  *
   3738  * Handles the chunked read logic: issue additional read if necessary, or calls sensorSpiCallback()
   3739  * if the entire FIFO is read.
   3740  *
   3741  * @param cookie extra data
   3742  * @param err    error
   3743  *
   3744  * @see sensorSpiCallback()
   3745  */
   3746 static void chunkedReadSpiCallback(void *cookie, int err) {
   3747     TASK = (_Task*) cookie;
   3748 
   3749     T(spiInUse) = false;
   3750     DEBUG_PRINT_IF(err !=0 || GET_STATE() != SENSOR_INT_1_HANDLING,
   3751             "crcb,e:%d,s:%d", err, (int)GET_STATE());
   3752     bool int1 = gpioGet(T(Int1));
   3753     if (err != 0) {
   3754         DEBUG_PRINT_IF(DBG_CHUNKED, "spi err, crd retry");
   3755         // read full fifo length to be safe
   3756         chunkedReadInit(0, FIFO_READ_SIZE);
   3757         return;
   3758     }
   3759 
   3760     *T(dataBuffer) = BMI160_FRAME_HEADER_SKIP; // fill the 0x00/0xff hole at the first byte
   3761     uint8_t* end = shallowParseFrame(T(dataBuffer), T(chunkReadSize));
   3762 
   3763     if (end == NULL) {
   3764         // if interrupt is still set after read for some reason, set the pending interrupt
   3765         // to handle it immediately after data is handled.
   3766         T(pending_int[0]) = T(pending_int[0]) || int1;
   3767 
   3768         // recover the buffer and valid data size to make it looks like a single read so that
   3769         // real frame parse works properly
   3770         T(dataBuffer) = T(txrxBuffer);
   3771         T(xferCnt) = FIFO_READ_SIZE;
   3772         sensorSpiCallback(cookie, err);
   3773     } else {
   3774         DEBUG_PRINT_IF(DBG_CHUNKED, "crd cont");
   3775         chunkedReadInit(end - T(txrxBuffer), CHUNKED_READ_SIZE);
   3776     }
   3777 }
   3778 
   3779 /**
   3780  * Initiate read of sensor fifo.
   3781  *
   3782  * If task is in idle state, init chunked FIFO read; otherwise, submit an interrupt message or mark
   3783  * the read pending depending if it is called in interrupt context.
   3784  *
   3785  * @param isInterruptContext true if called from interrupt context; false otherwise.
   3786  *
   3787  */
   3788 static void initiateFifoRead_(TASK, bool isInterruptContext) {
   3789     if (trySwitchState(SENSOR_INT_1_HANDLING)) {
   3790         // estimate first read size to be watermark + 1 more sample + some extra
   3791         int firstReadSize = T(watermark) * 4 + 32; // 1+6+6+8+1+3 + extra = 25 + extra = 32
   3792         if (firstReadSize < CHUNKED_READ_SIZE) {
   3793             firstReadSize = CHUNKED_READ_SIZE;
   3794         }
   3795         chunkedReadInit(0, firstReadSize);
   3796     } else {
   3797         if (isInterruptContext) {
   3798             // called from interrupt context, queue event
   3799             if (!osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, _task, NULL, T(tid)))
   3800                 ERROR_PRINT("initiateFifoRead_: osEnqueuePrivateEvt() failed\n");
   3801         } else {
   3802             // non-interrupt context, set pending flag, so next time it will be picked up after
   3803             // switching back to idle.
   3804             // Note: even if we are still in SENSOR_INT_1_HANDLING, the SPI may already finished and
   3805             // we need to issue another SPI read to get the latest status.
   3806             T(pending_int[0]) = true;
   3807         }
   3808     }
   3809 }
   3810 
   3811 /**
   3812  * Calculate fifo size using normalized input.
   3813  *
   3814  * @param iPeriod normalized period vector
   3815  * @param iLatency normalized latency vector
   3816  * @param factor vector that contains size factor for each sensor
   3817  * @param n size of the vectors
   3818  *
   3819  * @return max size of FIFO to guarantee latency requirements of all sensors or SIZE_MAX if no
   3820  * sensor is active.
   3821  */
   3822 static size_t calcFifoSize(const int* iPeriod, const int* iLatency, const int* factor, int n) {
   3823     int i;
   3824 
   3825     int minLatency = INT_MAX;
   3826     for (i = 0; i < n; i++) {
   3827         if (iLatency[i] > 0) {
   3828             minLatency = iLatency[i] < minLatency ? iLatency[i] : minLatency;
   3829         }
   3830     }
   3831     DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo: min latency %d unit", minLatency);
   3832 
   3833     bool anyActive = false;
   3834     size_t s = 0;
   3835     size_t head = 0;
   3836     for (i = 0; i < n; i++) {
   3837         if (iPeriod[i] > 0) {
   3838             anyActive = true;
   3839             size_t t =  minLatency / iPeriod[i];
   3840             head = t > head ? t : head;
   3841             s += t * factor[i];
   3842             DEBUG_PRINT_IF(DBG_WM_CALC, "cfifo: %d, s+= %d*%d, head = %d", i, t, factor[i], head);
   3843         }
   3844     }
   3845 
   3846     return anyActive ? head + s : SIZE_MAX;
   3847 }
   3848 
   3849 /**
   3850  * Calculate the watermark setting from sensor registration information
   3851  *
   3852  * It is assumed  that all sensor period share a common denominator (true for BMI160) and the
   3853  * latency of sensor will be lower bounded by its sampling period.
   3854  *
   3855  * @return watermark register setting
   3856  */
   3857 static uint8_t calcWatermark2_(TASK) {
   3858     int period[] = {-1, -1, -1};
   3859     int latency[] = {-1, -1, -1};
   3860     const int factor[] = {6, 6, 8};
   3861     int i;
   3862 
   3863     for (i = FIRST_CONT_SENSOR; i < NUM_CONT_SENSOR; ++i) {
   3864         if (T(sensors[i]).configed) {
   3865             period[i - ACC] = SENSOR_HZ((float)WATERMARK_MAX_SENSOR_RATE) / T(sensors[i]).rate;
   3866             latency[i - ACC] = U64_DIV_BY_U64_CONSTANT(
   3867                     T(sensors[i]).latency + WATERMARK_TIME_UNIT_NS/2, WATERMARK_TIME_UNIT_NS);
   3868             DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2: f %dHz, l %dus => T %d unit, L %d unit",
   3869                     (int) T(sensors[i]).rate/1024,
   3870                     (int) U64_DIV_BY_U64_CONSTANT(T(sensors[i]).latency, 1000),
   3871                     period[i-ACC], latency[i-ACC]);
   3872         }
   3873     }
   3874 
   3875 
   3876     size_t watermark = calcFifoSize(period, latency, factor, NUM_CONT_SENSOR) / 4;
   3877     DEBUG_PRINT_IF(DBG_WM_CALC, "cwm2: wm = %d", watermark);
   3878     watermark = watermark < WATERMARK_MIN ? WATERMARK_MIN : watermark;
   3879     watermark = watermark > WATERMARK_MAX ? WATERMARK_MAX : watermark;
   3880 
   3881     return watermark;
   3882 }
   3883 
   3884 static bool dumpBinaryPutC(void* p, char c) {
   3885     *(*(char**)p)++ = c;
   3886     return true;
   3887 }
   3888 
   3889 static uint32_t cvprintf_ellipsis(printf_write_c writeF, void* writeD, const char* fmtStr, ...) {
   3890     va_list vl;
   3891     uint32_t ret;
   3892 
   3893     va_start(vl, fmtStr);
   3894     ret = cvprintf(writeF, writeD, fmtStr, vl);
   3895     va_end(vl);
   3896 
   3897     return ret;
   3898 }
   3899 
   3900 static void dumpBinary(void* buf, unsigned int address, size_t size) {
   3901     size_t i, j;
   3902     char buffer[5+16*3+1+2]; //5: address, 3:each byte+space, 1: middle space, 1: \n and \0
   3903     char* p;
   3904 
   3905     for (i = 0; i < size; ) {
   3906         p = buffer;
   3907         cvprintf_ellipsis(dumpBinaryPutC, &p, "%08x:", address);
   3908         for (j = 0; j < 0x10 && i < size; ++i, ++j) {
   3909             if (j == 0x8) {
   3910                 *p++ = ' ';
   3911             }
   3912             cvprintf_ellipsis(dumpBinaryPutC, &p, " %02x", ((unsigned char *)buf)[i]);
   3913         }
   3914         *p = '\0';
   3915 
   3916         osLog(LOG_INFO, "%s\n", buffer);
   3917         address += 0x10;
   3918     }
   3919 }
   3920 
   3921 INTERNAL_APP_INIT(BMI160_APP_ID, BMI160_APP_VERSION, startTask, endTask, handleEvent);
   3922