[U-Boot] [PATCH 04/11] efi_loader: SIMPLE_TEXT_INPUT_EX plus wire up objects properly

[Rob Clark <robdclark@gmail.com>]

We need the _EX version for SCT.. and we need to wire up the
corresponding objects in the systab properly, as well as dealing
with the console_in object advertising multiple protocols.

Signed-off-by: Rob Clark <robdclark@gmail.com>
---
 include/efi_api.h             |  61 +++++++++-
 include/efi_loader.h          |  10 +-
 lib/efi_loader/efi_boottime.c |   3 +
 lib/efi_loader/efi_console.c  | 264 +++++++++++++++++++++++++++++++++++++++---
 4 files changed, 308 insertions(+), 30 deletions(-)

diff --git a/include/efi_api.h b/include/efi_api.h
index 38dd1240c1..58bf15b8e6 100644
--- a/include/efi_api.h
+++ b/include/efi_api.h
@@ -243,11 +243,11 @@ struct efi_system_table {
 	struct efi_table_hdr hdr;
 	unsigned long fw_vendor;   /* physical addr of wchar_t vendor string */
 	u32 fw_revision;
-	unsigned long con_in_handle;
+	efi_handle_t con_in_handle;
 	struct efi_simple_input_interface *con_in;
-	unsigned long con_out_handle;
+	efi_handle_t con_out_handle;
 	struct efi_simple_text_output_protocol *con_out;
-	unsigned long stderr_handle;
+	efi_handle_t stderr_handle;
 	struct efi_simple_text_output_protocol *std_err;
 	struct efi_runtime_services *runtime;
 	struct efi_boot_services *boottime;
@@ -474,6 +474,61 @@ struct efi_simple_input_interface {
 	struct efi_event *wait_for_key;
 };
 
+
+#define EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL_GUID \
+	EFI_GUID(0xdd9e7534, 0x7762, 0x4698, \
+		 0x8c, 0x14, 0xf5, 0x85, 0x17, 0xa6, 0x25, 0xaa)
+
+/* key-shift state: */
+#define EFI_SHIFT_STATE_VALID     0x80000000
+#define EFI_RIGHT_SHIFT_PRESSED   0x00000001
+#define EFI_LEFT_SHIFT_PRESSED    0x00000002
+#define EFI_RIGHT_CONTROL_PRESSED 0x00000004
+#define EFI_LEFT_CONTROL_PRESSED  0x00000008
+#define EFI_RIGHT_ALT_PRESSED     0x00000010
+#define EFI_EFI_LEFT_ALT_PRESSED  0x00000020
+#define EFI_RIGHT_LOGO_PRESSED    0x00000040
+#define EFI_LEFT_LOGO_PRESSED     0x00000080
+#define EFI_MENU_KEY_PRESSED      0x00000100
+#define EFI_SYS_REQ_PRESSED       0x00000200
+
+/* key-toggle state: */
+#define EFI_TOGGLE_STATE_VALID 0x80
+#define EFI_SCROLL_LOCK_ACTIVE 0x01
+#define EFI_NUM_LOCK_ACTIVE    0x02
+#define EFI_CAPS_LOCK_ACTIVE   0x04
+
+struct efi_key_state {
+	uint32_t key_shift_state;
+	uint8_t  key_toggle_state;
+};
+
+struct efi_key_data {
+	struct efi_input_key key;
+	struct efi_key_state key_state;
+};
+
+struct efi_simple_text_input_ex_interface {
+	efi_status_t (EFIAPI *reset)(
+			struct efi_simple_text_input_ex_interface *this,
+			bool ExtendedVerification);
+	efi_status_t (EFIAPI *read_key_stroke)(
+			struct efi_simple_text_input_ex_interface *this,
+			struct efi_key_data *key_data);
+	struct efi_event *wait_for_key;
+	efi_status_t (EFIAPI *set_state)(
+			struct efi_simple_text_input_ex_interface *this,
+			uint8_t key_toggle_state);
+	efi_status_t (EFIAPI *register_key_notify)(
+			struct efi_simple_text_input_ex_interface *this,
+			struct efi_key_data *key_data,
+			efi_status_t (EFIAPI *notify_fn)(struct efi_key_data *key_data),
+			efi_handle_t *notify_handle);
+	efi_status_t (EFIAPI *unregister_key_notify)(
+			struct efi_simple_text_input_ex_interface *this,
+			efi_handle_t notify_handle);
+};
+
 #define CONSOLE_CONTROL_GUID \
 	EFI_GUID(0xf42f7782, 0x12e, 0x4c12, \
 		 0x99, 0x56, 0x49, 0xf9, 0x43, 0x4, 0xf7, 0x21)
diff --git a/include/efi_loader.h b/include/efi_loader.h
index af6812b2b4..e6e55d2cb4 100644
--- a/include/efi_loader.h
+++ b/include/efi_loader.h
@@ -75,7 +75,9 @@ const char *__efi_nesting_dec(void);
 extern struct efi_runtime_services efi_runtime_services;
 extern struct efi_system_table systab;
 
+extern struct efi_object efi_console_output_obj;
 extern const struct efi_simple_text_output_protocol efi_con_out;
+extern struct efi_object efi_console_input_obj;
 extern struct efi_simple_input_interface efi_con_in;
 extern const struct efi_console_control_protocol efi_console_control;
 extern const struct efi_device_path_to_text_protocol efi_device_path_to_text;
@@ -129,14 +131,6 @@ struct efi_object {
 	void *handle;
 };
 
-#define EFI_PROTOCOL_OBJECT(_guid, _protocol) (struct efi_object){	\
-	.protocols = {{							\
-		.guid = &(_guid),	 				\
-		.protocol_interface = (void *)(_protocol), 		\
-	}},								\
-	.handle = (void *)(_protocol),					\
-}
-
 /**
  * struct efi_event
  *
diff --git a/lib/efi_loader/efi_boottime.c b/lib/efi_loader/efi_boottime.c
index b568f3f162..39dcc72648 100644
--- a/lib/efi_loader/efi_boottime.c
+++ b/lib/efi_loader/efi_boottime.c
@@ -2077,8 +2077,11 @@ struct efi_system_table __efi_runtime_data systab = {
 		.headersize = sizeof(struct efi_table_hdr),
 	},
 	.fw_vendor = (long)firmware_vendor,
+	.con_in_handle = &efi_console_input_obj,
 	.con_in = (void*)&efi_con_in,
+	.con_out_handle = &efi_console_output_obj,
 	.con_out = (void*)&efi_con_out,
+	.stderr_handle = &efi_console_output_obj,
 	.std_err = (void*)&efi_con_out,
 	.runtime = (void*)&efi_runtime_services,
 	.boottime = (void*)&efi_boot_services,
diff --git a/lib/efi_loader/efi_console.c b/lib/efi_loader/efi_console.c
index 1bdf36b4ae..f508b79ab8 100644
--- a/lib/efi_loader/efi_console.c
+++ b/lib/efi_loader/efi_console.c
@@ -50,6 +50,10 @@ const efi_guid_t efi_guid_console_control = CONSOLE_CONTROL_GUID;
 #define cESC '\x1b'
 #define ESC "\x1b"
 
+/*
+ * EFI_CONSOLE_CONTROL:
+ */
+
 static efi_status_t EFIAPI efi_cin_get_mode(
 			struct efi_console_control_protocol *this,
 			int *mode, char *uga_exists, char *std_in_locked)
@@ -97,6 +101,11 @@ static struct simple_text_output_mode efi_con_mode = {
 	.cursor_visible = 1,
 };
 
+
+/*
+ * EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL:
+ */
+
 static int term_read_reply(int *n, int maxnum, char end_char)
 {
 	char c;
@@ -364,32 +373,77 @@ const struct efi_simple_text_output_protocol efi_con_out = {
 	.mode = (void*)&efi_con_mode,
 };
 
+
+/*
+ * EFI_SIMPLE_TEXT_INPUT_PROTOCOL:
+ */
+
+/*
+ * FIFO to buffer up key-strokes, to allow dispatching key event
+ * notifications in advance of someone calling ReadKeyStroke().
+ */
+
+struct key_fifo {
+	unsigned rd, wr;
+	struct efi_key_data key[32]; /* use PoT size */
+};
+
+/* number of item's queued in fifo: */
+static unsigned fifo_count(struct key_fifo *fifo)
+{
+	return (ARRAY_SIZE(fifo->key) + fifo->wr - fifo->rd) % ARRAY_SIZE(fifo->key);
+}
+
+/* remaining space to queue items in fifo: */
+static unsigned fifo_space(struct key_fifo *fifo)
+{
+	return ARRAY_SIZE(fifo->key) - 1 - fifo_count(fifo);
+}
+
+/* push an item onto the tail of the fifo: */
+static void fifo_push(struct key_fifo *fifo, struct efi_key_data *key)
+{
+	assert(fifo_space(fifo) >= 1);
+	fifo->key[fifo->wr] = *key;
+	fifo->wr = (fifo->wr + 1) % ARRAY_SIZE(fifo->key);
+}
+
+/* pop an item from the head of the fifo: */
+static void fifo_pop(struct key_fifo *fifo, struct efi_key_data *key)
+{
+	assert(fifo_count(fifo) >= 1);
+	*key = fifo->key[fifo->rd];
+	fifo->rd = (fifo->rd + 1) % ARRAY_SIZE(fifo->key);
+}
+
+static struct key_fifo fifo;
+
+static void notify_key(struct efi_key_data *key);
+
 static efi_status_t EFIAPI efi_cin_reset(
 			struct efi_simple_input_interface *this,
 			bool extended_verification)
 {
 	EFI_ENTRY("%p, %d", this, extended_verification);
+	fifo.rd = fifo.wr = 0;
 	return EFI_EXIT(EFI_UNSUPPORTED);
 }
 
-static efi_status_t EFIAPI efi_cin_read_key_stroke(
-			struct efi_simple_input_interface *this,
-			struct efi_input_key *key)
+static efi_status_t read_key_stroke(struct efi_key_data *key_data)
 {
 	struct efi_input_key pressed_key = {
 		.scan_code = 0,
 		.unicode_char = 0,
 	};
+	struct efi_key_state key_state = {
+		.key_shift_state = 0,
+		.key_toggle_state = 0,
+	};
 	char ch;
 
-	EFI_ENTRY("%p, %p", this, key);
-
-	/* We don't do interrupts, so check for timers cooperatively */
-	efi_timer_check();
-
 	if (!tstc()) {
 		/* No key pressed */
-		return EFI_EXIT(EFI_NOT_READY);
+		return EFI_NOT_READY;
 	}
 
 	ch = getc();
@@ -404,7 +458,8 @@ static efi_status_t EFIAPI efi_cin_read_key_stroke(
 			pressed_key.scan_code = getc() - 'P' + 11;
 			break;
 		case 'a'...'z':
-			ch = ch - 'a';
+			key_state.key_shift_state =
+				EFI_SHIFT_STATE_VALID | EFI_EFI_LEFT_ALT_PRESSED;
 			break;
 		case '[':
 			ch = getc();
@@ -433,14 +488,61 @@ static efi_status_t EFIAPI efi_cin_read_key_stroke(
 			}
 			break;
 		}
+	} else if (0x01 <= ch && ch <= 0x1a && ch != '\t' && ch != '\b' &&
+		   ch != '\n' && ch != '\r') {
+		/*
+		 * Ctrl + <letter>.. except for a few cases that conflict
+		 * with unmodified chars
+		 */
+		ch = ch + 'a' - 1;
+		key_state.key_shift_state =
+			EFI_SHIFT_STATE_VALID | EFI_LEFT_CONTROL_PRESSED;
 	} else if (ch == 0x7f) {
 		/* Backspace */
 		ch = 0x08;
 	}
 	pressed_key.unicode_char = ch;
-	*key = pressed_key;
+	key_data->key = pressed_key;
+	key_data->key_state = key_state;
 
-	return EFI_EXIT(EFI_SUCCESS);
+	return EFI_SUCCESS;
+}
+
+static void read_keys(void)
+{
+	struct efi_key_data key;
+
+	while (fifo_space(&fifo) > 0 && read_key_stroke(&key) == EFI_SUCCESS) {
+		notify_key(&key);
+		fifo_push(&fifo, &key);
+	}
+}
+
+static efi_status_t EFIAPI efi_cin_read_key_stroke(
+			struct efi_simple_input_interface *this,
+			struct efi_input_key *key)
+{
+	struct efi_key_data key_data;
+
+	EFI_ENTRY("%p, %p", this, key);
+
+	while (true) {
+		efi_timer_check();
+		read_keys();
+
+		if (fifo_count(&fifo) == 0)
+			return EFI_EXIT(EFI_NOT_READY);
+
+		fifo_pop(&fifo, &key_data);
+
+		/* ignore ctrl/alt/etc */
+		if (key_data.key_state.key_shift_state)
+			continue;
+
+		*key = key_data.key;
+
+		return EFI_EXIT(EFI_SUCCESS);
+	}
 }
 
 struct efi_simple_input_interface efi_con_in = {
@@ -460,19 +562,143 @@ static void EFIAPI efi_console_timer_notify(struct efi_event *event,
 {
 	EFI_ENTRY("%p, %p", event, context);
 	if (tstc()) {
+		read_keys();
 		efi_con_in.wait_for_key->is_signaled = true;
 		efi_signal_event(efi_con_in.wait_for_key);
-		}
+	}
 	EFI_EXIT(EFI_SUCCESS);
 }
 
 
-static struct efi_object efi_console_control_obj =
-	EFI_PROTOCOL_OBJECT(efi_guid_console_control, &efi_console_control);
-static struct efi_object efi_console_output_obj =
-	EFI_PROTOCOL_OBJECT(EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL_GUID, &efi_con_out);
-static struct efi_object efi_console_input_obj =
-	EFI_PROTOCOL_OBJECT(EFI_SIMPLE_TEXT_INPUT_PROTOCOL_GUID, &efi_con_in);
+/*
+ * EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL
+ */
+
+struct key_notifier {
+	struct list_head link;
+	struct efi_key_data key;
+	efi_status_t (EFIAPI *notify)(struct efi_key_data *key);
+};
+
+static LIST_HEAD(key_notifiers);  /* list of key_notifier */
+
+static bool match_key(struct efi_key_data *a, struct efi_key_data *b)
+{
+	return (a->key.scan_code == b->key.scan_code) &&
+	       (a->key.unicode_char == b->key.unicode_char) &&
+	       (a->key_state.key_shift_state == b->key_state.key_shift_state) &&
+	       (a->key_state.key_toggle_state == b->key_state.key_toggle_state);
+}
+
+static void notify_key(struct efi_key_data *key)
+{
+	struct key_notifier *notifier;
+
+	list_for_each_entry(notifier, &key_notifiers, link)
+		if (match_key(&notifier->key, key))
+			EFI_CALL(notifier->notify(key));
+}
+
+static efi_status_t EFIAPI efi_cin_ex_reset(
+		struct efi_simple_text_input_ex_interface *this,
+		bool extended_verification)
+{
+	EFI_ENTRY("%p, %d", this, extended_verification);
+	fifo.rd = fifo.wr = 0;
+	return EFI_EXIT(EFI_UNSUPPORTED);
+}
+
+static efi_status_t EFIAPI efi_cin_ex_read_key_stroke(
+		struct efi_simple_text_input_ex_interface *this,
+		struct efi_key_data *key_data)
+{
+	EFI_ENTRY("%p, %p", this, key_data);
+
+	/* We don't do interrupts, so check for timers cooperatively */
+	efi_timer_check();
+	read_keys();
+
+	if (fifo_count(&fifo) == 0)
+		return EFI_EXIT(EFI_NOT_READY);
+
+	fifo_pop(&fifo, key_data);
+
+	return EFI_EXIT(EFI_SUCCESS);
+}
+
+static efi_status_t EFIAPI efi_cin_ex_set_state(
+		struct efi_simple_text_input_ex_interface *this,
+		uint8_t key_toggle_state)
+{
+	EFI_ENTRY("%p, %x", this, key_toggle_state);
+	return EFI_EXIT(EFI_SUCCESS);
+}
+
+static efi_status_t EFIAPI efi_cin_ex_register_key_notify(
+		struct efi_simple_text_input_ex_interface *this,
+		struct efi_key_data *key_data,
+		efi_status_t (EFIAPI *notify_fn)(struct efi_key_data *key_data),
+		efi_handle_t *notify_handle)
+{
+	struct key_notifier *notifier;
+
+	EFI_ENTRY("%p, %p, %p", this, notify_fn, notify_handle);
+	notifier = calloc(1, sizeof(*notifier));
+	if (!notifier)
+		return EFI_EXIT(EFI_OUT_OF_RESOURCES);
+
+	notifier->notify = notify_fn;
+	notifier->key = *key_data;
+
+	list_add_tail(&notifier->link, &key_notifiers);
+
+	return EFI_EXIT(EFI_SUCCESS);
+}
+
+static efi_status_t EFIAPI efi_cin_ex_unregister_key_notify(
+		struct efi_simple_text_input_ex_interface *this,
+		efi_handle_t notify_handle)
+{
+	struct key_notifier *notifier = notify_handle;
+
+	EFI_ENTRY("%p, %p", this, notify_handle);
+
+	list_del(&notifier->link);
+	free(notifier);
+
+	return EFI_EXIT(EFI_SUCCESS);
+}
+
+static struct efi_simple_text_input_ex_interface efi_con_in_ex = {
+	.reset = efi_cin_ex_reset,
+	.read_key_stroke = efi_cin_ex_read_key_stroke,
+	.wait_for_key = NULL,
+	.set_state = efi_cin_ex_set_state,
+	.register_key_notify = efi_cin_ex_register_key_notify,
+	.unregister_key_notify = efi_cin_ex_unregister_key_notify,
+};
+
+static struct efi_object efi_console_control_obj = {
+	.protocols =  {
+		{ &efi_guid_console_control, (void *)&efi_console_control },
+	},
+	.handle = &efi_console_control_obj,
+};
+
+struct efi_object efi_console_output_obj = {
+	.protocols = {
+		{&EFI_SIMPLE_TEXT_OUTPUT_PROTOCOL_GUID, (void *)&efi_con_out},
+	},
+	.handle = &efi_console_output_obj,
+};
+
+struct efi_object efi_console_input_obj = {
+	.protocols = {
+		{&EFI_SIMPLE_TEXT_INPUT_PROTOCOL_GUID,    (void *)&efi_con_in},
+		{&EFI_SIMPLE_TEXT_INPUT_EX_PROTOCOL_GUID, (void *)&efi_con_in_ex},
+	},
+	.handle = &efi_console_input_obj,
+};
 
 /* This gets called from do_bootefi_exec(). */
 int efi_console_register(void)
-- 
2.13.6