Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / extern / Eigen2 / Eigen / src / Core / CacheFriendlyProduct.h
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra. Eigen itself is part of the KDE project.
3 //
4 // Copyright (C) 2008 Gael Guennebaud <g.gael@free.fr>
5 //
6 // Eigen is free software; you can redistribute it and/or
7 // modify it under the terms of the GNU Lesser General Public
8 // License as published by the Free Software Foundation; either
9 // version 3 of the License, or (at your option) any later version.
10 //
11 // Alternatively, you can redistribute it and/or
12 // modify it under the terms of the GNU General Public License as
13 // published by the Free Software Foundation; either version 2 of
14 // the License, or (at your option) any later version.
15 //
16 // Eigen is distributed in the hope that it will be useful, but WITHOUT ANY
17 // WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
18 // FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License or the
19 // GNU General Public License for more details.
20 //
21 // You should have received a copy of the GNU Lesser General Public
22 // License and a copy of the GNU General Public License along with
23 // Eigen. If not, see <http://www.gnu.org/licenses/>.
24
25 #ifndef EIGEN_CACHE_FRIENDLY_PRODUCT_H
26 #define EIGEN_CACHE_FRIENDLY_PRODUCT_H
27
28 template <int L2MemorySize,typename Scalar>
29 struct ei_L2_block_traits {
30   enum {width = 8 * ei_meta_sqrt<L2MemorySize/(64*sizeof(Scalar))>::ret };
31 };
32
33 #ifndef EIGEN_EXTERN_INSTANTIATIONS
34
35 template<typename Scalar>
36 static void ei_cache_friendly_product(
37   int _rows, int _cols, int depth,
38   bool _lhsRowMajor, const Scalar* _lhs, int _lhsStride,
39   bool _rhsRowMajor, const Scalar* _rhs, int _rhsStride,
40   bool resRowMajor, Scalar* res, int resStride)
41 {
42   const Scalar* EIGEN_RESTRICT lhs;
43   const Scalar* EIGEN_RESTRICT rhs;
44   int lhsStride, rhsStride, rows, cols;
45   bool lhsRowMajor;
46
47   if (resRowMajor)
48   {
49     lhs = _rhs;
50     rhs = _lhs;
51     lhsStride = _rhsStride;
52     rhsStride = _lhsStride;
53     cols = _rows;
54     rows = _cols;
55     lhsRowMajor = !_rhsRowMajor;
56     ei_assert(_lhsRowMajor);
57   }
58   else
59   {
60     lhs = _lhs;
61     rhs = _rhs;
62     lhsStride = _lhsStride;
63     rhsStride = _rhsStride;
64     rows = _rows;
65     cols = _cols;
66     lhsRowMajor = _lhsRowMajor;
67     ei_assert(!_rhsRowMajor);
68   }
69
70   typedef typename ei_packet_traits<Scalar>::type PacketType;
71
72   enum {
73     PacketSize = sizeof(PacketType)/sizeof(Scalar),
74     #if (defined __i386__)
75     // i386 architecture provides only 8 xmm registers,
76     // so let's reduce the max number of rows processed at once.
77     MaxBlockRows = 4,
78     MaxBlockRows_ClampingMask = 0xFFFFFC,
79     #else
80     MaxBlockRows = 8,
81     MaxBlockRows_ClampingMask = 0xFFFFF8,
82     #endif
83     // maximal size of the blocks fitted in L2 cache
84     MaxL2BlockSize = ei_L2_block_traits<EIGEN_TUNE_FOR_CPU_CACHE_SIZE,Scalar>::width
85   };
86
87   const bool resIsAligned = (PacketSize==1) || (((resStride%PacketSize) == 0) && (size_t(res)%16==0));
88
89   const int remainingSize = depth % PacketSize;
90   const int size = depth - remainingSize; // third dimension of the product clamped to packet boundaries
91   const int l2BlockRows = MaxL2BlockSize > rows ? rows : MaxL2BlockSize;
92   const int l2BlockCols = MaxL2BlockSize > cols ? cols : MaxL2BlockSize;
93   const int l2BlockSize = MaxL2BlockSize > size ? size : MaxL2BlockSize;
94   const int l2BlockSizeAligned = (1 + std::max(l2BlockSize,l2BlockCols)/PacketSize)*PacketSize;
95   const bool needRhsCopy = (PacketSize>1) && ((rhsStride%PacketSize!=0) || (size_t(rhs)%16!=0));
96   Scalar* EIGEN_RESTRICT block = 0;
97   const int allocBlockSize = l2BlockRows*size;
98   block = ei_aligned_stack_new(Scalar, allocBlockSize);
99   Scalar* EIGEN_RESTRICT rhsCopy
100     = ei_aligned_stack_new(Scalar, l2BlockSizeAligned*l2BlockSizeAligned);
101
102   // loops on each L2 cache friendly blocks of the result
103   for(int l2i=0; l2i<rows; l2i+=l2BlockRows)
104   {
105     const int l2blockRowEnd = std::min(l2i+l2BlockRows, rows);
106     const int l2blockRowEndBW = l2blockRowEnd & MaxBlockRows_ClampingMask;    // end of the rows aligned to bw
107     const int l2blockRemainingRows = l2blockRowEnd - l2blockRowEndBW;         // number of remaining rows
108     //const int l2blockRowEndBWPlusOne = l2blockRowEndBW + (l2blockRemainingRows?0:MaxBlockRows);
109
110     // build a cache friendly blocky matrix
111     int count = 0;
112
113     // copy l2blocksize rows of m_lhs to blocks of ps x bw
114     for(int l2k=0; l2k<size; l2k+=l2BlockSize)
115     {
116       const int l2blockSizeEnd = std::min(l2k+l2BlockSize, size);
117
118       for (int i = l2i; i<l2blockRowEndBW/*PlusOne*/; i+=MaxBlockRows)
119       {
120         // TODO merge the "if l2blockRemainingRows" using something like:
121         // const int blockRows = std::min(i+MaxBlockRows, rows) - i;
122
123         for (int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
124         {
125           // TODO write these loops using meta unrolling
126           // negligible for large matrices but useful for small ones
127           if (lhsRowMajor)
128           {
129             for (int w=0; w<MaxBlockRows; ++w)
130               for (int s=0; s<PacketSize; ++s)
131                 block[count++] = lhs[(i+w)*lhsStride + (k+s)];
132           }
133           else
134           {
135             for (int w=0; w<MaxBlockRows; ++w)
136               for (int s=0; s<PacketSize; ++s)
137                 block[count++] = lhs[(i+w) + (k+s)*lhsStride];
138           }
139         }
140       }
141       if (l2blockRemainingRows>0)
142       {
143         for (int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
144         {
145           if (lhsRowMajor)
146           {
147             for (int w=0; w<l2blockRemainingRows; ++w)
148               for (int s=0; s<PacketSize; ++s)
149                 block[count++] = lhs[(l2blockRowEndBW+w)*lhsStride + (k+s)];
150           }
151           else
152           {
153             for (int w=0; w<l2blockRemainingRows; ++w)
154               for (int s=0; s<PacketSize; ++s)
155                 block[count++] = lhs[(l2blockRowEndBW+w) + (k+s)*lhsStride];
156           }
157         }
158       }
159     }
160
161     for(int l2j=0; l2j<cols; l2j+=l2BlockCols)
162     {
163       int l2blockColEnd = std::min(l2j+l2BlockCols, cols);
164
165       for(int l2k=0; l2k<size; l2k+=l2BlockSize)
166       {
167         // acumulate bw rows of lhs time a single column of rhs to a bw x 1 block of res
168         int l2blockSizeEnd = std::min(l2k+l2BlockSize, size);
169
170         // if not aligned, copy the rhs block
171         if (needRhsCopy)
172           for(int l1j=l2j; l1j<l2blockColEnd; l1j+=1)
173           {
174             ei_internal_assert(l2BlockSizeAligned*(l1j-l2j)+(l2blockSizeEnd-l2k) < l2BlockSizeAligned*l2BlockSizeAligned);
175             memcpy(rhsCopy+l2BlockSizeAligned*(l1j-l2j),&(rhs[l1j*rhsStride+l2k]),(l2blockSizeEnd-l2k)*sizeof(Scalar));
176           }
177
178         // for each bw x 1 result's block
179         for(int l1i=l2i; l1i<l2blockRowEndBW; l1i+=MaxBlockRows)
180         {
181           int offsetblock = l2k * (l2blockRowEnd-l2i) + (l1i-l2i)*(l2blockSizeEnd-l2k) - l2k*MaxBlockRows;
182           const Scalar* EIGEN_RESTRICT localB = &block[offsetblock];
183
184           for(int l1j=l2j; l1j<l2blockColEnd; l1j+=1)
185           {
186             const Scalar* EIGEN_RESTRICT rhsColumn;
187             if (needRhsCopy)
188               rhsColumn = &(rhsCopy[l2BlockSizeAligned*(l1j-l2j)-l2k]);
189             else
190               rhsColumn = &(rhs[l1j*rhsStride]);
191
192             PacketType dst[MaxBlockRows];
193             dst[3] = dst[2] = dst[1] = dst[0] = ei_pset1(Scalar(0.));
194             if (MaxBlockRows==8)
195               dst[7] = dst[6] = dst[5] = dst[4] = dst[0];
196
197             PacketType tmp;
198
199             for(int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
200             {
201               tmp = ei_ploadu(&rhsColumn[k]);
202               PacketType A0, A1, A2, A3, A4, A5;
203               A0 = ei_pload(localB + k*MaxBlockRows);
204               A1 = ei_pload(localB + k*MaxBlockRows+1*PacketSize);
205               A2 = ei_pload(localB + k*MaxBlockRows+2*PacketSize);
206               A3 = ei_pload(localB + k*MaxBlockRows+3*PacketSize);
207               if (MaxBlockRows==8) A4 = ei_pload(localB + k*MaxBlockRows+4*PacketSize);
208               if (MaxBlockRows==8) A5 = ei_pload(localB + k*MaxBlockRows+5*PacketSize);
209               dst[0] = ei_pmadd(tmp, A0, dst[0]);
210               if (MaxBlockRows==8) A0 = ei_pload(localB + k*MaxBlockRows+6*PacketSize);
211               dst[1] = ei_pmadd(tmp, A1, dst[1]);
212               if (MaxBlockRows==8) A1 = ei_pload(localB + k*MaxBlockRows+7*PacketSize);
213               dst[2] = ei_pmadd(tmp, A2, dst[2]);
214               dst[3] = ei_pmadd(tmp, A3, dst[3]);
215               if (MaxBlockRows==8)
216               {
217                 dst[4] = ei_pmadd(tmp, A4, dst[4]);
218                 dst[5] = ei_pmadd(tmp, A5, dst[5]);
219                 dst[6] = ei_pmadd(tmp, A0, dst[6]);
220                 dst[7] = ei_pmadd(tmp, A1, dst[7]);
221               }
222             }
223
224             Scalar* EIGEN_RESTRICT localRes = &(res[l1i + l1j*resStride]);
225
226             if (PacketSize>1 && resIsAligned)
227             {
228               // the result is aligned: let's do packet reduction
229               ei_pstore(&(localRes[0]), ei_padd(ei_pload(&(localRes[0])), ei_preduxp(&dst[0])));
230               if (PacketSize==2)
231                 ei_pstore(&(localRes[2]), ei_padd(ei_pload(&(localRes[2])), ei_preduxp(&(dst[2]))));
232               if (MaxBlockRows==8)
233               {
234                 ei_pstore(&(localRes[4]), ei_padd(ei_pload(&(localRes[4])), ei_preduxp(&(dst[4]))));
235                 if (PacketSize==2)
236                   ei_pstore(&(localRes[6]), ei_padd(ei_pload(&(localRes[6])), ei_preduxp(&(dst[6]))));
237               }
238             }
239             else
240             {
241               // not aligned => per coeff packet reduction
242               localRes[0] += ei_predux(dst[0]);
243               localRes[1] += ei_predux(dst[1]);
244               localRes[2] += ei_predux(dst[2]);
245               localRes[3] += ei_predux(dst[3]);
246               if (MaxBlockRows==8)
247               {
248                 localRes[4] += ei_predux(dst[4]);
249                 localRes[5] += ei_predux(dst[5]);
250                 localRes[6] += ei_predux(dst[6]);
251                 localRes[7] += ei_predux(dst[7]);
252               }
253             }
254           }
255         }
256         if (l2blockRemainingRows>0)
257         {
258           int offsetblock = l2k * (l2blockRowEnd-l2i) + (l2blockRowEndBW-l2i)*(l2blockSizeEnd-l2k) - l2k*l2blockRemainingRows;
259           const Scalar* localB = &block[offsetblock];
260
261           for(int l1j=l2j; l1j<l2blockColEnd; l1j+=1)
262           {
263             const Scalar* EIGEN_RESTRICT rhsColumn;
264             if (needRhsCopy)
265               rhsColumn = &(rhsCopy[l2BlockSizeAligned*(l1j-l2j)-l2k]);
266             else
267               rhsColumn = &(rhs[l1j*rhsStride]);
268
269             PacketType dst[MaxBlockRows];
270             dst[3] = dst[2] = dst[1] = dst[0] = ei_pset1(Scalar(0.));
271             if (MaxBlockRows==8)
272               dst[7] = dst[6] = dst[5] = dst[4] = dst[0];
273
274             // let's declare a few other temporary registers
275             PacketType tmp;
276
277             for(int k=l2k; k<l2blockSizeEnd; k+=PacketSize)
278             {
279               tmp = ei_pload(&rhsColumn[k]);
280
281                                            dst[0] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows             ])), dst[0]);
282               if (l2blockRemainingRows>=2) dst[1] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+  PacketSize])), dst[1]);
283               if (l2blockRemainingRows>=3) dst[2] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+2*PacketSize])), dst[2]);
284               if (l2blockRemainingRows>=4) dst[3] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+3*PacketSize])), dst[3]);
285               if (MaxBlockRows==8)
286               {
287                 if (l2blockRemainingRows>=5) dst[4] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+4*PacketSize])), dst[4]);
288                 if (l2blockRemainingRows>=6) dst[5] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+5*PacketSize])), dst[5]);
289                 if (l2blockRemainingRows>=7) dst[6] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+6*PacketSize])), dst[6]);
290                 if (l2blockRemainingRows>=8) dst[7] = ei_pmadd(tmp, ei_pload(&(localB[k*l2blockRemainingRows+7*PacketSize])), dst[7]);
291               }
292             }
293
294             Scalar* EIGEN_RESTRICT localRes = &(res[l2blockRowEndBW + l1j*resStride]);
295
296             // process the remaining rows once at a time
297                                          localRes[0] += ei_predux(dst[0]);
298             if (l2blockRemainingRows>=2) localRes[1] += ei_predux(dst[1]);
299             if (l2blockRemainingRows>=3) localRes[2] += ei_predux(dst[2]);
300             if (l2blockRemainingRows>=4) localRes[3] += ei_predux(dst[3]);
301             if (MaxBlockRows==8)
302             {
303               if (l2blockRemainingRows>=5) localRes[4] += ei_predux(dst[4]);
304               if (l2blockRemainingRows>=6) localRes[5] += ei_predux(dst[5]);
305               if (l2blockRemainingRows>=7) localRes[6] += ei_predux(dst[6]);
306               if (l2blockRemainingRows>=8) localRes[7] += ei_predux(dst[7]);
307             }
308
309           }
310         }
311       }
312     }
313   }
314   if (PacketSize>1 && remainingSize)
315   {
316     if (lhsRowMajor)
317     {
318       for (int j=0; j<cols; ++j)
319         for (int i=0; i<rows; ++i)
320         {
321           Scalar tmp = lhs[i*lhsStride+size] * rhs[j*rhsStride+size];
322           // FIXME this loop get vectorized by the compiler !
323           for (int k=1; k<remainingSize; ++k)
324             tmp += lhs[i*lhsStride+size+k] * rhs[j*rhsStride+size+k];
325           res[i+j*resStride] += tmp;
326         }
327     }
328     else
329     {
330       for (int j=0; j<cols; ++j)
331         for (int i=0; i<rows; ++i)
332         {
333           Scalar tmp = lhs[i+size*lhsStride] * rhs[j*rhsStride+size];
334           for (int k=1; k<remainingSize; ++k)
335             tmp += lhs[i+(size+k)*lhsStride] * rhs[j*rhsStride+size+k];
336           res[i+j*resStride] += tmp;
337         }
338     }
339   }
340
341   ei_aligned_stack_delete(Scalar, block, allocBlockSize);
342   ei_aligned_stack_delete(Scalar, rhsCopy, l2BlockSizeAligned*l2BlockSizeAligned);
343 }
344
345 #endif // EIGEN_EXTERN_INSTANTIATIONS
346
347 /* Optimized col-major matrix * vector product:
348  * This algorithm processes 4 columns at onces that allows to both reduce
349  * the number of load/stores of the result by a factor 4 and to reduce
350  * the instruction dependency. Moreover, we know that all bands have the
351  * same alignment pattern.
352  * TODO: since rhs gets evaluated only once, no need to evaluate it
353  */
354 template<typename Scalar, typename RhsType>
355 static EIGEN_DONT_INLINE void ei_cache_friendly_product_colmajor_times_vector(
356   int size,
357   const Scalar* lhs, int lhsStride,
358   const RhsType& rhs,
359   Scalar* res)
360 {
361   #ifdef _EIGEN_ACCUMULATE_PACKETS
362   #error _EIGEN_ACCUMULATE_PACKETS has already been defined
363   #endif
364   #define _EIGEN_ACCUMULATE_PACKETS(A0,A13,A2) \
365     ei_pstore(&res[j], \
366       ei_padd(ei_pload(&res[j]), \
367         ei_padd( \
368           ei_padd(ei_pmul(ptmp0,EIGEN_CAT(ei_ploa , A0)(&lhs0[j])), \
369                   ei_pmul(ptmp1,EIGEN_CAT(ei_ploa , A13)(&lhs1[j]))), \
370           ei_padd(ei_pmul(ptmp2,EIGEN_CAT(ei_ploa , A2)(&lhs2[j])), \
371                   ei_pmul(ptmp3,EIGEN_CAT(ei_ploa , A13)(&lhs3[j]))) )))
372
373   typedef typename ei_packet_traits<Scalar>::type Packet;
374   const int PacketSize = sizeof(Packet)/sizeof(Scalar);
375
376   enum { AllAligned = 0, EvenAligned, FirstAligned, NoneAligned };
377   const int columnsAtOnce = 4;
378   const int peels = 2;
379   const int PacketAlignedMask = PacketSize-1;
380   const int PeelAlignedMask = PacketSize*peels-1;
381
382   // How many coeffs of the result do we have to skip to be aligned.
383   // Here we assume data are at least aligned on the base scalar type that is mandatory anyway.
384   const int alignedStart = ei_alignmentOffset(res,size);
385   const int alignedSize = PacketSize>1 ? alignedStart + ((size-alignedStart) & ~PacketAlignedMask) : 0;
386   const int peeledSize  = peels>1 ? alignedStart + ((alignedSize-alignedStart) & ~PeelAlignedMask) : alignedStart;
387
388   const int alignmentStep = PacketSize>1 ? (PacketSize - lhsStride % PacketSize) & PacketAlignedMask : 0;
389   int alignmentPattern = alignmentStep==0 ? AllAligned
390                        : alignmentStep==(PacketSize/2) ? EvenAligned
391                        : FirstAligned;
392
393   // we cannot assume the first element is aligned because of sub-matrices
394   const int lhsAlignmentOffset = ei_alignmentOffset(lhs,size);
395
396   // find how many columns do we have to skip to be aligned with the result (if possible)
397   int skipColumns = 0;
398   if (PacketSize>1)
399   {
400     ei_internal_assert(size_t(lhs+lhsAlignmentOffset)%sizeof(Packet)==0 || size<PacketSize);
401
402     while (skipColumns<PacketSize &&
403            alignedStart != ((lhsAlignmentOffset + alignmentStep*skipColumns)%PacketSize))
404       ++skipColumns;
405     if (skipColumns==PacketSize)
406     {
407       // nothing can be aligned, no need to skip any column
408       alignmentPattern = NoneAligned;
409       skipColumns = 0;
410     }
411     else
412     {
413       skipColumns = std::min(skipColumns,rhs.size());
414       // note that the skiped columns are processed later.
415     }
416
417     ei_internal_assert((alignmentPattern==NoneAligned) || (size_t(lhs+alignedStart+lhsStride*skipColumns)%sizeof(Packet))==0);
418   }
419
420   int offset1 = (FirstAligned && alignmentStep==1?3:1);
421   int offset3 = (FirstAligned && alignmentStep==1?1:3);
422
423   int columnBound = ((rhs.size()-skipColumns)/columnsAtOnce)*columnsAtOnce + skipColumns;
424   for (int i=skipColumns; i<columnBound; i+=columnsAtOnce)
425   {
426     Packet ptmp0 = ei_pset1(rhs[i]),   ptmp1 = ei_pset1(rhs[i+offset1]),
427            ptmp2 = ei_pset1(rhs[i+2]), ptmp3 = ei_pset1(rhs[i+offset3]);
428
429     // this helps a lot generating better binary code
430     const Scalar *lhs0 = lhs + i*lhsStride, *lhs1 = lhs + (i+offset1)*lhsStride,
431                  *lhs2 = lhs + (i+2)*lhsStride, *lhs3 = lhs + (i+offset3)*lhsStride;
432
433     if (PacketSize>1)
434     {
435       /* explicit vectorization */
436       // process initial unaligned coeffs
437       for (int j=0; j<alignedStart; ++j)
438         res[j] += ei_pfirst(ptmp0)*lhs0[j] + ei_pfirst(ptmp1)*lhs1[j] + ei_pfirst(ptmp2)*lhs2[j] + ei_pfirst(ptmp3)*lhs3[j];
439
440       if (alignedSize>alignedStart)
441       {
442         switch(alignmentPattern)
443         {
444           case AllAligned:
445             for (int j = alignedStart; j<alignedSize; j+=PacketSize)
446               _EIGEN_ACCUMULATE_PACKETS(d,d,d);
447             break;
448           case EvenAligned:
449             for (int j = alignedStart; j<alignedSize; j+=PacketSize)
450               _EIGEN_ACCUMULATE_PACKETS(d,du,d);
451             break;
452           case FirstAligned:
453             if(peels>1)
454             {
455               Packet A00, A01, A02, A03, A10, A11, A12, A13;
456
457               A01 = ei_pload(&lhs1[alignedStart-1]);
458               A02 = ei_pload(&lhs2[alignedStart-2]);
459               A03 = ei_pload(&lhs3[alignedStart-3]);
460
461               for (int j = alignedStart; j<peeledSize; j+=peels*PacketSize)
462               {
463                 A11 = ei_pload(&lhs1[j-1+PacketSize]);  ei_palign<1>(A01,A11);
464                 A12 = ei_pload(&lhs2[j-2+PacketSize]);  ei_palign<2>(A02,A12);
465                 A13 = ei_pload(&lhs3[j-3+PacketSize]);  ei_palign<3>(A03,A13);
466
467                 A00 = ei_pload (&lhs0[j]);
468                 A10 = ei_pload (&lhs0[j+PacketSize]);
469                 A00 = ei_pmadd(ptmp0, A00, ei_pload(&res[j]));
470                 A10 = ei_pmadd(ptmp0, A10, ei_pload(&res[j+PacketSize]));
471
472                 A00 = ei_pmadd(ptmp1, A01, A00);
473                 A01 = ei_pload(&lhs1[j-1+2*PacketSize]);  ei_palign<1>(A11,A01);
474                 A00 = ei_pmadd(ptmp2, A02, A00);
475                 A02 = ei_pload(&lhs2[j-2+2*PacketSize]);  ei_palign<2>(A12,A02);
476                 A00 = ei_pmadd(ptmp3, A03, A00);
477                 ei_pstore(&res[j],A00);
478                 A03 = ei_pload(&lhs3[j-3+2*PacketSize]);  ei_palign<3>(A13,A03);
479                 A10 = ei_pmadd(ptmp1, A11, A10);
480                 A10 = ei_pmadd(ptmp2, A12, A10);
481                 A10 = ei_pmadd(ptmp3, A13, A10);
482                 ei_pstore(&res[j+PacketSize],A10);
483               }
484             }
485             for (int j = peeledSize; j<alignedSize; j+=PacketSize)
486               _EIGEN_ACCUMULATE_PACKETS(d,du,du);
487             break;
488           default:
489             for (int j = alignedStart; j<alignedSize; j+=PacketSize)
490               _EIGEN_ACCUMULATE_PACKETS(du,du,du);
491             break;
492         }
493       }
494     } // end explicit vectorization
495
496     /* process remaining coeffs (or all if there is no explicit vectorization) */
497     for (int j=alignedSize; j<size; ++j)
498       res[j] += ei_pfirst(ptmp0)*lhs0[j] + ei_pfirst(ptmp1)*lhs1[j] + ei_pfirst(ptmp2)*lhs2[j] + ei_pfirst(ptmp3)*lhs3[j];
499   }
500
501   // process remaining first and last columns (at most columnsAtOnce-1)
502   int end = rhs.size();
503   int start = columnBound;
504   do
505   {
506     for (int i=start; i<end; ++i)
507     {
508       Packet ptmp0 = ei_pset1(rhs[i]);
509       const Scalar* lhs0 = lhs + i*lhsStride;
510
511       if (PacketSize>1)
512       {
513         /* explicit vectorization */
514         // process first unaligned result's coeffs
515         for (int j=0; j<alignedStart; ++j)
516           res[j] += ei_pfirst(ptmp0) * lhs0[j];
517
518         // process aligned result's coeffs
519         if ((size_t(lhs0+alignedStart)%sizeof(Packet))==0)
520           for (int j = alignedStart;j<alignedSize;j+=PacketSize)
521             ei_pstore(&res[j], ei_pmadd(ptmp0,ei_pload(&lhs0[j]),ei_pload(&res[j])));
522         else
523           for (int j = alignedStart;j<alignedSize;j+=PacketSize)
524             ei_pstore(&res[j], ei_pmadd(ptmp0,ei_ploadu(&lhs0[j]),ei_pload(&res[j])));
525       }
526
527       // process remaining scalars (or all if no explicit vectorization)
528       for (int j=alignedSize; j<size; ++j)
529         res[j] += ei_pfirst(ptmp0) * lhs0[j];
530     }
531     if (skipColumns)
532     {
533       start = 0;
534       end = skipColumns;
535       skipColumns = 0;
536     }
537     else
538       break;
539   } while(PacketSize>1);
540   #undef _EIGEN_ACCUMULATE_PACKETS
541 }
542
543 // TODO add peeling to mask unaligned load/stores
544 template<typename Scalar, typename ResType>
545 static EIGEN_DONT_INLINE void ei_cache_friendly_product_rowmajor_times_vector(
546   const Scalar* lhs, int lhsStride,
547   const Scalar* rhs, int rhsSize,
548   ResType& res)
549 {
550   #ifdef _EIGEN_ACCUMULATE_PACKETS
551   #error _EIGEN_ACCUMULATE_PACKETS has already been defined
552   #endif
553
554   #define _EIGEN_ACCUMULATE_PACKETS(A0,A13,A2) {\
555     Packet b = ei_pload(&rhs[j]); \
556     ptmp0 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A0) (&lhs0[j]), ptmp0); \
557     ptmp1 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A13)(&lhs1[j]), ptmp1); \
558     ptmp2 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A2) (&lhs2[j]), ptmp2); \
559     ptmp3 = ei_pmadd(b, EIGEN_CAT(ei_ploa,A13)(&lhs3[j]), ptmp3); }
560
561   typedef typename ei_packet_traits<Scalar>::type Packet;
562   const int PacketSize = sizeof(Packet)/sizeof(Scalar);
563
564   enum { AllAligned=0, EvenAligned=1, FirstAligned=2, NoneAligned=3 };
565   const int rowsAtOnce = 4;
566   const int peels = 2;
567   const int PacketAlignedMask = PacketSize-1;
568   const int PeelAlignedMask = PacketSize*peels-1;
569   const int size = rhsSize;
570
571   // How many coeffs of the result do we have to skip to be aligned.
572   // Here we assume data are at least aligned on the base scalar type that is mandatory anyway.
573   const int alignedStart = ei_alignmentOffset(rhs, size);
574   const int alignedSize = PacketSize>1 ? alignedStart + ((size-alignedStart) & ~PacketAlignedMask) : 0;
575   const int peeledSize  = peels>1 ? alignedStart + ((alignedSize-alignedStart) & ~PeelAlignedMask) : alignedStart;
576
577   const int alignmentStep = PacketSize>1 ? (PacketSize - lhsStride % PacketSize) & PacketAlignedMask : 0;
578   int alignmentPattern = alignmentStep==0 ? AllAligned
579                        : alignmentStep==(PacketSize/2) ? EvenAligned
580                        : FirstAligned;
581
582   // we cannot assume the first element is aligned because of sub-matrices
583   const int lhsAlignmentOffset = ei_alignmentOffset(lhs,size);
584
585   // find how many rows do we have to skip to be aligned with rhs (if possible)
586   int skipRows = 0;
587   if (PacketSize>1)
588   {
589     ei_internal_assert(size_t(lhs+lhsAlignmentOffset)%sizeof(Packet)==0  || size<PacketSize);
590
591     while (skipRows<PacketSize &&
592            alignedStart != ((lhsAlignmentOffset + alignmentStep*skipRows)%PacketSize))
593       ++skipRows;
594     if (skipRows==PacketSize)
595     {
596       // nothing can be aligned, no need to skip any column
597       alignmentPattern = NoneAligned;
598       skipRows = 0;
599     }
600     else
601     {
602       skipRows = std::min(skipRows,res.size());
603       // note that the skiped columns are processed later.
604     }
605     ei_internal_assert((alignmentPattern==NoneAligned) || PacketSize==1
606       || (size_t(lhs+alignedStart+lhsStride*skipRows)%sizeof(Packet))==0);
607   }
608
609   int offset1 = (FirstAligned && alignmentStep==1?3:1);
610   int offset3 = (FirstAligned && alignmentStep==1?1:3);
611
612   int rowBound = ((res.size()-skipRows)/rowsAtOnce)*rowsAtOnce + skipRows;
613   for (int i=skipRows; i<rowBound; i+=rowsAtOnce)
614   {
615     Scalar tmp0 = Scalar(0), tmp1 = Scalar(0), tmp2 = Scalar(0), tmp3 = Scalar(0);
616
617     // this helps the compiler generating good binary code
618     const Scalar *lhs0 = lhs + i*lhsStride,     *lhs1 = lhs + (i+offset1)*lhsStride,
619                  *lhs2 = lhs + (i+2)*lhsStride, *lhs3 = lhs + (i+offset3)*lhsStride;
620
621     if (PacketSize>1)
622     {
623       /* explicit vectorization */
624       Packet ptmp0 = ei_pset1(Scalar(0)), ptmp1 = ei_pset1(Scalar(0)), ptmp2 = ei_pset1(Scalar(0)), ptmp3 = ei_pset1(Scalar(0));
625
626       // process initial unaligned coeffs
627       // FIXME this loop get vectorized by the compiler !
628       for (int j=0; j<alignedStart; ++j)
629       {
630         Scalar b = rhs[j];
631         tmp0 += b*lhs0[j]; tmp1 += b*lhs1[j]; tmp2 += b*lhs2[j]; tmp3 += b*lhs3[j];
632       }
633
634       if (alignedSize>alignedStart)
635       {
636         switch(alignmentPattern)
637         {
638           case AllAligned:
639             for (int j = alignedStart; j<alignedSize; j+=PacketSize)
640               _EIGEN_ACCUMULATE_PACKETS(d,d,d);
641             break;
642           case EvenAligned:
643             for (int j = alignedStart; j<alignedSize; j+=PacketSize)
644               _EIGEN_ACCUMULATE_PACKETS(d,du,d);
645             break;
646           case FirstAligned:
647             if (peels>1)
648             {
649               /* Here we proccess 4 rows with with two peeled iterations to hide
650                * tghe overhead of unaligned loads. Moreover unaligned loads are handled
651                * using special shift/move operations between the two aligned packets
652                * overlaping the desired unaligned packet. This is *much* more efficient
653                * than basic unaligned loads.
654                */
655               Packet A01, A02, A03, b, A11, A12, A13;
656               A01 = ei_pload(&lhs1[alignedStart-1]);
657               A02 = ei_pload(&lhs2[alignedStart-2]);
658               A03 = ei_pload(&lhs3[alignedStart-3]);
659
660               for (int j = alignedStart; j<peeledSize; j+=peels*PacketSize)
661               {
662                 b = ei_pload(&rhs[j]);
663                 A11 = ei_pload(&lhs1[j-1+PacketSize]);  ei_palign<1>(A01,A11);
664                 A12 = ei_pload(&lhs2[j-2+PacketSize]);  ei_palign<2>(A02,A12);
665                 A13 = ei_pload(&lhs3[j-3+PacketSize]);  ei_palign<3>(A03,A13);
666
667                 ptmp0 = ei_pmadd(b, ei_pload (&lhs0[j]), ptmp0);
668                 ptmp1 = ei_pmadd(b, A01, ptmp1);
669                 A01 = ei_pload(&lhs1[j-1+2*PacketSize]);  ei_palign<1>(A11,A01);
670                 ptmp2 = ei_pmadd(b, A02, ptmp2);
671                 A02 = ei_pload(&lhs2[j-2+2*PacketSize]);  ei_palign<2>(A12,A02);
672                 ptmp3 = ei_pmadd(b, A03, ptmp3);
673                 A03 = ei_pload(&lhs3[j-3+2*PacketSize]);  ei_palign<3>(A13,A03);
674
675                 b = ei_pload(&rhs[j+PacketSize]);
676                 ptmp0 = ei_pmadd(b, ei_pload (&lhs0[j+PacketSize]), ptmp0);
677                 ptmp1 = ei_pmadd(b, A11, ptmp1);
678                 ptmp2 = ei_pmadd(b, A12, ptmp2);
679                 ptmp3 = ei_pmadd(b, A13, ptmp3);
680               }
681             }
682             for (int j = peeledSize; j<alignedSize; j+=PacketSize)
683               _EIGEN_ACCUMULATE_PACKETS(d,du,du);
684             break;
685           default:
686             for (int j = alignedStart; j<alignedSize; j+=PacketSize)
687               _EIGEN_ACCUMULATE_PACKETS(du,du,du);
688             break;
689         }
690         tmp0 += ei_predux(ptmp0);
691         tmp1 += ei_predux(ptmp1);
692         tmp2 += ei_predux(ptmp2);
693         tmp3 += ei_predux(ptmp3);
694       }
695     } // end explicit vectorization
696
697     // process remaining coeffs (or all if no explicit vectorization)
698     // FIXME this loop get vectorized by the compiler !
699     for (int j=alignedSize; j<size; ++j)
700     {
701       Scalar b = rhs[j];
702       tmp0 += b*lhs0[j]; tmp1 += b*lhs1[j]; tmp2 += b*lhs2[j]; tmp3 += b*lhs3[j];
703     }
704     res[i] += tmp0; res[i+offset1] += tmp1; res[i+2] += tmp2; res[i+offset3] += tmp3;
705   }
706
707   // process remaining first and last rows (at most columnsAtOnce-1)
708   int end = res.size();
709   int start = rowBound;
710   do
711   {
712     for (int i=start; i<end; ++i)
713     {
714       Scalar tmp0 = Scalar(0);
715       Packet ptmp0 = ei_pset1(tmp0);
716       const Scalar* lhs0 = lhs + i*lhsStride;
717       // process first unaligned result's coeffs
718       // FIXME this loop get vectorized by the compiler !
719       for (int j=0; j<alignedStart; ++j)
720         tmp0 += rhs[j] * lhs0[j];
721
722       if (alignedSize>alignedStart)
723       {
724         // process aligned rhs coeffs
725         if ((size_t(lhs0+alignedStart)%sizeof(Packet))==0)
726           for (int j = alignedStart;j<alignedSize;j+=PacketSize)
727             ptmp0 = ei_pmadd(ei_pload(&rhs[j]), ei_pload(&lhs0[j]), ptmp0);
728         else
729           for (int j = alignedStart;j<alignedSize;j+=PacketSize)
730             ptmp0 = ei_pmadd(ei_pload(&rhs[j]), ei_ploadu(&lhs0[j]), ptmp0);
731         tmp0 += ei_predux(ptmp0);
732       }
733
734       // process remaining scalars
735       // FIXME this loop get vectorized by the compiler !
736       for (int j=alignedSize; j<size; ++j)
737         tmp0 += rhs[j] * lhs0[j];
738       res[i] += tmp0;
739     }
740     if (skipRows)
741     {
742       start = 0;
743       end = skipRows;
744       skipRows = 0;
745     }
746     else
747       break;
748   } while(PacketSize>1);
749
750   #undef _EIGEN_ACCUMULATE_PACKETS
751 }
752
753 #endif // EIGEN_CACHE_FRIENDLY_PRODUCT_H