Logo Search packages:      
Sourcecode: linux-fsl-imx51 version File versions  Download package

stfm1000-rds.c

/*
 * Copyright 2008-2009 Freescale Semiconductor, Inc. All Rights Reserved.
 */

/*
 * The code contained herein is licensed under the GNU General Public
 * License. You may obtain a copy of the GNU General Public License
 * Version 2 or later at the following locations:
 *
 * http://www.opensource.org/licenses/gpl-license.html
 * http://www.gnu.org/copyleft/gpl.html
 */
#include <linux/init.h>

#include "stfm1000.h"

#include "stfm1000-rds.h"

#define bitstream_to_rds_state(b)   \
      container_of(b, struct stfm1000_rds_state, bitstream)
#define demod_to_rds_state(d)       \
      container_of(d, struct stfm1000_rds_state, demod)
#define pkt_to_rds_state(p)         \
      container_of(p, struct stfm1000_rds_state, pkt)
#define text_to_rds_state(t)        \
      container_of(t, struct stfm1000_rds_state, text)
#define rds_state_to_stfm1000(r)    \
      container_of(r, struct stfm1000, rds_state)

#define TADJSH 8 /*Shifts used in bitslice loop filter */

/* Reverse of Matlab's Fquant (see MatchedFilterDecomposition.m), so that */
/* mixandsum code is easy;   Used by rds_bitstream_stfmdemod.arm */
const s16 u16_rds_basis[2*RDS_BASISLENGTH+8] = {
      14, 24, 34, 43, 50, 56, 60, 62, 62,
      60, 55, 49, 41, 32, 22, 11, 14, 24,
      34, 43, 50, 56, 60, 62, 62, 60, 55,
      49, 41, 32, 22, 11, 14, 24, 34, 43,
      50, 56, 60, 62
};

static int bits_free(struct stfm1000_rds_bitstream *rdsb)
{
      /* Do not show the last one word free. */
      int FreeSpace = rdsb->TailBitCount - rdsb->HeadBitCount - 32;

      if (FreeSpace < 0)
            FreeSpace = (RDS_BITBUFSIZE * 32) + FreeSpace;
      return FreeSpace;
}

static void put1bit(struct stfm1000_rds_bitstream *rdsb, int bit)
{
      int index = (rdsb->HeadBitCount >> 5);
      u32 CurBit = (rdsb->HeadBitCount & 0x1f);
      u32 CurWord = rdsb->buf[index];

      if (CurBit == 0)
            CurWord = 0;

      CurWord = CurWord | (((u32)bit & 1) << CurBit);
      rdsb->buf[index] = CurWord;
      rdsb->HeadBitCount++;
      if (rdsb->HeadBitCount >= RDS_BITBUFSIZE * 32)
            rdsb->HeadBitCount = 0;
}

static int get1bit(struct stfm1000_rds_bitstream *rdsb)
{
      int Bit = 0;
      int index = (rdsb->TailBitCount >> 5);
      int CurBit = (rdsb->TailBitCount & 0x1f);
      u32 CurWord = rdsb->buf[index];

      Bit = (CurWord >> CurBit) & 1;
      rdsb->TailBitCount++;
      if (rdsb->TailBitCount == RDS_BITBUFSIZE*32)
            rdsb->TailBitCount = 0;

      return Bit;
}

static int bits_filled(struct stfm1000_rds_bitstream *rdsb)
{
      int FilledSpace = rdsb->HeadBitCount - rdsb->TailBitCount;

      if (FilledSpace < 0)
            FilledSpace = (RDS_BITBUFSIZE * 32) + FilledSpace;
      return FilledSpace;
}

static void rds_mix_msg(struct stfm1000_rds_demod *rdsd, u8 MixSetting)
{
      if (rdsd->mix_msg_pending)
            rdsd->mix_msg_overrun++;
      rdsd->mix_msg = MixSetting;
      rdsd->mix_msg_pending = 1;

      /* signal monitor thread */
      stfm1000_monitor_signal(
            rds_state_to_stfm1000(demod_to_rds_state(rdsd)),
            EVENT_RDS_MIXFILT);
}

/* call with interrupts disabled please */
int stfm1000_rds_mix_msg_get(struct stfm1000_rds_state *rds)
{
      struct stfm1000_rds_demod *rdsd = &rds->demod;

      if (!rdsd->mix_msg_pending)
            return -1;

      return rdsd->mix_msg;
}

/* call with interrupts disabled please */
int stfm1000_rds_mix_msg_processed(struct stfm1000_rds_state *rds, int mix_msg)
{
      struct stfm1000_rds_demod *rdsd = &rds->demod;

      if (!rdsd->mix_msg_pending)
            return -1;

      rdsd->mix_msg_pending = 0;

      /* update the completion indication bit */
      if ((mix_msg & 0x8) == 0)
            rdsd->MixPopDone = 1;

      /* this is reflected off the hardware register */
      rdsd->rds_mix_offset = mix_msg & 1;

      if (rdsd->mix_msg != mix_msg) {
            rdsd->mix_msg_processed_changed++;
            return -1;
      }
      return 0;
}

static void rds_sdnominal_msg(struct stfm1000_rds_demod *rdsd, int sdnominal)
{
      if (rdsd->sdnominal_msg_pending)
            rdsd->sdnominal_msg_overrun++;
      rdsd->sdnominal_msg = sdnominal;
      rdsd->sdnominal_msg_pending = 1;

      /* signal monitor thread */
      stfm1000_monitor_signal(
            rds_state_to_stfm1000(demod_to_rds_state(rdsd)),
            EVENT_RDS_SDNOMINAL);
}

/* call with interrupts disabled please */
int stfm1000_rds_sdnominal_msg_get(struct stfm1000_rds_state *rds)
{
      struct stfm1000_rds_demod *rdsd = &rds->demod;

      if (!rdsd->sdnominal_msg_pending)
            return 0;

      return rdsd->sdnominal_msg;
}

/* call with interrupts disabled please */
int stfm1000_rds_sdnominal_msg_processed(struct stfm1000_rds_state *rds,
      int sdnominal_msg)
{
      struct stfm1000_rds_demod *rdsd = &rds->demod;

      if (!rdsd->sdnominal_msg_pending)
            return -1;

      rdsd->sdnominal_msg_pending = 0;
      return 0;
}

void demod_loop(struct stfm1000_rds_bitstream *rdsb,
      struct stfm1000_rds_demod *rdsd)
{
      s32 filter_out;
      u32 freeSpace;
      s32 decomp_hist_pp;
      u8 phase;

      /* Check if we're at a half-basis point */
      if ((rdsd->i & (RDS_BASISLENGTH/2 - 1)) != 0)
            return;     /* Nope, return */

      /* Yes, time to do our work */
      /* Rotate the length 3 history buffer */
      decomp_hist_pp = rdsd->decomp_hist_p;
      rdsd->decomp_hist_p = rdsd->decomp_hist;
      if ((rdsd->i & (RDS_BASISLENGTH-1)) == 0) {
            rdsd->decomp_hist = rdsd->mixandsum1>>9; /* Grab output of
                                            * mixandsum1/512 */
            rdsd->mixandsum1 = 0;    /* Reset mixandsum #1 */
      } else {
            rdsd->decomp_hist = rdsd->mixandsum2>>9; /*Grab output of
                                            * mixandsum2/512 */
            rdsd->mixandsum2 = 0;   /* Reset mixandsum #2 */
      }

      /* Form correlator/decimator output by convolving with the
       * decomposition coefficients, DecompQuant from Matlab work. */
      filter_out = (-58*rdsd->decomp_hist + 59*decomp_hist_pp)>>7;

      /*Figure out which half-basis we are in (out of a bit-length cycle) */
      phase = rdsd->i*2/RDS_BASISLENGTH;
      /*Now what we do depends on the phase variable */
      /*Phase 0:      Bitslice and do timing alignment */
      /*others (1-3): Keep value for timing alignment */

      if (phase == 0) {   /*Main processing (bitslice) */
            u32 Ph;
            u8 OldBit = rdsd->sliced_data;      /* Save the previous value */

            rdsd->return_num = 1;
            if (filter_out >= 0) { /*This bit is "1" */
                  /*return value is XOR of previous bit (still in
                   * sliced_data) w/ this */
                  /*  bit (1), which equals (NOT of the previous bit) */
                  rdsd->return_rdsdemod = !OldBit;
                  rdsd->sliced_data = 1;  /*Newest bit value is 1 */
            } else { /*This bit is "0" */
                  /*return value is XOR of previous bit (still in
                   * sliced_data) w/ this */
                  /*  bit (0), which equals the previous bit */
                  rdsd->return_rdsdemod = OldBit;
                  rdsd->sliced_data = 0;  /*Newest bit value is 0 */
            }

            freeSpace = bits_free(rdsb);

            if (freeSpace > 0)
                  put1bit(rdsb, rdsd->return_rdsdemod);
            else
                  rdsd->RdsDemodSkippedBitCnt++;

            /*Increment bits received counter */
            rdsd->BitAlignmentCounter++;
            /*If mixer phase determination hasn't been done, start it */
            if ((rdsd->MixPhaseState == 0) && (!rdsd->MixPhaseDetInProg)) {
                  rdsd->MixPhaseDetInProg = 1;
                  /*Go to first mixer setting (0) */
                  rds_mix_msg(rdsd, 0);
            }

            /* Do bit-slicing time adaption after the mixer phase
             * determination */
            if (!(rdsd->MixPhaseDetInProg) && !(rdsd->Synchronous)) {

                  /* Bitslice Timing Adjust Code (runs after
                   * MixPhaseDetInProg and if RDS is not synchronous to
                   * the FM pilot. */

                  u8 BigPh2;  /* Expecting a large value in
                               * PhaseValue[2] */
                  u32 MaxRMS = 0;   /*Largest phase RMS */
                  s8 MaxPh = 0;     /*Index of largest phase RMS */
                  s32 zerocross;

                  /* Locate the largest phase RMS
                   * (should be at phase zero) */
                  for (Ph = 0; Ph < 4; Ph++)
                        if (rdsd->Ph_RMS[Ph] > MaxRMS) {
                              MaxRMS = rdsd->Ph_RMS[Ph];
                              MaxPh = Ph;
                        }

                  /* During each bit time we expect the four phases to
                   * take one of the following patterns, where 1
                   * corresponds to maximum modulation:
                   *   1,    0,  -1,    0             Case I
                   *  -1,    0,   1,    0             Case II
                   *   1,  1/2,   0, -1/2             Case III
                   *  -1, -1/2,   0,  1/2             Case IV
                   * We need to distinguish between cases in order to do
                   * the timing adjustment. Below we compare the
                   * correlation of the samples with Case I and Case III
                   * to see which has a bigger abs(correlation).  Thus
                   * BigPh2, if set, means that we decided on Case I or
                   * Case II; if BigPh2 clear, we decided Case III or IV.
                   */
                  BigPh2 = abs(rdsd->PhaseValue[0]-rdsd->PhaseValue[2]) >
                         abs(rdsd->PhaseValue[0] +
                         ((rdsd->PhaseValue[1]-
                              rdsd->PhaseValue[3])>>1));
                  /* If BigPh2, use the difference between phase 1 value
                   * (downgoing for Case I, upgoing for Case II) and
                   * phase 3 value (upgoing for Case I, downgoing for
                   * Case II, thus the subtraction) to indicate timing
                   * error.  If not BigPh2, use the sum of the phase 1
                   * value (downgoing for Case III, upgoing for Case IV)
                   * and phase 3 value (downgoing for Case III, upgoing
                   * for Case IV, thus the addition) to indicate timing
                   * error.  If BigPh2, the slopes at phase 1 & phase 3
                   * are approximately double that if not BigPh2.
                   * Since we are trying to measure timing, scale
                   * by 1/2 in the BigPh2 case. */
                  if (BigPh2)
                        zerocross = (rdsd->PhaseValue[1]-
                              rdsd->PhaseValue[3])>>1;
                  else
                        zerocross = rdsd->PhaseValue[1]+
                              rdsd->PhaseValue[3];
                  /* Now if the prev bit was a "1", then the first zero
                   * crossing (phase 1 if BigPh2, phase 2 if !BigPh2)
                   * was a falling one, and if we were late then
                   * zerocross should be negative. If the prev bit was a
                   * "0", then the first zero crossing was a rising one,
                   * and if we were late then zerocross would be
                   * positive. If we are "late" it means that we need to
                   * do a shorter cycle of, say, 15 samples instead of
                   * 16, to "catch up" so that in the future we will be
                   * sampling earlier.  We shorten the cycle by adding
                   * to i, so "late" is going to mean "increment i".
                   * Therefore "late" should be positive, which is done
                   * here by inverting zerocross if the previous bit was
                   * 1.  You could say that this step reflects cases I
                   * and III into II and IV, respectively. */
                  if (OldBit)
                        zerocross = -zerocross;
                  if (!rdsd->DisablePushing) {
                        /*The algorithm so far has a stable operating
                         * point 17 phases away from the correct one.
                         * The following code is experimental and may
                         * be deleterious in low SNR conditions, but is
                         * an attempt to move off of the incorrect
                         * operating point. */

                        if (MaxPh != 0) {
                              /* If it isn't the same MaxPh as the
                               * last non-zero one, clear the counter
                               */
                              if (MaxPh != rdsd->PushLastMaxPh) {
                                    /*Reset the counter */
                                    rdsd->PushCounter = 0;
                                    /*Record which phase we're now
                                     * counting */
                                    rdsd->PushLastMaxPh = MaxPh;
                              }
                              /* If the Max RMS is on the same
                               * non-zero phase, count up */
                              rdsd->PushCounter++;
                        }
                        /* Once every 128 bits, check and then reset
                         * PushCounter */
                        if (!(rdsd->BitAlignmentCounter & 0x0FF)) {
                              /*If 90% of the time the max phase has
                               * been from the same non-zero phase,
                               * decide that we are latched onto a 0
                               * lock point. Do a large push of the
                               * timing. */
                              if (rdsd->PushCounter > 230) {
                                    s32 pshiph;
                                    /*Convert from phase number to
                                     * the number of filter
                                     *  output samples that we need
                                     *  to shift */
                                    if (rdsd->PushLastMaxPh >= 2)
                                          pshiph =
                                                4 - (s8)rdsd->
                                                PushLastMaxPh;
                                    else
                                          pshiph =
                                                -(s8)rdsd->
                                                PushLastMaxPh;
                                    /* Scale by the number of i-
                                     * phases per output sample */
                                    pshiph <<=
                                          RDS_BASISSHIFTS-1;
                                    /* Perform big pop to get near
                                     * correct timing */
                                    rdsd->i += (RDS_BASISLENGTH<<1)
                                          + pshiph;
                                    /* Set status indicating big
                                     * pop was needed.  Reset all
                                     * leaky-bucket and summation
                                     * variables because the big
                                     * timing shift has invalidated
                                     * them. Ph_RMS values don't
                                     * need to be reset because
                                     * they will shift over to
                                     * reasonable values again
                                     * before their erroneous
                                     * values could have effect. */
                                    rdsd->rds_big_timeshift = 1;
                                    /*rdsd->Ph_RMS[0] = 0; */
                                    /*rdsd->Ph_RMS[1] = 0; */
                                    /*rdsd->Ph_RMS[2] = 0; */
                                    /*rdsd->Ph_RMS[3] = 0; */
                                    rdsd->mixandsum1 = 0;
                                    rdsd->mixandsum2 = 0;
                                    rdsd->SkipsAccum +=
                                          pshiph;

                                    /* Make adjustments in other
                                     * values because of the push
                                     * (they wouldn't otherwise be
                                     * able to use the information
                                     * that a push was needed in
                                     * their future control
                                     * decisions). */
                                    if (rdsd->PushLastMaxPh != 2) {
                                          /* If we weren't
                                           * pushing from phase
                                           * two, accumulate (for
                                           * use in adapting
                                           * SDNOMINAL) the
                                           * phases moved by
                                           * pushing. Phase two
                                           * pushes are not used;
                                           * the push direction
                                           * is arbitrary since
                                           * Phase 2 is 180
                                           * degrees out.  Also,
                                           * phase 2 pushes don't
                                           * result from
                                           * reasonable slippage.
                                           * */

                                          if (rdsd->sdnom_adapt)
                                                rdsd->SdnomSk
                                                += pshiph;

                                          /* Modify timing_adj to
                                           * account for half of
                                           * the DC response that
                                           * would have occurred
                                           * in timing_adj if
                                           * that control loop
                                           * had seen the push
                                           * happen. (Why half?
                                           * Because the loop has
                                           * already seen a
                                           * history of zerocross
                                           * values that heads it
                                           * in the same
                                           * direction as this
                                           * adjustment, but may
                                           * have seen as few as
                                           * half of what it
                                           * should have.) */
                                          rdsd->timing_adj +=
                                                pshiph <<
                                                (TADJSH+1);
                                    }
                                    /*Set countdown timer that will
                                     * prevent any mixer popping
                                     * until the Ph_RMS variables
                                     * have had enough time to
                                     * stabilize */

                                    /* 2.5 time constants */
                                    rdsd->PushSafetyZone = 5;
                              }
                              /*Reset the push counter */
                              rdsd->PushCounter = 0;
                        }  /*end once every 128 bits */
                  }  /*end if !DisablePushing */

                  /* Further possible additions:
                   *
                   * 1. Pushes modify timing_adj to decrease convergence
                   *    time.
                   * 2. Separate timing_adj into pilottracking and non-pt
                   *    cases (avoids convergence time after stereo/mono
                   *    transitions)
                   *
                   * Old loop filter was a leaky bucket integrator, and
                   * it always lagged behind if the FM station had RDS
                   * asynchronous to the pilot, because the control loop
                   * needs another integrator to converge on a frequency
                   * error.
                   * New loop filter = 1/(1-1/z) * (a-1/z) * k,
                   * where a = 1+1/256 and k = 1/1024.
                   * You can narrow the loop bandwidth by making "a"
                   * twice as close to 1 and halving k, e.g. a = 1+1/512
                   * and k = 1/2048.
                   * (The way implemented, that narrowing loop BW by
                   * a factor of 2 can be done by incrementing TADJSH.)
                   *
                   * TGR 8/31/2007 */

                  /*Integrator, 1/(1-1/z) */
                  rdsd->timing_adj += zerocross;
                  /*Limit to 1 phase every 8 samples */
                  if (rdsd->SkipSafetyZone) {
                        rdsd->SkipSafetyZone--;
                        rdsd->sampskip = 0;
                  } else {
                        /*sampskip of non-zero is allowed,
                         * calculate what it really is */

                        /*Saturate timing_adj to 2's comp
                         * (2*TADJSH+4)-bit range. */
                        if (rdsd->timing_adj > (1<<(2*TADJSH+3))-1)
                              rdsd->timing_adj = (1<<(2*TADJSH+3))-1;
                        if (rdsd->timing_adj < -(1<<(2*TADJSH+3)))
                              rdsd->timing_adj = -(1<<(2*TADJSH+3));

                        /* Zero, implemented after the integrator
                         * output.
                         * (a-1/z) = (1+1/256) - 1/z = (1-1/z) + 1/256.
                         * But (1 - 1/z) is timing_adj-
                         * prev_timing_adj = zerocross. */
                        rdsd->sampskip = zerocross    /* 1 - 1/z */
                              /* 1/256 (with rounding) */
                              + ((rdsd->timing_adj
                                    + (1<<(TADJSH-1)))>>TADJSH);
                        /*Round and apply k */
                        rdsd->sampskip += (1<<(TADJSH+1));
                        rdsd->sampskip >>= (TADJSH+2);
                        /*Limit to [-1,+1] inclusive */
                        if (rdsd->sampskip > 1)
                              rdsd->sampskip = 1;
                        if (rdsd->sampskip < -1)
                              rdsd->sampskip = -1;
                        /* If non-zero, start the skip safety zone,
                         * which excludes more sample skipping for a
                         * while.  Note that the safety zone only
                         * applies to the skips -- pushes can still
                         * happen inside a SkipSafetyZone. */
                        if (rdsd->sampskip)
                              rdsd->SkipSafetyZone = 8-1;
                  }
                  /**********************************************
                  * End Timing Adjust Code
                  **********************************************/

                  /**********************************************
                  * Begin Phase Popper Code
                  **********************************************/
                  /* If Phase Popping is enabled and 1/2 of a
                   * time constant has gone by... */
                  if (rdsd->PhasePoppingEnabled &&
                        !(rdsd->BitAlignmentCounter &
                              ((1<<(RMSALPHASHIFTS-1))-1))) {

                        u8 ForcePop = 0; /* Used to force a pop */

                        /*Record the maximum of the envelope */
                        if (MaxRMS > rdsd->PhasePopMaxRMS)
                              rdsd->PhasePopMaxRMS = MaxRMS;
                        /* Also track MaxRMS into MixPhase0/1Mag, so
                         * that we can see what the largest RMS on each
                         * of those phases is.  On synchronous stations
                         * (meaning the RDS carrier and bit rate are
                         * synchronized with the pilot), the right mix
                         * phase will always be big and the wrong phase
                         * small. On asynchronous stations (and
                         * stations without RDS), both phases will at
                         * some time or other have about the
                         * same amplitude on each of the phases. */
                        if (rdsd->rds_mix_offset) {
                              if (MaxRMS > rdsd->MixPhase1Mag)
                                    rdsd->MixPhase1Mag = MaxRMS;
                        } else {
                              if (MaxRMS > rdsd->MixPhase0Mag)
                                    rdsd->MixPhase0Mag = MaxRMS;
                        }
                        /* Update PopSafetyZone and PushSafetyZone
                         * counters.  With RMSALPHASHIFTS = 5, each
                         * tick is 16/1187.5 =~ 13.5 ms. */
                        if (rdsd->PopSafetyZone) {
                              rdsd->PopSafetyZone--;
                              /* If safety zone just ended and this
                               * mix phase is giving smaller RMS than
                               * before the pop, then the pop was a
                               * mistake.  Go back to previous mixer
                               * phase */
                              if (!(rdsd->PopSafetyZone)
                                    && (rdsd->PhasePopMaxRMS <
                                          rdsd->PrePopRMS))
                                    ForcePop = 1;
                        }
                        /* If there is no recent push, and Phase 0 has
                         * the maximum RMS, and at least 1/7th of a
                         * second has passed since the last phase pop,
                         * and ((the RMS is less than 1/2 of
                         * PhasePopMaxRMS) or (the RMS is less than
                         * 100)), then try a phase pop. */
                        if (/* (rdsd->Ph_RMS[0] == MaxRMS) &&
                                    * Phase 0 has maximum RMS  */
                              !(rdsd->PopSafetyZone)) {
                               /* and Long enough since last
                                * phase pop */

                              /* Eligible for a pop, see if one of
                               * the pop conditions is met */
                              if ((MaxRMS<<1) <
                                          rdsd->PhasePopMaxRMS) {
                                    /*RMS decline from its peak */
                                    ForcePop = 1;
                              } else if ((MaxRMS>>RMSALPHASHIFTS)
                                    < 50) {
                                    /*RMS too small to receive,
                                     * either there's no RDS or
                                     * this is the wrong phase */
                                    ForcePop = 1;
                              }
                        }
                        if (ForcePop) {

                              /*Pop to opposite setting */
                              rds_mix_msg(rdsd, 0x8 |
                                    !rdsd->rds_mix_offset);

                              /*Save the pre-pop RMS so that later we
                               * can see if the pop was actually
                               * effective */
                              rdsd->PrePopRMS = MaxRMS;
                              /*Reset the PhasePopMaxRMS.  We rely on
                               * the PopSafetyZone to give time to
                               * get a new valid max RMS before we're
                               * eligible for the next phase pop.  If
                               * there were no reset we'd be forever
                               * incrementing PhasePopMaxRMS due
                               * to just happenstance large-noise
                               * samples and it might eventually get
                               * some freakish large value causing
                               * frequent erroneous pops. */
                              rdsd->PhasePopMaxRMS = 0;
                              /* Pop Safety zone length is decided by
                               * how much of an asynchronous
                               * frequency can be supported. Allowing
                              * 50 ppm of transmitter error (error
                              * between their own pilot, that we
                              * should be locked to, and their RDS
                              * carrier (which by RDS spec should be
                              * locked to their pilot, but we've
                              * recently found frequently isn't).
                              * 50ppm * 57kHz = 2.85Hz.
                              * (2.85 cycles/sec)(4 pops/cycle)
                              * = 11.4 pops/second.
                              * Safety zone = (1/11.4) seconds =~ 104
                              * bits, round down to 96 bits which
                              * yields 6 ticks if RMSALPHASHIFTS = 5.
                              * */
                              rdsd->PopSafetyZone = 96>>
                                    (RMSALPHASHIFTS-1);
                        }
                  }
                  /******************************************************
                  * End Phase Popper Code
                  ******************************************************/

                  /* SDNOMINAL adaption */
                  if (rdsd->sdnom_adapt) {
                        rdsd->SdnomSk += rdsd->sampskip;
                        if (rdsd->pCoefForcedMono &&
                              (rdsd->BitAlignmentCounter & 0xFFF) ==
                                    0x800) {

                              rds_sdnominal_msg(rdsd,
                                    -(rdsd->SdnomSk<<9));

                              /*Reset skips counter */
                              rdsd->SdnomSk = 0;
                        }
                  }

                  rdsd->SkipsAccum += rdsd->sampskip;
                  /* Once per 3.45 seconds, print out signal strength,
                   * skips and pops. Then reset the variables totalling
                   * those occurrences */
                  if (!(rdsd->BitAlignmentCounter & 0xFFF)) {
                        /* During very noisy input (or if no RDS, or no
                         * station present), timing_adj can go crazy,
                         * since it is the integral of noise.  Although
                         * it is a saturated value (earlier, in the
                         * timing adjust code), the level at which we
                         * can saturate still leaves room for
                         * timing_adj to get too big.  A large value of
                         * timing_adj is a persistent pathology because
                         * the phase is shifting so quickly that the
                         * push detector (which relies on stable
                         * phase-RMS values) never triggers, thus there
                         * is no implemented rescue besides this
                         * clearing that restores proper function. */
                        if (abs(rdsd->SkipsAccum) > 300)
                              rdsd->timing_adj = 0;
                        /*Reset the accumulations. */
                        rdsd->SkipsAccum = 0;
                  }
            } /*End of bit timing adaption */

            /* If mixer phase determination in progress,
             * perform actions at certain times */
            if (rdsd->MixPhaseDetInProg) {
                  /*~10ms settling time after mixer phase change */
                  #define MIXPHASE_STARTMEAS 12
                  /*~20ms measurement window */
                  #define MIXPHASE_ENDMEAS (MIXPHASE_STARTMEAS+24)
                  if (rdsd->BitAlignmentCounter == MIXPHASE_STARTMEAS) {
                        /*Reset the RMS variables */
                        rdsd->Ph_RMS[0] = 0;
                        rdsd->Ph_RMS[1] = 0;
                        rdsd->Ph_RMS[2] = 0;
                        rdsd->Ph_RMS[3] = 0;
                        /* Don't reset mixandsum values because at
                         * least they have filtered continuously.  All
                         * we really need for the mixer phase decision
                         * is a constant measurement window. */
                  } else if (rdsd->BitAlignmentCounter ==
                              MIXPHASE_ENDMEAS) {
                        /*Measurement = mean of RMS values */
                        u32 Ndx, MeasVal = 0;
                        for (Ndx = 0; Ndx < 4;
                              MeasVal += rdsd->Ph_RMS[Ndx++]>>2);
                        /*Store measurement in correct place */
                        if (rdsd->MixPhaseState == 1) {
                              rdsd->MixPhase0Mag = MeasVal;
                              /*Go to next mixer setting */
                              rds_mix_msg(rdsd, 1);
                        } else if (rdsd->MixPhaseState == 2) {
                              u8 NextMixSetting;
                              rdsd->MixPhase1Mag = MeasVal;
                              /* Both measurements done now, see what
                               * mixer setting we need to use.
                               * 0 if MixPhase0Mag > MixPhase1Mag,
                               * 1 otherwise. */
                              NextMixSetting = (rdsd->MixPhase0Mag
                                    <= rdsd->MixPhase1Mag);
                              /* If the mixer setting needed is 1,
                               * that is already the current setting.
                               * Terminate mixer phase determination.
                               * Otherwise send message to switch the
                               * mixer phase setting. */
                              if (NextMixSetting) {
                                    rdsd->MixPhaseState = 3;
                                    rdsd->MixPhaseDetInProg = 0;
                              } else
                                    rds_mix_msg(rdsd, 0);
                        }
                  }
                  /* Reset BitAlignmentCounter if the Mixer just popped
                   * Change state, if required.  States are:
                   * 0: Initial state, send msg causing RDS_MIXOFFSET=>0
                   * 1: Measure with RDS_MIXOFFSET = 0.
                   *    Lasts just over 30 ms.
                   * 2: Measure with RDS_MIXOFFSET = 1.
                   *    Lasts just over 30 ms.
                   * 3: At final RDS_MIXOFFSET value.
                   *    Lasts as long as RDS continues. */
                  if (rdsd->MixPopDone) {
                        rdsd->MixPopDone = 0;
                        rdsd->BitAlignmentCounter = 0;
                        rdsd->MixPhaseState++;  /*Go to next state */
                        /* If we got to state 3, turn off mixer phase
                         * determination code */
                        if (rdsd->MixPhaseState == 3)
                              rdsd->MixPhaseDetInProg = 0;
                  }
            }

            /* Update status variables */
            rdsd->RDS_BIT_AMP_STAT_REG9 = rdsd->Ph_RMS[0]>>RMSALPHASHIFTS;
            /*Saturate */
            if (rdsd->RDS_BIT_AMP_STAT_REG9 > 511)
                  rdsd->RDS_BIT_AMP_STAT_REG9 = 511;
      }  /*End phase 0 code */

      /***************************************************
      * Actions common to all phases
      ***************************************************/

      /* Save the output of each phase for possible
       * calculations during phase 0 */
      rdsd->PhaseValue[phase] = filter_out;

      /*So that we can measure signal amplitude and/or determine what (if */
      /*  any) big jump is needed, maintain the RMS of each phase.  Phase */
      /*  0 RMS is already in Ph_RMS[0] (see bitslicing code, earlier). */
      rdsd->Ph_RMS[phase] += abs(filter_out) -
            (rdsd->Ph_RMS[phase]>>RMSALPHASHIFTS);
}

#if defined(CONFIG_ARM)

/* assembly version for ARM */
#define RDS_MAC(_acc, _x, _y) \
      __asm__ __volatile__ (               \
      "smlabb     %0, %1, %2, %0\n"        \
      : "=&r" (_acc)                       \
      : "r" (_x), "r" (_y)                 \
      : "cc")

#else

/* all others, use standard C */
#define RDS_MAC(_acc, _x, _y)                    \
      do {                           \
            (_acc) += (s16)(_x) * (s16)(_y); \
      } while (0)

#endif

static void rds_demod(const u16 *data, struct stfm1000_rds_demod *rdsd,
      struct stfm1000_rds_bitstream *rbit, int total)
{
      register const s16 *basis0;
      register const s16 *basis1;
      register s16 val;
      register int i;
      register int sampskip;
      register s32 acc1;
      register s32 acc2;

      /* point to the table */
      basis0 = u16_rds_basis;
      basis1 = basis0 + 8;

      rdsd->return_num = 0;

      /* restore state */
      i = rdsd->i;
      acc1 = rdsd->mixandsum1;
      acc2 = rdsd->mixandsum2;      /* 64 bit */
      sampskip = rdsd->sampskip;

      while (total-- > 0) {

            val = data[3];    /* load RDS data */
            data += 4;
            if (val == 0x7fff)      /* illegal RDS sample */
                  continue;

            RDS_MAC(acc1, val, basis0[i]);
            RDS_MAC(acc2, val, basis1[i]);

            if (i == 4) {
                  i += sampskip;
                  sampskip = 0;
            }

            if ((i & (RDS_BASISLENGTH / 2 - 1)) == 0) {

                  /* save state */
                  rdsd->mixandsum1 = acc1;
                  rdsd->mixandsum2 = acc2;
                  rdsd->i = i;
                  rdsd->sampskip = sampskip;

                  demod_loop(rbit, rdsd);

                  /* restore state */
                  acc1 = rdsd->mixandsum1;
                  acc2 = rdsd->mixandsum2;
                  i = rdsd->i;
                  sampskip = rdsd->sampskip;
            }
            i = (i + 1) & 31;
      }

      /* save state */
      rdsd->mixandsum1 = acc1;
      rdsd->mixandsum2 = acc2;
      rdsd->i = i;
      rdsd->sampskip = sampskip;
}

void stfm1000_rds_demod(struct stfm1000_rds_state *rds, const u16 *dri_data,
      int total)
{
      rds_demod(dri_data, &rds->demod, &rds->bitstream, total);

      /* signal only when we have enough */
      if (bits_filled(&rds->bitstream) > 128)
            stfm1000_monitor_signal(rds_state_to_stfm1000(rds),
                        EVENT_RDS_BITS);
}

static void bitstream_reset(struct stfm1000_rds_bitstream *rdsb)
{
      memset(rdsb, 0, sizeof(*rdsb));
}

static void demod_reset(struct stfm1000_rds_demod *rdsd)
{
      memset(rdsd, 0, sizeof(*rdsd));
      rdsd->sdnom_adapt = 0;  /* XXX this doesn't really work right */
                              /* it causes underruns at ALSA */
      rdsd->PhasePoppingEnabled = 1;      /* does this? */
}

static void packet_reset(struct stfm1000_rds_pkt *rdsp)
{
      memset(rdsp, 0, sizeof(*rdsp));
      rdsp->state = SYNC_OFFSET_A;
}

static void text_reset(struct stfm1000_rds_text *rdst)
{
      memset(rdst, 0, sizeof(*rdst));
}

void stfm1000_rds_reset(struct stfm1000_rds_state *rds)
{
      bitstream_reset(&rds->bitstream);
      demod_reset(&rds->demod);
      packet_reset(&rds->pkt);
      text_reset(&rds->text);
      rds->reset_req = 0;
}

int stfm1000_rds_bits_available(struct stfm1000_rds_state *rds)
{
      return bits_filled(&rds->bitstream);
}

int stmf1000_rds_get_bit(struct stfm1000_rds_state *rds)
{
      if (bits_filled(&rds->bitstream) == 0)
            return -1;
      return get1bit(&rds->bitstream);
}

int stmf1000_rds_avail_bits(struct stfm1000_rds_state *rds)
{
      return bits_filled(&rds->bitstream);
}

static const u32 rds_ParityCheck[] = {
      0x31B, 0x38F, 0x2A7, 0x0F7, 0x1EE,
      0x3DC, 0x201, 0x1BB, 0x376, 0x355,
      0x313, 0x39F, 0x287, 0x0B7, 0x16E,
      0x2DC, 0x001, 0x002, 0x004, 0x008,
      0x010, 0x020, 0x040, 0x080, 0x100,
      0x200
};

static int calc_syndrome(u32 rdscrc)
{
      int i;
      u32 syndrome = 0;
      int word = 0x1;

      for (i = 0; i < 26; i++) {
            if (rdscrc & word)
                  syndrome ^= rds_ParityCheck[i];
            word <<= 1;
      }
      return syndrome;
}

static u32 ecc_table[1024];
static int ecc_table_generated;

static void generate_ecc_table(void)
{
      int i, j, size;
      u32 syndrome, word;

      for (i = 0; i < ECC_TBL_SIZE; i++)
            ecc_table[i] = 0xFFFFFFFF;
      ecc_table[0] = 0x0;

      for (j = 0; j < 5; j++) {
            word = (1 << (j + 1)) - 1;    /* 0x01 0x03 0x07 0x0f 0x1f */
            size = 26 - j;                /* 26, 25, 24, 23, 22 */
            syndrome = 0;
            for (i = 0; i < size; i++) {
                  syndrome = calc_syndrome(word);
                  ecc_table[syndrome] = word;
                  word <<= 1;
            }
      }
}

static u32 ecc_correct(u32 rdsBits, int *recovered)
{
      u32 syndrome;
      u32 errorBits;

      if (recovered)
            *recovered = 0;

      /* Calculate Syndrome on Received Packet */
      syndrome = calc_syndrome(rdsBits);

      if (syndrome == 0)
            return rdsBits; /* block is clean */

      /* generate table first time we get here */
      if (!ecc_table_generated) {
            generate_ecc_table();
            ecc_table_generated = 1;
      }

      /* Attempt to recover block */
      errorBits = ecc_table[syndrome];
      if (errorBits == UNRECOVERABLE_RDS_BLOCK)
            return UNRECOVERABLE_RDS_BLOCK;     /* Block can not be recovered.
                                     * it is bad packet */

      rdsBits = rdsBits ^ errorBits;
      if (recovered)
            (*recovered)++;
      return rdsBits;                 /* ECC correct */
}

/* The following table lists the RDS and RBDS Program Type codes
 * and their meanings:
 * PTY code RDS Program type RBDS Program type */
static const struct stfm1000_rds_pty stc_tss_pty_tab[] = {
      {  0, "No program type",    "No program type"},
      {  1, "News",               "News"},
      {  2, "Current affairs",    "Information"},
      {  3, "Information",        "Sports"},
      {  4, "Sports",             "Talk"},
      {  5, "Education",          "Rock"},
      {  6, "Drama",              "Classic Rock"},
      {  7, "Culture",            "Adult Hits"},
      {  8, "Science",            "Soft Rock"},
      {  9, "Varied",             "Top 40"},
      { 10, "Pop",                "Music Country"},
      { 11, "Rock",               "Music Oldies"},
      { 12, "M.O.R.",             "Music Soft"},
      { 13, "Light classical",    "Nostalgia"},
      { 14, "Serious",            "Classical Jazz"},
      { 15, "Other Music",        "Classical"},
      { 16, "Weather",            "Rhythm and Blues"},
      { 17, "Finance",            "Soft Rhythm and Blues"},
      { 18, "Children's programs", "Language"},
      { 19, "Social Affairs",     "Religious Music"},
      { 20, "Religion",           "Religious Talk"},
      { 21, "Phone In",           "Personality"},
      { 22, "Travel",             "Public"},
      { 23, "Leisure",            "College"},
      { 24, "Jazz Music",         "Unassigned"},
      { 25, "Country Music",      "Unassigned"},
      { 26, "National Music",     "Unassigned"},
      { 27, "Oldies Music",       "Unassigned"},
      { 28, "Folk Music",         "Unassigned"},
      { 29, "Documentary",        "Weather"},
      { 30, "Alarm Test",         "Emergency Test"},
      { 31, "Alarm",              "Emergency"},
};

#if 0
static const char *rds_group_txt[] = {
      [RDS_GROUP_TYPE_0A] = "Basic tuning and switching information (0A)",
      [RDS_GROUP_TYPE_0B] = "Basic tuning and switching information (0B)",
      [RDS_GROUP_TYPE_1A] = "Program item number and slow labeling codes",
      [RDS_GROUP_TYPE_1B] = "Program item number",
      [RDS_GROUP_TYPE_2A] = "Radio Text (2A)",
      [RDS_GROUP_TYPE_2B] = "Radio Text (2B)",
      [RDS_GROUP_TYPE_3A] = "Application identification for ODA only",
      [RDS_GROUP_TYPE_3B] = "Open data applications",
      [RDS_GROUP_TYPE_4A] = "Clock-time and date",
      [RDS_GROUP_TYPE_4B] = "Open data applications",
      [RDS_GROUP_TYPE_5A] = "Transparent Data Channels (32 ch.) or ODA (5A)",
      [RDS_GROUP_TYPE_5B] = "Transparent Data Channels (32 ch.) or ODA (5B)",
      [RDS_GROUP_TYPE_6A] = "In House Applications or ODA (6A)",
      [RDS_GROUP_TYPE_6B] = "In House Applications or ODA (6B)",
      [RDS_GROUP_TYPE_7A] = "Radio Paging or ODA",
      [RDS_GROUP_TYPE_7B] = "Open Data Applications",
      [RDS_GROUP_TYPE_8A] = "Traffic Message Channel or ODA",
      [RDS_GROUP_TYPE_8B] = "Open Data Applications",
      [RDS_GROUP_TYPE_9A] = "Emergency warning system or ODA",
      [RDS_GROUP_TYPE_9B] = "Open Data Applications",
      [RDS_GROUP_TYPE_10A] = "Program Type Name",
      [RDS_GROUP_TYPE_10B] = "Open Data Applications (10B)",
      [RDS_GROUP_TYPE_11A] = "Open Data Applications (11A)",
      [RDS_GROUP_TYPE_11B] = "Open Data Applications (11B)",
      [RDS_GROUP_TYPE_12A] = "Open Data Applications (12A)",
      [RDS_GROUP_TYPE_12B] = "Open Data Applications (12B)",
      [RDS_GROUP_TYPE_13A] = "Enhanced Radio Paging or ODA",
      [RDS_GROUP_TYPE_13B] = "Open Data Applications",
      [RDS_GROUP_TYPE_14A] = "Enhanced Other Networks information (14A)",
      [RDS_GROUP_TYPE_14B] = "Enhanced Other Networks information (14B)",
      [RDS_GROUP_TYPE_15A] = "Defined in RBDS",
      [RDS_GROUP_TYPE_15B] = "Fast switching information",
};
#endif

static void dump_rds_packet(u8 *buf)
{
      u16 pi, offb;

      pi   = (u16)(buf[0] << 8) | buf[1];
      offb = (u16)(buf[1] << 8) | buf[2];

      printk(KERN_INFO "GRP: "
            "PI=0x%04x "
            "GT=%2d VER=%d TP=%d PTY=%2d "
            "PS_SEG=%2d RT_AB=%2d RT_SEG=%2d\n", pi,
            RDS_GROUP_TYPE(offb), RDS_VERSION(offb), RDS_TP(offb),
            RDS_PTY(offb),
            RDS_PS_SEG(offb), RDS_RT_AB(offb), RDS_RT_SEG(offb));
}

void stfm1000_rds_process_packet(struct stfm1000_rds_state *rds, u8 *buffer)
{
      struct stfm1000_rds_text *rdst = &rds->text;
      /* char tempCallLetters[5] = {0}; */
      struct rds_group_data grp;
      int grpno;
      u32 offset;
      char tps[9];
      int i, seg, idx;

      grp.piCode  = ((u16)buffer[0] << 8) | buffer[1];
      grp.offsetB = ((u16)buffer[2] << 8) | buffer[3];
      grp.offsetC = ((u16)buffer[4] << 8) | buffer[5];
      grp.offsetD = ((u16)buffer[6] << 8) | buffer[7];

      grpno = (grp.offsetB >> (8 + 3)) & 0x1f;

      if (rds_state_to_stfm1000(rds)->rds_info)
            dump_rds_packet(buffer);

      /* Is this the first time through? */
      if (!rdst->bRds_detected) {
            rdst->pi            = grp.piCode;
            rdst->tp            = RDS_TP(grp.offsetB);
            rdst->version       = RDS_VERSION(grp.offsetB);
            rdst->pty.id        = RDS_PTY(grp.offsetB);
            rdst->pty.pRds      = stc_tss_pty_tab[rdst->pty.id].pRds;
            rdst->pty.pRdbs     = stc_tss_pty_tab[rdst->pty.id].pRdbs;
            rdst->bRds_detected = 1;
      }

      /* Have we process too many PI errors? */
      if (grp.piCode != rdst->pi) {
            if (rdst->mismatch++ > 10) {

                  /* requested reset of RDS */
                  rds->reset_req = 1;

                  /* signal monitor thread */
                  stfm1000_monitor_signal(rds_state_to_stfm1000(rds),
                        EVENT_RDS_RESET);

                  if (rds_state_to_stfm1000(rds)->rds_info)
                        printk(KERN_INFO "RDS: RESET!!!\n");

                  text_reset(rdst);
            }
            rdst->consecutiveGood = 0;
            return;
      }

      if (rdst->consecutiveGood++ > 10)
            rdst->mismatch = 0;            /* reset bad count */

      if (rdst->consecutiveGood > rdst->consecutiveGoodMax)
            rdst->consecutiveGoodMax = rdst->consecutiveGood;

      switch (grpno) {
      case RDS_GROUP_TYPE_0A:
      case RDS_GROUP_TYPE_0B:
            /* Extract Service Name information */
            offset = RDS_PS_SEG(grp.offsetB) * 2;
            rdst->wk_ps[offset]     = buffer[6];      /* better */
            rdst->wk_ps[offset + 1] = buffer[7];
            rdst->wk_ps_mask |= 1 << RDS_PS_SEG(grp.offsetB);

            if (rds_state_to_stfm1000(rds)->rds_info) {
                  for (i = 0; i < 8; i++) {
                        if (rdst->wk_ps_mask & (1 << i)) {
                              tps[i * 2] =
                                    rdst->wk_ps[i * 2];
                              tps[i * 2 + 1] =
                                    rdst->wk_ps[i * 2 + 1];
                        } else {
                              tps[i * 2] = '_';
                              tps[i * 2 + 1] = '_';
                        }
                  }
                  tps[ARRAY_SIZE(tps) - 1] = '\0';
                  if (rds_state_to_stfm1000(rds)->rds_info)
                        printk(KERN_INFO "RDS-PS (curr): %s\n", tps);
            }

            if (rdst->wk_ps_mask != ALL_SEGMENT_BITS)
                  break;

            if (rdst->ps_valid) {
                  if (memcmp(rdst->ps, rdst->wk_ps, 8) != 0) {
                        memset(rdst->cp_ps, 0, 8);
                        memset(rdst->wk_ps, 0, 8);
                        rdst->wk_ps_mask = 0;
                  }

                  memset(rdst->ps, 0, 8);
                  rdst->ps_valid = 0;
                  break;
            }

            /* does working buffer == compare buffer */
            if (memcmp(rdst->cp_ps, rdst->wk_ps, 8) != 0) {
                  /* just copy from working to compare buffer */
                  memcpy(rdst->cp_ps, rdst->wk_ps, 8);
                  rdst->wk_ps_mask = 0;
                  break;
            }

            /* working buffer matches compare buffer, send to UI */
            memcpy(rdst->ps, rdst->cp_ps, 8);
            rdst->ps_valid = 1;

            if (rds_state_to_stfm1000(rds)->rds_info)
                  printk(KERN_INFO "RDS: PS '%s'\n", rdst->ps);

            /* clear working mask-only */
            rdst->wk_ps_mask = 0;
            break;

      case RDS_GROUP_TYPE_2A:

            /* Clear buffer */
            if (rdst->textAB_flag != RDS_RT_AB(grp.offsetB)) {
                  memset(rdst->wk_text, 0, 64);
                  rdst->wk_text_mask = 0;
                  rdst->textAB_flag  = RDS_RT_AB(grp.offsetB);
            }

            /* Extract Text */
            seg = RDS_RT_SEG(grp.offsetB);
            idx = seg * 4;

            #define CNVT_EOT(x) ((x) != RDS_EOT ? (x) : 0)
            rdst->wk_text[idx++] = CNVT_EOT(buffer[4]);
            rdst->wk_text[idx++] = CNVT_EOT(buffer[5]);
            rdst->wk_text[idx++] = CNVT_EOT(buffer[6]);
            rdst->wk_text[idx++] = CNVT_EOT(buffer[7]);

            rdst->wk_text_mask |= 1 << seg;
            /* scan msg data for EOT.  If found set all higher
             * mask bits */
            for (idx = 0; idx < 4; idx++) {
                  if (rdst->text[idx] == RDS_EOT)
                        break;
            }
            if (idx < 4) {
                  /* set current and all higher bits  */
                  for (idx = RDS_RT_SEG(grp.offsetB); idx < 16;
                        idx++)
                        rdst->wk_text_mask |= 1 << idx;
            }

            /* Process buffer when filled */
            if (rdst->wk_text_mask != ALL_TEXT_BITS)
                  break;

            if (!rdst->text_valid)
                  rdst->text_valid = 1;
            else if (memcmp(rdst->text, rdst->wk_text, 64) == 0)
                  break;

            memcpy(rdst->text, rdst->wk_text, 64);

            if (rds_state_to_stfm1000(rds)->rds_info)
                  printk(KERN_INFO "RDS: TEXT '%s'\n", rdst->text);

            memset(rdst->wk_text, 0, 64);
            rdst->wk_text_mask = 0;
            break;

      default:
            break;
      }
}

int stfm1000_rds_packet_dequeue(struct stfm1000_rds_state *rds, u8 *buf)
{
      struct stfm1000_rds_pkt *pkt = &rds->pkt;

      if (pkt->buf_cnt == 0)
            return -1;

      memcpy(buf, &pkt->buf_queue[pkt->buf_tail][0], 8);
      if (++pkt->buf_tail >= RDS_PKT_QUEUE)
            pkt->buf_tail = 0;
      pkt->buf_cnt--;

      return 0;
}

void stfm1000_rds_packet_bit(struct stfm1000_rds_state *rds, int bit)
{
      struct stfm1000_rds_pkt *pkt = &rds->pkt;
      u32 rdsdata, rdscrc, rdscrc_c, rdscrc_cp;
      int correct, correct2, recovered, recovered2;
      int RetVal;

      /* Stick into shift register */
      pkt->rdsstream = ((pkt->rdsstream << 1) | bit) & 0x03ffffff;
      pkt->bitsinfifo++;
      pkt->bitcount++;

      /* wait for 26 bits of block */
      if (pkt->bitsinfifo < 26)
            return;

      rdsdata = pkt->rdsstream & 0x03fffc00;    /* 16 bits of Info. word */
      rdscrc = pkt->rdsstream & 0x3ff;    /* 10 bits of Checkword */

      switch (pkt->state) {
      case SYNC_OFFSET_A:

            RetVal = calc_syndrome(pkt->rdsstream);

            switch (RetVal) {
            case RDS_SYNDROME_OFFSETA:
                  pkt->state = OFFSET_B;
                  break;
            case RDS_SYNDROME_OFFSETB:
                  pkt->state = OFFSET_C_CP;
                  break;
            case RDS_SYNDROME_OFFSETC:
                  pkt->state = OFFSET_D;
                  break;
            case RDS_SYNDROME_OFFSETCP:
                  pkt->state = OFFSET_D;
                  break;
            case RDS_SYNDROME_OFFSETD:
                  pkt->state = OFFSET_A;
                  break;
            default:
                  pkt->state = SYNC_OFFSET_A;
                  break;
            }
            if (pkt->state == SYNC_OFFSET_A) {
                  pkt->sync_lost_packets++;
                  /* XXX send info? */
                  break;
            }

            pkt->good_packets++;

            rdsdata = pkt->rdsstream & 0x03fffc00;

            /* Save type A packet in buffer */
            rdsdata >>= 10;
            pkt->buffer[0] = (rdsdata >> 8);
            pkt->buffer[1] = (rdsdata & 0xff);
            pkt->bitsinfifo = 0;

            /* We found a block with zero errors, but it is not at the
             * start of the group. */
            if (pkt->state == OFFSET_B)
                  pkt->discardpacket = 0;
            else
                  pkt->discardpacket = 1;
            break;

      case OFFSET_A:          /* Type A: we are in sync now */
            rdscrc ^= RDS_OFFSETA;
            correct = ecc_correct(rdsdata | rdscrc, &recovered);
            if (correct == UNRECOVERABLE_RDS_BLOCK) {
                  pkt->bad_packets++;
                  pkt->discardpacket++;
                  pkt->state++;
                  pkt->bitsinfifo = 0;
                  break;
            }

            if (recovered)
                  pkt->recovered_packets++;
            pkt->good_packets++;

            /* Attempt to see, if we can get the entire group.
             * Don't discard. */
            pkt->discardpacket = 0;
            rdsdata = correct & 0x03fffc00;

            /* Save type A packet in buffer */
            rdsdata >>= 10;
            pkt->buffer[0] = (rdsdata >> 8);
            pkt->buffer[1] = (rdsdata & 0xff);
            pkt->bitsinfifo = 0;
            pkt->state++;
            break;

      case OFFSET_B:          /* Waiting for type B */
            rdscrc ^= RDS_OFFSETB;
            correct = ecc_correct(rdsdata | rdscrc, &recovered);
            if (correct == UNRECOVERABLE_RDS_BLOCK) {
                  pkt->bad_packets++;
                  pkt->discardpacket++;
                  pkt->state++;
                  pkt->bitsinfifo = 0;
                  break;
            }
            if (recovered)
                  pkt->recovered_packets++;
            pkt->good_packets++;

            rdsdata = correct & 0x03fffc00;

            /* Save type B packet in buffer */
            rdsdata >>= 10;
            pkt->buffer[2] = (rdsdata >> 8);
            pkt->buffer[3] = (rdsdata & 0xff);
            pkt->bitsinfifo = 0;
            pkt->state++;
            break;

      case OFFSET_C_CP: /* Waiting for type C or C' */
            rdscrc_c = rdscrc ^ RDS_OFFSETC;
            rdscrc_cp = rdscrc ^ RDS_OFFSETCP;
            correct = ecc_correct(rdsdata | rdscrc_c, &recovered);
            correct2 = ecc_correct(rdsdata | rdscrc_cp, &recovered2);
            if (correct == UNRECOVERABLE_RDS_BLOCK
                && correct2 == UNRECOVERABLE_RDS_BLOCK) {
                  pkt->bad_packets++;
                  pkt->discardpacket++;
                  pkt->state++;
                  pkt->bitsinfifo = 0;
                  break;
            }

            if (recovered || recovered2)
                  pkt->recovered_packets++;
            pkt->good_packets++;

            if (correct == UNRECOVERABLE_RDS_BLOCK)
                  correct = correct2;

            rdsdata = correct & 0x03fffc00;

            /* Save type C packet in buffer */
            rdsdata >>= 10;
            pkt->buffer[4] = (rdsdata >> 8);
            pkt->buffer[5] = (rdsdata & 0xff);
            pkt->bitsinfifo = 0;
            pkt->state++;
            break;

      case OFFSET_D:          /* Waiting for type D */
            rdscrc ^= RDS_OFFSETD;
            correct = ecc_correct(rdsdata | rdscrc, &recovered);
            if (correct == UNRECOVERABLE_RDS_BLOCK) {
                  pkt->bad_packets++;
                  pkt->discardpacket++;
                  pkt->state = OFFSET_A;
                  pkt->bitsinfifo = 0;
                  break;
            }

            if (recovered)
                  pkt->recovered_packets++;
            pkt->good_packets++;

            rdsdata = correct & 0x03fffc00;

            /* Save type D packet in buffer */
            rdsdata >>= 10;
            pkt->buffer[6] = (rdsdata >> 8);
            pkt->buffer[7] = (rdsdata & 0xff);

            /* buffer it if all segments were ok */
            if (pkt->discardpacket) {
                  /* We're still in sync, so back to state 1 */
                  pkt->state = OFFSET_A;
                  pkt->bitsinfifo = 0;
                  pkt->discardpacket = 0;
                  break;
            }

            pkt->state++;
            /* fall-through */

      case PACKET_OUT:
            pkt->GroupDropOnce = 1;

            /* queue packet */
            if (pkt->buf_cnt < RDS_PKT_QUEUE) {
                  memcpy(&pkt->buf_queue[pkt->buf_head][0],
                        pkt->buffer, 8);
                  if (++pkt->buf_head >= RDS_PKT_QUEUE)
                        pkt->buf_head = 0;
                  pkt->buf_cnt++;
            } else
                  pkt->buf_overruns++;

            /* We're still in sync, so back to state 1 */
            pkt->state = OFFSET_A;
            pkt->bitsinfifo = 0;
            pkt->discardpacket = 0;
            break;

      }

      /* Lots of errors? If so, go back to resync mode */
      if (pkt->discardpacket >= 10) {
            pkt->state = SYNC_OFFSET_A;   /* reset sync state */
            pkt->bitsinfifo = 26;   /* resync a bit faster */
      }
}

/* GROUP_TYPE 0A-0B (buffer must have enough space for 9 bytes) */
int stfm1000_rds_get_ps(struct stfm1000_rds_state *rds, u8 *buffer,
      int bufsize)
{
      struct stfm1000_rds_text *rdst = &rds->text;

      if (bufsize < 9)
            return -1;

      if (!rdst->ps_valid)
            return -1;

      memcpy(buffer, rdst->ps, 8);
      buffer[8] = '\0';

      return 8;
}

/* GROUP_TYPE 2A (buffer must have enough space for 65 bytes) */
int stfm1000_rds_get_text(struct stfm1000_rds_state *rds, u8 *buffer,
      int bufsize)
{
      struct stfm1000_rds_text *rdst = &rds->text;

      if (bufsize < 9)
            return -1;

      if (!rdst->text_valid)
            return -1;

      memcpy(buffer, rdst->text, 64);
      buffer[64] = '\0';

      return 64;
}

Generated by  Doxygen 1.6.0   Back to index