imx-media-fim.c

  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
 18enum {
 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
 27enum {
 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
 39struct 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
 71static bool icap_enabled(struct imx_media_fim *fim)
 72{
 73	return fim->icap_flags != IRQ_TYPE_NONE;
 74}
 75
 76static 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
 91static 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
131static 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 */
146static 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
187out_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 */
201static 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 */
216static 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
244static const struct v4l2_ctrl_ops fim_ctrl_ops = {
245	.s_ctrl = fim_s_ctrl,
246};
247
248static 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
301static 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
324static 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;
348err_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 */
361void 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 */
374void 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;
396out:
397	v4l2_ctrl_unlock(fim->ctrl[FIM_CL_ENABLE]);
398}
399
400int 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 */
408struct 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
428void imx_media_fim_free(struct imx_media_fim *fim)
429{
430	v4l2_ctrl_handler_free(&fim->ctrl_handler);
431}