Update main.c

Add Code from SnowLeopard71

https://github.com/SnowLeopard71/rpi-fbcp/commit/35b1463251307a37c84f326c389826e9751d3959