http://mbed.org/users/donatien/code/DebugLib/#bd1844de60f1