2 files changed, 70 insertions, 36 deletions

diff --git a/drivers/firmware/tegra/bpmp-debugfs.c b/drivers/firmware/tegra/bpmp-debugfs.c
index f7f6a0a5cb07..a84df1a8ca2b 100644
--- a/drivers/firmware/tegra/bpmp-debugfs.c
+++ b/drivers/firmware/tegra/bpmp-debugfs.c
@@ -379,33 +379,6 @@ static int create_debugfs_mirror(struct tegra_bpmp *bpmp, void *buf,
 	return err;
 }
 
-static int mrq_is_supported(struct tegra_bpmp *bpmp, unsigned int mrq)
-{
-	struct mrq_query_abi_request req = { .mrq = cpu_to_le32(mrq) };
-	struct mrq_query_abi_response resp;
-	struct tegra_bpmp_message msg = {
-		.mrq = MRQ_QUERY_ABI,
-		.tx = {
-			.data = &req,
-			.size = sizeof(req),
-		},
-		.rx = {
-			.data = &resp,
-			.size = sizeof(resp),
-		},
-	};
-	int ret;
-
-	ret = tegra_bpmp_transfer(bpmp, &msg);
-	if (ret < 0) {
-		/* something went wrong; assume not supported */
-		dev_warn(bpmp->dev, "tegra_bpmp_transfer failed (%d)\n", ret);
-		return 0;
-	}
-
-	return resp.status ? 0 : 1;
-}
-
 int tegra_bpmp_init_debugfs(struct tegra_bpmp *bpmp)
 {
 	dma_addr_t phys;
@@ -415,7 +388,7 @@ int tegra_bpmp_init_debugfs(struct tegra_bpmp *bpmp)
 	int ret;
 	struct dentry *root;
 
-	if (!mrq_is_supported(bpmp, MRQ_DEBUGFS))
+	if (!tegra_bpmp_mrq_is_supported(bpmp, MRQ_DEBUGFS))
 		return 0;
 
 	root = debugfs_create_dir("bpmp", NULL);
diff --git a/drivers/firmware/tegra/bpmp.c b/drivers/firmware/tegra/bpmp.c
index a3d5b518c10e..689478b92bce 100644
--- a/drivers/firmware/tegra/bpmp.c
+++ b/drivers/firmware/tegra/bpmp.c
@@ -28,6 +28,7 @@
 
 #define MSG_ACK		BIT(0)
 #define MSG_RING	BIT(1)
+#define TAG_SZ		32
 
 static inline struct tegra_bpmp *
 mbox_client_to_bpmp(struct mbox_client *client)
@@ -470,6 +471,31 @@ unlock:
 }
 EXPORT_SYMBOL_GPL(tegra_bpmp_free_mrq);
 
+bool tegra_bpmp_mrq_is_supported(struct tegra_bpmp *bpmp, unsigned int mrq)
+{
+	struct mrq_query_abi_request req = { .mrq = cpu_to_le32(mrq) };
+	struct mrq_query_abi_response resp;
+	struct tegra_bpmp_message msg = {
+		.mrq = MRQ_QUERY_ABI,
+		.tx = {
+			.data = &req,
+			.size = sizeof(req),
+		},
+		.rx = {
+			.data = &resp,
+			.size = sizeof(resp),
+		},
+	};
+	int ret;
+
+	ret = tegra_bpmp_transfer(bpmp, &msg);
+	if (ret || msg.rx.ret)
+		return false;
+
+	return resp.status == 0;
+}
+EXPORT_SYMBOL_GPL(tegra_bpmp_mrq_is_supported);
+
 static void tegra_bpmp_mrq_handle_ping(unsigned int mrq,
 				       struct tegra_bpmp_channel *channel,
 				       void *data)
@@ -521,8 +547,9 @@ static int tegra_bpmp_ping(struct tegra_bpmp *bpmp)
 	return err;
 }
 
-static int tegra_bpmp_get_firmware_tag(struct tegra_bpmp *bpmp, char *tag,
-				       size_t size)
+/* deprecated version of tag query */
+static int tegra_bpmp_get_firmware_tag_old(struct tegra_bpmp *bpmp, char *tag,
+					   size_t size)
 {
 	struct mrq_query_tag_request request;
 	struct tegra_bpmp_message msg;
@@ -531,7 +558,10 @@ static int tegra_bpmp_get_firmware_tag(struct tegra_bpmp *bpmp, char *tag,
 	void *virt;
 	int err;
 
-	virt = dma_alloc_coherent(bpmp->dev, MSG_DATA_MIN_SZ, &phys,
+	if (size != TAG_SZ)
+		return -EINVAL;
+
+	virt = dma_alloc_coherent(bpmp->dev, TAG_SZ, &phys,
 				  GFP_KERNEL | GFP_DMA32);
 	if (!virt)
 		return -ENOMEM;
@@ -549,13 +579,44 @@ static int tegra_bpmp_get_firmware_tag(struct tegra_bpmp *bpmp, char *tag,
 	local_irq_restore(flags);
 
 	if (err == 0)
-		strlcpy(tag, virt, size);
+		memcpy(tag, virt, TAG_SZ);
 
-	dma_free_coherent(bpmp->dev, MSG_DATA_MIN_SZ, virt, phys);
+	dma_free_coherent(bpmp->dev, TAG_SZ, virt, phys);
 
 	return err;
 }
 
+static int tegra_bpmp_get_firmware_tag(struct tegra_bpmp *bpmp, char *tag,
+				       size_t size)
+{
+	if (tegra_bpmp_mrq_is_supported(bpmp, MRQ_QUERY_FW_TAG)) {
+		struct mrq_query_fw_tag_response resp;
+		struct tegra_bpmp_message msg = {
+			.mrq = MRQ_QUERY_FW_TAG,
+			.rx = {
+				.data = &resp,
+				.size = sizeof(resp),
+			},
+		};
+		int err;
+
+		if (size != sizeof(resp.tag))
+			return -EINVAL;
+
+		err = tegra_bpmp_transfer(bpmp, &msg);
+
+		if (err)
+			return err;
+		if (msg.rx.ret < 0)
+			return -EINVAL;
+
+		memcpy(tag, resp.tag, sizeof(resp.tag));
+		return 0;
+	}
+
+	return tegra_bpmp_get_firmware_tag_old(bpmp, tag, size);
+}
+
 static void tegra_bpmp_channel_signal(struct tegra_bpmp_channel *channel)
 {
 	unsigned long flags = channel->ob->flags;
@@ -664,7 +725,7 @@ static int tegra_bpmp_probe(struct platform_device *pdev)
 {
 	struct tegra_bpmp *bpmp;
 	unsigned int i;
-	char tag[32];
+	char tag[TAG_SZ];
 	size_t size;
 	int err;
 
@@ -792,13 +853,13 @@ static int tegra_bpmp_probe(struct platform_device *pdev)
 		goto free_mrq;
 	}
 
-	err = tegra_bpmp_get_firmware_tag(bpmp, tag, sizeof(tag) - 1);
+	err = tegra_bpmp_get_firmware_tag(bpmp, tag, sizeof(tag));
 	if (err < 0) {
 		dev_err(&pdev->dev, "failed to get firmware tag: %d\n", err);
 		goto free_mrq;
 	}
 
-	dev_info(&pdev->dev, "firmware: %s\n", tag);
+	dev_info(&pdev->dev, "firmware: %.*s\n", (int)sizeof(tag), tag);
 
 	platform_set_drvdata(pdev, bpmp);