decoder.c

Go to the documentation of this file.
00001 /*
00002  * $Id: decoder.c,v 1.35 2004/02/26 07:33:34 troth Exp $
00003  *
00004  ****************************************************************************
00005  *
00006  * simulavr - A simulator for the Atmel AVR family of microcontrollers.
00007  * Copyright (C) 2001, 2002, 2003  Theodore A. Roth
00008  *
00009  * This program is free software; you can redistribute it and/or modify
00010  * it under the terms of the GNU General Public License as published by
00011  * the Free Software Foundation; either version 2 of the License, or
00012  * (at your option) any later version.
00013  *
00014  * This program is distributed in the hope that it will be useful,
00015  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00016  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017  * GNU General Public License for more details.
00018  *
00019  * You should have received a copy of the GNU General Public License
00020  * along with this program; if not, write to the Free Software
00021  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00022  *
00023  ****************************************************************************
00024  */
00025 
00026 /**
00027  * \file decoder.c
00028  * \brief Module for handling opcode decoding.
00029  *
00030  * The heart of the instruction decoder is the decode_opcode() function.
00031  *
00032  * The decode_opcode() function examines the given opcode to
00033  * determine which instruction applies and returns a pointer to a function to
00034  * handler performing the instruction's operation. If the given opcode does
00035  * not map to an instruction handler, NULL is returned.
00036  *
00037  * Nearly every instruction in Atmel's Instruction Set Data Sheet will have a
00038  * handler function defined. Each handler will perform all the operations
00039  * described in the data sheet for a given instruction. A few instructions
00040  * have synonyms. For example, CBR is a synonym for ANDI.
00041  *
00042  * This should all be fairly straight forward.
00043  */
00044 
00045 #include <config.h>
00046 
00047 #include <stdio.h>
00048 #include <stdlib.h>
00049 
00050 #include "avrerror.h"
00051 #include "avrmalloc.h"
00052 #include "avrclass.h"
00053 #include "utils.h"
00054 #include "callback.h"
00055 #include "op_names.h"
00056 
00057 #include "storage.h"
00058 #include "flash.h"
00059 
00060 #include "vdevs.h"
00061 #include "memory.h"
00062 #include "stack.h"
00063 #include "register.h"
00064 #include "sram.h"
00065 #include "eeprom.h"
00066 #include "timers.h"
00067 #include "ports.h"
00068 
00069 #include "avrcore.h"
00070 
00071 #include "decoder.h"
00072 
00073 struct opcode_info *global_opcode_lookup_table;
00074 
00075 /** \brief Masks to help extracting information from opcodes. */
00076 
00077 enum decoder_operand_masks
00078 {
00079     /** 2 bit register id  ( R24, R26, R28, R30 ) */
00080     mask_Rd_2 = 0x0030,
00081     /** 3 bit register id  ( R16 - R23 ) */
00082     mask_Rd_3 = 0x0070,
00083     /** 4 bit register id  ( R16 - R31 ) */
00084     mask_Rd_4 = 0x00f0,
00085     /** 5 bit register id  ( R00 - R31 ) */
00086     mask_Rd_5 = 0x01f0,
00087 
00088     /** 3 bit register id  ( R16 - R23 ) */
00089     mask_Rr_3 = 0x0007,
00090     /** 4 bit register id  ( R16 - R31 ) */
00091     mask_Rr_4 = 0x000f,
00092     /** 5 bit register id  ( R00 - R31 ) */
00093     mask_Rr_5 = 0x020f,
00094 
00095     /** for 8 bit constant */
00096     mask_K_8 = 0x0F0F,
00097     /** for 6 bit constant */
00098     mask_K_6 = 0x00CF,
00099 
00100     /** for 7 bit relative address */
00101     mask_k_7 = 0x03F8,
00102     /** for 12 bit relative address */
00103     mask_k_12 = 0x0FFF,
00104     /** for 22 bit absolute address */
00105     mask_k_22 = 0x01F1,
00106 
00107     /** register bit select */
00108     mask_reg_bit = 0x0007,
00109     /** status register bit select */
00110     mask_sreg_bit = 0x0070,
00111     /** address displacement (q) */
00112     mask_q_displ = 0x2C07,
00113 
00114     /** 5 bit register id  ( R00 - R31 ) */
00115     mask_A_5 = 0x00F8,
00116     /** 6 bit IO port id */
00117     mask_A_6 = 0x060F,
00118 };
00119 
00120 /* Some handlers need predeclared */
00121 static int avr_op_CALL (AvrCore *core, uint16_t opcode, unsigned int arg1,
00122                         unsigned int arg2);
00123 static int avr_op_JMP (AvrCore *core, uint16_t opcode, unsigned int arg1,
00124                        unsigned int arg2);
00125 static int avr_op_LDS (AvrCore *core, uint16_t opcode, unsigned int arg1,
00126                        unsigned int arg2);
00127 static int avr_op_STS (AvrCore *core, uint16_t opcode, unsigned int arg1,
00128                        unsigned int arg2);
00129 
00130 /****************************************************************************\
00131  *
00132  * Helper functions to extract information from the opcodes.
00133  *
00134  \***************************************************************************/
00135 
00136 static inline int
00137 get_rd_2 (uint16_t opcode)
00138 {
00139     int reg = ((opcode & mask_Rd_2) >> 4) & 0x3;
00140     return (reg * 2) + 24;
00141 }
00142 
00143 static inline int
00144 get_rd_3 (uint16_t opcode)
00145 {
00146     int reg = opcode & mask_Rd_3;
00147     return ((reg >> 4) & 0x7) + 16;
00148 }
00149 
00150 static inline int
00151 get_rd_4 (uint16_t opcode)
00152 {
00153     int reg = opcode & mask_Rd_4;
00154     return ((reg >> 4) & 0xf) + 16;
00155 }
00156 
00157 static inline int
00158 get_rd_5 (uint16_t opcode)
00159 {
00160     int reg = opcode & mask_Rd_5;
00161     return ((reg >> 4) & 0x1f);
00162 }
00163 
00164 static inline int
00165 get_rr_3 (uint16_t opcode)
00166 {
00167     return (opcode & mask_Rr_3) + 16;
00168 }
00169 
00170 static inline int
00171 get_rr_4 (uint16_t opcode)
00172 {
00173     return (opcode & mask_Rr_4) + 16;
00174 }
00175 
00176 static inline int
00177 get_rr_5 (uint16_t opcode)
00178 {
00179     int reg = opcode & mask_Rr_5;
00180     return (reg & 0xf) + ((reg >> 5) & 0x10);
00181 }
00182 
00183 static inline uint8_t
00184 get_K_8 (uint16_t opcode)
00185 {
00186     int K = opcode & mask_K_8;
00187     return ((K >> 4) & 0xf0) + (K & 0xf);
00188 }
00189 
00190 static inline uint8_t
00191 get_K_6 (uint16_t opcode)
00192 {
00193     int K = opcode & mask_K_6;
00194     return ((K >> 2) & 0x0030) + (K & 0xf);
00195 }
00196 
00197 static inline int
00198 get_k_7 (uint16_t opcode)
00199 {
00200     return (((opcode & mask_k_7) >> 3) & 0x7f);
00201 }
00202 
00203 static inline int
00204 get_k_12 (uint16_t opcode)
00205 {
00206     return (opcode & mask_k_12);
00207 }
00208 
00209 static inline int
00210 get_k_22 (uint16_t opcode)
00211 {
00212     /* Masks only the upper 6 bits of the address, the other 16 bits
00213      * are in PC + 1. */
00214     int k = opcode & mask_k_22;
00215     return ((k >> 3) & 0x003e) + (k & 0x1);
00216 }
00217 
00218 static inline int
00219 get_reg_bit (uint16_t opcode)
00220 {
00221     return opcode & mask_reg_bit;
00222 }
00223 
00224 static inline int
00225 get_sreg_bit (uint16_t opcode)
00226 {
00227     return (opcode & mask_sreg_bit) >> 4;
00228 }
00229 
00230 static inline int
00231 get_q (uint16_t opcode)
00232 {
00233     /* 00q0 qq00 0000 0qqq : Yuck! */
00234     int q = opcode & mask_q_displ;
00235     int qq = (((q >> 1) & 0x1000) + (q & 0x0c00)) >> 7;
00236     return (qq & 0x0038) + (q & 0x7);
00237 }
00238 
00239 static inline int
00240 get_A_5 (uint16_t opcode)
00241 {
00242     return (opcode & mask_A_5) >> 3;
00243 }
00244 
00245 static inline int
00246 get_A_6 (uint16_t opcode)
00247 {
00248     int A = opcode & mask_A_6;
00249     return ((A >> 5) & 0x0030) + (A & 0xf);
00250 }
00251 
00252 /****************************************************************************\
00253  *
00254  * Helper functions for calculating the status register bit values.
00255  * See the Atmel data sheet for the instuction set for more info.
00256  *
00257 \****************************************************************************/
00258 
00259 static inline int
00260 get_add_carry (uint8_t res, uint8_t rd, uint8_t rr, int b)
00261 {
00262     uint8_t resb = res >> b & 0x1;
00263     uint8_t rdb = rd >> b & 0x1;
00264     uint8_t rrb = rr >> b & 0x1;
00265     return (rdb & rrb) | (rrb & ~resb) | (~resb & rdb);
00266 }
00267 
00268 static inline int
00269 get_add_overflow (uint8_t res, uint8_t rd, uint8_t rr)
00270 {
00271     uint8_t res7 = res >> 7 & 0x1;
00272     uint8_t rd7 = rd >> 7 & 0x1;
00273     uint8_t rr7 = rr >> 7 & 0x1;
00274     return (rd7 & rr7 & ~res7) | (~rd7 & ~rr7 & res7);
00275 }
00276 
00277 static inline int
00278 get_sub_carry (uint8_t res, uint8_t rd, uint8_t rr, int b)
00279 {
00280     uint8_t resb = res >> b & 0x1;
00281     uint8_t rdb = rd >> b & 0x1;
00282     uint8_t rrb = rr >> b & 0x1;
00283     return (~rdb & rrb) | (rrb & resb) | (resb & ~rdb);
00284 }
00285 
00286 static inline int
00287 get_sub_overflow (uint8_t res, uint8_t rd, uint8_t rr)
00288 {
00289     uint8_t res7 = res >> 7 & 0x1;
00290     uint8_t rd7 = rd >> 7 & 0x1;
00291     uint8_t rr7 = rr >> 7 & 0x1;
00292     return (rd7 & ~rr7 & ~res7) | (~rd7 & rr7 & res7);
00293 }
00294 
00295 static inline int
00296 get_compare_carry (uint8_t res, uint8_t rd, uint8_t rr, int b)
00297 {
00298     uint8_t resb = res >> b & 0x1;
00299     uint8_t rdb = rd >> b & 0x1;
00300     uint8_t rrb = rr >> b & 0x1;
00301     return (~rdb & rrb) | (rrb & resb) | (resb & ~rdb);
00302 }
00303 
00304 static inline int
00305 get_compare_overflow (uint8_t res, uint8_t rd, uint8_t rr)
00306 {
00307     uint8_t res7 = res >> 7 & 0x1;
00308     uint8_t rd7 = rd >> 7 & 0x1;
00309     uint8_t rr7 = rr >> 7 & 0x1;
00310     /* The atmel data sheet says the second term is ~rd7 for CP
00311      * but that doesn't make any sense. You be the judge. */
00312     return (rd7 & ~rr7 & ~res7) | (~rd7 & rr7 & res7);
00313 }
00314 
00315 /****************************************************************************\
00316  *
00317  * Misc Helper functions
00318  *
00319 \****************************************************************************/
00320 
00321 static inline int
00322 is_next_inst_2_words (AvrCore *core)
00323 {
00324     /* See if next is a two word instruction
00325      * CALL, JMP, LDS, and STS are the only two word (32 bit) instructions. */
00326     uint16_t next_opcode =
00327         flash_read (core->flash, avr_core_PC_get (core) + 1);
00328     struct opcode_info *opi = decode_opcode (next_opcode);
00329 
00330     return ((opi->func == avr_op_CALL) || (opi->func == avr_op_JMP)
00331             || (opi->func == avr_op_LDS) || (opi->func == avr_op_STS));
00332 }
00333 
00334 static inline int
00335 n_bit_unsigned_to_signed (unsigned int val, int n)
00336 {
00337     /* Convert n-bit unsigned value to a signed value. */
00338     unsigned int mask;
00339 
00340     if ((val & (1 << (n - 1))) == 0)
00341         return (int)val;
00342 
00343     /* manually calculate two's complement */
00344     mask = (1 << n) - 1;
00345     return -1 * ((~val & mask) + 1);
00346 }
00347 
00348 /****************************************************************************\
00349  *
00350  * Opcode handler functions.
00351  *
00352 \****************************************************************************/
00353 
00354 static int
00355 avr_op_ADC (AvrCore *core, uint16_t opcode, unsigned int arg1,
00356             unsigned int arg2)
00357 {
00358     /*
00359      * Add with Carry.
00360      *       
00361      * Opcode     : 0001 11rd dddd rrrr 
00362      * Usage      : ADC  Rd, Rr
00363      * Operation  : Rd <- Rd + Rr + C
00364      * Flags      : Z,C,N,V,S,H
00365      * Num Clocks : 1
00366      */
00367     int H, V, N, S, Z, C;
00368 
00369     int Rd = arg1;
00370     int Rr = arg2;
00371 
00372     uint8_t rd = avr_core_gpwr_get (core, Rd);
00373     uint8_t rr = avr_core_gpwr_get (core, Rr);
00374 
00375     uint8_t res = rd + rr + avr_core_sreg_get_bit (core, SREG_C);
00376 
00377     uint8_t sreg = avr_core_sreg_get (core);
00378 
00379     sreg = set_bit_in_byte (sreg, SREG_H, H = get_add_carry (res, rd, rr, 3));
00380     sreg = set_bit_in_byte (sreg, SREG_V, V = get_add_overflow (res, rd, rr));
00381     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
00382     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
00383     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
00384     sreg = set_bit_in_byte (sreg, SREG_C, C = get_add_carry (res, rd, rr, 7));
00385 
00386     avr_core_sreg_set (core, sreg);
00387 
00388     avr_core_gpwr_set (core, Rd, res);
00389     avr_core_PC_incr (core, 1);
00390     avr_core_inst_CKS_set (core, 1);
00391 
00392     return opcode_ADC;
00393 }
00394 
00395 static int
00396 avr_op_ADD (AvrCore *core, uint16_t opcode, unsigned int arg1,
00397             unsigned int arg2)
00398 {
00399     /*
00400      * Add without Carry.
00401      *
00402      * Opcode     : 0000 11rd dddd rrrr 
00403      * Usage      : ADD  Rd, Rr
00404      * Operation  : Rd <- Rd + Rr
00405      * Flags      : Z,C,N,V,S,H
00406      * Num Clocks : 1
00407      */
00408     int H, V, N, S, Z, C;
00409 
00410     int Rd = arg1;
00411     int Rr = arg2;
00412 
00413     uint8_t rd = avr_core_gpwr_get (core, Rd);
00414     uint8_t rr = avr_core_gpwr_get (core, Rr);
00415 
00416     uint8_t res = rd + rr;
00417 
00418     uint8_t sreg = avr_core_sreg_get (core);
00419 
00420     sreg = set_bit_in_byte (sreg, SREG_H, H = get_add_carry (res, rd, rr, 3));
00421     sreg = set_bit_in_byte (sreg, SREG_V, V = get_add_overflow (res, rd, rr));
00422     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
00423     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
00424     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
00425     sreg = set_bit_in_byte (sreg, SREG_C, C = get_add_carry (res, rd, rr, 7));
00426 
00427     avr_core_sreg_set (core, sreg);
00428 
00429     avr_core_gpwr_set (core, Rd, res);
00430     avr_core_PC_incr (core, 1);
00431     avr_core_inst_CKS_set (core, 1);
00432 
00433     return opcode_ADD;
00434 }
00435 
00436 static int
00437 avr_op_ADIW (AvrCore *core, uint16_t opcode, unsigned int arg1,
00438              unsigned int arg2)
00439 {
00440     /*
00441      * Add Immediate to Word.
00442      *
00443      * Opcode     : 1001 0110 KKdd KKKK 
00444      * Usage      : ADIW  Rd, K
00445      * Operation  : Rd+1:Rd <- Rd+1:Rd + K
00446      * Flags      : Z,C,N,V,S
00447      * Num Clocks : 2
00448      */
00449     int C, Z, S, N, V;
00450 
00451     int Rd = arg1;
00452     uint8_t K = arg2;
00453 
00454     uint8_t rdl = avr_core_gpwr_get (core, Rd);
00455     uint8_t rdh = avr_core_gpwr_get (core, Rd + 1);
00456 
00457     uint16_t rd = (rdh << 8) + rdl;
00458     uint16_t res = rd + K;
00459 
00460     uint8_t sreg = avr_core_sreg_get (core);
00461 
00462     sreg = set_bit_in_byte (sreg, SREG_V, V =
00463                             (~(rdh >> 7 & 0x1) & (res >> 15 & 0x1)));
00464     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 15) & 0x1));
00465     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
00466     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xffff) == 0));
00467     sreg = set_bit_in_byte (sreg, SREG_C, C =
00468                             (~(res >> 15 & 0x1) & (rdh >> 7 & 0x1)));
00469 
00470     avr_core_sreg_set (core, sreg);
00471 
00472     avr_core_gpwr_set (core, Rd, res & 0xff);
00473     avr_core_gpwr_set (core, Rd + 1, res >> 8);
00474 
00475     avr_core_PC_incr (core, 1);
00476     avr_core_inst_CKS_set (core, 2);
00477 
00478     return opcode_ADIW;
00479 }
00480 
00481 static int
00482 avr_op_AND (AvrCore *core, uint16_t opcode, unsigned int arg1,
00483             unsigned int arg2)
00484 {
00485     /*
00486      * Logical AND.
00487      *
00488      * Opcode     : 0010 00rd dddd rrrr 
00489      * Usage      : AND  Rd, Rr
00490      * Operation  : Rd <- Rd & Rr
00491      * Flags      : Z,N,V,S
00492      * Num Clocks : 1
00493      */
00494     int Z, N, V, S;
00495 
00496     int Rd = arg1;
00497     int Rr = arg2;
00498 
00499     uint8_t rd = avr_core_gpwr_get (core, Rd);
00500     uint8_t rr = avr_core_gpwr_get (core, Rr);
00501     uint8_t res = rd & rr;
00502 
00503     uint8_t sreg = avr_core_sreg_get (core);
00504 
00505     sreg = set_bit_in_byte (sreg, SREG_V, V = 0);
00506     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
00507     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
00508     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
00509 
00510     avr_core_sreg_set (core, sreg);
00511 
00512     avr_core_gpwr_set (core, Rd, res);
00513     avr_core_PC_incr (core, 1);
00514     avr_core_inst_CKS_set (core, 1);
00515 
00516     return opcode_AND;
00517 }
00518 
00519 static int
00520 avr_op_ANDI (AvrCore *core, uint16_t opcode, unsigned int arg1,
00521              unsigned int arg2)
00522 {
00523     /*
00524      * Logical AND with Immed.
00525      *
00526      * Opcode     : 0111 KKKK dddd KKKK 
00527      * Usage      : ANDI  Rd, K
00528      * Operation  : Rd <- Rd & K
00529      * Flags      : Z,N,V,S
00530      * Num Clocks : 1
00531      */
00532     int Z, N, V, S;
00533 
00534     int Rd = arg1;
00535     uint8_t K = arg2;
00536 
00537     uint8_t rd = avr_core_gpwr_get (core, Rd);
00538     uint8_t res = rd & K;
00539 
00540     uint8_t sreg = avr_core_sreg_get (core);
00541 
00542     sreg = set_bit_in_byte (sreg, SREG_V, V = 0);
00543     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
00544     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
00545     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
00546 
00547     avr_core_sreg_set (core, sreg);
00548 
00549     avr_core_gpwr_set (core, Rd, res);
00550     avr_core_PC_incr (core, 1);
00551     avr_core_inst_CKS_set (core, 1);
00552 
00553     return opcode_ANDI;
00554 }
00555 
00556 static int
00557 avr_op_ASR (AvrCore *core, uint16_t opcode, unsigned int arg1,
00558             unsigned int arg2)
00559 {
00560     /*
00561      * Arithmetic Shift Right.
00562      *
00563      * Opcode     : 1001 010d dddd 0101 
00564      * Usage      : ASR  Rd
00565      * Operation  : Rd(n) <- Rd(n+1), n=0..6
00566      * Flags      : Z,C,N,V,S
00567      * Num Clocks : 1
00568      */
00569     int Z, C, N, V, S;
00570 
00571     int Rd = arg1;
00572 
00573     uint8_t rd = avr_core_gpwr_get (core, Rd);
00574     uint8_t res = (rd >> 1) + (rd & 0x80);
00575 
00576     uint8_t sreg = avr_core_sreg_get (core);
00577 
00578     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
00579     sreg = set_bit_in_byte (sreg, SREG_C, C = (rd & 0x1));
00580     sreg = set_bit_in_byte (sreg, SREG_V, V = (N ^ C));
00581     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
00582     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
00583 
00584     avr_core_sreg_set (core, sreg);
00585 
00586     avr_core_gpwr_set (core, Rd, res);
00587     avr_core_PC_incr (core, 1);
00588     avr_core_inst_CKS_set (core, 1);
00589 
00590     return opcode_ASR;
00591 }
00592 
00593 static int
00594 avr_op_BCLR (AvrCore *core, uint16_t opcode, unsigned int arg1,
00595              unsigned int arg2)
00596 {
00597     /*
00598      * Clear a single flag or bit in SREG.
00599      *
00600      * Opcode     : 1001 0100 1sss 1000 
00601      * Usage      : BCLR  
00602      * Operation  : SREG(s) <- 0
00603      * Flags      : SREG(s)
00604      * Num Clocks : 1
00605      */
00606     avr_core_sreg_set_bit (core, arg1, 0);
00607     avr_core_PC_incr (core, 1);
00608     avr_core_inst_CKS_set (core, 1);
00609 
00610     return opcode_BCLR;
00611 }
00612 
00613 static int
00614 avr_op_BLD (AvrCore *core, uint16_t opcode, unsigned int arg1,
00615             unsigned int arg2)
00616 {
00617     /* Bit load from T to Register.
00618      *
00619      * Opcode     : 1111 100d dddd 0bbb 
00620      * Usage      : BLD  Rd, b
00621      * Operation  : Rd(b) <- T
00622      * Flags      : None
00623      * Num Clocks : 1
00624      */
00625     int Rd = arg1;
00626     int bit = arg2;
00627 
00628     uint8_t rd = avr_core_gpwr_get (core, Rd);
00629     int T = avr_core_sreg_get_bit (core, SREG_T);
00630     uint8_t res;
00631 
00632     if (T == 0)
00633         res = rd & ~(1 << bit);
00634     else
00635         res = rd | (1 << bit);
00636 
00637     avr_core_gpwr_set (core, Rd, res);
00638     avr_core_PC_incr (core, 1);
00639     avr_core_inst_CKS_set (core, 1);
00640 
00641     return opcode_BLD;
00642 }
00643 
00644 static int
00645 avr_op_BRBC (AvrCore *core, uint16_t opcode, unsigned int arg1,
00646              unsigned int arg2)
00647 {
00648     /*
00649      * Branch if Status Flag Cleared.
00650      *
00651      * Pass control directly to the specific bit operation.
00652      *
00653      * Opcode     : 1111 01kk kkkk ksss 
00654      * Usage      : BRBC  s, k
00655      * Operation  : if (SREG(s) = 0) then PC <- PC + k + 1
00656      * Flags      : None
00657      * Num Clocks : 1 / 2
00658      *
00659      * k is an relative address represented in two's complements.
00660      * (64 < k <= 64)
00661      */
00662     int bit = arg1;
00663     int k = arg2;
00664 
00665     if (avr_core_sreg_get_bit (core, bit) == 0)
00666     {
00667         avr_core_PC_incr (core, k + 1);
00668         avr_core_inst_CKS_set (core, 2);
00669     }
00670     else
00671     {
00672         avr_core_PC_incr (core, 1);
00673         avr_core_inst_CKS_set (core, 1);
00674     }
00675 
00676     return opcode_BRBC;
00677 }
00678 
00679 static int
00680 avr_op_BRBS (AvrCore *core, uint16_t opcode, unsigned int arg1,
00681              unsigned int arg2)
00682 {
00683     /*
00684      * Branch if Status Flag Set.
00685      *
00686      * Pass control directly to the specific bit operation.
00687      *
00688      * Opcode     : 1111 00kk kkkk ksss 
00689      * Usage      : BRBS  s, k
00690      * Operation  : if (SREG(s) = 1) then PC <- PC + k + 1
00691      * Flags      : None
00692      * Num Clocks : 1 / 2
00693      *
00694      * k is an relative address represented in two's complements.
00695      * (64 < k <= 64)
00696      */
00697     int bit = arg1;
00698     int k = arg2;
00699 
00700     if (avr_core_sreg_get_bit (core, bit) != 0)
00701     {
00702         avr_core_PC_incr (core, k + 1);
00703         avr_core_inst_CKS_set (core, 2);
00704     }
00705     else
00706     {
00707         avr_core_PC_incr (core, 1);
00708         avr_core_inst_CKS_set (core, 1);
00709     }
00710 
00711     return opcode_BRBS;
00712 }
00713 
00714 static int
00715 avr_op_BREAK (AvrCore *core, uint16_t opcode, unsigned int arg1,
00716               unsigned int arg2)
00717 {
00718     /*
00719      * The BREAK instruction only available on devices with JTAG support. We
00720      * use it to implement break points for all devices though. When the
00721      * debugger sets a break point we will replace the insn at the requested
00722      * PC with the BREAK insn and save the PC and original insn on the break
00723      * point list. Thus, we only need to walk the break point list when we
00724      * reach a break insn.
00725      *
00726      * When a break occurs, we will return control to the caller _without_
00727      * incrementing PC as the insn set datasheet says.
00728      *
00729      * Opcode     : 1001 0101 1001 1000
00730      * Usage      : BREAK
00731      * Operation  : Puts the in Stopped Mode if supported, NOP otherwise.
00732      * Flags      : None
00733      * Num Clocks : 1
00734      */
00735 
00736     avr_message ("BREAK POINT: PC = 0x%08x: clock = %lld\n",
00737                  avr_core_PC_get (core), avr_core_CK_get (core));
00738 
00739     /* FIXME: TRoth/2002-10-15: Should we execute the original insn which the
00740        break replaced here or let the caller handle that? For now we defer
00741        that to the caller. */
00742 
00743 /*     return opcode_BREAK; */
00744     return BREAK_POINT;
00745 }
00746 
00747 static int
00748 avr_op_BSET (AvrCore *core, uint16_t opcode, unsigned int arg1,
00749              unsigned int arg2)
00750 {
00751     /*
00752      * Set a single flag or bit in SREG.
00753      *
00754      * Opcode     : 1001 0100 0sss 1000 
00755      * Usage      : BSET
00756      * Operation  : SREG(s) <- 1
00757      * Flags      : SREG(s)
00758      * Num Clocks : 1
00759      */
00760     avr_core_sreg_set_bit (core, arg1, 1);
00761     avr_core_PC_incr (core, 1);
00762     avr_core_inst_CKS_set (core, 1);
00763 
00764     return opcode_BSET;
00765 }
00766 
00767 static int
00768 avr_op_BST (AvrCore *core, uint16_t opcode, unsigned int arg1,
00769             unsigned int arg2)
00770 {
00771     /*
00772      * Bit Store from Register to T.
00773      *
00774      * Opcode     : 1111 101d dddd 0bbb 
00775      * Usage      : BST  Rd, b
00776      * Operation  : T <- Rd(b)
00777      * Flags      : T
00778      * Num Clocks : 1
00779      */
00780     int Rd = arg1;
00781     int bit = arg2;
00782 
00783     uint8_t rd = avr_core_gpwr_get (core, Rd);
00784     avr_core_sreg_set_bit (core, SREG_T, (rd >> bit) & 0x1);
00785 
00786     avr_core_PC_incr (core, 1);
00787     avr_core_inst_CKS_set (core, 1);
00788 
00789     return opcode_BST;
00790 }
00791 
00792 static int
00793 avr_op_CALL (AvrCore *core, uint16_t opcode, unsigned int arg1,
00794              unsigned int arg2)
00795 {
00796     /*
00797      * Call Subroutine.
00798      *
00799      * Opcode     : 1001 010k kkkk 111k kkkk kkkk kkkk kkkk
00800      * Usage      : CALL  k
00801      * Operation  : PC <- k
00802      * Flags      : None
00803      * Num Clocks : 4 / 5
00804      */
00805     int pc = avr_core_PC_get (core);
00806     int pc_bytes = avr_core_PC_size (core);
00807 
00808     int kh = arg1;
00809     int kl = flash_read (core->flash, pc + 1);
00810 
00811     int k = (kh << 16) + kl;
00812 
00813     if ((pc_bytes == 2) && (k > 0xffff))
00814         avr_error ("Address out of allowed range: 0x%06x", k);
00815 
00816     avr_core_stack_push (core, pc_bytes, pc + 2);
00817 
00818     avr_core_PC_set (core, k);
00819     avr_core_inst_CKS_set (core, pc_bytes + 2);
00820 
00821     return opcode_CALL;
00822 }
00823 
00824 static int
00825 avr_op_CBI (AvrCore *core, uint16_t opcode, unsigned int arg1,
00826             unsigned int arg2)
00827 {
00828     /*
00829      * Clear Bit in I/O Register.
00830      *
00831      * Opcode     : 1001 1000 AAAA Abbb 
00832      * Usage      : CBI  A, b
00833      * Operation  : I/O(A, b) <- 0
00834      * Flags      : None
00835      * Num Clocks : 2
00836      */
00837     int A = arg1;
00838     int b = arg2;
00839 
00840     uint8_t val = avr_core_io_read (core, A);
00841     avr_core_io_write (core, A, val & ~(1 << b));
00842 
00843     avr_core_PC_incr (core, 1);
00844     avr_core_inst_CKS_set (core, 2);
00845 
00846     return opcode_CBI;
00847 }
00848 
00849 static int
00850 avr_op_COM (AvrCore *core, uint16_t opcode, unsigned int arg1,
00851             unsigned int arg2)
00852 {
00853     /*
00854      * One's Complement.
00855      *
00856      * Opcode     : 1001 010d dddd 0000 
00857      * Usage      : COM  Rd
00858      * Operation  : Rd <- $FF - Rd
00859      * Flags      : Z,C,N,V,S
00860      * Num Clocks : 1
00861      */
00862     int Z, C, N, V, S;
00863 
00864     int Rd = arg1;
00865 
00866     uint8_t rd = avr_core_gpwr_get (core, Rd);
00867     uint8_t res = 0xff - rd;
00868 
00869     uint8_t sreg = avr_core_sreg_get (core);
00870 
00871     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
00872     sreg = set_bit_in_byte (sreg, SREG_C, C = 1);
00873     sreg = set_bit_in_byte (sreg, SREG_V, V = 0);
00874     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
00875     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
00876 
00877     avr_core_sreg_set (core, sreg);
00878 
00879     avr_core_gpwr_set (core, Rd, res);
00880     avr_core_PC_incr (core, 1);
00881     avr_core_inst_CKS_set (core, 1);
00882 
00883     return opcode_COM;
00884 }
00885 
00886 static int
00887 avr_op_CP (AvrCore *core, uint16_t opcode, unsigned int arg1,
00888            unsigned int arg2)
00889 {
00890     /*
00891      * Compare.
00892      *
00893      * Opcode     : 0001 01rd dddd rrrr 
00894      * Usage      : CP  Rd, Rr
00895      * Operation  : Rd - Rr
00896      * Flags      : Z,C,N,V,S,H
00897      * Num Clocks : 1
00898      */
00899     int Z, C, N, V, S, H;
00900 
00901     int Rd = arg1;
00902     int Rr = arg2;
00903 
00904     uint8_t rd = avr_core_gpwr_get (core, Rd);
00905     uint8_t rr = avr_core_gpwr_get (core, Rr);
00906     uint8_t res = rd - rr;
00907 
00908     uint8_t sreg = avr_core_sreg_get (core);
00909 
00910     sreg = set_bit_in_byte (sreg, SREG_H, H =
00911                             get_compare_carry (res, rd, rr, 3));
00912     sreg = set_bit_in_byte (sreg, SREG_V, V =
00913                             get_compare_overflow (res, rd, rr));
00914     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
00915     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
00916     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
00917     sreg = set_bit_in_byte (sreg, SREG_C, C =
00918                             get_compare_carry (res, rd, rr, 7));
00919 
00920     avr_core_sreg_set (core, sreg);
00921 
00922     avr_core_PC_incr (core, 1);
00923     avr_core_inst_CKS_set (core, 1);
00924 
00925     return opcode_CP;
00926 }
00927 
00928 static int
00929 avr_op_CPC (AvrCore *core, uint16_t opcode, unsigned int arg1,
00930             unsigned int arg2)
00931 {
00932     /*
00933      * Compare with Carry.
00934      *
00935      * Opcode     : 0000 01rd dddd rrrr 
00936      * Usage      : CPC  Rd, Rr
00937      * Operation  : Rd - Rr - C
00938      * Flags      : Z,C,N,V,S,H
00939      * Num Clocks : 1
00940      */
00941     int Z, C, N, V, S, H, prev_Z;
00942 
00943     int Rd = arg1;
00944     int Rr = arg2;
00945 
00946     uint8_t rd = avr_core_gpwr_get (core, Rd);
00947     uint8_t rr = avr_core_gpwr_get (core, Rr);
00948     uint8_t res = rd - rr - avr_core_sreg_get_bit (core, SREG_C);
00949 
00950     uint8_t sreg = avr_core_sreg_get (core);
00951 
00952     sreg = set_bit_in_byte (sreg, SREG_H, H =
00953                             get_compare_carry (res, rd, rr, 3));
00954     sreg = set_bit_in_byte (sreg, SREG_V, V =
00955                             get_compare_overflow (res, rd, rr));
00956     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
00957     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
00958     sreg = set_bit_in_byte (sreg, SREG_C, C =
00959                             get_compare_carry (res, rd, rr, 7));
00960 
00961     /* Previous value remains unchanged when result is 0; cleared otherwise */
00962     Z = ((res & 0xff) == 0);
00963     prev_Z = avr_core_sreg_get_bit (core, SREG_Z);
00964     sreg = set_bit_in_byte (sreg, SREG_Z, Z && prev_Z);
00965 
00966     avr_core_sreg_set (core, sreg);
00967 
00968     avr_core_PC_incr (core, 1);
00969     avr_core_inst_CKS_set (core, 1);
00970 
00971     return opcode_CPC;
00972 }
00973 
00974 static int
00975 avr_op_CPI (AvrCore *core, uint16_t opcode, unsigned int arg1,
00976             unsigned int arg2)
00977 {
00978     /*
00979      * Compare with Immediate.
00980      *
00981      * Opcode     : 0011 KKKK dddd KKKK 
00982      * Usage      : CPI  Rd, K
00983      * Operation  : Rd - K
00984      * Flags      : Z,C,N,V,S,H
00985      * Num Clocks : 1
00986      */
00987     int Z, C, N, V, S, H;
00988 
00989     int Rd = arg1;
00990     uint8_t K = arg2;
00991 
00992     uint8_t rd = avr_core_gpwr_get (core, Rd);
00993     uint8_t res = rd - K;
00994 
00995     uint8_t sreg = avr_core_sreg_get (core);
00996 
00997     sreg = set_bit_in_byte (sreg, SREG_H, H =
00998                             get_compare_carry (res, rd, K, 3));
00999     sreg = set_bit_in_byte (sreg, SREG_V, V =
01000                             get_compare_overflow (res, rd, K));
01001     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
01002     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
01003     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
01004     sreg = set_bit_in_byte (sreg, SREG_C, C =
01005                             get_compare_carry (res, rd, K, 7));
01006 
01007     avr_core_sreg_set (core, sreg);
01008 
01009     avr_core_PC_incr (core, 1);
01010     avr_core_inst_CKS_set (core, 1);
01011 
01012     return opcode_CPI;
01013 }
01014 
01015 static int
01016 avr_op_CPSE (AvrCore *core, uint16_t opcode, unsigned int arg1,
01017              unsigned int arg2)
01018 {
01019     /*
01020      * Compare, Skip if Equal.
01021      *
01022      * Opcode     : 0001 00rd dddd rrrr 
01023      * Usage      : CPSE  Rd, Rr
01024      * Operation  : if (Rd = Rr) PC <- PC + 2 or 3
01025      * Flags      : None
01026      * Num Clocks : 1 / 2 / 3
01027      */
01028     int skip;
01029 
01030     int Rd = arg1;
01031     int Rr = arg2;
01032 
01033     uint8_t rd = avr_core_gpwr_get (core, Rd);
01034     uint8_t rr = avr_core_gpwr_get (core, Rr);
01035 
01036     if (is_next_inst_2_words (core))
01037         skip = 3;
01038     else
01039         skip = 2;
01040 
01041     if (rd == rr)
01042     {
01043         avr_core_PC_incr (core, skip);
01044         avr_core_inst_CKS_set (core, skip);
01045     }
01046     else
01047     {
01048         avr_core_PC_incr (core, 1);
01049         avr_core_inst_CKS_set (core, 1);
01050     }
01051 
01052     return opcode_CPSE;
01053 }
01054 
01055 static int
01056 avr_op_DEC (AvrCore *core, uint16_t opcode, unsigned int arg1,
01057             unsigned int arg2)
01058 {
01059     /*
01060      * Decrement.
01061      *
01062      * Opcode     : 1001 010d dddd 1010 
01063      * Usage      : DEC  Rd
01064      * Operation  : Rd <- Rd - 1
01065      * Flags      : Z,N,V,S
01066      * Num Clocks : 1
01067      */
01068     int Z, N, V, S;
01069 
01070     int Rd = arg1;
01071     uint8_t rd = avr_core_gpwr_get (core, Rd);
01072     uint8_t res = rd - 1;
01073 
01074     uint8_t sreg = avr_core_sreg_get (core);
01075 
01076     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
01077     sreg = set_bit_in_byte (sreg, SREG_V, V = (rd == 0x80));
01078     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
01079     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
01080 
01081     avr_core_sreg_set (core, sreg);
01082 
01083     avr_core_gpwr_set (core, Rd, res);
01084 
01085     avr_core_PC_incr (core, 1);
01086     avr_core_inst_CKS_set (core, 1);
01087 
01088     return opcode_DEC;
01089 }
01090 
01091 static int
01092 avr_op_EICALL (AvrCore *core, uint16_t opcode, unsigned int arg1,
01093                unsigned int arg2)
01094 {
01095     /*
01096      * Extended Indirect Call to (Z).
01097      *
01098      * Opcode     : 1001 0101 0001 1001 
01099      * Usage      : EICALL  
01100      * Operation  : PC(15:0) <- Z, PC(21:16) <- EIND
01101      * Flags      : None
01102      * Num Clocks : 4
01103      */
01104     int pc = avr_core_PC_get (core);
01105     int pc_bytes = 3;
01106 
01107     /* Z is R31:R30 */
01108     int new_pc =
01109         ((core->EIND & 0x3f) << 16) + (avr_core_gpwr_get (core, 31) << 8) +
01110         avr_core_gpwr_get (core, 30);
01111 
01112     avr_warning ("needs serious code review\n");
01113 
01114     avr_core_stack_push (core, pc_bytes, pc + 1);
01115 
01116     avr_core_PC_set (core, new_pc);
01117     avr_core_inst_CKS_set (core, 4);
01118 
01119     return opcode_EICALL;
01120 }
01121 
01122 static int
01123 avr_op_EIJMP (AvrCore *core, uint16_t opcode, unsigned int arg1,
01124               unsigned int arg2)
01125 {
01126     /*
01127      * Extended Indirect Jmp to (Z).
01128      *
01129      * Opcode     : 1001 0100 0001 1001 
01130      * Usage      : EIJMP  
01131      * Operation  : PC(15:0) <- Z, PC(21:16) <- EIND
01132      * Flags      : None
01133      * Num Clocks : 2
01134      */
01135 
01136     /* Z is R31:R30 */
01137     int new_pc =
01138         ((core->EIND & 0x3f) << 16) + (avr_core_gpwr_get (core, 31) << 8) +
01139         avr_core_gpwr_get (core, 30);
01140 
01141     avr_warning ("needs serious code review\n");
01142 
01143     avr_core_PC_set (core, new_pc);
01144     avr_core_inst_CKS_set (core, 2);
01145 
01146     return opcode_EIJMP;
01147 }
01148 
01149 static int
01150 avr_op_ELPM_Z (AvrCore *core, uint16_t opcode, unsigned int arg1,
01151                unsigned int arg2)
01152 {
01153     /*
01154      * Extended Load Program Memory.
01155      *
01156      * Opcode     : 1001 000d dddd 0110 
01157      * Usage      : ELPM  Rd, Z
01158      * Operation  : R <- (RAMPZ:Z)
01159      * Flags      : None
01160      * Num Clocks : 3
01161      */
01162     int Z, high_byte, flash_addr;
01163     uint16_t data;
01164 
01165     int Rd = arg1;
01166 
01167     if ((Rd == 30) || (Rd == 31))
01168         avr_error ("Results of operation are undefined");
01169 
01170     avr_warning ("needs serious code review\n");
01171 
01172     /* FIXME: Is this correct? */
01173     /* Z is R31:R30 */
01174     Z = ((avr_core_rampz_get (core) & 0x3f) << 16) +
01175         (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
01176 
01177     high_byte = Z & 0x1;
01178 
01179     flash_addr = Z / 2;
01180 
01181     data = flash_read (core->flash, flash_addr);
01182 
01183     if (high_byte == 1)
01184         avr_core_gpwr_set (core, Rd, data >> 8);
01185     else
01186         avr_core_gpwr_set (core, Rd, data & 0xff);
01187 
01188     avr_core_PC_incr (core, 1);
01189     avr_core_inst_CKS_set (core, 3);
01190 
01191     return opcode_ELPM_Z;
01192 }
01193 
01194 static int
01195 avr_op_ELPM_Z_incr (AvrCore *core, uint16_t opcode, unsigned int arg1,
01196                     unsigned int arg2)
01197 {
01198     /*
01199      * Extended Ld Prg Mem and Post-Incr.
01200      *
01201      * Opcode     : 1001 000d dddd 0111 
01202      * Usage      : ELPM  Rd, Z+
01203      * Operation  : Rd <- (RAMPZ:Z), Z <- Z + 1
01204      * Flags      : None
01205      * Num Clocks : 3
01206      */
01207     int Z, high_byte, flash_addr;
01208     uint16_t data;
01209 
01210     int Rd = arg1;
01211 
01212     if ((Rd == 30) || (Rd == 31))
01213         avr_error ("Results of operation are undefined");
01214 
01215     /* TRoth/2002-08-14: This seems to work ok for me. */
01216     /* avr_warning( "needs serious code review\n" ); */
01217 
01218     /* FIXME: Is this correct? */
01219     /* Z is R31:R30 */
01220     Z = ((avr_core_rampz_get (core) & 0x3f) << 16) +
01221         (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
01222 
01223     high_byte = Z & 0x1;
01224 
01225     flash_addr = Z / 2;
01226 
01227     data = flash_read (core->flash, flash_addr);
01228 
01229     if (high_byte == 1)
01230         avr_core_gpwr_set (core, Rd, data >> 8);
01231     else
01232         avr_core_gpwr_set (core, Rd, data & 0xff);
01233 
01234     /* post increment Z */
01235     Z += 1;
01236     avr_core_gpwr_set (core, 30, Z & 0xff);
01237     avr_core_gpwr_set (core, 31, Z >> 8);
01238     avr_core_rampz_set (core, (Z >> 16) & 0x3f);
01239 
01240     avr_core_PC_incr (core, 1);
01241     avr_core_inst_CKS_set (core, 3);
01242 
01243     return opcode_ELPM_Z_incr;
01244 }
01245 
01246 static int
01247 avr_op_ELPM (AvrCore *core, uint16_t opcode, unsigned int arg1,
01248              unsigned int arg2)
01249 {
01250     /*
01251      * Extended Load Program Memory.
01252      *
01253      * This is the same as avr_op_ELPM_Z with Rd = R0.
01254      *
01255      * Opcode     : 1001 0101 1101 1000 
01256      * Usage      : ELPM  
01257      * Operation  : R0 <- (RAMPZ:Z)
01258      * Flags      : None
01259      * Num Clocks : 3
01260      */
01261     avr_op_ELPM_Z (core, 0x9006, arg1, arg2);
01262     return opcode_ELPM;
01263 }
01264 
01265 static int
01266 avr_op_EOR (AvrCore *core, uint16_t opcode, unsigned int arg1,
01267             unsigned int arg2)
01268 {
01269     /*
01270      * Exclusive OR.
01271      *
01272      * Opcode     : 0010 01rd dddd rrrr 
01273      * Usage      : EOR  Rd, Rr
01274      * Operation  : Rd <- Rd ^ Rr
01275      * Flags      : Z,N,V,S
01276      * Num Clocks : 1
01277      */
01278     int Z, N, V, S;
01279 
01280     int Rd = arg1;
01281     int Rr = arg2;
01282 
01283     uint8_t rd = avr_core_gpwr_get (core, Rd);
01284     uint8_t rr = avr_core_gpwr_get (core, Rr);
01285 
01286     uint8_t res = rd ^ rr;
01287 
01288     uint8_t sreg = avr_core_sreg_get (core);
01289 
01290     sreg = set_bit_in_byte (sreg, SREG_V, V = 0);
01291     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
01292     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
01293     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
01294 
01295     avr_core_sreg_set (core, sreg);
01296 
01297     avr_core_gpwr_set (core, Rd, res);
01298 
01299     avr_core_PC_incr (core, 1);
01300     avr_core_inst_CKS_set (core, 1);
01301 
01302     return opcode_EOR;
01303 }
01304 
01305 static int
01306 avr_op_ESPM (AvrCore *core, uint16_t opcode, unsigned int arg1,
01307              unsigned int arg2)
01308 {
01309     /*
01310      * Extended Store Program Memory.
01311      *
01312      * Opcode     : 1001 0101 1111 1000 
01313      * Usage      : ESPM  
01314      * Operation  : (RAMPZ:Z) <- R1:R0
01315      * Flags      : None
01316      * Num Clocks : -
01317      */
01318     avr_error ("This opcode is not implemented yet: 0x%04x", opcode);
01319     return opcode_ESPM;
01320 }
01321 
01322 /**
01323  ** I don't know how this Fractional Multiplication works.
01324  ** If someone wishes to enlighten me, I write these.
01325  **/
01326 
01327 static int
01328 avr_op_FMUL (AvrCore *core, uint16_t opcode, unsigned int arg1,
01329              unsigned int arg2)
01330 {
01331     /*
01332      * Fractional Mult Unsigned.
01333      *
01334      * Opcode     : 0000 0011 0ddd 1rrr 
01335      * Usage      : FMUL  Rd, Rr
01336      * Operation  : R1:R0 <- (Rd * Rr)<<1 (UU)
01337      * Flags      : Z,C
01338      * Num Clocks : 2
01339      */
01340     int Rd = arg1;
01341     int Rr = arg2;
01342 
01343     uint8_t rd = avr_core_gpwr_get (core, Rd);
01344     uint8_t rr = avr_core_gpwr_get (core, Rr);
01345 
01346     uint16_t resp = rd * rr;
01347     uint16_t res = resp << 1;
01348 
01349     uint8_t sreg = avr_core_sreg_get (core);
01350 
01351     sreg = set_bit_in_byte (sreg, SREG_Z, ((res & 0xffff) == 0));
01352     sreg = set_bit_in_byte (sreg, SREG_C, ((resp >> 15) & 0x1));
01353 
01354     avr_core_sreg_set (core, sreg);
01355 
01356     /* result goes in R1:R0 */
01357     avr_core_gpwr_set (core, 1, res >> 8);
01358     avr_core_gpwr_set (core, 0, res & 0xff);
01359 
01360     avr_core_PC_incr (core, 1);
01361     avr_core_inst_CKS_set (core, 2);
01362 
01363     return opcode_FMUL;
01364 }
01365 
01366 static int
01367 avr_op_FMULS (AvrCore *core, uint16_t opcode, unsigned int arg1,
01368               unsigned int arg2)
01369 {
01370     /*
01371      * Fractional Mult Signed.
01372      *
01373      * Opcode     : 0000 0011 1ddd 0rrr 
01374      * Usage      : FMULS  Rd, Rr
01375      * Operation  : R1:R0 <- (Rd * Rr)<<1 (SS)
01376      * Flags      : Z,C
01377      * Num Clocks : 2
01378      */
01379     int Rd = arg1;
01380     int Rr = arg2;
01381 
01382     int8_t rd = avr_core_gpwr_get (core, Rd);
01383     int8_t rr = avr_core_gpwr_get (core, Rr);
01384 
01385     uint16_t resp = rd * rr;
01386     uint16_t res = resp << 1;
01387 
01388     uint8_t sreg = avr_core_sreg_get (core);
01389 
01390     sreg = set_bit_in_byte (sreg, SREG_Z, ((res & 0xffff) == 0));
01391     sreg = set_bit_in_byte (sreg, SREG_C, ((resp >> 15) & 0x1));
01392 
01393     avr_core_sreg_set (core, sreg);
01394 
01395     /* result goes in R1:R0 */
01396     avr_core_gpwr_set (core, 1, res >> 8);
01397     avr_core_gpwr_set (core, 0, res & 0xff);
01398 
01399     avr_core_PC_incr (core, 1);
01400     avr_core_inst_CKS_set (core, 2);
01401 
01402     return opcode_FMULS;
01403 }
01404 
01405 static int
01406 avr_op_FMULSU (AvrCore *core, uint16_t opcode, unsigned int arg1,
01407                unsigned int arg2)
01408 {
01409     /*
01410      * Fract Mult Signed w/ Unsigned.
01411      *
01412      * Opcode     : 0000 0011 1ddd 1rrr 
01413      * Usage      : FMULSU  Rd, Rr
01414      * Operation  : R1:R0 <- (Rd * Rr)<<1 (SU)
01415      * Flags      : Z,C
01416      * Num Clocks : 2
01417      */
01418     int Rd = arg1;
01419     int Rr = arg2;
01420 
01421     int8_t rd = avr_core_gpwr_get (core, Rd);
01422     uint8_t rr = avr_core_gpwr_get (core, Rr);
01423 
01424     uint16_t resp = rd * rr;
01425     uint16_t res = resp << 1;
01426 
01427     uint8_t sreg = avr_core_sreg_get (core);
01428 
01429     sreg = set_bit_in_byte (sreg, SREG_Z, ((res & 0xffff) == 0));
01430     sreg = set_bit_in_byte (sreg, SREG_C, ((resp >> 15) & 0x1));
01431 
01432     avr_core_sreg_set (core, sreg);
01433 
01434     /* result goes in R1:R0 */
01435     avr_core_gpwr_set (core, 1, res >> 8);
01436     avr_core_gpwr_set (core, 0, res & 0xff);
01437 
01438     avr_core_PC_incr (core, 1);
01439     avr_core_inst_CKS_set (core, 2);
01440 
01441     return opcode_FMULSU;
01442 }
01443 
01444 static int
01445 avr_op_ICALL (AvrCore *core, uint16_t opcode, unsigned int arg1,
01446               unsigned int arg2)
01447 {
01448     /*
01449      * Indirect Call to (Z).
01450      *
01451      * Opcode     : 1001 0101 0000 1001 
01452      * Usage      : ICALL  
01453      * Operation  : PC(15:0) <- Z, PC(21:16) <- 0
01454      * Flags      : None
01455      * Num Clocks : 3 / 4
01456      */
01457     int pc = avr_core_PC_get (core);
01458     int pc_bytes = avr_core_PC_size (core);
01459 
01460     /* Z is R31:R30 */
01461     int new_pc =
01462         (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
01463 
01464     avr_core_stack_push (core, pc_bytes, pc + 1);
01465 
01466     avr_core_PC_set (core, new_pc);
01467     avr_core_inst_CKS_set (core, pc_bytes + 1);
01468 
01469     return opcode_ICALL;
01470 }
01471 
01472 static int
01473 avr_op_IJMP (AvrCore *core, uint16_t opcode, unsigned int arg1,
01474              unsigned int arg2)
01475 {
01476     /*
01477      * Indirect Jump to (Z).
01478      *
01479      * Opcode     : 1001 0100 0000 1001 
01480      * Usage      : IJMP  
01481      * Operation  : PC(15:0) <- Z, PC(21:16) <- 0
01482      * Flags      : None
01483      * Num Clocks : 2
01484      */
01485 
01486     /* Z is R31:R30 */
01487     int new_pc =
01488         (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
01489     avr_core_PC_set (core, new_pc);
01490     avr_core_inst_CKS_set (core, 2);
01491 
01492     return opcode_IJMP;
01493 }
01494 
01495 static int
01496 avr_op_IN (AvrCore *core, uint16_t opcode, unsigned int arg1,
01497            unsigned int arg2)
01498 {
01499     /*
01500      * In From I/O Location.
01501      *
01502      * Opcode     : 1011 0AAd dddd AAAA 
01503      * Usage      : IN  Rd, A
01504      * Operation  : Rd <- I/O(A)
01505      * Flags      : None
01506      * Num Clocks : 1
01507      */
01508     int Rd = arg1;
01509     int A = arg2;
01510 
01511     avr_core_gpwr_set (core, Rd, avr_core_io_read (core, A));
01512 
01513     avr_core_PC_incr (core, 1);
01514     avr_core_inst_CKS_set (core, 1);
01515 
01516     return opcode_IN;
01517 }
01518 
01519 static int
01520 avr_op_INC (AvrCore *core, uint16_t opcode, unsigned int arg1,
01521             unsigned int arg2)
01522 {
01523     /*
01524      * Increment.
01525      *
01526      * Opcode     : 1001 010d dddd 0011 
01527      * Usage      : INC  Rd
01528      * Operation  : Rd <- Rd + 1
01529      * Flags      : Z,N,V,S
01530      * Num Clocks : 1
01531      */
01532     int Z, N, V, S;
01533 
01534     int Rd = arg1;
01535     uint8_t rd = avr_core_gpwr_get (core, Rd);
01536     uint8_t res = rd + 1;
01537 
01538     uint8_t sreg = avr_core_sreg_get (core);
01539 
01540     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
01541     sreg = set_bit_in_byte (sreg, SREG_V, V = (rd == 0x7f));
01542     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
01543     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
01544 
01545     avr_core_sreg_set (core, sreg);
01546 
01547     avr_core_gpwr_set (core, Rd, res);
01548 
01549     avr_core_PC_incr (core, 1);
01550     avr_core_inst_CKS_set (core, 1);
01551 
01552     return opcode_INC;
01553 }
01554 
01555 static int
01556 avr_op_JMP (AvrCore *core, uint16_t opcode, unsigned int arg1,
01557             unsigned int arg2)
01558 {
01559     /*
01560      * Jump.
01561      *
01562      * Opcode     : 1001 010k kkkk 110k kkkk kkkk kkkk kkkk
01563      * Usage      : JMP  k
01564      * Operation  : PC <- k
01565      * Flags      : None
01566      * Num Clocks : 3
01567      */
01568     int kh = arg1;
01569     int kl = flash_read (core->flash, avr_core_PC_get (core) + 1);
01570 
01571     int k = (kh << 16) + kl;
01572 
01573     avr_core_PC_set (core, k);
01574     avr_core_inst_CKS_set (core, 3);
01575 
01576     return opcode_JMP;
01577 }
01578 
01579 static int
01580 avr_op_LDD_Y (AvrCore *core, uint16_t opcode, unsigned int arg1,
01581               unsigned int arg2)
01582 {
01583     /*
01584      * Load Indirect with Displacement using index Y.
01585      *
01586      * Opcode     : 10q0 qq0d dddd 1qqq 
01587      * Usage      : LDD  Rd, Y+q
01588      * Operation  : Rd <- (Y + q)
01589      * Flags      : None
01590      * Num Clocks : 2
01591      */
01592     uint16_t Y;
01593     int Rd = arg1;
01594     int q = arg2;
01595 
01596     /* Y is R29:R28 */
01597     Y = (avr_core_gpwr_get (core, 29) << 8) + avr_core_gpwr_get (core, 28);
01598 
01599     avr_core_gpwr_set (core, Rd, avr_core_mem_read (core, Y + q));
01600 
01601     avr_core_PC_incr (core, 1);
01602     avr_core_inst_CKS_set (core, 2);
01603 
01604     return opcode_LDD_Y;
01605 }
01606 
01607 static int
01608 avr_op_LDD_Z (AvrCore *core, uint16_t opcode, unsigned int arg1,
01609               unsigned int arg2)
01610 {
01611     /*
01612      * Load Indirect with Displacement using index Z.
01613      *
01614      * Opcode     : 10q0 qq0d dddd 0qqq 
01615      * Usage      : LDD  Rd, Z+q
01616      * Operation  : Rd <- (Z + q)
01617      * Flags      : None
01618      * Num Clocks : 2
01619      */
01620     uint16_t Z;
01621 
01622     int Rd = arg1;
01623     int q = arg2;
01624 
01625     /* Z is R31:R30 */
01626     Z = (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
01627 
01628     avr_core_gpwr_set (core, Rd, avr_core_mem_read (core, Z + q));
01629 
01630     avr_core_PC_incr (core, 1);
01631     avr_core_inst_CKS_set (core, 2);
01632 
01633     return opcode_LDD_Z;
01634 }
01635 
01636 static int
01637 avr_op_LDI (AvrCore *core, uint16_t opcode, unsigned int arg1,
01638             unsigned int arg2)
01639 {
01640     /*
01641      * Load Immediate.
01642      *
01643      * Opcode     : 1110 KKKK dddd KKKK 
01644      * Usage      : LDI  Rd, K
01645      * Operation  : Rd  <- K
01646      * Flags      : None
01647      * Num Clocks : 1
01648      */
01649     int Rd = arg1;
01650     uint8_t K = arg2;
01651 
01652     avr_core_gpwr_set (core, Rd, K);
01653 
01654     avr_core_PC_incr (core, 1);
01655     avr_core_inst_CKS_set (core, 1);
01656 
01657     return opcode_LDI;
01658 }
01659 
01660 static int
01661 avr_op_LDS (AvrCore *core, uint16_t opcode, unsigned int arg1,
01662             unsigned int arg2)
01663 {
01664     /*
01665      * Load Direct from data space.
01666      *
01667      * Opcode     : 1001 000d dddd 0000 kkkk kkkk kkkk kkkk
01668      * Usage      : LDS  Rd, k
01669      * Operation  : Rd <- (k)
01670      * Flags      : None
01671      * Num Clocks : 2
01672      */
01673     int Rd = arg1;
01674 
01675     /* Get data at k in current data segment and put into Rd */
01676     int k_pc = avr_core_PC_get (core) + 1;
01677     int k = flash_read (core->flash, k_pc);
01678 
01679     avr_core_gpwr_set (core, Rd, avr_core_mem_read (core, k));
01680 
01681     avr_core_PC_incr (core, 2);
01682     avr_core_inst_CKS_set (core, 2);
01683 
01684     return opcode_LDS;
01685 }
01686 
01687 static int
01688 avr_op_LD_X (AvrCore *core, uint16_t opcode, unsigned int arg1,
01689              unsigned int arg2)
01690 {
01691     /*
01692      * Load Indirect using index X.
01693      *
01694      * Opcode     : 1001 000d dddd 1100 
01695      * Usage      : LD  Rd, X
01696      * Operation  : Rd <- (X)
01697      * Flags      : None
01698      * Num Clocks : 2
01699      */
01700     uint16_t X;
01701 
01702     int Rd = arg1;
01703 
01704     /* X is R27:R26 */
01705     X = (avr_core_gpwr_get (core, 27) << 8) + avr_core_gpwr_get (core, 26);
01706 
01707     avr_core_gpwr_set (core, Rd, avr_core_mem_read (core, X));
01708 
01709     avr_core_PC_incr (core, 1);
01710     avr_core_inst_CKS_set (core, 2);
01711 
01712     return opcode_LD_X;
01713 }
01714 
01715 static int
01716 avr_op_LD_X_decr (AvrCore *core, uint16_t opcode, unsigned int arg1,
01717                   unsigned int arg2)
01718 {
01719     /*
01720      * Load Indirect and Pre-Decrement using index X.
01721      *
01722      * Opcode     : 1001 000d dddd 1110 
01723      * Usage      : LD  Rd, -X
01724      * Operation  : X <- X - 1, Rd <- (X)
01725      * Flags      : None
01726      * Num Clocks : 2
01727      */
01728     uint16_t X;
01729 
01730     int Rd = arg1;
01731 
01732     if ((Rd == 26) || (Rd == 27))
01733         avr_error ("Results of operation are undefined");
01734 
01735     /* X is R27:R26 */
01736     X = (avr_core_gpwr_get (core, 27) << 8) + avr_core_gpwr_get (core, 26);
01737 
01738     /* Perform pre-decrement */
01739     X -= 1;
01740     avr_core_gpwr_set (core, 26, X & 0xff);
01741     avr_core_gpwr_set (core, 27, X >> 8);
01742 
01743     avr_core_gpwr_set (core, Rd, avr_core_mem_read (core, X));
01744 
01745     avr_core_PC_incr (core, 1);
01746     avr_core_inst_CKS_set (core, 2);
01747 
01748     return opcode_LD_X_decr;
01749 }
01750 
01751 static int
01752 avr_op_LD_X_incr (AvrCore *core, uint16_t opcode, unsigned int arg1,
01753                   unsigned int arg2)
01754 {
01755     /*
01756      * Load Indirect and Post-Increment using index X.
01757      *
01758      * Opcode     : 1001 000d dddd 1101 
01759      * Usage      : LD  Rd, X+
01760      * Operation  : Rd <- (X), X <- X + 1
01761      * Flags      : None
01762      * Num Clocks : 2
01763      */
01764     uint16_t X;
01765 
01766     int Rd = arg1;
01767 
01768     if ((Rd == 26) || (Rd == 27))
01769         avr_error ("Results of operation are undefined");
01770 
01771     /* X is R27:R26 */
01772     X = (avr_core_gpwr_get (core, 27) << 8) + avr_core_gpwr_get (core, 26);
01773 
01774     avr_core_gpwr_set (core, Rd, avr_core_mem_read (core, X));
01775 
01776     /* Perform post-increment */
01777     X += 1;
01778     avr_core_gpwr_set (core, 26, X & 0xff);
01779     avr_core_gpwr_set (core, 27, X >> 8);
01780 
01781     avr_core_PC_incr (core, 1);
01782     avr_core_inst_CKS_set (core, 2);
01783 
01784     return opcode_LD_X_incr;
01785 }
01786 
01787 static int
01788 avr_op_LD_Y_decr (AvrCore *core, uint16_t opcode, unsigned int arg1,
01789                   unsigned int arg2)
01790 {
01791     /*
01792      * Load Indirect and PreDecrement using index Y.
01793      *
01794      * Opcode     : 1001 000d dddd 1010 
01795      * Usage      : LD  Rd, -Y
01796      * Operation  : Y <- Y - 1, Rd <- (Y)
01797      * Flags      : None
01798      * Num Clocks : 2
01799      */
01800     uint16_t Y;
01801 
01802     int Rd = arg1;
01803 
01804     if ((Rd == 28) || (Rd == 29))
01805         avr_error ("Results of operation are undefined");
01806 
01807     /* Y is R29:R28 */
01808     Y = (avr_core_gpwr_get (core, 29) << 8) + avr_core_gpwr_get (core, 28);
01809 
01810     /* Perform pre-decrement */
01811     Y -= 1;
01812     avr_core_gpwr_set (core, 28, Y & 0xff);
01813     avr_core_gpwr_set (core, 29, Y >> 8);
01814 
01815     avr_core_gpwr_set (core, Rd, avr_core_mem_read (core, Y));
01816 
01817     avr_core_PC_incr (core, 1);
01818     avr_core_inst_CKS_set (core, 2);
01819 
01820     return opcode_LD_Y_decr;
01821 }
01822 
01823 static int
01824 avr_op_LD_Y_incr (AvrCore *core, uint16_t opcode, unsigned int arg1,
01825                   unsigned int arg2)
01826 {
01827     /*
01828      * Load Indirect and Post-Increment using index Y.
01829      *
01830      * Opcode     : 1001 000d dddd 1001 
01831      * Usage      : LD  Rd, Y+
01832      * Operation  : Rd <- (Y), Y <- Y + 1
01833      * Flags      : None
01834      * Num Clocks : 2
01835      */
01836     uint16_t Y;
01837 
01838     int Rd = arg1;
01839 
01840     if ((Rd == 28) || (Rd == 29))
01841         avr_error ("Results of operation are undefined");
01842 
01843     /* Y is R29:R28 */
01844     Y = (avr_core_gpwr_get (core, 29) << 8) + avr_core_gpwr_get (core, 28);
01845 
01846     avr_core_gpwr_set (core, Rd, avr_core_mem_read (core, Y));
01847 
01848     /* Perform post-increment */
01849     Y += 1;
01850     avr_core_gpwr_set (core, 28, Y & 0xff);
01851     avr_core_gpwr_set (core, 29, Y >> 8);
01852 
01853     avr_core_PC_incr (core, 1);
01854     avr_core_inst_CKS_set (core, 2);
01855 
01856     return opcode_LD_Y_incr;
01857 }
01858 
01859 static int
01860 avr_op_LD_Z_incr (AvrCore *core, uint16_t opcode, unsigned int arg1,
01861                   unsigned int arg2)
01862 {
01863     /*
01864      * Load Indirect and Post-Increment using index Z.
01865      *
01866      * Opcode     : 1001 000d dddd 0001 
01867      * Usage      : LD  Rd, Z+
01868      * Operation  : Rd <- (Z), Z <- Z+1
01869      * Flags      : None
01870      * Num Clocks : 2
01871      */
01872     uint16_t Z;
01873 
01874     int Rd = arg1;
01875 
01876     if ((Rd == 30) || (Rd == 31))
01877         avr_error ("Results of operation are undefined");
01878 
01879     /* Z is R31:R30 */
01880     Z = (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
01881 
01882     avr_core_gpwr_set (core, Rd, avr_core_mem_read (core, Z));
01883 
01884     /* Perform post-increment */
01885     Z += 1;
01886     avr_core_gpwr_set (core, 30, Z & 0xff);
01887     avr_core_gpwr_set (core, 31, Z >> 8);
01888 
01889     avr_core_PC_incr (core, 1);
01890     avr_core_inst_CKS_set (core, 2);
01891 
01892     return opcode_LD_Z_incr;
01893 }
01894 
01895 static int
01896 avr_op_LD_Z_decr (AvrCore *core, uint16_t opcode, unsigned int arg1,
01897                   unsigned int arg2)
01898 {
01899     /*
01900      * Load Indirect and Pre-Decrement using index Z.
01901      *
01902      * Opcode     : 1001 000d dddd 0010 
01903      * Usage      : LD  Rd, -Z
01904      * Operation  : Z <- Z - 1, Rd <- (Z)
01905      * Flags      : None
01906      * Num Clocks : 2
01907      */
01908     uint16_t Z;
01909 
01910     int Rd = arg1;
01911 
01912     if ((Rd == 30) || (Rd == 31))
01913         avr_error ("Results of operation are undefined");
01914 
01915     /* Z is R31:R30 */
01916     Z = (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
01917 
01918     /* Perform pre-decrement */
01919     Z -= 1;
01920     avr_core_gpwr_set (core, 30, Z & 0xff);
01921     avr_core_gpwr_set (core, 31, Z >> 8);
01922 
01923     avr_core_gpwr_set (core, Rd, avr_core_mem_read (core, Z));
01924 
01925     avr_core_PC_incr (core, 1);
01926     avr_core_inst_CKS_set (core, 2);
01927 
01928     return opcode_LD_Z_decr;
01929 }
01930 
01931 static int
01932 avr_op_LPM_Z (AvrCore *core, uint16_t opcode, unsigned int arg1,
01933               unsigned int arg2)
01934 {
01935     /*
01936      * Load Program Memory.
01937      *
01938      * Opcode     : 1001 000d dddd 0100 
01939      * Usage      : LPM  Rd, Z
01940      * Operation  : Rd <- (Z)
01941      * Flags      : None
01942      * Num Clocks : 3
01943      */
01944     uint16_t Z, high_byte;
01945     uint16_t data;
01946 
01947     int Rd = arg1;
01948 
01949     /* Z is R31:R30 */
01950     Z = (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
01951     high_byte = Z & 0x1;
01952 
01953     /* FIXME: I don't know if this is the right thing to do. I'm not sure that
01954        I understand what the instruction data sheet is saying about Z.
01955        Dividing by 2 seems to give the address that we want though. */
01956 
01957     data = flash_read (core->flash, Z / 2);
01958 
01959     if (high_byte == 1)
01960         avr_core_gpwr_set (core, Rd, data >> 8);
01961     else
01962         avr_core_gpwr_set (core, Rd, data & 0xff);
01963 
01964     avr_core_PC_incr (core, 1);
01965     avr_core_inst_CKS_set (core, 3);
01966 
01967     return opcode_LPM_Z;
01968 }
01969 
01970 static int
01971 avr_op_LPM (AvrCore *core, uint16_t opcode, unsigned int arg1,
01972             unsigned int arg2)
01973 {
01974     /* Load Program Memory.
01975      *
01976      * This the same as avr_op_LPM_Z with Rd = R0.
01977      *
01978      * Opcode     : 1001 0101 1100 1000 
01979      * Usage      : LPM  
01980      * Operation  : R0 <- (Z)
01981      * Flags      : None
01982      * Num Clocks : 3
01983      */
01984     return avr_op_LPM_Z (core, 0x9004, 0, arg2);
01985 }
01986 
01987 static int
01988 avr_op_LPM_Z_incr (AvrCore *core, uint16_t opcode, unsigned int arg1,
01989                    unsigned int arg2)
01990 {
01991     /*
01992      * Load Program Memory and Post-Incr.
01993      *
01994      * Opcode     : 1001 000d dddd 0101 
01995      * Usage      : LPM  Rd, Z+
01996      * Operation  : Rd <- (Z), Z <- Z + 1
01997      * Flags      : None
01998      * Num Clocks : 3
01999      */
02000     uint16_t Z, high_byte;
02001     uint16_t data;
02002 
02003     int Rd = arg1;
02004 
02005     if ((Rd == 30) || (Rd == 31))
02006         avr_error ("Results of operation are undefined");
02007 
02008     /* Z is R31:R30 */
02009     Z = (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
02010     high_byte = Z & 0x1;
02011 
02012     /* FIXME: I don't know if this is the right thing to do. I'm not sure that
02013        I understand what the instruction data sheet is saying about Z.
02014        Dividing by 2 seems to give the address that we want though. */
02015 
02016     data = flash_read (core->flash, Z / 2);
02017 
02018     if (high_byte == 1)
02019         avr_core_gpwr_set (core, Rd, data >> 8);
02020     else
02021         avr_core_gpwr_set (core, Rd, data & 0xff);
02022 
02023     /* Perform post-increment */
02024     Z += 1;
02025     avr_core_gpwr_set (core, 30, Z & 0xff);
02026     avr_core_gpwr_set (core, 31, Z >> 8);
02027 
02028     avr_core_PC_incr (core, 1);
02029     avr_core_inst_CKS_set (core, 3);
02030 
02031     return opcode_LPM_Z_incr;
02032 }
02033 
02034 static int
02035 avr_op_LSR (AvrCore *core, uint16_t opcode, unsigned int arg1,
02036             unsigned int arg2)
02037 {
02038     /*
02039      * Logical Shift Right.
02040      *
02041      * Opcode     : 1001 010d dddd 0110 
02042      * Usage      : LSR  Rd
02043      * Operation  : Rd(n) <- Rd(n+1), Rd(7) <- 0, C <- Rd(0)
02044      * Flags      : Z,C,N,V,S
02045      * Num Clocks : 1
02046      */
02047     int Z, C, N, V, S;
02048 
02049     int Rd = arg1;
02050     uint8_t rd = avr_core_gpwr_get (core, Rd);
02051 
02052     uint8_t res = (rd >> 1) & 0x7f;
02053 
02054     uint8_t sreg = avr_core_sreg_get (core);
02055 
02056     sreg = set_bit_in_byte (sreg, SREG_C, C = (rd & 0x1));
02057     sreg = set_bit_in_byte (sreg, SREG_N, N = (0));
02058     sreg = set_bit_in_byte (sreg, SREG_V, V = (N ^ C));
02059     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
02060     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
02061 
02062     avr_core_sreg_set (core, sreg);
02063 
02064     avr_core_gpwr_set (core, Rd, res);
02065 
02066     avr_core_PC_incr (core, 1);
02067     avr_core_inst_CKS_set (core, 1);
02068 
02069     return opcode_LSR;
02070 }
02071 
02072 static int
02073 avr_op_MOV (AvrCore *core, uint16_t opcode, unsigned int arg1,
02074             unsigned int arg2)
02075 {
02076     /* Copy Register.
02077      *
02078      * Opcode     : 0010 11rd dddd rrrr 
02079      * Usage      : MOV  Rd, Rr
02080      * Operation  : Rd <- Rr
02081      * Flags      : None
02082      * Num Clocks : 1
02083      */
02084     int Rd = arg1;
02085     int Rr = arg2;
02086 
02087     avr_core_gpwr_set (core, Rd, avr_core_gpwr_get (core, Rr));
02088 
02089     avr_core_PC_incr (core, 1);
02090     avr_core_inst_CKS_set (core, 1);
02091 
02092     return opcode_MOV;
02093 }
02094 
02095 static int
02096 avr_op_MOVW (AvrCore *core, uint16_t opcode, unsigned int arg1,
02097              unsigned int arg2)
02098 {
02099     /*
02100      *Copy Register Pair.
02101      *
02102      * Opcode     : 0000 0001 dddd rrrr 
02103      * Usage      : MOVW  Rd, Rr
02104      * Operation  : Rd+1:Rd <- Rr+1:Rr
02105      * Flags      : None
02106      * Num Clocks : 1
02107      */
02108     int Rd = arg1;
02109     int Rr = arg2;
02110 
02111     /* get_rd_4() returns 16 <= r <= 31, but here Rd and Rr */
02112     /* are even from 0 <= r <= 30. So we translate. */
02113     Rd = (Rd - 16) * 2;
02114     Rr = (Rr - 16) * 2;
02115 
02116     avr_core_gpwr_set (core, Rd, avr_core_gpwr_get (core, Rr));
02117     avr_core_gpwr_set (core, Rd + 1, avr_core_gpwr_get (core, Rr + 1));
02118 
02119     avr_core_PC_incr (core, 1);
02120     avr_core_inst_CKS_set (core, 1);
02121 
02122     return opcode_MOVW;
02123 }
02124 
02125 static int
02126 avr_op_MUL (AvrCore *core, uint16_t opcode, unsigned int arg1,
02127             unsigned int arg2)
02128 {
02129     /*
02130      * Mult Unsigned.
02131      *
02132      * Opcode     : 1001 11rd dddd rrrr 
02133      * Usage      : MUL  Rd, Rr
02134      * Operation  : R1:R0 <- Rd * Rr (UU)
02135      * Flags      : Z,C
02136      * Num Clocks : 2
02137      */
02138     int Rd = arg1;
02139     int Rr = arg2;
02140 
02141     uint8_t rd = avr_core_gpwr_get (core, Rd);
02142     uint8_t rr = avr_core_gpwr_get (core, Rr);
02143 
02144     uint16_t res = rd * rr;
02145 
02146     uint8_t sreg = avr_core_sreg_get (core);
02147 
02148     sreg = set_bit_in_byte (sreg, SREG_Z, ((res & 0xffff) == 0));
02149     sreg = set_bit_in_byte (sreg, SREG_C, ((res >> 15) & 0x1));
02150 
02151     avr_core_sreg_set (core, sreg);
02152 
02153     /* result goes in R1:R0 */
02154 
02155     avr_core_gpwr_set (core, 1, res >> 8);
02156     avr_core_gpwr_set (core, 0, res & 0xff);
02157 
02158     avr_core_PC_incr (core, 1);
02159     avr_core_inst_CKS_set (core, 2);
02160 
02161     return opcode_MUL;
02162 }
02163 
02164 static int
02165 avr_op_MULS (AvrCore *core, uint16_t opcode, unsigned int arg1,
02166              unsigned int arg2)
02167 {
02168     /*
02169      * Mult Signed.
02170      *
02171      * Opcode     : 0000 0010 dddd rrrr 
02172      * Usage      : MULS  Rd, Rr
02173      * Operation  : R1:R0 <- Rd * Rr (SS)
02174      * Flags      : Z,C
02175      * Num Clocks : 2
02176      */
02177     int Rd = arg1;
02178     int Rr = arg2;
02179 
02180     int8_t rd = (int8_t) avr_core_gpwr_get (core, Rd);
02181     int8_t rr = (int8_t) avr_core_gpwr_get (core, Rr);
02182     int16_t res = rd * rr;
02183 
02184     uint8_t sreg = avr_core_sreg_get (core);
02185 
02186     sreg = set_bit_in_byte (sreg, SREG_Z, ((res & 0xffff) == 0));
02187     sreg = set_bit_in_byte (sreg, SREG_C, ((res >> 15) & 0x1));
02188 
02189     avr_core_sreg_set (core, sreg);
02190 
02191     /* result goes in R1:R0 */
02192     avr_core_gpwr_set (core, 1, res >> 8);
02193     avr_core_gpwr_set (core, 0, res & 0xff);
02194 
02195     avr_core_PC_incr (core, 1);
02196     avr_core_inst_CKS_set (core, 2);
02197 
02198     return opcode_MULS;
02199 }
02200 
02201 static int
02202 avr_op_MULSU (AvrCore *core, uint16_t opcode, unsigned int arg1,
02203               unsigned int arg2)
02204 {
02205     /*
02206      * Mult Signed with Unsigned.
02207      * 
02208      * Rd(unsigned),Rr(signed), result (signed)
02209      *
02210      * Opcode     : 0000 0011 0ddd 0rrr 
02211      * Usage      : MULSU  Rd, Rr
02212      * Operation  : R1:R0 <- Rd * Rr (SU)
02213      * Flags      : Z,C
02214      * Num Clocks : 2
02215      */
02216     int Rd = arg1;
02217     int Rr = arg2;
02218 
02219     int8_t rd = (int8_t) avr_core_gpwr_get (core, Rd);
02220     uint8_t rr = avr_core_gpwr_get (core, Rr);
02221 
02222     int16_t res = rd * rr;
02223 
02224     uint8_t sreg = avr_core_sreg_get (core);
02225 
02226     sreg = set_bit_in_byte (sreg, SREG_Z, ((res & 0xffff) == 0));
02227     sreg = set_bit_in_byte (sreg, SREG_C, ((res >> 15) & 0x1));
02228 
02229     avr_core_sreg_set (core, sreg);
02230 
02231     /* result goes in R1:R0 */
02232     avr_core_gpwr_set (core, 1, res >> 8);
02233     avr_core_gpwr_set (core, 0, res & 0xff);
02234 
02235     avr_core_PC_incr (core, 1);
02236     avr_core_inst_CKS_set (core, 2);
02237 
02238     return opcode_MULSU;
02239 }
02240 
02241 static int
02242 avr_op_NEG (AvrCore *core, uint16_t opcode, unsigned int arg1,
02243             unsigned int arg2)
02244 {
02245     /*
02246      * Two's Complement.
02247      *
02248      * Opcode     : 1001 010d dddd 0001 
02249      * Usage      : NEG  Rd
02250      * Operation  : Rd <- $00 - Rd
02251      * Flags      : Z,C,N,V,S,H
02252      * Num Clocks : 1
02253      */
02254     int Z, C, N, V, S, H;
02255 
02256     int Rd = arg1;
02257 
02258     uint8_t rd = avr_core_gpwr_get (core, Rd);
02259     uint8_t res = (0x0 - rd) & 0xff;
02260 
02261     uint8_t sreg = avr_core_sreg_get (core);
02262 
02263     sreg = set_bit_in_byte (sreg, SREG_H, H =
02264                             (((res >> 3) | (rd >> 3)) & 0x1));
02265     sreg = set_bit_in_byte (sreg, SREG_V, V = (res == 0x80));
02266     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
02267     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
02268     sreg = set_bit_in_byte (sreg, SREG_Z, Z = (res == 0x0));
02269     sreg = set_bit_in_byte (sreg, SREG_C, C = (res != 0x0));
02270 
02271     avr_core_sreg_set (core, sreg);
02272 
02273     avr_core_gpwr_set (core, Rd, res);
02274 
02275     avr_core_PC_incr (core, 1);
02276     avr_core_inst_CKS_set (core, 1);
02277 
02278     return opcode_NEG;
02279 }
02280 
02281 static int
02282 avr_op_NOP (AvrCore *core, uint16_t opcode, unsigned int arg1,
02283             unsigned int arg2)
02284 {
02285     /*
02286      * No Operation.
02287      *
02288      * Opcode     : 0000 0000 0000 0000 
02289      * Usage      : NOP  
02290      * Operation  : None
02291      * Flags      : None
02292      * Num Clocks : 1
02293      */
02294     avr_core_PC_incr (core, 1);
02295     avr_core_inst_CKS_set (core, 1);
02296     return opcode_NOP;
02297 }
02298 
02299 static int
02300 avr_op_OR (AvrCore *core, uint16_t opcode, unsigned int arg1,
02301            unsigned int arg2)
02302 {
02303     /*
02304      * Logical OR.
02305      *
02306      * Opcode     : 0010 10rd dddd rrrr 
02307      * Usage      : OR  Rd, Rr
02308      * Operation  : Rd <- Rd or Rr
02309      * Flags      : Z,N,V,S
02310      * Num Clocks : 1
02311      */
02312     int Z, N, V, S;
02313 
02314     int Rd = arg1;
02315     int Rr = arg2;
02316 
02317     uint8_t res = avr_core_gpwr_get (core, Rd) | avr_core_gpwr_get (core, Rr);
02318 
02319     uint8_t sreg = avr_core_sreg_get (core);
02320 
02321     sreg = set_bit_in_byte (sreg, SREG_V, V = (0));
02322     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
02323     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
02324     sreg = set_bit_in_byte (sreg, SREG_Z, Z = (res == 0x0));
02325 
02326     avr_core_sreg_set (core, sreg);
02327 
02328     avr_core_gpwr_set (core, Rd, res);
02329 
02330     avr_core_PC_incr (core, 1);
02331     avr_core_inst_CKS_set (core, 1);
02332 
02333     return opcode_OR;
02334 }
02335 
02336 static int
02337 avr_op_ORI (AvrCore *core, uint16_t opcode, unsigned int arg1,
02338             unsigned int arg2)
02339 {
02340     /*
02341      * Logical OR with Immed.
02342      *
02343      * Opcode     : 0110 KKKK dddd KKKK 
02344      * Usage      : ORI  Rd, K
02345      * Operation  : Rd <- Rd or K
02346      * Flags      : Z,N,V,S
02347      * Num Clocks : 1
02348      */
02349     int Z, N, V, S;
02350 
02351     int Rd = arg1;
02352     uint8_t K = arg2;
02353 
02354     uint8_t res = avr_core_gpwr_get (core, Rd) | K;
02355 
02356     uint8_t sreg = avr_core_sreg_get (core);
02357 
02358     sreg = set_bit_in_byte (sreg, SREG_V, V = (0));
02359     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
02360     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
02361     sreg = set_bit_in_byte (sreg, SREG_Z, Z = (res == 0x0));
02362 
02363     avr_core_sreg_set (core, sreg);
02364 
02365     avr_core_gpwr_set (core, Rd, res);
02366 
02367     avr_core_PC_incr (core, 1);
02368     avr_core_inst_CKS_set (core, 1);
02369 
02370     return opcode_ORI;
02371 }
02372 
02373 static int
02374 avr_op_OUT (AvrCore *core, uint16_t opcode, unsigned int arg1,
02375             unsigned int arg2)
02376 {
02377     /*
02378      * Out To I/O Location.
02379      *
02380      * Opcode     : 1011 1AAd dddd AAAA 
02381      * Usage      : OUT  A, Rd
02382      * Operation  : I/O(A) <- Rd
02383      * Flags      : None
02384      * Num Clocks : 1
02385      */
02386 
02387     /* Even though the args in the comment are reversed (out arg2, arg1), the
02388        following is correct: Rd=arg1, A=arg2. */
02389     int Rd = arg1;
02390     int A = arg2;
02391 
02392     avr_core_io_write (core, A, avr_core_gpwr_get (core, Rd));
02393 
02394     avr_core_PC_incr (core, 1);
02395     avr_core_inst_CKS_set (core, 1);
02396 
02397     return opcode_OUT;
02398 }
02399 
02400 static int
02401 avr_op_POP (AvrCore *core, uint16_t opcode, unsigned int arg1,
02402             unsigned int arg2)
02403 {
02404     /*
02405      * Pop Register from Stack.
02406      *
02407      * Opcode     : 1001 000d dddd 1111 
02408      * Usage      : POP  Rd
02409      * Operation  : Rd <- STACK
02410      * Flags      : None
02411      * Num Clocks : 2
02412      */
02413     int Rd = arg1;
02414 
02415     avr_core_gpwr_set (core, Rd, avr_core_stack_pop (core, 1));
02416 
02417     avr_core_PC_incr (core, 1);
02418     avr_core_inst_CKS_set (core, 2);
02419 
02420     return opcode_POP;
02421 }
02422 
02423 static int
02424 avr_op_PUSH (AvrCore *core, uint16_t opcode, unsigned int arg1,
02425              unsigned int arg2)
02426 {
02427     /*
02428      * Push Register on Stack.
02429      *
02430      * Opcode     : 1001 001d dddd 1111 
02431      * Usage      : PUSH  Rd
02432      * Operation  : STACK <- Rd
02433      * Flags      : None
02434      * Num Clocks : 2
02435      */
02436     int Rd = arg1;
02437 
02438     avr_core_stack_push (core, 1, avr_core_gpwr_get (core, Rd));
02439 
02440     avr_core_PC_incr (core, 1);
02441     avr_core_inst_CKS_set (core, 2);
02442 
02443     return opcode_PUSH;
02444 }
02445 
02446 static int
02447 avr_op_RCALL (AvrCore *core, uint16_t opcode, unsigned int arg1,
02448               unsigned int arg2)
02449 {
02450     /*
02451      * Relative Call Subroutine.
02452      *
02453      * Opcode     : 1101 kkkk kkkk kkkk 
02454      * Usage      : RCALL  k
02455      * Operation  : PC <- PC + k + 1
02456      * Flags      : None
02457      * Num Clocks : 3 / 4
02458      */
02459     int k = arg1;
02460 
02461     int pc = avr_core_PC_get (core);
02462     int pc_bytes = avr_core_PC_size (core);
02463 
02464     avr_core_stack_push (core, pc_bytes, pc + 1);
02465 
02466     avr_core_PC_incr (core, k + 1);
02467     avr_core_inst_CKS_set (core, pc_bytes + 1);
02468 
02469     return opcode_RCALL;
02470 }
02471 
02472 static int
02473 avr_op_RET (AvrCore *core, uint16_t opcode, unsigned int arg1,
02474             unsigned int arg2)
02475 {
02476     /*
02477      * Subroutine Return.
02478      *
02479      * Opcode     : 1001 0101 0000 1000 
02480      * Usage      : RET  
02481      * Operation  : PC <- STACK
02482      * Flags      : None
02483      * Num Clocks : 4 / 5
02484      */
02485     int pc_bytes = avr_core_PC_size (core);
02486     int pc = avr_core_stack_pop (core, pc_bytes);
02487 
02488     avr_core_PC_set (core, pc);
02489     avr_core_inst_CKS_set (core, pc_bytes + 2);
02490 
02491     return opcode_RET;
02492 }
02493 
02494 static int
02495 avr_op_RETI (AvrCore *core, uint16_t opcode, unsigned int arg1,
02496              unsigned int arg2)
02497 {
02498     /*
02499      * Interrupt Return.
02500      *
02501      * Opcode     : 1001 0101 0001 1000 
02502      * Usage      : RETI  
02503      * Operation  : PC <- STACK
02504      * Flags      : I
02505      * Num Clocks : 4 / 5
02506      */
02507     int pc_bytes = avr_core_PC_size (core);
02508     int pc = avr_core_stack_pop (core, pc_bytes);
02509 
02510     avr_core_PC_set (core, pc);
02511     avr_core_inst_CKS_set (core, pc_bytes + 2);
02512 
02513     avr_core_sreg_set_bit (core, SREG_I, 1);
02514 
02515     return opcode_RETI;
02516 }
02517 
02518 static int
02519 avr_op_RJMP (AvrCore *core, uint16_t opcode, unsigned int arg1,
02520              unsigned int arg2)
02521 {
02522     /*
02523      * Relative Jump.
02524      *
02525      * Opcode     : 1100 kkkk kkkk kkkk 
02526      * Usage      : RJMP  k
02527      * Operation  : PC <- PC + k + 1
02528      * Flags      : None
02529      * Num Clocks : 2
02530      */
02531     int k = arg1;
02532 
02533     avr_core_PC_incr (core, k + 1);
02534     avr_core_inst_CKS_set (core, 2);
02535 
02536     return opcode_RJMP;
02537 }
02538 
02539 static int
02540 avr_op_ROR (AvrCore *core, uint16_t opcode, unsigned int arg1,
02541             unsigned int arg2)
02542 {
02543     /*
02544      * Rotate Right Though Carry.
02545      *
02546      * Opcode     : 1001 010d dddd 0111 
02547      * Usage      : ROR  Rd
02548      * Operation  : Rd(7) <- C, Rd(n) <- Rd(n+1), C <- Rd(0)
02549      * Flags      : Z,C,N,V,S
02550      * Num Clocks : 1
02551      */
02552     int Z, C, N, V, S;
02553 
02554     int Rd = arg1;
02555     uint8_t rd = avr_core_gpwr_get (core, Rd);
02556 
02557     uint8_t res =
02558         (rd >> 1) | ((avr_core_sreg_get_bit (core, SREG_C) << 7) & 0x80);
02559 
02560     uint8_t sreg = avr_core_sreg_get (core);
02561 
02562     sreg = set_bit_in_byte (sreg, SREG_C, C = (rd & 0x1));
02563     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
02564     sreg = set_bit_in_byte (sreg, SREG_V, V = (N ^ C));
02565     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
02566     sreg = set_bit_in_byte (sreg, SREG_Z, Z = (res == 0));
02567 
02568     avr_core_sreg_set (core, sreg);
02569 
02570     avr_core_gpwr_set (core, Rd, res);
02571 
02572     avr_core_PC_incr (core, 1);
02573     avr_core_inst_CKS_set (core, 1);
02574 
02575     return opcode_ROR;
02576 }
02577 
02578 static int
02579 avr_op_SBC (AvrCore *core, uint16_t opcode, unsigned int arg1,
02580             unsigned int arg2)
02581 {
02582     /*
02583      * Subtract with Carry.
02584      *
02585      * Opcode     : 0000 10rd dddd rrrr 
02586      * Usage      : SBC  Rd, Rr
02587      * Operation  : Rd <- Rd - Rr - C
02588      * Flags      : Z,C,N,V,S,H
02589      * Num Clocks : 1
02590      */
02591     int Z, C, N, V, S, H;
02592 
02593     int Rd = arg1;
02594     int Rr = arg2;
02595 
02596     uint8_t rd = avr_core_gpwr_get (core, Rd);
02597     uint8_t rr = avr_core_gpwr_get (core, Rr);
02598 
02599     uint8_t res = rd - rr - avr_core_sreg_get_bit (core, SREG_C);
02600 
02601     uint8_t sreg = avr_core_sreg_get (core);
02602 
02603     sreg = set_bit_in_byte (sreg, SREG_H, H =
02604                             (get_sub_carry (res, rd, rr, 3)));
02605     sreg = set_bit_in_byte (sreg, SREG_V, V =
02606                             (get_sub_overflow (res, rd, rr)));
02607     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
02608     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
02609     sreg = set_bit_in_byte (sreg, SREG_C, C =
02610                             (get_sub_carry (res, rd, rr, 7)));
02611 
02612     if ((res & 0xff) != 0)
02613         sreg = set_bit_in_byte (sreg, SREG_Z, Z = (0));
02614 
02615     avr_core_sreg_set (core, sreg);
02616 
02617     avr_core_gpwr_set (core, Rd, res);
02618 
02619     avr_core_PC_incr (core, 1);
02620     avr_core_inst_CKS_set (core, 1);
02621 
02622     return opcode_SBC;
02623 }
02624 
02625 static int
02626 avr_op_SBCI (AvrCore *core, uint16_t opcode, unsigned int arg1,
02627              unsigned int arg2)
02628 {
02629     /*
02630      * Subtract Immediate with Carry.
02631      *
02632      * Opcode     : 0100 KKKK dddd KKKK 
02633      * Usage      : SBCI  Rd, K
02634      * Operation  : Rd <- Rd - K - C
02635      * Flags      : Z,C,N,V,S,H
02636      * Num Clocks : 1
02637      */
02638     int Z, C, N, V, S, H;
02639 
02640     int Rd = arg1;
02641     uint8_t K = arg2;
02642 
02643     uint8_t rd = avr_core_gpwr_get (core, Rd);
02644 
02645     uint8_t res = rd - K - avr_core_sreg_get_bit (core, SREG_C);
02646 
02647     uint8_t sreg = avr_core_sreg_get (core);
02648 
02649     sreg = set_bit_in_byte (sreg, SREG_H, H =
02650                             (get_sub_carry (res, rd, K, 3)));
02651     sreg = set_bit_in_byte (sreg, SREG_V, V =
02652                             (get_sub_overflow (res, rd, K)));
02653     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
02654     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
02655     sreg = set_bit_in_byte (sreg, SREG_C, C =
02656                             (get_sub_carry (res, rd, K, 7)));
02657 
02658     if ((res & 0xff) != 0)
02659         sreg = set_bit_in_byte (sreg, SREG_Z, Z = 0);
02660 
02661     avr_core_sreg_set (core, sreg);
02662 
02663     avr_core_gpwr_set (core, Rd, res);
02664 
02665     avr_core_PC_incr (core, 1);
02666     avr_core_inst_CKS_set (core, 1);
02667 
02668     return opcode_SBCI;
02669 }
02670 
02671 static int
02672 avr_op_SBI (AvrCore *core, uint16_t opcode, unsigned int arg1,
02673             unsigned int arg2)
02674 {
02675     /*
02676      * Set Bit in I/O Register.
02677      *
02678      * Opcode     : 1001 1010 AAAA Abbb 
02679      * Usage      : SBI  A, b
02680      * Operation  : I/O(A, b) <- 1
02681      * Flags      : None
02682      * Num Clocks : 2
02683      */
02684     int A = arg1;
02685     int b = arg2;
02686 
02687     uint8_t val = avr_core_io_read (core, A);
02688     avr_core_io_write (core, A, val | (1 << b));
02689 
02690     avr_core_PC_incr (core, 1);
02691     avr_core_inst_CKS_set (core, 2);
02692 
02693     return opcode_SBI;
02694 }
02695 
02696 static int
02697 avr_op_SBIC (AvrCore *core, uint16_t opcode, unsigned int arg1,
02698              unsigned int arg2)
02699 {
02700     /*
02701      * Skip if Bit in I/O Reg Cleared.
02702      *
02703      * Opcode     : 1001 1001 AAAA Abbb 
02704      * Usage      : SBIC  A, b
02705      * Operation  : if (I/O(A,b) = 0) PC <- PC + 2 or 3
02706      * Flags      : None
02707      * Num Clocks : 1 / 2 / 3
02708      */
02709     int skip;
02710 
02711     int A = arg1;
02712     int b = arg2;
02713 
02714     if (is_next_inst_2_words (core))
02715         skip = 3;
02716     else
02717         skip = 2;
02718 
02719     if ((avr_core_io_read (core, A) & (1 << b)) == 0)
02720     {
02721         avr_core_PC_incr (core, skip);
02722         avr_core_inst_CKS_set (core, skip);
02723     }
02724     else
02725     {
02726         avr_core_PC_incr (core, 1);
02727         avr_core_inst_CKS_set (core, 1);
02728     }
02729 
02730     return opcode_SBIC;
02731 }
02732 
02733 static int
02734 avr_op_SBIS (AvrCore *core, uint16_t opcode, unsigned int arg1,
02735              unsigned int arg2)
02736 {
02737     /*
02738      * Skip if Bit in I/O Reg Set.
02739      *
02740      * Opcode     : 1001 1011 AAAA Abbb 
02741      * Usage      : SBIS  A, b
02742      * Operation  : if (I/O(A,b) = 1) PC <- PC + 2 or 3
02743      * Flags      : None
02744      * Num Clocks : 1 / 2 / 3
02745      */
02746     int skip;
02747 
02748     int A = arg1;
02749     int b = arg2;
02750 
02751     if (is_next_inst_2_words (core))
02752         skip = 3;
02753     else
02754         skip = 2;
02755 
02756     if ((avr_core_io_read (core, A) & (1 << b)) != 0)
02757     {
02758         avr_core_PC_incr (core, skip);
02759         avr_core_inst_CKS_set (core, skip);
02760     }
02761     else
02762     {
02763         avr_core_PC_incr (core, 1);
02764         avr_core_inst_CKS_set (core, 1);
02765     }
02766 
02767     return opcode_SBIS;
02768 }
02769 
02770 static int
02771 avr_op_SBIW (AvrCore *core, uint16_t opcode, unsigned int arg1,
02772              unsigned int arg2)
02773 {
02774     /*
02775      * Subtract Immed from Word.
02776      *
02777      * Opcode     : 1001 0111 KKdd KKKK 
02778      * Usage      : SBIW  Rd, K
02779      * Operation  : Rd+1:Rd <- Rd+1:Rd - K
02780      * Flags      : Z,C,N,V,S
02781      * Num Clocks : 2
02782      */
02783     int Z, C, N, V, S;
02784 
02785     int Rd = arg1;
02786     uint8_t K = arg2;
02787 
02788     uint8_t rdl = avr_core_gpwr_get (core, Rd);
02789     uint8_t rdh = avr_core_gpwr_get (core, Rd + 1);
02790 
02791     uint16_t rd = (rdh << 8) + rdl;
02792 
02793     uint16_t res = rd - K;
02794 
02795     uint8_t sreg = avr_core_sreg_get (core);
02796 
02797     sreg = set_bit_in_byte (sreg, SREG_V, V =
02798                             ((rdh >> 7 & 0x1) & ~(res >> 15 & 0x1)));
02799     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 15) & 0x1));
02800     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
02801     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xffff) == 0));
02802     sreg = set_bit_in_byte (sreg, SREG_C, C =
02803                             ((res >> 15 & 0x1) & ~(rdh >> 7 & 0x1)));
02804 
02805     avr_core_sreg_set (core, sreg);
02806 
02807     avr_core_gpwr_set (core, Rd, res & 0xff);
02808     avr_core_gpwr_set (core, Rd + 1, res >> 8);
02809 
02810     avr_core_PC_incr (core, 1);
02811     avr_core_inst_CKS_set (core, 2);
02812 
02813     return opcode_SBIW;
02814 }
02815 
02816 static int
02817 avr_op_SBRC (AvrCore *core, uint16_t opcode, unsigned int arg1,
02818              unsigned int arg2)
02819 {
02820     /*
02821      * Skip if Bit in Reg Cleared.
02822      *
02823      * Opcode     : 1111 110d dddd 0bbb 
02824      * Usage      : SBRC  Rd, b
02825      * Operation  : if (Rd(b) = 0) PC <- PC + 2 or 3
02826      * Flags      : None
02827      * Num Clocks : 1 / 2 / 3
02828      */
02829     int skip;
02830 
02831     int Rd = arg1;
02832     int b = arg2;
02833 
02834     if (is_next_inst_2_words (core))
02835         skip = 3;
02836     else
02837         skip = 2;
02838 
02839     if (((avr_core_gpwr_get (core, Rd) >> b) & 0x1) == 0)
02840     {
02841         avr_core_PC_incr (core, skip);
02842         avr_core_inst_CKS_set (core, skip);
02843     }
02844     else
02845     {
02846         avr_core_PC_incr (core, 1);
02847         avr_core_inst_CKS_set (core, 1);
02848     }
02849 
02850     return opcode_SBRC;
02851 }
02852 
02853 static int
02854 avr_op_SBRS (AvrCore *core, uint16_t opcode, unsigned int arg1,
02855              unsigned int arg2)
02856 {
02857     /*
02858      * Skip if Bit in Reg Set.
02859      *
02860      * Opcode     : 1111 111d dddd 0bbb 
02861      * Usage      : SBRS  Rd, b
02862      * Operation  : if (Rd(b) = 1) PC <- PC + 2 or 3
02863      * Flags      : None
02864      * Num Clocks : 1 / 2 / 3
02865      */
02866     int skip;
02867 
02868     int Rd = arg1;
02869     int b = arg2;
02870 
02871     if (is_next_inst_2_words (core))
02872         skip = 3;
02873     else
02874         skip = 2;
02875 
02876     if (((avr_core_gpwr_get (core, Rd) >> b) & 0x1) != 0)
02877     {
02878         avr_core_PC_incr (core, skip);
02879         avr_core_inst_CKS_set (core, skip);
02880     }
02881     else
02882     {
02883         avr_core_PC_incr (core, 1);
02884         avr_core_inst_CKS_set (core, 1);
02885     }
02886 
02887     return opcode_SBRS;
02888 }
02889 
02890 static int
02891 avr_op_SLEEP (AvrCore *core, uint16_t opcode, unsigned int arg1,
02892               unsigned int arg2)
02893 {
02894     /*
02895      * Sleep.
02896      *
02897      * This is device specific and should be overridden by sub-class.
02898      *
02899      * Opcode     : 1001 0101 1000 1000 
02900      * Usage      : SLEEP  
02901      * Operation  : (see specific hardware specification for Sleep)
02902      * Flags      : None
02903      * Num Clocks : 1
02904      */
02905     MCUCR *mcucr = (MCUCR *)avr_core_get_vdev_by_name (core, "MCUCR");
02906 
02907     if (mcucr == NULL)
02908         avr_error ("MCUCR register not installed");
02909 
02910     /* See if sleep mode is enabled */
02911     if (mcucr_get_bit (mcucr, bit_SE))
02912     {
02913         if (mcucr_get_bit (mcucr, bit_SM) == 0)
02914         {
02915             /* Idle Mode */
02916             avr_core_set_sleep_mode (core, SLEEP_MODE_IDLE);
02917         }
02918         else
02919         {
02920             /* Power Down Mode */
02921             avr_core_set_sleep_mode (core, SLEEP_MODE_PWR_DOWN);
02922         }
02923     }
02924 
02925     avr_core_PC_incr (core, 1);
02926     avr_core_inst_CKS_set (core, 1);
02927 
02928     return opcode_SLEEP;
02929 }
02930 
02931 static int
02932 avr_op_SPM (AvrCore *core, uint16_t opcode, unsigned int arg1,
02933             unsigned int arg2)
02934 {
02935     /*
02936      * Store Program Memory.
02937      *
02938      * Opcode     : 1001 0101 1110 1000 
02939      * Usage      : SPM  
02940      * Operation  : (Z) <- R1:R0
02941      * Flags      : None
02942      * Num Clocks : -
02943      */
02944     avr_error ("This opcode is not implemented yet: 0x%04x", opcode);
02945     return opcode_SPM;
02946 }
02947 
02948 static int
02949 avr_op_STD_Y (AvrCore *core, uint16_t opcode, unsigned int arg1,
02950               unsigned int arg2)
02951 {
02952     /*
02953      * Store Indirect with Displacement.
02954      *
02955      * Opcode     : 10q0 qq1d dddd 1qqq 
02956      * Usage      : STD  Y+q, Rd
02957      * Operation  : (Y + q) <- Rd
02958      * Flags      : None
02959      * Num Clocks : 2
02960      */
02961     int Y;
02962 
02963     int q = arg2;
02964     int Rd = arg1;
02965 
02966     /* Y is R29:R28 */
02967     Y = (avr_core_gpwr_get (core, 29) << 8) + avr_core_gpwr_get (core, 28);
02968 
02969     avr_core_mem_write (core, Y + q, avr_core_gpwr_get (core, Rd));
02970 
02971     avr_core_PC_incr (core, 1);
02972     avr_core_inst_CKS_set (core, 2);
02973 
02974     return opcode_STD_Y;
02975 }
02976 
02977 static int
02978 avr_op_STD_Z (AvrCore *core, uint16_t opcode, unsigned int arg1,
02979               unsigned int arg2)
02980 {
02981     /*
02982      * Store Indirect with Displacement.
02983      *
02984      * Opcode     : 10q0 qq1d dddd 0qqq 
02985      * Usage      : STD  Z+q, Rd
02986      * Operation  : (Z + q) <- Rd
02987      * Flags      : None
02988      * Num Clocks : 2
02989      */
02990     int Z;
02991 
02992     int q = arg2;
02993     int Rd = arg1;
02994 
02995     /* Z is R31:R30 */
02996     Z = (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
02997 
02998     avr_core_mem_write (core, Z + q, avr_core_gpwr_get (core, Rd));
02999 
03000     avr_core_PC_incr (core, 1);
03001     avr_core_inst_CKS_set (core, 2);
03002 
03003     return opcode_STD_Z;
03004 }
03005 
03006 static int
03007 avr_op_STS (AvrCore *core, uint16_t opcode, unsigned int arg1,
03008             unsigned int arg2)
03009 {
03010     /*
03011      * Store Direct to data space.
03012      *
03013      * Opcode     : 1001 001d dddd 0000 kkkk kkkk kkkk kkkk
03014      * Usage      : STS  k, Rd
03015      * Operation  : (k) <- Rd
03016      * Flags      : None
03017      * Num Clocks : 2
03018      */
03019     int Rd = arg1;
03020 
03021     /* Get data at k in current data segment and put into Rd */
03022     int k_pc = avr_core_PC_get (core) + 1;
03023     int k = flash_read (core->flash, k_pc);
03024 
03025     avr_core_mem_write (core, k, avr_core_gpwr_get (core, Rd));
03026 
03027     avr_core_PC_incr (core, 2);
03028     avr_core_inst_CKS_set (core, 2);
03029 
03030     return opcode_STS;
03031 }
03032 
03033 static int
03034 avr_op_ST_X (AvrCore *core, uint16_t opcode, unsigned int arg1,
03035              unsigned int arg2)
03036 {
03037     /*
03038      * Store Indirect using index X.
03039      *
03040      * Opcode     : 1001 001d dddd 1100 
03041      * Usage      : ST  X, Rd
03042      * Operation  : (X) <- Rd
03043      * Flags      : None
03044      * Num Clocks : 2
03045      */
03046     uint16_t X;
03047 
03048     int Rd = arg1;
03049 
03050     /* X is R27:R26 */
03051     X = (avr_core_gpwr_get (core, 27) << 8) + avr_core_gpwr_get (core, 26);
03052 
03053     avr_core_mem_write (core, X, avr_core_gpwr_get (core, Rd));
03054 
03055     avr_core_PC_incr (core, 1);
03056     avr_core_inst_CKS_set (core, 2);
03057 
03058     return opcode_ST_X;
03059 }
03060 
03061 static int
03062 avr_op_ST_X_decr (AvrCore *core, uint16_t opcode, unsigned int arg1,
03063                   unsigned int arg2)
03064 {
03065     /*
03066      * Store Indirect and Pre-Decrement using index X.
03067      *
03068      * Opcode     : 1001 001d dddd 1110 
03069      * Usage      : ST  -X, Rd
03070      * Operation  : X <- X - 1, (X) <- Rd
03071      * Flags      : None
03072      * Num Clocks : 2
03073      */
03074     uint16_t X;
03075 
03076     int Rd = arg1;
03077 
03078     if ((Rd == 26) || (Rd == 27))
03079         avr_error ("Results of operation are undefined: 0x%04x", opcode);
03080 
03081     /* X is R27:R26 */
03082     X = (avr_core_gpwr_get (core, 27) << 8) + avr_core_gpwr_get (core, 26);
03083 
03084     /* Perform pre-decrement */
03085     X -= 1;
03086     avr_core_gpwr_set (core, 26, X & 0xff);
03087     avr_core_gpwr_set (core, 27, X >> 8);
03088 
03089     avr_core_mem_write (core, X, avr_core_gpwr_get (core, Rd));
03090 
03091     avr_core_PC_incr (core, 1);
03092     avr_core_inst_CKS_set (core, 2);
03093 
03094     return opcode_ST_X_decr;
03095 }
03096 
03097 static int
03098 avr_op_ST_X_incr (AvrCore *core, uint16_t opcode, unsigned int arg1,
03099                   unsigned int arg2)
03100 {
03101     /*
03102      * Store Indirect and Post-Increment using index X.
03103      *
03104      * Opcode     : 1001 001d dddd 1101 
03105      * Usage      : ST  X+, Rd
03106      * Operation  : (X) <- Rd, X <- X + 1
03107      * Flags      : None
03108      * Num Clocks : 2
03109      */
03110     uint16_t X;
03111 
03112     int Rd = arg1;
03113 
03114     if ((Rd == 26) || (Rd == 27))
03115         avr_error ("Results of operation are undefined: 0x%04x", opcode);
03116 
03117     /* X is R27:R26 */
03118     X = (avr_core_gpwr_get (core, 27) << 8) + avr_core_gpwr_get (core, 26);
03119 
03120     avr_core_mem_write (core, X, avr_core_gpwr_get (core, Rd));
03121 
03122     /* Perform post-increment */
03123     X += 1;
03124     avr_core_gpwr_set (core, 26, X & 0xff);
03125     avr_core_gpwr_set (core, 27, X >> 8);
03126 
03127     avr_core_PC_incr (core, 1);
03128     avr_core_inst_CKS_set (core, 2);
03129 
03130     return opcode_ST_X_incr;
03131 }
03132 
03133 static int
03134 avr_op_ST_Y_decr (AvrCore *core, uint16_t opcode, unsigned int arg1,
03135                   unsigned int arg2)
03136 {
03137     /*
03138      * Store Indirect and Pre-Decrement using index Y.
03139      *
03140      * Opcode     : 1001 001d dddd 1010 
03141      * Usage      : ST  -Y, Rd
03142      * Operation  : Y <- Y - 1, (Y) <- Rd
03143      * Flags      : None
03144      * Num Clocks : 2
03145      */
03146     uint16_t Y;
03147 
03148     int Rd = arg1;
03149 
03150     if ((Rd == 28) || (Rd == 29))
03151         avr_error ("Results of operation are undefined: 0x%04x", opcode);
03152 
03153     /* Y is R29:R28 */
03154     Y = (avr_core_gpwr_get (core, 29) << 8) + avr_core_gpwr_get (core, 28);
03155 
03156     /* Perform pre-decrement */
03157     Y -= 1;
03158     avr_core_gpwr_set (core, 28, Y & 0xff);
03159     avr_core_gpwr_set (core, 29, Y >> 8);
03160 
03161     avr_core_mem_write (core, Y, avr_core_gpwr_get (core, Rd));
03162 
03163     avr_core_PC_incr (core, 1);
03164     avr_core_inst_CKS_set (core, 2);
03165 
03166     return opcode_ST_Y_decr;
03167 }
03168 
03169 static int
03170 avr_op_ST_Y_incr (AvrCore *core, uint16_t opcode, unsigned int arg1,
03171                   unsigned int arg2)
03172 {
03173     /*
03174      * Store Indirect and Post-Increment using index Y.
03175      *
03176      * Opcode     : 1001 001d dddd 1001 
03177      * Usage      : ST  Y+, Rd
03178      * Operation  : (Y) <- Rd, Y <- Y + 1
03179      * Flags      : None
03180      * Num Clocks : 2
03181      */
03182     uint16_t Y;
03183 
03184     int Rd = arg1;
03185 
03186     if ((Rd == 28) || (Rd == 29))
03187         avr_error ("Results of operation are undefined: 0x%04x", opcode);
03188 
03189     /* Y is R29:R28 */
03190     Y = (avr_core_gpwr_get (core, 29) << 8) + avr_core_gpwr_get (core, 28);
03191 
03192     avr_core_mem_write (core, Y, avr_core_gpwr_get (core, Rd));
03193 
03194     /* Perform post-increment */
03195     Y += 1;
03196     avr_core_gpwr_set (core, 28, Y & 0xff);
03197     avr_core_gpwr_set (core, 29, Y >> 8);
03198 
03199     avr_core_PC_incr (core, 1);
03200     avr_core_inst_CKS_set (core, 2);
03201 
03202     return opcode_ST_Y_incr;
03203 }
03204 
03205 static int
03206 avr_op_ST_Z_decr (AvrCore *core, uint16_t opcode, unsigned int arg1,
03207                   unsigned int arg2)
03208 {
03209     /*
03210      * Store Indirect and Pre-Decrement using index Z.
03211      *
03212      * Opcode     : 1001 001d dddd 0010 
03213      * Usage      : ST  -Z, Rd
03214      * Operation  : Z <- Z - 1, (Z) <- Rd
03215      * Flags      : None
03216      * Num Clocks : 2
03217      */
03218     uint16_t Z;
03219 
03220     int Rd = arg1;
03221 
03222     if ((Rd == 30) || (Rd == 31))
03223         avr_error ("Results of operation are undefined: 0x%04x", opcode);
03224 
03225     /* Z is R31:R30 */
03226     Z = (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
03227 
03228     /* Perform pre-decrement */
03229     Z -= 1;
03230     avr_core_gpwr_set (core, 30, Z & 0xff);
03231     avr_core_gpwr_set (core, 31, Z >> 8);
03232 
03233     avr_core_mem_write (core, Z, avr_core_gpwr_get (core, Rd));
03234 
03235     avr_core_PC_incr (core, 1);
03236     avr_core_inst_CKS_set (core, 2);
03237 
03238     return opcode_ST_Z_decr;
03239 }
03240 
03241 static int
03242 avr_op_ST_Z_incr (AvrCore *core, uint16_t opcode, unsigned int arg1,
03243                   unsigned int arg2)
03244 {
03245     /*
03246      * Store Indirect and Post-Increment using index Z.
03247      *
03248      * Opcode     : 1001 001d dddd 0001 
03249      * Usage      : ST  Z+, Rd
03250      * Operation  : (Z) <- Rd, Z <- Z + 1
03251      * Flags      : None
03252      * Num Clocks : 2
03253      */
03254     uint16_t Z;
03255 
03256     int Rd = arg1;
03257 
03258     if ((Rd == 30) || (Rd == 31))
03259         avr_error ("Results of operation are undefined: 0x%04x", opcode);
03260 
03261     /* Z is R31:R30 */
03262     Z = (avr_core_gpwr_get (core, 31) << 8) + avr_core_gpwr_get (core, 30);
03263 
03264     avr_core_mem_write (core, Z, avr_core_gpwr_get (core, Rd));
03265 
03266     /* Perform post-increment */
03267     Z += 1;
03268     avr_core_gpwr_set (core, 30, Z & 0xff);
03269     avr_core_gpwr_set (core, 31, Z >> 8);
03270 
03271     avr_core_PC_incr (core, 1);
03272     avr_core_inst_CKS_set (core, 2);
03273 
03274     return opcode_ST_Z_incr;
03275 }
03276 
03277 static int
03278 avr_op_SUB (AvrCore *core, uint16_t opcode, unsigned int arg1,
03279             unsigned int arg2)
03280 {
03281     /*
03282      * Subtract without Carry.
03283      *
03284      * Opcode     : 0001 10rd dddd rrrr 
03285      * Usage      : SUB  Rd, Rr
03286      * Operation  : Rd <- Rd - Rr
03287      * Flags      : Z,C,N,V,S,H
03288      * Num Clocks : 1
03289      */
03290     int Z, C, N, V, S, H;
03291 
03292     int Rd = arg1;
03293     int Rr = arg2;
03294 
03295     uint8_t rd = avr_core_gpwr_get (core, Rd);
03296     uint8_t rr = avr_core_gpwr_get (core, Rr);
03297 
03298     uint8_t res = rd - rr;
03299 
03300     uint8_t sreg = avr_core_sreg_get (core);
03301 
03302     sreg = set_bit_in_byte (sreg, SREG_H, H =
03303                             (get_sub_carry (res, rd, rr, 3)));
03304     sreg = set_bit_in_byte (sreg, SREG_V, V =
03305                             (get_sub_overflow (res, rd, rr)));
03306     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
03307     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
03308     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
03309     sreg = set_bit_in_byte (sreg, SREG_C, C =
03310                             (get_sub_carry (res, rd, rr, 7)));
03311 
03312     avr_core_sreg_set (core, sreg);
03313 
03314     avr_core_gpwr_set (core, Rd, res);
03315 
03316     avr_core_PC_incr (core, 1);
03317     avr_core_inst_CKS_set (core, 1);
03318 
03319     return opcode_SUB;
03320 }
03321 
03322 static int
03323 avr_op_SUBI (AvrCore *core, uint16_t opcode, unsigned int arg1,
03324              unsigned int arg2)
03325 {
03326     /*
03327      * Subtract Immediate.
03328      *
03329      * Opcode     : 0101 KKKK dddd KKKK 
03330      * Usage      : SUBI  Rd, K
03331      * Operation  : Rd <- Rd - K
03332      * Flags      : Z,C,N,V,S,H
03333      * Num Clocks : 1
03334      */
03335     int Z, C, N, V, S, H;
03336 
03337     int Rd = arg1;
03338     uint8_t K = arg2;
03339 
03340     uint8_t rd = avr_core_gpwr_get (core, Rd);
03341 
03342     uint8_t res = rd - K;
03343 
03344     uint8_t sreg = avr_core_sreg_get (core);
03345 
03346     sreg = set_bit_in_byte (sreg, SREG_H, H =
03347                             (get_sub_carry (res, rd, K, 3)));
03348     sreg = set_bit_in_byte (sreg, SREG_V, V =
03349                             (get_sub_overflow (res, rd, K)));
03350     sreg = set_bit_in_byte (sreg, SREG_N, N = ((res >> 7) & 0x1));
03351     sreg = set_bit_in_byte (sreg, SREG_S, S = (N ^ V));
03352     sreg = set_bit_in_byte (sreg, SREG_Z, Z = ((res & 0xff) == 0));
03353     sreg = set_bit_in_byte (sreg, SREG_C, C =
03354                             (get_sub_carry (res, rd, K, 7)));
03355 
03356     avr_core_sreg_set (core, sreg);
03357 
03358     avr_core_gpwr_set (core, Rd, res);
03359 
03360     avr_core_PC_incr (core, 1);
03361     avr_core_inst_CKS_set (core, 1);
03362 
03363     return opcode_SUBI;
03364 }
03365 
03366 static int
03367 avr_op_SWAP (AvrCore *core, uint16_t opcode, unsigned int arg1,
03368              unsigned int arg2)
03369 {
03370     /*
03371      * Swap Nibbles.
03372      * 
03373      * Opcode     : 1001 010d dddd 0010 
03374      * Usage      : SWAP  Rd
03375      * Operation  : Rd(3..0) <--> Rd(7..4)
03376      * Flags      : None
03377      * Num Clocks : 1
03378      */
03379     int Rd = arg1;
03380     uint8_t rd = avr_core_gpwr_get (core, Rd);
03381 
03382     avr_core_gpwr_set (core, Rd, ((rd << 4) & 0xf0) | ((rd >> 4) & 0x0f));
03383 
03384     avr_core_PC_incr (core, 1);
03385     avr_core_inst_CKS_set (core, 1);
03386 
03387     return opcode_SWAP;
03388 }
03389 
03390 static int
03391 avr_op_WDR (AvrCore *core, uint16_t opcode, unsigned int arg1,
03392             unsigned int arg2)
03393 {
03394     /* 
03395      * Watchdog Reset.
03396      * 
03397      * This is device specific and must be overridden by sub-class.
03398      *
03399      * Opcode     : 1001 0101 1010 1000 
03400      * Usage      : WDR  
03401      * Operation  : (see specific hardware specification for WDR)
03402      * Flags      : None
03403      * Num Clocks : 1
03404      */
03405     WDTCR *wdtcr = (WDTCR *)avr_core_get_vdev_by_name (core, "WDTCR");
03406 
03407     if (wdtcr == NULL)
03408         avr_error ("Core device doesn't have WDTCR attached");
03409 
03410     wdtcr_update (wdtcr);
03411 
03412     avr_core_PC_incr (core, 1);
03413     avr_core_inst_CKS_set (core, 1);
03414 
03415     return opcode_WDR;
03416 }
03417 
03418 int
03419 avr_op_UNKNOWN (AvrCore *core, uint16_t opcode, unsigned int arg1,
03420                 unsigned int arg2)
03421 {
03422     /*
03423      * An unknown opcode was seen. Treat it as a NOP, but return the UNKNOWN
03424      * so that the main loop can issue a warning.
03425      */
03426     avr_op_NOP (core, opcode, arg1, arg2);
03427     return opcode_UNKNOWN;
03428 }
03429 
03430 /******************************************************************************\
03431  *
03432  * Decode an opcode into the opcode handler function.
03433  *
03434  * Generates a warning and returns NULL if opcode is invalid.
03435  *
03436  * Returns a pointer to the function to handle the opcode.
03437  *
03438 \******************************************************************************/
03439 
03440 static void
03441 lookup_opcode (uint16_t opcode, struct opcode_info *opi)
03442 {
03443     uint16_t decode;
03444 
03445     opi->arg1 = -1;
03446     opi->arg2 = -1;
03447     switch (opcode)
03448     {
03449             /* opcodes with no operands */
03450         case 0x9598:
03451             opi->func = avr_op_BREAK;
03452             return;             /* 1001 0101 1001 1000 | BREAK */
03453         case 0x9519:
03454             opi->func = avr_op_EICALL;
03455             return;             /* 1001 0101 0001 1001 | EICALL */
03456         case 0x9419:
03457             opi->func = avr_op_EIJMP;
03458             return;             /* 1001 0100 0001 1001 | EIJMP */
03459         case 0x95D8:
03460             opi->func = avr_op_ELPM;
03461             return;             /* 1001 0101 1101 1000 | ELPM */
03462         case 0x95F8:
03463             opi->func = avr_op_ESPM;
03464             return;             /* 1001 0101 1111 1000 | ESPM */
03465         case 0x9509:
03466             opi->func = avr_op_ICALL;
03467             return;             /* 1001 0101 0000 1001 | ICALL */
03468         case 0x9409:
03469             opi->func = avr_op_IJMP;
03470             return;             /* 1001 0100 0000 1001 | IJMP */
03471         case 0x95C8:
03472             opi->func = avr_op_LPM;
03473             return;             /* 1001 0101 1100 1000 | LPM */
03474         case 0x0000:
03475             opi->func = avr_op_NOP;
03476             return;             /* 0000 0000 0000 0000 | NOP */
03477         case 0x9508:
03478             opi->func = avr_op_RET;
03479             return;             /* 1001 0101 0000 1000 | RET */
03480         case 0x9518:
03481             opi->func = avr_op_RETI;
03482             return;             /* 1001 0101 0001 1000 | RETI */
03483         case 0x9588:
03484             opi->func = avr_op_SLEEP;
03485             return;             /* 1001 0101 1000 1000 | SLEEP */
03486         case 0x95E8:
03487             opi->func = avr_op_SPM;
03488             return;             /* 1001 0101 1110 1000 | SPM */
03489         case 0x95A8:
03490             opi->func = avr_op_WDR;
03491             return;             /* 1001 0101 1010 1000 | WDR */
03492 
03493         default:
03494             {
03495                 /* opcodes with two 5-bit register (Rd and Rr) operands */
03496                 decode = opcode & ~(mask_Rd_5 | mask_Rr_5);
03497                 opi->arg1 = get_rd_5 (opcode);
03498                 opi->arg2 = get_rr_5 (opcode);
03499                 switch (decode)
03500                 {
03501                     case 0x1C00:
03502                         opi->func = avr_op_ADC;
03503                         return; /* 0001 11rd dddd rrrr | ADC or ROL */
03504                     case 0x0C00:
03505                         opi->func = avr_op_ADD;
03506                         return; /* 0000 11rd dddd rrrr | ADD or LSL */
03507                     case 0x2000:
03508                         opi->func = avr_op_AND;
03509                         return; /* 0010 00rd dddd rrrr | AND or TST */
03510                     case 0x1400:
03511                         opi->func = avr_op_CP;
03512                         return; /* 0001 01rd dddd rrrr | CP */
03513                     case 0x0400:
03514                         opi->func = avr_op_CPC;
03515                         return; /* 0000 01rd dddd rrrr | CPC */
03516                     case 0x1000:
03517                         opi->func = avr_op_CPSE;
03518                         return; /* 0001 00rd dddd rrrr | CPSE */
03519                     case 0x2400:
03520                         opi->func = avr_op_EOR;
03521                         return; /* 0010 01rd dddd rrrr | EOR or CLR */
03522                     case 0x2C00:
03523                         opi->func = avr_op_MOV;
03524                         return; /* 0010 11rd dddd rrrr | MOV */
03525                     case 0x9C00:
03526                         opi->func = avr_op_MUL;
03527                         return; /* 1001 11rd dddd rrrr | MUL */
03528                     case 0x2800:
03529                         opi->func = avr_op_OR;
03530                         return; /* 0010 10rd dddd rrrr | OR */
03531                     case 0x0800:
03532                         opi->func = avr_op_SBC;
03533                         return; /* 0000 10rd dddd rrrr | SBC */
03534                     case 0x1800:
03535                         opi->func = avr_op_SUB;
03536                         return; /* 0001 10rd dddd rrrr | SUB */
03537                 }
03538 
03539                 /* opcode with a single register (Rd) as operand */
03540                 decode = opcode & ~(mask_Rd_5);
03541                 opi->arg1 = get_rd_5 (opcode);
03542                 opi->arg2 = -1;
03543                 switch (decode)
03544                 {
03545                     case 0x9405:
03546                         opi->func = avr_op_ASR;
03547                         return; /* 1001 010d dddd 0101 | ASR */
03548                     case 0x9400:
03549                         opi->func = avr_op_COM;
03550                         return; /* 1001 010d dddd 0000 | COM */
03551                     case 0x940A:
03552                         opi->func = avr_op_DEC;
03553                         return; /* 1001 010d dddd 1010 | DEC */
03554                     case 0x9006:
03555                         opi->func = avr_op_ELPM_Z;
03556                         return; /* 1001 000d dddd 0110 | ELPM */
03557                     case 0x9007:
03558                         opi->func = avr_op_ELPM_Z_incr;
03559                         return; /* 1001 000d dddd 0111 | ELPM */
03560                     case 0x9403:
03561                         opi->func = avr_op_INC;
03562                         return; /* 1001 010d dddd 0011 | INC */
03563                     case 0x9000:
03564                         opi->func = avr_op_LDS;
03565                         return; /* 1001 000d dddd 0000 | LDS */
03566                     case 0x900C:
03567                         opi->func = avr_op_LD_X;
03568                         return; /* 1001 000d dddd 1100 | LD */
03569                     case 0x900E:
03570                         opi->func = avr_op_LD_X_decr;
03571                         return; /* 1001 000d dddd 1110 | LD */
03572                     case 0x900D:
03573                         opi->func = avr_op_LD_X_incr;
03574                         return; /* 1001 000d dddd 1101 | LD */
03575                     case 0x900A:
03576                         opi->func = avr_op_LD_Y_decr;
03577                         return; /* 1001 000d dddd 1010 | LD */
03578                     case 0x9009:
03579                         opi->func = avr_op_LD_Y_incr;
03580                         return; /* 1001 000d dddd 1001 | LD */
03581                     case 0x9002:
03582                         opi->func = avr_op_LD_Z_decr;
03583                         return; /* 1001 000d dddd 0010 | LD */
03584                     case 0x9001:
03585                         opi->func = avr_op_LD_Z_incr;
03586                         return; /* 1001 000d dddd 0001 | LD */
03587                     case 0x9004:
03588                         opi->func = avr_op_LPM_Z;
03589                         return; /* 1001 000d dddd 0100 | LPM */
03590                     case 0x9005:
03591                         opi->func = avr_op_LPM_Z_incr;
03592                         return; /* 1001 000d dddd 0101 | LPM */
03593                     case 0x9406:
03594                         opi->func = avr_op_LSR;
03595                         return; /* 1001 010d dddd 0110 | LSR */
03596                     case 0x9401:
03597                         opi->func = avr_op_NEG;
03598                         return; /* 1001 010d dddd 0001 | NEG */
03599                     case 0x900F:
03600                         opi->func = avr_op_POP;
03601                         return; /* 1001 000d dddd 1111 | POP */
03602                     case 0x920F:
03603                         opi->func = avr_op_PUSH;
03604                         return; /* 1001 001d dddd 1111 | PUSH */
03605                     case 0x9407:
03606                         opi->func = avr_op_ROR;
03607                         return; /* 1001 010d dddd 0111 | ROR */
03608                     case 0x9200:
03609                         opi->func = avr_op_STS;
03610                         return; /* 1001 001d dddd 0000 | STS */
03611                     case 0x920C:
03612                         opi->func = avr_op_ST_X;
03613                         return; /* 1001 001d dddd 1100 | ST */
03614                     case 0x920E:
03615                         opi->func = avr_op_ST_X_decr;
03616                         return; /* 1001 001d dddd 1110 | ST */
03617                     case 0x920D:
03618                         opi->func = avr_op_ST_X_incr;
03619                         return; /* 1001 001d dddd 1101 | ST */
03620                     case 0x920A:
03621                         opi->func = avr_op_ST_Y_decr;
03622                         return; /* 1001 001d dddd 1010 | ST */
03623                     case 0x9209:
03624                         opi->func = avr_op_ST_Y_incr;
03625                         return; /* 1001 001d dddd 1001 | ST */
03626                     case 0x9202:
03627                         opi->func = avr_op_ST_Z_decr;
03628                         return; /* 1001 001d dddd 0010 | ST */
03629                     case 0x9201:
03630                         opi->func = avr_op_ST_Z_incr;
03631                         return; /* 1001 001d dddd 0001 | ST */
03632                     case 0x9402:
03633                         opi->func = avr_op_SWAP;
03634                         return; /* 1001 010d dddd 0010 | SWAP */
03635                 }
03636 
03637                 /* opcodes with a register (Rd) and a constant data (K) as
03638                    operands */
03639                 decode = opcode & ~(mask_Rd_4 | mask_K_8);
03640                 opi->arg1 = get_rd_4 (opcode);
03641                 opi->arg2 = get_K_8 (opcode);
03642                 switch (decode)
03643                 {
03644                     case 0x7000:
03645                         opi->func = avr_op_ANDI;
03646                         return; /* 0111 KKKK dddd KKKK | CBR or ANDI */
03647                     case 0x3000:
03648                         opi->func = avr_op_CPI;
03649                         return; /* 0011 KKKK dddd KKKK | CPI */
03650                     case 0xE000:
03651                         opi->func = avr_op_LDI;
03652                         return; /* 1110 KKKK dddd KKKK | LDI or SER */
03653                     case 0x6000:
03654                         opi->func = avr_op_ORI;
03655                         return; /* 0110 KKKK dddd KKKK | SBR or ORI */
03656                     case 0x4000:
03657                         opi->func = avr_op_SBCI;
03658                         return; /* 0100 KKKK dddd KKKK | SBCI */
03659                     case 0x5000:
03660                         opi->func = avr_op_SUBI;
03661                         return; /* 0101 KKKK dddd KKKK | SUBI */
03662                 }
03663 
03664                 /* opcodes with a register (Rd) and a register bit number (b)
03665                    as operands */
03666                 decode = opcode & ~(mask_Rd_5 | mask_reg_bit);
03667                 opi->arg1 = get_rd_5 (opcode);
03668                 opi->arg2 = get_reg_bit (opcode);
03669                 switch (decode)
03670                 {
03671                     case 0xF800:
03672                         opi->func = avr_op_BLD;
03673                         return; /* 1111 100d dddd 0bbb | BLD */
03674                     case 0xFA00:
03675                         opi->func = avr_op_BST;
03676                         return; /* 1111 101d dddd 0bbb | BST */
03677                     case 0xFC00:
03678                         opi->func = avr_op_SBRC;
03679                         return; /* 1111 110d dddd 0bbb | SBRC */
03680                     case 0xFE00:
03681                         opi->func = avr_op_SBRS;
03682                         return; /* 1111 111d dddd 0bbb | SBRS */
03683                 }
03684 
03685                 /* opcodes with a relative 7-bit address (k) and a register
03686                    bit number (b) as operands */
03687                 decode = opcode & ~(mask_k_7 | mask_reg_bit);
03688                 opi->arg1 = get_reg_bit (opcode);
03689                 opi->arg2 = n_bit_unsigned_to_signed (get_k_7 (opcode), 7);
03690                 switch (decode)
03691                 {
03692                     case 0xF400:
03693                         opi->func = avr_op_BRBC;
03694                         return; /* 1111 01kk kkkk kbbb | BRBC */
03695                     case 0xF000:
03696                         opi->func = avr_op_BRBS;
03697                         return; /* 1111 00kk kkkk kbbb | BRBS */
03698                 }
03699 
03700                 /* opcodes with a 6-bit address displacement (q) and a
03701                    register (Rd) as operands */
03702                 decode = opcode & ~(mask_Rd_5 | mask_q_displ);
03703                 opi->arg1 = get_rd_5 (opcode);
03704                 opi->arg2 = get_q (opcode);
03705                 switch (decode)
03706                 {
03707                     case 0x8008:
03708                         opi->func = avr_op_LDD_Y;
03709                         return; /* 10q0 qq0d dddd 1qqq | LDD */
03710                     case 0x8000:
03711                         opi->func = avr_op_LDD_Z;
03712                         return; /* 10q0 qq0d dddd 0qqq | LDD */
03713                     case 0x8208:
03714                         opi->func = avr_op_STD_Y;
03715                         return; /* 10q0 qq1d dddd 1qqq | STD */
03716                     case 0x8200:
03717                         opi->func = avr_op_STD_Z;
03718                         return; /* 10q0 qq1d dddd 0qqq | STD */
03719                 }
03720 
03721                 /* opcodes with a absolute 22-bit address (k) operand */
03722                 decode = opcode & ~(mask_k_22);
03723                 opi->arg1 = get_k_22 (opcode);
03724                 opi->arg2 = -1;
03725                 switch (decode)
03726                 {
03727                     case 0x940E:
03728                         opi->func = avr_op_CALL;
03729                         return; /* 1001 010k kkkk 111k | CALL */
03730                     case 0x940C:
03731                         opi->func = avr_op_JMP;
03732                         return; /* 1001 010k kkkk 110k | JMP */
03733                 }
03734 
03735                 /* opcode with a sreg bit select (s) operand */
03736                 decode = opcode & ~(mask_sreg_bit);
03737                 opi->arg1 = get_sreg_bit (opcode);
03738                 opi->arg2 = -1;
03739                 switch (decode)
03740                 {
03741                         /* BCLR takes place of CL{C,Z,N,V,S,H,T,I} */
03742                         /* BSET takes place of SE{C,Z,N,V,S,H,T,I} */
03743                     case 0x9488:
03744                         opi->func = avr_op_BCLR;
03745                         return; /* 1001 0100 1sss 1000 | BCLR */
03746                     case 0x9408:
03747                         opi->func = avr_op_BSET;
03748                         return; /* 1001 0100 0sss 1000 | BSET */
03749                 }
03750 
03751                 /* opcodes with a 6-bit constant (K) and a register (Rd) as
03752                    operands */
03753                 decode = opcode & ~(mask_K_6 | mask_Rd_2);
03754                 opi->arg1 = get_rd_2 (opcode);
03755                 opi->arg2 = get_K_6 (opcode);
03756                 switch (decode)
03757                 {
03758                     case 0x9600:
03759                         opi->func = avr_op_ADIW;
03760                         return; /* 1001 0110 KKdd KKKK | ADIW */
03761                     case 0x9700:
03762                         opi->func = avr_op_SBIW;
03763                         return; /* 1001 0111 KKdd KKKK | SBIW */
03764                 }
03765 
03766                 /* opcodes with a 5-bit IO Addr (A) and register bit number
03767                    (b) as operands */
03768                 decode = opcode & ~(mask_A_5 | mask_reg_bit);
03769                 opi->arg1 = get_A_5 (opcode);
03770                 opi->arg2 = get_reg_bit (opcode);
03771                 switch (decode)
03772                 {
03773                     case 0x9800:
03774                         opi->func = avr_op_CBI;
03775                         return; /* 1001 1000 AAAA Abbb | CBI */
03776                     case 0x9A00:
03777                         opi->func = avr_op_SBI;
03778                         return; /* 1001 1010 AAAA Abbb | SBI */
03779                     case 0x9900:
03780                         opi->func = avr_op_SBIC;
03781                         return; /* 1001 1001 AAAA Abbb | SBIC */
03782                     case 0x9B00:
03783                         opi->func = avr_op_SBIS;
03784                         return; /* 1001 1011 AAAA Abbb | SBIS */
03785                 }
03786 
03787                 /* opcodes with a 6-bit IO Addr (A) and register (Rd) as
03788                    operands */
03789                 decode = opcode & ~(mask_A_6 | mask_Rd_5);
03790                 opi->arg1 = get_rd_5 (opcode);
03791                 opi->arg2 = get_A_6 (opcode);
03792                 switch (decode)
03793                 {
03794                     case 0xB000:
03795                         opi->func = avr_op_IN;
03796                         return; /* 1011 0AAd dddd AAAA | IN */
03797                     case 0xB800:
03798                         opi->func = avr_op_OUT;
03799                         return; /* 1011 1AAd dddd AAAA | OUT */
03800                 }
03801 
03802                 /* opcodes with a relative 12-bit address (k) operand */
03803                 decode = opcode & ~(mask_k_12);
03804                 opi->arg1 = n_bit_unsigned_to_signed (get_k_12 (opcode), 12);
03805                 opi->arg2 = -1;
03806                 switch (decode)
03807                 {
03808                     case 0xD000:
03809                         opi->func = avr_op_RCALL;
03810                         return; /* 1101 kkkk kkkk kkkk | RCALL */
03811                     case 0xC000:
03812                         opi->func = avr_op_RJMP;
03813                         return; /* 1100 kkkk kkkk kkkk | RJMP */
03814                 }
03815 
03816                 /* opcodes with two 4-bit register (Rd and Rr) operands */
03817                 decode = opcode & ~(mask_Rd_4 | mask_Rr_4);
03818                 opi->arg1 = get_rd_4 (opcode);
03819                 opi->arg2 = get_rr_4 (opcode);
03820                 switch (decode)
03821                 {
03822                     case 0x0100:
03823                         opi->func = avr_op_MOVW;
03824                         return; /* 0000 0001 dddd rrrr | MOVW */
03825                     case 0x0200:
03826                         opi->func = avr_op_MULS;
03827                         return; /* 0000 0010 dddd rrrr | MULS */
03828                 }
03829 
03830                 /* opcodes with two 3-bit register (Rd and Rr) operands */
03831                 decode = opcode & ~(mask_Rd_3 | mask_Rr_3);
03832                 opi->arg1 = get_rd_3 (opcode);
03833                 opi->arg2 = get_rr_3 (opcode);
03834                 switch (decode)
03835                 {
03836                     case 0x0300:
03837                         opi->func = avr_op_MULSU;
03838                         return; /* 0000 0011 0ddd 0rrr | MULSU */
03839                     case 0x0308:
03840                         opi->func = avr_op_FMUL;
03841                         return; /* 0000 0011 0ddd 1rrr | FMUL */
03842                     case 0x0380:
03843                         opi->func = avr_op_FMULS;
03844                         return; /* 0000 0011 1ddd 0rrr | FMULS */
03845                     case 0x0388:
03846                         opi->func = avr_op_FMULSU;
03847                         return; /* 0000 0011 1ddd 1rrr | FMULSU */
03848                 }
03849 
03850             }                   /* default */
03851     }                           /* first switch */
03852 
03853     opi->func = avr_op_UNKNOWN;
03854     opi->arg1 = -1;
03855     opi->arg2 = -1;
03856 
03857 }                               /* decode opcode function */
03858 
03859 /**
03860  * \brief Initialize the decoder lookup table.
03861  *
03862  * This is automatically called by avr_core_construct().
03863  *
03864  * It is safe to call this function many times, since if will only create the
03865  * table the first time it is called.
03866  */
03867 
03868 void
03869 decode_init_lookup_table (void)
03870 {
03871     if (global_opcode_lookup_table == NULL)
03872     {
03873         int num_ops = 0x10000;
03874         int i;
03875         avr_message ("generating opcode lookup_table\n");
03876         global_opcode_lookup_table = avr_new0 (struct opcode_info, num_ops);
03877         for (i = 0; i < num_ops; i++)
03878         {
03879             lookup_opcode (i, global_opcode_lookup_table + i);
03880         }
03881     }
03882 }
03883 
03884 /**
03885  * \brief Decode an opcode into the opcode handler function.
03886  *
03887  * Generates a warning and returns NULL if opcode is invalid.
03888  *
03889  * Returns a pointer to the function to handle the opcode.
03890  */
03891 
03892 extern inline struct opcode_info *decode_opcode (uint16_t opcode);

Automatically generated by Doxygen 1.5.2 on 13 Feb 2008.