Да там вроде всего в двух местах, где во втором такте SP изменяется, надо if(ready) поставить.