/* * 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 #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; }