common/iomux.c

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