1 // SPDX-License-Identifier: (GPL-2.0+ OR MIT) 2 /* 3 * Rockchip ISP1 Driver - Stats subdevice 4 * 5 * Copyright (C) 2017 Rockchip Electronics Co., Ltd. 6 */ 7 8 #include <media/v4l2-common.h> 9 #include <media/v4l2-event.h> 10 #include <media/v4l2-ioctl.h> 11 #include <media/videobuf2-core.h> 12 #include <media/videobuf2-vmalloc.h> /* for ISP statistics */ 13 14 #include "rkisp1-common.h" 15 16 #define RKISP1_STATS_DEV_NAME RKISP1_DRIVER_NAME "_stats" 17 18 #define RKISP1_ISP_STATS_REQ_BUFS_MIN 2 19 #define RKISP1_ISP_STATS_REQ_BUFS_MAX 8 20 21 static int rkisp1_stats_enum_fmt_meta_cap(struct file *file, void *priv, 22 struct v4l2_fmtdesc *f) 23 { 24 struct video_device *video = video_devdata(file); 25 struct rkisp1_stats *stats = video_get_drvdata(video); 26 27 if (f->index > 0 || f->type != video->queue->type) 28 return -EINVAL; 29 30 f->pixelformat = stats->vdev_fmt.fmt.meta.dataformat; 31 return 0; 32 } 33 34 static int rkisp1_stats_g_fmt_meta_cap(struct file *file, void *priv, 35 struct v4l2_format *f) 36 { 37 struct video_device *video = video_devdata(file); 38 struct rkisp1_stats *stats = video_get_drvdata(video); 39 struct v4l2_meta_format *meta = &f->fmt.meta; 40 41 if (f->type != video->queue->type) 42 return -EINVAL; 43 44 memset(meta, 0, sizeof(*meta)); 45 meta->dataformat = stats->vdev_fmt.fmt.meta.dataformat; 46 meta->buffersize = stats->vdev_fmt.fmt.meta.buffersize; 47 48 return 0; 49 } 50 51 static int rkisp1_stats_querycap(struct file *file, 52 void *priv, struct v4l2_capability *cap) 53 { 54 struct video_device *vdev = video_devdata(file); 55 56 strscpy(cap->driver, RKISP1_DRIVER_NAME, sizeof(cap->driver)); 57 strscpy(cap->card, vdev->name, sizeof(cap->card)); 58 strscpy(cap->bus_info, RKISP1_BUS_INFO, sizeof(cap->bus_info)); 59 60 return 0; 61 } 62 63 /* ISP video device IOCTLs */ 64 static const struct v4l2_ioctl_ops rkisp1_stats_ioctl = { 65 .vidioc_reqbufs = vb2_ioctl_reqbufs, 66 .vidioc_querybuf = vb2_ioctl_querybuf, 67 .vidioc_create_bufs = vb2_ioctl_create_bufs, 68 .vidioc_qbuf = vb2_ioctl_qbuf, 69 .vidioc_dqbuf = vb2_ioctl_dqbuf, 70 .vidioc_prepare_buf = vb2_ioctl_prepare_buf, 71 .vidioc_expbuf = vb2_ioctl_expbuf, 72 .vidioc_streamon = vb2_ioctl_streamon, 73 .vidioc_streamoff = vb2_ioctl_streamoff, 74 .vidioc_enum_fmt_meta_cap = rkisp1_stats_enum_fmt_meta_cap, 75 .vidioc_g_fmt_meta_cap = rkisp1_stats_g_fmt_meta_cap, 76 .vidioc_s_fmt_meta_cap = rkisp1_stats_g_fmt_meta_cap, 77 .vidioc_try_fmt_meta_cap = rkisp1_stats_g_fmt_meta_cap, 78 .vidioc_querycap = rkisp1_stats_querycap, 79 .vidioc_subscribe_event = v4l2_ctrl_subscribe_event, 80 .vidioc_unsubscribe_event = v4l2_event_unsubscribe, 81 }; 82 83 static const struct v4l2_file_operations rkisp1_stats_fops = { 84 .mmap = vb2_fop_mmap, 85 .unlocked_ioctl = video_ioctl2, 86 .poll = vb2_fop_poll, 87 .open = v4l2_fh_open, 88 .release = vb2_fop_release 89 }; 90 91 static int rkisp1_stats_vb2_queue_setup(struct vb2_queue *vq, 92 unsigned int *num_buffers, 93 unsigned int *num_planes, 94 unsigned int sizes[], 95 struct device *alloc_devs[]) 96 { 97 *num_planes = 1; 98 99 *num_buffers = clamp_t(u32, *num_buffers, RKISP1_ISP_STATS_REQ_BUFS_MIN, 100 RKISP1_ISP_STATS_REQ_BUFS_MAX); 101 102 sizes[0] = sizeof(struct rkisp1_stat_buffer); 103 104 return 0; 105 } 106 107 static void rkisp1_stats_vb2_buf_queue(struct vb2_buffer *vb) 108 { 109 struct vb2_v4l2_buffer *vbuf = to_vb2_v4l2_buffer(vb); 110 struct rkisp1_buffer *stats_buf = 111 container_of(vbuf, struct rkisp1_buffer, vb); 112 struct vb2_queue *vq = vb->vb2_queue; 113 struct rkisp1_stats *stats_dev = vq->drv_priv; 114 115 116 spin_lock_irq(&stats_dev->lock); 117 list_add_tail(&stats_buf->queue, &stats_dev->stat); 118 spin_unlock_irq(&stats_dev->lock); 119 } 120 121 static int rkisp1_stats_vb2_buf_prepare(struct vb2_buffer *vb) 122 { 123 if (vb2_plane_size(vb, 0) < sizeof(struct rkisp1_stat_buffer)) 124 return -EINVAL; 125 126 vb2_set_plane_payload(vb, 0, sizeof(struct rkisp1_stat_buffer)); 127 128 return 0; 129 } 130 131 static void rkisp1_stats_vb2_stop_streaming(struct vb2_queue *vq) 132 { 133 struct rkisp1_stats *stats = vq->drv_priv; 134 struct rkisp1_buffer *buf; 135 unsigned int i; 136 137 spin_lock_irq(&stats->lock); 138 for (i = 0; i < RKISP1_ISP_STATS_REQ_BUFS_MAX; i++) { 139 if (list_empty(&stats->stat)) 140 break; 141 buf = list_first_entry(&stats->stat, 142 struct rkisp1_buffer, queue); 143 list_del(&buf->queue); 144 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); 145 } 146 spin_unlock_irq(&stats->lock); 147 } 148 149 static const struct vb2_ops rkisp1_stats_vb2_ops = { 150 .queue_setup = rkisp1_stats_vb2_queue_setup, 151 .buf_queue = rkisp1_stats_vb2_buf_queue, 152 .buf_prepare = rkisp1_stats_vb2_buf_prepare, 153 .stop_streaming = rkisp1_stats_vb2_stop_streaming, 154 }; 155 156 static int 157 rkisp1_stats_init_vb2_queue(struct vb2_queue *q, struct rkisp1_stats *stats) 158 { 159 struct rkisp1_vdev_node *node; 160 161 node = container_of(q, struct rkisp1_vdev_node, buf_queue); 162 163 q->type = V4L2_BUF_TYPE_META_CAPTURE; 164 q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; 165 q->drv_priv = stats; 166 q->ops = &rkisp1_stats_vb2_ops; 167 q->mem_ops = &vb2_vmalloc_memops; 168 q->buf_struct_size = sizeof(struct rkisp1_buffer); 169 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 170 q->lock = &node->vlock; 171 172 return vb2_queue_init(q); 173 } 174 175 static void rkisp1_stats_get_awb_meas_v10(struct rkisp1_stats *stats, 176 struct rkisp1_stat_buffer *pbuf) 177 { 178 /* Protect against concurrent access from ISR? */ 179 struct rkisp1_device *rkisp1 = stats->rkisp1; 180 u32 reg_val; 181 182 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_AWB; 183 reg_val = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AWB_WHITE_CNT_V10); 184 pbuf->params.awb.awb_mean[0].cnt = 185 RKISP1_CIF_ISP_AWB_GET_PIXEL_CNT(reg_val); 186 reg_val = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AWB_MEAN_V10); 187 188 pbuf->params.awb.awb_mean[0].mean_cr_or_r = 189 RKISP1_CIF_ISP_AWB_GET_MEAN_CR_R(reg_val); 190 pbuf->params.awb.awb_mean[0].mean_cb_or_b = 191 RKISP1_CIF_ISP_AWB_GET_MEAN_CB_B(reg_val); 192 pbuf->params.awb.awb_mean[0].mean_y_or_g = 193 RKISP1_CIF_ISP_AWB_GET_MEAN_Y_G(reg_val); 194 } 195 196 static void rkisp1_stats_get_awb_meas_v12(struct rkisp1_stats *stats, 197 struct rkisp1_stat_buffer *pbuf) 198 { 199 /* Protect against concurrent access from ISR? */ 200 struct rkisp1_device *rkisp1 = stats->rkisp1; 201 u32 reg_val; 202 203 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_AWB; 204 reg_val = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AWB_WHITE_CNT_V12); 205 pbuf->params.awb.awb_mean[0].cnt = 206 RKISP1_CIF_ISP_AWB_GET_PIXEL_CNT(reg_val); 207 reg_val = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AWB_MEAN_V12); 208 209 pbuf->params.awb.awb_mean[0].mean_cr_or_r = 210 RKISP1_CIF_ISP_AWB_GET_MEAN_CR_R(reg_val); 211 pbuf->params.awb.awb_mean[0].mean_cb_or_b = 212 RKISP1_CIF_ISP_AWB_GET_MEAN_CB_B(reg_val); 213 pbuf->params.awb.awb_mean[0].mean_y_or_g = 214 RKISP1_CIF_ISP_AWB_GET_MEAN_Y_G(reg_val); 215 } 216 217 static void rkisp1_stats_get_aec_meas_v10(struct rkisp1_stats *stats, 218 struct rkisp1_stat_buffer *pbuf) 219 { 220 struct rkisp1_device *rkisp1 = stats->rkisp1; 221 unsigned int i; 222 223 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_AUTOEXP; 224 for (i = 0; i < RKISP1_CIF_ISP_AE_MEAN_MAX_V10; i++) 225 pbuf->params.ae.exp_mean[i] = 226 (u8)rkisp1_read(rkisp1, 227 RKISP1_CIF_ISP_EXP_MEAN_00_V10 + i * 4); 228 } 229 230 static void rkisp1_stats_get_aec_meas_v12(struct rkisp1_stats *stats, 231 struct rkisp1_stat_buffer *pbuf) 232 { 233 struct rkisp1_device *rkisp1 = stats->rkisp1; 234 u32 value; 235 int i; 236 237 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_AUTOEXP; 238 for (i = 0; i < RKISP1_CIF_ISP_AE_MEAN_MAX_V12 / 4; i++) { 239 value = rkisp1_read(rkisp1, RKISP1_CIF_ISP_EXP_MEAN_V12 + i * 4); 240 pbuf->params.ae.exp_mean[4 * i + 0] = 241 RKISP1_CIF_ISP_EXP_GET_MEAN_xy0_V12(value); 242 pbuf->params.ae.exp_mean[4 * i + 1] = 243 RKISP1_CIF_ISP_EXP_GET_MEAN_xy1_V12(value); 244 pbuf->params.ae.exp_mean[4 * i + 2] = 245 RKISP1_CIF_ISP_EXP_GET_MEAN_xy2_V12(value); 246 pbuf->params.ae.exp_mean[4 * i + 3] = 247 RKISP1_CIF_ISP_EXP_GET_MEAN_xy3_V12(value); 248 } 249 250 value = rkisp1_read(rkisp1, RKISP1_CIF_ISP_EXP_MEAN_V12 + i * 4); 251 pbuf->params.ae.exp_mean[4 * i + 0] = RKISP1_CIF_ISP_EXP_GET_MEAN_xy0_V12(value); 252 } 253 254 static void rkisp1_stats_get_afc_meas(struct rkisp1_stats *stats, 255 struct rkisp1_stat_buffer *pbuf) 256 { 257 struct rkisp1_device *rkisp1 = stats->rkisp1; 258 struct rkisp1_cif_isp_af_stat *af; 259 260 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_AFM; 261 262 af = &pbuf->params.af; 263 af->window[0].sum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_SUM_A); 264 af->window[0].lum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_LUM_A); 265 af->window[1].sum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_SUM_B); 266 af->window[1].lum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_LUM_B); 267 af->window[2].sum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_SUM_C); 268 af->window[2].lum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_LUM_C); 269 } 270 271 static void rkisp1_stats_get_hst_meas_v10(struct rkisp1_stats *stats, 272 struct rkisp1_stat_buffer *pbuf) 273 { 274 struct rkisp1_device *rkisp1 = stats->rkisp1; 275 unsigned int i; 276 277 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_HIST; 278 for (i = 0; i < RKISP1_CIF_ISP_HIST_BIN_N_MAX_V10; i++) { 279 u32 reg_val = rkisp1_read(rkisp1, RKISP1_CIF_ISP_HIST_BIN_0_V10 + i * 4); 280 281 pbuf->params.hist.hist_bins[i] = RKISP1_CIF_ISP_HIST_GET_BIN_V10(reg_val); 282 } 283 } 284 285 static void rkisp1_stats_get_hst_meas_v12(struct rkisp1_stats *stats, 286 struct rkisp1_stat_buffer *pbuf) 287 { 288 struct rkisp1_device *rkisp1 = stats->rkisp1; 289 u32 value; 290 int i; 291 292 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_HIST; 293 for (i = 0; i < RKISP1_CIF_ISP_HIST_BIN_N_MAX_V12 / 2; i++) { 294 value = rkisp1_read(rkisp1, RKISP1_CIF_ISP_HIST_BIN_V12 + i * 4); 295 pbuf->params.hist.hist_bins[2 * i] = 296 RKISP1_CIF_ISP_HIST_GET_BIN0_V12(value); 297 pbuf->params.hist.hist_bins[2 * i + 1] = 298 RKISP1_CIF_ISP_HIST_GET_BIN1_V12(value); 299 } 300 } 301 302 static void rkisp1_stats_get_bls_meas(struct rkisp1_stats *stats, 303 struct rkisp1_stat_buffer *pbuf) 304 { 305 static const u32 regs[] = { 306 RKISP1_CIF_ISP_BLS_A_MEASURED, 307 RKISP1_CIF_ISP_BLS_B_MEASURED, 308 RKISP1_CIF_ISP_BLS_C_MEASURED, 309 RKISP1_CIF_ISP_BLS_D_MEASURED, 310 }; 311 struct rkisp1_device *rkisp1 = stats->rkisp1; 312 const struct rkisp1_mbus_info *in_fmt = rkisp1->isp.sink_fmt; 313 struct rkisp1_cif_isp_bls_meas_val *bls_val; 314 u32 swapped[4]; 315 316 rkisp1_bls_swap_regs(in_fmt->bayer_pat, regs, swapped); 317 318 bls_val = &pbuf->params.ae.bls_val; 319 320 bls_val->meas_r = rkisp1_read(rkisp1, swapped[0]); 321 bls_val->meas_gr = rkisp1_read(rkisp1, swapped[1]); 322 bls_val->meas_gb = rkisp1_read(rkisp1, swapped[2]); 323 bls_val->meas_b = rkisp1_read(rkisp1, swapped[3]); 324 } 325 326 static const struct rkisp1_stats_ops rkisp1_v10_stats_ops = { 327 .get_awb_meas = rkisp1_stats_get_awb_meas_v10, 328 .get_aec_meas = rkisp1_stats_get_aec_meas_v10, 329 .get_hst_meas = rkisp1_stats_get_hst_meas_v10, 330 }; 331 332 static struct rkisp1_stats_ops rkisp1_v12_stats_ops = { 333 .get_awb_meas = rkisp1_stats_get_awb_meas_v12, 334 .get_aec_meas = rkisp1_stats_get_aec_meas_v12, 335 .get_hst_meas = rkisp1_stats_get_hst_meas_v12, 336 }; 337 338 static void 339 rkisp1_stats_send_measurement(struct rkisp1_stats *stats, u32 isp_ris) 340 { 341 struct rkisp1_stat_buffer *cur_stat_buf; 342 struct rkisp1_buffer *cur_buf = NULL; 343 unsigned int frame_sequence = stats->rkisp1->isp.frame_sequence; 344 u64 timestamp = ktime_get_ns(); 345 346 /* get one empty buffer */ 347 if (!list_empty(&stats->stat)) { 348 cur_buf = list_first_entry(&stats->stat, 349 struct rkisp1_buffer, queue); 350 list_del(&cur_buf->queue); 351 } 352 353 if (!cur_buf) 354 return; 355 356 cur_stat_buf = (struct rkisp1_stat_buffer *) 357 vb2_plane_vaddr(&cur_buf->vb.vb2_buf, 0); 358 if (isp_ris & RKISP1_CIF_ISP_AWB_DONE) 359 stats->ops->get_awb_meas(stats, cur_stat_buf); 360 361 if (isp_ris & RKISP1_CIF_ISP_AFM_FIN) 362 rkisp1_stats_get_afc_meas(stats, cur_stat_buf); 363 364 if (isp_ris & RKISP1_CIF_ISP_EXP_END) { 365 stats->ops->get_aec_meas(stats, cur_stat_buf); 366 rkisp1_stats_get_bls_meas(stats, cur_stat_buf); 367 } 368 369 if (isp_ris & RKISP1_CIF_ISP_HIST_MEASURE_RDY) 370 stats->ops->get_hst_meas(stats, cur_stat_buf); 371 372 vb2_set_plane_payload(&cur_buf->vb.vb2_buf, 0, 373 sizeof(struct rkisp1_stat_buffer)); 374 cur_buf->vb.sequence = frame_sequence; 375 cur_buf->vb.vb2_buf.timestamp = timestamp; 376 vb2_buffer_done(&cur_buf->vb.vb2_buf, VB2_BUF_STATE_DONE); 377 } 378 379 void rkisp1_stats_isr(struct rkisp1_stats *stats, u32 isp_ris) 380 { 381 struct rkisp1_device *rkisp1 = stats->rkisp1; 382 unsigned int isp_mis_tmp = 0; 383 384 spin_lock(&stats->lock); 385 386 rkisp1_write(rkisp1, RKISP1_CIF_ISP_ICR, RKISP1_STATS_MEAS_MASK); 387 388 isp_mis_tmp = rkisp1_read(rkisp1, RKISP1_CIF_ISP_MIS); 389 if (isp_mis_tmp & RKISP1_STATS_MEAS_MASK) 390 rkisp1->debug.stats_error++; 391 392 if (isp_ris & RKISP1_STATS_MEAS_MASK) 393 rkisp1_stats_send_measurement(stats, isp_ris); 394 395 spin_unlock(&stats->lock); 396 } 397 398 static void rkisp1_init_stats(struct rkisp1_stats *stats) 399 { 400 stats->vdev_fmt.fmt.meta.dataformat = 401 V4L2_META_FMT_RK_ISP1_STAT_3A; 402 stats->vdev_fmt.fmt.meta.buffersize = 403 sizeof(struct rkisp1_stat_buffer); 404 405 if (stats->rkisp1->info->isp_ver == RKISP1_V12) 406 stats->ops = &rkisp1_v12_stats_ops; 407 else 408 stats->ops = &rkisp1_v10_stats_ops; 409 } 410 411 int rkisp1_stats_register(struct rkisp1_device *rkisp1) 412 { 413 struct rkisp1_stats *stats = &rkisp1->stats; 414 struct rkisp1_vdev_node *node = &stats->vnode; 415 struct video_device *vdev = &node->vdev; 416 int ret; 417 418 stats->rkisp1 = rkisp1; 419 mutex_init(&node->vlock); 420 INIT_LIST_HEAD(&stats->stat); 421 spin_lock_init(&stats->lock); 422 423 strscpy(vdev->name, RKISP1_STATS_DEV_NAME, sizeof(vdev->name)); 424 425 video_set_drvdata(vdev, stats); 426 vdev->ioctl_ops = &rkisp1_stats_ioctl; 427 vdev->fops = &rkisp1_stats_fops; 428 vdev->release = video_device_release_empty; 429 vdev->lock = &node->vlock; 430 vdev->v4l2_dev = &rkisp1->v4l2_dev; 431 vdev->queue = &node->buf_queue; 432 vdev->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING; 433 vdev->vfl_dir = VFL_DIR_RX; 434 rkisp1_stats_init_vb2_queue(vdev->queue, stats); 435 rkisp1_init_stats(stats); 436 video_set_drvdata(vdev, stats); 437 438 node->pad.flags = MEDIA_PAD_FL_SINK; 439 ret = media_entity_pads_init(&vdev->entity, 1, &node->pad); 440 if (ret) 441 goto error; 442 443 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 444 if (ret) { 445 dev_err(&vdev->dev, 446 "failed to register %s, ret=%d\n", vdev->name, ret); 447 goto error; 448 } 449 450 return 0; 451 452 error: 453 media_entity_cleanup(&vdev->entity); 454 mutex_destroy(&node->vlock); 455 stats->rkisp1 = NULL; 456 return ret; 457 } 458 459 void rkisp1_stats_unregister(struct rkisp1_device *rkisp1) 460 { 461 struct rkisp1_stats *stats = &rkisp1->stats; 462 struct rkisp1_vdev_node *node = &stats->vnode; 463 struct video_device *vdev = &node->vdev; 464 465 if (!stats->rkisp1) 466 return; 467 468 vb2_video_unregister_device(vdev); 469 media_entity_cleanup(&vdev->entity); 470 mutex_destroy(&node->vlock); 471 } 472