common/iomux.c

   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)
  13 void 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
  23 int 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. */
  34 int 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
 143 int 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 */