Merge pull request #2 in PROCESSOR-SDK/audio-preprocessing-fw from omapl137_cmb_rt...
[processor-sdk/audio-preprocessing.git] / common / components / mss / src / mssproc.c
1 /**\r
2  *  @file   mssproc.c\r
3  *  @brief  Contains Main Processing Routines for Multi-Source Selection \r
4  *          (MSS) module.\r
5  *\r
6  * Copyright (c) 2016, Texas Instruments Incorporated
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * *  Redistributions of source code must retain the above copyright
14  *    notice, this list of conditions and the following disclaimer.
15  *
16  * *  Redistributions in binary form must reproduce the above copyright
17  *    notice, this list of conditions and the following disclaimer in the
18  *    documentation and/or other materials provided with the distribution.
19  *
20  * *  Neither the name of Texas Instruments Incorporated nor the names of
21  *    its contributors may be used to endorse or promote products derived
22  *    from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
25  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
26  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
27  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
28  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
29  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
30  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
31  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
32  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
33  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
34  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
35  *
36  */
37 \r
38 \r
39 #include <xdc/std.h>\r
40 #include <types.h>\r
41 #include <ti/mas/fract/fract.h>\r
42 #include <ti/mas/util/utl.h>\r
43 #include <ti/mas/vpe/svd.h>\r
44 #include "string.h"\r
45 \r
46 //#include <ti/mas/aer/mss.h>\r
47 //#include <ti/mas/aer/src/mssloc.h>\r
48 \r
49 #include "../mss.h"\r
50 #include "mssloc.h"\r
51 \r
52 #define MSS_MIN_POW 2684  /* correpsonds to -50dBm */\r
53 \r
54 /******************************************************************************\r
55  * FUNCTION PURPOSE: Main Processing API of MSS \r
56  ******************************************************************************\r
57  * DESCRIPTION: This function processes the multi-source inputs and selects one\r
58  *              source as the output. Crossfading is implemented during the \r
59  *              switch of two sources. \r
60  *\r
61  *  tint mssProcess (\r
62  *  Input:\r
63  *    void *mssInst,      - pointer to MSS instance\r
64  *    void *rx_out_sync,  - synchronized Rx out, NOT to be selected, but used to\r
65  *                          help make right selection.\r
66  *    void *mic_fix[],    - data from fixed mics, may be selected.\r
67  *    void *mic_rem[],    - data from remote mics, may be selected.\r
68  *    void *mic_cln[],    - data from clean mics, may be selected.\r
69  *    void *mic_arr[],    - data from microphone arrays, NOT to be selected, but\r
70  *                          used to help make right selection.\r
71  *    void *beam[],       - data from beamformers, may be selected.\r
72  *  Output:\r
73  *    void *out,          - data of the selected source, or a mix of two sources\r
74  *                          during the switch. \r
75  *\r
76  *****************************************************************************/\r
77 tint mssProcess(void *mssInst, void *out, void *rx_out_sync, void *mic_fix[],\r
78                 void *mic_rem[], void *mic_cln[], void *mic_arr[], void *beam[])\r
79 {\r
80   tint src_err; \r
81   void *first_src;\r
82   mssInst_t * inst = (mssInst_t *)mssInst;\r
83   \r
84   /* Return error if state is not OPENED */  \r
85   if(inst->state != MSS_OPEN) {\r
86     return(mss_ERR_NOTOPENED);\r
87   }\r
88 \r
89   /* Check if all sources are valid and return error if any one is invalid. */  \r
90   src_err = mss_src_validation(inst, rx_out_sync, mic_fix, mic_rem, mic_cln,\r
91                                mic_arr, beam);                             \r
92   if(src_err == mss_ERR_INVALIDSRC) {\r
93     return(mss_ERR_INVALIDSRC);\r
94   }\r
95 \r
96   /* The BYPASS mode just gets the first available source passed to the output */\r
97   if(mss_chkbit(inst->modes_bf, mss_CTL_MODES_BYPASS)) {\r
98     first_src = mss_get_first_src(inst, mic_fix, mic_rem, mic_cln);\r
99     memcpy(out, first_src, inst->frame_size*sizeof(linSample));\r
100   }\r
101   else { /* If not bypass, check for FREEZE */\r
102     if(!mss_chkbit(inst->modes_bf, mss_CTL_MODES_FREEZE)) {\r
103       /* Multi-source selection: \r
104          Currently only supports fixed, remote and clean mics. */\r
105       mss_src_selection(inst, mic_fix, mic_rem, mic_cln); \r
106     }\r
107 \r
108     /* Output selected source, crossfading during switch between two sources. */\r
109     mss_src_output(inst, out, mic_fix, mic_rem, mic_cln);\r
110   }\r
111   \r
112   return src_err;\r
113 } /* mssProcess */\r
114 \r
115 /******************************************************************************\r
116  * FUNCTION PURPOSE: Get the First Valid Source \r
117  ******************************************************************************\r
118  * DESCRIPTION: This function orted.\r
119  *\r
120  *  tint mss_get_first_src (\r
121  *  Input:\r
122  *    void *inst,         - pointer to MSS instance\r
123  *    void *mic_fix[],    - data from fixed mics, may be selected.\r
124  *    void *mic_rem[],    - data from remote mics, may be selected.\r
125  *    void *mic_cln[],    - data from clean mics, may be selected.\r
126  *  Output:\r
127  *    void * ,            - address of the first valid source\r
128  *****************************************************************************/\r
129 void * mss_get_first_src(mssInst_t *inst, void *mic_fix[], void *mic_rem[], \r
130                          void *mic_cln[])\r
131 {\r
132   void * first_src;\r
133   \r
134   if(inst->num_src_per_group[mss_SRC_MIC_FIXED] > 0) {\r
135     first_src = mic_fix[0];\r
136   }\r
137   else if(inst->num_src_per_group[mss_SRC_MIC_REMOTE] > 0) {\r
138     first_src = mic_rem[0];\r
139   }\r
140   else {\r
141     first_src = mic_cln[0];\r
142   }\r
143   \r
144   return first_src;\r
145 } /* mss_get_first_src */                        \r
146 \r
147 /******************************************************************************\r
148  * FUNCTION PURPOSE: Source selection of MSS \r
149  ******************************************************************************\r
150  * DESCRIPTION: This function processes the multi-source inputs and selects one\r
151  *              source as the output. The selectin criteria is the exponential\r
152  *              average of power. The source with maximum power is selected. \r
153  *              Currently, only fixed mic, remote mic, and clean mic are supported.\r
154  *\r
155  *  tint mss_src_selection (\r
156  *  Input:\r
157  *    void *inst,         - pointer to MSS instance\r
158  *    void *mic_fix[],    - data from fixed mics, may be selected.\r
159  *    void *mic_rem[],    - data from remote mics, may be selected.\r
160  *    void *mic_cln[],    - data from clean mics, may be selected.\r
161  *  Output:\r
162  *    Instance variables: MSS_STATE_BIT_SWITCH_SRC of inst->state_bf\r
163  *                        temp_src\r
164  *                        switch_hang_cnt\r
165  *                        new_src\r
166  *                        crossfade_phase\r
167  *                        crossface_cnt\r
168  * \r
169  *****************************************************************************/\r
170 void mss_src_selection(mssInst_t *inst, void *mic_fix[], void *mic_rem[], \r
171                         void *mic_cln[])\r
172 {\r
173   int i, max_pow_src_index, svd_decision;\r
174   LFract max_pow, sig_pow;\r
175   tword *svd_ptr;\r
176   mssSrcType_t max_pow_src_group;\r
177   tint frm_len;\r
178 \r
179   /* Initialization */\r
180   svd_ptr = inst->svd;\r
181   frm_len = inst->frame_size;\r
182   max_pow = (LFract)0;\r
183   max_pow_src_group = mss_SRC_INVALID;\r
184   \r
185   /*\r
186    * Task 1: Analyze sources to select the best one.\r
187    * Current selection criteria is the exponential average power. Whichever\r
188    * source has the largest power will be selected. \r
189    */\r
190   /* Analyze fixed microphones */ \r
191   for (i=0; i<inst->num_src_per_group[mss_SRC_MIC_FIXED]; i++) {\r
192     svdProcess(svd_ptr, mic_fix[i], frm_len);\r
193     svd_decision = svdGetDecision(svd_ptr, &sig_pow);\r
194     \r
195     /* only look at power of mics having speech (MSS_MIN_POW is -50.0109dBm) */\r
196     if(   (svd_decision != svd_VOICE_INACTIVE)  /* even when undefined? */\r
197        && (sig_pow > MSS_MIN_POW) && (sig_pow > max_pow)) {\r
198       max_pow = sig_pow;\r
199       max_pow_src_group = mss_SRC_MIC_FIXED;\r
200       max_pow_src_index = i;\r
201     }\r
202     \r
203     svd_ptr += inst->svd_size;\r
204   } /* num_mic_fix */\r
205 \r
206   /* Analyze remote microphones */ \r
207   for (i=0; i<inst->num_src_per_group[mss_SRC_MIC_REMOTE]; i++) {\r
208     svdProcess(svd_ptr, mic_rem[i], frm_len);\r
209     svd_decision = svdGetDecision(svd_ptr, &sig_pow);\r
210     \r
211     /* only look at power of mics having speech */\r
212     if(   (svd_decision != svd_VOICE_INACTIVE) \r
213        && (sig_pow > MSS_MIN_POW) && (sig_pow > max_pow)) {\r
214       max_pow = sig_pow;\r
215       max_pow_src_group = mss_SRC_MIC_REMOTE;\r
216       max_pow_src_index = i;\r
217     }\r
218     \r
219     svd_ptr += inst->svd_size;\r
220   } /* num_mic_remote */\r
221 \r
222   /* Analyze clean microphones */ \r
223   for (i=0; i<inst->num_src_per_group[mss_SRC_MIC_CLEAN]; i++) {\r
224     svdProcess(svd_ptr, mic_cln[i], frm_len);\r
225     svd_decision = svdGetDecision(svd_ptr, &sig_pow);\r
226     \r
227     /* only look at power of mics having speech */\r
228     if(   (svd_decision != svd_VOICE_INACTIVE) \r
229        && (sig_pow > MSS_MIN_POW) && (sig_pow > max_pow)) {\r
230       max_pow = sig_pow;\r
231       max_pow_src_group = mss_SRC_MIC_CLEAN;\r
232       max_pow_src_index = i;\r
233     }\r
234     \r
235     svd_ptr += inst->svd_size;\r
236   } /* num_mic_clean */\r
237 \r
238   /* for debugging only */\r
239   inst->temp_src.group = max_pow_src_group;\r
240   inst->temp_src.index = max_pow_src_index;\r
241   \r
242   /*\r
243    * Task 2: Decide whether to switch source based on the selection obtained above.\r
244    */  \r
245   /* Don's consider switching source in the middle of a switch */\r
246   if(!mss_chkbit(inst->state_bf, MSS_STATE_BIT_SWITCH_SRC)) {\r
247     /* If not in the middle of a switch, wait until hangover expires. */  \r
248     if(inst->switch_hang_cnt > 0) {\r
249       inst->switch_hang_cnt -= frm_len;  /* expiring hangover */\r
250     }\r
251     else { /* hangover expires and we can look for switch again */\r
252       /* Don't consider switching source if:\r
253          1. speech is not present in any source (max_pow_src_group == mss_SRC_MAX), or \r
254          2. source with max power is the current selected source.\r
255       */\r
256       if(  (max_pow_src_group == mss_SRC_INVALID)  \r
257         || (   (max_pow_src_group == inst->cur_src.group) \r
258             && (max_pow_src_index == inst->cur_src.index) )\r
259         ) {\r
260         if (inst->switch_src_cnt > frm_len) {\r
261           inst->switch_src_cnt -= frm_len;     /* Count in opposite direction with same speed */\r
262         }\r
263         else {  /* give up on "new src" */\r
264           inst->switch_src_cnt = 0;\r
265           inst->new_src = inst->cur_src;\r
266         }\r
267       }\r
268       /* Consider switching source if the source with max power is not the \r
269          current active source. */\r
270       else if(   (max_pow_src_group == inst->new_src.group)\r
271               && (max_pow_src_index == inst->new_src.index) \r
272              ) {\r
273         /* Increment counter since the source with max power doesn't change \r
274            from previous frame. */\r
275         inst->switch_src_cnt += frm_len;\r
276         if(inst->switch_src_cnt >= inst->params.switch_threshold) {\r
277           /* Confirm and switch source because the same source had max power long enough */\r
278           inst->switch_src_cnt  = 0;\r
279           inst->crossfade_phase = MSS_FADE_PHA_INIT;\r
280           inst->crossfade_cnt  = 0;\r
281           inst->switch_hang_cnt = inst->params.switch_hangover;\r
282           mss_setbit(inst->state_bf, MSS_STATE_BIT_SWITCH_SRC);\r
283         }\r
284       }\r
285       else {  /* Source with max power might be changing to something other then current/new */\r
286         if (inst->switch_src_cnt > frm_len) {\r
287           inst->switch_src_cnt -= frm_len/2;      /* Lower speed, this should smooth the "new" selection */\r
288         }\r
289         else {\r
290           /* Don't switch since source with max power is changing. */\r
291           inst->new_src.group  = max_pow_src_group;\r
292           inst->new_src.index  = max_pow_src_index;\r
293           inst->switch_src_cnt = 0;\r
294         }\r
295       }\r
296     } /* hangover expires */\r
297   } /* !mss_chkbit(inst->state_bf, MSS_STATE_BIT_SWITCH_SRC) */ \r
298 } /* mss_src_selection */                       \r
299 \r
300 /******************************************************************************\r
301  * FUNCTION PURPOSE: Generating Single Output from Multiple Sources \r
302  ******************************************************************************\r
303  * DESCRIPTION: This function generates single output from multiple input sources.\r
304  *              Generally, it passes the selected source to the output, but when\r
305  *              there is a switch from one source to another, it will mix the\r
306  *              the previously selected and newly selected sources. \r
307  *\r
308  *  tint mss_src_output (\r
309  *  Input:\r
310  *    void *inst,         - pointer to MSS instance\r
311  *    void *mic_fix[],    - data from fixed mics, may be selected.\r
312  *    void *mic_rem[],    - data from remote mics, may be selected.\r
313  *    void *mic_cln[],    - data from clean mics, may be selected.\r
314  *  Output:\r
315  *    void *out           - single source or mixed sources\r
316  *****************************************************************************/\r
317 #define MSS_SIN_PIOVER2 16388\r
318 void mss_src_output(mssInst_t *inst, void *out, void *mic_fix[], \r
319                      void *mic_rem[], void *mic_cln[])\r
320 {\r
321   int i;\r
322   linSample * cur_sel_src;\r
323   linSample * new_sel_src;\r
324   linSample * output;\r
325   Fract fade_in_alpha, fade_out_alpha, sin_phase;\r
326   LFract phase, phase_inc;\r
327   \r
328   /* Identify the buffer of the current selected source. */\r
329   if(inst->cur_src.group == mss_SRC_MIC_FIXED) {\r
330     cur_sel_src = mic_fix[inst->cur_src.index];\r
331   }\r
332   else if (inst->cur_src.group == mss_SRC_MIC_REMOTE) {\r
333     cur_sel_src = mic_rem[inst->cur_src.index];\r
334   }\r
335   else if (inst->cur_src.group == mss_SRC_MIC_CLEAN) {\r
336     cur_sel_src = mic_cln[inst->cur_src.index];\r
337   }\r
338   else {\r
339     /* error message */\r
340   }\r
341 \r
342   /* Check if in the middle of a switch. */\r
343   if(!mss_chkbit(inst->state_bf, MSS_STATE_BIT_SWITCH_SRC)) {\r
344     /* If it is not during a switch, copy the current selected source to output \r
345        without any modification. */\r
346     memcpy(out, cur_sel_src, inst->frame_size*sizeof(linSample));\r
347   }  \r
348   else {\r
349     /* If it is during a switch, identify the buffer of the new selected source. */\r
350     if(inst->new_src.group == mss_SRC_MIC_FIXED) {\r
351       new_sel_src = mic_fix[inst->new_src.index];\r
352     }\r
353     else if (inst->new_src.group == mss_SRC_MIC_REMOTE) {\r
354       new_sel_src = mic_rem[inst->new_src.index];\r
355     }\r
356     else if (inst->new_src.group == mss_SRC_MIC_CLEAN) {\r
357       new_sel_src = mic_cln[inst->new_src.index];\r
358     }\r
359     else {\r
360       /* error message */\r
361     }   \r
362   \r
363     /* Mix the previously selected and newly selected sources by crossfading: \r
364        fading out previously selected source and fading in new selected source. */\r
365     phase = inst->crossfade_phase; \r
366     phase_inc = inst->crossfade_phase_inc;\r
367     output = (linSample *)out;\r
368 \r
369     for(i=0; i<inst->frame_size; i++) {\r
370 //      alpha = (1-utlSin(phase>>16)) / 2; /* to use macros */\r
371       sin_phase = utlSin(phase>>16);   /* Q.31 to Q.15 */\r
372       fade_out_alpha = MSS_SIN_PIOVER2/2 - sin_phase/2; /* (1-sin(pha))/2 in Q.14 */\r
373       fade_in_alpha  = MSS_SIN_PIOVER2/2 + sin_phase/2; /* (1+sin(pha))/2 in Q.14 */\r
374       fade_out_alpha = mss_MIN(16383, fade_out_alpha);\r
375       fade_in_alpha  = mss_MIN(16383, fade_in_alpha);\r
376       output[i] = frctMul(cur_sel_src[i], 0, fade_out_alpha, 14, 0) \r
377                  +frctMul(new_sel_src[i], 0, fade_in_alpha,  14, 0);\r
378       phase += phase_inc;\r
379     }\r
380     inst->crossfade_phase = phase;\r
381 \r
382     /* for debugging only */\r
383     inst->gain_in  = fade_in_alpha;\r
384     inst->gain_out = fade_out_alpha;\r
385      \r
386     inst->crossfade_cnt += inst->frame_size;\r
387     if(inst->crossfade_cnt >= inst->params.switch_duration) {\r
388       inst->crossfade_cnt  = 0;\r
389       inst->cur_src = inst->new_src;\r
390       mss_clrbit(inst->state_bf, MSS_STATE_BIT_SWITCH_SRC);\r
391     }    \r
392   } \r
393 } /* mss_src_output */                    \r
394 \r
395 /* Nothing past this point */\r