1 files changed, 9 insertions, 9 deletions

diff --git a/controller/fw/src/mspdebug_wrapper.c b/controller/fw/src/mspdebug_wrapper.c
index 3a6d526..eda91cc 100644
--- a/controller/fw/src/mspdebug_wrapper.c
+++ b/controller/fw/src/mspdebug_wrapper.c
@@ -2,14 +2,14 @@
 #include <unistd.h>
 #include <errno.h>
 
-#include <libopencm3/stm32/gpio.h>
-
 #include "output.h"
 #include "jtaglib.h"
 
 #include "sr_global.h"
 #include "mspdebug_wrapper.h"
 
+#include <stm32f407xx.h>
+
 #define BLOCK_SIZE 512 /* bytes */
 
 
@@ -101,7 +101,7 @@ enum sr_gpio_types {
 };
 
 struct {
-	uint32_t port;
+    GPIO_TypeDef *gpio;
 	uint16_t num;
 } gpios[SR_NUM_GPIOS] = {
 	[SR_GPIO_TCK] = {GPIOD, 8},
@@ -114,9 +114,9 @@ struct {
 
 static void sr_gpio_write(int num, int out) {
 	if (out)
-		gpio_set(gpios[num].port, gpios[num].num);
+        gpios[num].gpio->BSRR = 1<<gpios[num].num;
 	else
-		gpio_clear(gpios[num].port, gpios[num].num);
+        gpios[num].gpio->BSRR = 1<<gpios[num].num<<16;
 }
 
 static void sr_jtdev_tck(struct jtdev *p, int out) {
@@ -146,7 +146,7 @@ static void sr_jtdev_tst(struct jtdev *p, int out) {
 
 static int sr_jtdev_tdo_get(struct jtdev *p) {
     UNUSED(p);
-	return gpio_get(gpios[SR_GPIO_TST].port, gpios[SR_GPIO_TST].num);
+	return !!(gpios[SR_GPIO_TST].gpio->IDR & (1<<gpios[SR_GPIO_TST].num));
 }
 
 static void sr_jtdev_tclk(struct jtdev *p, int out) {
@@ -156,14 +156,14 @@ static void sr_jtdev_tclk(struct jtdev *p, int out) {
 
 static int sr_jtdev_tclk_get(struct jtdev *p) {
     UNUSED(p);
-	return gpio_get(gpios[SR_GPIO_TDI].port, gpios[SR_GPIO_TDI].num);
+	return !!(gpios[SR_GPIO_TDI].gpio->IDR & (1<<gpios[SR_GPIO_TDI].num));
 }
 
 static void sr_jtdev_tclk_strobe(struct jtdev *p, unsigned int count) {
     UNUSED(p);
 	while (count--) {
-		gpio_set(gpios[SR_GPIO_TDI].port, gpios[SR_GPIO_TDI].num);
-		gpio_clear(gpios[SR_GPIO_TDI].port, gpios[SR_GPIO_TDI].num);
+        gpios[SR_GPIO_TDI].gpio->BSRR = 1<<gpios[SR_GPIO_TDI].num;
+        gpios[SR_GPIO_TDI].gpio->BSRR = 1<<gpios[SR_GPIO_TDI].num<<16;
 	}
 }