In the file Marlin_main.cpp on line 7131 there is a switch case:

(To turn on line numbers go to File>Preferences and click Display line numbers.)

case 'M': switch (codenum) {
  #if ENABLED(ULTIPANEL)
    case 0: // M0 - Unconditional stop - Wait for user button press on LCD
    case 1: // M1 - Conditional stop - Wait for user button press on LCD
      gcode_M0_M1();
      break;
  #endif // ULTIPANEL

  case 17:
    gcode_M17();
    break;
  etc.....

Adding another case with an unused number such as 5 and then the code you want followed by a break should do the trick. Ex:

case 5:
   doABunchofCoolStuff();
   myservo.write(thebestposition);
   break;

-AC