drivers/staging/iio/accel/lis3l02dq_ring.c

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