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(&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 
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(&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(&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(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 
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(&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