#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++){}
  }
}