LXR uboot/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)
  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