drivers/media/test-drivers/vimc/vimc-sensor.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * vimc-sensor.c Virtual Media Controller Driver
   4  *
   5  * Copyright (C) 2015-2017 Helen Koike <helen.fornazier@gmail.com>
   6  */
   7
   8 #include <linux/v4l2-mediabus.h>
   9 #include <linux/vmalloc.h>
  10 #include <media/v4l2-ctrls.h>
  11 #include <media/v4l2-event.h>
  12 #include <media/v4l2-subdev.h>
  13 #include <media/tpg/v4l2-tpg.h>
  14
  15 #include "vimc-common.h"
  16
  17 enum vimc_sen_osd_mode {
  18         VIMC_SEN_OSD_SHOW_ALL = 0,
  19         VIMC_SEN_OSD_SHOW_COUNTERS = 1,
  20         VIMC_SEN_OSD_SHOW_NONE = 2
  21 };
  22
  23 struct vimc_sen_device {
  24         struct vimc_ent_device ved;
  25         struct v4l2_subdev sd;
  26         struct tpg_data tpg;
  27         u8 *frame;
  28         enum vimc_sen_osd_mode osd_value;
  29         u64 start_stream_ts;
  30         /* The active format */
  31         struct v4l2_mbus_framefmt mbus_format;
  32         struct v4l2_ctrl_handler hdl;
  33         struct media_pad pad;
  34 };
  35
  36 static const struct v4l2_mbus_framefmt fmt_default = {
  37         .width = 640,
  38         .height = 480,
  39         .code = MEDIA_BUS_FMT_RGB888_1X24,
  40         .field = V4L2_FIELD_NONE,
  41         .colorspace = V4L2_COLORSPACE_SRGB,
  42 };
  43
  44 static int vimc_sen_init_cfg(struct v4l2_subdev *sd,
  45                              struct v4l2_subdev_pad_config *cfg)
  46 {
  47         unsigned int i;
  48
  49         for (i = 0; i < sd->entity.num_pads; i++) {
  50                 struct v4l2_mbus_framefmt *mf;
  51
  52                 mf = v4l2_subdev_get_try_format(sd, cfg, i);
  53                 *mf = fmt_default;
  54         }
  55
  56         return 0;
  57 }
  58
  59 static int vimc_sen_enum_mbus_code(struct v4l2_subdev *sd,
  60                                    struct v4l2_subdev_pad_config *cfg,
  61                                    struct v4l2_subdev_mbus_code_enum *code)
  62 {
  63         u32 mbus_code = vimc_mbus_code_by_index(code->index);
  64
  65         if (!mbus_code)
  66                 return -EINVAL;
  67
  68         code->code = mbus_code;
  69
  70         return 0;
  71 }
  72
  73 static int vimc_sen_enum_frame_size(struct v4l2_subdev *sd,
  74                                     struct v4l2_subdev_pad_config *cfg,
  75                                     struct v4l2_subdev_frame_size_enum *fse)
  76 {
  77         const struct vimc_pix_map *vpix;
  78
  79         if (fse->index)
  80                 return -EINVAL;
  81
  82         /* Only accept code in the pix map table */
  83         vpix = vimc_pix_map_by_code(fse->code);
  84         if (!vpix)
  85                 return -EINVAL;
  86
  87         fse->min_width = VIMC_FRAME_MIN_WIDTH;
  88         fse->max_width = VIMC_FRAME_MAX_WIDTH;
  89         fse->min_height = VIMC_FRAME_MIN_HEIGHT;
  90         fse->max_height = VIMC_FRAME_MAX_HEIGHT;
  91
  92         return 0;
  93 }
  94
  95 static int vimc_sen_get_fmt(struct v4l2_subdev *sd,
  96                             struct v4l2_subdev_pad_config *cfg,
  97                             struct v4l2_subdev_format *fmt)
  98 {
  99         struct vimc_sen_device *vsen =
 100                                 container_of(sd, struct vimc_sen_device, sd);
 101
 102         fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
 103                       *v4l2_subdev_get_try_format(sd, cfg, fmt->pad) :
 104                       vsen->mbus_format;
 105
 106         return 0;
 107 }
 108
 109 static void vimc_sen_tpg_s_format(struct vimc_sen_device *vsen)
 110 {
 111         const struct vimc_pix_map *vpix =
 112                                 vimc_pix_map_by_code(vsen->mbus_format.code);
 113
 114         tpg_reset_source(&vsen->tpg, vsen->mbus_format.width,
 115                          vsen->mbus_format.height, vsen->mbus_format.field);
 116         tpg_s_bytesperline(&vsen->tpg, 0, vsen->mbus_format.width * vpix->bpp);
 117         tpg_s_buf_height(&vsen->tpg, vsen->mbus_format.height);
 118         tpg_s_fourcc(&vsen->tpg, vpix->pixelformat);
 119         /* TODO: add support for V4L2_FIELD_ALTERNATE */
 120         tpg_s_field(&vsen->tpg, vsen->mbus_format.field, false);
 121         tpg_s_colorspace(&vsen->tpg, vsen->mbus_format.colorspace);
 122         tpg_s_ycbcr_enc(&vsen->tpg, vsen->mbus_format.ycbcr_enc);
 123         tpg_s_quantization(&vsen->tpg, vsen->mbus_format.quantization);
 124         tpg_s_xfer_func(&vsen->tpg, vsen->mbus_format.xfer_func);
 125 }
 126
 127 static void vimc_sen_adjust_fmt(struct v4l2_mbus_framefmt *fmt)
 128 {
 129         const struct vimc_pix_map *vpix;
 130
 131         /* Only accept code in the pix map table */
 132         vpix = vimc_pix_map_by_code(fmt->code);
 133         if (!vpix)
 134                 fmt->code = fmt_default.code;
 135
 136         fmt->width = clamp_t(u32, fmt->width, VIMC_FRAME_MIN_WIDTH,
 137                              VIMC_FRAME_MAX_WIDTH) & ~1;
 138         fmt->height = clamp_t(u32, fmt->height, VIMC_FRAME_MIN_HEIGHT,
 139                               VIMC_FRAME_MAX_HEIGHT) & ~1;
 140
 141         /* TODO: add support for V4L2_FIELD_ALTERNATE */
 142         if (fmt->field == V4L2_FIELD_ANY || fmt->field == V4L2_FIELD_ALTERNATE)
 143                 fmt->field = fmt_default.field;
 144
 145         vimc_colorimetry_clamp(fmt);
 146 }
 147
 148 static int vimc_sen_set_fmt(struct v4l2_subdev *sd,
 149                             struct v4l2_subdev_pad_config *cfg,
 150                             struct v4l2_subdev_format *fmt)
 151 {
 152         struct vimc_sen_device *vsen = v4l2_get_subdevdata(sd);
 153         struct v4l2_mbus_framefmt *mf;
 154
 155         if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE) {
 156                 /* Do not change the format while stream is on */
 157                 if (vsen->frame)
 158                         return -EBUSY;
 159
 160                 mf = &vsen->mbus_format;
 161         } else {
 162                 mf = v4l2_subdev_get_try_format(sd, cfg, fmt->pad);
 163         }
 164
 165         /* Set the new format */
 166         vimc_sen_adjust_fmt(&fmt->format);
 167
 168         dev_dbg(vsen->ved.dev, "%s: format update: "
 169                 "old:%dx%d (0x%x, %d, %d, %d, %d) "
 170                 "new:%dx%d (0x%x, %d, %d, %d, %d)\n", vsen->sd.name,
 171                 /* old */
 172                 mf->width, mf->height, mf->code,
 173                 mf->colorspace, mf->quantization,
 174                 mf->xfer_func, mf->ycbcr_enc,
 175                 /* new */
 176                 fmt->format.width, fmt->format.height, fmt->format.code,
 177                 fmt->format.colorspace, fmt->format.quantization,
 178                 fmt->format.xfer_func, fmt->format.ycbcr_enc);
 179
 180         *mf = fmt->format;
 181
 182         return 0;
 183 }
 184
 185 static const struct v4l2_subdev_pad_ops vimc_sen_pad_ops = {
 186         .init_cfg               = vimc_sen_init_cfg,
 187         .enum_mbus_code         = vimc_sen_enum_mbus_code,
 188         .enum_frame_size        = vimc_sen_enum_frame_size,
 189         .get_fmt                = vimc_sen_get_fmt,
 190         .set_fmt                = vimc_sen_set_fmt,
 191 };
 192
 193 static void *vimc_sen_process_frame(struct vimc_ent_device *ved,
 194                                     const void *sink_frame)
 195 {
 196         struct vimc_sen_device *vsen = container_of(ved, struct vimc_sen_device,
 197                                                     ved);
 198         const unsigned int line_height = 16;
 199         u8 *basep[TPG_MAX_PLANES][2];
 200         unsigned int line = 1;
 201         char str[100];
 202
 203         tpg_fill_plane_buffer(&vsen->tpg, 0, 0, vsen->frame);
 204         tpg_calc_text_basep(&vsen->tpg, basep, 0, vsen->frame);
 205         switch (vsen->osd_value) {
 206         case VIMC_SEN_OSD_SHOW_ALL: {
 207                 const char *order = tpg_g_color_order(&vsen->tpg);
 208
 209                 tpg_gen_text(&vsen->tpg, basep, line++ * line_height,
 210                              16, order);
 211                 snprintf(str, sizeof(str),
 212                          "brightness %3d, contrast %3d, saturation %3d, hue %d ",
 213                          vsen->tpg.brightness,
 214                          vsen->tpg.contrast,
 215                          vsen->tpg.saturation,
 216                          vsen->tpg.hue);
 217                 tpg_gen_text(&vsen->tpg, basep, line++ * line_height, 16, str);
 218                 snprintf(str, sizeof(str), "sensor size: %dx%d",
 219                          vsen->mbus_format.width,
 220                          vsen->mbus_format.height);
 221                 tpg_gen_text(&vsen->tpg, basep, line++ * line_height, 16, str);
 222                 fallthrough;
 223         }
 224         case VIMC_SEN_OSD_SHOW_COUNTERS: {
 225                 unsigned int ms;
 226
 227                 ms = div_u64(ktime_get_ns() - vsen->start_stream_ts, 1000000);
 228                 snprintf(str, sizeof(str), "%02d:%02d:%02d:%03d",
 229                          (ms / (60 * 60 * 1000)) % 24,
 230                          (ms / (60 * 1000)) % 60,
 231                          (ms / 1000) % 60,
 232                          ms % 1000);
 233                 tpg_gen_text(&vsen->tpg, basep, line++ * line_height, 16, str);
 234                 break;
 235         }
 236         case VIMC_SEN_OSD_SHOW_NONE:
 237         default:
 238                 break;
 239         }
 240
 241         return vsen->frame;
 242 }
 243
 244 static int vimc_sen_s_stream(struct v4l2_subdev *sd, int enable)
 245 {
 246         struct vimc_sen_device *vsen =
 247                                 container_of(sd, struct vimc_sen_device, sd);
 248
 249         if (enable) {
 250                 const struct vimc_pix_map *vpix;
 251                 unsigned int frame_size;
 252
 253                 vsen->start_stream_ts = ktime_get_ns();
 254
 255                 /* Calculate the frame size */
 256                 vpix = vimc_pix_map_by_code(vsen->mbus_format.code);
 257                 frame_size = vsen->mbus_format.width * vpix->bpp *
 258                              vsen->mbus_format.height;
 259
 260                 /*
 261                  * Allocate the frame buffer. Use vmalloc to be able to
 262                  * allocate a large amount of memory
 263                  */
 264                 vsen->frame = vmalloc(frame_size);
 265                 if (!vsen->frame)
 266                         return -ENOMEM;
 267
 268                 /* configure the test pattern generator */
 269                 vimc_sen_tpg_s_format(vsen);
 270
 271         } else {
 272
 273                 vfree(vsen->frame);
 274                 vsen->frame = NULL;
 275         }
 276
 277         return 0;
 278 }
 279
 280 static const struct v4l2_subdev_core_ops vimc_sen_core_ops = {
 281         .log_status = v4l2_ctrl_subdev_log_status,
 282         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
 283         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
 284 };
 285
 286 static const struct v4l2_subdev_video_ops vimc_sen_video_ops = {
 287         .s_stream = vimc_sen_s_stream,
 288 };
 289
 290 static const struct v4l2_subdev_ops vimc_sen_ops = {
 291         .core = &vimc_sen_core_ops,
 292         .pad = &vimc_sen_pad_ops,
 293         .video = &vimc_sen_video_ops,
 294 };
 295
 296 static int vimc_sen_s_ctrl(struct v4l2_ctrl *ctrl)
 297 {
 298         struct vimc_sen_device *vsen =
 299                 container_of(ctrl->handler, struct vimc_sen_device, hdl);
 300
 301         switch (ctrl->id) {
 302         case VIMC_CID_TEST_PATTERN:
 303                 tpg_s_pattern(&vsen->tpg, ctrl->val);
 304                 break;
 305         case V4L2_CID_HFLIP:
 306                 tpg_s_hflip(&vsen->tpg, ctrl->val);
 307                 break;
 308         case V4L2_CID_VFLIP:
 309                 tpg_s_vflip(&vsen->tpg, ctrl->val);
 310                 break;
 311         case V4L2_CID_BRIGHTNESS:
 312                 tpg_s_brightness(&vsen->tpg, ctrl->val);
 313                 break;
 314         case V4L2_CID_CONTRAST:
 315                 tpg_s_contrast(&vsen->tpg, ctrl->val);
 316                 break;
 317         case V4L2_CID_HUE:
 318                 tpg_s_hue(&vsen->tpg, ctrl->val);
 319                 break;
 320         case V4L2_CID_SATURATION:
 321                 tpg_s_saturation(&vsen->tpg, ctrl->val);
 322                 break;
 323         case VIMC_CID_OSD_TEXT_MODE:
 324                 vsen->osd_value = ctrl->val;
 325                 break;
 326         default:
 327                 return -EINVAL;
 328         }
 329         return 0;
 330 }
 331
 332 static const struct v4l2_ctrl_ops vimc_sen_ctrl_ops = {
 333         .s_ctrl = vimc_sen_s_ctrl,
 334 };
 335
 336 static void vimc_sen_release(struct vimc_ent_device *ved)
 337 {
 338         struct vimc_sen_device *vsen =
 339                 container_of(ved, struct vimc_sen_device, ved);
 340
 341         v4l2_ctrl_handler_free(&vsen->hdl);
 342         tpg_free(&vsen->tpg);
 343         media_entity_cleanup(vsen->ved.ent);
 344         kfree(vsen);
 345 }
 346
 347 /* Image Processing Controls */
 348 static const struct v4l2_ctrl_config vimc_sen_ctrl_class = {
 349         .flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
 350         .id = VIMC_CID_VIMC_CLASS,
 351         .name = "VIMC Controls",
 352         .type = V4L2_CTRL_TYPE_CTRL_CLASS,
 353 };
 354
 355 static const struct v4l2_ctrl_config vimc_sen_ctrl_test_pattern = {
 356         .ops = &vimc_sen_ctrl_ops,
 357         .id = VIMC_CID_TEST_PATTERN,
 358         .name = "Test Pattern",
 359         .type = V4L2_CTRL_TYPE_MENU,
 360         .max = TPG_PAT_NOISE,
 361         .qmenu = tpg_pattern_strings,
 362 };
 363
 364 static const char * const vimc_ctrl_osd_mode_strings[] = {
 365         "All",
 366         "Counters Only",
 367         "None",
 368         NULL,
 369 };
 370
 371 static const struct v4l2_ctrl_config vimc_sen_ctrl_osd_mode = {
 372         .ops = &vimc_sen_ctrl_ops,
 373         .id = VIMC_CID_OSD_TEXT_MODE,
 374         .name = "Show Information",
 375         .type = V4L2_CTRL_TYPE_MENU,
 376         .max = ARRAY_SIZE(vimc_ctrl_osd_mode_strings) - 2,
 377         .qmenu = vimc_ctrl_osd_mode_strings,
 378 };
 379
 380 static struct vimc_ent_device *vimc_sen_add(struct vimc_device *vimc,
 381                                             const char *vcfg_name)
 382 {
 383         struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
 384         struct vimc_sen_device *vsen;
 385         int ret;
 386
 387         /* Allocate the vsen struct */
 388         vsen = kzalloc(sizeof(*vsen), GFP_KERNEL);
 389         if (!vsen)
 390                 return ERR_PTR(-ENOMEM);
 391
 392         v4l2_ctrl_handler_init(&vsen->hdl, 4);
 393
 394         v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_class, NULL);
 395         v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_test_pattern, NULL);
 396         v4l2_ctrl_new_custom(&vsen->hdl, &vimc_sen_ctrl_osd_mode, NULL);
 397         v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
 398                           V4L2_CID_VFLIP, 0, 1, 1, 0);
 399         v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
 400                           V4L2_CID_HFLIP, 0, 1, 1, 0);
 401         v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
 402                           V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
 403         v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
 404                           V4L2_CID_CONTRAST, 0, 255, 1, 128);
 405         v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
 406                           V4L2_CID_HUE, -128, 127, 1, 0);
 407         v4l2_ctrl_new_std(&vsen->hdl, &vimc_sen_ctrl_ops,
 408                           V4L2_CID_SATURATION, 0, 255, 1, 128);
 409         vsen->sd.ctrl_handler = &vsen->hdl;
 410         if (vsen->hdl.error) {
 411                 ret = vsen->hdl.error;
 412                 goto err_free_vsen;
 413         }
 414
 415         /* Initialize the test pattern generator */
 416         tpg_init(&vsen->tpg, vsen->mbus_format.width,
 417                  vsen->mbus_format.height);
 418         ret = tpg_alloc(&vsen->tpg, VIMC_FRAME_MAX_WIDTH);
 419         if (ret)
 420                 goto err_free_hdl;
 421
 422         /* Initialize ved and sd */
 423         vsen->pad.flags = MEDIA_PAD_FL_SOURCE;
 424         ret = vimc_ent_sd_register(&vsen->ved, &vsen->sd, v4l2_dev,
 425                                    vcfg_name,
 426                                    MEDIA_ENT_F_CAM_SENSOR, 1, &vsen->pad,
 427                                    &vimc_sen_ops);
 428         if (ret)
 429                 goto err_free_tpg;
 430
 431         vsen->ved.process_frame = vimc_sen_process_frame;
 432         vsen->ved.dev = vimc->mdev.dev;
 433
 434         /* Initialize the frame format */
 435         vsen->mbus_format = fmt_default;
 436
 437         return &vsen->ved;
 438
 439 err_free_tpg:
 440         tpg_free(&vsen->tpg);
 441 err_free_hdl:
 442         v4l2_ctrl_handler_free(&vsen->hdl);
 443 err_free_vsen:
 444         kfree(vsen);
 445
 446         return ERR_PTR(ret);
 447 }
 448
 449 struct vimc_ent_type vimc_sen_type = {
 450         .add = vimc_sen_add,
 451         .release = vimc_sen_release
 452 };