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