1 | // SPDX-License-Identifier: GPL-2.0+ |
2 | /* |
3 | * Frame Interval Monitor. |
4 | * |
5 | * Copyright (c) 2016 Mentor Graphics Inc. |
6 | */ |
7 | #include <linux/delay.h> |
8 | #include <linux/irq.h> |
9 | #include <linux/module.h> |
10 | #include <linux/platform_device.h> |
11 | #include <linux/slab.h> |
12 | #include <linux/spinlock.h> |
13 | #include <media/v4l2-ctrls.h> |
14 | #include <media/v4l2-subdev.h> |
15 | #include <media/imx.h> |
16 | #include "imx-media.h" |
17 | |
18 | enum { |
19 | FIM_CL_ENABLE = 0, |
20 | FIM_CL_NUM, |
21 | FIM_CL_TOLERANCE_MIN, |
22 | FIM_CL_TOLERANCE_MAX, |
23 | FIM_CL_NUM_SKIP, |
24 | FIM_NUM_CONTROLS, |
25 | }; |
26 | |
27 | enum { |
28 | FIM_CL_ICAP_EDGE = 0, |
29 | FIM_CL_ICAP_CHANNEL, |
30 | FIM_NUM_ICAP_CONTROLS, |
31 | }; |
32 | |
33 | #define FIM_CL_ENABLE_DEF 0 /* FIM disabled by default */ |
34 | #define FIM_CL_NUM_DEF 8 /* average 8 frames */ |
35 | #define FIM_CL_NUM_SKIP_DEF 2 /* skip 2 frames after restart */ |
36 | #define FIM_CL_TOLERANCE_MIN_DEF 50 /* usec */ |
37 | #define FIM_CL_TOLERANCE_MAX_DEF 0 /* no max tolerance (unbounded) */ |
38 | |
39 | struct imx_media_fim { |
40 | /* the owning subdev of this fim instance */ |
41 | struct v4l2_subdev *sd; |
42 | |
43 | /* FIM's control handler */ |
44 | struct v4l2_ctrl_handler ctrl_handler; |
45 | |
46 | /* control clusters */ |
47 | struct v4l2_ctrl *ctrl[FIM_NUM_CONTROLS]; |
48 | struct v4l2_ctrl *icap_ctrl[FIM_NUM_ICAP_CONTROLS]; |
49 | |
50 | spinlock_t lock; /* protect control values */ |
51 | |
52 | /* current control values */ |
53 | bool enabled; |
54 | int num_avg; |
55 | int num_skip; |
56 | unsigned long tolerance_min; /* usec */ |
57 | unsigned long tolerance_max; /* usec */ |
58 | /* input capture method of measuring FI */ |
59 | int icap_channel; |
60 | int icap_flags; |
61 | |
62 | int counter; |
63 | ktime_t last_ts; |
64 | unsigned long sum; /* usec */ |
65 | unsigned long nominal; /* usec */ |
66 | |
67 | struct completion icap_first_event; |
68 | bool stream_on; |
69 | }; |
70 | |
71 | static bool icap_enabled(struct imx_media_fim *fim) |
72 | { |
73 | return fim->icap_flags != IRQ_TYPE_NONE; |
74 | } |
75 | |
76 | static void update_fim_nominal(struct imx_media_fim *fim, |
77 | const struct v4l2_fract *fi) |
78 | { |
79 | if (fi->denominator == 0) { |
80 | dev_dbg(fim->sd->dev, "no frame interval, FIM disabled\n" ); |
81 | fim->enabled = false; |
82 | return; |
83 | } |
84 | |
85 | fim->nominal = DIV_ROUND_CLOSEST_ULL(1000000ULL * (u64)fi->numerator, |
86 | fi->denominator); |
87 | |
88 | dev_dbg(fim->sd->dev, "FI=%lu usec\n" , fim->nominal); |
89 | } |
90 | |
91 | static void reset_fim(struct imx_media_fim *fim, bool curval) |
92 | { |
93 | struct v4l2_ctrl *icap_chan = fim->icap_ctrl[FIM_CL_ICAP_CHANNEL]; |
94 | struct v4l2_ctrl *icap_edge = fim->icap_ctrl[FIM_CL_ICAP_EDGE]; |
95 | struct v4l2_ctrl *en = fim->ctrl[FIM_CL_ENABLE]; |
96 | struct v4l2_ctrl *num = fim->ctrl[FIM_CL_NUM]; |
97 | struct v4l2_ctrl *skip = fim->ctrl[FIM_CL_NUM_SKIP]; |
98 | struct v4l2_ctrl *tol_min = fim->ctrl[FIM_CL_TOLERANCE_MIN]; |
99 | struct v4l2_ctrl *tol_max = fim->ctrl[FIM_CL_TOLERANCE_MAX]; |
100 | |
101 | if (curval) { |
102 | fim->enabled = en->cur.val; |
103 | fim->icap_flags = icap_edge->cur.val; |
104 | fim->icap_channel = icap_chan->cur.val; |
105 | fim->num_avg = num->cur.val; |
106 | fim->num_skip = skip->cur.val; |
107 | fim->tolerance_min = tol_min->cur.val; |
108 | fim->tolerance_max = tol_max->cur.val; |
109 | } else { |
110 | fim->enabled = en->val; |
111 | fim->icap_flags = icap_edge->val; |
112 | fim->icap_channel = icap_chan->val; |
113 | fim->num_avg = num->val; |
114 | fim->num_skip = skip->val; |
115 | fim->tolerance_min = tol_min->val; |
116 | fim->tolerance_max = tol_max->val; |
117 | } |
118 | |
119 | /* disable tolerance range if max <= min */ |
120 | if (fim->tolerance_max <= fim->tolerance_min) |
121 | fim->tolerance_max = 0; |
122 | |
123 | /* num_skip must be >= 1 if input capture not used */ |
124 | if (!icap_enabled(fim)) |
125 | fim->num_skip = max_t(int, fim->num_skip, 1); |
126 | |
127 | fim->counter = -fim->num_skip; |
128 | fim->sum = 0; |
129 | } |
130 | |
131 | static void send_fim_event(struct imx_media_fim *fim, unsigned long error) |
132 | { |
133 | static const struct v4l2_event ev = { |
134 | .type = V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR, |
135 | }; |
136 | |
137 | v4l2_subdev_notify_event(sd: fim->sd, ev: &ev); |
138 | } |
139 | |
140 | /* |
141 | * Monitor an averaged frame interval. If the average deviates too much |
142 | * from the nominal frame rate, send the frame interval error event. The |
143 | * frame intervals are averaged in order to quiet noise from |
144 | * (presumably random) interrupt latency. |
145 | */ |
146 | static void frame_interval_monitor(struct imx_media_fim *fim, |
147 | ktime_t timestamp) |
148 | { |
149 | long long interval, error; |
150 | unsigned long error_avg; |
151 | bool send_event = false; |
152 | |
153 | if (!fim->enabled || ++fim->counter <= 0) |
154 | goto out_update_ts; |
155 | |
156 | /* max error is less than l00µs, so use 32-bit division or fail */ |
157 | interval = ktime_to_ns(ktime_sub(timestamp, fim->last_ts)); |
158 | error = abs(interval - NSEC_PER_USEC * (u64)fim->nominal); |
159 | if (error > U32_MAX) |
160 | error = U32_MAX; |
161 | else |
162 | error = abs((u32)error / NSEC_PER_USEC); |
163 | |
164 | if (fim->tolerance_max && error >= fim->tolerance_max) { |
165 | dev_dbg(fim->sd->dev, |
166 | "FIM: %llu ignored, out of tolerance bounds\n" , |
167 | error); |
168 | fim->counter--; |
169 | goto out_update_ts; |
170 | } |
171 | |
172 | fim->sum += error; |
173 | |
174 | if (fim->counter == fim->num_avg) { |
175 | error_avg = DIV_ROUND_CLOSEST(fim->sum, fim->num_avg); |
176 | |
177 | if (error_avg > fim->tolerance_min) |
178 | send_event = true; |
179 | |
180 | dev_dbg(fim->sd->dev, "FIM: error: %lu usec%s\n" , |
181 | error_avg, send_event ? " (!!!)" : "" ); |
182 | |
183 | fim->counter = 0; |
184 | fim->sum = 0; |
185 | } |
186 | |
187 | out_update_ts: |
188 | fim->last_ts = timestamp; |
189 | if (send_event) |
190 | send_fim_event(fim, error: error_avg); |
191 | } |
192 | |
193 | /* |
194 | * In case we are monitoring the first frame interval after streamon |
195 | * (when fim->num_skip = 0), we need a valid fim->last_ts before we |
196 | * can begin. This only applies to the input capture method. It is not |
197 | * possible to accurately measure the first FI after streamon using the |
198 | * EOF method, so fim->num_skip minimum is set to 1 in that case, so this |
199 | * function is a noop when the EOF method is used. |
200 | */ |
201 | static void fim_acquire_first_ts(struct imx_media_fim *fim) |
202 | { |
203 | unsigned long ret; |
204 | |
205 | if (!fim->enabled || fim->num_skip > 0) |
206 | return; |
207 | |
208 | ret = wait_for_completion_timeout( |
209 | x: &fim->icap_first_event, |
210 | timeout: msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT)); |
211 | if (ret == 0) |
212 | v4l2_warn(fim->sd, "wait first icap event timeout\n" ); |
213 | } |
214 | |
215 | /* FIM Controls */ |
216 | static int fim_s_ctrl(struct v4l2_ctrl *ctrl) |
217 | { |
218 | struct imx_media_fim *fim = container_of(ctrl->handler, |
219 | struct imx_media_fim, |
220 | ctrl_handler); |
221 | unsigned long flags; |
222 | int ret = 0; |
223 | |
224 | spin_lock_irqsave(&fim->lock, flags); |
225 | |
226 | switch (ctrl->id) { |
227 | case V4L2_CID_IMX_FIM_ENABLE: |
228 | break; |
229 | case V4L2_CID_IMX_FIM_ICAP_EDGE: |
230 | if (fim->stream_on) |
231 | ret = -EBUSY; |
232 | break; |
233 | default: |
234 | ret = -EINVAL; |
235 | } |
236 | |
237 | if (!ret) |
238 | reset_fim(fim, curval: false); |
239 | |
240 | spin_unlock_irqrestore(lock: &fim->lock, flags); |
241 | return ret; |
242 | } |
243 | |
244 | static const struct v4l2_ctrl_ops fim_ctrl_ops = { |
245 | .s_ctrl = fim_s_ctrl, |
246 | }; |
247 | |
248 | static const struct v4l2_ctrl_config fim_ctrl[] = { |
249 | [FIM_CL_ENABLE] = { |
250 | .ops = &fim_ctrl_ops, |
251 | .id = V4L2_CID_IMX_FIM_ENABLE, |
252 | .name = "FIM Enable" , |
253 | .type = V4L2_CTRL_TYPE_BOOLEAN, |
254 | .def = FIM_CL_ENABLE_DEF, |
255 | .min = 0, |
256 | .max = 1, |
257 | .step = 1, |
258 | }, |
259 | [FIM_CL_NUM] = { |
260 | .ops = &fim_ctrl_ops, |
261 | .id = V4L2_CID_IMX_FIM_NUM, |
262 | .name = "FIM Num Average" , |
263 | .type = V4L2_CTRL_TYPE_INTEGER, |
264 | .def = FIM_CL_NUM_DEF, |
265 | .min = 1, /* no averaging */ |
266 | .max = 64, /* average 64 frames */ |
267 | .step = 1, |
268 | }, |
269 | [FIM_CL_TOLERANCE_MIN] = { |
270 | .ops = &fim_ctrl_ops, |
271 | .id = V4L2_CID_IMX_FIM_TOLERANCE_MIN, |
272 | .name = "FIM Tolerance Min" , |
273 | .type = V4L2_CTRL_TYPE_INTEGER, |
274 | .def = FIM_CL_TOLERANCE_MIN_DEF, |
275 | .min = 2, |
276 | .max = 200, |
277 | .step = 1, |
278 | }, |
279 | [FIM_CL_TOLERANCE_MAX] = { |
280 | .ops = &fim_ctrl_ops, |
281 | .id = V4L2_CID_IMX_FIM_TOLERANCE_MAX, |
282 | .name = "FIM Tolerance Max" , |
283 | .type = V4L2_CTRL_TYPE_INTEGER, |
284 | .def = FIM_CL_TOLERANCE_MAX_DEF, |
285 | .min = 0, |
286 | .max = 500, |
287 | .step = 1, |
288 | }, |
289 | [FIM_CL_NUM_SKIP] = { |
290 | .ops = &fim_ctrl_ops, |
291 | .id = V4L2_CID_IMX_FIM_NUM_SKIP, |
292 | .name = "FIM Num Skip" , |
293 | .type = V4L2_CTRL_TYPE_INTEGER, |
294 | .def = FIM_CL_NUM_SKIP_DEF, |
295 | .min = 0, /* skip no frames */ |
296 | .max = 256, /* skip 256 frames */ |
297 | .step = 1, |
298 | }, |
299 | }; |
300 | |
301 | static const struct v4l2_ctrl_config fim_icap_ctrl[] = { |
302 | [FIM_CL_ICAP_EDGE] = { |
303 | .ops = &fim_ctrl_ops, |
304 | .id = V4L2_CID_IMX_FIM_ICAP_EDGE, |
305 | .name = "FIM Input Capture Edge" , |
306 | .type = V4L2_CTRL_TYPE_INTEGER, |
307 | .def = IRQ_TYPE_NONE, /* input capture disabled by default */ |
308 | .min = IRQ_TYPE_NONE, |
309 | .max = IRQ_TYPE_EDGE_BOTH, |
310 | .step = 1, |
311 | }, |
312 | [FIM_CL_ICAP_CHANNEL] = { |
313 | .ops = &fim_ctrl_ops, |
314 | .id = V4L2_CID_IMX_FIM_ICAP_CHANNEL, |
315 | .name = "FIM Input Capture Channel" , |
316 | .type = V4L2_CTRL_TYPE_INTEGER, |
317 | .def = 0, |
318 | .min = 0, |
319 | .max = 1, |
320 | .step = 1, |
321 | }, |
322 | }; |
323 | |
324 | static int init_fim_controls(struct imx_media_fim *fim) |
325 | { |
326 | struct v4l2_ctrl_handler *hdlr = &fim->ctrl_handler; |
327 | int i, ret; |
328 | |
329 | v4l2_ctrl_handler_init(hdlr, FIM_NUM_CONTROLS + FIM_NUM_ICAP_CONTROLS); |
330 | |
331 | for (i = 0; i < FIM_NUM_CONTROLS; i++) |
332 | fim->ctrl[i] = v4l2_ctrl_new_custom(hdl: hdlr, |
333 | cfg: &fim_ctrl[i], |
334 | NULL); |
335 | for (i = 0; i < FIM_NUM_ICAP_CONTROLS; i++) |
336 | fim->icap_ctrl[i] = v4l2_ctrl_new_custom(hdl: hdlr, |
337 | cfg: &fim_icap_ctrl[i], |
338 | NULL); |
339 | if (hdlr->error) { |
340 | ret = hdlr->error; |
341 | goto err_free; |
342 | } |
343 | |
344 | v4l2_ctrl_cluster(ncontrols: FIM_NUM_CONTROLS, controls: fim->ctrl); |
345 | v4l2_ctrl_cluster(ncontrols: FIM_NUM_ICAP_CONTROLS, controls: fim->icap_ctrl); |
346 | |
347 | return 0; |
348 | err_free: |
349 | v4l2_ctrl_handler_free(hdl: hdlr); |
350 | return ret; |
351 | } |
352 | |
353 | /* |
354 | * Monitor frame intervals via EOF interrupt. This method is |
355 | * subject to uncertainty errors introduced by interrupt latency. |
356 | * |
357 | * This is a noop if the Input Capture method is being used, since |
358 | * the frame_interval_monitor() is called by the input capture event |
359 | * callback handler in that case. |
360 | */ |
361 | void imx_media_fim_eof_monitor(struct imx_media_fim *fim, ktime_t timestamp) |
362 | { |
363 | unsigned long flags; |
364 | |
365 | spin_lock_irqsave(&fim->lock, flags); |
366 | |
367 | if (!icap_enabled(fim)) |
368 | frame_interval_monitor(fim, timestamp); |
369 | |
370 | spin_unlock_irqrestore(lock: &fim->lock, flags); |
371 | } |
372 | |
373 | /* Called by the subdev in its s_stream callback */ |
374 | void imx_media_fim_set_stream(struct imx_media_fim *fim, |
375 | const struct v4l2_fract *fi, |
376 | bool on) |
377 | { |
378 | unsigned long flags; |
379 | |
380 | v4l2_ctrl_lock(ctrl: fim->ctrl[FIM_CL_ENABLE]); |
381 | |
382 | if (fim->stream_on == on) |
383 | goto out; |
384 | |
385 | if (on) { |
386 | spin_lock_irqsave(&fim->lock, flags); |
387 | reset_fim(fim, curval: true); |
388 | update_fim_nominal(fim, fi); |
389 | spin_unlock_irqrestore(lock: &fim->lock, flags); |
390 | |
391 | if (icap_enabled(fim)) |
392 | fim_acquire_first_ts(fim); |
393 | } |
394 | |
395 | fim->stream_on = on; |
396 | out: |
397 | v4l2_ctrl_unlock(ctrl: fim->ctrl[FIM_CL_ENABLE]); |
398 | } |
399 | |
400 | int imx_media_fim_add_controls(struct imx_media_fim *fim) |
401 | { |
402 | /* add the FIM controls to the calling subdev ctrl handler */ |
403 | return v4l2_ctrl_add_handler(hdl: fim->sd->ctrl_handler, |
404 | add: &fim->ctrl_handler, NULL, from_other_dev: true); |
405 | } |
406 | |
407 | /* Called by the subdev in its subdev registered callback */ |
408 | struct imx_media_fim *imx_media_fim_init(struct v4l2_subdev *sd) |
409 | { |
410 | struct imx_media_fim *fim; |
411 | int ret; |
412 | |
413 | fim = devm_kzalloc(dev: sd->dev, size: sizeof(*fim), GFP_KERNEL); |
414 | if (!fim) |
415 | return ERR_PTR(error: -ENOMEM); |
416 | |
417 | fim->sd = sd; |
418 | |
419 | spin_lock_init(&fim->lock); |
420 | |
421 | ret = init_fim_controls(fim); |
422 | if (ret) |
423 | return ERR_PTR(error: ret); |
424 | |
425 | return fim; |
426 | } |
427 | |
428 | void imx_media_fim_free(struct imx_media_fim *fim) |
429 | { |
430 | v4l2_ctrl_handler_free(hdl: &fim->ctrl_handler); |
431 | } |
432 | |