diff options
Diffstat (limited to 'drivers/staging/iio/accel/lis3l02dq_ring.c')
-rw-r--r-- | drivers/staging/iio/accel/lis3l02dq_ring.c | 210 |
1 files changed, 61 insertions, 149 deletions
diff --git a/drivers/staging/iio/accel/lis3l02dq_ring.c b/drivers/staging/iio/accel/lis3l02dq_ring.c index e4e202e6cb3a..a960a8ff3c40 100644 --- a/drivers/staging/iio/accel/lis3l02dq_ring.c +++ b/drivers/staging/iio/accel/lis3l02dq_ring.c @@ -103,13 +103,15 @@ static struct attribute_group lis3l02dq_scan_el_group = { * lis3l02dq_poll_func_th() top half interrupt handler called by trigger * @private_data: iio_dev **/ -static void lis3l02dq_poll_func_th(struct iio_dev *indio_dev) +static void lis3l02dq_poll_func_th(struct iio_dev *indio_dev, s64 time) { - struct lis3l02dq_state *st = iio_dev_get_devdata(indio_dev); - st->last_timestamp = indio_dev->trig->timestamp; - schedule_work(&st->work_trigger_to_ring); - /* Indicate that this interrupt is being handled */ + struct iio_sw_ring_helper_state *h + = iio_dev_get_devdata(indio_dev); + struct lis3l02dq_state *st = lis3l02dq_h_to_s(h); + /* in this case we need to slightly extend the helper function */ + iio_sw_poll_func_th(indio_dev, time); + /* Indicate that this interrupt is being handled */ /* Technically this is trigger related, but without this * handler running there is currently now way for the interrupt * to clear. @@ -120,16 +122,16 @@ static void lis3l02dq_poll_func_th(struct iio_dev *indio_dev) /** * lis3l02dq_data_rdy_trig_poll() the event handler for the data rdy trig **/ -static int lis3l02dq_data_rdy_trig_poll(struct iio_dev *dev_info, +static int lis3l02dq_data_rdy_trig_poll(struct iio_dev *indio_dev, int index, s64 timestamp, int no_test) { - struct lis3l02dq_state *st = iio_dev_get_devdata(dev_info); - struct iio_trigger *trig = st->trig; + struct iio_sw_ring_helper_state *h + = iio_dev_get_devdata(indio_dev); + struct lis3l02dq_state *st = lis3l02dq_h_to_s(h); - trig->timestamp = timestamp; - iio_trigger_poll(trig); + iio_trigger_poll(st->trig, timestamp); return IRQ_HANDLED; } @@ -213,7 +215,7 @@ static int lis3l02dq_read_all(struct lis3l02dq_state *st, u8 *rx_array) struct spi_message msg; int ret, i, j = 0; - xfers = kzalloc((st->indio_dev->scan_count) * 2 + xfers = kzalloc((st->help.indio_dev->scan_count) * 2 * sizeof(*xfers), GFP_KERNEL); if (!xfers) return -ENOMEM; @@ -221,7 +223,7 @@ static int lis3l02dq_read_all(struct lis3l02dq_state *st, u8 *rx_array) mutex_lock(&st->buf_lock); for (i = 0; i < ARRAY_SIZE(read_all_tx_array)/4; i++) { - if (st->indio_dev->scan_mask & (1 << i)) { + if (st->help.indio_dev->scan_mask & (1 << i)) { /* lower byte */ xfers[j].tx_buf = st->tx + 2*j; st->tx[2*j] = read_all_tx_array[i*4]; @@ -249,7 +251,7 @@ static int lis3l02dq_read_all(struct lis3l02dq_state *st, u8 *rx_array) * values in alternate bytes */ spi_message_init(&msg); - for (j = 0; j < st->indio_dev->scan_count * 2; j++) + for (j = 0; j < st->help.indio_dev->scan_count * 2; j++) spi_message_add_tail(&xfers[j], &msg); ret = spi_sync(st->us, &msg); @@ -259,102 +261,37 @@ static int lis3l02dq_read_all(struct lis3l02dq_state *st, u8 *rx_array) return ret; } - -/* 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 lis3l02dq_trigger_bh_to_ring(struct work_struct *work_s) { - struct lis3l02dq_state *st - = container_of(work_s, struct lis3l02dq_state, - work_trigger_to_ring); - - u8 *rx_array; - int i = 0; - u16 *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; - } - /* Due to interleaved nature of transmission this buffer must be - * twice the number of bytes, or 4 times the number of channels - */ - rx_array = kmalloc(4 * (st->indio_dev->scan_count), GFP_KERNEL); - if (rx_array == NULL) { - dev_err(&st->us->dev, "memory alloc failed in ring bh"); - kfree(data); - return; - } + struct iio_sw_ring_helper_state *h + = container_of(work_s, struct iio_sw_ring_helper_state, + work_trigger_to_ring); + struct lis3l02dq_state *st = lis3l02dq_h_to_s(h); - /* whilst trigger specific, if this read does nto occur the data - ready interrupt will not be cleared. Need to add a mechanism - to provide a dummy read function if this is not triggering on - the data ready function but something else is. - */ st->inter = 0; - - if (st->indio_dev->scan_count) - if (lis3l02dq_read_all(st, rx_array) >= 0) - for (; i < st->indio_dev->scan_count; i++) - data[i] = combine_8_to_16(rx_array[i*4+1], - rx_array[i*4+3]); - /* 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(rx_array); - kfree(data); - - return; -} -/* in these circumstances is it better to go with unaligned packing and - * deal with the cost?*/ -static int lis3l02dq_data_rdy_ring_preenable(struct iio_dev *indio_dev) -{ - size_t size; - /* Check if there are any scan elements enabled, if not fail*/ - if (!(indio_dev->scan_count || indio_dev->scan_timestamp)) - return -EINVAL; - - if (indio_dev->ring->access.set_bpd) { - if (indio_dev->scan_timestamp) - if (indio_dev->scan_count) /* Timestamp and data */ - size = 2*sizeof(s64); - else /* Timestamp only */ - size = sizeof(s64); - else /* Data only */ - size = indio_dev->scan_count*sizeof(s16); - indio_dev->ring->access.set_bpd(indio_dev->ring, size); - } - - return 0; + iio_sw_trigger_bh_to_ring(work_s); } -static int lis3l02dq_data_rdy_ring_postenable(struct iio_dev *indio_dev) +static int lis3l02dq_get_ring_element(struct iio_sw_ring_helper_state *h, + u8 *buf) { - return indio_dev->trig - ? iio_trigger_attach_poll_func(indio_dev->trig, - indio_dev->pollfunc) - : 0; -} + int ret, i; + u8 *rx_array ; + s16 *data = (s16 *)buf; -static int lis3l02dq_data_rdy_ring_predisable(struct iio_dev *indio_dev) -{ - return indio_dev->trig - ? iio_trigger_dettach_poll_func(indio_dev->trig, - indio_dev->pollfunc) - : 0; -} + rx_array = kzalloc(4 * (h->indio_dev->scan_count), GFP_KERNEL); + if (rx_array == NULL) + return -ENOMEM; + ret = lis3l02dq_read_all(lis3l02dq_h_to_s(h), rx_array); + if (ret < 0) + return ret; + for (i = 0; i < h->indio_dev->scan_count; i++) + data[i] = combine_8_to_16(rx_array[i*4+1], + rx_array[i*4+3]); + kfree(rx_array); + return i*sizeof(data[0]); +} /* Caller responsible for locking as necessary. */ static int @@ -427,7 +364,7 @@ static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig, struct lis3l02dq_state *st = trig->private_data; int ret = 0; u8 t; - __lis3l02dq_write_data_ready_config(&st->indio_dev->dev, + __lis3l02dq_write_data_ready_config(&st->help.indio_dev->dev, &iio_event_data_rdy_trig, state); if (state == false) { @@ -437,7 +374,7 @@ static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig, /* Clear any outstanding ready events */ ret = lis3l02dq_read_all(st, NULL); } - lis3l02dq_spi_read_reg_8(&st->indio_dev->dev, + lis3l02dq_spi_read_reg_8(&st->help.indio_dev->dev, LIS3L02DQ_REG_WAKE_UP_SRC_ADDR, &t); return ret; @@ -495,14 +432,14 @@ int lis3l02dq_probe_trigger(struct iio_dev *indio_dev) if (!state->trig) return -ENOMEM; - state->trig->name = kmalloc(IIO_TRIGGER_NAME_LENGTH, GFP_KERNEL); + state->trig->name = kasprintf(GFP_KERNEL, + "lis3l02dq-dev%d", + indio_dev->id); if (!state->trig->name) { ret = -ENOMEM; goto error_free_trig; } - snprintf((char *)state->trig->name, - IIO_TRIGGER_NAME_LENGTH, - "lis3l02dq-dev%d", indio_dev->id); + state->trig->dev.parent = &state->us->dev; state->trig->owner = THIS_MODULE; state->trig->private_data = state; @@ -540,12 +477,12 @@ void lis3l02dq_unconfigure_ring(struct iio_dev *indio_dev) int lis3l02dq_configure_ring(struct iio_dev *indio_dev) { - int ret = 0; - struct lis3l02dq_state *st = indio_dev->dev_data; - struct iio_ring_buffer *ring; - INIT_WORK(&st->work_trigger_to_ring, lis3l02dq_trigger_bh_to_ring); - /* Set default scan mode */ + int ret; + struct iio_sw_ring_helper_state *h = iio_dev_get_devdata(indio_dev); + INIT_WORK(&h->work_trigger_to_ring, lis3l02dq_trigger_bh_to_ring); + /* Set default scan mode */ + h->get_ring_element = &lis3l02dq_get_ring_element; iio_scan_mask_set(indio_dev, iio_scan_el_accel_x.number); iio_scan_mask_set(indio_dev, iio_scan_el_accel_y.number); iio_scan_mask_set(indio_dev, iio_scan_el_accel_z.number); @@ -553,26 +490,21 @@ int lis3l02dq_configure_ring(struct iio_dev *indio_dev) indio_dev->scan_el_attrs = &lis3l02dq_scan_el_group; - ring = iio_sw_rb_allocate(indio_dev); - if (!ring) { - ret = -ENOMEM; - return ret; - } - indio_dev->ring = ring; + indio_dev->ring = iio_sw_rb_allocate(indio_dev); + if (!indio_dev->ring) + return -ENOMEM; + /* Effectively select the ring buffer implementation */ - iio_ring_sw_register_funcs(&ring->access); - ring->preenable = &lis3l02dq_data_rdy_ring_preenable; - ring->postenable = &lis3l02dq_data_rdy_ring_postenable; - ring->predisable = &lis3l02dq_data_rdy_ring_predisable; - ring->owner = THIS_MODULE; - - indio_dev->pollfunc = kzalloc(sizeof(*indio_dev->pollfunc), GFP_KERNEL); - if (indio_dev->pollfunc == NULL) { - ret = -ENOMEM; + iio_ring_sw_register_funcs(&indio_dev->ring->access); + indio_dev->ring->bpe = 2; + indio_dev->ring->preenable = &iio_sw_ring_preenable; + indio_dev->ring->postenable = &iio_triggered_ring_postenable; + indio_dev->ring->predisable = &iio_triggered_ring_predisable; + indio_dev->ring->owner = THIS_MODULE; + + ret = iio_alloc_pollfunc(indio_dev, NULL, &lis3l02dq_poll_func_th); + if (ret) goto error_iio_sw_rb_free;; - } - indio_dev->pollfunc->poll_func_main = &lis3l02dq_poll_func_th; - indio_dev->pollfunc->private_data = indio_dev; indio_dev->modes |= INDIO_RING_TRIGGERED; return 0; @@ -580,23 +512,3 @@ error_iio_sw_rb_free: iio_sw_rb_free(indio_dev->ring); return ret; } - -int lis3l02dq_initialize_ring(struct iio_ring_buffer *ring) -{ - return iio_ring_buffer_register(ring, 0); -} - -void lis3l02dq_uninitialize_ring(struct iio_ring_buffer *ring) -{ - iio_ring_buffer_unregister(ring); -} - -int lis3l02dq_set_ring_length(struct iio_dev *indio_dev, int length) -{ - /* Set sensible defaults for the ring buffer */ - if (indio_dev->ring->access.set_length) - return indio_dev->ring->access.set_length(indio_dev->ring, 500); - return 0; -} - - |