/* -------------------------------------------------------------------------- */
static int ak8975_init(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
unsigned char serial_data[COMPASS_NUM_AXES];
struct ak8975_private_data *private_data;
private_data = (struct ak8975_private_data *)
kzalloc(sizeof(struct ak8975_private_data), GFP_KERNEL);
if (!private_data)
return INV_ERROR_MEMORY_EXAUSTED;
result = inv_serial_single_write(mlsl_handle, pdata->address,
AK8975_REG_CNTL,
AK8975_CNTL_MODE_POWER_DOWN);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Wait at least 100us */
udelay(100);
result = inv_serial_single_write(mlsl_handle, pdata->address,
AK8975_REG_CNTL,
AK8975_CNTL_MODE_FUSE_ROM_ACCESS);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Wait at least 200us */
udelay(200);
result = inv_serial_read(mlsl_handle, pdata->address,
AK8975_REG_ASAX,
COMPASS_NUM_AXES, serial_data);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
pdata->private_data = private_data;
private_data->init.asa[0] = serial_data[0];
private_data->init.asa[1] = serial_data[1];
private_data->init.asa[2] = serial_data[2];
result = inv_serial_single_write(mlsl_handle, pdata->address,
AK8975_REG_CNTL,
AK8975_CNTL_MODE_POWER_DOWN);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
udelay(100);
return INV_SUCCESS;
}
/**
* Set the full scale range of the accels
*
* @param config pointer to configuration
* @param fsr requested full scale range
*/
static int kxtf9_set_fsr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct kxtf9_config *config, int apply, long fsr)
{
int result = INV_SUCCESS;
config->ctrl_reg1 = (config->ctrl_reg1 & 0xE7);
if (fsr <= 2000) {
config->fsr = 2000;
config->ctrl_reg1 |= 0x00;
} else if (fsr <= 4000) {
config->fsr = 4000;
config->ctrl_reg1 |= 0x08;
} else {
config->fsr = 8000;
config->ctrl_reg1 |= 0x10;
}
if (apply) {
/* Must clear bit 7 before writing new configuration */
result = inv_serial_single_write(mlsl_handle, pdata->address,
KXTF9_CTRL_REG1, 0x40);
result = inv_serial_single_write(mlsl_handle, pdata->address,
KXTF9_CTRL_REG1,
config->ctrl_reg1);
}
return result;
}
/**
* Sets the IRQ to fire when one of the IRQ events occur. Threshold and
* duration will not be used uless the type is MOT or NMOT.
*
* @param config configuration to apply to, suspend or resume
* @param irq_type The type of IRQ. Valid values are
* - MPU_SLAVE_IRQ_TYPE_NONE
* - MPU_SLAVE_IRQ_TYPE_MOTION
* - MPU_SLAVE_IRQ_TYPE_DATA_READY
*/
static int lis331dlh_set_irq(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct lis331dlh_config *config,
int apply, long irq_type)
{
int result = INV_SUCCESS;
unsigned char reg1;
unsigned char reg2;
config->irq_type = (unsigned char)irq_type;
if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
reg1 = 0x02;
reg2 = 0x00;
} else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
reg1 = 0x00;
reg2 = config->mot_int1_cfg;
} else {
reg1 = 0x00;
reg2 = 0x00;
}
if (apply) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
LIS331_CTRL_REG3, reg1);
result = inv_serial_single_write(mlsl_handle, pdata->address,
LIS331_INT1_CFG, reg2);
}
return result;
}
/**
* @brief Simply disables the IRQ since it is not usable on BMA150 devices.
*
* @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
* configuration to apply to, suspend or resume
* @param apply
* whether to apply immediately or save the settings to be applied
* at the next resume.
* @param irq_type
* the type of IRQ. Valid values are
* - MPU_SLAVE_IRQ_TYPE_NONE
* - MPU_SLAVE_IRQ_TYPE_MOTION
* - MPU_SLAVE_IRQ_TYPE_DATA_READY
* The only supported IRQ type is MPU_SLAVE_IRQ_TYPE_NONE which
* corresponds to disabling the IRQ completely.
*
* @return INV_SUCCESS if successful or a non-zero error code.
*/
static int bma150_set_irq(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma150_config *config,
int apply,
long irq_type)
{
int result = INV_SUCCESS;
if (irq_type != MPU_SLAVE_IRQ_TYPE_NONE)
return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
config->irq_type = MPU_SLAVE_IRQ_TYPE_NONE;
config->int_reg = 0x00;
if (apply) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
BMA150_CTRL_REG, config->ctrl_reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_serial_single_write(mlsl_handle, pdata->address,
BMA150_INT_REG, config->int_reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
return result;
}
void set_ak89xx_enable(struct iio_dev *indio_dev, bool enable)
{
struct inv_ak89xx_state_s *st = iio_priv(indio_dev);
int result = 0;
unsigned char scale = 0;
if (st->compass_id == COMPASS_ID_AK8963)
scale = st->compass_scale;
dev_dbg(&st->i2c->dev, "%s, enable=%d\n", __func__, enable);
if (enable) {
result = pm_runtime_get_sync(&st->i2c->dev);
if (result < 0) {
dev_err(&st->i2c->dev,
"%s, line=%d\n", __func__, __LINE__);
pm_runtime_put_noidle(&st->i2c->dev);
return;
}
result = inv_serial_single_write(st, AK89XX_REG_CNTL,
(scale << 4) | AK89XX_CNTL_MODE_SNG_MEASURE);
if (result)
pr_err("%s, line=%d\n", __func__, __LINE__);
schedule_delayed_work(&st->work,
msecs_to_jiffies(st->delay));
} else {
cancel_delayed_work_sync(&st->work);
result = inv_serial_single_write(st, AK89XX_REG_CNTL,
(scale << 4) | AK89XX_CNTL_MODE_POWER_DOWN);
if (result)
pr_err("%s, line=%d\n", __func__, __LINE__);
mdelay(1); /* wait at least 100us */
pm_runtime_mark_last_busy(&st->i2c->dev);
pm_runtime_put_autosuspend(&st->i2c->dev);
}
}
static int hscdtd002b_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = INV_SUCCESS;
/* Soft reset */
result =
inv_serial_single_write(mlsl_handle, pdata->address,
COMPASS_HSCDTD002B_CTRL3, 0x80);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Force state; Power mode: active */
result =
inv_serial_single_write(mlsl_handle, pdata->address,
COMPASS_HSCDTD002B_CTRL1, 0x82);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Data ready enable */
result =
inv_serial_single_write(mlsl_handle, pdata->address,
COMPASS_HSCDTD002B_CTRL2, 0x08);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
msleep(1); /* turn-on time */
return result;
}
static int ami30x_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = INV_SUCCESS;
/* Set CNTL1 reg to power model active */
result =
inv_serial_single_write(mlsl_handle, pdata->address,
AMI30X_REG_CNTL1,
AMI30X_BIT_CNTL1_PC1 |
AMI30X_BIT_CNTL1_FS1);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Set CNTL2 reg to DRDY active high and enabled */
result =
inv_serial_single_write(mlsl_handle, pdata->address,
AMI30X_REG_CNTL2,
AMI30X_BIT_CNTL2_DREN |
AMI30X_BIT_CNTL2_DRP);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Set CNTL3 reg to forced measurement period */
result =
inv_serial_single_write(mlsl_handle, pdata->address,
AMI30X_REG_CNTL3, AMI30X_BIT_CNTL3_F0RCE);
return result;
}
static int lsm303dlx_m_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = INV_SUCCESS;
/* Use single measurement mode. Start at sleep state. */
result =
inv_serial_single_write(mlsl_handle, pdata->address,
LSM_REG_MODE, LSM_MODE_SLEEP);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Config normal measurement */
result =
inv_serial_single_write(mlsl_handle, pdata->address,
LSM_REG_CONF_A, 0);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* Adjust gain to 320 LSB/Gauss */
result =
inv_serial_single_write(mlsl_handle, pdata->address,
LSM_REG_CONF_B, LSM_CONF_B_GAIN_5_5);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
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 INV_SUCCESS if successful or a non-zero error code.
*/
static int mma8450_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = INV_SUCCESS;
struct mma8450_private_data *private_data = pdata->private_data;
/* Full Scale */
if (private_data->resume.fsr == 4000)
slave->range.mantissa = 4;
else if (private_data->resume.fsr == 8000)
slave->range.mantissa = 8;
else
slave->range.mantissa = 2;
slave->range.fraction = 0;
ERROR_CHECK(result);
result = inv_serial_single_write(mlsl_handle, pdata->address,
ACCEL_MMA8450_CTRL_REG1, 0);
ERROR_CHECK(result);
if (private_data->resume.ctrl_reg1) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
ACCEL_MMA8450_CTRL_REG1,
private_data->resume.ctrl_reg1);
ERROR_CHECK(result);
}
result = mma8450_set_irq(mlsl_handle, pdata,
&private_data->resume,
TRUE, private_data->resume.irq_type);
ERROR_CHECK(result);
return result;
}
void set_ak89xx_enable(struct iio_dev *indio_dev, bool enable)
{
struct inv_ak89xx_state_s *st = iio_priv(indio_dev);
int result = 0;
unsigned char scale = 0;
if (st->compass_id == COMPASS_ID_AK8963)
scale = st->compass_scale;
if (enable) {
result = inv_serial_single_write(st, AK89XX_REG_CNTL,
(scale << 4) | AK89XX_CNTL_MODE_SNG_MEASURE);
if (result)
pr_err("%s, line=%d\n", __func__, __LINE__);
schedule_delayed_work(&st->work,
msecs_to_jiffies(st->delay));
} else {
cancel_delayed_work_sync(&st->work);
result = inv_serial_single_write(st, AK89XX_REG_CNTL,
(scale << 4) | AK89XX_CNTL_MODE_POWER_DOWN);
if (result)
pr_err("%s, line=%d\n", __func__, __LINE__);
mdelay(1); /* wait at least 100us */
}
}
/**
* @brief one-time device driver initialization function.
* If the driver is built as a kernel module, this function will be
* called when the module is loaded in the kernel.
* If the driver is built-in in the kernel, this function will be
* called at boot time.
*
* @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 INV_SUCCESS if successful or a non-zero error code.
*/
static int bma222_init(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
struct bma222_private_data *private_data;
private_data = (struct bma222_private_data *)
kzalloc(sizeof(struct bma222_private_data), GFP_KERNEL);
if (!private_data)
return INV_ERROR_MEMORY_EXAUSTED;
pdata->private_data = private_data;
result = inv_serial_single_write(mlsl_handle, pdata->address,
BMA222_SOFTRESET_REG, BMA222_SOFTRESET_MASK);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
msleep(1);
result = bma222_set_odr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 0);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = bma222_set_odr(mlsl_handle, pdata, &private_data->resume,
FALSE, 200000);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = bma222_set_fsr(mlsl_handle, pdata, &private_data->suspend,
FALSE, 2000);
result = bma222_set_fsr(mlsl_handle, pdata, &private_data->resume,
FALSE, 2000);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_serial_single_write(mlsl_handle, pdata->address,
BMA222_PWR_REG, BMA222_PWR_SLEEP_MASK);
if (result) {
LOG_RESULT_LOCATION(result);
return 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 kxtf9_set_odr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct kxtf9_config *config, int apply, long odr)
{
unsigned char bits;
int result = INV_SUCCESS;
/* Data sheet says there is 12.5 hz, but that seems to produce a single
* correct data value, thus we remove it from the table */
if (odr > 400000) {
config->odr = 800000;
bits = 0x06;
} else if (odr > 200000) {
config->odr = 400000;
bits = 0x05;
} else if (odr > 100000) {
config->odr = 200000;
bits = 0x04;
} else if (odr > 50000) {
config->odr = 100000;
bits = 0x03;
} else if (odr > 25000) {
config->odr = 50000;
bits = 0x02;
} else if (odr != 0) {
config->odr = 25000;
bits = 0x01;
} else {
config->odr = 0;
bits = 0;
}
if (odr != 0)
config->ctrl_reg1 |= 0x80;
else
config->ctrl_reg1 &= ~0x80;
config->reg_odr = bits;
kxtf9_set_dur(mlsl_handle, pdata, config, apply, config->dur);
if (apply) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
KXTF9_DATA_CTRL_REG,
config->reg_odr);
result = inv_serial_single_write(mlsl_handle, pdata->address,
KXTF9_CTRL_REG1, 0x40);
result = inv_serial_single_write(mlsl_handle, pdata->address,
KXTF9_CTRL_REG1,
config->ctrl_reg1);
}
return result;
}
/**
* @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 INV_SUCCESS if successful or a non-zero error code.
*/
static int adxl34x_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
/*
struct adxl34x_config *suspend_config =
&((struct adxl34x_private_data *)pdata->private_data)->suspend;
result = adxl34x_set_odr(mlsl_handle, pdata, suspend_config,
true, suspend_config->odr);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = adxl34x_set_fsr(mlsl_handle, pdata, suspend_config,
true, suspend_config->fsr);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
*/
/*
Page 25
When clearing the sleep bit, it is recommended that the part
be placed into standby mode and then set back to measurement mode
with a subsequent write.
This is done to ensure that the device is properly biased if sleep
mode is manually disabled; otherwise, the first few samples of data
after the sleep bit is cleared may have additional noise,
especially if the device was asleep when the bit was cleared. */
/* go in standy-by mode (suspends measurements) */
result = inv_serial_single_write(mlsl_handle, pdata->address,
ADXL34X_PWR_REG, ADXL34X_PWR_MEAS_MASK);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
/* and then in sleep */
result = inv_serial_single_write(mlsl_handle, pdata->address,
ADXL34X_PWR_REG,
ADXL34X_PWR_MEAS_MASK | ADXL34X_PWR_SLEEP_MASK);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
return result;
}
/**
* @brief Set the output data rate for the particular configuration.
*
* @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
* Config to modify with new ODR.
* @param apply
* whether to apply immediately or save the settings to be applied
* at the next resume.
* @param odr
* Output data rate in units of 1/1000Hz (mHz).
*
* @return INV_SUCCESS if successful or a non-zero error code.
*/
static int mma8450_set_odr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct mma8450_config *config,
int apply,
long odr)
{
unsigned char bits;
int result = INV_SUCCESS;
if (odr > 200000) {
config->odr = 400000;
bits = 0x00;
} else if (odr > 100000) {
config->odr = 200000;
bits = 0x04;
} else if (odr > 50000) {
config->odr = 100000;
bits = 0x08;
} else if (odr > 25000) {
config->odr = 50000;
bits = 0x0B;
} else if (odr > 12500) {
config->odr = 25000;
bits = 0x40; /* Sleep -> Auto wake mode */
} else if (odr > 1563) {
config->odr = 12500;
bits = 0x10;
} else if (odr > 0) {
config->odr = 1563;
bits = 0x14;
} else {
config->ctrl_reg1 = 0; /* Set FS1.FS2 to Standby */
config->odr = 0;
bits = 0;
}
config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0x3);
if (apply) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
ACCEL_MMA8450_CTRL_REG1, 0);
ERROR_CHECK(result);
result = inv_serial_single_write(mlsl_handle, pdata->address,
ACCEL_MMA8450_CTRL_REG1, config->ctrl_reg1);
ERROR_CHECK(result);
MPL_LOGV("ODR: %d mHz, 0x%02x\n",
config->odr, (int)config->ctrl_reg1);
}
return result;
}
/**
* @brief Set the output data rate for the particular configuration.
*
* @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
* Config to modify with new ODR.
* @param apply
* whether to apply immediately or save the settings to be applied
* at the next resume.
* @param odr
* Output data rate in units of 1/1000Hz (mHz).
*
* @return INV_SUCCESS if successful or a non-zero error code.
*/
static int bma150_set_odr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma150_config *config,
int apply,
long odr)
{
unsigned char odr_bits = 0;
unsigned char wup_bits = 0;
int result = INV_SUCCESS;
if (odr > 100000) {
config->odr = 190000;
odr_bits = 0x03;
} else if (odr > 50000) {
config->odr = 100000;
odr_bits = 0x02;
} else if (odr > 25000) {
config->odr = 50000;
odr_bits = 0x01;
} else if (odr > 0) {
config->odr = 25000;
odr_bits = 0x00;
} else {
config->odr = 0;
wup_bits = 0x00;
}
config->int_reg &= BMA150_INT_MASK_WUP;
config->ctrl_reg &= BMA150_CTRL_MASK_ODR;
config->ctrl_reg |= odr_bits;
MPL_LOGV("ODR: %d\n", config->odr);
if (apply) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
BMA150_CTRL_REG, config->ctrl_reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_serial_single_write(mlsl_handle, pdata->address,
BMA150_INT_REG, config->int_reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
return result;
}
int hscdtd004a_read(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata,
unsigned char *data)
{
unsigned char stat;
inv_error_t result = INV_SUCCESS;
int status = INV_SUCCESS;
/* Read status reg. to check if data is ready */
result =
inv_serial_read(mlsl_handle, pdata->address,
COMPASS_HSCDTD004A_STAT, 1, &stat);
ERROR_CHECK(result);
if (stat & 0x48) {
result =
inv_serial_read(mlsl_handle, pdata->address,
COMPASS_HSCDTD004A_DATAX, 6,
(unsigned char *)data);
ERROR_CHECK(result);
status = INV_SUCCESS;
} else if (stat & 0x68) {
status = INV_ERROR_COMPASS_DATA_OVERFLOW;
} else {
status = INV_ERROR_COMPASS_DATA_NOT_READY;
}
/* trigger next measurement read */
result =
inv_serial_single_write(mlsl_handle, pdata->address,
COMPASS_HSCDTD004A_CTRL3, 0x40);
ERROR_CHECK(result);
return status;
}
/* -------------------------------------------------------------------------- */
static int ami30x_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
unsigned char reg;
result =
inv_serial_read(mlsl_handle, pdata->address, AMI30X_REG_CNTL1,
1, ®);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
reg &= ~(AMI30X_BIT_CNTL1_PC1 | AMI30X_BIT_CNTL1_FS1);
result =
inv_serial_single_write(mlsl_handle, pdata->address,
AMI30X_REG_CNTL1, reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
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 INV_SUCCESS if successful or a non-zero error code.
*/
static int bma222_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
struct bma222_config *resume_config =
&((struct bma222_private_data *)pdata->private_data)->resume;
/* Soft reset */
result = inv_serial_single_write(mlsl_handle, pdata->address,
BMA222_SOFTRESET_REG, BMA222_SOFTRESET_MASK);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
msleep(10);
result = bma222_set_odr(mlsl_handle, pdata, resume_config,
TRUE, resume_config->odr);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = bma222_set_fsr(mlsl_handle, pdata, resume_config,
TRUE, resume_config->fsr);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
return result;
}
/**
* @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 INV_SUCCESS if successful or a non-zero error code.
*/
static int bma222_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result;
struct bma222_config *suspend_config =
&((struct bma222_private_data *)pdata->private_data)->suspend;
result = bma222_set_odr(mlsl_handle, pdata, suspend_config,
TRUE, suspend_config->odr);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = bma222_set_fsr(mlsl_handle, pdata, suspend_config,
TRUE, suspend_config->fsr);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_serial_single_write(mlsl_handle, pdata->address,
BMA222_PWR_REG, BMA222_PWR_SLEEP_MASK);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
msleep(3); /* 3 ms powerup time maximum */
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 INV_SUCCESS if successful or a non-zero error code.
*/
static int bma222_set_fsr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct bma222_config *config,
int apply,
long fsr)
{
int result = INV_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 = inv_serial_single_write(mlsl_handle, pdata->address,
BMA222_FSR_REG, reg_fsr_mask);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
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 INV_SUCCESS if successful or a non-zero error code.
*/
static int mma845x_resume(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
int result = INV_SUCCESS;
struct mma845x_private_data *private_data = pdata->private_data;
/* Full Scale */
if (private_data->resume.fsr == 4000)
slave->range.mantissa = 4;
else if (private_data->resume.fsr == 8000)
slave->range.mantissa = 8;
else
slave->range.mantissa = 2;
slave->range.fraction = 0;
result = mma845x_set_fsr(mlsl_handle, pdata,
&private_data->resume,
true, private_data->resume.fsr);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result = inv_serial_single_write(mlsl_handle, pdata->address,
ACCEL_MMA845X_CTRL_REG1,
private_data->resume.ctrl_reg1);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
return result;
}
int mantis_suspend(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata)
{
unsigned char reg;
int result;
struct mantis_private_data *private_data =
(struct mantis_private_data *)pdata->private_data;
result = mantis_set_odr(mlsl_handle, pdata, &private_data->suspend,
TRUE, private_data->suspend.odr);
ERROR_CHECK(result);
result = mantis_set_irq(mlsl_handle, pdata, &private_data->suspend,
TRUE, private_data->suspend.irq_type);
ERROR_CHECK(result);
result = inv_serial_read(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_2, 1, ®);
ERROR_CHECK(result);
reg |= (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_PWR_MGMT_2, reg);
ERROR_CHECK(result);
return INV_SUCCESS;
}
static int mantis_set_fsr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct mantis_config *config, long apply, long fsr)
{
unsigned char fsr_mask;
int result;
if (fsr <= 2000) {
config->fsr = 2000;
fsr_mask = 0x00;
} else if (fsr <= 4000) {
config->fsr = 4000;
fsr_mask = 0x08;
} else if (fsr <= 8000) {
config->fsr = 8000;
fsr_mask = 0x10;
} else { /* fsr = [8001, oo) */
config->fsr = 16000;
fsr_mask = 0x18;
}
if (apply) {
unsigned char reg;
result = inv_serial_read(mlsl_handle, pdata->address,
MPUREG_ACCEL_CONFIG, 1, ®);
ERROR_CHECK(result);
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_ACCEL_CONFIG,
reg | fsr_mask);
ERROR_CHECK(result);
MPL_LOGV("FSR: %d\n", config->fsr);
}
return INV_SUCCESS;
}
/**
* @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 INV_SUCCESS if successful or a non-zero error code.
*/
static int mma8450_set_fsr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct mma8450_config *config,
int apply,
long fsr)
{
unsigned char bits;
int result = INV_SUCCESS;
if (fsr <= 2000) {
bits = 0x01;
config->fsr = 2000;
} else if (fsr <= 4000) {
bits = 0x02;
config->fsr = 4000;
} else {
bits = 0x03;
config->fsr = 8000;
}
config->ctrl_reg1 = bits | (config->ctrl_reg1 & 0xFC);
if (apply) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
ACCEL_MMA8450_CTRL_REG1, config->ctrl_reg1);
ERROR_CHECK(result);
MPL_LOGV("FSR: %d mg\n", config->fsr);
}
return result;
}
static int mantis_set_irq(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct mantis_config *config, long apply,
long irq_type)
{
int result = INV_SUCCESS;
unsigned char reg_int_cfg;
switch (irq_type) {
case MPU_SLAVE_IRQ_TYPE_DATA_READY:
config->irq_type = irq_type;
reg_int_cfg = 0x01;
break;
/* todo: add MOTION, NO_MOTION, and FREEFALL */
case MPU_SLAVE_IRQ_TYPE_NONE:
/* Do nothing, not even set the interrupt because it is
shared with the gyro */
config->irq_type = irq_type;
return INV_SUCCESS;
break;
default:
return INV_ERROR_INVALID_PARAMETER;
break;
}
if (apply) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
MPUREG_INT_ENABLE,
reg_int_cfg);
ERROR_CHECK(result);
MPL_LOGV("irq_type: %d\n", config->irq_type);
}
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 = INV_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 = inv_serial_single_write(mlsl_handle, pdata->address,
LIS331_CTRL_REG4, reg1);
return result;
}
/**
* @internal
* @brief Retrieve the unique MPU device identifier from the internal OTP
* bank 0 memory.
* @param mlsl_handle
* serial interface handle to allow serial communication with the
* device, both gyro and accelerometer.
* @return 0 on success, a non-zero error code from the serial layer on error.
*/
static inv_error_t get_mpu_unique_id(void *mlsl_handle)
{
inv_error_t result;
unsigned char otp0[8];
result =
inv_serial_read_mem(mlsl_handle, mldl_cfg->mpu_chip_info->addr,
(BIT_PRFTCH_EN | BIT_CFG_USER_BANK | MPU_MEM_OTP_BANK_0) << 8 |
0x00, 6, otp0);
if (result)
goto close;
MPL_LOGI("\n");
MPL_LOGI("DIE_ID : %06X\n",
((int)otp0[1] << 8 | otp0[0]) & 0x1fff);
MPL_LOGI("WAFER_ID : %06X\n",
(((int)otp0[2] << 8 | otp0[1]) & 0x03ff ) >> 5);
MPL_LOGI("A_LOT_ID : %06X\n",
( ((int)otp0[4] << 16 | (int)otp0[3] << 8 |
otp0[2]) & 0x3ffff) >> 2);
MPL_LOGI("W_LOT_ID : %06X\n",
( ((int)otp0[5] << 8 | otp0[4]) & 0x3fff) >> 2);
MPL_LOGI("WP_ID : %06X\n",
( ((int)otp0[6] << 8 | otp0[5]) & 0x03ff) >> 7);
MPL_LOGI("REV_ID : %06X\n", otp0[6] >> 2);
MPL_LOGI("\n");
close:
result =
inv_serial_single_write(mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_BANK_SEL, 0x00);
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 INV_SUCCESS if successful or a non-zero error code.
*/
static int mma845x_set_fsr(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct mma845x_config *config,
int apply,
long fsr)
{
unsigned char bits;
int result = INV_SUCCESS;
if (fsr <= 2000) {
bits = 0x00;
config->fsr = 2000;
} else if (fsr <= 4000) {
bits = 0x01;
config->fsr = 4000;
} else {
bits = 0x02;
config->fsr = 8000;
}
if (apply) {
result = inv_serial_single_write(mlsl_handle, pdata->address,
ACCEL_MMA845X_XYZ_DATA_CFG,
bits);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
MPL_LOGV("FSR: %d mg\n", config->fsr);
}
return result;
}
/**
* @brief Stop the DMP running
*
* @return INV_SUCCESS or non-zero error code
*/
static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle)
{
unsigned char user_ctrl_reg;
int result;
if (mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)
return INV_SUCCESS;
result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
MPUREG_USER_CTRL, 1, &user_ctrl_reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
user_ctrl_reg = (user_ctrl_reg & (~BIT_FIFO_EN)) | BIT_FIFO_RST;
user_ctrl_reg = (user_ctrl_reg & (~BIT_DMP_EN)) | BIT_DMP_RST;
result = inv_serial_single_write(gyro_handle,
mldl_cfg->mpu_chip_info->addr,
MPUREG_USER_CTRL, user_ctrl_reg);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
mldl_cfg->inv_mpu_state->status |= MPU_DMP_IS_SUSPENDED;
return result;
}
/**
* Sets the IRQ to fire when one of the IRQ events occur. Threshold and
* duration will not be used uless the type is MOT or NMOT.
*
* @param config configuration to apply to, suspend or resume
* @param irq_type The type of IRQ. Valid values are
* - MPU_SLAVE_IRQ_TYPE_NONE
* - MPU_SLAVE_IRQ_TYPE_MOTION
* - MPU_SLAVE_IRQ_TYPE_DATA_READY
*/
static int kxtf9_set_irq(void *mlsl_handle,
struct ext_slave_platform_data *pdata,
struct kxtf9_config *config, int apply, long irq_type)
{
int result = INV_SUCCESS;
struct kxtf9_private_data *private_data = pdata->private_data;
config->irq_type = (unsigned char)irq_type;
config->ctrl_reg1 &= ~0x22;
if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
config->ctrl_reg1 |= 0x20;
config->reg_int_cfg1 = 0x38;
config->reg_int_cfg2 = 0x00;
} else if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) {
config->ctrl_reg1 |= 0x02;
if ((unsigned long)config ==
(unsigned long)&private_data->suspend)
config->reg_int_cfg1 = 0x34;
else
config->reg_int_cfg1 = 0x24;
config->reg_int_cfg2 = 0xE0;
} else {
config->reg_int_cfg1 = 0x00;
config->reg_int_cfg2 = 0x00;
}
if (apply) {
/* Must clear bit 7 before writing new configuration */
result = inv_serial_single_write(mlsl_handle, pdata->address,
KXTF9_CTRL_REG1, 0x40);
result = inv_serial_single_write(mlsl_handle, pdata->address,
KXTF9_INT_CTRL_REG1,
config->reg_int_cfg1);
result = inv_serial_single_write(mlsl_handle, pdata->address,
KXTF9_INT_CTRL_REG2,
config->reg_int_cfg2);
result = inv_serial_single_write(mlsl_handle, pdata->address,
KXTF9_CTRL_REG1,
config->ctrl_reg1);
}
MPL_LOGV("CTRL_REG1: %lx, INT_CFG1: %lx, INT_CFG2: %lx\n",
(unsigned long)config->ctrl_reg1,
(unsigned long)config->reg_int_cfg1,
(unsigned long)config->reg_int_cfg2);
return result;
}