What is the setting for SOPT1? Note that some SOPT1 bits are write-once. Also, what are the instructions for entering WAIT mode, typically defined in deriviative.h? For example here is what I am using for MCF51QE128.
// Watchdog disabled. Stop Mode Enable. Background Pin enable. RESET pin enable
SOPT1 = SOPT1_BKGDPE_MASK | SOPT1_STOPE_MASK;
#define _Stop asm ( mov3q #4,d0; bclr.b d0,SOPT1; stop #0x2000; )
/*!< Macro to enter stop modes, STOPE bit in SOPT1 register must be set prior to executing this macro */
#define _Wait asm ( mov3q #4,d0; bset.b d0,SOPT1; nop; stop #0x2000; )
/*!< Macro to enter wait mode */
Also, is CPUCR being set? If so, to what value? We were using an early version of the MCF51QE128 which had a timing problem when entering stop and accessing flash memory, so interrupt vectors were moved to ram. (this has since been fixed in the MCF51QE128), however, we also had to set CPUCR as follows, although you may need different values:
asm ( move.l #0xD2000000, D0)
asm ( movec D0, CPUCR)
SPMSC2 = SPMSC2_LPWUI_MASK | SPMSC2_PPDE_MASK ;
INTC_WCR = 0x80; // needed to wake from stop mode
// and interrupt (not reset) with errors
SPMSC1 = 0;
Wayne