Port of relevant nophead changes from https://github.com/nophead/Marlin