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 .wait_prepare = vb2_ops_wait_prepare, 154 .wait_finish = vb2_ops_wait_finish, 155 .stop_streaming = rkisp1_stats_vb2_stop_streaming, 156 }; 157 158 static int 159 rkisp1_stats_init_vb2_queue(struct vb2_queue *q, struct rkisp1_stats *stats) 160 { 161 struct rkisp1_vdev_node *node; 162 163 node = container_of(q, struct rkisp1_vdev_node, buf_queue); 164 165 q->type = V4L2_BUF_TYPE_META_CAPTURE; 166 q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_DMABUF; 167 q->drv_priv = stats; 168 q->ops = &rkisp1_stats_vb2_ops; 169 q->mem_ops = &vb2_vmalloc_memops; 170 q->buf_struct_size = sizeof(struct rkisp1_buffer); 171 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC; 172 q->lock = &node->vlock; 173 174 return vb2_queue_init(q); 175 } 176 177 static void rkisp1_stats_get_awb_meas(struct rkisp1_stats *stats, 178 struct rkisp1_stat_buffer *pbuf) 179 { 180 /* Protect against concurrent access from ISR? */ 181 struct rkisp1_device *rkisp1 = stats->rkisp1; 182 u32 reg_val; 183 184 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_AWB; 185 reg_val = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AWB_WHITE_CNT); 186 pbuf->params.awb.awb_mean[0].cnt = 187 RKISP1_CIF_ISP_AWB_GET_PIXEL_CNT(reg_val); 188 reg_val = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AWB_MEAN); 189 190 pbuf->params.awb.awb_mean[0].mean_cr_or_r = 191 RKISP1_CIF_ISP_AWB_GET_MEAN_CR_R(reg_val); 192 pbuf->params.awb.awb_mean[0].mean_cb_or_b = 193 RKISP1_CIF_ISP_AWB_GET_MEAN_CB_B(reg_val); 194 pbuf->params.awb.awb_mean[0].mean_y_or_g = 195 RKISP1_CIF_ISP_AWB_GET_MEAN_Y_G(reg_val); 196 } 197 198 static void rkisp1_stats_get_aec_meas(struct rkisp1_stats *stats, 199 struct rkisp1_stat_buffer *pbuf) 200 { 201 struct rkisp1_device *rkisp1 = stats->rkisp1; 202 unsigned int i; 203 204 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_AUTOEXP; 205 for (i = 0; i < RKISP1_CIF_ISP_AE_MEAN_MAX_V10; i++) 206 pbuf->params.ae.exp_mean[i] = 207 (u8)rkisp1_read(rkisp1, 208 RKISP1_CIF_ISP_EXP_MEAN_00 + i * 4); 209 } 210 211 static void rkisp1_stats_get_afc_meas(struct rkisp1_stats *stats, 212 struct rkisp1_stat_buffer *pbuf) 213 { 214 struct rkisp1_device *rkisp1 = stats->rkisp1; 215 struct rkisp1_cif_isp_af_stat *af; 216 217 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_AFM; 218 219 af = &pbuf->params.af; 220 af->window[0].sum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_SUM_A); 221 af->window[0].lum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_LUM_A); 222 af->window[1].sum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_SUM_B); 223 af->window[1].lum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_LUM_B); 224 af->window[2].sum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_SUM_C); 225 af->window[2].lum = rkisp1_read(rkisp1, RKISP1_CIF_ISP_AFM_LUM_C); 226 } 227 228 static void rkisp1_stats_get_hst_meas(struct rkisp1_stats *stats, 229 struct rkisp1_stat_buffer *pbuf) 230 { 231 struct rkisp1_device *rkisp1 = stats->rkisp1; 232 unsigned int i; 233 234 pbuf->meas_type |= RKISP1_CIF_ISP_STAT_HIST; 235 for (i = 0; i < RKISP1_CIF_ISP_HIST_BIN_N_MAX_V10; i++) { 236 u32 reg_val = rkisp1_read(rkisp1, RKISP1_CIF_ISP_HIST_BIN_0 + i * 4); 237 238 pbuf->params.hist.hist_bins[i] = RKISP1_CIF_ISP_HIST_GET_BIN(reg_val); 239 } 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 = (struct rkisp1_stat_buffer *) 308 vb2_plane_vaddr(&cur_buf->vb.vb2_buf, 0); 309 if (isp_ris & RKISP1_CIF_ISP_AWB_DONE) 310 rkisp1_stats_get_awb_meas(stats, cur_stat_buf); 311 312 if (isp_ris & RKISP1_CIF_ISP_AFM_FIN) 313 rkisp1_stats_get_afc_meas(stats, cur_stat_buf); 314 315 if (isp_ris & RKISP1_CIF_ISP_EXP_END) { 316 rkisp1_stats_get_aec_meas(stats, cur_stat_buf); 317 rkisp1_stats_get_bls_meas(stats, cur_stat_buf); 318 } 319 320 if (isp_ris & RKISP1_CIF_ISP_HIST_MEASURE_RDY) 321 rkisp1_stats_get_hst_meas(stats, cur_stat_buf); 322 323 vb2_set_plane_payload(&cur_buf->vb.vb2_buf, 0, 324 sizeof(struct rkisp1_stat_buffer)); 325 cur_buf->vb.sequence = frame_sequence; 326 cur_buf->vb.vb2_buf.timestamp = timestamp; 327 vb2_buffer_done(&cur_buf->vb.vb2_buf, VB2_BUF_STATE_DONE); 328 } 329 330 void rkisp1_stats_isr(struct rkisp1_stats *stats, u32 isp_ris) 331 { 332 struct rkisp1_device *rkisp1 = stats->rkisp1; 333 unsigned int isp_mis_tmp = 0; 334 335 spin_lock(&stats->lock); 336 337 rkisp1_write(rkisp1, RKISP1_STATS_MEAS_MASK, RKISP1_CIF_ISP_ICR); 338 339 isp_mis_tmp = rkisp1_read(rkisp1, RKISP1_CIF_ISP_MIS); 340 if (isp_mis_tmp & RKISP1_STATS_MEAS_MASK) 341 rkisp1->debug.stats_error++; 342 343 if (isp_ris & RKISP1_STATS_MEAS_MASK) 344 rkisp1_stats_send_measurement(stats, isp_ris); 345 346 spin_unlock(&stats->lock); 347 } 348 349 static void rkisp1_init_stats(struct rkisp1_stats *stats) 350 { 351 stats->vdev_fmt.fmt.meta.dataformat = 352 V4L2_META_FMT_RK_ISP1_STAT_3A; 353 stats->vdev_fmt.fmt.meta.buffersize = 354 sizeof(struct rkisp1_stat_buffer); 355 } 356 357 int rkisp1_stats_register(struct rkisp1_device *rkisp1) 358 { 359 struct rkisp1_stats *stats = &rkisp1->stats; 360 struct rkisp1_vdev_node *node = &stats->vnode; 361 struct video_device *vdev = &node->vdev; 362 int ret; 363 364 stats->rkisp1 = rkisp1; 365 mutex_init(&node->vlock); 366 INIT_LIST_HEAD(&stats->stat); 367 spin_lock_init(&stats->lock); 368 369 strscpy(vdev->name, RKISP1_STATS_DEV_NAME, sizeof(vdev->name)); 370 371 video_set_drvdata(vdev, stats); 372 vdev->ioctl_ops = &rkisp1_stats_ioctl; 373 vdev->fops = &rkisp1_stats_fops; 374 vdev->release = video_device_release_empty; 375 vdev->lock = &node->vlock; 376 vdev->v4l2_dev = &rkisp1->v4l2_dev; 377 vdev->queue = &node->buf_queue; 378 vdev->device_caps = V4L2_CAP_META_CAPTURE | V4L2_CAP_STREAMING; 379 vdev->vfl_dir = VFL_DIR_RX; 380 rkisp1_stats_init_vb2_queue(vdev->queue, stats); 381 rkisp1_init_stats(stats); 382 video_set_drvdata(vdev, stats); 383 384 node->pad.flags = MEDIA_PAD_FL_SINK; 385 ret = media_entity_pads_init(&vdev->entity, 1, &node->pad); 386 if (ret) 387 goto err_mutex_destroy; 388 389 ret = video_register_device(vdev, VFL_TYPE_VIDEO, -1); 390 if (ret) { 391 dev_err(&vdev->dev, 392 "failed to register %s, ret=%d\n", vdev->name, ret); 393 goto err_cleanup_media_entity; 394 } 395 396 return 0; 397 398 err_cleanup_media_entity: 399 media_entity_cleanup(&vdev->entity); 400 err_mutex_destroy: 401 mutex_destroy(&node->vlock); 402 return ret; 403 } 404 405 void rkisp1_stats_unregister(struct rkisp1_device *rkisp1) 406 { 407 struct rkisp1_stats *stats = &rkisp1->stats; 408 struct rkisp1_vdev_node *node = &stats->vnode; 409 struct video_device *vdev = &node->vdev; 410 411 vb2_video_unregister_device(vdev); 412 media_entity_cleanup(&vdev->entity); 413 mutex_destroy(&node->vlock); 414 } 415