1// SPDX-License-Identifier: GPL-2.0-only
2/*
3 * i.MX8 ISI - Input crossbar switch
4 *
5 * Copyright (c) 2022 Laurent Pinchart <laurent.pinchart@ideasonboard.com>
6 */
7
8#include <linux/device.h>
9#include <linux/errno.h>
10#include <linux/kernel.h>
11#include <linux/minmax.h>
12#include <linux/regmap.h>
13#include <linux/slab.h>
14#include <linux/string.h>
15#include <linux/types.h>
16
17#include <media/media-entity.h>
18#include <media/v4l2-subdev.h>
19
20#include "imx8-isi-core.h"
21
22static inline struct mxc_isi_crossbar *to_isi_crossbar(struct v4l2_subdev *sd)
23{
24	return container_of(sd, struct mxc_isi_crossbar, sd);
25}
26
27static int mxc_isi_crossbar_gasket_enable(struct mxc_isi_crossbar *xbar,
28					  struct v4l2_subdev_state *state,
29					  struct v4l2_subdev *remote_sd,
30					  u32 remote_pad, unsigned int port)
31{
32	struct mxc_isi_dev *isi = xbar->isi;
33	const struct mxc_gasket_ops *gasket_ops = isi->pdata->gasket_ops;
34	const struct v4l2_mbus_framefmt *fmt;
35	struct v4l2_mbus_frame_desc fd;
36	int ret;
37
38	if (!gasket_ops)
39		return 0;
40
41	/*
42	 * Configure and enable the gasket with the frame size and CSI-2 data
43	 * type. For YUV422 8-bit, enable dual component mode unconditionally,
44	 * to match the configuration of the CSIS.
45	 */
46
47	ret = v4l2_subdev_call(remote_sd, pad, get_frame_desc, remote_pad, &fd);
48	if (ret) {
49		dev_err(isi->dev,
50			"failed to get frame descriptor from '%s':%u: %d\n",
51			remote_sd->name, remote_pad, ret);
52		return ret;
53	}
54
55	if (fd.num_entries != 1) {
56		dev_err(isi->dev, "invalid frame descriptor for '%s':%u\n",
57			remote_sd->name, remote_pad);
58		return -EINVAL;
59	}
60
61	fmt = v4l2_subdev_state_get_format(state, port, 0);
62	if (!fmt)
63		return -EINVAL;
64
65	gasket_ops->enable(isi, &fd, fmt, port);
66	return 0;
67}
68
69static void mxc_isi_crossbar_gasket_disable(struct mxc_isi_crossbar *xbar,
70					    unsigned int port)
71{
72	struct mxc_isi_dev *isi = xbar->isi;
73	const struct mxc_gasket_ops *gasket_ops = isi->pdata->gasket_ops;
74
75	if (!gasket_ops)
76		return;
77
78	gasket_ops->disable(isi, port);
79}
80
81/* -----------------------------------------------------------------------------
82 * V4L2 subdev operations
83 */
84
85static const struct v4l2_mbus_framefmt mxc_isi_crossbar_default_format = {
86	.code = MXC_ISI_DEF_MBUS_CODE_SINK,
87	.width = MXC_ISI_DEF_WIDTH,
88	.height = MXC_ISI_DEF_HEIGHT,
89	.field = V4L2_FIELD_NONE,
90	.colorspace = MXC_ISI_DEF_COLOR_SPACE,
91	.ycbcr_enc = MXC_ISI_DEF_YCBCR_ENC,
92	.quantization = MXC_ISI_DEF_QUANTIZATION,
93	.xfer_func = MXC_ISI_DEF_XFER_FUNC,
94};
95
96static int __mxc_isi_crossbar_set_routing(struct v4l2_subdev *sd,
97					  struct v4l2_subdev_state *state,
98					  struct v4l2_subdev_krouting *routing)
99{
100	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
101	struct v4l2_subdev_route *route;
102	int ret;
103
104	ret = v4l2_subdev_routing_validate(sd, routing,
105					   V4L2_SUBDEV_ROUTING_NO_N_TO_1);
106	if (ret)
107		return ret;
108
109	/* The memory input can be routed to the first pipeline only. */
110	for_each_active_route(&state->routing, route) {
111		if (route->sink_pad == xbar->num_sinks - 1 &&
112		    route->source_pad != xbar->num_sinks) {
113			dev_dbg(xbar->isi->dev,
114				"invalid route from memory input (%u) to pipe %u\n",
115				route->sink_pad,
116				route->source_pad - xbar->num_sinks);
117			return -EINVAL;
118		}
119	}
120
121	return v4l2_subdev_set_routing_with_fmt(sd, state, routing,
122						&mxc_isi_crossbar_default_format);
123}
124
125static struct v4l2_subdev *
126mxc_isi_crossbar_xlate_streams(struct mxc_isi_crossbar *xbar,
127			       struct v4l2_subdev_state *state,
128			       u32 source_pad, u64 source_streams,
129			       u32 *__sink_pad, u64 *__sink_streams,
130			       u32 *remote_pad)
131{
132	struct v4l2_subdev_route *route;
133	struct v4l2_subdev *sd;
134	struct media_pad *pad;
135	u64 sink_streams = 0;
136	int sink_pad = -1;
137
138	/*
139	 * Translate the source pad and streams to the sink side. The routing
140	 * validation forbids stream merging, so all matching entries in the
141	 * routing table are guaranteed to have the same sink pad.
142	 *
143	 * TODO: This is likely worth a helper function, it could perhaps be
144	 * supported by v4l2_subdev_state_xlate_streams() with pad1 set to -1.
145	 */
146	for_each_active_route(&state->routing, route) {
147		if (route->source_pad != source_pad ||
148		    !(source_streams & BIT(route->source_stream)))
149			continue;
150
151		sink_streams |= BIT(route->sink_stream);
152		sink_pad = route->sink_pad;
153	}
154
155	if (sink_pad < 0) {
156		dev_dbg(xbar->isi->dev,
157			"no stream connected to pipeline %u\n",
158			source_pad - xbar->num_sinks);
159		return ERR_PTR(-EPIPE);
160	}
161
162	pad = media_pad_remote_pad_first(&xbar->pads[sink_pad]);
163	sd = media_entity_to_v4l2_subdev(pad->entity);
164	if (!sd) {
165		dev_dbg(xbar->isi->dev,
166			"no entity connected to crossbar input %u\n",
167			sink_pad);
168		return ERR_PTR(-EPIPE);
169	}
170
171	*__sink_pad = sink_pad;
172	*__sink_streams = sink_streams;
173	*remote_pad = pad->index;
174
175	return sd;
176}
177
178static int mxc_isi_crossbar_init_state(struct v4l2_subdev *sd,
179				       struct v4l2_subdev_state *state)
180{
181	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
182	struct v4l2_subdev_krouting routing = { };
183	struct v4l2_subdev_route *routes;
184	unsigned int i;
185	int ret;
186
187	/*
188	 * Create a 1:1 mapping between pixel link inputs and outputs to
189	 * pipelines by default.
190	 */
191	routes = kcalloc(xbar->num_sources, sizeof(*routes), GFP_KERNEL);
192	if (!routes)
193		return -ENOMEM;
194
195	for (i = 0; i < xbar->num_sources; ++i) {
196		struct v4l2_subdev_route *route = &routes[i];
197
198		route->sink_pad = i;
199		route->source_pad = i + xbar->num_sinks;
200		route->flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE;
201	}
202
203	routing.num_routes = xbar->num_sources;
204	routing.routes = routes;
205
206	ret = __mxc_isi_crossbar_set_routing(sd, state, &routing);
207
208	kfree(routes);
209
210	return ret;
211}
212
213static int mxc_isi_crossbar_enum_mbus_code(struct v4l2_subdev *sd,
214					   struct v4l2_subdev_state *state,
215					   struct v4l2_subdev_mbus_code_enum *code)
216{
217	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
218	const struct mxc_isi_bus_format_info *info;
219
220	if (code->pad >= xbar->num_sinks) {
221		const struct v4l2_mbus_framefmt *format;
222
223		/*
224		 * The media bus code on source pads is identical to the
225		 * connected sink pad.
226		 */
227		if (code->index > 0)
228			return -EINVAL;
229
230		format = v4l2_subdev_state_get_opposite_stream_format(state,
231								      code->pad,
232								      code->stream);
233		if (!format)
234			return -EINVAL;
235
236		code->code = format->code;
237
238		return 0;
239	}
240
241	info = mxc_isi_bus_format_by_index(code->index, MXC_ISI_PIPE_PAD_SINK);
242	if (!info)
243		return -EINVAL;
244
245	code->code = info->mbus_code;
246
247	return 0;
248}
249
250static int mxc_isi_crossbar_set_fmt(struct v4l2_subdev *sd,
251				    struct v4l2_subdev_state *state,
252				    struct v4l2_subdev_format *fmt)
253{
254	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
255	struct v4l2_mbus_framefmt *sink_fmt;
256	struct v4l2_subdev_route *route;
257
258	if (fmt->which == V4L2_SUBDEV_FORMAT_ACTIVE &&
259	    media_pad_is_streaming(&xbar->pads[fmt->pad]))
260		return -EBUSY;
261
262	/*
263	 * The source pad format is always identical to the sink pad format and
264	 * can't be modified.
265	 */
266	if (fmt->pad >= xbar->num_sinks)
267		return v4l2_subdev_get_fmt(sd, state, fmt);
268
269	/* Validate the requested format. */
270	if (!mxc_isi_bus_format_by_code(fmt->format.code, MXC_ISI_PIPE_PAD_SINK))
271		fmt->format.code = MXC_ISI_DEF_MBUS_CODE_SINK;
272
273	fmt->format.width = clamp_t(unsigned int, fmt->format.width,
274				    MXC_ISI_MIN_WIDTH, MXC_ISI_MAX_WIDTH_CHAINED);
275	fmt->format.height = clamp_t(unsigned int, fmt->format.height,
276				     MXC_ISI_MIN_HEIGHT, MXC_ISI_MAX_HEIGHT);
277	fmt->format.field = V4L2_FIELD_NONE;
278
279	/*
280	 * Set the format on the sink stream and propagate it to the source
281	 * streams.
282	 */
283	sink_fmt = v4l2_subdev_state_get_format(state, fmt->pad, fmt->stream);
284	if (!sink_fmt)
285		return -EINVAL;
286
287	*sink_fmt = fmt->format;
288
289	/* TODO: A format propagation helper would be useful. */
290	for_each_active_route(&state->routing, route) {
291		struct v4l2_mbus_framefmt *source_fmt;
292
293		if (route->sink_pad != fmt->pad ||
294		    route->sink_stream != fmt->stream)
295			continue;
296
297		source_fmt = v4l2_subdev_state_get_format(state,
298							  route->source_pad,
299							  route->source_stream);
300		if (!source_fmt)
301			return -EINVAL;
302
303		*source_fmt = fmt->format;
304	}
305
306	return 0;
307}
308
309static int mxc_isi_crossbar_set_routing(struct v4l2_subdev *sd,
310					struct v4l2_subdev_state *state,
311					enum v4l2_subdev_format_whence which,
312					struct v4l2_subdev_krouting *routing)
313{
314	if (which == V4L2_SUBDEV_FORMAT_ACTIVE &&
315	    media_entity_is_streaming(&sd->entity))
316		return -EBUSY;
317
318	return __mxc_isi_crossbar_set_routing(sd, state, routing);
319}
320
321static int mxc_isi_crossbar_enable_streams(struct v4l2_subdev *sd,
322					   struct v4l2_subdev_state *state,
323					   u32 pad, u64 streams_mask)
324{
325	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
326	struct v4l2_subdev *remote_sd;
327	struct mxc_isi_input *input;
328	u64 sink_streams;
329	u32 sink_pad;
330	u32 remote_pad;
331	int ret;
332
333	remote_sd = mxc_isi_crossbar_xlate_streams(xbar, state, pad, streams_mask,
334						   &sink_pad, &sink_streams,
335						   &remote_pad);
336	if (IS_ERR(remote_sd))
337		return PTR_ERR(remote_sd);
338
339	input = &xbar->inputs[sink_pad];
340
341	/*
342	 * TODO: Track per-stream enable counts to support multiplexed
343	 * streams.
344	 */
345	if (!input->enable_count) {
346		ret = mxc_isi_crossbar_gasket_enable(xbar, state, remote_sd,
347						     remote_pad, sink_pad);
348		if (ret)
349			return ret;
350
351		ret = v4l2_subdev_enable_streams(remote_sd, remote_pad,
352						 sink_streams);
353		if (ret) {
354			dev_err(xbar->isi->dev,
355				"failed to %s streams 0x%llx on '%s':%u: %d\n",
356				"enable", sink_streams, remote_sd->name,
357				remote_pad, ret);
358			mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
359			return ret;
360		}
361	}
362
363	input->enable_count++;
364
365	return 0;
366}
367
368static int mxc_isi_crossbar_disable_streams(struct v4l2_subdev *sd,
369					    struct v4l2_subdev_state *state,
370					    u32 pad, u64 streams_mask)
371{
372	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
373	struct v4l2_subdev *remote_sd;
374	struct mxc_isi_input *input;
375	u64 sink_streams;
376	u32 sink_pad;
377	u32 remote_pad;
378	int ret = 0;
379
380	remote_sd = mxc_isi_crossbar_xlate_streams(xbar, state, pad, streams_mask,
381						   &sink_pad, &sink_streams,
382						   &remote_pad);
383	if (IS_ERR(remote_sd))
384		return PTR_ERR(remote_sd);
385
386	input = &xbar->inputs[sink_pad];
387
388	input->enable_count--;
389
390	if (!input->enable_count) {
391		ret = v4l2_subdev_disable_streams(remote_sd, remote_pad,
392						  sink_streams);
393		if (ret)
394			dev_err(xbar->isi->dev,
395				"failed to %s streams 0x%llx on '%s':%u: %d\n",
396				"disable", sink_streams, remote_sd->name,
397				remote_pad, ret);
398
399		mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
400	}
401
402	return ret;
403}
404
405static const struct v4l2_subdev_pad_ops mxc_isi_crossbar_subdev_pad_ops = {
406	.enum_mbus_code = mxc_isi_crossbar_enum_mbus_code,
407	.get_fmt = v4l2_subdev_get_fmt,
408	.set_fmt = mxc_isi_crossbar_set_fmt,
409	.set_routing = mxc_isi_crossbar_set_routing,
410	.enable_streams = mxc_isi_crossbar_enable_streams,
411	.disable_streams = mxc_isi_crossbar_disable_streams,
412};
413
414static const struct v4l2_subdev_ops mxc_isi_crossbar_subdev_ops = {
415	.pad = &mxc_isi_crossbar_subdev_pad_ops,
416};
417
418static const struct v4l2_subdev_internal_ops mxc_isi_crossbar_internal_ops = {
419	.init_state = mxc_isi_crossbar_init_state,
420};
421
422static const struct media_entity_operations mxc_isi_cross_entity_ops = {
423	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
424	.link_validate	= v4l2_subdev_link_validate,
425	.has_pad_interdep = v4l2_subdev_has_pad_interdep,
426};
427
428/* -----------------------------------------------------------------------------
429 * Init & cleanup
430 */
431
432int mxc_isi_crossbar_init(struct mxc_isi_dev *isi)
433{
434	struct mxc_isi_crossbar *xbar = &isi->crossbar;
435	struct v4l2_subdev *sd = &xbar->sd;
436	unsigned int num_pads;
437	unsigned int i;
438	int ret;
439
440	xbar->isi = isi;
441
442	v4l2_subdev_init(sd, &mxc_isi_crossbar_subdev_ops);
443	sd->internal_ops = &mxc_isi_crossbar_internal_ops;
444	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
445	strscpy(sd->name, "crossbar", sizeof(sd->name));
446	sd->dev = isi->dev;
447
448	sd->entity.function = MEDIA_ENT_F_VID_MUX;
449	sd->entity.ops = &mxc_isi_cross_entity_ops;
450
451	/*
452	 * The subdev has one sink and one source per port, plus one sink for
453	 * the memory input.
454	 */
455	xbar->num_sinks = isi->pdata->num_ports + 1;
456	xbar->num_sources = isi->pdata->num_ports;
457	num_pads = xbar->num_sinks + xbar->num_sources;
458
459	xbar->pads = kcalloc(num_pads, sizeof(*xbar->pads), GFP_KERNEL);
460	if (!xbar->pads)
461		return -ENOMEM;
462
463	xbar->inputs = kcalloc(xbar->num_sinks, sizeof(*xbar->inputs),
464			       GFP_KERNEL);
465	if (!xbar->inputs) {
466		ret = -ENOMEM;
467		goto err_free;
468	}
469
470	for (i = 0; i < xbar->num_sinks; ++i)
471		xbar->pads[i].flags = MEDIA_PAD_FL_SINK
472				    | MEDIA_PAD_FL_MUST_CONNECT;
473	for (i = 0; i < xbar->num_sources; ++i)
474		xbar->pads[i + xbar->num_sinks].flags = MEDIA_PAD_FL_SOURCE;
475
476	ret = media_entity_pads_init(&sd->entity, num_pads, xbar->pads);
477	if (ret)
478		goto err_free;
479
480	ret = v4l2_subdev_init_finalize(sd);
481	if (ret < 0)
482		goto err_entity;
483
484	return 0;
485
486err_entity:
487	media_entity_cleanup(&sd->entity);
488err_free:
489	kfree(xbar->pads);
490	kfree(xbar->inputs);
491
492	return ret;
493}
494
495void mxc_isi_crossbar_cleanup(struct mxc_isi_crossbar *xbar)
496{
497	media_entity_cleanup(&xbar->sd.entity);
498	kfree(xbar->pads);
499	kfree(xbar->inputs);
500}
501
502int mxc_isi_crossbar_register(struct mxc_isi_crossbar *xbar)
503{
504	return v4l2_device_register_subdev(&xbar->isi->v4l2_dev, &xbar->sd);
505}
506
507void mxc_isi_crossbar_unregister(struct mxc_isi_crossbar *xbar)
508{
509}
510