Added changes from beause/chibios/genistCustom branch