target/linux/brcm2708/patches-4.4/0533-brcmvirt_gpio-Create-coherent-buffer-and-push-to-fir.patch

   1 From 07afae52a73991a3ea948aab5d0303a5a9805b41 Mon Sep 17 00:00:00 2001
   2 From: popcornmix <popcornmix@gmail.com>
   3 Date: Wed, 9 Nov 2016 22:42:39 +0000
   4 Subject: [PATCH] brcmvirt_gpio: Create coherent buffer and push to firmware
   5
   6 ---
   7  drivers/gpio/gpio-bcm-virt.c               | 88 +++++++++++++++++++++---------
   8  include/soc/bcm2835/raspberrypi-firmware.h |  1 +
   9  2 files changed, 62 insertions(+), 27 deletions(-)
  10
  11 --- a/drivers/gpio/gpio-bcm-virt.c
  12 +++ b/drivers/gpio/gpio-bcm-virt.c
  13 @@ -15,6 +15,7 @@
  14  #include <linux/module.h>
  15  #include <linux/basic_mmio_gpio.h>
  16  #include <linux/platform_device.h>
  17 +#include <linux/dma-mapping.h>
  18  #include <soc/bcm2835/raspberrypi-firmware.h>
  19
  20  #define MODULE_NAME "brcmvirt-gpio"
  21 @@ -26,6 +27,7 @@ struct brcmvirt_gpio {
  22         /* two packed 16-bit counts of enabled and disables
  23             Allows host to detect a brief enable that was missed */
  24         u32                     enables_disables[NUM_GPIO];
  25 +       dma_addr_t              bus_addr;
  26  };
  27
  28  static int brcmvirt_gpio_dir_in(struct gpio_chip *gc, unsigned off)
  29 @@ -76,13 +78,13 @@ static void brcmvirt_gpio_set(struct gpi
  30
  31  static int brcmvirt_gpio_probe(struct platform_device *pdev)
  32  {
  33 +       int err = 0;
  34         struct device *dev = &pdev->dev;
  35         struct device_node *np = dev->of_node;
  36         struct device_node *fw_node;
  37         struct rpi_firmware *fw;
  38         struct brcmvirt_gpio *ucb;
  39         u32 gpiovirtbuf;
  40 -       int err = 0;
  41
  42         fw_node = of_parse_phandle(np, "firmware", 0);
  43         if (!fw_node) {
  44 @@ -94,35 +96,56 @@ static int brcmvirt_gpio_probe(struct pl
  45         if (!fw)
  46                 return -EPROBE_DEFER;
  47
  48 -       err = rpi_firmware_property(fw, RPI_FIRMWARE_FRAMEBUFFER_GET_GPIOVIRTBUF,
  49 -                                   &gpiovirtbuf, sizeof(gpiovirtbuf));
  50 -
  51 -       if (err) {
  52 -               dev_err(dev, "Failed to get gpiovirtbuf\n");
  53 -               goto err;
  54 -       }
  55 -
  56 -       if (!gpiovirtbuf) {
  57 -               dev_err(dev, "No virtgpio buffer\n");
  58 -               err = -ENOENT;
  59 -               goto err;
  60 -       }
  61 -
  62         ucb = devm_kzalloc(dev, sizeof *ucb, GFP_KERNEL);
  63         if (!ucb) {
  64                 err = -EINVAL;
  65 -               goto err;
  66 +               goto out;
  67         }
  68
  69 -       // mmap the physical memory
  70 -       gpiovirtbuf &= ~0xc0000000;
  71 -       ucb->ts_base = ioremap(gpiovirtbuf, 4096);
  72 -       if (ucb->ts_base == NULL) {
  73 -               dev_err(dev, "Failed to map physical address\n");
  74 -               err = -ENOENT;
  75 -               goto err;
  76 +       ucb->ts_base = dma_zalloc_coherent(NULL, PAGE_SIZE, &ucb->bus_addr, GFP_KERNEL);
  77 +       if (!ucb->ts_base) {
  78 +               pr_err("[%s]: failed to dma_alloc_coherent(%ld)\n",
  79 +                               __func__, PAGE_SIZE);
  80 +               err = -ENOMEM;
  81 +               goto out;
  82         }
  83
  84 +       gpiovirtbuf = (u32)ucb->bus_addr;
  85 +       err = rpi_firmware_property(fw, RPI_FIRMWARE_FRAMEBUFFER_SET_GPIOVIRTBUF,
  86 +                                   &gpiovirtbuf, sizeof(gpiovirtbuf));
  87 +
  88 +       if (err || gpiovirtbuf != 0) {
  89 +               dev_warn(dev, "Failed to set gpiovirtbuf, trying to get err:%x\n", err);
  90 +               dma_free_coherent(NULL, PAGE_SIZE, ucb->ts_base, ucb->bus_addr);
  91 +               ucb->ts_base = 0;
  92 +               ucb->bus_addr = 0;
  93 +       }
  94 +
  95 +       if (!ucb->ts_base) {
  96 +               err = rpi_firmware_property(fw, RPI_FIRMWARE_FRAMEBUFFER_GET_GPIOVIRTBUF,
  97 +                                           &gpiovirtbuf, sizeof(gpiovirtbuf));
  98 +
  99 +               if (err) {
 100 +                       dev_err(dev, "Failed to get gpiovirtbuf\n");
 101 +                       goto out;
 102 +               }
 103 +
 104 +               if (!gpiovirtbuf) {
 105 +                       dev_err(dev, "No virtgpio buffer\n");
 106 +                       err = -ENOENT;
 107 +                       goto out;
 108 +               }
 109 +
 110 +               // mmap the physical memory
 111 +               gpiovirtbuf &= ~0xc0000000;
 112 +               ucb->ts_base = ioremap(gpiovirtbuf, 4096);
 113 +               if (ucb->ts_base == NULL) {
 114 +                       dev_err(dev, "Failed to map physical address\n");
 115 +                       err = -ENOENT;
 116 +                       goto out;
 117 +               }
 118 +               ucb->bus_addr = 0;
 119 +       }
 120         ucb->gc.label = MODULE_NAME;
 121         ucb->gc.owner = THIS_MODULE;
 122         ucb->gc.dev = dev;
 123 @@ -138,13 +161,21 @@ static int brcmvirt_gpio_probe(struct pl
 124
 125         err = gpiochip_add(&ucb->gc);
 126         if (err)
 127 -               goto err;
 128 +               goto out;
 129
 130         platform_set_drvdata(pdev, ucb);
 131
 132 -err:
 133 +       return 0;
 134 +out:
 135 +       if (ucb->bus_addr) {
 136 +               dma_free_coherent(NULL, PAGE_SIZE, ucb->ts_base, ucb->bus_addr);
 137 +               ucb->bus_addr = 0;
 138 +               ucb->ts_base = NULL;
 139 +       } else if (ucb->ts_base) {
 140 +               iounmap(ucb->ts_base);
 141 +               ucb->ts_base = NULL;
 142 +       }
 143         return err;
 144 -
 145  }
 146
 147  static int brcmvirt_gpio_remove(struct platform_device *pdev)
 148 @@ -153,7 +184,10 @@ static int brcmvirt_gpio_remove(struct p
 149         struct brcmvirt_gpio *ucb = platform_get_drvdata(pdev);
 150
 151         gpiochip_remove(&ucb->gc);
 152 -       iounmap(ucb->ts_base);
 153 +       if (ucb->bus_addr)
 154 +               dma_free_coherent(NULL, PAGE_SIZE, ucb->ts_base, ucb->bus_addr);
 155 +       else if (ucb->ts_base)
 156 +               iounmap(ucb->ts_base);
 157         return err;
 158  }
 159
 160 --- a/include/soc/bcm2835/raspberrypi-firmware.h
 161 +++ b/include/soc/bcm2835/raspberrypi-firmware.h
 162 @@ -118,6 +118,7 @@ enum rpi_firmware_property_tag {
 163         RPI_FIRMWARE_FRAMEBUFFER_SET_OVERSCAN =               0x0004800a,
 164         RPI_FIRMWARE_FRAMEBUFFER_SET_PALETTE =                0x0004800b,
 165         RPI_FIRMWARE_FRAMEBUFFER_SET_TOUCHBUF =               0x0004801f,
 166 +       RPI_FIRMWARE_FRAMEBUFFER_SET_GPIOVIRTBUF =            0x00048020,
 167         RPI_FIRMWARE_FRAMEBUFFER_SET_VSYNC =                  0x0004800e,
 168         RPI_FIRMWARE_FRAMEBUFFER_SET_BACKLIGHT =              0x0004800f,
 169