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 <linux/jiffies.h> |
9 | #include "vivid-core.h" |
10 | #include "vivid-kthread-touch.h" |
11 | #include "vivid-touch-cap.h" |
12 | |
13 | static 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(lock: &dev->slock); |
19 | if (!list_empty(head: &dev->touch_cap_active)) { |
20 | tch_cap_buf = list_entry(dev->touch_cap_active.next, |
21 | struct vivid_buffer, list); |
22 | list_del(entry: &tch_cap_buf->list); |
23 | } |
24 | |
25 | spin_unlock(lock: &dev->slock); |
26 | |
27 | if (tch_cap_buf) { |
28 | v4l2_ctrl_request_setup(req: tch_cap_buf->vb.vb2_buf.req_obj.req, |
29 | parent: &dev->ctrl_hdl_touch_cap); |
30 | |
31 | vivid_fillbuff_tch(dev, buf: tch_cap_buf); |
32 | v4l2_ctrl_request_complete(req: tch_cap_buf->vb.vb2_buf.req_obj.req, |
33 | parent: &dev->ctrl_hdl_touch_cap); |
34 | vb2_buffer_done(vb: &tch_cap_buf->vb.vb2_buf, state: 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 | |
44 | static 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 | else |
69 | dev->time_wrap_offset = 0; |
70 | |
71 | for (;;) { |
72 | try_to_freeze(); |
73 | if (kthread_should_stop()) |
74 | break; |
75 | |
76 | if (!mutex_trylock(lock: &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 of |
99 | * 'jiffies-per-day' to ease jiffies_to_msecs calculation) |
100 | * jiffies have passed since we started streaming reset the |
101 | * 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' streamed |
117 | * 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(lock: &dev->mutex); |
125 | |
126 | /* |
127 | * Calculate when that next buffer is supposed to start |
128 | * 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 | |
146 | int 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(ptr: dev->kthread_touch_cap)) { |
158 | int err = PTR_ERR(ptr: 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 | |
169 | void 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(head: &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(entry: &buf->list); |
182 | v4l2_ctrl_request_complete(req: buf->vb.vb2_buf.req_obj.req, |
183 | parent: &dev->ctrl_hdl_touch_cap); |
184 | vb2_buffer_done(vb: &buf->vb.vb2_buf, state: 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(k: dev->kthread_touch_cap); |
190 | dev->kthread_touch_cap = NULL; |
191 | } |
192 | |