--- tme/ic/m68k/m68k-insns.c	2018/04/24 16:37:52	1.1
+++ tme/ic/m68k/m68k-insns.c	2018/04/24 16:40:21	1.1.1.3
@@ -1,11 +1,42 @@
-/* m68k-insns.c - m68k instruction functions: */
+/* $Id: m68k-insns.c,v 1.1.1.3 2018/04/24 16:40:21 root Exp $ */
 
-/* $Header: /var/lib/cvsd/tme/tme/ic/m68k/m68k-insns.c,v 1.1 2018/04/24 16:37:52 root Exp $ */
+/* ic/m68k/m68k-insns.c - m68k instruction functions: */
+
+/*
+ * Copyright (c) 2002, 2003 Matt Fredette
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. All advertising materials mentioning features or use of this software
+ *    must display the following acknowledgement:
+ *      This product includes software developed by Matt Fredette.
+ * 4. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
+ * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
+ * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
+ * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
 
 /* includes: */
 #include "m68k-impl.h"
 
-_TME_RCSID("$Id: m68k-insns.c,v 1.1 2018/04/24 16:37:52 root Exp $");
+_TME_RCSID("$Id: m68k-insns.c,v 1.1.1.3 2018/04/24 16:40:21 root Exp $");
 
 #define TME_M68K_STD_FLAGS \
 do { \
@@ -128,16 +159,26 @@ TME_M68K_INSN(tme_m68k_dbcc)
 }
 
 /* this cannot fault: */
-TME_M68K_INSN(tme_m68k_bcc)
+static void
+_tme_m68k_bcc(struct tme_m68k *ic, tme_int32_t disp)
 {
   if (TME_M68K_COND_TRUE(ic, TME_FIELD_EXTRACTU(TME_M68K_INSN_OPCODE, 8, 4))) {
-    TME_M68K_INSN_BRANCH(ic->tme_m68k_ireg_pc + 2 + TME_M68K_INSN_OP0(tme_uint32_t));
+    TME_M68K_INSN_BRANCH(ic->tme_m68k_ireg_pc + 2 + disp);
   }
   TME_M68K_INSN_OK;
 }
+TME_M68K_INSN(tme_m68k_bcc)
+{
+  _tme_m68k_bcc(ic, TME_EXT_S8_S32((tme_int8_t) TME_M68K_INSN_OPCODE));
+}
+TME_M68K_INSN(tme_m68k_bccl)
+{
+  _tme_m68k_bcc(ic, TME_M68K_INSN_OP0(tme_int32_t));
+}
     
 /* this can fault: */
-TME_M68K_INSN(tme_m68k_bsr)
+static void
+_tme_m68k_bsr(struct tme_m68k *ic, tme_int32_t disp)
 {
   TME_M68K_INSN_CANFAULT;
   tme_m68k_push32(ic, ic->tme_m68k_ireg_pc_next);
@@ -145,9 +186,17 @@ TME_M68K_INSN(tme_m68k_bsr)
      restarting here, because after this point, nothing can fault for
      the remainder of this instruction (the executor makes no stores
      on behalf of a bsr): */
-  TME_M68K_INSN_BRANCH(ic->tme_m68k_ireg_pc + 2 + TME_M68K_INSN_OP0(tme_uint32_t));
+  TME_M68K_INSN_BRANCH(ic->tme_m68k_ireg_pc + 2 + disp);
   TME_M68K_INSN_OK;
 }
+TME_M68K_INSN(tme_m68k_bsr)
+{
+  _tme_m68k_bsr(ic, TME_EXT_S8_S32((tme_int8_t) TME_M68K_INSN_OPCODE));
+}
+TME_M68K_INSN(tme_m68k_bsrl)
+{
+  _tme_m68k_bsr(ic, TME_M68K_INSN_OP0(tme_int32_t));
+}
 
 /* this can fault: */
 TME_M68K_INSN(tme_m68k_pea)
@@ -164,13 +213,13 @@ TME_M68K_INSN(tme_m68k_pea)
 /* this cannot fault: */
 TME_M68K_INSN(tme_m68k_bkpt)
 {
-  TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_GROUP1_ILL);
+  TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_ILL);
 }
 
 /* this cannot fault: */
 TME_M68K_INSN(tme_m68k_illegal)
 {
-  TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_GROUP1_ILL);
+  TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_ILL);
 }
 
 /* this can fault: */
@@ -246,15 +295,16 @@ TME_M68K_INSN(tme_m68k_move_usp)
 TME_M68K_INSN(tme_m68k_trap)
 {
   ic->tme_m68k_ireg_pc = ic->tme_m68k_ireg_pc_next;
-  TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_GROUP2(32 + TME_FIELD_EXTRACTU(TME_M68K_INSN_OPCODE, 0, 4)));
+  TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_INST(TME_M68K_VECTOR_TRAP_0 + TME_FIELD_EXTRACTU(TME_M68K_INSN_OPCODE, 0, 4)));
 }
 
 /* this cannot fault: */
 TME_M68K_INSN(tme_m68k_trapv)
 {
   if (ic->tme_m68k_ireg_ccr & TME_M68K_FLAG_V) {
+    ic->tme_m68k_ireg_pc_last = ic->tme_m68k_ireg_pc;
     ic->tme_m68k_ireg_pc = ic->tme_m68k_ireg_pc_next;
-    TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_GROUP2(7));
+    TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_INST(TME_M68K_VECTOR_TRAP));
   }
   TME_M68K_INSN_OK;
 }
@@ -283,20 +333,31 @@ TME_M68K_INSN(tme_m68k_movec)
 {
   int ireg;
   tme_uint32_t *creg;
+  tme_uint32_t mask;
+  int illegal;
   TME_M68K_INSN_PRIV;
   /* in case we're reading the msp or isp and we're on that stack,
      this flushes %a7 to that register: */
   tme_m68k_change_sr(ic, ic->tme_m68k_ireg_sr);
   ireg = TME_M68K_IREG_D0 + TME_FIELD_EXTRACTU(TME_M68K_INSN_SPECOP, 12, 4);
+  mask = 0xffffffff;
+  illegal = FALSE;
   switch (TME_FIELD_EXTRACTU(TME_M68K_INSN_SPECOP, 0, 12)) {
-  case 0x000: creg = &ic->tme_m68k_ireg_sfc; break;
-  case 0x001: creg = &ic->tme_m68k_ireg_dfc; break;
+  case 0x000: creg = &ic->tme_m68k_ireg_sfc; mask = TME_M68K_FC_7; break;
+  case 0x001: creg = &ic->tme_m68k_ireg_dfc; mask = TME_M68K_FC_7; break;
+  case 0x002: creg = &ic->tme_m68k_ireg_cacr; mask = 0x3; illegal = (ic->tme_m68k_type < TME_M68K_M68020); break;
   case 0x800: creg = &ic->tme_m68k_ireg_usp; break;
   case 0x801: creg = &ic->tme_m68k_ireg_vbr; break;
-  default: TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_GROUP1_ILL); creg = NULL;
+  case 0x802: creg = &ic->tme_m68k_ireg_caar; illegal = (ic->tme_m68k_type != TME_M68K_M68020); break;
+  case 0x803: creg = &ic->tme_m68k_ireg_msp; illegal = (ic->tme_m68k_type < TME_M68K_M68020); break;
+  case 0x804: creg = &ic->tme_m68k_ireg_isp; illegal = (ic->tme_m68k_type < TME_M68K_M68020); break;
+  default: illegal = TRUE; creg = NULL;
+  }
+  if (illegal) {
+    TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_ILL);
   }
   if (TME_M68K_INSN_OPCODE & TME_BIT(0)) {
-    *creg = ic->tme_m68k_ireg_uint32(ireg);
+    *creg = ic->tme_m68k_ireg_uint32(ireg) & mask;
   }
   else {
     ic->tme_m68k_ireg_uint32(ireg) = *creg;
@@ -310,8 +371,32 @@ TME_M68K_INSN(tme_m68k_movec)
 /* this cannot fault: */
 TME_M68K_INSN(tme_m68k_reset)
 {
+  struct tme_bus_connection *conn_bus;
+  int rc;
+
   TME_M68K_INSN_PRIV;
-  abort();
+  
+  /* get the bus connection: */
+  conn_bus = &ic->_tme_m68k_bus_connection->tme_m68k_bus_connection;
+
+  /* assert the RESET line: */
+  rc = (*conn_bus->tme_bus_signal)
+    (conn_bus,
+     (TME_BUS_SIGNAL_RESET
+      | TME_BUS_SIGNAL_LEVEL_ASSERTED));
+  assert (rc == TME_OK);
+
+  /* XXX RESET is supposed to be asserted for 512 clocks, 
+     so a sleep is needed here: */
+
+  /* negate the RESET line: */
+  rc = (*conn_bus->tme_bus_signal)
+    (conn_bus,
+     (TME_BUS_SIGNAL_RESET
+      | TME_BUS_SIGNAL_LEVEL_NEGATED));
+  assert (rc == TME_OK);
+  
+  TME_M68K_INSN_OK;
 }
 
 /* this can fault: */
@@ -393,7 +478,7 @@ TME_M68K_INSN(tme_m68k_stop)
 /* this cannot fault: */
 TME_M68K_INSN(tme_m68k_priv)
 {
-  TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_GROUP1_PRIV);
+  TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_PRIV);
 }
 
 /* this can fault: */
@@ -478,8 +563,9 @@ TME_M68K_INSN(tme_m68k_cmp2_chk2)
 	   : (uvalue < ulower || uvalue > uupper)) {
     ic->tme_m68k_ireg_ccr |= TME_M68K_FLAG_C;
     if (TME_M68K_INSN_OPCODE & TME_BIT(11)) {
+      ic->tme_m68k_ireg_pc_last = ic->tme_m68k_ireg_pc;
       ic->tme_m68k_ireg_pc = ic->tme_m68k_ireg_pc_next;
-      TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_GROUP2(6));
+      TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_INST(TME_M68K_VECTOR_CHK));
     }
   }
 
@@ -502,8 +588,9 @@ TME_M68K_INSN(tme_m68k_rtm)
 TME_M68K_INSN(tme_m68k_trapcc)
 {
   if (TME_M68K_COND_TRUE(ic, TME_FIELD_EXTRACTU(TME_M68K_INSN_OPCODE, 8, 4))) {
+    ic->tme_m68k_ireg_pc_last = ic->tme_m68k_ireg_pc;
     ic->tme_m68k_ireg_pc = ic->tme_m68k_ireg_pc_next;
-    TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_GROUP2(7));
+    TME_M68K_INSN_EXCEPTION(TME_M68K_EXCEPTION_INST(TME_M68K_VECTOR_TRAP));
   }
   TME_M68K_INSN_OK;
 }
@@ -552,6 +639,7 @@ TME_M68K_INSN(tme_m68k_bfffo)
   tme_int32_t bf_offset;
   unsigned int bf_width;
   tme_uint32_t bf_value;
+  unsigned int bf_pos;
     
   /* get the bitfield offset from a data register or as an immediate: */
   specop = TME_M68K_INSN_SPECOP;
@@ -565,9 +653,14 @@ TME_M68K_INSN(tme_m68k_bfffo)
   /* get the bitfield value: */
   bf_value = tme_m68k_bitfield_read_unsigned(ic);
 
+  /* find the first set bit: */
+  for (bf_pos = 0, bf_value <<= (32 - bf_width);
+       bf_pos < bf_width && !(bf_value & 0x80000000);
+       bf_pos++, bf_value <<= 1);
+
   /* set the result: */
   ic->tme_m68k_ireg_uint32(TME_M68K_IREG_D0 + TME_FIELD_EXTRACTU(specop, 12, 3))
-    = bf_offset + bf_width - ffs((int) bf_value);
+    = bf_offset + bf_pos;
 
   TME_M68K_INSN_OK;
 }
@@ -697,6 +790,26 @@ TME_M68K_INSN(tme_m68k_unpk)
   TME_M68K_INSN_OK;
 }
 
+/* these just redispatch into one of the multiply or divide insns: */
+TME_M68K_INSN(tme_m68k_mull)
+{
+  if (TME_M68K_INSN_SPECOP & TME_BIT(11)) {
+    tme_m68k_mulsl(ic, _op0, _op1);
+  }
+  else {
+    tme_m68k_mulul(ic, _op0, _op1);
+  }
+}
+TME_M68K_INSN(tme_m68k_divl)
+{
+  if (TME_M68K_INSN_SPECOP & TME_BIT(11)) {
+    tme_m68k_divsl(ic, _op0, _op1);
+  }
+  else {
+    tme_m68k_divul(ic, _op0, _op1);
+  }
+}
+
 #include "m68k-bus-auto.c"
 
 #include "m68k-insns-auto.c"