#ifdef STM
#define trace(op, ...) {};
#else
-#define trace(op, ...) printf("pc: %d, sp: %d, op: " op "\n", pc, sp, ##__VA_ARGS__);
+#define trace(op, ...) {};
+//#define trace(op, ...) printf("pc: %d, sp: %d, op: " op "\n", pc, sp, ##__VA_ARGS__);
#endif
+#define f16(p) program[pc]*265+program[pc+1]
+
void run_task(struct task *t)
{
uint8_t *program = t->bc;
int sp = 0;
char stack[STACKSIZE] = {0};
debug("Running task with length: %d", plen);
- while(pc != plen){
- debug("program: %d", program[pc]);
- debug("stack: %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
- stack[0], stack[1], stack[2], stack[3], stack[4],
- stack[5], stack[6], stack[7], stack[8], stack[9]);
+ while(pc < plen){
+// debug("program: %d", program[pc]);
+// debug("stack: %02x %02x %02x %02x %02x %02x %02x %02x %02x %02x",
+// stack[0], stack[1], stack[2], stack[3], stack[4],
+// stack[5], stack[6], stack[7], stack[8], stack[9]);
switch(program[pc++]){
case BCNOP: trace("nop");
pc++;
break;
case BCPUSH: trace("push %d", program[pc]*265+program[pc+1]);
- stack[sp++] = program[pc]*265 + program[pc+1];
+ stack[sp++] = f16(pc);
pc+=2;
break;
case BCPOP: trace("pop");
sp--;
break;
- case BCSDSSTORE: trace("sds store: %d", program[pc]);
- sds_store(program[pc++], stack[--sp]);
+ case BCSDSSTORE: trace("sds store: %d", f16(pc));
+ sds_store(f16(pc), stack[--sp]);
+ pc+=2;
break;
- case BCSDSFETCH: trace("sds fetch: %d", program[pc]);
- stack[sp++] = sds_fetch(program[pc++]);
+ case BCSDSFETCH: trace("sds fetch: %d", f16(pc));
+ stack[sp++] = sds_fetch(f16(pc));
+ pc+=2;
break;
- case BCSDSPUBLISH: trace("sds publish %d", program[pc]);
- sds_publish(program[pc++]);
+ case BCSDSPUBLISH: trace("sds publish %d", f16(pc));
+ sds_publish(f16(pc));
+ pc+=2;
break;
case BCNOT: trace("not");
stack[sp] = stack[sp] > 0 ? 0 : 1;
case BCJMPT: trace("jmpt to %d", program[pc]);
pc = stack[--sp] ? program[pc]-1 : pc+1;
break;
- case BCJMPF: trace("jmpf to %d", program[pc]);
+ case BCJMPF: trace("jmpf(%d) to %d", stack[sp-1], program[pc]);
pc = stack[--sp] ? pc+1 : program[pc]-1;
break;
case BCSERIALAVAIL: trace("SerialAvailable()");