;
; * Set the M, P, U and W bits correctly in R0 (1/0/1/1 for POP,
;   1/1/0/1 for PUSH).
; * Set R1 to the correct base register number (R13).

                BIC     R3, R0, #0xFE00
                MOV     R1, #(0xD :SHL: ARM_Rn_pos)
                TST     R0, #Thumb_L_bit
                ORREQ   R0, R0, #ARM_M_bit + ARM_P_bit + ARM_W_bit
                ORRNE   R0, R0, #ARM_M_bit + ARM_U_bit + ARM_W_bit
                B       ARM_LDM_STM_OK

; Analysis of Thumb PC-based LDM/STM instructions
; -----------------------------------------------

Thumb_LDM_STM

; Checks for errors:

; * Empty register mask - register mask gets calculated at the same
;   time and put in R3.

                BICS    R3, R0, #0xFF00
                MOVEQ   R6, #DABORT_ERROR_LDMSTM_EMPTY
                BEQ     CallOSHandlerWithError

; * Writeback and load of the same register. We've definitely got
;   writeback, so it's just a question of whether the base register
;   appears in the register list. First, get the base register number
;   into R1 and put it into ARM base register position (where it is
;   needed later anyway). Then check whether it appears in the
;   register list.

                AND     R1, R0, #Thumb_unusual_reg_mask
                MOV     R1, R1, LSL #(ARM_Rn_pos - Thumb_unusual_reg_pos)

                MOV     R7, R1, LSR #ARM_Rn_pos
                MOV     R7, R3, LSR R7
                TST     R7, #1
                MOVNE   R6, #DABORT_ERROR_LOAD_WB
                BNE     CallOSHandlerWithError

; We will branch into the ARM LDM/STM code at the point where all
; error checks have been performed. The only thing we still need to do
; is set the M, P, U and W bits correctly in R0 (1/0/1/1 for both LDM
; and STM).

                ORR     R0, R0, #ARM_M_bit + ARM_U_bit + ARM_W_bit
                B       ARM_LDM_STM_OK

; Analysis of Thumb LDRx/STRx with register offset
; ------------------------------------------------
;
; R0 bits should be M=0, P=1, U=1, W=0. R1 and R3 are right; R2 value
; should be obtained from the Thumb instruction's Rm field. There are
; no problems with an index register of R15 or with writeback with
; index = base, since the Thumb instruction doesn't permit a "high"
; index register or base register writeback.

Thumb_RegOffset
                ORR     R0, R0, #ARM_P_bit + ARM_U_bit
                AND     R2, R0, #Thumb_usual_Rm_mask
                LDR     R2, [R13, R2, LSR #(Thumb_usual_Rm_pos - 2)]
                B       RegisterAdjust

; Analysis of Thumb SP-based LDR/STR
; ----------------------------------
;
; R0 bits should be M=0, P=1, U=1, W=0. R1 should indicate R13. R3
; should be calculated from the variant Rd position used for this
; instruction. Finally, the offset is extracted from the instruction
; and shifted into "times 4" position.

Thumb_SPbased
                ORR     R0, R0, #ARM_P_bit + ARM_U_bit
                MOV     R1, #(0xD :SHL: ARM_Rn_pos)
                AND     R3, R0, #Thumb_unusual_reg_mask
                MOV     R3, R3, LSL #(ARM_Rd_pos - Thumb_unusual_reg_pos)
                AND     R2, R0, #Thumb_Imm8_mask
                MOV     R2, R2, LSL #(2 - Thumb_Imm8_pos)
                B       RegisterAdjust

; Analysis of Thumb PC-based LDR
; ------------------------------

Thumb_PCbased

; Check instruction is right (not the data processing instructions
; with the same major opcode, which look like PC-based STRs).

                TST     R0, #Thumb_L_bit
                BEQ     ARM_Should_Not_Happen

; R0 bits should be M=0, P=1, U=1, W=0. R1 should indicate R15; the
; "RegisterAdjust" code below will apply the appropriate
; Thumb-specific modifications to it. R3 should be calculated from the
; variant Rd position used for this instruction. Finally, the offset
; is extracted from the instruction and shifted into "times 4"
; position.

                ORR     R0, R0, #ARM_P_bit + ARM_U_bit
                MOV     R1, #(0xF :SHL: ARM_Rn_pos)
                AND     R3, R0, #Thumb_unusual_reg_mask
                MOV     R3, R3, LSL #(ARM_Rd_pos - Thumb_unusual_reg_pos)
                AND     R2, R0, #Thumb_Imm8_mask
                MOV     R2, R2, LSL #(2 - Thumb_Imm8_pos)
                B       RegisterAdjust

; Analysis of Thumb LDR/STR with immediate offset
; -----------------------------------------------
;
; R1 and R3 values are already correct; R0 bits should be M=0, P=1,
; U=1, W=0; offset needs to be extracted from the instruction and
; shifted into "times 4" position.

Thumb_LDR_STR
                ORR     R0, R0, #ARM_P_bit + ARM_U_bit
                AND     R2, R0, #Thumb_Imm5_mask
                MOV     R2, R2, LSR #(Thumb_Imm5_pos - 2)
                B       RegisterAdjust

; Analysis of Thumb LDRB/STRB with immediate offset
; -------------------------------------------------
;
; R1 and R3 values are already correct; R0 bits should be M=0, P=1,
; U=1, W=0; offset needs to be extracted from the instruction and
; shifted into "times 1" position.

Thumb_LDRB_STRB
                ORR     R0, R0, #ARM_P_bit + ARM_U_bit
                AND     R2, R0, #Thumb_Imm5_mask
                MOV     R2, R2, LSR #(Thumb_Imm5_pos - 0)
                B       RegisterAdjust

; Analysis of Thumb LDRH/STRH with immediate offset
; -------------------------------------------------
;
; R1 and R3 values are already correct; R0 bits should be M=0, P=1,
; U=1, W=0; offset needs to be extracted from the instruction and
; shifted into "times 2" position.

Thumb_LDRH_STRH
                ORR     R0, R0, #ARM_P_bit + ARM_U_bit
                AND     R2, R0, #Thumb_Imm5_mask
                MOV     R2, R2, LSR #(Thumb_Imm5_pos - 1)

; Fall through to RegisterAdjust.

          ] ; of SuptThumb section

RegisterAdjust

; *** 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.
;          L bit (bit 20) indicating whether a load or a store, at
;            least when writeback is occurring.
;     R1:  Number of base register, still in ARM instruction position.
;     R2:  Offset value.
;     R3:  Number of destination register, still in ARM instruction
;          position.
;     R4:  Pointer to aborting instruction
;     R5:  SPSR value
;     R6:  Error code
;     R8:  Abort model (if relevant)
;     R13: Stack pointer (pointing to register dump)
;
; This code is shared between all instructions except LDM/STMs and
; LDCs/STCs. First, check for the "Load and write back to same
; register" case.

                CMP     R3, R1, LSR #(ARM_Rn_pos - ARM_Rd_pos)
                BNE     RegisterAdjust2
                TST     R0, #ARM_W_bit
                TSTNE   R0, #ARM_L_bit
                MOVNE   R6, #DABORT_ERROR_LOAD_WB
                BNE     CallOSHandlerWithError

RegisterAdjust2

; *** 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 ARM instruction position.
;     R2:  Offset value.
;     R4:  Pointer to aborting instruction
;     R5:  SPSR value
;     R6:  Error code
;     R8:  Abort model (if relevant)
;     R13: Stack pointer (pointing to register dump)
;
; If we're required to produce the transfer address, calculate the
; offset from the base address (after it has been corrected) to the
; transfer address.

          [ PassXferAddr
                TST     R0, #ARM_U_bit
                MOVNE   R3, R2
                RSBEQ   R3, R2, #0
                TST     R0, #ARM_P_bit
                MOVEQ   R3, #0
          ]

RegisterAdjust3

; *** 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 ARM instruction position.
;     R2:  Offset value.
;     R3:  Transfer address - base address (if "PassXferAddr"
;          specified).
;     R4:  Pointer to aborting instruction
;     R5:  SPSR value
;     R6:  Error code
;     R8:  Abort model (if relevant)
;     R13: Stack pointer (pointing to register dump)
;
; There is one more error to check for, namely use of a base register
; of R15 with writeback. We will get hold of the base register value
; at the same time, as it happens to be convenient to use the R15 test
; for that purpose as well.

                CMP     R1, #0xF :SHL: ARM_Rn_pos
                LDRNE   R7, [R13, R1, LSR #(ARM_Rn_pos - 2)]
                BNE     RegisterAdjust4

                TST     R0, #ARM_W_bit
                MOVNE   R6, #DABORT_ERROR_R15_WB
                BNE     CallOSHandlerWithError

; Add correct offset to instruction pointer to get the right R15 base
; value. For Thumb, the only PC-based load/store instruction also
; clears bit 1.

                ADD     R7, R4, #8
        [ SuptThumb
                TST     R5, #T_bit
                ADDNE   R7, R4, #4
                BICNE   R7, R7, #2
        ]

RegisterAdjust4

; *** Live register values at this point are:
;     R0:  M bit (bit 27) indicating multiple vs. single transfer.
;          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 ARM instruction position.
;     R2:  Offset value.
;     R3:  Transfer address - base address (if "PassXferAddr"
;          specified).
;     R4:  Pointer to aborting instruction
;     R5:  SPSR value
;     R6:  Error code
;     R7:  Value of base register
;     R8:  Abort model (if relevant)
;     R13: Stack pointer (pointing to register dump)
;
; We're finally ready to do base register restoration if required. The
; rules here are:
;
; * If we've got just a single abort model, follow its dictates about
;   base register restoration.
;
; * Otherwise, use the appropriate bit of the abort model number in R8.

          [ AbortModelCount = 1
            [ BaseRestored
                ; Fall through (next chunk of code won't be assembled).
            ]
            [ BaseUpdated
                ; Fall through
            ]
            [ EarlyAbort
                TST     R0, #ARM_M_bit
                BEQ     CallOSHandlerNoError
            ]
          |
                TST     R0, #ARM_M_bit
                MOVEQ   R8, R8, LSR #1
                TST     R8, #1
                BEQ     CallOSHandlerNoError
          ]

          [ :LNOT:((AbortModelCount = 1) :LAND: BaseRestored)

; If we get here, we need to restore the base register value
; appropriately, provided it has actually been written back.

                TST     R0, #ARM_W_bit
                BEQ     CallOSHandlerNoError

                TST     R0, #ARM_U_bit
                ADDEQ   R7, R7, R2      ;Add if originally subtracted
                SUBNE   R7, R7, R2      ; and vice versa
                STR     R7, [R13, R1, LSR #(ARM_Rn_pos - 2)]

          ]

CallOSHandlerNoError

          [ PassXferAddr

; Produce transfer address from corrected base address and previously
; calculated difference.

                ADD     R7, R7, R3

          ]

        ] ; of complex condition saying whether to analyse instruction

CallOSHandlerWithError

; OS-specific handler call
; ========================
;
; *** Live register values at this point are:
;     R4:  Pointer to aborting instruction
;     R5:  SPSR value
;     R6:  Error code
;     R7:  Transfer address (if wanted & relevant; otherwise junk)
;     R13: Stack pointer (pointing to register dump)
;
; Switch into the correct mode (if necessary) - but first we may need
; to put the register dump pointer somewhere safe. R8 is a suitable
; callee-save register.

        [ PassRegDumpAddr :LOR: (HandlerCallMode <> "Abort")
                MOV     R8, R13
        ]

        [ HandlerCallMode <> "Abort"
                MRS     R3, CPSR
                BIC     R3, R3, #Mode_FullMask
                ORR     R3, R3, #Mode_Callee
                MSR     CPSR$all_fields, R3
        ]

; Marshall the arguments required. This uses some rather messy
; conditional assembly, the idea of which is to marshall the arguments
; in reverse order in R0-R3, dumping partial lists to the stack. First
; count the arguments.

                GBLA    ArgCount
ArgCount        SETA    1
        [ PassSPSR
ArgCount        SETA    ArgCount+1
        ]
        [ PassInstrAddr
ArgCount        SETA    ArgCount+1
        ]
        [ PassRegDumpAddr
ArgCount        SETA    ArgCount+1
        ]
        [ PassXferAddr
ArgCount        SETA    ArgCount+1
        ]

; Now ArgVar counts down through the arguments; ArgVar2 determines
; which register each should go to; ArgString contains the arguments
; not yet on the stack, each preceded by a comma (the first comma
; needs stripping before using ArgString in an STMFD instruction.

                GBLA    ArgVar
ArgVar          SETA    ArgCount
                GBLA    ArgVar2
                GBLS    ArgString
ArgString       SETS    ""

        [ PassXferAddr
ArgVar          SETA    ArgVar-1
ArgVar2         SETA    ArgVar :AND: 3
ArgString       SETS    ",R$ArgVar2" :CC: ArgString
                MOV     R$ArgVar2, R7
          [ ArgVar2 = 0 :LAND: ArgVar <> 0
ArgString       SETS    ArgString :RIGHT: (:LEN:ArgString - 1)
                STMFD   R13!,{$ArgString}
ArgString       SETS    ""
          ]
        ]