/**
* inv_mpu6050_set_enable() - enable chip functions.
* @indio_dev: Device driver instance.
* @enable: enable/disable
*/
static int inv_mpu6050_set_enable(struct iio_dev *indio_dev, bool enable)
{
struct inv_mpu6050_state *st = iio_priv(indio_dev);
int result;
if (enable) {
result = inv_mpu6050_set_power_itg(st, true);
if (result)
return result;
inv_scan_query(indio_dev);
if (st->chip_config.gyro_fifo_enable) {
result = inv_mpu6050_switch_engine(st, true,
INV_MPU6050_BIT_PWR_GYRO_STBY);
if (result)
return result;
}
if (st->chip_config.accl_fifo_enable) {
result = inv_mpu6050_switch_engine(st, true,
INV_MPU6050_BIT_PWR_ACCL_STBY);
if (result)
return result;
}
result = inv_reset_fifo(indio_dev);
if (result)
return result;
} else {
result = inv_mpu6050_write_reg(st, st->reg->fifo_en, 0);
if (result)
return result;
result = inv_mpu6050_write_reg(st, st->reg->int_enable, 0);
if (result)
return result;
result = inv_mpu6050_write_reg(st, st->reg->user_ctrl, 0);
if (result)
return result;
result = inv_mpu6050_switch_engine(st, false,
INV_MPU6050_BIT_PWR_GYRO_STBY);
if (result)
return result;
result = inv_mpu6050_switch_engine(st, false,
INV_MPU6050_BIT_PWR_ACCL_STBY);
if (result)
return result;
result = inv_mpu6050_set_power_itg(st, false);
if (result)
return result;
}
st->chip_config.enable = enable;
return 0;
}
/**
* inv_mpu6050_read_fifo() - Transfer data from hardware FIFO to KFIFO.
*/
irqreturn_t inv_mpu6050_read_fifo(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct inv_mpu6050_state *st = iio_priv(indio_dev);
size_t bytes_per_datum;
int result;
u8 data[INV_MPU6050_OUTPUT_DATA_SIZE];
u16 fifo_count;
s64 timestamp;
mutex_lock(&indio_dev->mlock);
if (!(st->chip_config.accl_fifo_enable |
st->chip_config.gyro_fifo_enable))
goto end_session;
bytes_per_datum = 0;
if (st->chip_config.accl_fifo_enable)
bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
if (st->chip_config.gyro_fifo_enable)
bytes_per_datum += INV_MPU6050_BYTES_PER_3AXIS_SENSOR;
/*
* read fifo_count register to know how many bytes inside FIFO
* right now
*/
result = i2c_smbus_read_i2c_block_data(st->client,
st->reg->fifo_count_h,
INV_MPU6050_FIFO_COUNT_BYTE, data);
if (result != INV_MPU6050_FIFO_COUNT_BYTE)
goto end_session;
fifo_count = be16_to_cpup((__be16 *)(&data[0]));
if (fifo_count < bytes_per_datum)
goto end_session;
/* fifo count can't be odd number, if it is odd, reset fifo*/
if (fifo_count & 1)
goto flush_fifo;
if (fifo_count > INV_MPU6050_FIFO_THRESHOLD)
goto flush_fifo;
/* Timestamp mismatch. */
if (kfifo_len(&st->timestamps) >
fifo_count / bytes_per_datum + INV_MPU6050_TIME_STAMP_TOR)
goto flush_fifo;
while (fifo_count >= bytes_per_datum) {
result = i2c_smbus_read_i2c_block_data(st->client,
st->reg->fifo_r_w,
bytes_per_datum, data);
if (result != bytes_per_datum)
goto flush_fifo;
result = kfifo_out(&st->timestamps, ×tamp, 1);
/* when there is no timestamp, put timestamp as 0 */
if (0 == result)
timestamp = 0;
result = iio_push_to_buffers_with_timestamp(indio_dev, data,
timestamp);
if (result)
goto flush_fifo;
fifo_count -= bytes_per_datum;
}
end_session:
mutex_unlock(&indio_dev->mlock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
flush_fifo:
/* Flush HW and SW FIFOs. */
inv_reset_fifo(indio_dev);
inv_clear_kfifo(st);
mutex_unlock(&indio_dev->mlock);
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
/**
* @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;
}
