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