Main program, template from: http://elinux.org/RPi_Low-level_peripherals#C_2