From ce69217c7b8f81e5bcdf1337ceb729100acf8747 Mon Sep 17 00:00:00 2001
From: Andrew Turner <andrew@FreeBSD.org>
Date: Tue, 28 Jul 2020 11:32:45 +0000
Subject: [PATCH] Add a workaround for a bug when setting the Raspberry GIO
 config and state

The Raspberry Pi GPIO config and state messages incorrectly return with
the tag length set to 0. We then check this value to have the response
flag set. Work around this by setting the response flag when setting the
GPIO config or state and this value is zero.

Sponsored by:	Innovate UK
---
 sys/arm/broadcom/bcm2835/bcm2835_mbox.c | 11 +++++++++++
 1 file changed, 11 insertions(+)

diff --git a/sys/arm/broadcom/bcm2835/bcm2835_mbox.c b/sys/arm/broadcom/bcm2835/bcm2835_mbox.c
index b3d012a417fe..d722c7aae7a7 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_mbox.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_mbox.c
@@ -43,6 +43,7 @@ __FBSDID("$FreeBSD$");
 #include <dev/ofw/ofw_bus.h>
 #include <dev/ofw/ofw_bus_subr.h>
 
+#include <arm/broadcom/bcm2835/bcm2835_firmware.h>
 #include <arm/broadcom/bcm2835/bcm2835_mbox.h>
 #include <arm/broadcom/bcm2835/bcm2835_mbox_prop.h>
 #include <arm/broadcom/bcm2835/bcm2835_vcbus.h>
@@ -362,6 +363,16 @@ bcm2835_mbox_err(device_t dev, bus_addr_t msg_phys, uint32_t resp_phys,
 	tag = (struct bcm2835_mbox_tag_hdr *)(msg + 1);
 	last = (uint8_t *)msg + len;
 	for (idx = 0; tag->tag != 0; idx++) {
+		/*
+		 * When setting the GPIO config or state the firmware doesn't
+		 * set tag->val_len correctly.
+		 */
+		if ((tag->tag == BCM2835_FIRMWARE_TAG_SET_GPIO_CONFIG ||
+		     tag->tag == BCM2835_FIRMWARE_TAG_SET_GPIO_STATE) &&
+		    tag->val_len == 0) {
+			tag->val_len = BCM2835_MBOX_TAG_VAL_LEN_RESPONSE |
+			    tag->val_buf_size;
+		}
 		if ((tag->val_len & BCM2835_MBOX_TAG_VAL_LEN_RESPONSE) == 0) {
 			device_printf(dev, "tag %d response error\n", idx);
 			return (EIO);