Logo Search packages:      
Sourcecode: baycomusb version File versions  Download package

audio.c

/*****************************************************************************/

/*
 *      audio.c  --  Audio processing for "virtual transceiver".
 *
 *      Copyright (C) 1999, 2001  Thomas Sailer (t.sailer@alumni.ethz.ch)
 *
 *      This program is free software; you can redistribute it and/or modify
 *      it under the terms of the GNU General Public License as published by
 *      the Free Software Foundation; either version 2 of the License, or
 *      (at your option) any later version.
 *
 *      This program is distributed in the hope that it will be useful,
 *      but WITHOUT ANY WARRANTY; without even the implied warranty of
 *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *      GNU General Public License for more details.
 *
 *      You should have received a copy of the GNU General Public License
 *      along with this program; if not, write to the Free Software
 *      Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 *  Please note that the GPL allows you to use the driver, NOT the radio.
 *  In order to use the radio, you need a license from the communications
 *  authority of your country.
 *
 */

/*****************************************************************************/

#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

/* AIX requires this to be the first thing in the file.  */
#ifndef __GNUC__
# if HAVE_ALLOCA_H
#  include <alloca.h>
# else
#  ifdef _AIX
#pragma alloca
#  else
#   ifndef alloca /* predefined by HP cc +Olibcalls */
char *alloca ();
#   endif
#  endif
# endif
#endif

#include <math.h>
#include "trx.h"
#include "simd.h"

/* ---------------------------------------------------------------------- */

#define PWRINTERVAL     4096         /* must be even */
#define PWRINTERVAL2      64         /* must be even */
#define PWRNOISECALIB   4096

#define RCOSALPHA 0.4
//#define FILTERRELAX 1.4
#define FILTERRELAX 1.0

#define MAXSAMPLERATE  0.5  /* relative to AUDIOSAMPLERATE */

/* --------------------------------------------------------------------- */

static inline double sinc(double x)
{
        double arg = x * M_PI;

        if (fabs(arg) < 1e-10)
                return 1;
        return sin(arg) / arg;
}

static inline double hamming(double x)
{
        return 0.54-0.46*cos(2*M_PI*x);
}

static void compute_filter(double tmul, int16_t filter[AUDIONUMFILTER][AUDIOFILTERLEN])
{
      int i, j;
        float f1, f2, time, alphatime;
        float c[AUDIONUMFILTER * AUDIOFILTERLEN];

      if (tmul > MAXSAMPLERATE)
            tmul = MAXSAMPLERATE;
#if 0
      tmul *= (FILTERRELAX / AUDIONUMFILTER);
        for (i = 0; i < AUDIONUMFILTER * AUDIOFILTERLEN; i++) {
                f1 = i - ((AUDIONUMFILTER * AUDIOFILTERLEN - 1.0) / 2.0);
                f1 *= tmul;
                f2 = i * (1.0 / (AUDIONUMFILTER * AUDIOFILTERLEN - 1.0));
                c[i] = sinc(f1) * hamming(f2);
        }
#else
      tmul *= (1.0 / AUDIONUMFILTER);
        for (i = 0; i < AUDIONUMFILTER * AUDIOFILTERLEN; i++) {
                time = i - ((AUDIONUMFILTER * AUDIOFILTERLEN - 1.0) / 2.0);
                time *= tmul;
                alphatime = time * RCOSALPHA;
                f1 = 1 - 4 * alphatime * alphatime;
                if (fabs(f1) < 1e-10)
                        f2 = M_PI * (1.0 / 8.0) * sin(M_PI * alphatime) / alphatime;
                else
                        f2 = cos(M_PI * alphatime) / f1;
                c[i] = sinc(time) * f2;
        }
#endif
        f1 = 0;
        for (i = 0; i < AUDIONUMFILTER; i++) {
                for (f2 = 0, j = i; j < AUDIONUMFILTER * AUDIOFILTERLEN; j += AUDIONUMFILTER)
                        f2 += fabs(c[j]);
                if (f2 > f1)
                        f1 = f2;
        }
        f1 = 32767.0 / f1;
        for (i = 0; i < AUDIONUMFILTER; i++)
                for (j = 0; j < AUDIOFILTERLEN; j++)
                        filter[i][AUDIOFILTERLEN-1-j] = f1 * c[j * AUDIONUMFILTER + i];
}

/* ---------------------------------------------------------------------- */

#if 0
static inline int16_t fir(const int16_t *p1, const int16_t *p2, int len)
{
        int32_t sum = 0;

        for(; len > 0; len--, p1++, p2++)
                sum += ((int32_t)*p1) * ((int32_t)*p2);
        return sum >> 16;
}
#endif

/* ---------------------------------------------------------------------- */

void audioproc_init(struct trx_thread_state *state)
{
      if (state->cfg.adapt.rfsquelch <= RSSI_MIN)
            state->audioparam.rfsquelch = 0;
      else if (state->cfg.adapt.rfsquelch >= RSSI_MAX)
            state->audioparam.rfsquelch = 255;
      else
            state->audioparam.rfsquelch = (state->cfg.adapt.rfsquelch - RSSI_MIN) * (255.0/(RSSI_MAX-RSSI_MIN));
      /* init input sampling rate converter */
      memset(&state->du.a.isrc, 0, sizeof(state->du.a.isrc));
      state->du.a.isrc.phinc = ((1 << AUDIOSRCPHASEFRAC) * state->du.a.p.sratedspin + state->du.a.p.srateusb / 2) / state->du.a.p.srateusb;
      lprintf(2, "rec phase inc: 0x%05x\n", state->du.a.isrc.phinc);
      compute_filter(state->du.a.p.srateusb / (double)state->du.a.p.sratedspin, state->du.a.isrc.filter);
      /* init output sampling rate converter */
      memset(&state->du.a.osrc, 0, sizeof(state->du.a.osrc));
      state->du.a.osrc.phinc = ((1 << AUDIOSRCPHASEFRAC) * state->du.a.p.srateusb + state->du.a.p.sratedspout / 2) / state->du.a.p.sratedspout;
      lprintf(2, "play phase inc: 0x%05x\n", state->du.a.osrc.phinc);
      compute_filter(state->du.a.p.sratedspout / (double)state->du.a.p.srateusb, state->du.a.osrc.filter);
}

unsigned int audioproc_convertinput_isamples(struct trx_thread_state *state, unsigned int osamples)
{
      return (osamples * state->du.a.isrc.phinc + (state->du.a.isrc.ph & AUDIOSRCPHASEMASK)) >> AUDIOSRCPHASEFRAC;
}

void audioproc_convertinput(struct trx_thread_state *state, unsigned int inum, unsigned int onum, const int16_t *ibuf, signed char *obuf)
{
      int16_t *ib;
      unsigned int i;
      float pw;
      int s;
      
      /* handle overlap!! */
      for (i = 0; i < inum; i++) {
            state->du.a.isrc.pwracc += ibuf[i] * ibuf[i];
            if ((++state->du.a.isrc.pwrcnt) >= PWRINTERVAL) {
                  pw = state->du.a.isrc.pwracc;
                  state->du.a.isrc.pwracc = 0;
                  state->du.a.isrc.pwrcnt = 0;
                  pw *= (1.0 / PWRINTERVAL);
                  if (pw < 1.6384)
                        pw = -40;
                  else
                        pw = (10.0 / M_LN10) * log(pw) - 42.144;
                  state->du.a.isrc.sigpwr = pw;
            }
      }
      /* handle overlap */
      ib = alloca((inum + AUDIOOVERLAP) * sizeof(ib[0]));
      memcpy(ib, state->du.a.isrc.sample, AUDIOOVERLAP * sizeof(ib[0]));
      memcpy(ib + AUDIOOVERLAP, ibuf, inum * sizeof(ib[0]));
      memcpy(state->du.a.isrc.sample, ib + inum, AUDIOOVERLAP * sizeof(ib[0]));
      /* output filtering */
      state->du.a.isrc.ph &= AUDIOSRCPHASEMASK;
      simdpreparefpu();
      for (i = 0; i < onum; i++) {
            s = simdfir16(state->du.a.isrc.filter[AUDIOFILTERIDX(state->du.a.isrc.ph)], ib+AUDIOFILTERSAMPLE(state->du.a.isrc.ph), AUDIOFILTERLEN) >> 16;
            s += SIN(state->du.a.isrc.ph0) >> 1;
            s += SIN(state->du.a.isrc.ph1) >> 1;
            s >>= 8;
            if (s > 127)
                  s = 127;
            if (s < -128)
                  s = -128;
            state->du.a.isrc.abuf[state->du.a.isrc.aptr] = s;
            state->du.a.isrc.aptr = (state->du.a.isrc.aptr + 1) % (sizeof(state->du.a.isrc.abuf) / sizeof(state->du.a.isrc.abuf[0]));
            obuf[i] = s;
            state->du.a.isrc.ph += state->du.a.isrc.phinc;
            state->du.a.isrc.ph0 += state->du.a.isrc.freq0;
            state->du.a.isrc.ph1 += state->du.a.isrc.freq1;
      }
      simdpreparefpu();
}

int audioproc_adjustinput(struct trx_thread_state *state, unsigned int usbdelay)
{
      if (usbdelay > SNDLATENCY) {
            state->du.a.isrc.phinc++;
            state->du.a.isrc.ph += AUDIOSRCPHASEMASK >> 2;
            if (state->du.a.isrc.ph > AUDIOSRCPHASEMASK)
                  state->du.a.isrc.ph = AUDIOSRCPHASEMASK;
      } else if (usbdelay < SNDLATENCY) {
            state->du.a.isrc.phinc--;
            state->du.a.isrc.ph -= AUDIOSRCPHASEMASK >> 2;
            if (state->du.a.isrc.ph > AUDIOSRCPHASEMASK)
                  state->du.a.isrc.ph = 0;
      }
      if (state->du.a.isrc.phinc < AUDIOSRCMINPHINC) {
            lprintf(0, "phinc (0x%05x) out of range\n", state->du.a.isrc.phinc);
            state->du.a.isrc.phinc = AUDIOSRCMINPHINC;
      } else if (state->du.a.isrc.phinc > AUDIOSRCMAXPHINC) {
            lprintf(0, "phinc (0x%05x) out of range\n", state->du.a.isrc.phinc);
            state->du.a.isrc.phinc = AUDIOSRCMAXPHINC;
      }
      lprintf(20, "in: usbdelay %d phase increment: 0x%05x\n", usbdelay, state->du.a.isrc.phinc);
      return 0;
}

unsigned int audioproc_convertoutput(struct trx_thread_state *state, unsigned int inum, const signed char *ibuf, int16_t *obuf)
{
      unsigned int cnt = 0;
      unsigned int i;
      float pw1, pw2;
      int s;
      int16_t *ib;

      if (!inum)
            return 0;
      if (!state->audioparam.audiosquelch)
            state->dcd = state->rssi >= state->audioparam.rfsquelch;
      ib = alloca((inum + AUDIOOVERLAP) * sizeof(ib[0]));
      memcpy(ib, state->du.a.osrc.sample, AUDIOOVERLAP * sizeof(ib[0]));
      for (i = 0; i < inum; i++) {
            s = state->du.a.osrc.abuf[state->du.a.osrc.aptr] = ibuf[i];
            state->du.a.osrc.aptr = (state->du.a.osrc.aptr + 1) % (sizeof(state->du.a.osrc.abuf) / sizeof(state->du.a.osrc.abuf[0]));
            ib[i + AUDIOOVERLAP] = s << 8;
            state->du.a.osrc.pwracc1 += s * s;
            switch (state->du.a.osrc.pwrcnt2 & 3) {
            case 0:
                  state->du.a.osrc.pwracc2 -= s;
                  break;

            case 1:
                  state->du.a.osrc.pwracc3 -= s;
                  break;

            case 2:
                  state->du.a.osrc.pwracc2 += s;
                  break;
                  
            default:
                  state->du.a.osrc.pwracc3 += s;
                  break;
            }
            if ((++state->du.a.osrc.pwrcnt2) >= PWRINTERVAL2) {
                  state->du.a.osrc.pwracc4 += state->du.a.osrc.pwracc2 * state->du.a.osrc.pwracc2 + state->du.a.osrc.pwracc3 * state->du.a.osrc.pwracc3;
                  state->du.a.osrc.pwracc2 = 0;
                  state->du.a.osrc.pwracc3 = 0;
                  state->du.a.osrc.pwrcnt2 = 0;             
            }
            if ((++state->du.a.osrc.pwrcnt) >= PWRINTERVAL) {
                  pw1 = state->du.a.osrc.pwracc1;
                  pw2 = state->du.a.osrc.pwracc4;
                  state->du.a.osrc.pwracc1 = 0;
                  state->du.a.osrc.pwracc4 = 0;
                  state->du.a.osrc.pwrcnt = 0;
                  pw1 *= (1.0 / PWRINTERVAL);
                  pw2 *= (1.0 / PWRINTERVAL / PWRINTERVAL) * PWRNOISECALIB;
                  if (pw1 < 1.6384)
                        pw1 = -40;
                  else
                        pw1 = (10.0 / M_LN10) * log(pw1) - 42.144;
                  if (pw2 < 1.6384)
                        pw2 = -40;
                  else
                        pw2 = (10.0 / M_LN10) * log(pw2) - 42.144;
                  if (pw2 > 0)
                        pw2 = 0;
                  state->du.a.osrc.sigpwr = pw1;
                  state->du.a.osrc.noisepwr = pw2;
                  if (state->audioparam.audiosquelch)
                        state->dcd = (pw1 > -30) && (pw2 < (pw1 / 2 - 20));
            }
      }
      state->du.a.osrc.ph &= AUDIOSRCPHASEMASK;
      memcpy(state->du.a.osrc.sample, ib + inum, AUDIOOVERLAP * sizeof(ib[0]));
      simdpreparefpu();
      while (state->du.a.osrc.ph < (inum << AUDIOSRCPHASEFRAC)) {
            obuf[cnt] = simdfir16(state->du.a.osrc.filter[AUDIOFILTERIDX(state->du.a.osrc.ph)], ib+AUDIOFILTERSAMPLE(state->du.a.osrc.ph), AUDIOFILTERLEN) >> 16;
            state->du.a.osrc.ph += state->du.a.osrc.phinc;
            cnt++;
      }
      simdpreparefpu();
#if 0
      printf("audiosq %u rfsq %u rssi %u dcd %u\n", state->audioparam.audiosquelch, state->audioparam.rfsquelch, state->rssi, state->dcd);
#endif
      if (!state->dcd)
            memset(obuf, 0, 2*cnt);
      return cnt;
}

int audioproc_adjustoutput(struct trx_thread_state *state, unsigned int dspdelay)
{
      state->du.a.osrc.ph &= AUDIOSRCPHASEMASK;
      if (dspdelay > SNDLATENCY) {
            state->du.a.osrc.phinc++;
            state->du.a.osrc.ph += AUDIOSRCPHASEMASK >> 2;
            if (state->du.a.osrc.ph > AUDIOSRCPHASEMASK)
                  state->du.a.osrc.ph = AUDIOSRCPHASEMASK;
      } else if (dspdelay < SNDLATENCY) {
            state->du.a.osrc.phinc--;
            state->du.a.osrc.ph -= AUDIOSRCPHASEMASK >> 2;
            if (state->du.a.osrc.ph > AUDIOSRCPHASEMASK)
                  state->du.a.osrc.ph = 0;
      }
      if (state->du.a.osrc.phinc < AUDIOSRCMINPHINC) {
            lprintf(0, "phinc (0x%05x) out of range\n", state->du.a.osrc.phinc);
            state->du.a.osrc.phinc = AUDIOSRCMINPHINC;
      } else if (state->du.a.osrc.phinc > AUDIOSRCMAXPHINC) {
            lprintf(0, "phinc (0x%05x) out of range\n", state->du.a.osrc.phinc);
            state->du.a.osrc.phinc = AUDIOSRCMAXPHINC;
      }
      lprintf(20, "out: dspdelay %d phase increment: 0x%05x\n", dspdelay, state->du.a.osrc.phinc);
      return 0;
}

/* ---------------------------------------------------------------------- */

Generated by  Doxygen 1.6.0   Back to index