source/gles2glide64/src/Glide64/3dmathneon.cpp

   1 #include "3dmath.h"
   2
   3 static void MultMatrix_neon( float m0[4][4], float m1[4][4], float dest[4][4])
   4 {
   5     asm volatile (
   6         "vld1.32                {d0, d1}, [%1]!                 \n\t"   //q0 = m1
   7         "vld1.32                {d2, d3}, [%1]!         \n\t"   //q1 = m1+4
   8         "vld1.32                {d4, d5}, [%1]!         \n\t"   //q2 = m1+8
   9         "vld1.32                {d6, d7}, [%1]          \n\t"   //q3 = m1+12
  10         "vld1.32                {d16, d17}, [%0]!               \n\t"   //q8 = m0
  11         "vld1.32                {d18, d19}, [%0]!       \n\t"   //q9 = m0+4
  12         "vld1.32                {d20, d21}, [%0]!       \n\t"   //q10 = m0+8
  13         "vld1.32                {d22, d23}, [%0]        \n\t"   //q11 = m0+12
  14
  15         "vmul.f32               q12, q8, d0[0]                  \n\t"   //q12 = q8 * d0[0]
  16         "vmul.f32               q13, q8, d2[0]              \n\t"       //q13 = q8 * d2[0]
  17         "vmul.f32               q14, q8, d4[0]              \n\t"       //q14 = q8 * d4[0]
  18         "vmul.f32               q15, q8, d6[0]                  \n\t"   //q15 = q8 * d6[0]
  19         "vmla.f32               q12, q9, d0[1]                  \n\t"   //q12 = q9 * d0[1]
  20         "vmla.f32               q13, q9, d2[1]              \n\t"       //q13 = q9 * d2[1]
  21         "vmla.f32               q14, q9, d4[1]              \n\t"       //q14 = q9 * d4[1]
  22         "vmla.f32               q15, q9, d6[1]              \n\t"       //q15 = q9 * d6[1]
  23         "vmla.f32               q12, q10, d1[0]                 \n\t"   //q12 = q10 * d0[0]
  24         "vmla.f32               q13, q10, d3[0]                 \n\t"   //q13 = q10 * d2[0]
  25         "vmla.f32               q14, q10, d5[0]                 \n\t"   //q14 = q10 * d4[0]
  26         "vmla.f32               q15, q10, d7[0]                 \n\t"   //q15 = q10 * d6[0]
  27         "vmla.f32               q12, q11, d1[1]                 \n\t"   //q12 = q11 * d0[1]
  28         "vmla.f32               q13, q11, d3[1]                 \n\t"   //q13 = q11 * d2[1]
  29         "vmla.f32               q14, q11, d5[1]                 \n\t"   //q14 = q11 * d4[1]
  30         "vmla.f32               q15, q11, d7[1]             \n\t"       //q15 = q11 * d6[1]
  31
  32         "vst1.32                {d24, d25}, [%2]!               \n\t"   //d = q12
  33         "vst1.32                {d26, d27}, [%2]!           \n\t"       //d+4 = q13
  34         "vst1.32                {d28, d29}, [%2]!           \n\t"       //d+8 = q14
  35         "vst1.32                {d30, d31}, [%2]            \n\t"       //d+12 = q15
  36
  37         :"+r"(m0), "+r"(m1), "+r"(dest):
  38     : "d0", "d1", "d2", "d3", "d4", "d5", "d6", "d7",
  39     "d16", "d17", "d18", "d19", "d20", "d21", "d22", "d23",
  40     "d24", "d25", "d26", "d27", "d28", "d29", "d30", "d31",
  41     "memory"
  42         );
  43 }
  44
  45 static void TransformVectorNormalize_neon(float vec[3], float mtx[4][4])
  46 {
  47         asm volatile (
  48         "vld1.32                {d0}, [%1]                      \n\t"   //Q0 = v
  49         "flds                   s2, [%1, #8]                    \n\t"   //Q0 = v
  50         "vld1.32                {d18, d19}, [%0]!               \n\t"   //Q1 = m
  51         "vld1.32                {d20, d21}, [%0]!           \n\t"       //Q2 = m+4
  52         "vld1.32                {d22, d23}, [%0]            \n\t"       //Q3 = m+8
  53
  54         "vmul.f32               q2, q9, d0[0]                   \n\t"   //q2 = q9*Q0[0]
  55         "vmla.f32               q2, q10, d0[1]                  \n\t"   //Q5 += Q1*Q0[1]
  56         "vmla.f32               q2, q11, d1[0]                  \n\t"   //Q5 += Q2*Q0[2]
  57
  58     "vmul.f32           d0, d4, d4                              \n\t"   //d0 = d0*d0
  59         "vpadd.f32              d0, d0, d0                              \n\t"   //d0 = d[0] + d[1]
  60     "vmla.f32           d0, d5, d5                              \n\t"   //d0 = d0 + d1*d1
  61
  62         "vmov.f32               d1, d0                                  \n\t"   //d1 = d0
  63         "vrsqrte.f32    d0, d0                                  \n\t"   //d0 = ~ 1.0 / sqrt(d0)
  64         "vmul.f32               d2, d0, d1                              \n\t"   //d2 = d0 * d1
  65         "vrsqrts.f32    d3, d2, d0                              \n\t"   //d3 = (3 - d0 * d2) / 2
  66         "vmul.f32               d0, d0, d3                              \n\t"   //d0 = d0 * d3
  67         "vmul.f32               d2, d0, d1                              \n\t"   //d2 = d0 * d1
  68         "vrsqrts.f32    d3, d2, d0                              \n\t"   //d3 = (3 - d0 * d3) / 2
  69         "vmul.f32               d0, d0, d3                              \n\t"   //d0 = d0 * d4
  70
  71         "vmul.f32               q2, q2, d0[0]                   \n\t"   //d0= d2*d4
  72
  73         "vst1.32                {d4}, [%1]                  \n\t"       //Q4 = m+12
  74         "fsts                   s10, [%1, #8]           \n\t"   //Q4 = m+12
  75         : "+r"(mtx): "r"(vec)
  76     : "d0","d1","d2","d3","d18","d19","d20","d21","d22", "d23", "memory"
  77         );
  78 }
  79
  80 static void Normalize_neon(float v[3])
  81 {
  82         asm volatile (
  83         "vld1.32                {d4}, [%0]!                     \n\t"   //d4={x,y}
  84         "flds                   s10, [%0]               \n\t"   //d5[0] = z
  85         "sub                    %0, %0, #8              \n\t"   //d5[0] = z
  86         "vmul.f32               d0, d4, d4                              \n\t"   //d0= d4*d4
  87         "vpadd.f32              d0, d0, d0                              \n\t"   //d0 = d[0] + d[1]
  88     "vmla.f32           d0, d5, d5                              \n\t"   //d0 = d0 + d5*d5
  89
  90         "vmov.f32               d1, d0                                  \n\t"   //d1 = d0
  91         "vrsqrte.f32    d0, d0                                  \n\t"   //d0 = ~ 1.0 / sqrt(d0)
  92         "vmul.f32               d2, d0, d1                              \n\t"   //d2 = d0 * d1
  93         "vrsqrts.f32    d3, d2, d0                              \n\t"   //d3 = (3 - d0 * d2) / 2
  94         "vmul.f32               d0, d0, d3                              \n\t"   //d0 = d0 * d3
  95         "vmul.f32               d2, d0, d1                              \n\t"   //d2 = d0 * d1
  96         "vrsqrts.f32    d3, d2, d0                              \n\t"   //d3 = (3 - d0 * d3) / 2
  97         "vmul.f32               d0, d0, d3                              \n\t"   //d0 = d0 * d4
  98
  99         "vmul.f32               q2, q2, d0[0]                   \n\t"   //d0= d2*d4
 100         "vst1.32                {d4}, [%0]!                     \n\t"   //d2={x0,y0}, d3={z0, w0}
 101         "fsts                   s10, [%0]                       \n\t"   //d2={x0,y0}, d3={z0, w0}
 102
 103         :"+r"(v) :
 104     : "d0", "d1", "d2", "d3", "d4", "d5", "memory"
 105         );
 106 }
 107
 108 static float DotProduct_neon( float v0[3], float v1[3] )
 109 {
 110     float dot;
 111         asm volatile (
 112         "vld1.32                {d8}, [%1]!                     \n\t"   //d8={x0,y0}
 113         "vld1.32                {d10}, [%2]!            \n\t"   //d10={x1,y1}
 114         "flds                   s18, [%1, #0]       \n\t"       //d9[0]={z0}
 115         "flds                   s22, [%2, #0]       \n\t"       //d11[0]={z1}
 116         "vmul.f32               d12, d8, d10            \n\t"   //d0= d2*d4
 117         "vpadd.f32              d12, d12, d12           \n\t"   //d0 = d[0] + d[1]
 118         "vmla.f32               d12, d9, d11            \n\t"   //d0 = d0 + d3*d5
 119     "fmrs               %0, s24                 \n\t"   //r0 = s0
 120         : "=r"(dot), "+r"(v0), "+r"(v1):
 121     : "d8", "d9", "d10", "d11", "d12"
 122
 123         );
 124     return dot;
 125 }
 126
 127 void MathInitNeon()
 128 {
 129     MulMatrices = MultMatrix_neon;
 130     //TransformVectorNormalize = TransformVectorNormalize_neon;
 131     NormalizeVector = Normalize_neon;
 132     DotProduct = DotProduct_neon;
 133 }