#include <stdio.h>
#include <bcm2835.h>

int assign[15] = {17,18,27,22,23,24,25,4,2,3,10,9,11,8,7};

void GPIO_out(int data, int mode){
	int i;
	mode = mode & 1;	// mode=0 : active high / mode=1 : active low
	data = data & 255;
	for (i=0; i<8; i++) {
		bcm2835_gpio_write( assign[i], ( ( (data >>i) & 1) ^ mode ) );
	}
}

void Select_out(int data){
	int i;
	data = data & 7;
	for (i=0; i<3; i++) {
		bcm2835_gpio_write( assign[i+10], ( (data >>i) & 1)  );
	}
	bcm2835_gpio_write( assign[13], 0 );
	bcm2835_gpio_write( assign[13], 1 );
}

int main(void)
{
	int i, j, k, l;
	if (!bcm2835_init()){
		printf("GPIO is not found.\n");
		return 1;
	}
	for (i=0; i<15; i++){
		bcm2835_gpio_fsel(assign[i], BCM2835_GPIO_FSEL_OUTP);
	}

	for (l=0; l<256; l++) {
		GPIO_out(l, 0);
		Select_out(3);
		Select_out(7);
		for (k=0; k<256; k++) {
			GPIO_out(k, 0);
			Select_out(2);
			Select_out(6);
			for (j=0; j<256; j++) {
				GPIO_out(j, 0);
				Select_out(1);
				Select_out(5);
				for (i=0; i<256; i++) {
					GPIO_out(i, 0);
					Select_out(0);
					Select_out(4);
				}
			}
		}
	}

	bcm2835_close();
	return 0;
}