Fixed context swich rutines, test4 on AVR passed