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 /* 191 * In case we are monitoring the first frame interval after streamon 192 * (when fim->num_skip = 0), we need a valid fim->last_ts before we 193 * can begin. This only applies to the input capture method. It is not 194 * possible to accurately measure the first FI after streamon using the 195 * EOF method, so fim->num_skip minimum is set to 1 in that case, so this 196 * function is a noop when the EOF method is used. 197 */ 198 static void fim_acquire_first_ts(struct imx_media_fim *fim) 199 { 200 unsigned long ret; 201 202 if (!fim->enabled || fim->num_skip > 0) 203 return; 204 205 ret = wait_for_completion_timeout( 206 &fim->icap_first_event, 207 msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT)); 208 if (ret == 0) 209 v4l2_warn(fim->sd, "wait first icap event timeout\n"); 210 } 211 212 /* FIM Controls */ 213 static int fim_s_ctrl(struct v4l2_ctrl *ctrl) 214 { 215 struct imx_media_fim *fim = container_of(ctrl->handler, 216 struct imx_media_fim, 217 ctrl_handler); 218 unsigned long flags; 219 int ret = 0; 220 221 spin_lock_irqsave(&fim->lock, flags); 222 223 switch (ctrl->id) { 224 case V4L2_CID_IMX_FIM_ENABLE: 225 break; 226 case V4L2_CID_IMX_FIM_ICAP_EDGE: 227 if (fim->stream_on) 228 ret = -EBUSY; 229 break; 230 default: 231 ret = -EINVAL; 232 } 233 234 if (!ret) 235 reset_fim(fim, false); 236 237 spin_unlock_irqrestore(&fim->lock, flags); 238 return ret; 239 } 240 241 static const struct v4l2_ctrl_ops fim_ctrl_ops = { 242 .s_ctrl = fim_s_ctrl, 243 }; 244 245 static const struct v4l2_ctrl_config fim_ctrl[] = { 246 [FIM_CL_ENABLE] = { 247 .ops = &fim_ctrl_ops, 248 .id = V4L2_CID_IMX_FIM_ENABLE, 249 .name = "FIM Enable", 250 .type = V4L2_CTRL_TYPE_BOOLEAN, 251 .def = FIM_CL_ENABLE_DEF, 252 .min = 0, 253 .max = 1, 254 .step = 1, 255 }, 256 [FIM_CL_NUM] = { 257 .ops = &fim_ctrl_ops, 258 .id = V4L2_CID_IMX_FIM_NUM, 259 .name = "FIM Num Average", 260 .type = V4L2_CTRL_TYPE_INTEGER, 261 .def = FIM_CL_NUM_DEF, 262 .min = 1, /* no averaging */ 263 .max = 64, /* average 64 frames */ 264 .step = 1, 265 }, 266 [FIM_CL_TOLERANCE_MIN] = { 267 .ops = &fim_ctrl_ops, 268 .id = V4L2_CID_IMX_FIM_TOLERANCE_MIN, 269 .name = "FIM Tolerance Min", 270 .type = V4L2_CTRL_TYPE_INTEGER, 271 .def = FIM_CL_TOLERANCE_MIN_DEF, 272 .min = 2, 273 .max = 200, 274 .step = 1, 275 }, 276 [FIM_CL_TOLERANCE_MAX] = { 277 .ops = &fim_ctrl_ops, 278 .id = V4L2_CID_IMX_FIM_TOLERANCE_MAX, 279 .name = "FIM Tolerance Max", 280 .type = V4L2_CTRL_TYPE_INTEGER, 281 .def = FIM_CL_TOLERANCE_MAX_DEF, 282 .min = 0, 283 .max = 500, 284 .step = 1, 285 }, 286 [FIM_CL_NUM_SKIP] = { 287 .ops = &fim_ctrl_ops, 288 .id = V4L2_CID_IMX_FIM_NUM_SKIP, 289 .name = "FIM Num Skip", 290 .type = V4L2_CTRL_TYPE_INTEGER, 291 .def = FIM_CL_NUM_SKIP_DEF, 292 .min = 0, /* skip no frames */ 293 .max = 256, /* skip 256 frames */ 294 .step = 1, 295 }, 296 }; 297 298 static const struct v4l2_ctrl_config fim_icap_ctrl[] = { 299 [FIM_CL_ICAP_EDGE] = { 300 .ops = &fim_ctrl_ops, 301 .id = V4L2_CID_IMX_FIM_ICAP_EDGE, 302 .name = "FIM Input Capture Edge", 303 .type = V4L2_CTRL_TYPE_INTEGER, 304 .def = IRQ_TYPE_NONE, /* input capture disabled by default */ 305 .min = IRQ_TYPE_NONE, 306 .max = IRQ_TYPE_EDGE_BOTH, 307 .step = 1, 308 }, 309 [FIM_CL_ICAP_CHANNEL] = { 310 .ops = &fim_ctrl_ops, 311 .id = V4L2_CID_IMX_FIM_ICAP_CHANNEL, 312 .name = "FIM Input Capture Channel", 313 .type = V4L2_CTRL_TYPE_INTEGER, 314 .def = 0, 315 .min = 0, 316 .max = 1, 317 .step = 1, 318 }, 319 }; 320 321 static int init_fim_controls(struct imx_media_fim *fim) 322 { 323 struct v4l2_ctrl_handler *hdlr = &fim->ctrl_handler; 324 int i, ret; 325 326 v4l2_ctrl_handler_init(hdlr, FIM_NUM_CONTROLS + FIM_NUM_ICAP_CONTROLS); 327 328 for (i = 0; i < FIM_NUM_CONTROLS; i++) 329 fim->ctrl[i] = v4l2_ctrl_new_custom(hdlr, 330 &fim_ctrl[i], 331 NULL); 332 for (i = 0; i < FIM_NUM_ICAP_CONTROLS; i++) 333 fim->icap_ctrl[i] = v4l2_ctrl_new_custom(hdlr, 334 &fim_icap_ctrl[i], 335 NULL); 336 if (hdlr->error) { 337 ret = hdlr->error; 338 goto err_free; 339 } 340 341 v4l2_ctrl_cluster(FIM_NUM_CONTROLS, fim->ctrl); 342 v4l2_ctrl_cluster(FIM_NUM_ICAP_CONTROLS, fim->icap_ctrl); 343 344 return 0; 345 err_free: 346 v4l2_ctrl_handler_free(hdlr); 347 return ret; 348 } 349 350 /* 351 * Monitor frame intervals via EOF interrupt. This method is 352 * subject to uncertainty errors introduced by interrupt latency. 353 * 354 * This is a noop if the Input Capture method is being used, since 355 * the frame_interval_monitor() is called by the input capture event 356 * callback handler in that case. 357 */ 358 void imx_media_fim_eof_monitor(struct imx_media_fim *fim, ktime_t timestamp) 359 { 360 unsigned long flags; 361 362 spin_lock_irqsave(&fim->lock, flags); 363 364 if (!icap_enabled(fim)) 365 frame_interval_monitor(fim, timestamp); 366 367 spin_unlock_irqrestore(&fim->lock, flags); 368 } 369 370 /* Called by the subdev in its s_stream callback */ 371 int imx_media_fim_set_stream(struct imx_media_fim *fim, 372 const struct v4l2_fract *fi, 373 bool on) 374 { 375 unsigned long flags; 376 int ret = 0; 377 378 v4l2_ctrl_lock(fim->ctrl[FIM_CL_ENABLE]); 379 380 if (fim->stream_on == on) 381 goto out; 382 383 if (on) { 384 spin_lock_irqsave(&fim->lock, flags); 385 reset_fim(fim, true); 386 update_fim_nominal(fim, fi); 387 spin_unlock_irqrestore(&fim->lock, flags); 388 389 if (icap_enabled(fim)) 390 fim_acquire_first_ts(fim); 391 } 392 393 fim->stream_on = on; 394 out: 395 v4l2_ctrl_unlock(fim->ctrl[FIM_CL_ENABLE]); 396 return ret; 397 } 398 399 int imx_media_fim_add_controls(struct imx_media_fim *fim) 400 { 401 /* add the FIM controls to the calling subdev ctrl handler */ 402 return v4l2_ctrl_add_handler(fim->sd->ctrl_handler, 403 &fim->ctrl_handler, NULL, false); 404 } 405 406 /* Called by the subdev in its subdev registered callback */ 407 struct imx_media_fim *imx_media_fim_init(struct v4l2_subdev *sd) 408 { 409 struct imx_media_fim *fim; 410 int ret; 411 412 fim = devm_kzalloc(sd->dev, sizeof(*fim), GFP_KERNEL); 413 if (!fim) 414 return ERR_PTR(-ENOMEM); 415 416 fim->sd = sd; 417 418 spin_lock_init(&fim->lock); 419 420 ret = init_fim_controls(fim); 421 if (ret) 422 return ERR_PTR(ret); 423 424 return fim; 425 } 426 427 void imx_media_fim_free(struct imx_media_fim *fim) 428 { 429 v4l2_ctrl_handler_free(&fim->ctrl_handler); 430 } 431