; Analysis of ARM SWP/SWPB/LDRH/LDRSB/LDRSH/STRH instructions
; -----------------------------------------------------------
;
; Start by distinguishing SWP instructions from the rest.

ARM_Odds_And_Ends

                TST     R0, #0x3 :SHL: 5
                BNE     ARM_LDRH_etc

; Analysis of ARM SWP/SWPB instructions
; -------------------------------------
;
; This will behave like a pre-indexed instruction with an offset of 0
; and no writeback - i.e. P=1, U=don't care, W=0. SWP/SWPB
; instructions should already be like this, and it is an error if they
; are not.

ARM_SWP

                AND     R7, R0, #ARM_P_bit + ARM_W_bit
                CMP     R7, #ARM_P_bit
                BNE     ARM_Should_Not_Happen

; A base register of R15 is also an error.

                CMP     R1, #0xF :SHL: ARM_Rn_pos
                MOVEQ   R6, #DABORT_ERROR_BASE_R15
                BEQ     CallOSHandlerWithError

; Set an offset of 0 and continue.

                MOV     R2, #0          ;Set offset of 0
                B       RegisterAdjust

; Analysis of ARM LDRH/LDRSB/LDRSH/STRH instructions
; --------------------------------------------------
;
; First thing is to force writeback to be set if post-indexed; then
; split into immediate and register forms.

ARM_LDRH_etc

                TST     R0, #ARM_P_bit
                ORREQ   R0, R0, #ARM_W_bit

                TST     R0, #ARM_S_bit
                BEQ     ARM_LDRH_etc_Reg

ARM_LDRH_etc_Immed

; We just have to generate the correct offset.

                AND     R2, R0, #0xF
                AND     R7, R0, #0xF00
                ORR     R2, R2, R7, LSR #4
                B       RegisterAdjust

ARM_LDRH_etc_Reg

; There are a number of errors to detect:
;
; * An index register of R15.

                AND     R2, R0, #ARM_Rm_mask
                CMP     R2, #0xF :SHL: ARM_Rm_pos
                MOVEQ   R6, #DABORT_ERROR_INDEX_R15
                BEQ     CallOSHandlerWithError

; * Base register = index register, with writeback.

                CMP     R2, R1, LSR #(ARM_Rn_pos - ARM_Rm_pos)
                BNE     ARM_LDRH_etc_Reg_OK
                TST     R0, #ARM_W_bit
                BNE     ARM_LDR_STR_Reg_NotOK   ;To shared error code

ARM_LDRH_etc_Reg_OK

; Get the index register value and go to common code.

                LDR     R2, [R13, R2, LSL #(2 - ARM_Rm_pos)]
                B       RegisterAdjust

; Analysis of ARM LDC/STC instructions
; ------------------------------------
;
; Offset comes direct from the instruction. M, P, U, W and L bits are
; already right.

ARM_LDC_STC
                AND     R2, R0, #0xFF
                MOV     R2, R2, LSL #2
                B       RegisterAdjust2 ;Avoid "load and w/back" check

; Analysis of ARM LDM/STM instructions
; ------------------------------------
;
; Offset is implied by number of set bits in register mask; M, U, W
; and L bits are set correctly. P bit cannot be set in a manner that
; corresponds properly to the other instructions, so this case doesn't
; share all of the standard "RegisterAdjust" code.

ARM_LDM_STM

; Need to check for some error conditions:
;
; * Base register of R15.

                CMP     R1, #0xF :SHL: ARM_Rn_pos
                MOVEQ   R6, #DABORT_ERROR_BASE_R15
                BEQ     CallOSHandlerWithError

; * Register mask empty. (Calculate register mask at the same time and
;   put it into top end of R3.)

                MOVS    R3, R0, LSL #16         ;Isolate register mask
                MOVEQ   R6, #DABORT_ERROR_LDMSTM_EMPTY
                BEQ     CallOSHandlerWithError

; * Writeback and load of same register.

                TST     R0, #ARM_W_bit          ;Writeback?
                TSTNE   R0, #ARM_L_bit          ;And a load?
                MOVNE   R7, R1, LSR #ARM_Rn_pos
                MOVNE   R7, R3, LSR R7
                TSTNE   R7, #0x10000            ;And base in list?
                MOVNE   R6, #DABORT_ERROR_LOAD_WB
                BNE     CallOSHandlerWithError

; * Writeback in user bank form.

                TST     R0, #ARM_W_bit          ;Writeback?
                TSTNE   R0, #ARM_S_bit          ;Potentially user bank?
                BEQ     ARM_LDM_STM_OK
                TST     R3, #0x10000 :SHL: 15   ;Is it loading R15?
                TSTNE   R0, #ARM_L_bit          ;And a load?
                MOVEQ   R6, #DABORT_ERROR_USERBANK_WB
                BEQ     CallOSHandlerWithError

ARM_LDM_STM_OK

; *** Live register values at this point are:
;     R0:  M bit (bit 27) indicating multiple vs. single transfer.
;          P bit (bit 24) indicating pre- vs. post-indexing.
;          U bit (bit 23) indicating whether indexing is up or down.
;          W bit (bit 21) indicating whether base register writeback
;            is required.
;     R1:  Number of base register, still in instruction position.
;     R3:  Register list mask (only the number of set bits matters).
;     R4:  Pointer to aborting instruction
;     R5:  SPSR value
;     R6:  Error code
;     R8:  Abort model (if relevant)
;     R13: Stack pointer (pointing to register dump)
;
; Calculate offset from mask, by repeatedly isolating and removing the
; least significant bit in the mask until it is zero. Note we know the
; mask is non-zero.

                MOV     R2, #0

ARM_LDM_STM_OffsetLoop
                ADD     R2, R2, #4
                RSB     R7, R3, #0      ;Unequal above lowest 1, equal
                                        ; at lowest 1 and below
                BICS    R3, R3, R7      ;So this clears lowest 1
                BNE     ARM_LDM_STM_OffsetLoop

        [ PassXferAddr

; We need to know what the difference between the transfer address and
; the (possibly corrected) base address is. This is given by the
; following table:
;
;   P bit  U bit  Addressing mode  Transfer address - base address
;   --------------------------------------------------------------
;     0      0         DA           4 - R2
;     0      1         IA           0
;     1      0         DB           -R2
;     1      1         IB           4
;
; The following code puts the appropriate value in R3.

                TST     R0, #ARM_P_bit
                MOVEQ   R3, #4
                MOVNE   R3, #0
                TST     R0, #ARM_U_bit
                SUBEQ   R3, R3, R2
                RSBNE   R3, R3, #4

        ]

                B       RegisterAdjust3

; Analysis of ARM LDR/STR instructions with register offset
; ---------------------------------------------------------
;
; Offset is Rm, shifted appropriately; force writeback if
; post-indexed. M, P, U and L bits are already right.

ARM_LDR_STR_Reg

                TST     R0, #ARM_P_bit
                ORREQ   R0, R0, #ARM_W_bit
                AND     R2, R0, #ARM_Rm_mask

; Need to check for some error conditions:
;
; * An invalid instruction.

                TST     R0, #0x00000010
                BNE     ARM_Should_Not_Happen

; * An index register of R15.

                CMP     R2, #0xF :SHL: ARM_Rm_pos
                MOVEQ   R6, #DABORT_ERROR_INDEX_R15
                BEQ     CallOSHandlerWithError

; * Base register = index register, with writeback.

                CMP     R2, R1, LSR #(ARM_Rn_pos - ARM_Rm_pos)
                BNE     ARM_LDR_STR_Reg_OK
                TST     R0, #ARM_W_bit
                BNE     ARM_LDR_STR_Reg_NotOK

ARM_LDR_STR_Reg_OK

; Get the index register value.

                LDR     R2, [R13, R2, LSL #(2 - ARM_Rm_pos)]

; Now we need to apply the shift. Split according to the shift type.

                AND     R7, R0, #3 :SHL: 5
                ADD     PC, PC, R7, LSR #3

                NOP                             ;Branch table padding

                B       ARM_LDR_STR_Reg_LSL
                B       ARM_LDR_STR_Reg_LSR
                B       ARM_LDR_STR_Reg_ASR
ARM_LDR_STR_Reg_ROR
                ANDS    R7, R0, #0x1F :SHL: 7
                MOVNE   R7, R7, LSR #7          ;If amount non-zero,
                MOVNE   R2, R2, ROR R7          ; ROR correctly
                BNE     RegisterAdjust

; We've got an RRX shift. This has got to be silly, but it's just as
; easy to handle it correctly as to produce an error.

                MOVS    R7, R5, LSL #3          ;Caller's C -> C
                MOV     R2, R2, RRX
                B       RegisterAdjust

ARM_LDR_STR_Reg_ASR
                ANDS    R7, R0, #0x1F :SHL: 7
                MOVNE   R7, R7, LSR #7          ;If amount non-zero,
                MOVNE   R2, R2, ASR R7          ; ASR correctly
                MOVEQ   R2, R2, ASR #32         ;Else ASR by 32
                B       RegisterAdjust

ARM_LDR_STR_Reg_LSR
                ANDS    R7, R0, #0x1F :SHL: 7
                MOVNE   R7, R7, LSR #7          ;If amount non-zero,
                MOVNE   R2, R2, LSR R7          ; LSR correctly
                MOVEQ   R2, R2, LSR #32         ;Else LSR by 32
                B       RegisterAdjust

ARM_LDR_STR_Reg_LSL
                AND     R7, R0, #0x1F :SHL: 7
                MOV     R7, R7, LSR #7
                MOV     R2, R2, LSL R7
                B       RegisterAdjust

ARM_LDR_STR_Reg_NotOK
                TST     R0, #ARM_P_bit
                MOVEQ   R6, #DABORT_ERROR_BASEEQINDEX_POST
                MOVNE   R6, #DABORT_ERROR_BASEEQINDEX_PRE
                B       CallOSHandlerWithError

; Analysis of ARM LDR/STR instructions with immediate offset
; ----------------------------------------------------------
;
; Offset comes direct from the instruction; force writeback if
; post-indexed. M, P, U and L bits are already right.

ARM_LDR_STR_Immed
                MOV     R2, R0, LSL #20
                MOV     R2, R2, LSR #20
                TST     R0, #ARM_P_bit
                ORREQ   R0, R0, #ARM_W_bit

; Fall through to RegisterAdjust if following code isn't assembled.

          [ SuptThumb

                B       RegisterAdjust

                LTORG

ThumbInstruction

; Thumb instruction analysis
; ==========================
;
; Get the instruction. We can use a normal LDRH instruction to do this,
; rather than faking an "LDRHT" from an LDRT, for the same reasons that we
; can use LDR rather than LDRT to fetch an ARM instruction - see "ARM
; instruction analysis" above.

                LDRH    R0, [R4]

; *** Live register values at this point are:
;     R0:  Aborting instruction
;     R4:  Pointer to aborting instruction
;     R5:  SPSR value
;     R6:  Error code
;     R8:  Abort model (if relevant)
;     R13: Stack pointer (pointing to register dump)
;
; Now start analysing the instruction. The objective of this stage is
; to end up with the same register contents as the ARM instruction analysis,
; i.e.:
;
;     R0:  M bit (bit 27) indicating multiple vs. single transfer.
;          P bit (bit 24) indicating pre- vs. post-indexing.
;          U bit (bit 23) indicating whether indexing is up or down.
;          W bit (bit 21) indicating whether base register writeback
;            is required.
;          [ L bit (bit 20) indicating whether a load or a store, at least
;            when writeback is involved or there is a potential "user bank"
;            LDM. Not needed in general for Thumb instructions - the
;            writebacks for LDM/POP/PUSH/STM will be dealt with specially. ]
;     R1:  Number of base register, in ARM instruction position.
;     R2:  Offset value.
;     R3:  Number of destination register, in ARM instruction position
;          (for all but LDM/POP/PUSH/STM).
;
; Unlike the ARM instruction case, we will have to do a lot of "faking" to
; get things right. We do at least have the advantage that all the relevant
; bits of R0 are known to be zero at this point.
;
; Set R1 and R3 from the most usual positions of the base and destination
; registers in Thumb instructions.

                AND     R1, R0, #Thumb_usual_Rn_mask
                MOV     R1, R1, LSL #(ARM_Rn_pos - Thumb_usual_Rn_pos)
                AND     R3, R0, #Thumb_usual_Rd_mask
                MOV     R3, R3, LSL #(ARM_Rd_pos - Thumb_usual_Rd_pos)

; Now split according to the major class of the instruction - i.e.
; bits 15:12.

                AND     R2, R0, #(0xF:SHL:12)
                ADD     PC, PC, R2, LSR #10

                NOP                             ;Branch table padding

                B       ARM_Should_Not_Happen   ;(Shift imm.)
                B       ARM_Should_Not_Happen   ;(Shift imm., add/sub)
                B       ARM_Should_Not_Happen   ;(Add/sub/compare/move
                B       ARM_Should_Not_Happen   ; immediate)
                B       Thumb_PCbased           ;(Also data processing)
                B       Thumb_RegOffset
                B       Thumb_LDR_STR
                B       Thumb_LDRB_STRB
                B       Thumb_LDRH_STRH
                B       Thumb_SPbased
                B       ARM_Should_Not_Happen   ;(ADR from PC/SP)
                B       Thumb_PUSH_POP          ;(Also SP adjust/Undef)
                B       Thumb_LDM_STM
                B       ARM_Should_Not_Happen   ;(Bcc/SWI/Undef)
                B       ARM_Should_Not_Happen   ;(Uncond. branch/Undef)
                B       ARM_Should_Not_Happen   ;(BL high/low)

; Analysis of Thumb PC-based PUSH/POP instructions
; ------------------------------------------------

Thumb_PUSH_POP

; Checks for errors:
;
; * Instruction not in fact PUSH/POP:

                TST     R0, #0x0400
                BEQ     ARM_Should_Not_Happen

; * Empty register mask - register mask gets calculated at the same
;   time and put in R3. Note that only the number of set bits in the
;   register mask matters, so we don't have to shift the LR/PC bit to
;   the correct position.