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 stats_buf->vaddr = vb2_plane_vaddr(vb, 0); 116 117 spin_lock_irq(&stats_dev->lock); 118 list_add_tail(&stats_buf->queue, &stats_dev->stat); 119 spin_unlock_irq(&stats_dev->lock); 120 } 121 122 static int rkisp1_stats_vb2_buf_prepare(struct vb2_buffer *vb) 123 { 124 if (vb2_plane_size(vb, 0) < sizeof(struct rkisp1_stat_buffer)) 125 return -EINVAL; 126 127 vb2_set_plane_payload(vb, 0, sizeof(struct rkisp1_stat_buffer)); 128 129 return 0; 130 } 131 132 static void rkisp1_stats_vb2_stop_streaming(struct vb2_queue *vq) 133 { 134 struct rkisp1_stats *stats = vq->drv_priv; 135 struct rkisp1_buffer *buf; 136 unsigned int i; 137 138 spin_lock_irq(&stats->lock); 139 for (i = 0; i < RKISP1_ISP_STATS_REQ_BUFS_MAX; i++) { 140 if (list_empty(&stats->stat)) 141 break; 142 buf = list_first_entry(&stats->stat, 143 struct rkisp1_buffer, queue); 144 list_del(&buf->queue); 145 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR); 146 } 147 spin_unlock_irq(&stats->lock); 148 } 149 150 static const struct vb2_ops rkisp1_stats_vb2_ops = { 151 .queue_setup = rkisp1_stats_vb2_queue_setup, 152 .buf_queue = rkisp1_stats_vb2_buf_queue, 153 .buf_prepare = rkisp1_stats_vb2_buf_prepare, 154 .wait_prepare = vb2_ops_wait_prepare, 155 .wait_finish = vb2_ops_wait_finish, 156 .stop_streaming = rkisp1_stats_vb2_stop_streaming, 157 }; 158 159 static int 160 rkisp1_stats_init_vb2_queue(struct vb2_queue *q, struct rkisp1_stats *stats) 161 { 162 struct rkisp1_vdev_node *node; 163 164 node = container_of(q, struct rkisp1_vdev_node, buf_queue); 165 166 q->type = V4L2_BUF_TYPE_META_CAPTURE; 167 q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; 168 q->drv_priv = stats; 169 q->ops = &rkisp1_stats_vb2_ops; 170 q->mem_ops = &vb2_vmalloc_memops; 171 q->buf_struct_size = sizeof(struct rkisp1_buffer); 172 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 173 q->lock = &node->vlock; 174 175 return vb2_queue_init(q); 176 } 177 178 static void rkisp1_stats_get_awb_meas(struct rkisp1_stats *stats, 179 struct rkisp1_stat_buffer *pbuf) 180 { 181 /* Protect against concurrent access from ISR? */ 182 struct rkisp1_device *rkisp1 = stats->rkisp1; 183 u32 reg_val; 184 185 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_AWB; 186 reg_val = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AWB_WHITE_CNT); 187 pbuf->params.awb.awb_mean[0].cnt = 188 RKISP1_CIF_ISP_AWB_GET_PIXEL_CNT(reg_val); 189 reg_val = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AWB_MEAN); 190 191 pbuf->params.awb.awb_mean[0].mean_cr_or_r = 192 RKISP1_CIF_ISP_AWB_GET_MEAN_CR_R(reg_val); 193 pbuf->params.awb.awb_mean[0].mean_cb_or_b = 194 RKISP1_CIF_ISP_AWB_GET_MEAN_CB_B(reg_val); 195 pbuf->params.awb.awb_mean[0].mean_y_or_g = 196 RKISP1_CIF_ISP_AWB_GET_MEAN_Y_G(reg_val); 197 } 198 199 static void rkisp1_stats_get_aec_meas(struct rkisp1_stats *stats, 200 struct rkisp1_stat_buffer *pbuf) 201 { 202 struct rkisp1_device *rkisp1 = stats->rkisp1; 203 unsigned int i; 204 205 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_AUTOEXP; 206 for (i = 0; i < RKISP1_CIF_ISP_AE_MEAN_MAX; i++) 207 pbuf->params.ae.exp_mean[i] = 208 (u8)rkisp1_read(rkisp1, 209 RKISP1_CIF_ISP_EXP_MEAN_00 + i * 4); 210 } 211 212 static void rkisp1_stats_get_afc_meas(struct rkisp1_stats *stats, 213 struct rkisp1_stat_buffer *pbuf) 214 { 215 struct rkisp1_device *rkisp1 = stats->rkisp1; 216 struct rkisp1_cif_isp_af_stat *af; 217 218 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_AFM; 219 220 af = &pbuf->params.af; 221 af->window[0].sum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_SUM_A); 222 af->window[0].lum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_LUM_A); 223 af->window[1].sum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_SUM_B); 224 af->window[1].lum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_LUM_B); 225 af->window[2].sum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_SUM_C); 226 af->window[2].lum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_LUM_C); 227 } 228 229 static void rkisp1_stats_get_hst_meas(struct rkisp1_stats *stats, 230 struct rkisp1_stat_buffer *pbuf) 231 { 232 struct rkisp1_device *rkisp1 = stats->rkisp1; 233 unsigned int i; 234 235 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_HIST; 236 for (i = 0; i < RKISP1_CIF_ISP_HIST_BIN_N_MAX; i++) 237 pbuf->params.hist.hist_bins[i] = 238 (u8)rkisp1_read(rkisp1, 239 RKISP1_CIF_ISP_HIST_BIN_0 + i * 4); 240 } 241 242 static void rkisp1_stats_get_bls_meas(struct rkisp1_stats *stats, 243 struct rkisp1_stat_buffer *pbuf) 244 { 245 struct rkisp1_device *rkisp1 = stats->rkisp1; 246 const struct rkisp1_isp_mbus_info *in_fmt = rkisp1->isp.sink_fmt; 247 struct rkisp1_cif_isp_bls_meas_val *bls_val; 248 249 bls_val = &pbuf->params.ae.bls_val; 250 if (in_fmt->bayer_pat == RKISP1_RAW_BGGR) { 251 bls_val->meas_b = 252 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED); 253 bls_val->meas_gb = 254 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED); 255 bls_val->meas_gr = 256 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED); 257 bls_val->meas_r = 258 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED); 259 } else if (in_fmt->bayer_pat == RKISP1_RAW_GBRG) { 260 bls_val->meas_gb = 261 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED); 262 bls_val->meas_b = 263 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED); 264 bls_val->meas_r = 265 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED); 266 bls_val->meas_gr = 267 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED); 268 } else if (in_fmt->bayer_pat == RKISP1_RAW_GRBG) { 269 bls_val->meas_gr = 270 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED); 271 bls_val->meas_r = 272 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED); 273 bls_val->meas_b = 274 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED); 275 bls_val->meas_gb = 276 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED); 277 } else if (in_fmt->bayer_pat == RKISP1_RAW_RGGB) { 278 bls_val->meas_r = 279 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED); 280 bls_val->meas_gr = 281 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED); 282 bls_val->meas_gb = 283 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED); 284 bls_val->meas_b = 285 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED); 286 } 287 } 288 289 static void 290 rkisp1_stats_send_measurement(struct rkisp1_stats *stats, u32 isp_ris) 291 { 292 struct rkisp1_stat_buffer *cur_stat_buf; 293 struct rkisp1_buffer *cur_buf = NULL; 294 unsigned int frame_sequence = stats->rkisp1->isp.frame_sequence; 295 u64 timestamp = ktime_get_ns(); 296 297 /* get one empty buffer */ 298 if (!list_empty(&stats->stat)) { 299 cur_buf = list_first_entry(&stats->stat, 300 struct rkisp1_buffer, queue); 301 list_del(&cur_buf->queue); 302 } 303 304 if (!cur_buf) 305 return; 306 307 cur_stat_buf = 308 (struct rkisp1_stat_buffer *)(cur_buf->vaddr); 309 310 if (isp_ris & RKISP1_CIF_ISP_AWB_DONE) 311 rkisp1_stats_get_awb_meas(stats, cur_stat_buf); 312 313 if (isp_ris & RKISP1_CIF_ISP_AFM_FIN) 314 rkisp1_stats_get_afc_meas(stats, cur_stat_buf); 315 316 if (isp_ris & RKISP1_CIF_ISP_EXP_END) { 317 rkisp1_stats_get_aec_meas(stats, cur_stat_buf); 318 rkisp1_stats_get_bls_meas(stats, cur_stat_buf); 319 } 320 321 if (isp_ris & RKISP1_CIF_ISP_HIST_MEASURE_RDY) 322 rkisp1_stats_get_hst_meas(stats, cur_stat_buf); 323 324 vb2_set_plane_payload(&cur_buf->vb.vb2_buf, 0, 325 sizeof(struct rkisp1_stat_buffer)); 326 cur_buf->vb.sequence = frame_sequence; 327 cur_buf->vb.vb2_buf.timestamp = timestamp; 328 vb2_buffer_done(&cur_buf->vb.vb2_buf, VB2_BUF_STATE_DONE); 329 } 330 331 void rkisp1_stats_isr(struct rkisp1_stats *stats, u32 isp_ris) 332 { 333 struct rkisp1_device *rkisp1 = stats->rkisp1; 334 unsigned int isp_mis_tmp = 0; 335 336 spin_lock(&stats->lock); 337 338 rkisp1_write(rkisp1, RKISP1_STATS_MEAS_MASK, RKISP1_CIF_ISP_ICR); 339 340 isp_mis_tmp = rkisp1_read(rkisp1, RKISP1_CIF_ISP_MIS); 341 if (isp_mis_tmp & RKISP1_STATS_MEAS_MASK) 342 rkisp1->debug.stats_error++; 343 344 if (isp_ris & RKISP1_STATS_MEAS_MASK) 345 rkisp1_stats_send_measurement(stats, isp_ris); 346 347 spin_unlock(&stats->lock); 348 } 349 350 static void rkisp1_init_stats(struct rkisp1_stats *stats) 351 { 352 stats->vdev_fmt.fmt.meta.dataformat = 353 V4L2_META_FMT_RK_ISP1_STAT_3A; 354 stats->vdev_fmt.fmt.meta.buffersize = 355 sizeof(struct rkisp1_stat_buffer); 356 } 357 358 int rkisp1_stats_register(struct rkisp1_device *rkisp1) 359 { 360 struct rkisp1_stats *stats = &rkisp1->stats; 361 struct rkisp1_vdev_node *node = &stats->vnode; 362 struct video_device *vdev = &node->vdev; 363 int ret; 364 365 stats->rkisp1 = rkisp1; 366 mutex_init(&node->vlock); 367 INIT_LIST_HEAD(&stats->stat); 368 spin_lock_init(&stats->lock); 369 370 strscpy(vdev->name, RKISP1_STATS_DEV_NAME, sizeof(vdev->name)); 371 372 video_set_drvdata(vdev, stats); 373 vdev->ioctl_ops = &rkisp1_stats_ioctl; 374 vdev->fops = &rkisp1_stats_fops; 375 vdev->release = video_device_release_empty; 376 vdev->lock = &node->vlock; 377 vdev->v4l2_dev = &rkisp1->v4l2_dev; 378 vdev->queue = &node->buf_queue; 379 vdev->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING; 380 vdev->vfl_dir = VFL_DIR_RX; 381 rkisp1_stats_init_vb2_queue(vdev->queue, stats); 382 rkisp1_init_stats(stats); 383 video_set_drvdata(vdev, stats); 384 385 node->pad.flags = MEDIA_PAD_FL_SINK; 386 ret = media_entity_pads_init(&vdev->entity, 1, &node->pad); 387 if (ret) 388 goto err_mutex_destroy; 389 390 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 391 if (ret) { 392 dev_err(&vdev->dev, 393 "failed to register %s, ret=%d\n", vdev->name, ret); 394 goto err_cleanup_media_entity; 395 } 396 397 return 0; 398 399 err_cleanup_media_entity: 400 media_entity_cleanup(&vdev->entity); 401 err_mutex_destroy: 402 mutex_destroy(&node->vlock); 403 return ret; 404 } 405 406 void rkisp1_stats_unregister(struct rkisp1_device *rkisp1) 407 { 408 struct rkisp1_stats *stats = &rkisp1->stats; 409 struct rkisp1_vdev_node *node = &stats->vnode; 410 struct video_device *vdev = &node->vdev; 411 412 vb2_video_unregister_device(vdev); 413 media_entity_cleanup(&vdev->entity); 414 mutex_destroy(&node->vlock); 415 } 416