diff options
Diffstat (limited to 'drivers/media/radio/stfm1000/stfm1000-rds.c')
| -rw-r--r-- | drivers/media/radio/stfm1000/stfm1000-rds.c | 1529 |
1 files changed, 1529 insertions, 0 deletions
diff --git a/drivers/media/radio/stfm1000/stfm1000-rds.c b/drivers/media/radio/stfm1000/stfm1000-rds.c new file mode 100644 index 000000000000..5f63052d00c2 --- /dev/null +++ b/drivers/media/radio/stfm1000/stfm1000-rds.c @@ -0,0 +1,1529 @@ +/* + * 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; +} |