/**
* @brief suspends the device to put it in its lowest power mode.
*
* @param mlsl_handle
* the handle to the serial channel the device is connected to.
* @param slave
* a pointer to the slave descriptor data structure.
* @param pdata
* a pointer to the slave platform data.
*
* @return ML_SUCCESS if successful or a non-zero error code.
*/
static int bma250_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = 0;
if (pdata->check_sleep_status && pdata->vote_sleep_status) {
mutex_lock(&bma250_power_lock);
pdata->vote_sleep_status(BMA250_SLAVE2, BMA250_SLEEP);
if (pdata->check_sleep_status() == BMA250_SLEEP) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_PWR_REG,
BMA250_PWR_SLEEP_MASK);
ERROR_CHECK_MUTEX(result, bma250_power_lock);
MLOSSleep(3); /* 3 ms powerup time maximum */
}
mutex_unlock(&bma250_power_lock);
} else {
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_PWR_REG, BMA250_PWR_SLEEP_MASK);
ERROR_CHECK(result);
MLOSSleep(3); /* 3 ms powerup time maximum */
}
return result;
}
static int bma250_set_irq(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma250_config *config,
int apply,
long irq_type)
{
unsigned char irq_bits = 0;
int result = ML_SUCCESS;
return ML_SUCCESS;
if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION)
return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
config->irq_type = (unsigned char)irq_type;
if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
irq_bits = 0x20;
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
} else {
irq_bits = 0x00;
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
}
config->int_reg &= ACCEL_BOSCH_INT_MASK_IRQ;
config->int_reg |= irq_bits;
if (apply) {
if (!config->power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_REG_SOFT_RESET,
0xB6);
ERROR_CHECK(result);
MLOSSleep(1);
}
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_CTRL_REG, config->ctrl_reg);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_INT_REG, config->int_reg);
ERROR_CHECK(result);
if (!config->power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BOSCH_PWR_REG, 0x80);
ERROR_CHECK(result);
MLOSSleep(1);
} else {
result = set_normal_mode(mlsl_handle, pdata);
ERROR_CHECK(result);
}
}
return result;
}
int lsm303dlha_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
unsigned char reg;
result =
MLSLSerialRead(mlsl_handle, pdata->address, ACCEL_ST_SLEEP_REG,
1, ®);
ERROR_CHECK(result);
reg |= 0x27;
/*wake up if sleeping */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
ACCEL_ST_SLEEP_REG, reg);
ERROR_CHECK(result);
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x20, 0x37);
ERROR_CHECK(result);
MLOSSleep(500);
reg = 0x40;
/* Full Scale */
reg &= ~ACCEL_ST_CTRL_MASK;
if (slave->range.mantissa == 2
&& slave->range.fraction == 480) {
reg |= 0x00;
} else if (slave->range.mantissa == 4
&& slave->range.fraction == 960) {
reg |= 0x10;
} else if (slave->range.mantissa == 8
&& slave->range.fraction == 1920) {
reg |= 0x30;
}
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x23, reg);
ERROR_CHECK(result);
/* Configure high pass filter */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x21, 0x0F);
ERROR_CHECK(result);
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x32, 0x00);
ERROR_CHECK(result);
/* Configure INT1_DURATION */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x33, 0x7F);
ERROR_CHECK(result);
/* Configure INT1_CFG */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x30, 0x95);
ERROR_CHECK(result);
MLOSSleep(50);
return result;
}
/**
* @brief resume the device in the proper power state given the configuration
* chosen.
*
* @param mlsl_handle
* the handle to the serial channel the device is connected to.
* @param slave
* a pointer to the slave descriptor data structure.
* @param pdata
* a pointer to the slave platform data.
*
* @return ML_SUCCESS if successful or a non-zero error code.
*/
static int bma250_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
struct bma250_config *resume_config =
&((struct bma250_private_data *)pdata->private_data)->resume;
if (pdata->check_sleep_status && pdata->vote_sleep_status) {
mutex_lock(&bma250_power_lock);
if (pdata->check_sleep_status() == BMA250_SLEEP) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_SOFTRESET_REG,
BMA250_SOFTRESET_MASK);
ERROR_CHECK_MUTEX(result, bma250_power_lock);
/* 4 ms needed for to be stable after reset. */
MLOSSleep(4);
}
result = bma250_set_odr(mlsl_handle, pdata, resume_config,
TRUE, resume_config->odr);
ERROR_CHECK_MUTEX(result, bma250_power_lock);
result = bma250_set_fsr(mlsl_handle, pdata, resume_config,
TRUE, resume_config->fsr);
ERROR_CHECK_MUTEX(result, bma250_power_lock);
if (pdata->check_sleep_status() == BMA250_SLEEP) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_PWR_REG,
BMA250_PWR_AWAKE_MASK);
ERROR_CHECK_MUTEX(result, bma250_power_lock);
}
pdata->vote_sleep_status(BMA250_SLAVE2, BMA250_AWAKE);
mutex_unlock(&bma250_power_lock);
} else {
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_SOFTRESET_REG, BMA250_SOFTRESET_MASK);
ERROR_CHECK(result);
/* 4 ms needed for to be stable after reset. */
MLOSSleep(4);
result = bma250_set_odr(mlsl_handle, pdata, resume_config,
TRUE, resume_config->odr);
ERROR_CHECK(result);
result = bma250_set_fsr(mlsl_handle, pdata, resume_config,
TRUE, resume_config->fsr);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_PWR_REG, BMA250_PWR_AWAKE_MASK);
ERROR_CHECK(result);
}
return result;
}
static int kxtf9_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
unsigned char reg;
printk("accelerometer sensor resume : kxtf9_resume\n");
/* RAM reset */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1d, 0xcd);
ERROR_CHECK(result);
MLOSSleep(10);
/* Wake up */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1b, 0x42);
ERROR_CHECK(result);
/* INT_CTRL_REG1: */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1e, 0x14);
ERROR_CHECK(result);
/* WUF_THRESH: */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x5a, 0x00);
ERROR_CHECK(result);
/* DATA_CTRL_REG */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x21, 0x04);
ERROR_CHECK(result);
/* WUF_TIMER */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x29, 0x02);
ERROR_CHECK(result);
/* Full Scale */
reg = 0xc2;
reg &= ~ACCEL_KIONIX_CTRL_MASK;
reg |= 0x00;
if (slave->range.mantissa == 4)
reg |= 0x08;
else if (slave->range.mantissa == 8)
reg |= 0x10;
else {
slave->range.mantissa = 2;
reg |= 0x00;
}
slave->range.fraction = 0;
/* Normal operation */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1b, reg);
ERROR_CHECK(result);
MLOSSleep(50);
return ML_SUCCESS;
}
int hscdtd00xx_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
/* Soft reset */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
COMPASS_HSCDTD00XX_CTRL3, 0x80);
ERROR_CHECK(result);
/* Force state; Power mode: active */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
COMPASS_HSCDTD00XX_CTRL1, 0x82);
ERROR_CHECK(result);
/* Data ready enable */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
COMPASS_HSCDTD00XX_CTRL2, 0x08);
ERROR_CHECK(result);
MLOSSleep(1); /* turn-on time */
return result;
}
static int bma150_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0a, 0x01);
MLOSSleep(3); /* 3 ms powerup time maximum */
ERROR_CHECK(result);
return result;
}
int ak8975_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
AK8975_REG_CNTL,
AK8975_CNTL_MODE_POWER_DOWN);
MLOSSleep(1); /* wait at least 100us */
ERROR_CHECK(result);
return result;
}
int lsm303dlhm_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
#ifdef CONFIG_INPUT_LSM303DLH
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LSM_REG_MODE, LSM_MODE_SLEEP);
ERROR_CHECK(result);
MLOSSleep(3);
#endif
return result;
}
static int lis3dh_init(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
tMLError result;
struct lis3dh_private_data *private_data;
private_data = (struct lis3dh_private_data *)
MLOSMalloc(sizeof(struct lis3dh_private_data));
if (!private_data)
return ML_ERROR_MEMORY_EXAUSTED;
pdata->private_data = private_data;
private_data->resume.ctrl_reg1 = 0x67;
private_data->suspend.ctrl_reg1 = 0x18;
private_data->resume.mot_int1_cfg = 0x95;
private_data->suspend.mot_int1_cfg = 0x2a;
lis3dh_set_odr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 0);
lis3dh_set_odr(mlsl_handle, pdata, &private_data->resume,
FALSE, 200000);
lis3dh_set_fsr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 2048);
lis3dh_set_fsr(mlsl_handle, pdata, &private_data->resume,
FALSE, 2048);
lis3dh_set_ths(mlsl_handle, pdata, &private_data->suspend,
FALSE, 80);
lis3dh_set_ths(mlsl_handle, pdata, &private_data->resume,
FALSE, 40);
lis3dh_set_dur(mlsl_handle, pdata, &private_data->suspend,
FALSE, 1000);
lis3dh_set_dur(mlsl_handle, pdata, &private_data->resume,
FALSE, 2540);
lis3dh_set_irq(mlsl_handle, pdata, &private_data->suspend,
FALSE,
MPU_SLAVE_IRQ_TYPE_NONE);
lis3dh_set_irq(mlsl_handle, pdata, &private_data->resume,
FALSE,
MPU_SLAVE_IRQ_TYPE_NONE);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS3DH_CTRL_REG1, 0x07);
MLOSSleep(6);
return ML_SUCCESS;
}
static int bma250_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
#if 1
//--- 11/07/14 added by hkkwon@PS2 ---//
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x11, 0x80); /* BMA250 : suspend mode */
MLOSSleep(3); /* 3 ms powerup time maximum */
ERROR_CHECK(result);
#else
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x14, 0xb6); /* BMA250 : Software reset */
#endif
return result;
}
int hscdtd00xx_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
/* Power mode: stand-by */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
COMPASS_HSCDTD00XX_CTRL1, 0x00);
ERROR_CHECK(result);
MLOSSleep(1); /* turn-off time */
return result;
}
int lis331dlh_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
unsigned char reg;
struct lis331dlh_private_data *private_data = pdata->private_data;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_CTRL_REG1,
private_data->resume.ctrl_reg1);
ERROR_CHECK(result);
MLOSSleep(6);
/* Full Scale */
reg = 0x40;
reg &= ~LIS331_CTRL_MASK;
if (private_data->resume.fsr == 8192)
reg |= 0x30;
else if (private_data->resume.fsr == 4096)
reg |= 0x10;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_CTRL_REG4, reg);
ERROR_CHECK(result);
/* Configure high pass filter */
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_CTRL_REG2, 0x0F);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_CTRL_REG3, 0x00);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_INT1_THS, 0x02);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_INT1_DURATION, 0x7F);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_INT1_CFG, 0x95);
ERROR_CHECK(result);
result = MLSLSerialRead(mlsl_handle, pdata->address,
LIS331_HP_FILTER_RESET, 1, ®);
ERROR_CHECK(result);
return result;
}
static int bma250_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
unsigned char ctrl_reg;
unsigned char bw_reg;
unsigned char int_reg;
struct bma250_private_data *private_data = pdata->private_data;
ctrl_reg = private_data->resume.ctrl_reg;
bw_reg = private_data->resume.bw_reg;
int_reg = private_data->resume.int_reg;
private_data->state = 0;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_REG_SOFT_RESET, 0xB6); /* BMA250: Software reset */
ERROR_CHECK(result);
MLOSSleep(1);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_CTRL_REG, ctrl_reg);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_BW_REG, bw_reg);
ERROR_CHECK(result);
/* TODO Use irq when necessary */
/*result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_INT_REG, int_reg);
ERROR_CHECK(result);*/
if (!private_data->resume.power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_PWR_REG, 0x80);
ERROR_CHECK(result);
} else {
result = set_normal_mode(mlsl_handle, pdata);
ERROR_CHECK(result);
}
return result;
}
static int bma250_resume(void * mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
unsigned char reg = 0;
/* Soft reset */
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x14, 0xb6 ); /* BMA250 : Software reset */
ERROR_CHECK(result);
MLOSSleep(10);
result = MLSLSerialRead(mlsl_handle, pdata->address, ACCEL_BMA250_CTRL_REG, 1, ®);
ERROR_CHECK(result);
/* BMA250 : Full Scale */
reg &= ~ACCEL_BMA250_CTRL_MASK;
reg |= 0x00;
if (slave->range.mantissa==2) {
reg |= 0x03;
} else if (slave->range.mantissa==4) {
reg |= 0x05;
} else if (slave->range.mantissa==8) {
reg |= 0x08;
}
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, ACCEL_BMA250_CTRL_REG, reg );
ERROR_CHECK(result);
result = MLSLSerialRead(mlsl_handle, pdata->address, ACCEL_BMA250_BW_REG, 1, ®);
ERROR_CHECK(result);
reg &= ~ACCEL_BMA250_BW_MASK;
reg |= 0x00;
/* BMA250: Bandwidth */
reg |= 0x0d; // bw=250
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, ACCEL_BMA250_BW_REG, reg );
ERROR_CHECK(result);
return result;
}
int lsm303dlhm_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
#ifdef CONFIG_INPUT_LSM303DLH
if (!lsm303dlh_mag_misc_data)
{
lsm303dlh_mag_misc_data = lsm303dlh_mag_get_instance_ext();
}
if (lsm303dlh_mag_misc_data)
{
lsm303dlh_mag_misc_data->ext_handle=mlsl_handle;
result = lsm303dlh_mag_enable_ext(lsm303dlh_mag_misc_data);
MLOSSleep(50);
}
else
{
pr_err("%s: lsm303dlh_mag_misc_data is NULL\n", __func__);
}
#endif
/* Use single measurement mode. Start at sleep state. */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LSM_REG_MODE, LSM_MODE_SLEEP);
ERROR_CHECK(result);
/* Config normal measurement */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LSM_REG_CONF_A, 0);
ERROR_CHECK(result);
/* Adjust gain to 320 LSB/Gauss */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LSM_REG_CONF_B, LSM_CONF_B_GAIN_5_5);
ERROR_CHECK(result);
return result;
}
static int bma222_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
unsigned char reg = 0;
/* Soft reset */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
ACCEL_BMA222_SFT_RST_REG, 0xB6);
ERROR_CHECK(result);
MLOSSleep(10);
/*Bandwidth */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
ACCEL_BMA222_BW_REG, 0x0C);
ERROR_CHECK(result);
/* Full Scale */
if (slave->range.mantissa == 4)
reg |= 0x05;
else if (slave->range.mantissa == 8)
reg |= 0x08;
else if (slave->range.mantissa == 16)
reg |= 0x0C;
else {
slave->range.mantissa = 2;
reg |= 0x03;
}
slave->range.fraction = 0;
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
ACCEL_BMA222_RANGE_REG, reg);
ERROR_CHECK(result);
return result;
}
static int bma150_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
unsigned char reg = 0;
/* Soft reset */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0a, 0x02);
ERROR_CHECK(result);
MLOSSleep(10);
result =
MLSLSerialRead(mlsl_handle, pdata->address, 0x14, 1, ®);
ERROR_CHECK(result);
/* Bandwidth */
reg &= 0xc0;
reg |= 3; /* 3=190 Hz */
/* Full Scale */
reg &= ~ACCEL_BOSCH_CTRL_MASK;
if (slave->range.mantissa == 4)
reg |= 0x08;
else if (slave->range.mantissa == 8)
reg |= 0x10;
else {
slave->range.mantissa = 2;
reg |= 0x00;
}
slave->range.fraction = 0;
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x14, reg);
ERROR_CHECK(result);
return result;
}
static int kxtf9_init(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
struct kxtf9_private_data *private_data;
int result = ML_SUCCESS;
private_data = (struct kxtf9_private_data *)
MLOSMalloc(sizeof(struct kxtf9_private_data));
if (!private_data)
return ML_ERROR_MEMORY_EXAUSTED;
/* RAM reset */
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
KXTF9_CTRL_REG1,
0x40); /* Fastest Reset */
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
KXTF9_DATA_CTRL_REG,
0x36); /* Fastest Reset */
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
KXTF9_CTRL_REG3, 0xcd); /* Reset */
ERROR_CHECK(result);
MLOSSleep(2);
pdata->private_data = private_data;
private_data->resume.ctrl_reg1 = 0xC0;
private_data->suspend.ctrl_reg1 = 0x40;
result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->suspend,
FALSE, 1000);
ERROR_CHECK(result);
result = kxtf9_set_dur(mlsl_handle, pdata, &private_data->resume,
FALSE, 2540);
ERROR_CHECK(result);
result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 50000);
ERROR_CHECK(result);
result = kxtf9_set_odr(mlsl_handle, pdata, &private_data->resume,
FALSE, 200000);
result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 2000);
ERROR_CHECK(result);
result = kxtf9_set_fsr(mlsl_handle, pdata, &private_data->resume,
FALSE, 2000);
ERROR_CHECK(result);
result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->suspend,
FALSE, 80);
ERROR_CHECK(result);
result = kxtf9_set_ths(mlsl_handle, pdata, &private_data->resume,
FALSE, 40);
ERROR_CHECK(result);
result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->suspend,
FALSE,
MPU_SLAVE_IRQ_TYPE_NONE);
ERROR_CHECK(result);
result = kxtf9_set_irq(mlsl_handle, pdata, &private_data->resume,
FALSE,
MPU_SLAVE_IRQ_TYPE_NONE);
ERROR_CHECK(result);
return result;
}
/**
* @brief enables/disables the I2C bypass to an external device
* connected to MPU's secondary I2C bus.
* @param enable
* Non-zero to enable pass through.
* @return ML_SUCCESS if successful, a non-zero error code otherwise.
*/
static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg,
void *mlsl_handle,
unsigned char enable)
{
unsigned char b, userctl;
int result;
if ((mldl_cfg->gyro_is_bypassed && enable) ||
(!mldl_cfg->gyro_is_bypassed && !enable))
return ML_SUCCESS;
/*---- get current 'USER_CTRL' into b ----*/
result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
MPUREG_USER_CTRL, 1, &b);
ERROR_CHECK(result);
b &= ~BIT_AUX_IF_EN;
mutex_lock(&mutex_fifo_reading_access_lock);
if (!enable) {
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_USER_CTRL,
(b | BIT_AUX_IF_EN));
ERROR_CHECK_MUTEX(result, mutex_fifo_reading_access_lock);
if (mldl_cfg->pdata->accel.bypass_state) {
userctl = (b | BIT_AUX_IF_EN);
mldl_cfg->pdata->accel.bypass_state(
SAVE_BYPASS_STATE, &userctl);
}
} else {
/* Coming out of I2C is tricky due to several erratta. Do not
* modify this algorithm
*/
/*
* 1) wait for the right time and send the command to change
* the aux i2c slave address to an invalid address that will
* get nack'ed
*
* 0x00 is broadcast. 0x7F is unlikely to be used by any aux.
*/
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_AUX_SLV_ADDR, 0x7F);
ERROR_CHECK_MUTEX(result, mutex_fifo_reading_access_lock);
/*
* 2) wait enough time for a nack to occur, then go into
* bypass mode:
*/
MLOSSleep(2);
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_USER_CTRL, (b));
ERROR_CHECK_MUTEX(result, mutex_fifo_reading_access_lock);
if (mldl_cfg->pdata->accel.bypass_state) {
userctl = b;
mldl_cfg->pdata->accel.bypass_state(
SAVE_BYPASS_STATE, &userctl);
}
/*
* 3) wait for up to one MPU cycle then restore the slave
* address
*/
MLOSSleep(SAMPLING_PERIOD_US(mldl_cfg) / 1000);
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_AUX_SLV_ADDR,
mldl_cfg->pdata->
accel.address);
ERROR_CHECK_MUTEX(result, mutex_fifo_reading_access_lock);
/*
* 4) reset the ime interface
*/
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_USER_CTRL,
(b | BIT_AUX_IF_RST));
ERROR_CHECK_MUTEX(result, mutex_fifo_reading_access_lock);
MLOSSleep(2);
}
mutex_unlock(&mutex_fifo_reading_access_lock);
mldl_cfg->gyro_is_bypassed = enable;
return result;
}
static int lis331dlh_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
unsigned char reg1;
unsigned char reg2;
struct lis331dlh_private_data *private_data = pdata->private_data;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_CTRL_REG1,
private_data->resume.ctrl_reg1);
ERROR_CHECK(result);
MLOSSleep(6);
/* Full Scale */
reg1 = 0x40;
if (private_data->resume.fsr == 8192)
reg1 |= 0x30;
else if (private_data->resume.fsr == 4096)
reg1 |= 0x10;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_CTRL_REG4, reg1);
ERROR_CHECK(result);
/* Configure high pass filter */
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_CTRL_REG2, 0x0F);
ERROR_CHECK(result);
if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
reg1 = 0x02;
reg2 = 0x00;
} else if (private_data->resume.irq_type ==
MPU_SLAVE_IRQ_TYPE_MOTION) {
reg1 = 0x00;
reg2 = private_data->resume.mot_int1_cfg;
} else {
reg1 = 0x00;
reg2 = 0x00;
}
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_CTRL_REG3, reg1);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_INT1_THS,
private_data->resume.reg_ths);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_INT1_DURATION,
private_data->resume.reg_dur);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_INT1_CFG, reg2);
ERROR_CHECK(result);
result = MLSLSerialRead(mlsl_handle, pdata->address,
LIS331_HP_FILTER_RESET, 1, ®1);
ERROR_CHECK(result);
return result;
}
static int bma250_set_irq(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma250_config *config,
int apply,
long irq_type)
{
unsigned char irq_bits = 0;
int result = ML_SUCCESS;
return ML_SUCCESS;
if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION)
return ML_ERROR_FEATURE_NOT_IMPLEMENTED;
config->irq_type = (unsigned char)irq_type;
if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
irq_bits = 0x20;
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
} else {
irq_bits = 0x00;
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
}
config->int_reg &= ACCEL_BOSCH_INT_MASK_IRQ;
config->int_reg |= irq_bits;
if (apply) {
#ifndef CONFIG_CIR_ALWAYS_READY
if (!config->power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_REG_SOFT_RESET,
0xB6);
ERROR_CHECK(result);
MLOSSleep(1);
}
#endif
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_CTRL_REG, config->ctrl_reg);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_INT_REG, config->int_reg);
ERROR_CHECK(result);
#ifdef CONFIG_CIR_ALWAYS_READY
if (!config->power_mode && !cir_flag) {
#else
if (!config->power_mode) {
#endif
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BOSCH_PWR_REG, 0x80);
ERROR_CHECK(result);
MLOSSleep(1);
} else {
result = set_normal_mode(mlsl_handle, pdata);
ERROR_CHECK(result);
}
}
return result;
}
static int bma250_set_odr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma250_config *config,
int apply,
long odr)
{
unsigned char odr_bits = 0;
unsigned char wup_bits = 0;
unsigned char read_from_chip_bw = 0;
int result = ML_SUCCESS;
if (odr > 100000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 50000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 20000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 15000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 0) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else {
config->odr = 0;
wup_bits = 0x00;
config->power_mode = 0;
}
switch (config->power_mode) {
case 1:
config->bw_reg &= BMA250_BW_MASK;
config->bw_reg |= odr_bits;
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
break;
case 0:
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
config->int_reg |= wup_bits;
break;
default:
break;
}
MPL_LOGV("ODR: %d \n", config->odr);
if (apply) {
#ifndef CONFIG_CIR_ALWAYS_READY
#if 0
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_REG_SOFT_RESET,
0xB6);
ERROR_CHECK(result);
MLOSSleep(1);
#endif
#endif
result = MLSLSerialRead(mlsl_handle,
pdata->address, BMA250_BW_REG, 1,
&read_from_chip_bw);
ERROR_CHECK(result);
if (odr_bits != read_from_chip_bw) {
D("%s: Really set ODR to %d\n", __func__,
config->odr);
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_BW_REG,
config->bw_reg);
ERROR_CHECK(result);
MLOSSleep(25);
}
if (!config->power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BOSCH_PWR_REG,
0x80);
ERROR_CHECK(result);
MLOSSleep(1);
} else {
#if 0
result = set_normal_mode(mlsl_handle, pdata);
ERROR_CHECK(result);
#endif
}
}
return result;
}
/**
* @brief enables/disables the I2C bypass to an external device
* connected to MPU's secondary I2C bus.
* @param enable
* Non-zero to enable pass through.
* @return ML_SUCCESS if successful, a non-zero error code otherwise.
*/
static int MLDLSetI2CBypass(struct mldl_cfg *mldl_cfg,
void *mlsl_handle,
unsigned char enable)
{
unsigned char b;
int result;
#ifdef ML_USE_DMP_SIM
/* done this way so that pc demo */
/* w/arm board works with universal api */
if (!MLGetGyroPresent())
return ML_SUCCESS;
#endif
/*---- get current 'USER_CTRL' into b ----*/
result = MLSLSerialRead(mlsl_handle, mldl_cfg->addr,
MPUREG_USER_CTRL, 1, &b);
ERROR_CHECK(result);
/* No change */
if ((b & BIT_AUX_IF_EN) != (enable * BIT_AUX_IF_EN))
return ML_SUCCESS;
b &= ~BIT_AUX_IF_EN;
if (!enable) {
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_USER_CTRL,
(b | BIT_AUX_IF_EN));
ERROR_CHECK(result);
} else {
/* Coming out of I2C is tricky due to several erratta. Do not
* modify this algorithm
*/
/*
* 1) wait for the right time and send the command to change
* the aux i2c slave address to an invalid address that will
* get nack'ed
*
* 0x00 is broadcast. 0x7F is unlikely to be used by any aux.
*/
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_AUX_SLV_ADDR, 0x7F);
ERROR_CHECK(result);
/*
* 2) wait enough time for a nack to occur, then go into
* bypass mode:
*/
MLOSSleep(2);
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_USER_CTRL, (b));
ERROR_CHECK(result);
/*
* 3) wait for up to one MPU cycle then restore the slave
* address
*/
MLOSSleep(5);
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_AUX_SLV_ADDR,
mldl_cfg->pdata->
accel.address);
ERROR_CHECK(result);
/*
* 4) reset the ime interface
*/
#ifdef M_HW
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_USER_CTRL,
(b | BIT_I2C_MST_RST));
#else
result = MLSLSerialWriteSingle(mlsl_handle, mldl_cfg->addr,
MPUREG_USER_CTRL,
(b | BIT_AUX_IF_RST));
#endif
ERROR_CHECK(result);
MLOSSleep(2);
}
return result;
}
static int bma250_init(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
tMLError result;
unsigned char reg = 0;
unsigned char bw_reg = 0;
struct bma250_private_data *private_data;
private_data = (struct bma250_private_data *)
MLOSMalloc(sizeof(struct bma250_private_data));
if (!private_data)
return ML_ERROR_MEMORY_EXAUSTED;
pdata->private_data = private_data;
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_REG_SOFT_RESET, 0xB6); /* BMA250: Software reset */
ERROR_CHECK(result);
MLOSSleep(1);
result =
MLSLSerialRead(mlsl_handle, pdata->address, BOSCH_CTRL_REG, 1,
®);
ERROR_CHECK(result);
result =
MLSLSerialRead(mlsl_handle, pdata->address, BMA250_BW_REG, 1,
&bw_reg);
ERROR_CHECK(result);
private_data->resume.ctrl_reg = reg;
private_data->suspend.ctrl_reg = reg;
private_data->resume.bw_reg = bw_reg;
private_data->suspend.bw_reg = bw_reg;
/* TODO Use irq when necessary */
/*result =
MLSLSerialRead(mlsl_handle, pdata->address, BOSCH_INT_REG, 1, ®);
ERROR_CHECK(result);*/
private_data->resume.int_reg = reg;
private_data->suspend.int_reg = reg;
private_data->resume.power_mode = 1;
private_data->suspend.power_mode = 0;
private_data->state = 0;
bma250_set_odr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 0);
bma250_set_odr(mlsl_handle, pdata, &private_data->resume,
FALSE, 25000);
bma250_set_fsr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 2048);
bma250_set_fsr(mlsl_handle, pdata, &private_data->resume,
FALSE, 2048);
/* TODO Use irq when necessary */
/*bma250_set_irq(mlsl_handle, pdata, &private_data->suspend,
FALSE,
MPU_SLAVE_IRQ_TYPE_NONE);
bma250_set_irq(mlsl_handle, pdata, &private_data->resume,
FALSE,
MPU_SLAVE_IRQ_TYPE_NONE);*/
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, BOSCH_PWR_REG,
0x80);
ERROR_CHECK(result);
return result;
}
static int bma250_set_fsr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma250_config *config,
int apply,
long fsr)
{
unsigned char fsr_bits;
int result = ML_SUCCESS;
if (fsr <= 2048) {
fsr_bits = 0x03;
config->fsr = 2048;
} else if (fsr <= 4096) {
fsr_bits = 0x03;
config->fsr = 2048;
} else if (fsr <= 8192) {
fsr_bits = 0x03;
config->fsr = 2048;
} else if (fsr <= 16384) {
fsr_bits = 0x03;
config->fsr = 2048;
} else {
fsr_bits = 0x03;
config->fsr = 2048;
}
config->ctrl_reg &= ACCEL_BOSCH_CTRL_MASK_FSR;
config->ctrl_reg |= fsr_bits;
MPL_LOGV("FSR: %d \n", config->fsr);
if (apply) {
#ifndef CONFIG_CIR_ALWAYS_READY
if (!config->power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_REG_SOFT_RESET, 0xB6);
ERROR_CHECK(result);
MLOSSleep(1);
}
#endif
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_CTRL_REG, config->ctrl_reg);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_CTRL_REG, config->ctrl_reg);
ERROR_CHECK(result);
if (!config->power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BOSCH_PWR_REG, 0x80);
ERROR_CHECK(result);
MLOSSleep(1);
} else {
result = set_normal_mode(mlsl_handle, pdata);
ERROR_CHECK(result);
}
}
return result;
}
/**
* Set the full scale range of the accels
*
* @param config pointer to configuration
* @param fsr requested full scale range
*/
static int bma250_set_fsr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma250_config *config,
int apply,
long fsr)
{
unsigned char fsr_bits;
int result = ML_SUCCESS;
/* TO DO use dynamic range when stability safe */
/*if (fsr <= 2048) {
fsr_bits = 0x03;
config->fsr = 2048;
} else if (fsr <= 4096) {
fsr_bits = 0x05;
config->fsr = 4096;
} else if (fsr <= 8192) {
fsr_bits = 0x08;
config->fsr = 8192;
} else if (fsr <= 16384) {
fsr_bits = 0x0C;
config->fsr = 16384;
} else {
fsr_bits = 0x03;
config->fsr = 2048;
}*/
if (fsr <= 2048) {
fsr_bits = 0x03;
config->fsr = 2048;
} else if (fsr <= 4096) {
fsr_bits = 0x03;
config->fsr = 2048;
} else if (fsr <= 8192) {
fsr_bits = 0x03;
config->fsr = 2048;
} else if (fsr <= 16384) {
fsr_bits = 0x03;
config->fsr = 2048;
} else {
fsr_bits = 0x03;
config->fsr = 2048;
}
config->ctrl_reg &= ACCEL_BOSCH_CTRL_MASK_FSR;
config->ctrl_reg |= fsr_bits;
MPL_LOGV("FSR: %d \n", config->fsr);
if (apply) {
if (!config->power_mode) {
/* BMA250: Software reset */
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_REG_SOFT_RESET, 0xB6);
ERROR_CHECK(result);
MLOSSleep(1);
}
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_CTRL_REG, config->ctrl_reg);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_CTRL_REG, config->ctrl_reg);
ERROR_CHECK(result);
if (!config->power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BOSCH_PWR_REG, 0x80);
ERROR_CHECK(result);
MLOSSleep(1);
}
}
return result;
}
/**
* Set the Output data rate for the particular configuration
*
* @param config Config to modify with new ODR
* @param odr Output data rate in units of 1/1000Hz
*/
static int bma250_set_odr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma250_config *config,
int apply,
long odr)
{
unsigned char odr_bits = 0;
unsigned char wup_bits = 0;
int result = ML_SUCCESS;
/* TO DO use dynamic bandwidth when stability safe */
/*if (odr > 100000) {
config->odr = 125000;
odr_bits = 0x0C;
config->power_mode = 1;
} else if (odr > 50000) {
config->odr = 62500;
odr_bits = 0x0B;
config->power_mode = 1;
} else if (odr > 20000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 15000) {
config->odr = 15630;
odr_bits = 0x09;
config->power_mode = 1;
} else if (odr > 0) {
config->odr = 7810;
odr_bits = 0x08;
config->power_mode = 1;
} else {
config->odr = 0;
wup_bits = 0x00;
config->power_mode = 0;
}*/
if (odr > 100000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 50000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 20000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 15000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 0) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else {
config->odr = 0;
wup_bits = 0x00;
config->power_mode = 0;
}
switch (config->power_mode) {
case 1:
config->bw_reg &= BMA250_BW_MASK;
config->bw_reg |= odr_bits;
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
break;
case 0:
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
config->int_reg |= wup_bits;
break;
default:
break;
}
MPL_LOGV("ODR: %d \n", config->odr);
if (apply) {
/* BMA250: Software reset */
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_REG_SOFT_RESET,
0xB6);
ERROR_CHECK(result);
MLOSSleep(1);
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_BW_REG,
config->bw_reg);
ERROR_CHECK(result);
/* TODO Use irq when necessary */
/*
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BOSCH_INT_REG,
config->int_reg);
ERROR_CHECK(result);*/
if (!config->power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BOSCH_PWR_REG,
0x80);
ERROR_CHECK(result);
MLOSSleep(1);
}
}
return result;
}
static int bma250_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = 0;
unsigned char ctrl_reg;
unsigned char int_reg;
struct bma250_private_data *private_data = pdata->private_data;
ctrl_reg = private_data->suspend.ctrl_reg;
int_reg = private_data->suspend.int_reg;
private_data->state = 1;
#ifdef CONFIG_CIR_ALWAYS_READY
if ((!private_data->suspend.power_mode && !cir_flag)) {
#else
if ((!private_data->suspend.power_mode)) {
#endif
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_PWR_REG, 0x80);
printk(KERN_INFO "[BMA250] GSensor Low Power Mode\n");
ERROR_CHECK(result);
}
return result;
}
static int bma250_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
unsigned char ctrl_reg;
unsigned char bw_reg;
unsigned char int_reg;
struct bma250_private_data *private_data = pdata->private_data;
ctrl_reg = private_data->resume.ctrl_reg;
bw_reg = private_data->resume.bw_reg;
int_reg = private_data->resume.int_reg;
private_data->state = 0;
#ifndef CONFIG_CIR_ALWAYS_READY
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_REG_SOFT_RESET, 0xB6);
ERROR_CHECK(result);
MLOSSleep(1);
#endif
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_CTRL_REG, ctrl_reg);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_BW_REG, bw_reg);
ERROR_CHECK(result);
#ifdef CONFIG_CIR_ALWAYS_READY
power_key_pressed = 0;
#endif
if (!private_data->resume.power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_PWR_REG, 0x80);
ERROR_CHECK(result);
} else {
result = set_normal_mode(mlsl_handle, pdata);
ERROR_CHECK(result);
}
return result;
}
static int bma250_set_odr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma250_config *config,
int apply,
long odr)
{
unsigned char odr_bits = 0;
unsigned char wup_bits = 0;
int result = ML_SUCCESS;
if (odr > 100000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 50000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 20000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 15000) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else if (odr > 0) {
config->odr = 31250;
odr_bits = 0x0A;
config->power_mode = 1;
} else {
config->odr = 0;
wup_bits = 0x00;
config->power_mode = 0;
}
switch (config->power_mode) {
case 1:
config->bw_reg &= BMA250_BW_MASK;
config->bw_reg |= odr_bits;
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
break;
case 0:
config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
config->int_reg |= wup_bits;
break;
default:
break;
}
MPL_LOGV("ODR: %d \n", config->odr);
if (apply) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_REG_SOFT_RESET,
0xB6);
ERROR_CHECK(result);
MLOSSleep(1);
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BMA250_BW_REG,
config->bw_reg);
ERROR_CHECK(result);
if (!config->power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BOSCH_PWR_REG,
0x80);
ERROR_CHECK(result);
MLOSSleep(1);
} else {
result = set_normal_mode(mlsl_handle, pdata);
ERROR_CHECK(result);
}
}
return result;
}
static int bma250_init(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
tMLError result;
unsigned char reg = 0;
unsigned char bw_reg = 0;
struct bma250_private_data *private_data;
private_data = (struct bma250_private_data *)
MLOSMalloc(sizeof(struct bma250_private_data));
printk(KERN_DEBUG "%s\n", __func__);
if (!private_data)
return ML_ERROR_MEMORY_EXAUSTED;
pdata->private_data = private_data;
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_REG_SOFT_RESET, 0xB6);
ERROR_CHECK(result);
MLOSSleep(1);
result =
MLSLSerialRead(mlsl_handle, pdata->address, BOSCH_CTRL_REG, 1,
®);
ERROR_CHECK(result);
result =
MLSLSerialRead(mlsl_handle, pdata->address, BMA250_BW_REG, 1,
&bw_reg);
ERROR_CHECK(result);
private_data->resume.ctrl_reg = reg;
private_data->suspend.ctrl_reg = reg;
private_data->resume.bw_reg = bw_reg;
private_data->suspend.bw_reg = bw_reg;
private_data->resume.int_reg = reg;
private_data->suspend.int_reg = reg;
private_data->resume.power_mode = 1;
private_data->suspend.power_mode = 0;
private_data->state = 0;
bma250_set_odr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 0);
bma250_set_odr(mlsl_handle, pdata, &private_data->resume,
TRUE, 25000);
bma250_set_fsr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 2048);
bma250_set_fsr(mlsl_handle, pdata, &private_data->resume,
FALSE, 2048);
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, BOSCH_PWR_REG,
0x80);
ERROR_CHECK(result);
return result;
}