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