tp: fix for G95 pausing bug from Rick

Conflicts:
	src/emc/tp/tp.c

"forward-ported" from g.l.o