src/Instructions.cpp

   1 /*
   2  *      avr-sim: An atmel AVR simulator
   3  *  Copyright (C) 2008  Tom Haber
   4  *
   5  *  This program is free software: you can redistribute it and/or modify
   6  *  it under the terms of the GNU General Public License as published by
   7  *  the Free Software Foundation, either version 3 of the License, or
   8  *  (at your option) any later version.
   9
  10  *  This program is distributed in the hope that it will be useful,
  11  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  *  GNU General Public License for more details.
  14  *
  15  *  You should have received a copy of the GNU General Public License
  16  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17  */
  18
  19 #include "Instructions.h"
  20 #include "Core.h"
  21 #include "DecoderHelp.h"
  22 #include "ImplementationException.h"
  23
  24 #define Xl 26
  25 #define Xh 27
  26 #define Yl 28
  27 #define Yh 29
  28 #define Zl 30
  29 #define Zh 31
  30 #define R0 0
  31 #define R1 1
  32 #define EIND 0x10 //TODO
  33
  34 namespace avr {
  35
  36         namespace op {
  37
  38                 class SReg {
  39                         public:
  40                                 SReg(Core *c) : c(c) {
  41                                         reg = c->readStatus();
  42                                 }
  43                                 ~SReg() {
  44                                         c->writeStatus(reg);
  45                                 }
  46
  47                         public:
  48                                 union {
  49                                     byte reg;
  50                                     struct {
  51                                         byte C:1;
  52                                         byte Z:1;
  53                                         byte N:1;
  54                                         byte V:1;
  55                                         byte S:1;
  56                                         byte H:1;
  57                                         byte T:1;
  58                                         byte I:1;
  59                                     };
  60                                 };
  61
  62                         private:
  63                                 Core *c;
  64                 };
  65
  66                 static int get_add_carry( byte res, byte rd, byte rr, int b ) {
  67                     byte resb = res >> b & 0x1;
  68                     byte rdb  = rd  >> b & 0x1;
  69                     byte rrb  = rr  >> b & 0x1;
  70                     return (rdb & rrb) | (rrb & ~resb) | (~resb & rdb);
  71                 }
  72
  73                 static int get_add_overflow( byte res, byte rd, byte rr ) {
  74                     byte res7 = res >> 7 & 0x1;
  75                     byte rd7  = rd  >> 7 & 0x1;
  76                     byte rr7  = rr  >> 7 & 0x1;
  77                     return (rd7 & rr7 & ~res7) | (~rd7 & ~rr7 & res7);
  78                 }
  79
  80                 static int get_sub_carry( byte res, byte rd, byte rr, int b ) {
  81                     byte resb = res >> b & 0x1;
  82                     byte rdb  = rd  >> b & 0x1;
  83                     byte rrb  = rr  >> b & 0x1;
  84                     return (~rdb & rrb) | (rrb & resb) | (resb & ~rdb);
  85                 }
  86
  87                 static int get_sub_overflow( byte res, byte rd, byte rr ) {
  88                     byte res7 = res >> 7 & 0x1;
  89                     byte rd7  = rd  >> 7 & 0x1;
  90                     byte rr7  = rr  >> 7 & 0x1;
  91                     return (rd7 & ~rr7 & ~res7) | (~rd7 & rr7 & res7);
  92                 }
  93
  94                 static int get_compare_carry( byte res, byte rd, byte rr, int b ) {
  95                     byte resb = res >> b & 0x1;
  96                     byte rdb  = rd  >> b & 0x1;
  97                     byte rrb  = rr  >> b & 0x1;
  98                     return (~rdb & rrb) | (rrb & resb) | (resb & ~rdb);
  99                 }
 100
 101                 static int get_compare_overflow( byte res, byte rd, byte rr ) {
 102                     byte res7 = res >> 7 & 0x1;
 103                     byte rd7  = rd  >> 7 & 0x1;
 104                     byte rr7  = rr  >> 7 & 0x1;
 105                     /* The atmel data sheet says the second term is ~rd7 for CP
 106                      * but that doesn't make any sense. You be the judge. */
 107                     return (rd7 & ~rr7 & ~res7) | (~rd7 & rr7 & res7);
 108                 }
 109
 110                 static int n_bit_unsigned_to_signed( unsigned int val, int n ) {
 111                     /* Convert n-bit unsigned value to a signed value. */
 112                     unsigned int mask;
 113
 114                     if ( (val & (1 << (n-1))) == 0)
 115                         return (int)val;
 116
 117                     /* manually calculate two's complement */
 118                     mask = (1 << n) - 1;
 119                     return -1 * ((~val & mask) + 1);
 120                 }
 121
 122                 ADC::ADC(word opcode) :
 123                         Rd( get_rd_5(opcode) ),
 124                         Rr( get_rr_5(opcode) ) {
 125                 }
 126
 127                 int ADC::operator ()(Core *core) const {
 128                         SReg status( core );
 129                     unsigned char rd = core->readRegister(Rd);
 130                     unsigned char rr = core->readRegister(Rr);
 131                     unsigned char res = rd + rr + status.C;
 132
 133                     status.H = get_add_carry( res, rd, rr, 3 );
 134                     status.V = get_add_overflow( res, rd, rr );
 135                     status.N = ((res >> 7) & 0x1);
 136                     status.S = (status.N ^ status.V);
 137                     status.Z = ((res & 0xff) == 0);
 138                     status.C = get_add_carry( res, rd, rr, 7 );
 139
 140                     core->writeRegister(Rd, res);
 141                     return 1;   //used clocks
 142                 }
 143
 144                 ADD::ADD(word opcode) :
 145                         Rd( get_rd_5(opcode) ),
 146                         Rr( get_rr_5(opcode) ) {
 147                 }
 148
 149                 int ADD::operator ()(Core *core) const {
 150                         SReg status( core );
 151                     unsigned char rd = core->readRegister(Rd);
 152                     unsigned char rr = core->readRegister(Rr);
 153                     unsigned char res = rd + rr ;
 154
 155                     status.H = get_add_carry( res, rd, rr, 3 ) ;
 156                     status.V = get_add_overflow( res, rd, rr ) ;
 157                     status.N = ((res >> 7) & 0x1)              ;
 158                     status.S = (status.N ^ status.V)   ;
 159                     status.Z = ((res & 0xff) == 0)             ;
 160                     status.C = get_add_carry( res, rd, rr, 7 ) ;
 161
 162                     core->writeRegister(Rd, res);
 163                     return 1;   //used clocks
 164                 }
 165
 166                 ADIW::ADIW(word opcode) :
 167                     Rl( get_rd_2(opcode) ),
 168                     Rh( get_rd_2(opcode) + 1 ),
 169                     K( get_K_6(opcode) ) {
 170                 }
 171
 172                 int ADIW::operator()(Core *core) const {
 173                         SReg status( core );
 174                     word rd = (core->readRegister(Rh) << 8) | core->readRegister(Rl);
 175                     word res = rd + K;
 176
 177                     status.V = (((~rd & res) >> 15) & 0x1);
 178                     status.N = ((res >> 15) & 0x1);
 179                     status.S = (status.N ^ status.V);
 180                     status.Z = ((res & 0xffff) == 0);
 181                     status.C = (((~res & rd) >> 15) & 0x1);
 182
 183                     core->writeRegister(Rl, res & 0xff);
 184                     core->writeRegister(Rh, (res >> 8) & 0xff);
 185                     return 2;
 186                 }
 187
 188                 AND::AND(word opcode) :
 189                         Rd( get_rd_5(opcode) ),
 190                         Rr( get_rr_5(opcode) ) {
 191                 }
 192
 193             int AND::operator()(Core *core) const {
 194                 SReg status( core );
 195                         byte res = core->readRegister(Rd) & core->readRegister(Rr);
 196
 197                 status.V = 0;
 198                 status.N = ((res >> 7) & 0x1)  ;
 199                 status.S = (status.N ^ status.V);
 200                 status.Z = ((res & 0xff) == 0);
 201
 202                 core->writeRegister(Rd, res);
 203                 return 1;
 204             }
 205
 206                 ANDI::ANDI(word opcode) :
 207                     Rd( get_rd_4(opcode) ),
 208                     K( get_K_8(opcode) ) {
 209             }
 210
 211                 int ANDI::operator()(Core *core) const {
 212                         SReg status( core );
 213                     byte rd = core->readRegister(Rd);
 214                     byte res = rd & K;
 215
 216                     status.V = 0;
 217                     status.N = ((res >> 7) & 0x1);
 218                     status.S = (status.N ^ status.V);
 219                     status.Z = ((res & 0xff) == 0);
 220
 221                     core->writeRegister(Rd, res);
 222                     return 1;
 223                 }
 224
 225                 ASR::ASR(word opcode) :
 226                     Rd( get_rd_5(opcode) ) {
 227                 }
 228
 229                 int ASR::operator()(Core *core) const {
 230                         SReg status( core );
 231                     byte rd = core->readRegister(Rd);
 232                     byte res = (rd >> 1) + (rd & 0x80);
 233
 234                     status.N = ((res >> 7) & 0x1);
 235                     status.C = (rd & 0x1);
 236                     status.V = (status.N ^ status.C);
 237                     status.S = (status.N ^ status.V);
 238                     status.Z = ((res & 0xff) == 0);
 239
 240                     core->writeRegister(Rd, res);
 241                     return 1;
 242                 }
 243
 244                 BCLR::BCLR(word opcode) :
 245                     K( ~(1<<get_sreg_bit(opcode)) ) {
 246                 }
 247
 248                 int BCLR::operator()(Core *core) const {
 249                         SReg status( core );
 250                     status.reg = status.reg & K;
 251                     return 1;
 252                 }
 253
 254                 BLD::BLD(word opcode) :
 255                         Rd( get_rd_5(opcode) ),
 256                     Kadd(1<<get_reg_bit(opcode)),
 257                     Kremove(~(1<<get_reg_bit(opcode))) {
 258                 }
 259
 260                 int BLD::operator()(Core *core) const {
 261                         SReg status( core );
 262                     byte rd  = core->readRegister(Rd);
 263                     int  T = status.T;
 264                     byte res;
 265
 266                     if( T == 0 )
 267                         res = rd & Kremove;
 268                     else
 269                         res = rd | Kadd;
 270
 271                     core->writeRegister(Rd, res);
 272                     return 1;
 273                 }
 274
 275                 BRBC::BRBC(word opcode) :
 276                     bitmask( 1 << get_reg_bit(opcode) ),
 277                     offset( n_bit_unsigned_to_signed( get_k_7(opcode), 7) ) {
 278                 }
 279
 280                 int BRBC::operator()(Core *core) const {
 281                         SReg status( core );
 282                     int clks;
 283
 284                     if( (status.reg & bitmask) == 0 ) {
 285                         core->jump( offset );
 286                         clks = 2;
 287                     } else {
 288                         clks = 1;
 289                     }
 290
 291                     return clks;
 292                 }
 293
 294
 295                 BRBS::BRBS(word opcode) :
 296                     bitmask( 1 << get_reg_bit(opcode) ),
 297                     offset( n_bit_unsigned_to_signed( get_k_7(opcode), 7) ) {
 298                 }
 299
 300                 int BRBS::operator()(Core *core) const {
 301                         SReg status( core );
 302                     int clks;
 303
 304                     if( (status.reg & bitmask) != 0 ) {
 305                         core->jump( offset );
 306                         clks = 2;
 307                     } else {
 308                         clks = 1;
 309                     }
 310
 311                     return clks;
 312                 }
 313
 314                 BSET::BSET(word opcode) :
 315                         K(1<<get_sreg_bit(opcode)) {
 316                 }
 317
 318                 int BSET::operator()(Core *core) const {
 319                         SReg status( core );
 320                         status.reg = status.reg | K;
 321                     return 1;
 322                 }
 323
 324                 BST::BST(word opcode) :
 325                         Rd( get_rd_5(opcode) ),
 326                         K( 1 << get_reg_bit(opcode) ) {
 327                 }
 328
 329                 int BST::operator()(Core *core) const {
 330                         SReg status( core );
 331                     status.T= ((core->readRegister(Rd) & K) != 0 );
 332                     return 1;
 333                 }
 334
 335                 CALL::CALL(word opcode) :
 336                         KH( get_k_22(opcode) ) {
 337                 }
 338
 339                 int CALL::operator()(Core *core) const {
 340                     word offset = core->fetchOperand();
 341                     dword k = (KH<<16) | offset;
 342
 343                     core->call( k - 1, true );
 344                     return core->pcBytes() + 2;
 345                 }
 346
 347                 CBI::CBI(word opcode) :
 348                         ioreg( get_A_5(opcode) ),
 349                         K( ~( 1 << get_reg_bit(opcode) ) ) {
 350                 }
 351
 352                 int CBI::operator()(Core *core) const {
 353                     core->writeIORegister(ioreg, core->readIORegister(ioreg) & K);
 354                     return 2;
 355                 }
 356
 357                 COM::COM(word opcode) :
 358                     Rd( get_rd_5(opcode) ) {
 359                 }
 360
 361                 int COM::operator()(Core *core) const {
 362                         SReg status( core );
 363                     byte rd  = core->readRegister(Rd);
 364                     byte res = 0xff - rd;
 365
 366                     status.N = ((res >> 7) & 0x1) ;
 367                     status.C = 1 ;
 368                     status.V = 0 ;
 369                     status.S = (status.N ^ status.V) ;
 370                     status.Z = ((res & 0xff) == 0) ;
 371
 372                     core->writeRegister(Rd, res);
 373                     return 1;
 374                 }
 375
 376                 CP::CP(word opcode) :
 377                         Rd( get_rd_5(opcode) ),
 378                         Rr( get_rr_5(opcode) ) {
 379                 }
 380
 381                 int CP::operator()(Core *core) const {
 382                         SReg status( core );
 383                     byte rd  = core->readRegister(Rd);
 384                     byte rr  = core->readRegister(Rr);
 385                     byte res = rd - rr;
 386
 387                     status.H = get_compare_carry( res, rd, rr, 3 ) ;
 388                     status.V = get_compare_overflow( res, rd, rr ) ;
 389                     status.N = ((res >> 7) & 0x1);
 390                     status.S = (status.N ^ status.V);
 391                     status.Z = ((res & 0xff) == 0) ;
 392                     status.C = get_compare_carry( res, rd, rr, 7 ) ;
 393
 394                     return 1;
 395                 }
 396
 397                 CPC::CPC(word opcode) :
 398                         Rd( get_rd_5(opcode) ),
 399                         Rr( get_rr_5(opcode) ) {
 400                 }
 401
 402                 int CPC::operator()(Core *core) const {
 403                         SReg status( core );
 404                     byte rd  = core->readRegister(Rd);
 405                     byte rr  = core->readRegister(Rr);
 406                     byte res = rd - rr - status.C;
 407
 408                     status.H = get_compare_carry( res, rd, rr, 3 ) ;
 409                     status.V = get_compare_overflow( res, rd, rr ) ;
 410                     status.N = ((res >> 7) & 0x1) ;
 411                     status.S = (status.N ^ status.V) ;
 412                     status.C = get_compare_carry( res, rd, rr, 7 ) ;
 413
 414                     /* Previous value remains unchanged when result is 0; cleared otherwise */
 415                     bool Z = ((res & 0xff) == 0);
 416                     bool prev_Z = status.Z;
 417                     status.Z = Z && prev_Z ;
 418
 419                     return 1;
 420                 }
 421
 422
 423                 CPI::CPI(word opcode) :
 424                         Rd( get_rd_4(opcode) ),
 425                         K(get_K_8(opcode)) {
 426                 }
 427
 428                 int CPI::operator()(Core *core) const{
 429                         SReg status( core );
 430                     byte rd  = core->readRegister(Rd);
 431                     byte res = rd - K;
 432
 433                     status.H = get_compare_carry( res, rd, K, 3 ) ;
 434                     status.V = get_compare_overflow( res, rd, K ) ;
 435                     status.N = ((res >> 7) & 0x1) ;
 436                     status.S = (status.N ^ status.V) ;
 437                     status.Z = ((res & 0xff) == 0) ;
 438                     status.C = get_compare_carry( res, rd, K, 7 ) ;
 439
 440                     return 1;
 441                 }
 442
 443                 CPSE::CPSE(word opcode) :
 444                         Rd( get_rd_5(opcode) ),
 445                         Rr( get_rr_5(opcode) ) {
 446                 }
 447
 448                 int CPSE::operator()(Core *core) const {
 449                     byte rd = core->readRegister(Rd);
 450                     byte rr = core->readRegister(Rr);
 451
 452                     int clks = 1;
 453                     if( rd == rr )
 454                         clks += core->skip();
 455
 456                     return clks;
 457                 }
 458
 459                 DEC::DEC(word opcode) :
 460                         Rd( get_rd_5(opcode) ) {
 461                 }
 462
 463                 int DEC::operator()(Core *core) const {
 464                         SReg status( core );
 465                     byte res = core->readRegister(Rd) - 1;
 466
 467                     status.N = ((res >> 7) & 0x1) ;
 468                     status.V = (res == 0x7f) ;
 469                     status.S = (status.N ^ status.V) ;
 470                     status.Z = ((res & 0xff) == 0) ;
 471
 472                     core->writeRegister(Rd, res);
 473                     return 1;
 474                 }
 475
 476                 EICALL:: EICALL(word /*opcode*/) :
 477                         eind( EIND ) {
 478                 }
 479
 480                 int  EICALL::operator()(Core *core) const {
 481                         // TODO
 482                         throw util::ImplementationException("Instruction EICALL not fully implemented");
 483                     int new_pc = core->readRegister(Zl) |
 484                                         (core->readRegister(Zh)<<8) |
 485                                         core->readIORegister(eind) << 16;
 486                     core->call( new_pc, true );
 487                     return 4;
 488                 }
 489
 490                 EIJMP::EIJMP(word /*opcode*/) :
 491                         eind( EIND ) {
 492                 }
 493
 494                 int EIJMP::operator()(Core *core) const {
 495                         // TODO
 496                         throw util::ImplementationException("Instruction EIJMP not fully implemented");
 497                     int new_pc = core->readRegister(Zl) |
 498                                         (core->readRegister(Zh)<<8) |
 499                                         (core->readIORegister(eind) & 0x3f) << 16;
 500                     core->call( new_pc, true );
 501                     return 2;
 502                 }
 503
 504                 ELPM_Z::ELPM_Z(word opcode) :
 505                         Rd( get_rd_5(opcode) ) {
 506                 }
 507
 508                 int ELPM_Z::operator()(Core *core) const {
 509                     word Z;
 510
 511                     // TODO
 512                     throw util::ImplementationException("Instruction ELPM_Z not fully implemented");
 513 #if 0
 514                     Z = ((core->GetRampz() & 0x3f) << 16) |
 515                         (Zh << 8) | Zl;
 516 #endif
 517
 518                     core->writeRegister( Rd, core->readFlash(Z) );
 519                     return 3;
 520                 }
 521
 522                 ELPM_Z_incr::ELPM_Z_incr(word opcode) :
 523                         Rd( get_rd_5(opcode) ) {
 524                 }
 525
 526                 int ELPM_Z_incr::operator()(Core *core) const {
 527                     word Z;
 528
 529                     // TODO
 530                     throw util::ImplementationException("Instruction ELPM_Z_incr not fully implemented");
 531 #if 0
 532                     Z = ((core->GetRampz() & 0x3f) << 16) |
 533                         (Zh << 8) | Zl;
 534 #endif
 535
 536                     core->writeRegister( Rd, core->readFlash(Z) );
 537
 538                     /* post increment Z */
 539                     Z += 1;
 540 #if 0
 541                     core->SetRampz((Z >> 16) & 0x3f);
 542 #endif
 543                     core->writeRegister(Zl, Z & 0xff);
 544                     core->writeRegister(Zh, (Z>>8 & 0xff));
 545                     return 3;
 546                 }
 547
 548                 ELPM::ELPM(word /*opcode*/) {}
 549
 550                 int ELPM::operator()(Core *core) const {
 551                     word Z;
 552
 553                     // TODO
 554                     throw util::ImplementationException("Instruction ELPM not fully implemented");
 555 #if 0
 556                     Z = ((core->GetRampz() & 0x3f) << 16) |
 557                         (Zh << 8) | Zl;
 558 #endif
 559
 560                     core->writeRegister( R0, core->readFlash(Z) );
 561                     return 3;
 562                 }
 563
 564                 EOR::EOR(word opcode) :
 565                         Rd( get_rd_5(opcode) ),
 566                         Rr( get_rr_5(opcode) ) {
 567                 }
 568
 569                 int EOR::operator()(Core *core) const {
 570                         SReg status( core );
 571                     byte rd = core->readRegister(Rd);
 572                     byte rr = core->readRegister(Rr);
 573
 574                     byte res = rd ^ rr;
 575
 576                     status.V = 0 ;
 577                     status.N = ((res >> 7) & 0x1) ;
 578                     status.S = (status.N ^ status.V) ;
 579                     status.Z = ((res & 0xff) == 0 ) ;
 580
 581                     core->writeRegister(Rd, res);
 582                     return 1;
 583                 }
 584
 585                 ESPM::ESPM(word /*opcode*/) {}
 586
 587                 int ESPM::operator()(Core * /*core*/) const {
 588                     return 0;
 589                 }
 590
 591                 FMUL::FMUL(word opcode) :
 592                         Rd( get_rd_3(opcode) ),
 593                         Rr( get_rr_3(opcode) ) {
 594                 }
 595
 596                 int FMUL::operator()(Core *core) const {
 597                         SReg status( core );
 598                     byte rd =  core->readRegister(Rd);
 599                     byte rr =  core->readRegister(Rr);
 600
 601                     word resp = rd * rr;
 602                     word res = resp << 1;
 603
 604                     status.Z = ((res & 0xffff) == 0) ;
 605                     status.C = ((resp >> 15) & 0x1);
 606
 607                     /* result goes in R1:R0 */
 608                     core->writeRegister(R0, res & 0xff);
 609                     core->writeRegister(R1, (res >> 8) & 0xff);
 610                     return 2;
 611                 }
 612
 613                 FMULS::FMULS(word opcode) :
 614                         Rd( get_rd_3(opcode) ),
 615                         Rr( get_rr_3(opcode) ) {
 616                 }
 617
 618                 int FMULS::operator()(Core *core) const {
 619                         SReg status( core );
 620                     sbyte rd = core->readRegister(Rd);
 621                     sbyte rr = core->readRegister(Rr);
 622
 623                     word resp = rd * rr;
 624                     word res = resp << 1;
 625
 626                     status.Z = ((res & 0xffff) == 0) ;
 627                     status.C = ((resp >> 15) & 0x1) ;
 628
 629                     /* result goes in R1:R0 */
 630                     core->writeRegister(R0, res & 0xff);
 631                     core->writeRegister(R1, (res >> 8) & 0xff);
 632                     return 2;
 633                 }
 634
 635                 FMULSU::FMULSU(word opcode) :
 636                         Rd( get_rd_3(opcode) ),
 637                         Rr( get_rr_3(opcode) ) {
 638                 }
 639
 640                 int FMULSU::operator()(Core *core) const {
 641                         SReg status( core );
 642                     sbyte rd = core->readRegister(Rd);
 643                     sbyte rr = core->readRegister(Rr);
 644
 645                     word resp = rd * rr;
 646                     word res = resp << 1;
 647
 648                     status.Z = ((res & 0xffff) == 0) ;
 649                     status.C = ((resp >> 15) & 0x1) ;
 650
 651                     /* result goes in R1:R0 */
 652                     core->writeRegister(R0, res & 0xff);
 653                     core->writeRegister(R1, (res >> 8) & 0xff);
 654                     return 2;
 655                 }
 656
 657                 ICALL::ICALL(word /*opcode*/) {}
 658
 659                 int ICALL::operator()(Core *core) const {
 660                     /* Z is R31:R30 */
 661                     int new_pc = (core->readRegister(Zh) << 8) | core->readRegister(Zl);
 662
 663                     core->call( new_pc - 1, true );
 664                     return core->pcBytes() + 1;
 665                 }
 666
 667
 668                 IJMP::IJMP(word /*opcode*/) {}
 669
 670                 int IJMP::operator()(Core *core) const {
 671                         int new_pc = (core->readRegister(Zh) << 8) | core->readRegister(Zl);
 672                     core->call( new_pc - 1, false );
 673
 674                     return 2;
 675                 }
 676
 677                 IN::IN(word opcode) :
 678                         Rd( get_rd_5(opcode) ),
 679                         ioreg( get_A_6(opcode) ) {
 680                 }
 681
 682                 int IN::operator()(Core *core) const {
 683                         core->writeRegister(Rd, core->readIORegister(ioreg) );
 684                     return 1;
 685                 }
 686
 687                 INC::INC(word opcode) :
 688                         Rd( get_rd_5(opcode) ) {
 689                 }
 690
 691                 int INC::operator()(Core *core) const {
 692                         SReg status( core );
 693                     byte rd  = core->readRegister(Rd);
 694                     byte res = rd + 1;
 695
 696                     status.N = ((res >> 7) & 0x1) ;
 697                     status.V = (rd == 0x7f) ;
 698                     status.S = (status.N ^ status.V) ;
 699                     status.Z = ((res & 0xff) == 0) ;
 700
 701                     core->writeRegister(Rd, res);
 702                     return 1;
 703                 }
 704
 705                 JMP::JMP(word opcode) :
 706                         K( get_k_22(opcode) ) {
 707                 }
 708
 709                 int JMP::operator()(Core *core) const {
 710                     word offset = core->fetchOperand();
 711                     core->call( K + offset - 1, false );
 712                     return 3;
 713                 }
 714
 715                 LDD_Y::LDD_Y(word opcode) :
 716                         Rd( get_rd_5(opcode) ),
 717                         K( get_q(opcode) ) {
 718                 }
 719
 720                 int LDD_Y::operator()(Core *core) const {
 721                     /* Y is R29:R28 */
 722                     word Y = ( core->readRegister(Yh) << 8) | core->readRegister(Yl);
 723                     byte res = core->readByte(Y+K);
 724                     core->writeRegister(Rd, res);
 725                     return 2;
 726                 }
 727
 728                 LDD_Z::LDD_Z(word opcode):
 729                         Rd( get_rd_5(opcode) ),
 730                         K( get_q(opcode) ) {
 731                 }
 732
 733                 int LDD_Z::operator()(Core *core) const {
 734                     /* Z is R31:R30 */
 735                     word Z = ( core->readRegister(Zh) << 8) | core->readRegister(Zl);
 736                     byte res = core->readByte(Z+K);
 737                     core->writeRegister(Rd, res);
 738                     return 2;
 739                 }
 740
 741                 LDI::LDI(word opcode) :
 742                         Rd( get_rd_4(opcode) ),
 743                         K( get_K_8(opcode) ) {
 744                 }
 745
 746                 int LDI::operator()(Core *core) const {
 747                     core->writeRegister(Rd, K);
 748                     return 1;
 749                 }
 750
 751                 LDS::LDS(word opcode) :
 752                         Rd( get_rd_5(opcode) ) {
 753                 }
 754
 755                 int LDS::operator()(Core *core) const {
 756                         /* TODO
 757                          * The LDS instruction uses the RAMPD register to access memory
 758                          * above 64K
 759                          */
 760                     /* Get data at k in current data segment and put into Rd */
 761                     word offset = core->fetchOperand();
 762                     byte res = core->readByte(offset);
 763                     core->writeRegister(Rd, res);
 764                     return 2;
 765                 }
 766
 767                 LD_X::LD_X(word opcode) :
 768                         Rd( get_rd_5(opcode) ) {
 769                 }
 770
 771                 int LD_X::operator()(Core *core) const {
 772                     /* X is R27:R26 */
 773                     word X = ( core->readRegister(Xh) << 8) | core->readRegister(Xl);
 774                     byte res = core->readByte(X);
 775                     core->writeRegister(Rd, res);
 776                     return 2;
 777                 }
 778
 779                 LD_X_decr::LD_X_decr(word opcode) : LD_X(opcode) {}
 780
 781                 int LD_X_decr::operator()(Core *core) const {
 782                     /* X is R27:R26 */
 783                     word X = ( core->readRegister(Xh) << 8) | core->readRegister(Xl);
 784
 785                     /* Perform pre-decrement */
 786                     X -= 1;
 787
 788                     byte res = core->readByte(X);
 789                     core->writeRegister(Rd, res);
 790
 791                     core->writeRegister(Xl, X & 0xff);
 792                     core->writeRegister(Xh, (X>>8) & 0xff);
 793                     return 2;
 794                 }
 795
 796                 LD_X_incr::LD_X_incr(word opcode) : LD_X(opcode) {}
 797
 798                 int LD_X_incr::operator()(Core *core) const {
 799                     /* X is R27:R26 */
 800                     word X = ( core->readRegister(Xh) << 8) | core->readRegister(Xl);
 801
 802                     byte res = core->readByte(X);
 803                     core->writeRegister(Rd, res);
 804
 805                     /* Perform post-increment */
 806                     X += 1;
 807                     core->writeRegister(Xl, X & 0xff);
 808                     core->writeRegister(Xh, (X>>8) & 0xff);
 809                     return 2;
 810                 }
 811
 812
 813                 LD_Y_decr::LD_Y_decr(word opcode) :
 814                         Rd( get_rd_5(opcode) ) {
 815                 }
 816
 817                 int LD_Y_decr::operator()(Core *core) const {
 818                     /* Y is R29:R28 */
 819                     word Y = ( core->readRegister(Yh) << 8) | core->readRegister(Yl);
 820
 821                     /* Perform pre-decrement */
 822                     Y -= 1;
 823                     byte res = core->readByte(Y);
 824                     core->writeRegister(Rd, res);
 825
 826                     core->writeRegister(Yl, Y & 0xff);
 827                     core->writeRegister(Yh, (Y>>8) & 0xff);
 828                     return 2;
 829                 }
 830
 831
 832                 LD_Y_incr::LD_Y_incr(word opcode) :
 833                         Rd( get_rd_5(opcode) ) {
 834                 }
 835
 836                 int LD_Y_incr::operator()(Core *core) const {
 837                     /* Y is R29:R28 */
 838                     word Y = ( core->readRegister(Yh) << 8) | core->readRegister(Yl);
 839
 840                     byte res = core->readByte(Y);
 841                     core->writeRegister(Rd, res);
 842
 843                     /* Post-increment */
 844                     Y += 1;
 845                     core->writeRegister(Yl, Y & 0xff);
 846                     core->writeRegister(Yh, (Y>>8) & 0xff);
 847                     return 2;
 848                 }
 849
 850                 LD_Z_incr::LD_Z_incr(word opcode) :
 851                         Rd( get_rd_5(opcode) ) {
 852                 }
 853
 854                 int LD_Z_incr::operator()(Core *core) const {
 855                     /* Z is R31:R30 */
 856                     word Z = ( core->readRegister(Zh) << 8) | core->readRegister(Zl);
 857
 858                     byte res = core->readByte(Z);
 859                     core->writeRegister(Rd, res);
 860
 861                     /* Perform post-increment */
 862                     Z += 1;
 863                     core->writeRegister(Zl, Z & 0xff);
 864                     core->writeRegister(Zh, (Z>>8) & 0xff);
 865                     return 2;
 866                 }
 867
 868                 LD_Z_decr::LD_Z_decr(word opcode) :
 869                         Rd( get_rd_5(opcode) ) {
 870                 }
 871
 872                 int LD_Z_decr::operator()(Core *core) const {
 873                     /* Z is R31:R30 */
 874                     word Z = ( core->readRegister(Zh) << 8) | core->readRegister(Zl);
 875
 876                     /* Perform pre-decrement */
 877                     Z -= 1;
 878
 879                     byte res = core->readByte(Z);
 880                     core->writeRegister(Rd, res);
 881
 882                     core->writeRegister(Zl, Z & 0xff);
 883                     core->writeRegister(Zh, (Z>>8) & 0xff);
 884                     return 2;
 885                 }
 886
 887                 LPM_Z::LPM_Z(word opcode) :
 888                         Rd( get_rd_5(opcode) ) {
 889                 }
 890
 891                 int LPM_Z::operator()(Core *core) const {
 892                     /* Z is R31:R30 */
 893                     word Z = ( core->readRegister(Zh) << 8) | core->readRegister(Zl);
 894
 895                     byte data = core->readFlash(Z);
 896                     core->writeRegister(Rd, data);
 897                     return 3;
 898                 }
 899
 900                 LPM::LPM(word /*opcode*/) {}
 901
 902                 int LPM::operator()(Core *core) const {
 903                     /* Z is R31:R30 */
 904                     word Z = ( core->readRegister(Zh) << 8) | core->readRegister(Zl);
 905
 906                     byte data = core->readFlash(Z);
 907                     core->writeRegister(R0, data);
 908                     return 3;
 909                 }
 910
 911                 LPM_Z_incr::LPM_Z_incr(word opcode) :
 912                         Rd( get_rd_5(opcode) ) {
 913                 }
 914
 915                 int LPM_Z_incr::operator()(Core *core) const {
 916                     /* Z is R31:R30 */
 917                     word Z = ( core->readRegister(Zh) << 8) | core->readRegister(Zl);
 918
 919                     byte data = core->readFlash(Z);
 920                     core->writeRegister(Rd, data);
 921
 922                     Z += 1;
 923                     core->writeRegister(Zl, Z & 0xff);
 924                     core->writeRegister(Zh, (Z>>8) & 0xff);
 925                     return 3;
 926                 }
 927
 928                 LSR::LSR(word opcode) :
 929                         Rd( get_rd_5(opcode) ) {
 930                 }
 931
 932                 int LSR::operator()(Core *core) const {
 933                         SReg status( core );
 934                     byte rd = core->readRegister(Rd);
 935                     byte res = (rd >> 1) & 0x7f;
 936
 937                     status.C = (rd & 0x1) ;
 938                     status.N = (0) ;
 939                     status.V = (status.N ^ status.C) ;
 940                     status.S = (status.N ^ status.V) ;
 941                     status.Z = ((res & 0xff) == 0) ;
 942
 943                     core->writeRegister(Rd, res);
 944                     return 1;
 945                 }
 946
 947                 MOV::MOV(word opcode) :
 948                         Rd( get_rd_5(opcode) ),
 949                         Rr( get_rr_5(opcode) ) {
 950                 }
 951
 952                 int MOV::operator()(Core *core) const {
 953                     core->writeRegister(Rd, core->readRegister(Rr) );
 954                     return 1;
 955                 }
 956
 957                 MOVW::MOVW(word opcode) :
 958                         Rdl( (get_rd_4(opcode) - 16)<<1 ),
 959                         Rdh( ((get_rd_4(opcode) - 16)<<1) + 1),
 960                         Rrl( (get_rr_4(opcode) - 16)<<1 ),
 961                         Rrh( ((get_rr_4(opcode) - 16)<<1) + 1) {
 962                 }
 963
 964                 int MOVW::operator()(Core *core) const {
 965                         core->writeRegister(Rdl, core->readRegister(Rrl) );
 966                         core->writeRegister(Rdh, core->readRegister(Rrh) );
 967                     return 1;
 968                 }
 969
 970                 MUL::MUL(word opcode) :
 971                         Rd( get_rd_5(opcode) ),
 972                         Rr( get_rr_5(opcode) ) {
 973                 }
 974
 975                 int MUL::operator()(Core *core) const {
 976                         SReg status( core );
 977                     byte rd = core->readRegister(Rd);
 978                     byte rr = core->readRegister(Rr);
 979
 980                     word res = rd * rr;
 981
 982                     status.Z = ((res & 0xffff) == 0);
 983                     status.C = ((res >> 15) & 0x1);
 984
 985                     /* result goes in R1:R0 */
 986                     core->writeRegister(R0, res & 0xff);
 987                     core->writeRegister(R1, (res>>8) & 0xff);
 988                     return 2;
 989                 }
 990
 991                 MULS::MULS(word opcode) :
 992                         Rd( get_rd_4(opcode) ),
 993                         Rr( get_rr_4(opcode) ) {
 994                 }
 995
 996                 int MULS::operator()(Core *core) const {
 997                         SReg status( core );
 998                     sbyte rd = (sbyte)core->readRegister(Rd);
 999                     sbyte rr = (sbyte)core->readRegister(Rr);
1000                     sword res = rd * rr;
1001
1002                     status.Z = ((res & 0xffff) == 0) ;
1003                     status.C = ((res >> 15) & 0x1) ;
1004
1005                     /* result goes in R1:R0 */
1006                     core->writeRegister(R0, res & 0xff);
1007                     core->writeRegister(R1, (res>>8) & 0xff);
1008                     return 2;
1009                 }
1010
1011                 MULSU::MULSU(word opcode) :
1012                         Rd( get_rd_3(opcode) ),
1013                         Rr( get_rr_3(opcode) ) {
1014                 }
1015
1016                 int MULSU::operator()(Core *core) const {
1017                         SReg status( core );
1018                     sbyte rd = (sbyte)core->readRegister(Rd);
1019                     byte rr = core->readRegister(Rr);
1020
1021                     sword res = rd * rr;
1022
1023                     status.Z = ((res & 0xffff) == 0);
1024                     status.C = ((res >> 15) & 0x1);
1025
1026                     /* result goes in R1:R0 */
1027                     core->writeRegister(R0, res & 0xff);
1028                     core->writeRegister(R1, (res>>8) & 0xff);
1029                     return 2;
1030                 }
1031
1032                 NEG::NEG(word opcode) :
1033                         Rd( get_rd_5(opcode) ) {
1034                 }
1035
1036                 int NEG::operator()(Core *core) const {
1037                         SReg status( core );
1038                     byte rd  = core->readRegister(Rd);
1039                     byte res = (0x0 - rd) & 0xff;
1040
1041                     status.H = (((res >> 3) | (rd >> 3)) & 0x1);
1042                     status.V = (res == 0x80);
1043                     status.N = ((res >> 7) & 0x1);
1044                     status.S = (status.N ^ status.V);
1045                     status.Z = (res == 0x0);
1046                     status.C = (res != 0x0);
1047
1048                     core->writeRegister(Rd, res);
1049                     return 1;
1050                 }
1051
1052                 NOP::NOP(word /*opcode*/) {}
1053
1054                 int NOP::operator()(Core */*core*/) const {
1055                     return 1;
1056                 }
1057
1058                 OR::OR(word opcode) :
1059                         Rd( get_rd_5(opcode) ),
1060                         Rr(get_rr_5(opcode) ) {
1061                 }
1062
1063                 int OR::operator()(Core *core) const{
1064                         SReg status( core );
1065                     byte res = core->readRegister(Rd) | core->readRegister(Rr);
1066
1067                     status.V = (0);
1068                     status.N = ((res >> 7) & 0x1);
1069                     status.S = (status.N ^ status.V);
1070                     status.Z = (res == 0x0);
1071
1072                     core->writeRegister(Rd, res);
1073                     return 1;
1074                 }
1075
1076                 ORI::ORI(word opcode) :
1077                         Rd( get_rd_4(opcode) ),
1078                         K( get_K_8(opcode) ) {
1079                 }
1080
1081                 int ORI::operator()(Core *core) const{
1082                         SReg status( core );
1083                     byte res = core->readRegister(Rd) | K;
1084
1085                     status.V = (0);
1086                     status.N = ((res >> 7) & 0x1);
1087                     status.S = (status.N ^ status.V);
1088                     status.Z = (res == 0x0);
1089
1090                     core->writeRegister(Rd, res);
1091                     return 1;
1092                 }
1093
1094                 OUT::OUT(word opcode) :
1095                         Rd( get_rd_5(opcode) ),
1096                         ioreg( get_A_6(opcode) ) {
1097                 }
1098
1099                 int OUT::operator()(Core *core) const {
1100                     core->writeIORegister(ioreg, core->readRegister(Rd));
1101                     return 1;
1102                 }
1103
1104                 POP::POP(word opcode) :
1105                         Rd( get_rd_5(opcode) ) {
1106                 }
1107
1108                 int POP::operator()(Core *core) const {
1109                         core->writeRegister(Rd, core->pop());
1110                     return 2;
1111                 }
1112
1113                 PUSH::PUSH(word opcode) :
1114                         Rd( get_rd_5(opcode) ) {
1115                 }
1116
1117                 int PUSH::operator()(Core *core) const {
1118                         core->push( core->readRegister(Rd) );
1119                     return 2;
1120                 }
1121
1122                 RCALL::RCALL(word opcode) :
1123                         K( n_bit_unsigned_to_signed( get_k_12(opcode), 12 ) ) {
1124                 }
1125
1126                 int RCALL::operator()(Core *core) const {
1127                         int cost = core->pcBytes();
1128                         core->jump(K, true);
1129                     return cost + 1;
1130                 }
1131
1132                 RET::RET(word /*opcode*/) { }
1133
1134                 int RET::operator()(Core *core) const {
1135                         int cost = core->pcBytes();
1136                         core->ret(false);
1137                     return cost + 2;
1138                 }
1139
1140                 RETI::RETI(word /*opcode*/) {}
1141
1142                 int RETI::operator()(Core *core) const {
1143                         SReg status( core );
1144                         int cost = core->pcBytes();
1145                         core->ret(true);
1146                         status.I = 1;
1147                     return cost + 2;
1148                 }
1149
1150                 RJMP::RJMP(word opcode) :
1151                         K( n_bit_unsigned_to_signed( get_k_12(opcode), 12 ) ) {
1152                 }
1153
1154                 int RJMP::operator()(Core *core) const {
1155                         core->jump( K, false );
1156                     return 2;
1157                 }
1158
1159                 ROR::ROR(word opcode) :
1160                         Rd( get_rd_5(opcode) ) {
1161                 }
1162
1163                 int ROR::operator()(Core *core) const {
1164                         SReg status( core );
1165                     byte rd = core->readRegister(Rd);
1166
1167                     byte res = (rd >> 1) | ((( status.C ) << 7) & 0x80);
1168
1169                     status.C = (rd & 0x1) ;
1170                     status.N = ((res >> 7) & 0x1) ;
1171                     status.V = (status.N ^ status.C) ;
1172                     status.S = (status.N ^ status.V) ;
1173                     status.Z = (res == 0) ;
1174
1175                     core->writeRegister(Rd, res);
1176                     return 1;
1177                 }
1178
1179                 SBC::SBC(word opcode) :
1180                         Rd( get_rd_5(opcode) ),
1181                         Rr( get_rr_5(opcode) ) {
1182                 }
1183
1184                 int SBC::operator()(Core *core) const {
1185                         SReg status( core );
1186                     byte rd = core->readRegister(Rd);
1187                     byte rr = core->readRegister(Rr);
1188
1189                     byte res = rd - rr - ( status.C );
1190
1191                     status.H = (get_sub_carry( res, rd, rr, 3 )) ;
1192                     status.V = (get_sub_overflow( res, rd, rr )) ;
1193                     status.N = ((res >> 7) & 0x1) ;
1194                     status.S = (status.N ^ status.V) ;
1195                     status.C = (get_sub_carry( res, rd, rr, 7 )) ;
1196
1197                     if ((res & 0xff) != 0)
1198                         status.Z = (0);
1199
1200                     core->writeRegister(Rd, res);
1201                     return 1;
1202                 }
1203
1204                 SBCI::SBCI(word opcode) :
1205                         Rd( get_rd_4(opcode) ),
1206                         K( get_K_8(opcode) ) {
1207                 }
1208
1209                 int SBCI::operator()(Core *core) const {
1210                         SReg status( core );
1211                     byte rd = core->readRegister(Rd);
1212
1213                     byte res = rd - K - ( status.C );
1214
1215                     status.H = (get_sub_carry( res, rd, K, 3 )) ;
1216                     status.V = (get_sub_overflow( res, rd, K )) ;
1217                     status.N = ((res >> 7) & 0x1) ;
1218                     status.S = (status.N ^ status.V) ;
1219                     status.C = (get_sub_carry( res, rd, K, 7 )) ;
1220
1221                     if ((res & 0xff) != 0)
1222                         status.Z = 0 ;
1223
1224                     core->writeRegister(Rd, res);
1225                     return 1;
1226                 }
1227
1228                 SBI::SBI(word opcode) :
1229                         ioreg(get_A_5(opcode) ),
1230                         K( 1 << get_reg_bit(opcode) ) {
1231                 }
1232
1233                 int SBI::operator()(Core *core) const {
1234                         core->writeIORegister(ioreg, core->readIORegister(ioreg) | K);
1235                     return 2;
1236                 }
1237
1238                 SBIC::SBIC(word opcode) :
1239                         ioreg( get_A_5(opcode) ),
1240                         K( 1 << get_reg_bit(opcode) ) {
1241                 }
1242
1243                 int SBIC::operator()(Core *core) const {
1244                         int cost = 1;
1245                         if( (core->readIORegister(ioreg) & K) == 0 )
1246                                 cost += core->skip();
1247                         return cost;
1248                 }
1249
1250                 SBIS::SBIS(word opcode) :
1251                         ioreg( get_A_5(opcode) ),
1252                         K( 1 << get_reg_bit(opcode) ) {
1253                 }
1254
1255                 int SBIS::operator()(Core *core) const {
1256                         int cost = 1;
1257                         if( (core->readIORegister(ioreg) & K) != 0 )
1258                                 cost += core->skip();
1259                         return cost;
1260                 }
1261
1262                 SBIW::SBIW(word opcode) :
1263                         Rl( get_rd_2(opcode) ),
1264                         Rh( get_rd_2(opcode)+1 ),
1265                         K( get_K_6(opcode) ) {
1266                 }
1267
1268                 int SBIW::operator()(Core *core) const {
1269                         SReg status( core );
1270                     byte rdl = core->readRegister(Rl);
1271                     byte rdh = core->readRegister(Rh);
1272
1273                     word rd = (rdh << 8) + rdl;
1274
1275                     word res = rd - K;
1276
1277                     status.V = ((rdh >> 7 & 0x1) & ~(res >> 15 & 0x1)) ;
1278                     status.N = ((res >> 15) & 0x1) ;
1279                     status.S = (status.N ^ status.V) ;
1280                     status.Z = ((res & 0xffff) == 0) ;
1281                     status.C = ((res >> 15 & 0x1) & ~(rdh >> 7 & 0x1)) ;
1282
1283                     core->writeRegister(Rl, res & 0xff);
1284                     core->writeRegister(Rh, (res>>8) & 0xff);
1285                     return 2;
1286                 }
1287
1288                 SBRC::SBRC(word opcode) :
1289                         Rd( get_rd_5(opcode) ),
1290                         K( 1 << get_reg_bit(opcode) ) {
1291                 }
1292
1293                 int SBRC::operator()(Core *core) const {
1294                         int cost = 1;
1295                         if( (core->readRegister(Rd) & K) == 0 )
1296                                 cost += core->skip();
1297                         return cost;
1298                 }
1299
1300                 SBRS::SBRS(word opcode) :
1301                         Rd( get_rd_5(opcode) ),
1302                         K( 1 << get_reg_bit(opcode) ) {
1303                 }
1304
1305                 int SBRS::operator()(Core *core) const {
1306                         int cost = 1;
1307                         if( (core->readRegister(Rd) & K) != 0 )
1308                                 cost += core->skip();
1309                         return cost;
1310                 }
1311
1312                 SLEEP::SLEEP(word /*opcode*/) {}
1313
1314                 int SLEEP::operator()(Core *core) const {
1315                         core->sleep();
1316                     return 0;
1317                 }
1318
1319                 SPM::SPM(word /*opcode*/) {
1320                 }
1321
1322                 int SPM::operator()(Core *core) const {
1323                         word Z = ( core->readRegister(Zh) << 8 ) | core->readRegister(Zl);
1324
1325                         word data = ( core->readRegister(R1) << 8 ) | core->readRegister(R0);
1326                         int ret = core->writeFlash(Z, data);
1327                     return ret;
1328                 }
1329
1330                 STD_Y::STD_Y(word opcode) :
1331                         Rd( get_rd_5(opcode) ),
1332                         K( get_q(opcode) ) {
1333                 }
1334
1335                 int STD_Y::operator()(Core *core) const {
1336                     /* Y is R29:R28 */
1337                     word Y = ( core->readRegister(Yh) << 8 ) | core->readRegister(Yl);
1338
1339                     core->writeByte( Y+K, core->readRegister(Rd) );
1340                     return 2;
1341                 }
1342
1343                 STD_Z::STD_Z(word opcode) :
1344                         Rd( get_rd_5(opcode) ),
1345                         K( get_q(opcode) ) {
1346                 }
1347
1348                 int STD_Z::operator()(Core *core) const {
1349                     /* Z is R31:R30 */
1350                         word Z = ( core->readRegister(Zh) << 8 ) | core->readRegister(Zl);
1351
1352                     core->writeByte( Z+K, core->readRegister(Rd) );
1353                     return 2;
1354                 }
1355
1356                 STS::STS(word opcode) :
1357                         Rd( get_rd_5(opcode) ) {
1358                 }
1359
1360                 int STS::operator()(Core *core) const {
1361                         word k = core->fetchOperand();
1362                     core->writeByte( k, core->readRegister(Rd) );
1363                     return 2;
1364                 }
1365
1366                 ST_X::ST_X(word opcode) :
1367                         Rd( get_rd_5(opcode) ) {
1368                 }
1369
1370                 int ST_X::operator()(Core *core) const {
1371                     /* X is R27:R26 */
1372                     word X = ( core->readRegister(Xh) << 8) | core->readRegister(Xl);
1373
1374                     core->writeByte( X, core->readRegister(Rd) );
1375                     return 2;
1376                 }
1377
1378                 ST_X_decr::ST_X_decr(word opcode) : ST_X(opcode) {}
1379
1380                 int ST_X_decr::operator()(Core *core) const {
1381                     /* X is R27:R26 */
1382                     word X = ( core->readRegister(Xh) << 8) | core->readRegister(Xl);
1383
1384                     /* Perform pre-decrement */
1385                     X -= 1;
1386
1387                     core->writeByte( X, core->readRegister(Rd) );
1388
1389                     core->writeRegister(Xl, X & 0xff);
1390                     core->writeRegister(Xh, (X>>8) & 0xff);
1391                     return 2;
1392                 }
1393
1394                 ST_X_incr::ST_X_incr(word opcode) : ST_X(opcode) {}
1395
1396                 int ST_X_incr::operator()(Core *core) const {
1397                     /* X is R27:R26 */
1398                     word X = ( core->readRegister(Xh) << 8) | core->readRegister(Xl);
1399
1400                     core->writeByte( X, core->readRegister(Rd) );
1401
1402                     /* Perform post-increment */
1403                     X += 1;
1404                     core->writeRegister(Xl, X & 0xff);
1405                     core->writeRegister(Xh, (X>>8) & 0xff);
1406                     return 2;
1407                 }
1408
1409                 ST_Y_decr::ST_Y_decr(word opcode) :
1410                         Rd( get_rd_5(opcode) ) {
1411                 }
1412
1413                 int ST_Y_decr::operator()(Core *core) const {
1414                     /* Y is R29:R28 */
1415                     word Y = ( core->readRegister(Yh) << 8) | core->readRegister(Yl);
1416
1417                     /* Perform pre-decrement */
1418                     Y -= 1;
1419
1420                     core->writeByte( Y, core->readRegister(Rd) );
1421                     core->writeRegister(Yl, Y & 0xff);
1422                     core->writeRegister(Yh, (Y>>8) & 0xff);
1423                     return 2;
1424                 }
1425
1426                 ST_Y_incr::ST_Y_incr(word opcode) :
1427                         Rd( get_rd_5(opcode) ) {
1428                 }
1429
1430                 int ST_Y_incr::operator()(Core *core) const {
1431                     /* Y is R29:R28 */
1432                     word Y = ( core->readRegister(Yh) << 8) | core->readRegister(Yl);
1433
1434                     core->writeByte( Y, core->readRegister(Rd) );
1435
1436                     /* Perform post-increment */
1437                     Y += 1;
1438
1439                     core->writeRegister(Yl, Y & 0xff);
1440                     core->writeRegister(Yh, (Y>>8) & 0xff);
1441                     return 2;
1442                 }
1443
1444                 ST_Z_decr::ST_Z_decr(word opcode) :
1445                         Rd( get_rd_5(opcode) ) {
1446                 }
1447
1448                 int ST_Z_decr::operator()(Core *core) const {
1449                     /* Z is R31:R30 */
1450                     word Z = ( core->readRegister(Zh) << 8) | core->readRegister(Zl);
1451
1452                     /* Perform pre-decrement */
1453                     Z -= 1;
1454
1455                     core->writeByte( Z, core->readRegister(Rd) );
1456
1457                     core->writeRegister(Zl, Z & 0xff);
1458                     core->writeRegister(Zh, (Z>>8) & 0xff);
1459                     return 2;
1460                 }
1461
1462                 ST_Z_incr::ST_Z_incr(word opcode) :
1463                         Rd( get_rd_5(opcode) ) {
1464                 }
1465
1466                 int ST_Z_incr::operator()(Core *core) const {
1467                     /* Z is R31:R30 */
1468                     word Z = ( core->readRegister(Zh) << 8) | core->readRegister(Zl);
1469
1470                     core->writeByte( Z, core->readRegister(Rd) );
1471
1472                     /* Perform post-increment */
1473                     Z += 1;
1474
1475                     core->writeRegister(Zl, Z & 0xff);
1476                     core->writeRegister(Zh, (Z>>8) & 0xff);
1477                     return 2;
1478                 }
1479
1480                 SUB::SUB(word opcode) :
1481                         Rd( get_rd_5(opcode) ),
1482                         Rr( get_rr_5(opcode) ) {
1483                 }
1484
1485                 int SUB::operator()(Core *core) const {
1486                         SReg status( core );
1487                     byte rd = core->readRegister(Rd);
1488                     byte rr = core->readRegister(Rr);
1489
1490                     byte res = rd - rr;
1491
1492                     status.H = (get_sub_carry( res, rd, rr, 3 )) ;
1493                     status.V = (get_sub_overflow( res, rd, rr )) ;
1494                     status.N = ((res >> 7) & 0x1) ;
1495                     status.S = (status.N ^ status.V) ;
1496                     status.Z = ((res & 0xff) == 0) ;
1497                     status.C = (get_sub_carry( res, rd, rr, 7 )) ;
1498
1499                     core->writeRegister(Rd, res);
1500                     return 1;
1501                 }
1502
1503                 SUBI::SUBI(word opcode) :
1504                         Rd( get_rd_4(opcode) ),
1505                         K( get_K_8(opcode) ) {
1506                 }
1507
1508                 int SUBI::operator()(Core *core) const {
1509                         SReg status( core );
1510                     byte rd = core->readRegister(Rd);
1511
1512                     byte res = rd - K;
1513
1514                     status.H = (get_sub_carry( res, rd, K, 3 )) ;
1515                     status.V = (get_sub_overflow( res, rd, K )) ;
1516                     status.N = ((res >> 7) & 0x1) ;
1517                     status.S = (status.N ^ status.V) ;
1518                     status.Z = ((res & 0xff) == 0) ;
1519                     status.C = (get_sub_carry( res, rd, K, 7 )) ;
1520
1521                     core->writeRegister(Rd, res);
1522                     return 1;
1523                 }
1524
1525                 SWAP::SWAP(word opcode) :
1526                         Rd( get_rd_5(opcode) ) {
1527                 }
1528
1529                 int SWAP::operator()(Core *core) const {
1530                     byte rd = core->readRegister(Rd);
1531                     byte res = ((rd << 4) & 0xf0) | ((rd >> 4) & 0x0f);
1532                     core->writeRegister(Rd, res);
1533                     return 1;
1534                 }
1535
1536                 WDR::WDR(word /*opcode*/) {}
1537
1538                 int WDR::operator()(Core * /*core*/) const {
1539                         throw util::ImplementationException("Instruction WDR not fully implemented");
1540                     return 1;
1541                 }
1542
1543                 BREAK::BREAK(word /*opcode*/) {}
1544
1545                 int BREAK::operator()(Core *core) const {
1546                         core->systemBreak();
1547                     return 1;
1548                 }
1549
1550                 ILLEGAL::ILLEGAL(word opcode) : opcode(opcode) {}
1551
1552                 int ILLEGAL::operator()(Core * /*core*/) const {
1553                         throw IllegalInstruction(opcode);
1554                 }
1555
1556         }
1557
1558 }