88300c86ab86505ff74322914fb540c24033f7bd
[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 - 2018, 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, svd_decision;\r
174   int max_pow_src_index = 0;\r
175   LFract max_pow, sig_pow;\r
176   tword *svd_ptr;\r
177   mssSrcType_t max_pow_src_group;\r
178   tint frm_len;\r
179 \r
180   /* Initialization */\r
181   svd_ptr = inst->svd;\r
182   frm_len = inst->frame_size;\r
183   max_pow = (LFract)0;\r
184   max_pow_src_group = mss_SRC_INVALID;\r
185   \r
186   /*\r
187    * Task 1: Analyze sources to select the best one.\r
188    * Current selection criteria is the exponential average power. Whichever\r
189    * source has the largest power will be selected. \r
190    */\r
191   /* Analyze fixed microphones */ \r
192   for (i=0; i<inst->num_src_per_group[mss_SRC_MIC_FIXED]; i++) {\r
193     svdProcess(svd_ptr, mic_fix[i], frm_len);\r
194     svd_decision = svdGetDecision(svd_ptr, &sig_pow);\r
195     \r
196     /* only look at power of mics having speech (MSS_MIN_POW is -50.0109dBm) */\r
197     if(   (svd_decision != svd_VOICE_INACTIVE)  /* even when undefined? */\r
198        && (sig_pow > MSS_MIN_POW) && (sig_pow > max_pow)) {\r
199       max_pow = sig_pow;\r
200       max_pow_src_group = mss_SRC_MIC_FIXED;\r
201       max_pow_src_index = i;\r
202     }\r
203     \r
204     svd_ptr += inst->svd_size;\r
205   } /* num_mic_fix */\r
206 \r
207   /* Analyze remote microphones */ \r
208   for (i=0; i<inst->num_src_per_group[mss_SRC_MIC_REMOTE]; i++) {\r
209     svdProcess(svd_ptr, mic_rem[i], frm_len);\r
210     svd_decision = svdGetDecision(svd_ptr, &sig_pow);\r
211     \r
212     /* only look at power of mics having speech */\r
213     if(   (svd_decision != svd_VOICE_INACTIVE) \r
214        && (sig_pow > MSS_MIN_POW) && (sig_pow > max_pow)) {\r
215       max_pow = sig_pow;\r
216       max_pow_src_group = mss_SRC_MIC_REMOTE;\r
217       max_pow_src_index = i;\r
218     }\r
219     \r
220     svd_ptr += inst->svd_size;\r
221   } /* num_mic_remote */\r
222 \r
223   /* Analyze clean microphones */ \r
224   for (i=0; i<inst->num_src_per_group[mss_SRC_MIC_CLEAN]; i++) {\r
225     svdProcess(svd_ptr, mic_cln[i], frm_len);\r
226     svd_decision = svdGetDecision(svd_ptr, &sig_pow);\r
227     \r
228     /* only look at power of mics having speech */\r
229     if(   (svd_decision != svd_VOICE_INACTIVE) \r
230        && (sig_pow > MSS_MIN_POW) && (sig_pow > max_pow)) {\r
231       max_pow = sig_pow;\r
232       max_pow_src_group = mss_SRC_MIC_CLEAN;\r
233       max_pow_src_index = i;\r
234     }\r
235     \r
236     svd_ptr += inst->svd_size;\r
237   } /* num_mic_clean */\r
238 \r
239   /* for debugging only */\r
240   inst->temp_src.group = max_pow_src_group;\r
241   inst->temp_src.index = max_pow_src_index;\r
242   \r
243   /*\r
244    * Task 2: Decide whether to switch source based on the selection obtained above.\r
245    */  \r
246   /* Don's consider switching source in the middle of a switch */\r
247   if(!mss_chkbit(inst->state_bf, MSS_STATE_BIT_SWITCH_SRC)) {\r
248     /* If not in the middle of a switch, wait until hangover expires. */  \r
249     if(inst->switch_hang_cnt > 0) {\r
250       inst->switch_hang_cnt -= frm_len;  /* expiring hangover */\r
251     }\r
252     else { /* hangover expires and we can look for switch again */\r
253       /* Don't consider switching source if:\r
254          1. speech is not present in any source (max_pow_src_group == mss_SRC_MAX), or \r
255          2. source with max power is the current selected source.\r
256       */\r
257       if(  (max_pow_src_group == mss_SRC_INVALID)  \r
258         || (   (max_pow_src_group == inst->cur_src.group) \r
259             && (max_pow_src_index == inst->cur_src.index) )\r
260         ) {\r
261         if (inst->switch_src_cnt > frm_len) {\r
262           inst->switch_src_cnt -= frm_len;     /* Count in opposite direction with same speed */\r
263         }\r
264         else {  /* give up on "new src" */\r
265           inst->switch_src_cnt = 0;\r
266           inst->new_src = inst->cur_src;\r
267         }\r
268       }\r
269       /* Consider switching source if the source with max power is not the \r
270          current active source. */\r
271       else if(   (max_pow_src_group == inst->new_src.group)\r
272               && (max_pow_src_index == inst->new_src.index) \r
273              ) {\r
274         /* Increment counter since the source with max power doesn't change \r
275            from previous frame. */\r
276         inst->switch_src_cnt += frm_len;\r
277         if(inst->switch_src_cnt >= inst->params.switch_threshold) {\r
278           /* Confirm and switch source because the same source had max power long enough */\r
279           inst->switch_src_cnt  = 0;\r
280           inst->crossfade_phase = MSS_FADE_PHA_INIT;\r
281           inst->crossfade_cnt  = 0;\r
282           inst->switch_hang_cnt = inst->params.switch_hangover;\r
283           mss_setbit(inst->state_bf, MSS_STATE_BIT_SWITCH_SRC);\r
284         }\r
285       }\r
286       else {  /* Source with max power might be changing to something other then current/new */\r
287         if (inst->switch_src_cnt > frm_len) {\r
288           inst->switch_src_cnt -= frm_len/2;      /* Lower speed, this should smooth the "new" selection */\r
289         }\r
290         else {\r
291           /* Don't switch since source with max power is changing. */\r
292           inst->new_src.group  = max_pow_src_group;\r
293           inst->new_src.index  = max_pow_src_index;\r
294           inst->switch_src_cnt = 0;\r
295         }\r
296       }\r
297     } /* hangover expires */\r
298   } /* !mss_chkbit(inst->state_bf, MSS_STATE_BIT_SWITCH_SRC) */ \r
299 } /* mss_src_selection */                       \r
300 \r
301 /******************************************************************************\r
302  * FUNCTION PURPOSE: Generating Single Output from Multiple Sources \r
303  ******************************************************************************\r
304  * DESCRIPTION: This function generates single output from multiple input sources.\r
305  *              Generally, it passes the selected source to the output, but when\r
306  *              there is a switch from one source to another, it will mix the\r
307  *              the previously selected and newly selected sources. \r
308  *\r
309  *  tint mss_src_output (\r
310  *  Input:\r
311  *    void *inst,         - pointer to MSS instance\r
312  *    void *mic_fix[],    - data from fixed mics, may be selected.\r
313  *    void *mic_rem[],    - data from remote mics, may be selected.\r
314  *    void *mic_cln[],    - data from clean mics, may be selected.\r
315  *  Output:\r
316  *    void *out           - single source or mixed sources\r
317  *****************************************************************************/\r
318 #define MSS_SIN_PIOVER2 16388\r
319 void mss_src_output(mssInst_t *inst, void *out, void *mic_fix[], \r
320                      void *mic_rem[], void *mic_cln[])\r
321 {\r
322   int i;\r
323   linSample * cur_sel_src = NULL;\r
324   linSample * new_sel_src = NULL;\r
325   linSample * output;\r
326   Fract fade_in_alpha = 0;\r
327   Fract fade_out_alpha = 0;\r
328   Fract sin_phase = 0;\r
329   LFract phase, phase_inc;\r
330   \r
331   /* Identify the buffer of the current selected source. */\r
332   if(inst->cur_src.group == mss_SRC_MIC_FIXED) {\r
333     cur_sel_src = mic_fix[inst->cur_src.index];\r
334   }\r
335   else if (inst->cur_src.group == mss_SRC_MIC_REMOTE) {\r
336     cur_sel_src = mic_rem[inst->cur_src.index];\r
337   }\r
338   else if (inst->cur_src.group == mss_SRC_MIC_CLEAN) {\r
339     cur_sel_src = mic_cln[inst->cur_src.index];\r
340   }\r
341   else {\r
342     /* error message */\r
343   }\r
344 \r
345   /* Check if in the middle of a switch. */\r
346   if(!mss_chkbit(inst->state_bf, MSS_STATE_BIT_SWITCH_SRC) && cur_sel_src != NULL) {\r
347     /* If it is not during a switch, copy the current selected source to output \r
348        without any modification. */\r
349     memcpy(out, cur_sel_src, inst->frame_size*sizeof(linSample));\r
350   }  \r
351   else {\r
352     /* If it is during a switch, identify the buffer of the new selected source. */\r
353     if(inst->new_src.group == mss_SRC_MIC_FIXED) {\r
354       new_sel_src = mic_fix[inst->new_src.index];\r
355     }\r
356     else if (inst->new_src.group == mss_SRC_MIC_REMOTE) {\r
357       new_sel_src = mic_rem[inst->new_src.index];\r
358     }\r
359     else if (inst->new_src.group == mss_SRC_MIC_CLEAN) {\r
360       new_sel_src = mic_cln[inst->new_src.index];\r
361     }\r
362     else {\r
363       /* error message */\r
364     }   \r
365   \r
366     /* Mix the previously selected and newly selected sources by crossfading: \r
367        fading out previously selected source and fading in new selected source. */\r
368     phase = inst->crossfade_phase; \r
369     phase_inc = inst->crossfade_phase_inc;\r
370     output = (linSample *)out;\r
371 \r
372     if(cur_sel_src != NULL && new_sel_src != NULL)\r
373     {\r
374         for(i=0; i<inst->frame_size; i++) {\r
375 //          alpha = (1-utlSin(phase>>16)) / 2; /* to use macros */\r
376             sin_phase = utlSin(phase>>16);   /* Q.31 to Q.15 */\r
377             fade_out_alpha = MSS_SIN_PIOVER2/2 - sin_phase/2; /* (1-sin(pha))/2 in Q.14 */\r
378             fade_in_alpha  = MSS_SIN_PIOVER2/2 + sin_phase/2; /* (1+sin(pha))/2 in Q.14 */\r
379             fade_out_alpha = mss_MIN(16383, fade_out_alpha);\r
380             fade_in_alpha  = mss_MIN(16383, fade_in_alpha);\r
381             output[i] = frctMul(cur_sel_src[i], 0, fade_out_alpha, 14, 0) \r
382                  +frctMul(new_sel_src[i], 0, fade_in_alpha,  14, 0);\r
383             phase += phase_inc;\r
384         }\r
385     }\r
386     else {\r
387       /* error message */\r
388     }\r
389 \r
390     inst->crossfade_phase = phase;\r
391 \r
392     /* for debugging only */\r
393     inst->gain_in  = fade_in_alpha;\r
394     inst->gain_out = fade_out_alpha;\r
395      \r
396     inst->crossfade_cnt += inst->frame_size;\r
397     if(inst->crossfade_cnt >= inst->params.switch_duration) {\r
398       inst->crossfade_cnt  = 0;\r
399       inst->cur_src = inst->new_src;\r
400       mss_clrbit(inst->state_bf, MSS_STATE_BIT_SWITCH_SRC);\r
401     }    \r
402   } \r
403 } /* mss_src_output */                    \r
404 \r
405 /* Nothing past this point */\r