Torque2D/engine/source/Box2D/Particle/b2ParticleAssembly.neon.s

@
@ Copyright (c) 2014 Google, Inc.
@
@ This software is provided 'as-is', without any express or implied
@ warranty.  In no event will the authors be held liable for any damages
@ arising from the use of this software.
@ Permission is granted to anyone to use this software for any purpose,
@ including commercial applications, and to alter it and redistribute it
@ freely, subject to the following restrictions:
@ 1. The origin of this software must not be misrepresented; you must not
@ claim that you wrote the original software. If you use this software
@ in a product, an acknowledgment in the product documentation would be
@ appreciated but is not required.
@ 2. Altered source versions must be plainly marked as such, and must not be
@ misrepresented as being the original software.
@ 3. This notice may not be removed or altered from any source distribution.
@
        .text
        .syntax   unified

        .balign   4
        .global   CalculateTags_Simd
        .thumb_func

CalculateTags_Simd:
        @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
        @
        @ int CalculateTags_Simd(const b2Vec2* positions,
        @                        int count,
        @                        const float& inverseDiameter,
        @                        uint32* outTags)
        @
        @  r0: *positions
        @  r1: count
        @  r2: &inverseDiameter
        @  r3: *outTags
        @
        @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@

        @  q0 == x
        @  q1 == y
        @  q2 ==
        @  q3 ==
        @  q4 ==
        @  q5 ==
        @  q6 ==
        @  q7 ==
        @  q8 ==
        @  q9 ==
        @ q10 ==
        @ q11 ==
        @ q12 == inverseDiameter
        @ q13 == xScale
        @ q14 == xOffset
        @ q15 == yOffset

        @ Load constants. Literals are > 32, so must load as integers first.
        vld1.f32          {d24[],d25[]}, [r2] @ q12 = inverseDiameter
        vmov.i32          q13, #0x100   @ q13 = xScale = 1 << 8
        vmov.i32          q14, #0x80000 @ q14 = xOffset = (1 << 8) * (1 << 11)
                                        @               = (1 << 19) = 524288
        vmov.i32          q15, #0x800   @ q15 = xScale = 1 << 11 = 2048
        vcvt.f32.u32      q13, q13      @ convert to float
        vcvt.f32.u32      q14, q14
        vcvt.f32.u32      q15, q15

        @ Calculate tags four at a time, from positions.
.L_CalculateTags_MainLoop:
        @ We consume 32-bytes per iteration, so prefetch 4 iterations ahead.
        @ TODO: experiment with different prefetch lengths on different
        @ architectures.
        pld               [r0, #128]    @ Prefetch position data

        @ {q0, q1} == xPosition and yPosition
        @ Four values in each. q0 = (x0, x1, x2, x3)
        vld2.f32          {q0, q1}, [r0]! @ Read in positions; increment ptr

        @ Calculate tags four at a time.
        vmul.f32          q0, q0, q12   @ q0 = x = xPosition * inverseDiameter
        vmul.f32          q1, q1, q12   @ q1 = y = yPosition * inverseDiameter
        vmul.f32          q0, q0, q13   @ q0 = x * xScale
        vadd.f32          q1, q1, q15   @ q1 = y + yOffset
        vadd.f32          q0, q0, q14   @ q0 = x * xScale + xOffset
        vcvt.u32.f32      q1, q1        @ q1 = (uint32)(y + yOffset)
        vcvt.u32.f32      q0, q0        @ q0 = (uint32)(x * xScale + xOffset)
        vsli.u32          q0, q1, #20   @ q0 = tag
                                        @    = ((uint32)(y + yOffset) <<yShift)
                                        @    + (uint32)(xScale * x + xOffset)

        @ Decrement loop counter; sets the 'gt' flag used in 'bgt' below.
        @ Pipelining is best if there are instructions between the 'subs' and
        @ 'bgt' instructions, since it takes a few cycles for the result of
        @ 'subs' to propegate to the flags register.
        subs              r1, r1, #4

        @ Write out, ignoring index.
        pld               [r3, #64]     @ Prefetch output tag array
        vst1.f32          {q0}, [r3]!   @ write out tags; increment ptr

        bgt               .L_CalculateTags_MainLoop

.L_CalculateTags_Return:
        bx                lr



        .balign   4
        .thumb_func
        @
        @ Once four contacts have been found, calculate their weights and
        @ normals (using SIMD, so all at once).
        @
        @ Also, grab their flags from the flags buffer, and OR them together.
        @ This flag grabbing is slow because we access the flag buffer in a
        @ random order. We use prefetch instructions 'pld' to minimize the
        @ cost of cache misses.
        @
FindContacts_PostProcess:
        @ Preload first four flag addresses into cache.
        @ Note: hardware only has four preload slots.
        ldrh              r9, [r4]
        ldrh              r10, [r4, #2]
        ldrh              r11, [r4, #16]
        ldrh              r12, [r4, #18]
        pld               [r7, r9, lsl #2]
        pld               [r7, r10, lsl #2]
        pld               [r7, r11, lsl #2]
        pld               [r7, r12, lsl #2]

        @ q0 = packedIndices -- indices output to b2ParticleContact
        @ q1 = distBtParticlesSq -- will be used to calculate weight
        @ q2 = diffX -- will be used to calculate normal
        @ q3 = diffY -- will be used to calculate normal
        add               r8, r4, #32
        vld4.f32          {d0, d2, d4, d6}, [r4]
        vld4.f32          {d1, d3, d5, d7}, [r8]

        @ Use distSq to estimate 1 / dist.
        vrsqrte.f32       q8, q1      @ q8 = 1 / dist -- (rough estimate)
        vmul.f32          q9, q8, q1  @ q9 = 1 / dist * distSq -- (appr 'dist')
        vrsqrts.f32       q9, q9, q8  @ q9 = (3 - 1/dist * dist) / 2 -- (error)
        vmul.f32          q8, q8, q9  @ q8 = (error) / dist -- (estimate)
        vcgt.f32          q9, q8, #0  @ q8 = 1 / dist > 0 (true if not NaN)
        vand              q8, q8, q9  @ q8 = 1 / dist if valid, or 0 if NaN

        @ Since we expand the output to include 'weight', we need to preserve
        @ subsequent contacts. Note that there may be up to 7 contacts waiting
        @ to be post-processed, since we output contacts in up-to groups of 4.
        add               r8, r4, #64
        vldmia            r8, {q9, q10, q11}

        @ Load first four flags, 'or' them in pairs, then write to destination.
        ldr               r9, [r7, r9, lsl #2]
        ldr               r10, [r7, r10, lsl #2]
        ldr               r11, [r7, r11, lsl #2]
        ldr               r12, [r7, r12, lsl #2]
        orr               r9, r9, r10
        orr               r11, r11, r12
        str               r9, [r4, #16]
        str               r11, [r4, #36]

        @ Preload the next four flags into cache.
        ldrh              r9, [r4, #32]
        ldrh              r10, [r4, #34]
        ldrh              r11, [r4, #48]
        ldrh              r12, [r4, #50]
        pld               [r7, r9, lsl #2]
        pld               [r7, r10, lsl #2]
        pld               [r7, r11, lsl #2]
        pld               [r7, r12, lsl #2]

        @ Calculate normal and weight.
        vmul.f32          q1, q1, q8     @ q1 = distSq / dist = dist
        vmul.f32          q2, q2, q8     @ q2 = normX = diffX / dist
        vmul.f32          q1, q1, q14    @ q1 = dist / diameter
        vmul.f32          q3, q3, q8     @ q3 = normY = diffY / dist
        vsub.f32          q1, q12, q1    @ q1 = weight = 1 - dist / diameter

        @ Store again, making room for 'weight' member variable this time.
        @ TODO OPT: Interleave with 'or' instructions below.
        mov               r8, #20        @ r8 = 20 = sizeof(b2ParticleContact)
        vst4.f32          {d0[0], d2[0], d4[0], d6[0]}, [r4], r8
        vst4.f32          {d0[1], d2[1], d4[1], d6[1]}, [r4], r8
        vst4.f32          {d1[0], d3[0], d5[0], d7[0]}, [r4], r8
        vst4.f32          {d1[1], d3[1], d5[1], d7[1]}, [r4], r8
        mov               r8, #12        @ r8 = 12 = sizeof(FindContactInput)

        @ Load next four flags, 'or' them in pairs, then write to destination.
        ldr               r9, [r7, r9, lsl #2]
        ldr               r10, [r7, r10, lsl #2]
        ldr               r11, [r7, r11, lsl #2]
        ldr               r12, [r7, r12, lsl #2]
        orr               r9, r9, r10
        orr               r11, r11, r12
        str               r9, [r4, #-24]
        str               r11, [r4, #-4]

        @ Update output pointers. Since we output 4 contacts, and added 4 bytes
        @ for 'weight' on each contact, the output pointer must be advanced by
        @ 16 bytes.
        add               r3, r3, #16
        add               r5, r5, #4          @ numContacts += 4

        @ Restore subsequent contacts. That is, contacts that have yet to be
        @ post-processed.
        vstmia            r4, {q9, q10, q11}

        bx                lr


        @ When used with the 'vtbl' instruction, grabs the first byte of every
        @ word, and places it in the first word. Fills the second word with 0s.
        @ For example, (0xFFFFFFFF, 0x00000000, 0x00000000, 0xFFFFFFFF)
        @              ==> (0xFF0000FF, 0x00000000)
CONST_IS_CLOSE_TABLE_INDICES:
        .byte           0
        .byte           4
        .byte           8
        .byte           12
        .byte           0xFF
        .byte           0xFF
        .byte           0xFF
        .byte           0xFF


        .balign   4
        .global   FindContactsFromChecks_Simd
        .thumb_func
FindContactsFromChecks_Simd:
        @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
        @
        @ void FindContactsFromChecks_Simd(
        @       const FindContactInput* reordered,
        @       const FindContactCheck* checks,
        @       int numChecks,
        @       const float& particleDiameterSq,
        @       const float& particleDiameterInv,
        @       const uint32* flags,
        @       b2GrowableBuffer<b2ParticleContact>& contacts)
        @
        @    Parameters
        @  r0: *reordered
        @  r1: *checks
        @  r2: numChecks
        @  r3: particleDiameterSq
        @  [sp]: particleDiameterInv
        @  [sp+4]: *flags
        @  [sp+8]: contacts
        @
        @    Persistent Variables
        @  r0: *reordered (constant)
        @  r1: *checks (advance once per iteration)
        @  r2: numChecks (decrement once per iteration)
        @  r3: *out <-- next free entry of outContacts array
        @  r4: *postProcess <-- entry on-deck to be post-processed
        @  r5: numContacts
        @  r6: maxSafeContacts
        @  r7: *flags (constant)
        @  r8: 20 = sizeof(b2ParticleContact), or
        @      12 = sizeof(FindContactInput) (constants)
        @
        @    Scratch Variables
        @  r9:
        @  r10: address of current particle position
        @  r11: address of comparator particle positions
        @  r12: isClose (compacted)
        @
        @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
        @
        @    Scratch
        @  q0 == index     ------> packedIndices
        @  q1 == positionX ---_    distBtParticlesSq
        @  q2 == positionY --_ --> normX
        @  q3 ==              ---> normY
        @
        @    Unused (note: these are callee-saved)
        @  q4 ==
        @  q5 ==
        @  q6 ==
        @  q7 ==
        @
        @    Scratch
        @  q8 == comparatorIndices
        @  q9 == comparatorPositionX
        @ q10 == comparatorPositionY
        @ q11 ==
        @
        @    Constants
        @ q12 == 1.0f
        @ q13 == isClose table indices
        @ q14 == 1 / particleDiameter
        @ q15 == particleDiameterSq

        push              {r4-r11, lr}

        @ Load constants from registers and stack.
        vld1.f32          {d30[],d31[]}, [r3]  @ q15 = particleDiameterSq
        ldr               r12, [sp, #36]       @ r12 = particleDiameterInv
        vld1.f32          {d28[],d29[]}, [r12] @ q14 = particleDiameterInv
        ldr               r9, [sp, #44]    @ r9 = contacts
        ldr               r7, [sp, #40]    @ r7 = flags
        ldr               r3, [r9, #0]     @ r3 = out = contacts.data
        ldr               r6, [r9, #8]     @ r6 = contacts.capacity
        mov               r4, r3           @ r4 = postProcess = outContacts
        mov               r5, #0           @ r5 = numContacts
        sub               r6, r6, #8       @ r6 = maxSafeContacts = capacity - 8
        mov               r8, #12          @ r8 = 12 = sizeof(FindContactInput)

        @ Perform zero iterations if 'numChecks' is empty.
        @ Must happen after initializing r5 = numContacts = 0.
        cmp               r2, #0
        ble               .L_FindContacts_Return

        @ Load and calculate remaining constants.
        vmov.f32          q12, #1.0        @ q12 = 1.0f splatted
        adr               r12, CONST_IS_CLOSE_TABLE_INDICES
        vld1.8            {d26}, [r12]     @ q13 = *CONST_IS_CLOSE_TABLE_INDICES

.L_FindContacts_MainLoop:
        pld               [r1, #8]         @ prefetch two loops ahead

        @ r10 <== Address of 'position', the current particle position
        @ r11 <== Address of '&comparator[0]', the first particle position we
        @         compare against.
        ldr               r10, [r1], #4    @ r10 = positionIndex|comparatorIndex
        smlatb            r11, r10, r8, r0 @ r11 = address of first comparator
        smlabb            r10, r10, r8, r0 @ r10 = address of current input
        add               r12, r11, #24    @ r12 = address of third comparator

        @ Exit if not enough space in output array (part 1)
        cmp               r5, r6

        @ {q0, q1, q2} == index, positionX, positionY, splatted across vector
        vld3.f32          {d0[], d2[], d4[]}, [r10]
        vld3.f32          {d1[], d3[], d5[]}, [r10]

        @ {q8, q9, q10} == comparatorIndices, comparatorPosX and comparatorPosY
        @                 positions we compare against (positionX, positionY)
        vld3.f32          {d16, d18, d20}, [r11]
        vld3.f32          {d17, d19, d21}, [r12]

        @ q0 = packedIndices -- indices output to b2ParticleContact
        @ q1 = distBtParticlesSq -- will be used to calculate weight
        @ q2 = diffX -- will be used to calculate normal
        @ q3 = diffY -- will be used to calculate normal
        vsub.f32          q3, q10, q2  @ q3 = diffY = comparatorPosY - positionY
        vsub.f32          q2, q9, q1   @ q2 = diffX = comparatorPosX - positionX
        vsli.32           q0, q8, #16  @ q0 = comparatorIndex[i] << 16 | index
        vmul.f32          q1, q3, q3   @ q1 = diffX * diffX
        vmla.f32          q1, q2, q2   @ q1 = diffX * diffX + diffY * diffY

        @ Determine if each particle is close enough to output.
        @ Pack the isClose bitmap (four T or F) into a 32-bit bitmap.
        @ Move 32-bit bitmap to CPU register, for conditional operations.
        @ Note: NEON to CPU register moves are slow (20 cyclds) on some
        @ implementations of NEON.
        @   isClose = distBtParticlesSq < particleDiameterSq
        vclt.f32          q8, q1, q15          @ q8 == isClose
        vtbl.8            d16, {d16,d17}, d26  @ q8[0] == isClose(packed)
        vmov.32           r12, d16[0]          @ q8[0] ==> r12.

        @ If not enough space in output array, grow it.
        @ This is a heavy operation, but should happen rarely.
        ble               .L_FindContacts_Output
        ldr               r9, [sp, #44]        @ r9 = contacts
        str               r5, [r9, #4]         @ contacts.count = numContacts
        ldr               r10, [r9, #0]        @ r10 = contacts.data
        push              {r0-r3, r9, r10, r12}
        vpush             {q0, q1, q2, q3}
        vpush             {q12, q13, q14, q15}
        mov               r0, r9               @ r0 = contacts
        bl                GrowParticleContactBuffer
        vpop              {q12, q13, q14, q15}
        vpop              {q0, q1, q2, q3}
        pop               {r0-r3, r9, r10, r12}

        @ The output array was reallocated, so update 'out', 'postProcess' and
        @ 'maxSafeContacts' pointers.
        ldr               r6, [r9, #8]         @ r6 = contacts.capacity
        ldr               r9, [r9, #0]         @ r9 = contacts.data
        sub               r9, r9, r10          @ r9 = data buffer offset
        sub               r6, r6, #8           @ r6 = maxSafeContacts
        add               r3, r3, r9           @ r3 += data buffer offset
        add               r4, r4, r9           @ r4 += data buffer offset

.L_FindContacts_Output:
        @ Store results to memory, but only results that are close
        tst               r12, 0xFF
        it                ne
        vst4ne.32         {d0[0],d2[0],d4[0],d6[0]}, [r3]! @ Store 1st contact

        tst               r12, 0xFF00
        it                ne
        vst4ne.32         {d0[1],d2[1],d4[1],d6[1]}, [r3]! @ Store 2nd contact

        tst               r12, 0xFF0000
        it                ne
        vst4ne.32         {d1[0],d3[0],d5[0],d7[0]}, [r3]! @ Store 3rd contact

        tst               r12, 0xFF000000
        it                ne
        vst4ne.32         {d1[1],d3[1],d5[1],d7[1]}, [r3]! @ Store 4th contact

        @ post-process the last four elements that have been output
        @ r12 = 5th element to not be post-processed yet
        add               r12, r4, #64         @ r12 = nextPostProcess
        cmp               r3, r12
        it                ge
        blge              FindContacts_PostProcess

        @ decrement loop counter; sets the 'gt' flag used in 'bgt' below
        subs              r2, r2, #1
        bgt               .L_FindContacts_MainLoop

.L_FindContacts_PostProcessRemainingItems:
        @ If at least one output item needs post-processing, do it.
        subs              r12, r3, r4
        ble               .L_FindContacts_Return

        @ r12/16 = num extra contacts to process
        add               r5, r5, r12, lsr #4  @ numContacts += num extra
        push              {r5}                 @ Save numContacts, since stomped

        @ Ensure indices past end of array are zeroed out.
        @ We process 4 contacts in FindContacts_PostProcess, even if we only
        @ have one left to process.
        mov               r12, #0
        str               r12, [r3]
        str               r12, [r3, #16]
        str               r12, [r3, #32]

        bl                FindContacts_PostProcess
        pop               {r5}                 @ Restore numContacts

.L_FindContacts_Return:
        @ Set the final number of contacts in the output buffer.
        ldr               r9, [sp, #44]        @ r9 = contacts
        str               r5, [r9, #4]         @ contacts.count = numContacts

        @ Return by popping the original lr into pc.
        pop               {r4-r11, pc}