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
icap_enabled(struct imx_media_fim * fim)71 static bool icap_enabled(struct imx_media_fim *fim)
72 {
73 return fim->icap_flags != IRQ_TYPE_NONE;
74 }
75
update_fim_nominal(struct imx_media_fim * fim,const struct v4l2_fract * fi)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
reset_fim(struct imx_media_fim * fim,bool curval)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
send_fim_event(struct imx_media_fim * fim,unsigned long error)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(fim->sd, &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 */
frame_interval_monitor(struct imx_media_fim * fim,ktime_t timestamp)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_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 */
fim_acquire_first_ts(struct imx_media_fim * fim)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 &fim->icap_first_event,
210 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 */
fim_s_ctrl(struct v4l2_ctrl * ctrl)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, false);
239
240 spin_unlock_irqrestore(&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
init_fim_controls(struct imx_media_fim * fim)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(hdlr,
333 &fim_ctrl[i],
334 NULL);
335 for (i = 0; i < FIM_NUM_ICAP_CONTROLS; i++)
336 fim->icap_ctrl[i] = v4l2_ctrl_new_custom(hdlr,
337 &fim_icap_ctrl[i],
338 NULL);
339 if (hdlr->error) {
340 ret = hdlr->error;
341 goto err_free;
342 }
343
344 v4l2_ctrl_cluster(FIM_NUM_CONTROLS, fim->ctrl);
345 v4l2_ctrl_cluster(FIM_NUM_ICAP_CONTROLS, fim->icap_ctrl);
346
347 return 0;
348 err_free:
349 v4l2_ctrl_handler_free(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 */
imx_media_fim_eof_monitor(struct imx_media_fim * fim,ktime_t timestamp)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(&fim->lock, flags);
371 }
372
373 /* 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)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(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, true);
388 update_fim_nominal(fim, fi);
389 spin_unlock_irqrestore(&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(fim->ctrl[FIM_CL_ENABLE]);
398 }
399
imx_media_fim_add_controls(struct imx_media_fim * fim)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(fim->sd->ctrl_handler,
404 &fim->ctrl_handler, NULL, true);
405 }
406
407 /* Called by the subdev in its subdev registered callback */
imx_media_fim_init(struct v4l2_subdev * sd)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(sd->dev, sizeof(*fim), GFP_KERNEL);
414 if (!fim)
415 return ERR_PTR(-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(ret);
424
425 return fim;
426 }
427
imx_media_fim_free(struct imx_media_fim * fim)428 void imx_media_fim_free(struct imx_media_fim *fim)
429 {
430 v4l2_ctrl_handler_free(&fim->ctrl_handler);
431 }
432