From 3f948a5f5bb5c736fa051a98fffacc3200d0e40e Mon Sep 17 00:00:00 2001
From: popcornmix <popcornmix@gmail.com>
Date: Wed, 9 Nov 2016 22:42:39 +0000
Subject: [PATCH] brcmvirt_gpio: Create coherent buffer and push to firmware

---
 drivers/gpio/gpio-bcm-virt.c               | 88 +++++++++++++++++++++---------
 include/soc/bcm2835/raspberrypi-firmware.h |  1 +
 2 files changed, 62 insertions(+), 27 deletions(-)

--- a/drivers/gpio/gpio-bcm-virt.c
+++ b/drivers/gpio/gpio-bcm-virt.c
@@ -15,6 +15,7 @@
 #include <linux/module.h>
 #include <linux/basic_mmio_gpio.h>
 #include <linux/platform_device.h>
+#include <linux/dma-mapping.h>
 #include <soc/bcm2835/raspberrypi-firmware.h>
 
 #define MODULE_NAME "brcmvirt-gpio"
@@ -26,6 +27,7 @@ struct brcmvirt_gpio {
 	/* two packed 16-bit counts of enabled and disables
            Allows host to detect a brief enable that was missed */
 	u32			enables_disables[NUM_GPIO];
+	dma_addr_t		bus_addr;
 };
 
 static int brcmvirt_gpio_dir_in(struct gpio_chip *gc, unsigned off)
@@ -76,13 +78,13 @@ static void brcmvirt_gpio_set(struct gpi
 
 static int brcmvirt_gpio_probe(struct platform_device *pdev)
 {
+	int err = 0;
 	struct device *dev = &pdev->dev;
 	struct device_node *np = dev->of_node;
 	struct device_node *fw_node;
 	struct rpi_firmware *fw;
 	struct brcmvirt_gpio *ucb;
 	u32 gpiovirtbuf;
-	int err = 0;
 
 	fw_node = of_parse_phandle(np, "firmware", 0);
 	if (!fw_node) {
@@ -94,35 +96,56 @@ static int brcmvirt_gpio_probe(struct pl
 	if (!fw)
 		return -EPROBE_DEFER;
 
-	err = rpi_firmware_property(fw, RPI_FIRMWARE_FRAMEBUFFER_GET_GPIOVIRTBUF,
-				    &gpiovirtbuf, sizeof(gpiovirtbuf));
-
-	if (err) {
-		dev_err(dev, "Failed to get gpiovirtbuf\n");
-		goto err;
-	}
-
-	if (!gpiovirtbuf) {
-		dev_err(dev, "No virtgpio buffer\n");
-		err = -ENOENT;
-		goto err;
-	}
-
 	ucb = devm_kzalloc(dev, sizeof *ucb, GFP_KERNEL);
 	if (!ucb) {
 		err = -EINVAL;
-		goto err;
+		goto out;
 	}
 
-	// mmap the physical memory
-	gpiovirtbuf &= ~0xc0000000;
-	ucb->ts_base = ioremap(gpiovirtbuf, 4096);
-	if (ucb->ts_base == NULL) {
-		dev_err(dev, "Failed to map physical address\n");
-		err = -ENOENT;
-		goto err;
+	ucb->ts_base = dma_zalloc_coherent(NULL, PAGE_SIZE, &ucb->bus_addr, GFP_KERNEL);
+	if (!ucb->ts_base) {
+		pr_err("[%s]: failed to dma_alloc_coherent(%ld)\n",
+				__func__, PAGE_SIZE);
+		err = -ENOMEM;
+		goto out;
 	}
 
+	gpiovirtbuf = (u32)ucb->bus_addr;
+	err = rpi_firmware_property(fw, RPI_FIRMWARE_FRAMEBUFFER_SET_GPIOVIRTBUF,
+				    &gpiovirtbuf, sizeof(gpiovirtbuf));
+
+	if (err || gpiovirtbuf != 0) {
+		dev_warn(dev, "Failed to set gpiovirtbuf, trying to get err:%x\n", err);
+		dma_free_coherent(NULL, PAGE_SIZE, ucb->ts_base, ucb->bus_addr);
+		ucb->ts_base = 0;
+		ucb->bus_addr = 0;
+	}
+
+	if (!ucb->ts_base) {
+		err = rpi_firmware_property(fw, RPI_FIRMWARE_FRAMEBUFFER_GET_GPIOVIRTBUF,
+					    &gpiovirtbuf, sizeof(gpiovirtbuf));
+
+		if (err) {
+			dev_err(dev, "Failed to get gpiovirtbuf\n");
+			goto out;
+		}
+
+		if (!gpiovirtbuf) {
+			dev_err(dev, "No virtgpio buffer\n");
+			err = -ENOENT;
+			goto out;
+		}
+
+		// mmap the physical memory
+		gpiovirtbuf &= ~0xc0000000;
+		ucb->ts_base = ioremap(gpiovirtbuf, 4096);
+		if (ucb->ts_base == NULL) {
+			dev_err(dev, "Failed to map physical address\n");
+			err = -ENOENT;
+			goto out;
+		}
+		ucb->bus_addr = 0;
+	}
 	ucb->gc.label = MODULE_NAME;
 	ucb->gc.owner = THIS_MODULE;
 	ucb->gc.dev = dev;
@@ -138,13 +161,21 @@ static int brcmvirt_gpio_probe(struct pl
 
 	err = gpiochip_add(&ucb->gc);
 	if (err)
-		goto err;
+		goto out;
 
 	platform_set_drvdata(pdev, ucb);
 
-err:
+	return 0;
+out:
+	if (ucb->bus_addr) {
+		dma_free_coherent(NULL, PAGE_SIZE, ucb->ts_base, ucb->bus_addr);
+		ucb->bus_addr = 0;
+		ucb->ts_base = NULL;
+	} else if (ucb->ts_base) {
+		iounmap(ucb->ts_base);
+		ucb->ts_base = NULL;
+	}
 	return err;
-
 }
 
 static int brcmvirt_gpio_remove(struct platform_device *pdev)
@@ -153,7 +184,10 @@ static int brcmvirt_gpio_remove(struct p
 	struct brcmvirt_gpio *ucb = platform_get_drvdata(pdev);
 
 	gpiochip_remove(&ucb->gc);
-	iounmap(ucb->ts_base);
+	if (ucb->bus_addr)
+		dma_free_coherent(NULL, PAGE_SIZE, ucb->ts_base, ucb->bus_addr);
+	else if (ucb->ts_base)
+		iounmap(ucb->ts_base);
 	return err;
 }
 
--- a/include/soc/bcm2835/raspberrypi-firmware.h
+++ b/include/soc/bcm2835/raspberrypi-firmware.h
@@ -118,6 +118,7 @@ enum rpi_firmware_property_tag {
 	RPI_FIRMWARE_FRAMEBUFFER_SET_OVERSCAN =               0x0004800a,
 	RPI_FIRMWARE_FRAMEBUFFER_SET_PALETTE =                0x0004800b,
 	RPI_FIRMWARE_FRAMEBUFFER_SET_TOUCHBUF =               0x0004801f,
+	RPI_FIRMWARE_FRAMEBUFFER_SET_GPIOVIRTBUF =            0x00048020,
 	RPI_FIRMWARE_FRAMEBUFFER_SET_VSYNC =                  0x0004800e,
 	RPI_FIRMWARE_FRAMEBUFFER_SET_BACKLIGHT =              0x0004800f,