1
2
3
4
5
6
7
8
9
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
22enum {
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
31enum {
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
38#define FIM_CL_NUM_DEF 8
39#define FIM_CL_NUM_SKIP_DEF 2
40#define FIM_CL_TOLERANCE_MIN_DEF 50
41#define FIM_CL_TOLERANCE_MAX_DEF 0
42
43struct imx_media_fim {
44 struct imx_media_dev *md;
45
46
47 struct v4l2_subdev *sd;
48
49
50 struct v4l2_ctrl_handler ctrl_handler;
51
52
53 struct v4l2_ctrl *ctrl[FIM_NUM_CONTROLS];
54 struct v4l2_ctrl *icap_ctrl[FIM_NUM_ICAP_CONTROLS];
55
56 spinlock_t lock;
57
58
59 bool enabled;
60 int num_avg;
61 int num_skip;
62 unsigned long tolerance_min;
63 unsigned long tolerance_max;
64
65 int icap_channel;
66 int icap_flags;
67
68 int counter;
69 ktime_t last_ts;
70 unsigned long sum;
71 unsigned long nominal;
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
79static 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
94static 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
123 if (fim->tolerance_max <= fim->tolerance_min)
124 fim->tolerance_max = 0;
125
126
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
134static 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
145
146
147
148
149static 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
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
190out_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
199
200
201static 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
217static 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
226static void fim_free_input_capture(struct imx_media_fim *fim)
227{
228 mxc_free_input_capture(fim->icap_channel, fim);
229}
230
231#else
232
233static int fim_request_input_capture(struct imx_media_fim *fim)
234{
235 return 0;
236}
237
238static void fim_free_input_capture(struct imx_media_fim *fim)
239{
240}
241
242#endif
243
244
245
246
247
248
249
250
251
252static 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
267static 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
295static const struct v4l2_ctrl_ops fim_ctrl_ops = {
296 .s_ctrl = fim_s_ctrl,
297};
298
299static 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,
317 .max = 64,
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,
347 .max = 256,
348 .step = 1,
349 },
350};
351
352static 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,
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
375static 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;
399err_free:
400 v4l2_ctrl_handler_free(hdlr);
401 return ret;
402}
403
404
405
406
407
408
409
410
411
412void 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}
423EXPORT_SYMBOL_GPL(imx_media_fim_eof_monitor);
424
425
426int 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;
456out:
457 v4l2_ctrl_unlock(fim->ctrl[FIM_CL_ENABLE]);
458 return ret;
459}
460EXPORT_SYMBOL_GPL(imx_media_fim_set_stream);
461
462int imx_media_fim_add_controls(struct imx_media_fim *fim)
463{
464
465 return v4l2_ctrl_add_handler(fim->sd->ctrl_handler,
466 &fim->ctrl_handler, NULL);
467}
468EXPORT_SYMBOL_GPL(imx_media_fim_add_controls);
469
470
471struct 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
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}
492EXPORT_SYMBOL_GPL(imx_media_fim_init);
493
494void imx_media_fim_free(struct imx_media_fim *fim)
495{
496 v4l2_ctrl_handler_free(&fim->ctrl_handler);
497}
498EXPORT_SYMBOL_GPL(imx_media_fim_free);
499