1 files changed, 39 insertions, 0 deletions
diff --git a/src/libs/gpio.c b/src/libs/gpio.c
new file mode 100644
index 0000000..9d61d85
--- /dev/null
+++ b/src/libs/gpio.c
@@ -0,0 +1,39 @@
+#include "addrmap.h"
+#include "utils.h"
+#include "gpio.h"
+
+void gpio_init() { REG_WRITE_BITMAP_CLEAR(RESETS_BASE, 32);} // Reset bank0 (all gpios)
+
+void gpio_set_function(u8 gpio, u8 fn){
+  REG_WRITE(IO_BANK0_GPIO25_CTRL, fn); // Set function as SIO
+}
+
+void gpio_output_enable(u8 gpio){
+  u32 r=REG_READ(SIO_GPIO_OE)|1<<gpio;
+  REG_WRITE(SIO_GPIO_OE, r);
+}
+
+void gpio_toggle_state(u8 gpio){
+  REG_WRITE(SIO_GPIO_OUT_XOR,1<<gpio);
+}
+
+void gpio_toggle_led(){
+  // Ensure led pin is configured
+  u32 fn=REG_READ(IO_BANK0_GPIO25_CTRL);
+  u32 oe=REG_READ(SIO_GPIO_OE)&1<<25;
+  if(fn!=GPIO_FN_SIO || !oe){
+	gpio_set_function(25, GPIO_FN_SIO);
+	gpio_output_enable(25);
+  }
+  gpio_toggle_state(25); // Switch between ON/OFF
+}
+
+
+void gpio_blink_led(int n){
+  for(int i=0;i<n;i++){
+	gpio_toggle_led();
+	for(int j=0;j<5000000;j++){} // These are disgusting but I am lazy..
+	gpio_toggle_led();
+	for(int j=0;j<5000000;j++){}
+  }
+}