git.rot13.org Git - simavr/blob - simavr/sim/sim_core.c

   1 /*
   2         sim_core.c
   3
   4         Copyright 2008, 2009 Michel Pollet <buserror@gmail.com>
   5
   6         This file is part of simavr.
   7
   8         simavr is free software: you can redistribute it and/or modify
   9         it under the terms of the GNU General Public License as published by
  10         the Free Software Foundation, either version 3 of the License, or
  11         (at your option) any later version.
  12
  13         simavr is distributed in the hope that it will be useful,
  14         but WITHOUT ANY WARRANTY; without even the implied warranty of
  15         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16         GNU General Public License for more details.
  17
  18         You should have received a copy of the GNU General Public License
  19         along with simavr.  If not, see <http://www.gnu.org/licenses/>.
  20  */
  21
  22 #include <stdlib.h>
  23 #include <stdio.h>
  24 #include <string.h>
  25 #include <ctype.h>
  26 #include "sim_avr.h"
  27 #include "sim_core.h"
  28
  29 // SREG bit names
  30 const char * _sreg_bit_name = "cznvshti";
  31
  32 /*
  33  * Handle "touching" registers, marking them changed.
  34  * This is used only for debugging purposes to be able to
  35  * print the effects of each instructions on registers
  36  */
  37 #define REG_TOUCH(a, r) (a)->touched[(r) >> 5] |= (1 << ((r) & 0x1f))
  38 #define REG_ISTOUCHED(a, r) ((a)->touched[(r) >> 5] & (1 << ((r) & 0x1f)))
  39
  40 /*
  41  * This allows a "special case" to skip indtruction tracing when in these
  42  * symbols. since printf() is useful to have, but generates a lot of cycles
  43  */
  44 int dont_trace(const char * name)
  45 {
  46         return (
  47                 !strcmp(name, "uart_putchar") ||
  48                 !strcmp(name, "fputc") ||
  49                 !strcmp(name, "printf") ||
  50                 !strcmp(name, "vfprintf") ||
  51                 !strcmp(name, "__ultoa_invert") ||
  52                 !strcmp(name, "__prologue_saves__") ||
  53                 !strcmp(name, "__epilogue_restores__"));
  54 }
  55
  56 int donttrace = 0;
  57
  58 #define STATE(_f, args...) { \
  59         if (avr->trace) {\
  60                 if (avr->codeline[avr->pc>>1]) {\
  61                         const char * symn = avr->codeline[avr->pc>>1]->symbol; \
  62                         int dont = 0 && dont_trace(symn);\
  63                         if (dont!=donttrace) { \
  64                                 donttrace = dont;\
  65                                 DUMP_REG();\
  66                         }\
  67                         if (donttrace==0)\
  68                                 printf("%04x: %-25s " _f, avr->pc, symn, ## args);\
  69                 } else \
  70                         printf("%s: %04x: " _f, __FUNCTION__, avr->pc, ## args);\
  71                 }\
  72         }
  73 #define SREG() if (avr->trace && donttrace == 0) {\
  74         printf("%04x: \t\t\t\t\t\t\t\t\tSREG = ", avr->pc); \
  75         for (int _sbi = 0; _sbi < 8; _sbi++)\
  76                 printf("%c", avr->sreg[_sbi] ? toupper(_sreg_bit_name[_sbi]) : '.');\
  77         printf("\n");\
  78 }
  79
  80 /*
  81  * Set a register (r < 256)
  82  * if it's an IO regisrer (> 31) also (try to) call any callback that was
  83  * registered to track changes to that register.
  84  */
  85 static inline void _avr_set_r(avr_t * avr, uint8_t r, uint8_t v)
  86 {
  87         REG_TOUCH(avr, r);
  88
  89         if (r == R_SREG) {
  90                 avr->data[r] = v;
  91                 // unsplit the SREG
  92                 for (int i = 0; i < 8; i++)
  93                         avr->sreg[i] = (avr->data[R_SREG] & (1 << i)) != 0;
  94                 SREG();
  95         }
  96         if (r > 31) {
  97                 uint8_t io = AVR_DATA_TO_IO(r);
  98                 if (avr->iow[io].w)
  99                         avr->iow[io].w(avr, r, v, avr->iow[io].param);
 100                 else
 101                         avr->data[r] = v;
 102         } else
 103                 avr->data[r] = v;
 104 }
 105
 106 /*
 107  * Stack pointer access
 108  */
 109 inline uint16_t _avr_sp_get(avr_t * avr)
 110 {
 111         return avr->data[R_SPL] | (avr->data[R_SPH] << 8);
 112 }
 113
 114 inline void _avr_sp_set(avr_t * avr, uint16_t sp)
 115 {
 116         _avr_set_r(avr, R_SPL, sp);
 117         _avr_set_r(avr, R_SPH, sp >> 8);
 118 }
 119
 120 /*
 121  * Set any address to a value; split between registers and SRAM
 122  */
 123 static inline void _avr_set_ram(avr_t * avr, uint16_t addr, uint8_t v)
 124 {
 125         if (addr < 256)
 126                 _avr_set_r(avr, addr, v);
 127         else
 128                 avr_core_watch_write(avr, addr, v);
 129 }
 130
 131 /*
 132  * Get a value from SRAM.
 133  */
 134 static inline uint8_t _avr_get_ram(avr_t * avr, uint16_t addr)
 135 {
 136         if (addr > 31 && addr < 256) {
 137                 uint8_t io = AVR_DATA_TO_IO(addr);
 138                 if (avr->ior[io].r)
 139                         avr->data[addr] = avr->ior[io].r(avr, addr, avr->ior[io].param);
 140         }
 141         return avr_core_watch_read(avr, addr);
 142 }
 143
 144 /*
 145  * Stack oush accessors. Push/pop 8 and 16 bits
 146  */
 147 static inline void _avr_push8(avr_t * avr, uint16_t v)
 148 {
 149         uint16_t sp = _avr_sp_get(avr);
 150         _avr_set_ram(avr, sp, v);
 151         _avr_sp_set(avr, sp-1);
 152 }
 153
 154 static inline uint8_t _avr_pop8(avr_t * avr)
 155 {
 156         uint16_t sp = _avr_sp_get(avr) + 1;
 157         uint8_t res = _avr_get_ram(avr, sp);
 158         _avr_sp_set(avr, sp);
 159         return res;
 160 }
 161
 162 inline void _avr_push16(avr_t * avr, uint16_t v)
 163 {
 164         _avr_push8(avr, v >> 8);
 165         _avr_push8(avr, v);
 166 }
 167
 168 static inline uint16_t _avr_pop16(avr_t * avr)
 169 {
 170         uint16_t res = _avr_pop8(avr);
 171         res |= _avr_pop8(avr) << 8;
 172         return res;
 173 }
 174
 175 /*
 176  * "Pretty" register names
 177  */
 178 const char * reg_names[255] = {
 179                 [R_XH] = "XH", [R_XL] = "XL",
 180                 [R_YH] = "YH", [R_YL] = "YL",
 181                 [R_ZH] = "ZH", [R_ZL] = "ZL",
 182                 [R_SPH] = "SPH", [R_SPL] = "SPL",
 183                 [R_SREG] = "SREG",
 184 };
 185
 186
 187 const char * avr_regname(uint8_t reg)
 188 {
 189         if (!reg_names[reg]) {
 190                 char tt[16];
 191                 if (reg < 32)
 192                         sprintf(tt, "r%d", reg);
 193                 else
 194                         sprintf(tt, "io:%02x", reg);
 195                 reg_names[reg] = strdup(tt);
 196         }
 197         return reg_names[reg];
 198 }
 199
 200 /*
 201  * Called when an invalid opcode is decoded
 202  */
 203 static void _avr_invalid_opcode(avr_t * avr)
 204 {
 205         printf("\e[31m*** %04x: %-25s Invalid Opcode SP=%04x O=%04x \e[0m\n",
 206                         avr->pc, avr->codeline[avr->pc>>1]->symbol, _avr_sp_get(avr), avr->flash[avr->pc] | (avr->flash[avr->pc+1]<<8));
 207 }
 208
 209 /*
 210  * Dump changed registers when tracing
 211  */
 212 void avr_dump_state(avr_t * avr)
 213 {
 214         if (!avr->trace || donttrace)
 215                 return;
 216
 217         int doit = 0;
 218
 219         for (int r = 0; r < 3 && !doit; r++)
 220                 if (avr->touched[r])
 221                         doit = 1;
 222         if (!doit)
 223                 return;
 224         printf("                                       ->> ");
 225         const int r16[] = { R_SPL, R_XL, R_YL, R_ZL };
 226         for (int i = 0; i < 4; i++)
 227                 if (REG_ISTOUCHED(avr, r16[i]) || REG_ISTOUCHED(avr, r16[i]+1)) {
 228                         REG_TOUCH(avr, r16[i]);
 229                         REG_TOUCH(avr, r16[i]+1);
 230                 }
 231
 232         for (int i = 0; i < 3*32; i++)
 233                 if (REG_ISTOUCHED(avr, i)) {
 234                         printf("%s=%02x ", avr_regname(i), avr->data[i]);
 235                 }
 236         printf("\n");
 237 }
 238
 239 #define get_r_d_10(o) \
 240                 const uint8_t r = ((o >> 5) & 0x10) | (o & 0xf); \
 241                 const uint8_t d = (o >> 4) & 0x1f;\
 242                 const uint8_t vd = avr->data[d], vr =avr->data[r];
 243 #define get_k_r16(o) \
 244                 const uint8_t r = 16 + ((o >> 4) & 0xf); \
 245                 const uint8_t k = ((o & 0x0f00) >> 4) | (o & 0xf);
 246
 247 /*
 248  * Add a "jump" address to the jump trace buffer
 249  */
 250 #define TRACE_JUMP()\
 251         avr->old[avr->old_pci].pc = avr->pc;\
 252         avr->old[avr->old_pci].sp = _avr_sp_get(avr);\
 253         avr->old_pci = (avr->old_pci + 1) & (OLD_PC_SIZE-1);\
 254
 255 #if AVR_STACK_WATCH
 256 #define STACK_FRAME_PUSH()\
 257         avr->stack_frame[avr->stack_frame_index].pc = avr->pc;\
 258         avr->stack_frame[avr->stack_frame_index].sp = _avr_sp_get(avr);\
 259         avr->stack_frame_index++;
 260 #define STACK_FRAME_POP()\
 261         if (avr->stack_frame_index > 0) \
 262                 avr->stack_frame_index--;
 263 #else
 264 #define STACK_FRAME_PUSH()
 265 #define STACK_FRAME_POP()
 266 #endif
 267
 268 /****************************************************************************\
 269  *
 270  * Helper functions for calculating the status register bit values.
 271  * See the Atmel data sheet for the instuction set for more info.
 272  *
 273 \****************************************************************************/
 274
 275 static uint8_t
 276 get_add_carry (uint8_t res, uint8_t rd, uint8_t rr, int b)
 277 {
 278     uint8_t resb = res >> b & 0x1;
 279     uint8_t rdb = rd >> b & 0x1;
 280     uint8_t rrb = rr >> b & 0x1;
 281     return (rdb & rrb) | (rrb & ~resb) | (~resb & rdb);
 282 }
 283
 284 static  uint8_t
 285 get_add_overflow (uint8_t res, uint8_t rd, uint8_t rr)
 286 {
 287     uint8_t res7 = res >> 7 & 0x1;
 288     uint8_t rd7 = rd >> 7 & 0x1;
 289     uint8_t rr7 = rr >> 7 & 0x1;
 290     return (rd7 & rr7 & ~res7) | (~rd7 & ~rr7 & res7);
 291 }
 292
 293 static  uint8_t
 294 get_sub_carry (uint8_t res, uint8_t rd, uint8_t rr, int b)
 295 {
 296     uint8_t resb = res >> b & 0x1;
 297     uint8_t rdb = rd >> b & 0x1;
 298     uint8_t rrb = rr >> b & 0x1;
 299     return (~rdb & rrb) | (rrb & resb) | (resb & ~rdb);
 300 }
 301
 302 static  uint8_t
 303 get_sub_overflow (uint8_t res, uint8_t rd, uint8_t rr)
 304 {
 305     uint8_t res7 = res >> 7 & 0x1;
 306     uint8_t rd7 = rd >> 7 & 0x1;
 307     uint8_t rr7 = rr >> 7 & 0x1;
 308     return (rd7 & ~rr7 & ~res7) | (~rd7 & rr7 & res7);
 309 }
 310
 311 static  uint8_t
 312 get_compare_carry (uint8_t res, uint8_t rd, uint8_t rr, int b)
 313 {
 314     uint8_t resb = (res >> b) & 0x1;
 315     uint8_t rdb = (rd >> b) & 0x1;
 316     uint8_t rrb = (rr >> b) & 0x1;
 317     return (~rdb & rrb) | (rrb & resb) | (resb & ~rdb);
 318 }
 319
 320 static  uint8_t
 321 get_compare_overflow (uint8_t res, uint8_t rd, uint8_t rr)
 322 {
 323     res >>= 7; rd >>= 7; rr >>= 7;
 324     /* The atmel data sheet says the second term is ~rd7 for CP
 325      * but that doesn't make any sense. You be the judge. */
 326     return (rd & ~rr & ~res) | (~rd & rr & res);
 327 }
 328
 329 static inline int _avr_is_instruction_32_bits(avr_t * avr, uint32_t pc)
 330 {
 331         uint16_t o = (avr->flash[pc] | (avr->flash[pc+1] << 8)) & 0xfc0f;
 332         return  o == 0x9200 || // STS ! Store Direct to Data Space
 333                         o == 0x9000 || // LDS Load Direct from Data Space
 334                         o == 0x940c || // JMP Long Jump
 335                         o == 0x940d || // JMP Long Jump
 336                         o == 0x940e ||  // CALL Long Call to sub
 337                         o == 0x940f; // CALL Long Call to sub
 338 }
 339
 340 /*
 341  * Main opcode decoder
 342  *
 343  * The decoder was written by following the datasheet in no particular order.
 344  * As I went along, I noticed "bit patterns" that could be used to factor opcodes
 345  * However, a lot of these only becane apparent later on, so SOME instructions
 346  * (skip of bit set etc) are compact, and some could use some refactoring (the ALU
 347  * ones scream to be factored).
 348  * I assume that the decoder could easily be 2/3 of it's current size.
 349  *
 350  * + It lacks the "extended" XMega jumps.
 351  * + It also doesn't check wether the core it's
 352  *   emulating is suposed to have the fancy instructions, like multiply and such.
 353  *
 354  * for now all instructions take "one" cycle, the cycle+=<extra> needs to be added.
 355  */
 356 uint16_t avr_run_one(avr_t * avr)
 357 {
 358         /*
 359          * this traces spurious reset or bad jump/opcodes and dumps the last 32 "jumps" to track it down
 360          */
 361         if ((avr->pc == 0 && avr->cycle > 0) || avr->pc >= avr->codeend) {
 362                 avr->trace = 1;
 363                 STATE("RESET\n");
 364                 CRASH();
 365         }
 366
 367         uint32_t        opcode = (avr->flash[avr->pc + 1] << 8) | avr->flash[avr->pc];
 368         uint32_t        new_pc = avr->pc + 2;   // future "default" pc
 369         int             cycle = 1;
 370
 371         avr->touched[0] = avr->touched[1] = avr->touched[2] = 0;
 372
 373         switch (opcode & 0xf000) {
 374                 case 0x0000: {
 375                         switch (opcode) {
 376                                 case 0x0000: {  // NOP
 377                                         STATE("nop\n");
 378                                 }       break;
 379                                 default: {
 380                                         switch (opcode & 0xfc00) {
 381                                                 case 0x0400: {  // CPC compare with carry 0000 01rd dddd rrrr
 382                                                         get_r_d_10(opcode);
 383                                                         uint8_t res = vd - vr - avr->sreg[S_C];
 384                                                         STATE("cpc %s[%02x], %s[%02x] = %02x\n", avr_regname(d), vd, avr_regname(r), vr, res);
 385                                                         if (res)
 386                                                                 avr->sreg[S_Z] = 0;
 387                                                         avr->sreg[S_H] = get_compare_carry(res, vd, vr, 3);
 388                                                         avr->sreg[S_V] = get_compare_overflow(res, vd, vr);
 389                                                         avr->sreg[S_N] = (res >> 7) & 1;
 390                                                         avr->sreg[S_C] = get_compare_carry(res, vd, vr, 7);
 391                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 392                                                         SREG();
 393                                                 }       break;
 394                                                 case 0x0c00: {  // ADD without carry 0000 11 rd dddd rrrr
 395                                                         get_r_d_10(opcode);
 396                                                         uint8_t res = vd + vr;
 397                                                         if (r == d) {
 398                                                                 STATE("lsl %s[%02x] = %02x\n", avr_regname(d), vd, res & 0xff);
 399                                                         } else {
 400                                                                 STATE("add %s[%02x], %s[%02x] = %02x\n", avr_regname(d), vd, avr_regname(r), vr, res);
 401                                                         }
 402                                                         _avr_set_r(avr, d, res);
 403                                                         avr->sreg[S_Z] = res == 0;
 404                                                         avr->sreg[S_H] = get_add_carry(res, vd, vr, 3);
 405                                                         avr->sreg[S_V] = get_add_overflow(res, vd, vr);
 406                                                         avr->sreg[S_N] = (res >> 7) & 1;
 407                                                         avr->sreg[S_C] = get_add_carry(res, vd, vr, 7);
 408                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 409                                                         SREG();
 410                                                 }       break;
 411                                                 case 0x0800: {  // SBC substract with carry 0000 10rd dddd rrrr
 412                                                         get_r_d_10(opcode);
 413                                                         uint8_t res = vd - vr - avr->sreg[S_C];
 414                                                         STATE("sbc %s[%02x], %s[%02x] = %02x\n", avr_regname(d), avr->data[d], avr_regname(r), avr->data[r], res);
 415                                                         _avr_set_r(avr, d, res);
 416                                                         if (res)
 417                                                                 avr->sreg[S_Z] = 0;
 418                                                         avr->sreg[S_H] = get_sub_carry(res, vd, vr, 3);
 419                                                         avr->sreg[S_V] = get_sub_overflow(res, vd, vr);
 420                                                         avr->sreg[S_N] = (res >> 7) & 1;
 421                                                         avr->sreg[S_C] = get_sub_carry(res, vd, vr, 7);
 422                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 423                                                         SREG();
 424                                                 }       break;
 425                                                 default:
 426                                                         switch (opcode & 0xff00) {
 427                                                                 case 0x0100: {  // MOVW – Copy Register Word 0000 0001 dddd rrrr
 428                                                                         uint8_t d = ((opcode >> 4) & 0xf) << 1;
 429                                                                         uint8_t r = ((opcode) & 0xf) << 1;
 430                                                                         STATE("movw %s:%s, %s:%s[%02x%02x]\n", avr_regname(d), avr_regname(d+1), avr_regname(r), avr_regname(r+1), avr->data[r+1], avr->data[r]);
 431                                                                         _avr_set_r(avr, d, avr->data[r]);
 432                                                                         _avr_set_r(avr, d+1, avr->data[r+1]);
 433                                                                 }       break;
 434                                                                 case 0x0200: {  // MULS – Multiply Signed 0000 0010 dddd rrrr
 435                                                                         int8_t r = opcode & 0xf;
 436                                                                         int8_t d = (opcode >> 4) & 0xf;
 437                                                                         int16_t res = ((int8_t)avr->data[r]) * ((int8_t)avr->data[d]);
 438                                                                         STATE("muls %s[%d], %s[%02x] = %d\n", avr_regname(d), ((int8_t)avr->data[d]), avr_regname(r), ((int8_t)avr->data[r]), res);
 439                                                                         _avr_set_r(avr, 0, res);
 440                                                                         _avr_set_r(avr, 1, res >> 8);
 441                                                                         avr->sreg[S_C] = (res >> 15) & 1;
 442                                                                         avr->sreg[S_Z] = res == 0;
 443                                                                         SREG();
 444                                                                 }       break;
 445                                                                 case 0x0300: {  // multiplications
 446                                                                         int8_t r = 16 + (opcode & 0x7);
 447                                                                         int8_t d = 16 + ((opcode >> 4) & 0x7);
 448                                                                         int16_t res = 0;
 449                                                                         uint8_t c = 0;
 450                                                                         const char * name = "";
 451                                                                         switch (opcode & 0x88) {
 452                                                                                 case 0x00:      // MULSU – Multiply Signed Unsigned 0000 0011 0ddd 0rrr
 453                                                                                         res = ((uint8_t)avr->data[r]) * ((int8_t)avr->data[d]);
 454                                                                                         c = (res >> 15) & 1;
 455                                                                                         name = "mulsu";
 456                                                                                         break;
 457                                                                                 case 0x08:      // FMUL Fractional Multiply Unsigned 0000 0011 0ddd 1rrr
 458                                                                                         res = ((uint8_t)avr->data[r]) * ((uint8_t)avr->data[d]);
 459                                                                                         c = (res >> 15) & 1;
 460                                                                                         res <<= 1;
 461                                                                                         name = "fmul";
 462                                                                                         break;
 463                                                                                 case 0x80:      // FMULS – Multiply Signed  0000 0011 1ddd 0rrr
 464                                                                                         res = ((int8_t)avr->data[r]) * ((int8_t)avr->data[d]);
 465                                                                                         c = (res >> 15) & 1;
 466                                                                                         res <<= 1;
 467                                                                                         name = "fmuls";
 468                                                                                         break;
 469                                                                                 case 0x88:      // FMULSU – Multiply Signed Unsigned 0000 0011 1ddd 0rrr
 470                                                                                         res = ((uint8_t)avr->data[r]) * ((int8_t)avr->data[d]);
 471                                                                                         c = (res >> 15) & 1;
 472                                                                                         res <<= 1;
 473                                                                                         name = "fmulsu";
 474                                                                                         break;
 475                                                                         }
 476                                                                         cycle++;
 477                                                                         STATE("%s %s[%d], %s[%02x] = %d\n", name, avr_regname(d), ((int8_t)avr->data[d]), avr_regname(r), ((int8_t)avr->data[r]), res);
 478                                                                         _avr_set_r(avr, 0, res);
 479                                                                         _avr_set_r(avr, 1, res >> 8);
 480                                                                         avr->sreg[S_C] = c;
 481                                                                         avr->sreg[S_Z] = res == 0;
 482                                                                         SREG();
 483                                                                 }       break;
 484                                                                 default: _avr_invalid_opcode(avr);
 485                                                         }
 486                                         }
 487                                 }
 488                         }
 489                 }       break;
 490
 491                 case 0x1000: {
 492                         switch (opcode & 0xfc00) {
 493                                 case 0x1800: {  // SUB without carry 0000 10 rd dddd rrrr
 494                                         get_r_d_10(opcode);
 495                                         uint8_t res = vd - vr;
 496                                         STATE("sub %s[%02x], %s[%02x] = %02x\n", avr_regname(d), vd, avr_regname(r), vr, res);
 497                                         _avr_set_r(avr, d, res);
 498                                         avr->sreg[S_Z] = res == 0;
 499                                         avr->sreg[S_H] = get_sub_carry(res, vd, vr, 3);
 500                                         avr->sreg[S_V] = get_sub_overflow(res, vd, vr);
 501                                         avr->sreg[S_N] = (res >> 7) & 1;
 502                                         avr->sreg[S_C] = get_sub_carry(res, vd, vr, 7);
 503                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 504                                         SREG();
 505                                 }       break;
 506                                 case 0x1000: {  // CPSE Compare, skip if equal 0000 10 rd dddd rrrr
 507                                         get_r_d_10(opcode);
 508                                         uint16_t res = vd == vr;
 509                                         STATE("cpse %s[%02x], %s[%02x]\t; Will%s skip\n", avr_regname(d), avr->data[d], avr_regname(r), avr->data[r], res ? "":"not ");
 510                                         if (res) {
 511                                                 if (_avr_is_instruction_32_bits(avr, new_pc)) {
 512                                                         new_pc += 4; cycle += 2;
 513                                                 } else {
 514                                                         new_pc += 2; cycle++;
 515                                                 }
 516                                         }
 517                                 }       break;
 518                                 case 0x1400: {  // CP Compare 0000 10 rd dddd rrrr
 519                                         get_r_d_10(opcode);
 520                                         uint8_t res = vd - vr;
 521                                         STATE("cp %s[%02x], %s[%02x] = %02x\n", avr_regname(d), vd, avr_regname(r), vr, res);
 522                                         avr->sreg[S_Z] = res == 0;
 523                                         avr->sreg[S_H] = get_compare_carry(res, vd, vr, 3);
 524                                         avr->sreg[S_V] = get_compare_overflow(res, vd, vr);
 525                                         avr->sreg[S_N] = res >> 7;
 526                                         avr->sreg[S_C] = get_compare_carry(res, vd, vr, 7);
 527                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 528                                         SREG();
 529                                 }       break;
 530                                 case 0x1c00: {  // ADD with carry 0001 11 rd dddd rrrr
 531                                         get_r_d_10(opcode);
 532                                         uint8_t res = vd + vr + avr->sreg[S_C];
 533                                         if (r == d) {
 534                                                 STATE("rol %s[%02x] = %02x\n", avr_regname(d), avr->data[d], res);
 535                                         } else {
 536                                                 STATE("addc %s[%02x], %s[%02x] = %02x\n", avr_regname(d), avr->data[d], avr_regname(r), avr->data[r], res);
 537                                         }
 538                                         _avr_set_r(avr, d, res);
 539                                         avr->sreg[S_Z] = res == 0;
 540                                         avr->sreg[S_H] = get_add_carry(res, vd, vr, 3);
 541                                         avr->sreg[S_V] = get_add_overflow(res, vd, vr);
 542                                         avr->sreg[S_N] = (res >> 7) & 1;
 543                                         avr->sreg[S_C] = get_add_carry(res, vd, vr, 7);
 544                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 545                                         SREG();
 546                                 }       break;
 547                                 default: _avr_invalid_opcode(avr);
 548                         }
 549                 }       break;
 550
 551                 case 0x2000: {
 552                         switch (opcode & 0xfc00) {
 553                                 case 0x2000: {  // AND  0010 00rd dddd rrrr
 554                                         get_r_d_10(opcode);
 555                                         uint8_t res = vd & vr;
 556                                         if (r == d) {
 557                                                 STATE("tst %s[%02x]\n", avr_regname(d), avr->data[d]);
 558                                         } else {
 559                                                 STATE("and %s[%02x], %s[%02x] = %02x\n", avr_regname(d), vd, avr_regname(r), vr, res);
 560                                         }
 561                                         _avr_set_r(avr, d, res);
 562                                         avr->sreg[S_Z] = res == 0;
 563                                         avr->sreg[S_N] = (res >> 7) & 1;
 564                                         avr->sreg[S_V] = 0;
 565                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 566                                         SREG();
 567                                 }       break;
 568                                 case 0x2400: {  // EOR  0010 01rd dddd rrrr
 569                                         get_r_d_10(opcode);
 570                                         uint8_t res = vd ^ vr;
 571                                         if (r==d) {
 572                                                 STATE("clr %s[%02x]\n", avr_regname(d), avr->data[d]);
 573                                         } else {
 574                                                 STATE("eor %s[%02x], %s[%02x] = %02x\n", avr_regname(d), vd, avr_regname(r), vr, res);
 575                                         }
 576                                         _avr_set_r(avr, d, res);
 577                                         avr->sreg[S_Z] = res == 0;
 578                                         avr->sreg[S_N] = (res >> 7) & 1;
 579                                         avr->sreg[S_V] = 0;
 580                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 581                                         SREG();
 582                                 }       break;
 583                                 case 0x2800: {  // OR Logical OR        0010 10rd dddd rrrr
 584                                         get_r_d_10(opcode);
 585                                         uint8_t res = vd | vr;
 586                                         STATE("or %s[%02x], %s[%02x] = %02x\n", avr_regname(d), vd, avr_regname(r), vr, res);
 587                                         _avr_set_r(avr, d, res);
 588                                         avr->sreg[S_Z] = res == 0;
 589                                         avr->sreg[S_N] = (res >> 7) & 1;
 590                                         avr->sreg[S_V] = 0;
 591                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 592                                         SREG();
 593                                 }       break;
 594                                 case 0x2c00: {  // MOV  0010 11rd dddd rrrr
 595                                         get_r_d_10(opcode);
 596                                         uint8_t res = vr;
 597                                         STATE("mov %s[%02x], %s[%02x] = %02x\n", avr_regname(d), vd, avr_regname(r), vr, res);
 598                                         _avr_set_r(avr, d, res);
 599                                 }       break;
 600                                 default: _avr_invalid_opcode(avr);
 601                         }
 602                 }       break;
 603
 604                 case 0x3000: {  // CPI 0011 KKKK rrrr KKKK
 605                         get_k_r16(opcode);
 606                         uint8_t vr = avr->data[r];
 607                         uint8_t res = vr - k;
 608                         STATE("cpi %s[%02x], 0x%02x\n", avr_regname(r), vr, k);
 609
 610                         avr->sreg[S_Z] = res == 0;
 611                         avr->sreg[S_H] = get_compare_carry(res, vr, k, 3);
 612                         avr->sreg[S_V] = get_compare_overflow(res, vr, k);
 613                         avr->sreg[S_N] = (res >> 7) & 1;
 614                         avr->sreg[S_C] = get_compare_carry(res, vr, k, 7);
 615                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 616                         SREG();
 617                 }       break;
 618
 619                 case 0x4000: {  // SBCI Subtract Immediate With Carry 0101 10 kkkk dddd kkkk
 620                         get_k_r16(opcode);
 621                         uint8_t vr = avr->data[r];
 622                         uint8_t res = vr - k - avr->sreg[S_C];
 623                         STATE("sbci %s[%02x], 0x%02x = %02x\n", avr_regname(r), avr->data[r], k, res);
 624                         _avr_set_r(avr, r, res);
 625                         avr->sreg[S_Z] = res  == 0;
 626                         avr->sreg[S_N] = (res >> 7) & 1;
 627                         avr->sreg[S_C] = (k + avr->sreg[S_C]) > vr;
 628                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 629                         SREG();
 630                 }       break;
 631
 632                 case 0x5000: {  // SUB Subtract Immediate 0101 10 kkkk dddd kkkk
 633                         get_k_r16(opcode);
 634                         uint8_t vr = avr->data[r];
 635                         uint8_t res = vr - k;
 636                         STATE("subi %s[%02x], 0x%02x = %02x\n", avr_regname(r), avr->data[r], k, res);
 637                         _avr_set_r(avr, r, res);
 638                         avr->sreg[S_Z] = res  == 0;
 639                         avr->sreg[S_N] = (res >> 7) & 1;
 640                         avr->sreg[S_C] = k > vr;
 641                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 642                         SREG();
 643                 }       break;
 644
 645                 case 0x6000: {  // ORI aka SBR  Logical AND with Immediate      0110 kkkk dddd kkkk
 646                         get_k_r16(opcode);
 647                         uint8_t res = avr->data[r] | k;
 648                         STATE("ori %s[%02x], 0x%02x\n", avr_regname(r), avr->data[r], k);
 649                         _avr_set_r(avr, r, res);
 650                         avr->sreg[S_Z] = res == 0;
 651                         avr->sreg[S_N] = (res >> 7) & 1;
 652                         avr->sreg[S_V] = 0;
 653                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 654                         SREG();
 655                 }       break;
 656
 657                 case 0x7000: {  // ANDI Logical AND with Immediate      0111 kkkk dddd kkkk
 658                         get_k_r16(opcode);
 659                         uint8_t res = avr->data[r] & k;
 660                         STATE("andi %s[%02x], 0x%02x\n", avr_regname(r), avr->data[r], k);
 661                         _avr_set_r(avr, r, res);
 662                         avr->sreg[S_Z] = res == 0;
 663                         avr->sreg[S_N] = (res >> 7) & 1;
 664                         avr->sreg[S_V] = 0;
 665                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 666                         SREG();
 667                 }       break;
 668
 669                 case 0xa000:
 670                 case 0x8000: {
 671                         switch (opcode & 0xd008) {
 672                                 case 0xa000:
 673                                 case 0x8000: {  // LD (LDD) – Load Indirect using Z 10q0 qq0r rrrr 0qqq
 674                                         uint16_t v = avr->data[R_ZL] | (avr->data[R_ZH] << 8);
 675                                         uint8_t r = (opcode >> 4) & 0x1f;
 676                                         uint8_t q = ((opcode & 0x2000) >> 8) | ((opcode & 0x0c00) >> 7) | (opcode & 0x7);
 677
 678                                         if (opcode & 0x0200) {
 679                                                 STATE("st (Z+%d[%04x]), %s[%02x]\n", q, v+q, avr_regname(r), avr->data[r]);
 680                                                 _avr_set_ram(avr, v+q, avr->data[r]);
 681                                         } else {
 682                                                 STATE("ld %s, (Z+%d[%04x])=[%02x]\n", avr_regname(r), q, v+q, avr->data[v+q]);
 683                                                 _avr_set_r(avr, r, _avr_get_ram(avr, v+q));
 684                                         }
 685                                         cycle += 2;
 686                                 }       break;
 687                                 case 0xa008:
 688                                 case 0x8008: {  // LD (LDD) – Load Indirect using Y 10q0 qq0r rrrr 1qqq
 689                                         uint16_t v = avr->data[R_YL] | (avr->data[R_YH] << 8);
 690                                         uint8_t r = (opcode >> 4) & 0x1f;
 691                                         uint8_t q = ((opcode & 0x2000) >> 8) | ((opcode & 0x0c00) >> 7) | (opcode & 0x7);
 692
 693                                         if (opcode & 0x0200) {
 694                                                 STATE("st (Y+%d[%04x]), %s[%02x]\n", q, v+q, avr_regname(r), avr->data[r]);
 695                                                 _avr_set_ram(avr, v+q, avr->data[r]);
 696                                         } else {
 697                                                 STATE("ld %s, (Y+%d[%04x])=[%02x]\n", avr_regname(r), q, v+q, avr->data[v+q]);
 698                                                 _avr_set_r(avr, r, _avr_get_ram(avr, v+q));
 699                                         }
 700                                         cycle += 2;
 701                                 }       break;
 702                                 default: _avr_invalid_opcode(avr);
 703                         }
 704                 }       break;
 705
 706                 case 0x9000: {
 707                         /* this is an annoying special case, but at least these lines handle all the SREG set/clear opcodes */
 708                         if ((opcode & 0xff0f) == 0x9408) {
 709                                 uint8_t b = (opcode >> 4) & 7;
 710                                 STATE("%s%c\n", opcode & 0x0080 ? "cl" : "se", _sreg_bit_name[b]);
 711                                 avr->sreg[b] = (opcode & 0x0080) == 0;
 712                                 SREG();
 713                         } else switch (opcode) {
 714                                 case 0x9588: { // SLEEP
 715                                         STATE("sleep\n");
 716                                         avr->state = cpu_Sleeping;
 717                                 }       break;
 718                                 case 0x9598: { // BREAK
 719                                         STATE("break\n");
 720                                         if (avr->gdb)
 721                                                 avr->state = cpu_StepDone;
 722                                 }       break;
 723                                 case 0x95a8: { // WDR
 724                                         STATE("wdr\n");
 725                                 }       break;
 726                                 case 0x9409: { // IJMP Indirect jump
 727                                         uint16_t z = avr->data[R_ZL] | (avr->data[R_ZH] << 8);
 728                                         STATE("ijmp Z[%04x]\n", z << 1);
 729                                         new_pc = z << 1;
 730                                         cycle++;
 731                                         TRACE_JUMP();
 732                                 }       break;
 733                                 case 0x9509: { // ICALL Indirect Call to Subroutine
 734                                         uint16_t z = avr->data[R_ZL] | (avr->data[R_ZH] << 8);
 735                                         STATE("icall Z[%04x]\n", z << 1);
 736                                         _avr_push16(avr, new_pc >> 1);
 737                                         new_pc = z << 1;
 738                                         cycle += 2;
 739                                         TRACE_JUMP();
 740                                         STACK_FRAME_PUSH();
 741                                 }       break;
 742                                 case 0x9518:    // RETI
 743                                 case 0x9508: {  // RET
 744                                         new_pc = _avr_pop16(avr) << 1;
 745                                         if (opcode & 0x10)      // reti
 746                                                 avr->sreg[S_I] = 1;
 747                                         cycle += 3;
 748                                         STATE("ret%s\n", opcode & 0x10 ? "i" : "");
 749                                         TRACE_JUMP();
 750                                         STACK_FRAME_POP();
 751                                 }       break;
 752                                 case 0x95c8: {  // LPM Load Program Memory R0 <- (Z)
 753                                         uint16_t z = avr->data[R_ZL] | (avr->data[R_ZH] << 8);
 754                                         STATE("lpm %s, (Z[%04x])\n", avr_regname(0), z);
 755                                         _avr_set_r(avr, 0, avr->flash[z]);
 756                                 }       break;
 757                                 case 0x9408:case 0x9418:case 0x9428:case 0x9438:case 0x9448:case 0x9458:case 0x9468:
 758                                 case 0x9478:
 759                                 {       // BSET 1001 0100 0ddd 1000
 760                                         uint8_t b = (opcode >> 4) & 7;
 761                                         avr->sreg[b] = 1;
 762                                         STATE("bset %c\n", _sreg_bit_name[b]);
 763                                         SREG();
 764                                 }       break;
 765                                 case 0x9488:case 0x9498:case 0x94a8:case 0x94b8:case 0x94c8:case 0x94d8:case 0x94e8:
 766                                 case 0x94f8:
 767                                 {       // BSET 1001 0100 0ddd 1000
 768                                         uint8_t b = (opcode >> 4) & 7;
 769                                         avr->sreg[b] = 0;
 770                                         STATE("bclr %c\n", _sreg_bit_name[b]);
 771                                         SREG();
 772                                 }       break;
 773                                 default:  {
 774                                         switch (opcode & 0xfe0f) {
 775                                                 case 0x9000: {  // LDS Load Direct from Data Space, 32 bits
 776                                                         uint8_t r = (opcode >> 4) & 0x1f;
 777                                                         uint16_t x = (avr->flash[new_pc+1] << 8) | avr->flash[new_pc];
 778                                                         new_pc += 2;
 779                                                         STATE("lds %s[%02x], 0x%04x\n", avr_regname(r), avr->data[r], x);
 780                                                         _avr_set_r(avr, r, _avr_get_ram(avr, x));
 781                                                         cycle++;
 782                                                 }       break;
 783                                                 case 0x9005:
 784                                                 case 0x9004: {  // LPM Load Program Memory 1001 000d dddd 01oo
 785                                                         uint16_t z = avr->data[R_ZL] | (avr->data[R_ZH] << 8);
 786                                                         uint8_t r = (opcode >> 4) & 0x1f;
 787                                                         int op = opcode & 3;
 788                                                         STATE("lpm %s, (Z[%04x]%s)\n", avr_regname(r), z, opcode?"+":"");
 789                                                         _avr_set_r(avr, r, avr->flash[z]);
 790                                                         if (op == 1) {
 791                                                                 z++;
 792                                                                 _avr_set_r(avr, R_ZH, z >> 8);
 793                                                                 _avr_set_r(avr, R_ZL, z);
 794                                                         }
 795                                                         cycle += 2;
 796                                                 }       break;
 797                                                 case 0x900c:
 798                                                 case 0x900d:
 799                                                 case 0x900e: {  // LD Load Indirect from Data using X 1001 000r rrrr 11oo
 800                                                         int op = opcode & 3;
 801                                                         uint8_t r = (opcode >> 4) & 0x1f;
 802                                                         uint16_t x = (avr->data[R_XH] << 8) | avr->data[R_XL];
 803                                                         STATE("ld %s, %sX[%04x]%s\n", avr_regname(r), op == 2 ? "--" : "", x, op == 1 ? "++" : "");
 804
 805                                                         if (op == 2) x--;
 806                                                         _avr_set_r(avr, r, _avr_get_ram(avr, x));
 807                                                         if (op == 1) x++;
 808                                                         _avr_set_r(avr, R_XH, x >> 8);
 809                                                         _avr_set_r(avr, R_XL, x);
 810                                                 }       break;
 811                                                 case 0x920c:
 812                                                 case 0x920d:
 813                                                 case 0x920e: {  // ST Store Indirect Data Space X 1001 001r rrrr 11oo
 814                                                         int op = opcode & 3;
 815                                                         uint8_t r = (opcode >> 4) & 0x1f;
 816                                                         uint16_t x = (avr->data[R_XH] << 8) | avr->data[R_XL];
 817                                                         STATE("st %sX[%04x]%s, %s[%02x] \n", op == 2 ? "--" : "", x, op == 1 ? "++" : "", avr_regname(r), avr->data[r]);
 818                                                         cycle++;
 819                                                         if (op == 2) x--;
 820                                                         _avr_set_ram(avr, x, avr->data[r]);
 821                                                         if (op == 1) x++;
 822                                                         _avr_set_r(avr, R_XH, x >> 8);
 823                                                         _avr_set_r(avr, R_XL, x);
 824                                                 }       break;
 825                                                 case 0x9009:
 826                                                 case 0x900a: {  // LD Load Indirect from Data using Y 1001 000r rrrr 10oo
 827                                                         int op = opcode & 3;
 828                                                         uint8_t r = (opcode >> 4) & 0x1f;
 829                                                         uint16_t y = (avr->data[R_YH] << 8) | avr->data[R_YL];
 830                                                         STATE("ld %s, %sY[%04x]%s\n", avr_regname(r), op == 2 ? "--" : "", y, op == 1 ? "++" : "");
 831                                                         cycle++;
 832                                                         if (op == 2) y--;
 833                                                         _avr_set_r(avr, r, _avr_get_ram(avr, y));
 834                                                         if (op == 1) y++;
 835                                                         _avr_set_r(avr, R_YH, y >> 8);
 836                                                         _avr_set_r(avr, R_YL, y);
 837                                                 }       break;
 838                                                 case 0x9209:
 839                                                 case 0x920a: {  // ST Store Indirect Data Space Y 1001 001r rrrr 10oo
 840                                                         int op = opcode & 3;
 841                                                         uint8_t r = (opcode >> 4) & 0x1f;
 842                                                         uint16_t y = (avr->data[R_YH] << 8) | avr->data[R_YL];
 843                                                         STATE("st %sY[%04x]%s, %s[%02x] \n", op == 2 ? "--" : "", y, op == 1 ? "++" : "", avr_regname(r), avr->data[r]);
 844                                                         cycle++;
 845                                                         if (op == 2) y--;
 846                                                         _avr_set_ram(avr, y, avr->data[r]);
 847                                                         if (op == 1) y++;
 848                                                         _avr_set_r(avr, R_YH, y >> 8);
 849                                                         _avr_set_r(avr, R_YL, y);
 850                                                 }       break;
 851                                                 case 0x9200: {  // STS ! Store Direct to Data Space, 32 bits
 852                                                         uint8_t r = (opcode >> 4) & 0x1f;
 853                                                         uint16_t x = (avr->flash[new_pc+1] << 8) | avr->flash[new_pc];
 854                                                         new_pc += 2;
 855                                                         STATE("sts 0x%04x, %s[%02x]\n", x, avr_regname(r), avr->data[r]);
 856                                                         _avr_set_ram(avr, x, avr->data[r]);
 857                                                 }       break;
 858                                                 case 0x9001:
 859                                                 case 0x9002: {  // LD Load Indirect from Data using Z 1001 001r rrrr 00oo
 860                                                         int op = opcode & 3;
 861                                                         uint8_t r = (opcode >> 4) & 0x1f;
 862                                                         uint16_t z = (avr->data[R_ZH] << 8) | avr->data[R_ZL];
 863                                                         STATE("ld %s, %sZ[%04x]%s\n", avr_regname(r), op == 2 ? "--" : "", z, op == 1 ? "++" : "");
 864                                                         if (op == 2) z--;
 865                                                         _avr_set_r(avr, r, _avr_get_ram(avr, z));
 866                                                         if (op == 1) z++;
 867                                                         _avr_set_r(avr, R_ZH, z >> 8);
 868                                                         _avr_set_r(avr, R_ZL, z);
 869                                                 }       break;
 870                                                 case 0x9201:
 871                                                 case 0x9202: {  // ST Store Indirect Data Space Z 1001 001r rrrr 00oo
 872                                                         int op = opcode & 3;
 873                                                         uint8_t r = (opcode >> 4) & 0x1f;
 874                                                         uint16_t z = (avr->data[R_ZH] << 8) | avr->data[R_ZL];
 875                                                         STATE("st %sZ[%04x]%s, %s[%02x] \n", op == 2 ? "--" : "", z, op == 1 ? "++" : "", avr_regname(r), avr->data[r]);
 876                                                         if (op == 2) z--;
 877                                                         _avr_set_ram(avr, z, avr->data[r]);
 878                                                         if (op == 1) z++;
 879                                                         _avr_set_r(avr, R_ZH, z >> 8);
 880                                                         _avr_set_r(avr, R_ZL, z);
 881                                                 }       break;
 882                                                 case 0x900f: {  // POP 1001 000d dddd 1111
 883                                                         uint8_t r = (opcode >> 4) & 0x1f;
 884                                                         _avr_set_r(avr, r, _avr_pop8(avr));
 885                                                         uint16_t sp = _avr_sp_get(avr);
 886                                                         STATE("pop %s (@%04x)[%02x]\n", avr_regname(r), sp, avr->data[sp]);
 887                                                         cycle++;
 888                                                 }       break;
 889                                                 case 0x920f: {  // PUSH 1001 001d dddd 1111
 890                                                         uint8_t r = (opcode >> 4) & 0x1f;
 891                                                         _avr_push8(avr, avr->data[r]);
 892                                                         uint16_t sp = _avr_sp_get(avr);
 893                                                         STATE("push %s[%02x] (@%04x)\n", avr_regname(r), avr->data[r], sp);
 894                                                         cycle++;
 895                                                 }       break;
 896                                                 case 0x9400: {  // COM – One’s Complement
 897                                                         uint8_t r = (opcode >> 4) & 0x1f;
 898                                                         uint8_t res = 0xff - avr->data[r];
 899                                                         STATE("com %s[%02x] = %02x\n", avr_regname(r), avr->data[r], res);
 900                                                         _avr_set_r(avr, r, res);
 901                                                         avr->sreg[S_Z] = res == 0;
 902                                                         avr->sreg[S_N] = res >> 7;
 903                                                         avr->sreg[S_V] = 0;
 904                                                         avr->sreg[S_C] = 1;
 905                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 906                                                         SREG();
 907                                                 }       break;
 908                                                 case 0x9401: {  // NEG – One’s Complement
 909                                                         uint8_t r = (opcode >> 4) & 0x1f;
 910                                                         uint8_t rd = avr->data[r];
 911                                                         uint8_t res = 0x00 - rd;
 912                                                         STATE("neg %s[%02x] = %02x\n", avr_regname(r), rd, res);
 913                                                         _avr_set_r(avr, r, res);
 914                                                         avr->sreg[S_H] = ((res >> 3) | (rd >> 3)) & 1;
 915                                                         avr->sreg[S_Z] = res == 0;
 916                                                         avr->sreg[S_N] = res >> 7;
 917                                                         avr->sreg[S_V] = res == 0x80;
 918                                                         avr->sreg[S_C] = res != 0;
 919                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 920                                                         SREG();
 921                                                 }       break;
 922                                                 case 0x9402: {  // SWAP – Swap Nibbles
 923                                                         uint8_t r = (opcode >> 4) & 0x1f;
 924                                                         uint8_t res = (avr->data[r] >> 4) | (avr->data[r] << 4) ;
 925                                                         STATE("swap %s[%02x] = %02x\n", avr_regname(r), avr->data[r], res);
 926                                                         _avr_set_r(avr, r, res);
 927                                                 }       break;
 928                                                 case 0x9403: {  // INC – Increment
 929                                                         uint8_t r = (opcode >> 4) & 0x1f;
 930                                                         uint8_t res = avr->data[r] + 1;
 931                                                         STATE("inc %s[%02x] = %02x\n", avr_regname(r), avr->data[r], res);
 932                                                         _avr_set_r(avr, r, res);
 933                                                         avr->sreg[S_Z] = res == 0;
 934                                                         avr->sreg[S_N] = res >> 7;
 935                                                         avr->sreg[S_V] = res == 0x7f;
 936                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 937                                                         SREG();
 938                                                 }       break;
 939                                                 case 0x9405: {  // ASR – Arithmetic Shift Right 1001 010d dddd 0101
 940                                                         uint8_t r = (opcode >> 4) & 0x1f;
 941                                                         uint8_t vr = avr->data[r];
 942                                                         uint8_t res = (vr >> 1) | (vr & 0x80);
 943                                                         STATE("asr %s[%02x]\n", avr_regname(r), vr);
 944                                                         _avr_set_r(avr, r, res);
 945                                                         avr->sreg[S_Z] = res == 0;
 946                                                         avr->sreg[S_C] = vr & 1;
 947                                                         avr->sreg[S_N] = res >> 7;
 948                                                         avr->sreg[S_V] = avr->sreg[S_N] ^ avr->sreg[S_C];
 949                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 950                                                         SREG();
 951                                                 }       break;
 952                                                 case 0x9406: {  // LSR 1001 010d dddd 0110
 953                                                         uint8_t r = (opcode >> 4) & 0x1f;
 954                                                         uint8_t vr = avr->data[r];
 955                                                         uint8_t res = vr >> 1;
 956                                                         STATE("lsr %s[%02x]\n", avr_regname(r), vr);
 957                                                         _avr_set_r(avr, r, res);
 958                                                         avr->sreg[S_Z] = res == 0;
 959                                                         avr->sreg[S_C] = vr & 1;
 960                                                         avr->sreg[S_N] = 0;
 961                                                         avr->sreg[S_V] = avr->sreg[S_N] ^ avr->sreg[S_C];
 962                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 963                                                         SREG();
 964                                                 }       break;
 965                                                 case 0x9407: {  // ROR 1001 010d dddd 0111
 966                                                         uint8_t r = (opcode >> 4) & 0x1f;
 967                                                         uint8_t vr = avr->data[r];
 968                                                         uint8_t res = (avr->sreg[S_C] ? 0x80 : 0) | vr >> 1;
 969                                                         STATE("ror %s[%02x]\n", avr_regname(r), vr);
 970                                                         _avr_set_r(avr, r, res);
 971                                                         avr->sreg[S_Z] = res == 0;
 972                                                         avr->sreg[S_C] = vr & 1;
 973                                                         avr->sreg[S_N] = 0;
 974                                                         avr->sreg[S_V] = avr->sreg[S_N] ^ avr->sreg[S_C];
 975                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 976                                                         SREG();
 977                                                 }       break;
 978                                                 case 0x940a: {  // DEC – Decrement
 979                                                         uint8_t r = (opcode >> 4) & 0x1f;
 980                                                         uint8_t res = avr->data[r] - 1;
 981                                                         STATE("dec %s[%02x] = %02x\n", avr_regname(r), avr->data[r], res);
 982                                                         _avr_set_r(avr, r, res);
 983                                                         avr->sreg[S_Z] = res == 0;
 984                                                         avr->sreg[S_N] = res >> 7;
 985                                                         avr->sreg[S_V] = res == 0x80;
 986                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
 987                                                         SREG();
 988                                                 }       break;
 989                                                 case 0x940c:
 990                                                 case 0x940d: {  // JMP Long Call to sub, 32 bits
 991                                                         uint32_t a = ((opcode & 0x01f0) >> 3) | (opcode & 1);
 992                                                         uint16_t x = (avr->flash[new_pc+1] << 8) | avr->flash[new_pc];
 993                                                         a = (a << 16) | x;
 994                                                         STATE("jmp 0x%06x\n", a);
 995                                                         new_pc = a << 1;
 996                                                         cycle += 2;
 997                                                         TRACE_JUMP();
 998                                                 }       break;
 999                                                 case 0x940e:
1000                                                 case 0x940f: {  // CALL Long Call to sub, 32 bits
1001                                                         uint32_t a = ((opcode & 0x01f0) >> 3) | (opcode & 1);
1002                                                         uint16_t x = (avr->flash[new_pc+1] << 8) | avr->flash[new_pc];
1003                                                         a = (a << 16) | x;
1004                                                         STATE("call 0x%06x\n", a);
1005                                                         new_pc += 2;
1006                                                         _avr_push16(avr, new_pc >> 1);
1007                                                         new_pc = a << 1;
1008                                                         cycle += 3;     // 4 cycles
1009                                                         TRACE_JUMP();
1010                                                         STACK_FRAME_PUSH();
1011                                                 }       break;
1012
1013                                                 default: {
1014                                                         switch (opcode & 0xff00) {
1015                                                                 case 0x9600: {  // ADIW - Add Immediate to Word 1001 0110 KKdd KKKK
1016                                                                         uint8_t r = 24 + ((opcode >> 3) & 0x6);
1017                                                                         uint8_t k = ((opcode & 0x00c0) >> 2) | (opcode & 0xf);
1018                                                                         uint8_t rdl = avr->data[r], rdh = avr->data[r+1];
1019                                                                         uint32_t res = rdl | (rdh << 8);
1020                                                                         STATE("adiw %s:%s[%04x], 0x%02x\n", avr_regname(r), avr_regname(r+1), res, k);
1021                                                                         res += k;
1022                                                                         _avr_set_r(avr, r + 1, res >> 8);
1023                                                                         _avr_set_r(avr, r, res);
1024                                                                         avr->sreg[S_V] = ~(rdh >> 7) & ((res >> 15) & 1);
1025                                                                         avr->sreg[S_Z] = (res & 0xffff) == 0;
1026                                                                         avr->sreg[S_N] = (res >> 15) & 1;
1027                                                                         avr->sreg[S_C] = ~((res >> 15) & 1) & (rdh >> 7);
1028                                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
1029                                                                         SREG();
1030                                                                         cycle++;
1031                                                                 }       break;
1032                                                                 case 0x9700: {  // SBIW - Subtract Immediate from Word 1001 0110 KKdd KKKK
1033                                                                         uint8_t r = 24 + ((opcode >> 3) & 0x6);
1034                                                                         uint8_t k = ((opcode & 0x00c0) >> 2) | (opcode & 0xf);
1035                                                                         uint8_t rdl = avr->data[r], rdh = avr->data[r+1];
1036                                                                         uint32_t res = rdl | (rdh << 8);
1037                                                                         STATE("sbiw %s:%s[%04x], 0x%02x\n", avr_regname(r), avr_regname(r+1), res, k);
1038                                                                         res -= k;
1039                                                                         _avr_set_r(avr, r + 1, res >> 8);
1040                                                                         _avr_set_r(avr, r, res);
1041                                                                         avr->sreg[S_V] = (rdh >> 7) & (~(res >> 15) & 1);
1042                                                                         avr->sreg[S_Z] = (res & 0xffff) == 0;
1043                                                                         avr->sreg[S_N] = (res >> 15) & 1;
1044                                                                         avr->sreg[S_C] = ((res >> 15) & 1) & (~rdh >> 7);
1045                                                                         avr->sreg[S_S] = avr->sreg[S_N] ^ avr->sreg[S_V];
1046                                                                         SREG();
1047                                                                         cycle++;
1048                                                                 }       break;
1049                                                                 case 0x9800: {  // CBI - Clear Bit in I/O Registe 1001 1000 AAAA Abbb
1050                                                                         uint8_t io = ((opcode >> 3) & 0x1f) + 32;
1051                                                                         uint8_t b = opcode & 0x7;
1052                                                                         uint8_t res = _avr_get_ram(avr, io) & ~(1 << b);
1053                                                                         STATE("cbi %s[%04x], 0x%02x = %02x\n", avr_regname(io), avr->data[io], 1<<b, res);
1054                                                                         _avr_set_ram(avr, io, res);
1055                                                                         cycle++;
1056                                                                 }       break;
1057                                                                 case 0x9900: {  // SBIC - Skip if Bit in I/O Register is Cleared 1001 0111 AAAA Abbb
1058                                                                         uint8_t io = ((opcode >> 3) & 0x1f) + 32;
1059                                                                         uint8_t b = opcode & 0x7;
1060                                                                         uint8_t res = _avr_get_ram(avr, io) & (1 << b);
1061                                                                         STATE("sbic %s[%04x], 0x%02x\t; Will%s branch\n", avr_regname(io), avr->data[io], 1<<b, !res?"":"not ");
1062                                                                         if (!res) {
1063                                                                                 if (_avr_is_instruction_32_bits(avr, new_pc)) {
1064                                                                                         new_pc += 4; cycle += 2;
1065                                                                                 } else {
1066                                                                                         new_pc += 2; cycle++;
1067                                                                                 }
1068                                                                         }
1069                                                                 }       break;
1070                                                                 case 0x9a00: {  // SBI - Set Bit in I/O Register 1001 1000 AAAA Abbb
1071                                                                         uint8_t io = ((opcode >> 3) & 0x1f) + 32;
1072                                                                         uint8_t b = opcode & 0x7;
1073                                                                         uint8_t res = _avr_get_ram(avr, io) | (1 << b);
1074                                                                         STATE("sbi %s[%04x], 0x%02x = %02x\n", avr_regname(io), avr->data[io], 1<<b, res);
1075                                                                         _avr_set_ram(avr, io, res);
1076                                                                         cycle++;
1077                                                                 }       break;
1078                                                                 case 0x9b00: {  // SBIS - Skip if Bit in I/O Register is Cleared 1001 0111 AAAA Abbb
1079                                                                         uint8_t io = (opcode >> 3) & 0x1f;
1080                                                                         uint8_t b = opcode & 0x7;
1081                                                                         uint8_t res = _avr_get_ram(avr, io + 32) & (1 << b);
1082                                                                         STATE("sbis %s[%04x], 0x%02x\t; Will%s branch\n", avr_regname(io), avr->data[io], 1<<b, res?"":"not ");
1083                                                                         if (res) {
1084                                                                                 if (_avr_is_instruction_32_bits(avr, new_pc)) {
1085                                                                                         new_pc += 4; cycle += 2;
1086                                                                                 } else {
1087                                                                                         new_pc += 2; cycle++;
1088                                                                                 }
1089                                                                         }
1090                                                                 }       break;
1091                                                                 default:
1092                                                                         switch (opcode & 0xfc00) {
1093                                                                                 case 0x9c00: {  // MUL - Multiply Unsigned 1001 11rd dddd rrrr
1094                                                                                         get_r_d_10(opcode);
1095                                                                                         uint16_t res = vd * vr;
1096                                                                                         STATE("mul %s[%02x], %s[%02x] = %04x\n", avr_regname(d), vd, avr_regname(r), vr, res);
1097                                                                                         _avr_set_r(avr, 0, res);
1098                                                                                         _avr_set_r(avr, 1, res >> 8);
1099                                                                                         avr->sreg[S_Z] = res == 0;
1100                                                                                         avr->sreg[S_C] = (res >> 15) & 1;
1101                                                                                         SREG();
1102                                                                                 }       break;
1103                                                                                 default: _avr_invalid_opcode(avr);
1104                                                                         }
1105                                                         }
1106                                                 }       break;
1107                                         }
1108                                 }       break;
1109                         }
1110                 }       break;
1111
1112                 case 0xb000: {
1113                         switch (opcode & 0xf800) {
1114                                 case 0xb800: {  // OUT A,Rr 1011 1AAr rrrr AAAA
1115                                         uint8_t r = (opcode >> 4) & 0x1f;
1116                                         uint8_t A = ((((opcode >> 9) & 3) << 4) | ((opcode) & 0xf)) + 32;
1117                                         STATE("out %s, %s[%02x]\n", avr_regname(A), avr_regname(r), avr->data[r]);
1118                                         // todo: store to IO register
1119                                         _avr_set_ram(avr, A, avr->data[r]);
1120                                 //      avr->data[A] = ;
1121                                 }       break;
1122                                 case 0xb000: {  // IN Rd,A 1011 0AAr rrrr AAAA
1123                                         uint8_t r = (opcode >> 4) & 0x1f;
1124                                         uint8_t A = ((((opcode >> 9) & 3) << 4) | ((opcode) & 0xf)) + 32;
1125                                         STATE("in %s, %s[%02x]\n", avr_regname(r), avr_regname(A), avr->data[A]);
1126                                         // todo: get the IO register
1127                                         _avr_set_r(avr, r, _avr_get_ram(avr, A));
1128                                 }       break;
1129                                 default: _avr_invalid_opcode(avr);
1130                         }
1131                 }       break;
1132
1133                 case 0xc000: {
1134                         // RJMP 1100 kkkk kkkk kkkk
1135                         short o = ((short)(opcode << 4)) >> 4;
1136                         STATE("rjmp .%d [%04x]\n", o, new_pc + (o << 1));
1137                         new_pc = new_pc + (o << 1);
1138                         cycle++;
1139                         TRACE_JUMP();
1140                 }       break;
1141
1142                 case 0xd000: {
1143                         // RCALL 1100 kkkk kkkk kkkk
1144                         short o = ((short)(opcode << 4)) >> 4;
1145                         STATE("rcall .%d [%04x]\n", o, new_pc + (o << 1));
1146                         _avr_push16(avr, new_pc >> 1);
1147                         new_pc = new_pc + (o << 1);
1148                         cycle += 2;
1149                         // 'rcall .1' is used as a cheap "push 16 bits of room on the stack"
1150                         if (o != 0) {
1151                                 TRACE_JUMP();
1152                                 STACK_FRAME_PUSH();
1153                         }
1154                 }       break;
1155
1156                 case 0xe000: {  // LDI Rd, K 1110 KKKK RRRR KKKK -- aka SER (LDI r, 0xff)
1157                         uint8_t d = 16 + ((opcode >> 4) & 0xf);
1158                         uint8_t k = ((opcode & 0x0f00) >> 4) | (opcode & 0xf);
1159                         STATE("ldi %s, 0x%02x\n", avr_regname(d), k);
1160                         _avr_set_r(avr, d, k);
1161                 }       break;
1162
1163                 case 0xf000: {
1164                         switch (opcode & 0xfe00) {
1165                                 case 0xf000:
1166                                 case 0xf200:
1167                                 case 0xf400:
1168                                 case 0xf600: {  // All the SREG branches
1169                                         short o = ((short)(opcode << 6)) >> 9; // offset
1170                                         uint8_t s = opcode & 7;
1171                                         int set = (opcode & 0x0400) == 0;               // this bit means BRXC otherwise BRXS
1172                                         int branch = (avr->sreg[s] && set) || (!avr->sreg[s] && !set);
1173                                         const char *names[2][8] = {
1174                                                         { "brcc", "brne", "brpl", "brvc", NULL, "brhc", "brtc", "brid"},
1175                                                         { "brcs", "breq", "brmi", "brvs", NULL, "brhs", "brts", "brie"},
1176                                         };
1177                                         if (names[set][s]) {
1178                                                 STATE("%s .%d [%04x]\t; Will%s branch\n", names[set][s], o, new_pc + (o << 1), branch ? "":" not");
1179                                         } else {
1180                                                 STATE("%s%c .%d [%04x]\t; Will%s branch\n", set ? "brbs" : "brbc", _sreg_bit_name[s], o, new_pc + (o << 1), branch ? "":" not");
1181                                         }
1182                                         if (branch) {
1183                                                 cycle++;
1184                                                 new_pc = new_pc + (o << 1);
1185                                         }
1186                                 }       break;
1187                                 case 0xf800:
1188                                 case 0xf900: {  // BLD – Bit Store from T into a Bit in Register 1111 100r rrrr 0bbb
1189                                         uint8_t r = (opcode >> 4) & 0x1f; // register index
1190                                         uint8_t s = opcode & 7;
1191                                         uint8_t v = avr->data[r] | (avr->sreg[S_T] ? (1 << s) : 0);
1192                                         STATE("bld %s[%02x], 0x%02x = %02x\n", avr_regname(r), avr->data[r], 1 << s, v);
1193                                         _avr_set_r(avr, r, v);
1194                                 }       break;
1195                                 case 0xfa00:
1196                                 case 0xfb00:{   // BST – Bit Store into T from bit in Register 1111 100r rrrr 0bbb
1197                                         uint8_t r = (opcode >> 4) & 0x1f; // register index
1198                                         uint8_t s = opcode & 7;
1199                                         STATE("bst %s[%02x], 0x%02x\n", avr_regname(r), avr->data[r], 1 << s);
1200                                         avr->sreg[S_T] = (avr->data[r] >> s) & 1;
1201                                         SREG();
1202                                 }       break;
1203                                 case 0xfc00:
1204                                 case 0xfe00: {  // SBRS/SBRC – Skip if Bit in Register is Set/Clear 1111 11sr rrrr 0bbb
1205                                         uint8_t r = (opcode >> 4) & 0x1f; // register index
1206                                         uint8_t s = opcode & 7;
1207                                         int set = (opcode & 0x0200) != 0;
1208                                         int branch = ((avr->data[r] & (1 << s)) && set) || (!(avr->data[r] & (1 << s)) && !set);
1209                                         STATE("%s %s[%02x], 0x%02x\t; Will%s branch\n", set ? "sbrs" : "sbrc", avr_regname(r), avr->data[r], 1 << s, branch ? "":" not");
1210                                         if (branch)
1211                                                 new_pc = new_pc + 2;
1212                                 }       break;
1213                                 default: _avr_invalid_opcode(avr);
1214                         }
1215                 }       break;
1216
1217                 default: _avr_invalid_opcode(avr);
1218
1219         }
1220         avr->cycle += cycle;
1221         return new_pc;
1222 }
1223
1224