LXR linux/drivers/staging/iio/accel/lis3l02dq

   1#include <linux/interrupt.h>
   2#include <linux/gpio.h>
   3#include <linux/mutex.h>
   4#include <linux/kernel.h>
   5#include <linux/spi/spi.h>
   6#include <linux/slab.h>
   7#include <linux/export.h>
   8
   9#include <linux/iio/iio.h>
  10#include <linux/iio/kfifo_buf.h>
  11#include <linux/iio/trigger.h>
  12#include <linux/iio/trigger_consumer.h>
  13#include "lis3l02dq.h"
  14
  15/**
  16 * combine_8_to_16() utility function to munge two u8s into u16
  17 **/
  18static inline u16 combine_8_to_16(u8 lower, u8 upper)
  19{
  20        u16 _lower = lower;
  21        u16 _upper = upper;
  22        return _lower | (_upper << 8);
  23}
  24
  25/**
  26 * lis3l02dq_data_rdy_trig_poll() the event handler for the data rdy trig
  27 **/
  28irqreturn_t lis3l02dq_data_rdy_trig_poll(int irq, void *private)
  29{
  30        struct iio_dev *indio_dev = private;
  31        struct lis3l02dq_state *st = iio_priv(indio_dev);
  32
  33        if (st->trigger_on) {
  34                iio_trigger_poll(st->trig, iio_get_time_ns());
  35                return IRQ_HANDLED;
  36        } else
  37                return IRQ_WAKE_THREAD;
  38}
  39
  40static const u8 read_all_tx_array[] = {
  41        LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_L_ADDR), 0,
  42        LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_X_H_ADDR), 0,
  43        LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Y_L_ADDR), 0,
  44        LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Y_H_ADDR), 0,
  45        LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Z_L_ADDR), 0,
  46        LIS3L02DQ_READ_REG(LIS3L02DQ_REG_OUT_Z_H_ADDR), 0,
  47};
  48
  49/**
  50 * lis3l02dq_read_all() Reads all channels currently selected
  51 * @indio_dev:  IIO device state
  52 * @rx_array:   (dma capable) receive array, must be at least
  53 *              4*number of channels
  54 **/
  55static int lis3l02dq_read_all(struct iio_dev *indio_dev, u8 *rx_array)
  56{
  57        struct lis3l02dq_state *st = iio_priv(indio_dev);
  58        struct spi_transfer *xfers;
  59        struct spi_message msg;
  60        int ret, i, j = 0;
  61
  62        xfers = kcalloc(bitmap_weight(indio_dev->active_scan_mask,
  63                                      indio_dev->masklength) * 2,
  64                        sizeof(*xfers), GFP_KERNEL);
  65        if (!xfers)
  66                return -ENOMEM;
  67
  68        mutex_lock(&st->buf_lock);
  69
  70        for (i = 0; i < ARRAY_SIZE(read_all_tx_array)/4; i++)
  71                if (test_bit(i, indio_dev->active_scan_mask)) {
  72                        /* lower byte */
  73                        xfers[j].tx_buf = st->tx + 2*j;
  74                        st->tx[2*j] = read_all_tx_array[i*4];
  75                        st->tx[2*j + 1] = 0;
  76                        if (rx_array)
  77                                xfers[j].rx_buf = rx_array + j*2;
  78                        xfers[j].bits_per_word = 8;
  79                        xfers[j].len = 2;
  80                        xfers[j].cs_change = 1;
  81                        j++;
  82
  83                        /* upper byte */
  84                        xfers[j].tx_buf = st->tx + 2*j;
  85                        st->tx[2*j] = read_all_tx_array[i*4 + 2];
  86                        st->tx[2*j + 1] = 0;
  87                        if (rx_array)
  88                                xfers[j].rx_buf = rx_array + j*2;
  89                        xfers[j].bits_per_word = 8;
  90                        xfers[j].len = 2;
  91                        xfers[j].cs_change = 1;
  92                        j++;
  93                }
  94
  95        /* After these are transmitted, the rx_buff should have
  96         * values in alternate bytes
  97         */
  98        spi_message_init(&msg);
  99        for (j = 0; j < bitmap_weight(indio_dev->active_scan_mask,
 100                                      indio_dev->masklength) * 2; j++)
 101                spi_message_add_tail(&xfers[j], &msg);
 102
 103        ret = spi_sync(st->us, &msg);
 104        mutex_unlock(&st->buf_lock);
 105        kfree(xfers);
 106
 107        return ret;
 108}
 109
 110static int lis3l02dq_get_buffer_element(struct iio_dev *indio_dev,
 111                                u8 *buf)
 112{
 113        int ret, i;
 114        u8 *rx_array;
 115        s16 *data = (s16 *)buf;
 116        int scan_count = bitmap_weight(indio_dev->active_scan_mask,
 117                                       indio_dev->masklength);
 118
 119        rx_array = kzalloc(4 * scan_count, GFP_KERNEL);
 120        if (rx_array == NULL)
 121                return -ENOMEM;
 122        ret = lis3l02dq_read_all(indio_dev, rx_array);
 123        if (ret < 0) {
 124                kfree(rx_array);
 125                return ret;
 126        }
 127        for (i = 0; i < scan_count; i++)
 128                data[i] = combine_8_to_16(rx_array[i*4+1],
 129                                        rx_array[i*4+3]);
 130        kfree(rx_array);
 131
 132        return i*sizeof(data[0]);
 133}
 134
 135static irqreturn_t lis3l02dq_trigger_handler(int irq, void *p)
 136{
 137        struct iio_poll_func *pf = p;
 138        struct iio_dev *indio_dev = pf->indio_dev;
 139        int len = 0;
 140        char *data;
 141
 142        data = kmalloc(indio_dev->scan_bytes, GFP_KERNEL);
 143        if (data == NULL)
 144                goto done;
 145
 146        if (!bitmap_empty(indio_dev->active_scan_mask, indio_dev->masklength))
 147                len = lis3l02dq_get_buffer_element(indio_dev, data);
 148
 149        iio_push_to_buffers_with_timestamp(indio_dev, data, pf->timestamp);
 150
 151        kfree(data);
 152done:
 153        iio_trigger_notify_done(indio_dev->trig);
 154        return IRQ_HANDLED;
 155}
 156
 157/* Caller responsible for locking as necessary. */
 158static int
 159__lis3l02dq_write_data_ready_config(struct iio_dev *indio_dev, bool state)
 160{
 161        int ret;
 162        u8 valold;
 163        bool currentlyset;
 164        struct lis3l02dq_state *st = iio_priv(indio_dev);
 165
 166        /* Get the current event mask register */
 167        ret = lis3l02dq_spi_read_reg_8(indio_dev,
 168                                       LIS3L02DQ_REG_CTRL_2_ADDR,
 169                                       &valold);
 170        if (ret)
 171                goto error_ret;
 172        /* Find out if data ready is already on */
 173        currentlyset
 174                = valold & LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
 175
 176        /* Disable requested */
 177        if (!state && currentlyset) {
 178                /* Disable the data ready signal */
 179                valold &= ~LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
 180
 181                /* The double write is to overcome a hardware bug? */
 182                ret = lis3l02dq_spi_write_reg_8(indio_dev,
 183                                                LIS3L02DQ_REG_CTRL_2_ADDR,
 184                                                valold);
 185                if (ret)
 186                        goto error_ret;
 187                ret = lis3l02dq_spi_write_reg_8(indio_dev,
 188                                                LIS3L02DQ_REG_CTRL_2_ADDR,
 189                                                valold);
 190                if (ret)
 191                        goto error_ret;
 192                st->trigger_on = false;
 193        /* Enable requested */
 194        } else if (state && !currentlyset) {
 195                /* If not set, enable requested
 196                 * first disable all events */
 197                ret = lis3l02dq_disable_all_events(indio_dev);
 198                if (ret < 0)
 199                        goto error_ret;
 200
 201                valold = ret |
 202                        LIS3L02DQ_REG_CTRL_2_ENABLE_DATA_READY_GENERATION;
 203
 204                st->trigger_on = true;
 205                ret = lis3l02dq_spi_write_reg_8(indio_dev,
 206                                                LIS3L02DQ_REG_CTRL_2_ADDR,
 207                                                valold);
 208                if (ret)
 209                        goto error_ret;
 210        }
 211
 212        return 0;
 213error_ret:
 214        return ret;
 215}
 216
 217/**
 218 * lis3l02dq_data_rdy_trigger_set_state() set datardy interrupt state
 219 *
 220 * If disabling the interrupt also does a final read to ensure it is clear.
 221 * This is only important in some cases where the scan enable elements are
 222 * switched before the buffer is reenabled.
 223 **/
 224static int lis3l02dq_data_rdy_trigger_set_state(struct iio_trigger *trig,
 225                                                bool state)
 226{
 227        struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 228        int ret = 0;
 229        u8 t;
 230
 231        __lis3l02dq_write_data_ready_config(indio_dev, state);
 232        if (!state) {
 233                /*
 234                 * A possible quirk with the handler is currently worked around
 235                 * by ensuring outstanding read events are cleared.
 236                 */
 237                ret = lis3l02dq_read_all(indio_dev, NULL);
 238        }
 239        lis3l02dq_spi_read_reg_8(indio_dev,
 240                                 LIS3L02DQ_REG_WAKE_UP_SRC_ADDR,
 241                                 &t);
 242        return ret;
 243}
 244
 245/**
 246 * lis3l02dq_trig_try_reen() try reenabling irq for data rdy trigger
 247 * @trig:       the datardy trigger
 248 */
 249static int lis3l02dq_trig_try_reen(struct iio_trigger *trig)
 250{
 251        struct iio_dev *indio_dev = iio_trigger_get_drvdata(trig);
 252        struct lis3l02dq_state *st = iio_priv(indio_dev);
 253        int i;
 254
 255        /* If gpio still high (or high again)
 256         * In theory possible we will need to do this several times */
 257        for (i = 0; i < 5; i++)
 258                if (gpio_get_value(st->gpio))
 259                        lis3l02dq_read_all(indio_dev, NULL);
 260                else
 261                        break;
 262        if (i == 5)
 263                pr_info("Failed to clear the interrupt for lis3l02dq\n");
 264
 265        /* irq reenabled so success! */
 266        return 0;
 267}
 268
 269static const struct iio_trigger_ops lis3l02dq_trigger_ops = {
 270        .owner = THIS_MODULE,
 271        .set_trigger_state = &lis3l02dq_data_rdy_trigger_set_state,
 272        .try_reenable = &lis3l02dq_trig_try_reen,
 273};
 274
 275int lis3l02dq_probe_trigger(struct iio_dev *indio_dev)
 276{
 277        int ret;
 278        struct lis3l02dq_state *st = iio_priv(indio_dev);
 279
 280        st->trig = iio_trigger_alloc("lis3l02dq-dev%d", indio_dev->id);
 281        if (!st->trig) {
 282                ret = -ENOMEM;
 283                goto error_ret;
 284        }
 285
 286        st->trig->dev.parent = &st->us->dev;
 287        st->trig->ops = &lis3l02dq_trigger_ops;
 288        iio_trigger_set_drvdata(st->trig, indio_dev);
 289        ret = iio_trigger_register(st->trig);
 290        if (ret)
 291                goto error_free_trig;
 292
 293        return 0;
 294
 295error_free_trig:
 296        iio_trigger_free(st->trig);
 297error_ret:
 298        return ret;
 299}
 300
 301void lis3l02dq_remove_trigger(struct iio_dev *indio_dev)
 302{
 303        struct lis3l02dq_state *st = iio_priv(indio_dev);
 304
 305        iio_trigger_unregister(st->trig);
 306        iio_trigger_free(st->trig);
 307}
 308
 309void lis3l02dq_unconfigure_buffer(struct iio_dev *indio_dev)
 310{
 311        iio_dealloc_pollfunc(indio_dev->pollfunc);
 312        iio_kfifo_free(indio_dev->buffer);
 313}
 314
 315static int lis3l02dq_buffer_postenable(struct iio_dev *indio_dev)
 316{
 317        /* Disable unwanted channels otherwise the interrupt will not clear */
 318        u8 t;
 319        int ret;
 320        bool oneenabled = false;
 321
 322        ret = lis3l02dq_spi_read_reg_8(indio_dev,
 323                                       LIS3L02DQ_REG_CTRL_1_ADDR,
 324                                       &t);
 325        if (ret)
 326                goto error_ret;
 327
 328        if (test_bit(0, indio_dev->active_scan_mask)) {
 329                t |= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
 330                oneenabled = true;
 331        } else
 332                t &= ~LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE;
 333        if (test_bit(1, indio_dev->active_scan_mask)) {
 334                t |= LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
 335                oneenabled = true;
 336        } else
 337                t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE;
 338        if (test_bit(2, indio_dev->active_scan_mask)) {
 339                t |= LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
 340                oneenabled = true;
 341        } else
 342                t &= ~LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
 343
 344        if (!oneenabled) /* what happens in this case is unknown */
 345                return -EINVAL;
 346        ret = lis3l02dq_spi_write_reg_8(indio_dev,
 347                                        LIS3L02DQ_REG_CTRL_1_ADDR,
 348                                        t);
 349        if (ret)
 350                goto error_ret;
 351
 352        return iio_triggered_buffer_postenable(indio_dev);
 353error_ret:
 354        return ret;
 355}
 356
 357/* Turn all channels on again */
 358static int lis3l02dq_buffer_predisable(struct iio_dev *indio_dev)
 359{
 360        u8 t;
 361        int ret;
 362
 363        ret = iio_triggered_buffer_predisable(indio_dev);
 364        if (ret)
 365                goto error_ret;
 366
 367        ret = lis3l02dq_spi_read_reg_8(indio_dev,
 368                                       LIS3L02DQ_REG_CTRL_1_ADDR,
 369                                       &t);
 370        if (ret)
 371                goto error_ret;
 372        t |= LIS3L02DQ_REG_CTRL_1_AXES_X_ENABLE |
 373                LIS3L02DQ_REG_CTRL_1_AXES_Y_ENABLE |
 374                LIS3L02DQ_REG_CTRL_1_AXES_Z_ENABLE;
 375
 376        ret = lis3l02dq_spi_write_reg_8(indio_dev,
 377                                        LIS3L02DQ_REG_CTRL_1_ADDR,
 378                                        t);
 379
 380error_ret:
 381        return ret;
 382}
 383
 384static const struct iio_buffer_setup_ops lis3l02dq_buffer_setup_ops = {
 385        .postenable = &lis3l02dq_buffer_postenable,
 386        .predisable = &lis3l02dq_buffer_predisable,
 387};
 388
 389int lis3l02dq_configure_buffer(struct iio_dev *indio_dev)
 390{
 391        int ret;
 392        struct iio_buffer *buffer;
 393
 394        buffer = iio_kfifo_allocate(indio_dev);
 395        if (!buffer)
 396                return -ENOMEM;
 397
 398        iio_device_attach_buffer(indio_dev, buffer);
 399
 400        buffer->scan_timestamp = true;
 401        indio_dev->setup_ops = &lis3l02dq_buffer_setup_ops;
 402
 403        /* Functions are NULL as we set handler below */
 404        indio_dev->pollfunc = iio_alloc_pollfunc(&iio_pollfunc_store_time,
 405                                                 &lis3l02dq_trigger_handler,
 406                                                 0,
 407                                                 indio_dev,
 408                                                 "lis3l02dq_consumer%d",
 409                                                 indio_dev->id);
 410
 411        if (indio_dev->pollfunc == NULL) {
 412                ret = -ENOMEM;
 413                goto error_iio_sw_rb_free;
 414        }
 415
 416        indio_dev->modes |= INDIO_BUFFER_TRIGGERED;
 417        return 0;
 418
 419error_iio_sw_rb_free:
 420        iio_kfifo_free(indio_dev->buffer);
 421        return ret;
 422}
 423