Skip to content

Commit 0021ee2

Browse files
vfaziotom-van
vfazio
authored and
tom-van
committed
jtag/drivers/bcm2835gpio: Support all 54 GPIO pins
Previously, only the first 32 GPIO were supported on the BCM2835. Performance was cited as being the primary justification for not supporting all 54 pins, notably: 1. There is overhead for calculating the memory offset for the pin 2. GPIO values cannot be written in bulk if pins span memory offsets Now, all 54 GPIO pins are supported by the driver. Since pins may use different offsets, multiple pins cannot be toggled with one memory store. Multiple stores now need to occur when one sufficed before. To offset some of the performance overhead for the additional stores, memory addresses, masks, and shift bits are calculated once and cached into struct. Calculating these once reduces the number of instructions a function needs to run in order to manipulate a given GPIO. The following functions have been updated to leverage the new struct as they represent some of the hottest paths: bcm2835_swdio_drive bcm2835_swdio_read bcm2835gpio_swd_write_fast bcm2835gpio_read bcm2835gpio_write For `bcm2835gpio_swd_write_fast`, performance should be roughly the same as the number of memory stores hasn't changed. For `bcm2835_write`, there is a slight performance degradation since TMS/TDI/TCK are set separately which incurs an additional memory store. Instruction counts across the above functions are reduced by ~10-40%. Macros to access registers have been reworked into inline functions to support access to all pins and to avoid checkpatch headaches. The `initial_gpio_state.output_level` member has been retyped to bool to better align with the expected values. Support for adjusting pads for the expanded pin range has been left out as support for manipulating these settings should be moved out of this driver and into its own utility. Change-Id: I18853d1a2c86776658630326c71a6bf236fcc6da Signed-off-by: Vincent Fazio <[email protected]> Reviewed-on: https://review.openocd.org/c/openocd/+/7732 Reviewed-by: Tomas Vanek <[email protected]> Tested-by: jenkins
1 parent 4e78563 commit 0021ee2

File tree

2 files changed

+153
-64
lines changed

2 files changed

+153
-64
lines changed

‎doc/openocd.texi‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3472,8 +3472,7 @@ able to coexist nicely with both sysfs bitbanging and various
34723472
peripherals' kernel drivers. The driver restores the previous
34733473
configuration on exit.
34743474

3475-
GPIO numbers >= 32 can't be used for performance reasons. GPIO configuration is
3476-
handled by the generic command @ref{adapter gpio, @command{adapter gpio}}.
3475+
GPIO configuration is handled by the generic command @ref{adapter gpio, @command{adapter gpio}}.
34773476

34783477
See @file{interface/raspberrypi-native.cfg} for a sample config and
34793478
@file{interface/raspberrypi-gpio-connector.cfg} for pinout.

‎src/jtag/drivers/bcm2835gpio.c‎

Lines changed: 152 additions & 62 deletions
Original file line numberDiff line numberDiff line change
@@ -31,28 +31,22 @@ static off_t bcm2835_peri_base = 0x20000000;
3131
#define BCM2835_GPIO_MODE_OUTPUT 1
3232

3333
/* GPIO setup macros */
34-
#define MODE_GPIO(_g) ({ \
35-
typeof(_g) g = (_g); \
36-
*(pio_base + (g / 10)) >> ((g % 10) * 3) & 7; \
37-
})
34+
#define BCM2835_GPIO_REG_READ(offset) \
35+
(*(pio_base + (offset)))
3836

39-
#define INP_GPIO(_g) do { \
40-
typeof(_g) g1 = (_g); \
41-
*(pio_base + (g1 / 10)) &= ~(7 << ((g1 % 10) * 3)); \
42-
} while (0)
37+
#define BCM2835_GPIO_REG_WRITE(offset, value) \
38+
(*(pio_base + (offset)) = (value))
4339

44-
#define SET_MODE_GPIO(_g, m) do { \
45-
typeof(_g) g = (_g); \
46-
/* clear the mode bits first, then set as necessary */ \
47-
INP_GPIO(g); \
48-
*(pio_base + (g / 10)) |= ((m) << ((g % 10) * 3)); \
49-
} while (0)
40+
#define BCM2835_GPIO_SET_REG_BITS(offset, bit_mask) \
41+
(*(pio_base + (offset)) |= (bit_mask))
5042

51-
#define OUT_GPIO(g) SET_MODE_GPIO(g, BCM2835_GPIO_MODE_OUTPUT)
43+
#define BCM2835_GPIO_CLEAR_REG_BITS(offset, bit_mask) \
44+
(*(pio_base + (offset)) &= ~(bit_mask))
5245

53-
#define GPIO_SET (*(pio_base + 7)) /* sets bits which are 1, ignores bits which are 0 */
54-
#define GPIO_CLR (*(pio_base + 10)) /* clears bits which are 1, ignores bits which are 0 */
55-
#define GPIO_LEV (*(pio_base + 13)) /* current level of the pin */
46+
#define BCM2835_GPIO_MODE_ADDR(gpio_pin_num) (pio_base + ((gpio_pin_num) / 10))
47+
#define BCM2835_GPIO_SET_ADDR(gpio_pin_num) (pio_base + 7 + ((gpio_pin_num) / 32))
48+
#define BCM2835_GPIO_CLR_ADDR(gpio_pin_num) (pio_base + 10 + ((gpio_pin_num) / 32))
49+
#define BCM2835_GPIO_LEVEL_ADDR(gpio_pin_num) (pio_base + 13 + ((gpio_pin_num) / 32))
5650

5751
static int dev_mem_fd;
5852
static volatile uint32_t *pio_base = MAP_FAILED;
@@ -66,10 +60,75 @@ static unsigned int jtag_delay;
6660
static const struct adapter_gpio_config *adapter_gpio_config;
6761
static struct initial_gpio_state {
6862
unsigned int mode;
69-
unsigned int output_level;
63+
bool output_level;
7064
} initial_gpio_state[ADAPTER_GPIO_IDX_NUM];
7165
static uint32_t initial_drive_strength_etc;
7266

67+
static struct {
68+
volatile uint32_t *swdio_clr_set_addr[2];
69+
uint32_t swdio_mask;
70+
volatile uint32_t *swdio_read_level_addr;
71+
uint32_t swdio_level_shift_bits;
72+
bool swdio_active_low;
73+
volatile uint32_t *swdio_mode_addr;
74+
uint32_t swdio_mode_input_mask;
75+
uint32_t swdio_mode_output_mask;
76+
volatile uint32_t *swclk_clr_set_addr[2];
77+
uint32_t swclk_mask;
78+
volatile uint32_t *tdi_clr_set_addr[2];
79+
uint32_t tdi_mask;
80+
volatile uint32_t *tms_clr_set_addr[2];
81+
uint32_t tms_mask;
82+
volatile uint32_t *tck_clr_set_addr[2];
83+
uint32_t tck_mask;
84+
volatile uint32_t *tdo_read_level_addr;
85+
uint32_t tdo_level_shift_bits;
86+
bool tdo_active_low;
87+
} gpio_control;
88+
89+
/* GPFSEL[0,5], read the function select bits of specified GPIO pin number */
90+
static inline unsigned int bcm2835_get_mode(unsigned int gpio_pin_num)
91+
{
92+
return (BCM2835_GPIO_REG_READ((gpio_pin_num / 10)) >> ((gpio_pin_num % 10) * 3) & 7);
93+
}
94+
95+
/* GPLEV[13,14], current level of the pin */
96+
static inline bool bcm2835_get_level(unsigned int gpio_pin_num)
97+
{
98+
return ((BCM2835_GPIO_REG_READ((13 + (gpio_pin_num / 32))) >> (gpio_pin_num % 32)) & 1);
99+
}
100+
101+
/* set GPIO pin as input */
102+
static inline void bcm2835_set_input(unsigned int gpio_pin_num)
103+
{
104+
BCM2835_GPIO_CLEAR_REG_BITS((gpio_pin_num / 10), (7 << ((gpio_pin_num % 10) * 3)));
105+
}
106+
107+
/* clear the mode bits first, then set as necessary */
108+
static inline void bcm2835_set_mode(unsigned int gpio_pin_num, unsigned char mode)
109+
{
110+
bcm2835_set_input(gpio_pin_num);
111+
BCM2835_GPIO_SET_REG_BITS((gpio_pin_num / 10), (mode << ((gpio_pin_num % 10) * 3)));
112+
}
113+
114+
/* set GPIO pin as output */
115+
static inline void bcm2835_set_output(unsigned int gpio_pin_num)
116+
{
117+
bcm2835_set_mode(gpio_pin_num, BCM2835_GPIO_MODE_OUTPUT);
118+
}
119+
120+
/* GPSET[7,8], sets bits which are 1, ignores bits which are 0 */
121+
static inline void bcm2835_gpio_set(unsigned int gpio_pin_num)
122+
{
123+
BCM2835_GPIO_REG_WRITE((7 + (gpio_pin_num / 32)), (1 << (gpio_pin_num % 32)));
124+
}
125+
126+
/* GPCLR[10,11], clears bits which are 1, ignores bits which are 0 */
127+
static inline void bcm2835_gpio_clear(unsigned int gpio_pin_num)
128+
{
129+
BCM2835_GPIO_REG_WRITE((10 + (gpio_pin_num / 32)), (1 << (gpio_pin_num % 32)));
130+
}
131+
73132
static inline const char *bcm2835_get_mem_dev(void)
74133
{
75134
if (bcm2835_peri_mem_dev)
@@ -96,7 +155,7 @@ static inline void bcm2835_delay(void)
96155
static bool is_gpio_config_valid(enum adapter_gpio_config_index idx)
97156
{
98157
/* Only chip 0 is supported, accept unset value (-1) too */
99-
return adapter_gpio_config[idx].gpio_num <= 31;
158+
return adapter_gpio_config[idx].gpio_num <= 53;
100159
}
101160

102161
static void set_gpio_value(const struct adapter_gpio_config *gpio_config, int value)
@@ -105,27 +164,27 @@ static void set_gpio_value(const struct adapter_gpio_config *gpio_config, int va
105164
switch (gpio_config->drive) {
106165
case ADAPTER_GPIO_DRIVE_MODE_PUSH_PULL:
107166
if (value)
108-
GPIO_SET = 1 << gpio_config->gpio_num;
167+
bcm2835_gpio_set(gpio_config->gpio_num);
109168
else
110-
GPIO_CLR = 1 << gpio_config->gpio_num;
169+
bcm2835_gpio_clear(gpio_config->gpio_num);
111170
/* For performance reasons assume the GPIO is already set as an output
112171
* and therefore the call can be omitted here.
113172
*/
114173
break;
115174
case ADAPTER_GPIO_DRIVE_MODE_OPEN_DRAIN:
116175
if (value) {
117-
INP_GPIO(gpio_config->gpio_num);
176+
bcm2835_set_input(gpio_config->gpio_num);
118177
} else {
119-
GPIO_CLR = 1 << gpio_config->gpio_num;
120-
OUT_GPIO(gpio_config->gpio_num);
178+
bcm2835_gpio_clear(gpio_config->gpio_num);
179+
bcm2835_set_output(gpio_config->gpio_num);
121180
}
122181
break;
123182
case ADAPTER_GPIO_DRIVE_MODE_OPEN_SOURCE:
124183
if (value) {
125-
GPIO_SET = 1 << gpio_config->gpio_num;
126-
OUT_GPIO(gpio_config->gpio_num);
184+
bcm2835_gpio_set(gpio_config->gpio_num);
185+
bcm2835_set_output(gpio_config->gpio_num);
127186
} else {
128-
INP_GPIO(gpio_config->gpio_num);
187+
bcm2835_set_input(gpio_config->gpio_num);
129188
}
130189
break;
131190
}
@@ -135,12 +194,12 @@ static void set_gpio_value(const struct adapter_gpio_config *gpio_config, int va
135194
static void restore_gpio(enum adapter_gpio_config_index idx)
136195
{
137196
if (is_gpio_config_valid(idx)) {
138-
SET_MODE_GPIO(adapter_gpio_config[idx].gpio_num, initial_gpio_state[idx].mode);
197+
bcm2835_set_mode(adapter_gpio_config[idx].gpio_num, initial_gpio_state[idx].mode);
139198
if (initial_gpio_state[idx].mode == BCM2835_GPIO_MODE_OUTPUT) {
140199
if (initial_gpio_state[idx].output_level)
141-
GPIO_SET = 1 << adapter_gpio_config[idx].gpio_num;
200+
bcm2835_gpio_set(adapter_gpio_config[idx].gpio_num);
142201
else
143-
GPIO_CLR = 1 << adapter_gpio_config[idx].gpio_num;
202+
bcm2835_gpio_clear(adapter_gpio_config[idx].gpio_num);
144203
}
145204
}
146205
bcm2835_gpio_synchronize();
@@ -151,9 +210,8 @@ static void initialize_gpio(enum adapter_gpio_config_index idx)
151210
if (!is_gpio_config_valid(idx))
152211
return;
153212

154-
initial_gpio_state[idx].mode = MODE_GPIO(adapter_gpio_config[idx].gpio_num);
155-
unsigned int shift = adapter_gpio_config[idx].gpio_num;
156-
initial_gpio_state[idx].output_level = (GPIO_LEV >> shift) & 1;
213+
initial_gpio_state[idx].mode = bcm2835_get_mode(adapter_gpio_config[idx].gpio_num);
214+
initial_gpio_state[idx].output_level = bcm2835_get_level(adapter_gpio_config[idx].gpio_num);
157215
LOG_DEBUG("saved GPIO mode for %s (GPIO %d %d): %d",
158216
adapter_gpio_get_name(idx), adapter_gpio_config[idx].chip_num, adapter_gpio_config[idx].gpio_num,
159217
initial_gpio_state[idx].mode);
@@ -171,35 +229,29 @@ static void initialize_gpio(enum adapter_gpio_config_index idx)
171229
set_gpio_value(&adapter_gpio_config[idx], 1);
172230
break;
173231
case ADAPTER_GPIO_INIT_STATE_INPUT:
174-
INP_GPIO(adapter_gpio_config[idx].gpio_num);
232+
bcm2835_set_input(adapter_gpio_config[idx].gpio_num);
175233
break;
176234
}
177235

178236
/* Direction for non push-pull is already set by set_gpio_value() */
179237
if (adapter_gpio_config[idx].drive == ADAPTER_GPIO_DRIVE_MODE_PUSH_PULL
180238
&& adapter_gpio_config[idx].init_state != ADAPTER_GPIO_INIT_STATE_INPUT)
181-
OUT_GPIO(adapter_gpio_config[idx].gpio_num);
239+
bcm2835_set_output(adapter_gpio_config[idx].gpio_num);
182240
bcm2835_gpio_synchronize();
183241
}
184242

185243
static enum bb_value bcm2835gpio_read(void)
186244
{
187-
unsigned int shift = adapter_gpio_config[ADAPTER_GPIO_IDX_TDO].gpio_num;
188-
uint32_t value = (GPIO_LEV >> shift) & 1;
189-
return value ^ (adapter_gpio_config[ADAPTER_GPIO_IDX_TDO].active_low ? BB_HIGH : BB_LOW);
245+
bool value = ((*gpio_control.tdo_read_level_addr >> gpio_control.tdo_level_shift_bits) & 1);
246+
return (value ^ gpio_control.tdo_active_low) ? BB_HIGH : BB_LOW;
190247
}
191248

192249
static int bcm2835gpio_write(int tck, int tms, int tdi)
193250
{
194-
uint32_t set = tck << adapter_gpio_config[ADAPTER_GPIO_IDX_TCK].gpio_num |
195-
tms << adapter_gpio_config[ADAPTER_GPIO_IDX_TMS].gpio_num |
196-
tdi << adapter_gpio_config[ADAPTER_GPIO_IDX_TDI].gpio_num;
197-
uint32_t clear = !tck << adapter_gpio_config[ADAPTER_GPIO_IDX_TCK].gpio_num |
198-
!tms << adapter_gpio_config[ADAPTER_GPIO_IDX_TMS].gpio_num |
199-
!tdi << adapter_gpio_config[ADAPTER_GPIO_IDX_TDI].gpio_num;
251+
*gpio_control.tdi_clr_set_addr[tdi] = gpio_control.tdi_mask;
252+
*gpio_control.tms_clr_set_addr[tms] = gpio_control.tms_mask;
253+
*gpio_control.tck_clr_set_addr[tck] = gpio_control.tck_mask; /* Write clock last */
200254

201-
GPIO_SET = set;
202-
GPIO_CLR = clear;
203255
bcm2835_gpio_synchronize();
204256

205257
bcm2835_delay();
@@ -210,16 +262,9 @@ static int bcm2835gpio_write(int tck, int tms, int tdi)
210262
/* Requires push-pull drive mode for swclk and swdio */
211263
static int bcm2835gpio_swd_write_fast(int swclk, int swdio)
212264
{
213-
swclk = swclk ^ (adapter_gpio_config[ADAPTER_GPIO_IDX_SWCLK].active_low ? 1 : 0);
214-
swdio = swdio ^ (adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].active_low ? 1 : 0);
265+
*gpio_control.swdio_clr_set_addr[swdio] = gpio_control.swdio_mask;
266+
*gpio_control.swclk_clr_set_addr[swclk] = gpio_control.swclk_mask; /* Write clock last */
215267

216-
uint32_t set = swclk << adapter_gpio_config[ADAPTER_GPIO_IDX_SWCLK].gpio_num |
217-
swdio << adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num;
218-
uint32_t clear = !swclk << adapter_gpio_config[ADAPTER_GPIO_IDX_SWCLK].gpio_num |
219-
!swdio << adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num;
220-
221-
GPIO_SET = set;
222-
GPIO_CLR = clear;
223268
bcm2835_gpio_synchronize();
224269

225270
bcm2835_delay();
@@ -266,9 +311,11 @@ static void bcm2835_swdio_drive(bool is_output)
266311
if (is_output) {
267312
if (is_gpio_config_valid(ADAPTER_GPIO_IDX_SWDIO_DIR))
268313
set_gpio_value(&adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO_DIR], 1);
269-
OUT_GPIO(adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num);
314+
/* per bcm2835_set_output, clear mode bits then set the pin to output */
315+
*gpio_control.swdio_mode_addr = (gpio_control.swdio_mode_output_mask |
316+
(*gpio_control.swdio_mode_addr & gpio_control.swdio_mode_input_mask));
270317
} else {
271-
INP_GPIO(adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num);
318+
*gpio_control.swdio_mode_addr &= gpio_control.swdio_mode_input_mask;
272319
if (is_gpio_config_valid(ADAPTER_GPIO_IDX_SWDIO_DIR))
273320
set_gpio_value(&adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO_DIR], 0);
274321
}
@@ -277,9 +324,8 @@ static void bcm2835_swdio_drive(bool is_output)
277324

278325
static int bcm2835_swdio_read(void)
279326
{
280-
unsigned int shift = adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num;
281-
uint32_t value = (GPIO_LEV >> shift) & 1;
282-
return value ^ (adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].active_low ? 1 : 0);
327+
bool value = ((*gpio_control.swdio_read_level_addr >> gpio_control.swdio_level_shift_bits) & 1);
328+
return (int)(value ^ gpio_control.swdio_active_low);
283329
}
284330

285331
static int bcm2835gpio_khz(int khz, int *jtag_speed)
@@ -515,6 +561,26 @@ LOG_INFO("pads conf set to %08x", pads_base[BCM2835_PADS_GPIO_0_27_OFFSET]);
515561
initialize_gpio(ADAPTER_GPIO_IDX_TMS);
516562
initialize_gpio(ADAPTER_GPIO_IDX_TCK);
517563
initialize_gpio(ADAPTER_GPIO_IDX_TRST);
564+
565+
/* flip the addresses used when pins are active low */
566+
unsigned char idx = adapter_gpio_config[ADAPTER_GPIO_IDX_TMS].active_low ? 1 : 0;
567+
gpio_control.tms_clr_set_addr[idx] = BCM2835_GPIO_CLR_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_TMS].gpio_num);
568+
gpio_control.tms_clr_set_addr[!idx] = BCM2835_GPIO_SET_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_TMS].gpio_num);
569+
gpio_control.tms_mask = (1 << (adapter_gpio_config[ADAPTER_GPIO_IDX_TMS].gpio_num % 32));
570+
571+
idx = adapter_gpio_config[ADAPTER_GPIO_IDX_TDI].active_low ? 1 : 0;
572+
gpio_control.tdi_clr_set_addr[idx] = BCM2835_GPIO_CLR_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_TDI].gpio_num);
573+
gpio_control.tdi_clr_set_addr[!idx] = BCM2835_GPIO_SET_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_TDI].gpio_num);
574+
gpio_control.tdi_mask = (1 << (adapter_gpio_config[ADAPTER_GPIO_IDX_TDI].gpio_num % 32));
575+
576+
idx = adapter_gpio_config[ADAPTER_GPIO_IDX_TCK].active_low ? 1 : 0;
577+
gpio_control.tck_clr_set_addr[idx] = BCM2835_GPIO_CLR_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_TCK].gpio_num);
578+
gpio_control.tck_clr_set_addr[!idx] = BCM2835_GPIO_SET_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_TCK].gpio_num);
579+
gpio_control.tck_mask = (1 << (adapter_gpio_config[ADAPTER_GPIO_IDX_TCK].gpio_num % 32));
580+
581+
gpio_control.tdo_read_level_addr = BCM2835_GPIO_LEVEL_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_TDO].gpio_num);
582+
gpio_control.tdo_level_shift_bits = (adapter_gpio_config[ADAPTER_GPIO_IDX_TDO].gpio_num % 32);
583+
gpio_control.tdo_active_low = adapter_gpio_config[ADAPTER_GPIO_IDX_TDO].active_low;
518584
}
519585

520586
const struct bitbang_interface *bcm2835gpio_bitbang = &bcm2835gpio_bitbang_swd_write_generic;
@@ -538,6 +604,30 @@ LOG_INFO("pads conf set to %08x", pads_base[BCM2835_PADS_GPIO_0_27_OFFSET]);
538604

539605
if (adapter_gpio_config[ADAPTER_GPIO_IDX_SWCLK].drive == ADAPTER_GPIO_DRIVE_MODE_PUSH_PULL &&
540606
adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].drive == ADAPTER_GPIO_DRIVE_MODE_PUSH_PULL) {
607+
/* flip the addresses used when pins are active low */
608+
unsigned char idx = adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].active_low ? 1 : 0;
609+
gpio_control.swdio_clr_set_addr[idx] =
610+
BCM2835_GPIO_CLR_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num);
611+
gpio_control.swdio_clr_set_addr[!idx] =
612+
BCM2835_GPIO_SET_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num);
613+
gpio_control.swdio_mask = (1 << (adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num % 32));
614+
gpio_control.swdio_read_level_addr =
615+
BCM2835_GPIO_LEVEL_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num);
616+
gpio_control.swdio_level_shift_bits = (adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num % 32);
617+
gpio_control.swdio_active_low = adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].active_low;
618+
gpio_control.swdio_mode_addr = BCM2835_GPIO_MODE_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num);
619+
gpio_control.swdio_mode_input_mask =
620+
~(7 << ((adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num % 10) * 3));
621+
gpio_control.swdio_mode_output_mask =
622+
(1 << ((adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num % 10) * 3));
623+
624+
idx = adapter_gpio_config[ADAPTER_GPIO_IDX_SWCLK].active_low ? 1 : 0;
625+
gpio_control.swclk_clr_set_addr[idx] =
626+
BCM2835_GPIO_CLR_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_SWCLK].gpio_num);
627+
gpio_control.swclk_clr_set_addr[!idx] =
628+
BCM2835_GPIO_SET_ADDR(adapter_gpio_config[ADAPTER_GPIO_IDX_SWCLK].gpio_num);
629+
gpio_control.swclk_mask = (1 << (adapter_gpio_config[ADAPTER_GPIO_IDX_SWCLK].gpio_num % 32));
630+
541631
LOG_DEBUG("BCM2835 GPIO using fast mode for SWD write");
542632
bcm2835gpio_bitbang = &bcm2835gpio_bitbang_swd_write_fast;
543633
} else {
@@ -570,7 +660,7 @@ static int bcm2835gpio_quit(void)
570660
* current and final states of swdio and swdio_dir do not have to be
571661
* considered to calculate the safe restoration order.
572662
*/
573-
INP_GPIO(adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num);
663+
bcm2835_set_input(adapter_gpio_config[ADAPTER_GPIO_IDX_SWDIO].gpio_num);
574664
restore_gpio(ADAPTER_GPIO_IDX_SWDIO_DIR);
575665
restore_gpio(ADAPTER_GPIO_IDX_SWDIO);
576666
restore_gpio(ADAPTER_GPIO_IDX_SWCLK);

0 commit comments

Comments
 (0)