1 // SPDX-License-Identifier: GPL-2.0
2 /* Copyright (C) 2022 Rockchip Electronics Co., Ltd */
3
4 #define pr_fmt(fmt) "isp_rockit: %s:%d " fmt, __func__, __LINE__
5
6 #include <linux/of.h>
7 #include <linux/of_platform.h>
8 #include <soc/rockchip/rockchip_rockit.h>
9
10 #include "dev.h"
11
12 static struct rockit_rkcif_cfg *rockit_rkcif_cfg;
13
14 struct rkcif_rockit_buffer {
15 struct rkcif_buffer cif_buf;
16 struct dma_buf *dmabuf;
17 void *mpi_mem;
18 void *mpi_buf;
19 struct list_head queue;
20 int buf_id;
21 union {
22 u32 buff_addr;
23 void *vaddr;
24 };
25 };
26
rkcif_rockit_get_stream(struct rockit_rkcif_cfg * input_rockit_cfg)27 static struct rkcif_stream *rkcif_rockit_get_stream(struct rockit_rkcif_cfg *input_rockit_cfg)
28 {
29 struct rkcif_device *cif_dev = NULL;
30 struct rkcif_stream *stream = NULL;
31 u8 i;
32
33 if (!rockit_rkcif_cfg) {
34 pr_err("rockit_rkcif_cfg is null get stream failed\n");
35 return NULL;
36 }
37 if (!input_rockit_cfg) {
38 pr_err("input is null get stream failed\n");
39 return NULL;
40 }
41
42 for (i = 0; i < rockit_rkcif_cfg->cif_num; i++) {
43 if (!strcmp(rockit_rkcif_cfg->rkcif_dev_cfg[i].cif_name,
44 input_rockit_cfg->cur_name)) {
45 cif_dev = rockit_rkcif_cfg->rkcif_dev_cfg[i].cif_dev;
46 break;
47 }
48 }
49
50 if (cif_dev == NULL) {
51 pr_err("Can not find cif_dev!");
52 return NULL;
53 }
54
55 switch (input_rockit_cfg->nick_id) {
56 case 0:
57 stream = &cif_dev->stream[RKCIF_STREAM_MIPI_ID0];
58 break;
59 case 1:
60 stream = &cif_dev->stream[RKCIF_STREAM_MIPI_ID1];
61 break;
62 case 2:
63 stream = &cif_dev->stream[RKCIF_STREAM_MIPI_ID2];
64 break;
65 case 3:
66 stream = &cif_dev->stream[RKCIF_STREAM_MIPI_ID3];
67 break;
68 default:
69 stream = NULL;
70 break;
71 }
72
73 return stream;
74 }
75
rkcif_rockit_buf_queue(struct rockit_rkcif_cfg * input_rockit_cfg)76 int rkcif_rockit_buf_queue(struct rockit_rkcif_cfg *input_rockit_cfg)
77 {
78 struct rkcif_stream *stream = NULL;
79 struct rkcif_rockit_buffer *rkcif_buf = NULL;
80 struct rkcif_device *cif_dev = NULL;
81 const struct vb2_mem_ops *g_ops = NULL;
82 int i, ret, offset, dev_id;
83 struct rkcif_stream_cfg *stream_cfg = NULL;
84 void *mem = NULL;
85 struct sg_table *sg_tbl;
86 unsigned long lock_flags = 0;
87
88 stream = rkcif_rockit_get_stream(input_rockit_cfg);
89
90 if (stream == NULL) {
91 pr_err("the stream is NULL");
92 return -EINVAL;
93 }
94
95 cif_dev = stream->cifdev;
96 dev_id = cif_dev->csi_host_idx;
97 g_ops = cif_dev->hw_dev->mem_ops;
98
99 if (stream->id >= RKCIF_MAX_STREAM_MIPI)
100 return -EINVAL;
101
102 stream_cfg = &rockit_rkcif_cfg->rkcif_dev_cfg[dev_id].rkcif_stream_cfg[stream->id];
103
104 stream_cfg->node = input_rockit_cfg->node;
105
106 if (!input_rockit_cfg->buf)
107 return -EINVAL;
108
109 for (i = 0; i < ROCKIT_BUF_NUM_MAX; i++) {
110 if (stream_cfg->buff_id[i] == input_rockit_cfg->mpi_id) {
111 input_rockit_cfg->is_alloc = 0;
112 break;
113 }
114 }
115
116 if (input_rockit_cfg->is_alloc) {
117 for (i = 0; i < ROCKIT_BUF_NUM_MAX; i++) {
118 if (stream_cfg->buff_id[i] == 0) {
119 stream_cfg->rkcif_buff[i] =
120 kzalloc(sizeof(struct rkcif_rockit_buffer), GFP_KERNEL);
121 if (stream_cfg->rkcif_buff[i] == NULL) {
122 pr_err("rkisp_buff alloc failed!\n");
123 return -EINVAL;
124 }
125 stream_cfg->buff_id[i] = input_rockit_cfg->mpi_id;
126 break;
127 }
128 }
129 if (i == ROCKIT_BUF_NUM_MAX)
130 return -EINVAL;
131
132 rkcif_buf = stream_cfg->rkcif_buff[i];
133 rkcif_buf->mpi_buf = input_rockit_cfg->mpibuf;
134
135 mem = g_ops->attach_dmabuf(stream->cifdev->hw_dev->dev,
136 input_rockit_cfg->buf,
137 input_rockit_cfg->buf->size,
138 DMA_BIDIRECTIONAL);
139 if (IS_ERR(mem))
140 pr_err("the g_ops->attach_dmabuf is error!\n");
141
142 rkcif_buf->mpi_mem = mem;
143 rkcif_buf->dmabuf = input_rockit_cfg->buf;
144
145 ret = g_ops->map_dmabuf(mem);
146 if (ret)
147 pr_err("the g_ops->map_dmabuf is error!\n");
148
149 if (stream->cifdev->hw_dev->is_dma_sg_ops) {
150 sg_tbl = (struct sg_table *)g_ops->cookie(mem);
151 rkcif_buf->buff_addr = sg_dma_address(sg_tbl->sgl);
152 } else {
153 rkcif_buf->buff_addr = *((u32 *)g_ops->cookie(mem));
154 }
155 get_dma_buf(input_rockit_cfg->buf);
156 } else {
157 for (i = 0; i < ROCKIT_BUF_NUM_MAX; i++) {
158 rkcif_buf = stream_cfg->rkcif_buff[i];
159 if (stream_cfg->buff_id[i] == input_rockit_cfg->mpi_id)
160 break;
161 }
162 }
163
164 for (i = 0; i < stream->cif_fmt_out->mplanes; i++)
165 rkcif_buf->cif_buf.buff_addr[i] = rkcif_buf->buff_addr;
166
167 if (stream->cif_fmt_out->mplanes == 1) {
168 for (i = 0; i < stream->cif_fmt_out->cplanes - 1; i++) {
169 offset = stream->pixm.plane_fmt[i].bytesperline * stream->pixm.height;
170 rkcif_buf->cif_buf.buff_addr[i + 1] =
171 rkcif_buf->cif_buf.buff_addr[i] + offset;
172 }
173 }
174
175 v4l2_dbg(2, rkcif_debug, &cif_dev->v4l2_dev,
176 "stream:%d rockit_queue buf:0x%x\n",
177 stream->id, rkcif_buf->cif_buf.buff_addr[0]);
178
179 if (stream_cfg->is_discard)
180 return -EINVAL;
181
182 spin_lock_irqsave(&stream->vbq_lock, lock_flags);
183 list_add_tail(&rkcif_buf->cif_buf.queue, &stream->rockit_buf_head);
184 spin_unlock_irqrestore(&stream->vbq_lock, lock_flags);
185
186 return 0;
187 }
188
rkcif_rockit_buf_done(struct rkcif_stream * stream,struct rkcif_buffer * buf)189 int rkcif_rockit_buf_done(struct rkcif_stream *stream, struct rkcif_buffer *buf)
190 {
191 struct rkcif_device *dev = stream->cifdev;
192 struct rkcif_rockit_buffer *rkcif_buf = NULL;
193 struct rkcif_stream_cfg *stream_cfg = NULL;
194 u32 dev_id = stream->cifdev->csi_host_idx;
195
196 if (!rockit_rkcif_cfg ||
197 !rockit_rkcif_cfg->rkcif_rockit_mpibuf_done ||
198 stream->id >= RKCIF_MAX_STREAM_MIPI)
199 return -EINVAL;
200
201 stream_cfg = &rockit_rkcif_cfg->rkcif_dev_cfg[dev_id].rkcif_stream_cfg[stream->id];
202 rkcif_buf =
203 container_of(buf, struct rkcif_rockit_buffer, cif_buf);
204
205 rockit_rkcif_cfg->mpibuf = rkcif_buf->mpi_buf;
206 rockit_rkcif_cfg->buf = rkcif_buf->dmabuf;
207
208 rockit_rkcif_cfg->frame.u64PTS = buf->vb.vb2_buf.timestamp;
209
210 rockit_rkcif_cfg->frame.u32TimeRef = buf->vb.sequence;
211
212 rockit_rkcif_cfg->frame.u32Height = stream->pixm.height;
213
214 rockit_rkcif_cfg->frame.u32Width = stream->pixm.width;
215
216 rockit_rkcif_cfg->frame.enPixelFormat = stream->pixm.pixelformat;
217
218 rockit_rkcif_cfg->frame.u32VirWidth = stream->pixm.width;
219
220 rockit_rkcif_cfg->frame.u32VirHeight = stream->pixm.height;
221
222 rockit_rkcif_cfg->cur_name = dev_name(dev->dev);
223
224 rockit_rkcif_cfg->node = stream_cfg->node;
225
226 if (list_empty(&stream->rockit_buf_head))
227 rockit_rkcif_cfg->is_empty = true;
228 else
229 rockit_rkcif_cfg->is_empty = false;
230
231 if (rockit_rkcif_cfg->rkcif_rockit_mpibuf_done)
232 rockit_rkcif_cfg->rkcif_rockit_mpibuf_done(rockit_rkcif_cfg);
233
234 return 0;
235 }
236
rkcif_rockit_buf_free(struct rkcif_stream * stream)237 static int rkcif_rockit_buf_free(struct rkcif_stream *stream)
238 {
239 struct rkcif_rockit_buffer *rkcif_buf = NULL;
240 u32 i = 0, dev_id = stream->cifdev->csi_host_idx;
241 const struct vb2_mem_ops *g_ops = stream->cifdev->hw_dev->mem_ops;
242 struct rkcif_stream_cfg *stream_cfg = NULL;
243
244 if (!rockit_rkcif_cfg || stream->id >= RKCIF_MAX_STREAM_MIPI)
245 return -EINVAL;
246
247 stream_cfg = &rockit_rkcif_cfg->rkcif_dev_cfg[dev_id].rkcif_stream_cfg[stream->id];
248 stream_cfg->is_discard = false;
249 for (i = 0; i < ROCKIT_BUF_NUM_MAX; i++) {
250 if (stream_cfg->rkcif_buff[i]) {
251 rkcif_buf = (struct rkcif_rockit_buffer *)stream_cfg->rkcif_buff[i];
252 if (rkcif_buf->mpi_mem) {
253 g_ops->unmap_dmabuf(rkcif_buf->mpi_mem);
254 g_ops->detach_dmabuf(rkcif_buf->mpi_mem);
255 dma_buf_put(rkcif_buf->dmabuf);
256 }
257 kfree(stream_cfg->rkcif_buff[i]);
258 stream_cfg->rkcif_buff[i] = NULL;
259 stream_cfg->buff_id[i] = 0;
260 }
261 }
262 stream->curr_buf_rockit = NULL;
263 stream->next_buf_rockit = NULL;
264 INIT_LIST_HEAD(&stream->rockit_buf_head);
265 return 0;
266 }
267
rkcif_rockit_pause_stream(struct rockit_rkcif_cfg * input_rockit_cfg)268 int rkcif_rockit_pause_stream(struct rockit_rkcif_cfg *input_rockit_cfg)
269 {
270 struct rkcif_stream *stream = NULL;
271
272 stream = rkcif_rockit_get_stream(input_rockit_cfg);
273
274 if (stream == NULL) {
275 pr_err("the stream is NULL");
276 return -EINVAL;
277 }
278
279 rkcif_do_stop_stream(stream, RKCIF_STREAM_MODE_ROCKIT);
280 rkcif_rockit_buf_free(stream);
281 return 0;
282 }
283 EXPORT_SYMBOL(rkcif_rockit_pause_stream);
284
rkcif_rockit_config_stream(struct rockit_rkcif_cfg * input_rockit_cfg,int width,int height,int v4l2_fmt)285 int rkcif_rockit_config_stream(struct rockit_rkcif_cfg *input_rockit_cfg,
286 int width, int height, int v4l2_fmt)
287 {
288 struct rkcif_stream *stream = NULL;
289 int ret;
290
291 stream = rkcif_rockit_get_stream(input_rockit_cfg);
292
293 if (stream == NULL) {
294 pr_err("the stream is NULL");
295 return -EINVAL;
296 }
297 stream->pixm.pixelformat = v4l2_fmt;
298 stream->pixm.width = width;
299 stream->pixm.height = height;
300 stream->pixm.plane_fmt[0].bytesperline = 0;
301 ret = rkcif_set_fmt(stream, &stream->pixm, false);
302 if (ret < 0) {
303 pr_err("stream id %d config failed\n", stream->id);
304 return -EINVAL;
305 }
306
307 return 0;
308 }
309 EXPORT_SYMBOL(rkcif_rockit_config_stream);
310
rkcif_rockit_resume_stream(struct rockit_rkcif_cfg * input_rockit_cfg)311 int rkcif_rockit_resume_stream(struct rockit_rkcif_cfg *input_rockit_cfg)
312 {
313 struct rkcif_stream *stream = NULL;
314 int ret = 0;
315
316 stream = rkcif_rockit_get_stream(input_rockit_cfg);
317
318 if (stream == NULL) {
319 pr_err("the stream is NULL");
320 return -EINVAL;
321 }
322
323 ret = rkcif_do_start_stream(stream, RKCIF_STREAM_MODE_ROCKIT);
324 if (ret < 0) {
325 pr_err("stream id %d start failed\n", stream->id);
326 return -EINVAL;
327 }
328
329 return 0;
330 }
331 EXPORT_SYMBOL(rkcif_rockit_resume_stream);
332
rkcif_rockit_dev_init(struct rkcif_device * dev)333 void rkcif_rockit_dev_init(struct rkcif_device *dev)
334 {
335 int i;
336
337 if (rockit_rkcif_cfg == NULL) {
338 rockit_rkcif_cfg = kzalloc(sizeof(struct rockit_rkcif_cfg), GFP_KERNEL);
339 if (rockit_rkcif_cfg == NULL)
340 return;
341 }
342 rockit_rkcif_cfg->cif_num = dev->hw_dev->dev_num;
343 for (i = 0; i < rockit_rkcif_cfg->cif_num; i++) {
344 if (dev->hw_dev->cif_dev[i]) {
345 rockit_rkcif_cfg->rkcif_dev_cfg[i].cif_name = dev_name(dev->hw_dev->cif_dev[i]->dev);
346 rockit_rkcif_cfg->rkcif_dev_cfg[i].cif_dev =
347 dev->hw_dev->cif_dev[i];
348 }
349 }
350 }
351
rkcif_rockit_dev_deinit(void)352 void rkcif_rockit_dev_deinit(void)
353 {
354 kfree(rockit_rkcif_cfg);
355 rockit_rkcif_cfg = NULL;
356 }
357
rkcif_rockit_function_register(void * function,int cmd)358 void *rkcif_rockit_function_register(void *function, int cmd)
359 {
360 if (rockit_rkcif_cfg == NULL) {
361 pr_err("rockit_cfg is null function register failed");
362 return NULL;
363 }
364
365 switch (cmd) {
366 case ROCKIT_BUF_QUE:
367 function = rkcif_rockit_buf_queue;
368 break;
369 case ROCKIT_MPIBUF_DONE:
370 rockit_rkcif_cfg->rkcif_rockit_mpibuf_done = function;
371 if (!rockit_rkcif_cfg->rkcif_rockit_mpibuf_done)
372 pr_err("get rkcif_rockit_mpibuf_done failed!");
373 break;
374 default:
375 break;
376 }
377 return function;
378 }
379 EXPORT_SYMBOL(rkcif_rockit_function_register);
380
rkcif_rockit_get_cifdev(char ** name)381 int rkcif_rockit_get_cifdev(char **name)
382 {
383 int i = 0;
384
385 if (rockit_rkcif_cfg == NULL) {
386 pr_err("rockit_cfg is null");
387 return -EINVAL;
388 }
389
390 if (name == NULL) {
391 pr_err("the name is null");
392 return -EINVAL;
393 }
394
395 for (i = 0; i < rockit_rkcif_cfg->cif_num; i++)
396 name[i] = (char *)rockit_rkcif_cfg->rkcif_dev_cfg[i].cif_name;
397 if (name[0] == NULL)
398 return -EINVAL;
399 else
400 return 0;
401 }
402 EXPORT_SYMBOL(rkcif_rockit_get_cifdev);
403