drivers/media/test-drivers/vivid/vivid-kthread-touch.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * vivid-kthread-touch.c - touch capture thread support functions.
   4  *
   5  */
   6
   7 #include <linux/freezer.h>
   8 #include "vivid-core.h"
   9 #include "vivid-kthread-touch.h"
  10 #include "vivid-touch-cap.h"
  11
  12 static noinline_for_stack void vivid_thread_tch_cap_tick(struct vivid_dev *dev,
  13                                                          int dropped_bufs)
  14 {
  15         struct vivid_buffer *tch_cap_buf = NULL;
  16
  17         spin_lock(&dev->slock);
  18         if (!list_empty(&dev->touch_cap_active)) {
  19                 tch_cap_buf = list_entry(dev->touch_cap_active.next,
  20                                          struct vivid_buffer, list);
  21                 list_del(&tch_cap_buf->list);
  22         }
  23
  24         spin_unlock(&dev->slock);
  25
  26         if (tch_cap_buf) {
  27                 v4l2_ctrl_request_setup(tch_cap_buf->vb.vb2_buf.req_obj.req,
  28                                         &dev->ctrl_hdl_touch_cap);
  29
  30                 vivid_fillbuff_tch(dev, tch_cap_buf);
  31                 v4l2_ctrl_request_complete(tch_cap_buf->vb.vb2_buf.req_obj.req,
  32                                            &dev->ctrl_hdl_touch_cap);
  33                 vb2_buffer_done(&tch_cap_buf->vb.vb2_buf, dev->dqbuf_error ?
  34                                 VB2_BUF_STATE_ERROR : VB2_BUF_STATE_DONE);
  35                 dprintk(dev, 2, "touch_cap buffer %d done\n",
  36                         tch_cap_buf->vb.vb2_buf.index);
  37
  38                 tch_cap_buf->vb.vb2_buf.timestamp = ktime_get_ns() + dev->time_wrap_offset;
  39         }
  40         dev->dqbuf_error = false;
  41 }
  42
  43 static int vivid_thread_touch_cap(void *data)
  44 {
  45         struct vivid_dev *dev = data;
  46         u64 numerators_since_start;
  47         u64 buffers_since_start;
  48         u64 next_jiffies_since_start;
  49         unsigned long jiffies_since_start;
  50         unsigned long cur_jiffies;
  51         unsigned int wait_jiffies;
  52         unsigned int numerator;
  53         unsigned int denominator;
  54         int dropped_bufs;
  55
  56         dprintk(dev, 1, "Touch Capture Thread Start\n");
  57
  58         set_freezable();
  59
  60         /* Resets frame counters */
  61         dev->touch_cap_seq_offset = 0;
  62         dev->touch_cap_seq_count = 0;
  63         dev->touch_cap_seq_resync = false;
  64         dev->jiffies_touch_cap = jiffies;
  65
  66         for (;;) {
  67                 try_to_freeze();
  68                 if (kthread_should_stop())
  69                         break;
  70
  71                 if (!mutex_trylock(&dev->mutex)) {
  72                         schedule();
  73                         continue;
  74                 }
  75                 cur_jiffies = jiffies;
  76                 if (dev->touch_cap_seq_resync) {
  77                         dev->jiffies_touch_cap = cur_jiffies;
  78                         dev->touch_cap_seq_offset = dev->touch_cap_seq_count + 1;
  79                         dev->touch_cap_seq_count = 0;
  80                         dev->cap_seq_resync = false;
  81                 }
  82                 denominator = dev->timeperframe_tch_cap.denominator;
  83                 numerator = dev->timeperframe_tch_cap.numerator;
  84
  85                 /* Calculate the number of jiffies since we started streaming */
  86                 jiffies_since_start = cur_jiffies - dev->jiffies_touch_cap;
  87                 /* Get the number of buffers streamed since the start */
  88                 buffers_since_start = (u64)jiffies_since_start * denominator +
  89                                       (HZ * numerator) / 2;
  90                 do_div(buffers_since_start, HZ * numerator);
  91
  92                 /*
  93                  * After more than 0xf0000000 (rounded down to a multiple of
  94                  * 'jiffies-per-day' to ease jiffies_to_msecs calculation)
  95                  * jiffies have passed since we started streaming reset the
  96                  * counters and keep track of the sequence offset.
  97                  */
  98                 if (jiffies_since_start > JIFFIES_RESYNC) {
  99                         dev->jiffies_touch_cap = cur_jiffies;
 100                         dev->cap_seq_offset = buffers_since_start;
 101                         buffers_since_start = 0;
 102                 }
 103                 dropped_bufs = buffers_since_start + dev->touch_cap_seq_offset - dev->touch_cap_seq_count;
 104                 dev->touch_cap_seq_count = buffers_since_start + dev->touch_cap_seq_offset;
 105
 106                 vivid_thread_tch_cap_tick(dev, dropped_bufs);
 107
 108                 /*
 109                  * Calculate the number of 'numerators' streamed
 110                  * since we started, including the current buffer.
 111                  */
 112                 numerators_since_start = ++buffers_since_start * numerator;
 113
 114                 /* And the number of jiffies since we started */
 115                 jiffies_since_start = jiffies - dev->jiffies_touch_cap;
 116
 117                 mutex_unlock(&dev->mutex);
 118
 119                 /*
 120                  * Calculate when that next buffer is supposed to start
 121                  * in jiffies since we started streaming.
 122                  */
 123                 next_jiffies_since_start = numerators_since_start * HZ +
 124                                            denominator / 2;
 125                 do_div(next_jiffies_since_start, denominator);
 126                 /* If it is in the past, then just schedule asap */
 127                 if (next_jiffies_since_start < jiffies_since_start)
 128                         next_jiffies_since_start = jiffies_since_start;
 129
 130                 wait_jiffies = next_jiffies_since_start - jiffies_since_start;
 131                 while (jiffies - cur_jiffies < wait_jiffies &&
 132                        !kthread_should_stop())
 133                         schedule();
 134         }
 135         dprintk(dev, 1, "Touch Capture Thread End\n");
 136         return 0;
 137 }
 138
 139 int vivid_start_generating_touch_cap(struct vivid_dev *dev)
 140 {
 141         if (dev->kthread_touch_cap) {
 142                 dev->touch_cap_streaming = true;
 143                 return 0;
 144         }
 145
 146         dev->kthread_touch_cap = kthread_run(vivid_thread_touch_cap, dev,
 147                                              "%s-tch-cap", dev->v4l2_dev.name);
 148
 149         if (IS_ERR(dev->kthread_touch_cap)) {
 150                 int err = PTR_ERR(dev->kthread_touch_cap);
 151
 152                 dev->kthread_touch_cap = NULL;
 153                 v4l2_err(&dev->v4l2_dev, "kernel_thread() failed\n");
 154                 return err;
 155         }
 156         dev->touch_cap_streaming = true;
 157         dprintk(dev, 1, "returning from %s\n", __func__);
 158         return 0;
 159 }
 160
 161 void vivid_stop_generating_touch_cap(struct vivid_dev *dev)
 162 {
 163         if (!dev->kthread_touch_cap)
 164                 return;
 165
 166         dev->touch_cap_streaming = false;
 167
 168         while (!list_empty(&dev->touch_cap_active)) {
 169                 struct vivid_buffer *buf;
 170
 171                 buf = list_entry(dev->touch_cap_active.next,
 172                                  struct vivid_buffer, list);
 173                 list_del(&buf->list);
 174                 v4l2_ctrl_request_complete(buf->vb.vb2_buf.req_obj.req,
 175                                            &dev->ctrl_hdl_touch_cap);
 176                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
 177                 dprintk(dev, 2, "touch_cap buffer %d done\n",
 178                         buf->vb.vb2_buf.index);
 179         }
 180
 181         kthread_stop(dev->kthread_touch_cap);
 182         dev->kthread_touch_cap = NULL;
 183 }