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_sensor_osd_mode {
  18         VIMC_SENSOR_OSD_SHOW_ALL = 0,
  19         VIMC_SENSOR_OSD_SHOW_COUNTERS = 1,
  20         VIMC_SENSOR_OSD_SHOW_NONE = 2
  21 };
  22
  23 struct vimc_sensor_device {
  24         struct vimc_ent_device ved;
  25         struct v4l2_subdev sd;
  26         struct tpg_data tpg;
  27         u8 *frame;
  28         enum vimc_sensor_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_sensor_init_cfg(struct v4l2_subdev *sd,
  45                                 struct v4l2_subdev_state *sd_state)
  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, sd_state, i);
  53                 *mf = fmt_default;
  54         }
  55
  56         return 0;
  57 }
  58
  59 static int vimc_sensor_enum_mbus_code(struct v4l2_subdev *sd,
  60                                       struct v4l2_subdev_state *sd_state,
  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_sensor_enum_frame_size(struct v4l2_subdev *sd,
  74                                        struct v4l2_subdev_state *sd_state,
  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_sensor_get_fmt(struct v4l2_subdev *sd,
  96                                struct v4l2_subdev_state *sd_state,
  97                                struct v4l2_subdev_format *fmt)
  98 {
  99         struct vimc_sensor_device *vsensor =
 100                                 container_of(sd, struct vimc_sensor_device, sd);
 101
 102         fmt->format = fmt->which == V4L2_SUBDEV_FORMAT_TRY ?
 103                       *v4l2_subdev_get_try_format(sd, sd_state, fmt->pad) :
 104                       vsensor->mbus_format;
 105
 106         return 0;
 107 }
 108
 109 static void vimc_sensor_tpg_s_format(struct vimc_sensor_device *vsensor)
 110 {
 111         const struct vimc_pix_map *vpix =
 112                                 vimc_pix_map_by_code(vsensor->mbus_format.code);
 113
 114         tpg_reset_source(&vsensor->tpg, vsensor->mbus_format.width,
 115                          vsensor->mbus_format.height, vsensor->mbus_format.field);
 116         tpg_s_bytesperline(&vsensor->tpg, 0, vsensor->mbus_format.width * vpix->bpp);
 117         tpg_s_buf_height(&vsensor->tpg, vsensor->mbus_format.height);
 118         tpg_s_fourcc(&vsensor->tpg, vpix->pixelformat);
 119         /* TODO: add support for V4L2_FIELD_ALTERNATE */
 120         tpg_s_field(&vsensor->tpg, vsensor->mbus_format.field, false);
 121         tpg_s_colorspace(&vsensor->tpg, vsensor->mbus_format.colorspace);
 122         tpg_s_ycbcr_enc(&vsensor->tpg, vsensor->mbus_format.ycbcr_enc);
 123         tpg_s_quantization(&vsensor->tpg, vsensor->mbus_format.quantization);
 124         tpg_s_xfer_func(&vsensor->tpg, vsensor->mbus_format.xfer_func);
 125 }
 126
 127 static void vimc_sensor_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_sensor_set_fmt(struct v4l2_subdev *sd,
 149                                struct v4l2_subdev_state *sd_state,
 150                                struct v4l2_subdev_format *fmt)
 151 {
 152         struct vimc_sensor_device *vsensor = 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 (vsensor->frame)
 158                         return -EBUSY;
 159
 160                 mf = &vsensor->mbus_format;
 161         } else {
 162                 mf = v4l2_subdev_get_try_format(sd, sd_state, fmt->pad);
 163         }
 164
 165         /* Set the new format */
 166         vimc_sensor_adjust_fmt(&fmt->format);
 167
 168         dev_dbg(vsensor->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", vsensor->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_sensor_pad_ops = {
 186         .init_cfg               = vimc_sensor_init_cfg,
 187         .enum_mbus_code         = vimc_sensor_enum_mbus_code,
 188         .enum_frame_size        = vimc_sensor_enum_frame_size,
 189         .get_fmt                = vimc_sensor_get_fmt,
 190         .set_fmt                = vimc_sensor_set_fmt,
 191 };
 192
 193 static void *vimc_sensor_process_frame(struct vimc_ent_device *ved,
 194                                        const void *sink_frame)
 195 {
 196         struct vimc_sensor_device *vsensor =
 197                 container_of(ved, struct vimc_sensor_device, ved);
 198
 199         const unsigned int line_height = 16;
 200         u8 *basep[TPG_MAX_PLANES][2];
 201         unsigned int line = 1;
 202         char str[100];
 203
 204         tpg_fill_plane_buffer(&vsensor->tpg, 0, 0, vsensor->frame);
 205         tpg_calc_text_basep(&vsensor->tpg, basep, 0, vsensor->frame);
 206         switch (vsensor->osd_value) {
 207         case VIMC_SENSOR_OSD_SHOW_ALL: {
 208                 const char *order = tpg_g_color_order(&vsensor->tpg);
 209
 210                 tpg_gen_text(&vsensor->tpg, basep, line++ * line_height,
 211                              16, order);
 212                 snprintf(str, sizeof(str),
 213                          "brightness %3d, contrast %3d, saturation %3d, hue %d ",
 214                          vsensor->tpg.brightness,
 215                          vsensor->tpg.contrast,
 216                          vsensor->tpg.saturation,
 217                          vsensor->tpg.hue);
 218                 tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
 219                 snprintf(str, sizeof(str), "sensor size: %dx%d",
 220                          vsensor->mbus_format.width,
 221                          vsensor->mbus_format.height);
 222                 tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
 223                 fallthrough;
 224         }
 225         case VIMC_SENSOR_OSD_SHOW_COUNTERS: {
 226                 unsigned int ms;
 227
 228                 ms = div_u64(ktime_get_ns() - vsensor->start_stream_ts, 1000000);
 229                 snprintf(str, sizeof(str), "%02d:%02d:%02d:%03d",
 230                          (ms / (60 * 60 * 1000)) % 24,
 231                          (ms / (60 * 1000)) % 60,
 232                          (ms / 1000) % 60,
 233                          ms % 1000);
 234                 tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
 235                 break;
 236         }
 237         case VIMC_SENSOR_OSD_SHOW_NONE:
 238         default:
 239                 break;
 240         }
 241
 242         return vsensor->frame;
 243 }
 244
 245 static int vimc_sensor_s_stream(struct v4l2_subdev *sd, int enable)
 246 {
 247         struct vimc_sensor_device *vsensor =
 248                                 container_of(sd, struct vimc_sensor_device, sd);
 249
 250         if (enable) {
 251                 const struct vimc_pix_map *vpix;
 252                 unsigned int frame_size;
 253
 254                 vsensor->start_stream_ts = ktime_get_ns();
 255
 256                 /* Calculate the frame size */
 257                 vpix = vimc_pix_map_by_code(vsensor->mbus_format.code);
 258                 frame_size = vsensor->mbus_format.width * vpix->bpp *
 259                              vsensor->mbus_format.height;
 260
 261                 /*
 262                  * Allocate the frame buffer. Use vmalloc to be able to
 263                  * allocate a large amount of memory
 264                  */
 265                 vsensor->frame = vmalloc(frame_size);
 266                 if (!vsensor->frame)
 267                         return -ENOMEM;
 268
 269                 /* configure the test pattern generator */
 270                 vimc_sensor_tpg_s_format(vsensor);
 271
 272         } else {
 273
 274                 vfree(vsensor->frame);
 275                 vsensor->frame = NULL;
 276         }
 277
 278         return 0;
 279 }
 280
 281 static const struct v4l2_subdev_core_ops vimc_sensor_core_ops = {
 282         .log_status = v4l2_ctrl_subdev_log_status,
 283         .subscribe_event = v4l2_ctrl_subdev_subscribe_event,
 284         .unsubscribe_event = v4l2_event_subdev_unsubscribe,
 285 };
 286
 287 static const struct v4l2_subdev_video_ops vimc_sensor_video_ops = {
 288         .s_stream = vimc_sensor_s_stream,
 289 };
 290
 291 static const struct v4l2_subdev_ops vimc_sensor_ops = {
 292         .core = &vimc_sensor_core_ops,
 293         .pad = &vimc_sensor_pad_ops,
 294         .video = &vimc_sensor_video_ops,
 295 };
 296
 297 static int vimc_sensor_s_ctrl(struct v4l2_ctrl *ctrl)
 298 {
 299         struct vimc_sensor_device *vsensor =
 300                 container_of(ctrl->handler, struct vimc_sensor_device, hdl);
 301
 302         switch (ctrl->id) {
 303         case VIMC_CID_TEST_PATTERN:
 304                 tpg_s_pattern(&vsensor->tpg, ctrl->val);
 305                 break;
 306         case V4L2_CID_HFLIP:
 307                 tpg_s_hflip(&vsensor->tpg, ctrl->val);
 308                 break;
 309         case V4L2_CID_VFLIP:
 310                 tpg_s_vflip(&vsensor->tpg, ctrl->val);
 311                 break;
 312         case V4L2_CID_BRIGHTNESS:
 313                 tpg_s_brightness(&vsensor->tpg, ctrl->val);
 314                 break;
 315         case V4L2_CID_CONTRAST:
 316                 tpg_s_contrast(&vsensor->tpg, ctrl->val);
 317                 break;
 318         case V4L2_CID_HUE:
 319                 tpg_s_hue(&vsensor->tpg, ctrl->val);
 320                 break;
 321         case V4L2_CID_SATURATION:
 322                 tpg_s_saturation(&vsensor->tpg, ctrl->val);
 323                 break;
 324         case VIMC_CID_OSD_TEXT_MODE:
 325                 vsensor->osd_value = ctrl->val;
 326                 break;
 327         default:
 328                 return -EINVAL;
 329         }
 330         return 0;
 331 }
 332
 333 static const struct v4l2_ctrl_ops vimc_sensor_ctrl_ops = {
 334         .s_ctrl = vimc_sensor_s_ctrl,
 335 };
 336
 337 static void vimc_sensor_release(struct vimc_ent_device *ved)
 338 {
 339         struct vimc_sensor_device *vsensor =
 340                 container_of(ved, struct vimc_sensor_device, ved);
 341
 342         v4l2_ctrl_handler_free(&vsensor->hdl);
 343         tpg_free(&vsensor->tpg);
 344         media_entity_cleanup(vsensor->ved.ent);
 345         kfree(vsensor);
 346 }
 347
 348 /* Image Processing Controls */
 349 static const struct v4l2_ctrl_config vimc_sensor_ctrl_class = {
 350         .flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
 351         .id = VIMC_CID_VIMC_CLASS,
 352         .name = "VIMC Controls",
 353         .type = V4L2_CTRL_TYPE_CTRL_CLASS,
 354 };
 355
 356 static const struct v4l2_ctrl_config vimc_sensor_ctrl_test_pattern = {
 357         .ops = &vimc_sensor_ctrl_ops,
 358         .id = VIMC_CID_TEST_PATTERN,
 359         .name = "Test Pattern",
 360         .type = V4L2_CTRL_TYPE_MENU,
 361         .max = TPG_PAT_NOISE,
 362         .qmenu = tpg_pattern_strings,
 363 };
 364
 365 static const char * const vimc_ctrl_osd_mode_strings[] = {
 366         "All",
 367         "Counters Only",
 368         "None",
 369         NULL,
 370 };
 371
 372 static const struct v4l2_ctrl_config vimc_sensor_ctrl_osd_mode = {
 373         .ops = &vimc_sensor_ctrl_ops,
 374         .id = VIMC_CID_OSD_TEXT_MODE,
 375         .name = "Show Information",
 376         .type = V4L2_CTRL_TYPE_MENU,
 377         .max = ARRAY_SIZE(vimc_ctrl_osd_mode_strings) - 2,
 378         .qmenu = vimc_ctrl_osd_mode_strings,
 379 };
 380
 381 static struct vimc_ent_device *vimc_sensor_add(struct vimc_device *vimc,
 382                                                const char *vcfg_name)
 383 {
 384         struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
 385         struct vimc_sensor_device *vsensor;
 386         int ret;
 387
 388         /* Allocate the vsensor struct */
 389         vsensor = kzalloc(sizeof(*vsensor), GFP_KERNEL);
 390         if (!vsensor)
 391                 return ERR_PTR(-ENOMEM);
 392
 393         v4l2_ctrl_handler_init(&vsensor->hdl, 4);
 394
 395         v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_class, NULL);
 396         v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_test_pattern, NULL);
 397         v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_osd_mode, NULL);
 398         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 399                           V4L2_CID_VFLIP, 0, 1, 1, 0);
 400         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 401                           V4L2_CID_HFLIP, 0, 1, 1, 0);
 402         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 403                           V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
 404         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 405                           V4L2_CID_CONTRAST, 0, 255, 1, 128);
 406         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 407                           V4L2_CID_HUE, -128, 127, 1, 0);
 408         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 409                           V4L2_CID_SATURATION, 0, 255, 1, 128);
 410         vsensor->sd.ctrl_handler = &vsensor->hdl;
 411         if (vsensor->hdl.error) {
 412                 ret = vsensor->hdl.error;
 413                 goto err_free_vsensor;
 414         }
 415
 416         /* Initialize the test pattern generator */
 417         tpg_init(&vsensor->tpg, vsensor->mbus_format.width,
 418                  vsensor->mbus_format.height);
 419         ret = tpg_alloc(&vsensor->tpg, VIMC_FRAME_MAX_WIDTH);
 420         if (ret)
 421                 goto err_free_hdl;
 422
 423         /* Initialize ved and sd */
 424         vsensor->pad.flags = MEDIA_PAD_FL_SOURCE;
 425         ret = vimc_ent_sd_register(&vsensor->ved, &vsensor->sd, v4l2_dev,
 426                                    vcfg_name,
 427                                    MEDIA_ENT_F_CAM_SENSOR, 1, &vsensor->pad,
 428                                    &vimc_sensor_ops);
 429         if (ret)
 430                 goto err_free_tpg;
 431
 432         vsensor->ved.process_frame = vimc_sensor_process_frame;
 433         vsensor->ved.dev = vimc->mdev.dev;
 434
 435         /* Initialize the frame format */
 436         vsensor->mbus_format = fmt_default;
 437
 438         return &vsensor->ved;
 439
 440 err_free_tpg:
 441         tpg_free(&vsensor->tpg);
 442 err_free_hdl:
 443         v4l2_ctrl_handler_free(&vsensor->hdl);
 444 err_free_vsensor:
 445         kfree(vsensor);
 446
 447         return ERR_PTR(ret);
 448 }
 449
 450 struct vimc_ent_type vimc_sensor_type = {
 451         .add = vimc_sensor_add,
 452         .release = vimc_sensor_release
 453 };