imx8-isi-crossbar.c

  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	routing.num_routes = min(xbar->num_sinks - 1, xbar->num_sources);
192	routes = kcalloc(routing.num_routes, sizeof(*routes), GFP_KERNEL);
193	if (!routes)
194		return -ENOMEM;
195
196	for (i = 0; i < routing.num_routes; ++i) {
197		struct v4l2_subdev_route *route = &routes[i];
198
199		route->sink_pad = i;
200		route->source_pad = i + xbar->num_sinks;
201		route->flags = V4L2_SUBDEV_ROUTE_FL_ACTIVE;
202	}
203
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 enable streams 0x%llx on '%s':%u: %d\n",
356				sink_streams, remote_sd->name, remote_pad, ret);
357			mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
358			return ret;
359		}
360	}
361
362	input->enable_count++;
363
364	return 0;
365}
366
367static int mxc_isi_crossbar_disable_streams(struct v4l2_subdev *sd,
368					    struct v4l2_subdev_state *state,
369					    u32 pad, u64 streams_mask)
370{
371	struct mxc_isi_crossbar *xbar = to_isi_crossbar(sd);
372	struct v4l2_subdev *remote_sd;
373	struct mxc_isi_input *input;
374	u64 sink_streams;
375	u32 sink_pad;
376	u32 remote_pad;
377	int ret = 0;
378
379	remote_sd = mxc_isi_crossbar_xlate_streams(xbar, state, pad, streams_mask,
380						   &sink_pad, &sink_streams,
381						   &remote_pad);
382	if (IS_ERR(remote_sd))
383		return PTR_ERR(remote_sd);
384
385	input = &xbar->inputs[sink_pad];
386
387	input->enable_count--;
388
389	if (!input->enable_count) {
390		ret = v4l2_subdev_disable_streams(remote_sd, remote_pad,
391						  sink_streams);
392		if (ret)
393			dev_err(xbar->isi->dev,
394				"failed to disable streams 0x%llx on '%s':%u: %d\n",
395				sink_streams, remote_sd->name, remote_pad, ret);
396
397		mxc_isi_crossbar_gasket_disable(xbar, sink_pad);
398	}
399
400	return ret;
401}
402
403static const struct v4l2_subdev_pad_ops mxc_isi_crossbar_subdev_pad_ops = {
404	.enum_mbus_code = mxc_isi_crossbar_enum_mbus_code,
405	.get_fmt = v4l2_subdev_get_fmt,
406	.set_fmt = mxc_isi_crossbar_set_fmt,
407	.set_routing = mxc_isi_crossbar_set_routing,
408	.enable_streams = mxc_isi_crossbar_enable_streams,
409	.disable_streams = mxc_isi_crossbar_disable_streams,
410};
411
412static const struct v4l2_subdev_ops mxc_isi_crossbar_subdev_ops = {
413	.pad = &mxc_isi_crossbar_subdev_pad_ops,
414};
415
416static const struct v4l2_subdev_internal_ops mxc_isi_crossbar_internal_ops = {
417	.init_state = mxc_isi_crossbar_init_state,
418};
419
420static const struct media_entity_operations mxc_isi_cross_entity_ops = {
421	.get_fwnode_pad = v4l2_subdev_get_fwnode_pad_1_to_1,
422	.link_validate	= v4l2_subdev_link_validate,
423	.has_pad_interdep = v4l2_subdev_has_pad_interdep,
424};
425
426/* -----------------------------------------------------------------------------
427 * Init & cleanup
428 */
429
430int mxc_isi_crossbar_init(struct mxc_isi_dev *isi)
431{
432	struct mxc_isi_crossbar *xbar = &isi->crossbar;
433	struct v4l2_subdev *sd = &xbar->sd;
434	unsigned int num_pads;
435	unsigned int i;
436	int ret;
437
438	xbar->isi = isi;
439
440	v4l2_subdev_init(sd, &mxc_isi_crossbar_subdev_ops);
441	sd->internal_ops = &mxc_isi_crossbar_internal_ops;
442	sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE | V4L2_SUBDEV_FL_STREAMS;
443	strscpy(sd->name, "crossbar", sizeof(sd->name));
444	sd->dev = isi->dev;
445
446	sd->entity.function = MEDIA_ENT_F_VID_MUX;
447	sd->entity.ops = &mxc_isi_cross_entity_ops;
448
449	/*
450	 * The subdev has one sink and one source per port, plus one sink for
451	 * the memory input.
452	 */
453	xbar->num_sinks = isi->pdata->num_ports + 1;
454	xbar->num_sources = isi->pdata->num_channels;
455	num_pads = xbar->num_sinks + xbar->num_sources;
456
457	xbar->pads = kcalloc(num_pads, sizeof(*xbar->pads), GFP_KERNEL);
458	if (!xbar->pads)
459		return -ENOMEM;
460
461	xbar->inputs = kcalloc(xbar->num_sinks, sizeof(*xbar->inputs),
462			       GFP_KERNEL);
463	if (!xbar->inputs) {
464		ret = -ENOMEM;
465		goto err_free;
466	}
467
468	for (i = 0; i < xbar->num_sinks; ++i)
469		xbar->pads[i].flags = MEDIA_PAD_FL_SINK
470				    | MEDIA_PAD_FL_MUST_CONNECT;
471	for (i = 0; i < xbar->num_sources; ++i)
472		xbar->pads[i + xbar->num_sinks].flags = MEDIA_PAD_FL_SOURCE;
473
474	ret = media_entity_pads_init(&sd->entity, num_pads, xbar->pads);
475	if (ret)
476		goto err_free;
477
478	ret = v4l2_subdev_init_finalize(sd);
479	if (ret < 0)
480		goto err_entity;
481
482	return 0;
483
484err_entity:
485	media_entity_cleanup(&sd->entity);
486err_free:
487	kfree(xbar->pads);
488	kfree(xbar->inputs);
489
490	return ret;
491}
492
493void mxc_isi_crossbar_cleanup(struct mxc_isi_crossbar *xbar)
494{
495	media_entity_cleanup(&xbar->sd.entity);
496	kfree(xbar->pads);
497	kfree(xbar->inputs);
498}
499
500int mxc_isi_crossbar_register(struct mxc_isi_crossbar *xbar)
501{
502	return v4l2_device_register_subdev(&xbar->isi->v4l2_dev, &xbar->sd);
503}
504
505void mxc_isi_crossbar_unregister(struct mxc_isi_crossbar *xbar)
506{
507}