Modified to work with ChibiOS trunk