1
2
3
4
5
6
7
8
9
10#include <linux/device.h>
11#include <linux/gfp.h>
12
13#include <media/media-entity.h>
14#include <media/v4l2-ctrls.h>
15#include <media/v4l2-subdev.h>
16
17#include "vsp1.h"
18#include "vsp1_dl.h"
19#include "vsp1_entity.h"
20#include "vsp1_pipe.h"
21#include "vsp1_rwpf.h"
22
23void vsp1_entity_route_setup(struct vsp1_entity *entity,
24 struct vsp1_pipeline *pipe,
25 struct vsp1_dl_body *dlb)
26{
27 struct vsp1_entity *source;
28 u32 route;
29
30 if (entity->type == VSP1_ENTITY_HGO) {
31 u32 smppt;
32
33
34
35
36
37 source = entity->sources[0];
38 smppt = (pipe->output->entity.index << VI6_DPR_SMPPT_TGW_SHIFT)
39 | (source->route->output << VI6_DPR_SMPPT_PT_SHIFT);
40
41 vsp1_dl_body_write(dlb, VI6_DPR_HGO_SMPPT, smppt);
42 return;
43 } else if (entity->type == VSP1_ENTITY_HGT) {
44 u32 smppt;
45
46
47
48
49
50 source = entity->sources[0];
51 smppt = (pipe->output->entity.index << VI6_DPR_SMPPT_TGW_SHIFT)
52 | (source->route->output << VI6_DPR_SMPPT_PT_SHIFT);
53
54 vsp1_dl_body_write(dlb, VI6_DPR_HGT_SMPPT, smppt);
55 return;
56 }
57
58 source = entity;
59 if (source->route->reg == 0)
60 return;
61
62 route = source->sink->route->inputs[source->sink_pad];
63
64
65
66
67 if (source->type == VSP1_ENTITY_BRS)
68 route |= VI6_DPR_ROUTE_BRSSEL;
69 vsp1_dl_body_write(dlb, source->route->reg, route);
70}
71
72void vsp1_entity_configure_stream(struct vsp1_entity *entity,
73 struct vsp1_pipeline *pipe,
74 struct vsp1_dl_list *dl,
75 struct vsp1_dl_body *dlb)
76{
77 if (entity->ops->configure_stream)
78 entity->ops->configure_stream(entity, pipe, dl, dlb);
79}
80
81void vsp1_entity_configure_frame(struct vsp1_entity *entity,
82 struct vsp1_pipeline *pipe,
83 struct vsp1_dl_list *dl,
84 struct vsp1_dl_body *dlb)
85{
86 if (entity->ops->configure_frame)
87 entity->ops->configure_frame(entity, pipe, dl, dlb);
88}
89
90void vsp1_entity_configure_partition(struct vsp1_entity *entity,
91 struct vsp1_pipeline *pipe,
92 struct vsp1_dl_list *dl,
93 struct vsp1_dl_body *dlb)
94{
95 if (entity->ops->configure_partition)
96 entity->ops->configure_partition(entity, pipe, dl, dlb);
97}
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117struct v4l2_subdev_state *
118vsp1_entity_get_pad_config(struct vsp1_entity *entity,
119 struct v4l2_subdev_state *sd_state,
120 enum v4l2_subdev_format_whence which)
121{
122 switch (which) {
123 case V4L2_SUBDEV_FORMAT_ACTIVE:
124 return entity->config;
125 case V4L2_SUBDEV_FORMAT_TRY:
126 default:
127 return sd_state;
128 }
129}
130
131
132
133
134
135
136
137
138
139
140struct v4l2_mbus_framefmt *
141vsp1_entity_get_pad_format(struct vsp1_entity *entity,
142 struct v4l2_subdev_state *sd_state,
143 unsigned int pad)
144{
145 return v4l2_subdev_get_try_format(&entity->subdev, sd_state, pad);
146}
147
148
149
150
151
152
153
154
155
156
157
158
159struct v4l2_rect *
160vsp1_entity_get_pad_selection(struct vsp1_entity *entity,
161 struct v4l2_subdev_state *sd_state,
162 unsigned int pad, unsigned int target)
163{
164 switch (target) {
165 case V4L2_SEL_TGT_COMPOSE:
166 return v4l2_subdev_get_try_compose(&entity->subdev, sd_state,
167 pad);
168 case V4L2_SEL_TGT_CROP:
169 return v4l2_subdev_get_try_crop(&entity->subdev, sd_state,
170 pad);
171 default:
172 return NULL;
173 }
174}
175
176
177
178
179
180
181
182
183
184int vsp1_entity_init_cfg(struct v4l2_subdev *subdev,
185 struct v4l2_subdev_state *sd_state)
186{
187 struct v4l2_subdev_format format;
188 unsigned int pad;
189
190 for (pad = 0; pad < subdev->entity.num_pads - 1; ++pad) {
191 memset(&format, 0, sizeof(format));
192
193 format.pad = pad;
194 format.which = sd_state ? V4L2_SUBDEV_FORMAT_TRY
195 : V4L2_SUBDEV_FORMAT_ACTIVE;
196
197 v4l2_subdev_call(subdev, pad, set_fmt, sd_state, &format);
198 }
199
200 return 0;
201}
202
203
204
205
206
207
208
209
210
211
212int vsp1_subdev_get_pad_format(struct v4l2_subdev *subdev,
213 struct v4l2_subdev_state *sd_state,
214 struct v4l2_subdev_format *fmt)
215{
216 struct vsp1_entity *entity = to_vsp1_entity(subdev);
217 struct v4l2_subdev_state *config;
218
219 config = vsp1_entity_get_pad_config(entity, sd_state, fmt->which);
220 if (!config)
221 return -EINVAL;
222
223 mutex_lock(&entity->lock);
224 fmt->format = *vsp1_entity_get_pad_format(entity, config, fmt->pad);
225 mutex_unlock(&entity->lock);
226
227 return 0;
228}
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243int vsp1_subdev_enum_mbus_code(struct v4l2_subdev *subdev,
244 struct v4l2_subdev_state *sd_state,
245 struct v4l2_subdev_mbus_code_enum *code,
246 const unsigned int *codes, unsigned int ncodes)
247{
248 struct vsp1_entity *entity = to_vsp1_entity(subdev);
249
250 if (code->pad == 0) {
251 if (code->index >= ncodes)
252 return -EINVAL;
253
254 code->code = codes[code->index];
255 } else {
256 struct v4l2_subdev_state *config;
257 struct v4l2_mbus_framefmt *format;
258
259
260
261
262
263 if (code->index)
264 return -EINVAL;
265
266 config = vsp1_entity_get_pad_config(entity, sd_state,
267 code->which);
268 if (!config)
269 return -EINVAL;
270
271 mutex_lock(&entity->lock);
272 format = vsp1_entity_get_pad_format(entity, config, 0);
273 code->code = format->code;
274 mutex_unlock(&entity->lock);
275 }
276
277 return 0;
278}
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295int vsp1_subdev_enum_frame_size(struct v4l2_subdev *subdev,
296 struct v4l2_subdev_state *sd_state,
297 struct v4l2_subdev_frame_size_enum *fse,
298 unsigned int min_width, unsigned int min_height,
299 unsigned int max_width, unsigned int max_height)
300{
301 struct vsp1_entity *entity = to_vsp1_entity(subdev);
302 struct v4l2_subdev_state *config;
303 struct v4l2_mbus_framefmt *format;
304 int ret = 0;
305
306 config = vsp1_entity_get_pad_config(entity, sd_state, fse->which);
307 if (!config)
308 return -EINVAL;
309
310 format = vsp1_entity_get_pad_format(entity, config, fse->pad);
311
312 mutex_lock(&entity->lock);
313
314 if (fse->index || fse->code != format->code) {
315 ret = -EINVAL;
316 goto done;
317 }
318
319 if (fse->pad == 0) {
320 fse->min_width = min_width;
321 fse->max_width = max_width;
322 fse->min_height = min_height;
323 fse->max_height = max_height;
324 } else {
325
326
327
328
329 fse->min_width = format->width;
330 fse->max_width = format->width;
331 fse->min_height = format->height;
332 fse->max_height = format->height;
333 }
334
335done:
336 mutex_unlock(&entity->lock);
337 return ret;
338}
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358int vsp1_subdev_set_pad_format(struct v4l2_subdev *subdev,
359 struct v4l2_subdev_state *sd_state,
360 struct v4l2_subdev_format *fmt,
361 const unsigned int *codes, unsigned int ncodes,
362 unsigned int min_width, unsigned int min_height,
363 unsigned int max_width, unsigned int max_height)
364{
365 struct vsp1_entity *entity = to_vsp1_entity(subdev);
366 struct v4l2_subdev_state *config;
367 struct v4l2_mbus_framefmt *format;
368 struct v4l2_rect *selection;
369 unsigned int i;
370 int ret = 0;
371
372 mutex_lock(&entity->lock);
373
374 config = vsp1_entity_get_pad_config(entity, sd_state, fmt->which);
375 if (!config) {
376 ret = -EINVAL;
377 goto done;
378 }
379
380 format = vsp1_entity_get_pad_format(entity, config, fmt->pad);
381
382 if (fmt->pad == entity->source_pad) {
383
384 fmt->format = *format;
385 goto done;
386 }
387
388
389
390
391
392 for (i = 0; i < ncodes; ++i) {
393 if (fmt->format.code == codes[i])
394 break;
395 }
396
397 format->code = i < ncodes ? codes[i] : codes[0];
398 format->width = clamp_t(unsigned int, fmt->format.width,
399 min_width, max_width);
400 format->height = clamp_t(unsigned int, fmt->format.height,
401 min_height, max_height);
402 format->field = V4L2_FIELD_NONE;
403 format->colorspace = V4L2_COLORSPACE_SRGB;
404
405 fmt->format = *format;
406
407
408 format = vsp1_entity_get_pad_format(entity, config, entity->source_pad);
409 *format = fmt->format;
410
411
412 selection = vsp1_entity_get_pad_selection(entity, config, fmt->pad,
413 V4L2_SEL_TGT_CROP);
414 selection->left = 0;
415 selection->top = 0;
416 selection->width = format->width;
417 selection->height = format->height;
418
419 selection = vsp1_entity_get_pad_selection(entity, config, fmt->pad,
420 V4L2_SEL_TGT_COMPOSE);
421 selection->left = 0;
422 selection->top = 0;
423 selection->width = format->width;
424 selection->height = format->height;
425
426done:
427 mutex_unlock(&entity->lock);
428 return ret;
429}
430
431
432
433
434
435static inline struct vsp1_entity *
436media_entity_to_vsp1_entity(struct media_entity *entity)
437{
438 return container_of(entity, struct vsp1_entity, subdev.entity);
439}
440
441static int vsp1_entity_link_setup_source(const struct media_pad *source_pad,
442 const struct media_pad *sink_pad,
443 u32 flags)
444{
445 struct vsp1_entity *source;
446
447 source = media_entity_to_vsp1_entity(source_pad->entity);
448
449 if (!source->route)
450 return 0;
451
452 if (flags & MEDIA_LNK_FL_ENABLED) {
453 struct vsp1_entity *sink
454 = media_entity_to_vsp1_entity(sink_pad->entity);
455
456
457
458
459
460 if (sink->type != VSP1_ENTITY_HGO &&
461 sink->type != VSP1_ENTITY_HGT) {
462 if (source->sink)
463 return -EBUSY;
464 source->sink = sink;
465 source->sink_pad = sink_pad->index;
466 }
467 } else {
468 source->sink = NULL;
469 source->sink_pad = 0;
470 }
471
472 return 0;
473}
474
475static int vsp1_entity_link_setup_sink(const struct media_pad *source_pad,
476 const struct media_pad *sink_pad,
477 u32 flags)
478{
479 struct vsp1_entity *sink;
480 struct vsp1_entity *source;
481
482 sink = media_entity_to_vsp1_entity(sink_pad->entity);
483 source = media_entity_to_vsp1_entity(source_pad->entity);
484
485 if (flags & MEDIA_LNK_FL_ENABLED) {
486
487 if (sink->sources[sink_pad->index])
488 return -EBUSY;
489
490 sink->sources[sink_pad->index] = source;
491 } else {
492 sink->sources[sink_pad->index] = NULL;
493 }
494
495 return 0;
496}
497
498int vsp1_entity_link_setup(struct media_entity *entity,
499 const struct media_pad *local,
500 const struct media_pad *remote, u32 flags)
501{
502 if (local->flags & MEDIA_PAD_FL_SOURCE)
503 return vsp1_entity_link_setup_source(local, remote, flags);
504 else
505 return vsp1_entity_link_setup_sink(remote, local, flags);
506}
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525struct media_pad *vsp1_entity_remote_pad(struct media_pad *pad)
526{
527 struct media_link *link;
528
529 list_for_each_entry(link, &pad->entity->links, list) {
530 struct vsp1_entity *entity;
531
532 if (!(link->flags & MEDIA_LNK_FL_ENABLED))
533 continue;
534
535
536 if (link->sink == pad)
537 return link->source;
538
539 if (link->source != pad)
540 continue;
541
542
543 if (!is_media_entity_v4l2_subdev(link->sink->entity))
544 return link->sink;
545
546 entity = media_entity_to_vsp1_entity(link->sink->entity);
547 if (entity->type != VSP1_ENTITY_HGO &&
548 entity->type != VSP1_ENTITY_HGT)
549 return link->sink;
550 }
551
552 return NULL;
553
554}
555
556
557
558
559
560#define VSP1_ENTITY_ROUTE(ent) \
561 { VSP1_ENTITY_##ent, 0, VI6_DPR_##ent##_ROUTE, \
562 { VI6_DPR_NODE_##ent }, VI6_DPR_NODE_##ent }
563
564#define VSP1_ENTITY_ROUTE_RPF(idx) \
565 { VSP1_ENTITY_RPF, idx, VI6_DPR_RPF_ROUTE(idx), \
566 { 0, }, VI6_DPR_NODE_RPF(idx) }
567
568#define VSP1_ENTITY_ROUTE_UDS(idx) \
569 { VSP1_ENTITY_UDS, idx, VI6_DPR_UDS_ROUTE(idx), \
570 { VI6_DPR_NODE_UDS(idx) }, VI6_DPR_NODE_UDS(idx) }
571
572#define VSP1_ENTITY_ROUTE_UIF(idx) \
573 { VSP1_ENTITY_UIF, idx, VI6_DPR_UIF_ROUTE(idx), \
574 { VI6_DPR_NODE_UIF(idx) }, VI6_DPR_NODE_UIF(idx) }
575
576#define VSP1_ENTITY_ROUTE_WPF(idx) \
577 { VSP1_ENTITY_WPF, idx, 0, \
578 { VI6_DPR_NODE_WPF(idx) }, VI6_DPR_NODE_WPF(idx) }
579
580static const struct vsp1_route vsp1_routes[] = {
581 { VSP1_ENTITY_BRS, 0, VI6_DPR_ILV_BRS_ROUTE,
582 { VI6_DPR_NODE_BRS_IN(0), VI6_DPR_NODE_BRS_IN(1) }, 0 },
583 { VSP1_ENTITY_BRU, 0, VI6_DPR_BRU_ROUTE,
584 { VI6_DPR_NODE_BRU_IN(0), VI6_DPR_NODE_BRU_IN(1),
585 VI6_DPR_NODE_BRU_IN(2), VI6_DPR_NODE_BRU_IN(3),
586 VI6_DPR_NODE_BRU_IN(4) }, VI6_DPR_NODE_BRU_OUT },
587 VSP1_ENTITY_ROUTE(CLU),
588 { VSP1_ENTITY_HGO, 0, 0, { 0, }, 0 },
589 { VSP1_ENTITY_HGT, 0, 0, { 0, }, 0 },
590 VSP1_ENTITY_ROUTE(HSI),
591 VSP1_ENTITY_ROUTE(HST),
592 { VSP1_ENTITY_LIF, 0, 0, { 0, }, 0 },
593 { VSP1_ENTITY_LIF, 1, 0, { 0, }, 0 },
594 VSP1_ENTITY_ROUTE(LUT),
595 VSP1_ENTITY_ROUTE_RPF(0),
596 VSP1_ENTITY_ROUTE_RPF(1),
597 VSP1_ENTITY_ROUTE_RPF(2),
598 VSP1_ENTITY_ROUTE_RPF(3),
599 VSP1_ENTITY_ROUTE_RPF(4),
600 VSP1_ENTITY_ROUTE(SRU),
601 VSP1_ENTITY_ROUTE_UDS(0),
602 VSP1_ENTITY_ROUTE_UDS(1),
603 VSP1_ENTITY_ROUTE_UDS(2),
604 VSP1_ENTITY_ROUTE_UIF(0),
605 VSP1_ENTITY_ROUTE_UIF(1),
606 VSP1_ENTITY_ROUTE_WPF(0),
607 VSP1_ENTITY_ROUTE_WPF(1),
608 VSP1_ENTITY_ROUTE_WPF(2),
609 VSP1_ENTITY_ROUTE_WPF(3),
610};
611
612int vsp1_entity_init(struct vsp1_device *vsp1, struct vsp1_entity *entity,
613 const char *name, unsigned int num_pads,
614 const struct v4l2_subdev_ops *ops, u32 function)
615{
616 struct v4l2_subdev *subdev;
617 unsigned int i;
618 int ret;
619
620 for (i = 0; i < ARRAY_SIZE(vsp1_routes); ++i) {
621 if (vsp1_routes[i].type == entity->type &&
622 vsp1_routes[i].index == entity->index) {
623 entity->route = &vsp1_routes[i];
624 break;
625 }
626 }
627
628 if (i == ARRAY_SIZE(vsp1_routes))
629 return -EINVAL;
630
631 mutex_init(&entity->lock);
632
633 entity->vsp1 = vsp1;
634 entity->source_pad = num_pads - 1;
635
636
637 entity->pads = devm_kcalloc(vsp1->dev,
638 num_pads, sizeof(*entity->pads),
639 GFP_KERNEL);
640 if (entity->pads == NULL)
641 return -ENOMEM;
642
643 for (i = 0; i < num_pads - 1; ++i)
644 entity->pads[i].flags = MEDIA_PAD_FL_SINK;
645
646 entity->sources = devm_kcalloc(vsp1->dev, max(num_pads - 1, 1U),
647 sizeof(*entity->sources), GFP_KERNEL);
648 if (entity->sources == NULL)
649 return -ENOMEM;
650
651
652 entity->pads[num_pads - 1].flags = num_pads > 1 ? MEDIA_PAD_FL_SOURCE
653 : MEDIA_PAD_FL_SINK;
654
655
656 ret = media_entity_pads_init(&entity->subdev.entity, num_pads,
657 entity->pads);
658 if (ret < 0)
659 return ret;
660
661
662 subdev = &entity->subdev;
663 v4l2_subdev_init(subdev, ops);
664
665 subdev->entity.function = function;
666 subdev->entity.ops = &vsp1->media_ops;
667 subdev->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE;
668
669 snprintf(subdev->name, sizeof(subdev->name), "%s %s",
670 dev_name(vsp1->dev), name);
671
672 vsp1_entity_init_cfg(subdev, NULL);
673
674
675
676
677
678 entity->config = v4l2_subdev_alloc_state(&entity->subdev);
679 if (IS_ERR(entity->config)) {
680 media_entity_cleanup(&entity->subdev.entity);
681 return PTR_ERR(entity->config);
682 }
683
684 return 0;
685}
686
687void vsp1_entity_destroy(struct vsp1_entity *entity)
688{
689 if (entity->ops && entity->ops->destroy)
690 entity->ops->destroy(entity);
691 if (entity->subdev.ctrl_handler)
692 v4l2_ctrl_handler_free(entity->subdev.ctrl_handler);
693 v4l2_subdev_free_state(entity->config);
694 media_entity_cleanup(&entity->subdev.entity);
695}
696