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