arm_mat_mult_q15.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_q15.c      
*      
* Description:   Q15 matrix multiplication.      
*      
* 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_q15(     
  const arm_matrix_instance_q15 * pSrcA,     
  const arm_matrix_instance_q15 * pSrcB,     
  arm_matrix_instance_q15 * pDst,     
  q15_t *pScratch)     
{     
  q63_t sum;                                     /* accumulator */     
  q31_t in;                                      /* Temporary variable to hold the input value */     
  q15_t *pSrcBT = pScratch;                        /* input data matrix pointer for transpose */     
  q15_t *pInA = pSrcA->pData;                    /* input data matrix pointer A of Q15 type */     
  q15_t *pInB = pSrcB->pData;                    /* input data matrix pointer B of Q15 type */     
  q15_t *px;                                     /* Temporary output data matrix pointer */     
  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 numRowsB = pSrcB->numRows;            /* number of rows of input matrix A    */     
  uint16_t col, i = 0u, row = numRowsB, colCnt;  /* loop counters */     
  arm_status status;                             /* status of matrix multiplication */     
  
#ifdef UNALIGNED_SUPPORT_DISABLE  
  
  q15_t inA1, inB1, inA2, inB2;  
     
#else  
  
  q31_t pSourceA1, pSourceB1, pSourceA2, pSourceB2;   
  
#endif  
  
#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     
  {     
    /* Matrix transpose */     
    do     
    {     
      /* Apply loop unrolling and exchange the columns with row elements */     
      col = numColsB >> 2;     
     
      /* The pointer px is set to starting address of the column being processed */     
      px = pSrcBT + i;     
     
      /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.      
       ** a second loop below computes the remaining 1 to 3 samples. */     
      while(col > 0u)     
      {     
        /* Read two elements from the row */     
        in = *__SIMD32(pInB)++;     
     
        /* Unpack and store one element in the destination */     
#ifndef ARM_MATH_BIG_ENDIAN   
   
        *px = (q15_t) in;   
   
#else   
   
        *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16);   
   
#endif /*    #ifndef ARM_MATH_BIG_ENDIAN    */   
   
        /* Update the pointer px to point to the next row of the transposed matrix */     
        px += numRowsB;     
     
        /* Unpack and store the second element in the destination */     
#ifndef ARM_MATH_BIG_ENDIAN   
   
        *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16);   
   
#else   
   
        *px = (q15_t) in;   
   
#endif /*    #ifndef ARM_MATH_BIG_ENDIAN    */   
   
        /* Update the pointer px to point to the next row of the transposed matrix */     
        px += numRowsB;     
     
        /* Read two elements from the row */     
        in = *__SIMD32(pInB)++;     
     
        /* Unpack and store one element in the destination */     
#ifndef ARM_MATH_BIG_ENDIAN   
   
        *px = (q15_t) in;   
   
#else   
   
        *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16);   
   
#endif /*    #ifndef ARM_MATH_BIG_ENDIAN    */   
   
        /* Update the pointer px to point to the next row of the transposed matrix */     
        px += numRowsB;     
     
        /* Unpack and store the second element in the destination */     
   
#ifndef ARM_MATH_BIG_ENDIAN   
   
        *px = (q15_t) ((in & (q31_t) 0xffff0000) >> 16);   
   
#else   
   
        *px = (q15_t) in;   
   
#endif /*    #ifndef ARM_MATH_BIG_ENDIAN    */   
   
        /* Update the pointer px to point to the next row of the transposed matrix */     
        px += numRowsB;     
     
        /* Decrement the column loop counter */     
        col--;     
      }     
     
      /* If the columns of pSrcB is not a multiple of 4, compute any remaining output samples here.      
       ** No loop unrolling is used. */     
      col = numColsB % 0x4u;     
     
      while(col > 0u)     
      {     
        /* Read and store the input element in the destination */     
        *px = *pInB++;     
     
        /* Update the pointer px to point to the next row of the transposed matrix */     
        px += numRowsB;     
     
        /* Decrement the column loop counter */     
        col--;     
      }     
     
      i++;     
     
      /* Decrement the row loop counter */     
      row--;     
     
    } while(row > 0u);     
     
    /* Reset the variables for the usage in the following multiplication process */     
    row = numRowsA;     
    i = 0u;     
    px = pDst->pData;     
     
    /* The following loop performs the dot-product of each row in pSrcA with each column in pSrcB */     
    /* row loop */     
    do     
    {     
      /* 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 transposed pSrcB data */     
      pInB = pSrcBT;     
     
      /* column loop */     
      do     
      {     
        /* Set the variable sum, that acts as accumulator, to zero */     
        sum = 0;     
     
        /* Apply loop unrolling and compute 2 MACs simultaneously. */     
        colCnt = numColsA >> 2;     
     
        /* Initiate the pointer pIn1 to point to the starting address of the column being processed */     
        pInA = pSrcA->pData + i;   
   
           
        /* 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) */    
#ifdef UNALIGNED_SUPPORT_DISABLE  
  
          /* read real and imag values from pSrcA and pSrcB buffer */ 
          inA1 = *pInA++;  
          inB1 = *pInB++;  
          inA2 = *pInA++;  
          /* Multiply and Accumlates */ 
          sum += inA1 * inB1;  
          inB2 = *pInB++;  
  
          inA1 = *pInA++;  
          inB1 = *pInB++;  
          /* Multiply and Accumlates */ 
          sum += inA2 * inB2;  
          inA2 = *pInA++;  
          inB2 = *pInB++;  
  
          /* Multiply and Accumlates */ 
          sum += inA1 * inB1;  
          sum += inA2 * inB2;  
  
#else  
          /* read real and imag values from pSrcA and pSrcB buffer */ 
          pSourceA1 = *__SIMD32(pInA)++;       
          pSourceB1 = *__SIMD32(pInB)++;    
   
          pSourceA2 = *__SIMD32(pInA)++;       
          pSourceB2 = *__SIMD32(pInB)++;    
   
          /* Multiply and Accumlates */        
          sum = __SMLALD(pSourceA1, pSourceB1, sum);     
          sum = __SMLALD(pSourceA2, pSourceB2, sum);     
  
#endif      //  #ifdef UNALIGNED_SUPPORT_ENABLE  
  
          /* Decrement the loop counter */     
          colCnt--;     
        }     
   
        /* process remaining column samples */     
        colCnt = numColsA & 3u;   
            
        while(colCnt > 0u)     
        {   
          /* c(m,n) = a(1,1)*b(1,1) + a(1,2) * b(2,1) + .... + a(m,p)*b(p,n) */     
          sum += *pInA++ * *pInB++;  
             
          /* Decrement the loop counter */     
          colCnt--;     
       }     
     
        /* Saturate and store the result in the destination buffer */     
#ifdef CCS   
        *px = (q15_t) (__SSATA(sum, 15, 16));     
#else   
        *px = (q15_t) (__SSAT((sum >> 15), 16));     
#endif   
        px++;     
     
        /* Decrement the column loop counter */     
        col--;     
     
      } while(col > 0u);     
     
      i = i + 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);     
}