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 #define icap_enabled(fim) ((fim)->icap_flags != IRQ_TYPE_NONE)
72
update_fim_nominal(struct imx_media_fim * fim,const struct v4l2_fract * fi)73 static void update_fim_nominal(struct imx_media_fim *fim,
74 const struct v4l2_fract *fi)
75 {
76 if (fi->denominator == 0) {
77 dev_dbg(fim->sd->dev, "no frame interval, FIM disabled\n");
78 fim->enabled = false;
79 return;
80 }
81
82 fim->nominal = DIV_ROUND_CLOSEST_ULL(1000000ULL * (u64)fi->numerator,
83 fi->denominator);
84
85 dev_dbg(fim->sd->dev, "FI=%lu usec\n", fim->nominal);
86 }
87
reset_fim(struct imx_media_fim * fim,bool curval)88 static void reset_fim(struct imx_media_fim *fim, bool curval)
89 {
90 struct v4l2_ctrl *icap_chan = fim->icap_ctrl[FIM_CL_ICAP_CHANNEL];
91 struct v4l2_ctrl *icap_edge = fim->icap_ctrl[FIM_CL_ICAP_EDGE];
92 struct v4l2_ctrl *en = fim->ctrl[FIM_CL_ENABLE];
93 struct v4l2_ctrl *num = fim->ctrl[FIM_CL_NUM];
94 struct v4l2_ctrl *skip = fim->ctrl[FIM_CL_NUM_SKIP];
95 struct v4l2_ctrl *tol_min = fim->ctrl[FIM_CL_TOLERANCE_MIN];
96 struct v4l2_ctrl *tol_max = fim->ctrl[FIM_CL_TOLERANCE_MAX];
97
98 if (curval) {
99 fim->enabled = en->cur.val;
100 fim->icap_flags = icap_edge->cur.val;
101 fim->icap_channel = icap_chan->cur.val;
102 fim->num_avg = num->cur.val;
103 fim->num_skip = skip->cur.val;
104 fim->tolerance_min = tol_min->cur.val;
105 fim->tolerance_max = tol_max->cur.val;
106 } else {
107 fim->enabled = en->val;
108 fim->icap_flags = icap_edge->val;
109 fim->icap_channel = icap_chan->val;
110 fim->num_avg = num->val;
111 fim->num_skip = skip->val;
112 fim->tolerance_min = tol_min->val;
113 fim->tolerance_max = tol_max->val;
114 }
115
116 /* disable tolerance range if max <= min */
117 if (fim->tolerance_max <= fim->tolerance_min)
118 fim->tolerance_max = 0;
119
120 /* num_skip must be >= 1 if input capture not used */
121 if (!icap_enabled(fim))
122 fim->num_skip = max_t(int, fim->num_skip, 1);
123
124 fim->counter = -fim->num_skip;
125 fim->sum = 0;
126 }
127
send_fim_event(struct imx_media_fim * fim,unsigned long error)128 static void send_fim_event(struct imx_media_fim *fim, unsigned long error)
129 {
130 static const struct v4l2_event ev = {
131 .type = V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR,
132 };
133
134 v4l2_subdev_notify_event(fim->sd, &ev);
135 }
136
137 /*
138 * Monitor an averaged frame interval. If the average deviates too much
139 * from the nominal frame rate, send the frame interval error event. The
140 * frame intervals are averaged in order to quiet noise from
141 * (presumably random) interrupt latency.
142 */
frame_interval_monitor(struct imx_media_fim * fim,ktime_t timestamp)143 static void frame_interval_monitor(struct imx_media_fim *fim,
144 ktime_t timestamp)
145 {
146 long long interval, error;
147 unsigned long error_avg;
148 bool send_event = false;
149
150 if (!fim->enabled || ++fim->counter <= 0)
151 goto out_update_ts;
152
153 /* max error is less than l00µs, so use 32-bit division or fail */
154 interval = ktime_to_ns(ktime_sub(timestamp, fim->last_ts));
155 error = abs(interval - NSEC_PER_USEC * (u64)fim->nominal);
156 if (error > U32_MAX)
157 error = U32_MAX;
158 else
159 error = abs((u32)error / NSEC_PER_USEC);
160
161 if (fim->tolerance_max && error >= fim->tolerance_max) {
162 dev_dbg(fim->sd->dev,
163 "FIM: %llu ignored, out of tolerance bounds\n",
164 error);
165 fim->counter--;
166 goto out_update_ts;
167 }
168
169 fim->sum += error;
170
171 if (fim->counter == fim->num_avg) {
172 error_avg = DIV_ROUND_CLOSEST(fim->sum, fim->num_avg);
173
174 if (error_avg > fim->tolerance_min)
175 send_event = true;
176
177 dev_dbg(fim->sd->dev, "FIM: error: %lu usec%s\n",
178 error_avg, send_event ? " (!!!)" : "");
179
180 fim->counter = 0;
181 fim->sum = 0;
182 }
183
184 out_update_ts:
185 fim->last_ts = timestamp;
186 if (send_event)
187 send_fim_event(fim, error_avg);
188 }
189
190 #ifdef CONFIG_IMX_GPT_ICAP
191 /*
192 * Input Capture method of measuring frame intervals. Not subject
193 * to interrupt latency.
194 */
fim_input_capture_handler(int channel,void * dev_id,ktime_t timestamp)195 static void fim_input_capture_handler(int channel, void *dev_id,
196 ktime_t timestamp)
197 {
198 struct imx_media_fim *fim = dev_id;
199 unsigned long flags;
200
201 spin_lock_irqsave(&fim->lock, flags);
202
203 frame_interval_monitor(fim, timestamp);
204
205 if (!completion_done(&fim->icap_first_event))
206 complete(&fim->icap_first_event);
207
208 spin_unlock_irqrestore(&fim->lock, flags);
209 }
210
fim_request_input_capture(struct imx_media_fim * fim)211 static int fim_request_input_capture(struct imx_media_fim *fim)
212 {
213 init_completion(&fim->icap_first_event);
214
215 return mxc_request_input_capture(fim->icap_channel,
216 fim_input_capture_handler,
217 fim->icap_flags, fim);
218 }
219
fim_free_input_capture(struct imx_media_fim * fim)220 static void fim_free_input_capture(struct imx_media_fim *fim)
221 {
222 mxc_free_input_capture(fim->icap_channel, fim);
223 }
224
225 #else /* CONFIG_IMX_GPT_ICAP */
226
fim_request_input_capture(struct imx_media_fim * fim)227 static int fim_request_input_capture(struct imx_media_fim *fim)
228 {
229 return 0;
230 }
231
fim_free_input_capture(struct imx_media_fim * fim)232 static void fim_free_input_capture(struct imx_media_fim *fim)
233 {
234 }
235
236 #endif /* CONFIG_IMX_GPT_ICAP */
237
238 /*
239 * In case we are monitoring the first frame interval after streamon
240 * (when fim->num_skip = 0), we need a valid fim->last_ts before we
241 * can begin. This only applies to the input capture method. It is not
242 * possible to accurately measure the first FI after streamon using the
243 * EOF method, so fim->num_skip minimum is set to 1 in that case, so this
244 * function is a noop when the EOF method is used.
245 */
fim_acquire_first_ts(struct imx_media_fim * fim)246 static void fim_acquire_first_ts(struct imx_media_fim *fim)
247 {
248 unsigned long ret;
249
250 if (!fim->enabled || fim->num_skip > 0)
251 return;
252
253 ret = wait_for_completion_timeout(
254 &fim->icap_first_event,
255 msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
256 if (ret == 0)
257 v4l2_warn(fim->sd, "wait first icap event timeout\n");
258 }
259
260 /* FIM Controls */
fim_s_ctrl(struct v4l2_ctrl * ctrl)261 static int fim_s_ctrl(struct v4l2_ctrl *ctrl)
262 {
263 struct imx_media_fim *fim = container_of(ctrl->handler,
264 struct imx_media_fim,
265 ctrl_handler);
266 unsigned long flags;
267 int ret = 0;
268
269 spin_lock_irqsave(&fim->lock, flags);
270
271 switch (ctrl->id) {
272 case V4L2_CID_IMX_FIM_ENABLE:
273 break;
274 case V4L2_CID_IMX_FIM_ICAP_EDGE:
275 if (fim->stream_on)
276 ret = -EBUSY;
277 break;
278 default:
279 ret = -EINVAL;
280 }
281
282 if (!ret)
283 reset_fim(fim, false);
284
285 spin_unlock_irqrestore(&fim->lock, flags);
286 return ret;
287 }
288
289 static const struct v4l2_ctrl_ops fim_ctrl_ops = {
290 .s_ctrl = fim_s_ctrl,
291 };
292
293 static const struct v4l2_ctrl_config fim_ctrl[] = {
294 [FIM_CL_ENABLE] = {
295 .ops = &fim_ctrl_ops,
296 .id = V4L2_CID_IMX_FIM_ENABLE,
297 .name = "FIM Enable",
298 .type = V4L2_CTRL_TYPE_BOOLEAN,
299 .def = FIM_CL_ENABLE_DEF,
300 .min = 0,
301 .max = 1,
302 .step = 1,
303 },
304 [FIM_CL_NUM] = {
305 .ops = &fim_ctrl_ops,
306 .id = V4L2_CID_IMX_FIM_NUM,
307 .name = "FIM Num Average",
308 .type = V4L2_CTRL_TYPE_INTEGER,
309 .def = FIM_CL_NUM_DEF,
310 .min = 1, /* no averaging */
311 .max = 64, /* average 64 frames */
312 .step = 1,
313 },
314 [FIM_CL_TOLERANCE_MIN] = {
315 .ops = &fim_ctrl_ops,
316 .id = V4L2_CID_IMX_FIM_TOLERANCE_MIN,
317 .name = "FIM Tolerance Min",
318 .type = V4L2_CTRL_TYPE_INTEGER,
319 .def = FIM_CL_TOLERANCE_MIN_DEF,
320 .min = 2,
321 .max = 200,
322 .step = 1,
323 },
324 [FIM_CL_TOLERANCE_MAX] = {
325 .ops = &fim_ctrl_ops,
326 .id = V4L2_CID_IMX_FIM_TOLERANCE_MAX,
327 .name = "FIM Tolerance Max",
328 .type = V4L2_CTRL_TYPE_INTEGER,
329 .def = FIM_CL_TOLERANCE_MAX_DEF,
330 .min = 0,
331 .max = 500,
332 .step = 1,
333 },
334 [FIM_CL_NUM_SKIP] = {
335 .ops = &fim_ctrl_ops,
336 .id = V4L2_CID_IMX_FIM_NUM_SKIP,
337 .name = "FIM Num Skip",
338 .type = V4L2_CTRL_TYPE_INTEGER,
339 .def = FIM_CL_NUM_SKIP_DEF,
340 .min = 0, /* skip no frames */
341 .max = 256, /* skip 256 frames */
342 .step = 1,
343 },
344 };
345
346 static const struct v4l2_ctrl_config fim_icap_ctrl[] = {
347 [FIM_CL_ICAP_EDGE] = {
348 .ops = &fim_ctrl_ops,
349 .id = V4L2_CID_IMX_FIM_ICAP_EDGE,
350 .name = "FIM Input Capture Edge",
351 .type = V4L2_CTRL_TYPE_INTEGER,
352 .def = IRQ_TYPE_NONE, /* input capture disabled by default */
353 .min = IRQ_TYPE_NONE,
354 .max = IRQ_TYPE_EDGE_BOTH,
355 .step = 1,
356 },
357 [FIM_CL_ICAP_CHANNEL] = {
358 .ops = &fim_ctrl_ops,
359 .id = V4L2_CID_IMX_FIM_ICAP_CHANNEL,
360 .name = "FIM Input Capture Channel",
361 .type = V4L2_CTRL_TYPE_INTEGER,
362 .def = 0,
363 .min = 0,
364 .max = 1,
365 .step = 1,
366 },
367 };
368
init_fim_controls(struct imx_media_fim * fim)369 static int init_fim_controls(struct imx_media_fim *fim)
370 {
371 struct v4l2_ctrl_handler *hdlr = &fim->ctrl_handler;
372 int i, ret;
373
374 v4l2_ctrl_handler_init(hdlr, FIM_NUM_CONTROLS + FIM_NUM_ICAP_CONTROLS);
375
376 for (i = 0; i < FIM_NUM_CONTROLS; i++)
377 fim->ctrl[i] = v4l2_ctrl_new_custom(hdlr,
378 &fim_ctrl[i],
379 NULL);
380 for (i = 0; i < FIM_NUM_ICAP_CONTROLS; i++)
381 fim->icap_ctrl[i] = v4l2_ctrl_new_custom(hdlr,
382 &fim_icap_ctrl[i],
383 NULL);
384 if (hdlr->error) {
385 ret = hdlr->error;
386 goto err_free;
387 }
388
389 v4l2_ctrl_cluster(FIM_NUM_CONTROLS, fim->ctrl);
390 v4l2_ctrl_cluster(FIM_NUM_ICAP_CONTROLS, fim->icap_ctrl);
391
392 return 0;
393 err_free:
394 v4l2_ctrl_handler_free(hdlr);
395 return ret;
396 }
397
398 /*
399 * Monitor frame intervals via EOF interrupt. This method is
400 * subject to uncertainty errors introduced by interrupt latency.
401 *
402 * This is a noop if the Input Capture method is being used, since
403 * the frame_interval_monitor() is called by the input capture event
404 * callback handler in that case.
405 */
imx_media_fim_eof_monitor(struct imx_media_fim * fim,ktime_t timestamp)406 void imx_media_fim_eof_monitor(struct imx_media_fim *fim, ktime_t timestamp)
407 {
408 unsigned long flags;
409
410 spin_lock_irqsave(&fim->lock, flags);
411
412 if (!icap_enabled(fim))
413 frame_interval_monitor(fim, timestamp);
414
415 spin_unlock_irqrestore(&fim->lock, flags);
416 }
417
418 /* Called by the subdev in its s_stream callback */
imx_media_fim_set_stream(struct imx_media_fim * fim,const struct v4l2_fract * fi,bool on)419 int imx_media_fim_set_stream(struct imx_media_fim *fim,
420 const struct v4l2_fract *fi,
421 bool on)
422 {
423 unsigned long flags;
424 int ret = 0;
425
426 v4l2_ctrl_lock(fim->ctrl[FIM_CL_ENABLE]);
427
428 if (fim->stream_on == on)
429 goto out;
430
431 if (on) {
432 spin_lock_irqsave(&fim->lock, flags);
433 reset_fim(fim, true);
434 update_fim_nominal(fim, fi);
435 spin_unlock_irqrestore(&fim->lock, flags);
436
437 if (icap_enabled(fim)) {
438 ret = fim_request_input_capture(fim);
439 if (ret)
440 goto out;
441 fim_acquire_first_ts(fim);
442 }
443 } else {
444 if (icap_enabled(fim))
445 fim_free_input_capture(fim);
446 }
447
448 fim->stream_on = on;
449 out:
450 v4l2_ctrl_unlock(fim->ctrl[FIM_CL_ENABLE]);
451 return ret;
452 }
453
imx_media_fim_add_controls(struct imx_media_fim * fim)454 int imx_media_fim_add_controls(struct imx_media_fim *fim)
455 {
456 /* add the FIM controls to the calling subdev ctrl handler */
457 return v4l2_ctrl_add_handler(fim->sd->ctrl_handler,
458 &fim->ctrl_handler, NULL, false);
459 }
460
461 /* Called by the subdev in its subdev registered callback */
imx_media_fim_init(struct v4l2_subdev * sd)462 struct imx_media_fim *imx_media_fim_init(struct v4l2_subdev *sd)
463 {
464 struct imx_media_fim *fim;
465 int ret;
466
467 fim = devm_kzalloc(sd->dev, sizeof(*fim), GFP_KERNEL);
468 if (!fim)
469 return ERR_PTR(-ENOMEM);
470
471 fim->sd = sd;
472
473 spin_lock_init(&fim->lock);
474
475 ret = init_fim_controls(fim);
476 if (ret)
477 return ERR_PTR(ret);
478
479 return fim;
480 }
481
imx_media_fim_free(struct imx_media_fim * fim)482 void imx_media_fim_free(struct imx_media_fim *fim)
483 {
484 v4l2_ctrl_handler_free(&fim->ctrl_handler);
485 }
486