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