iomux.c 3.7 KB

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