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