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