硬核實戰 | Cortex-M85平臺Speex回聲消除與降噪算法移植指南

#ifndef OS_SUPPORT_CUSTOM_H#define OS_SUPPORT_CUSTOM_H#include <rtthread.h>#include "xprintf.h"#define OVERRIDE_SPEEX_ALLOC#define OVERRIDE_SPEEX_ALLOC_SCRATCH#define OVERRIDE_SPEEX_REALLOC#define OVERRIDE_SPEEX_FREE#define OVERRIDE_SPEEX_FREE_SCRATCHstatic size_t s_max_used = 0;static size_t s_cur_used = 0;static inline void *speex_alloc(int size){    void* p = rt_malloc(size+4);    memset(p,0,size);    *(uint32_t*)p = size;  /* 記錄分配的大小 用于調試使用*/    s_cur_used += size;    if(s_max_used < s_cur_used){        s_max_used = s_cur_used;    }    //xprintf("malloc:max:%u,cur:%u\r\n",s_max_used,s_cur_used);    return (uint8_t*)p+4;}static inline void *speex_alloc_scratch(int size){   /* Scratch space doesn't need to be cleared */    void* p = rt_malloc(size+4);  /* freertos malloc默認配置是8字節對齊,所以預留的4字節強制類型轉換不會有對齊問題 */    *(uint32_t*)p = size;  /* 記錄分配的大小 用于調試使用*/    s_cur_used += size;    if(s_max_used < s_cur_used){        s_max_used = s_cur_used;    }    //xprintf("malloc:max:%u,cur:%u\r\n",s_max_used,s_cur_used);    return (uint8_t*)p+4;}static inline void speex_free(void *ptr){    rt_free((uint8_t*)ptr-4);    s_cur_used -= *(uint32_t*)((uint8_t*)ptr-4);    //xprintf("free:max:%u,cur:%u\r\n",s_max_used,s_cur_used);}static inline void *speex_realloc(void *ptr, int size){    uint32_t len;    /* 原來為空直接申請 */    if(ptr == (void*)0){        return speex_alloc(size);    }    /* size為0則直接釋放 */    if(size == 0){        speex_free(ptr);        return (void*)0;    }    /* 申請新的大小 */    void* p = rt_malloc(size+4);    *(uint32_t*)p = size;  /* 記錄分配的大小 */    s_cur_used += size;    if(s_max_used < s_cur_used){        s_max_used = s_cur_used;    }    len = *(uint32_t*)((uint8_t*)ptr-4);    if(len > (uint32_t)size){        len = (uint32_t)size;  /* 注意可能放大也可能縮小 */    }    /* 復制的數據 */    memcpy((uint8_t*)p+4,ptr,len);    /* 釋放原來的 */    rt_free((uint8_t*)ptr-4);    s_cur_used -= len;    //xprintf("realloc:max:%u,cur:%u\r\n",s_max_used,s_cur_used);    return (uint8_t*)p+4;}static inline void speex_free_scratch(void *ptr){    rt_free((uint8_t*)ptr-4);    s_cur_used -= *(uint32_t*)((uint8_t*)ptr-4);    //xprintf("free:max:%u,cur:%u\r\n",s_max_used,s_cur_used);}#define OVERRIDE_SPEEX_COPY#define OVERRIDE_SPEEX_MOVE#define OVERRIDE_SPEEX_MEMSET#define SPEEX_COPY(dst, src, n) (memcpy((dst), (src), (n)*sizeof(*(dst)) + 0*((dst)-(src)) ))#define SPEEX_MOVE(dst, src, n) (memmove((dst), (src), (n)*sizeof(*(dst)) + 0*((dst)-(src)) ))#define SPEEX_MEMSET(dst, c, n) (memset((dst), (c), (n)*sizeof(*(dst))))#define OVERRIDE_SPEEX_FATALstatic inline void _speex_fatal(constchar *str, constchar *file, int line){   xprintf("Fatal (internal) error in %s, line %d: %s\n", file, line, str);   while(1);}#define OVERRIDE_SPEEX_WARNINGstatic inline void speex_warning(constchar *str){#ifndef DISABLE_WARNINGS   xprintf ("warning: %s\n", str);#endif}#define OVERRIDE_SPEEX_WARNING_INTstatic inline void speex_warning_int(constchar *str, int val){#ifndef DISABLE_WARNINGS   xprintf ("warning: %s %d\n", str, val);#endif}#define OVERRIDE_SPEEX_NOTIFYstatic inline void speex_notify(constchar *str){#ifndef DISABLE_NOTIFICATIONS   xprintf ("notification: %s\n", str);#endif}#define OVERRIDE_SPEEX_PUTCstatic inline void _speex_putc(int ch, void *file){   (void)file;   xprintf("%c", ch);}#define speex_fatal(str) _speex_fatal(str, __FILE__, __LINE__);#define speex_assert(cond) {if (!(cond)) {speex_fatal("assertion failed: " #cond);}}#ifndef RELEASEstatic inline void print_vec(float *vec, int len, char *name){   int i;   xprintf ("%s ", name);   for (i=0;i<len;i++)      xprintf (" %f", vec[i]);   xprintf ("\n");}#endif#endif

#ifdef HAVE_CONFIG_H#include "config.h"#endif#include <rtthread.h>#include "speex/speex_echo.h"#include "speex/speex_preprocess.h"#include <stdio.h>#include <stdlib.h>#include <string.h>#include "xprintf.h"static uint32_tget_tm_ms(void){    return rt_tick_get()*1000/RT_TICK_PER_SECOND;}/* WAV解析 */#define CHUNK_RIFF "RIFF"#define CHUNK_WAVE "WAVE"#define CHUNK_FMT "fmt "#define CHUNK_DATA "data"typedef struct{    uint32_t off;    uint32_t chunksize;    uint16_t audioformat;    uint16_t numchannels;    uint32_t samplerate;    uint32_t byterate;    uint16_t blockalign;    uint16_t bitspersample;    uint32_t datasize;}wav_t;static int wav_decode_head(uint8_t* buffer, wav_t* wav){    uint8_t* p = buffer;    uint32_t chunksize;    uint32_t subchunksize;    if(0 != memcmp(p,CHUNK_RIFF,4))    {        return -1;;    }    p += 4;    chunksize = (uint32_t)p[0] | ((uint32_t)p[1]<<8) | ((uint32_t)p[2]<<16) | ((uint32_t)p[3]<<24);    wav->chunksize = chunksize;    p += 4;    if(0 != memcmp(p,CHUNK_WAVE,4))    {        return -2;    }    p += 4;    do    {        if(0 == memcmp(p,CHUNK_FMT,4))        {            p += 4;            subchunksize = (uint32_t)p[0] | ((uint32_t)p[1]<<8) | ((uint32_t)p[2]<<16) | ((uint32_t)p[3]<<24);            p += 4;            /* 解析參數 */            wav->audioformat = (uint16_t)p[0] | ((uint16_t)p[1]<<8);            if((wav->audioformat == 0x0001) || (wav->audioformat == 0xFFFE))            {                p += 2;                wav->numchannels = (uint16_t)p[0] | ((uint16_t)p[1]<<8);                p += 2;                wav->samplerate = (uint32_t)p[0] | ((uint32_t)p[1]<<8) | ((uint32_t)p[2]<<16) | ((uint32_t)p[3]<<24);                p += 4;                wav->byterate = (uint32_t)p[0] | ((uint32_t)p[1]<<8) | ((uint32_t)p[2]<<16) | ((uint32_t)p[3]<<24);                p += 4;                wav->blockalign = (uint16_t)p[0] | ((uint16_t)p[1]<<8);                p += 2;                wav ->bitspersample = (uint16_t)p[0] | ((uint16_t)p[1]<<8);                p += 2;                if(subchunksize >16)                {                    /* 有ext區域 */                    uint16_t cbsize = (uint16_t)p[0] | ((uint16_t)p[1]<<8);                    p += 2;                    if(cbsize > 0)                    {                        /* ext數據 2字節有效bits wValidBitsPerSample ，4字節dwChannelMask 16字節SubFormat */                        p += 2;                        p += 4;                        /* 比對subformat */                        p += 16;                           }                }            }            else            {                p += subchunksize;            }        }        else if(0 == memcmp(p,CHUNK_DATA,4))        {            p += 4;            subchunksize = (uint32_t)p[0] | ((uint32_t)p[1]<<8) | ((uint32_t)p[2]<<16) | ((uint32_t)p[3]<<24);            wav->datasize = subchunksize;            p += 4;            wav->off = (uint32_t)(p- buffer);            return 0;        }        else        {            p += 4;            subchunksize = (uint32_t)p[0] | ((uint32_t)p[1]<<8) | ((uint32_t)p[2]<<16) | ((uint32_t)p[3]<<24);            p += 4;            p += subchunksize;        }    }while((uint32_t)(p - buffer) < (chunksize + 8));    return -3;}/* 填充44字節的wav頭 */static void wav_fill_head(uint8_t* buffer, int samples, int chnum, int freq){    /*     * 添加wav頭信息     */    uint32_t chunksize = 44-8+samples*chnum*16/8;    uint8_t* p = (uint8_t*)buffer;    uint32_t bps = freq*chnum*16/8;    uint32_t datalen = samples*chnum*16/8;    p[0] = 'R';    p[1] = 'I';    p[2] = 'F';    p[3] = 'F';    p[4] = chunksize & 0xFF;    p[5] = (chunksize>>8) & 0xFF;    p[6] = (chunksize>>16) & 0xFF;    p[7] = (chunksize>>24) & 0xFF;    p[8] = 'W';    p[9] = 'A';    p[10] = 'V';    p[11] = 'E';    p[12] = 'f';    p[13] = 'm';    p[14] = 't';    p[15] = ' ';    p[16] = 16;  /* Subchunk1Size */    p[17] = 0;    p[18] = 0;    p[19] = 0;    p[20] = 1;  /* PCM */    p[21] = 0;    p[22] = chnum; /* 通道數 */    p[23] = 0;    p[24] = freq & 0xFF;    p[25] = (freq>>8) & 0xFF;    p[26] = (freq>>16) & 0xFF;    p[27] = (freq>>24) & 0xFF;     p[28] = bps & 0xFF;      /* ByteRate */    p[29] = (bps>>8) & 0xFF;    p[30] = (bps>>16) & 0xFF;    p[31] = (bps>>24) & 0xFF;     p[32] = chnum*16/8; /* BlockAlign */    p[33] = 0;    p[34] = 16;  /* BitsPerSample */    p[35] = 0;    p[36] = 'd';    p[37] = 'a';    p[38] = 't';    p[39] = 'a';    p[40] = datalen & 0xFF;    p[41] = (datalen>>8) & 0xFF;    p[42] = (datalen>>16) & 0xFF;    p[43] = (datalen>>24) & 0xFF; }void wav_print(wav_t* wav){   xprintf("off:%d\r\n",wav->off);   xprintf("chunksize:%d\r\n",wav->chunksize);   xprintf("audioformat:%d\r\n",wav->audioformat);   xprintf("numchannels:%d\r\n",wav->numchannels);   xprintf("samplerate:%d\r\n",wav->samplerate);   xprintf("byterate:%d\r\n",wav->byterate);   xprintf("blockalign:%d\r\n",wav->blockalign);   xprintf("bitspersample:%d\r\n",wav->bitspersample);   xprintf("datasize:%d\r\n",wav->datasize);}#define NN 128#define TAIL 1024int speexecho(int argc, char **argv){   FILE *spk_fd, *mic_fd, *out_fd;      short spk_buf[NN], mic_buf[NN], out_buf[NN];   uint8_t spk_wav_buf[44]; /* 輸入spk wav文件頭緩存 */   uint8_t mic_wav_buf[44]; /* 輸入mic wav文件頭緩存 */   uint8_t out_wav_buf[44]; /* 輸出文件wav頭緩存 */   wav_t spk_wav;   wav_t mic_wav;   int samps;  /* 采樣點數 */   int times;    /* 讀取次數 */   SpeexEchoState *st;   SpeexPreprocessState *den;   int sampleRate;   char* mic_fname = argv[1];   char* spk_fname = argv[2];   char* out_fname = argv[3];   int ctl_i;   float ctl_f;   if (argc != 4)   {      xprintf("testecho mic.wav spk.wav out.wav\n");      return -1;   }   spk_fd = fopen(spk_fname, "rb");   if(spk_fd == NULL){      xprintf("open file %s err\n",spk_fname);      return -1;   }else{      xprintf("open file %s ok\n",spk_fname);   }   mic_fd  = fopen(mic_fname,  "rb");   if(mic_fd == NULL){      xprintf("open file %s err\n",mic_fname);      fclose(spk_fd);      return -1;   }else{      xprintf("open file %s ok\n",mic_fname);   }   out_fd    = fopen(out_fname, "wb");   if(out_fd == NULL){      xprintf("open file %s err\n",out_fname);      fclose(spk_fd);      fclose(mic_fd);      return-1;   }else{      xprintf("open file %s ok\n",out_fname);   }   if(44 != fread(mic_wav_buf, 1, 44, mic_fd)){      xprintf("read file %s err\n",mic_fname);      fclose(spk_fd);      fclose(mic_fd);      fclose(out_fd);      return-1;   }else{      xprintf("read file %s ok\n",mic_fname);   }   if(44 != fread(spk_wav_buf, 1, 44, spk_fd)){      xprintf("read file %s err\n",spk_fname);      fclose(spk_fd);      fclose(mic_fd);      fclose(out_fd);      return-1;   }else{      xprintf("read file %s ok\n",spk_fname);   }   if(0 != wav_decode_head(spk_wav_buf, &spk_wav)){      xprintf("decode file %s err\n",spk_fname);      fclose(spk_fd);      fclose(mic_fd);      fclose(out_fd);      return -1;   }else{      xprintf("decode file %s ok\n",spk_fname);   }   xprintf("[spk_wav]\r\n");   wav_print(&spk_wav);   if(0 != wav_decode_head(mic_wav_buf, &mic_wav)){      xprintf("decode file %s err\n",mic_fname);      fclose(spk_fd);      fclose(mic_fd);      fclose(out_fd);      return -1;   }else{      xprintf("decode file %s ok\n",mic_fname);   }   xprintf("[mic_wav]\r\n");   wav_print(&mic_wav);
   samps = spk_wav.datasize > mic_wav.datasize ? mic_wav.datasize : spk_wav.datasize; /* 獲取較小的數據大小 */   samps /= spk_wav.blockalign;  /* 采樣點數 =  數據大小 除以 blockalign */   xprintf("\r\nsamps:%d\r\n",samps);
   sampleRate = spk_wav.samplerate;
   wav_fill_head(out_wav_buf, samps, 1, sampleRate);  /* 輸出文件頭 */   if(44 != fwrite(out_wav_buf, 1, 44, out_fd)){      xprintf("write file %s err\n",out_fname);      fclose(spk_fd);      fclose(mic_fd);      fclose(out_fd);      return -1;   }
   st = speex_echo_state_init(NN, TAIL);   den = speex_preprocess_state_init(NN, sampleRate);   speex_echo_ctl(st, SPEEX_ECHO_SET_SAMPLING_RATE, &sampleRate);   ctl_i=1;   speex_preprocess_ctl(den, SPEEX_PREPROCESS_SET_DENOISE, &ctl_i); /* 打開降噪 ctl_i=1打開 0關閉*/   ctl_i=80;   speex_preprocess_ctl(den, SPEEX_PREPROCESS_SET_NOISE_SUPPRESS, &ctl_i);    ctl_i=80;   speex_preprocess_ctl(den, SPEEX_PREPROCESS_SET_ECHO_SUPPRESS, &ctl_i);   times = samps / NN;   /* 一次讀取NN個點,讀取times次 */   for(int i=0; i<times; i++)   {      if(NN != fread(mic_buf, sizeof(short), NN, mic_fd)){        xprintf("read file %s err\n",mic_fname);        fclose(spk_fd);        fclose(mic_fd);        fclose(out_fd);        return -1;      }      if(NN != fread(spk_buf, sizeof(short), NN, spk_fd)){        xprintf("read file %s err\n",spk_fname);        fclose(spk_fd);        fclose(mic_fd);        fclose(out_fd);        return -1;      }      uint32_t t0;      uint32_t t1;    t0 = get_tm_ms();      speex_echo_cancellation(st, mic_buf, spk_buf, out_buf);      speex_preprocess_run(den, out_buf);      t1 = get_tm_ms();      xprintf("used:%dmS\r\n",(t1-t0));      if(NN != fwrite(out_buf, sizeof(short), NN, out_fd)){        xprintf("write file %s err\n",out_fname);        fclose(spk_fd);        fclose(mic_fd);        fclose(out_fd);        return -1;      }   }   speex_echo_state_destroy(st);   speex_preprocess_state_destroy(den);   fclose(out_fd);   fclose(spk_fd);   fclose(mic_fd);   return0;}

static void speexechofunc(uint8_t* param);{ (uint8_t*)"speexecho", speexechofunc, (uint8_t*)"speexecho mic spk out"},extern int speexecho(int argc, char **argv);static void speexechofunc(uint8_t * cmdbuf){int len;char mic[32];char spk[32];char out[32];char *argv[4];argv[0]="speexecho";argv[1]=mic;argv[2]=spk;argv[3]=out;memset((void *)mic, 0, sizeof(mic));memset((void *)spk, 0, sizeof(spk));memset((void *)out, 0, sizeof(out));len = sscanf((charconst *)cmdbuf, "%*s %31s %31s %31s", mic,spk,out);if (len == 3){speexecho(4, argv);}else{}}