drivers/staging/media/imx/imx-media-fim.c

   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 }