file_demo_bios: use DRC processing to replace the VAD
[processor-sdk/audio-preprocessing.git] / common / components / clk.c
1 /*
2  * Copyright (c) 2017, Texas Instruments Incorporated\r
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * *  Redistributions of source code must retain the above copyright
10  *    notice, this list of conditions and the following disclaimer.
11  *
12  * *  Redistributions in binary form must reproduce the above copyright
13  *    notice, this list of conditions and the following disclaimer in the
14  *    documentation and/or other materials provided with the 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 "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
22  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
23  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
24  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;
27  * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
28  * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR
29  * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
30  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31  *
32  */
33 \r
34 /*=================================================================\r
35  *  clk.c: Clock functions, SWI's, Tx Task\r
36  *=================================================================*/\r
37 \r
38 #include <strings.h>\r
39 \r
40 #include "fil.h"                      /* FILE I/O implementation */\r
41 #include "sys.h"                      /* System API and structures */\r
42 #include "sysbfflt.h"                 /* System support for BF filters */\r
43 \r
44 /* The infamous xdc/std.h must come before any header file which uses XDC symbols */\r
45 #include <xdc/std.h>              /* mandatory - have to include first, for BIOS types */\r
46 #include <ti/mas/types/types.h>\r
47 #include <ti/mas/aer/bf.h>\r
48 //#include <ti/mas/aer/mss.h>\r
49 #include "mss/mss.h"              /* local version used */\r
50 #include <ti/mas/vpe/asnr.h>\r
51 #if (SYS_USE_DRC)\r
52 #include <ti/mas/vau/vau.h>\r
53 #include <ti/mas/aer/drc.h>\r
54 #endif\r
55 \r
56 /*----------------------------------------\r
57  *  BIOS header files\r
58  *----------------------------------------*/\r
59 #include <ti/sysbios/BIOS.h>          /* mandatory - if you call APIs like BIOS_start() */\r
60 #include <xdc/cfg/global.h>           /* header file for statically defined objects/handles */\r
61 #include <xdc/runtime/System.h>       /* for System_printf, and similar */\r
62 #include <xdc/runtime/Timestamp.h>    /* for benchmarking/profiling */\r
63 \r
64 #include <xdc/runtime/Log.h>          /* for tracing */\r
65 #include <xdc/runtime/Diags.h>\r
66 #include <ti/uia/events/UIAEvt.h>     /* and more tracing */\r
67 \r
68 #include <ti/sysbios/knl/Semaphore.h> /* this looks obvious */\r
69 \r
70 #define Timestamp_get Timestamp_get32 /* use 32-bit time stamps */\r
71 \r
72 #define MIN(a,b)    (((a)>(b))?(b):(a))     /* Min/Max macros */\r
73 #define MAX(a,b)    (((a)<(b))?(b):(a))\r
74 #define loop        while(1)                /* endless loop for the task */\r
75 \r
76 /*=============================\r
77  * Functions\r
78  *=============================*/\r
79 \r
80 /*=================================================================\r
81  *  void clkDataIn(void)\r
82  *\r
83  *  This is a clock function that would start SWI to read input data.\r
84  *  In this case from a file/memory. It is statically configured in BIOS.\r
85  *=================================================================*/\r
86 \r
87 void clkDataIn(void)\r
88 {\r
89   Swi_post(swidatainhandle);\r
90 } /* clkDataIn */\r
91 \r
92 /*===========================================\r
93  *  SWI for getting data in from file/memory or A/D\r
94  *\r
95  *  Uses filRead() API to read frame of samples from "memory file".\r
96  *  Swaps the read/write pointers so that the system can read new data.\r
97  *  It wakes up the txTask to do that. It is statically configured in BIOS.\r
98  *\r
99  *===========================================*/\r
100 \r
101 /* A few debug stats */\r
102 tuint  swiDataIn_errcnt = 0uL;      /* count errors reading data */\r
103 tuint  swiDataIn_overflow = 0uL;    /* count how many times r/w pointers were the same */\r
104 tint   swiDataIn_last_error;        /* last error code */\r
105 \r
106 void swiDataIn(void)\r
107 {\r
108   tint err;\r
109   void *fid, *in_w, *in_r;\r
110 \r
111   in_w = sysContext.in_w;     /* Fetch the read and write pointers for input buffer */\r
112   in_r = sysContext.in_r;\r
113   if (in_r == in_w) {         /* this should never happen */\r
114     swiDataIn_overflow++;\r
115     return;\r
116   }\r
117 \r
118   /* Assume the write pointer is valid and use it to read the frame in */\r
119   fid = filGetHandle();\r
120   err = filRead(fid, SYS_FRAME_LENGTH, in_w, SYS_FRAME_LENGTH);\r
121   if (err != SYS_ERR_SUCCESS) {\r
122     swiDataIn_errcnt++;\r
123     swiDataIn_last_error = err;\r
124     return;\r
125   }\r
126   else {  /* Swap the r/w pointers */\r
127     sysContext.in_r = in_w;   /* we can do this because this SWI is at higher priority */\r
128     sysContext.in_w = in_r;   /* than the Tx Task and we know that r/w are different */\r
129     /* We assume that the Tx Task already finished using the read pointer */\r
130     /* So, next time we come we would write into the "other" buffer. */\r
131   }\r
132 \r
133   /* Here we need to post a semaphore to wake up the Tx task */\r
134   Semaphore_post(semTxWakeUp);\r
135 \r
136 } /* swiDataIn */\r
137 \r
138 /*===========================================\r
139  *  Tx Task\r
140  *\r
141  *  It wakes up on semaphore (semTxWakeUp).\r
142  *  It performs the following tasks:\r
143  *    - give next frame to the BF's\r
144  *    - Run ASNR at the output of each BF to reduce noise further\r
145  *    - Provide all virtual mic outputs to the MSS for selection\r
146  *    - Pass output of MSS through VAU for speech activity detection\r
147  *    - Write the output signal to the output buffer/file or D/A\r
148  *    - Profile execution of all major modules\r
149  *    - Trace beam selection and VAU activity\r
150  *\r
151  *===========================================*/\r
152 \r
153 #if (SYS_USE_DRC)\r
154 /* Output frame for MSS, input for DRC */\r
155 #pragma DATA_ALIGN(txOutFrame1,8)\r
156 linSample txOutFrame1[SYS_FRAME_LENGTH];\r
157 \r
158 /* Output frame for DRC, input for VAU */\r
159 #pragma DATA_ALIGN(txOutFrame2,8)\r
160 linSample txOutFrame2[SYS_FRAME_LENGTH];\r
161 #else\r
162 /* Output frame for MSS, input for VAU */\r
163 #pragma DATA_ALIGN(txOutFrame,8)\r
164 linSample txOutFrame[SYS_FRAME_LENGTH];\r
165 #endif\r
166 \r
167 typedef struct txBfDebug_stc {\r
168  tulong frmcnt;     /* normal frames */\r
169  tulong silcnt;     /* silence frames */\r
170  tuint  invsrc;     /* no mic active, invalid output */\r
171  tuint  invopt;     /* >1 mic active, invalid output */\r
172 } txBfDebug_t;\r
173 \r
174 typedef struct txTaskDebug_stc {\r
175   tuint overrun;                    /* counts how many times we ran out of MIPS */\r
176   txBfDebug_t bf[SYS_VMICS_MAX];    /* beamformer statistics */\r
177 } txTaskDebug_t;\r
178 \r
179 txTaskDebug_t txTaskDebug;      /* Tx task debug stats */\r
180 \r
181 /* Profiling/benchmarking information for the Tx task */\r
182 typedef struct txTaskProfileData_stc {\r
183   tulong  min;              /* Minimum number of cycles */\r
184   tulong  max;              /* Maximum number of cycles */\r
185   tulong  n;                /* Number of measurements */\r
186   float   total;            /* Total number of cycles */\r
187 } txTaskProfileData_t;\r
188 \r
189 typedef struct txTaskProfile_stc {\r
190   txTaskProfileData_t   bf;       /* Beamformer profile */\r
191   txTaskProfileData_t   asnr;     /* ASNR profile */\r
192   txTaskProfileData_t   mss;      /* MSS profile */\r
193   txTaskProfileData_t   drc;      /* DRC profile */\r
194   txTaskProfileData_t   vau;      /* VAU profile */\r
195 } txTaskProfile_t;\r
196 volatile txTaskProfile_t  txTaskProfile = {\r
197   {~(0uL), 0, 0, 0.0f},\r
198   {~(0uL), 0, 0, 0.0f},\r
199   {~(0uL), 0, 0, 0.0f},\r
200   {~(0uL), 0, 0, 0.0f},\r
201   {~(0uL), 0, 0, 0.0f}\r
202 };\r
203 \r
204 /* To be used for debug trace */\r
205 mssSrc_t    mssDbgCurSrc = {\r
206   -1, -1                        /* Current source group/index */\r
207 };\r
208 mssSrc_t    mssDbgNewSrc = {\r
209   -1, -1                        /* New source group/index */\r
210 };\r
211 \r
212 /* Tx Task main routine */\r
213 void taskTx(void)\r
214 {\r
215   Int       semcnt;     /* count from a counting semaphore */\r
216   int       k;          /* loop counter */\r
217   tint      nmics, nvmics, err, angle, alert, oldalert = -1;\r
218 \r
219   volatile tulong t1, t2;       /* for profiling */\r
220   tulong          delta;\r
221 \r
222   void      *inst_p, *fid;\r
223   linSample *in_r;                      /* pointer to current microphone input buffer */\r
224   linSample *frame_p;                   /* pointer to signal frame */\r
225   linSample *outframe_p;                /* Output frame pointer for VAU */\r
226   linSample *mics_in[SYS_MICS_MAX];     /* pointers to microphone inputs */\r
227 \r
228   mssDebugStat_t  mssDbg;\r
229 \r
230   ISPHENC1_FrameType vauOldFrameType;       /* previous speech/noise */\r
231   ISPHENC1_FrameType vauFrameType;          /* use for every frame */\r
232 \r
233   vauOldFrameType = (ISPHENC1_FrameType)255;    /* Start with invalid, update only on changes */\r
234 \r
235   memset(&txTaskDebug,0,sizeof(txTaskDebug));   /* zero debug stats */\r
236 \r
237   Log_print0(Diags_USER1,"taskTxStart");        /* Timestamp the beginning of the task */\r
238 \r
239   fid = filGetHandle();\r
240   loop {\r
241 \r
242     /*-------------------------------------------------------------------------------------*/\r
243     Semaphore_pend(semTxWakeUp, BIOS_WAIT_FOREVER);   /* wait for swiDataIn to wake you up */\r
244     /*-------------------------------------------------------------------------------------*/\r
245 \r
246     /* In a real system, it could make sense to wait with time-out and post error condition\r
247      *    if the time-out expires. For example, if the frame duration is 10ms, we could wait\r
248      *    for 15ms or longer. */\r
249 \r
250     semcnt = Semaphore_getCount(semTxWakeUp);\r
251     if (semcnt > 0) {\r
252       txTaskDebug.overrun++;  /* indicate we ran out of MIPS since we were asked to be waken up\r
253                                * multiple times before getting here. There were more events pending.\r
254                                * We are dropping a frame of input data here. */\r
255       continue;               /* Skip this pass through the loop to catch up to the last pending event */\r
256     }\r
257     nmics = sysContext.nmics;                   /* fetch number of mics */\r
258     in_r  = (linSample *)sysContext.in_r;       /* this was written by swiDataIn */\r
259     for (k = 0; k < nmics; k++) {\r
260       mics_in[k] = &in_r[k*SYS_FRAME_LENGTH];   /* find the frame start for each microphone */\r
261     }\r
262     /* consume samples pointed to by read pointer in_r as provided in misc_in[] */\r
263 \r
264     /* Here comes a lot of work */\r
265     /* We start with beamformers */\r
266 \r
267     nvmics = sysContext.nvmics;\r
268     t1 = Timestamp_get();\r
269     for (k = 0; k < nvmics; k++) {\r
270       inst_p  = sysContext.bfInst_p[k];     /* fetch the bf instance pointer */\r
271       frame_p = sysContext.vmicfrm[k];      /* point to the output frame buffer */\r
272 \r
273       err = bfProcess(inst_p, (void*)&mics_in[0], (void*)frame_p);\r
274 \r
275       if (err == bf_NOERR) {\r
276         txTaskDebug.bf[k].frmcnt++;             /* Record some debug info */\r
277       }\r
278       else if (err == bf_ERR_NOTOPENED) {\r
279         SYS_CHECK_ERROR(SYS_ERR_BFERROR);\r
280       }\r
281       else if (err == bf_ERR_DISABLED) {\r
282         txTaskDebug.bf[k].silcnt++;\r
283       }\r
284       else if (err == bf_ERR_INVALIDSRC) {\r
285         txTaskDebug.bf[k].invsrc = TRUE;\r
286       }\r
287       else if (err == bf_ERR_INVALIDOPT) {\r
288         txTaskDebug.bf[k].invopt = TRUE;\r
289       }\r
290       else {\r
291         SYS_CHECK_ERROR(SYS_ERR_BFERROR);\r
292       } /* if */\r
293     } /* for */\r
294     t2 = Timestamp_get();\r
295     delta = t2-t1;\r
296     txTaskProfile.bf.min = MIN(txTaskProfile.bf.min,delta);\r
297     txTaskProfile.bf.max = MAX(txTaskProfile.bf.max,delta);\r
298     txTaskProfile.bf.n++;\r
299     txTaskProfile.bf.total += (float)delta;\r
300 \r
301     /* At this point we have consumed all input samples. Currently we did not implement\r
302      * any protection to prevent the swiDataIn from stepping over while we were doing this.\r
303      * We could let this task to handle the read pointer and SWI to handle write pointer which \r
304      * could be used to detect if such overrun would happen. */\r
305 \r
306     /* Done with the beamformers */\r
307     /* Start ASNR's */\r
308 \r
309     t1 = Timestamp_get();\r
310     for (k = 0; k < nvmics; k++) {\r
311       inst_p  = sysContext.asnrInst_p[k];   /* fetch the bf instance pointer */\r
312       frame_p = sysContext.vmicfrm[k];      /* point to the output frame buffer */\r
313 \r
314       err = asnrProcess(inst_p, (void*)frame_p, (void*)frame_p);\r
315 \r
316       if (err != asnr_NOERR) {\r
317         SYS_CHECK_ERROR(SYS_ERR_ASNRERROR);\r
318       } /* if */\r
319     } /* for */\r
320     t2 = Timestamp_get();\r
321     delta = t2-t1;\r
322     txTaskProfile.asnr.min = MIN(txTaskProfile.asnr.min,delta);\r
323     txTaskProfile.asnr.max = MAX(txTaskProfile.asnr.max,delta);\r
324     txTaskProfile.asnr.n++;\r
325     txTaskProfile.asnr.total += (float)delta;\r
326 \r
327     /* Done with the ASNR's */\r
328     /* Run MSS */\r
329 \r
330     t1 = Timestamp_get();\r
331     inst_p  = sysContext.mssInst_p;         /* fetch the MSS instance pointer */\r
332 #if (SYS_USE_DRC)\r
333             frame_p = txOutFrame1;              /* point to the output frame buffer */\r
334 #else\r
335     frame_p = txOutFrame;                   /* point to the output frame buffer */\r
336 #endif\r
337 \r
338     err = mssProcess(inst_p, (void*)frame_p,      /* instance and output frame pointers */\r
339                      (void*)frame_p,              /* WORKAROUND (not used, but no NULL) */\r
340                      (void**)sysContext.vmicfrm,  /* Virtual microphones (beams) */\r
341                      NULL,                        /* No remote mics */\r
342                      NULL,                        /* No clean mics */\r
343                      (void**)mics_in,             /* Raw microphone array inputs */\r
344                      NULL);                       /* Beam not supported (see fixed inputs) */\r
345 \r
346     if (err != mss_NOERR) {\r
347       SYS_CHECK_ERROR(SYS_ERR_MSSERROR);\r
348     } /* if */\r
349     t2 = Timestamp_get();\r
350     delta = t2-t1;\r
351     txTaskProfile.mss.min = MIN(txTaskProfile.mss.min,delta);\r
352     txTaskProfile.mss.max = MAX(txTaskProfile.mss.max,delta);\r
353     txTaskProfile.mss.n++;\r
354     txTaskProfile.mss.total += (float)delta;\r
355 \r
356     /* Trace source selection */\r
357     /*    Write Args:\r
358      *      arg2: (value) Angle in degrees\r
359      *      arg3: (aux1)  0 - current source, 1 - new source\r
360      *      arg4: (aux2)  source index\r
361      */\r
362     err = mssDebugStat(inst_p, &mssDbg);\r
363     if (err !=mss_NOERR) {\r
364       SYS_CHECK_ERROR(SYS_ERR_MSSDEBUG);\r
365     }\r
366     /* mssDbg.cur_src.group/.index has the current source */\r
367     /* mssDbg.new_src.group/.index has "proposed" source */\r
368     if (mssDbg.cur_src.group != mssDbgCurSrc.group ||\r
369         mssDbg.cur_src.index != mssDbgCurSrc.index)\r
370     {\r
371       mssDbgCurSrc = mssDbg.cur_src;\r
372       angle = sysBfFilterAngles[sysBfVMicAngles[mssDbgCurSrc.index]];\r
373       Log_write6(UIAEvt_intWithKey, angle, 0, mssDbgCurSrc.index, (IArg)"MSS-C: %d, G:%d", 0, mssDbgCurSrc.group);\r
374     }\r
375     if (mssDbg.new_src.group != mssDbgNewSrc.group ||\r
376         mssDbg.new_src.index != mssDbgNewSrc.index)\r
377     {\r
378       mssDbgNewSrc = mssDbg.new_src;\r
379       angle = sysBfFilterAngles[sysBfVMicAngles[mssDbgNewSrc.index]];\r
380       Log_write6(UIAEvt_intWithKey, angle, 1, mssDbgNewSrc.index, (IArg)"MSS-N: %d, G:%d", 1, mssDbgNewSrc.group);\r
381     }\r
382 \r
383     /* Done with MSS */\r
384 \r
385 #if (SYS_USE_DRC)\r
386     /* Run DRC */\r
387     t1 = Timestamp_get();\r
388     inst_p      = sysContext.drcInst_p;     /* fetch the DRC instance pointer */\r
389     frame_p     = txOutFrame1;              /* point to the MSS output frame buffer and use it as input */\r
390     outframe_p  = txOutFrame2;              /* point to DRC output frame */\r
391     err = drcProcess(inst_p, frame_p,       /* instance and input frame pointers */\r
392                      outframe_p);           /* pointer to output buffer pointer */\r
393     t2 = Timestamp_get();\r
394     delta = t2-t1;\r
395     txTaskProfile.drc.min = MIN(txTaskProfile.drc.min,delta);\r
396     txTaskProfile.drc.max = MAX(txTaskProfile.drc.max,delta);\r
397     txTaskProfile.drc.n++;\r
398     txTaskProfile.drc.total += (float)delta;\r
399     /* Done with DRC */\r
400 #else\r
401     outframe_p = frame_p;\r
402 #endif\r
403 \r
404     /* Run VAU */\r
405 #if 0\r
406     t1 = Timestamp_get();\r
407     inst_p  = sysContext.vauInst_p;         /* fetch the VAU instance pointer */\r
408     frame_p = txOutFrame;                   /* point to the Tx output frame buffer and use it as input */\r
409     alert = (tint)vauProcess(inst_p, frame_p,   /* instance and input frame pointers */\r
410                              &outframe_p,       /* pointer to output buffer pointer */\r
411                              &vauFrameType);    /* Indicating speech/noise */\r
412                                                 /*  ISPHENC1_FTYPE_SPEECH or ISPHENC1_FTYPE_NODATA */\r
413     t2 = Timestamp_get();\r
414     delta = t2-t1;\r
415     txTaskProfile.vau.min = MIN(txTaskProfile.vau.min,delta);\r
416     txTaskProfile.vau.max = MAX(txTaskProfile.vau.max,delta);\r
417     txTaskProfile.vau.n++;\r
418     txTaskProfile.vau.total += (float)delta;\r
419 \r
420     if (vauFrameType != vauOldFrameType) {\r
421       vauOldFrameType = vauFrameType;         /* Record new frame type */\r
422 \r
423       /* Trace source selection */\r
424       /*    Write Args:\r
425        *      arg2: (value) 2-(VAU Frame Type)  So, silence will be 0, speech will be 2\r
426        *      arg3: (aux1)  2 - indicate VAU trace\r
427        *      arg4: (aux2)  0\r
428        */\r
429 \r
430       Log_write4(UIAEvt_intWithKey, 15*(ISPHENC1_FTYPE_NODATA-vauFrameType), 2, 0, (IArg)"VAU-FT");\r
431     }\r
432     if (alert != oldalert) {\r
433       oldalert = alert;         /* Record new alert state */\r
434 \r
435       /* Trace source selection */\r
436       /*    Write Args:\r
437        *      arg2: (value) Alert status\r
438        *      arg3: (aux1)  3 - indicate VAU alert trace\r
439        *      arg4: (aux2)  0\r
440        */\r
441 \r
442       Log_write4(UIAEvt_intWithKey, 20*alert, 3, 0, (IArg)"VAU-ALERT");\r
443     }\r
444 #endif\r
445 \r
446     /*---------------------------------*/\r
447     /* Save samples to the output file */\r
448     /*---------------------------------*/\r
449 \r
450     err = filWrite(fid, SYS_FRAME_LENGTH, outframe_p);\r
451     if (err != SYS_ERR_SUCCESS) {\r
452       if (err == SYS_ERR_EOF) {\r
453         if (!sysContext.eof) {\r
454           sysContext.eof = TRUE;\r
455           System_printf("taskTx: EOF Reached.\n");\r
456           System_flush();\r
457         }\r
458       }\r
459       else {\r
460         SYS_CHECK_ERROR(err);\r
461       }\r
462     } /* if */\r
463   } /* loop */\r
464 } /* taskTx */\r
465 \r
466 /* nothing past this point */\r
467 \r