git.gag.com Git - fw/openocd/blob - src/jtag/drivers/bcm2835gpio.c

   1 /* SPDX-License-Identifier: GPL-2.0-or-later */
   2
   3 /***************************************************************************
   4  *   Copyright (C) 2013 by Paul Fertser, fercerpav@gmail.com               *
   5  *                                                                         *
   6  *   Copyright (C) 2012 by Creative Product Design, marc @ cpdesign.com.au *
   7  *   Based on at91rm9200.c (c) Anders Larsen                               *
   8  *   and RPi GPIO examples by Gert van Loo & Dom                           *
   9  ***************************************************************************/
  10
  11 #ifdef HAVE_CONFIG_H
  12 #include "config.h"
  13 #endif
  14
  15 #include <jtag/interface.h>
  16 #include <transport/transport.h>
  17 #include "bitbang.h"
  18
  19 #include <sys/mman.h>
  20
  21 uint32_t bcm2835_peri_base = 0x20000000;
  22 #define BCM2835_GPIO_BASE       (bcm2835_peri_base + 0x200000) /* GPIO controller */
  23
  24 #define BCM2835_PADS_GPIO_0_27          (bcm2835_peri_base + 0x100000)
  25 #define BCM2835_PADS_GPIO_0_27_OFFSET   (0x2c / 4)
  26
  27 /* GPIO setup macros */
  28 #define MODE_GPIO(g) (*(pio_base+((g)/10))>>(((g)%10)*3) & 7)
  29 #define INP_GPIO(g) do { *(pio_base+((g)/10)) &= ~(7<<(((g)%10)*3)); } while (0)
  30 #define SET_MODE_GPIO(g, m) do { /* clear the mode bits first, then set as necessary */ \
  31                 INP_GPIO(g);                                            \
  32                 *(pio_base+((g)/10)) |=  ((m)<<(((g)%10)*3)); } while (0)
  33 #define OUT_GPIO(g) SET_MODE_GPIO(g, 1)
  34
  35 #define GPIO_SET (*(pio_base+7))  /* sets   bits which are 1, ignores bits which are 0 */
  36 #define GPIO_CLR (*(pio_base+10)) /* clears bits which are 1, ignores bits which are 0 */
  37 #define GPIO_LEV (*(pio_base+13)) /* current level of the pin */
  38
  39 static int dev_mem_fd;
  40 static volatile uint32_t *pio_base = MAP_FAILED;
  41 static volatile uint32_t *pads_base = MAP_FAILED;
  42
  43 static bb_value_t bcm2835gpio_read(void);
  44 static int bcm2835gpio_write(int tck, int tms, int tdi);
  45
  46 static int bcm2835_swdio_read(void);
  47 static void bcm2835_swdio_drive(bool is_output);
  48 static int bcm2835gpio_swd_write(int swclk, int swdio);
  49
  50 static int bcm2835gpio_init(void);
  51 static int bcm2835gpio_quit(void);
  52
  53 static struct bitbang_interface bcm2835gpio_bitbang = {
  54         .read = bcm2835gpio_read,
  55         .write = bcm2835gpio_write,
  56         .swdio_read = bcm2835_swdio_read,
  57         .swdio_drive = bcm2835_swdio_drive,
  58         .swd_write = bcm2835gpio_swd_write,
  59         .blink = NULL
  60 };
  61
  62 /* GPIO numbers for each signal. Negative values are invalid */
  63 static int tck_gpio = -1;
  64 static int tck_gpio_mode;
  65 static int tms_gpio = -1;
  66 static int tms_gpio_mode;
  67 static int tdi_gpio = -1;
  68 static int tdi_gpio_mode;
  69 static int tdo_gpio = -1;
  70 static int tdo_gpio_mode;
  71 static int trst_gpio = -1;
  72 static int trst_gpio_mode;
  73 static int srst_gpio = -1;
  74 static int srst_gpio_mode;
  75 static int swclk_gpio = -1;
  76 static int swclk_gpio_mode;
  77 static int swdio_gpio = -1;
  78 static int swdio_gpio_mode;
  79 static int swdio_dir_gpio = -1;
  80 static int swdio_dir_gpio_mode;
  81
  82 /* Transition delay coefficients */
  83 static int speed_coeff = 113714;
  84 static int speed_offset = 28;
  85 static unsigned int jtag_delay;
  86
  87 static int is_gpio_valid(int gpio)
  88 {
  89         return gpio >= 0 && gpio <= 31;
  90 }
  91
  92 static bb_value_t bcm2835gpio_read(void)
  93 {
  94         return (GPIO_LEV & 1<<tdo_gpio) ? BB_HIGH : BB_LOW;
  95 }
  96
  97 static int bcm2835gpio_write(int tck, int tms, int tdi)
  98 {
  99         uint32_t set = tck<<tck_gpio | tms<<tms_gpio | tdi<<tdi_gpio;
 100         uint32_t clear = !tck<<tck_gpio | !tms<<tms_gpio | !tdi<<tdi_gpio;
 101
 102         GPIO_SET = set;
 103         GPIO_CLR = clear;
 104
 105         for (unsigned int i = 0; i < jtag_delay; i++)
 106                 asm volatile ("");
 107
 108         return ERROR_OK;
 109 }
 110
 111 static int bcm2835gpio_swd_write(int swclk, int swdio)
 112 {
 113         uint32_t set = swclk << swclk_gpio | swdio << swdio_gpio;
 114         uint32_t clear = !swclk << swclk_gpio | !swdio << swdio_gpio;
 115
 116         GPIO_SET = set;
 117         GPIO_CLR = clear;
 118
 119         for (unsigned int i = 0; i < jtag_delay; i++)
 120                 asm volatile ("");
 121
 122         return ERROR_OK;
 123 }
 124
 125 /* (1) assert or (0) deassert reset lines */
 126 static int bcm2835gpio_reset(int trst, int srst)
 127 {
 128         uint32_t set = 0;
 129         uint32_t clear = 0;
 130
 131         if (is_gpio_valid(trst_gpio)) {
 132                 set |= !trst<<trst_gpio;
 133                 clear |= trst<<trst_gpio;
 134         }
 135
 136         if (is_gpio_valid(srst_gpio)) {
 137                 set |= !srst<<srst_gpio;
 138                 clear |= srst<<srst_gpio;
 139         }
 140
 141         GPIO_SET = set;
 142         GPIO_CLR = clear;
 143
 144         return ERROR_OK;
 145 }
 146
 147 static void bcm2835_swdio_drive(bool is_output)
 148 {
 149         if (is_gpio_valid(swdio_dir_gpio)) {
 150                 if (is_output) {
 151                         GPIO_SET = 1 << swdio_dir_gpio;
 152                         OUT_GPIO(swdio_gpio);
 153                 } else {
 154                         INP_GPIO(swdio_gpio);
 155                         GPIO_CLR = 1 << swdio_dir_gpio;
 156                 }
 157         } else {
 158                 if (is_output)
 159                         OUT_GPIO(swdio_gpio);
 160                 else
 161                         INP_GPIO(swdio_gpio);
 162         }
 163 }
 164
 165 static int bcm2835_swdio_read(void)
 166 {
 167         return !!(GPIO_LEV & 1 << swdio_gpio);
 168 }
 169
 170 static int bcm2835gpio_khz(int khz, int *jtag_speed)
 171 {
 172         if (!khz) {
 173                 LOG_DEBUG("RCLK not supported");
 174                 return ERROR_FAIL;
 175         }
 176         *jtag_speed = speed_coeff/khz - speed_offset;
 177         if (*jtag_speed < 0)
 178                 *jtag_speed = 0;
 179         return ERROR_OK;
 180 }
 181
 182 static int bcm2835gpio_speed_div(int speed, int *khz)
 183 {
 184         *khz = speed_coeff/(speed + speed_offset);
 185         return ERROR_OK;
 186 }
 187
 188 static int bcm2835gpio_speed(int speed)
 189 {
 190         jtag_delay = speed;
 191         return ERROR_OK;
 192 }
 193
 194 COMMAND_HANDLER(bcm2835gpio_handle_jtag_gpionums)
 195 {
 196         if (CMD_ARGC == 4) {
 197                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tck_gpio);
 198                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], tms_gpio);
 199                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[2], tdi_gpio);
 200                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[3], tdo_gpio);
 201         } else if (CMD_ARGC != 0) {
 202                 return ERROR_COMMAND_SYNTAX_ERROR;
 203         }
 204
 205         command_print(CMD,
 206                         "BCM2835 GPIO config: tck = %d, tms = %d, tdi = %d, tdo = %d",
 207                         tck_gpio, tms_gpio, tdi_gpio, tdo_gpio);
 208
 209         return ERROR_OK;
 210 }
 211
 212 COMMAND_HANDLER(bcm2835gpio_handle_jtag_gpionum_tck)
 213 {
 214         if (CMD_ARGC == 1)
 215                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tck_gpio);
 216
 217         command_print(CMD, "BCM2835 GPIO config: tck = %d", tck_gpio);
 218         return ERROR_OK;
 219 }
 220
 221 COMMAND_HANDLER(bcm2835gpio_handle_jtag_gpionum_tms)
 222 {
 223         if (CMD_ARGC == 1)
 224                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tms_gpio);
 225
 226         command_print(CMD, "BCM2835 GPIO config: tms = %d", tms_gpio);
 227         return ERROR_OK;
 228 }
 229
 230 COMMAND_HANDLER(bcm2835gpio_handle_jtag_gpionum_tdo)
 231 {
 232         if (CMD_ARGC == 1)
 233                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tdo_gpio);
 234
 235         command_print(CMD, "BCM2835 GPIO config: tdo = %d", tdo_gpio);
 236         return ERROR_OK;
 237 }
 238
 239 COMMAND_HANDLER(bcm2835gpio_handle_jtag_gpionum_tdi)
 240 {
 241         if (CMD_ARGC == 1)
 242                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], tdi_gpio);
 243
 244         command_print(CMD, "BCM2835 GPIO config: tdi = %d", tdi_gpio);
 245         return ERROR_OK;
 246 }
 247
 248 COMMAND_HANDLER(bcm2835gpio_handle_jtag_gpionum_srst)
 249 {
 250         if (CMD_ARGC == 1)
 251                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], srst_gpio);
 252
 253         command_print(CMD, "BCM2835 GPIO config: srst = %d", srst_gpio);
 254         return ERROR_OK;
 255 }
 256
 257 COMMAND_HANDLER(bcm2835gpio_handle_jtag_gpionum_trst)
 258 {
 259         if (CMD_ARGC == 1)
 260                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], trst_gpio);
 261
 262         command_print(CMD, "BCM2835 GPIO config: trst = %d", trst_gpio);
 263         return ERROR_OK;
 264 }
 265
 266 COMMAND_HANDLER(bcm2835gpio_handle_swd_gpionums)
 267 {
 268         if (CMD_ARGC == 2) {
 269                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swclk_gpio);
 270                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], swdio_gpio);
 271         } else if (CMD_ARGC != 0) {
 272                 return ERROR_COMMAND_SYNTAX_ERROR;
 273         }
 274
 275         command_print(CMD,
 276                         "BCM2835 GPIO nums: swclk = %d, swdio = %d",
 277                         swclk_gpio, swdio_gpio);
 278
 279         return ERROR_OK;
 280 }
 281
 282 COMMAND_HANDLER(bcm2835gpio_handle_swd_gpionum_swclk)
 283 {
 284         if (CMD_ARGC == 1)
 285                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swclk_gpio);
 286
 287         command_print(CMD, "BCM2835 num: swclk = %d", swclk_gpio);
 288         return ERROR_OK;
 289 }
 290
 291 COMMAND_HANDLER(bcm2835gpio_handle_swd_gpionum_swdio)
 292 {
 293         if (CMD_ARGC == 1)
 294                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swdio_gpio);
 295
 296         command_print(CMD, "BCM2835 num: swdio = %d", swdio_gpio);
 297         return ERROR_OK;
 298 }
 299
 300 COMMAND_HANDLER(bcm2835gpio_handle_swd_dir_gpionum_swdio)
 301 {
 302         if (CMD_ARGC == 1)
 303                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], swdio_dir_gpio);
 304
 305         command_print(CMD, "BCM2835 num: swdio_dir = %d", swdio_dir_gpio);
 306         return ERROR_OK;
 307 }
 308
 309 COMMAND_HANDLER(bcm2835gpio_handle_speed_coeffs)
 310 {
 311         if (CMD_ARGC == 2) {
 312                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[0], speed_coeff);
 313                 COMMAND_PARSE_NUMBER(int, CMD_ARGV[1], speed_offset);
 314         }
 315
 316         command_print(CMD, "BCM2835 GPIO: speed_coeffs = %d, speed_offset = %d",
 317                                   speed_coeff, speed_offset);
 318         return ERROR_OK;
 319 }
 320
 321 COMMAND_HANDLER(bcm2835gpio_handle_peripheral_base)
 322 {
 323         if (CMD_ARGC == 1)
 324                 COMMAND_PARSE_NUMBER(u32, CMD_ARGV[0], bcm2835_peri_base);
 325
 326         command_print(CMD, "BCM2835 GPIO: peripheral_base = 0x%08x",
 327                                   bcm2835_peri_base);
 328         return ERROR_OK;
 329 }
 330
 331 static const struct command_registration bcm2835gpio_subcommand_handlers[] = {
 332         {
 333                 .name = "jtag_nums",
 334                 .handler = &bcm2835gpio_handle_jtag_gpionums,
 335                 .mode = COMMAND_CONFIG,
 336                 .help = "gpio numbers for tck, tms, tdi, tdo. (in that order)",
 337                 .usage = "[tck tms tdi tdo]",
 338         },
 339         {
 340                 .name = "tck_num",
 341                 .handler = &bcm2835gpio_handle_jtag_gpionum_tck,
 342                 .mode = COMMAND_CONFIG,
 343                 .help = "gpio number for tck.",
 344                 .usage = "[tck]",
 345         },
 346         {
 347                 .name = "tms_num",
 348                 .handler = &bcm2835gpio_handle_jtag_gpionum_tms,
 349                 .mode = COMMAND_CONFIG,
 350                 .help = "gpio number for tms.",
 351                 .usage = "[tms]",
 352         },
 353         {
 354                 .name = "tdo_num",
 355                 .handler = &bcm2835gpio_handle_jtag_gpionum_tdo,
 356                 .mode = COMMAND_CONFIG,
 357                 .help = "gpio number for tdo.",
 358                 .usage = "[tdo]",
 359         },
 360         {
 361                 .name = "tdi_num",
 362                 .handler = &bcm2835gpio_handle_jtag_gpionum_tdi,
 363                 .mode = COMMAND_CONFIG,
 364                 .help = "gpio number for tdi.",
 365                 .usage = "[tdi]",
 366         },
 367         {
 368                 .name = "swd_nums",
 369                 .handler = &bcm2835gpio_handle_swd_gpionums,
 370                 .mode = COMMAND_CONFIG,
 371                 .help = "gpio numbers for swclk, swdio. (in that order)",
 372                 .usage = "[swclk swdio]",
 373         },
 374         {
 375                 .name = "swclk_num",
 376                 .handler = &bcm2835gpio_handle_swd_gpionum_swclk,
 377                 .mode = COMMAND_CONFIG,
 378                 .help = "gpio number for swclk.",
 379                 .usage = "[swclk]",
 380         },
 381         {
 382                 .name = "swdio_num",
 383                 .handler = &bcm2835gpio_handle_swd_gpionum_swdio,
 384                 .mode = COMMAND_CONFIG,
 385                 .help = "gpio number for swdio.",
 386                 .usage = "[swdio]",
 387         },
 388         {
 389                 .name = "swdio_dir_num",
 390                 .handler = &bcm2835gpio_handle_swd_dir_gpionum_swdio,
 391                 .mode = COMMAND_CONFIG,
 392                 .help = "gpio number for swdio direction control pin (set=output mode, clear=input mode)",
 393                 .usage = "[swdio_dir]",
 394         },
 395         {
 396                 .name = "srst_num",
 397                 .handler = &bcm2835gpio_handle_jtag_gpionum_srst,
 398                 .mode = COMMAND_CONFIG,
 399                 .help = "gpio number for srst.",
 400                 .usage = "[srst]",
 401         },
 402         {
 403                 .name = "trst_num",
 404                 .handler = &bcm2835gpio_handle_jtag_gpionum_trst,
 405                 .mode = COMMAND_CONFIG,
 406                 .help = "gpio number for trst.",
 407                 .usage = "[trst]",
 408         },
 409         {
 410                 .name = "speed_coeffs",
 411                 .handler = &bcm2835gpio_handle_speed_coeffs,
 412                 .mode = COMMAND_CONFIG,
 413                 .help = "SPEED_COEFF and SPEED_OFFSET for delay calculations.",
 414                 .usage = "[SPEED_COEFF SPEED_OFFSET]",
 415         },
 416         {
 417                 .name = "peripheral_base",
 418                 .handler = &bcm2835gpio_handle_peripheral_base,
 419                 .mode = COMMAND_CONFIG,
 420                 .help = "peripheral base to access GPIOs (RPi1 0x20000000, RPi2 0x3F000000).",
 421                 .usage = "[base]",
 422         },
 423
 424         COMMAND_REGISTRATION_DONE
 425 };
 426
 427 static const struct command_registration bcm2835gpio_command_handlers[] = {
 428         {
 429                 .name = "bcm2835gpio",
 430                 .mode = COMMAND_ANY,
 431                 .help = "perform bcm2835gpio management",
 432                 .chain = bcm2835gpio_subcommand_handlers,
 433                 .usage = "",
 434         },
 435         COMMAND_REGISTRATION_DONE
 436 };
 437
 438 static const char * const bcm2835_transports[] = { "jtag", "swd", NULL };
 439
 440 static struct jtag_interface bcm2835gpio_interface = {
 441         .supported = DEBUG_CAP_TMS_SEQ,
 442         .execute_queue = bitbang_execute_queue,
 443 };
 444
 445 struct adapter_driver bcm2835gpio_adapter_driver = {
 446         .name = "bcm2835gpio",
 447         .transports = bcm2835_transports,
 448         .commands = bcm2835gpio_command_handlers,
 449
 450         .init = bcm2835gpio_init,
 451         .quit = bcm2835gpio_quit,
 452         .reset = bcm2835gpio_reset,
 453         .speed = bcm2835gpio_speed,
 454         .khz = bcm2835gpio_khz,
 455         .speed_div = bcm2835gpio_speed_div,
 456
 457         .jtag_ops = &bcm2835gpio_interface,
 458         .swd_ops = &bitbang_swd,
 459 };
 460
 461 static bool bcm2835gpio_jtag_mode_possible(void)
 462 {
 463         if (!is_gpio_valid(tck_gpio))
 464                 return 0;
 465         if (!is_gpio_valid(tms_gpio))
 466                 return 0;
 467         if (!is_gpio_valid(tdi_gpio))
 468                 return 0;
 469         if (!is_gpio_valid(tdo_gpio))
 470                 return 0;
 471         return 1;
 472 }
 473
 474 static bool bcm2835gpio_swd_mode_possible(void)
 475 {
 476         if (!is_gpio_valid(swclk_gpio))
 477                 return 0;
 478         if (!is_gpio_valid(swdio_gpio))
 479                 return 0;
 480         return 1;
 481 }
 482
 483 static void bcm2835gpio_munmap(void)
 484 {
 485         if (pio_base != MAP_FAILED) {
 486                 munmap((void *)pio_base, sysconf(_SC_PAGE_SIZE));
 487                 pio_base = MAP_FAILED;
 488         }
 489
 490         if (pads_base != MAP_FAILED) {
 491                 munmap((void *)pads_base, sysconf(_SC_PAGE_SIZE));
 492                 pads_base = MAP_FAILED;
 493         }
 494 }
 495
 496 static int bcm2835gpio_init(void)
 497 {
 498         bitbang_interface = &bcm2835gpio_bitbang;
 499
 500         LOG_INFO("BCM2835 GPIO JTAG/SWD bitbang driver");
 501
 502         if (transport_is_jtag() && !bcm2835gpio_jtag_mode_possible()) {
 503                 LOG_ERROR("Require tck, tms, tdi and tdo gpios for JTAG mode");
 504                 return ERROR_JTAG_INIT_FAILED;
 505         }
 506
 507         if (transport_is_swd() && !bcm2835gpio_swd_mode_possible()) {
 508                 LOG_ERROR("Require swclk and swdio gpio for SWD mode");
 509                 return ERROR_JTAG_INIT_FAILED;
 510         }
 511
 512         dev_mem_fd = open("/dev/gpiomem", O_RDWR | O_SYNC);
 513         if (dev_mem_fd < 0) {
 514                 LOG_DEBUG("Cannot open /dev/gpiomem, fallback to /dev/mem");
 515                 dev_mem_fd = open("/dev/mem", O_RDWR | O_SYNC);
 516         }
 517         if (dev_mem_fd < 0) {
 518                 LOG_ERROR("open: %s", strerror(errno));
 519                 return ERROR_JTAG_INIT_FAILED;
 520         }
 521
 522         pio_base = mmap(NULL, sysconf(_SC_PAGE_SIZE), PROT_READ | PROT_WRITE,
 523                                 MAP_SHARED, dev_mem_fd, BCM2835_GPIO_BASE);
 524
 525         if (pio_base == MAP_FAILED) {
 526                 LOG_ERROR("mmap: %s", strerror(errno));
 527                 close(dev_mem_fd);
 528                 return ERROR_JTAG_INIT_FAILED;
 529         }
 530
 531         pads_base = mmap(NULL, sysconf(_SC_PAGE_SIZE), PROT_READ | PROT_WRITE,
 532                                 MAP_SHARED, dev_mem_fd, BCM2835_PADS_GPIO_0_27);
 533
 534         if (pads_base == MAP_FAILED) {
 535                 LOG_ERROR("mmap: %s", strerror(errno));
 536                 bcm2835gpio_munmap();
 537                 close(dev_mem_fd);
 538                 return ERROR_JTAG_INIT_FAILED;
 539         }
 540
 541         close(dev_mem_fd);
 542
 543         /* set 4mA drive strength, slew rate limited, hysteresis on */
 544         pads_base[BCM2835_PADS_GPIO_0_27_OFFSET] = 0x5a000008 + 1;
 545
 546         /*
 547          * Configure TDO as an input, and TDI, TCK, TMS, TRST, SRST
 548          * as outputs.  Drive TDI and TCK low, and TMS/TRST/SRST high.
 549          */
 550         if (transport_is_jtag()) {
 551                 tdo_gpio_mode = MODE_GPIO(tdo_gpio);
 552                 tdi_gpio_mode = MODE_GPIO(tdi_gpio);
 553                 tck_gpio_mode = MODE_GPIO(tck_gpio);
 554                 tms_gpio_mode = MODE_GPIO(tms_gpio);
 555
 556                 INP_GPIO(tdo_gpio);
 557
 558                 GPIO_CLR = 1<<tdi_gpio | 1<<tck_gpio;
 559                 GPIO_SET = 1<<tms_gpio;
 560
 561                 OUT_GPIO(tdi_gpio);
 562                 OUT_GPIO(tck_gpio);
 563                 OUT_GPIO(tms_gpio);
 564
 565                 if (is_gpio_valid(trst_gpio)) {
 566                         trst_gpio_mode = MODE_GPIO(trst_gpio);
 567                         GPIO_SET = 1 << trst_gpio;
 568                         OUT_GPIO(trst_gpio);
 569                 }
 570         }
 571
 572         if (transport_is_swd()) {
 573                 /* Make buffer an output before the GPIO connected to it */
 574                 if (is_gpio_valid(swdio_dir_gpio)) {
 575                         swdio_dir_gpio_mode = MODE_GPIO(swdio_dir_gpio);
 576                         GPIO_SET = 1 << swdio_dir_gpio;
 577                         OUT_GPIO(swdio_dir_gpio);
 578                 }
 579
 580                 swclk_gpio_mode = MODE_GPIO(swclk_gpio);
 581                 swdio_gpio_mode = MODE_GPIO(swdio_gpio);
 582
 583                 GPIO_CLR = 1<<swdio_gpio | 1<<swclk_gpio;
 584
 585                 OUT_GPIO(swclk_gpio);
 586                 OUT_GPIO(swdio_gpio);
 587         }
 588
 589         if (is_gpio_valid(srst_gpio)) {
 590                 srst_gpio_mode = MODE_GPIO(srst_gpio);
 591                 GPIO_SET = 1 << srst_gpio;
 592                 OUT_GPIO(srst_gpio);
 593         }
 594
 595         LOG_DEBUG("saved pinmux settings: tck %d tms %d tdi %d "
 596                   "tdo %d trst %d srst %d", tck_gpio_mode, tms_gpio_mode,
 597                   tdi_gpio_mode, tdo_gpio_mode, trst_gpio_mode, srst_gpio_mode);
 598
 599         return ERROR_OK;
 600 }
 601
 602 static int bcm2835gpio_quit(void)
 603 {
 604         if (transport_is_jtag()) {
 605                 SET_MODE_GPIO(tdo_gpio, tdo_gpio_mode);
 606                 SET_MODE_GPIO(tdi_gpio, tdi_gpio_mode);
 607                 SET_MODE_GPIO(tck_gpio, tck_gpio_mode);
 608                 SET_MODE_GPIO(tms_gpio, tms_gpio_mode);
 609                 if (is_gpio_valid(trst_gpio))
 610                         SET_MODE_GPIO(trst_gpio, trst_gpio_mode);
 611         }
 612
 613         if (transport_is_swd()) {
 614                 SET_MODE_GPIO(swclk_gpio, swclk_gpio_mode);
 615                 SET_MODE_GPIO(swdio_gpio, swdio_gpio_mode);
 616         }
 617
 618         if (is_gpio_valid(srst_gpio))
 619                 SET_MODE_GPIO(srst_gpio, srst_gpio_mode);
 620
 621         if (is_gpio_valid(swdio_dir_gpio))
 622                 SET_MODE_GPIO(swdio_dir_gpio, swdio_dir_gpio_mode);
 623
 624         bcm2835gpio_munmap();
 625
 626         return ERROR_OK;
 627 }