media/imx/imx-media-fim.c

0001 // SPDX-License-Identifier: GPL-2.0+
0002 /*
0003  * Frame Interval Monitor.
0004  *
0005  * Copyright (c) 2016 Mentor Graphics Inc.
0006  */
0007 #include <linux/delay.h>
0008 #include <linux/irq.h>
0009 #include <linux/module.h>
0010 #include <linux/platform_device.h>
0011 #include <linux/slab.h>
0012 #include <linux/spinlock.h>
0013 #include <media/v4l2-ctrls.h>
0014 #include <media/v4l2-subdev.h>
0015 #include <media/imx.h>
0016 #include "imx-media.h"
0017
0018 enum {
0019     FIM_CL_ENABLE = 0,
0020     FIM_CL_NUM,
0021     FIM_CL_TOLERANCE_MIN,
0022     FIM_CL_TOLERANCE_MAX,
0023     FIM_CL_NUM_SKIP,
0024     FIM_NUM_CONTROLS,
0025 };
0026
0027 enum {
0028     FIM_CL_ICAP_EDGE = 0,
0029     FIM_CL_ICAP_CHANNEL,
0030     FIM_NUM_ICAP_CONTROLS,
0031 };
0032
0033 #define FIM_CL_ENABLE_DEF          0 /* FIM disabled by default */
0034 #define FIM_CL_NUM_DEF             8 /* average 8 frames */
0035 #define FIM_CL_NUM_SKIP_DEF        2 /* skip 2 frames after restart */
0036 #define FIM_CL_TOLERANCE_MIN_DEF  50 /* usec */
0037 #define FIM_CL_TOLERANCE_MAX_DEF   0 /* no max tolerance (unbounded) */
0038
0039 struct imx_media_fim {
0040     /* the owning subdev of this fim instance */
0041     struct v4l2_subdev *sd;
0042
0043     /* FIM's control handler */
0044     struct v4l2_ctrl_handler ctrl_handler;
0045
0046     /* control clusters */
0047     struct v4l2_ctrl  *ctrl[FIM_NUM_CONTROLS];
0048     struct v4l2_ctrl  *icap_ctrl[FIM_NUM_ICAP_CONTROLS];
0049
0050     spinlock_t        lock; /* protect control values */
0051
0052     /* current control values */
0053     bool              enabled;
0054     int               num_avg;
0055     int               num_skip;
0056     unsigned long     tolerance_min; /* usec */
0057     unsigned long     tolerance_max; /* usec */
0058     /* input capture method of measuring FI */
0059     int               icap_channel;
0060     int               icap_flags;
0061
0062     int               counter;
0063     ktime_t       last_ts;
0064     unsigned long     sum;       /* usec */
0065     unsigned long     nominal;   /* usec */
0066
0067     struct completion icap_first_event;
0068     bool              stream_on;
0069 };
0070
0071 #define icap_enabled(fim) ((fim)->icap_flags != IRQ_TYPE_NONE)
0072
0073 static void update_fim_nominal(struct imx_media_fim *fim,
0074                    const struct v4l2_fract *fi)
0075 {
0076     if (fi->denominator == 0) {
0077         dev_dbg(fim->sd->dev, "no frame interval, FIM disabled\n");
0078         fim->enabled = false;
0079         return;
0080     }
0081
0082     fim->nominal = DIV_ROUND_CLOSEST_ULL(1000000ULL * (u64)fi->numerator,
0083                          fi->denominator);
0084
0085     dev_dbg(fim->sd->dev, "FI=%lu usec\n", fim->nominal);
0086 }
0087
0088 static void reset_fim(struct imx_media_fim *fim, bool curval)
0089 {
0090     struct v4l2_ctrl *icap_chan = fim->icap_ctrl[FIM_CL_ICAP_CHANNEL];
0091     struct v4l2_ctrl *icap_edge = fim->icap_ctrl[FIM_CL_ICAP_EDGE];
0092     struct v4l2_ctrl *en = fim->ctrl[FIM_CL_ENABLE];
0093     struct v4l2_ctrl *num = fim->ctrl[FIM_CL_NUM];
0094     struct v4l2_ctrl *skip = fim->ctrl[FIM_CL_NUM_SKIP];
0095     struct v4l2_ctrl *tol_min = fim->ctrl[FIM_CL_TOLERANCE_MIN];
0096     struct v4l2_ctrl *tol_max = fim->ctrl[FIM_CL_TOLERANCE_MAX];
0097
0098     if (curval) {
0099         fim->enabled = en->cur.val;
0100         fim->icap_flags = icap_edge->cur.val;
0101         fim->icap_channel = icap_chan->cur.val;
0102         fim->num_avg = num->cur.val;
0103         fim->num_skip = skip->cur.val;
0104         fim->tolerance_min = tol_min->cur.val;
0105         fim->tolerance_max = tol_max->cur.val;
0106     } else {
0107         fim->enabled = en->val;
0108         fim->icap_flags = icap_edge->val;
0109         fim->icap_channel = icap_chan->val;
0110         fim->num_avg = num->val;
0111         fim->num_skip = skip->val;
0112         fim->tolerance_min = tol_min->val;
0113         fim->tolerance_max = tol_max->val;
0114     }
0115
0116     /* disable tolerance range if max <= min */
0117     if (fim->tolerance_max <= fim->tolerance_min)
0118         fim->tolerance_max = 0;
0119
0120     /* num_skip must be >= 1 if input capture not used */
0121     if (!icap_enabled(fim))
0122         fim->num_skip = max_t(int, fim->num_skip, 1);
0123
0124     fim->counter = -fim->num_skip;
0125     fim->sum = 0;
0126 }
0127
0128 static void send_fim_event(struct imx_media_fim *fim, unsigned long error)
0129 {
0130     static const struct v4l2_event ev = {
0131         .type = V4L2_EVENT_IMX_FRAME_INTERVAL_ERROR,
0132     };
0133
0134     v4l2_subdev_notify_event(fim->sd, &ev);
0135 }
0136
0137 /*
0138  * Monitor an averaged frame interval. If the average deviates too much
0139  * from the nominal frame rate, send the frame interval error event. The
0140  * frame intervals are averaged in order to quiet noise from
0141  * (presumably random) interrupt latency.
0142  */
0143 static void frame_interval_monitor(struct imx_media_fim *fim,
0144                    ktime_t timestamp)
0145 {
0146     long long interval, error;
0147     unsigned long error_avg;
0148     bool send_event = false;
0149
0150     if (!fim->enabled || ++fim->counter <= 0)
0151         goto out_update_ts;
0152
0153     /* max error is less than l00µs, so use 32-bit division or fail */
0154     interval = ktime_to_ns(ktime_sub(timestamp, fim->last_ts));
0155     error = abs(interval - NSEC_PER_USEC * (u64)fim->nominal);
0156     if (error > U32_MAX)
0157         error = U32_MAX;
0158     else
0159         error = abs((u32)error / NSEC_PER_USEC);
0160
0161     if (fim->tolerance_max && error >= fim->tolerance_max) {
0162         dev_dbg(fim->sd->dev,
0163             "FIM: %llu ignored, out of tolerance bounds\n",
0164             error);
0165         fim->counter--;
0166         goto out_update_ts;
0167     }
0168
0169     fim->sum += error;
0170
0171     if (fim->counter == fim->num_avg) {
0172         error_avg = DIV_ROUND_CLOSEST(fim->sum, fim->num_avg);
0173
0174         if (error_avg > fim->tolerance_min)
0175             send_event = true;
0176
0177         dev_dbg(fim->sd->dev, "FIM: error: %lu usec%s\n",
0178             error_avg, send_event ? " (!!!)" : "");
0179
0180         fim->counter = 0;
0181         fim->sum = 0;
0182     }
0183
0184 out_update_ts:
0185     fim->last_ts = timestamp;
0186     if (send_event)
0187         send_fim_event(fim, error_avg);
0188 }
0189
0190 #ifdef CONFIG_IMX_GPT_ICAP
0191 /*
0192  * Input Capture method of measuring frame intervals. Not subject
0193  * to interrupt latency.
0194  */
0195 static void fim_input_capture_handler(int channel, void *dev_id,
0196                       ktime_t timestamp)
0197 {
0198     struct imx_media_fim *fim = dev_id;
0199     unsigned long flags;
0200
0201     spin_lock_irqsave(&fim->lock, flags);
0202
0203     frame_interval_monitor(fim, timestamp);
0204
0205     if (!completion_done(&fim->icap_first_event))
0206         complete(&fim->icap_first_event);
0207
0208     spin_unlock_irqrestore(&fim->lock, flags);
0209 }
0210
0211 static int fim_request_input_capture(struct imx_media_fim *fim)
0212 {
0213     init_completion(&fim->icap_first_event);
0214
0215     return mxc_request_input_capture(fim->icap_channel,
0216                      fim_input_capture_handler,
0217                      fim->icap_flags, fim);
0218 }
0219
0220 static void fim_free_input_capture(struct imx_media_fim *fim)
0221 {
0222     mxc_free_input_capture(fim->icap_channel, fim);
0223 }
0224
0225 #else /* CONFIG_IMX_GPT_ICAP */
0226
0227 static int fim_request_input_capture(struct imx_media_fim *fim)
0228 {
0229     return 0;
0230 }
0231
0232 static void fim_free_input_capture(struct imx_media_fim *fim)
0233 {
0234 }
0235
0236 #endif /* CONFIG_IMX_GPT_ICAP */
0237
0238 /*
0239  * In case we are monitoring the first frame interval after streamon
0240  * (when fim->num_skip = 0), we need a valid fim->last_ts before we
0241  * can begin. This only applies to the input capture method. It is not
0242  * possible to accurately measure the first FI after streamon using the
0243  * EOF method, so fim->num_skip minimum is set to 1 in that case, so this
0244  * function is a noop when the EOF method is used.
0245  */
0246 static void fim_acquire_first_ts(struct imx_media_fim *fim)
0247 {
0248     unsigned long ret;
0249
0250     if (!fim->enabled || fim->num_skip > 0)
0251         return;
0252
0253     ret = wait_for_completion_timeout(
0254         &fim->icap_first_event,
0255         msecs_to_jiffies(IMX_MEDIA_EOF_TIMEOUT));
0256     if (ret == 0)
0257         v4l2_warn(fim->sd, "wait first icap event timeout\n");
0258 }
0259
0260 /* FIM Controls */
0261 static int fim_s_ctrl(struct v4l2_ctrl *ctrl)
0262 {
0263     struct imx_media_fim *fim = container_of(ctrl->handler,
0264                          struct imx_media_fim,
0265                          ctrl_handler);
0266     unsigned long flags;
0267     int ret = 0;
0268
0269     spin_lock_irqsave(&fim->lock, flags);
0270
0271     switch (ctrl->id) {
0272     case V4L2_CID_IMX_FIM_ENABLE:
0273         break;
0274     case V4L2_CID_IMX_FIM_ICAP_EDGE:
0275         if (fim->stream_on)
0276             ret = -EBUSY;
0277         break;
0278     default:
0279         ret = -EINVAL;
0280     }
0281
0282     if (!ret)
0283         reset_fim(fim, false);
0284
0285     spin_unlock_irqrestore(&fim->lock, flags);
0286     return ret;
0287 }
0288
0289 static const struct v4l2_ctrl_ops fim_ctrl_ops = {
0290     .s_ctrl = fim_s_ctrl,
0291 };
0292
0293 static const struct v4l2_ctrl_config fim_ctrl[] = {
0294     [FIM_CL_ENABLE] = {
0295         .ops = &fim_ctrl_ops,
0296         .id = V4L2_CID_IMX_FIM_ENABLE,
0297         .name = "FIM Enable",
0298         .type = V4L2_CTRL_TYPE_BOOLEAN,
0299         .def = FIM_CL_ENABLE_DEF,
0300         .min = 0,
0301         .max = 1,
0302         .step = 1,
0303     },
0304     [FIM_CL_NUM] = {
0305         .ops = &fim_ctrl_ops,
0306         .id = V4L2_CID_IMX_FIM_NUM,
0307         .name = "FIM Num Average",
0308         .type = V4L2_CTRL_TYPE_INTEGER,
0309         .def = FIM_CL_NUM_DEF,
0310         .min =  1, /* no averaging */
0311         .max = 64, /* average 64 frames */
0312         .step = 1,
0313     },
0314     [FIM_CL_TOLERANCE_MIN] = {
0315         .ops = &fim_ctrl_ops,
0316         .id = V4L2_CID_IMX_FIM_TOLERANCE_MIN,
0317         .name = "FIM Tolerance Min",
0318         .type = V4L2_CTRL_TYPE_INTEGER,
0319         .def = FIM_CL_TOLERANCE_MIN_DEF,
0320         .min =    2,
0321         .max =  200,
0322         .step =   1,
0323     },
0324     [FIM_CL_TOLERANCE_MAX] = {
0325         .ops = &fim_ctrl_ops,
0326         .id = V4L2_CID_IMX_FIM_TOLERANCE_MAX,
0327         .name = "FIM Tolerance Max",
0328         .type = V4L2_CTRL_TYPE_INTEGER,
0329         .def = FIM_CL_TOLERANCE_MAX_DEF,
0330         .min =    0,
0331         .max =  500,
0332         .step =   1,
0333     },
0334     [FIM_CL_NUM_SKIP] = {
0335         .ops = &fim_ctrl_ops,
0336         .id = V4L2_CID_IMX_FIM_NUM_SKIP,
0337         .name = "FIM Num Skip",
0338         .type = V4L2_CTRL_TYPE_INTEGER,
0339         .def = FIM_CL_NUM_SKIP_DEF,
0340         .min =   0, /* skip no frames */
0341         .max = 256, /* skip 256 frames */
0342         .step =  1,
0343     },
0344 };
0345
0346 static const struct v4l2_ctrl_config fim_icap_ctrl[] = {
0347     [FIM_CL_ICAP_EDGE] = {
0348         .ops = &fim_ctrl_ops,
0349         .id = V4L2_CID_IMX_FIM_ICAP_EDGE,
0350         .name = "FIM Input Capture Edge",
0351         .type = V4L2_CTRL_TYPE_INTEGER,
0352         .def =  IRQ_TYPE_NONE, /* input capture disabled by default */
0353         .min =  IRQ_TYPE_NONE,
0354         .max =  IRQ_TYPE_EDGE_BOTH,
0355         .step = 1,
0356     },
0357     [FIM_CL_ICAP_CHANNEL] = {
0358         .ops = &fim_ctrl_ops,
0359         .id = V4L2_CID_IMX_FIM_ICAP_CHANNEL,
0360         .name = "FIM Input Capture Channel",
0361         .type = V4L2_CTRL_TYPE_INTEGER,
0362         .def =  0,
0363         .min =  0,
0364         .max =  1,
0365         .step = 1,
0366     },
0367 };
0368
0369 static int init_fim_controls(struct imx_media_fim *fim)
0370 {
0371     struct v4l2_ctrl_handler *hdlr = &fim->ctrl_handler;
0372     int i, ret;
0373
0374     v4l2_ctrl_handler_init(hdlr, FIM_NUM_CONTROLS + FIM_NUM_ICAP_CONTROLS);
0375
0376     for (i = 0; i < FIM_NUM_CONTROLS; i++)
0377         fim->ctrl[i] = v4l2_ctrl_new_custom(hdlr,
0378                             &fim_ctrl[i],
0379                             NULL);
0380     for (i = 0; i < FIM_NUM_ICAP_CONTROLS; i++)
0381         fim->icap_ctrl[i] = v4l2_ctrl_new_custom(hdlr,
0382                              &fim_icap_ctrl[i],
0383                              NULL);
0384     if (hdlr->error) {
0385         ret = hdlr->error;
0386         goto err_free;
0387     }
0388
0389     v4l2_ctrl_cluster(FIM_NUM_CONTROLS, fim->ctrl);
0390     v4l2_ctrl_cluster(FIM_NUM_ICAP_CONTROLS, fim->icap_ctrl);
0391
0392     return 0;
0393 err_free:
0394     v4l2_ctrl_handler_free(hdlr);
0395     return ret;
0396 }
0397
0398 /*
0399  * Monitor frame intervals via EOF interrupt. This method is
0400  * subject to uncertainty errors introduced by interrupt latency.
0401  *
0402  * This is a noop if the Input Capture method is being used, since
0403  * the frame_interval_monitor() is called by the input capture event
0404  * callback handler in that case.
0405  */
0406 void imx_media_fim_eof_monitor(struct imx_media_fim *fim, ktime_t timestamp)
0407 {
0408     unsigned long flags;
0409
0410     spin_lock_irqsave(&fim->lock, flags);
0411
0412     if (!icap_enabled(fim))
0413         frame_interval_monitor(fim, timestamp);
0414
0415     spin_unlock_irqrestore(&fim->lock, flags);
0416 }
0417
0418 /* Called by the subdev in its s_stream callback */
0419 int imx_media_fim_set_stream(struct imx_media_fim *fim,
0420                  const struct v4l2_fract *fi,
0421                  bool on)
0422 {
0423     unsigned long flags;
0424     int ret = 0;
0425
0426     v4l2_ctrl_lock(fim->ctrl[FIM_CL_ENABLE]);
0427
0428     if (fim->stream_on == on)
0429         goto out;
0430
0431     if (on) {
0432         spin_lock_irqsave(&fim->lock, flags);
0433         reset_fim(fim, true);
0434         update_fim_nominal(fim, fi);
0435         spin_unlock_irqrestore(&fim->lock, flags);
0436
0437         if (icap_enabled(fim)) {
0438             ret = fim_request_input_capture(fim);
0439             if (ret)
0440                 goto out;
0441             fim_acquire_first_ts(fim);
0442         }
0443     } else {
0444         if (icap_enabled(fim))
0445             fim_free_input_capture(fim);
0446     }
0447
0448     fim->stream_on = on;
0449 out:
0450     v4l2_ctrl_unlock(fim->ctrl[FIM_CL_ENABLE]);
0451     return ret;
0452 }
0453
0454 int imx_media_fim_add_controls(struct imx_media_fim *fim)
0455 {
0456     /* add the FIM controls to the calling subdev ctrl handler */
0457     return v4l2_ctrl_add_handler(fim->sd->ctrl_handler,
0458                      &fim->ctrl_handler, NULL, false);
0459 }
0460
0461 /* Called by the subdev in its subdev registered callback */
0462 struct imx_media_fim *imx_media_fim_init(struct v4l2_subdev *sd)
0463 {
0464     struct imx_media_fim *fim;
0465     int ret;
0466
0467     fim = devm_kzalloc(sd->dev, sizeof(*fim), GFP_KERNEL);
0468     if (!fim)
0469         return ERR_PTR(-ENOMEM);
0470
0471     fim->sd = sd;
0472
0473     spin_lock_init(&fim->lock);
0474
0475     ret = init_fim_controls(fim);
0476     if (ret)
0477         return ERR_PTR(ret);
0478
0479     return fim;
0480 }
0481
0482 void imx_media_fim_free(struct imx_media_fim *fim)
0483 {
0484     v4l2_ctrl_handler_free(&fim->ctrl_handler);
0485 }