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_state(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_state_get_format(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_state_get_format(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_state_get_format(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         .enum_mbus_code         = vimc_sensor_enum_mbus_code,
 187         .enum_frame_size        = vimc_sensor_enum_frame_size,
 188         .get_fmt                = vimc_sensor_get_fmt,
 189         .set_fmt                = vimc_sensor_set_fmt,
 190 };
 191
 192 static void *vimc_sensor_process_frame(struct vimc_ent_device *ved,
 193                                        const void *sink_frame)
 194 {
 195         struct vimc_sensor_device *vsensor =
 196                 container_of(ved, struct vimc_sensor_device, ved);
 197
 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(&vsensor->tpg, 0, 0, vsensor->frame);
 204         tpg_calc_text_basep(&vsensor->tpg, basep, 0, vsensor->frame);
 205         switch (vsensor->osd_value) {
 206         case VIMC_SENSOR_OSD_SHOW_ALL: {
 207                 const char *order = tpg_g_color_order(&vsensor->tpg);
 208
 209                 tpg_gen_text(&vsensor->tpg, basep, line++ * line_height,
 210                              16, order);
 211                 snprintf(str, sizeof(str),
 212                          "brightness %3d, contrast %3d, saturation %3d, hue %d ",
 213                          vsensor->tpg.brightness,
 214                          vsensor->tpg.contrast,
 215                          vsensor->tpg.saturation,
 216                          vsensor->tpg.hue);
 217                 tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
 218                 snprintf(str, sizeof(str), "sensor size: %dx%d",
 219                          vsensor->mbus_format.width,
 220                          vsensor->mbus_format.height);
 221                 tpg_gen_text(&vsensor->tpg, basep, line++ * line_height, 16, str);
 222                 fallthrough;
 223         }
 224         case VIMC_SENSOR_OSD_SHOW_COUNTERS: {
 225                 unsigned int ms;
 226
 227                 ms = div_u64(ktime_get_ns() - vsensor->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(&vsensor->tpg, basep, line++ * line_height, 16, str);
 234                 break;
 235         }
 236         case VIMC_SENSOR_OSD_SHOW_NONE:
 237         default:
 238                 break;
 239         }
 240
 241         return vsensor->frame;
 242 }
 243
 244 static int vimc_sensor_s_stream(struct v4l2_subdev *sd, int enable)
 245 {
 246         struct vimc_sensor_device *vsensor =
 247                                 container_of(sd, struct vimc_sensor_device, sd);
 248
 249         if (enable) {
 250                 const struct vimc_pix_map *vpix;
 251                 unsigned int frame_size;
 252
 253                 vsensor->start_stream_ts = ktime_get_ns();
 254
 255                 /* Calculate the frame size */
 256                 vpix = vimc_pix_map_by_code(vsensor->mbus_format.code);
 257                 frame_size = vsensor->mbus_format.width * vpix->bpp *
 258                              vsensor->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                 vsensor->frame = vmalloc(frame_size);
 265                 if (!vsensor->frame)
 266                         return -ENOMEM;
 267
 268                 /* configure the test pattern generator */
 269                 vimc_sensor_tpg_s_format(vsensor);
 270
 271         } else {
 272
 273                 vfree(vsensor->frame);
 274                 vsensor->frame = NULL;
 275         }
 276
 277         return 0;
 278 }
 279
 280 static const struct v4l2_subdev_core_ops vimc_sensor_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_sensor_video_ops = {
 287         .s_stream = vimc_sensor_s_stream,
 288 };
 289
 290 static const struct v4l2_subdev_ops vimc_sensor_ops = {
 291         .core = &vimc_sensor_core_ops,
 292         .pad = &vimc_sensor_pad_ops,
 293         .video = &vimc_sensor_video_ops,
 294 };
 295
 296 static const struct v4l2_subdev_internal_ops vimc_sensor_internal_ops = {
 297         .init_state = vimc_sensor_init_state,
 298 };
 299
 300 static int vimc_sensor_s_ctrl(struct v4l2_ctrl *ctrl)
 301 {
 302         struct vimc_sensor_device *vsensor =
 303                 container_of(ctrl->handler, struct vimc_sensor_device, hdl);
 304
 305         switch (ctrl->id) {
 306         case VIMC_CID_TEST_PATTERN:
 307                 tpg_s_pattern(&vsensor->tpg, ctrl->val);
 308                 break;
 309         case V4L2_CID_HFLIP:
 310                 tpg_s_hflip(&vsensor->tpg, ctrl->val);
 311                 break;
 312         case V4L2_CID_VFLIP:
 313                 tpg_s_vflip(&vsensor->tpg, ctrl->val);
 314                 break;
 315         case V4L2_CID_BRIGHTNESS:
 316                 tpg_s_brightness(&vsensor->tpg, ctrl->val);
 317                 break;
 318         case V4L2_CID_CONTRAST:
 319                 tpg_s_contrast(&vsensor->tpg, ctrl->val);
 320                 break;
 321         case V4L2_CID_HUE:
 322                 tpg_s_hue(&vsensor->tpg, ctrl->val);
 323                 break;
 324         case V4L2_CID_SATURATION:
 325                 tpg_s_saturation(&vsensor->tpg, ctrl->val);
 326                 break;
 327         case VIMC_CID_OSD_TEXT_MODE:
 328                 vsensor->osd_value = ctrl->val;
 329                 break;
 330         default:
 331                 return -EINVAL;
 332         }
 333         return 0;
 334 }
 335
 336 static const struct v4l2_ctrl_ops vimc_sensor_ctrl_ops = {
 337         .s_ctrl = vimc_sensor_s_ctrl,
 338 };
 339
 340 static void vimc_sensor_release(struct vimc_ent_device *ved)
 341 {
 342         struct vimc_sensor_device *vsensor =
 343                 container_of(ved, struct vimc_sensor_device, ved);
 344
 345         v4l2_ctrl_handler_free(&vsensor->hdl);
 346         tpg_free(&vsensor->tpg);
 347         media_entity_cleanup(vsensor->ved.ent);
 348         kfree(vsensor);
 349 }
 350
 351 /* Image Processing Controls */
 352 static const struct v4l2_ctrl_config vimc_sensor_ctrl_class = {
 353         .flags = V4L2_CTRL_FLAG_READ_ONLY | V4L2_CTRL_FLAG_WRITE_ONLY,
 354         .id = VIMC_CID_VIMC_CLASS,
 355         .name = "VIMC Controls",
 356         .type = V4L2_CTRL_TYPE_CTRL_CLASS,
 357 };
 358
 359 static const struct v4l2_ctrl_config vimc_sensor_ctrl_test_pattern = {
 360         .ops = &vimc_sensor_ctrl_ops,
 361         .id = VIMC_CID_TEST_PATTERN,
 362         .name = "Test Pattern",
 363         .type = V4L2_CTRL_TYPE_MENU,
 364         .max = TPG_PAT_NOISE,
 365         .qmenu = tpg_pattern_strings,
 366 };
 367
 368 static const char * const vimc_ctrl_osd_mode_strings[] = {
 369         "All",
 370         "Counters Only",
 371         "None",
 372         NULL,
 373 };
 374
 375 static const struct v4l2_ctrl_config vimc_sensor_ctrl_osd_mode = {
 376         .ops = &vimc_sensor_ctrl_ops,
 377         .id = VIMC_CID_OSD_TEXT_MODE,
 378         .name = "Show Information",
 379         .type = V4L2_CTRL_TYPE_MENU,
 380         .max = ARRAY_SIZE(vimc_ctrl_osd_mode_strings) - 2,
 381         .qmenu = vimc_ctrl_osd_mode_strings,
 382 };
 383
 384 static struct vimc_ent_device *vimc_sensor_add(struct vimc_device *vimc,
 385                                                const char *vcfg_name)
 386 {
 387         struct v4l2_device *v4l2_dev = &vimc->v4l2_dev;
 388         struct vimc_sensor_device *vsensor;
 389         int ret;
 390
 391         /* Allocate the vsensor struct */
 392         vsensor = kzalloc(sizeof(*vsensor), GFP_KERNEL);
 393         if (!vsensor)
 394                 return ERR_PTR(-ENOMEM);
 395
 396         v4l2_ctrl_handler_init(&vsensor->hdl, 4);
 397
 398         v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_class, NULL);
 399         v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_test_pattern, NULL);
 400         v4l2_ctrl_new_custom(&vsensor->hdl, &vimc_sensor_ctrl_osd_mode, NULL);
 401         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 402                           V4L2_CID_VFLIP, 0, 1, 1, 0);
 403         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 404                           V4L2_CID_HFLIP, 0, 1, 1, 0);
 405         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 406                           V4L2_CID_BRIGHTNESS, 0, 255, 1, 128);
 407         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 408                           V4L2_CID_CONTRAST, 0, 255, 1, 128);
 409         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 410                           V4L2_CID_HUE, -128, 127, 1, 0);
 411         v4l2_ctrl_new_std(&vsensor->hdl, &vimc_sensor_ctrl_ops,
 412                           V4L2_CID_SATURATION, 0, 255, 1, 128);
 413         vsensor->sd.ctrl_handler = &vsensor->hdl;
 414         if (vsensor->hdl.error) {
 415                 ret = vsensor->hdl.error;
 416                 goto err_free_vsensor;
 417         }
 418
 419         /* Initialize the test pattern generator */
 420         tpg_init(&vsensor->tpg, vsensor->mbus_format.width,
 421                  vsensor->mbus_format.height);
 422         ret = tpg_alloc(&vsensor->tpg, VIMC_FRAME_MAX_WIDTH);
 423         if (ret)
 424                 goto err_free_hdl;
 425
 426         /* Initialize ved and sd */
 427         vsensor->pad.flags = MEDIA_PAD_FL_SOURCE;
 428         ret = vimc_ent_sd_register(&vsensor->ved, &vsensor->sd, v4l2_dev,
 429                                    vcfg_name,
 430                                    MEDIA_ENT_F_CAM_SENSOR, 1, &vsensor->pad,
 431                                    &vimc_sensor_ops);
 432         if (ret)
 433                 goto err_free_tpg;
 434
 435         vsensor->sd.internal_ops = &vimc_sensor_internal_ops;
 436
 437         vsensor->ved.process_frame = vimc_sensor_process_frame;
 438         vsensor->ved.dev = vimc->mdev.dev;
 439
 440         /* Initialize the frame format */
 441         vsensor->mbus_format = fmt_default;
 442
 443         return &vsensor->ved;
 444
 445 err_free_tpg:
 446         tpg_free(&vsensor->tpg);
 447 err_free_hdl:
 448         v4l2_ctrl_handler_free(&vsensor->hdl);
 449 err_free_vsensor:
 450         kfree(vsensor);
 451
 452         return ERR_PTR(ret);
 453 }
 454
 455 struct vimc_ent_type vimc_sensor_type = {
 456         .add = vimc_sensor_add,
 457         .release = vimc_sensor_release
 458 };