/***********************************************************************
*
*             Copyright 2006 Mentor Graphics Corporation
*                         All Rights Reserved.
*
* THIS WORK CONTAINS TRADE SECRET AND PROPRIETARY INFORMATION WHICH IS
* THE PROPERTY OF MENTOR GRAPHICS CORPORATION OR ITS LICENSORS AND IS
* SUBJECT TO LICENSE TERMS.
*
************************************************************************

************************************************************************
*
*   FILE NAME
*
*       arm_debug.c
*
*   DESCRIPTION
*
*       This file contains the ARM architecture debug functions
*
*   FUNCTIONS
*
*
*
*   DEPENDENCIES
*
*       nucleus.h
*
***********************************************************************/

/* Include required header files */
#include            "nucleus.h"

/* Exception handler */
VOID **(*ESAL_AR_DBG_OS_Exception_Handler)(VOID *stack_ptr);
UINT32                                  ESAL_AR_DBG_Exception_Return;
UINT32                                  ESAL_AR_DBG_Exception_SPSR;

/* Generic debug operation flag */
extern INT                              ESAL_GE_DBG_Debug_Operation;

/* Generic exception handlers */
extern VOID **                          (*ESAL_GE_DBG_OS_Breakpoint_Handler)(VOID * stack_ptr);
extern VOID **                          (*ESAL_GE_DBG_OS_Hardware_Step_Handler)(VOID * stack_ptr);
extern VOID **                          (*ESAL_GE_DBG_OS_Data_Abort_Handler)(VOID * stack_ptr);

/* An array of offset mappings into stack frame structure for architecture
   supported. The order of register offset should match with the register
   numbers defined */
static  INT                             ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_NUM_REGS];

/* Local definitions */
typedef VOID (*ESAL_AR_DBG_EXCEPTION_HANDLER)(INT       exception_vector, 
                                              VOID *    stack_ptr);

/* Original exception handlers */
ESAL_AR_DBG_EXCEPTION_HANDLER           ESAL_AR_DBG_Orig_Breakpoint_Handler;
ESAL_AR_DBG_EXCEPTION_HANDLER           ESAL_AR_DBG_Orig_Data_Abort_Handler;
                                              
/* Local function prototypes */
static INT ESAL_AR_DBG_Condition_Met(UINT cpsr,
                                     UINT inst,
                                     INT thumb);
                                          
static UINT ESAL_AR_DBG_Reg_Shifted_Get(UINT inst, 
                                        INT carry,
                                        UINT prog_counter,
                                        UINT cpsr, 
                                        VOID *stack_frame,
                                        INT stack_frame_type);
                                            
static VOID ESAL_AR_DBG_Breakpoint_Handler(INT exception_vector, 
                                           VOID *stack_ptr);

static VOID ESAL_AR_DBG_Data_Abort_Handler(INT exception_vector,
                                           VOID *stack_ptr);

                                           
/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Initialize
*
*   DESCRIPTION
*
*       This function initializes debugging at architecture level
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       None
*
*   OUTPUTS
*
*       None
*
***********************************************************************/
VOID    ESAL_AR_DBG_Initialize(VOID)
{
    /* Initialize register mapping table */
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R0] = ESAL_GE_STK_UNSOL_OFFSET (min_stack.r0);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R1] = ESAL_GE_STK_UNSOL_OFFSET (min_stack.r1);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R2] = ESAL_GE_STK_UNSOL_OFFSET (min_stack.r2);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R3] = ESAL_GE_STK_UNSOL_OFFSET (min_stack.r3);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R4] = ESAL_GE_STK_UNSOL_OFFSET (r4);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R5] = ESAL_GE_STK_UNSOL_OFFSET (r5);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R6] = ESAL_GE_STK_UNSOL_OFFSET (r6);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R7] = ESAL_GE_STK_UNSOL_OFFSET (r7);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R8] = ESAL_GE_STK_UNSOL_OFFSET (r8);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R9] = ESAL_GE_STK_UNSOL_OFFSET (r9);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R10] = ESAL_GE_STK_UNSOL_OFFSET (r10);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R11] = ESAL_GE_STK_UNSOL_OFFSET (r11);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R12] = ESAL_GE_STK_UNSOL_OFFSET (min_stack.r12);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R13] = ESAL_AR_DBG_REG_STACK_POINTER;
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R14] = ESAL_GE_STK_UNSOL_OFFSET (min_stack.lr);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_R15] = ESAL_GE_STK_UNSOL_OFFSET (min_stack.rtn_address);
    ESAL_AR_DBG_Reg_Mappings[ESAL_AR_DBG_REG_CPSR] = ESAL_GE_STK_UNSOL_OFFSET (min_stack.spsr);

    /* Setup breakpoint exception handler saving original handler. */
    ESAL_AR_DBG_Orig_Breakpoint_Handler = ESAL_GE_EXCEPT_HANDLER_GET(ESAL_AR_UNDEF_EXCEPT_VECTOR_ID);
    ESAL_GE_EXCEPT_HANDLER_SET(ESAL_AR_UNDEF_EXCEPT_VECTOR_ID, ESAL_AR_DBG_Breakpoint_Handler);

    /* Setup data abort exception handler saving original handler. */
    ESAL_AR_DBG_Orig_Data_Abort_Handler = ESAL_GE_EXCEPT_HANDLER_GET(ESAL_AR_DATA_EXCEPT_VECTOR_ID);
    ESAL_GE_EXCEPT_HANDLER_SET(ESAL_AR_DATA_EXCEPT_VECTOR_ID, ESAL_AR_DBG_Data_Abort_Handler);    
}
     
/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Terminate
*
*   DESCRIPTION
*
*       This function terminates debugging at architecture level
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       None
*
*   OUTPUTS
*
*       None
*
***********************************************************************/
VOID    ESAL_AR_DBG_Terminate(VOID)
{
    /* Restore all exception handlers. */
    ESAL_GE_EXCEPT_HANDLER_SET(ESAL_AR_UNDEF_EXCEPT_VECTOR_ID, ESAL_AR_DBG_Orig_Breakpoint_Handler);
    ESAL_GE_EXCEPT_HANDLER_SET(ESAL_AR_DATA_EXCEPT_VECTOR_ID, ESAL_AR_DBG_Orig_Data_Abort_Handler);    
}
     
/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Reg_Mappings_Get_Offset
*
*   DESCRIPTION
*
*       This function returns the offset of a register in a stack frame
*       if the register parameter is valid.
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       reg_no                              Register number
*
*   OUTPUTS
*
*       INT                                 Offset of specified register 
*                                           or a value indicating that
*                                           the register is not mapped.
*
***********************************************************************/
INT    ESAL_AR_DBG_Reg_Mappings_Get_Offset(INT reg_no)
{
    INT     offset;

    /* Ensure register number is valid. */
    
    if (((reg_no >= 0) && (reg_no < ESAL_AR_DBG_REG_FPR0)) | (reg_no == ESAL_AR_DBG_REG_CPSR))
    {
        /* Register number is valid so return the offset value stored
           in the register mapping array. */
           
        offset = ESAL_AR_DBG_Reg_Mappings[reg_no];
    
    }
    else
    {
        /* Register number is invalid. */
        
        offset = ESAL_AR_DBG_REG_NOT_MAPPED;
    
    } /* if */
    
    return (offset);
}

/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Reg_Read
*
*   DESCRIPTION
*
*       This function reads register stored in stack
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       p_stack_frame                       Start address of stack memory
*       stack_frame_type                    stack frame type
*       reg_no                              Register number
*       reg_val                             Register value for return
*
*   OUTPUTS
*
*       TRUE or FALSE
*
***********************************************************************/
INT    ESAL_AR_DBG_Reg_Read(VOID * p_stack_frame, INT stack_frame_type, INT reg_no, ESAL_GE_DBG_REG *reg_val)
{
    INT     retval;

    /* Determine how to proceed based on the stack frame type. */
    
    switch (stack_frame_type)
    {
        case ESAL_GE_DBG_STACK_FRAME_TYPE_THREAD :
        {
            INT     stack_type;
            INT     offset;            
            
            /* Working with an execution context (thread or HISR) stack
               frame. */
            
            /* Get stack type */
            stack_type = ESAL_GE_STK_TYPE_GET(p_stack_frame);
            
            /* Obtain register offset based on stack type (solicited or
               unsolicitied). */
            
            if (stack_type == ESAL_GE_STK_TS_TYPE)
            {
                /* Solicited stack frame */
                
                if ( (reg_no >= ESAL_AR_DBG_REG_R0 && reg_no <= ESAL_AR_DBG_REG_R3) ||
                      reg_no == ESAL_AR_DBG_REG_R12 || reg_no == ESAL_AR_DBG_REG_R14 || 
                      reg_no == ESAL_AR_DBG_REG_CPSR || reg_no == ESAL_AR_DBG_REG_FPS)
                {
                    /* The current stck frame is a solicited stack frame these registers are not available */
                    /* Value returned for registers R0, R1, R2, R3, R12, R14, CPSR, FPS */
                    
                    offset = ESAL_AR_DBG_REG_NOT_MAPPED;
                                
                }
                else if(reg_no == ESAL_AR_DBG_REG_R15)
                {
                   /* Offset of PC register for solicited stack is equal to that of R0 for unsolicited stack frame */
                   /* Value returned for register R15 */
                   
                   offset = ESAL_AR_DBG_Reg_Mappings_Get_Offset(ESAL_AR_DBG_REG_R0);
                  
                }
                else
                {
                   /* Obtain register offset */
                   /* Value returned for registers R4, R5, R6, R7, R8, R9, R10, R11, R13 */
                   
                   offset = ESAL_AR_DBG_Reg_Mappings_Get_Offset(reg_no);
                   
                }
                
            }
            else
            {
                /* Unsolicited stack frame */
                
                /* Obtain register offset */
            
                offset = ESAL_AR_DBG_Reg_Mappings_Get_Offset(reg_no);
            
            }
            
            /* Obtain register value */
            
            if ((offset == ESAL_AR_DBG_REG_STACK_POINTER) || (offset == ESAL_AR_DBG_REG_NOT_MAPPED))
            {
                /* If register requested is stack pointer, obtain stack pointer */
                
                if (offset == ESAL_AR_DBG_REG_STACK_POINTER)
                {
                    /* SP register should be calculated depending on stack type */
                    
                    if (stack_type == ESAL_GE_STK_TS_TYPE)
                    {
                        /* Get SP value for solicited stack frame */
                        
                        *reg_val = (UINT32)p_stack_frame + ESAL_GE_STK_MIN_FRAME_SIZE;
                        
                    }
                    else
                    {
                        /* Get SP value for unsolicited stack frame */
                        
                        *reg_val = (UINT32)p_stack_frame + ESAL_GE_STK_MAX_FRAME_SIZE;
                    }
                    
                    retval = NU_TRUE; 
                }
                
                /* if register requested is not mapped, return register value not available */
                
                if (offset == ESAL_AR_DBG_REG_NOT_MAPPED)
                {
                    retval = NU_FALSE;
                    
                }
                
            }
            else
            {
                /* Read register value from stack */
                
                *reg_val = ESAL_GE_MEM_READ32((VOID_CAST)p_stack_frame + offset);
                
                retval = NU_TRUE;
                
            }
    
            break;
    
        }
        
        case ESAL_GE_DBG_STACK_FRAME_TYPE_EXCEPTION :
        {
            ESAL_AR_STK_MIN *       p_min_stack_frame;
            
            /* Working with an exception stack frame. */
            
            p_min_stack_frame = (ESAL_AR_STK_MIN *)p_stack_frame;
            retval = NU_TRUE;
            
            switch (reg_no)
            {
                case ESAL_AR_DBG_REG_R0 :
                {
                    *reg_val = (ESAL_GE_DBG_REG)p_min_stack_frame -> r0;
                    
                    break;
                    
                }   
                 
                case ESAL_AR_DBG_REG_R1 :
                {
                    *reg_val = (ESAL_GE_DBG_REG)p_min_stack_frame -> r1;
                    
                    break;
                    
                }                  
               
                case ESAL_AR_DBG_REG_R2 :
                {
                    *reg_val = (ESAL_GE_DBG_REG)p_min_stack_frame -> r2;
                    
                    break;
                    
                }               
               
                case ESAL_AR_DBG_REG_R3 :
                {
                    *reg_val = (ESAL_GE_DBG_REG)p_min_stack_frame -> r3;
                    
                    break;
                    
                }                
               
                case ESAL_AR_DBG_REG_R12 :
                {
                    *reg_val = (ESAL_GE_DBG_REG)p_min_stack_frame -> r12;
                    
                    break;
                    
                }                
               
                case ESAL_AR_DBG_REG_R14 :
                {
                    *reg_val = (ESAL_GE_DBG_REG)p_min_stack_frame -> lr;
                    
                    break;
                    
                }                
               
                case ESAL_AR_DBG_REG_R15 :
                {
                    *reg_val = (ESAL_GE_DBG_REG)p_min_stack_frame -> rtn_address;
                    
                    break;
                    
                }               
               
                case ESAL_AR_DBG_REG_CPSR :
                {
                    *reg_val = (ESAL_GE_DBG_REG)p_min_stack_frame -> spsr;
                    
                    break;
                    
                }                 
               
                default :
                {
                    /* ERROR: Stack frame does not contain requested 
                       register. */
                    
                    retval = NU_FALSE;
                    
                    break;
                    
                }
            
            }
            
            break;
            
        }
        
        default :
        {
            /* ERROR: Invalid stack frame type. */
            
            retval = NU_FALSE;
            
            break;   
        }
    
    }
    
    return(retval);

}


/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Reg_Write
*
*   DESCRIPTION
*
*       This function write register stored in stack
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       p_stack_frame                       Start address of stack memory
*       stack_frame_type                    stack frame type
*       reg_no                              Register number
*       reg_val                             Register value to write
*
*   OUTPUTS
*
*       TRUE or FALSE
*
***********************************************************************/
INT    ESAL_AR_DBG_Reg_Write(VOID *p_stack_frame, INT stack_frame_type, INT reg_no, ESAL_GE_DBG_REG *reg_val)
{
    INT     retval;

    /* Determine how to proceed based on the stack frame type. */
    
    switch (stack_frame_type)
    {
        case ESAL_GE_DBG_STACK_FRAME_TYPE_THREAD :
        {
            INT     stack_type;
            INT     offset;            
            
            /* Working with an execution context (thread or HISR) stack
               frame. */

            /* Get stack type */
            
            stack_type = ESAL_GE_STK_TYPE_GET(p_stack_frame);
            
            /* Obtain register offset based on stack type (solicited or
               unsolicitied). */
            
            if (stack_type == ESAL_GE_STK_TS_TYPE)
            {
                /* Solicited stack frame */
                
                if ( (reg_no >= ESAL_AR_DBG_REG_R0 && reg_no <= ESAL_AR_DBG_REG_R3) ||
                     reg_no == ESAL_AR_DBG_REG_R12 || reg_no == ESAL_AR_DBG_REG_R14 || 
                     reg_no == ESAL_AR_DBG_REG_CPSR)
                {
                    /* The current stck frame is a solicited stack frame these registers are not available */
                
                    offset = ESAL_AR_DBG_REG_NOT_MAPPED;
                    retval = NU_FALSE;
                                
                }
                else if (reg_no == ESAL_AR_DBG_REG_R15)
                {
                    /* Offset of PC register for solicited stack is equal to that of R0 for unsolicited stack frame */
        
                    offset = ESAL_AR_DBG_Reg_Mappings_Get_Offset(ESAL_AR_DBG_REG_R0);
                    
                }
                else
                {
                    /* Obtain register offset */
                
                    offset = ESAL_AR_DBG_Reg_Mappings_Get_Offset(reg_no);
                    
                }
            
            }
            else
            {
                /* Unsolicited stack frame */
                
                /* Obtain register offset */
            
                offset = ESAL_AR_DBG_Reg_Mappings_Get_Offset(reg_no);
        
            }
            
            /* Get register value from stack frame */
            
            if (offset != ESAL_AR_DBG_REG_NOT_MAPPED)
            {
            
                /* Write register value to stack */
                
                *reg_val = ESAL_GE_MEM_WRITE32((VOID_CAST)p_stack_frame + offset, *reg_val);
                
                retval = NU_TRUE;
            
            }
            else
            {
                /* Writing SP register is not allowed */
                retval = NU_FALSE;
            }
    
            break;
    
        }
    
        case ESAL_GE_DBG_STACK_FRAME_TYPE_EXCEPTION :
        {
            ESAL_AR_STK_MIN *       p_min_stack_frame;
            
            /* Working with an exception stack frame. */
            
            p_min_stack_frame = (ESAL_AR_STK_MIN *)p_stack_frame;
            retval = NU_TRUE;
            
            switch (reg_no)
            {
                case ESAL_AR_DBG_REG_R0 :
                {
                    p_min_stack_frame -> r0 = *((UINT32 *)reg_val);
                    
                    break;
                    
                }   
                 
                case ESAL_AR_DBG_REG_R1 :
                {
                    p_min_stack_frame -> r1 = *((UINT32 *)reg_val);
                    
                    break;
                    
                }                  
               
                case ESAL_AR_DBG_REG_R2 :
                {
                    p_min_stack_frame -> r2 = *((UINT32 *)reg_val);
                    
                    break;
                    
                }               
               
                case ESAL_AR_DBG_REG_R3 :
                {
                    p_min_stack_frame -> r3 = *((UINT32 *)reg_val);
                    
                    break;
                    
                }                
               
                case ESAL_AR_DBG_REG_R12 :
                {
                    p_min_stack_frame -> r12 = *((UINT32 *)reg_val);
                    
                    break;
                    
                }                
               
                case ESAL_AR_DBG_REG_R14 :
                {
                    p_min_stack_frame -> lr = *((UINT32 *)reg_val);
                    
                    break;
                    
                }                
               
                case ESAL_AR_DBG_REG_R15 :
                {
                    p_min_stack_frame -> rtn_address = *((UINT32 *)reg_val);
                    
                    break;
                    
                }               
               
                case ESAL_AR_DBG_REG_CPSR :
                {
                    p_min_stack_frame -> spsr = *((UINT32 *)reg_val);
                    
                    break;
                    
                }                 
               
                default :
                {
                    /* ERROR: Stack frame does not contain requested 
                       register. */
                    
                    retval = NU_FALSE;
                    
                    break;
                    
                }
            
            }
            
            break;
            
        }
        
        default :
        {
            /* ERROR: Invalid stack frame type. */
            
            retval = NU_FALSE;
            
            break;   
        }
    
    }    

    return (retval);
}


/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Opcode_Read
*
*   DESCRIPTION
*
*       This function reads 4 byte (or 2 byte for thumb) instruction
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       read_addr                           Address to read
*
*   OUTPUTS
*
*       instruction
*
***********************************************************************/
ESAL_GE_DBG_OPCODE  ESAL_AR_DBG_Opcode_Read(VOID *read_addr)
{
    UINT32 address;
    UINT32 opcode;


    /* Get address in a UINT32 */
    address = (UINT32)read_addr;

    /* Check if a given address is THUMB mode */
    if(address & ESAL_AR_STK_THUMB_MASK)
    {
        /* THUMB mode, clear up the last bit */
        address &= ESAL_GE_MEM_32BIT_CLEAR(0);

        /* Read 16bit value from given address */
        opcode = ESAL_GE_MEM_READ16(address);
    }
    else
    {
        /* ARM mode, just read 32bit value from given address */
        opcode = ESAL_GE_MEM_READ32(address);
    }

    /* Return opcode to caller */
    return (opcode);
}


/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Opcode_Write
*
*   DESCRIPTION
*
*       This function writes 4 byte (or 2 byte for thumb) instruction
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       write_addr                          Address to write
*       opcode                              Value to write
*
*   OUTPUTS
*
*       None
*
***********************************************************************/
VOID    ESAL_AR_DBG_Opcode_Write(VOID *write_addr, 
                                 ESAL_GE_DBG_OPCODE opcode)
{
    UINT32 address;


    /* Get address in a UINT32 */
    address = (UINT32)write_addr;

    /* Check if a given address is THUMB mode */
    if(address & ESAL_AR_STK_THUMB_MASK)
    {
        /* THUMB mode, clear up the last bit */
        address &= ESAL_GE_MEM_32BIT_CLEAR(0);

        /* Write 16bit value to given address */
        ESAL_GE_MEM_WRITE16(address, opcode);

        /* Invalidate cache*/
        ESAL_GE_MEM_DCACHE_FLUSH_INVAL(address, 2);
        ESAL_GE_MEM_ICACHE_INVALIDATE(address, 2);
    }
    else
    {
        /* ARM mode, just write 32bit value to given address */
        ESAL_GE_MEM_WRITE32(address, opcode);

        /* Invalidate cache */
        ESAL_GE_MEM_DCACHE_FLUSH_INVAL(address, 4);
        ESAL_GE_MEM_ICACHE_INVALIDATE(address, 4);
        
    }
}

/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Opcode_Nop_Get
*
*   DESCRIPTION
*
*       This function returns NOP opcode depending on its mode
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       addr                                Address to check
*
*   OUTPUTS
*
*       NOP opcode
*
***********************************************************************/
ESAL_GE_DBG_OPCODE  ESAL_AR_DBG_Opcode_Nop_Get(VOID *addr)
{
    UINT32              address;
    ESAL_GE_DBG_OPCODE  opcode;


    /* Get address in a UINT32 */
    address = (UINT32)addr;

    /* Check if address is THUMB mode */
    if(address & ESAL_AR_STK_THUMB_MASK)
    {
        /* THUMB mode, return thumb breakpoint opcode */
        opcode = ESAL_AR_DBG_THUMB_NOP_OPCODE;
    }
    else
    {
        /* ARM mode, return arm breakpoint opcode */
        opcode = ESAL_AR_DBG_ARM_NOP_OPCODE;
    }

    /* Return the break opcode */
    return (opcode);
}

/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Opcode_Brk_Get
*
*   DESCRIPTION
*
*       This function returns breakpoint opcode depending on its mode
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       addr                                Address to check
*
*   OUTPUTS
*
*       Breakpoint opcode
*
***********************************************************************/
ESAL_GE_DBG_OPCODE  ESAL_AR_DBG_Opcode_Brk_Get(VOID *addr)
{
    UINT32              address;
    ESAL_GE_DBG_OPCODE  opcode;


    /* Get address in a UINT32 */
    address = (UINT32)addr;

    /* Check if address is THUMB mode */
    if(address & ESAL_AR_STK_THUMB_MASK)
    {
        /* THUMB mode, return thumb breakpoint opcode */
        opcode = ESAL_AR_DBG_THUMB_BRK_OPCODE;
    }
    else
    {
        /* ARM mode, return arm breakpoint opcode */
        opcode = ESAL_AR_DBG_ARM_BRK_OPCODE;
    }

    /* Return the break opcode */
    return (opcode);
}


/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Step_Addr_Get
*
*   DESCRIPTION
*
*       This function returns next program counter from given instruction
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       addr                                Address of current instruction
*       p_stack_frame                       Start address of stack memory
*       stack_frame_type                    stack frame type
*
*   OUTPUTS
*
*       Next program counter
*
***********************************************************************/
VOID    *ESAL_AR_DBG_Step_Addr_Get(VOID *addr, VOID *p_stack_frame, INT stack_frame_type)
{
    UINT    thumb;
    UINT    this_instr;
    UINT32  prog_counter = 0;
    UINT32  cpsr = 0;
    UINT32  nextpc;

    /* Read instruction from given address */
    this_instr = ESAL_GE_MEM_READ32((UINT32)addr);

    /* Get PC register from stack frame */
    ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, ESAL_AR_DBG_REG_R15, &prog_counter);

    /* Get CPSR register from stack frame */
    ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, ESAL_AR_DBG_REG_CPSR, &cpsr);

    /* Check if it's thumb mode */
    if((prog_counter & ESAL_AR_STK_THUMB_MASK) || (cpsr & ESAL_AR_INT_CPSR_THUMB))
    {
        /* THUMB mode */
        thumb = NU_TRUE;

        /* Copy instruction and clear up lower 16 bit */
        this_instr = (this_instr & 0x0000ffff) << 16;

        /* Get next thumb instruction */
        nextpc = (prog_counter & 0xfffffffe) + 2;
    }
    else
    {
        /* ARM mode */
        thumb = NU_FALSE;
        
        /* Usual next arm instruction */
        nextpc = prog_counter + 4;
    }

    /* Check if this instruction will execute (if conditional instruction) */
    if (ESAL_AR_DBG_Condition_Met(cpsr, this_instr, thumb))
    {
        if (!thumb)
        {
            /* Get ARM instruction type (bits 24-27) */
            switch (ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 24, 27))
            {
                case 0x1:
                {
                    /* Check for halfword or signed byte load to pc */
                    if (ESAL_GE_MEM_32BIT_TEST(this_instr, 4) &&
                        ESAL_GE_MEM_32BIT_TEST(this_instr, 7) &&
                        ESAL_GE_MEM_32BIT_TEST(this_instr, 20) &&
                        ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 5, 6) != 0 &&
                        ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 12, 15) == ESAL_AR_DBG_REG_R15)
                    {
                        return 0;           /* Bad opcode */
                    }
                }
                /* FALL THROUGH */
                case 0x0: /* Data processing, MULT, MULTL, (bits 27..24 are 0000) */
                case 0x2: /* Data processing (bits 27..24 are 0010) */
                case 0x3: /* Data processing (bits 27..24 are 0011) */
                {
                    UINT operand1, operand2, result = 0;
                    UINT rn;
                    INT carry;

                    /* See if Rd==PC */
                    if (ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 12, 15) != ESAL_AR_DBG_REG_R15)
                    {
                        /* Destination register is not PC, break from switch */
                        break;
                    }

                    /* See if instruction is AND or EOR with operand2 in a reg.
                       If so, bit4 must not equal bit7, so see if bits 7..4
                       are 1001, which is an error */
                    if (ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 22, 25) == 0 &&
                        ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 4, 7) == 9)
                    {
                        /* Multiply into PC */
                        return 0;
                    }

                    /* Check if instruction is BX, and PSR bit M4 = 1 */
                    if ((ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 4, 23) == 0x2fff1) &&
                        (cpsr & ESAL_AR_DBG_PSR_M32))
                    {
                        UINT32 regval;

                        /* Decode operand register number */
                        rn = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 0, 3);

                        /* Get register value */
                        
                        if (ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, rn, &regval) == NU_FALSE)
                        {
                            /* Couldn't read register, should never happen */
                            return 0;
                        }

                        /* If rn==15, behavior is undefined, so use PC+8 */
                        result = (rn==15) ? (prog_counter + 8) : regval;

                        /* Subsequent instructions are THUMB */
                        if (result & ESAL_AR_STK_THUMB_MASK)
                        {
                            /* Clear bit 0 */
                            result &= ESAL_GE_MEM_32BIT_CLEAR(0);
                        }

                        /* Set next PC to result */
                        nextpc = result;

                        /* Break from switch - next PC address found */
                        break;

                    } /* end if */

                    /* Data processing instruction with PC as destination reg */
                    /* Check carry flag */
                    carry = ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_C) ? 1 : 0;
                    {
                        UINT32 regval;

                        /* Decode operand register number */
                        rn = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 19);

                        /* Read register */
                        
                        if (ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, rn, &regval) == NU_FALSE)                        
                        {
                            /* Couldn't read register */
                            return 0;
                        }

                        operand1 = (rn==15) ? (prog_counter + 8) : regval;
                    }

                    /* Immediate operand */
                    if (ESAL_GE_MEM_32BIT_TEST(this_instr, 25))
                    {
                        /* Extract immediate value */
                        UINT immval = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 0, 7);

                        /* Shift applied to immediate value */
                        UINT rotate = 2 * ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 8, 11);
                        operand2 = (immval >> rotate) | (immval << (32-rotate));
                    }
                    else  /* Operand 2 is a shifted register */
                    {
                        operand2 = ESAL_AR_DBG_Reg_Shifted_Get(this_instr, carry,
                                                               prog_counter, cpsr,
                                                               p_stack_frame,
                                                               stack_frame_type);
                    }

                    switch (ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 21, 24)) /* Extract opcode */
                    {
                        case 0x0: /* AND */
                            result = operand1 & operand2;
                        break;

                        case 0x1: /* EOR */
                            result = operand1 ^ operand2;
                        break;

                        case 0x2: /* SUB */
                            result = operand1 - operand2;
                        break;

                        case 0x3: /* RSB */
                            result = operand2 - operand1;
                        break;

                        case 0x4: /* ADD */
                            result = operand1 + operand2;
                        break;

                        case 0x5: /* ADC */
                            result = operand1 + operand2 + carry;
                        break;

                        case 0x6: /* SBC */
                            result = operand1 - operand2 + carry;
                        break;

                        case 0x7: /* RSC */
                            result = operand2 - operand1 + carry;
                        break;

                        case 0x8: /* TST, TEQ, CMP, CMN */
                        case 0x9:
                        case 0xa:
                        case 0xb:
                            result = (INT)nextpc;
                        break;

                        case 0xc: /* ORR */
                            result = operand1 | operand2;
                        break;

                        case 0xd: /* MOV */
                            result = operand2;
                        break;

                        case 0xe: /* BIC */
                            result = operand1 & ~operand2;
                        break;

                        case 0xf: /* MVN */
                            result = ~operand2;
                        break;
                    }

                    nextpc = result;
                    break;

                } /* end case 0x0, 0x2, 0x3 */

                case 0x4:
                case 0x5: /* Single data transfer: LDR, STR */
                case 0x6:
                case 0x7:
                    if (ESAL_GE_MEM_32BIT_TEST(this_instr, 20))
                    {
                        /* Load from memory */
                        if (ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 12, 15) == 15)  /* Rd == pc */
                        {
                            UINT rn;
                            INT offset;
                            INT base;
                            INT c;

                            if (ESAL_GE_MEM_32BIT_TEST(this_instr, 22)) /* Transfer byte quantity */
                            {
                                return 0;
                            }

                            rn = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 19);  /* Base register */
                            {
                                UINT32 regval;

                                /* Read register */
                                
                                if (ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, rn, &regval) == NU_FALSE)                                
                                {
                                    /* Couldn't read register */
                                    return 0;
                                }

                                base = (rn == 15) ? (prog_counter + 8) : regval;
                            }

                            /* Test carry flag */
                            c = ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_C) ? 1 : 0;

                            /* Test immediate offset bit and set offset as appropriate */
                            offset = ESAL_GE_MEM_32BIT_TEST(this_instr, 25) ?
                                     ESAL_AR_DBG_Reg_Shifted_Get(this_instr, c,
                                                                 prog_counter,
                                                                 cpsr,
                                                                 p_stack_frame,
                                                                 stack_frame_type) : /* o/s in reg */
                                     ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 0, 11);  /* Immediate offset */

                            if (!ESAL_GE_MEM_32BIT_TEST(this_instr, 23))
                            {
                                offset *= -1;
                            }

                            /* Subtract offset from base */
                            if (ESAL_GE_MEM_32BIT_TEST(this_instr, 24))
                            {
                                /* Pre-indexed: add offset before transfer */
                                base += offset;
                            }

                            /* Read memory at location specified by base */
                            nextpc = ESAL_GE_MEM_READ32(base);
                        } /* end if (bits(this_instr, 12, 15) == 15) */
                    } /* end if (bit(this_instr, 20)) */

                break;

                case 0x8: /* Block data transfer, LDM, STM */
                case 0x9:

                    /* Load from memory LDM */
                    if (ESAL_GE_MEM_32BIT_TEST(this_instr, 20))
                    {
                        /* Loading pc, (msb of register list */
                        if (ESAL_GE_MEM_32BIT_TEST(this_instr, 15))
                        {
                            INT offset = 0;
                            INT rn = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 19);
                            UINT32 rn_val;

                            /* Read register */
                            
                            if (ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, rn, &rn_val) == NU_FALSE)                            
                            {
                                /* Couldn't read register */
                                return 0;
                            }

                            if (ESAL_GE_MEM_32BIT_TEST(this_instr, 23))  /* Up: add offset to base */
                            {
                                UINT reglist = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 0, 14);
                                UINT regbit;
                                for (; reglist != 0; reglist &= ~regbit)
                                {
                                    regbit = reglist & (-1 * (INT)reglist);
                                    offset += 4; /* Increment by 4 for each register in list */
                                }
                                if (ESAL_GE_MEM_32BIT_TEST(this_instr, 24))
                                {
                                    /* Pre indexing: add offset before transfer */
                                    offset += 4;
                                }
                            }
                            else if (ESAL_GE_MEM_32BIT_TEST(this_instr, 24))
                            /* Pre indexing: add offset before transfer */
                            {
                                offset = -4;
                            }

                            /* Read memory at location specified by rn_val + offset */
                            rn_val += offset;

                            /* Read memory location specified by rn_val to obtain nextpc */
                            nextpc = ESAL_GE_MEM_READ32(rn_val);
                        } /* end if (bit(this_instr, 15)) */
                    } /* end if (bit(this_instr, 20)) */

                break;

                case 0xb:           /* Bits 27..24 are 1011: branch & link */
                case 0xa:           /* Bits 27..24 are 1010: branch */
                {
                   INT32   branch_offset = ((this_instr << 8) >> 6);
                    
                   /* Check to see if negative or positive branch */
                   if (branch_offset & 0x02000000)
                   {
                        /* Sign-extend the offset */
                        branch_offset |= 0xFC000000;                        
                   }
                    
                   /* Calculate the branch destination */
                   nextpc = (prog_counter + 8 + branch_offset);
                   
                }               
                break;

                default:
                break;
            } /* end switch */

        }  /* if(!thumb) */
        else
        {  
            /* Execute based on value in bits 28-31 (thumb instruction is in bits 16-31) */
            switch(ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 28, 31))
            {
                case 0x4:   /* Branch,exchange,mov,add */
                    {
                        INT reg_no;
                        
                        if(ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 24, 31) == 0x46)
                        {   /* Mov(3) */
                            if((ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 23, 23) == 1) &&
                               (ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 18) == 7))
                            {   /* Mov to PC */
                                reg_no = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 19, 22);

                                if (ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, reg_no, &nextpc) == NU_FALSE)
                                {
                                    /* Couldn't read register */
                                    return 0;
                                }
                            }
                        }
                        else if(ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 24, 31) == 0x44)
                        {
                            /* Add(4) */
                            if((ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 23, 23) == 1) &&
                               (ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 18) == 7))
                            {   /* Add to PC */
                                reg_no = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 19, 22);
                                if (ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, reg_no, &nextpc) == NU_FALSE)
                                {
                                    /* Couldn't read register */
                                    return 0;
                                }

                                nextpc += prog_counter;
                            }
                        }
                        else if(ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 24, 31) == 0x47)
                        {
                            /* Branch/exchange is[3] */
                            if(ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 18) == 0)
                            {
                                /* BX, BLX */
                                reg_no = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 19, 22);
                                if (ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, reg_no, &nextpc) == NU_FALSE)
                                {
                                    /* Couldn't read register */
                                    return 0;
                                }
                            }
                        }
                    }
                break;
                
                case 0xa:   /* Add to pc */
                    if(ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 22, 22) == 0)
                    {
                        /* Add(5) */
                        nextpc = prog_counter + (ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 23) * 4);
                    }
                break;

                case 0xb:   /* Pop including pc */
                    if(ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 24, 27) == 0xd)
                    {
                        /* Valid pop PC instruction */
                        INT i, reglist;
                        UINT32 mem_addr = 0;

                        ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, ESAL_AR_DBG_REG_R13, &mem_addr);

                        reglist = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 23);
                        for(i = 0; i < 8; i++)
                        {
                            if(ESAL_GE_MEM_32BIT_VAL_GET(reglist, i, i))
                            {
                                mem_addr+=4;
                            }
                        }
                        nextpc = ESAL_GE_MEM_READ32(mem_addr);
                    }
                break;

                case 0xd:   /* Conditional branch */
                    /* B(1), if here, condition already checked */
                    {
                        UINT offset = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 23);
                        nextpc = prog_counter + 4 + ((offset << 24) >> 23);
                    }
                break;

                case 0xe:   /* Unconditional branch, blx suffix[4] */
                    if(!ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 27, 27))
                    {
                        /* B(2) */
                        UINT offset = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 26);
                        if (offset & 0x400)
                            offset |= 0xFFFFF800;
                        nextpc = prog_counter + 4 + (offset << 1);
                    }
                    else
                    {
                        /* BLX suffix */
                        UINT32 lrvalue;
                        UINT offset = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 26);
                        if (ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, ESAL_AR_DBG_REG_R14, &lrvalue) == NU_FALSE)
                        {
                            /* Couldn't read register */
                            return (VOID *)nextpc;
                        }
                        if (offset & 0x400)
                            offset |= 0xFFFFF800;
                        nextpc = lrvalue + (offset << 1);
                    }
                break;

                case 0xf:   /* BL/BLX prefix, BL suffix */
                    if(ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 17, 17))
                    {
                        UINT32 lrvalue;
                        UINT offset = ESAL_GE_MEM_32BIT_VAL_GET(this_instr, 16, 26);
                        if (ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, ESAL_AR_DBG_REG_R14, &lrvalue) == NU_FALSE)
                        {
                            /* Couldn't read register */
                            return (VOID *)nextpc;
                        }
                        if (offset & 0x400)
                            offset |= 0xFFFFF800;
                        nextpc = lrvalue + (offset << 1);
                    }
                break;

                default:
                break;

            } /* end switch */

        } /* end if !thumb */

    } /* end if ConditionIsMet() */

    /* Next PC Found:*/
    if (thumb)
    {
        /* Set bit 0 for THUMB next */
        nextpc |= 1;
    }
    else
    {
        /* Ensure bits 0-1 clear for ARM next */
        nextpc &= ~0x03;
    }

    return ((VOID *)nextpc);
}


/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Condition_Met
*
*   DESCRIPTION
*
*       Check if condition is met from the instruction
*
*   CALLED BY
*
*       ESAL_AR_DBG_Step_Addr_Get
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       cpsr                                CPSR register value
*       inst                                Instruction to decode
*       thumb                               Thumb  mode indicator
*
*   OUTPUTS
*
*       NU_TRUE if condition is met
*       NU_FALSE if condition is not met
*
*****************************************************************************/
static INT ESAL_AR_DBG_Condition_Met(UINT cpsr, UINT inst, INT thumb)
{
    INT    cond;
    INT    inst_type;
    INT    ret_val;


    /* Check if it is THUMB mode instruction */
    if(!thumb)
    {
        /* If ARM, condition is upper 4bit */
        cond = (INT)(inst>>28);
    }
    else
    {
        /* If THUMB, the instruction is expected in the upper part of the 32 bits value */
        inst_type = (INT)(inst>>28);
        cond = (INT)(inst>>24)&0xf;

        /* Check instruction type */
        if(inst_type != 0xd)
        {
            /* Only conditional branch */
            return NU_TRUE;
        }

        /* Check condition code */
        if(cond == 0xe || cond == 0xf)
        {
            /* Conditions not allowed in thumb */
            return NU_FALSE;
        }
    }

    /* Check condition */
    switch (cond)
    {
        case 0x0: /* EQ */
            ret_val = ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_Z);
        break;

        case 0x1: /* NE */
            ret_val = !ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_Z);
        break;

        case 0x2: /* CS */
            ret_val = ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_C);
        break;

        case 0x3: /* CC */
            ret_val = !ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_C);
        break;

        case 0x4: /* MI */
            ret_val = ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_N);
        break;

        case 0x5: /* PL */
            ret_val = !ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_N);
        break;

        case 0x6: /* VS */
            ret_val = ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_V);
        break;

        case 0x7: /* VC */
            ret_val = !ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_V);
        break;

        case 0x8: /* HI */
            ret_val = ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_C) && 
                      !ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_Z);
        break;

        case 0x9: /* LS */
            ret_val = !ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_C) ||  
                      ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_Z);
        break;

        case 0xa: /* GE */
            ret_val = (ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_N) &&  
                       ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_V)) ||
                      (!ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_N) && 
                       !ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_V));
        break;

        case 0xb: /* LT */
            ret_val = (ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_N) && 
                       !ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_V)) ||
                      (!ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_N) &&  
                       ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_V));
        break;

        case 0xc: /* GT */
            ret_val = !ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_Z) &&
                      ((ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_N) && 
                        ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_V)) ||
                      (!ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_N) && 
                       !ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_V)));
        break;

        case 0xd: /* LE */
            ret_val = ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_Z) ||
                      (ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_N) && 
                       !ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_V)) ||
                      (!ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_N) && 
                       ESAL_GE_MEM_32BIT_MASK_TEST(cpsr, ESAL_AR_DBG_PSR_V));
        break;

        case 0xe: /* AL */
            ret_val = NU_TRUE;
        break;

        case 0xf: /* NV */
        default:
            ret_val = NU_FALSE;
        break;
    }

    /* Return if condition is met or not */
    return (ret_val);
}

/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Reg_Shifted_Get
*
*   DESCRIPTION
*
*       Get Shifted register value
*
*   CALLED BY
*
*       ESAL_AR_DBG_Step_Addr_Get
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       inst                                Instruction to decode
*       carry                               Carry flag
*       prog_counter                        Program counter register
*       cpsr                                CPSR register
*       stack_frame                         Stack frame
*       stack_frame_type                    stack frame type
*
*   OUTPUTS
*
*       Shifted register value
*
*****************************************************************************/
static UINT ESAL_AR_DBG_Reg_Shifted_Get(UINT inst, INT carry,
                                        UINT prog_counter,
                                        UINT cpsr, VOID *stack_frame,
                                        INT stack_frame_type)
{
    UINT32                  regval;
    UINT                    result, shift;
    UINT                    rm;
    ESAL_AR_DBG_SHIFT_TYPE  shift_type;


    /* Get the second operand register */
    rm = ESAL_GE_MEM_32BIT_VAL_GET(inst, 0, 3);

    /* Get the shift type */
    shift_type = (ESAL_AR_DBG_SHIFT_TYPE)ESAL_GE_MEM_32BIT_VAL_GET(inst, 5, 6);

    /* Use shift reg,Rs. Shift amount given is bottom byte */
    if (ESAL_GE_MEM_32BIT_TEST(inst, 4))
    {
        /* Get register offset from instruction */
        INT rs = ESAL_GE_MEM_32BIT_VAL_GET(inst, 8, 11);

        /* Get register value */
        if (ESAL_AR_DBG_Reg_Read(stack_frame, stack_frame_type, rs*4, &regval) == NU_FALSE)
        {
            /* Couldn't get register, should never happen */
            return 0;
        }

        /* Calculate shift amount */
        shift = ((rs == 15) ? (prog_counter + 8) : regval) & 0xFF;
    }
    else
    {
        /* Bits 11..7 give shift amount */
        shift = ESAL_GE_MEM_32BIT_VAL_GET(inst, 7, 11);
    }

    if (ESAL_AR_DBG_Reg_Read(stack_frame, stack_frame_type, rm*4, &regval) == NU_FALSE)
    {
        /* Fail to get register value */
        return 0;
    }

    /* Set result to regval */
    result = regval;

    switch (shift_type)
    {
        case ESAL_ST_LSL:
            result = shift >= 32 ? 0 : result << shift;
        break;

        case ESAL_ST_LSR:
            result = shift >= 32 ? 0 : result >> shift;
        break;

        case ESAL_ST_ASR:
            /* Make sure shift value doesn't exceed 4 bytes */
            if (shift >= 32)
            {
                shift = 31;
            }
            result = (result & 0x80000000L) ? ~((~result) >> shift) : result >> shift;
        break;

        case ESAL_ST_ROR:
            shift &= 31;
            if (shift == 0)
            {
                result = (result >> 1) | (carry ? 0x80000000L : 0);
            }
            else
            {
                result = (result >> shift) | (result << (32-shift));
            }
        break;
    }

    /* Return shifted register result */
    return (result);
}

/*************************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Breakpoint_Handler
*
*   DESCRIPTION
*
*       This is the breakpoint handler for the architecture
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       exception_vector - The exception vector number.
*
*       stack_ptr - Pointer to the exception stack frame.
*
*   OUTPUTS
*
*       None
*
*************************************************************************/
static VOID ESAL_AR_DBG_Breakpoint_Handler(INT exception_vector, VOID *stack_ptr)
{
    ESAL_AR_STK_MIN *   stk_frame = (ESAL_AR_STK_MIN *)stack_ptr;
    UINT32              exception_opcode;
    INT                 brk_exception;

    /* Check if thumb mode */
    if (ESAL_GE_MEM_32BIT_MASK_TEST(stk_frame -> spsr, ESAL_AR_DBG_PSR_T) == NU_TRUE)
    {
        /* Increase return address by 2 */
        stk_frame -> rtn_address += 2;
        
        /* Read the exception opcode */
        exception_opcode = ESAL_GE_MEM_READ16(stk_frame -> rtn_address);
        
        /* Set flag based on if a THUMB breakpoint was hit or not */
        brk_exception = ((UINT16)exception_opcode == ESAL_AR_DBG_THUMB_BRK_OPCODE);
    }
    else
    {
        /* Read the exception opcode */
        exception_opcode = ESAL_GE_MEM_READ32(stk_frame -> rtn_address);
        
        /* Set flag based on if an ARM breakpoint was hit or not */
        brk_exception = (exception_opcode == ESAL_AR_DBG_ARM_BRK_OPCODE);
    }

    /* Check if this is a breakpoint exception */
    if (brk_exception)
    {
        /* Save exception return address and SPSR from stack */
        ESAL_AR_DBG_Exception_Return = stk_frame -> rtn_address;
        ESAL_AR_DBG_Exception_SPSR = stk_frame -> spsr;

        /* Set the generic exception handler for breakpoints. */
        ESAL_AR_DBG_OS_Exception_Handler = ESAL_GE_DBG_OS_Breakpoint_Handler;

        /* Change exception return address so an unsolicited context can 
           be saved and control returned to the OS */
        stk_frame -> rtn_address = (UINT32)ESAL_AR_DBG_Exception_Exit;

        /* Ensure interrupts stay locked-out */
        stk_frame -> spsr |= ESAL_AR_INTERRUPTS_DISABLE_BITS;
        
        /* Clear thumb bit to stay in arm mode */
        stk_frame -> spsr &= ~ESAL_AR_INT_CPSR_THUMB;
        
        /* Force system mode for breakpoint exception handling.  
           Previous mode will be restored at end of exception handling. */
        stk_frame -> spsr &= ~ESAL_AR_INT_CPSR_MODE_MASK;
        stk_frame -> spsr |= ESAL_AR_INT_CPSR_SYS_MODE;
        
    }
    else
    {
        /* Call originally registered exception handler */
        ESAL_AR_DBG_Orig_Breakpoint_Handler(exception_vector, stack_ptr);
    }
}

/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Data_Abort_Handler
*
*   DESCRIPTION
*
*       This is the data abort handler for the architecture
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       exception_vector - The exception vector number.
*
*       stack_ptr - Pointer to the exception stack frame.
*
*   OUTPUTS
*
*       None
*
***********************************************************************/
static VOID ESAL_AR_DBG_Data_Abort_Handler(INT exception_vector, VOID *stack_ptr)
{
    ESAL_AR_STK_MIN *   stk_frame = (ESAL_AR_STK_MIN *)stack_ptr;

    /* Check if thumb mode */
    if (ESAL_GE_MEM_32BIT_MASK_TEST(stk_frame -> spsr, ESAL_AR_DBG_PSR_T) == NU_TRUE)
    {
        /* Increase return address by 2 */
        stk_frame -> rtn_address += 2;
    }

    /* Determine how to proceed based on whether a debug operation is
       occuring or not. */
    if (ESAL_GE_DBG_Debug_Operation == 1)
    {
        /* Save exception return address and SPSR from stack */
        ESAL_AR_DBG_Exception_Return = stk_frame -> rtn_address;
        ESAL_AR_DBG_Exception_SPSR = stk_frame -> spsr;
    
        /* Set the generic exception handler for data aborts. */
        ESAL_AR_DBG_OS_Exception_Handler = ESAL_GE_DBG_OS_Data_Abort_Handler;
    
        /* Change exception return address so an unsolicited context can 
           be saved and control returned to the OS */
        stk_frame -> rtn_address = (UINT32)ESAL_AR_DBG_Exception_Exit;
    
        /* Ensure interrupts stay locked-out */
        stk_frame -> spsr |= ESAL_AR_INTERRUPTS_DISABLE_BITS;
        
        /* Clear thumb bit to stay in arm mode */
        stk_frame -> spsr &= ~ESAL_AR_INT_CPSR_THUMB;
        
        /* Force system mode for breakpoint exception handling.  
           Previous mode will be restored at end of exception handling. */
        stk_frame -> spsr &= ~ESAL_AR_INT_CPSR_MODE_MASK;
        stk_frame -> spsr |= ESAL_AR_INT_CPSR_SYS_MODE;
    }
    else
    {
        /* Call originally registered exception handler */
        ESAL_AR_DBG_Orig_Data_Abort_Handler(exception_vector, stack_ptr);
    }
}

/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Set_Protocol
*
*   DESCRIPTION
*
*       This function returns the register model for the protocol
*       set by the debugging agent OS service.
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       dbg_prot                            debug protocol to use
*       stk_ptr                             Obtains the stack pointer
*                                           register id     
*       pc                                  Obtains the pc register id
*
*   OUTPUTS
*
*       TRUE or FALSE
*
***********************************************************************/
INT ESAL_AR_DBG_Set_Protocol(UINT8  dbg_prot, UINT8*    stk_ptr, UINT8* pc)
{
    
    /* Set stack pointer registe id*/
    *stk_ptr = ESAL_AR_DBG_STACK_PTR_REGID;
    
    /* Set pc registe id*/
    *pc      = ESAL_AR_DBG_PC_IDX_REGID;
    
    return NU_TRUE;
}

/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Reg_Block_Read
*
*   DESCRIPTION
*
*       This function reads a block of registers stored in the stack
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       p_stack_frame                       Start address of stack mem
*       stack_frame_type                    stack frame type
*       reg_buff                            Buffer address to copy 
*                                           register block.
*
*   OUTPUTS
*
*       TRUE or FALSE
*
***********************************************************************/
INT ESAL_AR_DBG_Reg_Block_Read(VOID *p_stack_frame, INT stack_frame_type, VOID* reg_buff, UINT32* blk_size)
{
    INT                 status;
    UINT8               i;
    ESAL_GE_DBG_REG *   reg_ptr;
    UINT32              block_size=0;
    
    /* Initialize temp reg pointer */
    reg_ptr = reg_buff; 
    
    /* Read a block of registers */ 
    for(i = 0; i < ESAL_AR_DBG_RSP_NUM_REGS; i++)
    {
        /* Check if register being read is one of the block registers */
        if(i < ESAL_AR_DBG_REG_FPR0)
        {
            /* Read register */
            status = ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, i, reg_ptr);
            
            /* If register is not available fill with known value */
            if(status != NU_TRUE)
            {
                *reg_ptr = 0x0BAD0BAD;
            }
            
            /* increment buffer pointer */
            reg_ptr++;
            
            /* increment block size */
            block_size += 4;
        }
        
        if((i > ESAL_AR_DBG_REG_R15)&&(i < ESAL_AR_DBG_REG_FPS))
        {
            *reg_ptr = 0x0BAD0BAD;
            reg_ptr++;
        
            *reg_ptr = 0x0BAD0BAD;
            reg_ptr++;
            
            *reg_ptr = 0x0BAD0BAD;
            reg_ptr++;
            
            /* increment block size */
            block_size += 12;
        }
        
        if((i > ESAL_AR_DBG_REG_FPR7)&&(i <= ESAL_AR_DBG_REG_CPSR))
        {
            /* Read register */
            status = ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, i, reg_ptr);
            
            /* If register is not available fill with known value */
            if(status != NU_TRUE)
            {
                *reg_ptr = 0x0BAD0BAD;
            }
            
            /* increment buffer pointer */
            reg_ptr++;
            
            /* increment block size */
            block_size += 4;
        }
    }
    
    /* return the block size */
    *blk_size = block_size;
    
    return NU_TRUE;
}

/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Reg_Block_Write
*
*   DESCRIPTION
*
*       This function writes a block of registers stored in the stack
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       p_stack_frame                       Start address of stack mem
*       stack_frame_type                    stack frame type
*       reg_buff                            Buffer containing reg block
*
*   OUTPUTS
*
*       TRUE or FALSE
*
***********************************************************************/
INT ESAL_AR_DBG_Reg_Block_Write(VOID *p_stack_frame, INT stack_frame_type, VOID* reg_buff)
{
    UINT8               i;
    ESAL_GE_DBG_REG *   reg_ptr;
    
    /* Initialize temp reg pointer */
    reg_ptr = (ESAL_GE_DBG_REG *)reg_buff; 
    
    /* Write a block of registers */ 
    for(i = 0; i < ESAL_AR_DBG_RSP_NUM_REGS; i++)
    {
        /* Check if register being written to is writable */
        if(i < ESAL_AR_DBG_REG_FPR0)
        {
            /* Write register */
            ESAL_AR_DBG_Reg_Write(p_stack_frame, stack_frame_type, i, reg_ptr);
            
            /* increment buffer pointer */
            reg_ptr++;
            
        }
        
        if((i > ESAL_AR_DBG_REG_R15)&&(i < ESAL_AR_DBG_REG_FPS))
        {
            /* Code to write floating point registers */
            
            /* Increment 12 bytes per register write request */
            reg_ptr++;
            reg_ptr++;
            reg_ptr++;
    
        }
        
        if(i == ESAL_AR_DBG_REG_FPS)
        {
            /* Code to write floating point register FPS */
            
            /* increment buffer pointer by 4 bytes */
            reg_ptr++;
            
        }
        
        if(i == ESAL_AR_DBG_REG_CPSR)
        {
            /* Write register */
            ESAL_AR_DBG_Reg_Write(p_stack_frame, stack_frame_type, i, reg_ptr);
            
            /* increment buffer pointer */
            reg_ptr++;
            
        }
    }
    
    return NU_TRUE;
}


/***********************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Reg_Expedite_Read
*
*   DESCRIPTION
*
*       This function reads the registers needed for a stop reply packet.
*       This group is defined for ARM in the reg-arm.dat file in the GDB
*       distribution.  The registers returned for ARM are r11, sp and pc.
*       This function retrives the register values stored in the stack.
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       p_stack_frame                       Start address of stack memory
*       stack_frame_type                    stack frame type
*       reg_buff                            Buffer address to copy 
*                                           register block.
*
*   OUTPUTS
*
*       TRUE or FALSE
*
***********************************************************************/
INT ESAL_AR_DBG_Reg_Expedite_Read(VOID *p_stack_frame, INT stack_frame_type, VOID* reg_buff, UINT32* blk_size)
{
    INT                 status;
    UINT32              i;
    ESAL_GE_DBG_REG *   reg_ptr;
    UINT32              block_size=0;
    UINT32              expedite_regs[]={ESAL_AR_DBG_REG_R11, 
                                         ESAL_AR_DBG_REG_R13, 
                                         ESAL_AR_DBG_REG_R15};

    /* Initialize temp reg pointer */
    reg_ptr = reg_buff; 
    
    /* Read the ARM expedited registers */ 
    for(i = 0; i < ESAL_AR_DBG_EXP_NUM_REGS; i++)
    {   
        /* Write the register number into the buffer. */
        *reg_ptr = expedite_regs[i]; 
    
        /* increment buffer pointer */
        reg_ptr++;
            
        /* increment block size */
        block_size += 4;
    
        /* Read register */
        status = ESAL_AR_DBG_Reg_Read(p_stack_frame, stack_frame_type, expedite_regs[i], reg_ptr);
            
        /* If register is not available fill with known value */
        if(status != NU_TRUE)
        {
            *reg_ptr = 0x0BAD0BAD;
        }

        /* increment buffer pointer */
        reg_ptr++;
            
        /* increment block size */
        block_size += 4;
    }        
    
    /* return the block size */
    *blk_size = block_size;
    
    return NU_TRUE;
}


/*************************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Hardware_Step
*
*   DESCRIPTION
*
*       This function performs a hardware-supported step operation.
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       p_stack_frame                       Start address of stack mem
*       stack_frame_type                    Type of stack frame
*
*   OUTPUTS
*
*       TRUE or FALSE
*
*************************************************************************/
INT ESAL_AR_DBG_Hardware_Step(VOID * p_stack_frame, INT stack_frame_type) 
{
    return(NU_TRUE);
}

/*************************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Get_Support_Flags
*
*   DESCRIPTION
*
*       This function retreives the debug support flags.
*
*   CALLED BY
*
*       OS Services
*
*   CALLS
*
*       None
*
*   INPUTS
*
*       flags                           The debug support flags value.
*
*   OUTPUTS
*
*       TRUE or FALSE
*
*************************************************************************/
INT ESAL_AR_DBG_Get_Support_Flags(UINT32 * flags)
{
    *flags = 0x0;
    return(NU_TRUE);
}

/*************************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Int_Read
*
*   DESCRIPTION
*
*       This function retrieves the interrupt state from the specified
*       stack frame.
*
*   INPUTS
*
*       p_stack_frame - Start address of stack memory.
*
*       stack_frame_type - stack frame type.
*
*       int_state - Return parameter that will contain the interrupt
*                   state if the operation is successful.  If the
*                   operation fails the value is undefined.
*
*   OUTPUTS
*
*       NU_TRUE - Indicates successful operation.
*
*       NU_FALSE - Indicates failure or error during operation.
*
*************************************************************************/
INT ESAL_AR_DBG_Int_Read(VOID *              p_stack_frame,
                         INT                 stack_frame_type,
                         UINT32 *            int_state)
{
    INT         esal_status;
    UINT32      cpsr_value;

    /* Read CPSR register value. */
    esal_status = ESAL_AR_DBG_Reg_Read(p_stack_frame,
                                       stack_frame_type,
                                       ESAL_AR_DBG_REG_CPSR,
                                       &cpsr_value);

    if (esal_status == NU_TRUE)
    {
        /* Save interrupt state values. */
        *int_state = (cpsr_value & ((UINT32)(ESAL_AR_DBG_PSR_F | ESAL_AR_DBG_PSR_I)));

    }

    return (esal_status);
}

/*************************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Int_Write
*
*   DESCRIPTION
*
*       This function writes the interrupt state in the specified stack
*       frame.
*
*   INPUTS
*
*       p_stack_frame - Start address of stack memory.
*
*       stack_frame_type - stack frame type.
*
*       int_state - The interrupt state value to set.
*
*   OUTPUTS
*
*       NU_TRUE - Indicates successful operation.
*
*       NU_FALSE - Indicates failure or error during operation.
*
*************************************************************************/
INT ESAL_AR_DBG_Int_Write(VOID *              p_stack_frame,
                          INT                 stack_frame_type,
                          UINT32              int_state)
{
    INT         esal_status;
    UINT32      cpsr_value;

    /* Read CPSR register value. */
    esal_status = ESAL_AR_DBG_Reg_Read(p_stack_frame,
                                       stack_frame_type,
                                       ESAL_AR_DBG_REG_CPSR,
                                       &cpsr_value);

    if (esal_status == NU_TRUE)
    {
        /* Restore interrupt state values. */
        cpsr_value = (cpsr_value & ~((UINT32)(ESAL_AR_DBG_PSR_F | ESAL_AR_DBG_PSR_I)));
        cpsr_value = (cpsr_value | int_state);

        /* Write CPSR register value. */
        esal_status = ESAL_AR_DBG_Reg_Write(p_stack_frame,
                                            stack_frame_type,
                                            ESAL_AR_DBG_REG_CPSR,
                                            &cpsr_value);
    }

    return (esal_status);
}

/*************************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Int_Enable
*
*   DESCRIPTION
*
*       This function enables interrupts in the specified stack frame.
*
*   INPUTS
*
*       p_stack_frame - Start address of stack memory.
*
*       stack_frame_type - stack frame type.
*
*   OUTPUTS
*
*       NU_TRUE - Indicates successful operation.
*
*       NU_FALSE - Indicates failure or error during operation.
*
*************************************************************************/
INT ESAL_AR_DBG_Int_Enable(VOID *              p_stack_frame,
                           INT                 stack_frame_type)
{
    INT         esal_status;
    UINT32      cpsr_value;

    /* Read CPSR register value. */
    esal_status = ESAL_AR_DBG_Reg_Read(p_stack_frame,
                                       stack_frame_type,
                                       ESAL_AR_DBG_REG_CPSR,
                                       &cpsr_value);

    if (esal_status == NU_TRUE)
    {
        /* Enable interrupts. */
        cpsr_value = (cpsr_value | ((UINT32)(ESAL_AR_DBG_PSR_F | ESAL_AR_DBG_PSR_I)));

        /* Write CPSR register value. */
        esal_status = ESAL_AR_DBG_Reg_Write(p_stack_frame,
                                            stack_frame_type,
                                            ESAL_AR_DBG_REG_CPSR,
                                            &cpsr_value);
    }

    return (esal_status);
}

/*************************************************************************
*
*   FUNCTION
*
*       ESAL_AR_DBG_Int_Disable
*
*   DESCRIPTION
*
*       This function disables interrupts in the specified stack frame.
*
*   INPUTS
*
*       p_stack_frame - Start address of stack memory.
*
*       stack_frame_type - stack frame type.
*
*   OUTPUTS
*
*       NU_TRUE - Indicates successful operation.
*
*       NU_FALSE - Indicates failure or error during operation.
*
*************************************************************************/
INT ESAL_AR_DBG_Int_Disable(VOID *              p_stack_frame,
                            INT                 stack_frame_type)
{
    INT         esal_status;
    UINT32      cpsr_value;

    /* Read CPSR register value. */
    esal_status = ESAL_AR_DBG_Reg_Read(p_stack_frame,
                                       stack_frame_type,
                                       ESAL_AR_DBG_REG_CPSR,
                                       &cpsr_value);

    if (esal_status == NU_TRUE)
    {
        /* Disable interrupts. */
        cpsr_value = (cpsr_value & ~((UINT32)(ESAL_AR_DBG_PSR_F | ESAL_AR_DBG_PSR_I)));

        /* Write CPSR register value. */
        esal_status = ESAL_AR_DBG_Reg_Write(p_stack_frame,
                                            stack_frame_type,
                                            ESAL_AR_DBG_REG_CPSR,
                                            &cpsr_value);
    }

    return (esal_status);
}
