| /* |
| * SSE Convolution |
| * Copyright (C) 2013 Thomas Tsou <tom@tsou.cc> |
| * |
| * This library is free software; you can redistribute it and/or |
| * modify it under the terms of the GNU Lesser General Public |
| * License as published by the Free Software Foundation; either |
| * version 2.1 of the License, or (at your option) any later version. |
| * |
| * This library 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 |
| * Lesser General Public License for more details. |
| * |
| * You should have received a copy of the GNU Lesser General Public |
| * License along with this library; if not, write to the Free Software |
| * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA |
| */ |
| |
| #include <radioInterface.h> |
| #include <Logger.h> |
| |
| #include "Resampler.h" |
| |
| extern "C" { |
| #include "convert.h" |
| } |
| |
| /* Resampling parameters for 64 MHz clocking */ |
| #define RESAMP_64M_INRATE 20 |
| #define RESAMP_64M_OUTRATE 80 |
| |
| /* Downlink block size */ |
| #define CHUNK 625 |
| |
| /* Universal resampling parameters */ |
| #define NUMCHUNKS 48 |
| |
| /* |
| * Resampling filter bandwidth scaling factor |
| * This narrows the filter cutoff relative to the output bandwidth |
| * of the polyphase resampler. At 4 samples-per-symbol using the |
| * 2 pulse Laurent GMSK approximation gives us below 0.5 degrees |
| * RMS phase error at the resampler output. |
| */ |
| #define RESAMP_TX4_FILTER 0.45 |
| |
| static size_t resamp_inrate = 0; |
| static size_t resamp_inchunk = 0; |
| static size_t resamp_outrate = 0; |
| static size_t resamp_outchunk = 0; |
| |
| RadioInterfaceDiversity::RadioInterfaceDiversity(RadioDevice *wRadio, |
| size_t tx_sps, size_t chans) |
| : RadioInterface(wRadio, tx_sps, 1, chans, 2), outerRecvBuffer(NULL), |
| mDiversity(false), mFreqSpacing(0.0) |
| { |
| } |
| |
| RadioInterfaceDiversity::~RadioInterfaceDiversity() |
| { |
| close(); |
| } |
| |
| void RadioInterfaceDiversity::close() |
| { |
| delete outerRecvBuffer; |
| delete dnsampler; |
| |
| dnsampler = NULL; |
| outerRecvBuffer = NULL; |
| |
| if (recvBuffer.size()) |
| recvBuffer[0] = NULL; |
| |
| RadioInterface::close(); |
| } |
| |
| bool RadioInterfaceDiversity::setupDiversityChannels() |
| { |
| size_t inner_rx_len; |
| |
| /* Inner and outer rates */ |
| resamp_inrate = RESAMP_64M_INRATE; |
| resamp_outrate = RESAMP_64M_OUTRATE; |
| resamp_inchunk = resamp_inrate * 4; |
| resamp_outchunk = resamp_outrate * 4; |
| |
| /* Buffer lengths */ |
| inner_rx_len = NUMCHUNKS * resamp_inchunk; |
| |
| /* Inside buffer must hold at least 2 bursts */ |
| if (inner_rx_len < 157 * mSPSRx * 2) { |
| LOG(ALERT) << "Invalid inner buffer size " << inner_rx_len; |
| return false; |
| } |
| |
| dnsampler = new Resampler(resamp_inrate, resamp_outrate); |
| if (!dnsampler->init()) { |
| LOG(ALERT) << "Rx resampler failed to initialize"; |
| return false; |
| } |
| |
| /* One Receive buffer and downsampler per diversity channel */ |
| for (size_t i = 0; i < mMIMO * mChans; i++) { |
| recvBuffer[i] = new RadioBuffer(NUMCHUNKS, |
| resamp_inchunk, 0, false); |
| } |
| |
| return true; |
| } |
| |
| /* Initialize I/O specific objects */ |
| bool RadioInterfaceDiversity::init(int type) |
| { |
| int outer_rx_len; |
| |
| if ((mMIMO != 2) || (mChans != 2)) { |
| LOG(ALERT) << "Unsupported channel configuration " << mChans; |
| return false; |
| } |
| |
| /* Resize for channel combination */ |
| sendBuffer.resize(mChans); |
| recvBuffer.resize(mChans * mMIMO); |
| convertSendBuffer.resize(mChans); |
| convertRecvBuffer.resize(mChans); |
| mReceiveFIFO.resize(mChans); |
| phases.resize(mChans); |
| |
| if (!setupDiversityChannels()) |
| return false; |
| |
| outer_rx_len = resamp_outchunk; |
| |
| for (size_t i = 0; i < mChans; i++) { |
| /* Full rate float and integer outer receive buffers */ |
| convertRecvBuffer[i] = new short[outer_rx_len * 2]; |
| |
| /* Send buffers (not-resampled) */ |
| sendBuffer[i] = new RadioBuffer(NUMCHUNKS, CHUNK * mSPSTx, 0, true); |
| convertSendBuffer[i] = new short[CHUNK * mSPSTx * 2]; |
| } |
| |
| outerRecvBuffer = new signalVector(outer_rx_len, dnsampler->len()); |
| |
| return true; |
| } |
| |
| bool RadioInterfaceDiversity::tuneRx(double freq, size_t chan) |
| { |
| double f0, f1; |
| |
| if (chan > 1) |
| return false; |
| |
| if (!mRadio->setRxFreq(freq, chan)) |
| return false; |
| |
| f0 = mRadio->getRxFreq(0); |
| f1 = mRadio->getRxFreq(1); |
| |
| mFreqSpacing = f1 - f0; |
| |
| if (abs(mFreqSpacing) <= 600e3) |
| mDiversity = true; |
| else |
| mDiversity = false; |
| |
| return true; |
| } |
| |
| /* Receive a timestamped chunk from the device */ |
| void RadioInterfaceDiversity::pullBuffer() |
| { |
| bool local_underrun; |
| int rc, num, path0, path1; |
| signalVector *shift, *base; |
| float *in, *out, rate = -mFreqSpacing * 2.0 * M_PI / 1.08333333e6; |
| |
| if (recvBuffer[0]->getFreeSegments() <= 0) |
| return; |
| |
| /* Outer buffer access size is fixed */ |
| num = mRadio->readSamples(convertRecvBuffer, |
| resamp_outchunk, |
| &overrun, |
| readTimestamp, |
| &local_underrun); |
| if ((size_t) num != resamp_outchunk) { |
| LOG(ALERT) << "Receive error " << num; |
| return; |
| } |
| |
| for (size_t i = 0; i < mChans; i++) { |
| convert_short_float((float *) outerRecvBuffer->begin(), |
| convertRecvBuffer[i], 2 * resamp_outchunk); |
| |
| if (!i) { |
| path0 = 0; |
| path1 = 2; |
| } else { |
| path0 = 3; |
| path1 = 1; |
| } |
| |
| /* Diversity path 1 */ |
| base = outerRecvBuffer; |
| in = (float *) base->begin(); |
| out = (float *) recvBuffer[path0]->getWriteSegment(); |
| |
| rc = dnsampler->rotate(in, resamp_outchunk, |
| out, resamp_inchunk); |
| if (rc < 0) { |
| LOG(ALERT) << "Sample rate downsampling error"; |
| } |
| |
| /* Enable path 2 if Nyquist bandwidth is sufficient */ |
| if (!mDiversity) |
| continue; |
| |
| /* Diversity path 2 */ |
| shift = new signalVector(base->size(), base->getStart()); |
| in = (float *) shift->begin(); |
| out = (float *) recvBuffer[path1]->getWriteSegment(); |
| |
| rate = i ? -rate : rate; |
| if (!frequencyShift(shift, base, rate, phases[i], &phases[i])) { |
| LOG(ALERT) << "Frequency shift failed"; |
| } |
| |
| rc = dnsampler->rotate(in, resamp_outchunk, |
| out, resamp_inchunk); |
| if (rc < 0) { |
| LOG(ALERT) << "Sample rate downsampling error"; |
| } |
| |
| delete shift; |
| } |
| |
| underrun |= local_underrun; |
| readTimestamp += (TIMESTAMP) resamp_outchunk; |
| } |