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;
}
/**
* Set the full scale range of the accels
*
* @param config pointer to configuration
* @param fsr requested full scale range
*/
static int lis331dlh_set_fsr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct lis331dlh_config *config,
int apply,
long fsr)
{
unsigned char reg1 = 0x40;
int result = ML_SUCCESS;
if (fsr <= 2048) {
config->fsr = 2048;
} else if (fsr <= 4096) {
reg1 |= 0x30;
config->fsr = 4096;
} else {
reg1 |= 0x10;
config->fsr = 8192;
}
lis331dlh_set_ths(mlsl_handle, pdata,
config, apply, config->ths);
MPL_LOGV("FSR: %d\n", config->fsr);
if (apply)
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_CTRL_REG4, reg1);
return result;
}
/**
* @brief Set the full scale range of the accels
*
* @param mlsl_handle
* the handle to the serial channel the device is connected to.
* @param pdata
* a pointer to the slave platform data.
* @param config
* pointer to configuration.
* @param apply
* whether to apply immediately or save the settings to be applied
* at the next resume.
* @param fsr
* requested full scale range.
*
* @return ML_SUCCESS if successful or a non-zero error code.
*/
static int bma250_set_fsr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma250_config *config,
int apply,
long fsr)
{
int result = ML_SUCCESS;
unsigned char reg_fsr_mask;
if (fsr <= 2000) {
reg_fsr_mask = 0x03;
config->fsr = 2000;
} else if (fsr <= 4000) {
reg_fsr_mask = 0x05;
config->fsr = 4000;
} else if (fsr <= 8000) {
reg_fsr_mask = 0x08;
config->fsr = 8000;
} else { /* 8001 -> oo */
reg_fsr_mask = 0x0C;
config->fsr = 16000;
}
if (apply) {
MPL_LOGV("FSR: %d\n", config->fsr);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BMA250_FSR_REG, reg_fsr_mask);
ERROR_CHECK(result);
}
return result;
}
int ami30x_read(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata, unsigned char *data)
{
unsigned char stat;
int result = ML_SUCCESS;
/* Read status reg and check if data ready (DRDY) */
result =
MLSLSerialRead(mlsl_handle, pdata->address, AMI30X_REG_STAT1,
1, &stat);
ERROR_CHECK(result);
if (stat & 0x40) {
result =
MLSLSerialRead(mlsl_handle, pdata->address,
AMI30X_REG_DATAX, 6,
(unsigned char *) data);
ERROR_CHECK(result);
/* start another measurement */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
AMI30X_REG_CNTL3,
AMI30X_BIT_CNTL3_F0RCE);
ERROR_CHECK(result);
return ML_SUCCESS;
}
return ML_ERROR_COMPASS_DATA_NOT_READY;
}
int hscdtd004a_read(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata,
unsigned char *data)
{
unsigned char stat;
tMLError result = ML_SUCCESS;
int status = ML_SUCCESS;
/* Read status reg. to check if data is ready */
result =
MLSLSerialRead(mlsl_handle, pdata->address,
COMPASS_HSCDTD004A_STAT, 1, &stat);
ERROR_CHECK(result);
if (stat & 0x48) {
result =
MLSLSerialRead(mlsl_handle, pdata->address,
COMPASS_HSCDTD004A_DATAX, 6,
(unsigned char *) data);
ERROR_CHECK(result);
status = ML_SUCCESS;
} else if (stat & 0x68) {
status = ML_ERROR_COMPASS_DATA_OVERFLOW;
} else {
status = ML_ERROR_COMPASS_DATA_NOT_READY;
}
/* trigger next measurement read */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
COMPASS_HSCDTD004A_CTRL3, 0x40);
ERROR_CHECK(result);
return status;
}
int ak8975_read(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata, unsigned char *data)
{
unsigned char regs[6];
int result = ML_SUCCESS;
int status = ML_SUCCESS;
mpu_dbg("%s\n", __func__);
result = MLSLSerialRead(mlsl_handle, pdata->address,
AK8975_REG_HXL, 6, regs);
ERROR_CHECK(result);
memcpy(data, ®s[0], 6);
status = ML_SUCCESS;
/*
* trigger next measurement if:
* - stat is non zero;
* - if stat is zero and stat2 is non zero.
* Won't trigger if data is not ready and there was no error.
*/
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
AK8975_REG_CNTL,
AK8975_CNTL_MODE_SINGLE_MEASUREMENT);
ERROR_CHECK(result);
return status;
}
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;
if (!private_data->suspend.power_mode) {
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
BOSCH_PWR_REG, 0x80);
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_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x14, 0xb6); /* BMA250 : Software reset */
return result;
}
/**
* @brief Starts the DMP running
*
* @return ML_SUCCESS or non-zero error code
*/
static int dmp_start(struct mldl_cfg *pdata, void *mlsl_handle)
{
unsigned char userCtrlReg;
int result;
if (pdata->dmp_is_running == pdata->dmp_enable)
return ML_SUCCESS;
result = MLSLSerialRead(mlsl_handle, pdata->addr,
MPUREG_USER_CTRL, 1, &userCtrlReg);
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
MPUREG_USER_CTRL,
((userCtrlReg & (~BIT_FIFO_EN))
| BIT_FIFO_RST));
ERROR_CHECK(result);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
MPUREG_USER_CTRL, userCtrlReg);
ERROR_CHECK(result);
result = MLSLSerialRead(mlsl_handle, pdata->addr,
MPUREG_USER_CTRL, 1, &userCtrlReg);
ERROR_CHECK(result);
if (pdata->dmp_enable)
userCtrlReg |= BIT_DMP_EN;
else
userCtrlReg &= ~BIT_DMP_EN;
if (pdata->fifo_enable)
userCtrlReg |= BIT_FIFO_EN;
else
userCtrlReg &= ~BIT_FIFO_EN;
userCtrlReg |= BIT_DMP_RST;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->addr,
MPUREG_USER_CTRL, userCtrlReg);
ERROR_CHECK(result);
pdata->dmp_is_running = pdata->dmp_enable;
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;
}
int cma3000_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
/* RAM reset */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1d, 0xcd);
return result;
}
static int kxtf9_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x1b, 0);
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;
}
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;
}
static int kxsd9_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
/* CTRL_REGB: low-power standby mode */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0d, 0x0);
ERROR_CHECK(result);
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 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;
}
int ak8975_resume(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_SINGLE_MEASUREMENT);
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;
}
int mma8450_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_MMA8450_CTRL_REG, 1, ®);
ERROR_CHECK(result);
/* data rate = 200Hz */
reg &= 0xE3;
reg |= 0x4;
/* Full Scale */
reg &= ~ACCEL_MMA8450_CTRL_MASK;
if (slave->range.mantissa == 4)
reg |= 0x2;
else if (slave->range.mantissa == 8)
reg |= 0x3;
else {
slave->range.mantissa = 2;
reg |= 0x1;
}
slave->range.fraction = 0;
/* XYZ_DATA_CFG: event flag enabled on all axis */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x16, 0x05);
ERROR_CHECK(result);
/* CTRL_REG1: rate + scale config + wakeup */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
ACCEL_MMA8450_CTRL_REG, reg);
ERROR_CHECK(result);
return result;
}
static int lis3dh_suspend(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 lis3dh_private_data *private_data = pdata->private_data;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS3DH_CTRL_REG1,
private_data->suspend.ctrl_reg1);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS3DH_CTRL_REG2, 0x31);
reg1 = 0x48;
if (private_data->suspend.fsr == 16384)
reg1 |= 0x30;
else if (private_data->suspend.fsr == 8192)
reg1 |= 0x20;
else if (private_data->suspend.fsr == 4096)
reg1 |= 0x10;
else if (private_data->suspend.fsr == 2048)
reg1 |= 0x00;
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS3DH_CTRL_REG4, reg1);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS3DH_INT1_THS,
private_data->suspend.reg_ths);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS3DH_INT1_DURATION,
private_data->suspend.reg_dur);
if (private_data->suspend.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
reg1 = 0x10;
reg2 = 0x00;
} else if (private_data->suspend.irq_type ==
MPU_SLAVE_IRQ_TYPE_MOTION) {
reg1 = 0x40;
reg2 = private_data->suspend.mot_int1_cfg;
} else {
reg1 = 0x00;
reg2 = 0x00;
}
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS3DH_CTRL_REG3, reg1);
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS3DH_INT1_CFG, reg2);
result = MLSLSerialRead(mlsl_handle, pdata->address,
LIS3DH_CTRL_REG6, 1, ®1);
return result;
}
static int set_normal_mode(void *mlsl_handle,
struct ext_slave_platform_data *pdata)
{
int result = 0;
result = MLSLSerialWriteSingle(mlsl_handle,
pdata->address, BOSCH_PWR_REG, 0x00);
ERROR_CHECK(result);
usleep(2000);
return 0;
}
static int bma222_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
ACCEL_BMA222_SUSPEND_REG, 0x80);
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;
}
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;
}
/**
* 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 lis331dlh_set_odr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct lis331dlh_config *config,
int apply,
long odr)
{
unsigned char bits;
int result = ML_SUCCESS;
if (odr > 400000) {
config->odr = 1000000;
bits = 0x38;
} else if (odr > 100000) {
config->odr = 400000;
bits = 0x30;
} else if (odr > 50000) {
config->odr = 100000;
bits = 0x28;
} else if (odr > 10000) {
config->odr = 50000;
bits = 0x20;
} else if (odr > 5000) {
config->odr = 10000;
bits = 0xC0;
} else if (odr > 2000) {
config->odr = 5000;
bits = 0xB0;
} else if (odr > 1000) {
config->odr = 2000;
bits = 0x80;
} else if (odr > 500) {
config->odr = 1000;
bits = 0x60;
} else if (odr > 0) {
config->odr = 500;
bits = 0x40;
} else {
config->odr = 0;
bits = 0;
}
config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x7);
lis331dlh_set_dur(mlsl_handle, pdata,
config, apply, config->dur);
MPL_LOGV("ODR: %d, 0x%02x\n", config->odr, (int)config->ctrl_reg1);
if (apply)
result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
LIS331_CTRL_REG1,
config->ctrl_reg1);
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 kxsd9_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = ML_SUCCESS;
unsigned char reg;
/* Full Scale */
reg = 0x0;
reg &= ~ACCEL_KIONIX_CTRL_MASK;
reg |= 0x00;
if (slave->range.mantissa == 4) { /* 4g scale = 4.9951 */
reg |= 0x2;
slave->range.fraction = 9951;
} else if (slave->range.mantissa == 7) { /* 6g scale = 7.5018 */
reg |= 0x1;
slave->range.fraction = 5018;
} else if (slave->range.mantissa == 9) { /* 8g scale = 9.9902 */
reg |= 0x0;
slave->range.fraction = 9902;
} else {
slave->range.mantissa = 2; /* 2g scale = 2.5006 */
slave->range.fraction = 5006;
reg |= 0x3;
}
reg |= 0xC0; /* 100Hz LPF */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address,
ACCEL_KIONIX_CTRL_REG, reg);
ERROR_CHECK(result);
/* normal operation */
result =
MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0d, 0x40);
ERROR_CHECK(result);
return ML_SUCCESS;
}
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;
}