1 // SPDX-License-Identifier: GPL-2.0+ 2 /* 3 * Copyright (C) 2025 Advanced Micro Devices, Inc. 4 */ 5 6 #include <linux/pm_domain.h> 7 #include <linux/units.h> 8 9 #include "isp4.h" 10 #include "isp4_debug.h" 11 #include "isp4_fw_cmd_resp.h" 12 #include "isp4_interface.h" 13 14 #define ISP4SD_MIN_BUF_CNT_BEF_START_STREAM 4 15 16 #define ISP4SD_PERFORMANCE_STATE_LOW 0 17 #define ISP4SD_PERFORMANCE_STATE_HIGH 1 18 19 /* align 32KB */ 20 #define ISP4SD_META_BUF_SIZE ALIGN(sizeof(struct isp4fw_meta_info), 0x8000) 21 22 #define to_isp4_subdev(sd) container_of(sd, struct isp4_subdev, sdev) 23 24 static const char *isp4sd_entity_name = "amd isp4"; 25 26 static const char *isp4sd_thread_name[ISP4SD_MAX_FW_RESP_STREAM_NUM] = { 27 "amd_isp4_thread_global", 28 "amd_isp4_thread_stream1", 29 }; 30 31 static void isp4sd_module_enable(struct isp4_subdev *isp_subdev, bool enable) 32 { 33 if (isp_subdev->enable_gpio) { 34 gpiod_set_value(isp_subdev->enable_gpio, enable ? 1 : 0); 35 dev_dbg(isp_subdev->dev, "%s isp_subdev module\n", 36 enable ? "enable" : "disable"); 37 } 38 } 39 40 static int isp4sd_setup_fw_mem_pool(struct isp4_subdev *isp_subdev) 41 { 42 struct isp4_interface *ispif = &isp_subdev->ispif; 43 struct isp4fw_cmd_send_buffer buf_type; 44 struct device *dev = isp_subdev->dev; 45 int ret; 46 47 if (!ispif->fw_mem_pool) { 48 dev_err(dev, "fail to alloc mem pool\n"); 49 return -ENOMEM; 50 } 51 52 /* 53 * The struct will be shared with ISP FW, use memset() to guarantee 54 * padding bits are zeroed, since this is not guaranteed on all 55 * compilers. 56 */ 57 memset(&buf_type, 0, sizeof(buf_type)); 58 buf_type.buffer_type = ISP4FW_BUFFER_TYPE_MEM_POOL; 59 buf_type.buffer.vmid_space.bit.space = ISP4FW_ADDR_SPACE_TYPE_GPU_VA; 60 isp4if_split_addr64(ispif->fw_mem_pool->gpu_mc_addr, 61 &buf_type.buffer.buf_base_a_lo, 62 &buf_type.buffer.buf_base_a_hi); 63 buf_type.buffer.buf_size_a = ispif->fw_mem_pool->mem_size; 64 65 ret = isp4if_send_command(ispif, ISP4FW_CMD_ID_SEND_BUFFER, 66 &buf_type, sizeof(buf_type)); 67 if (ret) { 68 dev_err(dev, "send fw mem pool 0x%llx(%u) fail %d\n", 69 ispif->fw_mem_pool->gpu_mc_addr, 70 buf_type.buffer.buf_size_a, ret); 71 return ret; 72 } 73 74 dev_dbg(dev, "send fw mem pool 0x%llx(%u) suc\n", 75 ispif->fw_mem_pool->gpu_mc_addr, buf_type.buffer.buf_size_a); 76 77 return 0; 78 } 79 80 static int isp4sd_set_stream_path(struct isp4_subdev *isp_subdev) 81 { 82 struct isp4_interface *ispif = &isp_subdev->ispif; 83 struct isp4fw_cmd_set_stream_cfg cmd; 84 struct device *dev = isp_subdev->dev; 85 86 /* 87 * The struct will be shared with ISP FW, use memset() to guarantee 88 * padding bits are zeroed, since this is not guaranteed on all 89 * compilers. 90 */ 91 memset(&cmd, 0, sizeof(cmd)); 92 cmd.stream_cfg.mipi_pipe_path_cfg.isp4fw_sensor_id = 93 ISP4FW_SENSOR_ID_ON_MIPI0; 94 cmd.stream_cfg.mipi_pipe_path_cfg.b_enable = true; 95 cmd.stream_cfg.isp_pipe_path_cfg.isp_pipe_id = 96 ISP4FW_MIPI0_ISP_PIPELINE_ID; 97 98 cmd.stream_cfg.b_enable_tnr = true; 99 dev_dbg(dev, "isp4fw_sensor_id %d, pipeId 0x%x EnableTnr %u\n", 100 cmd.stream_cfg.mipi_pipe_path_cfg.isp4fw_sensor_id, 101 cmd.stream_cfg.isp_pipe_path_cfg.isp_pipe_id, 102 cmd.stream_cfg.b_enable_tnr); 103 104 return isp4if_send_command(ispif, ISP4FW_CMD_ID_SET_STREAM_CONFIG, 105 &cmd, sizeof(cmd)); 106 } 107 108 static int isp4sd_send_meta_buf(struct isp4_subdev *isp_subdev) 109 { 110 struct isp4_interface *ispif = &isp_subdev->ispif; 111 struct isp4fw_cmd_send_buffer buf_type; 112 struct device *dev = isp_subdev->dev; 113 114 /* 115 * The struct will be shared with ISP FW, use memset() to guarantee 116 * padding bits are zeroed, since this is not guaranteed on all 117 * compilers. 118 */ 119 memset(&buf_type, 0, sizeof(buf_type)); 120 for (unsigned int i = 0; i < ISP4IF_MAX_STREAM_BUF_COUNT; i++) { 121 struct isp4if_gpu_mem_info *meta_info_buf = 122 isp_subdev->ispif.meta_info_buf[i]; 123 int ret; 124 125 if (!meta_info_buf) { 126 dev_err(dev, "fail for no meta info buf(%u)\n", i); 127 return -ENOMEM; 128 } 129 130 buf_type.buffer_type = ISP4FW_BUFFER_TYPE_META_INFO; 131 buf_type.buffer.vmid_space.bit.space = 132 ISP4FW_ADDR_SPACE_TYPE_GPU_VA; 133 isp4if_split_addr64(meta_info_buf->gpu_mc_addr, 134 &buf_type.buffer.buf_base_a_lo, 135 &buf_type.buffer.buf_base_a_hi); 136 buf_type.buffer.buf_size_a = meta_info_buf->mem_size; 137 ret = isp4if_send_command(ispif, ISP4FW_CMD_ID_SEND_BUFFER, 138 &buf_type, sizeof(buf_type)); 139 if (ret) { 140 dev_err(dev, "send meta info(%u) fail\n", i); 141 return ret; 142 } 143 } 144 145 dev_dbg(dev, "send meta info suc\n"); 146 return 0; 147 } 148 149 static bool isp4sd_get_str_out_prop(struct isp4_subdev *isp_subdev, 150 struct isp4fw_image_prop *out_prop, 151 struct v4l2_subdev_state *state, u32 pad) 152 { 153 struct device *dev = isp_subdev->dev; 154 struct v4l2_mbus_framefmt *format; 155 156 format = v4l2_subdev_state_get_format(state, pad, 0); 157 if (!format) { 158 dev_err(dev, "fail get subdev state format\n"); 159 return false; 160 } 161 162 switch (format->code) { 163 case MEDIA_BUS_FMT_YUYV8_1_5X8: 164 out_prop->image_format = ISP4FW_IMAGE_FORMAT_NV12; 165 out_prop->width = format->width; 166 out_prop->height = format->height; 167 out_prop->luma_pitch = format->width; 168 out_prop->chroma_pitch = out_prop->width; 169 break; 170 case MEDIA_BUS_FMT_YUYV8_1X16: 171 out_prop->image_format = ISP4FW_IMAGE_FORMAT_YUV422INTERLEAVED; 172 out_prop->width = format->width; 173 out_prop->height = format->height; 174 out_prop->luma_pitch = format->width * 2; 175 out_prop->chroma_pitch = 0; 176 break; 177 default: 178 dev_err(dev, "fail for bad image format:0x%x\n", 179 format->code); 180 return false; 181 } 182 183 if (!out_prop->width || !out_prop->height) 184 return false; 185 186 return true; 187 } 188 189 static int isp4sd_kickoff_stream(struct isp4_subdev *isp_subdev, u32 w, u32 h) 190 { 191 struct isp4sd_sensor_info *sensor_info = &isp_subdev->sensor_info; 192 struct isp4_interface *ispif = &isp_subdev->ispif; 193 struct device *dev = isp_subdev->dev; 194 195 if (sensor_info->status == ISP4SD_START_STATUS_STARTED) 196 return 0; 197 198 if (sensor_info->status == ISP4SD_START_STATUS_START_FAIL) { 199 dev_err(dev, "fail for previous start fail\n"); 200 return -EINVAL; 201 } 202 203 dev_dbg(dev, "w:%u,h:%u\n", w, h); 204 205 if (isp4sd_send_meta_buf(isp_subdev)) { 206 dev_err(dev, "fail to send meta buf\n"); 207 sensor_info->status = ISP4SD_START_STATUS_START_FAIL; 208 return -EINVAL; 209 } 210 211 sensor_info->status = ISP4SD_START_STATUS_OFF; 212 213 if (!sensor_info->start_stream_cmd_sent && 214 sensor_info->buf_sent_cnt >= ISP4SD_MIN_BUF_CNT_BEF_START_STREAM) { 215 int ret = isp4if_send_command(ispif, ISP4FW_CMD_ID_START_STREAM, 216 NULL, 0); 217 if (ret) { 218 dev_err(dev, "fail to start stream\n"); 219 return ret; 220 } 221 222 sensor_info->start_stream_cmd_sent = true; 223 } else { 224 dev_dbg(dev, 225 "no send START_STREAM, start_sent %u, buf_sent %u\n", 226 sensor_info->start_stream_cmd_sent, 227 sensor_info->buf_sent_cnt); 228 } 229 230 return 0; 231 } 232 233 static int isp4sd_setup_output(struct isp4_subdev *isp_subdev, 234 struct v4l2_subdev_state *state, u32 pad) 235 { 236 struct isp4sd_output_info *output_info = 237 &isp_subdev->sensor_info.output_info; 238 struct isp4sd_sensor_info *sensor_info = &isp_subdev->sensor_info; 239 struct isp4_interface *ispif = &isp_subdev->ispif; 240 struct isp4fw_cmd_set_out_ch_prop cmd_ch_prop; 241 struct isp4fw_cmd_enable_out_ch cmd_ch_en; 242 struct device *dev = isp_subdev->dev; 243 int ret; 244 245 if (output_info->start_status == ISP4SD_START_STATUS_STARTED) 246 return 0; 247 248 if (output_info->start_status == ISP4SD_START_STATUS_START_FAIL) { 249 dev_err(dev, "fail for previous start fail\n"); 250 return -EINVAL; 251 } 252 253 /* 254 * The struct will be shared with ISP FW, use memset() to guarantee 255 * padding bits are zeroed, since this is not guaranteed on all 256 * compilers. 257 */ 258 memset(&cmd_ch_prop, 0, sizeof(cmd_ch_prop)); 259 cmd_ch_prop.ch = ISP4FW_ISP_PIPE_OUT_CH_PREVIEW; 260 261 if (!isp4sd_get_str_out_prop(isp_subdev, 262 &cmd_ch_prop.image_prop, state, pad)) { 263 dev_err(dev, "fail to get out prop\n"); 264 return -EINVAL; 265 } 266 267 dev_dbg(dev, "channel:%s,fmt %s,w:h=%u:%u,lp:%u,cp%u\n", 268 isp4dbg_get_out_ch_str(cmd_ch_prop.ch), 269 isp4dbg_get_img_fmt_str(cmd_ch_prop.image_prop.image_format), 270 cmd_ch_prop.image_prop.width, cmd_ch_prop.image_prop.height, 271 cmd_ch_prop.image_prop.luma_pitch, 272 cmd_ch_prop.image_prop.chroma_pitch); 273 274 ret = isp4if_send_command(ispif, ISP4FW_CMD_ID_SET_OUT_CHAN_PROP, 275 &cmd_ch_prop, sizeof(cmd_ch_prop)); 276 if (ret) { 277 output_info->start_status = ISP4SD_START_STATUS_START_FAIL; 278 dev_err(dev, "fail to set out prop\n"); 279 return ret; 280 } 281 282 /* 283 * The struct will be shared with ISP FW, use memset() to guarantee 284 * padding bits are zeroed, since this is not guaranteed on all 285 * compilers. 286 */ 287 memset(&cmd_ch_en, 0, sizeof(cmd_ch_en)); 288 cmd_ch_en.ch = ISP4FW_ISP_PIPE_OUT_CH_PREVIEW; 289 cmd_ch_en.is_enable = true; 290 ret = isp4if_send_command(ispif, ISP4FW_CMD_ID_ENABLE_OUT_CHAN, 291 &cmd_ch_en, sizeof(cmd_ch_en)); 292 if (ret) { 293 output_info->start_status = ISP4SD_START_STATUS_START_FAIL; 294 dev_err(dev, "fail to enable channel\n"); 295 return ret; 296 } 297 298 dev_dbg(dev, "enable channel %s\n", 299 isp4dbg_get_out_ch_str(cmd_ch_en.ch)); 300 301 if (!sensor_info->start_stream_cmd_sent) { 302 ret = isp4sd_kickoff_stream(isp_subdev, 303 cmd_ch_prop.image_prop.width, 304 cmd_ch_prop.image_prop.height); 305 if (ret) { 306 dev_err(dev, "kickoff stream fail %d\n", ret); 307 return ret; 308 } 309 /* 310 * sensor_info->start_stream_cmd_sent will be set to true 311 * 1. in isp4sd_kickoff_stream, if app first send buffer then 312 * start stream 313 * 2. in isp_set_stream_buf, if app first start stream, then 314 * send buffer because ISP FW has the requirement, host needs 315 * to send buffer before send start stream cmd 316 */ 317 if (sensor_info->start_stream_cmd_sent) { 318 sensor_info->status = ISP4SD_START_STATUS_STARTED; 319 output_info->start_status = ISP4SD_START_STATUS_STARTED; 320 dev_dbg(dev, "kickoff stream suc,start cmd sent\n"); 321 } 322 } else { 323 dev_dbg(dev, "stream running, no need kickoff\n"); 324 output_info->start_status = ISP4SD_START_STATUS_STARTED; 325 } 326 327 dev_dbg(dev, "setup output suc\n"); 328 return 0; 329 } 330 331 static int isp4sd_init_stream(struct isp4_subdev *isp_subdev) 332 { 333 struct device *dev = isp_subdev->dev; 334 int ret; 335 336 ret = isp4sd_setup_fw_mem_pool(isp_subdev); 337 if (ret) { 338 dev_err(dev, "fail to setup fw mem pool\n"); 339 return ret; 340 } 341 342 ret = isp4sd_set_stream_path(isp_subdev); 343 if (ret) { 344 dev_err(dev, "fail to setup stream path\n"); 345 return ret; 346 } 347 348 return 0; 349 } 350 351 static void isp4sd_uninit_stream(struct isp4_subdev *isp_subdev, 352 struct v4l2_subdev_state *state, u32 pad) 353 { 354 struct isp4sd_sensor_info *sensor_info = &isp_subdev->sensor_info; 355 struct isp4sd_output_info *output_info = &sensor_info->output_info; 356 struct isp4_interface *ispif = &isp_subdev->ispif; 357 struct v4l2_mbus_framefmt *format; 358 359 format = v4l2_subdev_state_get_format(state, pad, 0); 360 if (!format) { 361 dev_err(isp_subdev->dev, "fail to get v4l2 format\n"); 362 } else { 363 memset(format, 0, sizeof(*format)); 364 format->code = MEDIA_BUS_FMT_YUYV8_1_5X8; 365 } 366 367 isp4if_clear_bufq(ispif); 368 isp4if_clear_cmdq(ispif); 369 370 sensor_info->start_stream_cmd_sent = false; 371 sensor_info->buf_sent_cnt = 0; 372 373 sensor_info->status = ISP4SD_START_STATUS_OFF; 374 output_info->start_status = ISP4SD_START_STATUS_OFF; 375 } 376 377 static void isp4sd_fw_resp_cmd_done(struct isp4_subdev *isp_subdev, 378 enum isp4if_stream_id stream_id, 379 struct isp4fw_resp_cmd_done *para) 380 { 381 struct isp4_interface *ispif = &isp_subdev->ispif; 382 struct isp4if_cmd_element *ele = 383 isp4if_rm_cmd_from_cmdq(ispif, para->cmd_seq_num, para->cmd_id); 384 struct device *dev = isp_subdev->dev; 385 386 dev_dbg(dev, "stream %d,cmd %s(0x%08x)(%d),seq %u, ele %p\n", 387 stream_id, 388 isp4dbg_get_cmd_str(para->cmd_id), 389 para->cmd_id, para->cmd_status, para->cmd_seq_num, 390 ele); 391 392 if (ele) { 393 complete(&ele->cmd_done); 394 if (atomic_dec_and_test(&ele->refcnt)) 395 kfree(ele); 396 } 397 } 398 399 static struct isp4fw_meta_info * 400 isp4sd_get_meta_by_mc(struct isp4_subdev *isp_subdev, u64 mc) 401 { 402 for (unsigned int i = 0; i < ISP4IF_MAX_STREAM_BUF_COUNT; i++) { 403 struct isp4if_gpu_mem_info *meta_info_buf = 404 isp_subdev->ispif.meta_info_buf[i]; 405 406 if (meta_info_buf->gpu_mc_addr == mc) 407 return meta_info_buf->sys_addr; 408 } 409 410 return NULL; 411 } 412 413 static void isp4sd_send_meta_info(struct isp4_subdev *isp_subdev, 414 u64 meta_info_mc) 415 { 416 struct isp4_interface *ispif = &isp_subdev->ispif; 417 struct isp4fw_cmd_send_buffer buf_type; 418 struct device *dev = isp_subdev->dev; 419 420 if (isp_subdev->sensor_info.status != ISP4SD_START_STATUS_STARTED) { 421 dev_warn(dev, "not working status %i, meta_info 0x%llx\n", 422 isp_subdev->sensor_info.status, meta_info_mc); 423 return; 424 } 425 426 /* 427 * The struct will be shared with ISP FW, use memset() to guarantee 428 * padding bits are zeroed, since this is not guaranteed on all 429 * compilers. 430 */ 431 memset(&buf_type, 0, sizeof(buf_type)); 432 buf_type.buffer_type = ISP4FW_BUFFER_TYPE_META_INFO; 433 buf_type.buffer.vmid_space.bit.space = ISP4FW_ADDR_SPACE_TYPE_GPU_VA; 434 isp4if_split_addr64(meta_info_mc, 435 &buf_type.buffer.buf_base_a_lo, 436 &buf_type.buffer.buf_base_a_hi); 437 buf_type.buffer.buf_size_a = ISP4SD_META_BUF_SIZE; 438 439 if (isp4if_send_command(ispif, ISP4FW_CMD_ID_SEND_BUFFER, 440 &buf_type, sizeof(buf_type))) 441 dev_err(dev, "fail send meta_info 0x%llx\n", 442 meta_info_mc); 443 else 444 dev_dbg(dev, "resend meta_info 0x%llx\n", meta_info_mc); 445 } 446 447 static void isp4sd_fw_resp_frame_done(struct isp4_subdev *isp_subdev, 448 enum isp4if_stream_id stream_id, 449 struct isp4fw_resp_param_package *para) 450 { 451 struct isp4_interface *ispif = &isp_subdev->ispif; 452 struct device *dev = isp_subdev->dev; 453 struct isp4if_img_buf_node *prev; 454 struct isp4fw_meta_info *meta; 455 u64 mc; 456 457 mc = isp4if_join_addr64(para->package_addr_lo, para->package_addr_hi); 458 meta = isp4sd_get_meta_by_mc(isp_subdev, mc); 459 if (!meta) { 460 dev_err(dev, "fail to get meta from mc %llx\n", mc); 461 return; 462 } 463 464 dev_dbg(dev, "ts:%llu,streamId:%d,poc:%u,preview_en:%u,status:%s(%i)\n", 465 ktime_get_ns(), stream_id, meta->poc, meta->preview.enabled, 466 isp4dbg_get_buf_done_str(meta->preview.status), 467 meta->preview.status); 468 469 if (meta->preview.enabled && 470 (meta->preview.status == ISP4FW_BUFFER_STATUS_SKIPPED || 471 meta->preview.status == ISP4FW_BUFFER_STATUS_DONE || 472 meta->preview.status == ISP4FW_BUFFER_STATUS_DIRTY)) { 473 prev = isp4if_dequeue_buffer(ispif); 474 if (prev) { 475 isp4dbg_show_bufmeta_info(dev, "prev", &meta->preview, 476 &prev->buf_info); 477 isp4vid_handle_frame_done(&isp_subdev->isp_vdev, 478 &prev->buf_info); 479 isp4if_dealloc_buffer_node(prev); 480 } else { 481 dev_err(dev, "fail null prev buf\n"); 482 } 483 } else if (meta->preview.enabled) { 484 dev_err(dev, "fail bad preview status %u(%s)\n", 485 meta->preview.status, 486 isp4dbg_get_buf_done_str(meta->preview.status)); 487 } 488 489 if (isp_subdev->sensor_info.status == ISP4SD_START_STATUS_STARTED) 490 isp4sd_send_meta_info(isp_subdev, mc); 491 492 dev_dbg(dev, "stream_id:%d, status:%d\n", stream_id, 493 isp_subdev->sensor_info.status); 494 } 495 496 static void isp4sd_fw_resp_func(struct isp4_subdev *isp_subdev, 497 enum isp4if_stream_id stream_id) 498 { 499 struct isp4_interface *ispif = &isp_subdev->ispif; 500 struct device *dev = isp_subdev->dev; 501 struct isp4fw_resp resp; 502 503 if (stream_id == ISP4IF_STREAM_ID_1) 504 isp_fw_log_print(isp_subdev); 505 506 while (true) { 507 if (isp4if_f2h_resp(ispif, stream_id, &resp)) { 508 /* Re-enable the interrupt */ 509 isp4_intr_enable(isp_subdev, stream_id, true); 510 /* 511 * Recheck to see if there is a new response. 512 * To ensure that an in-flight interrupt is not lost, 513 * enabling the interrupt must occur _before_ checking 514 * for a new response, hence a memory barrier is needed. 515 * Disable the interrupt again if there was a new 516 * response. 517 */ 518 mb(); 519 if (likely(isp4if_f2h_resp(ispif, stream_id, &resp))) 520 break; 521 522 isp4_intr_enable(isp_subdev, stream_id, false); 523 } 524 525 switch (resp.resp_id) { 526 case ISP4FW_RESP_ID_CMD_DONE: 527 isp4sd_fw_resp_cmd_done(isp_subdev, stream_id, 528 &resp.param.cmd_done); 529 break; 530 case ISP4FW_RESP_ID_NOTI_FRAME_DONE: 531 isp4sd_fw_resp_frame_done(isp_subdev, stream_id, 532 &resp.param.frame_done); 533 break; 534 default: 535 dev_err(dev, "-><- fail respid %s(0x%x)\n", 536 isp4dbg_get_resp_str(resp.resp_id), 537 resp.resp_id); 538 break; 539 } 540 } 541 } 542 543 static s32 isp4sd_fw_resp_thread(void *context) 544 { 545 struct isp4_subdev_thread_param *para = context; 546 struct isp4_subdev *isp_subdev = para->isp_subdev; 547 struct isp4sd_thread_handler *thread_ctx = 548 &isp_subdev->fw_resp_thread[para->idx]; 549 struct device *dev = isp_subdev->dev; 550 551 dev_dbg(dev, "[%u] fw resp thread started\n", para->idx); 552 while (true) { 553 wait_event_interruptible(thread_ctx->waitq, 554 thread_ctx->resp_ready); 555 thread_ctx->resp_ready = false; 556 557 if (kthread_should_stop()) { 558 dev_dbg(dev, "[%u] fw resp thread quit\n", para->idx); 559 break; 560 } 561 562 isp4sd_fw_resp_func(isp_subdev, para->idx); 563 } 564 565 return 0; 566 } 567 568 static int isp4sd_stop_resp_proc_threads(struct isp4_subdev *isp_subdev) 569 { 570 for (unsigned int i = 0; i < ISP4SD_MAX_FW_RESP_STREAM_NUM; i++) { 571 struct isp4sd_thread_handler *thread_ctx = 572 &isp_subdev->fw_resp_thread[i]; 573 574 if (thread_ctx->thread) { 575 kthread_stop(thread_ctx->thread); 576 thread_ctx->thread = NULL; 577 } 578 } 579 580 return 0; 581 } 582 583 static int isp4sd_start_resp_proc_threads(struct isp4_subdev *isp_subdev) 584 { 585 struct device *dev = isp_subdev->dev; 586 587 for (unsigned int i = 0; i < ISP4SD_MAX_FW_RESP_STREAM_NUM; i++) { 588 struct isp4sd_thread_handler *thread_ctx = 589 &isp_subdev->fw_resp_thread[i]; 590 591 isp_subdev->isp_resp_para[i].idx = i; 592 isp_subdev->isp_resp_para[i].isp_subdev = isp_subdev; 593 init_waitqueue_head(&thread_ctx->waitq); 594 thread_ctx->resp_ready = false; 595 596 thread_ctx->thread = kthread_run(isp4sd_fw_resp_thread, 597 &isp_subdev->isp_resp_para[i], 598 isp4sd_thread_name[i]); 599 if (IS_ERR(thread_ctx->thread)) { 600 dev_err(dev, "create thread [%d] fail\n", i); 601 thread_ctx->thread = NULL; 602 isp4sd_stop_resp_proc_threads(isp_subdev); 603 return -EINVAL; 604 } 605 } 606 607 return 0; 608 } 609 610 int isp4sd_pwroff_and_deinit(struct v4l2_subdev *sd) 611 { 612 struct isp4_subdev *isp_subdev = to_isp4_subdev(sd); 613 struct isp4sd_sensor_info *sensor_info = &isp_subdev->sensor_info; 614 unsigned int perf_state = ISP4SD_PERFORMANCE_STATE_LOW; 615 struct isp4_interface *ispif = &isp_subdev->ispif; 616 struct device *dev = isp_subdev->dev; 617 int ret; 618 619 guard(mutex)(&isp_subdev->ops_mutex); 620 if (sensor_info->status == ISP4SD_START_STATUS_STARTED) { 621 dev_err(dev, "fail for stream still running\n"); 622 return -EINVAL; 623 } 624 625 sensor_info->status = ISP4SD_START_STATUS_OFF; 626 627 if (isp_subdev->irq_enabled) { 628 for (unsigned int i = 0; i < ISP4SD_MAX_FW_RESP_STREAM_NUM; i++) 629 disable_irq(isp_subdev->irq[i]); 630 isp_subdev->irq_enabled = false; 631 } 632 633 isp4sd_stop_resp_proc_threads(isp_subdev); 634 dev_dbg(dev, "isp_subdev stop resp proc threads suc\n"); 635 636 isp4if_stop(ispif); 637 638 ret = dev_pm_genpd_set_performance_state(dev, perf_state); 639 if (ret) 640 dev_err(dev, 641 "fail to set isp_subdev performance state %u,ret %d\n", 642 perf_state, ret); 643 644 /* hold ccpu reset */ 645 isp4hw_wreg(isp_subdev->mmio, ISP_SOFT_RESET, 0); 646 isp4hw_wreg(isp_subdev->mmio, ISP_POWER_STATUS, 0); 647 ret = pm_runtime_put_sync(dev); 648 if (ret) 649 dev_err(dev, "power off isp_subdev fail %d\n", ret); 650 else 651 dev_dbg(dev, "power off isp_subdev suc\n"); 652 653 ispif->status = ISP4IF_STATUS_PWR_OFF; 654 isp4if_clear_cmdq(ispif); 655 isp4sd_module_enable(isp_subdev, false); 656 657 /* 658 * When opening the camera, isp4sd_module_enable(isp_subdev, true) is 659 * called. Hardware requires at least a 20ms delay between disabling 660 * and enabling the module, so a sleep is added to ensure ISP stability 661 * during quick reopen scenarios. 662 */ 663 msleep(20); 664 665 return 0; 666 } 667 668 int isp4sd_pwron_and_init(struct v4l2_subdev *sd) 669 { 670 struct isp4_subdev *isp_subdev = to_isp4_subdev(sd); 671 struct isp4_interface *ispif = &isp_subdev->ispif; 672 struct device *dev = isp_subdev->dev; 673 int ret; 674 675 guard(mutex)(&isp_subdev->ops_mutex); 676 if (ispif->status == ISP4IF_STATUS_FW_RUNNING) { 677 dev_dbg(dev, "camera already opened, do nothing\n"); 678 return 0; 679 } 680 681 isp4sd_module_enable(isp_subdev, true); 682 683 if (ispif->status < ISP4IF_STATUS_PWR_ON) { 684 unsigned int perf_state = ISP4SD_PERFORMANCE_STATE_HIGH; 685 686 ret = pm_runtime_resume_and_get(dev); 687 if (ret) { 688 dev_err(dev, "fail to power on isp_subdev ret %d\n", 689 ret); 690 goto err_deinit; 691 } 692 693 /* ISPPG ISP Power Status */ 694 isp4hw_wreg(isp_subdev->mmio, ISP_POWER_STATUS, 0x7FF); 695 ret = dev_pm_genpd_set_performance_state(dev, perf_state); 696 if (ret) { 697 dev_err(dev, 698 "fail to set performance state %u, ret %d\n", 699 perf_state, ret); 700 goto err_deinit; 701 } 702 703 ispif->status = ISP4IF_STATUS_PWR_ON; 704 } 705 706 isp_subdev->sensor_info.start_stream_cmd_sent = false; 707 isp_subdev->sensor_info.buf_sent_cnt = 0; 708 709 ret = isp4if_start(ispif); 710 if (ret) { 711 dev_err(dev, "fail to start isp_subdev interface\n"); 712 goto err_deinit; 713 } 714 715 if (isp4sd_start_resp_proc_threads(isp_subdev)) { 716 dev_err(dev, "isp_start_resp_proc_threads fail\n"); 717 goto err_deinit; 718 } 719 720 dev_dbg(dev, "create resp threads ok\n"); 721 722 for (unsigned int i = 0; i < ISP4SD_MAX_FW_RESP_STREAM_NUM; i++) 723 enable_irq(isp_subdev->irq[i]); 724 isp_subdev->irq_enabled = true; 725 726 return 0; 727 err_deinit: 728 isp4sd_pwroff_and_deinit(sd); 729 return -EINVAL; 730 } 731 732 static int isp4sd_stop_stream(struct isp4_subdev *isp_subdev, 733 struct v4l2_subdev_state *state, u32 pad) 734 { 735 struct isp4sd_sensor_info *sensor_info = &isp_subdev->sensor_info; 736 struct isp4sd_output_info *output_info = &sensor_info->output_info; 737 struct isp4_interface *ispif = &isp_subdev->ispif; 738 struct device *dev = isp_subdev->dev; 739 740 guard(mutex)(&isp_subdev->ops_mutex); 741 dev_dbg(dev, "status %i\n", output_info->start_status); 742 743 if (output_info->start_status == ISP4SD_START_STATUS_STARTED) { 744 struct isp4fw_cmd_enable_out_ch cmd_ch_disable; 745 int ret; 746 747 /* 748 * The struct will be shared with ISP FW, use memset() to 749 * guarantee padding bits are zeroed, since this is not 750 * guaranteed on all compilers. 751 */ 752 memset(&cmd_ch_disable, 0, sizeof(cmd_ch_disable)); 753 cmd_ch_disable.ch = ISP4FW_ISP_PIPE_OUT_CH_PREVIEW; 754 /* `cmd_ch_disable.is_enable` is already false */ 755 ret = isp4if_send_command_sync(ispif, 756 ISP4FW_CMD_ID_ENABLE_OUT_CHAN, 757 &cmd_ch_disable, 758 sizeof(cmd_ch_disable)); 759 if (ret) 760 dev_err(dev, "fail to disable stream\n"); 761 else 762 dev_dbg(dev, "wait disable stream suc\n"); 763 764 ret = isp4if_send_command_sync(ispif, ISP4FW_CMD_ID_STOP_STREAM, 765 NULL, 0); 766 if (ret) 767 dev_err(dev, "fail to stop stream\n"); 768 else 769 dev_dbg(dev, "wait stop stream suc\n"); 770 } 771 772 isp4sd_uninit_stream(isp_subdev, state, pad); 773 774 /* 775 * Return success to ensure the stop process proceeds, 776 * and disregard any errors since they are not fatal. 777 */ 778 return 0; 779 } 780 781 static int isp4sd_start_stream(struct isp4_subdev *isp_subdev, 782 struct v4l2_subdev_state *state, u32 pad) 783 { 784 struct isp4sd_output_info *output_info = 785 &isp_subdev->sensor_info.output_info; 786 struct isp4_interface *ispif = &isp_subdev->ispif; 787 struct device *dev = isp_subdev->dev; 788 int ret; 789 790 guard(mutex)(&isp_subdev->ops_mutex); 791 792 if (ispif->status != ISP4IF_STATUS_FW_RUNNING) { 793 dev_err(dev, "fail, bad fsm %d\n", ispif->status); 794 return -EINVAL; 795 } 796 797 switch (output_info->start_status) { 798 case ISP4SD_START_STATUS_OFF: 799 break; 800 case ISP4SD_START_STATUS_STARTED: 801 dev_dbg(dev, "stream already started, do nothing\n"); 802 return 0; 803 case ISP4SD_START_STATUS_START_FAIL: 804 dev_err(dev, "stream previously failed to start\n"); 805 return -EINVAL; 806 } 807 808 ret = isp4sd_init_stream(isp_subdev); 809 if (ret) { 810 dev_err(dev, "fail to init isp_subdev stream\n"); 811 goto err_stop_stream; 812 } 813 814 ret = isp4sd_setup_output(isp_subdev, state, pad); 815 if (ret) { 816 dev_err(dev, "fail to setup output\n"); 817 goto err_stop_stream; 818 } 819 820 return 0; 821 822 err_stop_stream: 823 isp4sd_stop_stream(isp_subdev, state, pad); 824 return ret; 825 } 826 827 int isp4sd_ioc_send_img_buf(struct v4l2_subdev *sd, 828 struct isp4if_img_buf_info *buf_info) 829 { 830 struct isp4_subdev *isp_subdev = to_isp4_subdev(sd); 831 struct isp4_interface *ispif = &isp_subdev->ispif; 832 struct isp4if_img_buf_node *buf_node; 833 struct device *dev = isp_subdev->dev; 834 int ret; 835 836 guard(mutex)(&isp_subdev->ops_mutex); 837 838 if (ispif->status != ISP4IF_STATUS_FW_RUNNING) { 839 dev_err(dev, "fail send img buf for bad fsm %d\n", 840 ispif->status); 841 return -EINVAL; 842 } 843 844 buf_node = isp4if_alloc_buffer_node(buf_info); 845 if (!buf_node) { 846 dev_err(dev, "fail alloc sys img buf info node\n"); 847 return -ENOMEM; 848 } 849 850 ret = isp4if_queue_buffer(ispif, buf_node); 851 if (ret) { 852 dev_err(dev, "fail to queue image buf, %d\n", ret); 853 goto error_release_buf_node; 854 } 855 856 if (!isp_subdev->sensor_info.start_stream_cmd_sent) { 857 isp_subdev->sensor_info.buf_sent_cnt++; 858 859 if (isp_subdev->sensor_info.buf_sent_cnt >= 860 ISP4SD_MIN_BUF_CNT_BEF_START_STREAM) { 861 ret = isp4if_send_command(ispif, 862 ISP4FW_CMD_ID_START_STREAM, 863 NULL, 0); 864 if (ret) { 865 dev_err(dev, "fail to START_STREAM"); 866 goto error_release_buf_node; 867 } 868 isp_subdev->sensor_info.start_stream_cmd_sent = true; 869 isp_subdev->sensor_info.output_info.start_status = 870 ISP4SD_START_STATUS_STARTED; 871 isp_subdev->sensor_info.status = 872 ISP4SD_START_STATUS_STARTED; 873 } else { 874 dev_dbg(dev, 875 "no send start, required %u, buf sent %u\n", 876 ISP4SD_MIN_BUF_CNT_BEF_START_STREAM, 877 isp_subdev->sensor_info.buf_sent_cnt); 878 } 879 } 880 881 return 0; 882 883 error_release_buf_node: 884 isp4if_dealloc_buffer_node(buf_node); 885 return ret; 886 } 887 888 static const struct v4l2_subdev_video_ops isp4sd_video_ops = { 889 .s_stream = v4l2_subdev_s_stream_helper, 890 }; 891 892 static int isp4sd_set_pad_format(struct v4l2_subdev *sd, 893 struct v4l2_subdev_state *sd_state, 894 struct v4l2_subdev_format *format) 895 { 896 struct isp4sd_output_info *stream_info = 897 &(to_isp4_subdev(sd)->sensor_info.output_info); 898 struct v4l2_mbus_framefmt *fmt; 899 900 fmt = v4l2_subdev_state_get_format(sd_state, format->pad); 901 902 if (!fmt) { 903 dev_err(sd->dev, "fail to get state format\n"); 904 return -EINVAL; 905 } 906 907 *fmt = format->format; 908 switch (fmt->code) { 909 case MEDIA_BUS_FMT_YUYV8_1X16: 910 stream_info->image_size = fmt->width * fmt->height * 2; 911 break; 912 case MEDIA_BUS_FMT_YUYV8_1_5X8: 913 default: 914 stream_info->image_size = fmt->width * fmt->height * 3 / 2; 915 break; 916 } 917 918 if (!stream_info->image_size) { 919 dev_err(sd->dev, 920 "fail set pad format,code 0x%x,width %u, height %u\n", 921 fmt->code, fmt->width, fmt->height); 922 return -EINVAL; 923 } 924 925 dev_dbg(sd->dev, "set pad format suc, code:%x w:%u h:%u size:%u\n", 926 fmt->code, fmt->width, fmt->height, 927 stream_info->image_size); 928 929 return 0; 930 } 931 932 static int isp4sd_enable_streams(struct v4l2_subdev *sd, 933 struct v4l2_subdev_state *state, u32 pad, 934 u64 streams_mask) 935 { 936 struct isp4_subdev *isp_subdev = to_isp4_subdev(sd); 937 938 return isp4sd_start_stream(isp_subdev, state, pad); 939 } 940 941 static int isp4sd_disable_streams(struct v4l2_subdev *sd, 942 struct v4l2_subdev_state *state, u32 pad, 943 u64 streams_mask) 944 { 945 struct isp4_subdev *isp_subdev = to_isp4_subdev(sd); 946 947 return isp4sd_stop_stream(isp_subdev, state, pad); 948 } 949 950 static const struct v4l2_subdev_pad_ops isp4sd_pad_ops = { 951 .get_fmt = v4l2_subdev_get_fmt, 952 .set_fmt = isp4sd_set_pad_format, 953 .enable_streams = isp4sd_enable_streams, 954 .disable_streams = isp4sd_disable_streams, 955 }; 956 957 static const struct v4l2_subdev_ops isp4sd_subdev_ops = { 958 .video = &isp4sd_video_ops, 959 .pad = &isp4sd_pad_ops, 960 }; 961 962 int isp4sd_init(struct isp4_subdev *isp_subdev, struct v4l2_device *v4l2_dev, 963 int irq[ISP4SD_MAX_FW_RESP_STREAM_NUM]) 964 { 965 struct isp4sd_sensor_info *sensor_info = &isp_subdev->sensor_info; 966 struct isp4_interface *ispif = &isp_subdev->ispif; 967 struct device *dev = v4l2_dev->dev; 968 int ret; 969 970 isp_subdev->dev = dev; 971 v4l2_subdev_init(&isp_subdev->sdev, &isp4sd_subdev_ops); 972 isp_subdev->sdev.owner = THIS_MODULE; 973 isp_subdev->sdev.dev = dev; 974 snprintf(isp_subdev->sdev.name, sizeof(isp_subdev->sdev.name), "%s", 975 dev_name(dev)); 976 977 isp_subdev->sdev.entity.name = isp4sd_entity_name; 978 isp_subdev->sdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_ISP; 979 isp_subdev->sdev_pad.flags = MEDIA_PAD_FL_SOURCE; 980 ret = media_entity_pads_init(&isp_subdev->sdev.entity, 1, 981 &isp_subdev->sdev_pad); 982 if (ret) { 983 dev_err(dev, "fail to init isp4 subdev entity pad %d\n", ret); 984 return ret; 985 } 986 987 ret = v4l2_subdev_init_finalize(&isp_subdev->sdev); 988 if (ret < 0) { 989 dev_err(dev, "fail to init finalize isp4 subdev %d\n", 990 ret); 991 return ret; 992 } 993 994 ret = v4l2_device_register_subdev(v4l2_dev, &isp_subdev->sdev); 995 if (ret) { 996 dev_err(dev, "fail to register isp4 subdev to V4L2 device %d\n", 997 ret); 998 goto err_media_clean_up; 999 } 1000 1001 isp4if_init(ispif, dev, isp_subdev->mmio); 1002 1003 mutex_init(&isp_subdev->ops_mutex); 1004 sensor_info->status = ISP4SD_START_STATUS_OFF; 1005 1006 /* create ISP enable gpio control */ 1007 isp_subdev->enable_gpio = devm_gpiod_get(isp_subdev->dev, 1008 "enable_isp", 1009 GPIOD_OUT_LOW); 1010 if (IS_ERR(isp_subdev->enable_gpio)) { 1011 ret = PTR_ERR(isp_subdev->enable_gpio); 1012 dev_err(dev, "fail to get gpiod %d\n", ret); 1013 goto err_subdev_unreg; 1014 } 1015 1016 for (unsigned int i = 0; i < ISP4SD_MAX_FW_RESP_STREAM_NUM; i++) 1017 isp_subdev->irq[i] = irq[i]; 1018 1019 isp_subdev->host2fw_seq_num = 1; 1020 ispif->status = ISP4IF_STATUS_PWR_OFF; 1021 1022 ret = isp4vid_dev_init(&isp_subdev->isp_vdev, &isp_subdev->sdev); 1023 if (ret) 1024 goto err_subdev_unreg; 1025 1026 return 0; 1027 1028 err_subdev_unreg: 1029 v4l2_device_unregister_subdev(&isp_subdev->sdev); 1030 err_media_clean_up: 1031 v4l2_subdev_cleanup(&isp_subdev->sdev); 1032 media_entity_cleanup(&isp_subdev->sdev.entity); 1033 return ret; 1034 } 1035 1036 void isp4sd_deinit(struct isp4_subdev *isp_subdev) 1037 { 1038 struct isp4_interface *ispif = &isp_subdev->ispif; 1039 1040 isp4vid_dev_deinit(&isp_subdev->isp_vdev); 1041 v4l2_device_unregister_subdev(&isp_subdev->sdev); 1042 media_entity_cleanup(&isp_subdev->sdev.entity); 1043 isp4if_deinit(ispif); 1044 isp4sd_module_enable(isp_subdev, false); 1045 1046 ispif->status = ISP4IF_STATUS_PWR_OFF; 1047 } 1048