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