static irqreturn_t adis_trigger_handler(int irq, void *p) { struct iio_poll_func *pf = p; struct iio_dev *indio_dev = pf->indio_dev; struct adis *adis = iio_device_get_drvdata(indio_dev); int ret; if (!adis->buffer) return -ENOMEM; if (adis->data->has_paging) { mutex_lock(&adis->txrx_lock); if (adis->current_page != 0) { adis->tx[0] = ADIS_WRITE_REG(ADIS_REG_PAGE_ID); adis->tx[1] = 0; spi_write(adis->spi, adis->tx, 2); } } ret = spi_sync(adis->spi, &adis->msg); if (ret) dev_err(&adis->spi->dev, "Failed to read data: %d", ret); if (adis->data->has_paging) { adis->current_page = 0; mutex_unlock(&adis->txrx_lock); } /* Guaranteed to be aligned with 8 byte boundary */ if (indio_dev->scan_timestamp) { void *b = adis->buffer + indio_dev->scan_bytes - sizeof(s64); *(s64 *)b = pf->timestamp; } iio_push_to_buffers(indio_dev, adis->buffer); iio_trigger_notify_done(indio_dev->trig); return IRQ_HANDLED; }
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; }
static irqreturn_t adc108s102_trigger_handler(int irq, void *p) { struct iio_poll_func *pf = p; struct iio_dev *indio_dev = pf->indio_dev; struct adc108s102_state *st = iio_priv(indio_dev); int ret; ret = spi_sync(st->spi, &st->ring_msg); if (ret < 0) goto out_notify; /* Skip the dummy response in the first slot */ iio_push_to_buffers_with_timestamp(indio_dev, (u8 *)&st->rx_buf[1], iio_get_time_ns(indio_dev)); out_notify: iio_trigger_notify_done(indio_dev->trig); return IRQ_HANDLED; }
/* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device * specific to be rolled into the core. */ static irqreturn_t ade7758_trigger_handler(int irq, void *p) { struct iio_poll_func *pf = p; struct iio_dev *indio_dev = pf->indio_dev; struct ade7758_state *st = iio_priv(indio_dev); s64 dat64[2]; u32 *dat32 = (u32 *)dat64; if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) if (ade7758_spi_read_burst(indio_dev) >= 0) *dat32 = get_unaligned_be32(&st->rx_buf[5]) & 0xFFFFFF; /* Guaranteed to be aligned with 8 byte boundary */ if (indio_dev->scan_timestamp) dat64[1] = pf->timestamp; iio_push_to_buffers(indio_dev, (u8 *)dat64); iio_trigger_notify_done(indio_dev->trig); return IRQ_HANDLED; }
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); int len = 0, i, j; int32_t *mag; mag = (int32_t *) kmalloc(indio_dev->scan_bytes, 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; } if ((pf->timestamp - st->old_timestamp) > ((1800 /st->sampling_frequency) * 1000000) && (st->old_timestamp != 0)) { if (indio_dev->scan_timestamp) *(s64 *)((u8 *)mag + ALIGN(len, sizeof(s64))) = (pf->timestamp + st->old_timestamp) >> 1; iio_push_to_buffers(indio_dev, (u8 *)mag); } /* Guaranteed to be aligned with 8 byte boundary */ if (indio_dev->scan_timestamp) *(s64 *)((u8 *)mag + ALIGN(len, sizeof(s64))) = pf->timestamp; iio_push_to_buffers(indio_dev, (u8 *)mag); st->old_timestamp = pf->timestamp; kfree(mag); done: iio_trigger_notify_done(indio_dev->trig); return IRQ_HANDLED; }
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 *acc; acc = (int32_t *) kmalloc(datasize, GFP_KERNEL); if (acc == NULL) { dev_err(indio_dev->dev.parent, "memory alloc failed in buffer bh"); return -ENOMEM; } 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)) { acc[j] = st->compass_data[i]; j++; } } len = j * 4; } /* Guaranteed to be aligned with 8 byte boundary */ if (buffer->scan_timestamp) *(s64 *)(((phys_addr_t)acc + len + sizeof(s64) - 1) & ~(sizeof(s64) - 1)) = pf->timestamp; buffer->access->store_to(buffer, (u8 *)acc, pf->timestamp); iio_trigger_notify_done(indio_dev->trig); kfree(acc); return IRQ_HANDLED; }
static irqreturn_t bma220_trigger_handler(int irq, void *p) { int ret; struct iio_poll_func *pf = p; struct iio_dev *indio_dev = pf->indio_dev; struct bma220_data *data = iio_priv(indio_dev); struct spi_device *spi = data->spi_device; mutex_lock(&data->lock); data->tx_buf[0] = BMA220_REG_ACCEL_X | BMA220_READ_MASK; ret = spi_write_then_read(spi, data->tx_buf, 1, data->buffer, ARRAY_SIZE(bma220_channels) - 1); if (ret < 0) goto err; iio_push_to_buffers_with_timestamp(indio_dev, data->buffer, pf->timestamp); err: mutex_unlock(&data->lock); iio_trigger_notify_done(indio_dev->trig); return IRQ_HANDLED; }
static irqreturn_t ade7758_trigger_handler(int irq, void *p) { struct iio_poll_func *pf = p; struct iio_dev *indio_dev = pf->indio_dev; struct iio_buffer *ring = indio_dev->buffer; struct ade7758_state *st = iio_priv(indio_dev); s64 dat64[2]; u32 *dat32 = (u32 *)dat64; if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) if (ade7758_spi_read_burst(&indio_dev->dev) >= 0) *dat32 = get_unaligned_be32(&st->rx_buf[5]) & 0xFFFFFF; if (ring->scan_timestamp) dat64[1] = pf->timestamp; ring->access->store_to(ring, (u8 *)dat64, pf->timestamp); iio_trigger_notify_done(indio_dev->trig); return IRQ_HANDLED; }
static irqreturn_t kxcjk1013_trigger_handler(int irq, void *p) { struct iio_poll_func *pf = p; struct iio_dev *indio_dev = pf->indio_dev; struct kxcjk1013_data *data = iio_priv(indio_dev); int ret; mutex_lock(&data->mutex); ret = i2c_smbus_read_i2c_block_data_or_emulated(data->client, KXCJK1013_REG_XOUT_L, AXIS_MAX * 2, (u8 *)data->buffer); mutex_unlock(&data->mutex); if (ret < 0) goto err; iio_push_to_buffers_with_timestamp(indio_dev, data->buffer, data->timestamp); err: iio_trigger_notify_done(indio_dev->trig); return IRQ_HANDLED; }
static irqreturn_t iio_simple_dummy_trigger_h(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; int len = 0; size_t datasize = buffer->access->get_bytes_per_datum(buffer); u16 *data = kmalloc(datasize, GFP_KERNEL); if (data == NULL) return -ENOMEM; if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength)) { int i, j; for (i = 0, j = 0; i < bitmap_weight(indio_dev->active_scan_mask, indio_dev->masklength); i++) { j = find_next_bit(buffer->scan_mask, indio_dev->masklength, j + 1); data[i] = fakedata[j]; len += 2; } } if (buffer->scan_timestamp) *(s64 *)(((phys_addr_t)data + len + sizeof(s64) - 1) & ~(sizeof(s64) - 1)) = iio_get_time_ns(); buffer->access->store_to(buffer, (u8 *)data, pf->timestamp); kfree(data); iio_trigger_notify_done(indio_dev->trig); return IRQ_HANDLED; }
/* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device * specific to be rolled into the core. */ static void adis16209_trigger_bh_to_ring(struct work_struct *work_s) { struct adis16209_state *st = container_of(work_s, struct adis16209_state, work_trigger_to_ring); int i = 0; s16 *data; size_t datasize = st->indio_dev ->ring->access.get_bpd(st->indio_dev->ring); data = kmalloc(datasize , GFP_KERNEL); if (data == NULL) { dev_err(&st->us->dev, "memory alloc failed in ring bh"); return; } if (st->indio_dev->scan_count) if (adis16209_read_ring_data(&st->indio_dev->dev, st->rx) >= 0) for (; i < st->indio_dev->scan_count; i++) { data[i] = combine_8_to_16(st->rx[i*2+1], st->rx[i*2]); } /* Guaranteed to be aligned with 8 byte boundary */ if (st->indio_dev->scan_timestamp) *((s64 *)(data + ((i + 3)/4)*4)) = st->last_timestamp; st->indio_dev->ring->access.store_to(st->indio_dev->ring, (u8 *)data, st->last_timestamp); iio_trigger_notify_done(st->indio_dev->trig); kfree(data); return; }
/* Whilst this makes a lot of calls to iio_sw_ring functions - it is to device * specific to be rolled into the core. */ static irqreturn_t adis16203_trigger_handler(int irq, void *p) { struct iio_poll_func *pf = p; struct iio_dev *indio_dev = pf->indio_dev; struct adis16203_state *st = iio_priv(indio_dev); struct iio_buffer *ring = indio_dev->buffer; int i = 0; s16 *data; data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL); if (data == NULL) { dev_err(&st->us->dev, "memory alloc failed in ring bh"); goto done; } if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength) && adis16203_read_ring_data(indio_dev, st->rx) >= 0) for (; i < bitmap_weight(indio_dev->active_scan_mask, indio_dev->masklength); i++) data[i] = be16_to_cpup((__be16 *)&(st->rx[i*2])); /* Guaranteed to be aligned with 8 byte boundary */ if (indio_dev->scan_timestamp) *((s64 *)(data + ((i + 3)/4)*4)) = pf->timestamp; ring->access->store_to(ring, (u8 *)data, pf->timestamp); kfree(data); done: iio_trigger_notify_done(indio_dev->trig); return IRQ_HANDLED; }
static void adis16240_trigger_bh_to_ring(struct work_struct *work_s) { struct adis16240_state *st = container_of(work_s, struct adis16240_state, work_trigger_to_ring); struct iio_ring_buffer *ring = st->indio_dev->ring; int i = 0; s16 *data; size_t datasize = ring->access.get_bytes_per_datum(ring); data = kmalloc(datasize , GFP_KERNEL); if (data == NULL) { dev_err(&st->us->dev, "memory alloc failed in ring bh"); return; } if (ring->scan_count) if (adis16240_read_ring_data(&st->indio_dev->dev, st->rx) >= 0) for (; i < ring->scan_count; i++) data[i] = be16_to_cpup( (__be16 *)&(st->rx[i*2])); /* Guaranteed to be aligned with 8 byte boundary */ if (ring->scan_timestamp) *((s64 *)(data + ((i + 3)/4)*4)) = st->last_timestamp; ring->access.store_to(ring, (u8 *)data, st->last_timestamp); iio_trigger_notify_done(st->indio_dev->trig); kfree(data); return; }
/** * 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; }
static irqreturn_t mpu3050_trigger_handler(int irq, void *p) { const struct iio_poll_func *pf = p; struct iio_dev *indio_dev = pf->indio_dev; struct mpu3050 *mpu3050 = iio_priv(indio_dev); int ret; /* * Temperature 1*16 bits * Three axes 3*16 bits * Timestamp 64 bits (4*16 bits) * Sum total 8*16 bits */ __be16 hw_values[8]; s64 timestamp; unsigned int datums_from_fifo = 0; /* * If we're using the hardware trigger, get the precise timestamp from * the top half of the threaded IRQ handler. Otherwise get the * timestamp here so it will be close in time to the actual values * read from the registers. */ if (iio_trigger_using_own(indio_dev)) timestamp = mpu3050->hw_timestamp; else timestamp = iio_get_time_ns(indio_dev); mutex_lock(&mpu3050->lock); /* Using the hardware IRQ trigger? Check the buffer then. */ if (mpu3050->hw_irq_trigger) { __be16 raw_fifocnt; u16 fifocnt; /* X, Y, Z + temperature */ unsigned int bytes_per_datum = 8; bool fifo_overflow = false; ret = regmap_bulk_read(mpu3050->map, MPU3050_FIFO_COUNT_H, &raw_fifocnt, sizeof(raw_fifocnt)); if (ret) goto out_trigger_unlock; fifocnt = be16_to_cpu(raw_fifocnt); if (fifocnt == 512) { dev_info(mpu3050->dev, "FIFO overflow! Emptying and resetting FIFO\n"); fifo_overflow = true; /* Reset and enable the FIFO */ ret = regmap_update_bits(mpu3050->map, MPU3050_USR_CTRL, MPU3050_USR_CTRL_FIFO_EN | MPU3050_USR_CTRL_FIFO_RST, MPU3050_USR_CTRL_FIFO_EN | MPU3050_USR_CTRL_FIFO_RST); if (ret) { dev_info(mpu3050->dev, "error resetting FIFO\n"); goto out_trigger_unlock; } mpu3050->pending_fifo_footer = false; } if (fifocnt) dev_dbg(mpu3050->dev, "%d bytes in the FIFO\n", fifocnt); while (!fifo_overflow && fifocnt > bytes_per_datum) { unsigned int toread; unsigned int offset; __be16 fifo_values[5]; /* * If there is a FIFO footer in the pipe, first clear * that out. This follows the complex algorithm in the * datasheet that states that you may never leave the * FIFO empty after the first reading: you have to * always leave two footer bytes in it. The footer is * in practice just two zero bytes. */ if (mpu3050->pending_fifo_footer) { toread = bytes_per_datum + 2; offset = 0; } else { toread = bytes_per_datum; offset = 1; /* Put in some dummy value */ fifo_values[0] = 0xAAAA; } ret = regmap_bulk_read(mpu3050->map, MPU3050_FIFO_R, &fifo_values[offset], toread); dev_dbg(mpu3050->dev, "%04x %04x %04x %04x %04x\n", fifo_values[0], fifo_values[1], fifo_values[2], fifo_values[3], fifo_values[4]); /* Index past the footer (fifo_values[0]) and push */ iio_push_to_buffers_with_timestamp(indio_dev, &fifo_values[1], timestamp); fifocnt -= toread; datums_from_fifo++; mpu3050->pending_fifo_footer = true; /* * If we're emptying the FIFO, just make sure to * check if something new appeared. */ if (fifocnt < bytes_per_datum) { ret = regmap_bulk_read(mpu3050->map, MPU3050_FIFO_COUNT_H, &raw_fifocnt, sizeof(raw_fifocnt)); if (ret) goto out_trigger_unlock; fifocnt = be16_to_cpu(raw_fifocnt); } if (fifocnt < bytes_per_datum) dev_dbg(mpu3050->dev, "%d bytes left in the FIFO\n", fifocnt); /* * At this point, the timestamp that triggered the * hardware interrupt is no longer valid for what * we are reading (the interrupt likely fired for * the value on the top of the FIFO), so set the * timestamp to zero and let userspace deal with it. */ timestamp = 0; } } /* * If we picked some datums from the FIFO that's enough, else * fall through and just read from the current value registers. * This happens in two cases: * * - We are using some other trigger (external, like an HRTimer) * than the sensor's own sample generator. In this case the * sensor is just set to the max sampling frequency and we give * the trigger a copy of the latest value every time we get here. * * - The hardware trigger is active but unused and we actually use * another trigger which calls here with a frequency higher * than what the device provides data. We will then just read * duplicate values directly from the hardware registers. */ if (datums_from_fifo) { dev_dbg(mpu3050->dev, "read %d datums from the FIFO\n", datums_from_fifo); goto out_trigger_unlock; } ret = regmap_bulk_read(mpu3050->map, MPU3050_TEMP_H, &hw_values, sizeof(hw_values)); if (ret) { dev_err(mpu3050->dev, "error reading axis data\n"); goto out_trigger_unlock; } iio_push_to_buffers_with_timestamp(indio_dev, hw_values, timestamp); out_trigger_unlock: mutex_unlock(&mpu3050->lock); iio_trigger_notify_done(indio_dev->trig); return IRQ_HANDLED; }