diff --git a/gcode_parse.c b/gcode_parse.c
index 4222bc9..e9fc162 100644
--- a/gcode_parse.c
+++ b/gcode_parse.c
@@ -183,6 +183,11 @@ void gcode_parse_char(uint8_t c) {
 					if (debug_flags & DEBUG_ECHO)
 						serwrite_uint16(next_target.P);
 					break;
+				case 'T':
+					next_target.T = read_digit.mantissa;
+					if (debug_flags & DEBUG_ECHO)
+						serwrite_uint8(next_target.T);
+					break;
 				case 'N':
 					next_target.N = decfloat_to_int(&read_digit, 1, 1);
 					if (debug_flags & DEBUG_ECHO)
@@ -244,6 +249,9 @@ void gcode_parse_char(uint8_t c) {
 			case 'P':
 				next_target.seen_P = 1;
 				break;
+			case 'T':
+				next_target.seen_T = 1;
+				break;
 			case 'N':
 				next_target.seen_N = 1;
 				break;
@@ -346,8 +354,8 @@ void gcode_parse_char(uint8_t c) {
 		// reset variables
 		next_target.seen_X = next_target.seen_Y = next_target.seen_Z = \
 			next_target.seen_E = next_target.seen_F = next_target.seen_S = \
-			next_target.seen_P = next_target.seen_N = next_target.seen_M = \
-			next_target.seen_checksum = next_target.seen_semi_comment = \
+			next_target.seen_P = next_target.seen_T = next_target.seen_N = \
+			next_target.seen_M = next_target.seen_checksum = next_target.seen_semi_comment = \
 			next_target.seen_parens_comment = next_target.checksum_read = \
 			next_target.checksum_calculated = 0;
 		last_field = 0;
diff --git a/gcode_parse.h b/gcode_parse.h
index 31dafb9..18e38ce 100644
--- a/gcode_parse.h
+++ b/gcode_parse.h
@@ -25,23 +25,28 @@ typedef struct {
 
 // this holds all the possible data from a received command
 typedef struct {
-	uint8_t					seen_G	:1;
-	uint8_t					seen_M	:1;
-	uint8_t					seen_X	:1;
-	uint8_t					seen_Y	:1;
-	uint8_t					seen_Z	:1;
-	uint8_t					seen_E	:1;
-	uint8_t					seen_F	:1;
-	uint8_t					seen_S	:1;
+	union {
+		struct {
+			uint8_t					seen_G	:1;
+			uint8_t					seen_M	:1;
+			uint8_t					seen_X	:1;
+			uint8_t					seen_Y	:1;
+			uint8_t					seen_Z	:1;
+			uint8_t					seen_E	:1;
+			uint8_t					seen_F	:1;
+			uint8_t					seen_S	:1;
 
-	uint8_t					seen_P	:1;
-	uint8_t					seen_N	:1;
-	uint8_t					seen_checksum				:1;
-	uint8_t					seen_semi_comment		:1;
-	uint8_t					seen_parens_comment	:1;
-
-	uint8_t					option_relative			:1;
-	uint8_t					option_inches				:1;
+			uint8_t					seen_P	:1;
+			uint8_t					seen_T	:1;
+			uint8_t					seen_N	:1;
+			uint8_t					seen_checksum				:1;
+			uint8_t					seen_semi_comment		:1;
+			uint8_t					seen_parens_comment	:1;
+			uint8_t					option_relative			:1;
+			uint8_t					option_inches				:1;
+		};
+		uint16_t				flags;
+	};
 
 	uint8_t						G;
 	uint8_t						M;
@@ -50,6 +55,8 @@ typedef struct {
 	int16_t						S;
 	uint16_t					P;
 
+	uint8_t						T;
+
 	uint32_t					N;
 	uint32_t					N_expected;
 
diff --git a/gcode_process.c b/gcode_process.c
index 72451f4..e16ea55 100644
--- a/gcode_process.c
+++ b/gcode_process.c
@@ -75,7 +75,11 @@ void process_gcode_command() {
 	// easier way to do this
 	// 	startpoint.E = 0;
 	// moved to dda.c, end of dda_create() and dda_queue.c, next_move()
-	
+
+	if (next_target.seen_T) {
+		next_tool = next_target.T;
+	}
+
 	if (next_target.seen_G) {
 		uint8_t axisSelected = 0;
 		switch (next_target.G) {
@@ -208,6 +212,11 @@ void process_gcode_command() {
 				for (;;)
 					wd_reset();
 				break;
+
+			// M6- tool change
+			case 6:
+				tool = next_tool;
+				break;
 			// M3/M101- extruder on
 			case 3:
 			case 101:
@@ -412,6 +421,6 @@ void process_gcode_command() {
 			default:
 				sersendf_P(PSTR("E: Bad M-code %d"), next_target.M);
 				// newline is sent from gcode_parse after we return
-		}
-	}
-}
+		} // switch (next_target.M)
+	} // else if (next_target.seen_M)
+} // process_gcode_command()
diff --git a/gcode_process.h b/gcode_process.h
index e78057d..7c453fc 100644
--- a/gcode_process.h
+++ b/gcode_process.h
@@ -3,6 +3,10 @@
 
 #include	"gcode_parse.h"
 
+// the current tool
+extern uint8_t tool;
+// the tool to be changed when we get an M6
+extern uint8_t next_tool;
 
 void zero_x(void);
 void zero_y(void);