38f7175e1bdc1989c6ccb2cc7966bd8d220a50ae
[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 <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 #if 0\r
231   ISPHENC1_FrameType vauOldFrameType;       /* previous speech/noise */\r
232   ISPHENC1_FrameType vauFrameType;          /* use for every frame */\r
233 \r
234   vauOldFrameType = (ISPHENC1_FrameType)255;    /* Start with invalid, update only on changes */\r
235 #endif\r
236 \r
237   memset(&txTaskDebug,0,sizeof(txTaskDebug));   /* zero debug stats */\r
238 \r
239   Log_print0(Diags_USER1,"taskTxStart");        /* Timestamp the beginning of the task */\r
240 \r
241   fid = filGetHandle();\r
242   loop {\r
243 \r
244     /*-------------------------------------------------------------------------------------*/\r
245     Semaphore_pend(semTxWakeUp, BIOS_WAIT_FOREVER);   /* wait for swiDataIn to wake you up */\r
246     /*-------------------------------------------------------------------------------------*/\r
247 \r
248     /* In a real system, it could make sense to wait with time-out and post error condition\r
249      *    if the time-out expires. For example, if the frame duration is 10ms, we could wait\r
250      *    for 15ms or longer. */\r
251 \r
252     semcnt = Semaphore_getCount(semTxWakeUp);\r
253     if (semcnt > 0) {\r
254       txTaskDebug.overrun++;  /* indicate we ran out of MIPS since we were asked to be waken up\r
255                                * multiple times before getting here. There were more events pending.\r
256                                * We are dropping a frame of input data here. */\r
257       continue;               /* Skip this pass through the loop to catch up to the last pending event */\r
258     }\r
259     nmics = sysContext.nmics;                   /* fetch number of mics */\r
260     in_r  = (linSample *)sysContext.in_r;       /* this was written by swiDataIn */\r
261     for (k = 0; k < nmics; k++) {\r
262       mics_in[k] = &in_r[k*SYS_FRAME_LENGTH];   /* find the frame start for each microphone */\r
263     }\r
264     /* consume samples pointed to by read pointer in_r as provided in misc_in[] */\r
265 \r
266     /* Here comes a lot of work */\r
267     /* We start with beamformers */\r
268 \r
269     nvmics = sysContext.nvmics;\r
270     t1 = Timestamp_get();\r
271     for (k = 0; k < nvmics; k++) {\r
272       inst_p  = sysContext.bfInst_p[k];     /* fetch the bf instance pointer */\r
273       frame_p = sysContext.vmicfrm[k];      /* point to the output frame buffer */\r
274 \r
275       err = bfProcess(inst_p, (void*)&mics_in[0], (void*)frame_p);\r
276 \r
277       if (err == bf_NOERR) {\r
278         txTaskDebug.bf[k].frmcnt++;             /* Record some debug info */\r
279       }\r
280       else if (err == bf_ERR_NOTOPENED) {\r
281         SYS_CHECK_ERROR(SYS_ERR_BFERROR);\r
282       }\r
283       else if (err == bf_ERR_DISABLED) {\r
284         txTaskDebug.bf[k].silcnt++;\r
285       }\r
286       else if (err == bf_ERR_INVALIDSRC) {\r
287         txTaskDebug.bf[k].invsrc = TRUE;\r
288       }\r
289       else if (err == bf_ERR_INVALIDOPT) {\r
290         txTaskDebug.bf[k].invopt = TRUE;\r
291       }\r
292       else {\r
293         SYS_CHECK_ERROR(SYS_ERR_BFERROR);\r
294       } /* if */\r
295     } /* for */\r
296     t2 = Timestamp_get();\r
297     delta = t2-t1;\r
298     txTaskProfile.bf.min = MIN(txTaskProfile.bf.min,delta);\r
299     txTaskProfile.bf.max = MAX(txTaskProfile.bf.max,delta);\r
300     txTaskProfile.bf.n++;\r
301     txTaskProfile.bf.total += (float)delta;\r
302 \r
303     /* At this point we have consumed all input samples. Currently we did not implement\r
304      * any protection to prevent the swiDataIn from stepping over while we were doing this.\r
305      * We could let this task to handle the read pointer and SWI to handle write pointer which \r
306      * could be used to detect if such overrun would happen. */\r
307 \r
308     /* Done with the beamformers */\r
309     /* Start ASNR's */\r
310 \r
311     t1 = Timestamp_get();\r
312     for (k = 0; k < nvmics; k++) {\r
313       inst_p  = sysContext.asnrInst_p[k];   /* fetch the bf instance pointer */\r
314       frame_p = sysContext.vmicfrm[k];      /* point to the output frame buffer */\r
315 \r
316       err = asnrProcess(inst_p, (void*)frame_p, (void*)frame_p);\r
317 \r
318       if (err != asnr_NOERR) {\r
319         SYS_CHECK_ERROR(SYS_ERR_ASNRERROR);\r
320       } /* if */\r
321     } /* for */\r
322     t2 = Timestamp_get();\r
323     delta = t2-t1;\r
324     txTaskProfile.asnr.min = MIN(txTaskProfile.asnr.min,delta);\r
325     txTaskProfile.asnr.max = MAX(txTaskProfile.asnr.max,delta);\r
326     txTaskProfile.asnr.n++;\r
327     txTaskProfile.asnr.total += (float)delta;\r
328 \r
329     /* Done with the ASNR's */\r
330     /* Run MSS */\r
331 \r
332     t1 = Timestamp_get();\r
333     inst_p  = sysContext.mssInst_p;         /* fetch the MSS instance pointer */\r
334 #if (SYS_USE_DRC)\r
335             frame_p = txOutFrame1;              /* point to the output frame buffer */\r
336 #else\r
337     frame_p = txOutFrame;                   /* point to the output frame buffer */\r
338 #endif\r
339 \r
340     err = mssProcess(inst_p, (void*)frame_p,      /* instance and output frame pointers */\r
341                      (void*)frame_p,              /* WORKAROUND (not used, but no NULL) */\r
342                      (void**)sysContext.vmicfrm,  /* Virtual microphones (beams) */\r
343                      NULL,                        /* No remote mics */\r
344                      NULL,                        /* No clean mics */\r
345                      (void**)mics_in,             /* Raw microphone array inputs */\r
346                      NULL);                       /* Beam not supported (see fixed inputs) */\r
347 \r
348     if (err != mss_NOERR) {\r
349       SYS_CHECK_ERROR(SYS_ERR_MSSERROR);\r
350     } /* if */\r
351     t2 = Timestamp_get();\r
352     delta = t2-t1;\r
353     txTaskProfile.mss.min = MIN(txTaskProfile.mss.min,delta);\r
354     txTaskProfile.mss.max = MAX(txTaskProfile.mss.max,delta);\r
355     txTaskProfile.mss.n++;\r
356     txTaskProfile.mss.total += (float)delta;\r
357 \r
358     /* Trace source selection */\r
359     /*    Write Args:\r
360      *      arg2: (value) Angle in degrees\r
361      *      arg3: (aux1)  0 - current source, 1 - new source\r
362      *      arg4: (aux2)  source index\r
363      */\r
364     err = mssDebugStat(inst_p, &mssDbg);\r
365     if (err !=mss_NOERR) {\r
366       SYS_CHECK_ERROR(SYS_ERR_MSSDEBUG);\r
367     }\r
368     /* mssDbg.cur_src.group/.index has the current source */\r
369     /* mssDbg.new_src.group/.index has "proposed" source */\r
370     if (mssDbg.cur_src.group != mssDbgCurSrc.group ||\r
371         mssDbg.cur_src.index != mssDbgCurSrc.index)\r
372     {\r
373       mssDbgCurSrc = mssDbg.cur_src;\r
374       angle = sysBfFilterAngles[sysBfVMicAngles[mssDbgCurSrc.index]];\r
375       Log_write6(UIAEvt_intWithKey, angle, 0, mssDbgCurSrc.index, (IArg)"MSS-C: %d, G:%d", 0, mssDbgCurSrc.group);\r
376     }\r
377     if (mssDbg.new_src.group != mssDbgNewSrc.group ||\r
378         mssDbg.new_src.index != mssDbgNewSrc.index)\r
379     {\r
380       mssDbgNewSrc = mssDbg.new_src;\r
381       angle = sysBfFilterAngles[sysBfVMicAngles[mssDbgNewSrc.index]];\r
382       Log_write6(UIAEvt_intWithKey, angle, 1, mssDbgNewSrc.index, (IArg)"MSS-N: %d, G:%d", 1, mssDbgNewSrc.group);\r
383     }\r
384 \r
385     /* Done with MSS */\r
386 \r
387 #if (SYS_USE_DRC)\r
388     /* Run DRC */\r
389     t1 = Timestamp_get();\r
390     inst_p      = sysContext.drcInst_p;     /* fetch the DRC instance pointer */\r
391     frame_p     = txOutFrame1;              /* point to the MSS output frame buffer and use it as input */\r
392     outframe_p  = txOutFrame2;              /* point to DRC output frame */\r
393     err = drcProcess(inst_p, frame_p,       /* instance and input frame pointers */\r
394                      outframe_p);           /* pointer to output buffer pointer */\r
395     t2 = Timestamp_get();\r
396     delta = t2-t1;\r
397     txTaskProfile.drc.min = MIN(txTaskProfile.drc.min,delta);\r
398     txTaskProfile.drc.max = MAX(txTaskProfile.drc.max,delta);\r
399     txTaskProfile.drc.n++;\r
400     txTaskProfile.drc.total += (float)delta;\r
401     /* Done with DRC */\r
402 #else\r
403     outframe_p = frame_p;\r
404 #endif\r
405 \r
406     /* Run VAU */\r
407 #if 0\r
408     t1 = Timestamp_get();\r
409     inst_p  = sysContext.vauInst_p;         /* fetch the VAU instance pointer */\r
410     frame_p = txOutFrame;                   /* point to the Tx output frame buffer and use it as input */\r
411     alert = (tint)vauProcess(inst_p, frame_p,   /* instance and input frame pointers */\r
412                              &outframe_p,       /* pointer to output buffer pointer */\r
413                              &vauFrameType);    /* Indicating speech/noise */\r
414                                                 /*  ISPHENC1_FTYPE_SPEECH or ISPHENC1_FTYPE_NODATA */\r
415     t2 = Timestamp_get();\r
416     delta = t2-t1;\r
417     txTaskProfile.vau.min = MIN(txTaskProfile.vau.min,delta);\r
418     txTaskProfile.vau.max = MAX(txTaskProfile.vau.max,delta);\r
419     txTaskProfile.vau.n++;\r
420     txTaskProfile.vau.total += (float)delta;\r
421 \r
422     if (vauFrameType != vauOldFrameType) {\r
423       vauOldFrameType = vauFrameType;         /* Record new frame type */\r
424 \r
425       /* Trace source selection */\r
426       /*    Write Args:\r
427        *      arg2: (value) 2-(VAU Frame Type)  So, silence will be 0, speech will be 2\r
428        *      arg3: (aux1)  2 - indicate VAU trace\r
429        *      arg4: (aux2)  0\r
430        */\r
431 \r
432       Log_write4(UIAEvt_intWithKey, 15*(ISPHENC1_FTYPE_NODATA-vauFrameType), 2, 0, (IArg)"VAU-FT");\r
433     }\r
434     if (alert != oldalert) {\r
435       oldalert = alert;         /* Record new alert state */\r
436 \r
437       /* Trace source selection */\r
438       /*    Write Args:\r
439        *      arg2: (value) Alert status\r
440        *      arg3: (aux1)  3 - indicate VAU alert trace\r
441        *      arg4: (aux2)  0\r
442        */\r
443 \r
444       Log_write4(UIAEvt_intWithKey, 20*alert, 3, 0, (IArg)"VAU-ALERT");\r
445     }\r
446 #endif\r
447 \r
448     /*---------------------------------*/\r
449     /* Save samples to the output file */\r
450     /*---------------------------------*/\r
451 \r
452     err = filWrite(fid, SYS_FRAME_LENGTH, outframe_p);\r
453     if (err != SYS_ERR_SUCCESS) {\r
454       if (err == SYS_ERR_EOF) {\r
455         if (!sysContext.eof) {\r
456           sysContext.eof = TRUE;\r
457           System_printf("taskTx: EOF Reached.\n");\r
458           System_flush();\r
459         }\r
460       }\r
461       else {\r
462         SYS_CHECK_ERROR(err);\r
463       }\r
464     } /* if */\r
465   } /* loop */\r
466 } /* taskTx */\r
467 \r
468 /* nothing past this point */\r
469 \r