Initial code package commit.
[apps/c55x-digital-mic-decimation.git] / src / diggain.c
1 /* ============================================================================
2  * Copyright (c) 2016 Texas Instruments Incorporated.
3  *
4  *  Redistribution and use in source and binary forms, with or without
5  *  modification, are permitted provided that the following conditions
6  *  are met:
7  *
8  *    Redistributions of source code must retain the above copyright
9  *    notice, this list of conditions and the following disclaimer.
10  *
11  *    Redistributions in binary form must reproduce the above copyright
12  *    notice, this list of conditions and the following disclaimer in the
13  *    documentation and/or other materials provided with the
14  *    distribution.
15  *
16  *    Neither the name of Texas Instruments Incorporated nor the names of
17  *    its contributors may be used to endorse or promote products derived
18  *    from this software without specific prior written permission.
19  *
20  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23  *  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
24  *  OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
25  *  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
26  *  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
27  *  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
28  *  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
29  *  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30  *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  *
32   ===============================================================================*/
34 #include "data_types.h"
35 #include "diggain.h"
37 #define QUANT_TRUNC         ( 0 )   /* truncate */
38 #define QUANT_RND_INF       ( 1 )   /* round to infinite */
39 #define QUANT_MODE          ( QUANT_RND_INF )
41 /* Applies digital gain and saturates output. */
42 /* S18Q16 input data, S16Q15 output data. */
43 /* U16Q8 digital gain. */
44 void appDiggain(
45     Int32   *inSamps,   /* input samples (S18Q16) */
46     Uint16  diggain,    /* digital gain (U16Q8) */
47     Int16   *outSamps,  /* output samples (S16Q15) */
48     Uint16  numInSamps  /* number of input samples */
49 )
50 {
51     Uint16 dataL;
52     Int16 dataH;
53     Uint32 prdLL;
54     Int32 prdLH;
55     Int64 acc0_40b;
56     Uint16 i;
57     
58     for (i = 0; i < numInSamps; i++)
59     {
60         /* S18Q16 */
61         dataL = (Uint16)inSamps[i];
62         dataH = (Int16)(inSamps[i] >> 16);
64         /* S18Q16 * U16Q8 = S34Q24 */
65         prdLL = (Uint32)dataL * diggain;
66         prdLH = (Int32)dataH * (Uint32)diggain;
68         /* S40Q24 + S34Q24 = S40Q24 */
69         acc0_40b = (Uint64)prdLL;
70         acc0_40b += (Int64)(prdLH << 16);
72 #if (QUANT_MODE == QUANT_RND_INF)
73         acc0_40b += (Uint16)1<<8; /* round to infinite */
74 #endif
75         acc0_40b >>= 9; /* truncate */
77 #ifdef _WIN32
78         /* Saturate output */
79         if (acc0_40b > (Int64)0x7FFF)
80         {
81             acc0_40b = (Int64)0x7FFF;
82         }
83         else if (acc0_40b < (Int64)0xFFFFFFFFFFFF8000)
84         {
85             acc0_40b = (Int64)0xFFFFFFFFFFFF8000;
86         }
87 #else
88         /* Saturate output */
89         if (acc0_40b > (Int64)0x7FFF)
90         {
91             acc0_40b = (Int64)0x7FFF;
92         }
93         else if (acc0_40b < (Int64)0xFFFFFF8000)
94         {
95             acc0_40b = (Int64)0xFFFFFF8000;
96         }
98 #endif        
100         outSamps[i] = (Int16)acc0_40b; /* S16Q15 */
101     }