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