drivers/staging/iio/accel/lis3l02dq_ring.c

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