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 22d237f22081d6a04edeedca2cb87920e5164cee..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);
@@ -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;
 
@@ -616,5 +621,7 @@ static int bcm2835gpio_quit(void)
        if (is_gpio_valid(swdio_dir_gpio))
                SET_MODE_GPIO(swdio_dir_gpio, swdio_dir_gpio_mode);
 
+       bcm2835gpio_munmap();
+
        return ERROR_OK;
 }