Code from tinito in forum thread: http://forum.chibios.org/phpbb/viewtopic.php?f=14&t=685 Updated to chibios trunk.