projects/linux·blob master · refs

1063 lines · 29.6 KiB

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
24static const char *isp4sd_entity_name = "amd isp4";
25
26static const char *isp4sd_thread_name[ISP4SD_MAX_FW_RESP_STREAM_NUM] = {
27 "amd_isp4_thread_global",
28 "amd_isp4_thread_stream1",
29};
30
31static 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
40static 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
80static 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
108static 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
149static 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
189static 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
233static 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
331static 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
351static 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
377static 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 (refcount_dec_and_test(&ele->refcnt))
395 kfree(ele);
396 }
397}
398
399static struct isp4fw_meta_info *
400isp4sd_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
413static 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
447static 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
496static 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
543static 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
568static 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
583static 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
610int 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
668int 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_module_disable;
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_power_off;
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_perf_restore;
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_stop_interface;
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
728err_stop_interface:
729 isp4if_stop(ispif);
730err_perf_restore:
731 ret = dev_pm_genpd_set_performance_state(dev, ISP4SD_PERFORMANCE_STATE_LOW);
732 if (ret)
733 dev_err(dev, "fail to set performance state %u, ret %d\n",
734 ISP4SD_PERFORMANCE_STATE_LOW, ret);
735err_power_off:
736 isp4hw_wreg(isp_subdev->mmio, ISP_SOFT_RESET, 0);
737 isp4hw_wreg(isp_subdev->mmio, ISP_POWER_STATUS, 0);
738 ret = pm_runtime_put_sync(dev);
739 if (ret)
740 dev_err(dev, "power off isp_subdev fail %d\n", ret);
741 ispif->status = ISP4IF_STATUS_PWR_OFF;
742err_module_disable:
743 isp4sd_module_enable(isp_subdev, false);
744 msleep(20);
745 return -EINVAL;
746}
747
748static int isp4sd_stop_stream(struct isp4_subdev *isp_subdev,
749 struct v4l2_subdev_state *state, u32 pad)
750{
751 struct isp4sd_sensor_info *sensor_info = &isp_subdev->sensor_info;
752 struct isp4sd_output_info *output_info = &sensor_info->output_info;
753 struct isp4_interface *ispif = &isp_subdev->ispif;
754 struct device *dev = isp_subdev->dev;
755
756 guard(mutex)(&isp_subdev->ops_mutex);
757 dev_dbg(dev, "status %i\n", output_info->start_status);
758
759 if (output_info->start_status == ISP4SD_START_STATUS_STARTED) {
760 struct isp4fw_cmd_enable_out_ch cmd_ch_disable;
761 int ret;
762
763 /*
764 * The struct will be shared with ISP FW, use memset() to
765 * guarantee padding bits are zeroed, since this is not
766 * guaranteed on all compilers.
767 */
768 memset(&cmd_ch_disable, 0, sizeof(cmd_ch_disable));
769 cmd_ch_disable.ch = ISP4FW_ISP_PIPE_OUT_CH_PREVIEW;
770 /* `cmd_ch_disable.is_enable` is already false */
771 ret = isp4if_send_command_sync(ispif,
772 ISP4FW_CMD_ID_ENABLE_OUT_CHAN,
773 &cmd_ch_disable,
774 sizeof(cmd_ch_disable));
775 if (ret)
776 dev_err(dev, "fail to disable stream\n");
777 else
778 dev_dbg(dev, "wait disable stream suc\n");
779
780 ret = isp4if_send_command_sync(ispif, ISP4FW_CMD_ID_STOP_STREAM,
781 NULL, 0);
782 if (ret)
783 dev_err(dev, "fail to stop stream\n");
784 else
785 dev_dbg(dev, "wait stop stream suc\n");
786 }
787
788 isp4sd_uninit_stream(isp_subdev, state, pad);
789
790 /*
791 * Return success to ensure the stop process proceeds,
792 * and disregard any errors since they are not fatal.
793 */
794 return 0;
795}
796
797static int isp4sd_start_stream(struct isp4_subdev *isp_subdev,
798 struct v4l2_subdev_state *state, u32 pad)
799{
800 struct isp4sd_output_info *output_info =
801 &isp_subdev->sensor_info.output_info;
802 struct isp4_interface *ispif = &isp_subdev->ispif;
803 struct device *dev = isp_subdev->dev;
804 int ret;
805
806 guard(mutex)(&isp_subdev->ops_mutex);
807
808 if (ispif->status != ISP4IF_STATUS_FW_RUNNING) {
809 dev_err(dev, "fail, bad fsm %d\n", ispif->status);
810 return -EINVAL;
811 }
812
813 switch (output_info->start_status) {
814 case ISP4SD_START_STATUS_OFF:
815 break;
816 case ISP4SD_START_STATUS_STARTED:
817 dev_dbg(dev, "stream already started, do nothing\n");
818 return 0;
819 case ISP4SD_START_STATUS_START_FAIL:
820 dev_err(dev, "stream previously failed to start\n");
821 return -EINVAL;
822 }
823
824 ret = isp4sd_init_stream(isp_subdev);
825 if (ret) {
826 dev_err(dev, "fail to init isp_subdev stream\n");
827 goto err_stop_stream;
828 }
829
830 ret = isp4sd_setup_output(isp_subdev, state, pad);
831 if (ret) {
832 dev_err(dev, "fail to setup output\n");
833 goto err_stop_stream;
834 }
835
836 return 0;
837
838err_stop_stream:
839 isp4sd_stop_stream(isp_subdev, state, pad);
840 return ret;
841}
842
843int isp4sd_ioc_send_img_buf(struct v4l2_subdev *sd,
844 struct isp4if_img_buf_info *buf_info)
845{
846 struct isp4_subdev *isp_subdev = to_isp4_subdev(sd);
847 struct isp4_interface *ispif = &isp_subdev->ispif;
848 struct isp4if_img_buf_node *buf_node;
849 struct device *dev = isp_subdev->dev;
850 int ret;
851
852 guard(mutex)(&isp_subdev->ops_mutex);
853
854 if (ispif->status != ISP4IF_STATUS_FW_RUNNING) {
855 dev_err(dev, "fail send img buf for bad fsm %d\n",
856 ispif->status);
857 return -EINVAL;
858 }
859
860 buf_node = isp4if_alloc_buffer_node(buf_info);
861 if (!buf_node) {
862 dev_err(dev, "fail alloc sys img buf info node\n");
863 return -ENOMEM;
864 }
865
866 ret = isp4if_queue_buffer(ispif, buf_node);
867 if (ret) {
868 dev_err(dev, "fail to queue image buf, %d\n", ret);
869 goto error_release_buf_node;
870 }
871
872 if (!isp_subdev->sensor_info.start_stream_cmd_sent) {
873 isp_subdev->sensor_info.buf_sent_cnt++;
874
875 if (isp_subdev->sensor_info.buf_sent_cnt >=
876 ISP4SD_MIN_BUF_CNT_BEF_START_STREAM) {
877 ret = isp4if_send_command(ispif,
878 ISP4FW_CMD_ID_START_STREAM,
879 NULL, 0);
880 if (ret) {
881 dev_err(dev, "fail to START_STREAM");
882 goto error_release_buf_node;
883 }
884 isp_subdev->sensor_info.start_stream_cmd_sent = true;
885 isp_subdev->sensor_info.output_info.start_status =
886 ISP4SD_START_STATUS_STARTED;
887 isp_subdev->sensor_info.status =
888 ISP4SD_START_STATUS_STARTED;
889 } else {
890 dev_dbg(dev,
891 "no send start, required %u, buf sent %u\n",
892 ISP4SD_MIN_BUF_CNT_BEF_START_STREAM,
893 isp_subdev->sensor_info.buf_sent_cnt);
894 }
895 }
896
897 return 0;
898
899error_release_buf_node:
900 isp4if_dealloc_buffer_node(buf_node);
901 return ret;
902}
903
904static const struct v4l2_subdev_video_ops isp4sd_video_ops = {
905 .s_stream = v4l2_subdev_s_stream_helper,
906};
907
908static int isp4sd_set_pad_format(struct v4l2_subdev *sd,
909 struct v4l2_subdev_state *sd_state,
910 struct v4l2_subdev_format *format)
911{
912 struct isp4sd_output_info *stream_info =
913 &(to_isp4_subdev(sd)->sensor_info.output_info);
914 struct v4l2_mbus_framefmt *fmt;
915
916 fmt = v4l2_subdev_state_get_format(sd_state, format->pad);
917
918 if (!fmt) {
919 dev_err(sd->dev, "fail to get state format\n");
920 return -EINVAL;
921 }
922
923 *fmt = format->format;
924 switch (fmt->code) {
925 case MEDIA_BUS_FMT_YUYV8_1X16:
926 stream_info->image_size = fmt->width * fmt->height * 2;
927 break;
928 case MEDIA_BUS_FMT_YUYV8_1_5X8:
929 default:
930 stream_info->image_size = fmt->width * fmt->height * 3 / 2;
931 break;
932 }
933
934 if (!stream_info->image_size) {
935 dev_err(sd->dev,
936 "fail set pad format,code 0x%x,width %u, height %u\n",
937 fmt->code, fmt->width, fmt->height);
938 return -EINVAL;
939 }
940
941 dev_dbg(sd->dev, "set pad format suc, code:%x w:%u h:%u size:%u\n",
942 fmt->code, fmt->width, fmt->height,
943 stream_info->image_size);
944
945 return 0;
946}
947
948static int isp4sd_enable_streams(struct v4l2_subdev *sd,
949 struct v4l2_subdev_state *state, u32 pad,
950 u64 streams_mask)
951{
952 struct isp4_subdev *isp_subdev = to_isp4_subdev(sd);
953
954 return isp4sd_start_stream(isp_subdev, state, pad);
955}
956
957static int isp4sd_disable_streams(struct v4l2_subdev *sd,
958 struct v4l2_subdev_state *state, u32 pad,
959 u64 streams_mask)
960{
961 struct isp4_subdev *isp_subdev = to_isp4_subdev(sd);
962
963 return isp4sd_stop_stream(isp_subdev, state, pad);
964}
965
966static const struct v4l2_subdev_pad_ops isp4sd_pad_ops = {
967 .get_fmt = v4l2_subdev_get_fmt,
968 .set_fmt = isp4sd_set_pad_format,
969 .enable_streams = isp4sd_enable_streams,
970 .disable_streams = isp4sd_disable_streams,
971};
972
973static const struct v4l2_subdev_ops isp4sd_subdev_ops = {
974 .video = &isp4sd_video_ops,
975 .pad = &isp4sd_pad_ops,
976};
977
978int isp4sd_init(struct isp4_subdev *isp_subdev, struct v4l2_device *v4l2_dev,
979 int irq[ISP4SD_MAX_FW_RESP_STREAM_NUM])
980{
981 struct isp4sd_sensor_info *sensor_info = &isp_subdev->sensor_info;
982 struct isp4_interface *ispif = &isp_subdev->ispif;
983 struct device *dev = v4l2_dev->dev;
984 int ret;
985
986 isp_subdev->dev = dev;
987 v4l2_subdev_init(&isp_subdev->sdev, &isp4sd_subdev_ops);
988 isp_subdev->sdev.owner = THIS_MODULE;
989 isp_subdev->sdev.dev = dev;
990 snprintf(isp_subdev->sdev.name, sizeof(isp_subdev->sdev.name), "%s",
991 dev_name(dev));
992
993 isp_subdev->sdev.entity.name = isp4sd_entity_name;
994 isp_subdev->sdev.entity.function = MEDIA_ENT_F_PROC_VIDEO_ISP;
995 isp_subdev->sdev_pad.flags = MEDIA_PAD_FL_SOURCE;
996 ret = media_entity_pads_init(&isp_subdev->sdev.entity, 1,
997 &isp_subdev->sdev_pad);
998 if (ret) {
999 dev_err(dev, "fail to init isp4 subdev entity pad %d\n", ret);
1000 return ret;
1001 }
1002
1003 ret = v4l2_subdev_init_finalize(&isp_subdev->sdev);
1004 if (ret < 0) {
1005 dev_err(dev, "fail to init finalize isp4 subdev %d\n",
1006 ret);
1007 return ret;
1008 }
1009
1010 ret = v4l2_device_register_subdev(v4l2_dev, &isp_subdev->sdev);
1011 if (ret) {
1012 dev_err(dev, "fail to register isp4 subdev to V4L2 device %d\n",
1013 ret);
1014 goto err_media_clean_up;
1015 }
1016
1017 isp4if_init(ispif, dev, isp_subdev->mmio);
1018
1019 mutex_init(&isp_subdev->ops_mutex);
1020 sensor_info->status = ISP4SD_START_STATUS_OFF;
1021
1022 /* create ISP enable gpio control */
1023 isp_subdev->enable_gpio = devm_gpiod_get(isp_subdev->dev,
1024 "enable_isp",
1025 GPIOD_OUT_LOW);
1026 if (IS_ERR(isp_subdev->enable_gpio)) {
1027 ret = PTR_ERR(isp_subdev->enable_gpio);
1028 dev_err(dev, "fail to get gpiod %d\n", ret);
1029 goto err_subdev_unreg;
1030 }
1031
1032 for (unsigned int i = 0; i < ISP4SD_MAX_FW_RESP_STREAM_NUM; i++)
1033 isp_subdev->irq[i] = irq[i];
1034
1035 isp_subdev->host2fw_seq_num = 1;
1036 ispif->status = ISP4IF_STATUS_PWR_OFF;
1037
1038 ret = isp4vid_dev_init(&isp_subdev->isp_vdev, &isp_subdev->sdev);
1039 if (ret)
1040 goto err_subdev_unreg;
1041
1042 return 0;
1043
1044err_subdev_unreg:
1045 v4l2_device_unregister_subdev(&isp_subdev->sdev);
1046err_media_clean_up:
1047 v4l2_subdev_cleanup(&isp_subdev->sdev);
1048 media_entity_cleanup(&isp_subdev->sdev.entity);
1049 return ret;
1050}
1051
1052void isp4sd_deinit(struct isp4_subdev *isp_subdev)
1053{
1054 struct isp4_interface *ispif = &isp_subdev->ispif;
1055
1056 isp4vid_dev_deinit(&isp_subdev->isp_vdev);
1057 v4l2_device_unregister_subdev(&isp_subdev->sdev);
1058 media_entity_cleanup(&isp_subdev->sdev.entity);
1059 isp4if_deinit(ispif);
1060 isp4sd_module_enable(isp_subdev, false);
1061
1062 ispif->status = ISP4IF_STATUS_PWR_OFF;
1063}