Keyboard working on Freescale KL25Z using C