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

   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);
 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);