1 files changed, 399 insertions, 37 deletions

diff --git a/drivers/firmware/tegra/bpmp-debugfs.c b/drivers/firmware/tegra/bpmp-debugfs.c
index 636b40d4364d..c1bbba9ee93a 100644
--- a/drivers/firmware/tegra/bpmp-debugfs.c
+++ b/drivers/firmware/tegra/bpmp-debugfs.c
@@ -4,11 +4,14 @@
  */
 #include <linux/debugfs.h>
 #include <linux/dma-mapping.h>
+#include <linux/slab.h>
 #include <linux/uaccess.h>
 
 #include <soc/tegra/bpmp.h>
 #include <soc/tegra/bpmp-abi.h>
 
+static DEFINE_MUTEX(bpmp_debug_lock);
+
 struct seqbuf {
 	char *buf;
 	size_t pos;
@@ -96,6 +99,354 @@ static const char *get_filename(struct tegra_bpmp *bpmp,
 	return filename;
 }
 
+static int mrq_debug_open(struct tegra_bpmp *bpmp, const char *name,
+			  uint32_t *fd, uint32_t *len, bool write)
+{
+	struct mrq_debug_request req = {
+		.cmd = cpu_to_le32(write ? CMD_DEBUG_OPEN_WO : CMD_DEBUG_OPEN_RO),
+	};
+	struct mrq_debug_response resp;
+	struct tegra_bpmp_message msg = {
+		.mrq = MRQ_DEBUG,
+		.tx = {
+			.data = &req,
+			.size = sizeof(req),
+		},
+		.rx = {
+			.data = &resp,
+			.size = sizeof(resp),
+		},
+	};
+	ssize_t sz_name;
+	int err = 0;
+
+	sz_name = strscpy(req.fop.name, name, sizeof(req.fop.name));
+	if (sz_name < 0) {
+		pr_err("File name too large: %s\n", name);
+		return -EINVAL;
+	}
+
+	err = tegra_bpmp_transfer(bpmp, &msg);
+	if (err < 0)
+		return err;
+	else if (msg.rx.ret < 0)
+		return -EINVAL;
+
+	*len = resp.fop.datalen;
+	*fd = resp.fop.fd;
+
+	return 0;
+}
+
+static int mrq_debug_close(struct tegra_bpmp *bpmp, uint32_t fd)
+{
+	struct mrq_debug_request req = {
+		.cmd = cpu_to_le32(CMD_DEBUG_CLOSE),
+		.frd = {
+			.fd = fd,
+		},
+	};
+	struct mrq_debug_response resp;
+	struct tegra_bpmp_message msg = {
+		.mrq = MRQ_DEBUG,
+		.tx = {
+			.data = &req,
+			.size = sizeof(req),
+		},
+		.rx = {
+			.data = &resp,
+			.size = sizeof(resp),
+		},
+	};
+	int err = 0;
+
+	err = tegra_bpmp_transfer(bpmp, &msg);
+	if (err < 0)
+		return err;
+	else if (msg.rx.ret < 0)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int mrq_debug_read(struct tegra_bpmp *bpmp, const char *name,
+			  char *data, size_t sz_data, uint32_t *nbytes)
+{
+	struct mrq_debug_request req = {
+		.cmd = cpu_to_le32(CMD_DEBUG_READ),
+	};
+	struct mrq_debug_response resp;
+	struct tegra_bpmp_message msg = {
+		.mrq = MRQ_DEBUG,
+		.tx = {
+			.data = &req,
+			.size = sizeof(req),
+		},
+		.rx = {
+			.data = &resp,
+			.size = sizeof(resp),
+		},
+	};
+	uint32_t fd = 0, len = 0;
+	int remaining, err;
+
+	mutex_lock(&bpmp_debug_lock);
+	err = mrq_debug_open(bpmp, name, &fd, &len, 0);
+	if (err)
+		goto out;
+
+	if (len > sz_data) {
+		err = -EFBIG;
+		goto close;
+	}
+
+	req.frd.fd = fd;
+	remaining = len;
+
+	while (remaining > 0) {
+		err = tegra_bpmp_transfer(bpmp, &msg);
+		if (err < 0) {
+			goto close;
+		} else if (msg.rx.ret < 0) {
+			err = -EINVAL;
+			goto close;
+		}
+
+		if (resp.frd.readlen > remaining) {
+			pr_err("%s: read data length invalid\n", __func__);
+			err = -EINVAL;
+			goto close;
+		}
+
+		memcpy(data, resp.frd.data, resp.frd.readlen);
+		data += resp.frd.readlen;
+		remaining -= resp.frd.readlen;
+	}
+
+	*nbytes = len;
+
+close:
+	err = mrq_debug_close(bpmp, fd);
+out:
+	mutex_unlock(&bpmp_debug_lock);
+	return err;
+}
+
+static int mrq_debug_write(struct tegra_bpmp *bpmp, const char *name,
+			   uint8_t *data, size_t sz_data)
+{
+	struct mrq_debug_request req = {
+		.cmd = cpu_to_le32(CMD_DEBUG_WRITE)
+	};
+	struct mrq_debug_response resp;
+	struct tegra_bpmp_message msg = {
+		.mrq = MRQ_DEBUG,
+		.tx = {
+			.data = &req,
+			.size = sizeof(req),
+		},
+		.rx = {
+			.data = &resp,
+			.size = sizeof(resp),
+		},
+	};
+	uint32_t fd = 0, len = 0;
+	size_t remaining;
+	int err;
+
+	mutex_lock(&bpmp_debug_lock);
+	err = mrq_debug_open(bpmp, name, &fd, &len, 1);
+	if (err)
+		goto out;
+
+	if (sz_data > len) {
+		err = -EINVAL;
+		goto close;
+	}
+
+	req.fwr.fd = fd;
+	remaining = sz_data;
+
+	while (remaining > 0) {
+		len = min(remaining, sizeof(req.fwr.data));
+		memcpy(req.fwr.data, data, len);
+		req.fwr.datalen = len;
+
+		err = tegra_bpmp_transfer(bpmp, &msg);
+		if (err < 0) {
+			goto close;
+		} else if (msg.rx.ret < 0) {
+			err = -EINVAL;
+			goto close;
+		}
+
+		data += req.fwr.datalen;
+		remaining -= req.fwr.datalen;
+	}
+
+close:
+	err = mrq_debug_close(bpmp, fd);
+out:
+	mutex_unlock(&bpmp_debug_lock);
+	return err;
+}
+
+static int bpmp_debug_show(struct seq_file *m, void *p)
+{
+	struct file *file = m->private;
+	struct inode *inode = file_inode(file);
+	struct tegra_bpmp *bpmp = inode->i_private;
+	char *databuf = NULL;
+	char fnamebuf[256];
+	const char *filename;
+	uint32_t nbytes = 0;
+	size_t len;
+	int err;
+
+	len = seq_get_buf(m, &databuf);
+	if (!databuf)
+		return -ENOMEM;
+
+	filename = get_filename(bpmp, file, fnamebuf, sizeof(fnamebuf));
+	if (!filename)
+		return -ENOENT;
+
+	err = mrq_debug_read(bpmp, filename, databuf, len, &nbytes);
+	if (!err)
+		seq_commit(m, nbytes);
+
+	return err;
+}
+
+static ssize_t bpmp_debug_store(struct file *file, const char __user *buf,
+		size_t count, loff_t *f_pos)
+{
+	struct inode *inode = file_inode(file);
+	struct tegra_bpmp *bpmp = inode->i_private;
+	char *databuf = NULL;
+	char fnamebuf[256];
+	const char *filename;
+	ssize_t err;
+
+	filename = get_filename(bpmp, file, fnamebuf, sizeof(fnamebuf));
+	if (!filename)
+		return -ENOENT;
+
+	databuf = kmalloc(count, GFP_KERNEL);
+	if (!databuf)
+		return -ENOMEM;
+
+	if (copy_from_user(databuf, buf, count)) {
+		err = -EFAULT;
+		goto free_ret;
+	}
+
+	err = mrq_debug_write(bpmp, filename, databuf, count);
+
+free_ret:
+	kfree(databuf);
+
+	return err ?: count;
+}
+
+static int bpmp_debug_open(struct inode *inode, struct file *file)
+{
+	return single_open_size(file, bpmp_debug_show, file, SZ_256K);
+}
+
+static const struct file_operations bpmp_debug_fops = {
+	.open		= bpmp_debug_open,
+	.read		= seq_read,
+	.llseek		= seq_lseek,
+	.write		= bpmp_debug_store,
+	.release	= single_release,
+};
+
+static int bpmp_populate_debugfs_inband(struct tegra_bpmp *bpmp,
+					struct dentry *parent,
+					char *ppath)
+{
+	const size_t pathlen = SZ_256;
+	const size_t bufsize = SZ_16K;
+	uint32_t dsize, attrs = 0;
+	struct dentry *dentry;
+	struct seqbuf seqbuf;
+	char *buf, *pathbuf;
+	const char *name;
+	int err = 0;
+
+	if (!bpmp || !parent || !ppath)
+		return -EINVAL;
+
+	buf = kmalloc(bufsize, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	pathbuf = kzalloc(pathlen, GFP_KERNEL);
+	if (!pathbuf) {
+		kfree(buf);
+		return -ENOMEM;
+	}
+
+	err = mrq_debug_read(bpmp, ppath, buf, bufsize, &dsize);
+	if (err)
+		goto out;
+
+	seqbuf_init(&seqbuf, buf, dsize);
+
+	while (!seqbuf_eof(&seqbuf)) {
+		err = seqbuf_read_u32(&seqbuf, &attrs);
+		if (err)
+			goto out;
+
+		err = seqbuf_read_str(&seqbuf, &name);
+		if (err < 0)
+			goto out;
+
+		if (attrs & DEBUGFS_S_ISDIR) {
+			size_t len;
+
+			dentry = debugfs_create_dir(name, parent);
+			if (IS_ERR(dentry)) {
+				err = PTR_ERR(dentry);
+				goto out;
+			}
+
+			len = strlen(ppath) + strlen(name) + 1;
+			if (len >= pathlen) {
+				err = -EINVAL;
+				goto out;
+			}
+
+			strncpy(pathbuf, ppath, pathlen);
+			strncat(pathbuf, name, strlen(name));
+			strcat(pathbuf, "/");
+
+			err = bpmp_populate_debugfs_inband(bpmp, dentry,
+							   pathbuf);
+			if (err < 0)
+				goto out;
+		} else {
+			umode_t mode;
+
+			mode = attrs & DEBUGFS_S_IRUSR ? 0400 : 0;
+			mode |= attrs & DEBUGFS_S_IWUSR ? 0200 : 0;
+			dentry = debugfs_create_file(name, mode, parent, bpmp,
+						     &bpmp_debug_fops);
+			if (!dentry) {
+				err = -ENOMEM;
+				goto out;
+			}
+		}
+	}
+
+out:
+	kfree(pathbuf);
+	kfree(buf);
+
+	return err;
+}
+
 static int mrq_debugfs_read(struct tegra_bpmp *bpmp,
 			    dma_addr_t name, size_t sz_name,
 			    dma_addr_t data, size_t sz_data,
@@ -127,6 +478,8 @@ static int mrq_debugfs_read(struct tegra_bpmp *bpmp,
 	err = tegra_bpmp_transfer(bpmp, &msg);
 	if (err < 0)
 		return err;
+	else if (msg.rx.ret < 0)
+		return -EINVAL;
 
 	*nbytes = (size_t)resp.fop.nbytes;
 
@@ -184,6 +537,8 @@ static int mrq_debugfs_dumpdir(struct tegra_bpmp *bpmp, dma_addr_t addr,
 	err = tegra_bpmp_transfer(bpmp, &msg);
 	if (err < 0)
 		return err;
+	else if (msg.rx.ret < 0)
+		return -EINVAL;
 
 	*nbytes = (size_t)resp.dumpdir.nbytes;
 
@@ -202,7 +557,7 @@ static int debugfs_show(struct seq_file *m, void *p)
 	char buf[256];
 	const char *filename;
 	size_t len, nbytes;
-	int ret;
+	int err;
 
 	filename = get_filename(bpmp, file, buf, sizeof(buf));
 	if (!filename)
@@ -216,24 +571,24 @@ static int debugfs_show(struct seq_file *m, void *p)
 	datavirt = dma_alloc_coherent(bpmp->dev, datasize, &dataphys,
 				      GFP_KERNEL | GFP_DMA32);
 	if (!datavirt) {
-		ret = -ENOMEM;
+		err = -ENOMEM;
 		goto free_namebuf;
 	}
 
 	len = strlen(filename);
 	strncpy(namevirt, filename, namesize);
 
-	ret = mrq_debugfs_read(bpmp, namephys, len, dataphys, datasize,
+	err = mrq_debugfs_read(bpmp, namephys, len, dataphys, datasize,
 			       &nbytes);
 
-	if (!ret)
+	if (!err)
 		seq_write(m, datavirt, nbytes);
 
 	dma_free_coherent(bpmp->dev, datasize, datavirt, dataphys);
 free_namebuf:
 	dma_free_coherent(bpmp->dev, namesize, namevirt, namephys);
 
-	return ret;
+	return err;
 }
 
 static int debugfs_open(struct inode *inode, struct file *file)
@@ -253,7 +608,7 @@ static ssize_t debugfs_store(struct file *file, const char __user *buf,
 	char fnamebuf[256];
 	const char *filename;
 	size_t len;
-	int ret;
+	int err;
 
 	filename = get_filename(bpmp, file, fnamebuf, sizeof(fnamebuf));
 	if (!filename)
@@ -267,7 +622,7 @@ static ssize_t debugfs_store(struct file *file, const char __user *buf,
 	datavirt = dma_alloc_coherent(bpmp->dev, datasize, &dataphys,
 				      GFP_KERNEL | GFP_DMA32);
 	if (!datavirt) {
-		ret = -ENOMEM;
+		err = -ENOMEM;
 		goto free_namebuf;
 	}
 
@@ -275,11 +630,11 @@ static ssize_t debugfs_store(struct file *file, const char __user *buf,
 	strncpy(namevirt, filename, namesize);
 
 	if (copy_from_user(datavirt, buf, count)) {
-		ret = -EFAULT;
+		err = -EFAULT;
 		goto free_databuf;
 	}
 
-	ret = mrq_debugfs_write(bpmp, namephys, len, dataphys,
+	err = mrq_debugfs_write(bpmp, namephys, len, dataphys,
 				count);
 
 free_databuf:
@@ -287,7 +642,7 @@ free_databuf:
 free_namebuf:
 	dma_free_coherent(bpmp->dev, namesize, namevirt, namephys);
 
-	return ret ?: count;
+	return err ?: count;
 }
 
 static const struct file_operations debugfs_fops = {
@@ -350,59 +705,66 @@ static int bpmp_populate_dir(struct tegra_bpmp *bpmp, struct seqbuf *seqbuf,
 	return 0;
 }
 
-static int create_debugfs_mirror(struct tegra_bpmp *bpmp, void *buf,
-				 size_t bufsize, struct dentry *root)
+static int bpmp_populate_debugfs_shmem(struct tegra_bpmp *bpmp)
 {
 	struct seqbuf seqbuf;
+	const size_t sz = SZ_512K;
+	dma_addr_t phys;
+	size_t nbytes;
+	void *virt;
 	int err;
 
-	bpmp->debugfs_mirror = debugfs_create_dir("debug", root);
-	if (!bpmp->debugfs_mirror)
+	virt = dma_alloc_coherent(bpmp->dev, sz, &phys,
+				  GFP_KERNEL | GFP_DMA32);
+	if (!virt)
 		return -ENOMEM;
 
-	seqbuf_init(&seqbuf, buf, bufsize);
-	err = bpmp_populate_dir(bpmp, &seqbuf, bpmp->debugfs_mirror, 0);
+	err = mrq_debugfs_dumpdir(bpmp, phys, sz, &nbytes);
 	if (err < 0) {
-		debugfs_remove_recursive(bpmp->debugfs_mirror);
-		bpmp->debugfs_mirror = NULL;
+		goto free;
+	} else if (nbytes > sz) {
+		err = -EINVAL;
+		goto free;
 	}
 
+	seqbuf_init(&seqbuf, virt, nbytes);
+	err = bpmp_populate_dir(bpmp, &seqbuf, bpmp->debugfs_mirror, 0);
+free:
+	dma_free_coherent(bpmp->dev, sz, virt, phys);
+
 	return err;
 }
 
 int tegra_bpmp_init_debugfs(struct tegra_bpmp *bpmp)
 {
-	dma_addr_t phys;
-	void *virt;
-	const size_t sz = SZ_256K;
-	size_t nbytes;
-	int ret;
 	struct dentry *root;
+	bool inband;
+	int err;
 
-	if (!tegra_bpmp_mrq_is_supported(bpmp, MRQ_DEBUGFS))
+	inband = tegra_bpmp_mrq_is_supported(bpmp, MRQ_DEBUG);
+
+	if (!inband && !tegra_bpmp_mrq_is_supported(bpmp, MRQ_DEBUGFS))
 		return 0;
 
 	root = debugfs_create_dir("bpmp", NULL);
 	if (!root)
 		return -ENOMEM;
 
-	virt = dma_alloc_coherent(bpmp->dev, sz, &phys,
-				  GFP_KERNEL | GFP_DMA32);
-	if (!virt) {
-		ret = -ENOMEM;
+	bpmp->debugfs_mirror = debugfs_create_dir("debug", root);
+	if (!bpmp->debugfs_mirror) {
+		err = -ENOMEM;
 		goto out;
 	}
 
-	ret = mrq_debugfs_dumpdir(bpmp, phys, sz, &nbytes);
-	if (ret < 0)
-		goto free;
+	if (inband)
+		err = bpmp_populate_debugfs_inband(bpmp, bpmp->debugfs_mirror,
+						   "/");
+	else
+		err = bpmp_populate_debugfs_shmem(bpmp);
 
-	ret = create_debugfs_mirror(bpmp, virt, nbytes, root);
-free:
-	dma_free_coherent(bpmp->dev, sz, virt, phys);
 out:
-	if (ret < 0)
-		debugfs_remove(root);
+	if (err < 0)
+		debugfs_remove_recursive(root);
 
-	return ret;
+	return err;
 }