| /* |
| * Copyright 2008, 2009, 2010 Free Software Foundation, Inc. |
| * |
| * This software is distributed under the terms of the GNU Public License. |
| * See the COPYING file in the main directory for details. |
| * |
| * This use of this software may be subject to additional restrictions. |
| * See the LEGAL file in the main directory for details. |
| |
| This program is free software: you can redistribute it and/or modify |
| it under the terms of the GNU General Public License as published by |
| the Free Software Foundation, either version 3 of the License, or |
| (at your option) any later version. |
| |
| This program is distributed in the hope that it will be useful, |
| but WITHOUT ANY WARRANTY; without even the implied warranty of |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| GNU General Public License for more details. |
| |
| You should have received a copy of the GNU General Public License |
| along with this program. If not, see <http://www.gnu.org/licenses/>. |
| */ |
| |
| #include "BitVector.h" |
| #include "osmocom/core/bits.h" |
| #include <stdio.h> |
| #include <Logger.h> |
| #include "Transceiver2.h" |
| #include <grgsm_vitac/grgsm_vitac.h> |
| |
| extern "C" { |
| #include "sch.h" |
| } |
| |
| #ifdef HAVE_CONFIG_H |
| #include "config.h" |
| #endif |
| |
| #include "l1if.h" |
| |
| using namespace GSM; |
| |
| #define USB_LATENCY_INTRVL 10,0 |
| |
| #if USE_UHD |
| # define USB_LATENCY_MIN 6,7 |
| #else |
| # define USB_LATENCY_MIN 1,1 |
| #endif |
| |
| /* Clock indication interval in frames */ |
| #define CLK_IND_INTERVAL 100 |
| |
| /* Number of running values use in noise average */ |
| #define NOISE_CNT 20 |
| #define FREQ_CNT 20 |
| |
| TransceiverState::TransceiverState() |
| : mRetrans(false), mFreqOffsets(FREQ_CNT), mode(Transceiver2::TRX_MODE_OFF) |
| { |
| for (int i = 0; i < 8; i++) { |
| chanType[i] = Transceiver2::NONE; |
| fillerModulus[i] = 26; |
| prevFrame[i] = NULL; |
| |
| for (int n = 0; n < 102; n++) |
| fillerTable[n][i] = NULL; |
| } |
| } |
| |
| TransceiverState::~TransceiverState() |
| { |
| for (int i = 0; i < 8; i++) { |
| for (int n = 0; n < 102; n++) |
| delete fillerTable[n][i]; |
| } |
| } |
| |
| void TransceiverState::init(size_t slot, signalVector *burst, bool fill) |
| { |
| signalVector *filler; |
| |
| for (int i = 0; i < 102; i++) { |
| if (fill) |
| filler = new signalVector(*burst); |
| else |
| filler = new signalVector(burst->size()); |
| |
| fillerTable[i][slot] = filler; |
| } |
| } |
| extern void initvita(); |
| Transceiver2::Transceiver2(int wBasePort, |
| const char *TRXAddress, |
| size_t wSPS, size_t wChans, |
| GSM::Time wTransmitLatency, |
| RadioInterface *wRadioInterface) |
| : mChans(wChans), rx_sps(4), tx_sps(4), mAddr(TRXAddress), mTransmitLatency(wTransmitLatency), |
| mTxPriorityQueues(mChans), mReceiveFIFO(mChans), mRxServiceLoopThreads(mChans), |
| mControlServiceLoopThreads(mChans), mTxPriorityQueueServiceLoopThreads(mChans), mRadioInterface(wRadioInterface), |
| mOn(false), mTxFreq(0.0), mRxFreq(0.0), mPower(-10), mMaxExpectedDelay(0), mBSIC(-1), mStates(mChans) |
| { |
| GSM::Time startTime(random() % gHyperframe,0); |
| |
| mLowerLoopThread = new Thread(32768); |
| |
| mTransmitDeadlineClock = startTime; |
| mLastClockUpdateTime = startTime; |
| mLatencyUpdateTime = startTime; |
| mRadioInterface->getClock()->set(startTime); |
| |
| txFullScale = mRadioInterface->fullScaleInputValue(); |
| rxFullScale = mRadioInterface->fullScaleOutputValue(); |
| |
| for (int i = 0; i < 8; i++) |
| mRxSlotMask[i] = 0; |
| |
| initvita(); |
| } |
| |
| Transceiver2::~Transceiver2() |
| { |
| stop(); |
| |
| sigProcLibDestroy(); |
| |
| for (size_t i = 0; i < mChans; i++) { |
| mTxPriorityQueues[i].clear(); |
| } |
| } |
| |
| bool Transceiver2::init(bool filler) |
| { |
| |
| if (!mChans) { |
| LOG(ALERT) << "No channels assigned"; |
| return false; |
| } |
| |
| if (!sigProcLibSetup()) { |
| LOG(ALERT) << "Failed to initialize signal processing library"; |
| return false; |
| } |
| |
| |
| |
| /* Filler table retransmissions - support only on channel 0 */ |
| if (filler) |
| mStates[0].mRetrans = true; |
| |
| |
| for (size_t i = 0; i < mChans; i++) { |
| mControlServiceLoopThreads[i] = new Thread(32768); |
| mTxPriorityQueueServiceLoopThreads[i] = new Thread(32768); |
| mRxServiceLoopThreads[i] = new Thread(32768); |
| |
| for (size_t n = 0; n < 8; n++) { |
| // burst = modulateBurst(gDummyBurst, 8 + (n % 4 == 0), tx_sps); |
| // scaleVector(*burst, txFullScale); |
| // mStates[i].init(n, burst, filler && !i); |
| // delete burst; |
| } |
| } |
| |
| return true; |
| } |
| |
| void Transceiver2::addRadioVector(size_t chan, BitVector &bits, |
| int RSSI, GSM::Time &wTime) |
| { |
| signalVector *burst; |
| radioVector *radio_burst; |
| |
| if (chan >= mTxPriorityQueues.size()) { |
| LOG(ALERT) << "Invalid channel " << chan; |
| return; |
| } |
| |
| if (wTime.TN() > 7) { |
| LOG(ALERT) << "Received burst with invalid slot " << wTime.TN(); |
| return; |
| } |
| |
| if (mStates[0].mode != TRX_MODE_BTS) |
| return; |
| |
| burst = modulateBurst(bits, 8 + (wTime.TN() % 4 == 0), tx_sps); |
| scaleVector(*burst, txFullScale * pow(10, -RSSI / 10)); |
| |
| radio_burst = new radioVector(wTime, burst); |
| |
| mTxPriorityQueues[chan].write(radio_burst); |
| } |
| |
| void Transceiver2::updateFillerTable(size_t chan, radioVector *burst) |
| { |
| int TN, modFN; |
| TransceiverState *state = &mStates[chan]; |
| |
| TN = burst->getTime().TN(); |
| modFN = burst->getTime().FN() % state->fillerModulus[TN]; |
| |
| delete state->fillerTable[modFN][TN]; |
| state->fillerTable[modFN][TN] = burst->getVector(); |
| burst->setVector(NULL); |
| } |
| |
| void Transceiver2::pushRadioVector(GSM::Time &nowTime) |
| { |
| int TN, modFN; |
| radioVector *burst; |
| TransceiverState *state; |
| std::vector<signalVector *> bursts(mChans); |
| std::vector<bool> zeros(mChans, false); |
| std::vector<bool> filler(mChans, true); |
| |
| for (size_t i = 0; i < mChans; i ++) { |
| state = &mStates[i]; |
| |
| while ((burst = mTxPriorityQueues[i].getStaleBurst(nowTime))) { |
| LOG(NOTICE) << "dumping STALE burst in TRX->USRP interface"; |
| if (state->mRetrans) |
| updateFillerTable(i, burst); |
| delete burst; |
| } |
| |
| TN = nowTime.TN(); |
| modFN = nowTime.FN() % state->fillerModulus[TN]; |
| |
| bursts[i] = state->fillerTable[modFN][TN]; |
| if (state->mode == TRX_MODE_BTS) |
| zeros[i] = state->chanType[TN] == NONE; |
| |
| if ((burst = mTxPriorityQueues[i].getCurrentBurst(nowTime))) { |
| bursts[i] = burst->getVector(); |
| |
| if (state->mRetrans) { |
| updateFillerTable(i, burst); |
| } else { |
| burst->setVector(NULL); |
| filler[i] = false; |
| } |
| |
| delete burst; |
| } |
| } |
| |
| mRadioInterface->driveTransmitRadio(bursts, zeros); |
| |
| for (size_t i = 0; i < mChans; i++) { |
| if (!filler[i]) |
| delete bursts[i]; |
| } |
| } |
| |
| void Transceiver2::setModulus(size_t timeslot, size_t chan) |
| { |
| TransceiverState *state = &mStates[chan]; |
| |
| switch (state->chanType[timeslot]) { |
| case NONE: |
| case I: |
| case II: |
| case III: |
| case FILL: |
| state->fillerModulus[timeslot] = 26; |
| break; |
| case IV: |
| case VI: |
| case V: |
| state->fillerModulus[timeslot] = 51; |
| break; |
| //case V: |
| case VII: |
| state->fillerModulus[timeslot] = 102; |
| break; |
| case XIII: |
| state->fillerModulus[timeslot] = 52; |
| break; |
| default: |
| break; |
| } |
| } |
| |
| |
| CorrType Transceiver2::expectedCorrType(GSM::Time currTime, |
| size_t chan) |
| { |
| TransceiverState *state = &mStates[chan]; |
| unsigned burstTN = currTime.TN(); |
| unsigned burstFN = currTime.FN(); |
| |
| if (state->mode == TRX_MODE_MS_TRACK) { |
| /* 102 modulus case currently unhandled */ |
| if (state->fillerModulus[burstTN] > 52) |
| return OFF; |
| |
| int modFN = burstFN % state->fillerModulus[burstTN]; |
| unsigned long long reg = (unsigned long long) 1 << modFN; |
| if (reg & mRxSlotMask[burstTN]) |
| return TSC; |
| else |
| return OFF; |
| } |
| |
| switch (state->chanType[burstTN]) { |
| case NONE: |
| return OFF; |
| break; |
| case FILL: |
| return IDLE; |
| break; |
| case I: |
| return TSC; |
| /*if (burstFN % 26 == 25) |
| return IDLE; |
| else |
| return TSC;*/ |
| break; |
| case II: |
| return TSC; |
| break; |
| case III: |
| return TSC; |
| break; |
| case IV: |
| case VI: |
| return RACH; |
| break; |
| case V: { |
| int mod51 = burstFN % 51; |
| if ((mod51 <= 36) && (mod51 >= 14)) |
| return RACH; |
| else if ((mod51 == 4) || (mod51 == 5)) |
| return RACH; |
| else if ((mod51 == 45) || (mod51 == 46)) |
| return RACH; |
| else |
| return TSC; |
| break; |
| } |
| case VII: |
| if ((burstFN % 51 <= 14) && (burstFN % 51 >= 12)) |
| return IDLE; |
| else |
| return TSC; |
| break; |
| case XIII: { |
| int mod52 = burstFN % 52; |
| if ((mod52 == 12) || (mod52 == 38)) |
| return RACH; |
| else if ((mod52 == 25) || (mod52 == 51)) |
| return IDLE; |
| else |
| return TSC; |
| break; |
| } |
| case LOOPBACK: |
| if ((burstFN % 51 <= 50) && (burstFN % 51 >=48)) |
| return IDLE; |
| else |
| return TSC; |
| break; |
| default: |
| return OFF; |
| break; |
| } |
| } |
| |
| |
| /* Detect SCH synchronization sequence within a burst */ |
| bool Transceiver2::detectSCH(TransceiverState *state, |
| signalVector &burst, |
| struct estim_burst_params *ebp) |
| { |
| int shift; |
| sch_detect_type full; |
| float mag, threshold = 5.0; |
| |
| full = (state->mode == TRX_MODE_MS_TRACK) ? |
| sch_detect_type::SCH_DETECT_NARROW : sch_detect_type::SCH_DETECT_FULL; |
| |
| if (!detectSCHBurst(burst, threshold, rx_sps, full, ebp)) |
| return false; |
| |
| std::clog << "SCH : Timing offset " << ebp->toa << " symbols" << std::endl; |
| |
| mag = fabsf(ebp->toa); |
| if (mag < 1.0f) |
| return true; |
| |
| shift = (int) (mag / 2.0f); |
| if (!shift) |
| shift++; |
| |
| shift = ebp->toa > 0 ? shift : -shift; |
| std::clog << "SCH : shift -> " << shift << " symbols" << std::endl; |
| mRadioInterface->applyOffset(shift); |
| return false; |
| } |
| |
| #define SCH_BIT_SCALE 64 |
| |
| /* Decode SCH burst */ |
| bool Transceiver2::decodeSCH(SoftVector *burst, GSM::Time *time) |
| { |
| int fn; |
| struct sch_info sch; |
| ubit_t info[GSM_SCH_INFO_LEN]; |
| sbit_t data[GSM_SCH_CODED_LEN]; |
| |
| if (burst->size() < 156) { |
| std::clog << "Invalid SCH burst length" << std::endl; |
| return false; |
| } |
| |
| float_to_sbit(&(*burst)[3], &data[0], SCH_BIT_SCALE, 39); |
| float_to_sbit(&(*burst)[106], &data[39], SCH_BIT_SCALE, 39); |
| |
| if (!gsm_sch_decode(info, data)) { |
| gsm_sch_parse(info, &sch); |
| |
| mBSIC = sch.bsic; |
| mTSC = mBSIC & 0x7; |
| |
| // std::clog << "SCH : Decoded values" << std::endl; |
| // std::clog << " BSIC: " << sch.bsic << std::endl; |
| // std::clog << " T1 : " << sch.t1 << std::endl; |
| // std::clog << " T2 : " << sch.t2 << std::endl; |
| // std::clog << " T3p : " << sch.t3p << std::endl; |
| // std::clog << " FN : " << gsm_sch_to_fn(&sch) << std::endl; |
| |
| fn = gsm_sch_to_fn(&sch); |
| if (fn < 0) { |
| std::clog << "SCH : Failed to convert FN " << std::endl; |
| return false; |
| } |
| |
| time->FN(fn); |
| time->TN(0); |
| } else { |
| std::clog << "Invalid SCH decode!!" << std::endl; |
| return false; |
| } |
| |
| return true; |
| } |
| |
| #define FCCH_OFFSET_LIMIT 5e3 |
| #define FCCH_ADJUST_LIMIT 20.0 |
| |
| /* Apply FCCH frequency correction */ |
| bool Transceiver2::correctFCCH(TransceiverState *state, signalVector *burst) |
| { |
| double offset; |
| |
| if (!burst) |
| return false; |
| |
| offset = gsm_fcch_offset((float *) burst->begin(), burst->size()); |
| //std::cout << "XXXX FCCH: Frequency offset " << offset << " Hz" << std::endl; |
| if (offset > FCCH_OFFSET_LIMIT) |
| return false; |
| |
| state->mFreqOffsets.insert(offset); |
| |
| if (state->mFreqOffsets.full()) { |
| double avg = state->mFreqOffsets.avg(); |
| std::clog << "FCCH: Frequency offset " << avg << " Hz" << std::endl; |
| |
| if (fabs(avg) > FCCH_ADJUST_LIMIT) { |
| mRadioInterface->tuneRxOffset(-avg); |
| state->mFreqOffsets.reset(); |
| } |
| } |
| |
| return true; |
| } |
| |
| extern int process_vita_burst(std::complex<float>* input, int tsc, unsigned char* output_binary); |
| /* |
| * Pull bursts from the FIFO and handle according to the slot |
| * and burst correlation type. Equalzation is currently disabled. |
| */ |
| SoftVector *Transceiver2::pullRadioVector(GSM::Time &wTime, int &RSSI, |
| int &timingOffset, size_t chan) __attribute__((optnone)) |
| { |
| struct estim_burst_params ebp; |
| int rc; |
| float pow, max = -1.0, avg = 1.0; |
| int max_i = -1; |
| signalVector *burst; |
| SoftVector *bits = NULL; |
| TransceiverState *state = &mStates[chan]; |
| bool printme = 0; |
| GSM::Time sch_time, burst_time, diff_time; |
| |
| /* Blocking FIFO read */ |
| radioVector *radio_burst = mReceiveFIFO[chan]->read(); |
| if (!radio_burst) |
| return NULL; |
| |
| /* Set time and determine correlation type */ |
| burst_time = radio_burst->getTime(); |
| |
| #if 0 |
| if (state->mode == TRX_MODE_MS_ACQUIRE) { |
| switch(fbsb_acq_buf.s.load()) { |
| case fbsb_par::fbsb_state::IDLE: |
| case fbsb_par::fbsb_state::INIT: |
| fbsb_acq_buf.s.store(fbsb_par::fbsb_state::ACQ); |
| case fbsb_par::fbsb_state::ACQ: |
| fbsb_acq_buf.take(radio_burst->getVector()->begin(), radio_burst->getVector()->size(), burst_time); |
| |
| if(!fbsb_acq_buf.done()) { |
| delete radio_burst; |
| return nullptr; |
| } |
| fbsb_acq_buf.s.store(fbsb_par::fbsb_state::ACQ_COMPL); |
| // break; |
| case fbsb_par::fbsb_state::ACQ_COMPL: |
| |
| |
| { |
| complex famp = 0; |
| int found_index; |
| auto foundat = fbsb_acq_buf.fcch(&famp, &found_index, false); |
| std::cerr << "@ " << found_index << std::endl; |
| foundat = found_index; |
| |
| auto framelen = (3 + 142 + 3 + 8.25); // 1sps!; |
| int searchbegin = 8*framelen-20; |
| int searchend = 10*framelen+20; |
| |
| if(famp.abs() < 2000 || foundat+searchend > fbsb_acq_buf.sz()) { |
| mReceiveFIFO[chan]->clear(); |
| delete radio_burst; |
| fbsb_acq_buf.reset(); |
| fbsb_acq_buf.s.store(fbsb_par::fbsb_state::ACQ); |
| return nullptr; |
| } |
| correctFCCH_raw(state, &fbsb_acq_buf.fbsb_buf[foundat], 98); |
| |
| burst = new signalVector(searchend-searchbegin, GSM::gRACHSynchSequence.size()); |
| // burst = new signalVector(fbsb_acq_buf.fbsb_buf, foundat+searchbegin, searchend-searchbegin); |
| memcpy(burst->begin(), &fbsb_acq_buf.fbsb_buf[foundat+searchbegin], (searchend-searchbegin) * sizeof(complex)); |
| success = detectSCH(state, *burst, amp, toa, 0); // will "fail" if sample adjustment is required |
| std::cerr << "###detected sch: " << success << "at toa " << toa << std::endl; |
| mReceiveFIFO[chan]->clear(); |
| delete radio_burst; |
| |
| if(toa > 0) { |
| int how_many_ts = (found_index+searchbegin)/framelen; |
| // int how_many_fn = how_many_ts/8; |
| auto t = fbsb_acq_buf.rcvClock[how_many_ts]; |
| |
| diff_time = GSM::Time(sch_time.FN() - t.FN(),-t.TN()); |
| mRadioInterface->adjustClock(diff_time); |
| mTransmitDeadlineClock = RadioClock::adjust( |
| mTransmitDeadlineClock, |
| diff_time); |
| } |
| if(!success) { |
| fbsb_acq_buf.reset(); |
| fbsb_acq_buf.s.store(fbsb_par::fbsb_state::ACQ); |
| } |
| else |
| fbsb_acq_buf.s.store(fbsb_par::fbsb_state::DONE); |
| return nullptr; |
| } |
| case fbsb_par::fbsb_state::DONE: |
| break; |
| default: |
| // fbsb_acq_buf.s = fbsb_par::fbsb_state::WAIT; |
| return nullptr; |
| break; |
| /* no-op */ |
| } |
| |
| } |
| #endif |
| CorrType type = expectedCorrType(burst_time, chan); |
| |
| switch (state->mode) { |
| case TRX_MODE_MS_ACQUIRE: |
| type = SCH; |
| break; |
| case TRX_MODE_MS_TRACK: |
| if (gsm_sch_check_fn(burst_time.FN()) && burst_time.TN() == 0) |
| type = SCH; |
| else if(burst_time.TN() == 0 && !gsm_fcch_check_fn(burst_time.FN())) // all ts0, but not fcch or sch.. |
| type = TSC; |
| else if (type == OFF) |
| goto release; |
| break; |
| case TRX_MODE_BTS: |
| if ((type == TSC) || (type == RACH)) |
| break; |
| case TRX_MODE_OFF: |
| default: |
| goto release; |
| } |
| |
| /* Select the diversity channel with highest energy */ |
| for (size_t i = 0; i < radio_burst->chans(); i++) { |
| float pow = energyDetect(*radio_burst->getVector(i), 20 * rx_sps); |
| if (pow > max) { |
| max = pow; |
| max_i = i; |
| } |
| avg += pow; |
| } |
| |
| if (max_i < 0) { |
| LOG(ALERT) << "Received empty burst"; |
| goto release; |
| } |
| |
| /* Average noise on diversity paths and update global levels */ |
| burst = radio_burst->getVector(max_i); |
| avg = sqrt(avg / radio_burst->chans()); |
| |
| |
| /* Detect normal or RACH bursts */ |
| if (type == SCH) { |
| rc = detectSCH(state, *burst, &ebp); |
| rc = rc > 0 ? rc : -1; |
| } else { |
| rc = detectAnyBurst(*burst, mTSC, BURST_THRESH, rx_sps, type, mMaxExpectedDelay, &ebp); |
| if(rc > 0) { |
| type = (CorrType)rc; |
| } |
| } |
| |
| |
| if (rc < 0) |
| goto release; |
| |
| #if 0 |
| if(type == SCH){ |
| unsigned char outbin[148] = {0}; |
| |
| bits = demodAnyBurst(*burst, type, rx_sps, &ebp); |
| |
| |
| auto start = reinterpret_cast<float*>(burst->begin()); |
| for(int i=0; i < 625*2; i++) |
| start[i] *= 1./2047.; |
| |
| auto ss = reinterpret_cast<std::complex<float>*>(burst->begin()); |
| |
| // int ret = process_vita_sc_burst(ss, mTSC, outbin, 0); |
| |
| { |
| std::vector<gr_complex> channel_imp_resp(CHAN_IMP_RESP_LENGTH* d_OSR); |
| /* Get channel impulse response */ |
| auto d_c0_burst_start = get_sch_chan_imp_resp(ss, &channel_imp_resp[0], 0, (SYNC_POS + SYNC_SEARCH_RANGE) * d_OSR + SYNC_POS * d_OSR ); |
| |
| if(d_c0_burst_start < 0) { |
| std::cerr << " fck! offset <0! " << ebp.toa << std::endl; |
| d_c0_burst_start = 0; |
| } else { |
| std::cerr << " ## offset " << d_c0_burst_start << " vs " << ebp.toa << std::endl; |
| } |
| /* Perform MLSE detection */ |
| detect_burst(ss, &channel_imp_resp[0], |
| d_c0_burst_start, outbin); |
| } |
| |
| // auto bits2 = new SoftVector(); |
| // bits2->resize(156); |
| // for(int i=0; i < 148; i++) |
| // (*bits2)[i] = outbin[i] < 1 ? -1 : 1; |
| |
| |
| |
| printme = true; |
| |
| if(printme) { |
| std::cerr << std::endl << "vita:" << std::endl; |
| for(auto i : outbin) |
| std::cerr << (int) i; |
| std::cerr << std::endl << "org:" << std::endl; |
| for(int i=0; i < 148; i++) |
| std::cerr << (int) (bits->operator[](i) > 0 ? 1 : 0); |
| } |
| |
| } else |
| #endif |
| if (type == TSC) { |
| unsigned char outbin[148]; |
| |
| // bits = demodAnyBurst(*burst, type, rx_sps, &ebp); |
| |
| { |
| auto start = reinterpret_cast<float *>(burst->begin()); |
| for (int i = 0; i < 625 * 2; i++) |
| start[i] *= 1. / 2047.; |
| |
| auto ss = reinterpret_cast<std::complex<float> *>(burst->begin()); |
| // int ret = process_vita_burst(reinterpret_cast<std::complex<float>*>(burst->begin()), mTSC, outbin); |
| |
| float ncmax, dcmax; |
| std::vector<gr_complex> channel_imp_resp(CHAN_IMP_RESP_LENGTH * d_OSR), |
| channel_imp_resp2(CHAN_IMP_RESP_LENGTH * d_OSR); |
| auto normal_burst_start = get_norm_chan_imp_resp(ss, &channel_imp_resp[0], &ncmax, mTSC); |
| auto dummy_burst_start = get_norm_chan_imp_resp(ss, &channel_imp_resp2[0], &dcmax, TS_DUMMY); |
| auto is_nb = ncmax > dcmax; |
| |
| std::cerr << " ## " << is_nb << " o nb " << normal_burst_start << " db " << dummy_burst_start |
| << " vs " << ebp.toa << std::endl; |
| |
| if (is_nb) |
| detect_burst(ss, &channel_imp_resp[0], normal_burst_start, outbin); |
| else |
| detect_burst(ss, &channel_imp_resp2[0], dummy_burst_start, outbin); |
| ; |
| } |
| |
| bits = new SoftVector(); |
| bits->resize(148); |
| for (int i = 0; i < 148; i++) |
| (*bits)[i] = outbin[i] < 1 ? -1 : 1; |
| |
| // printme = ret >= 0 ? true : false; |
| |
| // if(printme) { |
| // std::cerr << std::endl << "vita:" << std::endl; |
| // for(auto i : outbin) |
| // std::cerr << (int) i; |
| // std::cerr << std::endl << "org:" << std::endl; |
| // } |
| } else { |
| /* Ignore noise threshold on MS mode for now */ |
| //if ((type == SCH) || (avg - state->mNoiseLev > 0.0)) |
| bits = demodAnyBurst(*burst, type, rx_sps, &ebp); |
| |
| // if(printme) |
| // for(int i=0; i < 148; i++) |
| // std::cerr << (int) (bits->operator[](i) > 0 ? 1 : 0); |
| } |
| |
| /* MS: Decode SCH and adjust GSM clock */ |
| if ((type != TSC) && |
| ((state->mode == TRX_MODE_MS_ACQUIRE) || |
| (state->mode == TRX_MODE_MS_TRACK))) { |
| correctFCCH(state, state->prevFrame[burst_time.TN()]->getVector()); |
| |
| if (decodeSCH(bits, &sch_time)) { |
| if (state->mode == TRX_MODE_MS_ACQUIRE) { |
| diff_time = GSM::Time(sch_time.FN() - burst_time.FN(), |
| -burst_time.TN()); |
| mRadioInterface->adjustClock(diff_time); |
| mTransmitDeadlineClock = RadioClock::adjust( |
| mTransmitDeadlineClock, |
| diff_time); |
| state->mode = TRX_MODE_MS_TRACK; |
| |
| burst_time = sch_time; //sch bits passed up might have a different time, so upper layers complain |
| std::clog << "SCH : Locking GSM clock " << std::endl; |
| } else { |
| //std::clog << "SCH : Read SCH at FN " << burst_time.FN() << " FN51 " << burst_time.FN() % 51 << std::endl; |
| wTime = burst_time; |
| RSSI = (int) floor(20.0 * log10(rxFullScale / avg)); |
| timingOffset = (int) round(ebp.toa * 256.0 / rx_sps); |
| return bits; |
| } |
| } |
| else |
| std::clog << "SCH : FAIL!!!!! SCH at FN " << burst_time.FN() << std::endl; |
| |
| goto release; |
| } |
| |
| wTime = burst_time; |
| RSSI = (int) floor(20.0 * log10(rxFullScale / avg)); |
| timingOffset = (int) round(ebp.toa * 256.0 / rx_sps); |
| |
| delete state->prevFrame[burst_time.TN()]; |
| state->prevFrame[burst_time.TN()] = radio_burst; |
| |
| return bits; |
| |
| release: |
| delete state->prevFrame[burst_time.TN()]; |
| state->prevFrame[burst_time.TN()] = radio_burst; |
| delete bits; |
| return NULL; |
| } |
| |
| void Transceiver2::start() |
| { |
| TransceiverChannel *chan; |
| |
| for (size_t i = 0; i < mControlServiceLoopThreads.size(); i++) { |
| chan = new TransceiverChannel(this, i); |
| mControlServiceLoopThreads[i]->start((void * (*)(void*)) |
| ControlServiceLoopAdapter, (void*) chan); |
| } |
| } |
| |
| void Transceiver2::stop() |
| { |
| |
| if (!mOn) |
| return; |
| |
| LOG(NOTICE) << "Stopping the transceiver"; |
| mLowerLoopThread->cancel(); |
| mLowerLoopThread->join(); |
| delete mLowerLoopThread; |
| |
| |
| for (size_t i = 0; i < mChans; i++) { |
| mRxServiceLoopThreads[i]->cancel(); |
| mTxPriorityQueueServiceLoopThreads[i]->cancel(); |
| } |
| |
| LOG(INFO) << "Stopping the device"; |
| mRadioInterface->stop(); |
| |
| for (size_t i = 0; i < mChans; i++) { |
| mRxServiceLoopThreads[i]->join(); |
| mTxPriorityQueueServiceLoopThreads[i]->join(); |
| delete mRxServiceLoopThreads[i]; |
| delete mTxPriorityQueueServiceLoopThreads[i]; |
| |
| mTxPriorityQueues[i].clear(); |
| } |
| |
| mOn = false; |
| LOG(NOTICE) << "Transceiver stopped"; |
| } |
| |
| |
| void Transceiver2::reset() |
| { |
| for (size_t i = 0; i < mTxPriorityQueues.size(); i++) |
| mTxPriorityQueues[i].clear(); |
| } |
| |
| |
| void Transceiver2::driveControl(size_t chan) |
| { |
| //char response[MAX_PACKET_LENGTH]; |
| |
| // check control socket |
| // char buffer[MAX_PACKET_LENGTH]; |
| // int msgLen = -1; |
| // buffer[0] = '\0'; |
| |
| |
| //// msgLen = mCtrlSockets[chan]->read(buffer); |
| |
| // if (msgLen < 1) { |
| // return; |
| // } |
| |
| auto m = pop_c(); |
| if(!m) |
| return; |
| |
| auto response = (TRX_C*)malloc(sizeof(TRX_C)); |
| response->cmd[0] = '\0'; |
| commandhandler(m->cmd, response->cmd, chan); |
| free(m); |
| std::clog << "response is " << response->cmd << std::endl; |
| push_c(response); |
| //mCtrlSockets[chan]->write(response, strlen(response) + 1); |
| } |
| |
| void Transceiver2::commandhandler(char* buffer, char* response, int chan) |
| { |
| int MAX_PACKET_LENGTH = TRXC_BUF_SIZE; |
| |
| char cmdcheck[4]; |
| char command[MAX_PACKET_LENGTH]; |
| |
| |
| sscanf(buffer,"%3s %s",cmdcheck,command); |
| |
| if (!chan) |
| writeClockInterface(); |
| |
| if (strcmp(cmdcheck,"CMD")!=0) { |
| LOG(WARNING) << "bogus message on control interface"; |
| return; |
| } |
| std::clog << "command is " << buffer << std::endl << std::flush; |
| |
| if (strcmp(command,"MEASURE")==0) { |
| msleep(100); |
| int freq; |
| sscanf(buffer,"%3s %s %d",cmdcheck,command,&freq); |
| sprintf(response,"RSP MEASURE 0 %d -80",freq); |
| } |
| else if (strcmp(command,"ECHO")==0) { |
| msleep(100); |
| sprintf(response,"RSP ECHO 0"); |
| } |
| else if (strcmp(command,"POWEROFF")==0) { |
| // turn off transmitter/demod |
| sprintf(response,"RSP POWEROFF 0"); |
| } |
| else if (strcmp(command,"POWERON")==0) { |
| // turn on transmitter/demod |
| if (!mTxFreq || !mRxFreq) |
| sprintf(response,"RSP POWERON 1"); |
| else { |
| sprintf(response,"RSP POWERON 0"); |
| if (!chan && !mOn) { |
| // Prepare for thread start |
| mPower = -20; |
| mRadioInterface->start(); |
| |
| // Start radio interface threads. |
| mLowerLoopThread->start((void * (*)(void*)) |
| LowerLoopAdapter,(void*) this); |
| |
| for (size_t i = 0; i < mChans; i++) { |
| TransceiverChannel *chan = new TransceiverChannel(this, i); |
| mRxServiceLoopThreads[i]->start((void * (*)(void*)) |
| RxUpperLoopAdapter, (void*) chan); |
| |
| // chan = new TransceiverChannel(this, i); |
| // mTxPriorityQueueServiceLoopThreads[i]->start((void * (*)(void*)) |
| // TxUpperLoopAdapter, (void*) chan); |
| } |
| |
| writeClockInterface(); |
| mOn = true; |
| } |
| } |
| } |
| else if (strcmp(command,"SETMAXDLY")==0) { |
| //set expected maximum time-of-arrival |
| int maxDelay; |
| sscanf(buffer,"%3s %s %d",cmdcheck,command,&maxDelay); |
| mMaxExpectedDelay = maxDelay; // 1 GSM symbol is approx. 1 km |
| sprintf(response,"RSP SETMAXDLY 0 %d",maxDelay); |
| } |
| else if (strcmp(command,"SETRXGAIN")==0) { |
| //set expected maximum time-of-arrival |
| int newGain; |
| sscanf(buffer,"%3s %s %d",cmdcheck,command,&newGain); |
| newGain = mRadioInterface->setRxGain(newGain, chan); |
| sprintf(response,"RSP SETRXGAIN 0 %d",newGain); |
| } |
| else if (strcmp(command,"NOISELEV")==0) { |
| if (mOn) { |
| float lev = 0;//mStates[chan].mNoiseLev; |
| sprintf(response,"RSP NOISELEV 0 %d", |
| (int) round(20.0 * log10(rxFullScale / lev))); |
| } |
| else { |
| sprintf(response,"RSP NOISELEV 1 0"); |
| } |
| } |
| else if (!strcmp(command, "SETPOWER")) { |
| // set output power in dB |
| int dbPwr; |
| sscanf(buffer, "%3s %s %d", cmdcheck, command, &dbPwr); |
| if (!mOn) |
| sprintf(response, "RSP SETPOWER 1 %d", dbPwr); |
| else { |
| mPower = dbPwr; |
| mRadioInterface->setPowerAttenuation(mPower, chan); |
| sprintf(response, "RSP SETPOWER 0 %d", dbPwr); |
| } |
| } |
| else if (!strcmp(command,"ADJPOWER")) { |
| // adjust power in dB steps |
| int dbStep; |
| sscanf(buffer, "%3s %s %d", cmdcheck, command, &dbStep); |
| if (!mOn) |
| sprintf(response, "RSP ADJPOWER 1 %d", mPower); |
| else { |
| mPower += dbStep; |
| mRadioInterface->setPowerAttenuation(mPower, chan); |
| sprintf(response, "RSP ADJPOWER 0 %d", mPower); |
| } |
| } |
| else if (strcmp(command,"RXTUNE")==0) { |
| // tune receiver |
| int freqKhz; |
| sscanf(buffer,"%3s %s %d",cmdcheck,command,&freqKhz); |
| mRxFreq = freqKhz * 1e3; |
| if (!mRadioInterface->tuneRx(mRxFreq, chan)) { |
| LOG(ALERT) << "RX failed to tune"; |
| sprintf(response,"RSP RXTUNE 1 %d",freqKhz); |
| } |
| else |
| sprintf(response,"RSP RXTUNE 0 %d",freqKhz); |
| } |
| else if (strcmp(command,"TXTUNE")==0) { |
| // tune txmtr |
| int freqKhz; |
| sscanf(buffer,"%3s %s %d",cmdcheck,command,&freqKhz); |
| mTxFreq = freqKhz * 1e3; |
| if (!mRadioInterface->tuneTx(mTxFreq, chan)) { |
| LOG(ALERT) << "TX failed to tune"; |
| sprintf(response,"RSP TXTUNE 1 %d",freqKhz); |
| } |
| else |
| sprintf(response,"RSP TXTUNE 0 %d",freqKhz); |
| } |
| else if (!strcmp(command,"SETTSC")) { |
| // set TSC |
| unsigned TSC; |
| sscanf(buffer, "%3s %s %d", cmdcheck, command, &TSC); |
| if (mOn) |
| sprintf(response, "RSP SETTSC 1 %d", TSC); |
| else if (chan && (TSC != mTSC)) |
| sprintf(response, "RSP SETTSC 1 %d", TSC); |
| else { |
| mTSC = TSC; |
| //generateMidamble(rx_sps, TSC); |
| sprintf(response,"RSP SETTSC 0 %d", TSC); |
| } |
| } |
| else if (!strcmp(command,"GETBSIC")) { |
| if (mBSIC < 0) |
| sprintf(response, "RSP GETBSIC 1"); |
| else |
| sprintf(response, "RSP GETBSIC 0 %d", mBSIC); |
| } |
| else if (strcmp(command,"SETSLOT")==0) { |
| // set TSC |
| int corrCode; |
| int timeslot; |
| sscanf(buffer,"%3s %s %d %d",cmdcheck,command,×lot,&corrCode); |
| if ((timeslot < 0) || (timeslot > 7)) { |
| LOG(WARNING) << "bogus message on control interface"; |
| sprintf(response,"RSP SETSLOT 1 %d %d",timeslot,corrCode); |
| return; |
| } |
| mStates[chan].chanType[timeslot] = (ChannelCombination) corrCode; |
| setModulus(timeslot, chan); |
| sprintf(response,"RSP SETSLOT 0 %d %d",timeslot,corrCode); |
| } |
| else if (!strcmp(command,"SETRXMASK")) { |
| int slot; |
| unsigned long long mask; |
| sscanf(buffer,"%3s %s %d 0x%llx", cmdcheck, command, &slot, &mask); |
| if ((slot < 0) || (slot > 7)) { |
| sprintf(response, "RSP SETRXMASK 1"); |
| } else { |
| mRxSlotMask[slot] = mask; |
| sprintf(response, "RSP SETRXMASK 0 %d 0x%llx", slot, mask); |
| } |
| } |
| else if (!strcmp(command, "SYNC")) { |
| msleep(10); |
| mStates[0].mode = TRX_MODE_MS_ACQUIRE; |
| sprintf(response,"RSP SYNC 0"); |
| mMaxExpectedDelay = 10; |
| mRadioInterface->setRxGain(30, 0); |
| msleep(10); |
| } |
| else { |
| LOG(WARNING) << "bogus command " << command << " on control interface."; |
| } |
| |
| //mCtrlSockets[chan]->write(response, strlen(response) + 1); |
| } |
| |
| bool Transceiver2::driveTxPriorityQueue(size_t chan) |
| { |
| auto burst = pop_d(); |
| if(!burst) |
| return true; |
| |
| auto currTime = GSM::Time(burst->fn,burst->ts); |
| int RSSI = (int) burst->txlev; |
| |
| static BitVector newBurst(gSlotLen); |
| BitVector::iterator itr = newBurst.begin(); |
| auto *bufferItr = burst->symbols; |
| while (itr < newBurst.end()) |
| *itr++ = *bufferItr++; |
| |
| addRadioVector(chan, newBurst, RSSI, currTime); |
| free(burst); |
| return true; |
| |
| |
| |
| // char buffer[gSlotLen+50]; |
| |
| // // check data socket |
| // size_t msgLen = mDataSockets[chan]->read(buffer); |
| |
| // if (msgLen!=gSlotLen+1+4+1) { |
| // LOG(ERR) << "badly formatted packet on GSM->TRX interface"; |
| // return false; |
| // } |
| |
| // int timeSlot = (int) buffer[0]; |
| // uint64_t frameNum = 0; |
| // for (int i = 0; i < 4; i++) |
| // frameNum = (frameNum << 8) | (0x0ff & buffer[i+1]); |
| |
| // LOG(DEBUG) << "rcvd. burst at: " << GSM::Time(frameNum,timeSlot); |
| |
| // int RSSI = (int) buffer[5]; |
| // static BitVector newBurst(gSlotLen); |
| // BitVector::iterator itr = newBurst.begin(); |
| // char *bufferItr = buffer+6; |
| // while (itr < newBurst.end()) |
| // *itr++ = *bufferItr++; |
| |
| // GSM::Time currTime = GSM::Time(frameNum,timeSlot); |
| |
| // addRadioVector(chan, newBurst, RSSI, currTime); |
| |
| return true; |
| |
| |
| } |
| |
| void Transceiver2::driveReceiveRadio() |
| { |
| if (!mRadioInterface->driveReceiveRadio()) |
| usleep(100000); |
| } |
| |
| void Transceiver2::driveReceiveFIFO(size_t chan) |
| { |
| SoftVector *rxBurst = NULL; |
| int RSSI; |
| int TOA; // in 1/256 of a symbol |
| GSM::Time burstTime; |
| |
| rxBurst = pullRadioVector(burstTime, RSSI, TOA, chan); |
| |
| if (rxBurst) { |
| auto response = (trxd_from_trx*)malloc(sizeof(trxd_from_trx)); |
| #if 0 |
| if( !gsm_fcch_check_fn(burstTime.FN()) && !gsm_sch_check_fn(burstTime.FN())) |
| std::cerr << "burst parameters: " |
| << " time: " << burstTime |
| << " RSSI: " << RSSI |
| << " TOA: " << TOA |
| << " bits: " << *rxBurst << std::endl; |
| #endif |
| response->ts = burstTime.TN(); |
| response->fn = burstTime.FN(); |
| response->rssi = RSSI; |
| response->toa = TOA; |
| |
| SoftVector::const_iterator burstItr = rxBurst->begin(); |
| if(gsm_sch_check_fn(burstTime.FN())) { |
| for (unsigned int i = 0; i < gSlotLen; i++) |
| ((int8_t*)response->symbols)[i] = round(((*burstItr++)-0.5) * 64.0); |
| } else { |
| // invert and fix to +-127 sbits |
| for (int i = 0; i < 148; i++) |
| ((int8_t*)response->symbols)[i] = *burstItr++ > 0.0f ? -127 : 127; |
| } |
| |
| delete rxBurst; |
| push_d(response); |
| |
| } |
| } |
| |
| void Transceiver2::driveTxFIFO() |
| { |
| |
| /** |
| Features a carefully controlled latency mechanism, to |
| assure that transmit packets arrive at the radio/USRP |
| before they need to be transmitted. |
| |
| Deadline clock indicates the burst that needs to be |
| pushed into the FIFO right NOW. If transmit queue does |
| not have a burst, stick in filler data. |
| */ |
| |
| |
| RadioClock *radioClock = (mRadioInterface->getClock()); |
| |
| if (mOn) { |
| //radioClock->wait(); // wait until clock updates |
| LOG(DEBUG) << "radio clock " << radioClock->get(); |
| while (radioClock->get() + mTransmitLatency > mTransmitDeadlineClock) { |
| // if underrun, then we're not providing bursts to radio/USRP fast |
| // enough. Need to increase latency by one GSM frame. |
| if (mRadioInterface->getWindowType() == RadioDevice::TX_WINDOW_USRP1) { |
| if (mRadioInterface->isUnderrun()) { |
| // only update latency at the defined frame interval |
| if (radioClock->get() > mLatencyUpdateTime + GSM::Time(USB_LATENCY_INTRVL)) { |
| mTransmitLatency = mTransmitLatency + GSM::Time(1,0); |
| LOG(INFO) << "new latency: " << mTransmitLatency; |
| mLatencyUpdateTime = radioClock->get(); |
| } |
| } |
| else { |
| // if underrun hasn't occurred in the last sec (216 frames) drop |
| // transmit latency by a timeslot |
| if (mTransmitLatency > GSM::Time(USB_LATENCY_MIN)) { |
| if (radioClock->get() > mLatencyUpdateTime + GSM::Time(216,0)) { |
| mTransmitLatency.decTN(); |
| //LOG(INFO) << "reduced latency: " << mTransmitLatency; |
| mLatencyUpdateTime = radioClock->get(); |
| } |
| } |
| } |
| } |
| // time to push burst to transmit FIFO |
| pushRadioVector(mTransmitDeadlineClock); |
| |
| mTransmitDeadlineClock.incTN(); |
| |
| if (!mTransmitDeadlineClock.TN() && |
| !(mTransmitDeadlineClock.FN() % CLK_IND_INTERVAL)) { |
| writeClockInterface(); |
| } |
| } |
| } |
| |
| radioClock->wait(); |
| } |
| |
| |
| |
| void Transceiver2::writeClockInterface() |
| { |
| mLastClockUpdateTime = mTransmitDeadlineClock; |
| } |
| |
| void *RxUpperLoopAdapter(TransceiverChannel *chan) |
| { |
| Transceiver2 *trx = chan->trx; |
| size_t num = chan->num; |
| |
| delete chan; |
| |
| // trx->setPriority(0.42); |
| |
| while (1) { |
| trx->driveReceiveFIFO(num); |
| pthread_testcancel(); |
| } |
| return NULL; |
| } |
| |
| void *LowerLoopAdapter(Transceiver2 *transceiver) |
| { |
| // transceiver->setPriority(0.45); |
| |
| while (1) { |
| transceiver->driveReceiveRadio(); |
| //transceiver->driveTxFIFO(); |
| pthread_testcancel(); |
| } |
| return NULL; |
| } |
| |
| void *ControlServiceLoopAdapter(TransceiverChannel *chan) |
| { |
| Transceiver2 *trx = chan->trx; |
| size_t num = chan->num; |
| |
| delete chan; |
| |
| while (1) { |
| trx->driveControl(num); |
| pthread_testcancel(); |
| } |
| return NULL; |
| } |
| |
| void *TxUpperLoopAdapter(TransceiverChannel *chan) |
| { |
| Transceiver2 *trx = chan->trx; |
| size_t num = chan->num; |
| |
| delete chan; |
| |
| // trx->setPriority(0.40); |
| |
| while (1) { |
| bool stale = false; |
| // Flush the UDP packets until a successful transfer. |
| while (!trx->driveTxPriorityQueue(num)) { |
| stale = true; |
| } |
| if (!num && stale) { |
| // If a packet was stale, remind the GSM stack of the clock. |
| trx->writeClockInterface(); |
| } |
| pthread_testcancel(); |
| } |
| return NULL; |
| } |