arm_mat_mult_fast_q31.c

Go to the documentation of this file.

/* ----------------------------------------------------------------------      
* Copyright (C) 2011 ARM Limited. All rights reserved. 
*      
* $Date:        15. December 2011   
* $Revision:    V2.0.0  
*      
* Project:      Cortex-R DSP Library 
* Title:        arm_mat_mult_fast_q31.c      
*      
* Description:   Q31 matrix multiplication (fast variant).      
*      
* Target Processor:          Cortex-R4/R5
*
* Version 1.0.0 2011/03/08
*     Alpha release.
*
* Version 1.0.1 2011/09/30
*     Beta release.
*
* Version 2.0.0 2011/12/15
*     Final release. 
* 
* -------------------------------------------------------------------- */     
#include "arm_math.h"     
     
arm_status arm_mat_mult_fast_q31(     
  const arm_matrix_instance_q31 * pSrcA,     
  const arm_matrix_instance_q31 * pSrcB,     
  arm_matrix_instance_q31 * pDst)     
{     
  q31_t *pIn1 = pSrcA->pData;                    /* input data matrix pointer A */     
  q31_t *pIn2 = pSrcB->pData;                    /* input data matrix pointer B */     
  q31_t *pInA = pSrcA->pData;                    /* input data matrix pointer A */     
//  q31_t *pSrcB = pSrcB->pData;                    /* input data matrix pointer B */      
  q31_t *pOut = pDst->pData;                     /* output data matrix pointer */     
  q31_t *px;                                     /* Temporary output data matrix pointer */     
  q31_t sum;                                     /* Accumulator */     
  uint16_t numRowsA = pSrcA->numRows;            /* number of rows of input matrix A    */     
  uint16_t numColsB = pSrcB->numCols;            /* number of columns of input matrix B */     
  uint16_t numColsA = pSrcA->numCols;            /* number of columns of input matrix A */     
  uint16_t col, i = 0u, j, row = numRowsA, colCnt;      /* loop counters */     
  arm_status status;                             /* status of matrix multiplication */     
  q31_t inA1, inA2, inA3, inA4, inB1, inB2, inB3, inB4;   
     
#ifdef ARM_MATH_MATRIX_CHECK     
  /* Check for matrix mismatch condition */     
  if((pSrcA->numCols != pSrcB->numRows) ||     
     (pSrcA->numRows != pDst->numRows) || (pSrcB->numCols != pDst->numCols))     
  {     
    /* Set status as ARM_MATH_SIZE_MISMATCH */     
    status = ARM_MATH_SIZE_MISMATCH;     
  }     
  else     
#endif     
  {     
    /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */     
    /* row loop */     
    do     
    {     
      /* Output pointer is set to starting address of the row being processed */     
      px = pOut + i;     
     
      /* For every row wise process, the column loop counter is to be initiated */     
      col = numColsB;     
     
      /* For every row wise process, the pIn2 pointer is set      
       ** to the starting address of the pSrcB data */     
      pIn2 = pSrcB->pData;     
     
      j = 0u;     
     
      /* column loop */     
      do     
      {     
        /* Set the variable sum, that acts as accumulator, to zero */     
        sum = 0;     
     
        /* Initiate the pointer pIn1 to point to the starting address of pInA */     
        pIn1 = pInA;     
     
        /* Apply loop unrolling and compute 4 MACs simultaneously. */     
        colCnt = numColsA >> 2;     
     
     
        /* matrix multiplication */     
        while(colCnt > 0u)     
        {     
          /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */     
          /* Perform the multiply-accumulates */     
          inB1 = *pIn2;   
          pIn2 += numColsB;   
   
          inA1 = pIn1[0];   
          inA2 = pIn1[1];   
   
          inB2 = *pIn2;   
          pIn2 += numColsB;   
   
          inB3 = *pIn2;   
          pIn2 += numColsB;   
   
          sum = (q31_t) ((((q63_t) sum << 32) +     
                          ((q63_t) inA1 * inB1)) >> 32);     
          sum = (q31_t) ((((q63_t) sum << 32) +     
                          ((q63_t) inA2 * inB2)) >> 32);     
                                       
          inA3 = pIn1[2];   
          inA4 = pIn1[3];   
   
          inB4 = *pIn2;   
          pIn2 += numColsB;   
   
          sum = (q31_t) ((((q63_t) sum << 32) +     
                          ((q63_t) inA3 * inB3)) >> 32);     
          sum = (q31_t) ((((q63_t) sum << 32) +     
                          ((q63_t) inA4 * inB4)) >> 32);     
  
          pIn1 += 4u;  
   
          /* Decrement the loop counter */     
          colCnt--;     
        }     
     
        /* If the columns of pSrcA is not a multiple of 4, compute any remaining output samples here.      
         ** No loop unrolling is used. */     
        colCnt = numColsA % 0x4u;     
     
        while(colCnt > 0u)     
        {     
          /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */     
          inA1 = *pIn2;  
          inB1 = *pIn1++;  
          pIn2 += numColsB;  
          /* Perform the multiply-accumulates */     
          sum = (q31_t) ((((q63_t) sum << 32) +     
                          ((q63_t)inA1 * inB1)) >> 32);     
     
          /* Decrement the loop counter */     
          colCnt--;     
        }     
     
        /* Convert the result from 2.30 to 1.31 format and store in destination buffer */     
        *px++ = sum << 1;     
     
        /* Update the pointer pIn2 to point to the  starting address of the next column */     
        j++;     
        pIn2 = pSrcB->pData + j;     
     
        /* Decrement the column loop counter */     
        col--;     
     
      } while(col > 0u);     
     
      /* Update the pointer pInA to point to the  starting address of the next row */     
      i = i + numColsB;     
      pInA = pInA + numColsA;     
     
      /* Decrement the row loop counter */     
      row--;     
     
    } while(row > 0u);     
     
    /* set status as ARM_MATH_SUCCESS */     
    status = ARM_MATH_SUCCESS;     
  }     
  /* Return to application */     
  return (status);     
}