drivers/bcm2835gpio: Release resources on error and when quitting

[fw/openocd] / src / jtag / drivers / bcm2835gpio.c

diff --git a/src/jtag/drivers/bcm2835gpio.c b/src/jtag/drivers/bcm2835gpio.c
index fd6c28b964a203f12ff715bb8b8dbfff839dbd66..0bbbc6fced2f5d75f80d0f9b12697a4f92d3e0b9 100644 (file)
--- a/src/jtag/drivers/bcm2835gpio.c
+++ b/src/jtag/drivers/bcm2835gpio.c
@@ -1,22 +1,11 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+
 /***************************************************************************
  *   Copyright (C) 2013 by Paul Fertser, fercerpav@gmail.com               *
  *                                                                         *
  *   Copyright (C) 2012 by Creative Product Design, marc @ cpdesign.com.au *
  *   Based on at91rm9200.c (c) Anders Larsen                               *
  *   and RPi GPIO examples by Gert van Loo & Dom                           *
- *                                                                         *
- *   This program is free software; you can redistribute it and/or modify  *
- *   it under the terms of the GNU General Public License as published by  *
- *   the Free Software Foundation; either version 2 of the License, or     *
- *   (at your option) any later version.                                   *
- *                                                                         *
- *   This program is distributed in the hope that it will be useful,       *
- *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
- *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
- *   GNU General Public License for more details.                          *
- *                                                                         *
- *   You should have received a copy of the GNU General Public License     *
- *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  ***************************************************************************/
 
 #ifdef HAVE_CONFIG_H
@@ -48,7 +37,8 @@ uint32_t bcm2835_peri_base = 0x20000000;
 #define GPIO_LEV (*(pio_base+13)) /* current level of the pin */
 
 static int dev_mem_fd;
-static volatile uint32_t *pio_base;
+static volatile uint32_t *pio_base = MAP_FAILED;
+static volatile uint32_t *pads_base = MAP_FAILED;
 
 static bb_value_t bcm2835gpio_read(void);
 static int bcm2835gpio_write(int tck, int tms, int tdi);
@@ -94,6 +84,11 @@ static int speed_coeff = 113714;
 static int speed_offset = 28;
 static unsigned int jtag_delay;
 
+static int is_gpio_valid(int gpio)
+{
+       return gpio >= 0 && gpio <= 31;
+}
+
 static bb_value_t bcm2835gpio_read(void)
 {
        return (GPIO_LEV & 1<<tdo_gpio) ? BB_HIGH : BB_LOW;
@@ -133,12 +128,12 @@ static int bcm2835gpio_reset(int trst, int srst)
        uint32_t set = 0;
        uint32_t clear = 0;
 
-       if (trst_gpio > 0) {
+       if (is_gpio_valid(trst_gpio)) {
                set |= !trst<<trst_gpio;
                clear |= trst<<trst_gpio;
        }
 
-       if (srst_gpio > 0) {
+       if (is_gpio_valid(srst_gpio)) {
                set |= !srst<<srst_gpio;
                clear |= srst<<srst_gpio;
        }
@@ -151,7 +146,7 @@ static int bcm2835gpio_reset(int trst, int srst)
 
 static void bcm2835_swdio_drive(bool is_output)
 {
-       if (swdio_dir_gpio > 0) {
+       if (is_gpio_valid(swdio_dir_gpio)) {
                if (is_output) {
                        GPIO_SET = 1 << swdio_dir_gpio;
                        OUT_GPIO(swdio_gpio);
@@ -196,11 +191,6 @@ static int bcm2835gpio_speed(int speed)
        return ERROR_OK;
 }
 
-static int is_gpio_valid(int gpio)
-{
-       return gpio >= 0 && gpio <= 31;
-}
-
 COMMAND_HANDLER(bcm2835gpio_handle_jtag_gpionums)
 {
        if (CMD_ARGC == 4) {
@@ -490,6 +480,19 @@ static bool bcm2835gpio_swd_mode_possible(void)
        return 1;
 }
 
+static void bcm2835gpio_munmap(void)
+{
+       if (pio_base != MAP_FAILED) {
+               munmap((void *)pio_base, sysconf(_SC_PAGE_SIZE));
+               pio_base = MAP_FAILED;
+       }
+
+       if (pads_base != MAP_FAILED) {
+               munmap((void *)pads_base, sysconf(_SC_PAGE_SIZE));
+               pads_base = MAP_FAILED;
+       }
+}
+
 static int bcm2835gpio_init(void)
 {
        bitbang_interface = &bcm2835gpio_bitbang;
@@ -525,16 +528,18 @@ static int bcm2835gpio_init(void)
                return ERROR_JTAG_INIT_FAILED;
        }
 
-       static volatile uint32_t *pads_base;
        pads_base = mmap(NULL, sysconf(_SC_PAGE_SIZE), PROT_READ | PROT_WRITE,
                                MAP_SHARED, dev_mem_fd, BCM2835_PADS_GPIO_0_27);
 
        if (pads_base == MAP_FAILED) {
                LOG_ERROR("mmap: %s", strerror(errno));
+               bcm2835gpio_munmap();
                close(dev_mem_fd);
                return ERROR_JTAG_INIT_FAILED;
        }
 
+       close(dev_mem_fd);
+
        /* set 4mA drive strength, slew rate limited, hysteresis on */
        pads_base[BCM2835_PADS_GPIO_0_27_OFFSET] = 0x5a000008 + 1;
 
@@ -557,7 +562,7 @@ static int bcm2835gpio_init(void)
                OUT_GPIO(tck_gpio);
                OUT_GPIO(tms_gpio);
 
-               if (trst_gpio != -1) {
+               if (is_gpio_valid(trst_gpio)) {
                        trst_gpio_mode = MODE_GPIO(trst_gpio);
                        GPIO_SET = 1 << trst_gpio;
                        OUT_GPIO(trst_gpio);
@@ -565,6 +570,13 @@ static int bcm2835gpio_init(void)
        }
 
        if (transport_is_swd()) {
+               /* Make buffer an output before the GPIO connected to it */
+               if (is_gpio_valid(swdio_dir_gpio)) {
+                       swdio_dir_gpio_mode = MODE_GPIO(swdio_dir_gpio);
+                       GPIO_SET = 1 << swdio_dir_gpio;
+                       OUT_GPIO(swdio_dir_gpio);
+               }
+
                swclk_gpio_mode = MODE_GPIO(swclk_gpio);
                swdio_gpio_mode = MODE_GPIO(swdio_gpio);
 
@@ -574,18 +586,12 @@ static int bcm2835gpio_init(void)
                OUT_GPIO(swdio_gpio);
        }
 
-       if (srst_gpio != -1) {
+       if (is_gpio_valid(srst_gpio)) {
                srst_gpio_mode = MODE_GPIO(srst_gpio);
                GPIO_SET = 1 << srst_gpio;
                OUT_GPIO(srst_gpio);
        }
 
-       if (swdio_dir_gpio != -1) {
-               swdio_dir_gpio_mode = MODE_GPIO(swdio_dir_gpio);
-               GPIO_SET = 1 << swdio_dir_gpio;
-               OUT_GPIO(swdio_dir_gpio);
-       }
-
        LOG_DEBUG("saved pinmux settings: tck %d tms %d tdi %d "
                  "tdo %d trst %d srst %d", tck_gpio_mode, tms_gpio_mode,
                  tdi_gpio_mode, tdo_gpio_mode, trst_gpio_mode, srst_gpio_mode);
@@ -600,7 +606,7 @@ static int bcm2835gpio_quit(void)
                SET_MODE_GPIO(tdi_gpio, tdi_gpio_mode);
                SET_MODE_GPIO(tck_gpio, tck_gpio_mode);
                SET_MODE_GPIO(tms_gpio, tms_gpio_mode);
-               if (trst_gpio != -1)
+               if (is_gpio_valid(trst_gpio))
                        SET_MODE_GPIO(trst_gpio, trst_gpio_mode);
        }
 
@@ -609,11 +615,13 @@ static int bcm2835gpio_quit(void)
                SET_MODE_GPIO(swdio_gpio, swdio_gpio_mode);
        }
 
-       if (srst_gpio != -1)
+       if (is_gpio_valid(srst_gpio))
                SET_MODE_GPIO(srst_gpio, srst_gpio_mode);
 
-       if (swdio_dir_gpio != -1)
+       if (is_gpio_valid(swdio_dir_gpio))
                SET_MODE_GPIO(swdio_dir_gpio, swdio_dir_gpio_mode);
 
+       bcm2835gpio_munmap();
+
        return ERROR_OK;
 }