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_V10; 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_V10; i++) { 237 u32 reg_val = rkisp1_read(rkisp1, RKISP1_CIF_ISP_HIST_BIN_0 + i * 4); 238 239 pbuf->params.hist.hist_bins[i] = RKISP1_CIF_ISP_HIST_GET_BIN(reg_val); 240 } 241 } 242 243 static void rkisp1_stats_get_bls_meas(struct rkisp1_stats *stats, 244 struct rkisp1_stat_buffer *pbuf) 245 { 246 struct rkisp1_device *rkisp1 = stats->rkisp1; 247 const struct rkisp1_isp_mbus_info *in_fmt = rkisp1->isp.sink_fmt; 248 struct rkisp1_cif_isp_bls_meas_val *bls_val; 249 250 bls_val = &pbuf->params.ae.bls_val; 251 if (in_fmt->bayer_pat == RKISP1_RAW_BGGR) { 252 bls_val->meas_b = 253 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED); 254 bls_val->meas_gb = 255 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED); 256 bls_val->meas_gr = 257 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED); 258 bls_val->meas_r = 259 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED); 260 } else if (in_fmt->bayer_pat == RKISP1_RAW_GBRG) { 261 bls_val->meas_gb = 262 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED); 263 bls_val->meas_b = 264 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED); 265 bls_val->meas_r = 266 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED); 267 bls_val->meas_gr = 268 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED); 269 } else if (in_fmt->bayer_pat == RKISP1_RAW_GRBG) { 270 bls_val->meas_gr = 271 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED); 272 bls_val->meas_r = 273 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED); 274 bls_val->meas_b = 275 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED); 276 bls_val->meas_gb = 277 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED); 278 } else if (in_fmt->bayer_pat == RKISP1_RAW_RGGB) { 279 bls_val->meas_r = 280 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_A_MEASURED); 281 bls_val->meas_gr = 282 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_B_MEASURED); 283 bls_val->meas_gb = 284 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_C_MEASURED); 285 bls_val->meas_b = 286 rkisp1_read(rkisp1, RKISP1_CIF_ISP_BLS_D_MEASURED); 287 } 288 } 289 290 static void 291 rkisp1_stats_send_measurement(struct rkisp1_stats *stats, u32 isp_ris) 292 { 293 struct rkisp1_stat_buffer *cur_stat_buf; 294 struct rkisp1_buffer *cur_buf = NULL; 295 unsigned int frame_sequence = stats->rkisp1->isp.frame_sequence; 296 u64 timestamp = ktime_get_ns(); 297 298 /* get one empty buffer */ 299 if (!list_empty(&stats->stat)) { 300 cur_buf = list_first_entry(&stats->stat, 301 struct rkisp1_buffer, queue); 302 list_del(&cur_buf->queue); 303 } 304 305 if (!cur_buf) 306 return; 307 308 cur_stat_buf = 309 (struct rkisp1_stat_buffer *)(cur_buf->vaddr); 310 311 if (isp_ris & RKISP1_CIF_ISP_AWB_DONE) 312 rkisp1_stats_get_awb_meas(stats, cur_stat_buf); 313 314 if (isp_ris & RKISP1_CIF_ISP_AFM_FIN) 315 rkisp1_stats_get_afc_meas(stats, cur_stat_buf); 316 317 if (isp_ris & RKISP1_CIF_ISP_EXP_END) { 318 rkisp1_stats_get_aec_meas(stats, cur_stat_buf); 319 rkisp1_stats_get_bls_meas(stats, cur_stat_buf); 320 } 321 322 if (isp_ris & RKISP1_CIF_ISP_HIST_MEASURE_RDY) 323 rkisp1_stats_get_hst_meas(stats, cur_stat_buf); 324 325 vb2_set_plane_payload(&cur_buf->vb.vb2_buf, 0, 326 sizeof(struct rkisp1_stat_buffer)); 327 cur_buf->vb.sequence = frame_sequence; 328 cur_buf->vb.vb2_buf.timestamp = timestamp; 329 vb2_buffer_done(&cur_buf->vb.vb2_buf, VB2_BUF_STATE_DONE); 330 } 331 332 void rkisp1_stats_isr(struct rkisp1_stats *stats, u32 isp_ris) 333 { 334 struct rkisp1_device *rkisp1 = stats->rkisp1; 335 unsigned int isp_mis_tmp = 0; 336 337 spin_lock(&stats->lock); 338 339 rkisp1_write(rkisp1, RKISP1_STATS_MEAS_MASK, RKISP1_CIF_ISP_ICR); 340 341 isp_mis_tmp = rkisp1_read(rkisp1, RKISP1_CIF_ISP_MIS); 342 if (isp_mis_tmp & RKISP1_STATS_MEAS_MASK) 343 rkisp1->debug.stats_error++; 344 345 if (isp_ris & RKISP1_STATS_MEAS_MASK) 346 rkisp1_stats_send_measurement(stats, isp_ris); 347 348 spin_unlock(&stats->lock); 349 } 350 351 static void rkisp1_init_stats(struct rkisp1_stats *stats) 352 { 353 stats->vdev_fmt.fmt.meta.dataformat = 354 V4L2_META_FMT_RK_ISP1_STAT_3A; 355 stats->vdev_fmt.fmt.meta.buffersize = 356 sizeof(struct rkisp1_stat_buffer); 357 } 358 359 int rkisp1_stats_register(struct rkisp1_device *rkisp1) 360 { 361 struct rkisp1_stats *stats = &rkisp1->stats; 362 struct rkisp1_vdev_node *node = &stats->vnode; 363 struct video_device *vdev = &node->vdev; 364 int ret; 365 366 stats->rkisp1 = rkisp1; 367 mutex_init(&node->vlock); 368 INIT_LIST_HEAD(&stats->stat); 369 spin_lock_init(&stats->lock); 370 371 strscpy(vdev->name, RKISP1_STATS_DEV_NAME, sizeof(vdev->name)); 372 373 video_set_drvdata(vdev, stats); 374 vdev->ioctl_ops = &rkisp1_stats_ioctl; 375 vdev->fops = &rkisp1_stats_fops; 376 vdev->release = video_device_release_empty; 377 vdev->lock = &node->vlock; 378 vdev->v4l2_dev = &rkisp1->v4l2_dev; 379 vdev->queue = &node->buf_queue; 380 vdev->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING; 381 vdev->vfl_dir = VFL_DIR_RX; 382 rkisp1_stats_init_vb2_queue(vdev->queue, stats); 383 rkisp1_init_stats(stats); 384 video_set_drvdata(vdev, stats); 385 386 node->pad.flags = MEDIA_PAD_FL_SINK; 387 ret = media_entity_pads_init(&vdev->entity, 1, &node->pad); 388 if (ret) 389 goto err_mutex_destroy; 390 391 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 392 if (ret) { 393 dev_err(&vdev->dev, 394 "failed to register %s, ret=%d\n", vdev->name, ret); 395 goto err_cleanup_media_entity; 396 } 397 398 return 0; 399 400 err_cleanup_media_entity: 401 media_entity_cleanup(&vdev->entity); 402 err_mutex_destroy: 403 mutex_destroy(&node->vlock); 404 return ret; 405 } 406 407 void rkisp1_stats_unregister(struct rkisp1_device *rkisp1) 408 { 409 struct rkisp1_stats *stats = &rkisp1->stats; 410 struct rkisp1_vdev_node *node = &stats->vnode; 411 struct video_device *vdev = &node->vdev; 412 413 vb2_video_unregister_device(vdev); 414 media_entity_cleanup(&vdev->entity); 415 mutex_destroy(&node->vlock); 416 } 417