/*
 * Copyright (C) 2016-2017 STMicroelectronics
 *
 * Author: Denis Ciocca <denis.ciocca@st.com>
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <stdlib.h>
#include <string.h>
#include <sensors.h>
#include <slab.h>
#include <heap.h>
#include <halIntf.h>
#include <spi.h>
#include <gpio.h>
#include <atomic.h>
#include <timer.h>
#include <printf.h>
#include <isr.h>
#include <hostIntf.h>
#include <nanohubPacket.h>
#include <cpu/cpuMath.h>
#include <variant/sensType.h>
#include <plat/gpio.h>
#include <plat/syscfg.h>
#include <plat/exti.h>
#include <plat/rtc.h>
#include <calibration/accelerometer/accel_cal.h>
#include <calibration/gyroscope/gyro_cal.h>
#include <calibration/magnetometer/mag_cal/mag_cal.h>
#include <calibration/over_temp/over_temp_cal.h>
#include <algos/time_sync.h>

#include "st_lsm6dsm_lis3mdl_slave.h"
#include "st_lsm6dsm_lsm303agr_slave.h"
#include "st_lsm6dsm_ak09916_slave.h"
#include "st_lsm6dsm_lps22hb_slave.h"

#define LSM6DSM_APP_VERSION 2

#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) || defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
#define LSM6DSM_I2C_MASTER_ENABLED                      1
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

#if defined(LSM6DSM_MAGN_CALIB_ENABLED) && !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
#pragma message("LSM6DSM_MAGN_CALIB_ENABLED can not be used if no magnetometer sensors are enabled on I2C master. Disabling it!")
#undef LSM6DSM_MAGN_CALIB_ENABLED
#endif /* LSM6DSM_MAGN_CALIB_ENABLED, LSM6DSM_I2C_MASTER_ENABLED */

#if defined(LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP) && !defined(LSM6DSM_I2C_MASTER_ENABLED)
#pragma message("LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP has no meaning if no sensors are enabled on I2C master. Discarding it!")
#endif /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP, LSM6DSM_I2C_MASTER_ENABLED */

#if defined(LSM6DSM_OVERTEMP_CALIB_ENABLED) && !defined(LSM6DSM_GYRO_CALIB_ENABLED)
#pragma message("LSM6DSM_OVERTEMP_CALIB_ENABLED has no meaning if gyro calibration is not enabled. Discarding it!")
#undef LSM6DSM_OVERTEMP_CALIB_ENABLED
#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED, LSM6DSM_GYRO_CALIB_ENABLED */

#if !defined(LSM6DSM_SPI_SLAVE_BUS_ID) || !defined(LSM6DSM_SPI_SLAVE_FREQUENCY_HZ) || !defined(LSM6DSM_SPI_SLAVE_CS_GPIO)
#error "SPI macros not fully defined. Please check README file"
#endif /* LSM6DSM_SPI_SLAVE_BUS_ID, LSM6DSM_SPI_SLAVE_FREQUENCY_HZ, LSM6DSM_SPI_SLAVE_CS_GPIO */

#if !defined(LSM6DSM_INT_IRQ) || !defined(LSM6DSM_INT1_GPIO)
#error "Interrupts macros not fully defined. Please check README file"
#endif /* LSM6DSM_INT_IRQ, LSM6DSM_INT1_GPIO */

#if !defined(LSM6DSM_ACCEL_GYRO_ROT_MATRIX)
#error "Accel/gyro rotation matrix macro not defined. Please check README file"
#endif /* LSM6DSM_ACCEL_GYRO_ROT_MATRIX */

#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
#if !defined(LSM6DSM_MAGN_ROT_MATRIX)
#error "Magn rotation matrix macro not defined. Please check README file"
#endif /* LSM6DSM_MAGN_ROT_MATRIX */
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

#define LSM6DSM_APP_ID                                  APP_ID_MAKE(NANOHUB_VENDOR_STMICRO, 0)

#define LSM6DSM_WAI_VALUE                               (0x6a)
#define LSM6DSM_RETRY_CNT_WAI                           5               /* Retry #n times if WAI value is wrong. Maybe HW is not ready after power on */
#define LSM6DSM_ACCEL_KSCALE                            0.00239364f     /* Accel scale @8g in (m/s^2)/LSB */
#define LSM6DSM_GYRO_KSCALE                             0.00122173f     /* Gyro scale @2000dps in (rad/sec)/LSB */
#define LSM6DSM_ONE_SAMPLE_BYTE                         6               /* One sample of triaxial sensor is expressed on 6 byte */
#define LSM6DSM_TEMP_SAMPLE_BYTE                        2               /* One sample of temperature sensor is expressed on 2 byte */
#define LSM6DSM_TIMESTAMP_SAMPLE_BYTE                   3               /* One sample of timestamp is expressed on 3 byte */
#define LSM6DSM_TEMP_OFFSET                             (25.0f)
#define LSM6DSM_SC_DELTA_TIME_PERIOD_SEC                (1.6384f)       /* Step counter deltatime resolution */
#define LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE              15
#define LSM6DSM_MAX_WATERMARK_VALUE                     600             /* 4096byte = 682 samples, use 600 to avoid overflow */
#define LSM6DSM_TIME_RESOLUTION                         25000UL         /* 25us [ns] */
#define LSM6DSM_MASK_24BIT_TIMESTAMP                    0x00ffffff      /* mask to select 24bit data from 32bit storage data type */
#define LSM6DSM_TIMEDIFF_OVERFLOW_LSB                   8388608LL       /* If deltatime is bigger than 2^23 it means timer is overflowed */
#define LSM6DSM_SYNC_DELTA_INTERVAL                     100000000ULL    /* Sensor timestamp is synced with MCU every #n deltatime [ns] */
#define LSM6DSM_TRIAXIAL_NUM_AXIS                       3

/* SPI buffers */
#define LSM6DSM_SPI_PACKET_SIZE                         75
#define LSM6DSM_SPI_FIFO_SIZE                           1024
#define LSM6DSM_BUF_MARGIN                              100
#define SPI_BUF_SIZE                                    (LSM6DSM_SPI_FIFO_SIZE + LSM6DSM_BUF_MARGIN)

/* LSM6DSM status check registers */
#define LSM6DSM_FUNC_SRC_STEP_DETECTED                  (0x10)
#define LSM6DSM_FUNC_SRC_STEP_COUNT_DELTA_IA            (0x80)
#define LSM6DSM_FUNC_SRC_SIGN_MOTION                    (0x40)
#define LSM6DSM_FIFO_STATUS2_FIFO_EMPTY                 (0x10)
#define LSM6DSM_FIFO_STATUS2_FIFO_FULL_SMART            (0x20)
#define LSM6DSM_FIFO_STATUS2_FIFO_FULL_OVERRUN          (0x40)
#define LSM6DSM_FIFO_STATUS2_FIFO_ERROR                 (LSM6DSM_FIFO_STATUS2_FIFO_EMPTY | \
                                                         LSM6DSM_FIFO_STATUS2_FIFO_FULL_SMART | \
                                                         LSM6DSM_FIFO_STATUS2_FIFO_FULL_OVERRUN)

/* LSM6DSM ODR related */
#define LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON              80000
#define LSM6DSM_ODR_12HZ_ACCEL_STD                      1
#define LSM6DSM_ODR_26HZ_ACCEL_STD                      1
#define LSM6DSM_ODR_52HZ_ACCEL_STD                      1
#define LSM6DSM_ODR_104HZ_ACCEL_STD                     1
#define LSM6DSM_ODR_208HZ_ACCEL_STD                     1
#define LSM6DSM_ODR_416HZ_ACCEL_STD                     1
#define LSM6DSM_ODR_12HZ_GYRO_STD                       2
#define LSM6DSM_ODR_26HZ_GYRO_STD                       3
#define LSM6DSM_ODR_52HZ_GYRO_STD                       3
#define LSM6DSM_ODR_104HZ_GYRO_STD                      3
#define LSM6DSM_ODR_208HZ_GYRO_STD                      3
#define LSM6DSM_ODR_416HZ_GYRO_STD                      3

#define LSM6DSM_ODR_12HZ_REG_VALUE                      (0x10)
#define LSM6DSM_ODR_26HZ_REG_VALUE                      (0x20)
#define LSM6DSM_ODR_52HZ_REG_VALUE                      (0x30)
#define LSM6DSM_ODR_104HZ_REG_VALUE                     (0x40)
#define LSM6DSM_ODR_208HZ_REG_VALUE                     (0x50)
#define LSM6DSM_ODR_416HZ_REG_VALUE                     (0x60)

#define LSM6DSM_INT_FIFO_FTH_ENABLE_REG_VALUE           (0x08)
#define LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE      (0x80)
#define LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE       (0x80)
#define LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE        (0x40)

/* LSM6DSM registers */
#define LSM6DSM_FUNC_CFG_ACCESS_ADDR                    (0x01)
#define LSM6DSM_FIFO_CTRL1_ADDR                         (0x06)
#define LSM6DSM_FIFO_CTRL5_ADDR                         (0x0a)
#define LSM6DSM_DRDY_PULSE_CFG_ADDR                     (0x0b)
#define LSM6DSM_INT1_CTRL_ADDR                          (0x0d)
#define LSM6DSM_INT2_CTRL_ADDR                          (0x0e)
#define LSM6DSM_WAI_ADDR                                (0x0f)
#define LSM6DSM_CTRL1_XL_ADDR                           (0x10)
#define LSM6DSM_CTRL2_G_ADDR                            (0x11)
#define LSM6DSM_CTRL3_C_ADDR                            (0x12)
#define LSM6DSM_CTRL4_C_ADDR                            (0x13)
#define LSM6DSM_CTRL5_C_ADDR                            (0x14)
#define LSM6DSM_EBD_STEP_COUNT_DELTA_ADDR               (0x15)
#define LSM6DSM_CTRL10_C_ADDR                           (0x19)
#define LSM6DSM_MASTER_CONFIG_ADDR                      (0x1a)
#define LSM6DSM_STATUS_REG_ADDR                         (0x1e)
#define LSM6DSM_OUT_TEMP_L_ADDR                         (0x20)
#define LSM6DSM_OUTX_L_G_ADDR                           (0x22)
#define LSM6DSM_OUTX_L_XL_ADDR                          (0x28)
#define LSM6DSM_OUT_SENSORHUB1_ADDR                     (0x2e)
#define LSM6DSM_FIFO_STATUS1_ADDR                       (0x3a)
#define LSM6DSM_FIFO_DATA_OUT_L_ADDR                    (0x3e)
#define LSM6DSM_TIMESTAMP0_REG_ADDR                     (0x40)
#define LSM6DSM_TIMESTAMP2_REG_ADDR                     (0x42)
#define LSM6DSM_STEP_COUNTER_L_ADDR                     (0x4b)
#define LSM6DSM_FUNC_SRC_ADDR                           (0x53)
#define LSM6DSM_WAKE_UP_DUR_ADDR                        (0x5c)
#define LSM6DSM_X_OFS_USR_ADDR                          (0x73)

#define LSM6DSM_SW_RESET                                (0x01)
#define LSM6DSM_RESET_PEDOMETER                         (0x02)
#define LSM6DSM_ENABLE_FUNC_CFG_ACCESS                  (0x80)
#define LSM6DSM_ENABLE_DIGITAL_FUNC                     (0x04)
#define LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC           (0x10)
#define LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC         (0x01)
#define LSM6DSM_MASTER_CONFIG_PULL_UP_EN                (0x08)
#define LSM6DSM_MASTER_CONFIG_MASTER_ON                 (0x01)
#define LSM6DSM_ENABLE_FIFO_TIMESTAMP                   (0x80)
#define LSM6DSM_TIMESTAMP2_REG_RESET_TIMESTAMP          (0xaa)

/* LSM6DSM fifo modes */
#define LSM6DSM_FIFO_BYPASS_MODE                        (0x00)
#define LSM6DSM_FIFO_CONTINUOS_MODE                     (0x36)
#define LSM6DSM_FIFO_CTRL2_FTH_MASK                     (0x07)

/* LSM6DSM fifo decimators */
#define LSM6DSM_FIFO_SAMPLE_NOT_IN_FIFO                 (0x00)
#define LSM6DSM_FIFO_NO_DECIMATION                      (0x01)
#define LSM6DSM_FIFO_DECIMATION_FACTOR_2                (0x02)
#define LSM6DSM_FIFO_DECIMATION_FACTOR_3                (0x03)
#define LSM6DSM_FIFO_DECIMATION_FACTOR_4                (0x04)
#define LSM6DSM_FIFO_DECIMATION_FACTOR_8                (0x05)
#define LSM6DSM_FIFO_DECIMATION_FACTOR_16               (0x06)
#define LSM6DSM_FIFO_DECIMATION_FACTOR_32               (0x07)

/* LSM6DSM selftest related */
#define LSM6DSM_NUM_AVERAGE_SELFTEST                    5
#define LSM6DSM_NUM_AVERAGE_SELFTEST_SLOW               30
#define LSM6DSM_ACCEL_SELFTEST_PS                       (0x01)
#define LSM6DSM_GYRO_SELFTEST_PS                        (0x04)
#define LSM6DSM_ACCEL_SELFTEST_NS                       (0x02)
#define LSM6DSM_GYRO_SELFTEST_NS                        (0x0c)
#define LSM6DSM_ACCEL_SELFTEST_HIGH_THR_LSB             6967            /* 1700mg @8g in LSB */
#define LSM6DSM_ACCEL_SELFTEST_LOW_THR_LSB              368             /* 90mg @8g in LSB */
#define LSM6DSM_GYRO_SELFTEST_HIGH_THR_LSB              10000           /* 700dps @2000dps in LSB */
#define LSM6DSM_GYRO_SELFTEST_LOW_THR_LSB               2142            /* 150dps @2000dps in LSB */

/* LSM6DSM calibration related */
#define LSM6DSM_NUM_AVERAGE_CALIBRATION                 10
#define LSM6DSM_1G_IN_LSB_CALIBRATION                   4098            /* 1000mg @8g in LSB */
#define LSM6DSM_ACCEL_MAX_CALIBRATION_THR_LSB           127             /* 8-bit available */
#define LSM6DSM_ACCEL_LSB_TO_OFFSET_DIGIT_SCALE         0.2501f         /* @8g */

/* LSM6DSM embedded registers */
#define LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR                 (0x02)
#define LSM6DSM_EMBEDDED_SLV0_SUBADDR_ADDR              (0x03)
#define LSM6DSM_EMBEDDED_SLV0_CONFIG_ADDR               (0x04)
#define LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR                 (0x05)
#define LSM6DSM_EMBEDDED_SLV1_SUBADDR_ADDR              (0x06)
#define LSM6DSM_EMBEDDED_SLV1_CONFIG_ADDR               (0x07)
#define LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR                 (0x08)
#define LSM6DSM_EMBEDDED_SLV2_SUBADDR_ADDR              (0x09)
#define LSM6DSM_EMBEDDED_SLV2_CONFIG_ADDR               (0x0a)
#define LSM6DSM_EMBEDDED_SLV3_ADDR_ADDR                 (0x0b)
#define LSM6DSM_EMBEDDED_SLV3_SUBADDR_ADDR              (0x0c)
#define LSM6DSM_EMBEDDED_SLV3_CONFIG_ADDR               (0x0d)
#define LSM6DSM_EMBEDDED_DATAWRITE_SLV0_ADDR            (0x0e)
#define LSM6DSM_EMBEDDED_STEP_COUNT_DELTA_ADDR          (0x15)

#define LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB             (0x01)
#define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR     (0x10)
#define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR     (0x20)
#define LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_THREE_SENSOR   (0x30)
#define LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE         (0x20)
#define LSM6DSM_EMBEDDED_SLV0_WRITE_ADDR_SLEEP          (0x07)

/* LSM6DSM I2C master - slave devices */
#ifdef LSM6DSM_I2C_MASTER_LIS3MDL
#define LSM6DSM_MAGN_KSCALE                             LIS3MDL_KSCALE
#define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT         LIS3MDL_I2C_ADDRESS
#define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR        LIS3MDL_WAI_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR            LIS3MDL_CTRL2_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE           LIS3MDL_SW_RESET
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR            LIS3MDL_CTRL3_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE            LIS3MDL_CTRL3_BASE
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE        LIS3MDL_POWER_ON_VALUE
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE       LIS3MDL_POWER_OFF_VALUE
#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR              LIS3MDL_CTRL1_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE              LIS3MDL_CTRL1_BASE
#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR          LIS3MDL_OUTDATA_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN           LIS3MDL_OUTDATA_LEN
#define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i)    LIS3MDLMagnRatesRegValue[i]
#endif /* LSM6DSM_I2C_MASTER_LIS3MDL */

#ifdef LSM6DSM_I2C_MASTER_AK09916
#define LSM6DSM_MAGN_KSCALE                             AK09916_KSCALE
#define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT         AK09916_I2C_ADDRESS
#define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR        AK09916_WAI_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR            AK09916_CNTL3_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE           AK09916_SW_RESET
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR            AK09916_CNTL2_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE            AK09916_CNTL2_BASE
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE        AK09916_POWER_ON_VALUE
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE       AK09916_POWER_OFF_VALUE
#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR              AK09916_CNTL2_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE              AK09916_CNTL2_BASE
#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR          AK09916_OUTDATA_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN           AK09916_OUTDATA_LEN
#define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i)    AK09916MagnRatesRegValue[i]
#endif /* LSM6DSM_I2C_MASTER_AK09916 */

#ifdef LSM6DSM_I2C_MASTER_LSM303AGR
#define LSM6DSM_MAGN_KSCALE                             LSM303AGR_KSCALE
#define LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT         LSM303AGR_I2C_ADDRESS
#define LSM6DSM_SENSOR_SLAVE_MAGN_DUMMY_REG_ADDR        LSM303AGR_WAI_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR            LSM303AGR_CFG_REG_A_M_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE           LSM303AGR_SW_RESET
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR            LSM303AGR_CFG_REG_A_M_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE            LSM303AGR_CFG_REG_A_M_BASE
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE        LSM303AGR_POWER_ON_VALUE
#define LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE       LSM303AGR_POWER_OFF_VALUE
#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR              LSM303AGR_CFG_REG_A_M_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE              LSM303AGR_CFG_REG_A_M_BASE
#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR          LSM303AGR_OUTDATA_ADDR
#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN           LSM303AGR_OUTDATA_LEN
#define LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i)    LSM303AGRMagnRatesRegValue[i]
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR */

#ifdef LSM6DSM_I2C_MASTER_LPS22HB
#define LSM6DSM_PRESS_KSCALE                            LPS22HB_PRESS_KSCALE
#define LSM6DSM_TEMP_KSCALE                             LPS22HB_TEMP_KSCALE
#define LSM6DSM_PRESS_OUTDATA_LEN                       LPS22HB_OUTDATA_PRESS_BYTE
#define LSM6DSM_TEMP_OUTDATA_LEN                        LPS22HB_OUTDATA_TEMP_BYTE
#define LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT         LPS22HB_I2C_ADDRESS
#define LSM6DSM_SENSOR_SLAVE_BARO_DUMMY_REG_ADDR        LPS22HB_WAI_ADDR
#define LSM6DSM_SENSOR_SLAVE_BARO_RESET_ADDR            LPS22HB_CTRL2_ADDR
#define LSM6DSM_SENSOR_SLAVE_BARO_RESET_VALUE           LPS22HB_SW_RESET
#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR            LPS22HB_CTRL1_ADDR
#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE            LPS22HB_CTRL1_BASE
#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_ON_VALUE        LPS22HB_POWER_ON_VALUE
#define LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE       LPS22HB_POWER_OFF_VALUE
#define LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR              LPS22HB_CTRL1_ADDR
#define LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE              LPS22HB_CTRL1_BASE
#define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_ADDR          LPS22HB_OUTDATA_ADDR
#define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN           LPS22HB_OUTDATA_LEN
#define LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i)    LPS22HBBaroRatesRegValue[i]
#endif /* LSM6DSM_I2C_MASTER_LPS22HB */

#ifndef LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN
#define LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN           0
#endif /* LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN */
#ifndef LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN
#define LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN           0
#endif /* LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN */

/* Magn only enabled */
#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
#ifdef LSM6DSM_I2C_MASTER_AK09916
#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE           LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR
#else /* LSM6DSM_I2C_MASTER_AK09916 */
#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE           LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR
#endif /* LSM6DSM_I2C_MASTER_AK09916 */
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */

/* Baro only enabled */
#if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE           LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_ONE_SENSOR
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */

/* Magn & Baro both enabled */
#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
#ifdef LSM6DSM_I2C_MASTER_AK09916
#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE           LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_THREE_SENSOR
#else /* LSM6DSM_I2C_MASTER_AK09916 */
#define LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE           LSM6DSM_EMBEDDED_SENSOR_HUB_HAVE_TWO_SENSOR
#endif /* LSM6DSM_I2C_MASTER_AK09916 */
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */


/* LSM6DSM default base registers status */
/* LSM6DSM_FUNC_CFG_ACCESS_BASE: enable embedded functions register */
#define LSM6DSM_FUNC_CFG_ACCESS_BASE                    (0x00)

/* LSM6DSM_DRDY_PULSE_CFG_BASE: enable pulsed interrupt register */
#define LSM6DSM_DRDY_PULSE_CFG_BASE                     (0x00)

/* LSM6DSM_INT1_CTRL_BASE: interrupt 1 control register default settings */
#define LSM6DSM_INT1_CTRL_BASE                          ((0 << 7) |    /* INT1_STEP_DETECTOR */ \
                                                         (0 << 6) |    /* INT1_SIGN_MOT */ \
                                                         (1 << 5) |    /* INT1_FULL_FLAG */ \
                                                         (1 << 4) |    /* INT1_FIFO_OVR */ \
                                                         (1 << 3) |    /* INT1_FTH */ \
                                                         (0 << 2) |    /* INT1_BOOT */ \
                                                         (0 << 1) |    /* INT1_DRDY_G */ \
                                                         (0 << 0))     /* INT1_DRDY_XL */

/* LSM6DSM_INT2_CTRL_BASE: interrupt 2 control register default settings */
#define LSM6DSM_INT2_CTRL_BASE                          ((0 << 7) |    /* INT2_STEP_DELTA */ \
                                                         (0 << 6) |    /* INT2_STEP_OV */ \
                                                         (0 << 5) |    /* INT2_FULL_FLAG */ \
                                                         (0 << 4) |    /* INT2_FIFO_OVR */ \
                                                         (0 << 3) |    /* INT2_FTH */ \
                                                         (0 << 2) |    /* INT2_DRDY_TEMP */ \
                                                         (0 << 1) |    /* INT2_DRDY_G */ \
                                                         (0 << 0))     /* INT2_DRDY_XL */

/* LSM6DSM_CTRL1_XL_BASE: accelerometer sensor register default settings */
#define LSM6DSM_CTRL1_XL_BASE                           ((0 << 7) |    /* ODR_XL3 */ \
                                                         (0 << 6) |    /* ODR_XL2 */ \
                                                         (0 << 5) |    /* ODR_XL1 */ \
                                                         (0 << 4) |    /* ODR_XL0 */ \
                                                         (1 << 3) |    /* FS_XL1 */ \
                                                         (1 << 2) |    /* FS_XL0 */ \
                                                         (0 << 1) |    /* LPF1_BW_SEL */ \
                                                         (0 << 0))     /* (0) */

/* LSM6DSM_CTRL2_G_BASE: gyroscope sensor register default settings */
#define LSM6DSM_CTRL2_G_BASE                            ((0 << 7) |    /* ODR_G3 */ \
                                                         (0 << 6) |    /* ODR_G2 */ \
                                                         (0 << 5) |    /* ODR_G1 */ \
                                                         (0 << 4) |    /* ODR_G0 */ \
                                                         (1 << 3) |    /* FS_G1 */ \
                                                         (1 << 2) |    /* FS_G0 */ \
                                                         (0 << 1) |    /* FS_125 */ \
                                                         (0 << 0))     /* (0) */

/* LSM6DSM_CTRL3_C_BASE: control register 3 default settings */
#define LSM6DSM_CTRL3_C_BASE                            ((0 << 7) |    /* BOOT */ \
                                                         (1 << 6) |    /* BDU */ \
                                                         (0 << 5) |    /* H_LACTIVE */ \
                                                         (0 << 4) |    /* PP_OD */ \
                                                         (0 << 3) |    /* SIM */ \
                                                         (1 << 2) |    /* IF_INC */ \
                                                         (0 << 1) |    /* BLE */ \
                                                         (0 << 0))     /* SW_RESET */

/* LSM6DSM_CTRL4_C_BASE: control register 4 default settings */
#define LSM6DSM_CTRL4_C_BASE                            ((0 << 7) |    /* DEN_XL_EN */ \
                                                         (0 << 6) |    /* SLEEP */ \
                                                         (1 << 5) |    /* INT2_on_INT1 */ \
                                                         (0 << 4) |    /* DEN_DRDY_MASK */ \
                                                         (0 << 3) |    /* DRDY_MASK */ \
                                                         (1 << 2) |    /* I2C_disable */ \
                                                         (0 << 1) |    /* LPF1_SEL_G */ \
                                                         (0 << 0))     /* (0) */

/* LSM6DSM_CTRL5_C_BASE: control register 5 default settings */
#define LSM6DSM_CTRL5_C_BASE                            (0x00)

/* LSM6DSM_CTRL10_C_BASE: control register 10 default settings */
#define LSM6DSM_CTRL10_C_BASE                           ((0 << 7) |    /* (WRIST_TILT_EN) */ \
                                                         (0 << 6) |    /* (0) */ \
                                                         (1 << 5) |    /* TIMER_EN */ \
                                                         (0 << 4) |    /* PEDO_EN */ \
                                                         (0 << 3) |    /* TILT_EN */ \
                                                         (1 << 2) |    /* FUNC_EN */ \
                                                         (0 << 1) |    /* PEDO_RST_STEP */ \
                                                         (0 << 0))     /* SIGN_MOTION_EN */

/* LSM6DSM_MASTER_CONFIG_BASE: I2C master configuration register default value */
#ifdef LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP
#define LSM6DSM_MASTER_CONFIG_BASE                      (LSM6DSM_MASTER_CONFIG_PULL_UP_EN)
#else /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP */
#define LSM6DSM_MASTER_CONFIG_BASE                      (0x00)
#endif /* LSM6DSM_I2C_MASTER_USE_INTERNAL_PULLUP */

/* LSM6DSM_WAKE_UP_DUR_BASE: control register WK default settings */
#define LSM6DSM_WAKE_UP_DUR_BASE                        (0x10)         /* TIMER_HR */

#define LSM6DSM_X_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
                                                        ((r11 == 1 ? x : (r11 == -1 ? -x : 0)) + \
                                                        (r21 == 1 ? y : (r21 == -1 ? -y : 0)) + \
                                                        (r31 == 1 ? z : (r31 == -1 ? -z : 0)))

#define LSM6DSM_Y_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
                                                        ((r12 == 1 ? x : (r12 == -1 ? -x : 0)) + \
                                                        (r22 == 1 ? y : (r22 == -1 ? -y : 0)) + \
                                                        (r32 == 1 ? z : (r32 == -1 ? -z : 0)))

#define LSM6DSM_Z_MAP(x, y, z, r11, r12, r13, r21, r22, r23, r31, r32, r33) \
                                                        ((r13 == 1 ? x : (r13 == -1 ? -x : 0)) + \
                                                        (r23 == 1 ? y : (r23 == -1 ? -y : 0)) + \
                                                        (r33 == 1 ? z : (r33 == -1 ? -z : 0)))

#define LSM6DSM_REMAP_X_DATA(...)                       LSM6DSM_X_MAP(__VA_ARGS__)
#define LSM6DSM_REMAP_Y_DATA(...)                       LSM6DSM_Y_MAP(__VA_ARGS__)
#define LSM6DSM_REMAP_Z_DATA(...)                       LSM6DSM_Z_MAP(__VA_ARGS__)

enum SensorIndex {
    GYRO = 0,
    ACCEL,
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
    MAGN,
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
    PRESS,
    TEMP,
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    STEP_DETECTOR,
    STEP_COUNTER,
    SIGN_MOTION,
    NUM_SENSORS,
    EMBEDDED_TIMESTAMP
};

enum SensorFifoIndex {
    FIFO_GYRO,
    FIFO_ACCEL,
    FIFO_DS3,
    FIFO_DS4,
    FIFO_NUM
};

enum InitState {
    RESET_LSM6DSM = 0,
    INIT_LSM6DSM,
#ifdef LSM6DSM_I2C_MASTER_ENABLED
    INIT_I2C_MASTER_REGS_CONF,
    INIT_I2C_MASTER_SENSOR_RESET,
    INIT_I2C_MASTER_MAGN_SENSOR,
    INIT_I2C_MASTER_BARO_SENSOR,
    INIT_I2C_MASTER_SENSOR_END,
#endif /* LSM6DSM_I2C_MASTER_ENABLED */
    INIT_DONE,
};

enum SelfTestState {
    SELFTEST_INITIALIZATION = 0,
    SELFTEST_READ_EST_DATA,
    SELFTEST_SECOND_STEP_INITIALIZATION,
    SELFTEST_READ_NST_DATA,
    SELFTEST_VERIFICATION,
    SELFTEST_COMPLETED
};

enum CalibrationState {
    CALIBRATION_INITIALIZATION = 0,
    CALIBRATION_READ_DATA,
    CALIBRATION_VERIFICATION,
    CALIBRATION_COMPLETED
};

enum SensorEvents {
    NO_EVT = -1,
    EVT_SPI_DONE = EVT_APP_START + 1,
    EVT_START_ACCEL_TIME_CALIB,
    EVT_SENSOR_INTERRUPT_1,
    EVT_SENSOR_POWERING_UP,
    EVT_SENSOR_POWERING_DOWN,
    EVT_SENSOR_CONFIG_CHANGING,
    EVT_SENSOR_RESTORE_IDLE,
    EVT_TIME_SYNC
};

enum SensorState {
    SENSOR_BOOT = 0,
    SENSOR_VERIFY_WAI,
    SENSOR_INITIALIZATION,
    SENSOR_IDLE,
    SENSOR_POWERING_UP,
    SENSOR_POWERING_DOWN,
    SENSOR_CONFIG_CHANGING,
    SENSOR_CONFIG_WATERMARK_CHANGING,
    SENSOR_CALIBRATION,
    SENSOR_STORE_CALIBRATION_DATA,
    SENSOR_SELFTEST,
    SENSOR_INT1_STATUS_REG_HANDLING,
    SENSOR_INT1_OUTPUT_DATA_HANDLING,
    SENSOR_TIME_SYNC,
    SENSOR_BARO_READ_DATA,
    SENSOR_INVALID_STATE
};

static void lsm6dsm_spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay);
static void lsm6dsm_spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay);
static void lsm6dsm_spiQueueMultiwrite(uint8_t addr, uint8_t *data, size_t size, uint32_t delay);

#define SPI_MULTIWRITE_0(addr, data, size)                          lsm6dsm_spiQueueMultiwrite(addr, data, size, 2)
#define SPI_MULTIWRITE_1(addr, data, size, delay)                   lsm6dsm_spiQueueMultiwrite(addr, data, size, delay)
#define GET_SPI_MULTIWRITE_MACRO(_1, _2, _3, _4, NAME, ...)         NAME
#define SPI_MULTIWRITE(...)                                         GET_SPI_MULTIWRITE_MACRO(__VA_ARGS__, SPI_MULTIWRITE_1, SPI_MULTIWRITE_0)(__VA_ARGS__)

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

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

#ifdef LSM6DSM_I2C_MASTER_ENABLED
static void lsm6dsm_writeSlaveRegister(uint8_t addr, uint8_t value, uint32_t accelRate, uint32_t delay, enum SensorIndex si);

#define SPI_WRITE_SS_REGISTER_0(addr, value, accelRate, si)         lsm6dsm_writeSlaveRegister(addr, value, accelRate, 0, si)
#define SPI_WRITE_SS_REGISTER_1(addr, value, accelRate, si, delay)  lsm6dsm_writeSlaveRegister(addr, value, accelRate, delay, si)
#define GET_SPI_WRITE_SS_MACRO(_1, _2, _3, _4, _5, NAME, ...)       NAME
#define SPI_WRITE_SLAVE_SENSOR_REGISTER(...)                        GET_SPI_WRITE_SS_MACRO(__VA_ARGS__, SPI_WRITE_SS_REGISTER_1, \
                                                                        SPI_WRITE_SS_REGISTER_0)(__VA_ARGS__)
#endif /* LSM6DSM_I2C_MASTER_ENABLED */

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

#define DEBUG_PRINT(fmt, ...) \
    do { \
        if (LSM6DSM_DBG_ENABLED) { \
            osLog(LOG_DEBUG, "%s " fmt, "[LSM6DSM]", ##__VA_ARGS__); \
        } \
    } while (0);

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

/* DO NOT MODIFY, just to avoid compiler error if not defined using FLAGS */
#ifndef LSM6DSM_DBG_ENABLED
#define LSM6DSM_DBG_ENABLED                             0
#endif /* LSM6DSM_DBG_ENABLED */


/*
 * struct LSM6DSMSPISlaveInterface: SPI slave data interface
 * @packets: spi packets needed to perform read/write operations.
 * @txrxBuffer: spi data buffer.
 * @spiDev: spi device info.
 * @mode: spi mode info (frequency, polarity, etc).
 * @mWbufCnt: counter of total data in spi buffer.
 * @cs: chip select used by SPI slave.
 * @funcSrcBuffer: pointer of txrxBuffer to access func source register data.
 * @tmpDataBuffer: pointer of txrxBuffer to access sporadic temp read.
 * @fifoDataBuffer: pointer of txrxBuffer to access fifo data.
 * @fifoStatusRegBuffer: pointer of txrxBuffer to access fifo status registers.
 * @stepCounterDataBuffer: pointer of txrxBuffer to access step counter data.
 * @tempDataBuffer: pointer of txrxBuffer to access sensor temperature data needed by calibration algos.
 * @timestampDataBuffer: pointer of txrxBuffer to access sensor timestamp data in order to syncronize time.
 * @timestampDataBufferBaro: pointer of txrxBuffer to access sensor timestamp data for barometer when not in FIFO.
 * @baroDataBuffer: pointer of txrx to access barometer data from DSM when not in FIFO.
 * @mRegCnt: spi packet num counter.
 * @spiInUse: flag used to check if SPI is currently busy.
 */
struct LSM6DSMSPISlaveInterface {
    struct SpiPacket packets[LSM6DSM_SPI_PACKET_SIZE];
    uint8_t txrxBuffer[SPI_BUF_SIZE];
    struct SpiDevice *spiDev;
    struct SpiMode mode;

    uint16_t mWbufCnt;

    spi_cs_t cs;

    uint8_t *funcSrcBuffer;
    uint8_t *tmpDataBuffer;
    uint8_t *fifoDataBuffer;
    uint8_t *fifoStatusRegBuffer;
    uint8_t *stepCounterDataBuffer;
#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
    uint8_t *tempDataBuffer;
#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
    uint8_t *timestampDataBuffer;
#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
    uint8_t *timestampDataBufferBaro;
    uint8_t *baroDataBuffer;
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    uint8_t mRegCnt;

    bool spiInUse;
};

/*
 * struct LSM6DSMConfigStatus: temporary data of pending events
 * @latency: value to be used in next setRate operation [ns].
 * @rate: value to be used in next setRate operation [Hz * 1024].
 * @enable: value to be used in next setEnable.
 */
struct LSM6DSMConfigStatus {
    uint64_t latency;
    uint32_t rate;
    bool enable;
};

/*
 * struct LSM6DSMSensor: sensor status data
 * @pConfig: temporary data of pending events.
 * @tADataEvt: three axis sensor data to send to nanohub.
 * @sADataEvt: one axis sensor data to send to nanohub.
 * @latency: current value of latency [n].
 * @pushedTimestamp: latest sample timestamp pusshed to nanohub.
 * @handle: sensor handle obtained by sensorRegister.
 * @rate: current value of rates based on dependecies [Hz * 1024].
 * @hwRate: current value of physical rate [Hz * 1024].
 * @idx: enum SensorIndex.
 * @samplesToDiscard: samples to discard after enable or odr switch.
 * @samplesDecimator: sw decimator factor to achieve lower odr that cannot be achieved only by FIFO decimator. For example accel is used by dependecies.
 * @samplesDecimatorCounter: samples counter working together with samplesDecimator.
 * @samplesFifoDecimator: sw decimator factor to achieve lower odr that cannot be achived by FIFO decimator.
 * @samplesFifoDecimatorCounter: samples counter working together with sampleFifoDecimator.
 * @dependenciesRequireData: mask used to verify if dependencies needs data or not. For example accel is used for internal algos.
 * enabled: current status of sensor.
 */
struct LSM6DSMSensor {
    struct LSM6DSMConfigStatus pConfig;

    union {
        struct TripleAxisDataEvent *tADataEvt;
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
        struct SingleAxisDataEvent *sADataEvt;
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    };

    uint64_t latency;
    uint64_t pushedTimestamp;
    uint32_t handle;
    uint32_t rate[NUM_SENSORS];
    uint32_t hwRate;
    enum SensorIndex idx;
    uint8_t samplesToDiscard;
    uint8_t samplesDecimator;
    uint8_t samplesDecimatorCounter;
    uint8_t samplesFifoDecimator;
    uint8_t samplesFifoDecimatorCounter;
    bool dependenciesRequireData[NUM_SENSORS];
    bool enabled;
};

/*
 * struct LSM6DSMFifoCntl: fifo control data
 * @decimatorsIdx: give who is the sensor that store data in that FIFO slot.
 * @triggerRate: frequency of FIFO [Hz * 1024].
 * @watermark: watermark value in #num of samples.
 * @decimators: fifo decimators value.
 * @minDecimator: min value of decimators.
 * @maxDecimator: max value of decimators.
 * @maxMinDecimator: maxDecimator devided by minDecimator.
 * @totalSip: total number of samples in one pattern.
 * @timestampPosition: since timestamp in FIFO is the latest sensor, we need to know where is located during FIFO parsing.
 */
struct LSM6DSMFifoCntl {
    enum SensorIndex decimatorsIdx[FIFO_NUM];
    uint32_t triggerRate;
    uint16_t watermark;
    uint8_t decimators[FIFO_NUM];
    uint8_t minDecimator;
    uint8_t maxDecimator;
    uint8_t maxMinDecimator;
    uint8_t totalSip;
    uint8_t timestampPosition[32];
};

/*
 * struct LSM6DSMTimeCalibrationWithoutTimer: data used when time calibration is performed during FIFO read.
 *      If latency is smaller than LSM6DSM_SYNC_DELTA_INTERVAL no need to use a timer but we can read timestamp before read FIFO data.
 * @lastTimestampDataAvlRtcTime: last time we perform a timestamp read from LSM6DSM based on RTC time.
 * @newTimestampDataAvl: when deltatime is enough we can read again timestamp from LSM6DSM.
 */
struct LSM6DSMTimeCalibrationWithoutTimer {
    uint64_t lastTimestampDataAvlRtcTime;
    bool newTimestampDataAvl;
};

enum LSM6DSMTimeCalibrationStatus {
    TIME_SYNC_DISABLED,
    TIME_SYNC_TIMER,
    TIME_SYNC_DURING_FIFO_READ
};

/*
 * struct LSM6DSMTimeCalibration: time calibration task data
 * @sensorTimeToRtcData: timeSync algo data.
 * @noTimer: if timer is not used to perform time sync, those data will be used.
 * @lastSampleTimestamp: last sample timestamp from FIFO. Already coverted to RTC time.
 * @timeSyncRtcTime: Rtc time while performing timestamp read from LSM6DSM.
 * @sampleTimestampFromFifoLSB: current timestamp from FIFO in LSB. Needs to be stored becasue of overflow.
 * @timestampSyncTaskLSB: when timer is used to sync time, this is the last timestamp read from LSM6DSM in LSB. Needs to be stored becasue of overflow.
 * @deltaTimeMarginLSB: is it used to verify if timestamp from FIFO is valid, this is max jitter that timestamp can have from FIFO.
 * @timestampBaroLSB: if magn and baro are both enabled, barometer data are read with a timer because no slots are available in FIFO. This is the timestamp of baro data.
 * @theoreticalDeltaTimeLSB: theoretical value of timestamp based on sensor frequency.
 * @timestampIsValid: flag that indicate if current timestamp parsing FIFO is valid.
 */
struct LSM6DSMTimeCalibration {
    time_sync_t sensorTimeToRtcData;
    struct LSM6DSMTimeCalibrationWithoutTimer noTimer;
    uint64_t lastSampleTimestamp;
    uint64_t timeSyncRtcTime;
    enum LSM6DSMTimeCalibrationStatus status;
    uint32_t sampleTimestampFromFifoLSB;
    uint32_t timestampSyncTaskLSB;
    uint32_t deltaTimeMarginLSB;
#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
    uint32_t timestampBaroLSB;
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    uint32_t theoreticalDeltaTimeLSB;
    bool timestampIsValid;
};

/*
 * struct LSM6DSMSelfTestResultPkt: self-test packet result data
 * @header: describe packet size and application ID of packet.
 * @dataHeader: payload of message.
 */
struct LSM6DSMSelfTestResultPkt {
    struct HostHubRawPacket header;
    struct SensorAppEventHeader dataHeader;
} __attribute__((packed));

/*
 * struct LSM6DSMCalibrationResultPkt: calibration packet result data
 * @header: describe packet size and application ID of packet.
 * @dataHeader: payload of header message.
 * @xBias: raw offset value X axis.
 * @yBias: raw offset value Y axis.
 * @zBias: raw offset value Z axis.
 */
struct LSM6DSMCalibrationResultPkt {
    struct HostHubRawPacket header;
    struct SensorAppEventHeader dataHeader;
    int32_t xBias;
    int32_t yBias;
    int32_t zBias;
} __attribute__((packed));

/*
 * struct LSM6DSMAccelGyroCfgData: configuration packet data
 * @hw: chip level calibration data.
 * @sw: software level calibration data (algos).
 */
struct LSM6DSMAccelGyroCfgData {
    int32_t hw[LSM6DSM_TRIAXIAL_NUM_AXIS];
    float sw[LSM6DSM_TRIAXIAL_NUM_AXIS];
};

/*
 * struct LSM6DSMTask: driver task data
 * @sensors: sensor status data list.
 * @slaveConn: slave interface / communication data.
 * @accelCal: accelerometer calibration algo data.
 * @gyroCal: gyroscope calibration algo data.
 * @overTempCal: gyroscope over temperature calibration algo data.
 * @magnCal: magnetometer calibration algo data.
 * @int1: int1 gpio data.
 * @isr1: isr1 data.
 * @mDataSlabThreeAxis: memory used to store three axis sensors data.
 * @mDataSlabOneAxis: memory used to store one axis sensors data.
 * @fifoCntl: fifo control data.
 * @time: time calibration data.
 * @currentTemperature: sensor temperature data value used by gyroscope/accelerometer bias calibration libs.
 * @lastFifoReadTimestamp: store when last time FIFO was read.
 * @initState: initialization is done in several steps (enum InitState).
 * @selftestState: self-test is performed in several steps (enum SelfTestState).
 * @calibrationState: sensor calibration is done in several steps (enum CalibrationState).
 * @tid: task id.
 * @totalNumSteps: total number of steps of step counter sensor.
 * @fifoDataToRead: number of byte to read in current FIFO read.
 * @fifoDataToReadPending: in order to reduce txrxBuffer, FIFO read is performed in several read. This value tell how many data still need to read from FIFO.
 * @baroTimerId: barometer task timer id.
 * @dataSelftestEnabled: sensor data read during GapSelfTestProgram while self-test bit is set.
 * @dataSelftestNotEnabled: sensor data read during GapSelfTestProgram while self-test bit is not set.
 * @dataCalibration: sensor data read during calibration program.
 * @accelCalibrationData: accelerometer offset value (hw) to store into sensor.
 * @gyroCalibrationData: gyroscope offset value (hw) applied to each sample (by software).
 * @state: task state, driver manage operations using a state machine (enum SensorState).
 * @numSamplesSelftest: temp variable storing number of samples read by self-test program.
 * @numSamplesCalibration: temp variable storing number of samples read by calibration program.
 * @mRetryLeft: counter used to retry operations #n times before return a failure.
 * @pedometerDependencies: dependencies mask of sensors that are using embedded functions.
 * @masterConfigDependencies: dependencies mask of sensors that are using I2C master.
 * @int1Register: interrupt 1 register content (addr: 0x0d).
 * @int2Register: interrupt 2 register content (addr: 0x0e).
 * @embeddedFunctionsRegister: embedded register content (addr: 0x19).
 * @pendingFlush: number of flush requested for each sensor.
 * @masterConfigRegister: i2c master register content (addr: 0x1a).
 * @readSteps: flag used to indicate if interrupt task need to read number of steps.
 * @sendFlushEvt: if flush is requested, send it out after FIFO read is completed.
 * @pendingEnableConfig: pending setEnable operations to be executed.
 * @pendingRateConfig: pending setRate operations to be executed.
 * @pendingInt: pending interrupt task to be executed.
 * @pendingTimeSyncTask: pending time sync task to be executed.
 * @pendingBaroTimerTask: pending baro read data task to be executed.
 * @pendingStoreAccelCalibData: pending calibration data store task to be executed.
 */
typedef struct LSM6DSMTask {
    struct LSM6DSMSensor sensors[NUM_SENSORS];
    struct LSM6DSMSPISlaveInterface slaveConn;

#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
    struct AccelCal accelCal;
#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
#ifdef LSM6DSM_GYRO_CALIB_ENABLED
    struct GyroCal gyroCal;
#ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED
    struct OverTempCal overTempCal;
#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
#ifdef LSM6DSM_MAGN_CALIB_ENABLED
    struct MagCal magnCal;
#endif /* LSM6DSM_MAGN_CALIB_ENABLED */

    struct Gpio *int1;
    struct ChainedIsr isr1;
    struct SlabAllocator *mDataSlabThreeAxis;
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
    struct SlabAllocator *mDataSlabOneAxis;
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    struct LSM6DSMFifoCntl fifoCntl;
    struct LSM6DSMTimeCalibration time;

#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
    float currentTemperature;
#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */

    uint64_t lastFifoReadTimestamp;

    enum InitState initState;
    enum SelfTestState selftestState;
    enum CalibrationState calibrationState;

    uint32_t tid;
    uint32_t totalNumSteps;
    uint32_t fifoDataToRead;
    uint32_t fifoDataToReadPending;
#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
    uint32_t baroTimerId;
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    int32_t dataSelftestEnabled[LSM6DSM_TRIAXIAL_NUM_AXIS];
    int32_t dataSelftestNotEnabled[LSM6DSM_TRIAXIAL_NUM_AXIS];
    int32_t dataCalibration[LSM6DSM_TRIAXIAL_NUM_AXIS];
    int32_t accelCalibrationData[LSM6DSM_TRIAXIAL_NUM_AXIS];
    int32_t gyroCalibrationData[LSM6DSM_TRIAXIAL_NUM_AXIS];

    volatile uint8_t state;

    uint8_t numSamplesSelftest;
    uint8_t numSamplesCalibration;
    uint8_t mRetryLeft;
    uint8_t pedometerDependencies;
    uint8_t masterConfigDependencies;
    uint8_t int1Register;
    uint8_t int2Register;
    uint8_t embeddedFunctionsRegister;
    uint8_t pendingFlush[NUM_SENSORS];
#ifdef LSM6DSM_I2C_MASTER_ENABLED
    uint8_t masterConfigRegister;
#endif /* LSM6DSM_I2C_MASTER_ENABLED */

    bool readSteps;
    bool sendFlushEvt[NUM_SENSORS];
    bool pendingEnableConfig[NUM_SENSORS];
    bool pendingRateConfig[NUM_SENSORS];
    bool pendingInt;
    bool pendingTimeSyncTask;
#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
    bool pendingBaroTimerTask;
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    bool pendingStoreAccelCalibData;
} LSM6DSMTask;

static LSM6DSMTask mTask;

#define TASK                                            LSM6DSMTask* const _task
#define TDECL()                                         TASK = &mTask; (void)_task
#define T(v)                                            (_task->v)
#define T_SLAVE_INTERFACE(v)                            (_task->slaveConn.v)

#define BIT(x)                                          (0x01 << x)
#define SENSOR_HZ_RATE_TO_US(x)                         (1024000000UL / x)
#define NS_TO_US(ns)                                    cpuMathU64DivByU16(ns, 1000)

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

/* Atomic set state, this set the state to arbitrary value, use with caution */
#define SET_STATE(s) \
    do { \
        atomicWriteByte(&(_task->state), (s)); \
    } while (0)

static bool trySwitchState_(TASK, enum SensorState newState)
{
    return atomicCmpXchgByte(&T(state), SENSOR_IDLE, newState);
}
#define trySwitchState(s) trySwitchState_(_task, (s))

static void lsm6dsm_readStatusReg_(TASK, bool isInterruptContext);
#define lsm6dsm_readStatusReg(a)                        lsm6dsm_readStatusReg_(_task, (a))

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

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

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

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

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

/*
 * LSM6DSMImuRates: supported frequencies by accelerometer and gyroscope sensors
 * LSM6DSMImuRatesRegValue, LSM6DSMRatesSamplesToDiscardGyroPowerOn, LSM6DSMAccelRatesSamplesToDiscard,
 *     LSM6DSMGyroRatesSamplesToDiscard must have same length.
 */
static uint32_t LSM6DSMImuRates[] = {
    SENSOR_HZ(26.0f / 32.0f),       /* 0.8125Hz */
    SENSOR_HZ(26.0f / 16.0f),       /* 1.625Hz */
    SENSOR_HZ(26.0f / 8.0f),        /* 3.25Hz */
    SENSOR_HZ(26.0f / 4.0f),        /* 6.5Hz */
    SENSOR_HZ(26.0f / 2.0f),        /* 12.5Hz */
    SENSOR_HZ(26.0f),               /* 26Hz */
    SENSOR_HZ(52.0f),               /* 52Hz */
    SENSOR_HZ(104.0f),              /* 104Hz */
    SENSOR_HZ(208.0f),              /* 208Hz */
    SENSOR_HZ(416.0f),              /* 416Hz */
    0,
};

static uint32_t LSM6DSMImuRatesInNs[] = {
    1230769230,                     /* 0.8125Hz */
    615384615,                      /* 1.625Hz */
    307692308,                      /* 3.25Hz */
    153846154,                      /* 6.5Hz */
    80000000,                       /* 12.5Hz */
    38461538,                       /* 26Hz */
    19230769,                       /* 52Hz */
    9615385,                        /* 104Hz */
    4807692,                        /* 208Hz */
    2403846,                        /* 416Hz */
    0,
};

static uint8_t LSM6DSMImuRatesRegValue[] = {
    LSM6DSM_ODR_12HZ_REG_VALUE,     /* 0.8125Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_REG_VALUE,     /* 1.625Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_REG_VALUE,     /* 3.25Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_REG_VALUE,     /* 6.5Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_REG_VALUE,     /* 12.5Hz */
    LSM6DSM_ODR_26HZ_REG_VALUE,     /* 26Hz */
    LSM6DSM_ODR_52HZ_REG_VALUE,     /* 52Hz */
    LSM6DSM_ODR_104HZ_REG_VALUE,    /* 104Hz */
    LSM6DSM_ODR_208HZ_REG_VALUE,    /* 208Hz */
    LSM6DSM_ODR_416HZ_REG_VALUE,    /* 416Hz */
};

/* When sensors switch status from power-down, constant boottime must be considered, some samples should be discarded */
static uint8_t LSM6DSMRatesSamplesToDiscardGyroPowerOn[] = {
    LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 0.8125Hz - do not exist, use 12.5Hz = 80000us */
    LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 1.625Hz - do not exist, use 12.5Hz = 80000us */
    LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 3.25Hz - do not exist, use 12.5Hz = 80000us */
    LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 6.5Hz - do not exist, use 12.5Hz = 80000us */
    LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 80000, /* 12.5Hz = 80000us */
    LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 38461, /* 26Hz = 38461us */
    LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 19230, /* 52Hz = 19230s */
    LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 9615,  /* 104Hz = 9615us */
    LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 4807,  /* 208Hz = 4807us */
    LSM6DSM_ODR_DELAY_US_GYRO_POWER_ON / 2403,  /* 416Hz = 2403us */
};

/* When accelerometer change odr but sensor is already on, few samples should be discarded */
static uint8_t LSM6DSMAccelRatesSamplesToDiscard[] = {
    LSM6DSM_ODR_12HZ_ACCEL_STD,     /* 0.8125Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_ACCEL_STD,     /* 1.625Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_ACCEL_STD,     /* 3.25Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_ACCEL_STD,     /* 6.5Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_ACCEL_STD,     /* 12.5Hz */
    LSM6DSM_ODR_26HZ_ACCEL_STD,     /* 26Hz */
    LSM6DSM_ODR_52HZ_ACCEL_STD,     /* 52Hz */
    LSM6DSM_ODR_104HZ_ACCEL_STD,    /* 104Hz */
    LSM6DSM_ODR_208HZ_ACCEL_STD,    /* 208Hz */
    LSM6DSM_ODR_416HZ_ACCEL_STD,    /* 416Hz */
};

/* When gyroscope change odr but sensor is already on, few samples should be discarded */
static uint8_t LSM6DSMGyroRatesSamplesToDiscard[] = {
    LSM6DSM_ODR_12HZ_GYRO_STD,      /* 0.8125Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_GYRO_STD,      /* 1.625Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_GYRO_STD,      /* 3.25Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_GYRO_STD,      /* 6.5Hz - do not exist, use 12.5Hz */
    LSM6DSM_ODR_12HZ_GYRO_STD,      /* 12.5Hz */
    LSM6DSM_ODR_26HZ_GYRO_STD,      /* 26Hz */
    LSM6DSM_ODR_52HZ_GYRO_STD,      /* 52Hz */
    LSM6DSM_ODR_104HZ_GYRO_STD,     /* 104Hz */
    LSM6DSM_ODR_208HZ_GYRO_STD,     /* 208Hz */
    LSM6DSM_ODR_416HZ_GYRO_STD,     /* 416Hz */
};

#ifdef LSM6DSM_I2C_MASTER_ENABLED
static uint32_t LSM6DSMSHRates[] = {
    SENSOR_HZ(26.0f / 32.0f),       /* 0.8125Hz */
    SENSOR_HZ(26.0f / 16.0f),       /* 1.625Hz */
    SENSOR_HZ(26.0f / 8.0f),        /* 3.25Hz */
    SENSOR_HZ(26.0f / 4.0f),        /* 6.5Hz */
    SENSOR_HZ(26.0f / 2.0f),        /* 12.5Hz */
    SENSOR_HZ(26.0f),               /* 26Hz */
    SENSOR_HZ(52.0f),               /* 52Hz */
    SENSOR_HZ(104.0f),              /* 104Hz */
    0,
};
#endif /* LSM6DSM_I2C_MASTER_ENABLED */

static uint32_t LSM6DSMStepCounterRates[] = {
    SENSOR_HZ(1.0f / (128 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)), /* 209.715 sec */
    SENSOR_HZ(1.0f / (64 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)),  /* 104.857 sec */
    SENSOR_HZ(1.0f / (32 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)),  /* 52.4288 sec */
    SENSOR_HZ(1.0f / (16 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)),  /* 26.1574 sec */
    SENSOR_HZ(1.0f / (8 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)),   /* 13.0787 sec */
    SENSOR_HZ(1.0f / (4 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)),   /* 6.53936 sec */
    SENSOR_HZ(1.0f / (2 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)),   /* 3.26968 sec */
    SENSOR_HZ(1.0f / (1 * LSM6DSM_SC_DELTA_TIME_PERIOD_SEC)),   /* 1.63840 sec */
    SENSOR_RATE_ONCHANGE,
    0,
};

static const struct SensorInfo LSM6DSMSensorInfo[NUM_SENSORS] = {
    {
#ifdef LSM6DSM_GYRO_CALIB_ENABLED
        DEC_INFO_RATE_BIAS("Gyroscope", LSM6DSMImuRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20, SENS_TYPE_GYRO_BIAS)
#else /* LSM6DSM_GYRO_CALIB_ENABLED */
        DEC_INFO_RATE("Gyroscope", LSM6DSMImuRates, SENS_TYPE_GYRO, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 20)
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
    },
    {
#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
        DEC_INFO_RATE_RAW_BIAS("Accelerometer", LSM6DSMImuRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 3000,
            SENS_TYPE_ACCEL_RAW, 1.0f / LSM6DSM_ACCEL_KSCALE, SENS_TYPE_ACCEL_BIAS)
#else /* LSM6DSM_ACCEL_CALIB_ENABLED */
        DEC_INFO_RATE_RAW("Accelerometer", LSM6DSMImuRates, SENS_TYPE_ACCEL, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 3000,
            SENS_TYPE_ACCEL_RAW, 1.0f / LSM6DSM_ACCEL_KSCALE)
#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
    },
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
    {
#ifdef LSM6DSM_MAGN_CALIB_ENABLED
        DEC_INFO_RATE_BIAS("Magnetometer", LSM6DSMSHRates, SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 600, SENS_TYPE_MAG_BIAS)
#else /* LSM6DSM_MAGN_CALIB_ENABLED */
        DEC_INFO_RATE("Magnetometer", LSM6DSMSHRates, SENS_TYPE_MAG, NUM_AXIS_THREE, NANOHUB_INT_NONWAKEUP, 600)
#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
    },
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
    {
        DEC_INFO_RATE("Pressure", LSM6DSMSHRates, SENS_TYPE_BARO, NUM_AXIS_ONE, NANOHUB_INT_NONWAKEUP, 300)
    },
    {
        DEC_INFO_RATE("Temperature", LSM6DSMSHRates, SENS_TYPE_TEMP, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20)
    },
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    {
        DEC_INFO("Step Detector", SENS_TYPE_STEP_DETECT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 100)
    },
    {
        DEC_INFO_RATE("Step Counter", LSM6DSMStepCounterRates, SENS_TYPE_STEP_COUNT, NUM_AXIS_EMBEDDED, NANOHUB_INT_NONWAKEUP, 20)
    },
    {
        DEC_INFO("Significant Motion", SENS_TYPE_SIG_MOTION, NUM_AXIS_EMBEDDED, NANOHUB_INT_WAKEUP, 1)
    },
};

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

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

#define DEC_OPS_CFG_SELFTEST(power, firmware, rate, flush, cfgData, selftest) \
    DEC_OPS(power, firmware, rate, flush), \
    .sensorCfgData = cfgData, \
    .sensorSelfTest = selftest

#define DEC_OPS_CAL_CFG_SELFTEST(power, firmware, rate, flush, cal, cfgData, selftest) \
    DEC_OPS(power, firmware, rate, flush), \
    .sensorCalibrate = cal, \
    .sensorCfgData = cfgData, \
    .sensorSelfTest = selftest

static bool lsm6dsm_setAccelPower(bool on, void *cookie);
static bool lsm6dsm_setGyroPower(bool on, void *cookie);
static bool lsm6dsm_setStepDetectorPower(bool on, void *cookie);
static bool lsm6dsm_setStepCounterPower(bool on, void *cookie);
static bool lsm6dsm_setSignMotionPower(bool on, void *cookie);
static bool lsm6dsm_accelFirmwareUpload(void *cookie);
static bool lsm6dsm_gyroFirmwareUpload(void *cookie);
static bool lsm6dsm_stepDetectorFirmwareUpload(void *cookie);
static bool lsm6dsm_stepCounterFirmwareUpload(void *cookie);
static bool lsm6dsm_signMotionFirmwareUpload(void *cookie);
static bool lsm6dsm_setAccelRate(uint32_t rate, uint64_t latency, void *cookie);
static bool lsm6dsm_setGyroRate(uint32_t rate, uint64_t latency, void *cookie);
static bool lsm6dsm_setStepDetectorRate(uint32_t rate, uint64_t latency, void *cookie);
static bool lsm6dsm_setStepCounterRate(uint32_t rate, uint64_t latency, void *cookie);
static bool lsm6dsm_setSignMotionRate(uint32_t rate, uint64_t latency, void *cookie);
static bool lsm6dsm_accelFlush(void *cookie);
static bool lsm6dsm_gyroFlush(void *cookie);
static bool lsm6dsm_stepDetectorFlush(void *cookie);
static bool lsm6dsm_stepCounterFlush(void *cookie);
static bool lsm6dsm_signMotionFlush(void *cookie);
static bool lsm6dsm_stepCounterSendLastData(void *cookie, uint32_t tid);
static bool lsm6dsm_runAccelSelfTest(void *cookie);
static bool lsm6dsm_runGyroSelfTest(void *cookie);
static bool lsm6dsm_runAccelCalibration(void *cookie);
static bool lsm6dsm_runGyroCalibration(void *cookie);
static bool lsm6dsm_accelCfgData(void *data, void *cookie);
static bool lsm6dsm_gyroCfgData(void *data, void *cookie);

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
static bool lsm6dsm_setMagnPower(bool on, void *cookie);
static bool lsm6dsm_magnFirmwareUpload(void *cookie);
static bool lsm6dsm_setMagnRate(uint32_t rate, uint64_t latency, void *cookie);
static bool lsm6dsm_magnFlush(void *cookie);
static bool lsm6dsm_runMagnSelfTest(void *cookie);
static bool lsm6dsm_magnCfgData(void *data, void *cookie);
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
static bool lsm6dsm_setPressPower(bool on, void *cookie);
static bool lsm6dsm_pressFirmwareUpload(void *cookie);
static bool lsm6dsm_setPressRate(uint32_t rate, uint64_t latency, void *cookie);
static bool lsm6dsm_pressFlush(void *cookie);
static bool lsm6dsm_setTempPower(bool on, void *cookie);
static bool lsm6dsm_tempFirmwareUpload(void *cookie);
static bool lsm6dsm_setTempRate(uint32_t rate, uint64_t latency, void *cookie);
static bool lsm6dsm_tempFlush(void *cookie);
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

static const struct SensorOps LSM6DSMSensorOps[NUM_SENSORS] = {
    { DEC_OPS_CAL_CFG_SELFTEST(lsm6dsm_setGyroPower, lsm6dsm_gyroFirmwareUpload, lsm6dsm_setGyroRate,
                                lsm6dsm_gyroFlush, lsm6dsm_runGyroCalibration, lsm6dsm_gyroCfgData, lsm6dsm_runGyroSelfTest) },
    { DEC_OPS_CAL_CFG_SELFTEST(lsm6dsm_setAccelPower, lsm6dsm_accelFirmwareUpload, lsm6dsm_setAccelRate,
                                lsm6dsm_accelFlush, lsm6dsm_runAccelCalibration, lsm6dsm_accelCfgData, lsm6dsm_runAccelSelfTest) },
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
    { DEC_OPS_CFG_SELFTEST(lsm6dsm_setMagnPower, lsm6dsm_magnFirmwareUpload, lsm6dsm_setMagnRate,
                                lsm6dsm_magnFlush, lsm6dsm_magnCfgData, lsm6dsm_runMagnSelfTest) },
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
    { DEC_OPS(lsm6dsm_setPressPower, lsm6dsm_pressFirmwareUpload, lsm6dsm_setPressRate, lsm6dsm_pressFlush) },
    { DEC_OPS(lsm6dsm_setTempPower, lsm6dsm_tempFirmwareUpload, lsm6dsm_setTempRate, lsm6dsm_tempFlush) },
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    { DEC_OPS(lsm6dsm_setStepDetectorPower, lsm6dsm_stepDetectorFirmwareUpload, lsm6dsm_setStepDetectorRate, lsm6dsm_stepDetectorFlush) },
    { DEC_OPS_SEND(lsm6dsm_setStepCounterPower, lsm6dsm_stepCounterFirmwareUpload, lsm6dsm_setStepCounterRate,
                                lsm6dsm_stepCounterFlush, lsm6dsm_stepCounterSendLastData) },
    { DEC_OPS(lsm6dsm_setSignMotionPower, lsm6dsm_signMotionFirmwareUpload, lsm6dsm_setSignMotionRate, lsm6dsm_signMotionFlush) },
};

static void lsm6dsm_processPendingEvt(void);

/*
 * lsm6dsm_spiQueueRead: enqueue a new SPI read that will be performed after lsm6dsm_spiBatchTxRx function is called
 * @addr: start reading from this register address.
 * @size: number of byte to read.
 * @buf: address of pointer where store data.
 * @delay: wait `delay time' after read is completed. [us]
 */
static void lsm6dsm_spiQueueRead(uint8_t addr, size_t size, uint8_t **buf, uint32_t delay)
{
    TDECL();

    if (T_SLAVE_INTERFACE(spiInUse)) {
        ERROR_PRINT("spiQueueRead: SPI in use, cannot queue read (addr=%x len=%d)\n", addr, (int)size);
        return;
    }

    *buf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);

    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).size = size + 1;
    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).txBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);
    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).rxBuf = *buf;
    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).delay = delay * 1000;

    T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = addr | 0x80;
    T_SLAVE_INTERFACE(mWbufCnt) += size;
    T_SLAVE_INTERFACE(mRegCnt)++;
}

/*
 * lsm6dsm_spiQueueWrite: enqueue a new SPI 1-byte write that will be performed after lsm6dsm_spiBatchTxRx function is called
 * @addr: write byte to this register address.
 * @data: value to write.
 * @delay: wait `delay time' after write is completed. [us]
 */
static void lsm6dsm_spiQueueWrite(uint8_t addr, uint8_t data, uint32_t delay)
{
    TDECL();

    if (T_SLAVE_INTERFACE(spiInUse)) {
        ERROR_PRINT("spiQueueWrite: SPI in use, cannot queue 1-byte write (addr=%x data=%x)\n", addr, data);
        return;
    }

    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).size = 2;
    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).txBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);
    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).rxBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);
    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).delay = delay * 1000;

    T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = addr;
    T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = data;
    T_SLAVE_INTERFACE(mRegCnt)++;
}

/*
 * lsm6dsm_spiQueueMultiwrite: enqueue a new SPI n-byte write that will be performed after lsm6dsm_spiBatchTxRx function is called
 * @addr: start writing from this register address.
 * @data: array data to write.
 * @size: number of byte to write.
 * @delay: wait `delay time' after write is completed. [us]
 */
static void lsm6dsm_spiQueueMultiwrite(uint8_t addr, uint8_t *data, size_t size, uint32_t delay)
{
    TDECL();
    uint8_t i;

    if (T_SLAVE_INTERFACE(spiInUse)) {
        ERROR_PRINT("spiQueueMultiwrite: SPI in use, cannot queue multiwrite (addr=%x size=%d)\n", addr, (int)size);
        return;
    }

    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).size = 1 + size;
    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).txBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);
    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).rxBuf = &T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)]);
    T_SLAVE_INTERFACE(packets[T_SLAVE_INTERFACE(mRegCnt)]).delay = delay * 1000;

    T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = addr;

    for (i = 0; i < size; i++)
        T_SLAVE_INTERFACE(txrxBuffer[T_SLAVE_INTERFACE(mWbufCnt)++]) = data[i];

    T_SLAVE_INTERFACE(mRegCnt)++;
}

/*
 * lsm6dsm_spiBatchTxRx: perform SPI read and/or write enqueued before
 * @mode: SPI configuration data.
 * @callback: callback function triggered when all transactions are terminated.
 * @cookie: private data delivered to callback function.
 * @src: function name and/or custom string used during print to trace the callstack.
 */
static void lsm6dsm_spiBatchTxRx(struct SpiMode *mode, SpiCbkF callback, void *cookie, const char *src)
{
    TDECL();
    uint8_t regCount;

    if (T_SLAVE_INTERFACE(mWbufCnt) > SPI_BUF_SIZE) {
        ERROR_PRINT("spiBatchTxRx: not enough SPI buffer space, dropping transaction. Ref. %s\n", src);
        return;
    }

    if (T_SLAVE_INTERFACE(mRegCnt) > LSM6DSM_SPI_PACKET_SIZE) {
        ERROR_PRINT("spiBatchTxRx: too many packets! Ref. %s\n", src);
        return;
    }

    /* Reset variables before issuing SPI transaction.
       SPI may finish before spiMasterRxTx finish */
    regCount = T_SLAVE_INTERFACE(mRegCnt);
    T_SLAVE_INTERFACE(spiInUse) = true;
    T_SLAVE_INTERFACE(mRegCnt) = 0;
    T_SLAVE_INTERFACE(mWbufCnt) = 0;

    if (spiMasterRxTx(T_SLAVE_INTERFACE(spiDev), T_SLAVE_INTERFACE(cs), T_SLAVE_INTERFACE(packets), regCount, mode, callback, cookie)) {
        ERROR_PRINT("spiBatchTxRx: transaction failed!\n");
    }
}

/*
 * lsm6dsm_timerCallback: timer callback routine used to retry WAI read
 * @timerId: timer identificator.
 * @data: private data delivered to private event handler.
 */
static void lsm6dsm_timerCallback(uint32_t timerId, void *data)
{
    osEnqueuePrivateEvt(EVT_SPI_DONE, data, NULL, mTask.tid);
}

/*
 * lsm6dsm_timerSyncCallback: time syncronization timer callback routine
 * @timerId: timer identificator.
 * @data: private data delivered to private event handler.
 */
static void lsm6dsm_timerSyncCallback(uint32_t timerId, void *data)
{
    osEnqueuePrivateEvt(EVT_TIME_SYNC, data, NULL, mTask.tid);
}

/*
 * lsm6dsm_spiCallback: SPI callback function
 * @cookie: private data from lsm6dsm_spiBatchTxRx function.
 * @err: error code from SPI transfer.
 */
static void lsm6dsm_spiCallback(void *cookie, int err)
{
    TDECL();

    T_SLAVE_INTERFACE(spiInUse) = false;
    osEnqueuePrivateEvt(EVT_SPI_DONE, cookie, NULL, mTask.tid);
}

#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
/*
 * lsm6dsm_baroTimerTask: baro read data task
 */
static void lsm6dsm_baroTimerTask(void)
{
    TDECL();

    if (trySwitchState(SENSOR_BARO_READ_DATA)) {
        SPI_READ(LSM6DSM_TIMESTAMP0_REG_ADDR, LSM6DSM_TIMESTAMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(timestampDataBufferBaro));
        SPI_READ(LSM6DSM_OUT_SENSORHUB1_ADDR + LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN,
                LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN, &T_SLAVE_INTERFACE(baroDataBuffer));

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
    } else
        T(pendingBaroTimerTask) = true;

    return;
}

/*
 * lsm6dsm_baroTimerCallback: baro timer callback routine
 * @timerId: timer identificator.
 * @data: private data delivered to private event handler.
 */
static void lsm6dsm_baroTimerCallback(uint32_t timerId, void *data)
{
    lsm6dsm_baroTimerTask();
}
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

/*
 * lsm6dsm_timeSyncTask: time syncronization task by timer
 */
static void lsm6dsm_timeSyncTask(void)
{
    TDECL();

    if (T(time).status != TIME_SYNC_TIMER)
        return;

    if (trySwitchState(SENSOR_TIME_SYNC)) {
        SPI_READ(LSM6DSM_TIMESTAMP0_REG_ADDR, LSM6DSM_TIMESTAMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(timestampDataBuffer));
#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
        SPI_READ(LSM6DSM_OUT_TEMP_L_ADDR, LSM6DSM_TEMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tempDataBuffer));
#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */

        T(time).timeSyncRtcTime = sensorGetTime();

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
    } else
        T(pendingTimeSyncTask) = true;
}

/*
 * lsm6dsm_readStatusReg_: read status registers (interrupt arrived)
 * @TASK: task id.
 * @isInterruptContext: function is called directly by ISR.
 */
static void lsm6dsm_readStatusReg_(TASK, bool isInterruptContext)
{
    if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
        if (T(sensors[STEP_DETECTOR]).enabled || T(sensors[STEP_COUNTER]).enabled || T(sensors[SIGN_MOTION]).enabled)
            SPI_READ(LSM6DSM_FUNC_SRC_ADDR, 1, &T_SLAVE_INTERFACE(funcSrcBuffer));

        SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
    } else {
        if (isInterruptContext)
            osEnqueuePrivateEvt(EVT_SENSOR_INTERRUPT_1, _task, NULL, T(tid));
        else
            T(pendingInt) = true;
    }
}

/*
 * lsm6dsm_isr1: INT-1 line service routine
 * @isr: isr data.
 */
static bool lsm6dsm_isr1(struct ChainedIsr *isr)
{
    TDECL();

    if (!extiIsPendingGpio(T(int1)))
        return false;

    lsm6dsm_readStatusReg(true);

    extiClearPendingGpio(T(int1));

    return true;
}

/*
 * lsm6dsm_enableInterrupt: enable driver interrupt capability
 * @pin: gpio data.
 * @isr: isr data.
 */
static void lsm6dsm_enableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
{
    gpioConfigInput(pin, GPIO_SPEED_LOW, GPIO_PULL_NONE);
    syscfgSetExtiPort(pin);
    extiEnableIntGpio(pin, EXTI_TRIGGER_RISING);
    extiChainIsr(LSM6DSM_INT_IRQ, isr);
}

/*
 * lsm6dsm_disableInterrupt: disable driver interrupt capability
 * @pin: gpio data.
 * @isr: isr data.
 */
static void lsm6dsm_disableInterrupt(struct Gpio *pin, struct ChainedIsr *isr)
{
    extiUnchainIsr(LSM6DSM_INT_IRQ, isr);
    extiDisableIntGpio(pin);
}

/*
 * lsm6dsm_sendSelfTestResult: send to nanohub result of self-test
 * @sensorType: android sensor type.
 * @result: status message to send (PASS/ERROR).
 */
static void lsm6dsm_sendSelfTestResult(uint8_t sensorType, uint8_t result)
{
    struct LSM6DSMSelfTestResultPkt *data;

    data = heapAlloc(sizeof(struct LSM6DSMSelfTestResultPkt));
    if (!data) {
        ERROR_PRINT("sendSelfTestResult: cannot allocate self-test result packet\n");
        return;
    }

    data->header.appId = LSM6DSM_APP_ID;
    data->header.dataLen = (sizeof(struct LSM6DSMSelfTestResultPkt) - sizeof(struct HostHubRawPacket));

    data->dataHeader.msgId = SENSOR_APP_MSG_ID_TEST_RESULT;
    data->dataHeader.sensorType = sensorType;
    data->dataHeader.status = result;

    if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) {
        ERROR_PRINT("sendSelfTestResult: failed to enqueue self-test result packet\n");
    }
}

/*
 * lsm6dsm_sendCalibrationResult: send to nanohub result of calibration
 * @sensorType: android sensor type.
 * @result: status message to send (VALID/ERROR).
 * @xBias: raw offset value X axis.
 * @yBias: raw offset value Y axis.
 * @zBias: raw offset value Z axis.
 */
static void lsm6dsm_sendCalibrationResult(uint8_t sensorType, uint8_t result, int32_t xBias, int32_t yBias, int32_t zBias)
{
    struct LSM6DSMCalibrationResultPkt *data;

    data = heapAlloc(sizeof(struct LSM6DSMCalibrationResultPkt));
    if (!data) {
        ERROR_PRINT("sendCalibrationResult: cannot allocate calibration result packet\n");
        return;
    }

    data->header.appId = LSM6DSM_APP_ID;
    data->header.dataLen = (sizeof(struct LSM6DSMCalibrationResultPkt) - sizeof(struct HostHubRawPacket));

    data->dataHeader.msgId = SENSOR_APP_MSG_ID_CAL_RESULT;
    data->dataHeader.sensorType = sensorType;
    data->dataHeader.status = result;

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

    if (!osEnqueueEvtOrFree(EVT_APP_TO_HOST, data, heapFree)) {
        ERROR_PRINT("sendCalibrationResult: failed to enqueue calibration result packet\n");
    }
}

/*
 * lsm6dsm_runGapSelfTestProgram: state machine that is executing self-test verifying data gap
 * @idx: sensor driver index.
 */
static void lsm6dsm_runGapSelfTestProgram(enum SensorIndex idx)
{
    TDECL();
    uint8_t *sensorData, numberOfAverage;

    numberOfAverage = LSM6DSM_NUM_AVERAGE_SELFTEST;

    switch (T(selftestState)) {
    case SELFTEST_INITIALIZATION:
        DEBUG_PRINT("runGapSelfTestProgram: initialization\n");

        T(numSamplesSelftest) = 0;
        memset(T(dataSelftestEnabled), 0, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t));
        memset(T(dataSelftestNotEnabled), 0, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t));

        /* Enable self-test & power on sensor */
        switch (idx) {
        case ACCEL:
            SPI_WRITE(LSM6DSM_CTRL5_C_ADDR, LSM6DSM_CTRL5_C_BASE | LSM6DSM_ACCEL_SELFTEST_PS);
            SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE, 30000);
            break;
        case GYRO:
            SPI_WRITE(LSM6DSM_CTRL5_C_ADDR, LSM6DSM_CTRL5_C_BASE | LSM6DSM_GYRO_SELFTEST_PS);
            SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE | LSM6DSM_ODR_104HZ_REG_VALUE, 30000);
            break;
#if defined(LSM6DSM_I2C_MASTER_LSM303AGR) || defined(LSM6DSM_I2C_MASTER_LIS3MDL)
        case MAGN:
            /* Enable accelerometer and sensor-hub */
            SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE);
            T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;

            uint8_t rateIndex = ARRAY_SIZE(LSM6DSMSHRates) - 2;

#ifdef LSM6DSM_I2C_MASTER_LSM303AGR
            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_B_M_ADDR, LSM303AGR_OFFSET_CANCELLATION, SENSOR_HZ(104.0f), MAGN);
            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_C_M_ADDR,
                    LSM303AGR_CFG_REG_C_M_BASE | LSM303AGR_ENABLE_SELFTEST, SENSOR_HZ(104.0f), MAGN);

            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR,
                    LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE |
                    LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(rateIndex), SENSOR_HZ(104.0f), MAGN, 200000);
#else /* LSM6DSM_I2C_MASTER_LSM303AGR */
            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR,
                    LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE, SENSOR_HZ(104.0f), MAGN);
            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR,
                    LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(rateIndex) | LIS3MDL_ENABLE_SELFTEST,
                    SENSOR_HZ(104.0f), MAGN);
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR */
            break;
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR, LSM6DSM_I2C_MASTER_LIS3MDL */
        default:
            return;
        }

        T(selftestState) = SELFTEST_READ_EST_DATA;
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__);
        break;

    case SELFTEST_READ_EST_DATA:
#ifdef LSM6DSM_I2C_MASTER_LSM303AGR
        if (idx == MAGN)
            numberOfAverage = LSM6DSM_NUM_AVERAGE_SELFTEST_SLOW;
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR */

        if (T(numSamplesSelftest) > 0) {
            sensorData = &T_SLAVE_INTERFACE(tmpDataBuffer[1]);
            T(dataSelftestEnabled[0]) += (int16_t)*((uint16_t *)&sensorData[0]);
            T(dataSelftestEnabled[1]) += (int16_t)*((uint16_t *)&sensorData[2]);
            T(dataSelftestEnabled[2]) += (int16_t)*((uint16_t *)&sensorData[4]);
        }
        T(numSamplesSelftest)++;

        if (T(numSamplesSelftest) <= numberOfAverage) {
            DEBUG_PRINT("runGapSelfTestProgram: reading output data while self-test is enabled\n");

            switch (idx) {
            case ACCEL:
                SPI_READ(LSM6DSM_OUTX_L_XL_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000);
                break;
            case GYRO:
                SPI_READ(LSM6DSM_OUTX_L_G_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000);
                break;
#if defined(LSM6DSM_I2C_MASTER_LSM303AGR) || defined(LSM6DSM_I2C_MASTER_LIS3MDL)
            case MAGN:
                SPI_READ(LSM6DSM_OUT_SENSORHUB1_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 20000);
                break;
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR, LSM6DSM_I2C_MASTER_LIS3MDL */
            default:
                return;
            }
            lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__);
            break;
        }

        T(dataSelftestEnabled[0]) /= numberOfAverage;
        T(dataSelftestEnabled[1]) /= numberOfAverage;
        T(dataSelftestEnabled[2]) /= numberOfAverage;
        T(selftestState) = SELFTEST_SECOND_STEP_INITIALIZATION;

    case SELFTEST_SECOND_STEP_INITIALIZATION:
        DEBUG_PRINT("runGapSelfTestProgram: second step initialization\n");

        T(numSamplesSelftest) = 0;

        /* Disable self-test */
        switch (idx) {
        case ACCEL:
        case GYRO:
            SPI_WRITE(LSM6DSM_CTRL5_C_ADDR, LSM6DSM_CTRL5_C_BASE, 30000);
            break;
#if defined(LSM6DSM_I2C_MASTER_LSM303AGR) || defined(LSM6DSM_I2C_MASTER_LIS3MDL)
        case MAGN: ;
#ifdef LSM6DSM_I2C_MASTER_LSM303AGR
            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_C_M_ADDR, LSM303AGR_CFG_REG_C_M_BASE, SENSOR_HZ(104.0f), MAGN, 200000);
#else /* LSM6DSM_I2C_MASTER_LSM303AGR */
            uint8_t rateIndex = ARRAY_SIZE(LSM6DSMSHRates) - 2;

            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR,
                    LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(rateIndex),
                    SENSOR_HZ(104.0f), MAGN);
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR */
            break;
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR, LSM6DSM_I2C_MASTER_LIS3MDL */
        default:
            return;
        }

        T(selftestState) = SELFTEST_READ_NST_DATA;
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__);
        break;

    case SELFTEST_READ_NST_DATA:
#ifdef LSM6DSM_I2C_MASTER_LSM303AGR
        if (idx == MAGN)
            numberOfAverage = LSM6DSM_NUM_AVERAGE_SELFTEST_SLOW;
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR */

        if (T(numSamplesSelftest) > 0) {
            sensorData = &T_SLAVE_INTERFACE(tmpDataBuffer[1]);
            T(dataSelftestNotEnabled[0]) += (int16_t)*((uint16_t *)&sensorData[0]);
            T(dataSelftestNotEnabled[1]) += (int16_t)*((uint16_t *)&sensorData[2]);
            T(dataSelftestNotEnabled[2]) += (int16_t)*((uint16_t *)&sensorData[4]);
        }
        T(numSamplesSelftest)++;

        if (T(numSamplesSelftest) <= numberOfAverage) {
            DEBUG_PRINT("runGapSelfTestProgram: reading output data while self-test is disabled\n");

            switch (idx) {
            case ACCEL:
                SPI_READ(LSM6DSM_OUTX_L_XL_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000);
                break;
            case GYRO:
                SPI_READ(LSM6DSM_OUTX_L_G_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000);
                break;
#if defined(LSM6DSM_I2C_MASTER_LSM303AGR) || defined(LSM6DSM_I2C_MASTER_LIS3MDL)
            case MAGN:
                SPI_READ(LSM6DSM_OUT_SENSORHUB1_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 20000);
                break;
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR, LSM6DSM_I2C_MASTER_LIS3MDL */
            default:
                return;
            }
            lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__);
            break;
        }

        T(dataSelftestNotEnabled[0]) /= numberOfAverage;
        T(dataSelftestNotEnabled[1]) /= numberOfAverage;
        T(dataSelftestNotEnabled[2]) /= numberOfAverage;
        T(selftestState) = SELFTEST_VERIFICATION;

    case SELFTEST_VERIFICATION: ;
        uint8_t i, sType;
        int32_t dataGap[3];
        bool testPassed = true;
        int32_t lower_threshold[3], higher_threshold[3];

        dataGap[0] = abs(T(dataSelftestEnabled[0]) - T(dataSelftestNotEnabled[0]));
        dataGap[1] = abs(T(dataSelftestEnabled[1]) - T(dataSelftestNotEnabled[1]));
        dataGap[2] = abs(T(dataSelftestEnabled[2]) - T(dataSelftestNotEnabled[2]));

        switch (idx) {
        case ACCEL:
            sType = SENS_TYPE_ACCEL;
            lower_threshold[0] = lower_threshold[1] = lower_threshold[2] = LSM6DSM_ACCEL_SELFTEST_LOW_THR_LSB;
            higher_threshold[0] = higher_threshold[1] = higher_threshold[2] = LSM6DSM_ACCEL_SELFTEST_HIGH_THR_LSB;

            /* Power off sensor */
            SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE);
            break;
        case GYRO:
            sType = SENS_TYPE_GYRO;
            lower_threshold[0] = lower_threshold[1] = lower_threshold[2] = LSM6DSM_GYRO_SELFTEST_LOW_THR_LSB;
            higher_threshold[0] = higher_threshold[1] = higher_threshold[2] = LSM6DSM_GYRO_SELFTEST_HIGH_THR_LSB;

            /* Power off sensor */
            SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE);
            break;
#if defined(LSM6DSM_I2C_MASTER_LSM303AGR) || defined(LSM6DSM_I2C_MASTER_LIS3MDL)
        case MAGN:
            sType = SENS_TYPE_MAG;

#ifdef LSM6DSM_I2C_MASTER_LSM303AGR
            lower_threshold[0] = lower_threshold[1] = lower_threshold[2] = LSM303AGR_SELFTEST_LOW_THR_LSB;
            higher_threshold[0] = higher_threshold[1] = higher_threshold[2] = LSM303AGR_SELFTEST_HIGH_THR_LSB;

            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_B_M_ADDR, 0x00, SENSOR_HZ(104.0f), MAGN);
#else /* LSM6DSM_I2C_MASTER_LSM303AGR */
            lower_threshold[0] = lower_threshold[1] = LIS3MDL_SELFTEST_LOW_THR_XY_LSB;
            higher_threshold[0] = higher_threshold[1] = LIS3MDL_SELFTEST_HIGH_THR_XY_LSB;
            lower_threshold[2] = LIS3MDL_SELFTEST_LOW_THR_Z_LSB;
            higher_threshold[2] = LIS3MDL_SELFTEST_HIGH_THR_Z_LSB;
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR */

            /* Power off sensor */
            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR,
                    LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE, SENSOR_HZ(104.0f), MAGN);

            /* Disable accelerometer and sensor-hub */
            SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE);
            SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE);
            T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
            break;
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR, LSM6DSM_I2C_MASTER_LIS3MDL */
        default:
            return;
        }

        for (i = 0; i < 3; i++) {
            if ((dataGap[i] < lower_threshold[i]) || (dataGap[i] > higher_threshold[i])) {
                testPassed = false;
                ERROR_PRINT("runGapSelfTestProgram: axis-%d out of spec! test-enabled: %ldLSB ** test-disabled: %ldLSB, ** delta: %ldLSB\n",
                            i, T(dataSelftestEnabled[i]), T(dataSelftestNotEnabled[i]), dataGap[i]);
            }
        }
        INFO_PRINT("runGapSelfTestProgram: completed. Test result: %s\n", testPassed ? "pass" : "fail");

        if (testPassed)
            lsm6dsm_sendSelfTestResult(sType, SENSOR_APP_EVT_STATUS_SUCCESS);
        else
            lsm6dsm_sendSelfTestResult(sType, SENSOR_APP_EVT_STATUS_ERROR);

        T(selftestState) = SELFTEST_COMPLETED;
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__);
        break;

    default:
        break;
    }
}

/*
 * lsm6dsm_convertAccelOffsetValue: convert accel LSB value to offset digit
 * @val: LSB axis offset value.
 */
static uint8_t lsm6dsm_convertAccelOffsetValue(int32_t val)
{
    float temp;

    temp = val * LSM6DSM_ACCEL_LSB_TO_OFFSET_DIGIT_SCALE;
    if (temp > LSM6DSM_ACCEL_MAX_CALIBRATION_THR_LSB)
        temp = LSM6DSM_ACCEL_MAX_CALIBRATION_THR_LSB;

    if (temp < -LSM6DSM_ACCEL_MAX_CALIBRATION_THR_LSB)
        temp = -LSM6DSM_ACCEL_MAX_CALIBRATION_THR_LSB;

    return (uint8_t)((int8_t)temp);
}

/*
 * lsm6dsm_runCalibrationProgram: state machine that is executing calibration
 * @idx: sensor driver index.
 */
static void lsm6dsm_runCalibrationProgram(enum SensorIndex idx)
{
    TDECL();
    uint8_t *sensorData, numberOfAverage;
    uint8_t buffer[LSM6DSM_TRIAXIAL_NUM_AXIS] = { 0 };

    numberOfAverage = LSM6DSM_NUM_AVERAGE_CALIBRATION;

    switch (T(calibrationState)) {
    case CALIBRATION_INITIALIZATION:
        DEBUG_PRINT("runCalibrationProgram: initialization\n");

        T(numSamplesCalibration) = 0;
        memset(T(dataCalibration), 0, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t));

        /* Power on sensor */
        switch (idx) {
        case ACCEL:
            SPI_MULTIWRITE(LSM6DSM_X_OFS_USR_ADDR, buffer, LSM6DSM_TRIAXIAL_NUM_AXIS, 500);
            SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE, 30000);
            break;
        case GYRO:
            SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE | LSM6DSM_ODR_104HZ_REG_VALUE, 100000);
            break;
        default:
            return;
        }

        T(calibrationState) = CALIBRATION_READ_DATA;
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__);
        break;

    case CALIBRATION_READ_DATA:
        if (T(numSamplesCalibration) > 0) {
            sensorData = &T_SLAVE_INTERFACE(tmpDataBuffer[1]);
            T(dataCalibration[0]) += (int16_t)*((uint16_t *)&sensorData[0]);
            T(dataCalibration[1]) += (int16_t)*((uint16_t *)&sensorData[2]);
            T(dataCalibration[2]) += (int16_t)*((uint16_t *)&sensorData[4]);
        }
        T(numSamplesCalibration)++;

        if (T(numSamplesCalibration) <= numberOfAverage) {
            DEBUG_PRINT("runCalibrationProgram: reading output data\n");

            switch (idx) {
            case ACCEL:
                SPI_READ(LSM6DSM_OUTX_L_XL_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000);
                break;
            case GYRO:
                SPI_READ(LSM6DSM_OUTX_L_G_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 10000);
                break;
            default:
                return;
            }
            lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__);
            break;
        }

        T(dataCalibration[0]) /= numberOfAverage;
        T(dataCalibration[1]) /= numberOfAverage;
        T(dataCalibration[2]) /= numberOfAverage;
        T(calibrationState) = CALIBRATION_VERIFICATION;

    case CALIBRATION_VERIFICATION: ;
        uint8_t sType;

        switch (idx) {
        case ACCEL:
            sType = SENS_TYPE_ACCEL;

            /* Power off sensor */
            SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE);

            /* Supposed 0,0,1g (Android coordinate system) */
            T(dataCalibration[0]) = -T(dataCalibration[0]);
            T(dataCalibration[1]) = -T(dataCalibration[1]);
            T(dataCalibration[2]) = T(dataCalibration[2]) - LSM6DSM_1G_IN_LSB_CALIBRATION;

            for (int8_t i = 0; i < LSM6DSM_TRIAXIAL_NUM_AXIS; i++)
                buffer[i] = lsm6dsm_convertAccelOffsetValue(T(dataCalibration[i]));

            SPI_MULTIWRITE(LSM6DSM_X_OFS_USR_ADDR, buffer, LSM6DSM_TRIAXIAL_NUM_AXIS);
            break;
        case GYRO:
            sType = SENS_TYPE_GYRO;

            /* Power off sensor */
            SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE);

            memcpy(T(gyroCalibrationData), T(dataCalibration), LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t));
            break;
        default:
            return;
        }

        INFO_PRINT("runCalibrationProgram: completed. offset [LSB]: %ld %ld %ld\n", T(dataCalibration[0]), T(dataCalibration[1]), T(dataCalibration[2]));
        lsm6dsm_sendCalibrationResult(sType, SENSOR_APP_EVT_STATUS_SUCCESS, T(dataCalibration[0]), T(dataCalibration[1]), T(dataCalibration[2]));

        T(calibrationState) = CALIBRATION_COMPLETED;
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[idx]), __FUNCTION__);
        break;

    default:
        break;
    }
}


#ifdef LSM6DSM_I2C_MASTER_AK09916
/*
 * lsm6dsm_runAbsoluteSelfTestProgram: state machine that is executing self-test verifying absolute value
 */
static void lsm6dsm_runAbsoluteSelfTestProgram(void)
{
    TDECL();
    uint8_t *sensorData;

    switch (T(selftestState)) {
    case SELFTEST_INITIALIZATION: ;
        DEBUG_PRINT("runAbsoluteSelfTestProgram: initialization\n");

        T(numSamplesSelftest) = 0;
        memset(T(dataSelftestEnabled), 0, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t));

        /* Enable accelerometer and sensor-hub */
        SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE);
        T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;

        SPI_WRITE_SLAVE_SENSOR_REGISTER(AK09916_CNTL2_ADDR, AK09916_ENABLE_SELFTEST_MODE, SENSOR_HZ(104.0f), MAGN, 20000);

        T(selftestState) = SELFTEST_READ_EST_DATA;
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__);
        break;

    case SELFTEST_READ_EST_DATA:
        if (T(numSamplesSelftest) > 0) {
            sensorData = &T_SLAVE_INTERFACE(tmpDataBuffer[1]);
            T(dataSelftestEnabled[0]) += (int16_t)*((uint16_t *)&sensorData[0]);
            T(dataSelftestEnabled[1]) += (int16_t)*((uint16_t *)&sensorData[2]);
            T(dataSelftestEnabled[2]) += (int16_t)*((uint16_t *)&sensorData[4]);
        }
        T(numSamplesSelftest)++;

        if (T(numSamplesSelftest) <= LSM6DSM_NUM_AVERAGE_SELFTEST) {
            DEBUG_PRINT("runAbsoluteSelfTestProgram: reading output data while self-test is enabled\n");

            SPI_READ(LSM6DSM_OUT_SENSORHUB1_ADDR, LSM6DSM_ONE_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tmpDataBuffer), 20000);
            lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__);
            break;
        }

        T(dataSelftestEnabled[0]) /= LSM6DSM_NUM_AVERAGE_SELFTEST;
        T(dataSelftestEnabled[1]) /= LSM6DSM_NUM_AVERAGE_SELFTEST;
        T(dataSelftestEnabled[2]) /= LSM6DSM_NUM_AVERAGE_SELFTEST;
        T(selftestState) = SELFTEST_VERIFICATION;

    case SELFTEST_VERIFICATION: ;
        bool testPassed = true;

        if ((T(dataSelftestEnabled[0]) < AK09916_SELFTEST_LOW_THR_XY_LSB) ||
                (T(dataSelftestEnabled[0]) > AK09916_SELFTEST_HIGH_THR_XYZ_LSB)) {
            testPassed = false;
            ERROR_PRINT("runAbsoluteSelfTestProgram: axis-0 out of spec! Read: %ldLSB\n", T(dataSelftestEnabled[0]));
        }
        if ((T(dataSelftestEnabled[1]) < AK09916_SELFTEST_LOW_THR_XY_LSB) ||
                (T(dataSelftestEnabled[1]) > AK09916_SELFTEST_HIGH_THR_XYZ_LSB)) {
            testPassed = false;
            ERROR_PRINT("runAbsoluteSelfTestProgram: axis-1 out of spec! Read: %ldLSB\n", T(dataSelftestEnabled[1]));
        }
        if ((T(dataSelftestEnabled[2]) < AK09916_SELFTEST_LOW_THR_Z_LSB) ||
                (T(dataSelftestEnabled[2]) > AK09916_SELFTEST_HIGH_THR_XYZ_LSB)) {
            testPassed = false;
            ERROR_PRINT("runAbsoluteSelfTestProgram: axis-2 out of spec! Read: %ldLSB\n", T(dataSelftestEnabled[2]));
        }

        INFO_PRINT("runAbsoluteSelfTestProgram: completed. Test result: %s\n", testPassed ? "pass" : "fail");

        if (testPassed)
            lsm6dsm_sendSelfTestResult(SENS_TYPE_MAG, SENSOR_APP_EVT_STATUS_SUCCESS);
        else
            lsm6dsm_sendSelfTestResult(SENS_TYPE_MAG, SENSOR_APP_EVT_STATUS_ERROR);

        /* Disable accelerometer and sensor-hub */
        SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE);
        SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE);
        T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;

        T(selftestState) = SELFTEST_COMPLETED;
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__);
        break;

    default:
        break;
    }
}
#endif /* LSM6DSM_I2C_MASTER_AK09916 */

/*
 * lsm6dsm_writeEmbeddedRegister: write embedded register
 * @addr: address of register to be written.
 * @value: value to write.
 */
static void lsm6dsm_writeEmbeddedRegister(uint8_t addr, uint8_t value)
{
    TDECL();

#ifdef LSM6DSM_I2C_MASTER_ENABLED
    SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE);
#endif /* LSM6DSM_I2C_MASTER_ENABLED */
    SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000);
    SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50);

    SPI_WRITE(addr, value);

    SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
#ifdef LSM6DSM_I2C_MASTER_ENABLED
    SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
#endif /* LSM6DSM_I2C_MASTER_ENABLED */
    SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
}

#ifdef LSM6DSM_I2C_MASTER_ENABLED
/*
 * lsm6dsm_writeSlaveRegister: write I2C slave register using sensor-hub feature
 * @addr: address of register to be written.
 * @value: value to write.
 * @accelRate: sensor-hub is using accel odr as trigger. This is current accel odr value.
 * @delay: perform a delay after write is completed.
 * @si: which slave sensor needs to be written.
 */
static void lsm6dsm_writeSlaveRegister(uint8_t addr, uint8_t value, uint32_t accelRate, uint32_t delay, enum SensorIndex si)
{
    TDECL();
    uint8_t slave_addr, buffer[2];
    uint32_t SHOpCompleteTime;

    switch (si) {
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
    case MAGN:
        slave_addr = LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT;
        break;
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
    case PRESS:
    case TEMP:
        slave_addr = LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT;
        break;
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    default:
        return;
    }

    if (accelRate > SENSOR_HZ(104.0f))
        SHOpCompleteTime = SENSOR_HZ_RATE_TO_US(SENSOR_HZ(104.0f));
    else
        SHOpCompleteTime = SENSOR_HZ_RATE_TO_US(accelRate);

    /* Perform write to slave sensor and wait write is done (1 accel ODR) */
    SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE);
    SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000);
    SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50);

    buffer[0] = slave_addr << 1;                                     /* LSM6DSM_EMBEDDED_SLV0_ADDR */
    buffer[1] = addr;                                                /* LSM6DSM_EMBEDDED_SLV0_SUBADDR */
    SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR, buffer, 2);
    SPI_WRITE(LSM6DSM_EMBEDDED_DATAWRITE_SLV0_ADDR, value);

    SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
    SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister) | LSM6DSM_MASTER_CONFIG_MASTER_ON);
    SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister), (3 * SHOpCompleteTime) / 2);

    /* After write is completed slave 0 must be set to sleep mode */
    SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE);
    SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister) & ~LSM6DSM_ENABLE_DIGITAL_FUNC, 3000);
    SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50);

    buffer[0] = LSM6DSM_EMBEDDED_SLV0_WRITE_ADDR_SLEEP;              /* LSM6DSM_EMBEDDED_SLV0_ADDR */
    buffer[1] = addr;                                                /* LSM6DSM_EMBEDDED_SLV0_SUBADDR */
    SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR, buffer, 2);

    SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
    SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
    SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister), delay);
}
#endif /* LSM6DSM_I2C_MASTER_ENABLED */

/*
 * lsm6dsm_computeOdr: get index of LSM6DSMImuRates array based on selected rate
 * @rate: ODR value expressed in SENSOR_HZ(x).
 */
static uint8_t lsm6dsm_computeOdr(uint32_t rate)
{
    int i;

    for (i = 0; i < (ARRAY_SIZE(LSM6DSMImuRates) - 1); i++) {
        if (LSM6DSMImuRates[i] == rate)
            break;
    }
    if (i == (ARRAY_SIZE(LSM6DSMImuRates) - 1)) {
        ERROR_PRINT("computeOdr: ODR not valid! Selected smallest ODR available\n");
        i = 0;
    }

    return i;
}

/*
 * lsm6dsm_sensorHzToNs: return delta time of specifi sensor rate
 * @rate: sensor rate expressed in SENSOR_HZ(x).
 */
static uint32_t lsm6dsm_sensorHzToNs(uint32_t rate)
{
    int i;

    for (i = 0; i < (ARRAY_SIZE(LSM6DSMImuRates) - 1); i++) {
        if (LSM6DSMImuRates[i] == rate)
            break;
    }
    if (i == (ARRAY_SIZE(LSM6DSMImuRates) - 1)) {
        ERROR_PRINT("sensorHzToNs: rate not available. Selected smaller rate\n");
        i = 0;
    }

    return LSM6DSMImuRatesInNs[i];
}

/*
 * lsm6dsm_decimatorToFifoDecimatorReg: get decimator reg value based on decimation factor
 * @dec: FIFO sample decimation factor.
 */
static uint8_t lsm6dsm_decimatorToFifoDecimatorReg(uint8_t dec)
{
    uint8_t regValue;

    switch (dec) {
    case 1:
        regValue = LSM6DSM_FIFO_NO_DECIMATION;
        break;
    case 2:
        regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_2;
        break;
    case 3:
        regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_3;
        break;
    case 4:
        regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_4;
        break;
    case 8:
        regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_8;
        break;
    case 16:
        regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_16;
        break;
    case 32:
        regValue = LSM6DSM_FIFO_DECIMATION_FACTOR_32;
        break;
    default:
        regValue = LSM6DSM_FIFO_SAMPLE_NOT_IN_FIFO;
        break;
    }

    return regValue;
}

/*
 * lsm6dsm_calculateFifoDecimators: calculate fifo decimators
 * @RequestedRate: list of ODRs requested by driver for each sensor in FIFO.
 * @minLatency: the function will set the min latency based on all sensors enabled in FIFO.
 */
static bool lsm6dsm_calculateFifoDecimators(uint32_t RequestedRate[FIFO_NUM], uint64_t *minLatency)
{
    TDECL();
    uint8_t i, n, tempDec, decimators[FIFO_NUM] = { 0 }, minDec = UINT8_MAX, maxDec = 0;
    enum SensorIndex sidx;
    bool changed = false;

    T(fifoCntl).triggerRate = T(sensors[ACCEL]).hwRate;
    if (T(sensors[GYRO]).hwRate > T(fifoCntl).triggerRate)
        T(fifoCntl).triggerRate = T(sensors[GYRO]).hwRate;

    for (i = 0; i < FIFO_NUM; i++) {
        sidx = T(fifoCntl).decimatorsIdx[i];
        if (sidx >= NUM_SENSORS)
            continue;

        if (T(sensors[i]).latency < *minLatency)
            *minLatency = T(sensors[i]).latency;
    }

    for (i = 0; i < FIFO_NUM; i++) {
        sidx = T(fifoCntl).decimatorsIdx[i];
        if (sidx >= NUM_SENSORS)
            continue;

        if (RequestedRate[i]) {
            decimators[i] = (T(fifoCntl).triggerRate / RequestedRate[i]) <= 32 ? (T(fifoCntl).triggerRate / RequestedRate[i]) : 32;

            tempDec = decimators[i];
            while (decimators[i] > 1) {
                if (((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * decimators[i]) > *minLatency)
                    decimators[i] /= 2;
                else
                    break;
            }
            T(sensors[sidx]).samplesFifoDecimator = tempDec / decimators[i];
            T(sensors[sidx]).samplesFifoDecimatorCounter = T(sensors[sidx]).samplesFifoDecimator - 1;

            if (decimators[i] < minDec)
                minDec = decimators[i];

            if (decimators[i] > maxDec)
                maxDec = decimators[i];
        }

        DEBUG_PRINT("calculateFifoDecimators: sensorIndex=%d, fifo decimator=%d, software decimation=%d\n", sidx, decimators[i], T(sensors[sidx]).samplesFifoDecimator);

        if (T(fifoCntl).decimators[i] != decimators[i]) {
            T(fifoCntl).decimators[i] = decimators[i];
            changed = true;
        }
    }

    /* Embedded timestamp slot */
    T(fifoCntl).decimators[FIFO_DS4] = minDec;

    T(fifoCntl).minDecimator = minDec;
    T(fifoCntl).maxDecimator = maxDec;
    T(fifoCntl).maxMinDecimator = maxDec / minDec;
    T(fifoCntl).totalSip = 0;

    if (maxDec > 0) {
        T(time).theoreticalDeltaTimeLSB = cpuMathU64DivByU16((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).minDecimator, LSM6DSM_TIME_RESOLUTION);
        T(time).deltaTimeMarginLSB = ((T(time).theoreticalDeltaTimeLSB) * 10) / 100;

        for (i = 0; i < FIFO_NUM; i++) {
            if (T(fifoCntl).decimators[i] > 0)
                T(fifoCntl).totalSip += (maxDec / T(fifoCntl).decimators[i]);
        }
    }

    DEBUG_PRINT("calculateFifoDecimators: samples in pattern=%d\n", T(fifoCntl).totalSip);

    for (i = 0; i < T(fifoCntl).maxMinDecimator; i++) {
        T(fifoCntl).timestampPosition[i] = 0;
        for (n = 0; n < FIFO_NUM - 1; n++) {
            if ((T(fifoCntl).decimators[n] > 0) && ((i % (T(fifoCntl).decimators[n] / T(fifoCntl).minDecimator)) == 0))
                T(fifoCntl).timestampPosition[i] += LSM6DSM_ONE_SAMPLE_BYTE;
        }
    }

    return changed;
}

/*
 * lsm6dsm_calculateWatermark: calculate fifo watermark level
 * @minLatency: min latency requested by system based on all sensors in FIFO.
 */
static bool lsm6dsm_calculateWatermark(uint64_t *minLatency)
{
    TDECL();
    uint64_t patternRate, tempLatency;
    uint16_t watermark;
    uint16_t i = 1;

    if (T(fifoCntl).totalSip > 0) {
        patternRate = (uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator;

        do {
            tempLatency = patternRate * (++i);
        } while ((tempLatency < *minLatency) && (i <= LSM6DSM_MAX_WATERMARK_VALUE));

        watermark = (i - 1) * T(fifoCntl).totalSip;

        while (watermark > LSM6DSM_MAX_WATERMARK_VALUE) {
            watermark /= 2;
            watermark = watermark - (watermark % T(fifoCntl).totalSip);
        }

        DEBUG_PRINT("calculateWatermark: level=#%d, min latency=%lldns\n", watermark, *minLatency);

        if (T(fifoCntl).watermark != watermark) {
            T(fifoCntl).watermark = watermark;
            return true;
        }
    }

   return false;
}

/*
 * lsm6dsm_resetTimestampSync: reset all variables used by sync timestamp task
 */
static inline void lsm6dsm_resetTimestampSync(void)
{
    TDECL();

    T(lastFifoReadTimestamp) = 0;

    T(time).sampleTimestampFromFifoLSB = 0;
    T(time).timestampIsValid = false;
    T(time).lastSampleTimestamp = 0;
    T(time).noTimer.lastTimestampDataAvlRtcTime = 0;
    T(time).noTimer.newTimestampDataAvl = false;
    T(time).timestampSyncTaskLSB = 0;

    time_sync_reset(&T(time).sensorTimeToRtcData);
}

/*
 * lsm6dsm_updateSyncTaskMode: set the best way to sync timestamp
 * @minLatency: min latency of sensors using FIFO.
 */
static inline void lsm6dsm_updateSyncTaskMode(uint64_t *minLatency)
{
    TDECL();

    /* If minLatency is `small` do not use timer to read timestamp and
        temperature but read it during FIFO read. */
    if (*minLatency < LSM6DSM_SYNC_DELTA_INTERVAL) {
        T(time).status = TIME_SYNC_DURING_FIFO_READ;
    } else {
        T(time).status = TIME_SYNC_TIMER;

        if (!osEnqueuePrivateEvt(EVT_TIME_SYNC, 0, NULL, mTask.tid)) {
            T(pendingTimeSyncTask) = true;
            ERROR_PRINT("updateSyncTaskMode: failed to enqueue time sync event\n");
        }
    }
}

/*
 * lsm6dsm_updateOdrs: update ODRs based on rates
 */
static bool lsm6dsm_updateOdrs(void)
{
    TDECL();
    bool accelOdrChanged = false, gyroOdrChanged = false, decChanged, watermarkChanged, gyroFirstEnable = false;
    uint32_t maxRate, maxPushDataRate[FIFO_NUM] = { 0 };
    uint64_t minLatency = UINT64_MAX;
    uint8_t i, regValue, buffer[5];
    uint16_t watermarkReg;

    maxRate = 0;

    /* Verify accel odr */
    for (i = 0; i < NUM_SENSORS; i++) {
        if (T(sensors[ACCEL]).rate[i] > maxRate)
            maxRate = T(sensors[ACCEL]).rate[i] < SENSOR_HZ(26.0f / 2.0f) ? SENSOR_HZ(26.0f / 2.0f) : T(sensors[ACCEL]).rate[i];

        if ((T(sensors[ACCEL]).rate[i] > maxPushDataRate[FIFO_ACCEL]) && T(sensors[ACCEL]).dependenciesRequireData[i])
            maxPushDataRate[FIFO_ACCEL] = T(sensors[ACCEL]).rate[i];
    }
    if (T(sensors[ACCEL]).hwRate != maxRate) {
        T(sensors[ACCEL]).hwRate = maxRate;
        accelOdrChanged = true;
    }

    maxRate = 0;

    /* Verify gyro odr */
    for (i = 0; i < NUM_SENSORS; i++) {
        if (T(sensors[GYRO]).rate[i] > maxRate)
            maxRate = T(sensors[GYRO]).rate[i] < SENSOR_HZ(26.0f / 2.0f) ? SENSOR_HZ(26.0f / 2.0f) : T(sensors[GYRO]).rate[i];

        if (T(sensors[GYRO]).rate[i] > maxPushDataRate[FIFO_GYRO])
            maxPushDataRate[FIFO_GYRO] = T(sensors[GYRO]).rate[i];
    }
    if (T(sensors[GYRO]).hwRate != maxRate) {
        /* If gyro is enabled from PowerDown more samples needs to be discarded */
        if (T(sensors[GYRO]).hwRate == 0)
            gyroFirstEnable = true;

        T(sensors[GYRO]).hwRate = maxRate;
        gyroOdrChanged = true;
    }

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
    /* If magnetometer is enabled, FIFO is used for it */
    maxPushDataRate[FIFO_DS3] = T(sensors[MAGN]).rate[MAGN];
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
    /* If magnetometer is not available FIFO can be used to store barometer sensor data */
    maxPushDataRate[FIFO_DS3] = T(sensors[PRESS]).rate[PRESS] > T(sensors[TEMP]).rate[TEMP] ? T(sensors[PRESS]).rate[PRESS] : T(sensors[TEMP]).rate[TEMP];
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED, LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

    decChanged = lsm6dsm_calculateFifoDecimators(maxPushDataRate, &minLatency);
    watermarkChanged = lsm6dsm_calculateWatermark(&minLatency);
    watermarkReg = T(fifoCntl).watermark * 3;

    if (accelOdrChanged || gyroOdrChanged || decChanged) {
        /* read all FIFO content and disable it */
        DEBUG_PRINT("updateOdrs: disabling FIFO\n");
        T(time).status = TIME_SYNC_DISABLED;

        SPI_WRITE(LSM6DSM_TIMESTAMP2_REG_ADDR, LSM6DSM_TIMESTAMP2_REG_RESET_TIMESTAMP);
        SPI_WRITE(LSM6DSM_FIFO_CTRL5_ADDR, LSM6DSM_FIFO_BYPASS_MODE, 25);
    }

    if (accelOdrChanged) {
        if (T(sensors[ACCEL]).hwRate == 0) {
            DEBUG_PRINT("updateOdrs: no one is using accel, disabling it\n");
            regValue = 0;
        } else {
            DEBUG_PRINT("updateOdrs: accel in use, updating odr to %dHz\n", (int)(T(sensors[ACCEL]).hwRate / 1024));
            i = lsm6dsm_computeOdr(T(sensors[ACCEL]).hwRate);
            regValue = LSM6DSMImuRatesRegValue[i];
            T(sensors[ACCEL]).samplesToDiscard = LSM6DSMAccelRatesSamplesToDiscard[i] /
                                                    (T(sensors[ACCEL]).hwRate / (T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_ACCEL]));

            if (T(sensors[ACCEL]).samplesToDiscard == 0)
                T(sensors[ACCEL]).samplesToDiscard = 1;

            T(sensors[ACCEL]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_ACCEL]) /
                                                    T(sensors[ACCEL]).samplesFifoDecimator) / T(sensors[ACCEL]).rate[ACCEL];
            T(sensors[ACCEL]).samplesDecimatorCounter = T(sensors[ACCEL]).samplesDecimator - 1;
        }
        SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | regValue, 30);
    }

    if (gyroOdrChanged) {
        if (T(sensors[GYRO]).hwRate == 0) {
            DEBUG_PRINT("updateOdrs: no one is using gyro, disabling it\n");
            regValue = 0;
        } else {
            DEBUG_PRINT("updateOdrs: gyro in use, updating odr to %dHz\n", (int)(T(sensors[GYRO]).hwRate / 1024));
            i = lsm6dsm_computeOdr(T(sensors[GYRO]).hwRate);
            regValue = LSM6DSMImuRatesRegValue[i];
            T(sensors[GYRO]).samplesToDiscard = LSM6DSMGyroRatesSamplesToDiscard[i];

            if (gyroFirstEnable)
                T(sensors[GYRO]).samplesToDiscard += LSM6DSMRatesSamplesToDiscardGyroPowerOn[i];

            T(sensors[GYRO]).samplesToDiscard /= (T(sensors[GYRO]).hwRate / (T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_GYRO]));

            if (T(sensors[GYRO]).samplesToDiscard == 0)
                T(sensors[GYRO]).samplesToDiscard = 1;

            T(sensors[GYRO]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_GYRO]) /
                                                    T(sensors[GYRO]).samplesFifoDecimator) / T(sensors[GYRO]).rate[GYRO];
            T(sensors[GYRO]).samplesDecimatorCounter = T(sensors[GYRO]).samplesDecimator - 1;
        }
        SPI_WRITE(LSM6DSM_CTRL2_G_ADDR, LSM6DSM_CTRL2_G_BASE | regValue, 30);
    }

    /* Program Fifo and enable or disable it */
    if (accelOdrChanged || gyroOdrChanged || decChanged) {
        buffer[0] = *((uint8_t *)&watermarkReg);
        buffer[1] = (*((uint8_t *)&watermarkReg + 1) & LSM6DSM_FIFO_CTRL2_FTH_MASK) | LSM6DSM_ENABLE_FIFO_TIMESTAMP;
        buffer[2] = (lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_GYRO]) << 3) |
                    lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_ACCEL]);
        buffer[3] = (lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_DS4]) << 3) |
                    lsm6dsm_decimatorToFifoDecimatorReg(T(fifoCntl).decimators[FIFO_DS3]);

        for (i = 0; i < FIFO_NUM - 1; i++) {
            if (T(fifoCntl).decimators[i] > 0)
                break;
        }
        if (i < (FIFO_NUM - 1)) {
            /* Someone want to use FIFO */
            DEBUG_PRINT("updateOdrs: enabling FIFO in continuos mode\n");
            buffer[4] = LSM6DSM_FIFO_CONTINUOS_MODE;

            lsm6dsm_resetTimestampSync();
            lsm6dsm_updateSyncTaskMode(&minLatency);
        } else {
            /* No one is using FIFO */
            buffer[4] = LSM6DSM_FIFO_BYPASS_MODE;

#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
            if ((T(sensors[PRESS]).rate[PRESS] > 0) || (T(sensors[TEMP]).rate[TEMP] > 0)) {
                uint64_t latencyOnlyBaro = LSM6DSM_SYNC_DELTA_INTERVAL;
                lsm6dsm_resetTimestampSync();
                lsm6dsm_updateSyncTaskMode(&latencyOnlyBaro);
            }
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
        }

        SPI_MULTIWRITE(LSM6DSM_FIFO_CTRL1_ADDR, buffer, 5);
    } else {
        if (watermarkChanged) {
            lsm6dsm_updateSyncTaskMode(&minLatency);

            buffer[0] = *((uint8_t *)&watermarkReg);
            buffer[1] = (*((uint8_t *)&watermarkReg + 1) & LSM6DSM_FIFO_CTRL2_FTH_MASK) | LSM6DSM_ENABLE_FIFO_TIMESTAMP;
            SPI_MULTIWRITE(LSM6DSM_FIFO_CTRL1_ADDR, buffer, 2);
        }
    }

    if (accelOdrChanged || gyroOdrChanged || decChanged || watermarkChanged)
        return true;

    return false;
}

/*
 * lsm6dsm_setAccelPower: enable/disable accelerometer sensor
 * @on: enable or disable sensor.
 * @cookie: private data.
 */
static bool lsm6dsm_setAccelPower(bool on, void *cookie)
{
    TDECL();

    /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
        If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
    if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
        INFO_PRINT("setAccelPower: %s\n", on ? "enable" : "disable");

        if (on)
            osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[ACCEL]), NULL, mTask.tid);
        else {
            T(sensors[ACCEL]).rate[ACCEL] = 0;
            T(sensors[ACCEL]).latency = UINT64_MAX;

            if (lsm6dsm_updateOdrs())
                lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[ACCEL]), __FUNCTION__);
            else
                osEnqueuePrivateEvt(EVT_SENSOR_POWERING_DOWN, &T(sensors[ACCEL]), NULL, mTask.tid);
        }
    } else {
        T(pendingEnableConfig[ACCEL]) = true;
        T(sensors[ACCEL]).pConfig.enable = on;
    }

    return true;
}

/*
 * lsm6dsm_setGyroPower: enable/disable gyroscope sensor
 * @on: enable or disable sensor.
 * @cookie: private data.
 */
static bool lsm6dsm_setGyroPower(bool on, void *cookie)
{
    TDECL();

    /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
        If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
    if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
        INFO_PRINT("setGyroPower: %s\n", on ? "enable" : "disable");

        if (on)
            osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[GYRO]), NULL, mTask.tid);
        else {
#ifdef LSM6DSM_GYRO_CALIB_ENABLED
            T(sensors[ACCEL]).rate[GYRO] = 0;
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
            T(sensors[GYRO]).rate[GYRO] = 0;
            T(sensors[GYRO]).latency = UINT64_MAX;

            if (lsm6dsm_updateOdrs()) {
                lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__);
            } else
                osEnqueuePrivateEvt(EVT_SENSOR_POWERING_DOWN, &T(sensors[GYRO]), NULL, mTask.tid);
        }
    } else {
        T(pendingEnableConfig[GYRO]) = true;
        T(sensors[GYRO]).pConfig.enable = on;
    }

    return true;
}

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
/*
 * lsm6dsm_setMagnPower: enable/disable magnetometer sensor
 * @on: enable or disable sensor.
 * @cookie: private data.
 */
static bool lsm6dsm_setMagnPower(bool on, void *cookie)
{
    TDECL();

    /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
        If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
    if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
        INFO_PRINT("setMagnPower: %s\n", on ? "enable" : "disable");

        if (on) {
            if (T(masterConfigDependencies) != 0) {
                T(masterConfigDependencies) |= BIT(MAGN);

                osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[MAGN]), NULL, mTask.tid);
            } else {
                T(masterConfigDependencies) |= BIT(MAGN);
                T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;

                SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));

                lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__);
            }
        } else {
            T(masterConfigDependencies) &= ~BIT(MAGN);

            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR,
                    LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE,
                    T(sensors[ACCEL]).hwRate, MAGN);

            if (T(masterConfigDependencies) == 0) {
                DEBUG_PRINT("setMagnPower: no sensors enabled on i2c master, disabling it\n");
                T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
                SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
            }

            T(sensors[ACCEL]).rate[MAGN] = 0;
            T(sensors[MAGN]).rate[MAGN] = 0;
            T(sensors[MAGN]).latency = UINT64_MAX;
            T(sensors[MAGN]).hwRate = 0;

            lsm6dsm_updateOdrs();

            lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__);
        }
    } else {
        T(pendingEnableConfig[MAGN]) = true;
        T(sensors[MAGN]).pConfig.enable = on;
    }

    return true;
}
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
/*
 * lsm6dsm_setPressPower: enable/disable pressure sensor
 * @on: enable or disable sensor.
 * @cookie: private data.
 */
static bool lsm6dsm_setPressPower(bool on, void *cookie)
{
    TDECL();

    /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
        If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
    if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
        INFO_PRINT("setPressPower: %s\n", on ? "enable" : "disable");

        if (on) {
            if (T(masterConfigDependencies) != 0) {
                T(masterConfigDependencies) |= BIT(PRESS);

                osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[PRESS]), NULL, mTask.tid);
            } else {
                T(masterConfigDependencies) |= BIT(PRESS);
                T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;

                SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));

                lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[PRESS]), __FUNCTION__);
            }
        } else {
            uint8_t i, reg_value = LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE;

            T(masterConfigDependencies) &= ~BIT(PRESS);

            if (T(sensors[TEMP]).enabled) {
                i = lsm6dsm_computeOdr(T(sensors[TEMP]).rate[TEMP]);
                reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i);
            } else
                reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE;

            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR, reg_value,
                    T(sensors[ACCEL]).hwRate, PRESS);

            if (T(masterConfigDependencies) == 0) {
                DEBUG_PRINT("setPressPower: no sensors enabled on i2c master, disabling it\n");
                T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
                SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
            }

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
            if (T(baroTimerId)) {
                timTimerCancel(T(baroTimerId));
                T(baroTimerId) = 0;

                T(pendingBaroTimerTask) = false;
                T(time).timestampBaroLSB = 0;

                if (T(sensors[TEMP]).enabled)
                    T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(T(sensors[TEMP]).rate[TEMP]), 0, 50, lsm6dsm_baroTimerCallback, NULL, false);
            }
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

            T(sensors[ACCEL]).rate[PRESS] = 0;
            T(sensors[PRESS]).rate[PRESS] = 0;
            T(sensors[PRESS]).latency = UINT64_MAX;
            T(sensors[PRESS]).hwRate = 0;

            lsm6dsm_updateOdrs();

            lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[PRESS]), __FUNCTION__);
        }
    } else {
        T(pendingEnableConfig[PRESS]) = true;
        T(sensors[PRESS]).pConfig.enable = on;
    }

    return true;
}

/*
 * lsm6dsm_setTempPower: enable/disable temperature sensor
 * @on: enable or disable sensor.
 * @cookie: private data.
 */
static bool lsm6dsm_setTempPower(bool on, void *cookie)
{
    TDECL();

    /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
        If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
    if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
        INFO_PRINT("setTempPower: %s\n", on ? "enable" : "disable");

        if (on) {
            if (T(masterConfigDependencies) != 0) {
                T(masterConfigDependencies) |= BIT(TEMP);

                osEnqueuePrivateEvt(EVT_SENSOR_POWERING_UP, &T(sensors[TEMP]), NULL, mTask.tid);
            } else {
                T(masterConfigDependencies) |= BIT(TEMP);
                T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;

                SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));

                lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[TEMP]), __FUNCTION__);
            }
        } else {
            uint8_t i, reg_value = LSM6DSM_SENSOR_SLAVE_BARO_POWER_BASE;

            T(masterConfigDependencies) &= ~BIT(TEMP);

            if (T(sensors[PRESS]).enabled) {
                i = lsm6dsm_computeOdr(T(sensors[PRESS]).rate[PRESS]);
                reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i);
            } else
                reg_value |= LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE;

            SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_POWER_ADDR, reg_value,
                    T(sensors[ACCEL]).hwRate, TEMP);

            if (T(masterConfigDependencies) == 0) {
                DEBUG_PRINT("setTempPower: no sensors enabled on i2c master, disabling it\n");
                T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;
                SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, T(masterConfigRegister));
            }

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
            if (T(baroTimerId)) {
                timTimerCancel(T(baroTimerId));
                T(baroTimerId) = 0;

                T(pendingBaroTimerTask) = false;
                T(time).timestampBaroLSB = 0;

                if (T(sensors[PRESS]).enabled)
                    T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(T(sensors[PRESS]).rate[PRESS]), 0, 50, lsm6dsm_baroTimerCallback, NULL, false);
            }
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

            T(sensors[ACCEL]).rate[TEMP] = 0;
            T(sensors[TEMP]).rate[TEMP] = 0;
            T(sensors[TEMP]).latency = UINT64_MAX;
            T(sensors[TEMP]).hwRate = 0;

            lsm6dsm_updateOdrs();

            lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[TEMP]), __FUNCTION__);
        }
    } else {
        T(pendingEnableConfig[TEMP]) = true;
        T(sensors[TEMP]).pConfig.enable = on;
    }

    return true;
}
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

/*
 * lsm6dsm_setStepDetectorPower: enable/disable step detector sensor
 * @on: enable or disable sensor.
 * @cookie: private data.
 */
static bool lsm6dsm_setStepDetectorPower(bool on, void *cookie)
{
    TDECL();

    /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
        If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
    if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
        INFO_PRINT("setStepDetectorPower: %s\n", on ? "enable" : "disable");

        if (on) {
            T(pedometerDependencies) |= BIT(STEP_DETECTOR);
            T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC;
            T(int1Register) |= LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE;

            T(sensors[ACCEL]).rate[STEP_DETECTOR] = SENSOR_HZ(26.0f);
            lsm6dsm_updateOdrs();

            SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
            SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
        } else {
            T(pedometerDependencies) &= ~BIT(STEP_DETECTOR);
            T(int1Register) &= ~LSM6DSM_INT_STEP_DETECTOR_ENABLE_REG_VALUE;

            if ((T(pedometerDependencies) & (BIT(STEP_COUNTER) | BIT(SIGN_MOTION))) == 0) {
                DEBUG_PRINT("setStepDetectorPower: no more need pedometer algo, disabling it\n");
                T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC;
            }

            T(sensors[ACCEL]).rate[STEP_DETECTOR] = 0;
            lsm6dsm_updateOdrs();

            SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
            SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
        }

        /* If enable, set INT bit enable and enable accelerometer sensor @26Hz if disabled. If disable, disable INT bit and disable accelerometer if no one need it */
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[STEP_DETECTOR]), __FUNCTION__);
    } else {
        T(pendingEnableConfig[STEP_DETECTOR]) = true;
        T(sensors[STEP_DETECTOR]).pConfig.enable = on;
    }

    return true;
}

/*
 * lsm6dsm_setStepCounterPower: enable/disable step counter sensor
 * @on: enable or disable sensor.
 * @cookie: private data.
 */
static bool lsm6dsm_setStepCounterPower(bool on, void *cookie)
{
    TDECL();

    /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
        If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
    if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
        INFO_PRINT("setStepCounterPower: %s\n", on ? "enable" : "disable");

        if (on) {
            T(readSteps) = false;
            T(pedometerDependencies) |= BIT(STEP_COUNTER);
            T(embeddedFunctionsRegister) |= LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC;
            T(int2Register) |= LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE;

            T(sensors[ACCEL]).rate[STEP_COUNTER] = SENSOR_HZ(26.0f);
            lsm6dsm_updateOdrs();

            SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
            SPI_WRITE(LSM6DSM_INT2_CTRL_ADDR, T(int2Register));
        } else {
            T(pedometerDependencies) &= ~BIT(STEP_COUNTER);
            T(int2Register) &= ~LSM6DSM_INT_STEP_COUNTER_ENABLE_REG_VALUE;

            if ((T(pedometerDependencies) & (BIT(STEP_DETECTOR) | BIT(SIGN_MOTION))) == 0) {
                DEBUG_PRINT("setStepCounterPower: no more need pedometer algo, disabling it\n");
                T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC;
            }

            T(sensors[ACCEL]).rate[STEP_COUNTER] = 0;
            lsm6dsm_updateOdrs();

            SPI_WRITE(LSM6DSM_INT2_CTRL_ADDR, T(int2Register));
            SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
        }

        /* If enable, set INT bit enable and enable accelerometer sensor @26Hz if disabled. If disable, disable INT bit and disable accelerometer if no one need it */
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[STEP_COUNTER]), __FUNCTION__);
    } else {
        T(pendingEnableConfig[STEP_COUNTER]) = true;
        T(sensors[STEP_COUNTER]).pConfig.enable = on;
    }

    return true;
}

/*
 * lsm6dsm_setSignMotionPower: enable/disable significant motion sensor
 * @on: enable or disable sensor.
 * @cookie: private data.
 */
static bool lsm6dsm_setSignMotionPower(bool on, void *cookie)
{
    TDECL();

    /* If current status is SENSOR_IDLE set state to SENSOR_POWERING_* and execute command directly.
        If current status is NOT SENSOR_IDLE add pending config that will be managed before go back to SENSOR_IDLE. */
    if (trySwitchState(on ? SENSOR_POWERING_UP : SENSOR_POWERING_DOWN)) {
        INFO_PRINT("setSignMotionPower: %s\n", on ? "enable" : "disable");

        if (on) {
            T(pedometerDependencies) |= BIT(SIGN_MOTION);
            T(embeddedFunctionsRegister) |= (LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC | LSM6DSM_ENABLE_PEDOMETER_DIGITAL_FUNC);
            T(int1Register) |= LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE;

            T(sensors[ACCEL]).rate[SIGN_MOTION] = SENSOR_HZ(26.0f);
            lsm6dsm_updateOdrs();

            SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
            SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register));
        } else {
            T(pedometerDependencies) &= ~BIT(SIGN_MOTION);
            T(int1Register) &= ~LSM6DSM_INT_SIGN_MOTION_ENABLE_REG_VALUE;

            if ((T(pedometerDependencies) & (BIT(STEP_DETECTOR) | BIT(STEP_COUNTER))) == 0) {
                DEBUG_PRINT("setSignMotionPower: no more need pedometer algo, disabling it\n");
                T(embeddedFunctionsRegister) &= ~LSM6DSM_ENABLE_SIGN_MOTION_DIGITAL_FUNC;
            }

            T(sensors[ACCEL]).rate[SIGN_MOTION] = 0;
            lsm6dsm_updateOdrs();

            SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, T(int1Register), 50000);
            SPI_WRITE(LSM6DSM_CTRL10_C_ADDR, T(embeddedFunctionsRegister));
        }

        /* If enable, set INT bit enable and enable accelerometer sensor @26Hz if disabled. If disable, disable INT bit and disable accelerometer if no one need it */
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[SIGN_MOTION]), __FUNCTION__);
    } else {
        T(pendingEnableConfig[SIGN_MOTION]) = true;
        T(sensors[SIGN_MOTION]).pConfig.enable = on;
    }

    return true;
}

/*
 * lsm6dsm_accelFirmwareUpload: upload accelerometer firmware
 * @cookie: private data.
 */
static bool lsm6dsm_accelFirmwareUpload(void *cookie)
{
    TDECL();

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

    return true;
}

/*
 * lsm6dsm_gyroFirmwareUpload: upload gyroscope firmware
 * @cookie: private data.
 */
static bool lsm6dsm_gyroFirmwareUpload(void *cookie)
{
    TDECL();

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

    return true;
}

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
/*
 * lsm6dsm_magnFirmwareUpload: upload magnetometer firmware
 * @cookie: private data.
 */
static bool lsm6dsm_magnFirmwareUpload(void *cookie)
{
    TDECL();

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

    return true;
}
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
/*
 * lsm6dsm_pressFirmwareUpload: upload pressure firmware
 * @cookie: private data.
 */
static bool lsm6dsm_pressFirmwareUpload(void *cookie)
{
    TDECL();

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

    return true;
}

/*
 * lsm6dsm_tempFirmwareUpload: upload pressure firmware
 * @cookie: private data.
 */
static bool lsm6dsm_tempFirmwareUpload(void *cookie)
{
    TDECL();

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

    return true;
}
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

/*
 * lsm6dsm_stepDetectorFirmwareUpload: upload step detector firmware
 * @cookie: private data.
 */
static bool lsm6dsm_stepDetectorFirmwareUpload(void *cookie)
{
    TDECL();

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

    return true;
}

/*
 * lsm6dsm_stepCounterFirmwareUpload: upload step counter firmware
 * @cookie: private data.
 */
static bool lsm6dsm_stepCounterFirmwareUpload(void *cookie)
{
    TDECL();

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

    return true;
}

/*
 * lsm6dsm_signMotionFirmwareUpload: upload significant motion firmware
 * @cookie: private data.
 */
static bool lsm6dsm_signMotionFirmwareUpload(void *cookie)
{
    TDECL();

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

    return true;
}

/*
 * lsm6dsm_setAccelRate: set accelerometer ODR and report latency (FIFO watermark related)
 * @rate: sensor rate expressed in SENSOR_HZ(x).
 * @latency: max latency valud in ns.
 * @cookie: private data.
 */
static bool lsm6dsm_setAccelRate(uint32_t rate, uint64_t latency, void *cookie)
{
    TDECL();

    if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
        INFO_PRINT("setAccelRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);

        T(sensors[ACCEL]).rate[ACCEL] = rate;
        T(sensors[ACCEL]).latency = latency;

        if (lsm6dsm_updateOdrs())
            lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[ACCEL]), __FUNCTION__);
        else
            osEnqueuePrivateEvt(EVT_SENSOR_CONFIG_CHANGING, &T(sensors[ACCEL]), NULL, mTask.tid);
    } else {
        T(pendingRateConfig[ACCEL]) = true;
        T(sensors[ACCEL].pConfig.rate) = rate;
        T(sensors[ACCEL]).pConfig.latency = latency;
    }

    return true;
}

/*
 * lsm6dsm_setGyroRate: set gyroscope ODR and report latency (FIFO watermark related)
 * @rate: sensor rate expressed in SENSOR_HZ(x).
 * @latency: max latency valud in ns.
 * @cookie: private data.
 */
static bool lsm6dsm_setGyroRate(uint32_t rate, uint64_t latency, void *cookie)
{
    TDECL();

    if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
        INFO_PRINT("setGyroRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);

#ifdef LSM6DSM_GYRO_CALIB_ENABLED
        T(sensors[ACCEL]).rate[GYRO] = rate;
        T(sensors[ACCEL]).dependenciesRequireData[GYRO] = true;
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
        T(sensors[GYRO]).rate[GYRO] = rate;
        T(sensors[GYRO]).latency = latency;

        if (lsm6dsm_updateOdrs())
            lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__);
        else
            osEnqueuePrivateEvt(EVT_SENSOR_CONFIG_CHANGING, &T(sensors[GYRO]), NULL, mTask.tid);
    } else {
        T(pendingRateConfig[GYRO]) = true;
        T(sensors[GYRO]).pConfig.rate = rate;
        T(sensors[GYRO]).pConfig.latency = latency;
    }

    return true;
}

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
/*
 * lsm6dsm_setMagnRate: set magnetometer ODR and report latency (FIFO watermark related)
 * @rate: sensor rate expressed in SENSOR_HZ(x).
 * @latency: max latency valud in ns.
 * @cookie: private data.
 */
static bool lsm6dsm_setMagnRate(uint32_t rate, uint64_t latency, void *cookie)
{
    TDECL();
    uint8_t i;

    if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
        INFO_PRINT("setMagnRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);

        T(sensors[ACCEL]).rate[MAGN] = rate;
#ifdef LSM6DSM_MAGN_CALIB_ENABLED
        T(sensors[ACCEL]).dependenciesRequireData[MAGN] = true;
#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
        T(sensors[MAGN]).rate[MAGN] = rate;
        T(sensors[MAGN]).latency = latency;

        lsm6dsm_updateOdrs();

        /* This call return index of LSM6DSMImuRates struct element */
        i = lsm6dsm_computeOdr(rate);
        T(sensors[MAGN]).hwRate = LSM6DSMSHRates[i];
        T(sensors[MAGN]).samplesToDiscard = 3;

        T(sensors[MAGN]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[MAGN]).samplesFifoDecimator) / T(sensors[MAGN]).rate[MAGN];
        T(sensors[MAGN]).samplesDecimatorCounter = T(sensors[MAGN]).samplesDecimator - 1;

#ifdef LSM6DSM_I2C_MASTER_LIS3MDL
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ADDR,
                LSM6DSM_SENSOR_SLAVE_MAGN_POWER_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE,
                T(sensors[ACCEL]).hwRate, MAGN);
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR,
                LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i),
                T(sensors[ACCEL]).hwRate, MAGN);
#else /* LSM6DSM_I2C_MASTER_LIS3MDL */
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_ODR_ADDR,
                LSM6DSM_SENSOR_SLAVE_MAGN_ODR_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_ON_VALUE | LSM6DSM_SENSOR_SLAVE_MAGN_RATES_REG_VALUE(i),
                T(sensors[ACCEL]).hwRate, MAGN);
#endif /* LSM6DSM_I2C_MASTER_LIS3MDL */

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[MAGN]), __FUNCTION__);
    } else {
        T(pendingRateConfig[MAGN]) = true;
        T(sensors[MAGN]).pConfig.rate = rate;
        T(sensors[MAGN]).pConfig.latency = latency;
    }

    return true;
}
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
/*
 * lsm6dsm_setPressRate: set pressure ODR and report latency (FIFO watermark related)
 * @rate: sensor rate expressed in SENSOR_HZ(x).
 * @latency: max latency valud in ns.
 * @cookie: private data.
 */
static bool lsm6dsm_setPressRate(uint32_t rate, uint64_t latency, void *cookie)
{
    TDECL();
    uint8_t i;

    if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
        INFO_PRINT("setPressRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);

        T(sensors[ACCEL]).rate[PRESS] = rate;
        T(sensors[PRESS]).rate[PRESS] = rate;
        T(sensors[PRESS]).latency = latency;

        lsm6dsm_updateOdrs();

        if (T(sensors[TEMP]).enabled) {
            if (rate < T(sensors[TEMP]).rate[TEMP])
                rate = T(sensors[TEMP]).rate[TEMP];
        }

        /* This call return index of LSM6DSMImuRates struct element */
        i = lsm6dsm_computeOdr(rate);
        T(sensors[PRESS]).hwRate = LSM6DSMSHRates[i];
        T(sensors[PRESS]).samplesToDiscard = 3;

#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
        if (T(baroTimerId)) {
            timTimerCancel(T(baroTimerId));
            T(baroTimerId) = 0;
            T(pendingBaroTimerTask) = false;
        }

        T(sensors[PRESS]).samplesDecimator = rate / T(sensors[PRESS]).rate[PRESS];
        T(sensors[TEMP]).samplesDecimator = rate / T(sensors[TEMP]).rate[TEMP];
        T(time).timestampBaroLSB = 0;

        T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(rate), 0, 50, lsm6dsm_baroTimerCallback, NULL, false);
#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
        T(sensors[PRESS]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[PRESS]).rate[PRESS];
        T(sensors[TEMP]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[TEMP]).rate[TEMP];
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

        T(sensors[PRESS]).samplesDecimatorCounter = T(sensors[PRESS]).samplesDecimator - 1;
        T(sensors[TEMP]).samplesDecimatorCounter = T(sensors[TEMP]).samplesDecimator - 1;

        SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR,
                LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE | LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i),
                T(sensors[ACCEL]).hwRate, PRESS);

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[PRESS]), __FUNCTION__);
    } else {
        T(pendingRateConfig[PRESS]) = true;
        T(sensors[PRESS]).pConfig.rate = rate;
        T(sensors[PRESS]).pConfig.latency = latency;
    }

    return true;
}

/*
 * lsm6dsm_setTempRate: set temperature ODR and report latency (FIFO watermark related)
 * @rate: sensor rate expressed in SENSOR_HZ(x).
 * @latency: max latency valud in ns.
 * @cookie: private data.
 */
static bool lsm6dsm_setTempRate(uint32_t rate, uint64_t latency, void *cookie)
{
    TDECL();
    uint8_t i;

    if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
        INFO_PRINT("setTempRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);

        T(sensors[ACCEL]).rate[TEMP] = rate;
        T(sensors[TEMP]).rate[TEMP] = rate;
        T(sensors[TEMP]).latency = latency;

        lsm6dsm_updateOdrs();

        if (T(sensors[PRESS]).enabled) {
            if (rate < T(sensors[PRESS]).rate[PRESS])
                rate = T(sensors[PRESS]).rate[PRESS];
        }

        /* This call return index of LSM6DSMImuRates struct element */
        i = lsm6dsm_computeOdr(rate);
        T(sensors[TEMP]).hwRate = LSM6DSMSHRates[i];
        T(sensors[TEMP]).samplesToDiscard = 3;

#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
        if (T(baroTimerId)) {
            timTimerCancel(T(baroTimerId));
            T(baroTimerId) = 0;
            T(pendingBaroTimerTask) = false;
        }

        T(sensors[TEMP]).samplesDecimator = rate / T(sensors[PRESS]).rate[PRESS];
        T(sensors[PRESS]).samplesDecimator = rate / T(sensors[PRESS]).rate[PRESS];
        T(time).timestampBaroLSB = 0;

        T(baroTimerId) = timTimerSet(lsm6dsm_sensorHzToNs(rate), 0, 50, lsm6dsm_baroTimerCallback, NULL, false);
#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
        T(sensors[TEMP]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[TEMP]).rate[TEMP];
        T(sensors[PRESS]).samplesDecimator = ((T(fifoCntl).triggerRate / T(fifoCntl).decimators[FIFO_DS3]) / T(sensors[PRESS]).samplesFifoDecimator) / T(sensors[PRESS]).rate[TEMP];
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

        T(sensors[TEMP]).samplesDecimatorCounter = T(sensors[TEMP]).samplesDecimator - 1;
        T(sensors[PRESS]).samplesDecimatorCounter = T(sensors[PRESS]).samplesDecimator - 1;

        SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_ODR_ADDR,
                LSM6DSM_SENSOR_SLAVE_BARO_ODR_BASE | LSM6DSM_SENSOR_SLAVE_BARO_RATES_REG_VALUE(i),
                T(sensors[ACCEL]).hwRate, TEMP);

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[TEMP]), __FUNCTION__);
    } else {
        T(pendingRateConfig[TEMP]) = true;
        T(sensors[TEMP]).pConfig.rate = rate;
        T(sensors[TEMP]).pConfig.latency = latency;
    }

    return true;
}
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

/*
 * lsm6dsm_setStepDetectorRate: set step detector report latency
 * @rate: sensor rate expressed in SENSOR_HZ(x).
 * @latency: max latency valud in ns.
 * @cookie: private data.
 */
static bool lsm6dsm_setStepDetectorRate(uint32_t rate, uint64_t latency, void *cookie)
{
    TDECL();

    INFO_PRINT("setStepDetectorRate: latency=%lldns\n", latency);

    T(sensors[STEP_DETECTOR]).hwRate = rate;
    T(sensors[STEP_DETECTOR]).latency = latency;

    sensorSignalInternalEvt(T(sensors[STEP_DETECTOR]).handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);

    return true;
}

/*
 * lsm6dsm_setStepCounterRate: set step counter report latency
 * @rate: sensor rate expressed in SENSOR_HZ(x).
 * @latency: max latency valud in ns.
 * @cookie: private data.
 */
static bool lsm6dsm_setStepCounterRate(uint32_t rate, uint64_t latency, void *cookie)
{
    TDECL();
    uint8_t i, regValue;

    if (trySwitchState(SENSOR_CONFIG_CHANGING)) {
        if (rate == SENSOR_RATE_ONCHANGE) {
            INFO_PRINT("setStepCounterRate: delivery-rate=on_change, latency=%lldns\n", latency);
        } else
            INFO_PRINT("setStepCounterRate: delivery_rate=%dms, latency=%lldns\n", (int)((1024.0f / rate) * 1000.0f), latency);

        T(sensors[STEP_COUNTER]).hwRate = rate;
        T(sensors[STEP_COUNTER]).latency = latency;

        if (rate != SENSOR_RATE_ONCHANGE) {
        for (i = 0; i < ARRAY_SIZE(LSM6DSMStepCounterRates); i++) {
            if (rate == LSM6DSMStepCounterRates[i])
                break;
        }
        if (i >= (ARRAY_SIZE(LSM6DSMStepCounterRates) - 2))
            regValue = 0;
        else
            regValue = (128 >> i);
        } else
            regValue = 0;

        lsm6dsm_writeEmbeddedRegister(LSM6DSM_EMBEDDED_STEP_COUNT_DELTA_ADDR, regValue);

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &T(sensors[GYRO]), __FUNCTION__);
    } else {
        T(pendingRateConfig[STEP_COUNTER]) = true;
        T(sensors[STEP_COUNTER]).pConfig.rate = rate;
        T(sensors[STEP_COUNTER]).pConfig.latency = latency;
    }

    return true;
}

/*
 * lsm6dsm_setSignMotionRate: set significant motion report latency
 * @rate: sensor rate expressed in SENSOR_HZ(x).
 * @latency: max latency valud in ns.
 * @cookie: private data.
 */
static bool lsm6dsm_setSignMotionRate(uint32_t rate, uint64_t latency, void *cookie)
{
    TDECL();

    DEBUG_PRINT("setSignMotionRate: rate=%dHz, latency=%lldns\n", (int)(rate / 1024), latency);

    T(sensors[SIGN_MOTION]).rate[SIGN_MOTION] = rate;
    T(sensors[SIGN_MOTION]).latency = latency;

    sensorSignalInternalEvt(T(sensors[SIGN_MOTION]).handle, SENSOR_INTERNAL_EVT_RATE_CHG, rate, latency);

    return true;
}

/*
 * lsm6dsm_accelFlush: send accelerometer flush event
 * @cookie: private data.
 */
static bool lsm6dsm_accelFlush(void *cookie)
{
    TDECL();

    if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
        INFO_PRINT("accelFlush: flush accelerometer data\n");

        if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) {
            osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_ACCEL), SENSOR_DATA_EVENT_FLUSH, NULL);
            osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid);
            return true;
        }

        T(sendFlushEvt[ACCEL]) = true;

        SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
    } else
        T(pendingFlush[ACCEL])++;

    return true;
}

/*
 * lsm6dsm_gyroFlush: send gyroscope flush event
 * @cookie: private data.
 */
static bool lsm6dsm_gyroFlush(void *cookie)
{
    TDECL();

    if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
        INFO_PRINT("gyroFlush: flush gyroscope data\n");

        if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) {
            osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_GYRO), SENSOR_DATA_EVENT_FLUSH, NULL);
            osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid);
            return true;
        }

        T(sendFlushEvt[GYRO]) = true;

        SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
    } else
        T(pendingFlush[GYRO])++;

    return true;
}

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
/*
 * lsm6dsm_magnFlush: send magnetometer flush event
 * @cookie: private data.
 */
static bool lsm6dsm_magnFlush(void *cookie)
{
    TDECL();

    if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
        INFO_PRINT("magnFlush: flush magnetometer data\n");

        if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) {
            osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_MAG), SENSOR_DATA_EVENT_FLUSH, NULL);
            osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid);
            return true;
        }

        T(sendFlushEvt[MAGN]) = true;

        SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
    } else
        T(pendingFlush[MAGN])++;

    return true;
}
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
/*
 * lsm6dsm_pressFlush: send pressure flush event
 * @cookie: private data.
 */
static bool lsm6dsm_pressFlush(void *cookie)
{
    TDECL();

#if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
    if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
        INFO_PRINT("pressFlush: flush pressure data\n");

        if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) {
            osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), SENSOR_DATA_EVENT_FLUSH, NULL);
            osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid);
            return true;
        }

        T(sendFlushEvt[PRESS]) = true;

        SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
    } else
        T(pendingFlush[PRESS])++;
#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
    INFO_PRINT("pressFlush: flush pressure data\n");

    osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_BARO), SENSOR_DATA_EVENT_FLUSH, NULL);
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

    return true;
}

/*
 * lsm6dsm_tempFlush: send temperature flush event
 * @cookie: private data.
 */
static bool lsm6dsm_tempFlush(void *cookie)
{
    TDECL();

#if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
    if (trySwitchState(SENSOR_INT1_STATUS_REG_HANDLING)) {
        INFO_PRINT("tempFlush: flush temperature data\n");

        if (sensorGetTime() <= (T(lastFifoReadTimestamp) + ((uint64_t)lsm6dsm_sensorHzToNs(T(fifoCntl).triggerRate) * T(fifoCntl).maxDecimator))) {
            osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), SENSOR_DATA_EVENT_FLUSH, NULL);
            osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid);
            return true;
        }

        T(sendFlushEvt[TEMP]) = true;

        SPI_READ(LSM6DSM_FIFO_STATUS1_ADDR, 2, &T_SLAVE_INTERFACE(fifoStatusRegBuffer));
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
    } else
        T(pendingFlush[TEMP])++;
#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
    INFO_PRINT("tempFlush: flush temperature data\n");

    osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), SENSOR_DATA_EVENT_FLUSH, NULL);
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

    return true;
}
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

/*
 * lsm6dsm_stepDetectorFlush: send step detector flush event
 * @cookie: private data.
 */
static bool lsm6dsm_stepDetectorFlush(void *cookie)
{
    TDECL();

    INFO_PRINT("stepDetectorFlush: flush step detector data\n");

    osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_DETECT), SENSOR_DATA_EVENT_FLUSH, NULL);

    return true;
}

/*
 * lsm6dsm_stepCounterFlush: send step counter flush event
 * @cookie: private data.
 */
static bool lsm6dsm_stepCounterFlush(void *cookie)
{
    TDECL();

    INFO_PRINT("stepCounterFlush: flush step counter data\n");

    osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), SENSOR_DATA_EVENT_FLUSH, NULL);

    return true;
}

/*
 * lsm6dsm_signMotionFlush: send significant motion flush event
 * @cookie: private data.
 */
static bool lsm6dsm_signMotionFlush(void *cookie)
{
    TDECL();

    INFO_PRINT("signMotionFlush: flush significant motion data\n");

    osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_SIG_MOTION), SENSOR_DATA_EVENT_FLUSH, NULL);

    return true;
}

/*
 * lsm6dsm_stepCounterSendLastData: send last number of steps
 * @cookie: private data.
 * @tid: task id.
 */
static bool lsm6dsm_stepCounterSendLastData(void *cookie, uint32_t tid)
{
    TDECL();

    INFO_PRINT("stepCounterSendLastData: %lu steps\n", T(totalNumSteps));

    osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), &T(totalNumSteps), NULL);

    return true;
}

/*
 * lsm6dsm_runAccelSelfTest: execute accelerometer self-test
 * @cookie: private data.
 */
static bool lsm6dsm_runAccelSelfTest(void *cookie)
{
    TDECL();

    if (trySwitchState(SENSOR_SELFTEST)) {
        if (!T(sensors[ACCEL]).enabled && (T(sensors[ACCEL]).hwRate == 0) && (T(sensors[GYRO]).hwRate == 0)) {
            INFO_PRINT("runAccelSelfTest: executing accelerometer selftest\n");
            T(selftestState) = SELFTEST_INITIALIZATION;
            lsm6dsm_runGapSelfTestProgram(ACCEL);
            return true;
        } else
            osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid);
    }

    ERROR_PRINT("runAccelSelfTest: cannot run selftest because sensor is busy!\n");
    lsm6dsm_sendSelfTestResult(SENS_TYPE_ACCEL, SENSOR_APP_EVT_STATUS_BUSY);

    return false;
}

/*
 * lsm6dsm_runGyroSelfTest: execute gyroscope self-test
 * @cookie: private data.
 */
static bool lsm6dsm_runGyroSelfTest(void *cookie)
{
    TDECL();

    if (trySwitchState(SENSOR_SELFTEST)) {
        if (!T(sensors[GYRO]).enabled && (T(sensors[GYRO]).hwRate == 0) && (T(sensors[ACCEL]).hwRate == 0)) {
            INFO_PRINT("runGyroSelfTest: executing gyroscope selftest\n");
            T(selftestState) = SELFTEST_INITIALIZATION;
            lsm6dsm_runGapSelfTestProgram(GYRO);
            return true;
        } else
            osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid);
    }

    ERROR_PRINT("runGyroSelfTest: cannot run selftest because sensor is busy!\n");
    lsm6dsm_sendSelfTestResult(SENS_TYPE_GYRO, SENSOR_APP_EVT_STATUS_BUSY);

    return false;
}

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
/*
 * lsm6dsm_runMagnSelfTest: execute magnetometer self-test
 * @cookie: private data.
 */
static bool lsm6dsm_runMagnSelfTest(void *cookie)
{
    TDECL();

    if (trySwitchState(SENSOR_SELFTEST)) {
        if (!T(sensors[MAGN]).enabled && (T(sensors[MAGN]).hwRate == 0) && (T(sensors[GYRO]).hwRate == 0) && (T(sensors[ACCEL]).hwRate == 0)) {
            INFO_PRINT("runMagnSelfTest: executing magnetometer selftest\n");
            T(selftestState) = SELFTEST_INITIALIZATION;
#ifdef LSM6DSM_I2C_MASTER_AK09916
            lsm6dsm_runAbsoluteSelfTestProgram();
#else
            lsm6dsm_runGapSelfTestProgram(MAGN);
#endif
            return true;
        } else
            osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid);
    }

    ERROR_PRINT("runMagnSelfTest: cannot run selftest because sensor is busy!\n");
    lsm6dsm_sendSelfTestResult(SENS_TYPE_MAG, SENSOR_APP_EVT_STATUS_BUSY);

    return false;
}

/*
 * lsm6dsm_magnCfgData: set sw magnetometer calibration values
 * @data: calibration data struct.
 * @cookie: private data.
 */
static bool lsm6dsm_magnCfgData(void *data, void *cookie)
{
    TDECL();
    const struct AppToSensorHalDataPayload *p = data;

    if (p->type == HALINTF_TYPE_MAG_CAL_BIAS && p->size == sizeof(struct MagCalBias)) {
        const struct MagCalBias *d = p->magCalBias;
        INFO_PRINT("lsm6dsm_magnCfgData: calibration %ldnT, %ldnT, %ldnT\n",
                (int32_t)(d->bias[0] * 1000),
                (int32_t)(d->bias[1] * 1000),
                (int32_t)(d->bias[2] * 1000));

        T(magnCal).x_bias = d->bias[0];
        T(magnCal).y_bias = d->bias[1];
        T(magnCal).z_bias = d->bias[2];
    } else if (p->type == HALINTF_TYPE_MAG_LOCAL_FIELD && p->size == sizeof(struct MagLocalField)) {
        const struct MagLocalField *d = p->magLocalField;
        INFO_PRINT("lsm6dsm_magnCfgData: local field strength %dnT, dec %ddeg, inc %ddeg\n",
                (int)(d->strength * 1000),
                (int)(d->declination * 180 / M_PI + 0.5f),
                (int)(d->inclination * 180 / M_PI + 0.5f));

        // Passing local field information to mag calibration routine
        diversityCheckerLocalFieldUpdate(&T(magnCal).diversity_checker, d->strength);

        // TODO: pass local field information to rotation vector sensor.
    } else {
        ERROR_PRINT("lsm6dsm_magnCfgData: unknown type 0x%04x, size %d", p->type, p->size);
    }

    return true;
}
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

/*
 * lsm6dsm_runAccelCalibration: execute accelerometer calibration
 * @cookie: private data.
 */
static bool lsm6dsm_runAccelCalibration(void *cookie)
{
    TDECL();

    if (trySwitchState(SENSOR_CALIBRATION)) {
        if (!T(sensors[ACCEL]).enabled && (T(sensors[ACCEL]).hwRate == 0) && (T(sensors[GYRO]).hwRate == 0)) {
            INFO_PRINT("runAccelCalibration: executing accelerometer calibration\n");
            T(calibrationState) = CALIBRATION_INITIALIZATION;
            lsm6dsm_runCalibrationProgram(ACCEL);
            return true;
        } else
            osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid);
    }

    ERROR_PRINT("runAccelCalibration: cannot run selftest because sensor is busy!\n");
    lsm6dsm_sendCalibrationResult(SENS_TYPE_ACCEL, SENSOR_APP_EVT_STATUS_BUSY, 0, 0, 0);

    return true;
}

/*
 * lsm6dsm_runGyroCalibration: execute gyroscope calibration
 * @cookie: private data.
 */
static bool lsm6dsm_runGyroCalibration(void *cookie)
{
    TDECL();

    if (trySwitchState(SENSOR_CALIBRATION)) {
        if (!T(sensors[GYRO]).enabled && (T(sensors[GYRO]).hwRate == 0) && (T(sensors[ACCEL]).hwRate == 0)) {
            INFO_PRINT("runGyroCalibration: executing gyroscope calibration\n");
            T(calibrationState) = CALIBRATION_INITIALIZATION;
            lsm6dsm_runCalibrationProgram(GYRO);
            return true;
        } else
            osEnqueuePrivateEvt(EVT_SENSOR_RESTORE_IDLE, cookie, NULL, mTask.tid);
    }

    ERROR_PRINT("runGyroCalibration: cannot run selftest because sensor is busy!\n");
    lsm6dsm_sendCalibrationResult(SENS_TYPE_GYRO, SENSOR_APP_EVT_STATUS_BUSY, 0, 0, 0);

    return true;
}

/*
 * lsm6dsm_storeAccelCalibrationData: store hw calibration into sensor
 */
static bool lsm6dsm_storeAccelCalibrationData(void)
{
    TDECL();
    uint8_t buffer[LSM6DSM_TRIAXIAL_NUM_AXIS];

    if (trySwitchState(SENSOR_STORE_CALIBRATION_DATA)) {
        for (uint8_t i = 0; i < LSM6DSM_TRIAXIAL_NUM_AXIS; i++)
            buffer[i] = lsm6dsm_convertAccelOffsetValue(T(accelCalibrationData[i]));

        SPI_MULTIWRITE(LSM6DSM_X_OFS_USR_ADDR, buffer, LSM6DSM_TRIAXIAL_NUM_AXIS);

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, NULL, __FUNCTION__);
    } else
        return false;

    return true;
}

/*
 * lsm6dsm_accelCfgData: set hw and sw accelerometer calibration values
 * @data: calibration data struct.
 * @cookie: private data.
 */
static bool lsm6dsm_accelCfgData(void *data, void *cookie)
{
    TDECL();
    struct LSM6DSMAccelGyroCfgData *cfgData = data;

#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
    accelCalBiasSet(&T(accelCal) , cfgData->sw[0], cfgData->sw[1], cfgData->sw[2]);
#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */

    DEBUG_PRINT("Accel hw bias data [LSB]: %ld %ld %ld\n", cfgData->hw[0], cfgData->hw[1], cfgData->hw[2]);

    memcpy(T(accelCalibrationData), cfgData->hw, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t));

    if (!lsm6dsm_storeAccelCalibrationData())
        T(pendingStoreAccelCalibData) = true;

    return true;
}

/*
 * lsm6dsm_gyroCfgData: set hw and sw gyroscope calibration values
 * @data: calibration data struct.
 * @cookie: private data.
 */
static bool lsm6dsm_gyroCfgData(void *data, void *cookie)
{
    TDECL();
    struct LSM6DSMAccelGyroCfgData *cfgData = data;

#ifdef LSM6DSM_GYRO_CALIB_ENABLED
    const float dummy_temperature_celsius = 25.0f;
    gyroCalSetBias(&T(gyroCal), cfgData->sw[0], cfgData->sw[1], cfgData->sw[2], dummy_temperature_celsius, sensorGetTime());
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */

    DEBUG_PRINT("Gyro hw bias data [LSB]: %ld %ld %ld\n", cfgData->hw[0], cfgData->hw[1], cfgData->hw[2]);

    memcpy(T(gyroCalibrationData), cfgData->hw, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t));

    return true;
}

/*
 * lsm6dsm_sensorInit: initial sensors configuration
 */
static void lsm6dsm_sensorInit(void)
{
    TDECL();
    uint8_t buffer[5];

    switch (T(initState)) {
    case RESET_LSM6DSM:
        INFO_PRINT("Performing soft-reset\n");

        T(initState) = INIT_LSM6DSM;

        /* Sensor SW-reset */
        SPI_WRITE(LSM6DSM_CTRL3_C_ADDR, LSM6DSM_SW_RESET, 20000);

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
        break;

    case INIT_LSM6DSM:
        INFO_PRINT("Initial registers configuration\n");

        /* During init, reset all configurable registers to default values */
        SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);
        SPI_WRITE(LSM6DSM_DRDY_PULSE_CFG_ADDR, LSM6DSM_DRDY_PULSE_CFG_BASE);

        buffer[0] = LSM6DSM_CTRL1_XL_BASE;                           /* LSM6DSM_CTRL1_XL */
        buffer[1] = LSM6DSM_CTRL2_G_BASE;                            /* LSM6DSM_CTRL2_G */
        buffer[2] = LSM6DSM_CTRL3_C_BASE;                            /* LSM6DSM_CTRL3_C */
        buffer[3] = LSM6DSM_CTRL4_C_BASE;                            /* LSM6DSM_CTRL4_C */
        buffer[4] = LSM6DSM_CTRL5_C_BASE;                            /* LSM6DSM_CTRL4_C */
        SPI_MULTIWRITE(LSM6DSM_CTRL1_XL_ADDR, buffer, 5);

        buffer[0] = LSM6DSM_CTRL10_C_BASE | LSM6DSM_RESET_PEDOMETER; /* LSM6DSM_CTRL10_C */
        buffer[1] = LSM6DSM_MASTER_CONFIG_BASE;                      /* LSM6DSM_MASTER_CONFIG */
        SPI_MULTIWRITE(LSM6DSM_CTRL10_C_ADDR, buffer, 2);

        SPI_WRITE(LSM6DSM_INT1_CTRL_ADDR, LSM6DSM_INT1_CTRL_BASE);
        SPI_WRITE(LSM6DSM_WAKE_UP_DUR_ADDR, LSM6DSM_WAKE_UP_DUR_BASE);

#ifdef LSM6DSM_I2C_MASTER_ENABLED
        T(initState) = INIT_I2C_MASTER_REGS_CONF;
#else /* LSM6DSM_I2C_MASTER_ENABLED */
        INFO_PRINT("Initialization completed successfully!\n");
        T(initState) = INIT_DONE;
#endif /* LSM6DSM_I2C_MASTER_ENABLED */

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
        break;

#ifdef LSM6DSM_I2C_MASTER_ENABLED
    case INIT_I2C_MASTER_REGS_CONF:
        INFO_PRINT("Initial I2C master registers configuration\n");

        /* Enable access for embedded registers */
        SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE | LSM6DSM_ENABLE_FUNC_CFG_ACCESS, 50);

        /* I2C-0 configuration */
        buffer[0] = LSM6DSM_EMBEDDED_SLV0_WRITE_ADDR_SLEEP;                                               /* LSM6DSM_EMBEDDED_SLV0_ADDR */
        buffer[1] = 0x00;                                                                                 /* LSM6DSM_EMBEDDED_SLV0_SUBADDR */
        buffer[2] = LSM6DSM_EMBEDDED_SENSOR_HUB_NUM_SLAVE;                                                /* LSM6DSM_EMBEDDED_SLV0_CONFIG */
        SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV0_ADDR_ADDR, buffer, 3);

#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)     /* Magn & Baro both enabled */
        /* I2C-1 configuration */
        buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV1_ADDR */
        buffer[1] = LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR;                                               /* LSM6DSM_EMBEDDED_SLV1_SUBADDR */
        buffer[2] = LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE | LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN;      /* LSM6DSM_EMBEDDED_SLV1_CONFIG */
        SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR, buffer, 3);

        /* I2C-2 configuration */
        buffer[0] = (LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV2_ADDR */
        buffer[1] = LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_ADDR;                                               /* LSM6DSM_EMBEDDED_SLV2_SUBADDR */
        buffer[2] = LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN;                                                /* LSM6DSM_EMBEDDED_SLV2_CONFIG */
        SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR, buffer, 3);

#ifdef LSM6DSM_I2C_MASTER_AK09916
        /* I2C-3 configuration */
        buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV3_ADDR */
        buffer[1] = AK09916_STATUS_DATA_ADDR;                                                             /* LSM6DSM_EMBEDDED_SLV3_SUBADDR */
        buffer[2] = 1;                                                                                    /* LSM6DSM_EMBEDDED_SLV3_CONFIG */
        SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV3_ADDR_ADDR, buffer, 3);
#endif /* LSM6DSM_I2C_MASTER_AK09916 */
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */

#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)    /* Magn only enabled */
        /* I2C-1 configuration */
        buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV1_ADDR */
        buffer[1] = LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_ADDR;                                               /* LSM6DSM_EMBEDDED_SLV1_SUBADDR */
        buffer[2] = LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE | LSM6DSM_SENSOR_SLAVE_MAGN_OUTDATA_LEN;      /* LSM6DSM_EMBEDDED_SLV1_CONFIG */
        SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR, buffer, 3);

#ifdef LSM6DSM_I2C_MASTER_AK09916
        /* I2C-2 configuration */
        buffer[0] = (LSM6DSM_SENSOR_SLAVE_MAGN_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV2_ADDR */
        buffer[1] = AK09916_STATUS_DATA_ADDR;                                                             /* LSM6DSM_EMBEDDED_SLV2_SUBADDR */
        buffer[2] = 0x01;                                                                                 /* LSM6DSM_EMBEDDED_SLV2_CONFIG */
        SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV2_ADDR_ADDR, buffer, 3);
#endif /* LSM6DSM_I2C_MASTER_AK09916 */
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */

#if !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)    /* Baro only enabled */
        /* I2C-1 configuration */
        buffer[0] = (LSM6DSM_SENSOR_SLAVE_BARO_I2C_ADDR_8BIT << 1) | LSM6DSM_EMBEDDED_READ_OP_SENSOR_HUB; /* LSM6DSM_EMBEDDED_SLV1_ADDR */
        buffer[1] = LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_ADDR;                                               /* LSM6DSM_EMBEDDED_SLV1_SUBADDR */
        buffer[2] = LSM6DSM_EMBEDDED_SLV1_CONFIG_WRITE_ONCE | LSM6DSM_SENSOR_SLAVE_BARO_OUTDATA_LEN;      /* LSM6DSM_EMBEDDED_SLV1_CONFIG */
        SPI_MULTIWRITE(LSM6DSM_EMBEDDED_SLV1_ADDR_ADDR, buffer, 3);
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) */

        /* Disable access for embedded registers */
        SPI_WRITE(LSM6DSM_FUNC_CFG_ACCESS_ADDR, LSM6DSM_FUNC_CFG_ACCESS_BASE, 50);

        T(initState) = INIT_I2C_MASTER_SENSOR_RESET;

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
        break;

    case INIT_I2C_MASTER_SENSOR_RESET:
        INFO_PRINT("Performing soft-reset slave sensors\n");
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
        T(initState) = INIT_I2C_MASTER_MAGN_SENSOR;
#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
        T(initState) = INIT_I2C_MASTER_BARO_SENSOR;
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

        /* Enable accelerometer and sensor-hub to initialize slave sensor */
        SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE | LSM6DSM_ODR_104HZ_REG_VALUE);
        T(masterConfigRegister) |= LSM6DSM_MASTER_CONFIG_MASTER_ON;

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_MAGN_RESET_ADDR, LSM6DSM_SENSOR_SLAVE_MAGN_RESET_VALUE, SENSOR_HZ(104.0f), MAGN, 20000);
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM6DSM_SENSOR_SLAVE_BARO_RESET_ADDR, LSM6DSM_SENSOR_SLAVE_BARO_RESET_VALUE, SENSOR_HZ(104.0f), PRESS, 20000);
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
        break;

    case INIT_I2C_MASTER_MAGN_SENSOR:
        INFO_PRINT("Initial slave magnetometer sensor registers configuration\n");
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
        T(initState) = INIT_I2C_MASTER_BARO_SENSOR;
#else /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
        T(initState) = INIT_I2C_MASTER_SENSOR_END;
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

#ifdef LSM6DSM_I2C_MASTER_LIS3MDL
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL1_ADDR, LIS3MDL_CTRL1_BASE, SENSOR_HZ(104.0f), MAGN);
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL2_ADDR, LIS3MDL_CTRL2_BASE, SENSOR_HZ(104.0f), MAGN);
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL3_ADDR, LIS3MDL_CTRL3_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE, SENSOR_HZ(104.0f), MAGN);
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL4_ADDR, LIS3MDL_CTRL4_BASE, SENSOR_HZ(104.0f), MAGN);
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LIS3MDL_CTRL5_ADDR, LIS3MDL_CTRL5_BASE, SENSOR_HZ(104.0f), MAGN);
#endif /* LSM6DSM_I2C_MASTER_LIS3MDL */

#ifdef LSM6DSM_I2C_MASTER_LSM303AGR
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_A_M_ADDR, LSM303AGR_CFG_REG_A_M_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE, SENSOR_HZ(104.0f), MAGN);
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LSM303AGR_CFG_REG_C_M_ADDR, LSM303AGR_CFG_REG_C_M_BASE, SENSOR_HZ(104.0f), MAGN);
#endif /* LSM6DSM_I2C_MASTER_LSM303AGR */

#ifdef LSM6DSM_I2C_MASTER_AK09916
        SPI_WRITE_SLAVE_SENSOR_REGISTER(AK09916_CNTL2_ADDR, AK09916_CNTL2_BASE | LSM6DSM_SENSOR_SLAVE_MAGN_POWER_OFF_VALUE, SENSOR_HZ(104.0f), MAGN);
#endif /* LSM6DSM_I2C_MASTER_AK09916 */

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
        break;

    case INIT_I2C_MASTER_BARO_SENSOR:
        INFO_PRINT("Initial slave barometer sensor registers configuration\n");
        T(initState) = INIT_I2C_MASTER_SENSOR_END;

#ifdef LSM6DSM_I2C_MASTER_LPS22HB
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LPS22HB_CTRL1_ADDR, LPS22HB_CTRL1_BASE | LSM6DSM_SENSOR_SLAVE_BARO_POWER_OFF_VALUE, SENSOR_HZ(104.0f), PRESS);
        SPI_WRITE_SLAVE_SENSOR_REGISTER(LPS22HB_CTRL2_ADDR, LPS22HB_CTRL2_BASE, SENSOR_HZ(104.0f), PRESS);
#endif /* LSM6DSM_I2C_MASTER_LPS22HB */

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
        break;

    case INIT_I2C_MASTER_SENSOR_END:
        INFO_PRINT("Initialization completed successfully!\n");
        T(initState) = INIT_DONE;

        /* Disable accelerometer and sensor-hub */
        SPI_WRITE(LSM6DSM_MASTER_CONFIG_ADDR, LSM6DSM_MASTER_CONFIG_BASE);
        SPI_WRITE(LSM6DSM_CTRL1_XL_ADDR, LSM6DSM_CTRL1_XL_BASE);
        T(masterConfigRegister) &= ~LSM6DSM_MASTER_CONFIG_MASTER_ON;

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
        break;
#endif /* LSM6DSM_I2C_MASTER_ENABLED */

    default:
        break;
    }
}

/*
 * lsm6dsm_processPendingEvt: process pending events
 */
static void lsm6dsm_processPendingEvt(void)
{
    TDECL();
    enum SensorIndex i;

    SET_STATE(SENSOR_IDLE);

    for (i = 0; i < NUM_SENSORS; i++) {
        if (T(pendingEnableConfig[i])) {
            T(pendingEnableConfig[i]) = false;
            LSM6DSMSensorOps[i].sensorPower(T(sensors[i]).pConfig.enable, (void *)i);
            return;
        }

        if (T(pendingRateConfig[i])) {
            T(pendingRateConfig[i]) = false;
            LSM6DSMSensorOps[i].sensorSetRate(T(sensors[i]).pConfig.rate, T(sensors[i]).pConfig.latency, (void *)i);
            return;
        }

        if (T(pendingFlush[i]) > 0) {
            T(pendingFlush[i])--;
            LSM6DSMSensorOps[i].sensorFlush((void *)i);
            return;
        }
    }

    if (T(pendingTimeSyncTask)) {
        T(pendingTimeSyncTask) = false;
        lsm6dsm_timeSyncTask();
        return;
    }

#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
    if (T(pendingBaroTimerTask)) {
        T(pendingBaroTimerTask) = false;
        lsm6dsm_baroTimerTask();
        return;
    }
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

    if (T(pendingStoreAccelCalibData)) {
        T(pendingStoreAccelCalibData) = lsm6dsm_storeAccelCalibrationData();
        return;
    }

    if (T(pendingInt)) {
        T(pendingInt) = false;
        lsm6dsm_readStatusReg(false);
        return;
    }

    if (gpioGet(T(int1)))
        lsm6dsm_readStatusReg(false);
}

/*
 * lsm6dsm_allocateThreeAxisDataEvt: allocate slab for three axis sensor data
 * @mSensor: sensor info.
 * @rtcTime: time of first sample in this block.
 */
static bool lsm6dsm_allocateThreeAxisDataEvt(struct LSM6DSMSensor *mSensor, uint64_t rtcTime)
{
    TDECL();

    mSensor->tADataEvt = slabAllocatorAlloc(T(mDataSlabThreeAxis));
    if (!mSensor->tADataEvt) {
        ERROR_PRINT("Failed to allocate memory!\n");
        return false;
    }

    memset(&mSensor->tADataEvt->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
    mSensor->tADataEvt->referenceTime = rtcTime;
    mSensor->pushedTimestamp = rtcTime;

    return true;
}

/*
 * lsm6dsm_threeAxisDataEvtFree: deallocate slab of three axis sensor.
 * @ptr: sensor data pointer.
 */
static void lsm6dsm_threeAxisDataEvtFree(void *ptr)
{
    TDECL();

    slabAllocatorFree(T(mDataSlabThreeAxis), (struct TripleAxisDataEvent *)ptr);
}

#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
/*
 * lsm6dsm_allocateOneAxisDataEvt: allocate slab for one axis sensor data
 * @mSensor: sensor info.
 * @rtcTime: time of first sample in this block.
 */
static bool lsm6dsm_allocateOneAxisDataEvt(struct LSM6DSMSensor *mSensor, uint64_t rtcTime)
{
    TDECL();

    mSensor->sADataEvt = slabAllocatorAlloc(T(mDataSlabOneAxis));
    if (!mSensor->sADataEvt) {
        ERROR_PRINT("Failed to allocate memory!\n");
        return false;
    }

    memset(&mSensor->sADataEvt->samples[0].firstSample, 0, sizeof(struct SensorFirstSample));
    mSensor->sADataEvt->referenceTime = rtcTime;
    mSensor->pushedTimestamp = rtcTime;

    return true;
}

/*
 * lsm6dsm_oneAxisDataEvtFree: deallocate slab of one axis sensor
 * @ptr: sensor data pointer.
 */
static void lsm6dsm_oneAxisDataEvtFree(void *ptr)
{
    TDECL();

    slabAllocatorFree(T(mDataSlabOneAxis), (struct SingleAxisDataEvent *)ptr);
}
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

/*
 * lsm6dsm_processSensorThreeAxisData: process three axis sensors data
 * @mSensor: sensor info.
 * @data: sensor data.
 * @sampleNum: number of samples in the current slab.
 * @timestamp: current sample timestamp;
 */
static bool lsm6dsm_processSensorThreeAxisData(struct LSM6DSMSensor *mSensor, uint8_t *data, uint16_t *sampleNum, uint64_t *timestamp)
{
    TDECL();
    int16_t x, y, z;
    float x_remap, y_remap, z_remap;
    struct TripleAxisDataPoint *samples;

    if (*timestamp == 0)
        return false;

    if (mSensor->tADataEvt == NULL) {
        if (!lsm6dsm_allocateThreeAxisDataEvt(mSensor, *timestamp))
            return false;
    }
    samples = mSensor->tADataEvt->samples;

    x = (int16_t)(data[1] << 8) | data[0];
    y = (int16_t)(data[3] << 8) | data[2];
    z = (int16_t)(data[5] << 8) | data[4];

    switch (mSensor->idx) {
    case ACCEL:
        x_remap = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_ACCEL_KSCALE;
        y_remap = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_ACCEL_KSCALE;
        z_remap = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_ACCEL_KSCALE;

#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
        accelCalRun(&T(accelCal), *timestamp, x_remap, y_remap, z_remap, T(currentTemperature));
        accelCalBiasRemove(&T(accelCal), &x_remap, &y_remap, &z_remap);

        if (accelCalUpdateBias(&T(accelCal), &samples[*sampleNum].x, &samples[*sampleNum].y, &samples[*sampleNum].z)) {
            if (!samples->firstSample.biasCurrent) {
                samples->firstSample.biasCurrent = true;
                samples->firstSample.biasPresent = 1;
                samples->firstSample.biasSample = *sampleNum;

                if (*sampleNum > 0)
                    samples[*sampleNum].deltaTime = 0;

                *sampleNum += 1;
            }
        }
#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */

#ifdef LSM6DSM_GYRO_CALIB_ENABLED
        if (T(sensors[GYRO].enabled))
            gyroCalUpdateAccel(&T(gyroCal), *timestamp, x_remap, y_remap, z_remap);
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */

        break;

    case GYRO:
        x -= (int16_t)T(gyroCalibrationData)[0];
        y -= (int16_t)T(gyroCalibrationData)[1];
        z -= (int16_t)T(gyroCalibrationData)[2];

        x_remap = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_GYRO_KSCALE;
        y_remap = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_GYRO_KSCALE;
        z_remap = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_ACCEL_GYRO_ROT_MATRIX) * LSM6DSM_GYRO_KSCALE;

#ifdef LSM6DSM_GYRO_CALIB_ENABLED
        gyroCalUpdateGyro(&T(gyroCal), *timestamp, x_remap, y_remap, z_remap, T(currentTemperature));

#ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED
        overTempCalSetTemperature(&T(overTempCal), *timestamp, T(currentTemperature));
#else /* LSM6DSM_OVERTEMP_CALIB_ENABLED */
        gyroCalRemoveBias(&T(gyroCal), x_remap, y_remap, z_remap, &x_remap, &y_remap, &z_remap);
#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */

        if (gyroCalNewBiasAvailable(&T(gyroCal))) {
            float biasTemperature, gyroOffset[3] = { 0.0f, 0.0f, 0.0f };
            uint64_t calTime;

            gyroCalGetBias(&T(gyroCal), &gyroOffset[0], &gyroOffset[1], &gyroOffset[2], &biasTemperature, &calTime);

            if (!samples->firstSample.biasCurrent) {
                samples->firstSample.biasCurrent = true;
                samples->firstSample.biasPresent = 1;
                samples->firstSample.biasSample = *sampleNum;

                if (*sampleNum > 0)
                    samples[*sampleNum].deltaTime = 0;

                samples[*sampleNum].x = gyroOffset[0];
                samples[*sampleNum].y = gyroOffset[1];
                samples[*sampleNum].z = gyroOffset[2];

                *sampleNum += 1;
            }

#ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED
            overTempCalUpdateSensorEstimate(&T(overTempCal), *timestamp, gyroOffset, biasTemperature);
            overTempCalRemoveOffset(&T(overTempCal), *timestamp, x_remap, y_remap, z_remap, &x_remap, &y_remap, &z_remap);
#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */
        } else {
#ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED
            overTempCalRemoveOffset(&T(overTempCal), *timestamp, x_remap, y_remap, z_remap, &x_remap, &y_remap, &z_remap);
#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */
        }
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */
        break;

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
    case MAGN: ;
#ifdef LSM6DSM_MAGN_CALIB_ENABLED
        bool newMagnCalibData;
        float magnOffX, magnOffY, magnOffZ;
#endif /* LSM6DSM_MAGN_CALIB_ENABLED */

        x_remap = LSM6DSM_REMAP_X_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX) * LSM6DSM_MAGN_KSCALE;
        y_remap = LSM6DSM_REMAP_Y_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX) * LSM6DSM_MAGN_KSCALE;
        z_remap = LSM6DSM_REMAP_Z_DATA(x, y, z, LSM6DSM_MAGN_ROT_MATRIX) * LSM6DSM_MAGN_KSCALE;

#ifdef LSM6DSM_MAGN_CALIB_ENABLED
        magCalRemoveSoftiron(&T(magnCal), x_remap, y_remap, z_remap, &magnOffX, &magnOffY, &magnOffZ);
        newMagnCalibData = magCalUpdate(&T(magnCal), NS_TO_US(*timestamp), magnOffX, magnOffY, magnOffZ);
        magCalRemoveBias(&T(magnCal), magnOffX, magnOffY, magnOffZ, &x_remap, &y_remap, &z_remap);

        if (newMagnCalibData && !samples->firstSample.biasCurrent) {
            samples->firstSample.biasCurrent = true;
            samples->firstSample.biasPresent = 1;
            samples->firstSample.biasSample = *sampleNum;

            if (*sampleNum > 0)
                samples[*sampleNum].deltaTime = 0;

            magCalGetBias(&T(magnCal), &samples[*sampleNum].x, &samples[*sampleNum].y, &samples[*sampleNum].z);

            *sampleNum += 1;
        }
#endif /* LSM6DSM_MAGN_CALIB_ENABLED */

        break;
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

    default:
        return false;
    }

    if (++mSensor->samplesDecimatorCounter >= mSensor->samplesDecimator) {
        samples[*sampleNum].x = x_remap;
        samples[*sampleNum].y = y_remap;
        samples[*sampleNum].z = z_remap;

        if (*sampleNum > 0) {
            samples[*sampleNum].deltaTime = *timestamp - mSensor->pushedTimestamp;
            mSensor->pushedTimestamp = *timestamp;
        }

        *sampleNum += 1;

        mSensor->samplesDecimatorCounter = 0;
    }

    return true;
}

#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
/*
 * lsm6dsm_processSensorOneAxisData: process single axis sensors data
 * @mSensor: sensor info.
 * @data: sensor data.
 * @sampleNum: number of samples in the current slab.
 * @timestamp: current sample timestamp;
 */
static bool lsm6dsm_processSensorOneAxisData(struct LSM6DSMSensor *mSensor, uint8_t *data, uint16_t *sampleNum, uint64_t *timestamp)
{
    TDECL();

    if (*timestamp == 0)
        return false;

    if (++mSensor->samplesDecimatorCounter >= mSensor->samplesDecimator) {
        if (mSensor->sADataEvt == NULL) {
            if (!lsm6dsm_allocateOneAxisDataEvt(mSensor, *timestamp))
                return false;
        }

        switch (mSensor->idx) {
        case PRESS:
            mSensor->sADataEvt->samples[*sampleNum].fdata = ((data[2] << 16) | (data[1] << 8) | data[0]) * LSM6DSM_PRESS_KSCALE;
            break;
        default:
            return false;
        }

        if (*sampleNum > 0) {
            mSensor->sADataEvt->samples[*sampleNum].deltaTime = *timestamp - mSensor->pushedTimestamp;
            mSensor->pushedTimestamp = *timestamp;
        }

        *sampleNum += 1;

        mSensor->samplesDecimatorCounter = 0;
    }

    return true;
}
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

/*
 * lsm6dsm_pushData: push slab to nanohub
 * @sidx: sensor index.
 * @numSamples: number of samples in the slab.
 */
static void lsm6dsm_pushData(enum SensorIndex sidx, uint16_t *numSamples)
{
    TDECL();
    bool triaxial = true;

#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
    if (sidx == PRESS)
        triaxial = false;
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

    if (triaxial) {
        T(sensors[sidx]).tADataEvt->samples[0].firstSample.numSamples = *numSamples;
        osEnqueueEvtOrFree(sensorGetMyEventType(LSM6DSMSensorInfo[sidx].sensorType), T(sensors[sidx]).tADataEvt, lsm6dsm_threeAxisDataEvtFree);
        T(sensors[sidx]).tADataEvt = NULL;
    } else {
#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
        T(sensors[sidx]).sADataEvt->samples[0].firstSample.numSamples = *numSamples;
        osEnqueueEvtOrFree(sensorGetMyEventType(LSM6DSMSensorInfo[sidx].sensorType), T(sensors[sidx]).sADataEvt, lsm6dsm_oneAxisDataEvtFree);
        T(sensors[sidx]).sADataEvt = NULL;
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    }

    *numSamples = 0;
}

/*
 * lsm6dsm_parseFifoData: processing FIFO data.
 * @data: FIFO data.
 * @numPattern: number of pattern inside data.
 */
static void lsm6dsm_parseFifoData(uint8_t *data, uint16_t numPattern)
{
    TDECL();
    uint16_t j, fifoCounter = 0, samplesCounter[FIFO_NUM] = { 0 };
    struct LSM6DSMSensor *sensor;
    uint32_t sampleTimestamp;
    int32_t timestampDiffLSB;
    uint64_t timestamp = 0;
    enum SensorIndex sidx;
    uint8_t i, n;

    for (j = 0; j < numPattern; j++) {
        for (i = 0; i < T(fifoCntl).maxMinDecimator; i++) {
            sampleTimestamp = ((data[fifoCounter + T(fifoCntl).timestampPosition[i] + 1] << 16) |
                            (data[fifoCounter + T(fifoCntl).timestampPosition[i]] << 8) |
                            data[fifoCounter + T(fifoCntl).timestampPosition[i] + 3]);

            if (T(time).sampleTimestampFromFifoLSB > 0) {
                timestampDiffLSB = (int32_t)sampleTimestamp - (int32_t)(T(time).sampleTimestampFromFifoLSB & LSM6DSM_MASK_24BIT_TIMESTAMP);

                if ((timestampDiffLSB < 0) || (timestampDiffLSB > (T(time).theoreticalDeltaTimeLSB + T(time).deltaTimeMarginLSB))) {
                    if (timestampDiffLSB < -LSM6DSM_TIMEDIFF_OVERFLOW_LSB) {
                        T(time).sampleTimestampFromFifoLSB += (UINT32_MAX >> 8) + 1;
                    } else {
                        if (T(time).timestampIsValid)
                            sampleTimestamp = (T(time).sampleTimestampFromFifoLSB & LSM6DSM_MASK_24BIT_TIMESTAMP) + T(time).theoreticalDeltaTimeLSB;
                        else
                            sampleTimestamp = 0;
                    }
                } else
                    T(time).timestampIsValid = true;
            }

            T(time).sampleTimestampFromFifoLSB = (T(time).sampleTimestampFromFifoLSB & ~LSM6DSM_MASK_24BIT_TIMESTAMP) + sampleTimestamp;

            if (T(time).timestampIsValid) {
                if (!time_sync_estimate_time1(&T(time).sensorTimeToRtcData, (uint64_t)T(time).sampleTimestampFromFifoLSB * LSM6DSM_TIME_RESOLUTION, &timestamp)) {
                    timestamp = 0;
                } else {
                    if (T(time).lastSampleTimestamp > 0) {
                        if ((int64_t)timestamp <= (int64_t)T(time).lastSampleTimestamp)
                            timestamp = 0;
                    }

                    T(time).lastSampleTimestamp = timestamp > 0 ? timestamp : T(time).lastSampleTimestamp;

                }
            }

            for (n = 0; n < FIFO_NUM; n++) {
                if ((T(fifoCntl).decimators[n] > 0) && ((i % (T(fifoCntl).decimators[n] / T(fifoCntl).minDecimator)) == 0)) {
                    sidx = T(fifoCntl).decimatorsIdx[n];
                    if (sidx != EMBEDDED_TIMESTAMP) {
                        sensor = &T(sensors[sidx]);

                        if (sensor->samplesToDiscard == 0) {
                            if (++sensor->samplesFifoDecimatorCounter >= sensor->samplesFifoDecimator) {
                                switch (sidx) {
                                case GYRO:
                                case ACCEL:
#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
                                case MAGN:
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
                                    lsm6dsm_processSensorThreeAxisData(sensor, &data[fifoCounter], &samplesCounter[n], &timestamp);
                                    break;

#if defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED) && !defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED)
                                case PRESS:
                                    if (T(sensors[PRESS]).enabled)
                                        lsm6dsm_processSensorOneAxisData(sensor, &data[fifoCounter], &samplesCounter[n], &timestamp);

                                    if (T(sensors[TEMP]).enabled) {
                                        union EmbeddedDataPoint tempData;

                                        tempData.fdata = ((int16_t)(data[fifoCounter + LSM6DSM_PRESS_OUTDATA_LEN + 1] << 8) |
                                                    data[fifoCounter + LSM6DSM_PRESS_OUTDATA_LEN]) * LSM6DSM_TEMP_KSCALE;

                                        osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), tempData.vptr, NULL);
                                    }

                                    break;
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED, LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

                                default:
                                    break;
                                }

                                sensor->samplesFifoDecimatorCounter = 0;

                                if (samplesCounter[n] >= (LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE - 1))
                                    lsm6dsm_pushData(sidx, &samplesCounter[n]);
                            }
                        } else
                            sensor->samplesToDiscard--;
                    } else {
                        if (T(sensors[STEP_COUNTER].enabled) && !T(readSteps)) {
                            uint16_t steps = data[fifoCounter + 4] | (data[fifoCounter + 5] << 8);

                            if (steps != T(totalNumSteps)) {
                                union EmbeddedDataPoint stepCntData;

                                stepCntData.idata = steps;
                                osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), stepCntData.vptr, NULL);
                                DEBUG_PRINT("Step Counter update: %ld steps\n", stepCntData.idata);
                                T(totalNumSteps) = stepCntData.idata;
                            }
                        }
                    }

                    fifoCounter += LSM6DSM_ONE_SAMPLE_BYTE;
                }
            }
        }
    }

    for (n = 0; n < FIFO_NUM; n++) {
        if (samplesCounter[n])
            lsm6dsm_pushData(T(fifoCntl).decimatorsIdx[n], &samplesCounter[n]);
    }
}

/*
 * lsm6dsm_updateSyncTaskValues: read timestamp used for time calibration and temperature
 */
static inline void lsm6dsm_updateSyncTaskValues(void)
{
    TDECL();
    uint32_t sensorTimestamp;

    sensorTimestamp = ((T_SLAVE_INTERFACE(timestampDataBuffer[1]) << 0) |
                    (T_SLAVE_INTERFACE(timestampDataBuffer[2]) << 8) |
                    (T_SLAVE_INTERFACE(timestampDataBuffer[3]) << 16));

    if (T(time).timestampSyncTaskLSB > 0) {
        if (((int32_t)sensorTimestamp - (int32_t)(T(time).timestampSyncTaskLSB & LSM6DSM_MASK_24BIT_TIMESTAMP)) < -LSM6DSM_TIMEDIFF_OVERFLOW_LSB)
            T(time).timestampSyncTaskLSB += (UINT32_MAX >> 8) + 1;
    }

    T(time).timestampSyncTaskLSB = (T(time).timestampSyncTaskLSB & ~LSM6DSM_MASK_24BIT_TIMESTAMP) + sensorTimestamp;

    time_sync_add(&T(time).sensorTimeToRtcData, T(time).timeSyncRtcTime, (uint64_t)T(time).timestampSyncTaskLSB * LSM6DSM_TIME_RESOLUTION);

#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
    T(currentTemperature) = LSM6DSM_TEMP_OFFSET +
            (float)((int16_t)((T_SLAVE_INTERFACE(tempDataBuffer[2]) << 8) | T_SLAVE_INTERFACE(tempDataBuffer[1]))) / 256.0f;
#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
}

/*
 * lsm6dsm_handleSpiDoneEvt: all SPI operation fall back here
 * @evtData: event data.
 */
static void lsm6dsm_handleSpiDoneEvt(const void *evtData)
{
    TDECL();
    bool returnIdle = false, resetFIFO = false;
    struct LSM6DSMSensor *mSensor;
    int i;

    switch (GET_STATE()) {
    case SENSOR_BOOT:
        SET_STATE(SENSOR_VERIFY_WAI);

        SPI_READ(LSM6DSM_WAI_ADDR, 1, &T_SLAVE_INTERFACE(tmpDataBuffer));
        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
        break;

    case SENSOR_VERIFY_WAI:
        if (T_SLAVE_INTERFACE(tmpDataBuffer[1]) != LSM6DSM_WAI_VALUE) {
            T(mRetryLeft)--;
            if (T(mRetryLeft) == 0)
                break;

            ERROR_PRINT("`Who-Am-I` register value not valid: %x\n", T_SLAVE_INTERFACE(tmpDataBuffer[1]));
            SET_STATE(SENSOR_BOOT);
            timTimerSet(100000000, 100, 100, lsm6dsm_timerCallback, NULL, true);
        } else {
            SET_STATE(SENSOR_INITIALIZATION);
            T(initState) = RESET_LSM6DSM;
            lsm6dsm_sensorInit();
        }

        break;

    case SENSOR_INITIALIZATION:
        if (T(initState) == INIT_DONE) {
            for (i = 0; i < NUM_SENSORS; i++) {
                sensorRegisterInitComplete(T(sensors[i]).handle);
            }

                returnIdle = true;
            } else
                lsm6dsm_sensorInit();

        break;

    case SENSOR_POWERING_UP:
        mSensor = (struct LSM6DSMSensor *)evtData;

        mSensor->enabled = true;
        sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0);
        returnIdle = true;
        break;

    case SENSOR_POWERING_DOWN:
        mSensor = (struct LSM6DSMSensor *)evtData;

        mSensor->enabled = false;
        sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0);
        returnIdle = true;
        break;

    case SENSOR_CONFIG_CHANGING:
        mSensor = (struct LSM6DSMSensor *)evtData;

        sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate[mSensor->idx], mSensor->latency);
        returnIdle = true;
        break;

    case SENSOR_CONFIG_WATERMARK_CHANGING:
        returnIdle = true;
        break;

    case SENSOR_CALIBRATION:
        mSensor = (struct LSM6DSMSensor *)evtData;

        if (T(calibrationState == CALIBRATION_COMPLETED)) {
            returnIdle = true;
        } else {
            lsm6dsm_runCalibrationProgram(mSensor->idx);
        }
        break;

    case SENSOR_STORE_CALIBRATION_DATA:
        returnIdle = true;
        break;

    case SENSOR_SELFTEST:
        mSensor = (struct LSM6DSMSensor *)evtData;

        if (T(selftestState == SELFTEST_COMPLETED)) {
            returnIdle = true;
        } else {
#ifdef LSM6DSM_I2C_MASTER_AK09916
            if (mSensor->idx == MAGN) {
                lsm6dsm_runAbsoluteSelfTestProgram();
            } else {
                lsm6dsm_runGapSelfTestProgram(mSensor->idx);
            }
#else /* LSM6DSM_I2C_MASTER_AK09916 */
            lsm6dsm_runGapSelfTestProgram(mSensor->idx);
#endif /* LSM6DSM_I2C_MASTER_AK09916 */
        }

        break;

    case SENSOR_INT1_STATUS_REG_HANDLING:
        if (T(sensors[STEP_DETECTOR].enabled) && (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_STEP_DETECTED)) {
            osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_DETECT), NULL, NULL);
            DEBUG_PRINT("Step Detected!\n");
        }

        if (T(sensors[SIGN_MOTION].enabled) && (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_SIGN_MOTION)) {
            osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_SIG_MOTION), NULL, NULL);
            DEBUG_PRINT("Significant Motion event!\n");
        }

        if ((T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_STATUS2_FIFO_ERROR) == 0) {
            T(fifoDataToRead) = (((T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_CTRL2_FTH_MASK) << 8) | T_SLAVE_INTERFACE(fifoStatusRegBuffer[1])) * 2;

            if (T(fifoDataToRead) > LSM6DSM_SPI_FIFO_SIZE) {
                T(fifoDataToReadPending) = T(fifoDataToRead);
                T(fifoDataToRead) = LSM6DSM_SPI_FIFO_SIZE - (LSM6DSM_SPI_FIFO_SIZE % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE));
                T(fifoDataToReadPending) -= T(fifoDataToRead);
            } else {
                T(fifoDataToReadPending) = 0;

                if (T(fifoDataToRead) >= (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE))
                    T(fifoDataToRead) -= T(fifoDataToRead) % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE);
                else
                    T(fifoDataToRead) = 0;
            }

            if (T(fifoDataToRead) > 0) {
                if (T(time).status == TIME_SYNC_DURING_FIFO_READ) {
                    uint64_t time = sensorGetTime();
                    if ((time - T(time).noTimer.lastTimestampDataAvlRtcTime) > LSM6DSM_SYNC_DELTA_INTERVAL) {
                        T(time).noTimer.newTimestampDataAvl = true;
                        T(time).noTimer.lastTimestampDataAvlRtcTime = time;

                        SPI_READ(LSM6DSM_TIMESTAMP0_REG_ADDR, LSM6DSM_TIMESTAMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(timestampDataBuffer));
#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
                        SPI_READ(LSM6DSM_OUT_TEMP_L_ADDR, LSM6DSM_TEMP_SAMPLE_BYTE, &T_SLAVE_INTERFACE(tempDataBuffer));
#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
                    }
                }

                SPI_READ(LSM6DSM_FIFO_DATA_OUT_L_ADDR, T(fifoDataToRead), &T_SLAVE_INTERFACE(fifoDataBuffer));
            }
        } else {
            T(fifoDataToRead) = 0;

            if ((T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_STATUS2_FIFO_FULL_SMART) ||
                                    (T_SLAVE_INTERFACE(fifoStatusRegBuffer[2]) & LSM6DSM_FIFO_STATUS2_FIFO_FULL_OVERRUN)) {
                resetFIFO = true;
                SPI_WRITE(LSM6DSM_FIFO_CTRL5_ADDR, LSM6DSM_FIFO_BYPASS_MODE, 25);
                SPI_WRITE(LSM6DSM_FIFO_CTRL5_ADDR, LSM6DSM_FIFO_CONTINUOS_MODE);
            }

            if (T(sensors[STEP_COUNTER].enabled) && (T_SLAVE_INTERFACE(funcSrcBuffer[1]) & LSM6DSM_FUNC_SRC_STEP_COUNT_DELTA_IA)) {
                T(readSteps) = true;
                SPI_READ(LSM6DSM_STEP_COUNTER_L_ADDR, 2, &T_SLAVE_INTERFACE(stepCounterDataBuffer));
            }
        }

        if (!T(readSteps) && (T(fifoDataToRead) == 0)) {
            for (i = 0; i < NUM_SENSORS; i++) {
                if (T(sendFlushEvt[i])) {
                    osEnqueueEvt(sensorGetMyEventType(LSM6DSMSensorInfo[i].sensorType), SENSOR_DATA_EVENT_FLUSH, NULL);
                    T(sendFlushEvt[i]) = false;
                }
            }

            if (resetFIFO) {
                SET_STATE(SENSOR_INVALID_STATE);
                lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
            } else
                returnIdle = true;

            break;
        }

        SET_STATE(SENSOR_INT1_OUTPUT_DATA_HANDLING);

        if (T(fifoDataToRead) > 0) {
            T(lastFifoReadTimestamp) = sensorGetTime();

            if (T(time).noTimer.newTimestampDataAvl)
                T(time).timeSyncRtcTime = T(lastFifoReadTimestamp);
        }

        lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);

        break;

    case SENSOR_INT1_OUTPUT_DATA_HANDLING:
        if (T(fifoDataToRead) > 0) {
            if (T(time).noTimer.newTimestampDataAvl) {
                T(time).noTimer.newTimestampDataAvl = false;
                lsm6dsm_updateSyncTaskValues();
            }

            lsm6dsm_parseFifoData(&T_SLAVE_INTERFACE(fifoDataBuffer[1]), (T(fifoDataToRead) / 6) / T(fifoCntl).totalSip);

            if (T(fifoDataToReadPending) > 0) {
                T(fifoDataToRead) = T(fifoDataToReadPending);

                if (T(fifoDataToRead) > LSM6DSM_SPI_FIFO_SIZE) {
                    T(fifoDataToReadPending) = T(fifoDataToRead);
                    T(fifoDataToRead) = LSM6DSM_SPI_FIFO_SIZE - (LSM6DSM_SPI_FIFO_SIZE % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE));
                    T(fifoDataToReadPending) -= T(fifoDataToRead);
                } else {
                    T(fifoDataToReadPending) = 0;

                    if (T(fifoDataToRead) >= (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE))
                        T(fifoDataToRead) -= T(fifoDataToRead) % (T(fifoCntl).totalSip * LSM6DSM_ONE_SAMPLE_BYTE);
                    else
                        T(fifoDataToRead) = 0;
                }

                if (T(fifoDataToRead) > 0) {
                    SPI_READ(LSM6DSM_FIFO_DATA_OUT_L_ADDR, T(fifoDataToRead), &T_SLAVE_INTERFACE(fifoDataBuffer));
                    lsm6dsm_spiBatchTxRx(&T_SLAVE_INTERFACE(mode), lsm6dsm_spiCallback, &mTask, __FUNCTION__);
                    return;
                }
            } else
                T(fifoDataToRead) = 0;
        }

        for (i = 0; i < NUM_SENSORS; i++) {
            if (T(sendFlushEvt[i])) {
                osEnqueueEvt(sensorGetMyEventType(LSM6DSMSensorInfo[i].sensorType), SENSOR_DATA_EVENT_FLUSH, NULL);
                T(sendFlushEvt[i]) = false;
            }
        }

        if (T(readSteps)) {
            union EmbeddedDataPoint stepCntData;

            stepCntData.idata = T_SLAVE_INTERFACE(stepCounterDataBuffer[1]) | (T_SLAVE_INTERFACE(stepCounterDataBuffer[2]) << 8);
            osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_STEP_COUNT), stepCntData.vptr, NULL);
            DEBUG_PRINT("Step Counter update: %ld steps\n", stepCntData.idata);
            T(totalNumSteps) = stepCntData.idata;
            T(readSteps) = false;
        }

        returnIdle = true;
        break;

    case SENSOR_TIME_SYNC: ;
        lsm6dsm_updateSyncTaskValues();

        if (T(time).status == TIME_SYNC_TIMER) {
            if (timTimerSet(LSM6DSM_SYNC_DELTA_INTERVAL, 100, 100, lsm6dsm_timerSyncCallback, NULL, true) == 0)
                ERROR_PRINT("Failed to set a timer for time sync\n");
        }

        returnIdle = true;
        break;

#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
    case SENSOR_BARO_READ_DATA: ;
        uint16_t samplesCounter = 0;
        uint32_t sensorTimestamp;
        uint64_t timestamp;

        sensorTimestamp = ((T_SLAVE_INTERFACE(timestampDataBufferBaro[1]) << 0) |
                        (T_SLAVE_INTERFACE(timestampDataBufferBaro[2]) << 8) |
                        (T_SLAVE_INTERFACE(timestampDataBufferBaro[3]) << 16));

        if (T(time).timestampBaroLSB > 0) {
            if (((int32_t)sensorTimestamp - (int32_t)(T(time).timestampBaroLSB & LSM6DSM_MASK_24BIT_TIMESTAMP)) < -LSM6DSM_TIMEDIFF_OVERFLOW_LSB)
                T(time).timestampBaroLSB += (UINT32_MAX >> 8) + 1;
        }

        T(time).timestampBaroLSB = (T(time).timestampBaroLSB & ~LSM6DSM_MASK_24BIT_TIMESTAMP) + sensorTimestamp;

        if (time_sync_estimate_time1(&T(time).sensorTimeToRtcData, (uint64_t)T(time).timestampBaroLSB * LSM6DSM_TIME_RESOLUTION, &timestamp)) {
            if (T(sensors[PRESS]).enabled) {
                lsm6dsm_processSensorOneAxisData(&T(sensors[PRESS]), &T_SLAVE_INTERFACE(baroDataBuffer[1]), &samplesCounter, &timestamp);
                lsm6dsm_pushData(PRESS, &samplesCounter);
            }
        }

        if (T(sensors[TEMP]).enabled) {
            union EmbeddedDataPoint tempData;

            tempData.fdata = ((int16_t)(T_SLAVE_INTERFACE(baroDataBuffer[LSM6DSM_PRESS_OUTDATA_LEN + 2]) << 8) |
                            T_SLAVE_INTERFACE(baroDataBuffer[LSM6DSM_PRESS_OUTDATA_LEN + 1])) * LSM6DSM_TEMP_KSCALE;

            osEnqueueEvt(sensorGetMyEventType(SENS_TYPE_TEMP), tempData.vptr, NULL);
        }

        returnIdle = true;
        break;
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

    default:
        returnIdle = true;
        break;
    }

    if (returnIdle)
        lsm6dsm_processPendingEvt();
}

/*
 * lsm6dsm_handleEvent: handle driver events
 * @evtType: event type.
 * @evtData: event data.
 */
static void lsm6dsm_handleEvent(uint32_t evtType, const void *evtData)
{
    TDECL();
    struct LSM6DSMSensor *mSensor;

    switch (evtType) {
    case EVT_APP_START: ;
        uint64_t currTime;

        T(mRetryLeft) = LSM6DSM_RETRY_CNT_WAI;
        SET_STATE(SENSOR_BOOT);
        osEventUnsubscribe(T(tid), EVT_APP_START);

        /* Sensor need 100ms to boot, use a timer callback to continue */
        currTime = timGetTime();
        if (currTime < 100000000ULL) {
            timTimerSet(100000000 - currTime, 100, 100, lsm6dsm_timerCallback, NULL, true);
            break;
        }

        /* If 100ms already passed just fall through next step */
    case EVT_SPI_DONE:
        lsm6dsm_handleSpiDoneEvt(evtData);
        break;

    case EVT_SENSOR_INTERRUPT_1:
        lsm6dsm_readStatusReg(false);
        break;

    case EVT_SENSOR_POWERING_UP:
        mSensor = (struct LSM6DSMSensor *)evtData;

        mSensor->enabled = true;
        sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 1, 0);
        lsm6dsm_processPendingEvt();
        break;

    case EVT_SENSOR_POWERING_DOWN:
        mSensor = (struct LSM6DSMSensor *)evtData;

        mSensor->enabled = false;
        sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_POWER_STATE_CHG, 0, 0);
        lsm6dsm_processPendingEvt();
        break;

    case EVT_SENSOR_CONFIG_CHANGING:
        mSensor = (struct LSM6DSMSensor *)evtData;

        sensorSignalInternalEvt(mSensor->handle, SENSOR_INTERNAL_EVT_RATE_CHG, mSensor->rate[mSensor->idx], mSensor->latency);
        lsm6dsm_processPendingEvt();
        break;

    case EVT_APP_FROM_HOST:
        break;

    case EVT_SENSOR_RESTORE_IDLE:
        lsm6dsm_processPendingEvt();
        break;

    case EVT_TIME_SYNC:
        lsm6dsm_timeSyncTask();
        break;

    default:
        break;
    }
}

/*
 * lsm6dsm_initSensorStruct: initialize sensor struct variable
 * @sensor: sensor info.
 * @idx: sensor index.
 */
static void lsm6dsm_initSensorStruct(struct LSM6DSMSensor *sensor, enum SensorIndex idx)
{
    TDECL();
    uint8_t i;

    for (i = 0; i < NUM_SENSORS; i++) {
        if (i == idx)
            sensor->dependenciesRequireData[i] = true;
        else
            sensor->dependenciesRequireData[i] = false;

        sensor->rate[i] = 0;
    }

    sensor->idx = idx;
    sensor->hwRate = 0;
    sensor->latency = UINT64_MAX;
    sensor->enabled = false;
    sensor->samplesToDiscard = 0;
    sensor->samplesDecimator = 1;
    sensor->samplesDecimatorCounter = 0;
    sensor->samplesFifoDecimator = 1;
    sensor->samplesFifoDecimatorCounter = 0;
    sensor->tADataEvt = NULL;
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
    sensor->sADataEvt = NULL;
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
}

/*
 * lsm6dsm_startTask: first function executed when App start
 * @taskId: task id.
 */
static bool lsm6dsm_startTask(uint32_t taskId)
{
    TDECL();
    enum SensorIndex i;
    size_t slabSize;
    int err;

    DEBUG_PRINT("IMU: %lu\n", taskId);

    T(tid) = taskId;
    T(int1) = gpioRequest(LSM6DSM_INT1_GPIO);
    T(isr1).func = lsm6dsm_isr1;

    T_SLAVE_INTERFACE(mode).speed = LSM6DSM_SPI_SLAVE_FREQUENCY_HZ;
    T_SLAVE_INTERFACE(mode).bitsPerWord = 8;
    T_SLAVE_INTERFACE(mode).cpol = SPI_CPOL_IDLE_HI;
    T_SLAVE_INTERFACE(mode).cpha = SPI_CPHA_TRAILING_EDGE;
    T_SLAVE_INTERFACE(mode).nssChange = true;
    T_SLAVE_INTERFACE(mode).format = SPI_FORMAT_MSB_FIRST;
    T_SLAVE_INTERFACE(cs) = LSM6DSM_SPI_SLAVE_CS_GPIO;

    DEBUG_PRINT("Requested SPI on bus #%d @%dHz, int1 on gpio#%d\n",
            LSM6DSM_SPI_SLAVE_BUS_ID, LSM6DSM_SPI_SLAVE_FREQUENCY_HZ, LSM6DSM_INT1_GPIO);

    err = spiMasterRequest(LSM6DSM_SPI_SLAVE_BUS_ID, &T_SLAVE_INTERFACE(spiDev));
    if (err < 0) {
        ERROR_PRINT("Failed to request SPI on this bus: #%d\n", LSM6DSM_SPI_SLAVE_BUS_ID);
        return false;
    }

    T(int1Register) = LSM6DSM_INT1_CTRL_BASE;
    T(int2Register) = LSM6DSM_INT2_CTRL_BASE;
    T(embeddedFunctionsRegister) = LSM6DSM_CTRL10_C_BASE;
    T(pedometerDependencies) = 0;
    T(pendingInt) = false;
    T(pendingTimeSyncTask) = false;
    T(lastFifoReadTimestamp) = 0;
    T(totalNumSteps) = 0;
    T(time).status = TIME_SYNC_DISABLED;
#if defined(LSM6DSM_GYRO_CALIB_ENABLED) || defined(LSM6DSM_ACCEL_CALIB_ENABLED)
    T(currentTemperature) = 0;
#endif /* LSM6DSM_GYRO_CALIB_ENABLED, LSM6DSM_ACCEL_CALIB_ENABLED */
#ifdef LSM6DSM_I2C_MASTER_ENABLED
    T(masterConfigRegister) = LSM6DSM_MASTER_CONFIG_BASE;
    T(masterConfigDependencies) = 0;
#endif /* LSM6DSM_I2C_MASTER_ENABLED */
#if defined(LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED) && defined(LSM6DSM_I2C_MASTER_BAROMETER_ENABLED)
    T(baroTimerId) = 0;
    T(pendingBaroTimerTask) = false;
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED, LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    memset(T(gyroCalibrationData), 0, LSM6DSM_TRIAXIAL_NUM_AXIS * sizeof(int32_t));

    slabSize = sizeof(struct TripleAxisDataEvent) + (LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE * sizeof(struct TripleAxisDataPoint));

    T(mDataSlabThreeAxis) = slabAllocatorNew(slabSize, 4, 20);
    if (!T(mDataSlabThreeAxis)) {
        ERROR_PRINT("Failed to allocate mDataSlabThreeAxis memory\n");
        spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
        return false;
    }

#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
    slabSize = sizeof(struct SingleAxisDataEvent) + (LSM6DSM_MAX_NUM_COMMS_EVENT_SAMPLE * sizeof(struct SingleAxisDataPoint));

    T(mDataSlabOneAxis) = slabAllocatorNew(slabSize, 4, 20);
    if (!T(mDataSlabOneAxis)) {
        ERROR_PRINT("Failed to allocate mDataSlabOneAxis memory\n");
        slabAllocatorDestroy(T(mDataSlabThreeAxis));
        spiMasterRelease(T_SLAVE_INTERFACE(spiDev));
        return false;
    }
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */

    for (i = 0; i < NUM_SENSORS; i++) {
        T(pendingEnableConfig[i]) = false;
        T(pendingRateConfig[i]) = false;
        T(pendingFlush[i]) = 0;
        T(sendFlushEvt[i]) = false;
        lsm6dsm_initSensorStruct(&T(sensors[i]), i);
        T(sensors[i]).handle = sensorRegister(&LSM6DSMSensorInfo[i], &LSM6DSMSensorOps[i], NULL, false);
    }

    T(fifoCntl).decimatorsIdx[FIFO_GYRO] = GYRO;
    T(fifoCntl).decimatorsIdx[FIFO_ACCEL] = ACCEL;
    T(fifoCntl).decimatorsIdx[FIFO_DS3] = NUM_SENSORS;
    T(fifoCntl).decimatorsIdx[FIFO_DS4] = EMBEDDED_TIMESTAMP;

#ifdef LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED
    T(fifoCntl).decimatorsIdx[FIFO_DS3] = MAGN;
#else /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
    T(fifoCntl).decimatorsIdx[FIFO_DS3] = PRESS;
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */
#endif /* LSM6DSM_I2C_MASTER_MAGNETOMETER_ENABLED */

#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
    // Initializes the accelerometer offset calibration algorithm.
    const struct AccelCalParameters accelCalParameters = {
        MSEC_TO_NANOS(800),  // t0
        5,                   // n_s
        15,                  // fx
        15,                  // fxb
        15,                  // fy
        15,                  // fyb
        15,                  // fz
        15,                  // fzb
        15,                  // fle
        0.00025f             // th
    };
    accelCalInit(&T(accelCal), &accelCalParameters);
#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */

#ifdef LSM6DSM_GYRO_CALIB_ENABLED
    const struct GyroCalParameters gyroCalParameters = {
        SEC_TO_NANOS(5),      // min_still_duration_nanos
        SEC_TO_NANOS(5.9f),   // max_still_duration_nanos [see, NOTE 1]
        0,                    // calibration_time_nanos
        SEC_TO_NANOS(1.5f),   // window_time_duration_nanos
        0,                    // bias_x
        0,                    // bias_y
        0,                    // bias_z
        0.95f,                // stillness_threshold
        MDEG_TO_RAD * 40.0f,  // stillness_mean_delta_limit [rad/sec]
        5e-5f,                // gyro_var_threshold [rad/sec]^2
        1e-5f,                // gyro_confidence_delta [rad/sec]^2
        8e-3f,                // accel_var_threshold [m/sec^2]^2
        1.6e-3f,              // accel_confidence_delta [m/sec^2]^2
        1.4f,                 // mag_var_threshold [uTesla]^2
        0.25f,                // mag_confidence_delta [uTesla]^2
        1.5f,                 // temperature_delta_limit_celsius
        true                  // gyro_calibration_enable
    };
    // [NOTE 1]: 'max_still_duration_nanos' is set to 5.9 seconds to achieve a
    // max stillness period of 6.0 seconds and avoid buffer boundary conditions
    // that could push the max stillness to the next multiple of the analysis
    // window length (i.e., 7.5 seconds).
    gyroCalInit(&T(gyroCal), &gyroCalParameters);
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */

#ifdef LSM6DSM_OVERTEMP_CALIB_ENABLED
    // Initializes the gyroscope over-temperature offset compensation algorithm.
    const struct OverTempCalParameters gyroOtcParameters = {
        MSEC_TO_NANOS(500),    // min_temp_update_period_nanos
        DAYS_TO_NANOS(2),      // age_limit_nanos
        0.75f,                 // delta_temp_per_bin
        40.0f * MDEG_TO_RAD,   // jump_tolerance
        50.0f * MDEG_TO_RAD,   // outlier_limit
        80.0f * MDEG_TO_RAD,   // temp_sensitivity_limit
        3.0e3f * MDEG_TO_RAD,  // sensor_intercept_limit
        0.1f * MDEG_TO_RAD,    // significant_offset_change
        5,                     // min_num_model_pts
        true                   // over_temp_enable
    };
    overTempCalInit(&T(overTempCal), &gyroOtcParameters);
#endif /* LSM6DSM_OVERTEMP_CALIB_ENABLED */

#ifdef LSM6DSM_MAGN_CALIB_ENABLED
    const struct MagCalParameters magCalParameters = {
        3000000,  // min_batch_window_in_micros
        0.0f,     // x_bias
        0.0f,     // y_bias
        0.0f,     // z_bias
        1.0f,     // c00
        0.0f,     // c01
        0.0f,     // c02
        0.0f,     // c10
        1.0f,     // c11
        0.0f,     // c12
        0.0f,     // c20
        0.0f,     // c21
        1.0f      // c22
    };

    // Initializes the magnetometer offset calibration algorithm with diversity
    // checker.
    const struct DiversityCheckerParameters magDiversityParameters = {
        6.0f,    // var_threshold
        10.0f,   // max_min_threshold
        48.0f,   // local_field
        0.5f,    // threshold_tuning_param
        2.552f,  // max_distance_tuning_param
        8,       // min_num_diverse_vectors
        1        // max_num_max_distance
    };
    initMagCal(&T(magnCal), &magCalParameters, &magDiversityParameters);
#endif /* LSM6DSM_MAGN_CALIB_ENABLED */

    /* Initialize index used to fill/get data from buffer */
    T_SLAVE_INTERFACE(mWbufCnt) = 0;
    T_SLAVE_INTERFACE(mRegCnt) = 0;

    time_sync_init(&T(time).sensorTimeToRtcData);

    osEventSubscribe(T(tid), EVT_APP_START);

    DEBUG_PRINT("Enabling gpio#%d connected to int1\n", LSM6DSM_INT1_GPIO);
    lsm6dsm_enableInterrupt(T(int1), &T(isr1));

    return true;
}

/*
 * lsm6dsm_endTask: last function executed when App end
 */
static void lsm6dsm_endTask(void)
{
    TDECL();
    enum SensorIndex i;

#ifdef LSM6DSM_ACCEL_CALIB_ENABLED
    accelCalDestroy(&T(accelCal));
#endif /* LSM6DSM_ACCEL_CALIB_ENABLED */
#ifdef LSM6DSM_MAGN_CALIB_ENABLED
    magCalDestroy(&T(magnCal));
#endif /* LSM6DSM_MAGN_CALIB_ENABLED */
#ifdef LSM6DSM_GYRO_CALIB_ENABLED
    gyroCalDestroy(&T(gyroCal));
#endif /* LSM6DSM_GYRO_CALIB_ENABLED */

    lsm6dsm_disableInterrupt(T(int1), &T(isr1));
#ifdef LSM6DSM_I2C_MASTER_BAROMETER_ENABLED
    slabAllocatorDestroy(T(mDataSlabOneAxis));
#endif /* LSM6DSM_I2C_MASTER_BAROMETER_ENABLED */
    slabAllocatorDestroy(T(mDataSlabThreeAxis));
    spiMasterRelease(T_SLAVE_INTERFACE(spiDev));

    for (i = 0; i < NUM_SENSORS; i++)
        sensorUnregister(T(sensors[i]).handle);

    gpioRelease(T(int1));
}

INTERNAL_APP_INIT(LSM6DSM_APP_ID, LSM6DSM_APP_VERSION, lsm6dsm_startTask, lsm6dsm_endTask, lsm6dsm_handleEvent);