Merge branch 'for-next/ratp'

11 files changed, 617 insertions, 98 deletions

diff --git a/common/Kconfig b/common/Kconfig
index 25de2485cd..af71d6888a 100644
--- a/common/Kconfig
+++ b/common/Kconfig
@@ -770,18 +770,7 @@ config PBL_CONSOLE
 	  must be running at the address it's linked at and bss must
 	  be cleared. On ARM that would be after setup_c().
 
-config CONSOLE_RATP
-	bool
-	select RATP
-	select CRC16
-	select POLLER
-	depends on CONSOLE_FULL
-	prompt "RATP console support"
-	help
-	  This option adds support for remote controlling barebox via serial
-	  port. The regular console is designed for human interaction whereas
-	  this option adds a machine readable interface for controlling barebox.
-	  Say yes here if you want to control barebox from a remote host.
+source common/ratp/Kconfig
 
 config PARTITION
 	bool
diff --git a/common/Makefile b/common/Makefile
index 5351ef0f77..a9abcd1bc6 100644
--- a/common/Makefile
+++ b/common/Makefile
@@ -46,7 +46,7 @@ obj-$(CONFIG_RESET_SOURCE)	+= reset_source.o
 obj-$(CONFIG_SHELL_HUSH)	+= hush.o
 obj-$(CONFIG_SHELL_SIMPLE)	+= parser.o
 obj-$(CONFIG_STATE)		+= state/
-obj-$(CONFIG_RATP)		+= ratp.o
+obj-$(CONFIG_RATP)		+= ratp/
 obj-$(CONFIG_BOOTCHOOSER)	+= bootchooser.o
 obj-$(CONFIG_UIMAGE)		+= image.o uimage.o
 obj-$(CONFIG_FITIMAGE)		+= image-fit.o
@@ -60,7 +60,6 @@ obj-$(CONFIG_FILE_LIST)		+= file-list.o
 obj-$(CONFIG_FIRMWARE)		+= firmware.o
 obj-$(CONFIG_UBIFORMAT)		+= ubiformat.o
 obj-$(CONFIG_BAREBOX_UPDATE_IMX_NAND_FCB) += imx-bbu-nand-fcb.o
-obj-$(CONFIG_CONSOLE_RATP)	+= ratp.o
 obj-$(CONFIG_BOOT)		+= boot.o
 
 ifdef CONFIG_PASSWORD
diff --git a/common/module.lds.S b/common/module.lds.S
index a03d04f401..f3dbb12f4a 100644
--- a/common/module.lds.S
+++ b/common/module.lds.S
@@ -35,6 +35,8 @@ SECTIONS
 	.got : { *(.got) }
 
 	.barebox_cmd : { BAREBOX_CMDS }
+	.barebox_ratp_cmd : { BAREBOX_RATP_CMDS }
+
 	. = ALIGN(4);
 	.bss : { *(.bss) }
 }
diff --git a/common/ratp/Kconfig b/common/ratp/Kconfig
new file mode 100644
index 0000000000..93ff75d64d
--- /dev/null
+++ b/common/ratp/Kconfig
@@ -0,0 +1,14 @@
+
+config CONSOLE_RATP
+	bool
+	select RATP
+	select CRC16
+	select POLLER
+	depends on CONSOLE_FULL
+	prompt "RATP console support"
+	help
+	  This option adds support for remote controlling barebox via serial
+	  port. The regular console is designed for human interaction whereas
+	  this option adds a machine readable interface for controlling barebox.
+	  Say yes here if you want to control barebox from a remote host.
+
diff --git a/common/ratp/Makefile b/common/ratp/Makefile
new file mode 100644
index 0000000000..2c6d674f61
--- /dev/null
+++ b/common/ratp/Makefile
@@ -0,0 +1,6 @@
+obj-y += ratp.o
+obj-y += ping.o
+obj-y += getenv.o
+obj-y += md.o
+obj-y += mw.o
+obj-y += reset.o
diff --git a/common/ratp/getenv.c b/common/ratp/getenv.c
new file mode 100644
index 0000000000..b409634886
--- /dev/null
+++ b/common/ratp/getenv.c
@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2018 Sascha Hauer <s.hauer@pengutronix.de>, Pengutronix
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ */
+
+/*
+ * RATP getenv
+ */
+
+#include <common.h>
+#include <ratp_bb.h>
+#include <malloc.h>
+#include <environment.h>
+
+static int ratp_cmd_getenv(const struct ratp_bb *req, int req_len,
+			   struct ratp_bb **rsp, int *rsp_len)
+{
+	int dlen = req_len - sizeof (struct ratp_bb);
+	char *varname;
+	const char *value;
+
+	varname = xstrndup ((const char *)req->data, dlen);
+	value = getenv (varname);
+	free (varname);
+
+	dlen = strlen (value);
+
+	*rsp_len = sizeof(struct ratp_bb) + dlen;
+	*rsp = xzalloc(*rsp_len);
+	(*rsp)->type = cpu_to_be16(BB_RATP_TYPE_GETENV_RETURN);
+	memcpy ((*rsp)->data, value, dlen);
+	return 0;
+}
+
+BAREBOX_RATP_CMD_START(GETENV)
+	.request_id = BB_RATP_TYPE_GETENV,
+	.response_id = BB_RATP_TYPE_GETENV_RETURN,
+	.cmd = ratp_cmd_getenv
+BAREBOX_RATP_CMD_END
diff --git a/common/ratp/md.c b/common/ratp/md.c
new file mode 100644
index 0000000000..9b8fc2bc50
--- /dev/null
+++ b/common/ratp/md.c
@@ -0,0 +1,202 @@
+/*
+ * Copyright (c) 2011-2018 Sascha Hauer <s.hauer@pengutronix.de>, Pengutronix
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * 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 <common.h>
+#include <ratp_bb.h>
+#include <init.h>
+#include <driver.h>
+#include <malloc.h>
+#include <errno.h>
+#include <fs.h>
+#include <libfile.h>
+#include <fcntl.h>
+#include <linux/stat.h>
+#include <xfuncs.h>
+
+/* NOTE:
+ *  - Fixed-size fields (e.g. integers) are given just after the header.
+ *  - Variable-length fields are stored inside the buffer[] and their position
+ *    within the buffer[] and their size are given as fixed-sized fields after
+ *    the header.
+ *  The message may be extended at any time keeping backwards compatibility,
+ *  as the position of the buffer[] is given by the buffer_offset field. i.e.
+ *  increasing the buffer_offset field we can extend the fixed-sized section
+ *  to add more fields.
+ */
+
+struct ratp_bb_md_request {
+	struct ratp_bb header;
+	uint16_t buffer_offset;
+	uint16_t addr;
+	uint16_t size;
+	uint16_t path_size;
+	uint16_t path_offset;
+	uint8_t  buffer[];
+} __attribute__((packed));
+
+struct ratp_bb_md_response {
+	struct ratp_bb header;
+	uint16_t buffer_offset;
+	uint32_t errno;
+	uint16_t data_size;
+	uint16_t data_offset;
+	uint8_t  buffer[];
+} __attribute__((packed));
+
+extern char *mem_rw_buf;
+
+static int do_ratp_mem_md(const char *filename,
+			  loff_t start,
+			  loff_t size,
+			  uint8_t *output)
+{
+	int r, now, t;
+	int ret = 0;
+	int fd;
+	void *map;
+
+	fd = open_and_lseek(filename, O_RWSIZE_1 | O_RDONLY, start);
+	if (fd < 0)
+		return -errno;
+
+	map = memmap(fd, PROT_READ);
+	if (map != (void *)-1) {
+		memcpy(output, (uint8_t *)(map + start), size);
+		goto out;
+	}
+
+	t = 0;
+	do {
+		now = min(size, (loff_t)RW_BUF_SIZE);
+		r = read(fd, mem_rw_buf, now);
+		if (r < 0) {
+			ret = -errno;
+			perror("read");
+			goto out;
+		}
+		if (!r)
+			goto out;
+
+		memcpy(output + t, (uint8_t *)(mem_rw_buf), r);
+
+		size  -= r;
+		t     += r;
+	} while (size);
+
+out:
+	close(fd);
+
+	return ret;
+}
+
+static int ratp_cmd_md(const struct ratp_bb *req, int req_len,
+		       struct ratp_bb **rsp, int *rsp_len)
+{
+	struct ratp_bb_md_request *md_req = (struct ratp_bb_md_request *)req;
+	struct ratp_bb_md_response *md_rsp;
+	uint8_t *buffer;
+	uint16_t buffer_offset;
+	uint16_t buffer_size;
+	int md_rsp_len;
+	uint16_t addr;
+	uint16_t size;
+	uint16_t path_size;
+	uint16_t path_offset;
+	char *path = NULL;
+	int ret = 0;
+
+	/* At least message header should be valid */
+	if (req_len < sizeof(*md_req)) {
+		printf("ratp md ignored: size mismatch (%d < %zu)\n",
+		       req_len, sizeof (*md_req));
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* Validate buffer position and size */
+	buffer_offset = be16_to_cpu(md_req->buffer_offset);
+	if (req_len < buffer_offset) {
+		printf("ratp md ignored: invalid buffer offset (%d < %hu)\n",
+		       req_len, buffer_offset);
+		ret = -EINVAL;
+		goto out;
+	}
+	buffer_size = req_len - buffer_offset;
+	buffer = ((uint8_t *)md_req) + buffer_offset;
+
+	/* Validate path position and size */
+	path_offset = be16_to_cpu(md_req->path_offset);
+	if (path_offset != 0) {
+		printf("ratp md ignored: invalid path offset\n");
+		ret = -EINVAL;
+		goto out;
+	}
+	path_size = be16_to_cpu(md_req->path_size);
+	if (!path_size) {
+		printf("ratp md ignored: no filepath given\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* Validate buffer size */
+	if (buffer_size < path_size) {
+		printf("ratp mw ignored: size mismatch (%d < %hu): path may not be fully given\n",
+		       req_len, path_size);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	addr = be16_to_cpu (md_req->addr);
+	size = be16_to_cpu (md_req->size);
+	path = xstrndup((const char *)&buffer[path_offset], path_size);
+
+out:
+	/* Avoid reading anything on error */
+	if (ret != 0)
+		size = 0;
+
+	md_rsp_len = sizeof(*md_rsp) + size;
+	md_rsp = xzalloc(md_rsp_len);
+	md_rsp->header.type = cpu_to_be16(BB_RATP_TYPE_MD_RETURN);
+	md_rsp->buffer_offset = cpu_to_be16(sizeof(*md_rsp));
+	md_rsp->data_offset = 0;
+
+	/* Don't read anything on error or if 0 bytes were requested */
+	if (size > 0)
+		ret = do_ratp_mem_md(path, addr, size, md_rsp->buffer);
+
+	if (ret != 0) {
+		md_rsp->data_size = 0;
+		md_rsp->errno = cpu_to_be32(ret);
+		md_rsp_len = sizeof(*md_rsp);
+	} else {
+		md_rsp->data_size = cpu_to_be16(size);
+		md_rsp->errno = 0;
+	}
+
+	*rsp = (struct ratp_bb *)md_rsp;
+	*rsp_len = md_rsp_len;
+
+	free (path);
+	return ret;
+}
+
+BAREBOX_RATP_CMD_START(MD)
+	.request_id = BB_RATP_TYPE_MD,
+	.response_id = BB_RATP_TYPE_MD_RETURN,
+	.cmd = ratp_cmd_md
+BAREBOX_RATP_CMD_END
diff --git a/common/ratp/mw.c b/common/ratp/mw.c
new file mode 100644
index 0000000000..7d6df3d0a0
--- /dev/null
+++ b/common/ratp/mw.c
@@ -0,0 +1,173 @@
+/*
+ * Copyright (c) 2011-2018 Sascha Hauer <s.hauer@pengutronix.de>, Pengutronix
+ * Copyright (c) 2018 Zodiac Inflight Innovations
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * 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 <common.h>
+#include <ratp_bb.h>
+#include <init.h>
+#include <driver.h>
+#include <malloc.h>
+#include <errno.h>
+#include <fs.h>
+#include <libfile.h>
+#include <fcntl.h>
+#include <xfuncs.h>
+
+/* NOTE:
+ *  - Fixed-size fields (e.g. integers) are given just after the header.
+ *  - Variable-length fields are stored inside the buffer[] and their position
+ *    within the buffer[] and their size are given as fixed-sized fields after
+ *    the header.
+ *  The message may be extended at any time keeping backwards compatibility,
+ *  as the position of the buffer[] is given by the buffer_offset field. i.e.
+ *  increasing the buffer_offset field we can extend the fixed-sized section
+ *  to add more fields.
+ */
+
+struct ratp_bb_mw_request {
+	struct ratp_bb header;
+	uint16_t buffer_offset;
+	uint16_t addr;
+	uint16_t path_size;
+	uint16_t path_offset;
+	uint16_t data_size;
+	uint16_t data_offset;
+	uint8_t  buffer[];
+} __attribute__((packed));
+
+struct ratp_bb_mw_response {
+	struct ratp_bb header;
+	uint16_t buffer_offset;
+	uint32_t errno;
+	uint16_t written;
+	uint8_t  buffer[];
+} __attribute__((packed));
+
+static int ratp_cmd_mw(const struct ratp_bb *req, int req_len,
+		       struct ratp_bb **rsp, int *rsp_len)
+{
+	struct ratp_bb_mw_request *mw_req = (struct ratp_bb_mw_request *)req;
+	struct ratp_bb_mw_response *mw_rsp;
+	uint8_t *buffer;
+	uint16_t buffer_offset;
+	uint16_t buffer_size;
+	uint16_t addr;
+	uint16_t path_size;
+	uint16_t path_offset;
+	uint16_t data_size;
+	uint16_t data_offset;
+	ssize_t written = 0;
+	char *path = NULL;
+	int fd;
+	int ret = 0;
+
+	/* At least message header should be valid */
+	if (req_len < sizeof(*mw_req)) {
+		printf("ratp mw ignored: size mismatch (%d < %zu)\n",
+		       req_len, sizeof (*mw_req));
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* Validate buffer position and size */
+	buffer_offset = be16_to_cpu(mw_req->buffer_offset);
+	if (req_len < buffer_offset) {
+		printf("ratp mw ignored: invalid buffer offset (%d < %hu)\n",
+		       req_len, buffer_offset);
+		ret = -EINVAL;
+		goto out;
+	}
+	buffer_size = req_len - buffer_offset;
+	buffer = ((uint8_t *)mw_req) + buffer_offset;
+
+	/* Validate path position and size */
+	path_offset = be16_to_cpu(mw_req->path_offset);
+	if (path_offset != 0) {
+		printf("ratp mw ignored: invalid path offset\n");
+		ret = -EINVAL;
+		goto out;
+	}
+	path_size = be16_to_cpu(mw_req->path_size);
+	if (!path_size) {
+		printf("ratp mw ignored: no filepath given\n");
+		ret = -EINVAL;
+		goto out;
+	}
+
+	/* Validate data position and size */
+	data_offset = be16_to_cpu(mw_req->data_offset);
+	if (data_offset != (path_offset + path_size)) {
+		printf("ratp mw ignored: invalid path offset\n");
+		ret = -EINVAL;
+		goto out;
+	}
+	data_size = be16_to_cpu(mw_req->data_size);
+	if (!data_size) {
+		/* Success */
+		goto out;
+	}
+
+	/* Validate buffer size */
+	if (buffer_size < (path_size + data_size)) {
+		printf("ratp mw ignored: size mismatch (%d < %hu): path or data not be fully given\n",
+		       req_len, path_size + data_size);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	addr = be16_to_cpu (mw_req->addr);
+	path = xstrndup((const char *)&buffer[path_offset], path_size);
+
+	fd = open_and_lseek(path, O_RWSIZE_1 | O_WRONLY, addr);
+	if (fd < 0) {
+		ret = -errno;
+		goto out;
+	}
+
+	written = write(fd, &buffer[data_offset], data_size);
+	if (written < 0) {
+		ret = -errno;
+		perror("write");
+	}
+
+	close(fd);
+
+out:
+	mw_rsp = xzalloc(sizeof(*mw_rsp));
+	mw_rsp->header.type = cpu_to_be16(BB_RATP_TYPE_MW_RETURN);
+	mw_rsp->buffer_offset = cpu_to_be16(sizeof(*mw_rsp)); /* n/a */
+
+	if (ret != 0) {
+		mw_rsp->written = 0;
+		mw_rsp->errno = cpu_to_be32(ret);
+	} else {
+		mw_rsp->written = cpu_to_be16((uint16_t)written);
+		mw_rsp->errno = 0;
+	}
+
+	*rsp = (struct ratp_bb *)mw_rsp;
+	*rsp_len = sizeof(*mw_rsp);
+
+	free (path);
+	return ret;
+}
+
+BAREBOX_RATP_CMD_START(MW)
+	.request_id = BB_RATP_TYPE_MW,
+	.response_id = BB_RATP_TYPE_MW_RETURN,
+	.cmd = ratp_cmd_mw
+BAREBOX_RATP_CMD_END
diff --git a/common/ratp/ping.c b/common/ratp/ping.c
new file mode 100644
index 0000000000..cc29ea36c6
--- /dev/null
+++ b/common/ratp/ping.c
@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2018 Sascha Hauer <s.hauer@pengutronix.de>, Pengutronix
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ */
+
+/*
+ * RATP ping
+ */
+
+#include <common.h>
+#include <ratp_bb.h>
+#include <malloc.h>
+
+static int ratp_cmd_ping(const struct ratp_bb *req, int req_len,
+			 struct ratp_bb **rsp, int *rsp_len)
+{
+	/* Just build response */
+	*rsp_len = sizeof(struct ratp_bb);
+	*rsp = xzalloc(*rsp_len);
+	(*rsp)->type = cpu_to_be16(BB_RATP_TYPE_PONG);
+	return 0;
+}
+
+BAREBOX_RATP_CMD_START(PING)
+	.request_id = BB_RATP_TYPE_PING,
+	.response_id = BB_RATP_TYPE_PONG,
+	.cmd = ratp_cmd_ping
+BAREBOX_RATP_CMD_END
diff --git a/common/ratp.c b/common/ratp/ratp.c
index 80863f81fb..fae9cec5b9 100644
--- a/common/ratp.c
+++ b/common/ratp/ratp.c
@@ -31,21 +31,10 @@
 #include <ratp_bb.h>
 #include <fs.h>
 
-#define BB_RATP_TYPE_COMMAND		1
-#define BB_RATP_TYPE_COMMAND_RETURN	2
-#define BB_RATP_TYPE_CONSOLEMSG		3
-#define BB_RATP_TYPE_PING		4
-#define BB_RATP_TYPE_PONG		5
-#define BB_RATP_TYPE_GETENV		6
-#define BB_RATP_TYPE_GETENV_RETURN	7
-#define BB_RATP_TYPE_FS			8
-#define BB_RATP_TYPE_FS_RETURN		9
-
-struct ratp_bb {
-	uint16_t type;
-	uint16_t flags;
-	uint8_t data[];
-};
+LIST_HEAD(ratp_command_list);
+EXPORT_SYMBOL(ratp_command_list);
+
+#define for_each_ratp_command(cmd) list_for_each_entry(cmd, &ratp_command_list, list)
 
 struct ratp_bb_command_return {
 	uint32_t errno;
@@ -66,6 +55,51 @@ struct ratp_ctx {
 	struct poller_struct poller;
 };
 
+static int compare_ratp_command(struct list_head *a, struct list_head *b)
+{
+	int id_a = list_entry(a, struct ratp_command, list)->request_id;
+	int id_b = list_entry(b, struct ratp_command, list)->request_id;
+
+	return (id_a - id_b);
+}
+
+int register_ratp_command(struct ratp_command *cmd)
+{
+	debug("register ratp command: request %hu, response %hu\n",
+	      cmd->request_id, cmd->response_id);
+	list_add_sort(&cmd->list, &ratp_command_list, compare_ratp_command);
+	return 0;
+}
+EXPORT_SYMBOL(register_ratp_command);
+
+struct ratp_command *find_ratp_request(uint16_t request_id)
+{
+	struct ratp_command *cmdtp;
+
+	for_each_ratp_command(cmdtp)
+		if (request_id == cmdtp->request_id)
+			return cmdtp;
+
+	return NULL;	/* not found */
+}
+
+extern struct ratp_command __barebox_ratp_cmd_start;
+extern struct ratp_command __barebox_ratp_cmd_end;
+
+static int init_ratp_command_list(void)
+{
+	struct ratp_command *cmdtp;
+
+	for (cmdtp = &__barebox_ratp_cmd_start;
+			cmdtp != &__barebox_ratp_cmd_end;
+			cmdtp++)
+		register_ratp_command(cmdtp);
+
+	return 0;
+}
+
+late_initcall(init_ratp_command_list);
+
 static int console_recv(struct ratp *r, uint8_t *data)
 {
 	struct ratp_ctx *ctx = container_of(r, struct ratp_ctx, ratp);
@@ -98,17 +132,6 @@ static int console_send(struct ratp *r, void *pkt, int len)
 	return 0;
 }
 
-static void *xmemdup_add_zero(const void *buf, int len)
-{
-	void *ret;
-
-	ret = xzalloc(len + 1);
-	*(uint8_t *)(ret + len) = 0;
-	memcpy(ret, buf, len);
-
-	return ret;
-}
-
 static void ratp_queue_console_tx(struct ratp_ctx *ctx)
 {
 	u8 buf[255];
@@ -155,48 +178,6 @@ static int ratp_bb_send_command_return(struct ratp_ctx *ctx, uint32_t errno)
 	return ret;
 }
 
-static int ratp_bb_send_pong(struct ratp_ctx *ctx)
-{
-	void *buf;
-	struct ratp_bb *rbb;
-	int len = sizeof(*rbb);
-	int ret;
-
-	buf = xzalloc(len);
-	rbb = buf;
-
-	rbb->type = cpu_to_be16(BB_RATP_TYPE_PONG);
-
-	ret = ratp_send(&ctx->ratp, buf, len);
-
-	free(buf);
-
-	return ret;
-}
-
-static int ratp_bb_send_getenv_return(struct ratp_ctx *ctx, const char *val)
-{
-	void *buf;
-	struct ratp_bb *rbb;
-	int len, ret;
-
-	if (!val)
-	    val = "";
-
-	len = sizeof(*rbb) + strlen(val);
-	buf = xzalloc(len);
-	rbb = buf;
-	strcpy(rbb->data, val);
-
-	rbb->type = cpu_to_be16(BB_RATP_TYPE_GETENV_RETURN);
-
-	ret = ratp_send(&ctx->ratp, buf, len);
-
-	free(buf);
-
-	return ret;
-}
-
 static char *ratp_command;
 static struct ratp_ctx *ratp_ctx;
 
@@ -205,39 +186,45 @@ static int ratp_bb_dispatch(struct ratp_ctx *ctx, const void *buf, int len)
 	const struct ratp_bb *rbb = buf;
 	struct ratp_bb_pkt *pkt;
 	int dlen = len - sizeof(struct ratp_bb);
-	char *varname;
 	int ret = 0;
+	uint16_t type = be16_to_cpu(rbb->type);
+	struct ratp_command *cmd;
+
+	/* See if there's a command registered to this type */
+	cmd = find_ratp_request(type);
+	if (cmd) {
+		struct ratp_bb *rsp = NULL;
+		int rsp_len = 0;
+
+		ret = cmd->cmd(rbb, len, &rsp, &rsp_len);
+		if (!ret)
+			ret = ratp_send(&ctx->ratp, rsp, rsp_len);
+
+		free(rsp);
+		return ret;
+	}
 
-	switch (be16_to_cpu(rbb->type)) {
+	switch (type) {
 	case BB_RATP_TYPE_COMMAND:
-		if (ratp_command)
+		if (!IS_ENABLED(CONFIG_CONSOLE_RATP) || ratp_command)
 			return 0;
 
-		ratp_command = xmemdup_add_zero(&rbb->data, dlen);
+		ratp_command = xstrndup((const char *)rbb->data, dlen);
 		ratp_ctx = ctx;
 		pr_debug("got command: %s\n", ratp_command);
 
 		break;
 
 	case BB_RATP_TYPE_COMMAND_RETURN:
-	case BB_RATP_TYPE_PONG:
 		break;
 
 	case BB_RATP_TYPE_CONSOLEMSG:
+		if (!IS_ENABLED(CONFIG_CONSOLE_RATP))
+			return 0;
 
 		kfifo_put(ctx->console_recv_fifo, rbb->data, dlen);
 		break;
 
-	case BB_RATP_TYPE_PING:
-		ret = ratp_bb_send_pong(ctx);
-		break;
-
-	case BB_RATP_TYPE_GETENV:
-		varname = xmemdup_add_zero(&rbb->data, dlen);
-
-		ret = ratp_bb_send_getenv_return(ctx, getenv(varname));
-		break;
-
 	case BB_RATP_TYPE_FS_RETURN:
 		pkt = xzalloc(sizeof(*pkt) + dlen);
 		pkt->len = dlen;
@@ -370,7 +357,8 @@ static void ratp_poller(struct poller_struct *poller)
 	size_t len;
 	void *buf;
 
-	ratp_queue_console_tx(ctx);
+	if (IS_ENABLED(CONFIG_CONSOLE_RATP))
+		ratp_queue_console_tx(ctx);
 
 	ret = ratp_poll(&ctx->ratp);
 	if (ret == -EINTR)
diff --git a/common/ratp/reset.c b/common/ratp/reset.c
new file mode 100644
index 0000000000..ca8be4e62f
--- /dev/null
+++ b/common/ratp/reset.c
@@ -0,0 +1,55 @@
+/*
+ * reset.c - reset the cpu
+ *
+ * Copyright (c) 2007 Sascha Hauer <s.hauer@pengutronix.de>, Pengutronix
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2
+ * as published by the Free Software Foundation.
+ *
+ * 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 <common.h>
+#include <command.h>
+#include <ratp_bb.h>
+#include <complete.h>
+#include <getopt.h>
+#include <restart.h>
+
+struct ratp_bb_reset {
+	struct ratp_bb header;
+	uint8_t        force;
+} __attribute__((packed));
+
+static int ratp_cmd_reset(const struct ratp_bb *req, int req_len,
+			  struct ratp_bb **rsp, int *rsp_len)
+{
+	struct ratp_bb_reset *reset_req = (struct ratp_bb_reset *)req;
+
+	if (req_len < sizeof (*reset_req)) {
+		printf ("ratp reset ignored: size mismatch (%d < %zu)\n", req_len, sizeof (*reset_req));
+		return 2;
+	}
+
+	debug("running reset %s\n", reset_req->force ? "(forced)" : "");
+
+	if (!reset_req->force)
+		shutdown_barebox();
+
+	restart_machine();
+	/* Not reached */
+	return 1;
+}
+
+BAREBOX_RATP_CMD_START(RESET)
+	.request_id = BB_RATP_TYPE_RESET,
+	.cmd = ratp_cmd_reset
+BAREBOX_RATP_CMD_END