/**
* @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;
}
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;
}
/* -------------------------------------------------------------------------- */
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;
}
/**
* @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;
}
static int kxtf9_read(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata,
unsigned char *data)
{
int result;
unsigned char reg;
result = inv_serial_read(mlsl_handle, pdata->address,
KXTF9_INT_SRC_REG2, 1, ®);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
if (!(reg & 0x10))
return INV_ERROR_ACCEL_DATA_NOT_READY;
result = inv_serial_read(mlsl_handle, pdata->address,
slave->read_reg, slave->read_len, data);
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;
}
static int inv_i2c_write(struct i2c_adapter *i2c_adap,
unsigned char address,
unsigned int len, unsigned char const *data)
{
struct i2c_msg msgs[1];
int res;
if (!data || !i2c_adap) {
LOG_RESULT_LOCATION(-EINVAL);
return -EINVAL;
}
msgs[0].addr = address;
msgs[0].flags = 0; /* write */
msgs[0].buf = (unsigned char *)data;
msgs[0].len = len;
res = i2c_transfer(i2c_adap, msgs, 1);
if (res < 1) {
if (res == 0)
res = -EIO;
LOG_RESULT_LOCATION(res);
return res;
} else
return 0;
}
int inv_serial_read(
void *sl_handle,
unsigned char slave_addr,
unsigned char register_addr,
unsigned short length,
unsigned char *data)
{
int result;
unsigned short bytes_read = 0;
if ((slave_addr & 0x7E) == DEFAULT_MPU_SLAVEADDR
&& (register_addr == MPUREG_FIFO_R_W ||
register_addr == MPUREG_MEM_R_W)) {
LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
return INV_ERROR_INVALID_PARAMETER;
}
while (bytes_read < length) {
unsigned short this_len =
min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
result = inv_i2c_read((struct i2c_adapter *)sl_handle,
slave_addr, register_addr + bytes_read,
this_len, &data[bytes_read]);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
bytes_read += this_len;
}
return 0;
}
/* -------------------------------------------------------------------------- */
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;
}
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;
}
/**
* @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;
}
static int mpu6050_input_set_fsr(struct mpu6050_input_data *data, int fsr)
{
unsigned char fsr_mask;
int result;
unsigned char reg;
if (fsr <= 2000) {
fsr_mask = 0x00;
} else if (fsr <= 4000) {
fsr_mask = 0x08;
} else if (fsr <= 8000) {
fsr_mask = 0x10;
} else { /* fsr = [8001, oo) */
fsr_mask = 0x18;
}
result =
mpu6050_i2c_read_reg(data->client,
MPUREG_ACCEL_CONFIG, 1, ®);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
result =
mpu6050_i2c_write_single_reg(data->client, MPUREG_ACCEL_CONFIG,
reg | fsr_mask);
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;
}
static int inv_i2c_read(struct i2c_adapter *i2c_adap,
unsigned char address, unsigned char reg,
unsigned int len, unsigned char *data)
{
struct i2c_msg msgs[2];
int res;
if (!data || !i2c_adap) {
LOG_RESULT_LOCATION(-EINVAL);
return -EINVAL;
}
msgs[0].addr = address;
msgs[0].flags = 0; /* write */
msgs[0].buf = ®
msgs[0].len = 1;
msgs[1].addr = address;
msgs[1].flags = I2C_M_RD;
msgs[1].buf = data;
msgs[1].len = len;
res = i2c_transfer(i2c_adap, msgs, 2);
if (res < 2) {
if (res >= 0)
res = -EIO;
LOG_RESULT_LOCATION(res);
return res;
} else
return 0;
}
/**
* @brief inv_set_control_sensitivity is used to set the sensitivity for a control
* signal.
*
* @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or
* inv_open_low_power_pedometer().
*
* @param controlSignal Indicates which control signal is being modified.
* Must be one of:
* - INV_CONTROL_1,
* - INV_CONTROL_2,
* - INV_CONTROL_3 or
* - INV_CONTROL_4.
*
* @param sensitivity The sensitivity of the control signal.
*
* @return error code
*/
inv_error_t inv_set_control_sensitivity(unsigned short controlSignal,
long sensitivity)
{
INVENSENSE_FUNC_START;
unsigned char regs[2];
long finalSens = 0;
inv_error_t result;
if (inv_get_state() < INV_STATE_DMP_OPENED)
return INV_ERROR_SM_IMPROPER_STATE;
finalSens = sensitivity * 100;
if (finalSens > 16384) {
finalSens = 16384;
}
regs[0] = (unsigned char)(finalSens / 256);
regs[1] = (unsigned char)(finalSens % 256);
switch (controlSignal) {
case INV_CONTROL_1:
result = inv_set_mpu_memory(KEY_D_0_224, 2, regs);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
cntrl_params.sensitivity[0] = (unsigned short)sensitivity;
break;
case INV_CONTROL_2:
result = inv_set_mpu_memory(KEY_D_0_228, 2, regs);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
cntrl_params.sensitivity[1] = (unsigned short)sensitivity;
break;
case INV_CONTROL_3:
result = inv_set_mpu_memory(KEY_D_0_232, 2, regs);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
cntrl_params.sensitivity[2] = (unsigned short)sensitivity;
break;
case INV_CONTROL_4:
result = inv_set_mpu_memory(KEY_D_0_236, 2, regs);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
cntrl_params.sensitivity[3] = (unsigned short)sensitivity;
break;
default:
break;
}
if (finalSens != sensitivity * 100) {
return INV_ERROR_INVALID_PARAMETER;
} else {
return INV_SUCCESS;
}
}
static int ak8963_get_config(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata,
struct ext_slave_config *data)
{
struct ak8963_private_data *private_data = pdata->private_data;
int result;
if (!data->data)
return INV_ERROR_INVALID_PARAMETER;
switch (data->key) {
case MPU_SLAVE_READ_REGISTERS:
{
unsigned char *serial_data =
(unsigned char *)data->data;
result =
inv_serial_read(mlsl_handle, pdata->address,
serial_data[0], data->len - 1,
&serial_data[1]);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
break;
}
case MPU_SLAVE_READ_SCALE:
{
unsigned char *serial_data =
(unsigned char *)data->data;
serial_data[0] = private_data->init.asa[0];
serial_data[1] = private_data->init.asa[1];
serial_data[2] = private_data->init.asa[2];
result = INV_SUCCESS;
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
break;
}
case MPU_SLAVE_CONFIG_ODR_SUSPEND:
(*(unsigned long *)data->data) = 0;
break;
case MPU_SLAVE_CONFIG_ODR_RESUME:
(*(unsigned long *)data->data) = 8000;
break;
case MPU_SLAVE_CONFIG_FSR_SUSPEND:
case MPU_SLAVE_CONFIG_FSR_RESUME:
case MPU_SLAVE_CONFIG_MOT_THS:
case MPU_SLAVE_CONFIG_NMOT_THS:
case MPU_SLAVE_CONFIG_MOT_DUR:
case MPU_SLAVE_CONFIG_NMOT_DUR:
case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
case MPU_SLAVE_CONFIG_IRQ_RESUME:
default:
return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
};
return INV_SUCCESS;
}
static int mpu_memory_read(struct i2c_adapter *i2c_adap,
unsigned char mpu_addr,
unsigned short mem_addr,
unsigned int len, unsigned char *data)
{
unsigned char bank[2];
unsigned char addr[2];
unsigned char buf;
struct i2c_msg msgs[4];
int res;
if (mpu_device_is_shutdown)
return -EIO;
if (!data || !i2c_adap) {
LOG_RESULT_LOCATION(-EINVAL);
return -EINVAL;
}
bank[0] = MPUREG_BANK_SEL;
bank[1] = mem_addr >> 8;
addr[0] = MPUREG_MEM_START_ADDR;
addr[1] = mem_addr & 0xFF;
buf = MPUREG_MEM_R_W;
/* write message */
msgs[0].addr = mpu_addr;
msgs[0].flags = 0;
msgs[0].buf = bank;
msgs[0].len = sizeof(bank);
msgs[1].addr = mpu_addr;
msgs[1].flags = 0;
msgs[1].buf = addr;
msgs[1].len = sizeof(addr);
msgs[2].addr = mpu_addr;
msgs[2].flags = 0;
msgs[2].buf = &buf;
msgs[2].len = 1;
msgs[3].addr = mpu_addr;
msgs[3].flags = I2C_M_RD;
msgs[3].buf = data;
msgs[3].len = len;
res = i2c_transfer(i2c_adap, msgs, 4);
if (res != 4) {
if (res >= 0)
res = -EIO;
LOG_RESULT_LOCATION(res);
return res;
} else
return 0;
}
static int mpu_memory_write(struct i2c_adapter *i2c_adap,
unsigned char mpu_addr,
unsigned short mem_addr,
unsigned int len, unsigned char const *data)
{
unsigned char bank[2];
unsigned char addr[2];
unsigned char buf[513];
struct i2c_msg msgs[3];
int res;
if (mpu_device_is_shutdown)
return -EIO;
if (!data || !i2c_adap) {
LOG_RESULT_LOCATION(-EINVAL);
return -EINVAL;
}
if (len >= (sizeof(buf) - 1)) {
LOG_RESULT_LOCATION(-ENOMEM);
return -ENOMEM;
}
bank[0] = MPUREG_BANK_SEL;
bank[1] = mem_addr >> 8;
addr[0] = MPUREG_MEM_START_ADDR;
addr[1] = mem_addr & 0xFF;
buf[0] = MPUREG_MEM_R_W;
memcpy(buf + 1, data, len);
/* write message */
msgs[0].addr = mpu_addr;
msgs[0].flags = 0;
msgs[0].buf = bank;
msgs[0].len = sizeof(bank);
msgs[1].addr = mpu_addr;
msgs[1].flags = 0;
msgs[1].buf = addr;
msgs[1].len = sizeof(addr);
msgs[2].addr = mpu_addr;
msgs[2].flags = 0;
msgs[2].buf = (unsigned char *)buf;
msgs[2].len = len + 1;
res = i2c_transfer(i2c_adap, msgs, 3);
if (res != 3) {
if (res >= 0)
res = -EIO;
LOG_RESULT_LOCATION(res);
return res;
} else
return 0;
}
/**
* @brief Get a sample of compass data from the device.
* @param data
* the buffer to store the compass raw data for
* X, Y, and Z axes.
* @return INV_SUCCESS or a non-zero error code.
*/
inv_error_t inv_get_compass_data(long *data)
{
inv_error_t result;
int ii;
struct mldl_cfg *mldl_cfg = inv_get_dl_config();
unsigned char *tmp = inv_obj.mag->raw_data;
if (mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->read_len >
sizeof(inv_obj.mag->raw_data)) {
LOG_RESULT_LOCATION(INV_ERROR_INVALID_CONFIGURATION);
return INV_ERROR_INVALID_CONFIGURATION;
}
if (mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_COMPASS]->bus ==
EXT_SLAVE_BUS_PRIMARY ||
!(mldl_cfg->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR)) {
/*--- read the compass sensor data.
The compass read function may return an INV_ERROR_COMPASS_* errors
when the data is not ready (read/refresh frequency mismatch) or
the internal data sampling timing of the device was not respected.
Returning the error code will make the sensor fusion supervisor
ignore this compass data sample. ---*/
result = (inv_error_t) inv_mpu_read_compass(mldl_cfg,
inv_get_serial_handle(),
inv_get_serial_handle(),
tmp);
if (result) {
if (COMPASS_DEBUG) {
MPL_LOGV("inv_mpu_read_compass returned %d\n", result);
}
return result;
}
for (ii = 0; ii < 3; ii++) {
if (EXT_SLAVE_BIG_ENDIAN ==
mldl_cfg->slave[EXT_SLAVE_TYPE_COMPASS]->endian)
data[ii] =
((long)((signed char)tmp[2 * ii]) << 8) + tmp[2 * ii + 1];
else
data[ii] =
((long)((signed char)tmp[2 * ii + 1]) << 8) + tmp[2 * ii];
}
} else { /* compass on the 2nd bus or DMP is off */
result = inv_get_external_sensor_data(data, 3);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
}
data[0] = inv_q30_mult(data[0], inv_obj.mag->asa[0]);
data[1] = inv_q30_mult(data[1], inv_obj.mag->asa[1]);
data[2] = inv_q30_mult(data[2], inv_obj.mag->asa[2]);
return INV_SUCCESS;
}
/**
* @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;
}
/**
* @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 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 inv_serial_write_fifo(
void *sl_handle,
unsigned char slave_addr,
unsigned short length,
unsigned char const *data)
{
int result;
unsigned char i2c_write[SERIAL_MAX_TRANSFER_SIZE + 1];
unsigned short bytes_written = 0;
if (length > FIFO_HW_SIZE) {
printk(KERN_ERR
"maximum fifo write length is %d\n", FIFO_HW_SIZE);
return INV_ERROR_INVALID_PARAMETER;
}
while (bytes_written < length) {
unsigned short this_len =
min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_written);
i2c_write[0] = MPUREG_FIFO_R_W;
memcpy(&i2c_write[1], &data[bytes_written], this_len);
result = inv_i2c_write((struct i2c_adapter *)sl_handle,
slave_addr, this_len + 1, i2c_write);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
bytes_written += this_len;
}
return 0;
}
inv_error_t inv_compass_check_range(void)
{
struct ext_slave_config config;
unsigned char data[3];
inv_error_t result;
config.key = MPU_SLAVE_RANGE_CHECK;
config.len = 3;
config.apply = TRUE;
config.data = data;
result = inv_mpu_get_compass_config(inv_get_dl_config(),
inv_get_serial_handle(),
inv_get_serial_handle(), &config);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
if(data[0] || data[1] || data[2]) {
/* some value clipped */
return INV_ERROR_COMPASS_DATA_ERROR;
}
return INV_SUCCESS;
}
static int ak8963_config(void *mlsl_handle,
struct ext_slave_descr *slave,
struct ext_slave_platform_data *pdata,
struct ext_slave_config *data)
{
int result;
if (!data->data)
return INV_ERROR_INVALID_PARAMETER;
switch (data->key) {
case MPU_SLAVE_WRITE_REGISTERS:
result = inv_serial_write(mlsl_handle, pdata->address,
data->len,
(unsigned char *)data->data);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
break;
case MPU_SLAVE_CONFIG_ODR_SUSPEND:
case MPU_SLAVE_CONFIG_ODR_RESUME:
case MPU_SLAVE_CONFIG_FSR_SUSPEND:
case MPU_SLAVE_CONFIG_FSR_RESUME:
case MPU_SLAVE_CONFIG_MOT_THS:
case MPU_SLAVE_CONFIG_NMOT_THS:
case MPU_SLAVE_CONFIG_MOT_DUR:
case MPU_SLAVE_CONFIG_NMOT_DUR:
case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
case MPU_SLAVE_CONFIG_IRQ_RESUME:
default:
return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
};
return INV_SUCCESS;
}
int inv_serial_read_mem(
void *sl_handle,
unsigned char slave_addr,
unsigned short mem_addr,
unsigned short length,
unsigned char *data)
{
int result;
unsigned short bytes_read = 0;
if ((mem_addr & 0xFF) + length > MPU_MEM_BANK_SIZE) {
printk
("memory read length (%d B) extends beyond its limits (%d) "
"if started at location %d\n", length,
MPU_MEM_BANK_SIZE, mem_addr & 0xFF);
return INV_ERROR_INVALID_PARAMETER;
}
while (bytes_read < length) {
unsigned short this_len =
min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
result =
mpu_memory_read((struct i2c_adapter *)sl_handle,
slave_addr, mem_addr + bytes_read,
this_len, &data[bytes_read]);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
bytes_read += this_len;
}
return 0;
}
/**
* @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 inv_set_control_func allows the user to choose how the sensor data will
* be processed in order to provide a control parameter.
* inv_set_control_func allows the user to choose which control functions
* will be incorporated in the sensor data processing.
* The control functions are:
* - INV_GRID
* Indicates that the user will be controlling a system that
* has discrete steps, such as icons, menu entries, pixels, etc.
* - INV_SMOOTH
* Indicates that noise from unintentional motion should be filtered out.
* - INV_DEAD_ZONE
* Indicates that a dead zone should be used, below which sensor
* data is set to zero.
* - INV_HYSTERESIS
* Indicates that, when INV_GRID is selected, hysteresis should
* be used to prevent the control signal from switching rapidly across
* elements of the grid.
*
* @pre inv_dmp_open() Must be called with MLDmpDefaultOpen() or
* inv_open_low_power_pedometer().
*
* @param function Indicates what functions will be used.
* Can be a bitwise OR of several values.
*
* @return Zero if the command is successful; an ML error code otherwise.
*/
inv_error_t inv_set_control_func(unsigned short function)
{
INVENSENSE_FUNC_START;
unsigned char regs[8] = { DINA06, DINA26,
DINA46, DINA66,
DINA0E, DINA2E,
DINA4E, DINA6E
};
unsigned char i;
inv_error_t result;
if (inv_get_state() < INV_STATE_DMP_OPENED)
return INV_ERROR_SM_IMPROPER_STATE;
if ((function & INV_SMOOTH) == 0) {
for (i = 0; i < 8; i++) {
regs[i] = DINA80 + 3;
}
}
result = inv_set_mpu_memory(KEY_CFG_4, 8, regs);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
cntrl_params.functions = function;
result = inv_set_dead_zone();
return result;
}
int inv_serial_read_fifo(
void *sl_handle,
unsigned char slave_addr,
unsigned short length,
unsigned char *data)
{
int result;
unsigned short bytes_read = 0;
if (length > FIFO_HW_SIZE) {
printk(KERN_ERR
"maximum fifo read length is %d\n", FIFO_HW_SIZE);
return INV_ERROR_INVALID_PARAMETER;
}
while (bytes_read < length) {
unsigned short this_len =
min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
result = inv_i2c_read((struct i2c_adapter *)sl_handle,
slave_addr, MPUREG_FIFO_R_W, this_len,
&data[bytes_read]);
if (result) {
LOG_RESULT_LOCATION(result);
return result;
}
bytes_read += this_len;
}
return 0;
}