/** * @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 lsm303dlha_suspend(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result = INV_SUCCESS; unsigned char reg1; unsigned char reg2; struct lsm303dlha_private_data *private_data = (struct lsm303dlha_private_data *)(pdata->private_data); result = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_CTRL_REG1, private_data->suspend.ctrl_reg1); result = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_CTRL_REG2, 0x0f); reg1 = 0x40; if (private_data->suspend.fsr == 8192) reg1 |= 0x30; else if (private_data->suspend.fsr == 4096) reg1 |= 0x10; /* else bits [4..5] are already zero */ result = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_CTRL_REG4, reg1); result = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_INT1_THS, private_data->suspend.reg_ths); result = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_INT1_DURATION, private_data->suspend.reg_dur); if (private_data->suspend.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) { reg1 = 0x02; reg2 = 0x00; } else if (private_data->suspend.irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) { reg1 = 0x00; reg2 = private_data->suspend.mot_int1_cfg; } else { reg1 = 0x00; reg2 = 0x00; } 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); result = inv_serial_read(mlsl_handle, pdata->address, LIS331_HP_FILTER_RESET, 1, ®1); return result; }
static int mpu6050_set_lp_mode(void *mlsl_handle, struct ext_slave_platform_data *pdata, unsigned char lpa_freq) { unsigned char b = 0; /* Reducing the duration setting for lp mode */ b = 1; inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_MOT_DUR, b); /* Setting the cycle bit and LPA wake up freq */ inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, 1, &b); b |= BIT_CYCLE | BIT_PD_PTAT; inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, b); inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_2, 1, &b); b |= lpa_freq & BITS_LPA_WAKE_CTRL; inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_2, b); return INV_SUCCESS; }
static int mpu6050_set_fp_mode(void *mlsl_handle, struct ext_slave_platform_data *pdata) { unsigned char b; struct mpu6050_private_data *private_data = (struct mpu6050_private_data *)pdata->private_data; /* Resetting the cycle bit and LPA wake up freq */ inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, 1, &b); b &= ~BIT_CYCLE & ~BIT_PD_PTAT; inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, b); inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_2, 1, &b); b &= ~BITS_LPA_WAKE_CTRL; inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_2, b); /* Resetting the duration setting for fp mode */ b = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB; inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_MOT_DUR, b); return INV_SUCCESS; }
//[ECID:000000] ZTEBSP wanghaifei start 20120221, add sys attr for product test static ssize_t mpu3050_id_show(struct device *dev, struct device_attribute *attr, char *buf) { unsigned char mpu_id = 0xFF; int result = -1; struct i2c_client *client = to_i2c_client(dev); struct i2c_adapter *mpu_adapter = client->adapter; unsigned char mpu_addr = 0x68; result = inv_serial_read(mpu_adapter, mpu_addr, MPUREG_WHO_AM_I, 1, &mpu_id); if (result) { return sprintf(buf, "0x%x\n", 0xFF); } else { return sprintf(buf, "0x%x\n", mpu_id); } }
/** * @brief read the sensor data from the device. * * @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. * @param data * a buffer to store the data read. * * @return INV_SUCCESS if successful or a non-zero error code. */ static int mma845x_read(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata, unsigned char *data) { int result; unsigned char local_data[7]; /* Status register + 6 bytes data */ result = inv_serial_read(mlsl_handle, pdata->address, slave->read_reg, sizeof(local_data), local_data); if (result) { LOG_RESULT_LOCATION(result); return result; } memcpy(data, &local_data[1], slave->read_len); return result; }
static int ak89xx_init(struct inv_ak89xx_state_s *st) { int result = 0; unsigned char serial_data[3]; result = inv_serial_single_write(st, AK89XX_REG_CNTL, AK89XX_CNTL_MODE_POWER_DOWN); if (result) { pr_err("%s, line=%d\n", __func__, __LINE__); return result; } /* Wait at least 100us */ udelay(100); result = inv_serial_single_write(st, AK89XX_REG_CNTL, AK89XX_CNTL_MODE_FUSE_ACCESS); if (result) { pr_err("%s, line=%d\n", __func__, __LINE__); return result; } /* Wait at least 200us */ udelay(200); result = inv_serial_read(st, AK89XX_FUSE_ASAX, 3, serial_data); if (result) { pr_err("%s, line=%d\n", __func__, __LINE__); return result; } st->asa[0] = serial_data[0]; st->asa[1] = serial_data[1]; st->asa[2] = serial_data[2]; result = inv_serial_single_write(st, AK89XX_REG_CNTL, AK89XX_CNTL_MODE_POWER_DOWN); if (result) { pr_err("%s, line=%d\n", __func__, __LINE__); return result; } udelay(100); return result; }
/** * @brief read the sensor data from the device. * * @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. * @param data * a buffer to store the data read. * * @return INV_SUCCESS if successful or a non-zero error code. */ static int mma8450_read(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata, unsigned char *data) { int result; unsigned char local_data[4]; /* Status register + 3 bytes data */ result = inv_serial_read(mlsl_handle, pdata->address, 0x00, sizeof(local_data), local_data); ERROR_CHECK(result); memcpy(data, &local_data[1], (slave->read_len) - 1); MPL_LOGV("Data Not Ready: %02x %02x %02x %02x\n", local_data[0], local_data[1], local_data[2], local_data[3]); if (!(local_data[0] & 0x04)) result = INV_ERROR_ACCEL_DATA_NOT_READY; return result; }
static int mpu6050_suspend(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { unsigned char reg; int result; struct mpu6050_private_data *private_data = (struct mpu6050_private_data *)pdata->private_data; result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->suspend, true, private_data->suspend.odr); if (result) { LOG_RESULT_LOCATION(result); return result; } result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->suspend, true, private_data->suspend.irq_type); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_2, 1, ®); if (result) { LOG_RESULT_LOCATION(result); return 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); if (result) { LOG_RESULT_LOCATION(result); return result; } return 0; }
static int mpu6050_set_fsr(void *mlsl_handle, struct ext_slave_platform_data *pdata, struct mpu6050_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, ®); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_CONFIG, reg | fsr_mask); if (result) { LOG_RESULT_LOCATION(result); return result; } MPL_LOGV("FSR: %d\n", config->fsr); } 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 in milli gees (mg). * * @return INV_SUCCESS if successful or a non-zero error code. */ static int adxl34x_set_fsr(void *mlsl_handle, struct ext_slave_platform_data *pdata, struct adxl34x_config *config, int apply, long fsr) { int result = INV_SUCCESS; if (fsr <= 2000) { config->fsr_reg_mask = 0x00; config->fsr = 2000; } else if (fsr <= 4000) { config->fsr_reg_mask = 0x01; config->fsr = 4000; } else if (fsr <= 8000) { config->fsr_reg_mask = 0x02; config->fsr = 8000; } else { /* 8001 -> oo */ config->fsr_reg_mask = 0x03; config->fsr = 16000; } if (apply) { unsigned char reg_df; result = inv_serial_read(mlsl_handle, pdata->address, ADXL34X_DATAFORMAT_REG, 1, ®_df); reg_df &= ~ADXL34X_DATAFORMAT_FSR_MASK; result = inv_serial_single_write(mlsl_handle, pdata->address, ADXL34X_DATAFORMAT_REG, reg_df | config->fsr_reg_mask); if (result) { LOG_RESULT_LOCATION(result); return result; } MPL_LOGV("FSR: %d mg\n", config->fsr); } return result; }
static ssize_t compass_id_show(struct device *dev, struct device_attribute *attr, char *buf) { unsigned char compass_id = 0xFF; int result = -1; struct i2c_client *client = to_i2c_client(dev); struct mpu_private_data *mpu = (struct mpu_private_data *)i2c_get_clientdata(client); struct ext_slave_platform_data **pdata_slave = mpu->mldl_cfg.pdata_slave; struct i2c_adapter *compass_adapter; unsigned char compass_addr; if (!pdata_slave[EXT_SLAVE_TYPE_COMPASS]) { return sprintf(buf, "0x%x\n", 0xFF); } compass_adapter = i2c_get_adapter(pdata_slave[EXT_SLAVE_TYPE_COMPASS]->adapt_num); compass_addr = pdata_slave[EXT_SLAVE_TYPE_COMPASS]->address; result = inv_serial_read(compass_adapter, compass_addr, AK8962_WHO_AM_I, 1, &compass_id); i2c_put_adapter(compass_adapter); if (result) { return sprintf(buf, "0x%x\n", 0xFF); } else { return sprintf(buf, "0x%x\n", compass_id); } }
static int mma845x_mod_probe(struct i2c_client *client, const struct i2c_device_id *devid) { struct ext_slave_platform_data *pdata; struct mma845x_mod_private_data *private_data; int result = 0; unsigned char product_id = 0xff; dev_info(&client->adapter->dev, "%s: %s\n", __func__, devid->name); if (!i2c_check_functionality(client->adapter, I2C_FUNC_I2C)) { result = -ENODEV; goto out_no_free; } pdata = client->dev.platform_data; if (!pdata) { dev_err(&client->adapter->dev, "Missing platform data for slave %s\n", devid->name); result = -EFAULT; goto out_no_free; } result = inv_serial_read(client->adapter, pdata->address, ACCEL_MMA845X_WHO_AM_I, sizeof(product_id), &product_id); printk("mma845x detect 0x%x\n", product_id); if (result || (product_id != ACCEL_MMA845X_ID)) { dev_err(&client->adapter->dev, "mma845x not found\n"); result = -ENODEV; goto out_no_free; } printk("mma845x find\n"); private_data = kzalloc(sizeof(*private_data), GFP_KERNEL); if (!private_data) { result = -ENOMEM; goto out_no_free; } i2c_set_clientdata(client, private_data); private_data->client = client; private_data->pdata = pdata; result = inv_mpu_register_slave(THIS_MODULE, client, pdata, mma845x_get_slave_descr); if (result) { dev_err(&client->adapter->dev, "Slave registration failed: %s, %d\n", devid->name, result); goto out_free_memory; } return result; out_free_memory: kfree(private_data); out_no_free: dev_err(&client->adapter->dev, "%s failed %d\n", __func__, result); return result; }
int mantis_resume(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result = INV_SUCCESS; unsigned char reg; struct mantis_private_data *private_data = (struct mantis_private_data *)pdata->private_data; result = inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, 1, ®); ERROR_CHECK(result); #if defined(CONFIG_MPU_SENSORS_MPU6050B1) if (reg & BIT_SLEEP) { result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, reg & ~BIT_SLEEP); ERROR_CHECK(result); } #else if ((reg & BITS_PWRSEL) != BITS_PWRSEL) { result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, reg | BITS_PWRSEL); ERROR_CHECK(result); } #endif inv_sleep(2); 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); /* settings */ result = mantis_set_fsr(mlsl_handle, pdata, &private_data->resume, TRUE, private_data->resume.fsr); ERROR_CHECK(result); result = mantis_set_odr(mlsl_handle, pdata, &private_data->resume, TRUE, private_data->resume.odr); ERROR_CHECK(result); result = mantis_set_irq(mlsl_handle, pdata, &private_data->resume, TRUE, private_data->resume.irq_type); ERROR_CHECK(result); /* motion, no_motion */ reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_THR_LSB; result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_MOT_THR, reg); ERROR_CHECK(result); reg = (unsigned char) ACCEL_ZRMOT_THR_LSB_CONVERSION(private_data->resume.ths); result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_ZRMOT_THR, reg); ERROR_CHECK(result); reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB; result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_MOT_DUR, reg); ERROR_CHECK(result); reg = (unsigned char)private_data->resume.ths / ACCEL_ZRMOT_DUR_LSB; result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_ZRMOT_DUR, reg); ERROR_CHECK(result); return result; }
static int lis3dh_resume(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result; unsigned char reg1; unsigned char reg2; struct lis3dh_private_data *private_data = pdata->private_data; result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_CTRL_REG1, private_data->resume.ctrl_reg1); if (result) { LOG_RESULT_LOCATION(result); return result; } msleep(6); /* Full Scale */ 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 = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_CTRL_REG4, reg1); if (result) { LOG_RESULT_LOCATION(result); return result; } if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) { reg1 = 0x10; reg2 = 0x00; } else if (private_data->resume.irq_type == MPU_SLAVE_IRQ_TYPE_MOTION) { reg1 = 0x40; reg2 = private_data->resume.mot_int1_cfg; } else { reg1 = 0x00; reg2 = 0x00; } result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_CTRL_REG3, reg1); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_INT1_THS, private_data->resume.reg_ths); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_INT1_DURATION, private_data->resume.reg_dur); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write(mlsl_handle, pdata->address, LIS3DH_INT1_CFG, reg2); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_read(mlsl_handle, pdata->address, LIS3DH_CTRL_REG6, 1, ®1); if (result) { LOG_RESULT_LOCATION(result); return result; } return result; }
int ak89xx_read(struct inv_ak89xx_state_s *st, short rawfixed[3]) { unsigned char regs[8]; unsigned char *stat = ®s[0]; unsigned char *stat2 = ®s[7]; int result = 0; int status = 0; result = inv_serial_read(st, AK89XX_REG_ST1, 8, regs); if (result) { pr_err("%s, line=%d\n", __func__, __LINE__); return result; } rawfixed[0] = (short)((regs[2]<<8) | regs[1]); rawfixed[1] = (short)((regs[4]<<8) | regs[3]); rawfixed[2] = (short)((regs[6]<<8) | regs[5]); /* * ST : data ready - * Measurement has been completed and data is ready to be read. */ if (*stat & 0x01) status = 0; /* * ST2 : data error - * occurs when data read is started outside of a readable period; * data read would not be correct. * Valid in continuous measurement mode only. * In single measurement mode this error should not occour but we * stil account for it and return an error, since the data would be * corrupted. * DERR bit is self-clearing when ST2 register is read. */ if (*stat2 & 0x04) status = 0x04; /* * ST2 : overflow - * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT. * This is likely to happen in presence of an external magnetic * disturbance; it indicates, the sensor data is incorrect and should * be ignored. * An error is returned. * HOFL bit clears when a new measurement starts. */ if (*stat2 & 0x08) status = 0x08; /* * ST : overrun - * the previous sample was not fetched and lost. * Valid in continuous measurement mode only. * In single measurement mode this error should not occour and we * don't consider this condition an error. * DOR bit is self-clearing when ST2 or any meas. data register is * read. */ if (*stat & 0x02) { /* status = INV_ERROR_COMPASS_DATA_UNDERFLOW; */ status = 0; } /* * 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. */ if (1) { unsigned char scale = 0; if (st->compass_id == COMPASS_ID_AK8963) scale = st->compass_scale; 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__); return result; } } else pr_err("%s, no next measure(0x%x,0x%x)\n", __func__, *stat, *stat2); if (status) pr_err("%s, line=%d, status=%d\n", __func__, __LINE__, status); return status; }
static int ak8963_read(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata, unsigned char *data) { unsigned char regs[8]; unsigned char *stat = ®s[0]; unsigned char *stat2 = ®s[7]; int result = INV_SUCCESS; int status = INV_SUCCESS; result = inv_serial_read(mlsl_handle, pdata->address, AK8963_REG_ST1, 8, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } /* Always return the data and the status registers */ memcpy(data, ®s[1], 6); data[6] = regs[0]; data[7] = regs[7]; /* * ST : data ready - * Measurement has been completed and data is ready to be read. */ if (*stat & 0x01) status = INV_SUCCESS; /* * ST2 : data error - * occurs when data read is started outside of a readable period; * data read would not be correct. * Valid in continuous measurement mode only. * In single measurement mode this error should not occour but we * stil account for it and return an error, since the data would be * corrupted. * DERR bit is self-clearing when ST2 register is read. */ // if (*stat2 & 0x04) // status = INV_ERROR_COMPASS_DATA_ERROR; /* * ST2 : overflow - * the sum of the absolute values of all axis |X|+|Y|+|Z| < 2400uT. * This is likely to happen in presence of an external magnetic * disturbance; it indicates, the sensor data is incorrect and should * be ignored. * An error is returned. * HOFL bit clears when a new measurement starts. */ if (*stat2 & 0x08) status = INV_ERROR_COMPASS_DATA_OVERFLOW; /* * ST : overrun - * the previous sample was not fetched and lost. * Valid in continuous measurement mode only. * In single measurement mode this error should not occour and we * don't consider this condition an error. * DOR bit is self-clearing when ST2 or any meas. data register is * read. */ if (*stat & 0x02) { /* status = INV_ERROR_COMPASS_DATA_UNDERFLOW; */ status = INV_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. */ if (*stat != 0x00 || (*stat2 & 0x08) != 0x00 ) { result = inv_serial_single_write( mlsl_handle, pdata->address, AK8963_REG_CNTL, AK8963_CNTL_MODE_SINGLE_MEASUREMENT); if (result) { LOG_RESULT_LOCATION(result); return result; } } return status; }
/* -------------------------------------------------------------------------- */ static int ak8963_init(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result; unsigned char serial_data[COMPASS_NUM_AXES]; printk("yemk:ak8963_init\n"); struct ak8963_private_data *private_data; private_data = (struct ak8963_private_data *) kzalloc(sizeof(struct ak8963_private_data), GFP_KERNEL); if (!private_data) return INV_ERROR_MEMORY_EXAUSTED; result = inv_serial_single_write(mlsl_handle, pdata->address, AK8963_REG_CNTL, AK8963_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, AK8963_REG_CNTL, AK8963_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, AK8963_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, AK8963_REG_CNTL, AK8963_CNTL_MODE_POWER_DOWN); if (result) { LOG_RESULT_LOCATION(result); return result; } printk("yemk:ak8963_init end\n"); udelay(100); printk(KERN_ERR "invensense: %s ok\n", __func__); return INV_SUCCESS; }
static int kxtf9_resume(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result = INV_SUCCESS; unsigned char data; struct kxtf9_private_data *private_data = pdata->private_data; /* Wake up */ result = inv_serial_single_write(mlsl_handle, pdata->address, KXTF9_CTRL_REG1, 0x40); if (result) { return result; } /* INT_CTRL_REG1: */ result = inv_serial_single_write(mlsl_handle, pdata->address, KXTF9_INT_CTRL_REG1, private_data->resume.reg_int_cfg1); if (result) { return result; } /* WUF_THRESH: */ result = inv_serial_single_write(mlsl_handle, pdata->address, KXTF9_WUF_THRESH, private_data->resume.reg_ths); if (result) { return result; } /* DATA_CTRL_REG */ result = inv_serial_single_write(mlsl_handle, pdata->address, KXTF9_DATA_CTRL_REG, private_data->resume.reg_odr); if (result) { return result; } /* WUF_TIMER */ result = inv_serial_single_write(mlsl_handle, pdata->address, KXTF9_WUF_TIMER, private_data->resume.reg_dur); if (result) { return result; } /* Normal operation */ result = inv_serial_single_write(mlsl_handle, pdata->address, KXTF9_CTRL_REG1, private_data->resume.ctrl_reg1); if (result) { return result; } result = inv_serial_read(mlsl_handle, pdata->address, KXTF9_INT_REL, 1, &data); if (result) { return result; } return INV_SUCCESS; }
/** * @brief Test the gyroscope sensor. * Implements the core logic of the MPU Self Test. * Produces the PASS/FAIL result. Loads the calculated gyro biases * and temperature datum into the corresponding pointers. * @param mlsl_handle * serial interface handle to allow serial communication with the * device, both gyro and accelerometer. * @param gyro_biases * output pointer to store the initial bias calculation provided * by the MPU Self Test. Requires 3 elements for gyro X, Y, * and Z. * @param temp_avg * output pointer to store the initial average temperature as * provided by the MPU Self Test. * @param perform_full_test * If 1: * Complete calibration test: * Calculate offset, drive frequency, and noise and compare it * against set thresholds. * When 0: * Skip the noise and drive frequency calculation, * simply calculate the gyro biases. * * @return 0 on success. * On error, the return value is a bitmask representing: * 0, 1, 2 Failures with PLLs on X, Y, Z gyros respectively * (decimal values will be 1, 2, 4 respectively). * 3, 4, 5 Excessive offset with X, Y, Z gyros respectively * (decimal values will be 8, 16, 32 respectively). * 6, 7, 8 Excessive noise with X, Y, Z gyros respectively * (decimal values will be 64, 128, 256 respectively). * 9 If any of the RMS noise values is zero, it may be * due to a non-functional gyro or FIFO/register failure. * (decimal value will be 512). */ int test_gyro(void *mlsl_handle, short gyro_biases[3], short *temp_avg, uint_fast8_t perform_full_test) { int ret_val = 0; inv_error_t result; int total_count = 0; int total_count_axis[3] = {0, 0, 0}; int packet_count; short x[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0}; short y[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0}; short z[DEF_PERIOD_CAL * DEF_TESTS_PER_AXIS / 8 * 4] = {0}; int temperature = 0; float avg[3]; float rms[3]; unsigned long test_start = inv_get_tick_count(); int i, j, tmp; char tmpStr[200]; unsigned char regs[7] = {0}; /* make sure the DMP is disabled first */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_USER_CTRL, 0x00); if (result) { LOG_RESULT_LOCATION(result); return result; } /* reset the gyro offset values */ regs[0] = MPUREG_XG_OFFS_USRH; result = inv_serial_write(mlsl_handle, mldl_cfg->mpu_chip_info->addr, 6, regs); if (result) { LOG_RESULT_LOCATION(result); return result; } /* sample rate */ if (perform_full_test) { /* = 8ms */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_SMPLRT_DIV, 0x07); test_setup.bias_thresh = (int)( DEF_BIAS_THRESH_CAL * test_setup.gyro_sens); } else { /* = 1ms */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_SMPLRT_DIV, 0x00); test_setup.bias_thresh = (int)( DEF_BIAS_THRESH_SELF * test_setup.gyro_sens); } if (result) { LOG_RESULT_LOCATION(result); return result; } regs[0] = 0x03; /* filter = 42Hz, analog_sample rate = 1 KHz */ switch (test_setup.gyro_fs) { case 2000: regs[0] |= 0x18; break; case 1000: regs[0] |= 0x10; break; case 500: regs[0] |= 0x08; break; case 250: default: regs[0] |= 0x00; break; } result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_CONFIG, regs[0]); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_INT_ENABLE, 0x00); if (result) { LOG_RESULT_LOCATION(result); return result; } /* 1st, timing test */ for (j = 0; j < 3; j++) { MPL_LOGI("Collecting gyro data from %s gyro PLL\n", a_name[j]); /* turn on all gyros, use gyro X for clocking Set to Y and Z for 2nd and 3rd iteration */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_PWR_MGMT_1, j + 1); if (result) { LOG_RESULT_LOCATION(result); return result; } /* wait for 2 ms after switching clock source */ usleep(2000); /* enable & reset FIFO */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_USER_CTRL, BIT_FIFO_EN | BIT_FIFO_RST); if (result) { LOG_RESULT_LOCATION(result); return result; } tmp = test_setup.tests_per_axis; while (tmp-- > 0) { const unsigned char fifo_en_reg = MPUREG_FIFO_EN; /* enable XYZ gyro in FIFO and nothing else */ result = inv_serial_single_write(mlsl_handle, mldl_cfg->mpu_chip_info->addr, fifo_en_reg, BIT_GYRO_XOUT | BIT_GYRO_YOUT | BIT_GYRO_ZOUT); if (result) { LOG_RESULT_LOCATION(result); return result; } /* wait one period for data */ if (perform_full_test) usleep(DEF_PERIOD_CAL * 1000); else usleep(DEF_PERIOD_SELF * 1000); /* stop storing gyro in the FIFO */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, fifo_en_reg, 0x00); if (result) { LOG_RESULT_LOCATION(result); return result; } /* Getting number of bytes in FIFO */ result = inv_serial_read( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_FIFO_COUNTH, 2, dataout); if (result) { LOG_RESULT_LOCATION(result); return result; } /* number of 6 B packets in the FIFO */ packet_count = inv_big8_to_int16(dataout) / 6; sprintf(tmpStr, "Packet Count: %d - ", packet_count); if (abs(packet_count - test_setup.packet_thresh) <= /* Within total_timing_tol % range, rounded up */ (int)(test_setup.total_timing_tol * test_setup.packet_thresh + 1)) { for (i = 0; i < packet_count; i++) { /* getting FIFO data */ result = inv_serial_read_fifo(mlsl_handle, mldl_cfg->mpu_chip_info->addr, 6, dataout); if (result) { LOG_RESULT_LOCATION(result); return result; } x[total_count + i] = inv_big8_to_int16(&dataout[0]); y[total_count + i] = inv_big8_to_int16(&dataout[2]); z[total_count + i] = inv_big8_to_int16(&dataout[4]); if (VERBOSE_OUT) { MPL_LOGI("Gyros %-4d : %+13d %+13d %+13d\n", total_count + i, x[total_count + i], y[total_count + i], z[total_count + i]); } } total_count += packet_count; total_count_axis[j] += packet_count; sprintf(tmpStr, "%sOK", tmpStr); } else { ret_val |= 1 << j; sprintf(tmpStr, "%sNOK - samples ignored", tmpStr); } MPL_LOGI("%s\n", tmpStr); } /* remove gyros from FIFO */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_FIFO_EN, 0x00); if (result) { LOG_RESULT_LOCATION(result); return result; } /* Read Temperature */ result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_TEMP_OUT_H, 2, dataout); if (result) { LOG_RESULT_LOCATION(result); return result; } temperature += (short)inv_big8_to_int16(dataout); } MPL_LOGI("\n"); MPL_LOGI("Total %d samples\n", total_count); MPL_LOGI("\n"); /* 2nd, check bias from X, Y, and Z PLL clock source */ tmp = total_count != 0 ? total_count : 1; for (i = 0, avg[X] = .0f, avg[Y] = .0f, avg[Z] = .0f; i < total_count; i++) { avg[X] += 1.f * x[i] / tmp; avg[Y] += 1.f * y[i] / tmp; avg[Z] += 1.f * z[i] / tmp; } MPL_LOGI("bias : %+13.3f %+13.3f %+13.3f (LSB)\n", avg[X], avg[Y], avg[Z]); if (VERBOSE_OUT) { MPL_LOGI(" : %+13.3f %+13.3f %+13.3f (dps)\n", avg[X] / adj_gyro_sens, avg[Y] / adj_gyro_sens, avg[Z] / adj_gyro_sens); } for (j = 0; j < 3; j++) { if (fabs(avg[j]) > test_setup.bias_thresh) { MPL_LOGI("%s-Gyro bias (%.0f) exceeded threshold " "(threshold = %d)\n", a_name[j], avg[j], test_setup.bias_thresh); ret_val |= 1 << (3+j); } } /* 3rd, check RMS for dead gyros If any of the RMS noise value returns zero, then we might have dead gyro or FIFO/register failure, the part is sleeping, or the part is not responsive */ for (i = 0, rms[X] = 0.f, rms[Y] = 0.f, rms[Z] = 0.f; i < total_count; i++) { rms[X] += (x[i] - avg[X]) * (x[i] - avg[X]); rms[Y] += (y[i] - avg[Y]) * (y[i] - avg[Y]); rms[Z] += (z[i] - avg[Z]) * (z[i] - avg[Z]); } if (rms[X] == 0 || rms[Y] == 0 || rms[Z] == 0) { ret_val |= 1 << 9; } /* 4th, temperature average */ temperature /= 3; if (VERBOSE_OUT) MPL_LOGI("Temperature : %+13.3f %13s %13s (deg. C)\n", (float)inv_decode_temperature(temperature) / (1L << 16), "", ""); /* load into final storage */ *temp_avg = (short)temperature; gyro_biases[X] = FLOAT_TO_SHORT(avg[X]); gyro_biases[Y] = FLOAT_TO_SHORT(avg[Y]); gyro_biases[Z] = FLOAT_TO_SHORT(avg[Z]); MPL_LOGI("\n"); MPL_LOGI("Test time : %ld ms\n", inv_get_tick_count() - test_start); return ret_val; }
static int lsm303dlx_m_read(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata, unsigned char *data) { unsigned char stat; int result = INV_SUCCESS; short axis_fixed; /* Read status reg. to check if data is ready */ result = inv_serial_read(mlsl_handle, pdata->address, LSM_REG_STATUS, 1, &stat); if (result) { LOG_RESULT_LOCATION(result); return result; } if (stat & 0x01) { result = inv_serial_read(mlsl_handle, pdata->address, LSM_REG_X_M, 6, (unsigned char *)data); if (result) { LOG_RESULT_LOCATION(result); return result; } /*drop data if overflows */ if ((data[0] == 0xf0) || (data[2] == 0xf0) || (data[4] == 0xf0)) { /* trigger next measurement read */ result = inv_serial_single_write(mlsl_handle, pdata->address, LSM_REG_MODE, LSM_MODE_SINGLE); if (result) { LOG_RESULT_LOCATION(result); return result; } return INV_ERROR_COMPASS_DATA_OVERFLOW; } /* convert to fixed point and apply sensitivity correction for Z-axis */ axis_fixed = (short)((unsigned short)data[5] + (unsigned short)data[4] * 256); /* scale up by 1.125 (36/32) approximate of 1.122 (320/285) */ if (slave->id == COMPASS_ID_LSM303DLM) { /* NOTE/IMPORTANT: lsm303dlm compass axis definition doesn't respect the right hand rule. We invert the sign of the Z axis to fix that. */ axis_fixed = (short)(-1 * axis_fixed * 36); } else { axis_fixed = (short)(axis_fixed * 36); } data[4] = axis_fixed >> 8; data[5] = axis_fixed & 0xFF; axis_fixed = (short)((unsigned short)data[3] + (unsigned short)data[2] * 256); axis_fixed = (short)(axis_fixed * 32); data[2] = axis_fixed >> 8; data[3] = axis_fixed & 0xFF; axis_fixed = (short)((unsigned short)data[1] + (unsigned short)data[0] * 256); axis_fixed = (short)(axis_fixed * 32); data[0] = axis_fixed >> 8; data[1] = axis_fixed & 0xFF; /* trigger next measurement read */ result = inv_serial_single_write(mlsl_handle, pdata->address, LSM_REG_MODE, LSM_MODE_SINGLE); if (result) { LOG_RESULT_LOCATION(result); return result; } return INV_SUCCESS; } else {
int hmc5883_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; unsigned char tmp; short axisFixed; /* Read status reg. to check if data is ready */ result = inv_serial_read(mlsl_handle, pdata->address, HMC_REG_STATUS, 1, &stat); ERROR_CHECK(result); if (stat & 0x01) { result = inv_serial_read(mlsl_handle, pdata->address, HMC_REG_X_M, 6, (unsigned char *)data); ERROR_CHECK(result); /* switch YZ axis to proper position */ tmp = data[2]; data[2] = data[4]; data[4] = tmp; tmp = data[3]; data[3] = data[5]; data[5] = tmp; /*drop data if overflows */ if ((data[0] == 0xf0) || (data[2] == 0xf0) || (data[4] == 0xf0)) { /* trigger next measurement read */ result = inv_serial_single_write(mlsl_handle, pdata->address, HMC_REG_MODE, HMC_MODE_SINGLE); ERROR_CHECK(result); return INV_ERROR_COMPASS_DATA_OVERFLOW; } /* convert to fixed point and apply sensitivity correction for Z-axis */ axisFixed = (short)((unsigned short)data[5] + (unsigned short)data[4] * 256); /* scale up by 1.125 (36/32) */ axisFixed = (short)(axisFixed * 36); data[4] = axisFixed >> 8; data[5] = axisFixed & 0xFF; axisFixed = (short)((unsigned short)data[3] + (unsigned short)data[2] * 256); axisFixed = (short)(axisFixed * 32); data[2] = axisFixed >> 8; data[3] = axisFixed & 0xFF; axisFixed = (short)((unsigned short)data[1] + (unsigned short)data[0] * 256); axisFixed = (short)(axisFixed * 32); data[0] = axisFixed >> 8; data[1] = axisFixed & 0xFF; /* trigger next measurement read */ result = inv_serial_single_write(mlsl_handle, pdata->address, HMC_REG_MODE, HMC_MODE_SINGLE); ERROR_CHECK(result); return INV_SUCCESS; } else {
/** * Record the odr for use in computing duration values. * * @param config Config to set, suspend or resume structure * @param odr output data rate in 1/1000 hz */ static int mpu6050_set_odr(void *mlsl_handle, struct ext_slave_platform_data *pdata, struct mpu6050_config *config, long apply, long odr) { int result; unsigned char b; unsigned char lpa_freq = 1; /* Default value */ long base; int total_divider; struct mpu6050_private_data *private_data = (struct mpu6050_private_data *)pdata->private_data; struct mldl_cfg *mldl_cfg_ref = (struct mldl_cfg *)private_data->mldl_cfg_ref; if (mldl_cfg_ref) { base = 1000 * inv_mpu_get_sampling_rate_hz(mldl_cfg_ref->mpu_gyro_cfg) * (mldl_cfg_ref->mpu_gyro_cfg->divider + 1); } else { /* have no reference to mldl_cfg => assume base rate is 1000 */ base = 1000000L; } if (odr != 0) { total_divider = (base / odr) - 1; /* final odr MAY be different from requested odr due to integer truncation */ config->odr = base / (total_divider + 1); } else { config->odr = 0; return 0; } /* if the DMP and/or gyros are on, don't set the ODR => the DMP/gyro mldl_cfg->divider setting will handle it */ if (apply && (mldl_cfg_ref && !(mldl_cfg_ref->inv_mpu_cfg->requested_sensors & INV_DMP_PROCESSOR))) { result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_SMPLRT_DIV, (unsigned char)total_divider); if (result) { LOG_RESULT_LOCATION(result); return result; } MPL_LOGI("ODR : %d mHz\n", config->odr); } /* Decide whether to put accel in LP mode or pull out of LP mode based on the odr. */ switch (odr) { case 1000: lpa_freq = BITS_LPA_WAKE_1HZ; break; case 2000: lpa_freq = BITS_LPA_WAKE_2HZ; break; case 10000: lpa_freq = BITS_LPA_WAKE_10HZ; break; case 40000: lpa_freq = BITS_LPA_WAKE_40HZ; break; default: inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, 1, &b); b &= BIT_CYCLE; if (b == BIT_CYCLE) { MPL_LOGI(" Accel LP - > FP mode. \n "); mpu6050_set_fp_mode(mlsl_handle, pdata); } } /* If lpa_freq default value was changed, set into LP mode */ if (lpa_freq != 1) { MPL_LOGI(" Accel FP - > LP mode. \n "); mpu6050_set_lp_mode(mlsl_handle, pdata, lpa_freq); } return 0; }
/** * @brief Starts the DMP running * * @return INV_SUCCESS or non-zero error code */ static int dmp_start(struct mldl_cfg *mldl_cfg, void *mlsl_handle) { unsigned char user_ctrl_reg; int result; if ((!(mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) && mldl_cfg->mpu_gyro_cfg->dmp_enable) || ((mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED) && !mldl_cfg->mpu_gyro_cfg->dmp_enable)) return INV_SUCCESS; result = inv_serial_read(mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_USER_CTRL, 1, &user_ctrl_reg); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_USER_CTRL, ((user_ctrl_reg & (~BIT_FIFO_EN)) | BIT_FIFO_RST)); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_USER_CTRL, user_ctrl_reg); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_read(mlsl_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 |= BIT_DMP_EN; if (mldl_cfg->mpu_gyro_cfg->fifo_enable) user_ctrl_reg |= BIT_FIFO_EN; else user_ctrl_reg &= ~BIT_FIFO_EN; user_ctrl_reg |= BIT_DMP_RST; result = inv_serial_single_write( mlsl_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 lis331dlh_resume(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result = INV_SUCCESS; unsigned char reg1; unsigned char reg2; struct lis331dlh_private_data *private_data = (struct lis331dlh_private_data *)(pdata->private_data); result = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_CTRL_REG1, private_data->resume.ctrl_reg1); if (result) { LOG_RESULT_LOCATION(result); return result; } msleep(6); /* Full Scale */ reg1 = 0x40; if (private_data->resume.fsr == 8192) reg1 |= 0x30; else if (private_data->resume.fsr == 4096) reg1 |= 0x10; result = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_CTRL_REG4, reg1); if (result) { LOG_RESULT_LOCATION(result); return result; } /* Configure high pass filter */ result = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_CTRL_REG2, 0x0F); if (result) { LOG_RESULT_LOCATION(result); return 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 = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_CTRL_REG3, reg1); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_INT1_THS, private_data->resume.reg_ths); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_INT1_DURATION, private_data->resume.reg_dur); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write(mlsl_handle, pdata->address, LIS331_INT1_CFG, reg2); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_read(mlsl_handle, pdata->address, LIS331_HP_FILTER_RESET, 1, ®1); if (result) { LOG_RESULT_LOCATION(result); return result; } return result; }
static int mpu6050_resume(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result; unsigned char reg; struct mpu6050_private_data *private_data = (struct mpu6050_private_data *)pdata->private_data; result = inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, 1, ®); if (result) { LOG_RESULT_LOCATION(result); return result; } if (reg & BIT_SLEEP) { result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_1, reg & ~BIT_SLEEP); if (result) { LOG_RESULT_LOCATION(result); return result; } } msleep(2); result = inv_serial_read(mlsl_handle, pdata->address, MPUREG_PWR_MGMT_2, 1, ®); if (result) { LOG_RESULT_LOCATION(result); return 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); if (result) { LOG_RESULT_LOCATION(result); return result; } /* settings */ result = mpu6050_set_fsr(mlsl_handle, pdata, &private_data->resume, true, private_data->resume.fsr); if (result) { LOG_RESULT_LOCATION(result); return result; } result = mpu6050_set_odr(mlsl_handle, pdata, &private_data->resume, true, private_data->resume.odr); if (result) { LOG_RESULT_LOCATION(result); return result; } result = mpu6050_set_irq(mlsl_handle, pdata, &private_data->resume, true, private_data->resume.irq_type); /* motion, no_motion */ /* TODO : port these in their respective _set_thrs and _set_dur functions and use the APPLY paremeter to apply just like _set_odr, _set_irq, and _set_fsr. */ reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_THR_LSB; result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_MOT_THR, reg); if (result) { LOG_RESULT_LOCATION(result); return result; } reg = (unsigned char) ACCEL_ZRMOT_THR_LSB_CONVERSION(private_data->resume.ths); result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_ZRMOT_THR, reg); if (result) { LOG_RESULT_LOCATION(result); return result; } reg = (unsigned char)private_data->suspend.ths / ACCEL_MOT_DUR_LSB; result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_MOT_DUR, reg); if (result) { LOG_RESULT_LOCATION(result); return result; } reg = (unsigned char)private_data->resume.ths / ACCEL_ZRMOT_DUR_LSB; result = inv_serial_single_write(mlsl_handle, pdata->address, MPUREG_ACCEL_ZRMOT_DUR, reg); if (result) { LOG_RESULT_LOCATION(result); return result; } return 0; }
/** * @internal * @brief used to get the FIFO data. * @param length * Number of bytes to read from the FIFO. * @param buffer * the bytes of FIFO data. * Note that this buffer <b>must</b> be large enough * to store and additional trailing FIFO footer when * expected. The callers must make sure enough space * is allocated. * @return number of valid bytes of data. **/ uint_fast16_t inv_get_fifo(uint_fast16_t length, unsigned char *buffer) { INVENSENSE_FUNC_START; inv_error_t result; uint_fast16_t inFifo; uint_fast16_t toRead; int_fast8_t kk; toRead = length - FIFO_FOOTER_SIZE + fifo_objHW.fifoCount; /*---- make sure length is correct ----*/ if (length > MAX_FIFO_LENGTH || toRead > length || NULL == buffer) { fifo_objHW.fifoError = INV_ERROR_INVALID_PARAMETER; return 0; } result = inv_get_fifo_length(&inFifo); if (INV_SUCCESS != result) { fifo_objHW.fifoError = result; return 0; } // fifo_objHW.fifoCount is the footer size left in the buffer, or // 0 if this is the first time reading the fifo since it was reset if (inFifo < length + fifo_objHW.fifoCount) { fifo_objHW.fifoError = INV_SUCCESS; return 0; } // if a trailing fifo count is expected - start storing data 2 bytes before result = inv_read_fifo(fifo_objHW.fifoCount > 0 ? buffer : buffer + FIFO_FOOTER_SIZE, toRead); if (INV_SUCCESS != result) { fifo_objHW.fifoError = result; return 0; } // Make sure the fifo didn't overflow before or during the read result = inv_serial_read(inv_get_serial_handle(), inv_get_mpu_slave_addr(), MPUREG_INT_STATUS, 1, &fifo_objHW.fifoOverflow); if (INV_SUCCESS != result) { fifo_objHW.fifoError = result; return 0; } if (fifo_objHW.fifoOverflow & BIT_INT_STATUS_FIFO_OVERLOW) { MPL_LOGV("Resetting Fifo : Overflow\n"); inv_reset_fifo(); fifo_objHW.fifoError = INV_ERROR_FIFO_OVERFLOW; return 0; } /* Check the Footer value to give us a chance at making sure data * didn't get corrupted */ for (kk = 0; kk < fifo_objHW.fifoCount; ++kk) { if (buffer[kk] != gFifoFooter[kk]) { MPL_LOGV("Resetting Fifo : Invalid footer : 0x%02x 0x%02x\n", buffer[0], buffer[1]); _fifoDebug(char out[200]; MPL_LOGW("fifoCount : %d\n", fifo_objHW.fifoCount); sprintf(out, "0x"); for (kk = 0; kk < (int)toRead; kk++) { sprintf(out, "%s%02X", out, buffer[kk]);} MPL_LOGW("%s\n", out);) inv_reset_fifo(); fifo_objHW.fifoError = INV_ERROR_FIFO_FOOTER; return 0; }
static int mpu3050_set_i2c_bypass(struct mldl_cfg *mldl_cfg, void *mlsl_handle, unsigned char enable) { unsigned char b; int result; unsigned char status = mldl_cfg->inv_mpu_state->status; if ((status & MPU_GYRO_IS_BYPASSED && enable) || (!(status & MPU_GYRO_IS_BYPASSED) && !enable)) return INV_SUCCESS; /*---- get current 'USER_CTRL' into b ----*/ result = inv_serial_read( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_USER_CTRL, 1, &b); if (result) { LOG_RESULT_LOCATION(result); return result; } b &= ~BIT_AUX_IF_EN; if (!enable) { result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_USER_CTRL, (b | BIT_AUX_IF_EN)); if (result) { LOG_RESULT_LOCATION(result); return 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 = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_AUX_SLV_ADDR, 0x7F); if (result) { LOG_RESULT_LOCATION(result); return result; } /* * 2) wait enough time for a nack to occur, then go into * bypass mode: */ msleep(2); result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_USER_CTRL, (b)); if (result) { LOG_RESULT_LOCATION(result); return result; } /* * 3) wait for up to one MPU cycle then restore the slave * address */ msleep(inv_mpu_get_sampling_period_us(mldl_cfg->mpu_gyro_cfg) / 1000); result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_AUX_SLV_ADDR, mldl_cfg->pdata_slave[EXT_SLAVE_TYPE_ACCEL] ->address); if (result) { LOG_RESULT_LOCATION(result); return result; } /* * 4) reset the ime interface */ result = inv_serial_single_write( mlsl_handle, mldl_cfg->mpu_chip_info->addr, MPUREG_USER_CTRL, (b | BIT_AUX_IF_RST)); if (result) { LOG_RESULT_LOCATION(result); return result; } msleep(2); } if (enable) mldl_cfg->inv_mpu_state->status |= MPU_GYRO_IS_BYPASSED; else mldl_cfg->inv_mpu_state->status &= ~MPU_GYRO_IS_BYPASSED; 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 *) inv_malloc(sizeof(struct ak8975_private_data)); 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); ERROR_CHECK(result); /* Wait at least 100us */ #ifdef __KERNEL__ udelay(100); #else inv_sleep(1); #endif result = inv_serial_single_write(mlsl_handle, pdata->address, AK8975_REG_CNTL, AK8975_CNTL_MODE_FUSE_ROM_ACCESS); ERROR_CHECK(result); /* Wait at least 200us */ #ifdef __KERNEL__ udelay(200); #else inv_sleep(1); #endif result = inv_serial_read(mlsl_handle, pdata->address, AK8975_REG_ASAX, COMPASS_NUM_AXES, serial_data); ERROR_CHECK(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); ERROR_CHECK(result); #ifdef __KERNEL__ udelay(100); #else inv_sleep(1); #endif return INV_SUCCESS; }
static int mpu_handle_mlsl(void *sl_handle, unsigned char addr, unsigned int cmd, struct mpu_read_write __user *usr_msg) { int retval = 0; struct mpu_read_write msg; unsigned char *user_data; retval = copy_from_user(&msg, usr_msg, sizeof(msg)); if (retval) return -EFAULT; user_data = msg.data; if (msg.length && msg.data) { unsigned char *data; data = kmalloc(msg.length, GFP_KERNEL); if (!data) return -ENOMEM; retval = copy_from_user(data, (void __user *)msg.data, msg.length); if (retval) { retval = -EFAULT; kfree(data); return retval; } msg.data = data; } else { return -EPERM; } switch (cmd) { case MPU_READ: retval = inv_serial_read(sl_handle, addr, msg.address, msg.length, msg.data); break; case MPU_WRITE: retval = inv_serial_write(sl_handle, addr, msg.length, msg.data); break; case MPU_READ_MEM: retval = inv_serial_read_mem(sl_handle, addr, msg.address, msg.length, msg.data); break; case MPU_WRITE_MEM: retval = inv_serial_write_mem(sl_handle, addr, msg.address, msg.length, msg.data); break; case MPU_READ_FIFO: retval = inv_serial_read_fifo(sl_handle, addr, msg.length, msg.data); break; case MPU_WRITE_FIFO: retval = inv_serial_write_fifo(sl_handle, addr, msg.length, msg.data); break; }; if (retval) { dev_err(&((struct i2c_adapter *)sl_handle)->dev, "%s: i2c %d error %d\n", __func__, cmd, retval); kfree(msg.data); return retval; } retval = copy_to_user((unsigned char __user *)user_data, msg.data, msg.length); kfree(msg.data); return retval; }
/** * @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 bma150_init(void *mlsl_handle, struct ext_slave_descr *slave, struct ext_slave_platform_data *pdata) { int result; unsigned char reg; long range; struct bma150_private_data *private_data; private_data = (struct bma150_private_data *) kzalloc(sizeof(struct bma150_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, BMA150_PWR_REG, BMA150_PWR_MASK_SOFT_RESET); if (result) { LOG_RESULT_LOCATION(result); return result; } msleep(1); result = inv_serial_read(mlsl_handle, pdata->address, BMA150_CTRL_REG, 1, ®); if (result) { LOG_RESULT_LOCATION(result); return result; } private_data->resume.ctrl_reg = reg; private_data->suspend.ctrl_reg = reg; result = inv_serial_read(mlsl_handle, pdata->address, BMA150_INT_REG, 1, ®); if (result) { LOG_RESULT_LOCATION(result); return result; } private_data->resume.int_reg = reg; private_data->suspend.int_reg = reg; result = bma150_set_odr(mlsl_handle, pdata, &private_data->suspend, FALSE, 0); if (result) { LOG_RESULT_LOCATION(result); return result; } result = bma150_set_odr(mlsl_handle, pdata, &private_data->resume, FALSE, 200000); if (result) { LOG_RESULT_LOCATION(result); return result; } range = range_fixedpoint_to_long_mg(slave->range); result = bma150_set_fsr(mlsl_handle, pdata, &private_data->suspend, FALSE, range); result = bma150_set_fsr(mlsl_handle, pdata, &private_data->resume, FALSE, range); if (result) { LOG_RESULT_LOCATION(result); return result; } result = bma150_set_irq(mlsl_handle, pdata, &private_data->suspend, FALSE, MPU_SLAVE_IRQ_TYPE_NONE); if (result) { LOG_RESULT_LOCATION(result); return result; } result = bma150_set_irq(mlsl_handle, pdata, &private_data->resume, FALSE, MPU_SLAVE_IRQ_TYPE_NONE); if (result) { LOG_RESULT_LOCATION(result); return result; } result = inv_serial_single_write(mlsl_handle, pdata->address, BMA150_PWR_REG, BMA150_PWR_MASK_SLEEP); if (result) { LOG_RESULT_LOCATION(result); return result; } return result; }