Clean up C code, almost all functions of GPIO moved to C