M2/M3/M5 Support

Added M2/M3 and M5 Support
Also formated code with AStyle