Main MRPT website > C++ reference
MRPT logo

PacketMath.h

Go to the documentation of this file.
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
00005 // Copyright (C) 2010 Konstantinos Margaritis <markos@codex.gr>
00006 // Heavily based on Gael's SSE version.
00007 //
00008 // Eigen is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 3 of the License, or (at your option) any later version.
00012 //
00013 // Alternatively, you can redistribute it and/or
00014 // modify it under the terms of the GNU General Public License as
00015 // published by the Free Software Foundation; either version 2 of
00016 // the License, or (at your option) any later version.
00017 //
00018 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
00019 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
00020 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
00021 // GNU General Public License for more details.
00022 //
00023 // You should have received a copy of the GNU Lesser General Public
00024 // License and a copy of the GNU General Public License along with
00025 // Eigen. If not, see <http://www.gnu.org/licenses/>.
00026 
00027 #ifndef EIGEN_PACKET_MATH_NEON_H
00028 #define EIGEN_PACKET_MATH_NEON_H
00029 
00030 namespace internal {
00031 
00032 #ifndef EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD
00033 #define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 8
00034 #endif
00035 
00036 #ifndef EIGEN_TUNE_FOR_CPU_CACHE_SIZE
00037 #define EIGEN_TUNE_FOR_CPU_CACHE_SIZE 4*192*192
00038 #endif
00039 
00040 // FIXME NEON has 16 quad registers, but since the current register allocator
00041 // is so bad, it is much better to reduce it to 8
00042 #ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS
00043 #define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 8
00044 #endif
00045 
00046 typedef float32x4_t Packet4f;
00047 typedef int32x4_t   Packet4i;
00048 typedef uint32x4_t   Packet4ui;
00049 
00050 #define _EIGEN_DECLARE_CONST_Packet4f(NAME,X) \
00051   const Packet4f p4f_##NAME = pset1<Packet4f>(X)
00052 
00053 #define _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(NAME,X) \
00054   const Packet4f p4f_##NAME = vreinterpretq_f32_u32(pset1<int>(X))
00055 
00056 #define _EIGEN_DECLARE_CONST_Packet4i(NAME,X) \
00057   const Packet4i p4i_##NAME = pset1<Packet4i>(X)
00058 
00059 #ifndef __pld
00060 #define __pld(x) asm volatile ( "   pld [%[addr]]\n" :: [addr] "r" (x) : "cc" );
00061 #endif
00062 
00063 template<> struct packet_traits<float>  : default_packet_traits
00064 {
00065   typedef Packet4f type;
00066   enum {
00067     Vectorizable = 1,
00068     AlignedOnScalar = 1,
00069     size = 4,
00070    
00071     HasDiv  = 1,
00072     // FIXME check the Has*
00073     HasSin  = 0,
00074     HasCos  = 0,
00075     HasLog  = 0,
00076     HasExp  = 0,
00077     HasSqrt = 0
00078   };
00079 };
00080 template<> struct packet_traits<int>    : default_packet_traits
00081 {
00082   typedef Packet4i type;
00083   enum {
00084     Vectorizable = 1,
00085     AlignedOnScalar = 1,
00086     size=4
00087     // FIXME check the Has*
00088   };
00089 };
00090 
00091 template<> struct unpacket_traits<Packet4f> { typedef float  type; enum {size=4}; };
00092 template<> struct unpacket_traits<Packet4i> { typedef int    type; enum {size=4}; };
00093 
00094 template<> EIGEN_STRONG_INLINE Packet4f pset1<Packet4f>(const float&  from) { return vdupq_n_f32(from); }
00095 template<> EIGEN_STRONG_INLINE Packet4i pset1<Packet4i>(const int&    from)   { return vdupq_n_s32(from); }
00096 
00097 template<> EIGEN_STRONG_INLINE Packet4f plset<float>(const float& a)
00098 {
00099   Packet4f countdown = { 3, 2, 1, 0 };
00100   return vaddq_f32(pset1<Packet4f>(a), countdown);
00101 }
00102 template<> EIGEN_STRONG_INLINE Packet4i plset<int>(const int& a)
00103 {
00104   Packet4i countdown = { 3, 2, 1, 0 };
00105   return vaddq_s32(pset1<Packet4i>(a), countdown);
00106 }
00107 
00108 template<> EIGEN_STRONG_INLINE Packet4f padd<Packet4f>(const Packet4f& a, const Packet4f& b) { return vaddq_f32(a,b); }
00109 template<> EIGEN_STRONG_INLINE Packet4i padd<Packet4i>(const Packet4i& a, const Packet4i& b) { return vaddq_s32(a,b); }
00110 
00111 template<> EIGEN_STRONG_INLINE Packet4f psub<Packet4f>(const Packet4f& a, const Packet4f& b) { return vsubq_f32(a,b); }
00112 template<> EIGEN_STRONG_INLINE Packet4i psub<Packet4i>(const Packet4i& a, const Packet4i& b) { return vsubq_s32(a,b); }
00113 
00114 template<> EIGEN_STRONG_INLINE Packet4f pnegate(const Packet4f& a) { return vnegq_f32(a); }
00115 template<> EIGEN_STRONG_INLINE Packet4i pnegate(const Packet4i& a) { return vnegq_s32(a); }
00116 
00117 template<> EIGEN_STRONG_INLINE Packet4f pmul<Packet4f>(const Packet4f& a, const Packet4f& b) { return vmulq_f32(a,b); }
00118 template<> EIGEN_STRONG_INLINE Packet4i pmul<Packet4i>(const Packet4i& a, const Packet4i& b) { return vmulq_s32(a,b); }
00119 
00120 template<> EIGEN_STRONG_INLINE Packet4f pdiv<Packet4f>(const Packet4f& a, const Packet4f& b)
00121 {
00122   Packet4f inv, restep, div;
00123 
00124   // NEON does not offer a divide instruction, we have to do a reciprocal approximation
00125   // However NEON in contrast to other SIMD engines (AltiVec/SSE), offers
00126   // a reciprocal estimate AND a reciprocal step -which saves a few instructions
00127   // vrecpeq_f32() returns an estimate to 1/b, which we will finetune with
00128   // Newton-Raphson and vrecpsq_f32()
00129   inv = vrecpeq_f32(b);
00130 
00131   // This returns a differential, by which we will have to multiply inv to get a better
00132   // approximation of 1/b.
00133   restep = vrecpsq_f32(b, inv);
00134   inv = vmulq_f32(restep, inv);
00135 
00136   // Finally, multiply a by 1/b and get the wanted result of the division.
00137   div = vmulq_f32(a, inv);
00138 
00139   return div;
00140 }
00141 template<> EIGEN_STRONG_INLINE Packet4i pdiv<Packet4i>(const Packet4i& /*a*/, const Packet4i& /*b*/)
00142 { eigen_assert(false && "packet integer division are not supported by NEON");
00143   return pset1<Packet4i>(0);
00144 }
00145 
00146 // for some weird raisons, it has to be overloaded for packet of integers
00147 template<> EIGEN_STRONG_INLINE Packet4i pmadd(const Packet4i& a, const Packet4i& b, const Packet4i& c) { return padd(pmul(a,b), c); }
00148 
00149 template<> EIGEN_STRONG_INLINE Packet4f pmin<Packet4f>(const Packet4f& a, const Packet4f& b) { return vminq_f32(a,b); }
00150 template<> EIGEN_STRONG_INLINE Packet4i pmin<Packet4i>(const Packet4i& a, const Packet4i& b) { return vminq_s32(a,b); }
00151 
00152 template<> EIGEN_STRONG_INLINE Packet4f pmax<Packet4f>(const Packet4f& a, const Packet4f& b) { return vmaxq_f32(a,b); }
00153 template<> EIGEN_STRONG_INLINE Packet4i pmax<Packet4i>(const Packet4i& a, const Packet4i& b) { return vmaxq_s32(a,b); }
00154 
00155 // Logical Operations are not supported for float, so we have to reinterpret casts using NEON intrinsics
00156 template<> EIGEN_STRONG_INLINE Packet4f pand<Packet4f>(const Packet4f& a, const Packet4f& b)
00157 {
00158   return vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b)));
00159 }
00160 template<> EIGEN_STRONG_INLINE Packet4i pand<Packet4i>(const Packet4i& a, const Packet4i& b) { return vandq_s32(a,b); }
00161 
00162 template<> EIGEN_STRONG_INLINE Packet4f por<Packet4f>(const Packet4f& a, const Packet4f& b)
00163 {
00164   return vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b)));
00165 }
00166 template<> EIGEN_STRONG_INLINE Packet4i por<Packet4i>(const Packet4i& a, const Packet4i& b) { return vorrq_s32(a,b); }
00167 
00168 template<> EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b)
00169 {
00170   return vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b)));
00171 }
00172 template<> EIGEN_STRONG_INLINE Packet4i pxor<Packet4i>(const Packet4i& a, const Packet4i& b) { return veorq_s32(a,b); }
00173 
00174 template<> EIGEN_STRONG_INLINE Packet4f pandnot<Packet4f>(const Packet4f& a, const Packet4f& b)
00175 {
00176   return vreinterpretq_f32_u32(vbicq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b)));
00177 }
00178 template<> EIGEN_STRONG_INLINE Packet4i pandnot<Packet4i>(const Packet4i& a, const Packet4i& b) { return vbicq_s32(a,b); }
00179 
00180 template<> EIGEN_STRONG_INLINE Packet4f pload<Packet4f>(const float* from) { EIGEN_DEBUG_ALIGNED_LOAD return vld1q_f32(from); }
00181 template<> EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int*   from) { EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s32(from); }
00182 
00183 template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float* from) { EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_f32(from); }
00184 template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int* from)   { EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s32(from); }
00185 
00186 template<> EIGEN_STRONG_INLINE Packet4f ploaddup<Packet4f>(const float*   from)
00187 {
00188   float32x2_t lo, hi;
00189   lo = vdup_n_f32(*from);
00190   hi = vdup_n_f32(*from);
00191   return vcombine_f32(lo, hi);
00192 }
00193 template<> EIGEN_STRONG_INLINE Packet4i ploaddup<Packet4i>(const int*     from)
00194 {
00195   int32x2_t lo, hi;
00196   lo = vdup_n_s32(*from);
00197   hi = vdup_n_s32(*from);
00198   return vcombine_s32(lo, hi);
00199 }
00200 
00201 template<> EIGEN_STRONG_INLINE void pstore<float>(float*   to, const Packet4f& from) { EIGEN_DEBUG_ALIGNED_STORE vst1q_f32(to, from); }
00202 template<> EIGEN_STRONG_INLINE void pstore<int>(int*       to, const Packet4i& from) { EIGEN_DEBUG_ALIGNED_STORE vst1q_s32(to, from); }
00203 
00204 template<> EIGEN_STRONG_INLINE void pstoreu<float>(float*  to, const Packet4f& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_f32(to, from); }
00205 template<> EIGEN_STRONG_INLINE void pstoreu<int>(int*      to, const Packet4i& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_s32(to, from); }
00206 
00207 template<> EIGEN_STRONG_INLINE void prefetch<float>(const float* addr) { __pld(addr); }
00208 template<> EIGEN_STRONG_INLINE void prefetch<int>(const int*     addr) { __pld(addr); }
00209 
00210 // FIXME only store the 2 first elements ?
00211 template<> EIGEN_STRONG_INLINE float  pfirst<Packet4f>(const Packet4f& a) { float EIGEN_ALIGN16 x[4]; vst1q_f32(x, a); return x[0]; }
00212 template<> EIGEN_STRONG_INLINE int    pfirst<Packet4i>(const Packet4i& a) { int   EIGEN_ALIGN16 x[4]; vst1q_s32(x, a); return x[0]; }
00213 
00214 template<> EIGEN_STRONG_INLINE Packet4f preverse(const Packet4f& a) {
00215   float32x2_t a_lo, a_hi;
00216   Packet4f a_r64;
00217 
00218   a_r64 = vrev64q_f32(a);
00219   a_lo = vget_low_f32(a_r64);
00220   a_hi = vget_high_f32(a_r64);
00221   return vcombine_f32(a_hi, a_lo);
00222 }
00223 template<> EIGEN_STRONG_INLINE Packet4i preverse(const Packet4i& a) {
00224   int32x2_t a_lo, a_hi;
00225   Packet4i a_r64;
00226 
00227   a_r64 = vrev64q_s32(a);
00228   a_lo = vget_low_s32(a_r64);
00229   a_hi = vget_high_s32(a_r64);
00230   return vcombine_s32(a_hi, a_lo);
00231 }
00232 template<> EIGEN_STRONG_INLINE Packet4f pabs(const Packet4f& a) { return vabsq_f32(a); }
00233 template<> EIGEN_STRONG_INLINE Packet4i pabs(const Packet4i& a) { return vabsq_s32(a); }
00234 
00235 template<> EIGEN_STRONG_INLINE float predux<Packet4f>(const Packet4f& a)
00236 {
00237   float32x2_t a_lo, a_hi, sum;
00238   float s[2];
00239 
00240   a_lo = vget_low_f32(a);
00241   a_hi = vget_high_f32(a);
00242   sum = vpadd_f32(a_lo, a_hi);
00243   sum = vpadd_f32(sum, sum);
00244   vst1_f32(s, sum);
00245 
00246   return s[0];
00247 }
00248 
00249 template<> EIGEN_STRONG_INLINE Packet4f preduxp<Packet4f>(const Packet4f* vecs)
00250 {
00251   float32x4x2_t vtrn1, vtrn2, res1, res2;
00252   Packet4f sum1, sum2, sum;
00253 
00254   // NEON zip performs interleaving of the supplied vectors.
00255   // We perform two interleaves in a row to acquire the transposed vector
00256   vtrn1 = vzipq_f32(vecs[0], vecs[2]);
00257   vtrn2 = vzipq_f32(vecs[1], vecs[3]);
00258   res1 = vzipq_f32(vtrn1.val[0], vtrn2.val[0]);
00259   res2 = vzipq_f32(vtrn1.val[1], vtrn2.val[1]);
00260 
00261   // Do the addition of the resulting vectors
00262   sum1 = vaddq_f32(res1.val[0], res1.val[1]);
00263   sum2 = vaddq_f32(res2.val[0], res2.val[1]);
00264   sum = vaddq_f32(sum1, sum2);
00265 
00266   return sum;
00267 }
00268 
00269 template<> EIGEN_STRONG_INLINE int predux<Packet4i>(const Packet4i& a)
00270 {
00271   int32x2_t a_lo, a_hi, sum;
00272   int32_t s[2];
00273 
00274   a_lo = vget_low_s32(a);
00275   a_hi = vget_high_s32(a);
00276   sum = vpadd_s32(a_lo, a_hi);
00277   sum = vpadd_s32(sum, sum);
00278   vst1_s32(s, sum);
00279 
00280   return s[0];
00281 }
00282 
00283 template<> EIGEN_STRONG_INLINE Packet4i preduxp<Packet4i>(const Packet4i* vecs)
00284 {
00285   int32x4x2_t vtrn1, vtrn2, res1, res2;
00286   Packet4i sum1, sum2, sum;
00287 
00288   // NEON zip performs interleaving of the supplied vectors.
00289   // We perform two interleaves in a row to acquire the transposed vector
00290   vtrn1 = vzipq_s32(vecs[0], vecs[2]);
00291   vtrn2 = vzipq_s32(vecs[1], vecs[3]);
00292   res1 = vzipq_s32(vtrn1.val[0], vtrn2.val[0]);
00293   res2 = vzipq_s32(vtrn1.val[1], vtrn2.val[1]);
00294 
00295   // Do the addition of the resulting vectors
00296   sum1 = vaddq_s32(res1.val[0], res1.val[1]);
00297   sum2 = vaddq_s32(res2.val[0], res2.val[1]);
00298   sum = vaddq_s32(sum1, sum2);
00299 
00300   return sum;
00301 }
00302 
00303 // Other reduction functions:
00304 // mul
00305 template<> EIGEN_STRONG_INLINE float predux_mul<Packet4f>(const Packet4f& a)
00306 {
00307   float32x2_t a_lo, a_hi, prod;
00308   float s[2];
00309 
00310   // Get a_lo = |a1|a2| and a_hi = |a3|a4|
00311   a_lo = vget_low_f32(a);
00312   a_hi = vget_high_f32(a);
00313   // Get the product of a_lo * a_hi -> |a1*a3|a2*a4|
00314   prod = vmul_f32(a_lo, a_hi);
00315   // Multiply prod with its swapped value |a2*a4|a1*a3|
00316   prod = vmul_f32(prod, vrev64_f32(prod));
00317   vst1_f32(s, prod);
00318 
00319   return s[0];
00320 }
00321 template<> EIGEN_STRONG_INLINE int predux_mul<Packet4i>(const Packet4i& a)
00322 {
00323   int32x2_t a_lo, a_hi, prod;
00324   int32_t s[2];
00325 
00326   // Get a_lo = |a1|a2| and a_hi = |a3|a4|
00327   a_lo = vget_low_s32(a);
00328   a_hi = vget_high_s32(a);
00329   // Get the product of a_lo * a_hi -> |a1*a3|a2*a4|
00330   prod = vmul_s32(a_lo, a_hi);
00331   // Multiply prod with its swapped value |a2*a4|a1*a3|
00332   prod = vmul_s32(prod, vrev64_s32(prod));
00333   vst1_s32(s, prod);
00334 
00335   return s[0];
00336 }
00337 
00338 // min
00339 template<> EIGEN_STRONG_INLINE float predux_min<Packet4f>(const Packet4f& a)
00340 {
00341   float32x2_t a_lo, a_hi, min;
00342   float s[2];
00343 
00344   a_lo = vget_low_f32(a);
00345   a_hi = vget_high_f32(a);
00346   min = vpmin_f32(a_lo, a_hi);
00347   min = vpmin_f32(min, min);
00348   vst1_f32(s, min);
00349 
00350   return s[0];
00351 }
00352 template<> EIGEN_STRONG_INLINE int predux_min<Packet4i>(const Packet4i& a)
00353 {
00354   int32x2_t a_lo, a_hi, min;
00355   int32_t s[2];
00356 
00357   a_lo = vget_low_s32(a);
00358   a_hi = vget_high_s32(a);
00359   min = vpmin_s32(a_lo, a_hi);
00360   min = vpmin_s32(min, min);
00361   vst1_s32(s, min);
00362 
00363   return s[0];
00364 }
00365 
00366 // max
00367 template<> EIGEN_STRONG_INLINE float predux_max<Packet4f>(const Packet4f& a)
00368 {
00369   float32x2_t a_lo, a_hi, max;
00370   float s[2];
00371 
00372   a_lo = vget_low_f32(a);
00373   a_hi = vget_high_f32(a);
00374   max = vpmax_f32(a_lo, a_hi);
00375   max = vpmax_f32(max, max);
00376   vst1_f32(s, max);
00377 
00378   return s[0];
00379 }
00380 template<> EIGEN_STRONG_INLINE int predux_max<Packet4i>(const Packet4i& a)
00381 {
00382   int32x2_t a_lo, a_hi, max;
00383   int32_t s[2];
00384 
00385   a_lo = vget_low_s32(a);
00386   a_hi = vget_high_s32(a);
00387   max = vpmax_s32(a_lo, a_hi);
00388   max = vpmax_s32(max, max);
00389   vst1_s32(s, max);
00390 
00391   return s[0];
00392 }
00393 
00394 template<int Offset>
00395 struct palign_impl<Offset,Packet4f>
00396 {
00397   EIGEN_STRONG_INLINE static void run(Packet4f& first, const Packet4f& second)
00398   {
00399     if (Offset!=0)
00400       first = vextq_f32(first, second, Offset);
00401   }
00402 };
00403 
00404 template<int Offset>
00405 struct palign_impl<Offset,Packet4i>
00406 {
00407   EIGEN_STRONG_INLINE static void run(Packet4i& first, const Packet4i& second)
00408   {
00409     if (Offset!=0)
00410       first = vextq_s32(first, second, Offset);
00411   }
00412 };
00413 
00414 } // end namespace internal
00415 
00416 #endif // EIGEN_PACKET_MATH_NEON_H



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011