06c0981b53018ff1b28f10793f20fb70a56aa626

Parts with more than 128KB of flash need an extra byte on the stack
Rework from nnayo branch for the atmega2560

Signed-off-by: Michel Pollet <buserror@gmail.com>

diff --git a/simavr/sim/sim_avr.c b/simavr/sim/sim_avr.c
index 8eaee52223c72a9813a675c799852c09444a7ea3..b47bd7229e67024daceef524bce7de6e5aaa440b 100644 (file)
--- a/simavr/sim/sim_avr.c
+++ b/simavr/sim/sim_avr.c
@@ -90,6 +90,8 @@ int avr_init(avr_t * avr)
        avr->run = avr_callback_run_raw;
        avr->sleep = avr_callback_sleep_raw;
        avr->state = cpu_Running;
+       // number of address bytes to push/pull on/off the stack
+       avr->address_size = avr->eind ? 3 : 2;
        avr->log = 1;
        avr_reset(avr); 
        return 0;

diff --git a/simavr/sim/sim_avr.h b/simavr/sim/sim_avr.h
index 848773eccb01be463f2273c1bc0d4d3c219c3be5..1e393a2ffffc5fb110d2aa186a77fa0e4c1152c2 100644 (file)
--- a/simavr/sim/sim_avr.h
+++ b/simavr/sim/sim_avr.h
@@ -147,7 +147,8 @@ typedef struct avr_t {
        uint8_t         fuse[4];
        avr_io_addr_t   rampz;  // optional, only for ELPM/SPM on >64Kb cores
        avr_io_addr_t   eind;   // optional, only for EIJMP/EICALL on >64Kb cores
-
+       uint8_t         address_size;   // 2, or 3 for cores >128KB in flash
+       
        // filled by the ELF data, this allow tracking of invalid jumps
        uint32_t                        codeend;
 

diff --git a/simavr/sim/sim_core.c b/simavr/sim/sim_core.c
index 38ba869a1b669a3bfc0a6ab1559bf9f28384296d..f9f269579b19e6f642a2213e9c0ab0463ae6bd8e 100644 (file)
--- a/simavr/sim/sim_core.c
+++ b/simavr/sim/sim_core.c
@@ -241,7 +241,7 @@ static inline uint8_t _avr_get_ram(avr_t * avr, uint16_t addr)
 }
 
 /*
- * Stack push accessors. Push/pop 8 and 16 bits
+ * Stack push accessors.
  */
 static inline void _avr_push8(avr_t * avr, uint16_t v)
 {
@@ -258,16 +258,26 @@ static inline uint8_t _avr_pop8(avr_t * avr)
        return res;
 }
 
-inline void _avr_push16(avr_t * avr, uint16_t v)
+int _avr_push_addr(avr_t * avr, avr_flashaddr_t addr)
 {
-       _avr_push8(avr, v);
-       _avr_push8(avr, v >> 8);
+       uint16_t sp = _avr_sp_get(avr);
+       addr >>= 1;
+       for (int i = 0; i < avr->address_size; i++, addr >>= 8, sp--) {
+               _avr_set_ram(avr, sp, addr);    
+       }
+       _avr_sp_set(avr, sp);
+       return avr->address_size;
 }
 
-static inline uint16_t _avr_pop16(avr_t * avr)
+avr_flashaddr_t _avr_pop_addr(avr_t * avr)
 {
-       uint16_t res = _avr_pop8(avr) << 8;
-       res |= _avr_pop8(avr);
+       uint16_t sp = _avr_sp_get(avr) + 1;
+       avr_flashaddr_t res = 0;
+       for (int i = 0; i < avr->address_size; i++, sp++) {
+               res = (res << 8) | _avr_get_ram(avr, sp);
+       }
+       res <<= 1;
+       _avr_sp_set(avr, sp -1);
        return res;
 }
 
@@ -873,20 +883,18 @@ avr_flashaddr_t avr_run_one(avr_t * avr)
                                        if (e)
                                                z |= avr->data[avr->eind] << 16;
                                        STATE("%si%s Z[%04x]\n", e?"e":"", p?"call":"jmp", z << 1);
-                                       if (p) {
-                                               cycle++;
-                                               _avr_push16(avr, new_pc >> 1);
-                                       }
+                                       if (p)
+                                               cycle += _avr_push_addr(avr, new_pc) - 1;
                                        new_pc = z << 1;
                                        cycle++;
                                        TRACE_JUMP();
                                }       break;
                                case 0x9518:    // RETI
                                case 0x9508: {  // RET
-                                       new_pc = _avr_pop16(avr) << 1;
+                                       new_pc = _avr_pop_addr(avr);
+                                       cycle += 1 + avr->address_size;
                                        if (opcode & 0x10)      // reti
                                                avr->sreg[S_I] = 1;
-                                       cycle += 3;
                                        STATE("ret%s\n", opcode & 0x10 ? "i" : "");
                                        TRACE_JUMP();
                                        STACK_FRAME_POP();
@@ -1174,9 +1182,8 @@ avr_flashaddr_t avr_run_one(avr_t * avr)
                                                        a = (a << 16) | x;
                                                        STATE("call 0x%06x\n", a);
                                                        new_pc += 2;
-                                                       _avr_push16(avr, new_pc >> 1);
+                                                       cycle += 1 + _avr_push_addr(avr, new_pc);
                                                        new_pc = a << 1;
-                                                       cycle += 3;     // 4 cycles; FIXME 5 on devices with 22 bit PC
                                                        TRACE_JUMP();
                                                        STACK_FRAME_PUSH();
                                                }       break;
@@ -1312,11 +1319,10 @@ avr_flashaddr_t avr_run_one(avr_t * avr)
                case 0xd000: {
                        // RCALL 1100 kkkk kkkk kkkk
 //                     int16_t o = ((int16_t)(opcode << 4)) >> 4; // CLANG BUG!
-                       int16_t o = ((int16_t)((opcode << 4)&0xffff)) >> 4;
+                       int16_t o = ((int16_t)((opcode << 4) & 0xffff)) >> 4;
                        STATE("rcall .%d [%04x]\n", o, new_pc + (o << 1));
-                       _avr_push16(avr, new_pc >> 1);
+                       cycle += _avr_push_addr(avr, new_pc) - 1;
                        new_pc = new_pc + (o << 1);
-                       cycle += 2;
                        // 'rcall .1' is used as a cheap "push 16 bits of room on the stack"
                        if (o != 0) {
                                TRACE_JUMP();

diff --git a/simavr/sim/sim_core.h b/simavr/sim/sim_core.h
index 9e826b4f63515d76bd76265ecefd9de279fd0e30..15b4c3bf4f5835265fab86d6d6d36cb241de166e 100644 (file)
--- a/simavr/sim/sim_core.h
+++ b/simavr/sim/sim_core.h
@@ -46,7 +46,7 @@ avr_flashaddr_t avr_run_one(avr_t * avr);
  */
 uint16_t _avr_sp_get(avr_t * avr);
 void _avr_sp_set(avr_t * avr, uint16_t sp);
-void _avr_push16(avr_t * avr, uint16_t v);
+int _avr_push_addr(avr_t * avr, avr_flashaddr_t addr);
 
 #if CONFIG_SIMAVR_TRACE
 

diff --git a/simavr/sim/sim_interrupts.c b/simavr/sim/sim_interrupts.c
index 7f21b8a294ef2f5508f25af8023dfd09d4517f42..1e3866c5559cd6135c69501cfb6533e383dac5c2 100644 (file)
--- a/simavr/sim/sim_interrupts.c
+++ b/simavr/sim/sim_interrupts.c
@@ -233,7 +233,7 @@ avr_service_interrupts(
        } else {
                if (vector && vector->trace)
                        printf("%s calling %d\n", __FUNCTION__, (int)vector->vector);
-               _avr_push16(avr, avr->pc >> 1);
+               _avr_push_addr(avr, avr->pc);
                avr->sreg[S_I] = 0;
                avr->pc = vector->vector * avr->vector_size;