1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/gpiotest1/test.c	Tue Mar 01 07:38:36 2016 +0000
     1.3 @@ -0,0 +1,115 @@
     1.4 +#include <stdio.h>
     1.5 +#include <signal.h>
     1.6 +#include <unistd.h>
     1.7 +#include <fcntl.h>
     1.8 +#include <sys/mman.h>
     1.9 +#include "bcm_host.h"
    1.10 +
    1.11 +#define REG_GPFSEL_BASE	0x200000
    1.12 +#define REG_GPSET_BASE	0x20001c
    1.13 +#define REG_GPCLR_BASE	0x200028
    1.14 +#define REG_GPLEV_BASE	0x200034
    1.15 +
    1.16 +#define FSEL_BITS	3
    1.17 +#define FSEL_INPUT	0
    1.18 +#define FSEL_OUTPUT	1
    1.19 +
    1.20 +#define FSEL_SHIFT(i)	(((i) % 10) * 3)
    1.21 +#define FSEL_MASK(i)	(3 << FSEL_SHIFT(i))
    1.22 +
    1.23 +#define REG(r)	(volatile uint32_t*)((unsigned char*)iomem + (r))
    1.24 +
    1.25 +void *iomem;
    1.26 +int quit;
    1.27 +
    1.28 +static inline void set_pin_func(int pin, int func);
    1.29 +static inline void set_pin_state(int pin, int s);
    1.30 +static inline int get_pin_state(int pin);
    1.31 +static void sighandler(int s);
    1.32 +
    1.33 +int main(void)
    1.34 +{
    1.35 +	int fd;
    1.36 +	int i;
    1.37 +	uint32_t io_addr, io_size;
    1.38 +
    1.39 +	signal(SIGINT, sighandler);
    1.40 +	signal(SIGQUIT, sighandler);
    1.41 +	signal(SIGSEGV, sighandler);
    1.42 +	signal(SIGILL, sighandler);
    1.43 +	signal(SIGFPE, sighandler);
    1.44 +
    1.45 +	io_addr = bcm_host_get_peripheral_address();
    1.46 +	io_size = bcm_host_get_peripheral_size();
    1.47 +	printf("I/O registers\n");
    1.48 +	printf("  addr: %x\n", io_addr);
    1.49 +	printf("  size: %d\n", io_size);
    1.50 +
    1.51 +	if((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1) {
    1.52 +		perror("failed to open /dev/mem");
    1.53 +		return 1;
    1.54 +	}
    1.55 +	if((iomem = mmap(0, io_size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, io_addr)) == (void*)-1) {
    1.56 +		perror("failed to map I/O registers range");
    1.57 +		return 1;
    1.58 +	}
    1.59 +
    1.60 +	/* configure GPIO pins 4 and 17 as outputs */
    1.61 +	set_pin_func(4, FSEL_OUTPUT);
    1.62 +	set_pin_func(17, FSEL_OUTPUT);
    1.63 +
    1.64 +	i = 0;
    1.65 +	while(!quit) {
    1.66 +		set_pin_state(4, i & 1);
    1.67 +		set_pin_state(17, (~i) & 1);
    1.68 +		++i;
    1.69 +		sleep(1);
    1.70 +	}
    1.71 +
    1.72 +	set_pin_state(4, 0);
    1.73 +	set_pin_state(17, 0);
    1.74 +	set_pin_func(4, FSEL_INPUT);
    1.75 +	set_pin_func(17, FSEL_INPUT);
    1.76 +
    1.77 +	munmap(iomem, io_size);
    1.78 +	close(fd);
    1.79 +	return 0;
    1.80 +}
    1.81 +
    1.82 +static inline void set_pin_func(int pin, int func)
    1.83 +{
    1.84 +	int reg_addr, val;
    1.85 +	reg_addr = REG_GPFSEL_BASE + (pin / 10) * 4;
    1.86 +	val = *REG(reg_addr);
    1.87 +	*REG(reg_addr) = (val & FSEL_MASK(pin)) | (func << FSEL_SHIFT(pin));
    1.88 +}
    1.89 +
    1.90 +static inline void set_pin_state(int pin, int s)
    1.91 +{
    1.92 +	int reg_addr;
    1.93 +	int regidx = pin / 32;
    1.94 +	int bit = pin % 32;
    1.95 +
    1.96 +	if(s) {
    1.97 +		reg_addr = REG_GPSET_BASE + regidx * 4;
    1.98 +		*REG(reg_addr) |= 1 << bit;
    1.99 +	} else {
   1.100 +		reg_addr = REG_GPCLR_BASE + regidx * 4;
   1.101 +		*REG(reg_addr) |= 1 << bit;
   1.102 +	}
   1.103 +}
   1.104 +
   1.105 +static inline int get_pin_state(int pin)
   1.106 +{
   1.107 +	int reg_addr;
   1.108 +	int regidx = pin / 32;
   1.109 +	int bit = pin % 32;
   1.110 +
   1.111 +	reg_addr = REG_GPLEV_BASE + regidx * 4;
   1.112 +	return (*REG(reg_addr) >> bit) & 1;
   1.113 +}
   1.114 +
   1.115 +static void sighandler(int s)
   1.116 +{
   1.117 +	quit = 1;
   1.118 +}