text: /*
   * Licensed to the Apache Software Foundation (ASF) under one
   * or more contributor license agreements.  See the NOTICE file
   * distributed with this work for additional information
   * resarding copyright ownership.  The ASF licenses this file
   * to you 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 <assert.h>
  #include <stdio.h>
  #include <errno.h>
  #include <string.h>
  
  #include "os/mynewt.h"
  #include "hal/hal_spi.h"
  #include "hal/hal_i2c.h"
  #include "i2cn/i2cn.h"
  #include "sensor/sensor.h"
  #include "sensor/accel.h"
  #include "lis2dw12/lis2dw12.h"
  #include "lis2dw12_priv.h"
  #include "hal/hal_gpio.h"
  #include "modlog/modlog.h"
  #include "stats/stats.h"
  #include <syscfg/syscfg.h>
  
  /*
   * Max time to wait for interrupt.
   */
  #define LIS2DW12_MAX_INT_WAIT (4 * OS_TICKS_PER_SEC)
  
  const struct lis2dw12_notif_cfg dflt_notif_cfg[] = {
      {
        .event     = SENSOR_EVENT_TYPE_SINGLE_TAP,
        .int_num   = 0,
        .notif_src = LIS2DW12_INT_SRC_STAP,
        .int_cfg   = LIS2DW12_INT1_CFG_SINGLE_TAP
      },
      {
        .event     = SENSOR_EVENT_TYPE_DOUBLE_TAP,
        .int_num   = 0,
        .notif_src = LIS2DW12_INT_SRC_DTAP,
        .int_cfg   = LIS2DW12_INT1_CFG_DOUBLE_TAP
      },
      {
        .event     = SENSOR_EVENT_TYPE_SLEEP,
        .int_num   = 1,
        .notif_src = LIS2DW12_STATUS_SLEEP_STATE,
        .int_cfg   = LIS2DW12_INT2_CFG_SLEEP_STATE
      },
      {
        .event     = SENSOR_EVENT_TYPE_FREE_FALL,
        .int_num   = 0,
        .notif_src = LIS2DW12_INT_SRC_FF_IA,
        .int_cfg   = LIS2DW12_INT1_CFG_FF
      },
      {
        .event     = SENSOR_EVENT_TYPE_WAKEUP,
        .int_num    = 0,
        .notif_src  = LIS2DW12_INT_SRC_WU_IA,
        .int_cfg    = LIS2DW12_INT1_CFG_WU
      },
      {
        .event     = SENSOR_EVENT_TYPE_SLEEP_CHANGE,
        .int_num   = 1,
        .notif_src = LIS2DW12_INT_SRC_SLP_CHG,
        .int_cfg   = LIS2DW12_INT2_CFG_SLEEP_CHG
      },
      {
        .event     = SENSOR_EVENT_TYPE_ORIENT_CHANGE,
        .int_num   = 0,
        .notif_src = LIS2DW12_SIXD_SRC_6D_IA,
        .int_cfg   = LIS2DW12_INT1_CFG_6D
      },
      {
        .event     = SENSOR_EVENT_TYPE_ORIENT_X_L_CHANGE,
        .int_num   = 0,
        .notif_src = LIS2DW12_SIXD_SRC_XL,
        .int_cfg   = LIS2DW12_INT1_CFG_6D
      },
      {
        .event     = SENSOR_EVENT_TYPE_ORIENT_Y_L_CHANGE,
        .int_num   = 0,
        .notif_src = LIS2DW12_SIXD_SRC_YL,
        .int_cfg   = LIS2DW12_INT1_CFG_6D
      },
      {
        .event     = SENSOR_EVENT_TYPE_ORIENT_Z_L_CHANGE,
        .int_num   = 0,
        .notif_src = LIS2DW12_SIXD_SRC_ZL,
        .int_cfg   = LIS2DW12_INT1_CFG_6D
      },
      {
        .event     = SENSOR_EVENT_TYPE_ORIENT_X_H_CHANGE,
        .int_num   = 0,
        .notif_src = LIS2DW12_SIXD_SRC_XH,
        .int_cfg   = LIS2DW12_INT1_CFG_6D
      },
      {
        .event     = SENSOR_EVENT_TYPE_ORIENT_Y_H_CHANGE,
        .int_num   = 0,
        .notif_src = LIS2DW12_SIXD_SRC_YH,
        .int_cfg   = LIS2DW12_INT1_CFG_6D
      },
      {
        .event     = SENSOR_EVENT_TYPE_ORIENT_Z_H_CHANGE,
        .int_num   = 0,
        .notif_src = LIS2DW12_SIXD_SRC_ZH,
        .int_cfg   = LIS2DW12_INT1_CFG_6D
      }
  
  };
  
  static struct hal_spi_settings spi_lis2dw12_settings = {
      .data_order = HAL_SPI_MSB_FIRST,
      .data_mode  = HAL_SPI_MODE3,
      .baudrate   = 4000,
      .word_size  = HAL_SPI_WORD_SIZE_8BIT,
  };
  
  /* Define the stats section and records */
  STATS_SECT_START(lis2dw12_stat_section)
      STATS_SECT_ENTRY(write_errors)
      STATS_SECT_ENTRY(read_errors)
  #if MYNEWT_VAL(LIS2DW12_NOTIF_STATS)
      STATS_SECT_ENTRY(single_tap_notify)
      STATS_SECT_ENTRY(double_tap_notify)
      STATS_SECT_ENTRY(free_fall_notify)
      STATS_SECT_ENTRY(sleep_notify)
      STATS_SECT_ENTRY(wakeup_notify)
      STATS_SECT_ENTRY(sleep_chg_notify)
      STATS_SECT_ENTRY(orient_chg_notify)
      STATS_SECT_ENTRY(orient_chg_x_l_notify)
      STATS_SECT_ENTRY(orient_chg_y_l_notify)
      STATS_SECT_ENTRY(orient_chg_z_l_notify)
      STATS_SECT_ENTRY(orient_chg_x_h_notify)
      STATS_SECT_ENTRY(orient_chg_y_h_notify)
      STATS_SECT_ENTRY(orient_chg_z_h_notify)
  #endif
  STATS_SECT_END
  
  /* Define stat names for querying */
  STATS_NAME_START(lis2dw12_stat_section)
      STATS_NAME(lis2dw12_stat_section, write_errors)
      STATS_NAME(lis2dw12_stat_section, read_errors)
  #if MYNEWT_VAL(LIS2DW12_NOTIF_STATS)
      STATS_NAME(lis2dw12_stat_section, single_tap_notify)
      STATS_NAME(lis2dw12_stat_section, double_tap_notify)
      STATS_NAME(lis2dw12_stat_section, free_fall_notify)
      STATS_NAME(lis2dw12_stat_section, sleep_notify)
      STATS_NAME(lis2dw12_stat_section, wakeup_notify)
      STATS_NAME(lis2dw12_stat_section, sleep_chg_notify)
      STATS_NAME(lis2dw12_stat_section, orient_chg_notify)
      STATS_NAME(lis2dw12_stat_section, orient_chg_x_l_notify)
      STATS_NAME(lis2dw12_stat_section, orient_chg_y_l_notify)
      STATS_NAME(lis2dw12_stat_section, orient_chg_z_l_notify)
      STATS_NAME(lis2dw12_stat_section, orient_chg_x_h_notify)
      STATS_NAME(lis2dw12_stat_section, orient_chg_y_h_notify)
      STATS_NAME(lis2dw12_stat_section, orient_chg_z_h_notify)
  #endif
  STATS_NAME_END(lis2dw12_stat_section)
  
  /* Global variable used to hold stats data */
  STATS_SECT_DECL(lis2dw12_stat_section) g_lis2dw12stats;
  
  #define LIS2DW12_LOG(lvl_, ...) \
      MODLOG_ ## lvl_(MYNEWT_VAL(LIS2DW12_LOG_MODULE), __VA_ARGS__)
  
  /* Exports for the sensor API */
  static int lis2dw12_sensor_read(struct sensor *, sensor_type_t,
          sensor_data_func_t, void *, uint32_t);
  static int lis2dw12_sensor_get_config(struct sensor *, sensor_type_t,
          struct sensor_cfg *);
  static int lis2dw12_sensor_set_notification(struct sensor *,
                                              sensor_event_type_t);
  static int lis2dw12_sensor_unset_notification(struct sensor *,
                                                sensor_event_type_t);
  static int lis2dw12_sensor_handle_interrupt(struct sensor *);
  static int lis2dw12_sensor_set_config(struct sensor *, void *);
  
  static const struct sensor_driver g_lis2dw12_sensor_driver = {
      .sd_read               = lis2dw12_sensor_read,
      .sd_set_config         = lis2dw12_sensor_set_config,
      .sd_get_config         = lis2dw12_sensor_get_config,
      .sd_set_notification   = lis2dw12_sensor_set_notification,
      .sd_unset_notification = lis2dw12_sensor_unset_notification,
      .sd_handle_interrupt   = lis2dw12_sensor_handle_interrupt
  
  };
  
  /**
   * Write multiple length data to LIS2DW12 sensor over I2C  (MAX: 19 bytes)
   *
   * @param The sensor interface
   * @param register address
   * @param variable length payload
   * @param length of the payload to write
   *
   * @return 0 on success, non-zero on failure
   */
  static int
  lis2dw12_i2c_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *buffer,
                        uint8_t len)
  {
      int rc;
      uint8_t payload[20] = { addr, 0, 0, 0, 0, 0, 0, 0,
                                 0, 0, 0, 0, 0, 0, 0, 0,
                                 0, 0, 0, 0};
  
      struct hal_i2c_master_data data_struct = {
          .address = itf->si_addr,
          .len = len + 1,
          .buffer = payload
      };
  
      if (len > (sizeof(payload) - 1)) {
          rc = OS_EINVAL;
          goto err;
      }
  
      memcpy(&payload[1], buffer, len);
  
      /* Register write */
      rc = i2cn_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1,
                             MYNEWT_VAL(LIS2DW12_I2C_RETRIES));
      if (rc) {
          LIS2DW12_LOG(ERROR, "I2C access failed at address 0x%02X\n",
                       data_struct.address);
          STATS_INC(g_lis2dw12stats, write_errors);
          goto err;
      }
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Write multiple length data to LIS2DW12 sensor over SPI
   *
   * @param The sensor interface
   * @param register address
   * @param variable length payload
   * @param length of the payload to write
   *
   * @return 0 on success, non-zero on failure
   */
  static int
  lis2dw12_spi_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *payload,
                        uint8_t len)
  {
      int i;
      int rc;
  
      /*
       * Auto register address increment is needed if the length
       * requested is moret than 1
       */
      if (len > 1) {
          addr |= LIS2DW12_SPI_READ_CMD_BIT;
      }
  
      /* Select the device */
      hal_gpio_write(itf->si_cs_pin, 0);
  
  
      /* Send the address */
      rc = hal_spi_tx_val(itf->si_num, addr);
      if (rc == 0xFFFF) {
          rc = SYS_EINVAL;
          LIS2DW12_LOG(ERROR, "SPI_%u register write failed addr:0x%02X\n",
                       itf->si_num, addr);
          STATS_INC(g_lis2dw12stats, write_errors);
          goto err;
      }
  
      for (i = 0; i < len; i++) {
          /* Read data */
          rc = hal_spi_tx_val(itf->si_num, payload[i]);
          if (rc == 0xFFFF) {
              rc = SYS_EINVAL;
              LIS2DW12_LOG(ERROR, "SPI_%u write failed addr:0x%02X:0x%02X\n",
                           itf->si_num, addr);
              STATS_INC(g_lis2dw12stats, write_errors);
              goto err;
          }
      }
  
  
      rc = 0;
  
  err:
      /* De-select the device */
      hal_gpio_write(itf->si_cs_pin, 1);
  
      return rc;
  }
  
  /**
   * Write multiple length data to LIS2DW12 sensor over different interfaces
   *
   * @param The sensor interface
   * @param register address
   * @param variable length payload
   * @param length of the payload to write
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *payload,
                    uint8_t len)
  {
      int rc;
  
      rc = sensor_itf_lock(itf, MYNEWT_VAL(LIS2DW12_ITF_LOCK_TMO));
      if (rc) {
          return rc;
      }
  
      if (itf->si_type == SENSOR_ITF_I2C) {
          rc = lis2dw12_i2c_writelen(itf, addr, payload, len);
      } else {
          rc = lis2dw12_spi_writelen(itf, addr, payload, len);
      }
  
      sensor_itf_unlock(itf);
  
      return rc;
  }
  
  /**
   * Read multiple bytes starting from specified register over i2c
   *
   * @param The sensor interface
   * @param The register address start reading from
   * @param Pointer to where the register value should be written
   * @param Number of bytes to read
   *
   * @return 0 on success, non-zero error on failure.
   */
  int
  lis2dw12_i2c_readlen(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer,
                       uint8_t len)
  {
      int rc;
  
      struct hal_i2c_master_data data_struct = {
          .address = itf->si_addr,
          .len = 1,
          .buffer = &reg
      };
  
      /* Register write */
      rc = i2cn_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1,
                             MYNEWT_VAL(LIS2DW12_I2C_RETRIES));
      if (rc) {
          LIS2DW12_LOG(ERROR, "I2C access failed at address 0x%02X\n",
                       itf->si_addr);
          STATS_INC(g_lis2dw12stats, write_errors);
          return rc;
      }
  
      /* Read data */
      data_struct.len = len;
      data_struct.buffer = buffer;
      rc = i2cn_master_read(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1,
                            MYNEWT_VAL(LIS2DW12_I2C_RETRIES));
  
      if (rc) {
          LIS2DW12_LOG(ERROR, "Failed to read from 0x%02X:0x%02X\n",
                       itf->si_addr, reg);
          STATS_INC(g_lis2dw12stats, read_errors);
      }
  
      return rc;
  }
  
  /**
   * Read multiple bytes starting from specified register over SPI
   *
   * @param The sensor interface
   * @param The register address start reading from
   * @param Pointer to where the register value should be written
   * @param Number of bytes to read
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_spi_readlen(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer,
                      uint8_t len)
  {
      int i;
      uint16_t retval;
      int rc = 0;
  
      /* Select the device */
      hal_gpio_write(itf->si_cs_pin, 0);
  
      /* Send the address */
      retval = hal_spi_tx_val(itf->si_num, reg | LIS2DW12_SPI_READ_CMD_BIT);
  
      if (retval == 0xFFFF) {
          rc = SYS_EINVAL;
          LIS2DW12_LOG(ERROR, "SPI_%u register write failed addr:0x%02X\n",
                       itf->si_num, reg);
          STATS_INC(g_lis2dw12stats, read_errors);
          goto err;
      }
  
      for (i = 0; i < len; i++) {
          /* Read data */
          retval = hal_spi_tx_val(itf->si_num, 0);
          if (retval == 0xFFFF) {
              rc = SYS_EINVAL;
              LIS2DW12_LOG(ERROR, "SPI_%u read failed addr:0x%02X\n",
                           itf->si_num, reg);
              STATS_INC(g_lis2dw12stats, read_errors);
              goto err;
          }
          buffer[i] = retval;
      }
  
  err:
      /* De-select the device */
      hal_gpio_write(itf->si_cs_pin, 1);
  
      return rc;
  }
  
  
  /**
   * Write byte to sensor over different interfaces
   *
   * @param The sensor interface
   * @param The register address to write to
   * @param The value to write
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_write8(struct sensor_itf *itf, uint8_t reg, uint8_t value)
  {
      int rc;
  
      rc = sensor_itf_lock(itf, MYNEWT_VAL(LIS2DW12_ITF_LOCK_TMO));
      if (rc) {
          return rc;
      }
  
      if (itf->si_type == SENSOR_ITF_I2C) {
          rc = lis2dw12_i2c_writelen(itf, reg, &value, 1);
      } else {
          rc = lis2dw12_spi_writelen(itf, reg, &value, 1);
      }
  
      sensor_itf_unlock(itf);
  
      return rc;
  }
  
  /**
   * Read byte data from sensor over different interfaces
   *
   * @param The sensor interface
   * @param The register address to read from
   * @param Pointer to where the register value should be written
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_read8(struct sensor_itf *itf, uint8_t reg, uint8_t *value)
  {
      int rc;
  
      rc = sensor_itf_lock(itf, MYNEWT_VAL(LIS2DW12_ITF_LOCK_TMO));
      if (rc) {
          return rc;
      }
  
      if (itf->si_type == SENSOR_ITF_I2C) {
          rc = lis2dw12_i2c_readlen(itf, reg, value, 1);
      } else {
          rc = lis2dw12_spi_readlen(itf, reg, value, 1);
      }
  
      sensor_itf_unlock(itf);
  
      return rc;
  }
  
  /**
   * Read multiple bytes starting from specified register over different interfaces
   *
   * @param The sensor interface
   * @param The register address start reading from
   * @param Pointer to where the register value should be written
   * @param Number of bytes to read
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_readlen(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer,
                  uint8_t len)
  {
      int rc;
  
      rc = sensor_itf_lock(itf, MYNEWT_VAL(LIS2DW12_ITF_LOCK_TMO));
      if (rc) {
          return rc;
      }
  
      if (itf->si_type == SENSOR_ITF_I2C) {
          rc = lis2dw12_i2c_readlen(itf, reg, buffer, len);
      } else {
          rc = lis2dw12_spi_readlen(itf, reg, buffer, len);
      }
  
      sensor_itf_unlock(itf);
  
      return rc;
  }
  
  /**
   * Calculates the acceleration in m/s^2 from mg
   *
   * @param acc value in mg
   * @param float ptr to return calculated value in ms2
   */
  void
  lis2dw12_calc_acc_ms2(int16_t acc_mg, float *acc_ms2)
  {
      *acc_ms2 = (acc_mg * STANDARD_ACCEL_GRAVITY)/1000;
  }
  
  /**
   * Calculates the acceleration in mg from m/s^2
   *
   * @param acc value in m/s^2
   * @param int16 ptr to return calculated value in mg
   */
  void
  lis2dw12_calc_acc_mg(float acc_ms2, int16_t *acc_mg)
  {
      *acc_mg = (acc_ms2 * 1000)/STANDARD_ACCEL_GRAVITY;
  }
  
  /**
   * Reset lis2dw12
   *
   * @param The sensor interface
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_reset(struct sensor_itf *itf)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG2, &reg);
      if (rc) {
          goto err;
      }
  
      reg |= LIS2DW12_CTRL_REG2_SOFT_RESET | LIS2DW12_CTRL_REG2_BOOT;
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG2, reg);
      if (rc) {
          goto err;
      }
  
      os_time_delay((OS_TICKS_PER_SEC * 6/1000) + 1);
  
  err:
      return rc;
  }
  
  /**
   * Get chip ID
   *
   * @param sensor interface
   * @param ptr to chip id to be filled up
   */
  int
  lis2dw12_get_chip_id(struct sensor_itf *itf, uint8_t *chip_id)
  {
      uint8_t reg;
      int rc;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WHO_AM_I, &reg);
  
      if (rc) {
          goto err;
      }
  
      *chip_id = reg;
  
  err:
      return rc;
  }
  
  /**
   * Sets the full scale selection
   *
   * @param The sensor interface
   * @param The fs setting
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_full_scale(struct sensor_itf *itf, uint8_t fs)
  {
      int rc;
      uint8_t reg;
  
      if (fs > LIS2DW12_FS_16G) {
          LIS2DW12_LOG(ERROR, "Invalid full scale value\n");
          rc = SYS_EINVAL;
          goto err;
      }
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG6, &reg);
      if (rc) {
          goto err;
      }
  
      reg &= ~LIS2DW12_CTRL_REG6_FS;
      reg |= (fs & LIS2DW12_CTRL_REG6_FS);
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG6, reg);
      if (rc) {
          goto err;
      }
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Gets the full scale selection
   *
   * @param The sensor interface
   * @param ptr to fs
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_get_full_scale(struct sensor_itf *itf, uint8_t *fs)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG6, &reg);
      if (rc) {
          goto err;
      }
  
      *fs = reg & LIS2DW12_CTRL_REG6_FS;
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Sets the rate
   *
   * @param The sensor interface
   * @param The rate
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_rate(struct sensor_itf *itf, uint8_t rate)
  {
      int rc;
      uint8_t reg;
  
      if (rate > LIS2DW12_DATA_RATE_1600HZ) {
          LIS2DW12_LOG(ERROR, "Invalid rate value\n");
          rc = SYS_EINVAL;
          goto err;
      }
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG1, &reg);
      if (rc) {
          goto err;
      }
  
      reg &= ~LIS2DW12_CTRL_REG1_ODR;
      reg |= (rate & LIS2DW12_CTRL_REG1_ODR);
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG1, reg);
      if (rc) {
          goto err;
      }
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Gets the rate
   *
   * @param The sensor ineterface
   * @param ptr to the rate
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_get_rate(struct sensor_itf *itf, uint8_t *rate)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG1, &reg);
      if (rc) {
          goto err;
      }
  
      *rate = reg & LIS2DW12_CTRL_REG1_ODR;
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Sets the low noise enable
   *
   * @param The sensor interface
   * @param low noise enabled
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_low_noise(struct sensor_itf *itf, uint8_t en)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG6, &reg);
      if (rc) {
          goto err;
      }
  
      if (en) {
          reg |= LIS2DW12_CTRL_REG6_LOW_NOISE;
      } else {
          reg &= ~LIS2DW12_CTRL_REG6_LOW_NOISE;
      }
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG6, reg);
      if (rc) {
          goto err;
      }
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Gets whether low noise is enabled
   *
   * @param The sensor interface
   * @param ptr to low noise settings read from sensor
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_get_low_noise(struct sensor_itf *itf, uint8_t *en)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG6, &reg);
      if (rc) {
          goto err;
      }
  
      *en = (reg & LIS2DW12_CTRL_REG6_LOW_NOISE) > 0;
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Sets the power mode of the sensor
   *
   * @param The sensor interface
   * @param power mode
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_power_mode(struct sensor_itf *itf, uint8_t mode)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG1, &reg);
      if (rc) {
          goto err;
      }
  
      reg &= ~LIS2DW12_CTRL_REG1_MODE;
      reg &= ~LIS2DW12_CTRL_REG1_LP_MODE;
      reg |= mode;
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG1, reg);
      if (rc) {
          goto err;
      }
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Gets the power mode of the sensor
   *
   * @param The sensor interface
   * @param ptr to power mode setting read from sensor
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_get_power_mode(struct sensor_itf *itf, uint8_t *mode)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG1, &reg);
      if (rc) {
          goto err;
      }
  
      *mode = reg & (LIS2DW12_CTRL_REG1_MODE | LIS2DW12_CTRL_REG1_LP_MODE);
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Sets the self test mode of the sensor
   *
   * @param The sensor interface
   * @param self test mode
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_self_test(struct sensor_itf *itf, uint8_t mode)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG3, &reg);
      if (rc) {
          goto err;
      }
  
      reg &= ~LIS2DW12_CTRL_REG3_ST_MODE;
      reg |= (mode & LIS2DW12_CTRL_REG3_ST_MODE);
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG3, reg);
      if (rc) {
          goto err;
      }
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Gets the self test mode of the sensor
   *
   * @param The sensor interface
   * @param ptr to self test mode read from sensor
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_get_self_test(struct sensor_itf *itf, uint8_t *mode)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG3, &reg);
      if (rc) {
          goto err;
      }
  
      *mode = reg & LIS2DW12_CTRL_REG3_ST_MODE;
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Sets the interrupt push-pull/open-drain selection
   *
   * @param The sensor interface
   * @param interrupt setting (0 = push-pull, 1 = open-drain)
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_int_pp_od(struct sensor_itf *itf, uint8_t mode)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG3, &reg);
      if (rc) {
          return rc;
      }
  
      reg &= ~LIS2DW12_CTRL_REG3_PP_OD;
      reg |= mode ? LIS2DW12_CTRL_REG3_PP_OD : 0;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG3, reg);
  }
  
  /**
   * Gets the interrupt push-pull/open-drain selection
   *
   * @param The sensor interface
   * @param ptr to store setting (0 = push-pull, 1 = open-drain)
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_get_int_pp_od(struct sensor_itf *itf, uint8_t *mode)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG3, &reg);
      if (rc) {
          return rc;
      }
  
      *mode = (reg & LIS2DW12_CTRL_REG3_PP_OD) ? 1 : 0;
  
      return 0;
  }
  
  /**
   * Sets whether latched interrupts are enabled
   *
   * @param The sensor interface
   * @param value to set (0 = not latched, 1 = latched)
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_latched_int(struct sensor_itf *itf, uint8_t en)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG3, &reg);
      if (rc) {
          return rc;
      }
  
      reg &= ~LIS2DW12_CTRL_REG3_LIR;
      reg |= en ? LIS2DW12_CTRL_REG3_LIR : 0;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG3, reg);
  
  }
  
  /**
   * Gets whether latched interrupts are enabled
   *
   * @param The sensor interface
   * @param ptr to store value (0 = not latched, 1 = latched)
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_get_latched_int(struct sensor_itf *itf, uint8_t *en)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG3, &reg);
      if (rc) {
          return rc;
      }
  
      *en = (reg & LIS2DW12_CTRL_REG3_LIR) ? 1 : 0;
  
      return 0;
  }
  
  /**
   * Sets whether interrupts are active high or low
   *
   * @param The sensor interface
   * @param value to set (0 = active high, 1 = active low)
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_int_active_low(struct sensor_itf *itf, uint8_t low)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG3, &reg);
      if (rc) {
          return rc;
      }
  
      reg &= ~LIS2DW12_CTRL_REG3_H_LACTIVE;
      reg |= low ? LIS2DW12_CTRL_REG3_H_LACTIVE : 0;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG3, reg);
  
  }
  
  /**
   * Gets whether interrupts are active high or low
   *
   * @param The sensor interface
   * @param ptr to store value (0 = active high, 1 = active low)
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_get_int_active_low(struct sensor_itf *itf, uint8_t *low)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG3, &reg);
      if (rc) {
          return rc;
      }
  
      *low = (reg & LIS2DW12_CTRL_REG3_H_LACTIVE) ? 1 : 0;
  
      return 0;
  
  }
  
  /**
   * Sets single data conversion mode
   *
   * @param The sensor interface
   * @param value to set (0 = trigger on INT2 pin, 1 = trigger on write to SLP_MODE_1)
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_slp_mode(struct sensor_itf *itf, uint8_t mode)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG3, &reg);
      if (rc) {
          return rc;
      }
  
      reg &= ~LIS2DW12_CTRL_REG3_SLP_MODE_SEL;
      reg |= mode ? LIS2DW12_CTRL_REG3_SLP_MODE_SEL : 0;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG3, reg);
  }
  
  /**
   * Gets single data conversion mode
   *
   * @param The sensor interface
   * @param ptr to store value (0 = trigger on INT2 pin, 1 = trigger on write to SLP_MODE_1)
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_get_slp_mode(struct sensor_itf *itf, uint8_t *mode)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG3, &reg);
      if (rc) {
          return rc;
      }
  
      *mode = (reg & LIS2DW12_CTRL_REG3_SLP_MODE_SEL) ? 1 : 0;
  
      return 0;
  }
  
  /**
   * Starts a data conversion in on demand mode
   *
   * @param The sensor interface
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_start_on_demand_conversion(struct sensor_itf *itf)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG3, &reg);
      if (rc) {
          return rc;
      }
  
      reg |= LIS2DW12_CTRL_REG3_SLP_MODE_1;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG3, reg);
  }
  
  
  /**
   * Set filter config
   *
   * @param the sensor interface
   * @param the filter bandwidth
   * @param filter type (1 = high pass, 0 = low pass)
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_filter_cfg(struct sensor_itf *itf, uint8_t bw, uint8_t type)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG6, &reg);
      if (rc) {
          goto err;
      }
  
      reg &= ~LIS2DW12_CTRL_REG6_BW_FILT;
      reg &= ~LIS2DW12_CTRL_REG6_FDS;
      reg |= (bw & 0x3) << 6;
      if (type) {
          reg |= LIS2DW12_CTRL_REG6_FDS;
      }
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG6, reg);
      if (rc) {
          goto err;
      }
  
      return 0;
  err:
      return rc;
  
  }
  
  /**
   * Get filter config
   *
   * @param the sensor interface
   * @param ptr to the filter bandwidth
   * @param ptr to filter type (1 = high pass, 0 = low pass)
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_get_filter_cfg(struct sensor_itf *itf, uint8_t *bw, uint8_t *type)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG6, &reg);
      if (rc) {
          goto err;
      }
  
      *bw = (reg & LIS2DW12_CTRL_REG6_BW_FILT) >> 6;
      *type = (reg & LIS2DW12_CTRL_REG6_FDS) > 0;
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Sets new offsets in sensor
   *
   * @param The sensor interface
   * @param X offset
   * @param Y offset
   * @param Z offset
   *
   * @return 0 on success, non-zero error on failure.
   */
  int
  lis2dw12_set_offsets(struct sensor_itf *itf, int8_t offset_x,
                       int8_t offset_y, int8_t offset_z, uint8_t weight)
  {
      uint8_t reg;
      int rc;
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_X_OFS, offset_x);
      if (rc) {
          return rc;
      }
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_Y_OFS, offset_y);
      if (rc) {
          return rc;
      }
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_Z_OFS, offset_z);
      if (rc) {
          return rc;
      }
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG7, &reg);
      if (rc) {
          return rc;
      }
  
      if (weight) {
          reg |= LIS2DW12_CTRL_REG7_USR_OFF_W;
      } else {
          reg &= ~LIS2DW12_CTRL_REG7_USR_OFF_W;
      }
  
      return lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG7, reg);
  }
  
  /**
   * Gets the offsets currently set
   *
   * @param The sensor interface
   * @param Pointer to location to store X offset
   * @param Pointer to location to store Y offset
   * @param Pointer to location to store Z offset
   *
   * @return 0 on success, non-zero error on failure.
   */
  int
  lis2dw12_get_offsets(struct sensor_itf *itf, int8_t *offset_x,
                       int8_t *offset_y, int8_t *offset_z, uint8_t *weight)
  {
      uint8_t reg;
      int rc;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_X_OFS, (uint8_t *)offset_x);
      if (rc) {
          return rc;
      }
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_Y_OFS, (uint8_t *)offset_y);
      if (rc) {
          return rc;
      }
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_Z_OFS, (uint8_t *)offset_z);
      if (rc) {
          return rc;
      }
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG7, &reg);
      if (rc) {
          return rc;
      }
  
      *weight = reg & LIS2DW12_CTRL_REG7_USR_OFF_W ? 1 : 0;
  
      return 0;
  }
  
  /**
   * Sets whether offset are enabled (only work when low pass filtering is enabled)
   *
   * @param The sensor interface
   * @param value to set (0 = disabled, 1 = enabled)
   *
   * @return 0 on success, non-zero error on failure.
   */
  int lis2dw12_set_offset_enable(struct sensor_itf *itf, uint8_t enabled)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG7, &reg);
      if (rc) {
          return rc;
      }
  
      if (enabled) {
          reg |= LIS2DW12_CTRL_REG7_USR_OFF_OUT;
      } else {
          reg &= ~LIS2DW12_CTRL_REG7_USR_OFF_OUT;
      }
  
      return lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG7, reg);
  }
  
  
  /**
   * Set tap detection configuration
   *
   * @param the sensor interface
   * @param the tap settings
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_set_tap_cfg(struct sensor_itf *itf, struct lis2dw12_tap_settings *cfg)
  {
      int rc;
      uint8_t reg;
  
      reg = cfg->en_4d ? LIS2DW12_TAP_THS_X_4D_EN : 0;
      reg |= (cfg->ths_6d & 0x3) << 5;
      reg |= cfg->tap_ths_x & LIS2DW12_TAP_THS_X_THS;
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_TAP_THS_X, reg);
      if (rc) {
          return rc;
      }
  
      reg = 0;
      reg |= (cfg->tap_priority & 0x7) << 5;
      reg |= cfg->tap_ths_y & LIS2DW12_TAP_THS_Y_THS;
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_TAP_THS_Y, reg);
      if (rc) {
          return rc;
      }
  
      reg = 0;
      reg |= cfg->en_x ? LIS2DW12_TAP_THS_Z_X_EN : 0;
      reg |= cfg->en_y ? LIS2DW12_TAP_THS_Z_Y_EN : 0;
      reg |= cfg->en_z ? LIS2DW12_TAP_THS_Z_Z_EN : 0;
      reg |= cfg->tap_ths_z & LIS2DW12_TAP_THS_Z_THS;
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_TAP_THS_Z, reg);
      if (rc) {
          return rc;
      }
  
      reg = 0;
      reg |= (cfg->latency & 0xf) << 4;
      reg |= (cfg->quiet & 0x3) << 2;
      reg |= cfg->shock & LIS2DW12_INT_DUR_SHOCK;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_INT_DUR, reg);
  }
  
  /**
   * Get tap detection config
   *
   * @param the sensor interface
   * @param ptr to the tap settings
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_tap_cfg(struct sensor_itf *itf, struct lis2dw12_tap_settings *cfg)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_TAP_THS_X, &reg);
      if (rc) {
          return rc;
      }
  
      cfg->en_4d = (reg & LIS2DW12_TAP_THS_X_4D_EN) > 0;
      cfg->ths_6d = (reg & LIS2DW12_TAP_THS_X_6D_THS) >> 5;
      cfg->tap_ths_x = reg & LIS2DW12_TAP_THS_X_THS;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_TAP_THS_Y, &reg);
      if (rc) {
          return rc;
      }
  
      cfg->tap_priority = (reg & LIS2DW12_TAP_THS_Y_PRIOR) >> 5;
      cfg->tap_ths_y = reg & LIS2DW12_TAP_THS_Y_THS;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_TAP_THS_Z, &reg);
      if (rc) {
          return rc;
      }
  
      cfg->en_x = (reg & LIS2DW12_TAP_THS_Z_X_EN) > 0;
      cfg->en_y = (reg & LIS2DW12_TAP_THS_Z_Y_EN) > 0;
      cfg->en_z = (reg & LIS2DW12_TAP_THS_Z_Z_EN) > 0;
      cfg->tap_ths_z = reg & LIS2DW12_TAP_THS_Z_THS;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_INT_DUR, &reg);
      if (rc) {
          return rc;
      }
  
      cfg->latency = (reg & LIS2DW12_INT_DUR_LATENCY) >> 4;
      cfg->quiet = (reg & LIS2DW12_INT_DUR_QUIET) >> 2;
      cfg->shock = reg & LIS2DW12_INT_DUR_SHOCK;
  
      return 0;
  }
  
  /**
   * Set freefall detection configuration
   *
   * @param the sensor interface
   * @param freefall duration (6 bits LSB = 1/ODR)
   * @param freefall threshold (3 bits)
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_set_freefall(struct sensor_itf *itf, uint8_t dur, uint8_t ths)
  {
      int rc;
      uint8_t reg;
  
      reg = 0;
      reg |= (dur & 0x1F) << 3;
      reg |= ths & LIS2DW12_FREEFALL_THS;
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_FREEFALL, reg);
      if (rc) {
          return rc;
      }
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_DUR, &reg);
      if (rc) {
          return rc;
      }
  
      reg &= ~LIS2DW12_WAKE_DUR_FF_DUR;
      reg |= dur & 0x20 ? LIS2DW12_WAKE_DUR_FF_DUR : 0;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_WAKE_UP_DUR, reg);
  }
  
  /**
   * Get freefall detection config
   *
   * @param the sensor interface
   * @param ptr to freefall duration
   * @param ptr to freefall threshold
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_freefall(struct sensor_itf *itf, uint8_t *dur, uint8_t *ths)
  {
      int rc;
      uint8_t ff_reg, wake_reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_FREEFALL, &ff_reg);
      if (rc) {
          return rc;
      }
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_DUR, &wake_reg);
      if (rc) {
          return rc;
      }
  
      *dur = (ff_reg & LIS2DW12_FREEFALL_DUR) >> 3;
      *dur |= wake_reg & LIS2DW12_WAKE_DUR_FF_DUR ? (1 << 5) : 0;
      *ths = ff_reg & LIS2DW12_FREEFALL_THS;
  
      return 0;
  }
  
  /**
   * Setup FIFO
   *
   * @param the sensor interface
   * @param FIFO mode to setup
   * @param Threshold to set for FIFO
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_set_fifo_cfg(struct sensor_itf *itf, enum lis2dw12_fifo_mode mode, uint8_t fifo_ths)
  {
      uint8_t reg = 0;
  
      reg |= fifo_ths & LIS2DW12_FIFO_CTRL_FTH;
      reg |= (mode & 0x7) << 5;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_FIFO_CTRL, reg);
  }
  
  /**
   * Get Number of Samples in FIFO
   *
   * @param the sensor interface
   * @param Pointer to return number of samples in
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_fifo_samples(struct sensor_itf *itf, uint8_t *samples)
  {
      uint8_t reg;
      int rc;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_FIFO_SAMPLES, &reg);
      if (rc) {
          return rc;
      }
  
      *samples = reg & LIS2DW12_FIFO_SAMPLES;
  
      return 0;
  }
  
  /**
   * Clear interrupt pin configuration for interrupt 1
   *
   * @param itf The sensor interface
   * @param cfg int1 config
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_clear_int1_pin_cfg(struct sensor_itf *itf, uint8_t cfg)
  {
      int rc;
      uint8_t reg;
  
      reg = 0;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG4, &reg);
      if (rc) {
          goto err;
      }
  
      reg &= ~cfg;
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG4, reg);
  
  err:
      return rc;
  }
  
  /**
   * Clear interrupt pin configuration for interrupt 2
   *
   * @param itf The sensor interface
   * @param cfg int2 config
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_clear_int2_pin_cfg(struct sensor_itf *itf, uint8_t cfg)
  {
      int rc;
      uint8_t reg;
  
      reg = 0;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG5, &reg);
      if (rc) {
          goto err;
      }
  
      reg &= ~cfg;
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG5, reg);
  
  err:
      return rc;
  }
  
  
  
  /**
   * Set interrupt pin configuration for interrupt 1
   *
   * @param the sensor interface
   * @param config
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_int1_pin_cfg(struct sensor_itf *itf, uint8_t cfg)
  {
      int rc;
      uint8_t reg;
  
      reg = 0;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG4, &reg);
      if (rc) {
          goto err;
      }
  
      reg |= cfg;
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG4, reg);
  
  err:
      return rc;
  }
  
  /**
   * Set interrupt pin configuration for interrupt 2
   *
   * @param the sensor interface
   * @param config
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_set_int2_pin_cfg(struct sensor_itf *itf, uint8_t cfg)
  {
      int rc;
      uint8_t reg;
  
      reg = 0;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG5, &reg);
      if (rc) {
         goto err;
      }
  
      reg |= cfg;
  
      rc = lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG5, reg);
  
  err:
      return rc;
  }
  
  /**
   * Set Wake Up Threshold configuration
   *
   * @param the sensor interface
   * @param wake_up_ths value to set
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_set_wake_up_ths(struct sensor_itf *itf, uint8_t val)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_THS, &reg);
      if (rc) {
          return rc;
      }
  
      reg &= ~LIS2DW12_WAKE_THS_THS;
      reg |= val & LIS2DW12_WAKE_THS_THS;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_WAKE_UP_THS, reg);
  }
  
  /**
   * Get Wake Up Threshold config
   *
   * @param the sensor interface
   * @param ptr to store wake_up_ths value
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_wake_up_ths(struct sensor_itf *itf, uint8_t *val)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_THS, &reg);
      if (rc) {
          return rc;
      }
  
      *val = reg & LIS2DW12_WAKE_THS_THS;
      return 0;
  }
  
  /**
   * Set whether sleep on inactivity is enabled
   *
   * @param the sensor interface
   * @param value to set (0 = disabled, 1 = enabled)
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_set_inactivity_sleep_en(struct sensor_itf *itf, uint8_t en)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_THS, &reg);
      if (rc) {
          return rc;
      }
  
      reg &= ~LIS2DW12_WAKE_THS_SLEEP_ON;
      reg |= en ? LIS2DW12_WAKE_THS_SLEEP_ON : 0;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_WAKE_UP_THS, reg);
  }
  
  /**
   * Get whether sleep on inactivity is enabled
   *
   * @param the sensor interface
   * @param ptr to store read state (0 = disabled, 1 = enabled)
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_inactivity_sleep_en(struct sensor_itf *itf, uint8_t *en)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_THS, &reg);
      if (rc) {
          return rc;
      }
  
      *en = (reg & LIS2DW12_WAKE_THS_SLEEP_ON) ? 1 : 0;
      return 0;
  
  }
  
  /**
   * Set whether double tap event is enabled
   *
   * @param the sensor interface
   * @param value to set (0 = disabled, 1 = enabled)
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_set_double_tap_event_en(struct sensor_itf *itf, uint8_t en)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_THS, &reg);
      if (rc) {
          return rc;
      }
  
      reg &= ~LIS2DW12_WAKE_THS_SINGLE_DOUBLE_TAP;
      reg |= en ? LIS2DW12_WAKE_THS_SINGLE_DOUBLE_TAP : en;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_WAKE_UP_THS, reg);
  }
  
  /**
   * Get whether double tap event is enabled
   *
   * @param the sensor interface
   * @param ptr to store read state (0 = disabled, 1 = enabled)
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_double_tap_event_en(struct sensor_itf *itf, uint8_t *en)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_THS, &reg);
      if (rc) {
          return rc;
      }
  
      *en = (reg & LIS2DW12_WAKE_THS_SINGLE_DOUBLE_TAP) ? 1 : 0;
      return 0;
  
  }
  
  /**
   * Set Wake Up Duration
   *
   * @param the sensor interface
   * @param value to set
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_set_wake_up_dur(struct sensor_itf *itf, uint8_t val)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_DUR, &reg);
      if (rc) {
          return rc;
      }
  
      reg &= ~LIS2DW12_WAKE_DUR_DUR;
      reg |= (val & LIS2DW12_WAKE_DUR_DUR) << 5;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_WAKE_UP_DUR, reg);
  
  }
  
  /**
   * Get Wake Up Duration
   *
   * @param the sensor interface
   * @param ptr to store wake_up_dur value
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_wake_up_dur(struct sensor_itf *itf, uint8_t *val)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_DUR, &reg);
      if (rc) {
          return rc;
      }
  
      *val = (reg & LIS2DW12_WAKE_DUR_DUR) >> 5;
      return 0;
  }
  
  /**
   * Set Sleep Duration
   *
   * @param the sensor interface
   * @param value to set
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_set_sleep_dur(struct sensor_itf *itf, uint8_t val)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_DUR, &reg);
      if (rc) {
          return rc;
      }
  
      reg &= ~LIS2DW12_WAKE_DUR_SLEEP_DUR;
      reg |= (val & LIS2DW12_WAKE_DUR_SLEEP_DUR);
  
      return lis2dw12_write8(itf, LIS2DW12_REG_WAKE_UP_DUR, reg);
  }
  
  /**
   * Get Sleep Duration
   *
   * @param the sensor interface
   * @param ptr to store sleep_dur value
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_sleep_dur(struct sensor_itf *itf, uint8_t *val)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_DUR, &reg);
      if (rc) {
          return rc;
      }
  
      *val = reg & LIS2DW12_WAKE_DUR_SLEEP_DUR;
      return 0;
  }
  
  /**
   * Set Stationary Detection Enable
   *
   * @param the sensor interface
   * @param value to set
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_set_stationary_en(struct sensor_itf *itf, uint8_t en)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_DUR, &reg);
      if (rc) {
          return rc;
      }
  
      reg &= ~LIS2DW12_WAKE_DUR_STATIONARY;
      reg |= en ? LIS2DW12_WAKE_DUR_STATIONARY : 0;
  
      return lis2dw12_write8(itf, LIS2DW12_REG_WAKE_UP_DUR, reg);
  
  }
  
  /**
   * Get Stationary Detection Enable
   *
   * @param the sensor interface
   * @param ptr to store sleep_dur value
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_stationary_en(struct sensor_itf *itf, uint8_t *en)
  {
      int rc;
      uint8_t reg;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_DUR, &reg);
      if (rc) {
          return rc;
      }
  
      *en = (reg & LIS2DW12_WAKE_DUR_STATIONARY) ? 1 : 0;
      return 0;
  }
  
  /**
   * Clear interrupt 1
   *
   * @param the sensor interface
   */
  int
  lis2dw12_clear_int(struct sensor_itf *itf, uint8_t *src)
  {
      return lis2dw12_read8(itf, LIS2DW12_REG_INT_SRC, src);
  }
  
  /**
   * Get Interrupt Status
   *
   * @param the sensor interface
   * @param pointer to return interrupt status in
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_int_status(struct sensor_itf *itf, uint8_t *status)
  {
      return lis2dw12_read8(itf, LIS2DW12_REG_STATUS_REG, status);
  }
  
  /**
   * Get Wake Up Source
   *
   * @param the sensor interface
   * @param pointer to return wake_up_src in
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_wake_up_src(struct sensor_itf *itf, uint8_t *status)
  {
      return lis2dw12_read8(itf, LIS2DW12_REG_WAKE_UP_SRC, status);
  }
  
  /**
   * Get Tap Source
   *
   * @param the sensor interface
   * @param pointer to return tap_src in
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_tap_src(struct sensor_itf *itf, uint8_t *status)
  {
      return lis2dw12_read8(itf, LIS2DW12_REG_TAP_SRC, status);
  }
  
  /**
   * Get 6D Source
   *
   * @param the sensor interface
   * @param pointer to return sixd_src in
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_sixd_src(struct sensor_itf *itf, uint8_t *status)
  {
      return lis2dw12_read8(itf, LIS2DW12_REG_SIXD_SRC, status);
  }
  
  
  /**
   * Set whether interrupts are enabled
   *
   * @param the sensor interface
   * @param value to set (0 = disabled, 1 = enabled)
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_set_int_enable(struct sensor_itf *itf, uint8_t enabled)
  {
      uint8_t reg;
      int rc;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG7, &reg);
      if (rc) {
          return rc;
      }
  
      if (enabled) {
          reg |= LIS2DW12_CTRL_REG7_INT_EN;
      } else {
          reg &= ~LIS2DW12_CTRL_REG7_INT_EN;
      }
  
      return lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG7, reg);
  }
  
  /**
   * Set whether interrupt 2 signals is mapped onto interrupt 1 pin
   *
   * @param the sensor interface
   * @param value to set (false = disabled, true = enabled)
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_set_int2_on_int1_map(struct sensor_itf *itf, bool enable)
  {
      uint8_t reg;
      int rc;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG7, &reg);
      if (rc) {
          return rc;
      }
  
      if (enable) {
          reg |= LIS2DW12_CTRL_REG7_INT2_ON_INT1;
      } else {
          reg &= ~LIS2DW12_CTRL_REG7_INT2_ON_INT1;
      }
  
      return lis2dw12_write8(itf, LIS2DW12_REG_CTRL_REG7, reg);
  }
  
  /**
   * Get whether interrupt 1 signals is mapped onto interrupt 2 pin
   *
   * @param the sensor interface
   * @param value to set (0 = disabled, 1 = enabled)
   * @return 0 on success, non-zero on failure
   */
  int lis2dw12_get_int1_on_int2_map(struct sensor_itf *itf, uint8_t *val)
  {
      uint8_t reg;
      int rc;
  
      rc = lis2dw12_read8(itf, LIS2DW12_REG_CTRL_REG7, &reg);
      if (rc) {
          return rc;
      }
  
      *val = (reg & LIS2DW12_CTRL_REG7_INT2_ON_INT1) >> 6;
      return 0;
  }
  
  /**
   * Run Self test on sensor
   *
   * @param the sensor interface
   * @param pointer to return test result in (0 on pass, non-zero on failure)
   *
   * @return 0 on sucess, non-zero on failure
   */
  int lis2dw12_run_self_test(struct sensor_itf *itf, int *result)
  {
      int rc;
      /*configure min and max values for reading 5 samples, and accounting for
       * both negative and positive offset */
      int min = LIS2DW12_ST_MIN*5*2;
      int max = LIS2DW12_ST_MAX*5*2;
  
      int16_t data[3], diff[3] = {0,0,0};
      int i;
      uint8_t prev_config[6];
      /* set config per datasheet, with positive self test mode enabled. */
      uint8_t st_config[] = {0x44, 0x04, 0x40, 0x00, 0x00, 0x10};
  
      rc = lis2dw12_readlen(itf, LIS2DW12_REG_CTRL_REG1, prev_config, 6);
      if (rc) {
          return rc;
      }
      rc = lis2dw12_writelen(itf, LIS2DW12_REG_CTRL_REG2, &st_config[1], 5);
      rc = lis2dw12_writelen(itf, LIS2DW12_REG_CTRL_REG1, st_config, 1);
      if (rc) {
          return rc;
      }
  
      /* go into self test mode 1 */
      rc = lis2dw12_set_self_test(itf, LIS2DW12_ST_MODE_MODE1);
      if (rc) {
          return rc;
      }
  
      /* wait 100ms */
      os_time_delay(OS_TICKS_PER_SEC / 100);
      rc = lis2dw12_get_data(itf, 2, &(data[0]), &(data[1]), &(data[2]));
      if (rc) {
          return rc;
      }
  
      /* take positive offset reading */
      for(i=0; i<5; i++) {
  
          rc = lis2dw12_get_data(itf, 2, &(data[0]), &(data[1]), &(data[2]));
          if (rc) {
              return rc;
          }
          diff[0] += data[0];
          diff[1] += data[1];
          diff[2] += data[2];
          /* wait at least 20 ms */
          os_time_delay(OS_TICKS_PER_SEC / 50 + 1);
      }
  
      /* go into self test mode 2 */
      rc = lis2dw12_set_self_test(itf, LIS2DW12_ST_MODE_MODE2);
      if (rc) {
          return rc;
      }
  
      os_time_delay(OS_TICKS_PER_SEC / 50 + 1);
      rc = lis2dw12_get_data(itf, 2, &(data[0]), &(data[1]), &(data[2]));
      if (rc) {
          return rc;
      }
  
      /* take negative offset reading */
      for (i=0; i<5; i++) {
  
              rc = lis2dw12_get_data(itf, 2, &(data[0]), &(data[1]), &(data[2]));
              if (rc) {
                  return rc;
              }
              diff[0] -= data[0];
              diff[1] -= data[1];
              diff[2] -= data[2];
              /* wait at least 20 ms */
              os_time_delay(OS_TICKS_PER_SEC / 50 + 1);
          }
  
      /* disable self test mod */
      rc = lis2dw12_writelen(itf, LIS2DW12_REG_CTRL_REG1, prev_config, 6);
          if (rc) {
              return rc;
          }
  
      /* compare values to thresholds */
      *result = 0;
      for (i = 0; i < 3; i++) {
          if ((diff[i] < min) || (diff[i] > max)) {
              *result -= 1;
          }
      }
  
      return 0;
  }
  
  
  static void
  init_interrupt(struct lis2dw12_int *interrupt, struct sensor_int *ints)
  {
      os_error_t error;
  
      error = os_sem_init(&interrupt->wait, 0);
      assert(error == OS_OK);
  
      interrupt->active = false;
      interrupt->asleep = false;
      interrupt->ints = ints;
  }
  
  static void
  undo_interrupt(struct lis2dw12_int * interrupt)
  {
      OS_ENTER_CRITICAL(interrupt->lock);
      interrupt->active = false;
      interrupt->asleep = false;
      OS_EXIT_CRITICAL(interrupt->lock);
  }
  
  static int
  wait_interrupt(struct lis2dw12_int *interrupt, uint8_t int_num)
  {
      bool wait;
      os_error_t error;
  
      OS_ENTER_CRITICAL(interrupt->lock);
  
      /* Check if we did not missed interrupt */
      if (hal_gpio_read(interrupt->ints[int_num].host_pin) ==
                                              interrupt->ints[int_num].active) {
          OS_EXIT_CRITICAL(interrupt->lock);
          return OS_OK;
      }
  
      if (interrupt->active) {
          interrupt->active = false;
          wait = false;
      } else {
          interrupt->asleep = true;
          wait = true;
      }
      OS_EXIT_CRITICAL(interrupt->lock);
  
      if (wait) {
          error = os_sem_pend(&interrupt->wait, LIS2DW12_MAX_INT_WAIT);
          if (error == OS_TIMEOUT) {
              return error;
          }
          assert(error == OS_OK);
      }
      return OS_OK;
  }
  
  static void
  wake_interrupt(struct lis2dw12_int *interrupt)
  {
      bool wake;
  
      OS_ENTER_CRITICAL(interrupt->lock);
      if (interrupt->asleep) {
          interrupt->asleep = false;
          wake = true;
      } else {
          interrupt->active = true;
          wake = false;
      }
      OS_EXIT_CRITICAL(interrupt->lock);
  
      if (wake) {
          os_error_t error;
  
          error = os_sem_release(&interrupt->wait);
          assert(error == OS_OK);
      }
  }
  
  static void
  lis2dw12_int_irq_handler(void *arg)
  {
      struct sensor *sensor = arg;
      struct lis2dw12 *lis2dw12;
  
      lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
  
      if(lis2dw12->pdd.interrupt) {
          wake_interrupt(lis2dw12->pdd.interrupt);
      }
  
      sensor_mgr_put_interrupt_evt(sensor);
  }
  
  static int
  init_intpin(struct lis2dw12 *lis2dw12, hal_gpio_irq_handler_t handler,
              void *arg)
  {
      hal_gpio_irq_trig_t trig;
      int pin = -1;
      int rc;
      int i;
  
      for (i = 0; i < MYNEWT_VAL(SENSOR_MAX_INTERRUPTS_PINS); i++){
          pin = lis2dw12->sensor.s_itf.si_ints[i].host_pin;
          if (pin >= 0) {
              break;
          }
      }
  
      if (pin < 0) {
          LIS2DW12_LOG(ERROR, "Interrupt pin not configured\n");
          return SYS_EINVAL;
      }
  
      if (lis2dw12->sensor.s_itf.si_ints[i].active) {
          trig = HAL_GPIO_TRIG_RISING;
      } else {
          trig = HAL_GPIO_TRIG_FALLING;
      }
  
      rc = hal_gpio_irq_init(pin,
                             handler,
                             arg,
                             trig,
                             HAL_GPIO_PULL_NONE);
      if (rc != 0) {
          LIS2DW12_LOG(ERROR, "Failed to initialise interrupt pin %d\n", pin);
          return rc;
      }
  
      return 0;
  }
  
  static int
  disable_interrupt(struct sensor *sensor, uint8_t int_to_disable, uint8_t int_num)
  {
      struct lis2dw12 *lis2dw12;
      struct lis2dw12_pdd *pdd;
      struct sensor_itf *itf;
      int rc;
  
      if (int_to_disable == 0) {
          return SYS_EINVAL;
      }
  
      lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
      itf = SENSOR_GET_ITF(sensor);
      pdd = &lis2dw12->pdd;
  
      pdd->int_enable &= ~(int_to_disable << (int_num * 8));
  
      /* disable int pin */
      if (!pdd->int_enable) {
          hal_gpio_irq_disable(itf->si_ints[int_num].host_pin);
          /* disable interrupt in device */
          rc = lis2dw12_set_int_enable(itf, 0);
          if (rc) {
              pdd->int_enable |= (int_to_disable << (int_num * 8));
              return rc;
          }
      }
  
      /* update interrupt setup in device */
      if (int_num == 0) {
          rc = lis2dw12_clear_int1_pin_cfg(itf, int_to_disable);
      } else {
          rc = lis2dw12_clear_int2_pin_cfg(itf, int_to_disable);
      }
  
      return rc;
  }
  
  
  static int
  enable_interrupt(struct sensor *sensor, uint8_t int_to_enable, uint8_t int_num)
  {
      struct lis2dw12 *lis2dw12;
      struct lis2dw12_pdd *pdd;
      struct sensor_itf *itf;
      uint8_t reg;
      int rc;
  
      if (!int_to_enable) {
          rc = SYS_EINVAL;
          goto err;
      }
  
      lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
      itf = SENSOR_GET_ITF(sensor);
      pdd = &lis2dw12->pdd;
  
      rc = lis2dw12_clear_int(itf, &reg);
      if (rc) {
          goto err;
      }
  
      /* if no interrupts are currently in use enable int pin */
      if (!pdd->int_enable) {
          hal_gpio_irq_enable(itf->si_ints[int_num].host_pin);
  
          rc = lis2dw12_set_int_enable(itf, 1);
          if (rc) {
              goto err;
          }
      }
  
      pdd->int_enable |= (int_to_enable << (int_num * 8));
  
      /* enable interrupt in device */
      if (int_num == 0) {
          rc = lis2dw12_set_int1_pin_cfg(itf, int_to_enable);
      } else {
          rc = lis2dw12_set_int2_pin_cfg(itf, int_to_enable);
      }
  
      if (rc) {
          disable_interrupt(sensor, int_to_enable, int_num);
          goto err;
      }
  
      return 0;
  err:
      return rc;
  }
  
  int
  lis2dw12_get_fs(struct sensor_itf *itf, uint8_t *fs)
  {
      int rc;
  
      rc = lis2dw12_get_full_scale(itf, fs);
      if (rc) {
          return rc;
      }
  
      if (*fs == LIS2DW12_FS_2G) {
          *fs = 2;
      } else if (*fs == LIS2DW12_FS_4G) {
          *fs = 4;
      } else if (*fs == LIS2DW12_FS_8G) {
          *fs = 8;
      } else if (*fs == LIS2DW12_FS_16G) {
          *fs = 16;
      } else {
          return SYS_EINVAL;
      }
  
      return 0;
  }
  
  /**
   * Gets a new data sample from the sensor.
   *
   * @param The sensor interface
   * @param x axis data
   * @param y axis data
   * @param z axis data
   *
   * @return 0 on success, non-zero on failure
   */
  int
  lis2dw12_get_data(struct sensor_itf *itf, uint8_t fs, int16_t *x, int16_t *y, int16_t *z)
  {
      int rc;
      uint8_t payload[6] = {0};
  
      *x = *y = *z = 0;
  
      rc = lis2dw12_readlen(itf, LIS2DW12_REG_OUT_X_L, payload, 6);
      if (rc) {
          goto err;
      }
  
      *x = payload[0] | (payload[1] << 8);
      *y = payload[2] | (payload[3] << 8);
      *z = payload[4] | (payload[5] << 8);
  
      /*
       * Since full scale is +/-(fs)g,
       * fs should be multiplied by 2 to account for full scale.
       * To calculate mg from g we use the 1000 multiple.
       * Since the full scale is represented by 16 bit value,
       * we use that as a divisor.
       */
      *x = (fs * 2 * 1000 * *x)/UINT16_MAX;
      *y = (fs * 2 * 1000 * *y)/UINT16_MAX;
      *z = (fs * 2 * 1000 * *z)/UINT16_MAX;
  
      return 0;
  err:
      return rc;
  }
  
  static int lis2dw12_do_read(struct sensor *sensor, sensor_data_func_t data_func,
                              void * data_arg, uint8_t fs)
  {
      struct sensor_accel_data sad;
      struct sensor_itf *itf;
      int16_t x, y ,z;
      float fx, fy ,fz;
      int rc;
  
      itf = SENSOR_GET_ITF(sensor);
  
      x = y = z = 0;
  
      rc = lis2dw12_get_data(itf, fs, &x, &y, &z);
      if (rc) {
          goto err;
      }
  
      /* converting values from mg to ms^2 */
      lis2dw12_calc_acc_ms2(x, &fx);
      lis2dw12_calc_acc_ms2(y, &fy);
      lis2dw12_calc_acc_ms2(z, &fz);
  
      sad.sad_x = fx;
      sad.sad_y = fy;
      sad.sad_z = fz;
  
      sad.sad_x_is_valid = 1;
      sad.sad_y_is_valid = 1;
      sad.sad_z_is_valid = 1;
  
      /* Call data function */
      rc = data_func(sensor, data_arg, &sad, SENSOR_TYPE_ACCELEROMETER);
      if (rc != 0) {
          goto err;
      }
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Do accelerometer polling reads
   *
   * @param The sensor ptr
   * @param The sensor type
   * @param The function pointer to invoke for each accelerometer reading.
   * @param The opaque pointer that will be passed in to the function.
   * @param If non-zero, how long the stream should run in milliseconds.
   *
   * @return 0 on success, non-zero on failure.
   */
  int
  lis2dw12_poll_read(struct sensor *sensor, sensor_type_t sensor_type,
                     sensor_data_func_t data_func, void *data_arg,
                     uint32_t timeout)
  {
      struct lis2dw12 *lis2dw12;
      struct lis2dw12_cfg *cfg;
      struct sensor_itf *itf;
      uint8_t fs;
      int rc;
  
      lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
      itf = SENSOR_GET_ITF(sensor);
      cfg = &lis2dw12->cfg;
  
      /* If the read isn't looking for accel data, don't do anything. */
      if (!(sensor_type & SENSOR_TYPE_ACCELEROMETER)) {
          rc = SYS_EINVAL;
          goto err;
      }
  
      if (cfg->read_mode.mode != LIS2DW12_READ_M_POLL) {
          rc = SYS_EINVAL;
          goto err;
      }
  
      rc = lis2dw12_get_fs(itf, &fs);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_do_read(sensor, data_func, data_arg, fs);
      if (rc) {
          goto err;
      }
  
      return 0;
  err:
      return rc;
  }
  
  int
  lis2dw12_stream_read(struct sensor *sensor,
                       sensor_type_t sensor_type,
                       sensor_data_func_t read_func,
                       void *read_arg,
                       uint32_t time_ms)
  {
      struct lis2dw12_pdd *pdd;
      struct lis2dw12 *lis2dw12;
      struct sensor_itf *itf;
      struct lis2dw12_cfg *cfg;
      os_time_t time_ticks;
      os_time_t stop_ticks = 0;
      uint8_t fifo_samples;
      uint8_t fs;
      int rc, rc2;
  
      /* If the read isn't looking for accel data, don't do anything. */
      if (!(sensor_type & SENSOR_TYPE_ACCELEROMETER)) {
          return SYS_EINVAL;
      }
  
      lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
      itf = SENSOR_GET_ITF(sensor);
      pdd = &lis2dw12->pdd;
      cfg = &lis2dw12->cfg;
  
      if (cfg->read_mode.mode != LIS2DW12_READ_M_STREAM) {
          return SYS_EINVAL;
      }
  
      undo_interrupt(&lis2dw12->intr);
  
      if (pdd->interrupt) {
          return SYS_EBUSY;
      }
  
      /* enable interrupt */
      pdd->interrupt = &lis2dw12->intr;
  
      rc = enable_interrupt(sensor, cfg->read_mode.int_cfg,
                            cfg->read_mode.int_num);
      if (rc) {
          return rc;
      }
  
      if (time_ms != 0) {
          rc = os_time_ms_to_ticks(time_ms, &time_ticks);
          if (rc) {
              goto err;
          }
          stop_ticks = os_time_get() + time_ticks;
      }
  
      rc = lis2dw12_get_fs(itf, &fs);
      if (rc) {
          goto err;
      }
  
      for (;;) {
          /* force at least one read for cases when fifo is disabled */
          rc = wait_interrupt(&lis2dw12->intr, cfg->read_mode.int_num);
          if (rc) {
              goto err;
          }
          fifo_samples = 1;
  
          while(fifo_samples > 0) {
  
              /* read all data we beleive is currently in fifo */
              while(fifo_samples > 0) {
                  rc = lis2dw12_do_read(sensor, read_func, read_arg, fs);
                  if (rc) {
                      goto err;
                  }
                  fifo_samples--;
  
              }
  
              /* check if any data is available in fifo */
              rc = lis2dw12_get_fifo_samples(itf, &fifo_samples);
              if (rc) {
                  goto err;
              }
  
          }
  
          if (time_ms != 0 && OS_TIME_TICK_GT(os_time_get(), stop_ticks)) {
              break;
          }
  
      }
  
  err:
      /* disable interrupt */
      pdd->interrupt = NULL;
      rc2 = disable_interrupt(sensor, cfg->read_mode.int_cfg,
                             cfg->read_mode.int_num);
      if (rc) {
          return rc;
      } else {
          return rc2;
      }
  }
  
  static int
  lis2dw12_sensor_read(struct sensor *sensor, sensor_type_t type,
          sensor_data_func_t data_func, void *data_arg, uint32_t timeout)
  {
      int rc;
      const struct lis2dw12_cfg *cfg;
      struct lis2dw12 *lis2dw12;
      struct sensor_itf *itf;
  
      /* If the read isn't looking for accel data, don't do anything. */
      if (!(type & SENSOR_TYPE_ACCELEROMETER)) {
          rc = SYS_EINVAL;
          goto err;
      }
  
      itf = SENSOR_GET_ITF(sensor);
  
      if (itf->si_type == SENSOR_ITF_SPI) {
  
          rc = hal_spi_disable(sensor->s_itf.si_num);
          if (rc) {
              goto err;
          }
  
          rc = hal_spi_config(sensor->s_itf.si_num, &spi_lis2dw12_settings);
          if (rc == EINVAL) {
              /* If spi is already enabled, for nrf52, it returns -1, We should not
               * fail if the spi is already enabled
               */
              goto err;
          }
  
          rc = hal_spi_enable(sensor->s_itf.si_num);
          if (rc) {
              goto err;
          }
      }
  
      lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
      cfg = &lis2dw12->cfg;
  
      if (cfg->read_mode.mode == LIS2DW12_READ_M_POLL) {
          rc = lis2dw12_poll_read(sensor, type, data_func, data_arg, timeout);
      } else {
          rc = lis2dw12_stream_read(sensor, type, data_func, data_arg, timeout);
      }
  err:
      if (rc) {
          return SYS_EINVAL; /* XXX */
      } else {
          return SYS_EOK;
      }
  }
  
  static struct lis2dw12_notif_cfg *
  lis2dw12_find_notif_cfg_by_event(sensor_event_type_t event,
                                   struct lis2dw12_cfg *cfg)
  {
      int i;
      struct lis2dw12_notif_cfg *notif_cfg = NULL;
  
      if (!cfg) {
          goto err;
      }
  
      for (i = 0; i < cfg->max_num_notif; i++) {
          if (event == cfg->notif_cfg[i].event) {
              notif_cfg = &cfg->notif_cfg[i];
              break;
          }
      }
  
      if (i == cfg->max_num_notif) {
         /* here if type is set to a non valid event or more than one event
          * we do not currently support registering for more than one event
          * per notification
          */
          goto err;
      }
  
      return notif_cfg;
  err:
      return NULL;
  }
  
  static int
  lis2dw12_sensor_set_notification(struct sensor *sensor, sensor_event_type_t event)
  {
      struct lis2dw12 *lis2dw12;
      struct lis2dw12_pdd *pdd;
      struct sensor_itf *itf;
      struct lis2dw12_notif_cfg *notif_cfg;
      int rc;
  
      lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
      itf = SENSOR_GET_ITF(sensor);
      pdd = &lis2dw12->pdd;
  
      notif_cfg = lis2dw12_find_notif_cfg_by_event(event, &lis2dw12->cfg);
      if (!notif_cfg) {
          rc = SYS_EINVAL;
          goto err;
      }
  
      rc = enable_interrupt(sensor, notif_cfg->int_cfg, notif_cfg->int_num);
      if (rc) {
          goto err;
      }
  
      /* enable double tap detection in wake_up_ths */
      if(event == SENSOR_EVENT_TYPE_DOUBLE_TAP) {
          rc = lis2dw12_set_double_tap_event_en(itf, 1);
          if (rc) {
              goto err;
          }
      }
  
      pdd->notify_ctx.snec_evtype |= event;
  
      return 0;
  err:
      return rc;
  }
  
  static int
  lis2dw12_sensor_unset_notification(struct sensor *sensor, sensor_event_type_t event)
  {
      struct lis2dw12_notif_cfg *notif_cfg;
      struct lis2dw12 *lis2dw12;
      struct sensor_itf *itf;
      int rc;
  
      lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
      itf = SENSOR_GET_ITF(sensor);
  
      lis2dw12->pdd.notify_ctx.snec_evtype &= ~event;
  
      if(event == SENSOR_EVENT_TYPE_DOUBLE_TAP) {
          rc = lis2dw12_set_double_tap_event_en(itf, 0);
          if (rc) {
              goto err;
          }
      }
  
      notif_cfg = lis2dw12_find_notif_cfg_by_event(event, &lis2dw12->cfg);
      if (!notif_cfg) {
          rc = SYS_EINVAL;
          goto err;
      }
  
      rc = disable_interrupt(sensor, notif_cfg->int_cfg, notif_cfg->int_num);
  
  err:
      return rc;
  }
  
  static int
  lis2dw12_sensor_set_config(struct sensor *sensor, void *cfg)
  {
      struct lis2dw12 *lis2dw12;
  
      lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
  
      return lis2dw12_config(lis2dw12, (struct lis2dw12_cfg*)cfg);
  }
  
  static void
  lis2dw12_inc_notif_stats(sensor_event_type_t event)
  {
  
  #if MYNEWT_VAL(LIS2DW12_NOTIF_STATS)
      switch (event) {
          case SENSOR_EVENT_TYPE_SLEEP:
              STATS_INC(g_lis2dw12stats, sleep_notify);
              break;
          case SENSOR_EVENT_TYPE_SINGLE_TAP:
              STATS_INC(g_lis2dw12stats, single_tap_notify);
              break;
          case SENSOR_EVENT_TYPE_DOUBLE_TAP:
              STATS_INC(g_lis2dw12stats, double_tap_notify);
              break;
          case SENSOR_EVENT_TYPE_ORIENT_CHANGE:
              STATS_INC(g_lis2dw12stats, orient_chg_notify);
              break;
          case SENSOR_EVENT_TYPE_ORIENT_X_L_CHANGE:
              STATS_INC(g_lis2dw12stats, orient_chg_x_l_notify);
              break;
          case SENSOR_EVENT_TYPE_ORIENT_Y_L_CHANGE:
              STATS_INC(g_lis2dw12stats, orient_chg_y_l_notify);
              break;
          case SENSOR_EVENT_TYPE_ORIENT_Z_L_CHANGE:
              STATS_INC(g_lis2dw12stats, orient_chg_z_l_notify);
              break;
          case SENSOR_EVENT_TYPE_ORIENT_X_H_CHANGE:
              STATS_INC(g_lis2dw12stats, orient_chg_x_h_notify);
              break;
          case SENSOR_EVENT_TYPE_ORIENT_Y_H_CHANGE:
              STATS_INC(g_lis2dw12stats, orient_chg_y_h_notify);
              break;
          case SENSOR_EVENT_TYPE_ORIENT_Z_H_CHANGE:
              STATS_INC(g_lis2dw12stats, orient_chg_z_h_notify);
              break;
          case SENSOR_EVENT_TYPE_SLEEP_CHANGE:
              STATS_INC(g_lis2dw12stats, sleep_chg_notify);
              break;
          case SENSOR_EVENT_TYPE_WAKEUP:
              STATS_INC(g_lis2dw12stats, wakeup_notify);
              break;
          case SENSOR_EVENT_TYPE_FREE_FALL:
              STATS_INC(g_lis2dw12stats, free_fall_notify);
              break;
          default:
              break;
      }
  #endif
  
      return;
  }
  
  static int
  lis2dw12_notify(struct lis2dw12 *lis2dw12, uint8_t src,
                      sensor_event_type_t event_type)
  {
      struct lis2dw12_notif_cfg *notif_cfg;
  
      notif_cfg = lis2dw12_find_notif_cfg_by_event(event_type, &lis2dw12->cfg);
      if (!notif_cfg) {
          return SYS_EINVAL;
      }
  
      if (src & notif_cfg->notif_src) {
          sensor_mgr_put_notify_evt(&lis2dw12->pdd.notify_ctx, event_type);
          lis2dw12_inc_notif_stats(event_type);
      }
  
      return 0;
  }
  
  static int
  lis2dw12_sensor_handle_interrupt(struct sensor *sensor)
  {
      struct lis2dw12 *lis2dw12;
      struct sensor_itf *itf;
      uint8_t int_src;
      uint8_t int_status;
      uint8_t sixd_src;
      int rc;
  
      lis2dw12 = (struct lis2dw12 *)SENSOR_GET_DEVICE(sensor);
      itf = SENSOR_GET_ITF(sensor);
  
      if (lis2dw12->pdd.notify_ctx.snec_evtype & SENSOR_EVENT_TYPE_SLEEP) {
          /*
           * We need to read this register only if we are
           * interested in the sleep event
           */
          rc = lis2dw12_get_int_status(itf, &int_status);
          if (rc) {
              LIS2DW12_LOG(ERROR, "Could not read int status err=0x%02x\n", rc);
              return rc;
          }
  
          rc = lis2dw12_notify(lis2dw12, int_status, SENSOR_EVENT_TYPE_SLEEP);
          if (rc) {
              goto err;
          }
      }
  
      rc = lis2dw12_get_sixd_src(itf, &sixd_src);
      if (rc) {
          LIS2DW12_LOG(ERROR, "Could not read sixd src err=0x%02x\n", rc);
          goto err;
      }
  
      rc = lis2dw12_notify(lis2dw12, sixd_src, SENSOR_EVENT_TYPE_ORIENT_CHANGE);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_notify(lis2dw12, sixd_src, SENSOR_EVENT_TYPE_ORIENT_X_L_CHANGE);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_notify(lis2dw12, sixd_src, SENSOR_EVENT_TYPE_ORIENT_Y_L_CHANGE);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_notify(lis2dw12, sixd_src, SENSOR_EVENT_TYPE_ORIENT_Z_L_CHANGE);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_notify(lis2dw12, sixd_src, SENSOR_EVENT_TYPE_ORIENT_X_H_CHANGE);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_notify(lis2dw12, sixd_src, SENSOR_EVENT_TYPE_ORIENT_Y_H_CHANGE);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_notify(lis2dw12, sixd_src, SENSOR_EVENT_TYPE_ORIENT_Z_H_CHANGE);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_clear_int(itf, &int_src);
      if (rc) {
          LIS2DW12_LOG(ERROR, "Could not read int src err=0x%02x\n", rc);
          return rc;
      }
  
      rc = lis2dw12_notify(lis2dw12, int_src, SENSOR_EVENT_TYPE_SINGLE_TAP);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_notify(lis2dw12, int_src, SENSOR_EVENT_TYPE_DOUBLE_TAP);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_notify(lis2dw12, int_src, SENSOR_EVENT_TYPE_FREE_FALL);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_notify(lis2dw12, int_src, SENSOR_EVENT_TYPE_WAKEUP);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_notify(lis2dw12, int_src, SENSOR_EVENT_TYPE_SLEEP_CHANGE);
      if (rc) {
          goto err;
      }
  
      return 0;
  err:
      return rc;
  }
  
  static int
  lis2dw12_sensor_get_config(struct sensor *sensor, sensor_type_t type,
          struct sensor_cfg *cfg)
  {
      int rc;
  
      if (type != SENSOR_TYPE_ACCELEROMETER) {
          rc = SYS_EINVAL;
          goto err;
      }
  
      cfg->sc_valtype = SENSOR_VALUE_TYPE_FLOAT_TRIPLET;
  
      return 0;
  err:
      return rc;
  }
  
  /**
   * Expects to be called back through os_dev_create().
   *
   * @param The device object associated with this accelerometer
   * @param Argument passed to OS device init, unused
   *
   * @return 0 on success, non-zero error on failure.
   */
  int
  lis2dw12_init(struct os_dev *dev, void *arg)
  {
      struct lis2dw12 *lis2dw12;
      struct sensor *sensor;
      int rc;
  
      if (!arg || !dev) {
          rc = SYS_ENODEV;
          goto err;
      }
  
      lis2dw12 = (struct lis2dw12 *) dev;
  
      lis2dw12->cfg.mask = SENSOR_TYPE_ALL;
  
      sensor = &lis2dw12->sensor;
  
      /* Initialise the stats entry */
      rc = stats_init(
          STATS_HDR(g_lis2dw12stats),
          STATS_SIZE_INIT_PARMS(g_lis2dw12stats, STATS_SIZE_32),
          STATS_NAME_INIT_PARMS(lis2dw12_stat_section));
      SYSINIT_PANIC_ASSERT(rc == 0);
      /* Register the entry with the stats registry */
      rc = stats_register(dev->od_name, STATS_HDR(g_lis2dw12stats));
      SYSINIT_PANIC_ASSERT(rc == 0);
  
      rc = sensor_init(sensor, dev);
      if (rc) {
          goto err;
      }
  
      /* Add the light driver */
      rc = sensor_set_driver(sensor, SENSOR_TYPE_ACCELEROMETER,
              (struct sensor_driver *) &g_lis2dw12_sensor_driver);
      if (rc) {
          goto err;
      }
  
      /* Set the interface */
      rc = sensor_set_interface(sensor, arg);
      if (rc) {
          goto err;
      }
  
      rc = sensor_mgr_register(sensor);
      if (rc) {
          goto err;
      }
  
      if (sensor->s_itf.si_type == SENSOR_ITF_SPI) {
  
          rc = hal_spi_disable(sensor->s_itf.si_num);
          if (rc) {
              goto err;
          }
  
          rc = hal_spi_config(sensor->s_itf.si_num, &spi_lis2dw12_settings);
          if (rc == EINVAL) {
              /* If spi is already enabled, for nrf52, it returns -1, We should not
               * fail if the spi is already enabled
               */
              goto err;
          }
  
          rc = hal_spi_enable(sensor->s_itf.si_num);
          if (rc) {
              goto err;
          }
  
          rc = hal_gpio_init_out(sensor->s_itf.si_cs_pin, 1);
          if (rc) {
              goto err;
          }
      }
  
  
      init_interrupt(&lis2dw12->intr, lis2dw12->sensor.s_itf.si_ints);
  
      lis2dw12->pdd.notify_ctx.snec_sensor = sensor;
      lis2dw12->pdd.interrupt = NULL;
  
      rc = init_intpin(lis2dw12, lis2dw12_int_irq_handler, sensor);
      if (rc) {
          return rc;
      }
  
      return 0;
  err:
      return rc;
  
  }
  
  /**
   * Configure the sensor
   *
   * @param ptr to sensor driver
   * @param ptr to sensor driver config
   */
  int
  lis2dw12_config(struct lis2dw12 *lis2dw12, struct lis2dw12_cfg *cfg)
  {
      int rc;
      struct sensor_itf *itf;
      uint8_t chip_id;
      struct sensor *sensor;
  
      itf = SENSOR_GET_ITF(&(lis2dw12->sensor));
      sensor = &(lis2dw12->sensor);
  
      if (itf->si_type == SENSOR_ITF_SPI) {
  
          rc = hal_spi_disable(sensor->s_itf.si_num);
          if (rc) {
              goto err;
          }
  
          rc = hal_spi_config(sensor->s_itf.si_num, &spi_lis2dw12_settings);
          if (rc == EINVAL) {
              /* If spi is already enabled, for nrf52, it returns -1, We should not
               * fail if the spi is already enabled
               */
              goto err;
          }
  
          rc = hal_spi_enable(sensor->s_itf.si_num);
          if (rc) {
              goto err;
          }
      }
  
      rc = lis2dw12_get_chip_id(itf, &chip_id);
      if (rc) {
          goto err;
      }
  
      if (chip_id != LIS2DW12_ID) {
          rc = SYS_EINVAL;
          goto err;
      }
  
      rc = lis2dw12_reset(itf);
      if (rc) {
          goto err;
      }
  
      rc = lis2dw12_set_int_pp_od(itf, cfg->int_pp_od);
      if (rc) {
          goto err;
      }
      lis2dw12->cfg.int_pp_od = cfg->int_pp_od;
  
      rc = lis2dw12_set_latched_int(itf, cfg->int_latched);
      if (rc) {
          goto err;
      }
      lis2dw12->cfg.int_latched = cfg->int_latched;
  
      rc = lis2dw12_set_int_active_low(itf, cfg->int_active_low);
      if (rc) {
          goto err;
      }
      lis2dw12->cfg.int_active_low = cfg->int_active_low;
  
      rc = lis2dw12_set_slp_mode(itf, cfg->slp_mode);
      if (rc) {
          goto err;
      }
      lis2dw12->cfg.slp_mode = cfg->slp_mode;
  
      rc = lis2dw12_set_offsets(itf, cfg->offset_x, cfg->offset_y, cfg->offset_z,
                                cfg->offset_weight);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.offset_x = cfg->offset_x;
      lis2dw12->cfg.offset_y = cfg->offset_y;
      lis2dw12->cfg.offset_z = cfg->offset_z;
      lis2dw12->cfg.offset_weight = cfg->offset_weight;
  
      rc = lis2dw12_set_offset_enable(itf, cfg->offset_en);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.offset_en = cfg->offset_en;
  
      rc = lis2dw12_set_filter_cfg(itf, cfg->filter_bw, cfg->high_pass);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.filter_bw = cfg->filter_bw;
      lis2dw12->cfg.high_pass = cfg->high_pass;
  
      rc = lis2dw12_set_full_scale(itf, cfg->fs);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.fs = cfg->fs;
  
      rc = lis2dw12_set_rate(itf, cfg->rate);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.rate = cfg->rate;
  
      rc = lis2dw12_set_power_mode(itf, cfg->power_mode);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.power_mode = cfg->power_mode;
  
      rc = lis2dw12_set_low_noise(itf, cfg->low_noise_enable);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.low_noise_enable = cfg->low_noise_enable;
  
      rc = lis2dw12_set_fifo_cfg(itf, cfg->fifo_mode, cfg->fifo_threshold);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.fifo_mode = cfg->fifo_mode;
      lis2dw12->cfg.fifo_threshold = cfg->fifo_threshold;
  
      rc = lis2dw12_set_wake_up_ths(itf, cfg->wake_up_ths);
      if (rc) {
          goto err;
      }
      lis2dw12->cfg.wake_up_ths = cfg->wake_up_ths;
  
      rc = lis2dw12_set_wake_up_dur(itf, cfg->wake_up_dur);
      if (rc) {
          goto err;
      }
      lis2dw12->cfg.wake_up_dur = cfg->wake_up_dur;
  
      rc = lis2dw12_set_sleep_dur(itf, cfg->sleep_duration);
      if (rc) {
          goto err;
      }
      lis2dw12->cfg.sleep_duration = cfg->sleep_duration;
  
      rc = lis2dw12_set_stationary_en(itf, cfg->stationary_detection_enable);
      if (rc) {
          goto err;
      }
      lis2dw12->cfg.stationary_detection_enable = cfg->stationary_detection_enable;
  
      rc = lis2dw12_set_inactivity_sleep_en(itf, cfg->inactivity_sleep_enable);
      if (rc) {
          goto err;
      }
      lis2dw12->cfg.inactivity_sleep_enable = cfg->inactivity_sleep_enable;
  
      rc = lis2dw12_set_double_tap_event_en(itf, cfg->double_tap_event_enable);
      if (rc) {
          goto err;
      }
      lis2dw12->cfg.double_tap_event_enable = cfg->double_tap_event_enable;
  
      rc = lis2dw12_set_freefall(itf, cfg->freefall_dur, cfg->freefall_ths);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.freefall_dur = cfg->freefall_dur;
      lis2dw12->cfg.freefall_ths = cfg->freefall_ths;
  
  
      rc = lis2dw12_set_int_enable(itf, cfg->int_enable);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.int_enable = cfg->int_enable;
  
      rc = lis2dw12_set_int1_pin_cfg(itf, cfg->int1_pin_cfg);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.int1_pin_cfg = cfg->int1_pin_cfg;
  
      rc = lis2dw12_set_int2_pin_cfg(itf, cfg->int2_pin_cfg);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.int2_pin_cfg = cfg->int2_pin_cfg;
  
      rc = lis2dw12_set_tap_cfg(itf, &cfg->tap);
      if (rc) {
          goto err;
      }
      lis2dw12->cfg.tap = cfg->tap;
  
      rc = lis2dw12_set_int2_on_int1_map(itf, cfg->map_int2_to_int1);
      if(rc) {
          goto err;
      }
      lis2dw12->cfg.map_int2_to_int1 = cfg->map_int2_to_int1;
  
      rc = sensor_set_type_mask(&(lis2dw12->sensor), cfg->mask);
      if (rc) {
          goto err;
      }
  
      lis2dw12->cfg.read_mode.int_cfg = cfg->read_mode.int_cfg;
      lis2dw12->cfg.read_mode.int_num = cfg->read_mode.int_num;
      lis2dw12->cfg.read_mode.mode = cfg->read_mode.mode;
  
      if (!cfg->notif_cfg) {
          lis2dw12->cfg.notif_cfg = (struct lis2dw12_notif_cfg *)dflt_notif_cfg;
          lis2dw12->cfg.max_num_notif = sizeof(dflt_notif_cfg)/sizeof(*dflt_notif_cfg);
      } else {
          lis2dw12->cfg.notif_cfg = cfg->notif_cfg;
          lis2dw12->cfg.max_num_notif = cfg->max_num_notif;
      }
  
      lis2dw12->cfg.mask = cfg->mask;
  
      return 0;
  err:
      return rc;
  }
  