text: /*
   * Copyright (c) 2015, Freescale Semiconductor, Inc.
   * Copyright 2016-2017 NXP
   * All rights reserved.
   *
   * SPDX-License-Identifier: BSD-3-Clause
   */
  
  #include "fsl_fxos.h"
  
  /******************************************************************************
   * Code
   ******************************************************************************/
  status_t FXOS_Init(fxos_handle_t *fxos_handle, fxos_config_t *config)
  {
      assert(fxos_handle);
      assert(config);
      assert(config->I2C_SendFunc);
      assert(config->I2C_ReceiveFunc);
  
      uint8_t tmp[1] = {0};
  
      /* Initialize the I2C access function. */
      fxos_handle->I2C_SendFunc    = config->I2C_SendFunc;
      fxos_handle->I2C_ReceiveFunc = config->I2C_ReceiveFunc;
      /* Set Slave Address. */
      fxos_handle->slaveAddress = config->slaveAddress;
  
      if (FXOS_ReadReg(fxos_handle, WHO_AM_I_REG, tmp, 1) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  
      if (tmp[0] != kFXOS_WHO_AM_I_Device_ID)
      {
          return kStatus_Fail;
      }
  
      /* setup auto sleep with FFMT trigger */
      /* go to standby */
      if (FXOS_ReadReg(fxos_handle, CTRL_REG1, tmp, 1) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  
      if (FXOS_WriteReg(fxos_handle, CTRL_REG1, tmp[0] & (uint8_t)~ACTIVE_MASK) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  
      /* Read again to make sure we are in standby mode. */
      if (FXOS_ReadReg(fxos_handle, CTRL_REG1, tmp, 1) != kStatus_Success)
      {
          return kStatus_Fail;
      }
      if ((tmp[0] & ACTIVE_MASK) == ACTIVE_MASK)
      {
          return kStatus_Fail;
      }
  
      /* Disable the FIFO */
      if (FXOS_WriteReg(fxos_handle, F_SETUP_REG, F_MODE_DISABLED) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  
  #ifdef LPSLEEP_HIRES
      /* enable auto-sleep, low power in sleep, high res in wake */
      if (FXOS_WriteReg(fxos_handle, CTRL_REG2, SLPE_MASK | SMOD_LOW_POWER | MOD_HIGH_RES) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  #else
      /* enable auto-sleep, low power in sleep, high res in wake */
      if (FXOS_WriteReg(fxos_handle, CTRL_REG2, MOD_HIGH_RES) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  
  #endif
  
      /* set up Mag OSR and Hybrid mode using M_CTRL_REG1, use default for Acc */
      if (FXOS_WriteReg(fxos_handle, M_CTRL_REG1, (M_RST_MASK | M_OSR_MASK | M_HMS_MASK)) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  
      /* Enable hyrid mode auto increment using M_CTRL_REG2 */
      if (FXOS_WriteReg(fxos_handle, M_CTRL_REG2, (M_HYB_AUTOINC_MASK)) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  
  #ifdef EN_FFMT
      /* enable FFMT for motion detect for X and Y axes, latch enable */
      if (FXOS_WriteReg(fxos_handle, FF_MT_CFG_REG, XEFE_MASK | YEFE_MASK | ELE_MASK | OAE_MASK) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  #endif
  
  #ifdef SET_THRESHOLD
      /* set threshold to about 0.25g */
      if (FXOS_WriteReg(fxos_handle, FT_MT_THS_REG, 0x04) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  #endif
  
  #ifdef SET_DEBOUNCE
      /* set debounce to zero */
      if (FXOS_WriteReg(fxos_handle, FF_MT_COUNT_REG, 0x00) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  #endif
  
  #ifdef EN_AUTO_SLEEP
      /* set auto-sleep wait period to 5s (=5/0.64=~8) */
      if (FXOS_WriteReg(fxos_handle, ASLP_COUNT_REG, 8) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  #endif
      /* default set to 4g mode */
      if (FXOS_WriteReg(fxos_handle, XYZ_DATA_CFG_REG, FULL_SCALE_4G) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  #ifdef EN_INTERRUPTS
      /* enable data-ready, auto-sleep and motion detection interrupts */
      /* FXOS1_WriteRegister(CTRL_REG4, INT_EN_DRDY_MASK | INT_EN_ASLP_MASK | INT_EN_FF_MT_MASK); */
      if (FXOS_WriteReg(fxos_handle, CTRL_REG4, 0x0) != kStatus_Success)
      {
          return kStatus_Fail;
      }
      /* route data-ready interrupts to INT1, others INT2 (default) */
      if (FXOS_WriteReg(fxos_handle, CTRL_REG5, INT_CFG_DRDY_MASK) != kStatus_Success)
      {
          return kStatus_Fail;
      }
      /* enable ffmt as a wake-up source */
      if (FXOS_WriteReg(fxos_handle, CTRL_REG3, WAKE_FF_MT_MASK) != kStatus_Success)
      {
          return kStatus_Fail;
      }
      /* finally activate accel_device with ASLP ODR=0.8Hz, ODR=100Hz, FSR=2g */
      if (FXOS_WriteReg(fxos_handle, CTRL_REG1, HYB_ASLP_RATE_0_8HZ | HYB_DATA_RATE_100HZ | ACTIVE_MASK) !=
          kStatus_Success)
      {
          return kStatus_Fail;
      }
  #else
      /* Setup the ODR for 50 Hz and activate the accelerometer */
      if (FXOS_WriteReg(fxos_handle, CTRL_REG1, (HYB_DATA_RATE_200HZ | ACTIVE_MASK)) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  #endif
  
      /* Read Control register again to ensure we are in active mode */
      if (FXOS_ReadReg(fxos_handle, CTRL_REG1, tmp, 1) != kStatus_Success)
      {
          return kStatus_Fail;
      }
  
      if ((tmp[0] & ACTIVE_MASK) != ACTIVE_MASK)
      {
          return kStatus_Fail;
      }
  
      return kStatus_Success;
  }
  
  status_t FXOS_ReadSensorData(fxos_handle_t *fxos_handle, fxos_data_t *sensorData)
  {
      status_t status     = kStatus_Success;
      uint8_t tmp_buff[6] = {0};
      uint8_t i           = 0;
  
      if (!FXOS_ReadReg(fxos_handle, OUT_X_MSB_REG, tmp_buff, 6) == kStatus_Success)
      {
          status = kStatus_Fail;
      }
  
      for (i = 0; i < 6; i++)
      {
          ((int8_t *)sensorData)[i] = tmp_buff[i];
      }
  
      if (!FXOS_ReadReg(fxos_handle, M_OUT_X_MSB_REG, tmp_buff, 6) == kStatus_Success)
      {
          status = kStatus_Fail;
      }
  
      for (i = 0; i < 6; i++)
      {
          ((int8_t *)sensorData)[i + 6] = tmp_buff[i];
      }
  
      return status;
  }
  
  status_t FXOS_ReadReg(fxos_handle_t *handle, uint8_t reg, uint8_t *val, uint8_t bytesNumber)
  {
      assert(handle);
      assert(val);
  
      if (!handle->I2C_ReceiveFunc)
      {
          return kStatus_Fail;
      }
  
      return handle->I2C_ReceiveFunc(handle->slaveAddress, reg, 1, val, bytesNumber);
  }
  
  status_t FXOS_WriteReg(fxos_handle_t *handle, uint8_t reg, uint8_t val)
  {
      assert(handle);
  
      if (!handle->I2C_SendFunc)
      {
          return kStatus_Fail;
      }
  
      return handle->I2C_SendFunc(handle->slaveAddress, reg, 1, val);
  }
  