1// SPDX-License-Identifier: GPL-2.0+
2/*
3 * (C) Copyright 2008
4 * Gary Jennejohn, DENX Software Engineering GmbH, garyj@denx.de.
5 */
6
7#include <common.h>
8#include <console.h>
9#include <serial.h>
10#include <malloc.h>
11
12#if CONFIG_IS_ENABLED(CONSOLE_MUX)
13void iomux_printdevs(const int console)
14{
15	int i;
16	struct stdio_dev *dev;
17
18	for_each_console_dev(i, console, dev)
19		printf("%s ", dev->name);
20	printf("\n");
21}
22
23int iomux_match_device(struct stdio_dev **set, const int n, struct stdio_dev *sdev)
24{
25	int i;
26
27	for (i = 0; i < n; i++)
28		if (sdev == set[i])
29			return i;
30	return -ENOENT;
31}
32
33/* This tries to preserve the old list if an error occurs. */
34int iomux_doenv(const int console, const char *arg)
35{
36	char *console_args, *temp, **start;
37	int i, j, io_flag, cs_idx, repeat;
38	struct stdio_dev **cons_set, **old_set;
39	struct stdio_dev *dev;
40
41	console_args = strdup(arg);
42	if (console_args == NULL)
43		return 1;
44	/*
45	 * Check whether a comma separated list of devices was
46	 * entered and count how many devices were entered.
47	 * The array start[] has pointers to the beginning of
48	 * each device name (up to MAX_CONSARGS devices).
49	 *
50	 * Have to do this twice - once to count the number of
51	 * commas and then again to populate start.
52	 */
53	i = 0;
54	temp = console_args;
55	for (;;) {
56		/* There's always one entry more than the number of commas. */
57		i++;
58
59		temp = strchr(temp, ',');
60		if (temp == NULL)
61			break;
62
63		temp++;
64	}
65	start = (char **)malloc(i * sizeof(char *));
66	if (start == NULL) {
67		free(console_args);
68		return 1;
69	}
70	i = 0;
71	start[0] = console_args;
72	for (;;) {
73		temp = strchr(start[i++], ',');
74		if (temp == NULL)
75			break;
76		*temp = '\0';
77		start[i] = temp + 1;
78	}
79	cons_set = (struct stdio_dev **)calloc(i, sizeof(struct stdio_dev *));
80	if (cons_set == NULL) {
81		free(start);
82		free(console_args);
83		return 1;
84	}
85
86	io_flag = stdio_file_to_flags(console);
87	if (io_flag < 0) {
88		free(start);
89		free(console_args);
90		free(cons_set);
91		return 1;
92	}
93
94	cs_idx = 0;
95	for (j = 0; j < i; j++) {
96		/*
97		 * Check whether the device exists and is valid.
98		 * console_assign() also calls console_search_dev(),
99		 * but I need the pointer to the device.
100		 */
101		dev = console_search_dev(io_flag, start[j]);
102		if (dev == NULL)
103			continue;
104		/*
105		 * Prevent multiple entries for a device.
106		 */
107		 repeat = iomux_match_device(cons_set, cs_idx, dev);
108		 if (repeat >= 0)
109			continue;
110		/*
111		 * Try assigning the specified device.
112		 * This could screw up the console settings for apps.
113		 */
114		if (console_assign(console, start[j]) < 0)
115			continue;
116		cons_set[cs_idx++] = dev;
117	}
118	free(console_args);
119	free(start);
120	/* failed to set any console */
121	if (cs_idx == 0) {
122		free(cons_set);
123		return 1;
124	}
125
126	old_set = console_devices[console];
127	repeat = cd_count[console];
128
129	console_devices[console] = cons_set;
130	cd_count[console] = cs_idx;
131
132	/* Stop dropped consoles */
133	for (i = 0; i < repeat; i++) {
134		j = iomux_match_device(cons_set, cs_idx, old_set[i]);
135		if (j == cs_idx)
136			console_stop(console, old_set[i]);
137	}
138
139	free(old_set);
140	return 0;
141}
142
143int iomux_replace_device(const int console, const char *old, const char *new)
144{
145	struct stdio_dev *dev;
146	char *arg = NULL;	/* Initial empty list */
147	int size = 1;		/* For NUL terminator */
148	int i, ret;
149
150	for_each_console_dev(i, console, dev) {
151		const char *name = strcmp(dev->name, old) ? dev->name : new;
152		char *tmp;
153
154		/* Append name with a ',' (comma) separator */
155		tmp = realloc(arg, size + strlen(name) + 1);
156		if (!tmp) {
157			free(arg);
158			return -ENOMEM;
159		}
160
161		if (arg) {
162			strcat(tmp, ",");
163			strcat(tmp, name);
164		}
165		else
166			strcpy(tmp, name);
167
168		arg = tmp;
169		size = strlen(tmp) + 1;
170	}
171
172	ret = iomux_doenv(console, arg);
173	if (ret)
174		ret = -EINVAL;
175
176	free(arg);
177	return ret;
178}
179#endif /* CONSOLE_MUX */
180