//To set high GPIO4 pin (GPFSEL0 and GPSET0 assumed to be adresses)
            #define GPIO4_GPFSEL 12
            #define GPIO4 4
            *GPFSEL0 |= (1 << GPIO4_GPFSEL);
            *GPSET0 |= (1 << 4);

gpio_set_mode(GPIO4, GPIO_OUTPUT);
gpio_set_high(GPIO4);

void gpio_set_mode(uint32_t gpio, gpio_mode mode)
{
   uint32_t gpfsel_addr = GPIO_BASE;
   uint32_t gpfsel_num = (uint32_t)(gpio / 10);
   uint32_t gpio_bit = ((((uint32_t)(gpio % 10)) * 3));

   for (int i = 0; i < 6; i++)
   {
       if (i == gpfsel_num)
           break;
       else
           gpfsel_addr += 0x04;
   }

   volatile uint32_t* GPFSEL = (volatile uint32_t*) gpfsel_addr;
   switch (mode)
   {
       case GPIO_OUTPUT:
           *GPFSEL |= (1 << gpio_bit);
       case GPIO_INPUT:
       {
       *GPFSEL &= ~(1 << gpio_bit);
       *GPFSEL &= ~(1 << (gpio_bit+1));
       *GPFSEL &= ~(1 << (gpio_bit+2));
       }
   }
}

void gpio_set_high(uint32_t gpio)
{
    volatile uint32_t* GPSET = ((gpio < 32) ? ( (volatile uint32_t*)(GPSET0)) : ( (volatile uint32_t*)(GPSET1) ));
    uint32_t gpio_bit = (gpio < 32) ? gpio : (gpio - 32);
    *GPSET |= (1 << gpio_bit);
}


void gpio_set_low(uint32_t gpio)
{
   volatile uint32_t* GPCLR = ((gpio < 32) ? ( (volatile uint32_t*)(GPCLR0) ) : ( (volatile uint32_t*)(GPCLR1)));
   uint32_t gpio_bit = ((gpio < 32) ? gpio : (gpio - 32));
   *GPCLR |= (1 << gpio_bit);
}

uint32_t gpio_status(uint32_t gpio)
{
   volatile  uint32_t* GPLEV = ((gpio < 32) ? ((volatile uint32_t*)(GPLEV0)) : ((volatile uint32_t*)(GPLEV1)));
   uint32_t  gpio_bit = ((gpio < 32) ? gpio : (gpio - 32));
   return (*GPLEV & (1 << gpio_bit));
}

-Wall -O2 -nostdlib -nostartfiles -ffreestanding