/* * AVR emulator * CPU emulation * * Copyright (C) 2007 Michael Buesch * * This program is free software; you can redistribute it and/or * modify it under the terms of the GNU General Public License * as published by the Free Software Foundation; either version 2 * of the License, or (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. */ #include "cpu.h" #include "util.h" #include "hardware.h" #include #include #include #include #include /* The CPU */ struct avr_cpu cpu; /* The flash memory */ static char *code; /* The POSIX CPU thread. */ static pthread_t cpu_thread; static uint8_t cpu_get_sreg(void); static inline bool in_cpu_thread(void) { return !!pthread_equal(pthread_self(), cpu_thread); } /* Get the ID of the instruction that modified this lazy flag. */ static inline uint8_t lazy_flag_get_insn(const struct lazy_flag *f) { return (uint8_t)(f - cpu.lazy_flag_cache); } /* Get the stack pointer. */ static inline uint16_t getSP(void) { return le16_to_cpu(*((le16_t *)&memory.io[memory.sp_offset])); } /* Write the stack pointer. */ static inline void setSP(uint16_t sp) { *((le16_t *)&memory.io[memory.sp_offset]) = cpu_to_le16(sp); } /* Get the X pointer */ static inline uint16_t getX(void) { const uint8_t *x = &(cpu.r[X_LO]); return le16_to_cpu(*(le16_t *)x); } /* Set the X pointer */ static inline void setX(uint16_t val) { le16_t *x = (le16_t *)(&(cpu.r[X_LO])); *x = cpu_to_le16(val); } /* Get the Y pointer */ static inline uint16_t getY(void) { const uint8_t *y = &(cpu.r[Y_LO]); return le16_to_cpu(*(le16_t *)y); } /* Set the Y pointer */ static inline void setY(uint16_t val) { le16_t *y = (le16_t *)(&(cpu.r[Y_LO])); *y = cpu_to_le16(val); } /* Get the Z pointer */ static inline uint16_t getZ(void) { const uint8_t *z = &(cpu.r[Z_LO]); return le16_to_cpu(*(le16_t *)z); } /* Set the Z pointer */ static inline void setZ(uint16_t val) { le16_t *z = (le16_t *)(&(cpu.r[Z_LO])); *z = cpu_to_le16(val); } static void dump_stack(FILE *fd) { uint16_t sp; int i = 0, count = 40; sp = getSP(); if (sp >= memory.size) { fprintf(fd, "Invalid stackpointer. Not dumping.\n"); return; } fprintf(fd, "Stackdump (%d bytes):\n", count); while (i < count && sp && sp < memory.size) { if ((i != 0) && (i % 4 == 0)) fprintf(fd, "\n"); fprintf(fd, "<0x%04X>: 0x%02X ", sp, memory.ram[sp]); sp++; i++; } fprintf(fd, "\n"); } #if 0 static void dump_lazy_flag(FILE *fd, const struct lazy_flag *f) { fprintf(fd, "insn: 0x%02X op[0]: 0x%02X op[1]: 0x%02X res: 0x%02X " "op16[0]: 0x%04X op16[1]: 0x%04X res16: 0x%04X\n", lazy_flag_get_insn(f), f->op[0], f->op[1], f->res, f->op16[0], f->op16[1], f->res16); } static void dump_lazy_flags(FILE *fd) { struct lazy_flag *f; int i; for (i = 0; i < ARRAY_SIZE(cpu.sreg_lazy); i++) { f = cpu.sreg_lazy[i]; if (f) { fprintf(fd, "LF%d -> ", i); dump_lazy_flag(fd, cpu.sreg_lazy[i]); } } } #endif void dump_cpu(FILE *fd) { int i; uint8_t sreg; fprintf(fd, "=== CPU dump ===\n"); fprintf(fd, "R: %u PC: 0x%08X (0x%08X) SP: 0x%04X Cycles: %llu\n", (unsigned)cpu.running, cpu.pc, cpu.pc * 2, getSP(), (unsigned long long)cpu.cycles_count); sreg = cpu_get_sreg(); fprintf(fd, "SREG: 0x%02X IRQs: %llu TRAP: %u\n", sreg, (unsigned long long)cpu.irq_count, reload_var(cpu.trap)); if (cpu.r) { for (i = 0; i < AVR_NR_REGS; i++) { if ((i != 0) && (i % 4 == 0)) fprintf(fd, "\n"); fprintf(fd, "r%02u: 0x%02X ", i, cpu.r[i]); } fprintf(fd, "\n"); fprintf(fd, "X: 0x%04X Y: 0x%04X Z: 0x%04X\n", getX(), getY(), getZ()); } else fprintf(fd, "Invalid register file. Not dumping.\n"); dump_stack(fd); } static void __attribute__((__noreturn__)) fault(const char *fmt, ...) { va_list va; fprintf(stderr, "\n---=== FAULT ===---\n"); va_start(va, fmt); vfprintf(stderr, fmt, va); va_end(va); fprintf(stderr, "\n"); dump_cpu(stderr); if (in_cpu_thread()) { pthread_exit(NULL); } else { fprintf(stderr, "\n>>> BUG: Faulting from outside of the CPU! <<<\n"); exit(1); } while (1) ; } /* Get the SREG Carry flag. */ static bool sreg_get_C(void) { const struct lazy_flag *f = cpu.sreg_lazy[SREG_C_BIT]; bool set; if (cpu.sreg_valid & SREG_C) return !!(cpu.sreg & SREG_C); switch (lazy_flag_get_insn(f)) { case INSN_SBCI: case INSN_SUBI: case INSN_CPI: case INSN_CPC: case INSN_SBC: case INSN_CP: case INSN_SUB: set = !!(((~(f->op[0]) & f->op[1]) | (f->op[1] & f->res) | (f->res & ~(f->op[0]))) & (1 << 7)); break; case INSN_ADD: case INSN_ADC: set = !!(((f->op[0] & f->op[1]) | (f->op[1] & ~(f->res)) | (~(f->res) & f->op[0])) & (1 << 7)); break; case INSN_COM: set = 1; break; case INSN_NEG: set = !!(f->res); break; case INSN_LSR: case INSN_ROR: case INSN_ASR: set = !!(f->op[0] & 0x01); break; case INSN_ADIW: set = (!!(~(f->res16) & 0x8000) & !!(f->op16[0] & 0x8000)); break; case INSN_SBIW: set = (!!(f->res16 & 0x8000) & !!(~(f->op16[0]) & 0x8000)); break; default: if (!in_cpu_thread()) return 0; fault("Lazy C flag: Unhandled instruction %u", lazy_flag_get_insn(f)); } cpu.sreg &= ~SREG_C; cpu.sreg |= (set << SREG_C_BIT); cpu.sreg_valid |= SREG_C; return set; } /* Set the SREG Carry flag. */ static inline void sreg_set_C(bool C) { cpu.sreg &= ~SREG_C; cpu.sreg |= (C << SREG_C_BIT); cpu.sreg_valid |= SREG_C; } /* Set the SREG Carry flag. LAZY */ static inline void sreg_set_C_lazy(uint8_t insn) { cpu.sreg_lazy[SREG_C_BIT] = &(cpu.lazy_flag_cache[insn]); } /* Get the SREG Zero flag. */ static bool sreg_get_Z(void) { const struct lazy_flag *f = cpu.sreg_lazy[SREG_Z_BIT]; bool set; if (cpu.sreg_valid & SREG_Z) return !!(cpu.sreg & SREG_Z); switch (lazy_flag_get_insn(f)) { case INSN_SBCI: case INSN_CPC: case INSN_SBC: if (f->res == 0) { /* Take the previously set Z flag. */ set = !!(cpu.sreg & SREG_Z); } else set = 0; break; case INSN_SUBI: case INSN_ORI: case INSN_ANDI: case INSN_CPI: case INSN_COM: case INSN_NEG: case INSN_DEC: case INSN_INC: case INSN_LSR: case INSN_ROR: case INSN_ASR: case INSN_ADD: case INSN_CP: case INSN_SUB: case INSN_ADC: case INSN_AND: case INSN_EOR: case INSN_OR: set = (f->res == 0); break; case INSN_ADIW: case INSN_SBIW: set = (f->res16 == 0); break; default: if (!in_cpu_thread()) return 0; fault("Lazy Z flag: Unhandled instruction %u", lazy_flag_get_insn(f)); } cpu.sreg &= ~SREG_Z; cpu.sreg |= (set << SREG_Z_BIT); cpu.sreg_valid |= SREG_Z; return set; } /* Set the SREG Zero flag. */ static inline void sreg_set_Z(bool Z) { cpu.sreg &= ~SREG_Z; cpu.sreg |= (Z << SREG_Z_BIT); cpu.sreg_valid |= SREG_Z; } /* Set the SREG Zero flag. LAZY */ static inline void sreg_set_Z_lazy(uint8_t insn) { cpu.sreg_lazy[SREG_Z_BIT] = &(cpu.lazy_flag_cache[insn]); } /* Get the SREG Negative flag. */ static bool sreg_get_N(void) { const struct lazy_flag *f = cpu.sreg_lazy[SREG_N_BIT]; bool set; if (cpu.sreg_valid & SREG_N) return !!(cpu.sreg & SREG_N); switch (lazy_flag_get_insn(f)) { case INSN_SBCI: case INSN_SUBI: case INSN_ORI: case INSN_ANDI: case INSN_CPI: case INSN_COM: case INSN_NEG: case INSN_DEC: case INSN_INC: case INSN_ROR: case INSN_ASR: case INSN_CPC: case INSN_SBC: case INSN_ADD: case INSN_CP: case INSN_SUB: case INSN_ADC: case INSN_AND: case INSN_EOR: case INSN_OR: set = !!(f->res & 0x80); break; case INSN_LSR: set = 0; break; case INSN_ADIW: case INSN_SBIW: set = !!(f->res16 & 0x8000); break; default: if (!in_cpu_thread()) return 0; fault("Lazy N flag: Unhandled instruction %u", lazy_flag_get_insn(f)); } cpu.sreg &= ~SREG_N; cpu.sreg |= (set << SREG_N_BIT); cpu.sreg_valid |= SREG_N; return set; } /* Set the SREG Negative flag. */ static inline void sreg_set_N(bool N) { cpu.sreg &= ~SREG_N; cpu.sreg |= (N << SREG_N_BIT); cpu.sreg_valid |= SREG_N; } /* Set the SREG Negative flag. LAZY */ static inline void sreg_set_N_lazy(uint8_t insn) { cpu.sreg_lazy[SREG_N_BIT] = &(cpu.lazy_flag_cache[insn]); } /* Get the SREG two's complement oVerflow flag. */ static bool sreg_get_V(void) { const struct lazy_flag *f = cpu.sreg_lazy[SREG_V_BIT]; bool set; if (cpu.sreg_valid & SREG_V) return !!(cpu.sreg & SREG_V); switch (lazy_flag_get_insn(f)) { case INSN_SBCI: case INSN_SUBI: case INSN_CPI: case INSN_CPC: case INSN_SBC: case INSN_CP: case INSN_SUB: set = !!(((f->op[0] & ~(f->op[1]) & ~(f->res)) | (~(f->op[0]) & f->op[1] & f->res)) & (1 << 7)); break; case INSN_ADD: case INSN_ADC: set = !!(((f->op[0] & f->op[1] & ~(f->res)) | (~(f->op[0]) & ~(f->op[1]) & f->res)) & (1 << 7)); break; case INSN_ORI: case INSN_ANDI: case INSN_COM: case INSN_AND: case INSN_EOR: case INSN_OR: set = 0; break; case INSN_NEG: case INSN_INC: set = (f->res == 0x80); break; case INSN_DEC: set = (f->op[0] == 0x80); break; case INSN_LSR: set = !!(0 ^ (f->op[0] & 0x01)); break; case INSN_ROR: case INSN_ASR: set = (!!(f->res & 0x80)) ^ (!!(f->op[0] & 0x01)); break; case INSN_ADIW: set = (!!(~(f->op16[0]) & 0x8000) & !!(f->res16 & 0x8000)); break; case INSN_SBIW: set = (!!(f->op16[0] & 0x8000) & !!(~(f->res16) & 0x8000)); break; default: if (!in_cpu_thread()) return 0; fault("Lazy V flag: Unhandled instruction %u", lazy_flag_get_insn(f)); } cpu.sreg &= ~SREG_V; cpu.sreg |= (set << SREG_V_BIT); cpu.sreg_valid |= SREG_V; return set; } /* Set the SREG two's complement oVerflow flag. */ static inline void sreg_set_V(bool V) { cpu.sreg &= ~SREG_V; cpu.sreg |= (V << SREG_V_BIT); cpu.sreg_valid |= SREG_V; } /* Set the SREG two's complement oVerflow flag. LAZY */ static inline void sreg_set_V_lazy(uint8_t insn) { cpu.sreg_lazy[SREG_V_BIT] = &(cpu.lazy_flag_cache[insn]); } /* Get the SREG Signed flag. */ static bool sreg_get_S(void) { bool set; if (cpu.sreg_valid & SREG_S) return !!(cpu.sreg & SREG_S); /* S is always N XOR V. */ set = sreg_get_N() ^ sreg_get_V(); cpu.sreg &= ~SREG_S; cpu.sreg |= (set << SREG_S_BIT); cpu.sreg_valid |= SREG_S; return set; } static inline void sreg_set_S(bool S) { cpu.sreg &= ~SREG_S; cpu.sreg |= (S << SREG_S_BIT); cpu.sreg_valid |= SREG_S; } static inline void sreg_set_S_lazy(uint8_t insn) { /* We don't need to set the pointer here, as S is calculated * from the N and V flags. */ /* cpu.sreg_lazy[SREG_S_BIT] = &(cpu.lazy_flag_cache[insn]); */ } /* Get the SREG Halfcarry flag. */ static bool sreg_get_H(void) { const struct lazy_flag *f = cpu.sreg_lazy[SREG_H_BIT]; bool set; if (cpu.sreg_valid & SREG_H) return !!(cpu.sreg & SREG_H); switch (lazy_flag_get_insn(f)) { case INSN_SBCI: case INSN_SUBI: case INSN_CPI: case INSN_CPC: case INSN_SBC: case INSN_CP: case INSN_SUB: set = !!(((~(f->op[0]) & f->op[1]) | (f->op[1] & f->res) | (f->res & ~(f->op[0]))) & (1 << 3)); break; case INSN_ADD: case INSN_ADC: set = !!(((f->op[0] & f->op[1]) | (f->op[1] & ~(f->res)) | (~(f->res) & f->op[0])) & (1 << 3)); break; case INSN_NEG: set = !!((f->res | f->op[0]) & (1 << 3)); break; default: if (!in_cpu_thread()) return 0; fault("Lazy H flag: Unhandled instruction %u", lazy_flag_get_insn(f)); } cpu.sreg &= ~SREG_H; cpu.sreg |= (set << SREG_H_BIT); cpu.sreg_valid |= SREG_H; return set; } /* Set the SREG Halfcarry flag. */ static inline void sreg_set_H(bool H) { cpu.sreg &= ~SREG_H; cpu.sreg |= (H << SREG_H_BIT); cpu.sreg_valid |= SREG_H; } /* Set the SREG Halfcarry flag. LAZY */ static inline void sreg_set_H_lazy(uint8_t insn) { cpu.sreg_lazy[SREG_H_BIT] = &(cpu.lazy_flag_cache[insn]); } /* Get the SREG Transfer bit flag. */ static inline bool sreg_get_T(void) { /* T is not lazy, so always valid. */ return !!(cpu.sreg & SREG_T); } /* Set the SREG Transfer bit flag. */ static inline void sreg_set_T(bool T) { cpu.sreg &= ~SREG_T; cpu.sreg |= (T << SREG_T_BIT); } /* Get the SREG global Interrupt enable flag. */ static inline bool sreg_get_I(void) { /* I is not lazy, so always valid. */ return !!(cpu.sreg & SREG_I); } /* Set the SREG global Interrupt enable flag. */ static inline void sreg_set_I(bool I) { cpu.sreg &= ~SREG_I; cpu.sreg |= (I << SREG_I_BIT); } #define LAZY_NONE 0xFFFFFFFF /* Setup lazy flags for an instruction that works on 8-bit operands. * This whole function will optimize to only a few Host-CPU instructions * without any branches. */ static inline void lazy_flags_setup(const uint8_t insn, const uint32_t oper0, const uint32_t oper1, const uint32_t res, const uint8_t sreg_invalidate_mask) { struct lazy_flag *f = &(cpu.lazy_flag_cache[insn]); break_linkage_on(!is_const_value(insn), lazy_flags_setup_insn_not_const); break_linkage_on(!is_const_value(sreg_invalidate_mask), lazy_flags_setup_sreg_mask_not_const); /* The if-branches are optimized away at compiletime, as this * is an inline function. The conditionals always evaluate into * constant expressions, so the compiler removes the entire branch * instruction. */ if (insn >= FIRST_16BIT_INSN) { if (!is_const_value(oper0) || oper0 != LAZY_NONE) f->op16[0] = oper0; if (!is_const_value(oper1) || oper1 != LAZY_NONE) f->op16[1] = oper1; if (!is_const_value(res) || res != LAZY_NONE) f->res16 = res; } else { if (!is_const_value(oper0) || oper0 != LAZY_NONE) f->op[0] = oper0; if (!is_const_value(oper1) || oper1 != LAZY_NONE) f->op[1] = oper1; if (!is_const_value(res) || res != LAZY_NONE) f->res = res; } /* The invalidation mask is constant. So the following if-branches * will optimize away. */ if (sreg_invalidate_mask & SREG_C) sreg_set_C_lazy(insn); if (sreg_invalidate_mask & SREG_Z) sreg_set_Z_lazy(insn); if (sreg_invalidate_mask & SREG_N) sreg_set_N_lazy(insn); if (sreg_invalidate_mask & SREG_V) sreg_set_V_lazy(insn); if (sreg_invalidate_mask & SREG_S) sreg_set_S_lazy(insn); if (sreg_invalidate_mask & SREG_H) sreg_set_H_lazy(insn); /* Invalidate all flags that are changed by this instruction. */ cpu.sreg_valid &= ~sreg_invalidate_mask; } /* Get the whole sreg. Note that this function is expensive. * Use the single bit fetch functions, if you only need a single * bit or a subset of the sreg bits. */ static uint8_t cpu_get_sreg(void) { uint8_t sreg = 0; sreg |= (uint8_t)sreg_get_C() << SREG_C_BIT; sreg |= (uint8_t)sreg_get_Z() << SREG_Z_BIT; sreg |= (uint8_t)sreg_get_N() << SREG_N_BIT; sreg |= (uint8_t)sreg_get_V() << SREG_V_BIT; sreg |= (uint8_t)sreg_get_S() << SREG_S_BIT; sreg |= (uint8_t)sreg_get_H() << SREG_H_BIT; sreg |= (uint8_t)sreg_get_T() << SREG_T_BIT; sreg |= (uint8_t)sreg_get_I() << SREG_I_BIT; return sreg; } /* Trigger the sideeffects of an I/O store (if registered any) */ static inline void memory_store_sideeffect_trigger(uint16_t offset, uint8_t old_val, uint8_t new_val) { io_write_handler_t handler; handler = memory.io_write_handlers[offset - IO_MEM_OFFSET]; if (handler) handler(offset, old_val, new_val); } /* Trigger the sideeffects of an I/O load (if registered any) */ static inline void memory_load_sideeffect_trigger(uint16_t offset) { io_read_handler_t handler; handler = memory.io_read_handlers[offset - IO_MEM_OFFSET]; if (handler) handler(offset); } /* Store a value to memory and trigger sideeffects if needed. */ static inline void memory_store(uint16_t offset, uint8_t value) { uint8_t old, is_io_memory; memory_lock(offset, &is_io_memory); old = memory.ram[offset]; memory.ram[offset] = value; if (is_io_memory) memory_store_sideeffect_trigger(offset, old, value); memory_unlock(is_io_memory); } /* Memory store to non-IO memory. */ static inline void memory_store_nonio(uint16_t offset, uint8_t value) { if (CHECK(mem_offset_in_io_region(offset))) { fault("Attempt to store to I/O memory by an instruction " "that should only access non-I/O memory."); } memory.ram[offset] = value; } /* Load a value from memory. Take care of locking. */ static inline uint8_t memory_load(uint16_t offset) { uint8_t ret, is_io_memory; memory_lock(offset, &is_io_memory); if (is_io_memory) memory_load_sideeffect_trigger(offset); ret = memory.ram[offset]; memory_unlock(is_io_memory); return ret; } /* Memory load from non-IO memory. */ static inline uint8_t memory_load_nonio(uint16_t offset) { if (CHECK(mem_offset_in_io_region(offset))) { fault("Attempt to load from I/O memory by an instruction " "that should only access non-I/O memory."); } return memory.ram[offset]; } static inline void insn_ret(void) { uint32_t a, b, c; uint16_t sp; sp = getSP(); if (active_setup.pc_22bit) { if (CHECK(sp >= memory.size - 3)) fault("RET instruction stack overflow"); c = memory_load_nonio(++sp); b = memory_load_nonio(++sp); a = memory_load_nonio(++sp); setSP(sp); cpu.pc = a | (b << 8) | (c << 16); } else { if (CHECK(sp >= memory.size - 2)) fault("RET instruction stack overflow"); b = memory_load_nonio(++sp); a = memory_load_nonio(++sp); setSP(sp); cpu.pc = a | (b << 8); } } static inline void insn_store_pc_on_stack(bool pc_22bit, uint8_t pc_inc) { uint32_t ret_pc; uint16_t sp; ret_pc = cpu.pc + pc_inc; sp = getSP(); if (pc_22bit) { if (CHECK(sp < 3)) fault("PC -> STACK operation stack underflow"); memory_store_nonio(sp--, ret_pc); memory_store_nonio(sp--, ret_pc >> 8); memory_store_nonio(sp--, ret_pc >> 16); setSP(sp); } else { if (CHECK(sp < 2)) fault("PC -> STACK operation stack underflow"); memory_store_nonio(sp--, ret_pc); memory_store_nonio(sp--, ret_pc >> 8); setSP(sp); } } /* Get the low 16bits of a 16bit or 32bit instruction at PC. */ static inline uint16_t get_lo16_insn(void) { uint32_t byteoffset; byteoffset = cpu.pc * 2; if (CHECK(byteoffset >= active_setup.flash_size)) fault("CPU Program Counter >= flash size"); return le16_to_cpu(*(le16_t *)(code + byteoffset)); } /* Get the high 16bits of a 32bit instruction at PC. */ static inline uint16_t get_hi16_insn(void) { uint32_t byteoffset; byteoffset = (cpu.pc + 1) * 2; if (CHECK(byteoffset >= active_setup.flash_size)) fault("HI16-fetch: CPU Program Counter >= flash size"); return le16_to_cpu(*(le16_t *)(code + byteoffset)); } static inline bool is_two_word_instruction(uint16_t insn) { switch (insn & 0xFE0F) { case 0x9000: /* lds */ case 0x9200: /* sts */ return 1; } switch (insn & 0xFE0E) { case 0x940c: /* jmp */ case 0x940e: /* call */ return 1; } return 0; } /* Trap the CPU. This is called from outside of the CPU thread. */ int trap_cpu(uint8_t id, bool synchronize) { pthread_mutex_lock(&cpu.trap_mutex); if (cpu.trap == id) { pthread_mutex_unlock(&cpu.trap_mutex); return 0; } cpu.trap = id; if (synchronize) pthread_cond_wait(&cpu.trapped_cond, &cpu.trap_mutex); pthread_mutex_unlock(&cpu.trap_mutex); return 0; } static int handle_trap(void) { uint8_t id; int stay_trapped; pthread_mutex_lock(&cpu.trap_mutex); id = cpu.trap; if (unlikely(id == CPU_TRAP_CONTINUE)) { stay_trapped = 0; goto out_unlock; } pthread_cond_broadcast(&cpu.trapped_cond);//FIXME only broadcast after the first trap cycle? switch (id) { case CPU_TRAP_WAIT: stay_trapped = 1; break; case CPU_TRAP_SINGLESTEP: /* Continue execution. We will get trapped again * after the next instruction. */ stay_trapped = 0; break; case CPU_TRAP_CONTINUE: /* Continue execution. */ stay_trapped = 0; break; case CPU_TRAP_EXIT: cpu.running = 0; pthread_mutex_unlock(&cpu.trap_mutex); pthread_exit(NULL); return 1; default: pthread_mutex_unlock(&cpu.trap_mutex); fault("Unknown CPU trap %u triggered\n", id); return 1; } out_unlock: pthread_mutex_unlock(&cpu.trap_mutex); if (stay_trapped) pthread_yield(); return stay_trapped; } static void handle_breakpoint(bool programmed_bp) { //FIXME: We don't really want to fault here. // But do this for now. fault("Breakpoint"); } /* Trigger an IRQ. This is called from outside of the CPU thread. */ int cpu_trigger_interrupt(uint8_t vector) { struct avr_irq_descriptor *desc; pthread_spin_lock(&cpu.irq_lock); if (unlikely(cpu.irqs_pending == MAX_IRQS_PENDING)) { pthread_spin_unlock(&cpu.irq_lock); return -EBUSY; } /* Get a free IRQ descriptor. */ desc = list_entry(cpu.irqdesc_cache.next, struct avr_irq_descriptor, list); list_move_tail(&desc->list, &cpu.irqdesc_pending); desc->vector = vector; cpu.irqs_pending++; pthread_spin_unlock(&cpu.irq_lock); return 0; } static void handle_irqs(void) { struct avr_irq_descriptor *desc; uint8_t vector; if (!sreg_get_I()) { /* Global interrupts are disabled. Handle the IRQ later. */ return; } pthread_spin_lock(&cpu.irq_lock); if (unlikely(!cpu.irqs_pending)) { pthread_spin_unlock(&cpu.irq_lock); return; } cpu.irqs_pending--; /* Read the IRQ descriptor and return it to the cache. */ desc = list_entry(cpu.irqdesc_pending.next, struct avr_irq_descriptor, list); vector = desc->vector; list_move(&desc->list, &cpu.irqdesc_cache); pthread_spin_unlock(&cpu.irq_lock); /* IRQs are entered with the I flag disabled. */ sreg_set_I(0); /* Store PC on the stack, so we can later return from the * IRQ with a reti instruction. */ insn_store_pc_on_stack(active_setup.pc_22bit, 0); /* Jump to the IRQ vector. */ cpu.pc = vector; cpu.irq_count++; } /* This is the function where the code gets executed. */ static void * cpu_thread_function(void *unused) { uint16_t insn; bool enable_irqs = 0; int res; //TODO implement memory RAMPD stuff. cpu.cycles_count = 0; while (1) { if (unlikely(enable_irqs)) { sreg_set_I(1); enable_irqs = 0; } next_insn_skip_irqcheck: if (unlikely(reload_var(cpu.trap))) { while (1) { res = handle_trap(); if (res == 0) break; /* Continue execution */ } } if (unlikely(cpu.breakpoint == cpu.pc)) handle_breakpoint(0); if (unlikely(reload_var(cpu.irqs_pending))) handle_irqs(); /* Get the instruction at PC and handle it. */ insn = get_lo16_insn(); //printf("PC: 0x%08X\n", cpu.pc * 2); switch (insn) { case 0x0000: /* nop */ cpu.pc++; cpu.cycles_count++; continue; case 0x9409: /* ijmp */ cpu.pc = getZ(); cpu.cycles_count += 2; continue; case 0x9419: /* eijmp */ cpu.pc = getZ(); //TODO load EIND into PC.high cpu.cycles_count += 2; continue; case 0x9508: /* ret */ insn_ret(); cpu.cycles_count += 4; continue; case 0x9509: /* icall */ insn_store_pc_on_stack(active_setup.pc_22bit, 1); cpu.pc = getZ(); cpu.cycles_count += 3; continue; case 0x9518: /* reti */ insn_ret(); /* XXX Also execute the next insn with IRQs still disabled here? */ sreg_set_I(1); cpu.cycles_count += 4; continue; case 0x9519: /* eicall */ insn_store_pc_on_stack(1, 1); cpu.pc = getZ(); //TODO load EIND into PC.high cpu.cycles_count += 4; continue; case 0x9588: /* sleep */ //TODO cpu.pc++; cpu.cycles_count++; continue; case 0x95a8: /* wdr */ //TODO cpu.pc++; cpu.cycles_count++; continue; case 0x95c8: { /* lpm */ uint16_t z = getZ(); if (CHECK(z >= active_setup.flash_size)) fault("LPM with Z >= flash size"); cpu.r[0] = code[z]; cpu.pc++; cpu.cycles_count += 3; continue; } case 0x95d8: { /* elpm */ uint32_t offset; fault("ELPM is not implemented"); offset = getZ(); //TODO fetch RAMPZ //TODO offset sanity check cpu.r[0] = code[offset]; cpu.pc++; cpu.cycles_count += 3; continue; } case 0x95e8: { /* spm */ uint32_t offset; fault("SPM is not implemented"); offset = getZ(); //TODO fetch RAMPZ code[offset] = 0xFF; code[offset + 1] = 0xFF; //TODO write the stuff. cpu.pc++; //TODO set cpu.cycles_count continue; } case 0x9598: /* break */ handle_breakpoint(1); cpu.pc++; cpu.cycles_count++; continue; } switch (insn & 0xFF8F) { case 0x9408: { /* bset */ uint8_t bitnr; cpu.pc++; cpu.cycles_count++; bitnr = (insn & 0x70) >> 4; switch (bitnr) { case SREG_C_BIT: sreg_set_C(1); break; case SREG_Z_BIT: sreg_set_Z(1); break; case SREG_N_BIT: sreg_set_N(1); break; case SREG_V_BIT: sreg_set_V(1); break; case SREG_S_BIT: sreg_set_S(1); break; case SREG_H_BIT: sreg_set_H(1); break; case SREG_T_BIT: sreg_set_T(1); break; case SREG_I_BIT: /* Enable IRQs after the _next_ instruction has finished. */ enable_irqs = 1; goto next_insn_skip_irqcheck; } continue; } case 0x9488: { /* bclr */ uint8_t bitnr; bitnr = (insn & 0x70) >> 4; switch (bitnr) { case SREG_C_BIT: sreg_set_C(0); break; case SREG_Z_BIT: sreg_set_Z(0); break; case SREG_N_BIT: sreg_set_N(0); break; case SREG_V_BIT: sreg_set_V(0); break; case SREG_S_BIT: sreg_set_S(0); break; case SREG_H_BIT: sreg_set_H(0); break; case SREG_T_BIT: sreg_set_T(0); break; case SREG_I_BIT: sreg_set_I(0); break; } cpu.pc++; cpu.cycles_count++; continue; } } switch (insn & 0xF000) { case 0x4000: { /* sbci */ uint8_t reg, orig, val, res; reg = ((insn & 0x00F0) >> 4) + 16; val = ((insn & 0x0F00) >> 4) | (insn & 0x000F); orig = cpu.r[reg]; res = orig - val - (uint8_t)sreg_get_C(); cpu.r[reg] = res; lazy_flags_setup(INSN_SBCI, orig, val, res, SREG_H | SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x5000: { /* subi */ uint8_t reg, orig, val, res; reg = ((insn & 0x00F0) >> 4) + 16; val = ((insn & 0x0F00) >> 4) | (insn & 0x000F); orig = cpu.r[reg]; res = orig - val; cpu.r[reg] = res; lazy_flags_setup(INSN_SUBI, orig, val, res, SREG_H | SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x6000: { /* ori */ uint8_t reg, orig, val, res; reg = ((insn & 0x00F0) >> 4) + 16; val = ((insn & 0x0F00) >> 4) | (insn & 0x000F); orig = cpu.r[reg]; res = orig | val; cpu.r[reg] = res; lazy_flags_setup(INSN_ORI, LAZY_NONE, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z); cpu.pc++; cpu.cycles_count++; continue; } case 0x7000: { /* andi */ uint8_t reg, orig, val, res; reg = ((insn & 0x00F0) >> 4) + 16; val = ((insn & 0x0F00) >> 4) | (insn & 0x000F); orig = cpu.r[reg]; res = orig & val; cpu.r[reg] = res; lazy_flags_setup(INSN_ANDI, LAZY_NONE, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z); cpu.pc++; cpu.cycles_count++; continue; } case 0xe000: { /* ldi */ uint8_t reg, val; reg = ((insn & 0x00F0) >> 4) + 16; val = ((insn & 0x0F00) >> 4) | (insn & 0x000F); cpu.r[reg] = val; cpu.pc++; cpu.cycles_count++; continue; } case 0xc000: { /* rjmp */ int32_t offset = 0; if (insn & 0x0800) offset = ~0x0FFF; offset |= insn & 0x0FFF; cpu.pc = cpu.pc + offset + 1; if (CHECK(cpu.pc >= active_setup.flash_size / 2)) fault("RJMP destination PC >= flash length"); cpu.cycles_count += 2; continue; } case 0xd000: { /* rcall */ int32_t offset = 0; if (insn & 0x0800) offset = ~0x0FFF; offset |= insn & 0x0FFF; insn_store_pc_on_stack(active_setup.pc_22bit, 1); cpu.pc = cpu.pc + offset + 1; if (CHECK(cpu.pc >= active_setup.flash_size / 2)) fault("RCALL destination PC >= flash length"); cpu.cycles_count += 3; continue; } case 0x3000: { /* cpi */ uint8_t reg, orig, val, res; reg = ((insn & 0x00F0) >> 4) + 16; val = ((insn & 0x0F00) >> 4) | (insn & 0x000F); orig = cpu.r[reg]; res = orig - val; lazy_flags_setup(INSN_CPI, orig, val, res, SREG_H | SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } } switch (insn & 0xFE0F) { case 0x9400: { /* com */ uint8_t reg, res; reg = (insn & 0x01F0) >> 4; res = ~(cpu.r[reg]); cpu.r[reg] = res; lazy_flags_setup(INSN_COM, LAZY_NONE, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x9401: { /* neg */ uint8_t reg, orig, res; reg = (insn & 0x01F0) >> 4; orig = cpu.r[reg]; res = (uint8_t)0x00 - orig; cpu.r[reg] = res; lazy_flags_setup(INSN_NEG, orig, LAZY_NONE, res, SREG_H | SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x940a: { /* dec */ uint8_t reg, orig, res; reg = (insn & 0x01F0) >> 4; orig = cpu.r[reg]; res = orig - 1; cpu.r[reg] = res; lazy_flags_setup(INSN_DEC, orig, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z); cpu.pc++; cpu.cycles_count++; continue; } case 0x9403: { /* inc */ uint8_t reg, orig, res; reg = (insn & 0x01F0) >> 4; orig = cpu.r[reg]; res = orig + 1; cpu.r[reg] = res; lazy_flags_setup(INSN_INC, orig, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z); cpu.pc++; cpu.cycles_count++; continue; } case 0x9402: { /* swap */ uint8_t reg, orig; reg = (insn & 0x01F0) >> 4; orig = cpu.r[reg]; cpu.r[reg] = (orig >> 4) | (orig << 4); cpu.pc++; cpu.cycles_count++; continue; } case 0x9406: { /* lsr */ uint8_t reg, orig, res; reg = (insn & 0x01F0) >> 4; orig = cpu.r[reg]; res = orig >> 1; cpu.r[reg] = res; lazy_flags_setup(INSN_LSR, orig, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x9407: { /* ror */ uint8_t reg, orig, res; reg = (insn & 0x01F0) >> 4; orig = cpu.r[reg]; res = (orig >> 1) | ((uint8_t)sreg_get_C() << 7); cpu.r[reg] = res; lazy_flags_setup(INSN_ROR, orig, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x9006: /* elpm Z */ fault("ELPM Z is not implemented");//TODO continue; case 0x9007: /* elpm Z+ */ fault("ELPM Z+ is not implemented");//TODO continue; case 0x900c: { /* ld X */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t x; x = cpu.r[X_LO]; if (CHECK(x >= memory.size)) fault("LD X pointer outside of memory region"); cpu.r[reg] = memory_load(x); } else { uint16_t x; x = getX(); if (CHECK(x >= memory.size)) fault("LD X pointer outside of memory region"); cpu.r[reg] = memory_load(x); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x900d: { /* ld X+ */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t x; x = cpu.r[X_LO]; if (CHECK(x >= memory.size)) fault("LD X+ pointer outside of memory region"); cpu.r[reg] = memory_load(x); cpu.r[X_LO]++; } else { uint16_t x; x = getX(); if (CHECK(x >= memory.size)) fault("LD X+ pointer outside of memory region"); cpu.r[reg] = memory_load(x); setX(x + 1); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x900e: { /* ld -X */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t x; x = cpu.r[X_LO] - 1; if (CHECK(x >= memory.size)) fault("LD -X pointer outside of memory region"); cpu.r[reg] = memory_load(x); cpu.r[X_LO] = x; } else { uint16_t x; x = getX() - 1; if (CHECK(x >= memory.size)) fault("LD -X pointer outside of memory region"); cpu.r[reg] = memory_load(x); setX(x); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x9009: { /* ld Y+ */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t y; y = cpu.r[Y_LO]; if (CHECK(y >= memory.size)) fault("LD Y+ pointer outside of memory region"); cpu.r[reg] = memory_load(y); cpu.r[Y_LO]++; } else { uint16_t y; y = getY(); if (CHECK(y >= memory.size)) fault("LD Y+ pointer outside of memory region"); cpu.r[reg] = memory_load(y); setY(y + 1); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x900a: { /* ld -Y */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t y; y = cpu.r[Y_LO] - 1; if (CHECK(y >= memory.size)) fault("LD -Y pointer outside of memory region"); cpu.r[reg] = memory_load(y); cpu.r[Y_LO] = y; } else { uint16_t y; y = getY() - 1; if (CHECK(y >= memory.size)) fault("LD -Y pointer outside of memory region"); cpu.r[reg] = memory_load(y); setY(y); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x9001: { /* ld Z+ */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t z; z = cpu.r[Z_LO]; if (CHECK(z >= memory.size)) fault("LD Z+ pointer outside of memory region"); cpu.r[reg] = memory_load(z); cpu.r[Z_LO]++; } else { uint16_t z; z = getZ(); if (CHECK(z >= memory.size)) fault("LD Z+ pointer outside of memory region"); cpu.r[reg] = memory_load(z); setZ(z + 1); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x9002: { /* ld -Z */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t z; z = cpu.r[Z_LO] - 1; if (CHECK(z >= memory.size)) fault("LD -Z pointer outside of memory region"); cpu.r[reg] = memory_load(z); cpu.r[Z_LO] = z; } else { uint16_t z; z = getZ() - 1; if (CHECK(z >= memory.size)) fault("LD -Z pointer outside of memory region"); cpu.r[reg] = memory_load(z); setZ(z); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x9004: { /* lpm Z */ uint8_t reg; uint16_t z; reg = (insn & 0x01F0) >> 4; z = getZ(); if (CHECK(z >= active_setup.flash_size)) fault("LPM Z pointer outside of flash region"); cpu.r[reg] = code[z]; cpu.pc++; cpu.cycles_count += 3; continue; } case 0x9005: { /* lpm Z+ */ uint8_t reg; uint16_t z; reg = (insn & 0x01F0) >> 4; z = getZ(); if (CHECK(z >= active_setup.flash_size)) fault("LPM Z+ pointer outside of flash region"); cpu.r[reg] = code[z]; setZ(z + 1); cpu.pc++; cpu.cycles_count += 3; continue; } case 0x920c: { /* st X */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t x; x = cpu.r[X_LO]; if (CHECK(x >= memory.size)) fault("ST X pointer outside of memory region"); memory_store(x, cpu.r[reg]); } else { uint16_t x; x = getX(); if (CHECK(x >= memory.size)) fault("ST X pointer outside of memory region"); memory_store(x, cpu.r[reg]); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x920d: { /* st X+ */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t x; x = cpu.r[X_LO]; if (CHECK(x >= memory.size)) fault("ST X+ pointer outside of memory region"); memory_store(x, cpu.r[reg]); cpu.r[X_LO]++; } else { uint16_t x; x = getX(); if (CHECK(x >= memory.size)) fault("ST X+ pointer outside of memory region"); memory_store(x, cpu.r[reg]); setX(x + 1); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x920e: { /* st -X */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t x; x = cpu.r[X_LO] - 1; if (CHECK(x >= memory.size)) fault("ST -X pointer outside of memory region"); memory_store(x, cpu.r[reg]); cpu.r[X_LO] = x; } else { uint16_t x; x = getX() - 1; if (CHECK(x >= memory.size)) fault("ST -X pointer outside of memory region"); memory_store(x, cpu.r[reg]); setX(x); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x9209: { /* st Y+ */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t y; y = cpu.r[Y_LO]; if (CHECK(y >= memory.size)) fault("ST Y+ pointer outside of memory region"); memory_store(y, cpu.r[reg]); cpu.r[Y_LO]++; } else { uint16_t y; y = getY(); if (CHECK(y >= memory.size)) fault("ST Y+ pointer outside of memory region"); memory_store(y, cpu.r[reg]); setY(y + 1); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x920a: { /* st -Y */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t y; y = cpu.r[Y_LO] - 1; if (CHECK(y >= memory.size)) fault("ST -Y pointer outside of memory region"); memory_store(y, cpu.r[reg]); cpu.r[Y_LO] = y; } else { uint16_t y; y = getY() - 1; if (CHECK(y >= memory.size)) fault("ST -Y pointer outside of memory region"); memory_store(y, cpu.r[reg]); setY(y); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x9201: { /* st Z+ */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t z; z = cpu.r[Z_LO]; if (CHECK(z >= memory.size)) fault("ST Z+ pointer outside of memory region"); memory_store(z, cpu.r[reg]); cpu.r[Z_LO]++; } else { uint16_t z; z = getZ(); if (CHECK(z >= memory.size)) fault("ST Z+ pointer outside of memory region"); memory_store(z, cpu.r[reg]); setZ(z + 1); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x9202: { /* st -Z */ uint8_t reg; reg = (insn & 0x01F0) >> 4; if (memory.size <= 256) { uint8_t z; z = cpu.r[Z_LO] - 1; if (CHECK(z >= memory.size)) fault("ST -Z pointer outside of memory region"); memory_store(z, cpu.r[reg]); cpu.r[Z_LO] = z; } else { uint16_t z; z = getZ() - 1; if (CHECK(z >= memory.size)) fault("ST -Z pointer outside of memory region"); memory_store(z, cpu.r[reg]); setZ(z); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x900f: { /* pop */ uint8_t reg; uint16_t sp; reg = (insn & 0x01F0) >> 4; sp = getSP() + 1; setSP(sp); if (CHECK(sp >= memory.size)) fault("POP stack pointer out of memory region"); cpu.r[reg] = memory_load_nonio(sp); cpu.pc++; cpu.cycles_count += 2; continue; } case 0x920f: { /* push */ uint8_t reg; uint16_t sp; reg = (insn & 0x01F0) >> 4; sp = getSP(); if (CHECK(sp >= memory.size)) fault("PUSH stack pointer out of memory region"); memory_store_nonio(sp, cpu.r[reg]); setSP(sp - 1); cpu.pc++; cpu.cycles_count += 2; continue; } case 0x9405: { /* asr */ uint8_t reg, orig, res; reg = (insn & 0x01F0) >> 4; orig = cpu.r[reg]; res = (orig >> 1) | (orig & 0x80); cpu.r[reg] = res; lazy_flags_setup(INSN_ASR, orig, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x9000: { /* lds (32-bit instruction) */ uint8_t reg; uint16_t offset; reg = (insn & 0x01F0) >> 4; offset = get_hi16_insn(); if (CHECK(offset >= memory.size)) fault("LDS offset >= memory size"); cpu.r[reg] = memory_load(offset); cpu.pc += 2; cpu.cycles_count += 2; continue; } case 0x9200: { /* sts (32-bit instruction) */ uint8_t reg; uint16_t offset; reg = (insn & 0x01F0) >> 4; offset = get_hi16_insn(); if (CHECK(offset >= memory.size)) fault("STS offset >= memory size"); memory_store(offset, cpu.r[reg]); cpu.pc += 2; cpu.cycles_count += 2; continue; } } switch (insn & 0xFE0E) { case 0x940c: { /* jmp (32-bit instruction) */ uint16_t hi16_insn; uint32_t target; hi16_insn = get_hi16_insn(); target = hi16_insn | ((insn & 0x0001) << 16) | ((insn & 0x01F0) << (17 - 4)); cpu.pc = target; cpu.cycles_count += 3; continue; } case 0x940e: { /* call (32-bit instruction) */ uint16_t hi16_insn; uint32_t target; hi16_insn = get_hi16_insn(); target = hi16_insn | ((insn & 0x0001) << 16) | ((insn & 0x01F0) << (17 - 4)); insn_store_pc_on_stack(active_setup.pc_22bit, 2); cpu.pc = target; cpu.cycles_count += 4; continue; } } switch (insn & 0xD208) { case 0x8200: { /* std Z */ uint8_t reg, displace; reg = (insn & 0x01F0) >> 4; displace = (insn & 0x0007) | ((insn & 0x0C00) >> 7) | ((insn & 0x2000) >> 8); if (memory.size <= 256) { uint8_t z; z = cpu.r[Z_LO] + displace; if (CHECK(z >= memory.size)) fault("STD(ST) Z pointer outside of memory region"); memory_store(z, cpu.r[reg]); } else { uint16_t z; z = getZ() + displace; if (CHECK(z >= memory.size)) fault("STD(ST) Z pointer outside of memory region"); memory_store(z, cpu.r[reg]); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x8208: { /* std Y */ uint8_t reg, displace; reg = (insn & 0x01F0) >> 4; displace = (insn & 0x0007) | ((insn & 0x0C00) >> 7) | ((insn & 0x2000) >> 8); if (memory.size <= 256) { uint8_t y; y = cpu.r[Y_LO] + displace; if (CHECK(y >= memory.size)) fault("STD(ST) Y pointer outside of memory region"); memory_store(y, cpu.r[reg]); } else { uint16_t y; y = getY() + displace; if (CHECK(y >= memory.size)) fault("STD(ST) Y pointer outside of memory region"); memory_store(y, cpu.r[reg]); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x8000: { /* ldd Z */ uint8_t reg, displace; reg = (insn & 0x01F0) >> 4; displace = (insn & 0x0007) | ((insn & 0x0C00) >> 7) | ((insn & 0x2000) >> 8); if (memory.size <= 256) { uint8_t z; z = cpu.r[Z_LO] + displace; if (CHECK(z >= memory.size)) fault("LDD(LD) Z pointer outside of memory region"); cpu.r[reg] = memory_load(z); } else { uint16_t z; z = getZ() + displace; if (CHECK(z >= memory.size)) fault("LDD(LD) Z pointer outside of memory region"); cpu.r[reg] = memory_load(z); } cpu.pc++; cpu.cycles_count += 2; continue; } case 0x8008: { /* ldd Y */ uint8_t reg, displace; reg = (insn & 0x01F0) >> 4; displace = (insn & 0x0007) | ((insn & 0x0C00) >> 7) | ((insn & 0x2000) >> 8); if (memory.size <= 256) { uint8_t y; y = cpu.r[Y_LO] + displace; if (CHECK(y >= memory.size)) fault("LDD(LD) Y pointer outside of memory region"); cpu.r[reg] = memory_load(y); } else { uint16_t y; y = getY() + displace; if (CHECK(y >= memory.size)) fault("LDD(LD) Y pointer outside of memory region"); cpu.r[reg] = memory_load(y); } cpu.pc++; cpu.cycles_count += 2; continue; } } switch (insn & 0xFF00) { case 0x0200: /* muls */ fault("MULS is not implemented");//TODO continue; case 0x0100: { /* movw */ uint8_t src, dst; src = ((insn & 0x000F) * 2); dst = ((insn & 0x00F0) >> 4) * 2; cpu.r[dst] = cpu.r[src]; cpu.r[dst + 1] = cpu.r[src + 1]; cpu.pc++; cpu.cycles_count++; continue; } case 0x9600: { /* adiw */ uint8_t reg; uint16_t orig, val, res; reg = (((insn & 0x0030) >> 4) * 2) + 24; val = ((insn & 0x000F) | ((insn & 0x00C0) >> 2)); orig = le16_to_cpu(*((le16_t *)&(cpu.r[reg]))); res = orig + val; *((le16_t *)&(cpu.r[reg])) = cpu_to_le16(res); lazy_flags_setup(INSN_ADIW, orig, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count += 2; continue; } case 0x9800: { /* cbi */ uint8_t io, bit, old_val, new_val; bit = (insn & 0x0007); io = (insn & 0x00F8) >> 3; pthread_spin_lock(&memory.io_lock); old_val = memory.io[io]; new_val = old_val & ~(1 << bit); memory.io[io] = new_val; memory_store_sideeffect_trigger(io + IO_MEM_OFFSET, old_val, new_val); pthread_spin_unlock(&memory.io_lock); cpu.pc++; cpu.cycles_count += 2; continue; } case 0x9a00: { /* sbi */ uint8_t io, bit, old_val, new_val; bit = (insn & 0x0007); io = (insn & 0x00F8) >> 3; pthread_spin_lock(&memory.io_lock); old_val = memory.io[io]; new_val = old_val | (1 << bit); memory.io[io] = new_val; memory_store_sideeffect_trigger(io + IO_MEM_OFFSET, old_val, new_val); pthread_spin_unlock(&memory.io_lock); cpu.pc++; cpu.cycles_count += 2; continue; } case 0x9900: { /* sbic */ uint8_t io, bit; bit = (insn & 0x0007); io = (insn & 0x00F8) >> 3; if (memory.io[io] & (1 << bit)) { cpu.pc++; cpu.cycles_count++; } else { uint16_t next_insn = get_hi16_insn(); /* If the next insn is 32bit we skip two words. */ if (is_two_word_instruction(next_insn)) { cpu.pc += 3; cpu.cycles_count += 3; } else { cpu.pc += 2; cpu.cycles_count += 2; } } continue; } case 0x9b00: { /* sbis */ uint8_t io, bit; bit = (insn & 0x0007); io = (insn & 0x00F8) >> 3; if (!(memory.io[io] & (1 << bit))) { cpu.pc++; cpu.cycles_count++; } else { uint16_t next_insn = get_hi16_insn(); /* If the next insn is 32bit we skip two words. */ if (is_two_word_instruction(next_insn)) { cpu.pc += 3; cpu.cycles_count += 3; } else { cpu.pc += 2; cpu.cycles_count += 2; } } continue; } case 0x9700: { /* sbiw */ uint8_t reg; uint16_t orig, val, res; reg = (((insn & 0x0030) >> 4) * 2) + 24; val = ((insn & 0x000F) | ((insn & 0x00C0) >> 2)); orig = le16_to_cpu(*((le16_t *)&(cpu.r[reg]))); res = orig - val; *((le16_t *)&(cpu.r[reg])) = cpu_to_le16(res); lazy_flags_setup(INSN_SBIW, orig, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count += 2; continue; } } switch (insn & 0xFC00) { case 0x0400: { /* cpc */ uint8_t reg_d, reg_r, rd, rr, res; reg_d = (insn & 0x01F0) >> 4; reg_r = (insn & 0x000F) | ((insn & 0x0200) >> 5); rd = cpu.r[reg_d]; rr = cpu.r[reg_r]; res = rd - rr - (uint8_t)sreg_get_C(); lazy_flags_setup(INSN_CPC, rd, rr, res, SREG_H | SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x0800: { /* sbc */ uint8_t reg_d, reg_r, rd, rr, res; reg_d = (insn & 0x01F0) >> 4; reg_r = (insn & 0x000F) | ((insn & 0x0200) >> 5); rd = cpu.r[reg_d]; rr = cpu.r[reg_r]; res = rd - rr - (uint8_t)sreg_get_C(); cpu.r[reg_d] = res; lazy_flags_setup(INSN_SBC, rd, rr, res, SREG_H | SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x0c00: { /* add */ uint8_t reg_d, reg_r, rd, rr, res; reg_d = (insn & 0x01F0) >> 4; reg_r = (insn & 0x000F) | ((insn & 0x0200) >> 5); rd = cpu.r[reg_d]; rr = cpu.r[reg_r]; res = rd + rr; cpu.r[reg_d] = res; lazy_flags_setup(INSN_ADD, rd, rr, res, SREG_H | SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x1000: { /* cpse */ uint8_t reg_d, reg_r, rd, rr; reg_d = (insn & 0x01F0) >> 4; reg_r = (insn & 0x000F) | ((insn & 0x0200) >> 5); rd = cpu.r[reg_d]; rr = cpu.r[reg_r]; if (rd != rr) { cpu.pc++; cpu.cycles_count++; } else { uint16_t next_insn = get_hi16_insn(); /* If the next insn is 32bit we skip two words. */ if (is_two_word_instruction(next_insn)) { cpu.pc += 3; cpu.cycles_count += 3; } else { cpu.pc += 2; cpu.cycles_count += 2; } } continue; } case 0x1400: { /* cp */ uint8_t reg_d, reg_r, rd, rr, res; reg_d = (insn & 0x01F0) >> 4; reg_r = (insn & 0x000F) | ((insn & 0x0200) >> 5); rd = cpu.r[reg_d]; rr = cpu.r[reg_r]; res = rd - rr; lazy_flags_setup(INSN_CP, rd, rr, res, SREG_H | SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x1800: { /* sub */ uint8_t reg_d, reg_r, rd, rr, res; reg_d = (insn & 0x01F0) >> 4; reg_r = (insn & 0x000F) | ((insn & 0x0200) >> 5); rd = cpu.r[reg_d]; rr = cpu.r[reg_r]; res = rd - rr; cpu.r[reg_d] = res; lazy_flags_setup(INSN_SUB, rd, rr, res, SREG_H | SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x1c00: { /* adc */ uint8_t reg_d, reg_r, rd, rr, res; reg_d = (insn & 0x01F0) >> 4; reg_r = (insn & 0x000F) | ((insn & 0x0200) >> 5); rd = cpu.r[reg_d]; rr = cpu.r[reg_r]; res = rd + rr + (uint8_t)sreg_get_C(); cpu.r[reg_d] = res; lazy_flags_setup(INSN_ADC, rd, rr, res, SREG_H | SREG_S | SREG_V | SREG_N | SREG_Z | SREG_C); cpu.pc++; cpu.cycles_count++; continue; } case 0x2000: { /* and */ uint8_t reg_d, reg_r, rd, rr, res; reg_d = (insn & 0x01F0) >> 4; reg_r = (insn & 0x000F) | ((insn & 0x0200) >> 5); rd = cpu.r[reg_d]; rr = cpu.r[reg_r]; res = rd & rr; cpu.r[reg_d] = res; lazy_flags_setup(INSN_AND, LAZY_NONE, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z); cpu.pc++; cpu.cycles_count++; continue; } case 0x2400: { /* eor */ uint8_t reg_d, reg_r, rd, rr, res; reg_d = (insn & 0x01F0) >> 4; reg_r = (insn & 0x000F) | ((insn & 0x0200) >> 5); rd = cpu.r[reg_d]; rr = cpu.r[reg_r]; res = rd ^ rr; cpu.r[reg_d] = res; lazy_flags_setup(INSN_EOR, LAZY_NONE, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z); cpu.pc++; cpu.cycles_count++; continue; } case 0x2800: { /* or */ uint8_t reg_d, reg_r, rd, rr, res; reg_d = (insn & 0x01F0) >> 4; reg_r = (insn & 0x000F) | ((insn & 0x0200) >> 5); rd = cpu.r[reg_d]; rr = cpu.r[reg_r]; res = rd | rr; cpu.r[reg_d] = res; lazy_flags_setup(INSN_OR, LAZY_NONE, LAZY_NONE, res, SREG_S | SREG_V | SREG_N | SREG_Z); cpu.pc++; cpu.cycles_count++; continue; } case 0x2c00: { /* mov */ uint8_t reg_d, reg_r; reg_d = (insn & 0x01F0) >> 4; reg_r = (insn & 0x000F) | ((insn & 0x0200) >> 5); cpu.r[reg_d] = cpu.r[reg_r]; cpu.pc++; cpu.cycles_count++; continue; } case 0x9c00: /* mul */ fault("MUL is not implemented");//TODO continue; case 0xf400: { /* brbc */ uint8_t bit; int8_t offset = 0; bool set; bit = (insn & 0x0007); if (insn & 0x0200) offset = ~0x007F; offset |= (insn & 0x03F8) >> 3; switch (bit) { case SREG_C_BIT: set = sreg_get_C(); break; case SREG_Z_BIT: set = sreg_get_Z(); break; case SREG_N_BIT: set = sreg_get_N(); break; case SREG_V_BIT: set = sreg_get_V(); break; case SREG_S_BIT: set = sreg_get_S(); break; case SREG_H_BIT: set = sreg_get_H(); break; case SREG_T_BIT: set = sreg_get_T(); break; case SREG_I_BIT: set = sreg_get_I(); break; default: fault("BRBC internal emulator bug"); } if (set) { cpu.pc++; cpu.cycles_count++; } else { cpu.pc += offset + 1; cpu.cycles_count += 2; } continue; } case 0xf000: { /* brbs */ uint8_t bit; int8_t offset = 0; bool set; bit = (insn & 0x0007); if (insn & 0x0200) offset = ~0x007F; offset |= (insn & 0x03F8) >> 3; switch (bit) { case SREG_C_BIT: set = sreg_get_C(); break; case SREG_Z_BIT: set = sreg_get_Z(); break; case SREG_N_BIT: set = sreg_get_N(); break; case SREG_V_BIT: set = sreg_get_V(); break; case SREG_S_BIT: set = sreg_get_S(); break; case SREG_H_BIT: set = sreg_get_H(); break; case SREG_T_BIT: set = sreg_get_T(); break; case SREG_I_BIT: set = sreg_get_I(); break; default: fault("BRBS internal emulator bug"); } if (set) { cpu.pc += offset + 1; cpu.cycles_count += 2; } else { cpu.pc++; cpu.cycles_count++; } continue; } } switch (insn & 0xFE08) { case 0xf800: { /* bld */ uint8_t reg, bit, val; reg = (insn & 0x01F0) >> 4; bit = (insn & 0x0007); val = (uint8_t)sreg_get_T() << bit; cpu.r[reg] &= ~(1 << bit); cpu.r[reg] |= val; cpu.pc++; cpu.cycles_count++; continue; } case 0xfa00: { /* bst */ uint8_t reg, bit; reg = (insn & 0x01F0) >> 4; bit = (insn & 0x0007); sreg_set_T(!!(cpu.r[reg] & (1 << bit))); cpu.pc++; cpu.cycles_count++; continue; } case 0xfc00: { /* sbrc */ uint8_t reg, bit; reg = (insn & 0x01F0) >> 4; bit = (insn & 0x0007); if (cpu.r[reg] & (1 << bit)) { cpu.pc++; cpu.cycles_count++; } else { uint16_t next_insn = get_hi16_insn(); /* If the next insn is 32bit we skip two words. */ if (is_two_word_instruction(next_insn)) { cpu.pc += 3; cpu.cycles_count += 3; } else { cpu.pc += 2; cpu.cycles_count += 2; } } continue; } case 0xfe00: { /* sbrs */ uint8_t reg, bit; reg = (insn & 0x01F0) >> 4; bit = (insn & 0x0007); if (!(cpu.r[reg] & (1 << bit))) { cpu.pc++; cpu.cycles_count++; } else { uint16_t next_insn = get_hi16_insn(); /* If the next insn is 32bit we skip two words. */ if (is_two_word_instruction(next_insn)) { cpu.pc += 3; cpu.cycles_count += 3; } else { cpu.pc += 2; cpu.cycles_count += 2; } } continue; } } switch (insn & 0xF800) { case 0xb000: { /* in */ uint8_t reg, io; reg = (insn & 0x01F0) >> 4; io = (insn & 0x000F) | ((insn & 0x0600) >> 5); pthread_spin_lock(&memory.io_lock); memory_load_sideeffect_trigger(io + IO_MEM_OFFSET); cpu.r[reg] = memory.io[io]; pthread_spin_unlock(&memory.io_lock); cpu.pc++; cpu.cycles_count++; continue; } case 0xb800: { /* out */ uint8_t reg, io, old_val, new_val; reg = (insn & 0x01F0) >> 4; io = (insn & 0x000F) | ((insn & 0x0600) >> 5); pthread_spin_lock(&memory.io_lock); old_val = memory.io[io]; new_val = cpu.r[reg]; memory.io[io] = new_val; memory_store_sideeffect_trigger(io + IO_MEM_OFFSET, old_val, new_val); pthread_spin_unlock(&memory.io_lock); cpu.pc++; cpu.cycles_count++; continue; } } switch (insn & 0xFF88) { case 0x0300: /* mulsu */ fault("MULSU is not implemented");//TODO continue; case 0x0388: /* fmulsu */ fault("FMULSU is not implemented");//TODO continue; case 0x0380: /* fmuls */ fault("FMULS is not implemented");//TODO continue; case 0x0308: /* fmul */ fault("FMUL is not implemented");//TODO continue; } fault("Unknown instruction 0x%04X\n", insn); }; return NULL; } static void cpu_io_write_sreg(uint16_t offset, uint8_t old_val, uint8_t new_val) { cpu.sreg = new_val; cpu.sreg_valid = 0xFF; } static void cpu_io_read_sreg(uint16_t offset) { memory.ram[offset] = cpu_get_sreg(); } static int register_cpu_io_handlers(void) { int err = 0; /* Register I/O handlers for CPU related I/O registers. */ err |= register_io_write_handler(cpu_io_write_sreg, IO_SREG); err |= register_io_read_handler(cpu_io_read_sreg, IO_SREG); if (err) return -1; return 0; } static int cpu_interrupts_init(void) { int i, err; struct avr_irq_descriptor *desc, *cache; INIT_LIST_HEAD(&cpu.irqdesc_pending); INIT_LIST_HEAD(&cpu.irqdesc_cache); err = pthread_spin_init(&cpu.irq_lock, 0); if (err) return err; err = -ENOMEM; cache = avr_malloc(sizeof(*cache) * MAX_IRQS_PENDING); if (!cache) goto err_spin_destroy; cpu.irq_cache_p = cache; for (i = 0; i < MAX_IRQS_PENDING; i++) { desc = &(cache[i]); INIT_LIST_HEAD(&desc->list); list_add(&desc->list, &cpu.irqdesc_cache); } return 0; err_spin_destroy: pthread_spin_destroy(&cpu.irq_lock); return err; } static void cpu_interrupts_exit(void) { free(cpu.irq_cache_p); pthread_spin_destroy(&cpu.irq_lock); } int cpu_initialize(const char *bin, size_t bin_size) { if (bin_size > active_setup.flash_size) { fprintf(stderr, "Code is bigger than the " "microcontroller flash size.\n"); return -EINVAL; } code = avr_malloc(active_setup.flash_size); if (!code) return -ENOMEM; memset(code, 0xFF, active_setup.flash_size); memcpy(code, bin, bin_size); cpu.initialized = 1; return 0; } static void cpu_cleanup(void) { if (cpu.running) trap_cpu(CPU_TRAP_EXIT, 1); if (cpu.initialized) { cpu.initialized = 0; cpu_interrupts_exit(); pthread_cond_destroy(&cpu.trapped_cond); pthread_mutex_destroy(&cpu.trap_mutex); } } int cpu_reset(void) { int err; cpu_cleanup(); memset(&cpu, 0, sizeof(cpu)); /* The application program will initialize the stack pointer. * So we leave it zero. */ /* Registers are mapped to the start of MMIO space. */ cpu.r = memory.ram; /* We start at the reset vector. */ cpu.pc = AVR_VECTOR_RESET; /* Set the breakpoint so that it never triggers. */ cpu.breakpoint = ~0; /* Init lazy flags. Set all to valid zeros. */ cpu.sreg = 0; cpu.sreg_valid = 0xFF; memset(&cpu.lazy_flag_cache, 0, sizeof(cpu.lazy_flag_cache)); /* Initialize the CPU trap. */ cpu.trap = CPU_TRAP_CONTINUE; err = pthread_mutex_init(&cpu.trap_mutex, NULL); if (err) goto out_error; err = pthread_cond_init(&cpu.trapped_cond, NULL); if (err) goto err_destr_trap; err = register_cpu_io_handlers(); if (err) goto err_destr_cond; err = cpu_interrupts_init(); if (err) goto err_destr_cond; /* Fire up the CPU! */ cpu.running = 1; err = pthread_create(&cpu_thread, NULL, cpu_thread_function, NULL); if (err) { cpu.running = 0; perror("Failed to start CPU thread"); goto err_irq_exit; } return 0; err_irq_exit: cpu_interrupts_exit(); err_destr_cond: pthread_cond_destroy(&cpu.trapped_cond); err_destr_trap: pthread_mutex_destroy(&cpu.trap_mutex); out_error: fprintf(stderr, "Failed to initialize the CPU data structure.\n"); return -ENOMEM; } void cpu_exit(void) { cpu_cleanup(); free(code); code = NULL; }