Check: https://github.com/prusa3d/Prusa-Firmware/pull/93 and https://github.com/madhunm/Prusa-Firmware/tree/MK2/Firmware