static irqreturn_t yas_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct yas_state *st = iio_priv(indio_dev);
struct iio_buffer *buffer = indio_dev->buffer;
size_t datasize = buffer->access->get_bytes_per_datum(buffer);
int len = 0, i, j;
int32_t *mag;
s64 timestamp;
mag = (int32_t *) kmalloc(datasize, GFP_KERNEL);
if (mag == NULL)
goto done;
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
j = 0;
for (i = 0; i < 3; i++) {
if (test_bit(i, indio_dev->active_scan_mask)) {
mag[j] = st->compass_data[i];
j++;
}
}
len = j * 4;
}
timestamp = yas_iio_get_boottime_ns();
*(s64 *)((u8 *)mag + ALIGN(len, sizeof(s64))) = timestamp;
iio_push_to_buffer(indio_dev->buffer, (u8 *)mag, 0);
kfree(mag);
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
static int ssp_push_1bytes_buffer(struct iio_dev *indio_dev,
u64 t, u8 *d)
{
u8 buf[IIO_BUFFER_1_BYTES];
memcpy(buf, d, sizeof(u8));
memcpy(buf + 1, &t, sizeof(t));
iio_push_to_buffer(indio_dev->buffer, buf, 0);
return 0;
}
static int ssp_push_12bytes_buffer(struct iio_dev *indio_dev, u64 t,
int *q)
{
u8 buf[IIO_BUFFER_12_BYTES];
int i;
for (i = 0; i < 3; i++)
memcpy(buf + 4 * i, &q[i], sizeof(q[i]));
memcpy(buf + 12, &t, sizeof(t));
iio_push_to_buffer(indio_dev->buffer, buf, 0);
return 0;
}
static irqreturn_t max1363_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct max1363_state *st = iio_priv(indio_dev);
s64 time_ns;
__u8 *rxbuf;
int b_sent;
size_t d_size;
unsigned long numvals = bitmap_weight(st->current_mode->modemask,
MAX1363_MAX_CHANNELS);
/* Ensure the timestamp is 8 byte aligned */
if (st->chip_info->bits != 8)
d_size = numvals*2;
else
d_size = numvals;
if (indio_dev->buffer->scan_timestamp) {
d_size += sizeof(s64);
if (d_size % sizeof(s64))
d_size += sizeof(s64) - (d_size % sizeof(s64));
}
/* Monitor mode prevents reading. Whilst not currently implemented
* might as well have this test in here in the meantime as it does
* no harm.
*/
if (numvals == 0)
return IRQ_HANDLED;
rxbuf = kmalloc(d_size, GFP_KERNEL);
if (rxbuf == NULL)
return -ENOMEM;
if (st->chip_info->bits != 8)
b_sent = i2c_master_recv(st->client, rxbuf, numvals*2);
else
b_sent = i2c_master_recv(st->client, rxbuf, numvals);
if (b_sent < 0)
goto done;
time_ns = iio_get_time_ns();
if (indio_dev->buffer->scan_timestamp)
memcpy(rxbuf + d_size - sizeof(s64), &time_ns, sizeof(time_ns));
iio_push_to_buffer(indio_dev->buffer, rxbuf, time_ns);
done:
iio_trigger_notify_done(indio_dev->trig);
kfree(rxbuf);
return IRQ_HANDLED;
}
static int ssp_push_24bytes_buffer(struct iio_dev *indio_dev, u64 t, s16 *q)
{
u8 buf[IIO_BUFFER_24_BYTES];
int i;
for (i = 0; i < 6; i++)
memcpy(buf + 2 * i, &q[i], sizeof(q[i]));
memcpy(buf + 12, &t, sizeof(t));
mutex_lock(&indio_dev->mlock);
iio_push_to_buffer(indio_dev->buffer, buf, 0);
mutex_unlock(&indio_dev->mlock);
return 0;
}
static int ssp_push_6bytes_buffer(struct iio_dev *indio_dev,
u64 t, s16 *d)
{
u8 buf[IIO_BUFFER_6_BYTES];
int i;
for (i = 0; i < 3; i++)
memcpy(buf + i * 2, &d[i], sizeof(d[i]));
memcpy(buf + 6, &t, sizeof(t));
iio_push_to_buffer(indio_dev->buffer, buf, 0);
return 0;
}
static int ssp_push_17bytes_buffer(struct iio_dev *indio_dev, u64 t, int *q)
{
u8 buf[IIO_BUFFER_17_BYTES];
int i;
for (i = 0; i < 4; i++)
memcpy(buf + 4 * i, &q[i], sizeof(q[i]));
buf[16] = (u8)q[4];
memcpy(buf + 17, &t, sizeof(t));
mutex_lock(&indio_dev->mlock);
iio_push_to_buffer(indio_dev->buffer, buf, 0);
mutex_unlock(&indio_dev->mlock);
return 0;
}
static int ssp_push_7bytes_buffer(struct iio_dev *indio_dev, u64 t, s16 *d,
u8 status)
{
u8 buf[IIO_BUFFER_7_BYTES];
int i;
for (i = 0; i < 3; i++)
memcpy(buf + i * 2, &d[i], sizeof(d[i]));
buf[6] = status;
memcpy(buf + 7, &t, sizeof(t));
mutex_lock(&indio_dev->mlock);
iio_push_to_buffer(indio_dev->buffer, buf, 0);
mutex_unlock(&indio_dev->mlock);
return 0;
}
static irqreturn_t yas_trigger_handler(int irq, void *p)
{
struct iio_poll_func *pf = p;
struct iio_dev *indio_dev = pf->indio_dev;
struct iio_buffer *buffer = indio_dev->buffer;
struct yas_state *st = iio_priv(indio_dev);
int len = 0, i, j;
size_t datasize = buffer->access->get_bytes_per_datum(buffer);
int32_t *mag;
struct timespec ts;
s64 timestamp;
mag = (int32_t *) kmalloc(datasize, GFP_KERNEL);
if (mag == NULL)
goto done;
if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) {
j = 0;
for (i = 0; i < 3; i++) {
if (test_bit(i, indio_dev->active_scan_mask)) {
mag[j] = st->compass_data[i];
j++;
}
}
len = j * 4;
}
/* Guaranteed to be aligned with 8 byte boundary */
//if (indio_dev->scan_timestamp)
// *(s64 *)((u8 *)mag + ALIGN(len, sizeof(s64))) = pf->timestamp;
ts = ktime_to_timespec(ktime_get_boottime());
timestamp = ts.tv_sec * 1000000000ULL + ts.tv_nsec;
*(s64 *)((u8 *)mag + ALIGN(len, sizeof(s64))) = timestamp;
if (timestamp <= 0)
pr_err("[%s] invalid time = %lld\n", __func__, timestamp);
iio_push_to_buffer(indio_dev->buffer, (u8 *)mag, 0);
kfree(mag);
done:
iio_trigger_notify_done(indio_dev->trig);
return IRQ_HANDLED;
}
/**
* 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;
u64 *tmp;
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;
tmp = (u64 *)data;
tmp[DIV_ROUND_UP(bytes_per_datum, 8)] = timestamp;
result = iio_push_to_buffer(indio_dev->buffer, data, pf->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;
}
