c5068e2f444a9ad12c5cd68cbf8800fa052040d9
[processor-sdk/audio-preprocessing.git] / file_demo_bios / k2g / components / mss / src / mss.c
1 /**\r
2  *  @file   mss.c\r
3  *  @brief  Contains external API for Multi-Source Selection (MSS) module \r
4  *          instantiation.\r
5  *\r
6  *  (C) Copyright 2011, Texas Instruments, Inc.\r
7  */\r
8 \r
9 #include <xdc/std.h>\r
10 #include <ti/mas/types/types.h>\r
11 #include <ti/mas/fract/fract.h>\r
12 #include <ti/mas/vpe/svd.h>\r
13 \r
14 //#include <ti/mas/aer/mss.h>\r
15 //#include <ti/mas/aer/src/mssloc.h>\r
16 #include "../mss.h"\r
17 #include "mssloc.h"\r
18 \r
19 #define MSS_DEF_SWITCH_WAIT_TIME  250     /* msec */\r
20 #define MSS_DEF_SWITCH_DURATION   500     /* msec */\r
21 #define MSS_DEF_SWITCH_HANGOVER   1000    /* msec */\r
22 #define MSS_DEF_FADE_PHA_INC_8KHZ 268435L  /* Q31 phase increment of crossfade for \r
23                                               switch duration of 1 second at 8kHz:\r
24                                               2^31/8000, where 2^31 corresponds to pi. */\r
25 /* MSS memory buffer descriptors */\r
26 #define MSS_NUM_BUFS 1\r
27 #define MSS_INSTANCE_BUF 0\r
28 ecomemBuffer_t mssBufs[MSS_NUM_BUFS] = {                             \r
29   /* CLASS, LOG2 ALIGNMENT, BUFFER SIZE, VOLATILITY, BASE */\r
30   ecomem_CLASS_EXTERNAL, TYP_STRUCT_LOG2ALIGN, 0, FALSE, NULL\r
31 };\r
32 \r
33 /******************************************************************************\r
34  * FUNCTION PURPOSE: Return buffer sizes for the MSS module\r
35  ******************************************************************************\r
36  * DESCRIPTION: Calculate the worst case buffer sizes for the MSS. Also specify\r
37  *              alignment and volatility requirements.\r
38  *\r
39  *  tint mssGetSizes (\r
40  *    tint              *nbufs,    - address to store number of buffers\r
41  *    const ecomemBuffer_t **bufs, - address to store buffer descriptors\r
42  *    mssSizeConfig_t   *cfg)      - information to be used in calculating the\r
43  *                                   worst case buffer sizes\r
44  *****************************************************************************/\r
45 tint mssGetSizes(tint *nbufs, const ecomemBuffer_t **bufs, mssSizeConfig_t *cfg)\r
46 {\r
47   tint svd_size, mss_size, mss_inst_size, num_src;\r
48 \r
49   /* Get memory requirement of SVD */\r
50   svdGetSizes (&svd_size);\r
51   \r
52   /* Calculate total memory of MSS: instance + all SVDs*/\r
53   svd_size = mss_SIZE_ALIGN(svd_size,TYP_STRUCT_LOG2ALIGN);\r
54   mss_inst_size = sizeof(mssInst_t);\r
55   mss_inst_size = mss_SIZE_ALIGN(mss_inst_size,TYP_STRUCT_LOG2ALIGN);\r
56   num_src =  cfg->max_num_mic_fixed + cfg->max_num_mic_remote\r
57            + cfg->max_num_mic_clean + cfg->max_num_beam;\r
58   mss_size = svd_size*num_src + mss_inst_size;\r
59   mssBufs[MSS_INSTANCE_BUF].size = mss_size;\r
60 \r
61   /* Return number of buffers required by MSS */  \r
62   *nbufs = MSS_NUM_BUFS;\r
63   \r
64   /* Return buffer specifications */\r
65   *bufs = &mssBufs[0];                  \r
66   \r
67   return(mss_NOERR);\r
68   \r
69 } /* mssGetSizes */\r
70 \r
71 /******************************************************************************\r
72  * FUNCTION PURPOSE: Create an instance of the MSS module.\r
73  ******************************************************************************\r
74  * DESCRIPTION: Creates an instance of the MSS and initializes memory buffers.\r
75  *\r
76  *  tint mssNew (\r
77  *    void            **mssInst,  - address of the memory location that will\r
78  *                                  receive a pointer to instance structure\r
79  *    tint            nbufs,      - number of memory buffers\r
80  *    ecomemBuffer_t  *bufs,      - a vector of buffer descriptors\r
81  *    mssNewConfig_t  *cfg)       - a pointer to configuration structure\r
82  *\r
83  *  Return value: \r
84  *          mss_NOERR             - success\r
85  *          mss_ERR_INVALIDPAR    - *mssInst is not NULL or nbufs is not correct \r
86  *          mss_ERR_NOMEMORY      - MSS instance pointer is not NULL, or\r
87  *                                  number of buffers is not correct, or\r
88  *                                  buffer properties are not correct\r
89  *****************************************************************************/\r
90 tint mssNew (void **mssInst, tint nbufs, ecomemBuffer_t *bufs, mssNewConfig_t *cfg)\r
91 {\r
92   mssInst_t       *inst;\r
93   ecomemBuffer_t  *bufp;\r
94   tint svd_size, mss_size, mss_inst_size, num_src;\r
95   int i;\r
96   \r
97   /* Test instance pointer (must be NULL) */\r
98   if ((*mssInst != NULL) || (nbufs != MSS_NUM_BUFS)) {\r
99     return(mss_ERR_INVALIDPAR);\r
100   }\r
101 \r
102   /* Check all dynamic buffers (base address != NULL) */\r
103   for (i=0, bufp=&bufs[0]; i<MSS_NUM_BUFS; i++, bufp++) {\r
104     if ((bufp->size>0) && (bufp->base==NULL)) {\r
105       return(mss_ERR_NOMEMORY);\r
106     }\r
107   }\r
108 \r
109   /* Get the instance buffer descriptor returned by the user */\r
110   bufp = &bufs[MSS_INSTANCE_BUF];\r
111 \r
112   /* Calculate total memory of MSS: instance + all SVDs */\r
113   svdGetSizes (&svd_size);\r
114   svd_size = mss_SIZE_ALIGN(svd_size,TYP_STRUCT_LOG2ALIGN);\r
115   mss_inst_size = sizeof(mssInst_t);\r
116   mss_inst_size = mss_SIZE_ALIGN(mss_inst_size,TYP_STRUCT_LOG2ALIGN);\r
117   num_src =  cfg->sizeCfg.max_num_mic_fixed + cfg->sizeCfg.max_num_mic_remote\r
118            + cfg->sizeCfg.max_num_mic_clean + cfg->sizeCfg.max_num_beam;\r
119   mss_size = svd_size*num_src + mss_inst_size;\r
120   \r
121   /* Check if the provided memory is good: size, alignment, and volatility */\r
122   if ( (bufp->size < mss_size) \r
123       || (!typChkAlign(bufp->base, bufp->log2align)) || bufp->volat) {\r
124     return(mss_ERR_NOMEMORY);\r
125   }\r
126   \r
127   /* Assign instance memory and initialize instance */\r
128   inst = (mssInst_t *)bufp->base;\r
129   inst->max_sampling_rate = cfg->sizeCfg.max_sampling_rate;\r
130   inst->svd = (tword *)bufp->base + mss_inst_size; /* SVD address for 1st src */\r
131   inst->svd_size = svd_size;\r
132   inst->handle = cfg->handle;        /* init handle in instance */\r
133   inst->state = MSS_CLOSED;          /* set MSS state to CLOSED */\r
134 \r
135   /* Return instance address */\r
136   *mssInst = (void *)inst;\r
137 \r
138   return(mss_NOERR);\r
139   \r
140 } /* mssNew() */\r
141 \r
142 /******************************************************************************\r
143  * FUNCTION PURPOSE: Control of MSS\r
144  ******************************************************************************\r
145  * DESCRIPTION: Controls the operation and parameters of the MSS.\r
146  *\r
147  *  tint mssControl (\r
148  *    void  *mssInst,              - a pointer to MSS instance\r
149  *    tint  mssControl *ctl)       - a pointer to the control code structure \r
150  *\r
151  *  Return value: mss_NOERR           - success\r
152  *                mss_ERR_NOTOPENED   - MSS not opened\r
153  *                mss_ERR_INVALIDSRC  - the manually selected source is invalid \r
154  *                mss_ERR_INVALIDPAR  - parameter is invalid \r
155  *****************************************************************************/\r
156 tint mssControl (void *mssInst, mssControl_t *ctl)\r
157 {\r
158   mssInst_t *inst = (mssInst_t *)mssInst;\r
159   tint ret_val;\r
160 \r
161   /* Make sure that MSS is open */\r
162   if (inst->state != MSS_OPEN) {\r
163     return (mss_ERR_NOTOPENED);\r
164   }  \r
165   \r
166   ret_val = mss_NOERR;\r
167 \r
168   /* Change the operation modes bitfield */\r
169   if(mss_chkbit(ctl->valid_bitfield, mss_CTL_MODES)) {\r
170     inst->modes_bf =  (inst->modes_bf & (~ctl->modes.mask))\r
171                     | (ctl->modes.value & ctl->modes.mask);\r
172   } /* mss_CTL_MODES */\r
173   \r
174   /* Manually select source */\r
175   if(mss_chkbit(ctl->valid_bitfield, mss_CTL_SELECT_SRC)) {\r
176     /* Check if selected source is valid:\r
177          - source group must be one of the groups defined in mss.h,      \r
178          - source index must be less than the number of sources in the group. */\r
179     if(   (ctl->source.group > mss_SRC_MAX)\r
180        || (ctl->source.group < mss_SRC_MIC_FIXED) ){\r
181       ret_val = mss_ERR_INVALIDSRC;\r
182     } \r
183     else if (ctl->source.index > inst->num_src_per_group[ctl->source.group]) {\r
184       ret_val = mss_ERR_INVALIDSRC;\r
185     }\r
186     else if (   (ctl->source.group != inst->cur_src.group) \r
187              || (ctl->source.index != inst->cur_src.index) ) {      \r
188       /* Prepare to switch source if selected source is valid. If in the middle of a \r
189          source switch initiated by MSS internally, internal switch will be stopped. */\r
190       inst->switch_src_cnt  = 0;\r
191       inst->crossfade_phase = MSS_FADE_PHA_INIT;\r
192       inst->switch_hang_cnt = inst->params.switch_hangover;\r
193       inst->new_src = ctl->source;\r
194       mss_setbit(inst->state_bf, MSS_STATE_BIT_SWITCH_SRC);\r
195     }\r
196   } /* mss_CTL_SELECT_SRC */\r
197 \r
198   /* Change the threshold of source switch */\r
199   if(mss_chkbit(ctl->valid_bitfield, mss_CTL_SWITCH_THRESH)) {\r
200     /* Convert parameter from msec to # of samples */\r
201     inst->params.switch_threshold = MSS_NUM_SAMP_1MS_8KHZ * inst->sampling_rate\r
202                                     * ctl->switch_thresh;\r
203   }\r
204   \r
205   /* Change the duration of source switch */\r
206   if(mss_chkbit(ctl->valid_bitfield, mss_CTL_SWITCH_DURATION)) {\r
207     /* Convert parameter from msec to # of samples */\r
208     inst->params.switch_duration = MSS_NUM_SAMP_1MS_8KHZ * inst->sampling_rate\r
209                                    * ctl->switch_duration;\r
210     /* Calculate per sample Q31 phase increment for crossfade implementation: \r
211        2^31/duration, where 2^31 corresponds to pi. */                                     \r
212     inst->crossfade_phase_inc = frct_LFRCT_MAX/inst->params.switch_duration;                                     \r
213   } /* mss_CTL_SWITCH_DURATION */\r
214 \r
215   /* Change hangover of source switch */\r
216   if(mss_chkbit(ctl->valid_bitfield, mss_CTL_SWITCH_HNAGOVER)) {\r
217     /* Convert parameter from msec to # of samples */\r
218     inst->params.switch_hangover = MSS_NUM_SAMP_1MS_8KHZ * inst->sampling_rate\r
219                                    * ctl->switch_hangover;\r
220   } /* mss_CTL_SWITCH_HNAGOVER */\r
221   \r
222   return ret_val;\r
223 } /* mssControl */\r
224   \r
225 /******************************************************************************\r
226  * FUNCTION PURPOSE: Open and configure an MSS instance.\r
227  ******************************************************************************\r
228  * DESCRIPTION: This function configures an instance of the MSS.\r
229  *\r
230  *  tint mssOpen (\r
231  *    void        *mssInst,   - a pointer to MSS instance\r
232  *    mssConfig_t *cfg)       - a pointer to MSS configuration structure\r
233  *\r
234  *****************************************************************************/\r
235 tint mssOpen (void *mssInst, mssConfig_t *cfg)\r
236 {\r
237   mssInst_t * inst = (mssInst_t *)mssInst;\r
238   tword * svd_ptr;\r
239   tint num_src, svd_size;\r
240   int i;\r
241   \r
242   /* Return error if state is not CLOSED */  \r
243   if(inst->state != MSS_CLOSED) {\r
244     return(mss_ERR_NOTCLOSED);\r
245   }\r
246   \r
247   /* Return error if no configuration parameters */\r
248   if(cfg == NULL){\r
249     return(mss_ERR_INVALIDPAR);\r
250   }\r
251   \r
252   /* Read configuration */\r
253   num_src = cfg->num_mic_fixed+cfg->num_mic_remote+cfg->num_mic_clean+cfg->num_beam;\r
254   if(num_src <= 0) {\r
255     return(mss_ERR_INVALIDPAR);\r
256   }\r
257   \r
258   inst->sampling_rate  = cfg->sampling_rate;\r
259   inst->num_src_per_group[mss_SRC_MIC_FIXED]  = cfg->num_mic_fixed;\r
260   inst->num_src_per_group[mss_SRC_MIC_REMOTE] = cfg->num_mic_remote;\r
261   inst->num_src_per_group[mss_SRC_MIC_CLEAN]  = cfg->num_mic_clean;\r
262   inst->num_src_per_group[mss_SRC_BEAM]       = cfg->num_beam;\r
263   inst->num_mic_array  = cfg->num_mic_array; /* not considered as source to select */\r
264   \r
265   /* Init SVDs */\r
266   svd_ptr = (tword *)inst->svd;\r
267   for(i=0; i<num_src; i++) {\r
268     if (!typChkAlign(svd_ptr, TYP_STRUCT_LOG2ALIGN)) {\r
269       return(mss_ERR_NOMEMORY);\r
270     }\r
271     svdInit((void *)svd_ptr, inst->sampling_rate);\r
272     svd_ptr += svd_size;\r
273   }    \r
274   \r
275   /* Set parameters to default */\r
276   inst->params.switch_threshold = MSS_DEF_SWITCH_WAIT_TIME * MSS_NUM_SAMP_1MS_8KHZ\r
277                                  * inst->sampling_rate; /* convert msec to # of samples */\r
278   inst->params.switch_duration = MSS_DEF_SWITCH_DURATION * MSS_NUM_SAMP_1MS_8KHZ\r
279                                  * inst->sampling_rate; /* convert msec to # of samples */\r
280   inst->crossfade_phase_inc    = MSS_DEF_FADE_PHA_INC_8KHZ / inst->sampling_rate;\r
281   inst->params.switch_hangover = MSS_DEF_SWITCH_HANGOVER * MSS_NUM_SAMP_1MS_8KHZ \r
282                                  * inst->sampling_rate; /* convert msec to # of samples */\r
283   inst->modes_bf = mss_CTL_MODES_DEFAULT;\r
284   \r
285   /* Set initially selected source - the first valid source */\r
286   if(cfg->num_mic_fixed > 0) {\r
287     inst->cur_src.group = mss_SRC_MIC_FIXED;\r
288     inst->cur_src.index = 0;\r
289   }\r
290   else if(cfg->num_mic_remote > 0) {\r
291     inst->cur_src.group = mss_SRC_MIC_REMOTE;\r
292     inst->cur_src.index = 0;\r
293   }\r
294   else if(cfg->num_mic_clean > 0) {\r
295     inst->cur_src.group = mss_SRC_MIC_CLEAN;\r
296     inst->cur_src.index = 0;\r
297   }\r
298   else {\r
299     inst->cur_src.group = mss_SRC_BEAM;\r
300     inst->cur_src.index = 0;\r
301   }\r
302   \r
303   /* Initialize instance */ \r
304   inst->frame_size = inst->sampling_rate * MSS_FRAME_SIZE_8KHZ;\r
305   inst->switch_hang_cnt = inst->params.switch_hangover;\r
306   inst->crossfade_cnt  = 0;\r
307   inst->state_bf = 0;\r
308   \r
309   /* Declare the instance OPEN */\r
310   inst->state = MSS_OPEN;   \r
311 \r
312   return(mss_NOERR);\r
313 } /* mssOpen */\r
314 \r
315 /******************************************************************************\r
316  * FUNCTION PURPOSE: Returns debuging information for an MSS instance.\r
317  ******************************************************************************\r
318  * DESCRIPTION: This function returns debuging information.\r
319  *\r
320  *  tint mssDebugStat (\r
321  *    void        *mssInst,   - a pointer to MSS instance\r
322  *    mssDebugStat_t *mssDbg) - a pointer to MSS debug structure\r
323  *\r
324  *****************************************************************************/\r
325 tint mssDebugStat(void *mssInst, mssDebugStat_t *mssDbg)\r
326 {\r
327   tint svd_decision;\r
328   tword *svd_ptr;\r
329   LFract sig_pow;\r
330   Fract spch_db, noise_db;\r
331   UFract svd_thresh;\r
332   mssInst_t *inst = (mssInst_t *)mssInst;\r
333 \r
334   /* Make sure that MSS is open */\r
335   if (inst->state != MSS_OPEN) {\r
336     return (mss_ERR_NOTOPENED);\r
337   }  \r
338   \r
339   mssDbg->cur_src = inst->cur_src;\r
340   mssDbg->new_src = inst->new_src;\r
341   mssDbg->states[0] = (tuint)(inst->crossfade_phase>>16);\r
342   mssDbg->states[1] = (tuint)inst->gain_in;\r
343   mssDbg->states[2] = (tuint)inst->gain_out;\r
344   mssDbg->states[3] = (tuint)inst->switch_src_cnt; \r
345   mssDbg->states[4] = (tuint)inst->state_bf;\r
346 \r
347   /* only debug fixed mics for now */\r
348   if(inst->num_src_per_group[mss_SRC_MIC_FIXED] > 1) {\r
349     svd_ptr = inst->svd;\r
350     svd_decision = svdGetDecision(svd_ptr, &sig_pow);\r
351     mssDbg->states[5] = (tuint)svd_decision;\r
352     \r
353     svdGetLevels (svd_ptr, &spch_db, &noise_db, &svd_thresh);\r
354     mssDbg->states[6] = (tuint)spch_db;\r
355     mssDbg->states[7] = (tuint)noise_db;\r
356   \r
357     svd_ptr += inst->svd_size;\r
358     svd_decision = svdGetDecision(svd_ptr, &sig_pow);\r
359     mssDbg->states[8] = (tuint)svd_decision;\r
360     \r
361     svdGetLevels (svd_ptr, &spch_db, &noise_db, &svd_thresh);\r
362     mssDbg->states[9] = (tuint)spch_db;\r
363     mssDbg->states[10] = (tuint)noise_db;\r
364   }\r
365   \r
366   return(mss_NOERR);\r
367   \r
368 } /* mssDebugStat */\r
369 \r
370 /******************************************************************************\r
371  * FUNCTION PURPOSE: Close an instance of MSS.\r
372  ******************************************************************************\r
373  * DESCRIPTION: This function closes an instance of MSS by simply changing\r
374  *              the state to CLOSED.\r
375  *\r
376  *  tint mssClose (\r
377  *    void *mssInst)    - a pointer to MSS instance\r
378  *\r
379  *****************************************************************************/\r
380 tint mssClose (void *mssInst)\r
381 {\r
382   mssInst_t *inst = (mssInst_t *)mssInst;\r
383 \r
384   if(inst->state != MSS_OPEN) {\r
385     return(mss_ERR_NOTOPENED);\r
386   }\r
387   \r
388   /*Change the MSS state to CLOSED */\r
389   inst->state = MSS_CLOSED;     \r
390   \r
391   return(mss_NOERR);\r
392 } /* mssClose */\r
393 \r
394 /******************************************************************************\r
395  * FUNCTION PURPOSE: Delete an MSS instance.\r
396  ******************************************************************************\r
397  * DESCRIPTION: Deletes an instance of MSS. Clears the instance pointer.\r
398  *              Actual "deallocation" of instance should be done elsewhere\r
399  *              after this function is called. Hence, one has to save the\r
400  *              buffer descriptors to make "deallocation" possible.\r
401  *\r
402  *  tint mssDelete (\r
403  *    void           **mssInst,  - an address of memory location that contains\r
404  *                                 a pointer to instance structure\r
405  *    tint           nbufs,      - number of memory buffers\r
406  *    ecomemBuffer_t *bufs)      - vector of buffer descriptors\r
407  *\r
408  *****************************************************************************/\r
409 tint mssDelete (void **mssInst, tint nbufs, ecomemBuffer_t *bufs)\r
410 {\r
411   mssInst_t *inst = (mssInst_t *)*mssInst;\r
412 \r
413   /* check if instance is already closed */\r
414   if (inst->state != MSS_CLOSED) {      \r
415     return (mss_ERR_NOTCLOSED);\r
416   }\r
417   \r
418   /* check if enough descriptors are provided to store all buffer addresses */\r
419   if (nbufs < MSS_NUM_BUFS) { \r
420     return (mss_ERR_NOMEMORY);\r
421   }\r
422  \r
423   /* return buffer addresses */\r
424   bufs[MSS_INSTANCE_BUF].base      = inst;   /* instance structure */\r
425 \r
426   /* clear the instance pointer */\r
427   *mssInst = NULL;                              \r
428 \r
429   return(mss_NOERR);\r
430 } /* mssDelete */\r
431 \r
432 /******************************************************************************\r
433  * FUNCTION PURPOSE: Validates the input sources to MSS.\r
434  ******************************************************************************\r
435  * DESCRIPTION: This function examines the input sources to MSS to make sure\r
436  *              all sources are valid.\r
437  *\r
438  *****************************************************************************/\r
439 \r
440 tint mss_src_validation(mssInst_t *inst, void *rx_out_sync, void *mic_fix[],\r
441                 void *mic_rem[], void *mic_cln[], void *mic_arr[], void *beam[])\r
442 {                \r
443   int i;\r
444   \r
445   /* Return error if synchronized Rx out doesn't have valid data (NOT USED!!!) */\r
446   /*\r
447    *  if ( rx_out_sync == NULL ) {  \r
448    *    return (mss_ERR_INVALIDSRC); \r
449    *  }\r
450    */\r
451 \r
452   /* Check if all sources have valid data.\r
453      Currently MSS only supports fixed mic, remote mic, and clean mic.\r
454   */\r
455   if(inst->num_src_per_group[mss_SRC_MIC_FIXED] > 0) {\r
456     if(mic_fix == NULL) {\r
457       return (mss_ERR_INVALIDSRC);\r
458     }\r
459     else {\r
460       for (i=0; i<inst->num_src_per_group[mss_SRC_MIC_FIXED]; i++) {\r
461         if(mic_fix[i] == NULL) {\r
462           return (mss_ERR_INVALIDSRC);\r
463         }\r
464       }\r
465     }\r
466   } /* mss_SRC_MIC_FIXED */\r
467   \r
468   if(inst->num_src_per_group[mss_SRC_MIC_REMOTE] > 0) {\r
469     if(mic_rem == NULL) {\r
470       return (mss_ERR_INVALIDSRC);\r
471     }\r
472     else {\r
473       for (i=0; i<inst->num_src_per_group[mss_SRC_MIC_REMOTE]; i++) {\r
474         if(mic_rem[i] == NULL) {\r
475           return (mss_ERR_INVALIDSRC);\r
476         }\r
477       }\r
478     }\r
479   } /* mss_SRC_MIC_REMOTE */\r
480   \r
481   if(inst->num_src_per_group[mss_SRC_MIC_CLEAN] > 0) {\r
482     if(mic_cln == NULL) {\r
483       return (mss_ERR_INVALIDSRC);\r
484     }\r
485     else {\r
486       for (i=0; i<inst->num_src_per_group[mss_SRC_MIC_CLEAN]; i++) {\r
487         if(mic_cln[i] == NULL) {\r
488           return (mss_ERR_INVALIDSRC);\r
489         }\r
490       }\r
491     }\r
492   } /* mss_SRC_MIC_CLEAN */\r
493  \r
494   return (mss_NOERR);  \r
495 } /* mss_src_validation */\r
496 \r
497 /* Nothing past this point */\r