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