1 files changed, 95 insertions, 65 deletions

diff --git a/common/ratp/ratp.c b/common/ratp/ratp.c
index d2fdb631b3..fddb286e01 100644
--- a/common/ratp/ratp.c
+++ b/common/ratp/ratp.c
@@ -15,18 +15,18 @@
 
 #include <common.h>
 #include <command.h>
-#include <kfifo.h>
 #include <malloc.h>
 #include <init.h>
 #include <ratp.h>
-#include <command.h>
 #include <byteorder.h>
 #include <environment.h>
 #include <kfifo.h>
 #include <poller.h>
+#include <work.h>
 #include <linux/sizes.h>
 #include <ratp_bb.h>
 #include <fs.h>
+#include <console_countdown.h>
 
 LIST_HEAD(ratp_command_list);
 EXPORT_SYMBOL(ratp_command_list);
@@ -50,6 +50,11 @@ struct ratp_ctx {
 	struct ratp_bb_pkt *fs_rx;
 
 	struct poller_struct poller;
+	struct work_queue wq;
+
+	bool console_registered;
+	bool poller_registered;
+	bool wq_registered;
 };
 
 static int compare_ratp_command(struct list_head *a, struct list_head *b)
@@ -191,9 +196,14 @@ static int ratp_bb_send_command_return(struct ratp_ctx *ctx, uint32_t errno)
 	return ret;
 }
 
-static char *ratp_command;
 static struct ratp_ctx *ratp_ctx;
 
+struct ratp_work {
+	struct work_struct work;
+	struct ratp_ctx *ctx;
+	char *command;
+};
+
 static int ratp_bb_dispatch(struct ratp_ctx *ctx, const void *buf, int len)
 {
 	const struct ratp_bb *rbb = buf;
@@ -202,6 +212,7 @@ static int ratp_bb_dispatch(struct ratp_ctx *ctx, const void *buf, int len)
 	int ret = 0;
 	uint16_t type = be16_to_cpu(rbb->type);
 	struct ratp_command *cmd;
+	struct ratp_work *rw;
 
 	/* See if there's a command registered to this type */
 	cmd = find_ratp_request(type);
@@ -219,12 +230,16 @@ static int ratp_bb_dispatch(struct ratp_ctx *ctx, const void *buf, int len)
 
 	switch (type) {
 	case BB_RATP_TYPE_COMMAND:
-		if (!IS_ENABLED(CONFIG_CONSOLE_RATP) || ratp_command)
+		if (!IS_ENABLED(CONFIG_CONSOLE_RATP))
 			return 0;
 
-		ratp_command = xstrndup((const char *)rbb->data, dlen);
-		ratp_ctx = ctx;
-		pr_debug("got command: %s\n", ratp_command);
+		rw = xzalloc(sizeof(*rw));
+		rw->ctx = ctx;
+		rw->command = xstrndup((const char *)rbb->data, dlen);
+
+		wq_queue_work(&ctx->wq, &rw->work);
+
+		pr_debug("got command: %s\n", rw->command);
 
 		break;
 
@@ -295,41 +310,20 @@ static void ratp_console_putc(struct console_device *cdev, char c)
 	kfifo_putc(ctx->console_transmit_fifo, c);
 }
 
-static int ratp_console_register(struct ratp_ctx *ctx)
-{
-	int ret;
-
-	ctx->ratp_console.tstc = ratp_console_tstc;
-	ctx->ratp_console.puts = ratp_console_puts;
-	ctx->ratp_console.putc = ratp_console_putc;
-	ctx->ratp_console.getc = ratp_console_getc;
-	ctx->ratp_console.devname = "ratpconsole";
-	ctx->ratp_console.devid = DEVICE_ID_SINGLE;
-
-	ret = console_register(&ctx->ratp_console);
-	if (ret) {
-		pr_err("registering failed with %s\n", strerror(-ret));
-		return ret;
-	}
-
-	return 0;
-}
-
-void barebox_ratp_command_run(void)
+static void ratp_command_run(struct work_struct *w)
 {
+	struct ratp_work *rw = container_of(w, struct ratp_work, work);
+	struct ratp_ctx *ctx = rw->ctx;
 	int ret;
 
-	if (!ratp_command)
-		return;
-
-	pr_debug("running command: %s\n", ratp_command);
+	pr_debug("running command: %s\n", rw->command);
 
-	ret = run_command(ratp_command);
+	ret = run_command(rw->command);
 
-	free(ratp_command);
-	ratp_command = NULL;
+	free(rw->command);
+	free(rw);
 
-	ratp_bb_send_command_return(ratp_ctx, ret);
+	ratp_bb_send_command_return(ctx, ret);
 }
 
 static const char *ratpfs_mount_path;
@@ -344,15 +338,30 @@ int barebox_ratp_fs_mount(const char *path)
 	return 0;
 }
 
-static void ratp_console_unregister(struct ratp_ctx *ctx)
+static void ratp_unregister(struct ratp_ctx *ctx)
 {
 	int ret;
 
-	console_set_active(&ctx->ratp_console, 0);
-	poller_unregister(&ctx->poller);
+	if (ctx->console_registered)
+		console_unregister(&ctx->ratp_console);
+
+	if (ctx->poller_registered)
+		poller_unregister(&ctx->poller);
+
+	if (ctx->wq_registered)
+		wq_unregister(&ctx->wq);
+
 	ratp_close(&ctx->ratp);
 	console_set_active(ctx->cdev, ctx->old_active);
-	ctx->cdev = NULL;
+
+	if (ctx->console_recv_fifo)
+		kfifo_free(ctx->console_recv_fifo);
+
+	if (ctx->console_transmit_fifo)
+		kfifo_free(ctx->console_transmit_fifo);
+
+	free(ctx);
+	ratp_ctx = NULL;
 
 	if (ratpfs_mount_path) {
 		ret = umount(ratpfs_mount_path);
@@ -374,6 +383,7 @@ static void ratp_poller(struct poller_struct *poller)
 	ret = ratp_poll(&ctx->ratp);
 	if (ret == -EINTR)
 		goto out;
+
 	if (ratp_closed(&ctx->ratp))
 		goto out;
 
@@ -388,7 +398,7 @@ static void ratp_poller(struct poller_struct *poller)
 	return;
 
 out:
-	ratp_console_unregister(ctx);
+	ratp_unregister(ctx);
 }
 
 int barebox_ratp_fs_call(struct ratp_bb_pkt *tx, struct ratp_bb_pkt **rx)
@@ -429,32 +439,46 @@ int barebox_ratp_fs_call(struct ratp_bb_pkt *tx, struct ratp_bb_pkt **rx)
 	return 0;
 }
 
+static void ratp_work_cancel(struct work_struct *w)
+{
+	struct ratp_work *rw = container_of(w, struct ratp_work, work);
+
+	free(rw->command);
+	free(rw);
+}
+
 int barebox_ratp(struct console_device *cdev)
 {
 	int ret;
 	struct ratp_ctx *ctx;
-	struct ratp *ratp;
 
 	if (!cdev->getc || !cdev->putc)
 		return -EINVAL;
 
-	if (ratp_ctx) {
-		ctx = ratp_ctx;
-	} else {
-		ctx = xzalloc(sizeof(*ctx));
-		ratp_ctx = ctx;
-		ctx->ratp.send = console_send;
-		ctx->ratp.recv = console_recv;
-		ctx->console_recv_fifo = kfifo_alloc(512);
-		ctx->console_transmit_fifo = kfifo_alloc(SZ_128K);
-		ctx->poller.func = ratp_poller;
-		ratp_console_register(ctx);
-	}
-
-	if (ctx->cdev)
+	if (ratp_ctx)
 		return -EBUSY;
 
-	ratp = &ctx->ratp;
+	ctx = xzalloc(sizeof(*ctx));
+	ratp_ctx = ctx;
+	ctx->ratp.send = console_send;
+	ctx->ratp.recv = console_recv;
+	ctx->console_recv_fifo = kfifo_alloc(512);
+	ctx->console_transmit_fifo = kfifo_alloc(SZ_128K);
+	ctx->poller.func = ratp_poller;
+	ctx->ratp_console.tstc = ratp_console_tstc;
+	ctx->ratp_console.puts = ratp_console_puts;
+	ctx->ratp_console.putc = ratp_console_putc;
+	ctx->ratp_console.getc = ratp_console_getc;
+	ctx->ratp_console.devname = "ratpconsole";
+	ctx->ratp_console.devid = DEVICE_ID_SINGLE;
+
+	ret = console_register(&ctx->ratp_console);
+	if (ret) {
+		pr_err("registering console failed with %s\n", strerror(-ret));
+		return ret;
+	}
+
+	ctx->console_registered = true;
 
 	ctx->old_active = console_get_active(cdev);
 	console_set_active(cdev, 0);
@@ -462,31 +486,37 @@ int barebox_ratp(struct console_device *cdev)
 	ctx->cdev = cdev;
 	ctx->have_synch = 1;
 
-	ret = ratp_establish(ratp, false, 100);
+	ret = ratp_establish(&ctx->ratp, false, 1000);
 	if (ret < 0)
 		goto out;
 
+	ctx->wq.fn = ratp_command_run;
+	ctx->wq.cancel = ratp_work_cancel;
+	wq_register(&ctx->wq);
+	ctx->wq_registered = true;
+
 	ret = poller_register(&ctx->poller, "ratp");
 	if (ret)
-		goto out1;
+		goto out;
+
+	ctx->poller_registered = true;
+
+	console_countdown_abort();
 
 	console_set_active(&ctx->ratp_console, CONSOLE_STDOUT | CONSOLE_STDERR |
 			CONSOLE_STDIN);
 
 	return 0;
 
-out1:
-	ratp_close(ratp);
 out:
-	console_set_active(ctx->cdev, ctx->old_active);
-	ctx->cdev = NULL;
+	ratp_unregister(ctx);
 
 	return ret;
 }
 
 static void barebox_ratp_close(void)
 {
-	if (ratp_ctx && ratp_ctx->cdev)
-		ratp_console_unregister(ratp_ctx);
+	if (ratp_ctx)
+		ratp_unregister(ratp_ctx);
 }
 predevshutdown_exitcall(barebox_ratp_close);