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