// This file contains the implementation of functions for emulating a 32-bit or
// 64-bit core, and is included by the logic in srve.h and srve_core.h with the
// appropriate defines set up.

#define INSTR_DESTREG(i) (((i) >> 7)&31)
#define INSTR_SRCREG1(i) (((i) >> 15)&31)
#define INSTR_SRCREG2(i) (((i) >> 20)&31)
#define INSTR_FUNCT3(i) (((i) >> 12)&7)
#define INSTR_FUNCT7(i) (((i) >> 25)&127)

#define INSTR_BTYPE_IMM(i) ((int32_t)(\
  ((((i)>>25)&63)<<5) \
  | ((((i)>>8)&15)<<1) \
  | ((((i)>>7)&1)<<11)))
#define INSTR_JTYPE_IMM(i) ((int32_t)(\
  ((((i)>>31)&1)?0xFFF00000:0) \
  | ((((i)>>21)&1023)<<1) \
  | ((((i)>>20)&1)<<11) \
  | ((((i)>>12)&255)<<12)))
#define INSTR_ITYPE_SIMM(i) ((int32_t)(((i)>>20)|((((i)>>31)&1)?(0xFFFFF<<12):0)))
#define INSTR_STYPE_SIMM(i) ((int32_t)(((INSTR_FUNCT7(i)|((((i)>>31)&1)?(0xFFFFFFF<<7):0))<<5)|(((i)>>7)&31)))
#define INSTR_ITYPE_UIMM(i) ((i)>>20)

int SRVE_CORE_INSTR_BAD(SRVE_CORE_T* core, uint32_t instr) {
  fprintf(stderr, "Bad instruction 0x%x major opcode %d (0x%x, 0b", instr, instr&127, instr&127);
  for (int i = 6; i >= 0; i--) {
    fprintf(stderr, (instr&(1<<i)) ? "1" : "0");
  }
  fprintf(stderr, ")\n");
  if ((instr & 3) != 3) {
    fprintf(stderr, "This appears to be a compressed instruction, these are not supported in this version of the debugger!\n");
  }
  return -1;
}

int SRVE_CORE_INSTR_REGALU(SRVE_CORE_T* core, uint32_t instr) {
  SRVE_SREG_T lhs = core->registers[INSTR_SRCREG1(instr)];
  SRVE_SREG_T rhs = core->registers[INSTR_SRCREG2(instr)];
  int op = INSTR_FUNCT3(instr);
  SRVE_SREG_T result;
  if (SRVE_CORE_DOALU(core, &result, lhs, op, rhs, INSTR_FUNCT7(instr)) != 0) {
    return -1;
  }
  core->registers[INSTR_DESTREG(instr)] = result;
  return 0;
}

// For 64-bit, an additional set of 32-bit ALU operations is added. For
// simplicity this implementation just invokes the 32-bit ALU, as long
// as that shortcut works...
#ifdef SRVE_CORE_64BIT
int srve_core64_instr_regalu32(SRVE_CORE_T* core, uint32_t instr) {
  int32_t lhs = (int32_t) core->registers[INSTR_SRCREG1(instr)];
  int32_t rhs = (int32_t) core->registers[INSTR_SRCREG2(instr)];
  int op = INSTR_FUNCT3(instr);
  int32_t result;
  if (srve_core32_doalu(core, &result, lhs, op, rhs, INSTR_FUNCT7(instr)) != 0) {
    return -1;
  }
  core->registers[INSTR_DESTREG(instr)] = (SRVE_SREG_T) result;
  return 0;
}
int srve_core64_instr_immalu32(SRVE_CORE_T* core, uint32_t instr) {
  int32_t lhs = (int32_t) core->registers[INSTR_SRCREG1(instr)];
  int32_t rhs = (int32_t) INSTR_ITYPE_SIMM(instr);
  int op = INSTR_FUNCT3(instr);
  int32_t result;
  if (srve_core32_doalu(core, &result, lhs, op, rhs, INSTR_FUNCT7(instr)) != 0) {
    return -1;
  }
  core->registers[INSTR_DESTREG(instr)] = (SRVE_SREG_T) result;
  return 0;
}
#endif

int SRVE_CORE_INSTR_IMMALU(SRVE_CORE_T* core, uint32_t instr) {
  SRVE_SREG_T lhs = core->registers[INSTR_SRCREG1(instr)];
  SRVE_SREG_T rhs = (SRVE_SREG_T) INSTR_ITYPE_SIMM(instr);
  int op = INSTR_FUNCT3(instr);
  SRVE_SREG_T result;
  if (SRVE_CORE_DOALU(core, &result, lhs, op, rhs, 0) != 0) {
    return -1;
  }
  core->registers[INSTR_DESTREG(instr)] = result;
  return 0;
}

int SRVE_CORE_INSTR_LOAD(SRVE_CORE_T* core, uint32_t instr) {
  int f = INSTR_FUNCT3(instr);
  SRVE_UREG_T addr = core->registers[INSTR_SRCREG1(instr)] + (SRVE_SREG_T) INSTR_ITYPE_SIMM(instr);
  switch (f) {

#ifdef SRVE_CORE_64BIT
  case 0x2: {
    // 32-bit load, signed
    int32_t value;
    if (SRVE_CORE_LOAD(core, addr, &value, 4) != 0) {
      return -1;
    }
    core->registers[INSTR_DESTREG(instr)] = (SRVE_SREG_T) value;
    return 0;
  }
  case 0x3: {
    // 64-bit load
    int i = SRVE_CORE_LOAD(core, addr, &core->registers[INSTR_DESTREG(instr)], 8);
    //fprintf(stderr, "Loaded 0x%llx from 0x%llx\n", core->registers[INSTR_DESTREG(instr)], addr);
    return i;
  }
  case 0x6: {
    // 32-bit load, unsigned
    uint32_t value;
    if (SRVE_CORE_LOAD(core, addr, &value, 4) != 0) {
      return -1;
    }
    core->registers[INSTR_DESTREG(instr)] = (SRVE_UREG_T) value;
    return 0;
  }
#endif

  case 0x0: {
    // 8-bit signed load
    int8_t value;
    if (SRVE_CORE_LOAD(core, addr, &value, 1) != 0) {
      return -1;
    }
    core->registers[INSTR_DESTREG(instr)] = (SRVE_SREG_T) value;
    return 0;
  }

  case 0x1: {
    // 16-bit signed load
    int16_t value;
    if (SRVE_CORE_LOAD(core, addr, &value, 2) != 0) {
      return -1;
    }
    core->registers[INSTR_DESTREG(instr)] = (SRVE_SREG_T) value;
    return 0;
  }

  case 0x4: {
    // 8-bit unsigned load
    uint8_t value;
    if (SRVE_CORE_LOAD(core, addr, &value, 1) != 0) {
      return -1;
    }
    core->registers[INSTR_DESTREG(instr)] = (SRVE_UREG_T) value;
    return 0;
  }

  case 0x5: {
    // 16-bit unsigned load
    uint16_t value;
    if (SRVE_CORE_LOAD(core, addr, &value, 2) != 0) {
      return -1;
    }
    core->registers[INSTR_DESTREG(instr)] = (SRVE_UREG_T) value;
    return 0;
  }
  default:
    fprintf(stderr, "Bad funct3 in load: 0x%x\n", f);
    return -1;
  }
}

int SRVE_CORE_INSTR_STORE(SRVE_CORE_T* core, uint32_t instr) {
  int f = INSTR_FUNCT3(instr);
  SRVE_UREG_T addr = core->registers[INSTR_SRCREG1(instr)] + INSTR_STYPE_SIMM(instr);
  switch (f) {
  case 0x0: {
    // 8-bit store
    int8_t value = (int8_t) core->registers[INSTR_SRCREG2(instr)];
    return SRVE_CORE_STORE(core, addr, &value, 2);
  }
  case 0x1: {
    // 16-bit store
    int16_t value = (int16_t) core->registers[INSTR_SRCREG2(instr)];
    return SRVE_CORE_STORE(core, addr, &value, 2);
  }

#ifdef SRVE_CORE_64BIT
  case 0x2: {
    // 32-bit store
    int32_t value = (int32_t) core->registers[INSTR_SRCREG2(instr)];
    return SRVE_CORE_STORE(core, addr, &value, 4);
  }
  case 0x3: {
    // 64-bit store
    int i = SRVE_CORE_STORE(core, addr, &core->registers[INSTR_SRCREG2(instr)], 8);
    //fprintf(stderr, "Stored 0x%llx at 0x%llx\n", core->registers[INSTR_SRCREG2(instr)], addr);
    return i;
  } break;
#endif

  default:
    fprintf(stderr, "Bad funct3 in store: 0x%x\n", f);
    return -1;
  }
}

int SRVE_CORE_INSTR_AMO(SRVE_CORE_T* core, uint32_t instr) {
  // TODO: These atomic operations should use similar atomic ops on the
  // host machine, this implementation is only fit for testing on single
  // core machines.
  uint32_t special = instr >> 27;
  uint32_t width = INSTR_FUNCT3(instr);
  SRVE_UREG_T addr = core->registers[INSTR_SRCREG1(instr)];
  
  if (special == 1 && width == 2) { // AMOSWAP.W
    int32_t valread;
    if (SRVE_CORE_LOAD(core, addr, &valread, 4) != 0) {
      return -1;
    }
    int32_t valwritten = (int32_t) core->registers[INSTR_SRCREG2(instr)];
    if (SRVE_CORE_STORE(core, addr, &valwritten, 4) != 0) {
      return -1;
    }
    core->registers[INSTR_DESTREG(instr)] = (SRVE_SREG_T) valread;
    return 0;
  }
  
  fprintf(stderr, "Unrecognised atomic memory operation %d (%x) width %d\n", special, special, width);
  return -1;
}

int SRVE_CORE_INSTR_FENCE(SRVE_CORE_T* core, uint32_t instr) {
  // TODO: This is taken as a no-op for now but will need to be finished
  // to test multicore setups.
  return 0;
}

int SRVE_CORE_INSTR_LUI(SRVE_CORE_T* core, uint32_t instr) {
  int32_t constval = (instr >> 12) << 12;
  //fprintf(stderr, "lui: Setting reg #%d to 0x%lx\n", INSTR_DESTREG(instr), ((SRVE_SREG_T) constval));
  core->registers[INSTR_DESTREG(instr)] = ((SRVE_SREG_T) constval);
  return 0;
}

int SRVE_CORE_INSTR_AUIPC(SRVE_CORE_T* core, uint32_t instr) {
  int32_t constval = (instr >> 12) << 12;
  //fprintf(stderr, "auipc: Setting reg #%d to 0x%lx\n", INSTR_DESTREG(instr), core->ip + ((SRVE_SREG_T) constval));
  core->registers[INSTR_DESTREG(instr)] = core->ip + ((SRVE_SREG_T) constval);
  return 0;
}

int SRVE_CORE_INSTR_SYSTEM(SRVE_CORE_T* core, uint32_t instr) {
  //fprintf(stderr, "Got system instruction, dest reg is %d funct3 is %d immediate is %d (0x%x)\n", INSTR_DESTREG(instr), INSTR_FUNCT3(instr), INSTR_ITYPE_UIMM(instr), INSTR_ITYPE_UIMM(instr));
  int f = INSTR_FUNCT3(instr);
  
  if (f == 0 && INSTR_FUNCT7(instr) == 0) { // ecall
    core->sepc = core->ip;
    core->ip_next = (core->stvec >> 2) << 2;
    core->scause = 8;
    return 0;
  } else if (f == 0 && INSTR_FUNCT7(instr) == 24) { // mret
    core->ip_next = core->mepc;
    return 0;
  } else if (f == 0 && INSTR_FUNCT7(instr) == 8) { // sret
    core->ip_next = core->sepc;
    return 0;
  } else if (f == 0 && INSTR_FUNCT7(instr) == 9) { // sfence.vma
    // TODO... This is probably safe as a no-op on single core runs
    return 0;
  } else if (f == 0) {
    printf("Unknown subfunction #%d\n", INSTR_FUNCT7(instr));
    return -1;
  }
  switch (f) {
  case 1: { // csrrw
    uint64_t tmp;
    if (SRVE_CORE_READCSR(core, INSTR_ITYPE_UIMM(instr), &tmp) != 0) {
      return -1;
    }
    SRVE_CORE_WRITECSR(core, INSTR_ITYPE_UIMM(instr), core->registers[INSTR_SRCREG1(instr)]);
    core->registers[INSTR_DESTREG(instr)] = tmp;
  } return 0;
  case 2: { // csrrs
    uint64_t tmp;
    if (SRVE_CORE_READCSR(core, INSTR_ITYPE_UIMM(instr), &tmp) != 0) {
      return -1;
    }
    SRVE_CORE_WRITECSR(core, INSTR_ITYPE_UIMM(instr), tmp | core->registers[INSTR_SRCREG1(instr)]);
    core->registers[INSTR_DESTREG(instr)] = tmp;
  } return 0;
  default:
    fprintf(stderr, "Bad funct3 in system instruction: 0x%x\n", f);
    return -1;
  }
  
  return -1;
}

int SRVE_CORE_INSTR_BRANCH(SRVE_CORE_T* core, uint32_t instr) {
  SRVE_UREG_T addr = core->ip + INSTR_BTYPE_IMM(instr);
  //fprintf(stderr, "instr: ");
  //for (int i = 63; i >=0; i--) { fprintf(stderr, (i%8)?"%s":"%s ", ((instr>>i)&1)?"1":"0"); }
  //fprintf(stderr, " addr: ");
  //for (int i = 63; i >=0; i--) { fprintf(stderr, (i%8)?"%s":"%s ", ((addr>>i)&1)?"1":"0"); }
  //fprintf(stderr, "\n");
  //fprintf(stderr, "b?? %lx <%s>\n", (long) addr, SRVE_CORE_FUNCNAME(core, addr));
  
  SRVE_SREG_T lhs = core->registers[INSTR_SRCREG1(instr)];
  SRVE_SREG_T rhs = core->registers[INSTR_SRCREG2(instr)];
  
  switch (INSTR_FUNCT3(instr)) {
  case 0:
    //printf("Comparing %ld to %ld\n", lhs, rhs);
    if (lhs == rhs) {
      goto dojump;
    }
    return 0;
  case 1:
    if (lhs != rhs) {
      goto dojump;
    }
    return 0;
  case 4:
    if (lhs < rhs) {
      goto dojump;
    }
    return 0;
  case 5:
    if (lhs >= rhs) {
      goto dojump;
    }
    return 0;
  case 6:
    if (((SRVE_UREG_T)lhs) < ((SRVE_UREG_T)rhs)) {
      goto dojump;
    }
    return 0;
  case 7:
    if (((SRVE_UREG_T)lhs) >= ((SRVE_UREG_T)rhs)) {
      goto dojump;
    }
    return 0;
  
  default:
    fprintf(stderr, "Bad branch comparison %d\n", INSTR_FUNCT3(instr));
    return -1;
  }
  
  dojump:
  core->ip_next = core->ip + INSTR_BTYPE_IMM(instr);
  return 0;
}

int SRVE_CORE_INSTR_JAL(SRVE_CORE_T* core, uint32_t instr) {
  SRVE_UREG_T addr = core->ip + (SRVE_SREG_T)INSTR_JTYPE_IMM(instr);
  //fprintf(stderr, "instr: ");
  //for (int i = 63; i >=0; i--) { fprintf(stderr, (i%8)?"%s":"%s ", ((instr>>i)&1)?"1":"0"); }
  //fprintf(stderr, " addr: ");
  //for (int i = 63; i >=0; i--) { fprintf(stderr, (i%8)?"%s":"%s ", ((addr>>i)&1)?"1":"0"); }
  //fprintf(stderr, " 0x550: ");
  //for (int i = 63; i >=0; i--) { fprintf(stderr, (i%8)?"%s":"%s ", ((0x550>>i)&1)?"1":"0"); }
  //fprintf(stderr, "\n");
  //fprintf(stderr, "jal %lx <%s>\n", (long) addr, SRVE_CORE_FUNCNAME(core, addr));
  core->registers[INSTR_DESTREG(instr)] = core->ip_next;
  core->ip_next = (core->ip + (SRVE_SREG_T)INSTR_JTYPE_IMM(instr));
  return 0;
}

int SRVE_CORE_INSTR_JALR(SRVE_CORE_T* core, uint32_t instr) {
  SRVE_UREG_T addr = core->registers[INSTR_SRCREG1(instr)] + (SRVE_SREG_T)INSTR_ITYPE_UIMM(instr);
  if (core->tracing) {
    fprintf(stderr, "instr: ");
    for (int i = 63; i >=0; i--) { fprintf(stderr, (i%8)?"%s":"%s ", ((instr>>i)&1)?"1":"0"); }
    fprintf(stderr, " addr: ");
    for (int i = 63; i >=0; i--) { fprintf(stderr, (i%8)?"%s":"%s ", ((addr>>i)&1)?"1":"0"); }
    fprintf(stderr, " 0x550: ");
    for (int i = 63; i >=0; i--) { fprintf(stderr, (i%8)?"%s":"%s ", ((0x550>>i)&1)?"1":"0"); }
    fprintf(stderr, "\n");
    fprintf(stderr, "jalr -> %lx <%s>\n", (long) addr, SRVE_CORE_FUNCNAME(core, addr));
  }
  core->registers[INSTR_DESTREG(instr)] = core->ip_next;
  core->ip_next = (core->registers[INSTR_SRCREG1(instr)] + INSTR_ITYPE_UIMM(instr));
  return 0;
}

int SRVE_CORE_DOALU(SRVE_CORE_T* core, SRVE_SREG_T* outputvar, SRVE_SREG_T lhs, int op, SRVE_SREG_T rhs, int modifier) {
  SRVE_SREG_T result;
  if (modifier == 1) { // Multiplication extensions
    switch (op) {
      case 0: // Multiply (signed?? does this differ between 32/64-bit RISC-V? TODO: Deep dive on these issues)
        result = lhs * rhs;
        break;
      case 4: // Division
        result = lhs / rhs;
        break;
      case 6: // Remainder
        result = lhs % rhs;
        break;
      default:
      fprintf(stderr, "Bad ALU multiplicative op %d\n", op);
      return -1;
    }
  } else { // Base ALU operations
    switch (op) {
    case 0: // Addition/subtraction
      if (modifier == 0x20) {
        result = lhs - rhs;
      } else {
        result = lhs + rhs;
      }
      break;
    case 1: // Shift left
      result = lhs << rhs;
      break;
    case 2: // Set less than, signed
      //printf("Comparing %ld < %ld\n", lhs, rhs);
      result = (((SRVE_SREG_T)lhs) < ((SRVE_SREG_T)rhs)) ? 1 : 0;
      break;
    case 3: // Set less than, unsigned
      result = (((SRVE_UREG_T)lhs) < ((SRVE_UREG_T)rhs)) ? 1 : 0;
      break;
    case 4: // Exclusive OR (XOR)
      //printf("xoring %ld ^ %ld\n", lhs, rhs);
      result = lhs ^ rhs;
      break;
    case 5: // Shift right, either signed or unsigned
      if (modifier) {
        result = ((SRVE_SREG_T)lhs) >> rhs;
      } else {
        result = ((SRVE_UREG_T)lhs) >> rhs;
      }
      break;
    case 6: // OR
      result = lhs | rhs;
      break;
    case 7: // AND
      result = lhs & rhs;
      break;
    default:
      fprintf(stderr, "Bad ALU op %d\n", op);
      return -1;
    }
  }
  
  *outputvar = result;
  return 0;
}

int SRVE_CORE_LOAD(SRVE_CORE_T* core, uint64_t addr, void* buffer, unsigned int size) {
  srve_memfunc_t memfunc = core->memfunc;
  addr = SRVE_CORE_TRANSLATE(core, addr);
  return memfunc(core->memory, addr, buffer, 0, size);
}

int SRVE_CORE_STORE(SRVE_CORE_T* core, uint64_t addr, void* buffer, unsigned int size) {
  srve_memfunc_t memfunc = core->memfunc;
  addr = SRVE_CORE_TRANSLATE(core, addr);
  return memfunc(core->memory, addr, buffer, 1, size);
}

int SRVE_CORE_READCSR(SRVE_CORE_T* core, int addr, SRVE_UREG_T* outputvar) {
  switch (addr) {
  case 0x100: // sstatus
    *outputvar = 0;
    return 0;
  case 0x104: // sie
    *outputvar = 0;
    return 0;
  case 0x105: // stvec
    *outputvar = core->stvec;
    return 0;
  case 0x140: // sscratch
    *outputvar = core->sscratch;
    return 0;
  case 0x141: // sepc
    *outputvar = core->sepc;
    return 0;
  case 0x142: // scause
    *outputvar = core->scause;
    return 0;
  case 0x14d: // stimecmp, apparently part of the Sstc extension
    *outputvar = 0;
    return 0;
  case 0x180: // satp
    *outputvar = (core->pagetable >> 12) | (core->pagetable ? (8ULL<<60) : 0);
    return 0;
  case 0x300: // mstatus
    *outputvar = 0;
    return 0;
  case 0x302: // medeleg
    *outputvar = 0;
    return 0;
  case 0x303: // mideleg
    *outputvar = 0;
    return 0;
  case 0x304: // mie
    *outputvar = 0;
    return 0;
  case 0x306: // mcounteren
    *outputvar = 0;
    return 0;
  case 0x30a: // menvcfg
    *outputvar = 0;
    return 0;
  case 0x341: // mepc
    *outputvar = core->mepc;
    return 0;
  case 0x3a0: // pmpcfg0
    *outputvar = 0;
    return 0;
  case 0x3b0: // pmpaddr0
    *outputvar = 0;
    return 0;
  case 0xc01: // time
    *outputvar = 0;
    return 0;
  case 0xf14: // hartid
    *outputvar = 0;
    return 0;
  default:
    fprintf(stderr, "Bad CSR #%d (0x%x)\n", addr, addr);
    return -1;
  }
}

int SRVE_CORE_WRITECSR(SRVE_CORE_T* core, int addr, SRVE_UREG_T value) {
  switch (addr) {
  case 0x105: // stvec
    core->stvec = value;
    return 0;
  case 0x140: // sscratch
    core->sscratch = value;
    return 0;
  case 0x141: // sepc
    core->sepc = value;
    return 0;
  case 0x142: // scause
    core->scause = value;
    return 0;
  case 0x180: // satp
    if ((value >> 60) == 8) {
      core->pagetable = value << 12;
      return 0;
    } else if (value == 0) {
      core->pagetable = 0;
      return 0;
    } else {
      fprintf(stderr, "Bad page table address encoding, high part of value is %ld (0x%lx)\n", value>>60, value>>60);
      return -1;
    }
  case 0x341:
    core->mepc = value;
    return 0;
  default:
    return -1;
  }
}

// Returns the "physical" address of the given "virtual" address.
SRVE_UREG_T SRVE_CORE_TRANSLATE(SRVE_CORE_T* core, SRVE_UREG_T vaddr) {
  SRVE_UREG_T outertbl = core->pagetable;
  if (!outertbl) {
    // If core->pagetable is not set via the satp CSR then address
    // translation is disabled, and the physical address is the same as
    // the "virtual" address.
    return vaddr;
  }
  srve_memfunc_t memfunc = core->memfunc;
  SRVE_UREG_T l2idx = (vaddr>>30)&511;
  SRVE_UREG_T l1idx = (vaddr>>21)&511;
  SRVE_UREG_T l0idx = (vaddr>>12)&511;
  SRVE_UREG_T l1tbl;
  SRVE_UREG_T l0tbl;
  SRVE_UREG_T result;
  if (memfunc(core->memory, outertbl+(l2idx*8), &l1tbl, 0, 8)) {
    return 0;
  }
  l1tbl = (l1tbl >> 10) << 12;
  if (memfunc(core->memory, l1tbl+(l1idx*8), &l0tbl, 0, 8)) {
    return 0;
  }
  l0tbl = (l0tbl >> 10) << 12;
  if (memfunc(core->memory, l0tbl+(l0idx*8), &result, 0, 8)) {
    return 0;
  }
  result = ((result >> 10) << 12) | (vaddr & 0xFFFULL);
  //fprintf(stderr, "Translated virtual address 0x%lx to physical address 0x%lx\n", vaddr, result);
  return result;
}

int SRVE_CORE_FETCH(SRVE_CORE_T* core, uint32_t* instrvar) {
  SRVE_UREG_T ip = core->ip;
  if (SRVE_CORE_LOAD(core, ip, instrvar, 4) < 0) {
    return -1;
  }
  core->ip_next = ip + 4;
  return 0;
}

const char* SRVE_CORE_FUNCNAME(SRVE_CORE_T* core, SRVE_UREG_T addr) {
  srve_debugsymbol_t* sym = core->symbols;
  while (sym != NULL) {
    if (sym->next == NULL) {
      if (sym->addr <= addr) {
        return sym->name;
      } else {
        return "???";
      }
    }
    if (sym->addr <= addr && sym->next->addr > addr) {
      return sym->name;
    }
    sym = sym->next;
  }
  return "???";
}

int SRVE_CORE_DISASM(SRVE_CORE_T* core, uint32_t instr) {
  FILE* output = stderr;
  switch (instr & 127) {
    case 0x03: {
      int f = INSTR_FUNCT3(instr);
      SRVE_UREG_T addr = core->registers[INSTR_SRCREG1(instr)] + INSTR_ITYPE_SIMM(instr);
      fprintf(output, "load size=%d dest=x%d src1=x%d imm=%ld\n", f, INSTR_DESTREG(instr), INSTR_SRCREG1(instr), (long) INSTR_ITYPE_SIMM(instr));
    } return 0;
    case 0x13: {
      SRVE_SREG_T rhs = INSTR_ITYPE_SIMM(instr);
      int op = INSTR_FUNCT3(instr);
      fprintf(output, "alu immediate op #%d, dest=x%d src1=x%d imm=%ld\n", op, INSTR_DESTREG(instr), INSTR_SRCREG1(instr), rhs);
    } return 0;
    case 0x17: {
      int32_t constval = (instr >> 12) << 12;
      fprintf(output, "auipc dest=x%d expanded value is 0x%x (%d)\n", INSTR_DESTREG(instr), constval, constval);
    } return 0;
    case 0x23: {
      int f = INSTR_FUNCT3(instr);
      SRVE_UREG_T addr = core->registers[INSTR_SRCREG1(instr)] + INSTR_STYPE_SIMM(instr);
      fprintf(output, "store size=%d src1=x%d src2=x%d imm=%d (0x%x)\n", f, INSTR_SRCREG1(instr), INSTR_SRCREG2(instr), INSTR_STYPE_SIMM(instr), INSTR_STYPE_SIMM(instr));
    } return 0;
    case 0x33: {
      int op = INSTR_FUNCT3(instr);
      int subop = INSTR_FUNCT7(instr);
      fprintf(output, "alu register op #%d subop %d, dest=x%d src1=x%d src2=%d\n", op, subop, INSTR_DESTREG(instr), INSTR_SRCREG1(instr), INSTR_SRCREG2(instr));
    } return 0;
    case 0x37: {
      int32_t constval = (instr >> 12) << 12;
      fprintf(output, "lui dest=x%d expanded value is 0x%x (%d)\n", INSTR_DESTREG(instr), constval, constval);
    } return 0;
    case 0x73: {
        fprintf(output, "system instruction, dest reg is %d funct3 is %d immediate is %d (0x%x)\n", INSTR_DESTREG(instr), INSTR_FUNCT3(instr), INSTR_ITYPE_UIMM(instr), INSTR_ITYPE_UIMM(instr));
    } return 0;
    case 0x63: case 0x67: case 0x6f: {
      fprintf(output, "TODO: branch/jump\n");
    } return 0;
    default:
      fprintf(output, "Unknown Instruction 0x%x, major opcode %d (0x%x)\n", instr, instr & 127, instr & 127);
      return -1;
  }
}

SRVE_SREG_T SRVE_CORE_DOINSTR(SRVE_CORE_T* core) {
  uint32_t instr;
  
  // Rather than having if statements to check if the zero register is used,
  // just set it to zero at the start of each instruction. It shouldn't matter
  // if it's overwritten because it'll be set to zero again before reading.
  core->registers[0] = 0;
  
  if (SRVE_CORE_FETCH(core, &instr) < 0) {
    return -1;
  }
  if (core->tracing /*(core->ip & 0x3ffffff000) == 0x3ffffff000*/) {
    fprintf(stderr, "%lX: in function '%s' instruction 0x%X op 0x%X\n", core->ip, SRVE_CORE_FUNCNAME(core, core->ip), instr, instr & 127);
    
    if (SRVE_CORE_DISASM(core, instr) != 0) {
      //return -1;
    }
  }
  // Look up instruction based on the lower 6 bits:
  SRVE_CORE_INSTRFUNC_T instrfunc = core->instrfuncs[instr & 127];
  
  if (instrfunc(core, instr) != 0) {
    fprintf(stderr, "%lX: FAILURE in function '%s' instruction 0x%X op 0x%X\n", core->ip, SRVE_CORE_FUNCNAME(core, core->ip), instr, instr & 127);

    return -1;
  }
  if (core->tracing) {
    for (int i = 1; i < 32; i++) {
      if (core->registers[i] != 0) {
        fprintf(stderr, "x%d: 0x%llx\n", i, core->registers[i]);
      }
    }
  }
  if (core->ip == core->ip_next) {
    fprintf(stderr, "Infinite loop detected, exiting emulator.\n");
    exit(-1);
  }
  
  core->ip = core->ip_next;
  
  return 0;
}

int SRVE_CORE_INIT(SRVE_CORE_T* core, srve_memfunc_t memfunc, void* memdata, SRVE_UREG_T ip) {
  core->memory = memdata;
  core->memfunc = memfunc;
  core->pagetable = 0;
  core->symbols = NULL;
  core->ip = ip;
  core->ip_next = ip;
  for (int i = 0; i < 32; i++) {
    core->registers[i] = 0;
  }
  for (int i = 0; i < 128; i++) {
    core->instrfuncs[i] = &SRVE_CORE_INSTR_BAD;
  }
  core->instrfuncs[0x03] = &SRVE_CORE_INSTR_LOAD;
  core->instrfuncs[0x0F] = &SRVE_CORE_INSTR_FENCE;
  core->instrfuncs[0x13] = &SRVE_CORE_INSTR_IMMALU;
  core->instrfuncs[0x17] = &SRVE_CORE_INSTR_AUIPC;
#ifdef SRVE_CORE_64BIT
  core->instrfuncs[0x1B] = &srve_core64_instr_immalu32;
  core->instrfuncs[0x3B] = &srve_core64_instr_regalu32;
#endif
  core->instrfuncs[0x23] = &SRVE_CORE_INSTR_STORE;
  core->instrfuncs[0x2F] = &SRVE_CORE_INSTR_AMO;
  core->instrfuncs[0x33] = &SRVE_CORE_INSTR_REGALU;
  core->instrfuncs[0x37] = &SRVE_CORE_INSTR_LUI;
  core->instrfuncs[0x63] = &SRVE_CORE_INSTR_BRANCH;
  core->instrfuncs[0x67] = &SRVE_CORE_INSTR_JALR;
  core->instrfuncs[0x6F] = &SRVE_CORE_INSTR_JAL;
  core->instrfuncs[0x73] = &SRVE_CORE_INSTR_SYSTEM;
  return 0;
}