From: Domenico Andreoli <[email-masked]>

A basic block here is the decompressor console driver defined by:

struct decomp_console_desc {
	const char devname[8];
	const char (*dt_compat)[16];
	int (*probe)(struct decomp_console_drv *drv);
	void (*putc)(struct decomp_console_drv *drv, int ch);
	void (*flush)(struct decomp_console_drv *drv);
};

This is all a console driver needs to define in order to go into the
decompressor and being used from there if so is determined at runtime.

All the decompressor console drivers need to be relocated to the
decompressor as well, something achieved with a mechanism different
from decompressor tags but subject to the same limitations.

Next is the console data defined by:

struct decomp_console_data {
	const char *drvname;
	const void *data;
	unsigned short elem_size;
	unsigned short num_elem;
};

It's not obvious but basically it's a driver's name and some data to
be passed to the driver, if found, in order to get a working console.
Those fields data, elem_size and num_elem are used to describe an array
of driver dependent "things". Will come back to the array later on.

This console data is wrapped in decompressor tags which then take name
of "console tags". So we can (well... must) specify driver and data
on a per-architecture basis. It's possible to share console data among
multiple console tags.

What happens in the decompressor with all this new shipped stuff?

These are the steps performed to setup the console:

 1) look for some console data associated to the machine
    (see fn setup_arch)

 2) look for the driver name specified in the console data
    (see fn drv_lookup)

 3) use the console device name prefix specified by the driver descriptor
    and walk the array of "things" passed in the console data
    (see fn get_console_data)

 4) each step of the walk increments a counter which is appended to
    the device name prefix. If the resulting name matches the one from
    the cmdline, pass the "thing" pointed by count in the array to the
    driver and set up a console
    (see fn get_console_data)

 5) if no suitable console is found, use fallbacks
    (see fn init_console)

Signed-off-by: Domenico Andreoli <[email-masked]>

---
 arch/arm/boot/Makefile                  |    1 +
 arch/arm/boot/compressed/Makefile       |    2 
 arch/arm/boot/compressed/arch.c         |   45 ++++++
 arch/arm/boot/compressed/arch.h         |    8 +
 arch/arm/boot/compressed/console.c      |  232 ++++++++++++++++++++++++++++++++
 arch/arm/boot/compressed/misc.c         |   27 ++--
 arch/arm/boot/compressed/vmlinux.lds.in |    3 +
 arch/arm/kernel/vmlinux.lds.S           |    5 +
 include/linux/decompress/arch.h         |    1 +
 include/linux/decompress/console.h      |  130 ++++++++++++++++++
 10 files changed, 440 insertions(+), 14 deletions(-)

Index: b/include/linux/decompress/console.h
===================================================================
--- /dev/null
+++ b/include/linux/decompress/console.h
@@ -0,0 +1,130 @@
+/*
+ * Handling of console specific bits in the decompressor
+ *
+ * Copyright (C) 2012 Domenico Andreoli <[email-masked]>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef LINUX_DECOMP_CONSOLE_H
+#define LINUX_DECOMP_CONSOLE_H
+
+#include <linux/init.h>
+#include <linux/decompress/arch.h>
+
+struct decomp_console_desc;
+
+/*
+ * Console driver in use, we have only one instance of this.
+ *
+ * @desc       points to the struct identifying the selected console driver.
+ *             It can be NULL, when no suitable driver is found, but its
+ *             putc/flush members always point to valid (or fallback) functions.
+ *
+ * @devdata    points to device specific data, like the base address
+ *             of the UART block. Such data comes either from builtin console
+ *             data (see struct decomp_console_data) or DeviceTree
+ *
+ * @putc
+ * @flush      point to respective ops of the selected console driver
+ */
+struct decomp_console_drv {
+	const struct decomp_console_desc *desc;
+	const void *devdata;
+	void (*putc)(struct decomp_console_drv *drv, int ch);
+	void (*flush)(struct decomp_console_drv *drv);
+};
+
+/*
+ * Console driver implementation, expect to have a certain number of these.
+ *
+ * @devname    the name prefix of the devices handled by this driver.
+ *             Example: ttySAC, ttyO, ttyAMA, ...
+ *
+ * @dt_compat  DeviceTree data used to lookup the console driver to be used.
+ *             This same lookup data is used also when the driver search
+ *             starts from builtin console data.
+ *
+ * @probe      optional function used to probe the hardware. Most of the
+ *             times it only checks that the UART is initialized by the boot
+ *             loader and rejects the handling otherwise.
+ *
+ * @putc       invoked to send a single character through the console pipe.
+ *             It may need to wait for some free space in the output
+ *             queue (ie UARTs)
+ *
+ * @flush      invoked to wait until the last sent character reached the
+ *             destination (ie delivered to the wires)
+ */
+struct decomp_console_desc {
+	const char devname[8];
+	const char (*dt_compat)[16];
+	int (*probe)(struct decomp_console_drv *drv);
+	void (*putc)(struct decomp_console_drv *drv, int ch);
+	void (*flush)(struct decomp_console_drv *drv);
+};
+
+/*
+ * Console platform data, there could be quite a lot of these.
+ *
+ * @drvname    the driver name called to handle this devices
+ *
+ * @data       points to the array of descriptors of a given kind of device,
+ *             usually the base addresses of a bunch of UARTs. Drivers may
+ *             add specific data (ie FIFOs size) here.
+ *
+ * @elem_size  size of the array base type
+ *
+ * @num_elem   number of elements in the array pointed by @data
+ */
+struct decomp_console_data {
+	const char *drvname;
+	const void *data;
+	unsigned short elem_size;
+	unsigned short num_elem;
+};
+
+struct decomp_console_tag {
+	struct decomp_tag_hdr hdr;
+	const struct decomp_console_data *console;
+};
+
+#define DECOMP_CONSOLE_DATA(_name, _drvname, _data)                     \
+const struct decomp_console_data _name __decomp_archdata = {            \
+	.drvname = _drvname,                                            \
+	.data = _data,                                                  \
+	.elem_size = sizeof(_data[0]),                                  \
+	.num_elem = sizeof(_data) / sizeof(_data[0]),                   \
+};
+
+#define DECOMP_CONSOLE_SOC(_machid, _soc_data)                          \
+extern const struct decomp_console_data _soc_data __decomp_archdata;    \
+DECOMP_TAG_START(struct decomp_console_tag, __decomp_cons_##_machid,    \
+	DECOMP_TAG_CONSOLE, MACH_TYPE_##_machid)                        \
+	.console = &_soc_data,                                          \
+DECOMP_TAG_END
+
+#define DECOMP_CONSOLE_MACH(_machid, _drvname, _data)                   \
+static const char _decomp_console_drvname_##_machid[]                   \
+	__decomp_archdata = _drvname;                                   \
+static DECOMP_CONSOLE_DATA(_decomp_console_data_##_machid,              \
+	_decomp_console_drvname_##_machid, _data);                      \
+DECOMP_CONSOLE_SOC(_machid, _decomp_console_data_##_machid)
+
+#define DECOMP_CONSOLE_START(_devname)                                  \
+static const struct decomp_console_desc __decomp_console_desc           \
+  __used                                                                \
+  __attribute__((__section__(".console.info.decomp"))) = {              \
+	.devname = _devname,
+
+#define DECOMP_CONSOLE_END                                              \
+};
+
+#endif /* LINUX_DECOMP_CONSOLE_H */
Index: b/arch/arm/boot/compressed/console.c
===================================================================
--- /dev/null
+++ b/arch/arm/boot/compressed/console.c
@@ -0,0 +1,232 @@
+/*
+ * Decompressor console
+ *
+ * Copyright (C) 2012 Domenico Andreoli <[email-masked]>
+ *
+ * This software is licensed under the terms of the GNU General Public
+ * License version 2, as published by the Free Software Foundation, and
+ * may be copied, distributed, and modified under those terms.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <asm/setup.h>
+#include <linux/compiler.h>
+#include <linux/types.h>
+#include <linux/linkage.h>
+#include <linux/string.h>
+
+#include <linux/decompress/arch.h>
+#include <linux/decompress/console.h>
+
+#include "arch.h"
+
+static const char *get_cmdline(void *atag_fdt)
+{
+	struct tag *atag = atag_fdt;
+
+	/* validate the ATAG */
+	if (atag->hdr.tag != ATAG_CORE ||
+	    (atag->hdr.size != tag_size(tag_core) && atag->hdr.size != 2))
+		return NULL;
+
+	for_each_tag(atag, atag_fdt)
+		if (atag->hdr.tag == ATAG_CMDLINE)
+			return atag->u.cmdline.cmdline;
+
+	return NULL;
+}
+
+static const char *get_param(const char *cmdline, const char *name, unsigned int *len)
+{
+	const char *param = cmdline, *end;
+
+	do {
+		param = strstr(param, name);
+		if (!param)
+			return NULL;
+		if (param > cmdline && *(param - 1) != ' ')
+			continue;
+
+		param += strlen(name);
+		end = strchr(param, ' ');
+		*len = end ? end - param : strlen(param);
+
+		if (*len == 0)
+			return param;
+
+	} while (*param++ != '=');
+
+	(*len)--;
+	return param;
+}
+
+struct decomp_console_drv console;
+char console_devname[64];
+
+void init_console(void *atag_fdt)
+{
+	const char *cmdline, *param;
+	unsigned int len = 0;
+	char *p;
+
+	/* fallbacks in case no suitable console is found */
+	console.putc = fallback_putc;
+	console.flush = fallback_flush;
+
+	cmdline = get_cmdline(atag_fdt);
+	if (!cmdline)
+		return;
+
+	/* extract the value of console= param from cmdline */
+	param = get_param(cmdline, "console", &len);
+	if (!param || !len || len > sizeof(console_devname))
+		return;
+
+	/* save the "ttySOME,..." part */
+	memcpy(console_devname, param, len);
+	console_devname[len] = '\0';
+
+	/* drop the ",..." part if present */
+	p = strchr(console_devname, ',');
+	if (p)
+		*p = '\0';
+}
+
+const void *get_console_data(const struct decomp_console_desc *desc,
+	const struct decomp_console_data *console)
+{
+	const char *prefix;
+	const void *data;
+	char devname[16];
+	int i, len;
+
+	prefix = desc->devname;
+	len = strlen(prefix);
+
+	if (len >= sizeof(devname))
+		return NULL;
+	if (len >= strlen(console_devname))
+		return NULL;
+	if (memcmp(prefix, console_devname, len))
+		return NULL;
+
+	memcpy(devname, prefix, len);
+	devname[len] = '0';
+	devname[len + 1] = 0;
+
+	data = fix_data_ptr(console->data);
+	for (i = 0; i < console->num_elem; i++) {
+		if (!strcmp(devname, console_devname))
+			return data;
+
+		/* 'increment' the console name (i.e. ttyS0 -> ttyS1) */
+		devname[len]++;
+		/* don't know the array base type but its size is required here */
+		data += console->elem_size;
+	}
+
+	return NULL;
+}
+
+static void dummy(void)
+{
+}
+
+extern const struct decomp_console_desc __decomp_console_desc_begin[];
+extern const struct decomp_console_desc __decomp_console_desc_end[];
+
+#define for_each_decomp_console_desc(p) \
+	for (p = __decomp_console_desc_begin; p < __decomp_console_desc_end; p++)
+
+static bool match_dt_compat(const struct decomp_console_desc *desc,
+	const char *compatible)
+{
+	const char (*dt_compat)[16];
+
+	dt_compat = (const char (*)[16]) fix_data_ptr(desc->dt_compat);
+	for ( ; strlen(*dt_compat); dt_compat++)
+		if (!strcmp(*dt_compat, compatible))
+			return true;
+
+	return false;
+}
+
+const struct decomp_console_desc *drv_lookup(const char *drvname)
+{
+	const struct decomp_console_desc *desc;
+
+	for_each_decomp_console_desc(desc)
+		if (match_dt_compat(desc, drvname))
+			return desc;
+
+	return NULL;
+}
+
+void setup_console(const struct decomp_console_desc *desc, const void *devdata)
+{
+	int (*probe)(struct decomp_console_drv *drv);
+
+	/* we already have a valid candidate */
+	if (console.desc)
+		return;
+
+	console.devdata = devdata;
+
+	probe = fix_text_ptr(desc->probe);
+	if (probe && probe(&console) < 0)
+		return;
+
+	console.desc = desc;
+	console.putc = fix_text_ptr(desc->putc);
+	console.flush = fix_text_ptr(desc->flush);
+
+	if (!desc->putc)
+		console.putc = (void *) dummy;
+	if (!desc->flush)
+		console.flush = (void *) dummy;
+
+	/* debug code */
+	putstr("Decompressor console drivers:");
+	for_each_decomp_console_desc(desc) {
+		putstr(" ");
+		putstr(desc->devname);
+
+		if (desc == console.desc) {
+			putstr("(->");
+			putstr(strlen(console_devname) ? console_devname : "console");
+			putstr(")");
+		}
+	}
+	putstr("\n");
+}
+
+void putstr(const char *ptr)
+{
+	char c;
+
+	while ((c = *ptr++) != '\0') {
+		if (c == '\n')
+			console.putc(&console, '\r');
+		console.putc(&console, c);
+	}
+
+	console.flush(&console);
+}
+
+void putn(unsigned long z)
+{
+	int i;
+	char x;
+
+	console.putc(&console, '0');
+	console.putc(&console, 'x');
+	for (i=0;i<8;i++) {
+		x='0'+((z>>((7-i)*4))&0xf);
+		if (x>'9') x=x-'0'+'a'-10;
+		console.putc(&console, x);
+	}
+}
Index: b/include/linux/decompress/arch.h
===================================================================
--- a/include/linux/decompress/arch.h
+++ b/include/linux/decompress/arch.h
@@ -20,6 +20,7 @@
 
 enum decomp_tag_type {
 	DECOMP_TAG_ARCH = 0,
+	DECOMP_TAG_CONSOLE,
 };
 
 struct decomp_tag_hdr {
Index: b/arch/arm/boot/compressed/vmlinux.lds.in
===================================================================
--- a/arch/arm/boot/compressed/vmlinux.lds.in
+++ b/arch/arm/boot/compressed/vmlinux.lds.in
@@ -46,6 +46,9 @@ SECTIONS
     __decomp_tags_begin = .;
     *(.init.decomp.tags)
     __decomp_tags_end = .;
+    __decomp_console_desc_begin = .;
+    *(.init.decomp.console)
+    __decomp_console_desc_end = .;
     __decomp_data_begin = .;
     *(.init.decomp.data)
   }
Index: b/arch/arm/kernel/vmlinux.lds.S
===================================================================
--- a/arch/arm/kernel/vmlinux.lds.S
+++ b/arch/arm/kernel/vmlinux.lds.S
@@ -160,6 +160,11 @@ SECTIONS
 		*(.arch.tags.decomp)
 		__decomp_tags_end = .;
 	}
+	.init.decomp.console : {
+		__decomp_console_desc_begin = .;
+		*(.console.info.decomp)
+		__decomp_console_desc_end = .;
+	}
 	.init.decomp.data : {
 		__decomp_data_begin = .;
 		LONG(__decomp_data_begin)
Index: b/arch/arm/boot/compressed/arch.c
===================================================================
--- a/arch/arm/boot/compressed/arch.c
+++ b/arch/arm/boot/compressed/arch.c
@@ -21,11 +21,26 @@ unsigned int __machine_arch_type;
 #include <linux/string.h>
 
 #include <linux/decompress/arch.h>
+#include <linux/decompress/console.h>
 
 #include "arch.h"
 
 const char *get_fdt_prop(void *fdt, const char *path, const char *prop);
 
+/* 1. init the console device name from cmdline or DT */
+void init_console(void *atag_fdt);
+
+/* 2. get the first driver compatible with drvname or NULL */
+const struct decomp_console_desc *drv_lookup(const char *drvname);
+
+/* 3. get suitable devdata for this board or NULL */
+const void *get_console_data(const struct decomp_console_desc *desc,
+	const struct decomp_console_data *console);
+
+/* 4. pass devdata to the driver and instantiate the console */
+void setup_console(const struct decomp_console_desc *desc, const void *devdata);
+
+/* the only way to allocate dynamic memory here (unused) */
 unsigned long free_mem_ptr;
 unsigned long free_mem_end_ptr;
 
@@ -76,6 +91,11 @@ void arch_setup(unsigned int arch_id, vo
 	const struct decomp_tag_hdr *hdr;
 	void (*arch_setup_p)(void);
 
+	const struct decomp_console_desc *desc;
+	const struct decomp_console_data *console;
+	struct decomp_console_tag *cons_tag;
+	void const *cons_data = NULL;
+
 	const char (*dt_compat)[16];
 	const char *compatible;
 
@@ -88,6 +108,8 @@ void arch_setup(unsigned int arch_id, vo
 	free_mem_ptr = free_mem_ptr_p;
 	free_mem_end_ptr = free_mem_ptr_end_p;
 
+	init_console(atag_fdt);
+
 	for_each_decomp_tag(hdr) {
 		if (hdr->arch_id != arch_id)
 			continue;
@@ -101,15 +123,34 @@ void arch_setup(unsigned int arch_id, vo
 		    !is_compatible(dt_compat, compatible))
 			continue;
 
-		if (hdr->type == DECOMP_TAG_ARCH) {
+		if (!arch_tag && hdr->type == DECOMP_TAG_ARCH) {
 			arch_tag = (struct decomp_arch_tag *) hdr;
 			arch_setup_p = fix_text_ptr(arch_tag->setup);
 			arch_error_p = fix_text_ptr(arch_tag->error);
-			break;
 		}
+		if (!cons_data && hdr->type == DECOMP_TAG_CONSOLE) {
+			cons_tag = (struct decomp_console_tag *) hdr;
+			if (!cons_tag->console)
+				continue;
+
+			console = fix_data_ptr(cons_tag->console);
+			desc = drv_lookup(fix_data_ptr(console->drvname));
+			if (!desc)
+				continue;
+
+			/* returns NULL when the console name provided by
+			 * the cmdline doesn't match the one in argument
+			 */
+			cons_data = get_console_data(desc, console);
+		}
+
+		if (arch_tag && cons_data)
+			break;
 	}
 
 	/* archs without decompressor setup have NULL here */
 	if (arch_setup_p)
 		arch_setup_p();
+	if (cons_data)
+		setup_console(desc, cons_data);
 }
Index: b/arch/arm/boot/Makefile
===================================================================
--- a/arch/arm/boot/Makefile
+++ b/arch/arm/boot/Makefile
@@ -50,6 +50,7 @@ $(obj)/Image: vmlinux FORCE
 
 OBJCOPYFLAGS_arch_reloc.o = -O elf32-littlearm \
 	-j .text.decomp \
+	-j .init.decomp.console \
 	-j .init.decomp.data \
 	-j .init.decomp.tags \
 	-j .ARM.attributes
Index: b/arch/arm/boot/compressed/misc.c
===================================================================
--- a/arch/arm/boot/compressed/misc.c
+++ b/arch/arm/boot/compressed/misc.c
@@ -20,7 +20,7 @@
 #include <linux/types.h>
 #include <linux/linkage.h>
 
-static void putstr(const char *ptr);
+void putstr(const char *ptr);
 void error(char *x);
 
 #ifndef CONFIG_ARCH_MULTIPLATFORM
@@ -97,6 +97,14 @@ void fallback_arch_error(char *x)
 {
 }
 
+void fallback_putc(struct decomp_console_drv *drv, int ch)
+{
+}
+
+void fallback_flush(struct decomp_console_drv *drv)
+{
+}
+
 #else
 
 #ifndef arch_error
@@ -117,21 +125,18 @@ void fallback_arch_error(char *x)
 	arch_error(x);
 }
 
-#endif /* CONFIG_ARCH_MULTIPLATFORM */
-
-static void putstr(const char *ptr)
+void fallback_putc(struct decomp_console_drv *drv, int ch)
 {
-	char c;
-
-	while ((c = *ptr++) != '\0') {
-		if (c == '\n')
-			putc('\r');
-		putc(c);
-	}
+	putc(ch);
+}
 
+void fallback_flush(struct decomp_console_drv *drv)
+{
 	flush();
 }
 
+#endif /* CONFIG_ARCH_MULTIPLATFORM */
+
 /*
  * gzip declarations
  */
Index: b/arch/arm/boot/compressed/arch.h
===================================================================
--- a/arch/arm/boot/compressed/arch.h
+++ b/arch/arm/boot/compressed/arch.h
@@ -16,6 +16,8 @@
 #ifndef ARM_BOOT_COMPRESSED_ARCH_H
 #define ARM_BOOT_COMPRESSED_ARCH_H
 
+struct decomp_console_drv;
+
 extern unsigned int __machine_arch_type;
 
 extern unsigned long free_mem_ptr;
@@ -26,7 +28,13 @@ extern void (*arch_error_p)(char *x);
 void fallback_arch_setup(void);
 void fallback_arch_error(char *x);
 
+void fallback_putc(struct decomp_console_drv *drv, int ch);
+void fallback_flush(struct decomp_console_drv *drv);
+
 const void *fix_text_ptr(const void *p);
 const void *fix_data_ptr(const void *p);
 
+void putstr(const char *ptr);
+void putn(unsigned long z);
+
 #endif /* ARM_BOOT_COMPRESSED_ARCH_H */
Index: b/arch/arm/boot/compressed/Makefile
===================================================================
--- a/arch/arm/boot/compressed/Makefile
+++ b/arch/arm/boot/compressed/Makefile
@@ -23,7 +23,7 @@ endif
 
 AFLAGS_head.o += -DTEXT_OFFSET=$(TEXT_OFFSET)
 HEAD	= head.o
-OBJS	+= misc.o decompress.o arch.o arch_reloc.o
+OBJS	+= misc.o decompress.o arch.o arch_reloc.o console.o
 FONTC	= $(srctree)/drivers/video/console/font_acorn_8x8.c
 
 # string library code (-Os is enforced to keep it much smaller)