viktorljung
442 lines
19 KiB
ArmAsm
442 lines
19 KiB
ArmAsm
@
|
|
@ 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}
|