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