commit c58471bb42049adb75fca97d435b06f519ebf58d Author: Steve Markgraf Date: Mon Oct 8 03:59:57 2012 +0200 import kalibrate 0.4.1 taken from: http://thre.at/kalibrate/kal-v0.4.1.tar.bz2 Signed-off-by: Steve Markgraf diff --git a/AUTHORS b/AUTHORS new file mode 100644 index 0000000..e999767 --- /dev/null +++ b/AUTHORS @@ -0,0 +1,3 @@ +Joshua Lackey +Alexander Chemeris + diff --git a/COPYING b/COPYING new file mode 100644 index 0000000..66116eb --- /dev/null +++ b/COPYING @@ -0,0 +1,24 @@ +Copyright (c) 2010, Joshua Lackey +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/ChangeLog b/ChangeLog new file mode 100644 index 0000000..e69de29 diff --git a/Makefile.am b/Makefile.am new file mode 100644 index 0000000..af437a6 --- /dev/null +++ b/Makefile.am @@ -0,0 +1 @@ +SUBDIRS = src diff --git a/NEWS b/NEWS new file mode 100644 index 0000000..e69de29 diff --git a/README b/README new file mode 100644 index 0000000..c4e433e --- /dev/null +++ b/README @@ -0,0 +1,7 @@ +Kalibrate, or kal, can scan for GSM base stations in a given frequency band and +can use those GSM base stations to calculate the local oscillator frequency +offset. + +See http://thre.at/kalibrate + +Copyright (c) 2010, Joshua Lackey (jl@thre.at) diff --git a/bootstrap b/bootstrap new file mode 100755 index 0000000..c292745 --- /dev/null +++ b/bootstrap @@ -0,0 +1,8 @@ +rm -rf config.cache autom4te*.cache +#autoreconf --install + +libtoolize --automake +aclocal +autoconf +autoheader +automake --add-missing diff --git a/configure.ac b/configure.ac new file mode 100644 index 0000000..652cfb2 --- /dev/null +++ b/configure.ac @@ -0,0 +1,43 @@ +AC_PREREQ([2.64]) +AC_INIT([kalibrate], [0.4.1], [jl@thre.at], [kal]) +AC_CONFIG_SRCDIR([src/fcch_detector.cc]) +AM_INIT_AUTOMAKE([-Wall -Werror]) +AC_CONFIG_HEADERS([config.h]) + +# Checks for programs. +AC_PROG_CXX +AC_PROG_CC +AC_PROG_LN_S +AC_PROG_RANLIB + +# Checks for header files. +AC_CHECK_HEADERS([stdlib.h string.h sys/time.h unistd.h]) + +# Checks for typedefs, structures, and compiler characteristics. +AC_HEADER_STDBOOL +AC_C_INLINE + +# Checks for library functions. +AC_FUNC_STRTOD +AC_CHECK_FUNCS([floor getpagesize memset sqrt strtoul strtol]) + +# Checks for libraries. +PKG_CHECK_MODULES(FFTW3, fftw3 >= 3.0) +AC_SUBST(FFTW3_LIBS) +AC_SUBST(FFTW3_CFLAGS) + +PKG_CHECK_MODULES(USRP, usrp >= 3.3) +AC_SUBST(USRP_LIBS) +AC_SUBST(USRP_CFLAGS) + +# OSX doesn't support System V shared memory +AC_CANONICAL_HOST +case "$host_os" in + darwin*) + AC_DEFINE([D_HOST_OSX], [], [building for OSX]) + ;; +esac + +AC_CONFIG_FILES([Makefile + src/Makefile]) +AC_OUTPUT diff --git a/src/Makefile.am b/src/Makefile.am new file mode 100644 index 0000000..a6e5123 --- /dev/null +++ b/src/Makefile.am @@ -0,0 +1,23 @@ +bin_PROGRAMS = kal + +kal_SOURCES = \ + arfcn_freq.cc \ + c0_detect.cc \ + circular_buffer.cc \ + fcch_detector.cc \ + kal.cc \ + offset.cc \ + usrp_source.cc \ + util.cc\ + arfcn_freq.h \ + c0_detect.h \ + circular_buffer.h \ + fcch_detector.h \ + offset.h \ + usrp_complex.h \ + usrp_source.h \ + util.h\ + version.h + +kal_CXXFLAGS = $(FFTW3_CFLAGS) $(USRP_CFLAGS) +kal_LDADD = $(FFTW3_LIBS) $(USRP_LIBS) diff --git a/src/arfcn_freq.cc b/src/arfcn_freq.cc new file mode 100644 index 0000000..d8734fd --- /dev/null +++ b/src/arfcn_freq.cc @@ -0,0 +1,291 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include "arfcn_freq.h" + + +const char *bi_to_str(int bi) { + + switch(bi) { + case GSM_850: + return "GSM-850"; + + case GSM_900: + return "GSM-900"; + + case GSM_E_900: + return "E-GSM-900"; + + case DCS_1800: + return "DCS-1800"; + + case PCS_1900: + return "PCS-1900"; + + default: + return "unknown band indicator"; + } +} + + +int str_to_bi(char *s) { + + if(!strcmp(s, "GSM850") || !strcmp(s, "GSM-850") || !strcmp(s, "850")) + return GSM_850; + + if(!strcmp(s, "GSM900") || !strcmp(s, "GSM-900") || !strcmp(s, "900")) + return GSM_900; + + if(!strcmp(s, "EGSM") || !strcmp(s, "E-GSM") || !strcmp(s, "EGSM900") || + !strcmp(s, "E-GSM900") || !strcmp(s, "E-GSM-900")) + return GSM_E_900; + + if(!strcmp(s, "DCS") || !strcmp(s, "DCS1800") || + !strcmp(s, "DCS-1800") || !strcmp(s, "1800")) + return DCS_1800; + + if(!strcmp(s, "PCS") || !strcmp(s, "PCS1900") || + !strcmp(s, "PCS-1900") || !strcmp(s, "1900")) + return PCS_1900; + + return -1; +} + + +double arfcn_to_freq(int n, int *bi) { + + if((128 <= n) && (n <= 251)) { + if(bi) + *bi = GSM_850; + return 824.2e6 + 0.2e6 * (n - 128) + 45.0e6; + } + + if((1 <= n) && (n <= 124)) { + if(bi) + *bi = GSM_900; + return 890.0e6 + 0.2e6 * n + 45.0e6; + } + + if(n == 0) { + if(bi) + *bi = GSM_E_900; + return 935e6; + } + if((975 <= n) && (n <= 1023)) { + if(bi) + *bi = GSM_E_900; + return 890.0e6 + 0.2e6 * (n - 1024) + 45.0e6; + } + + if((512 <= n) && (n <= 810)) { + if(!bi) { + fprintf(stderr, "error: ambiguous arfcn: %d\n", n); + return -1.0; + } + + if(*bi == DCS_1800) + return 1710.2e6 + 0.2e6 * (n - 512) + 95.0e6; + + if(*bi == PCS_1900) + return 1850.2e6 + 0.2e6 * (n - 512) + 80.0e6; + + fprintf(stderr, "error: bad (arfcn, band indicator) pair: " + "(%d, %s)\n", n, bi_to_str(*bi)); + return -1.0; + } + + if((811 <= n) && (n <= 885)) { + if(bi) + *bi = DCS_1800; + return 1710.2e6 + 0.2e6 * (n - 512) + 95.0e6; + } + + fprintf(stderr, "error: bad arfcn: %d\n", n); + return -1.0; +} + + +int freq_to_arfcn(double freq, int *bi) { + + if((869.2e6 <= freq) && (freq <= 893.8e6)) { + if(bi) + *bi = GSM_850; + return (int)((freq - 869.2e6) / 0.2e6) + 128; + } + + if((935.2e6 <= freq) && (freq <= 959.8e6)) { + if(bi) + *bi = GSM_900; + return (int)((freq - 935e6) / 0.2e6); + } + + if(935.0e6 == freq) { + if(bi) + *bi = GSM_E_900; + return 0; + } + if((925.2e6 <= freq) && (freq <= 934.8e6)) { + if(bi) + *bi = GSM_E_900; + return (int)((freq - 935e6) / 0.2e6) + 1024; + } + + if((1805.2e6 <= freq) && (freq <= 1879.8e6)) { + if(bi) + *bi = DCS_1800; + return (int)((freq - 1805.2e6) / 0.2e6) + 512; + } + + if((1930.2e6 <= freq) && (freq <= 1989.8e6)) { + if(bi) + *bi = PCS_1900; + return (int)((freq - 1930.2e6) / 0.2e6) + 512; + } + + fprintf(stderr, "error: bad frequency: %lf\n", freq); + return -1; +} + + +int first_chan(int bi) { + + switch(bi) { + case GSM_850: + return 128; + + case GSM_900: + return 1; + + case GSM_E_900: + return 0; + + case DCS_1800: + return 512; + + case PCS_1900: + return 512; + + default: + return -1; + } + + return -1; +} + + +int next_chan_loop(int chan, int bi) { + + switch(bi) { + case GSM_850: + if((128 <= chan) && (chan < 251)) + return chan + 1; + if(chan == 251) + return 128; + return -1; + + case GSM_900: + if((1 <= chan) && (chan < 124)) + return chan + 1; + if(chan == 124) + return 1; + return -1; + + case GSM_E_900: + if((0 <= chan) && (chan < 124)) + return chan + 1; + if(chan == 124) + return 975; + if((975 <= chan) && (chan < 1023)) + return chan + 1; + if(chan == 1023) + return 0; + return -1; + + case DCS_1800: + if((512 <= chan) && (chan < 885)) + return chan + 1; + if(chan == 885) + return 512; + return -1; + + case PCS_1900: + if((512 <= chan) && (chan < 810)) + return chan + 1; + if(chan == 810) + return 512; + return -1; + + default: + return -1; + } + + return -1; +} + + +int next_chan(int chan, int bi) { + + switch(bi) { + case GSM_850: + if((128 <= chan) && (chan < 251)) + return chan + 1; + return -1; + + case GSM_900: + if((1 <= chan) && (chan < 124)) + return chan + 1; + return -1; + + case GSM_E_900: + if((0 <= chan) && (chan < 124)) + return chan + 1; + if(chan == 124) + return 975; + if((975 <= chan) && (chan < 1023)) + return chan + 1; + return -1; + + case DCS_1800: + if((512 <= chan) && (chan < 885)) + return chan + 1; + return -1; + + case PCS_1900: + if((512 <= chan) && (chan < 810)) + return chan + 1; + return -1; + + default: + return -1; + } + + return -1; +} diff --git a/src/arfcn_freq.h b/src/arfcn_freq.h new file mode 100644 index 0000000..68c91b4 --- /dev/null +++ b/src/arfcn_freq.h @@ -0,0 +1,42 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +enum { + BI_NOT_DEFINED, + GSM_850, + GSM_900, + GSM_E_900, + DCS_1800, + PCS_1900 +}; + +const char *bi_to_str(int bi); +int str_to_bi(char *s); +double arfcn_to_freq(int n, int *bi = 0); +int freq_to_arfcn(double freq, int *bi = 0); +int first_chan(int bi); +int next_chan(int chan, int bi); diff --git a/src/c0_detect.cc b/src/c0_detect.cc new file mode 100644 index 0000000..55d5590 --- /dev/null +++ b/src/c0_detect.cc @@ -0,0 +1,172 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +#include "usrp_source.h" +#include "circular_buffer.h" +#include "fcch_detector.h" +#include "arfcn_freq.h" +#include "util.h" + +extern int g_verbosity; + +static const float ERROR_DETECT_OFFSET_MAX = 40e3; + +static double vectornorm2(const complex *v, const unsigned int len) { + + unsigned int i; + double e = 0.0; + + for(i = 0; i < len; i++) + e += norm(v[i]); + + return e; +} + + +int c0_detect(usrp_source *u, int bi) { + + static const double GSM_RATE = 1625000.0 / 6.0; + static const unsigned int NOTFOUND_MAX = 10; + + int i, chan_count; + unsigned int overruns, b_len, frames_len, found_count, notfound_count, r; + float offset, spower[BUFSIZ]; + double freq, sps, n, power[BUFSIZ], sum = 0, a; + complex *b; + circular_buffer *ub; + fcch_detector *l = new fcch_detector(u->sample_rate()); + + if(bi == BI_NOT_DEFINED) { + fprintf(stderr, "error: c0_detect: band not defined\n"); + return -1; + } + + sps = u->sample_rate() / GSM_RATE; + frames_len = (unsigned int)ceil((12 * 8 * 156.25 + 156.25) * sps); + ub = u->get_buffer(); + + // first, we calculate the power in each channel + if(g_verbosity > 2) { + fprintf(stderr, "calculate power in each channel:\n"); + } + u->start(); + u->flush(); + for(i = first_chan(bi); i > 0; i = next_chan(i, bi)) { + freq = arfcn_to_freq(i, &bi); + if(!u->tune(freq)) { + fprintf(stderr, "error: usrp_source::tune\n"); + return -1; + } + + do { + u->flush(); + if(u->fill(frames_len, &overruns)) { + fprintf(stderr, "error: usrp_source::fill\n"); + return -1; + } + } while(overruns); + + b = (complex *)ub->peek(&b_len); + n = sqrt(vectornorm2(b, frames_len)); + power[i] = n; + if(g_verbosity > 2) { + fprintf(stderr, "\tchan %d (%.1fMHz):\tpower: %lf\n", + i, freq / 1e6, n); + } + } + + /* + * We want to use the average to determine which channels have + * power, and hence a possibility of being channel 0 on a BTS. + * However, some channels in the band can be extremely noisy. (E.g., + * CDMA traffic in GSM-850.) Hence we won't consider the noisiest + * channels when we construct the average. + */ + chan_count = 0; + for(i = first_chan(bi); i > 0; i = next_chan(i, bi)) { + spower[chan_count++] = power[i]; + } + sort(spower, chan_count); + + // average the lowest %60 + a = avg(spower, chan_count - 4 * chan_count / 10, 0); + + if(g_verbosity > 0) { + fprintf(stderr, "channel detect threshold: %lf\n", a); + } + + // then we look for fcch bursts + printf("%s:\n", bi_to_str(bi)); + found_count = 0; + notfound_count = 0; + sum = 0; + i = first_chan(bi); + do { + if(power[i] <= a) { + i = next_chan(i, bi); + continue; + } + + freq = arfcn_to_freq(i, &bi); + if(!u->tune(freq)) { + fprintf(stderr, "error: usrp_source::tune\n"); + return -1; + } + + do { + u->flush(); + if(u->fill(frames_len, &overruns)) { + fprintf(stderr, "error: usrp_source::fill\n"); + return -1; + } + } while(overruns); + + b = (complex *)ub->peek(&b_len); + r = l->scan(b, b_len, &offset, 0); + if(r && (fabsf(offset - GSM_RATE / 4) < ERROR_DETECT_OFFSET_MAX)) { + // found + printf("\tchan: %d (%.1fMHz ", i, freq / 1e6); + display_freq(offset - GSM_RATE / 4); + printf(")\tpower: %6.2lf\n", power[i]); + notfound_count = 0; + i = next_chan(i, bi); + } else { + // not found + notfound_count += 1; + if(notfound_count >= NOTFOUND_MAX) { + notfound_count = 0; + i = next_chan(i, bi); + } + } + } while(i > 0); + + return 0; +} diff --git a/src/c0_detect.h b/src/c0_detect.h new file mode 100644 index 0000000..9a5c135 --- /dev/null +++ b/src/c0_detect.h @@ -0,0 +1,28 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +int c0_detect(usrp_source *u, int bi); diff --git a/src/circular_buffer.cc b/src/circular_buffer.cc new file mode 100644 index 0000000..cbf813f --- /dev/null +++ b/src/circular_buffer.cc @@ -0,0 +1,487 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#endif /* HAVE_CONFIG_H */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#ifndef D_HOST_OSX +#include +#else +#include +#include +#endif /* !D_HOST_OSX */ + +#include "circular_buffer.h" + + +#ifndef D_HOST_OSX +circular_buffer::circular_buffer(const unsigned int buf_len, + const unsigned int item_size, const unsigned int overwrite) { + + int shm_id_temp, shm_id_guard, shm_id_buf; + void *base; + + if(!buf_len) + throw std::runtime_error("circular_buffer: buffer len is 0"); + + if(!item_size) + throw std::runtime_error("circular_buffer: item size is 0"); + + // calculate buffer size + m_item_size = item_size; + m_buf_size = item_size * buf_len; + + m_pagesize = getpagesize(); + if(m_buf_size % m_pagesize) + m_buf_size = (m_buf_size + m_pagesize) & ~(m_pagesize - 1); + m_buf_len = m_buf_size / item_size; + + // create an address-range that can contain everything + if((shm_id_temp = shmget(IPC_PRIVATE, 2 * m_pagesize + 2 * m_buf_size, + IPC_CREAT | S_IRUSR | S_IWUSR)) == -1) { + perror("shmget"); + throw std::runtime_error("circular_buffer: shmget"); + } + + // create a read-only guard page + if((shm_id_guard = shmget(IPC_PRIVATE, m_pagesize, + IPC_CREAT | S_IRUSR)) == -1) { + shmctl(shm_id_temp, IPC_RMID, 0); + perror("shmget"); + throw std::runtime_error("circular_buffer: shmget"); + } + + // create the data buffer + if((shm_id_buf = shmget(IPC_PRIVATE, m_buf_size, IPC_CREAT | S_IRUSR | + S_IWUSR)) == -1) { + perror("shmget"); + shmctl(shm_id_temp, IPC_RMID, 0); + shmctl(shm_id_guard, IPC_RMID, 0); + throw std::runtime_error("circular_buffer: shmget"); + } + + // attach temporary memory to get an address-range + if((base = shmat(shm_id_temp, 0, 0)) == (void *)(-1)) { + perror("shmat"); + shmctl(shm_id_temp, IPC_RMID, 0); + shmctl(shm_id_guard, IPC_RMID, 0); + shmctl(shm_id_buf, IPC_RMID, 0); + throw std::runtime_error("circular_buffer: shmat"); + } + + // remove the temporary memory id + shmctl(shm_id_temp, IPC_RMID, 0); + + // detach and free the temporary memory + shmdt(base); + + // race condition here + + // map first copy of guard page with previous address + if(shmat(shm_id_guard, base, SHM_RDONLY) == (void *)(-1)) { + perror("shmat"); + shmctl(shm_id_guard, IPC_RMID, 0); + shmctl(shm_id_buf, IPC_RMID, 0); + throw std::runtime_error("circular_buffer: shmat"); + } + + // map first copy of the buffer + if(shmat(shm_id_buf, (char *)base + m_pagesize, 0) == (void *)(-1)) { + perror("shmat"); + shmctl(shm_id_guard, IPC_RMID, 0); + shmctl(shm_id_buf, IPC_RMID, 0); + shmdt(base); + throw std::runtime_error("circular_buffer: shmat"); + } + + // map second copy of the buffer + if(shmat(shm_id_buf, (char *)base + m_pagesize + m_buf_size, 0) == + (void *)(-1)) { + perror("shmat"); + shmctl(shm_id_guard, IPC_RMID, 0); + shmctl(shm_id_buf, IPC_RMID, 0); + shmdt((char *)base + m_pagesize); + shmdt(base); + throw std::runtime_error("circular_buffer: shmat"); + } + + // map second copy of guard page + if(shmat(shm_id_guard, (char *)base + m_pagesize + 2 * m_buf_size, + SHM_RDONLY) == (void *)(-1)) { + perror("shmat"); + shmctl(shm_id_guard, IPC_RMID, 0); + shmctl(shm_id_buf, IPC_RMID, 0); + shmdt((char *)base + m_pagesize + m_buf_size); + shmdt((char *)base + m_pagesize); + shmdt((char *)base); + throw std::runtime_error("circular_buffer: shmat"); + } + + // remove the id for the guard and buffer, we don't need them anymore + shmctl(shm_id_guard, IPC_RMID, 0); + shmctl(shm_id_buf, IPC_RMID, 0); + + // save the base address for detach later + m_base = base; + + // save a pointer to the data + m_buf = (char *)base + m_pagesize; + + m_r = m_w = 0; + m_read = m_written = 0; + + m_item_size = item_size; + + m_overwrite = overwrite; + + pthread_mutex_init(&m_mutex, 0); +} + + +circular_buffer::~circular_buffer() { + + shmdt((char *)m_base + m_pagesize + 2 * m_buf_size); + shmdt((char *)m_base + m_pagesize + m_buf_size); + shmdt((char *)m_base + m_pagesize); + shmdt((char *)m_base); +} +#else /* !D_HOST_OSX */ + + +/* + * OSX doesn't support System V shared memory. Using GNU Radio as an example, + * we'll implement this for OSX using Posix shared memory. I'm not exactly + * sure why GNU Radio prefers the System V usage, but I seem to recall there + * was a reason. + */ +circular_buffer::circular_buffer(const unsigned int buf_len, + const unsigned int item_size, const unsigned int overwrite) { + + int shm_fd; + char shm_name[255]; // XXX should be NAME_MAX + void *base; + + if(!buf_len) + throw std::runtime_error("circular_buffer: buffer len is 0"); + + if(!item_size) + throw std::runtime_error("circular_buffer: item size is 0"); + + // calculate buffer size + m_item_size = item_size; + m_buf_size = item_size * buf_len; + + m_pagesize = getpagesize(); + if(m_buf_size % m_pagesize) + m_buf_size = (m_buf_size + m_pagesize) & ~(m_pagesize - 1); + m_buf_len = m_buf_size / item_size; + + // create unique-ish name + snprintf(shm_name, sizeof(shm_name), "/kalibrate-%d", getpid()); + + // create a Posix shared memory object + if((shm_fd = shm_open(shm_name, O_RDWR | O_CREAT | O_EXCL, S_IRUSR | S_IWUSR)) == -1) { + perror("shm_open"); + throw std::runtime_error("circular_buffer: shm_open"); + } + + // create enough space to hold everything + if(ftruncate(shm_fd, 2 * m_pagesize + 2 * m_buf_size) == -1) { + perror("ftruncate"); + close(shm_fd); + shm_unlink(shm_name); + throw std::runtime_error("circular_buffer: ftruncate"); + } + + // get an address for the buffer + if((base = mmap(0, 2 * m_pagesize + 2 * m_buf_size, PROT_NONE, MAP_SHARED, shm_fd, 0)) == MAP_FAILED) { + perror("mmap"); + close(shm_fd); + shm_unlink(shm_name); + throw std::runtime_error("circular_buffer: mmap (base)"); + } + + // unmap everything but the first guard page + if(munmap((char *)base + m_pagesize, m_pagesize + 2 * m_buf_size) == -1) { + perror("munmap"); + close(shm_fd); + shm_unlink(shm_name); + throw std::runtime_error("circular_buffer: munmap"); + } + + // race condition + + // map first copy of the buffer + if(mmap((char *)base + m_pagesize, m_buf_size, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_FIXED, shm_fd, m_pagesize) == MAP_FAILED) { + perror("mmap"); + munmap(base, 2 * m_pagesize + 2 * m_buf_size); + close(shm_fd); + shm_unlink(shm_name); + throw std::runtime_error("circular_buffer: mmap (buf 1)"); + } + + // map second copy of the buffer + if(mmap((char *)base + m_pagesize + m_buf_size, m_buf_size, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_FIXED, shm_fd, m_pagesize) == MAP_FAILED) { + perror("mmap"); + munmap(base, 2 * m_pagesize + 2 * m_buf_size); + close(shm_fd); + shm_unlink(shm_name); + throw std::runtime_error("circular_buffer: mmap (buf 2)"); + } + + // map second copy of the guard page + if(mmap((char *)base + m_pagesize + 2 * m_buf_size, m_pagesize, PROT_NONE, MAP_SHARED | MAP_FIXED, shm_fd, 0) == MAP_FAILED) { + perror("mmap"); + munmap(base, 2 * m_pagesize + 2 * m_buf_size); + close(shm_fd); + shm_unlink(shm_name); + throw std::runtime_error("circular_buffer: mmap (guard)"); + } + + // both the file and name are unnecessary now + close(shm_fd); + shm_unlink(shm_name); + + // save the base address for unmap later + m_base = base; + + // save a pointer to the data + m_buf = (char *)base + m_pagesize; + + m_r = m_w = 0; + m_read = m_written = 0; + + m_item_size = item_size; + + m_overwrite = overwrite; + + pthread_mutex_init(&m_mutex, 0); +} + + +circular_buffer::~circular_buffer() { + + munmap(m_base, 2 * m_pagesize + 2 * m_buf_size); +} +#endif /* !D_HOST_OSX */ + + +/* + * The amount to read can only grow unless someone calls read after this is + * called. No real good way to tie the two together. + */ +unsigned int circular_buffer::data_available() { + + unsigned int amt; + + pthread_mutex_lock(&m_mutex); + amt = m_written - m_read; // item_size + pthread_mutex_unlock(&m_mutex); + + return amt; +} + + +unsigned int circular_buffer::space_available() { + + unsigned int amt; + + pthread_mutex_lock(&m_mutex); + amt = m_buf_len - (m_written - m_read); + pthread_mutex_unlock(&m_mutex); + + return amt; +} + + +#ifndef MIN +#define MIN(a, b) ((a)<(b)?(a):(b)) +#endif /* !MIN */ + +/* + * m_buf_size is in terms of bytes + * m_r and m_w are offsets in bytes + * m_buf_len is in terms of m_item_size + * buf_len is in terms of m_item_size + * len, m_written, and m_read are all in terms of m_item_size + */ +unsigned int circular_buffer::read(void *buf, const unsigned int buf_len) { + + unsigned int len; + + pthread_mutex_lock(&m_mutex); + len = MIN(buf_len, m_written - m_read); + memcpy(buf, (char *)m_buf + m_r, len * m_item_size); + m_read += len; + if(m_read == m_written) { + m_r = m_w = 0; + m_read = m_written = 0; + } else + m_r = (m_r + len * m_item_size) % m_buf_size; + pthread_mutex_unlock(&m_mutex); + + return len; +} + + +/* + * warning: + * + * Don't use read() while you are peek()'ing. write() should be + * okay unless you have an overwrite buffer. + */ +void *circular_buffer::peek(unsigned int *buf_len) { + + unsigned int len; + void *p; + + pthread_mutex_lock(&m_mutex); + len = m_written - m_read; + p = (char *)m_buf + m_r; + pthread_mutex_unlock(&m_mutex); + + if(buf_len) + *buf_len = len; + + return p; +} + + +void *circular_buffer::poke(unsigned int *buf_len) { + + unsigned int len; + void *p; + + pthread_mutex_lock(&m_mutex); + len = m_buf_len - (m_written - m_read); + p = (char *)m_buf + m_w; + pthread_mutex_unlock(&m_mutex); + + if(buf_len) + *buf_len = len; + + return p; +} + + +unsigned int circular_buffer::purge(const unsigned int buf_len) { + + unsigned int len; + + pthread_mutex_lock(&m_mutex); + len = MIN(buf_len, m_written - m_read); + m_read += len; + if(m_read == m_written) { + m_r = m_w = 0; + m_read = m_written = 0; + } else + m_r = (m_r + len * m_item_size) % m_buf_size; + pthread_mutex_unlock(&m_mutex); + + return len; +} + + +unsigned int circular_buffer::write(const void *buf, + const unsigned int buf_len) { + + unsigned int len, buf_off = 0; + + pthread_mutex_lock(&m_mutex); + if(m_overwrite) { + if(buf_len > m_buf_len) { + buf_off = buf_len - m_buf_len; + len = m_buf_len; + } else + len = buf_len; + } else + len = MIN(buf_len, m_buf_len - (m_written - m_read)); + memcpy((char *)m_buf + m_w, (char *)buf + buf_off * m_item_size, + len * m_item_size); + m_written += len; + m_w = (m_w + len * m_item_size) % m_buf_size; + if(m_written > m_buf_len + m_read) { + m_read = m_written - m_buf_len; + m_r = m_w; + } + pthread_mutex_unlock(&m_mutex); + + return len; +} + + +void circular_buffer::wrote(unsigned int len) { + + pthread_mutex_lock(&m_mutex); + m_written += len; + m_w = (m_w + len * m_item_size) % m_buf_size; + pthread_mutex_unlock(&m_mutex); +} + + +void circular_buffer::flush() { + + pthread_mutex_lock(&m_mutex); + m_read = m_written = 0; + m_r = m_w = 0; + pthread_mutex_unlock(&m_mutex); +} + + +void circular_buffer::flush_nolock() { + + m_read = m_written = 0; + m_r = m_w = 0; +} + + +void circular_buffer::lock() { + + pthread_mutex_lock(&m_mutex); +} + + +void circular_buffer::unlock() { + + pthread_mutex_unlock(&m_mutex); +} + + +unsigned int circular_buffer::buf_len() { + + return m_buf_len; +} diff --git a/src/circular_buffer.h b/src/circular_buffer.h new file mode 100644 index 0000000..9cd9a5e --- /dev/null +++ b/src/circular_buffer.h @@ -0,0 +1,79 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * circular_buffer + * + * This class is based heavily on the GNU Radio circular buffer. While this + * class was written from scratch and contains ideas not present in the GNU + * Radio implementation, the GNU Radio circular buffers were used as a + * reference while developing this class. + * + * This is more a warning that the above BSD-style license may not be the only + * copyright that applies. + */ + +#pragma once + +/* + * XXX If read doesn't catch up with write before 2**64 bytes are written, this + * will break. + */ + +#include + +class circular_buffer { +public: + circular_buffer(const unsigned int buf_len, const unsigned int item_size = 1, const unsigned int overwrite = 0); + ~circular_buffer(); + + unsigned int read(void *buf, const unsigned int buf_len); + void *peek(unsigned int *buf_len); + unsigned int purge(const unsigned int buf_len); + void *poke(unsigned int *buf_len); + void wrote(unsigned int len); + unsigned int write(const void *buf, const unsigned int buf_len); + unsigned int data_available(); + unsigned int space_available(); + void flush(); + void flush_nolock(); + void lock(); + void unlock(); + unsigned int buf_len(); + +private: + void *m_buf; + unsigned int m_buf_len, m_buf_size, m_r, m_w, m_item_size; + unsigned long long m_read, m_written; + + unsigned int m_overwrite; + + void *m_base; + unsigned int m_pagesize; + + pthread_mutex_t m_mutex; +}; diff --git a/src/fcch_detector.cc b/src/fcch_detector.cc new file mode 100644 index 0000000..4c348cf --- /dev/null +++ b/src/fcch_detector.cc @@ -0,0 +1,526 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * This is based on the algorithm found in the paper, + * + * Varma, G. Narendra, Usha Sahu, and G. Prabhu Charan. "Robust + * Frequency Burst Detection Algorithm for GSM / GPRS." + * + * The algorithm uses an adaptive filter to calculate the error difference from + * a pure tone. When the error goes low, the tone is detected. When it goes + * back high, the scan function returns and indicates the number of samples the + * error was low. + * + * The following code is an original work and the above BSD-license should + * apply. However, the algorithm itself may be patented and any use of this + * code should take that into consideration. + */ + +#include // for debug +#include + +#include +#include +#include "fcch_detector.h" + +extern int g_debug; + +static const char * const fftw_plan_name = ".kal_fftw_plan"; + + +fcch_detector::fcch_detector(const float sample_rate, const unsigned int D, + const float p, const float G) { + + FILE *plan_fp; + char plan_name[BUFSIZ]; + const char *home; + + m_D = D; + m_p = p; + m_G = G; + m_e = 0.0; + + m_sample_rate = sample_rate; + m_fcch_burst_len = + (unsigned int)(148.0 * (m_sample_rate / GSM_RATE)); + + m_filter_delay = 8; + m_w_len = 2 * m_filter_delay + 1; + m_w = new complex[m_w_len]; + memset(m_w, 0, sizeof(complex) * m_w_len); + + m_x_cb = new circular_buffer(1024, sizeof(complex), 0); + m_y_cb = new circular_buffer(1024, sizeof(complex), 1); + m_e_cb = new circular_buffer(1000000, sizeof(float), 0); + + m_in = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * FFT_SIZE); + m_out = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * FFT_SIZE); + if((!m_in) || (!m_out)) + throw std::runtime_error("fcch_detector: fftw_malloc failed!"); + + home = getenv("HOME"); + if(strlen(home) + strlen(fftw_plan_name) + 2 < sizeof(plan_name)) { + strcpy(plan_name, home); + strcat(plan_name, "/"); + strcat(plan_name, fftw_plan_name); + if((plan_fp = fopen(plan_name, "r"))) { + fftw_import_wisdom_from_file(plan_fp); + fclose(plan_fp); + } + m_plan = fftw_plan_dft_1d(FFT_SIZE, m_in, m_out, FFTW_FORWARD, + FFTW_MEASURE); + if((plan_fp = fopen(plan_name, "w"))) { + fftw_export_wisdom_to_file(plan_fp); + fclose(plan_fp); + } + } else + m_plan = fftw_plan_dft_1d(FFT_SIZE, m_in, m_out, FFTW_FORWARD, + FFTW_ESTIMATE); + if(!m_plan) + throw std::runtime_error("fcch_detector: fftw plan failed!"); +} + + +fcch_detector::~fcch_detector() { + + if(m_w) { + delete[] m_w; + m_w = 0; + } + if(m_x_cb) { + delete m_x_cb; + m_x_cb = 0; + } + if(m_y_cb) { + delete m_y_cb; + m_y_cb = 0; + } + if(m_e_cb) { + delete m_e_cb; + m_e_cb = 0; + } +} + + +enum { + LOW = 0, + HIGH = 1 +}; + +static unsigned int g_count = 0, + g_block_s = HIGH; + + +static inline void low_to_high_init() { + + g_count = 0; + g_block_s = HIGH; +} + + +static inline unsigned int low_to_high(float e, float a) { + + unsigned int r = 0; + + if(e > a) { + if(g_block_s == LOW) { + r = g_count; + g_block_s = HIGH; + g_count = 0; + } + g_count += 1; + } else { + if(g_block_s == HIGH) { + g_block_s = LOW; + g_count = 0; + } + g_count += 1; + } + + return r; +} + + +static inline int peak_valley(complex *c, unsigned int c_len, complex peak, unsigned int peak_i, unsigned int width, float *p2m) { + + float valley = 0.0; + unsigned int i, valley_count = 0; + + // these constants aren't the best for all burst types + for(i = 2; i < 2 + width; i++) { + if(i <= peak_i) { + valley += norm(c[peak_i - i]); + valley_count += 1; + } + if(peak_i + i < c_len) { + valley += norm(c[peak_i + i]); + valley_count += 1; + } + } + + if(valley_count < 2) { + fprintf(stderr, "error: bad valley_count\n"); + return -1; + } + valley = sqrtf(valley / (float)valley_count) + 0.00001; + + if(p2m) + *p2m = sqrtf(norm(peak)) / valley; + + return 0; +} + + +static inline float sinc(const float x) { + + if((x <= -0.0001) || (0.0001 <= x)) + return sinf(x) / x; + return 1.0; +} + + +static inline complex interpolate_point(const complex *s, const unsigned int s_len, const float s_i) { + + static const unsigned int filter_len = 21; + + int start, end, i; + unsigned int d; + complex point; + + d = (filter_len - 1) / 2; + start = (int)(floor(s_i) - d); + end = (int)(floor(s_i) + d + 1); + if(start < 0) + start = 0; + if(end > (int)(s_len - 1)) + end = s_len - 1; + for(point = 0.0, i = start; i <= end; i++) + point += s[i] * sinc(M_PI * (i - s_i)); + return point; +} + + +static inline float peak_detect(const complex *s, const unsigned int s_len, complex *peak, float *avg_power) { + + unsigned int i; + float max = -1.0, max_i = -1.0, sample_power, sum_power, early_i, late_i, incr; + complex early_p, late_p, cmax; + + sum_power = 0; + for(i = 0; i < s_len; i++) { + sample_power = norm(s[i]); + sum_power += sample_power; + if(sample_power > max) { + max = sample_power; + max_i = i; + } + } + early_i = (1 <= max_i)? (max_i - 1) : 0; + late_i = (max_i + 1 < s_len)? (max_i + 1) : s_len - 1; + + incr = 0.5; + while(incr > 1.0 / 1024.0) { + early_p = interpolate_point(s, s_len, early_i); + late_p = interpolate_point(s, s_len, late_i); + if(norm(early_p) < norm(late_p)) + early_i += incr; + else if(norm(early_p) > norm(late_p)) + early_i -= incr; + else + break; + incr /= 2.0; + late_i = early_i + 2.0; + } + max_i = early_i + 1.0; + cmax = interpolate_point(s, s_len, max_i); + + if(peak) + *peak = cmax; + + if(avg_power) + *avg_power = (sum_power - norm(cmax)) / (s_len - 1); + + return max_i; +} + + +static inline float itof(float index, float sample_rate, unsigned int fft_size) { + + double r = index * (sample_rate / (double)fft_size); + + /* + if(index > (double)fft_size / 2.0) + return r - sample_rate; + else + return r; + */ + return r; +} + + +static inline unsigned int ftoi(float frequency, float sample_rate, unsigned int fft_size) { + + unsigned int r = (frequency / sample_rate) * fft_size; + + return r; +} + + +#ifndef MIN +#define MIN(a, b) (a)<(b)?(a):(b) +#endif /* !MIN */ + + +float fcch_detector::freq_detect(const complex *s, const unsigned int s_len, float *pm) { + + unsigned int i, len; + float max_i, avg_power; + complex fft[FFT_SIZE], peak; + + len = MIN(s_len, FFT_SIZE); + for(i = 0; i < len; i++) { + m_in[i][0] = s[i].real(); + m_in[i][1] = s[i].imag(); + } + for(i = len; i < FFT_SIZE; i++) { + m_in[i][0] = 0; + m_in[i][1] = 0; + } + + fftw_execute(m_plan); + + for(i = 0; i < FFT_SIZE; i++) { + fft[i].real() = m_out[i][0]; + fft[i].imag() = m_out[i][1]; + } + + max_i = peak_detect(fft, FFT_SIZE, &peak, &avg_power); + if(pm) + *pm = norm(peak) / avg_power; + return itof(max_i, m_sample_rate, FFT_SIZE); +} + + +static inline void display_complex(const complex *s, unsigned int s_len) { + + for(unsigned int i = 0; i < s_len; i++) { + printf("%f\n", s[i].real()); + fprintf(stderr, "%f\n", s[i].imag()); + } +} + + +/* + * scan: + * 1. calculate average error + * 2. find neighborhoods with low error that satisfy minimum length + * 3. for each such neighborhood, take fft and calculate peak/mean + * 4. if peak/mean > 50, then this is a valid finding. + */ +unsigned int fcch_detector::scan(const complex *s, const unsigned int s_len, float *offset, unsigned int *consumed) { + + static const float sps = m_sample_rate / (1625000.0 / 6.0); + static const unsigned int MIN_FB_LEN = 100 * sps; + static const unsigned int MIN_PM = 50; // XXX arbitrary, depends on decimation + + unsigned int len = 0, t, e_count, i, l_count, y_offset, y_len; + float e, *a, loff = 0, pm; + double sum = 0.0, avg, limit; + const complex *y; + + // calculate the error for each sample + while(len < s_len) { + t = m_x_cb->write(s + len, 1); + len += t; + if(!next_norm_error(&e)) { + m_e_cb->write(&e, 1); + sum += e; + } + } + if(consumed) + *consumed = len; + + // calculate average error over entire buffer + a = (float *)m_e_cb->peek(&e_count); + avg = sum / (double)e_count; + limit = 0.7 * avg; + + if(g_debug) { + printf("debug: error limit: %.1lf\n", limit); + } + + // find neighborhoods where the error is smaller than the limit + low_to_high_init(); + for(i = 0; i < e_count; i++) { + l_count = low_to_high(a[i], limit); + + // see if p/m indicates a pure tone + pm = 0; + if(l_count >= MIN_FB_LEN) { + y_offset = i - l_count; + y_len = (l_count < m_fcch_burst_len)? l_count : m_fcch_burst_len; + y = s + y_offset; + loff = freq_detect(y, y_len, &pm); + if(g_debug) + printf("debug: %.0f\t%f\t%f\n", (double)l_count / sps, pm, loff); + if(pm > MIN_PM) + break; + } + } + // empty buffers for next call + m_e_cb->flush(); + m_x_cb->flush(); + m_y_cb->flush(); + + if(pm <= MIN_PM) + return 0; + + if(offset) + *offset = loff; + + if(g_debug) { + printf("debug: fcch_detector finished -----------------------------\n"); + } + + return 1; +} + + +unsigned int fcch_detector::update(const complex *s, const unsigned int s_len) { + + return m_x_cb->write(s, s_len); +} + + +unsigned int fcch_detector::get_delay() { + + return m_w_len - 1 + m_D; +} + + +unsigned int fcch_detector::filter_len() { + + return m_w_len; +} + + +static float vectornorm2(const complex *v, const unsigned int len) { + + unsigned int i; + float e = 0.0; + + for(i = 0; i < len; i++) + e += norm(v[i]); + + return e; +} + + +/* + * First y value comes out at sample x[n + m_D] = x[w_len - 1 + m_D]. + * + * y[0] = X(x[0], ..., x[w_len - 1 + m_D]) + * + * So y and e are delayed by w_len - 1 + m_D. + */ +int fcch_detector::next_norm_error(float *error) { + + unsigned int i, n, max; + float E; + complex *x, y, e; + + // n is "current" sample + n = m_w_len - 1; + + // ensure there are enough samples in the buffer + x = (complex *)m_x_cb->peek(&max); + if(n + m_D >= max) + return n + m_D - max + 1; + + // update G + E = vectornorm2(x, m_w_len); + if(m_G >= 2.0 / E) + m_G = 1.0 / E; + + // calculate filtered value + y = 0.0; + for(i = 0; i < m_w_len; i++) + y += std::conj(m_w[i]) * x[n - i]; + // m_y_cb->write(&y, 1); + m_y_cb->write(x + n + m_D, 1); // XXX save filtered value? + + // calculate error from desired signal + e = x[n + m_D] - y; + + // update filters with opposite gradient + for(i = 0; i < m_w_len; i++) + m_w[i] += m_G * std::conj(e) * x[n - i]; + + // update error average power + E /= m_w_len; + m_e = (1.0 - m_p) * m_e + m_p * norm(e); + + // return error ratio + if(error) + *error = m_e / E; + + // remove the processed sample from the buffer + m_x_cb->purge(1); + + return 0; +} + + +complex *fcch_detector::dump_x(unsigned int *x_len) { + + return (complex *)m_x_cb->peek(x_len); +} + + +complex *fcch_detector::dump_y(unsigned int *y_len) { + + return (complex *)m_y_cb->peek(y_len); +} + + +unsigned int fcch_detector::y_buf_len() { + + return m_y_cb->buf_len(); +} + + +unsigned int fcch_detector::x_buf_len() { + + return m_x_cb->buf_len(); +} + + +unsigned int fcch_detector::x_purge(unsigned int len) { + + return m_x_cb->purge(len); +} diff --git a/src/fcch_detector.h b/src/fcch_detector.h new file mode 100644 index 0000000..70603c2 --- /dev/null +++ b/src/fcch_detector.h @@ -0,0 +1,88 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * This is based on the algorithm found in the paper, + * + * Varma, G. Narendra, Usha Sahu, and G. Prabhu Charan. "Robust + * Frequency Burst Detection Algorithm for GSM / GPRS." + * + * The algorithm uses an adaptive filter to calculate the error difference from + * a pure tone. When the error goes low, the tone is detected. When it goes + * back high, the scan function returns and indicates the number of samples the + * error was low. + * + * The following code is an original work and the above BSD-license should + * apply. However, the algorithm itself may be patented and any use of this + * code should take that into consideration. + */ + +#include + +#include "circular_buffer.h" +#include "usrp_complex.h" + +class fcch_detector { + +public: + fcch_detector(const float sample_rate, const unsigned int D = 8, const float p = 1.0 / 32.0, const float G = 1.0 / 12.5); + ~fcch_detector(); + unsigned int scan(const complex *s, const unsigned int s_len, float *offset, unsigned int *consumed); + float freq_detect(const complex *s, const unsigned int s_len, float *pm); + unsigned int update(const complex *s, unsigned int s_len); + int next_norm_error(float *error); + complex *dump_x(unsigned int *); + complex *dump_y(unsigned int *); + unsigned int filter_delay() { return m_filter_delay; }; + unsigned int get_delay(); + unsigned int filter_len(); + unsigned int x_buf_len(); + unsigned int y_buf_len(); + unsigned int x_purge(unsigned int); + +private: + static const double GSM_RATE = 1625000.0 / 6.0; + static const unsigned int FFT_SIZE = 1024; + + unsigned int m_w_len, + m_D, + m_check_G, + m_filter_delay, + m_lpf_len, + m_fcch_burst_len; + float m_sample_rate, + m_p, + m_G, + m_e; + complex *m_w; + circular_buffer *m_x_cb, + *m_y_cb, + *m_e_cb; + + fftw_complex *m_in, *m_out; + fftw_plan m_plan; +}; diff --git a/src/kal.cc b/src/kal.cc new file mode 100644 index 0000000..29259f6 --- /dev/null +++ b/src/kal.cc @@ -0,0 +1,278 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +/* + * kal + * + * Two functions: + * + * 1. Calculates the frequency offset between a local GSM tower and the + * USRP clock. + * + * 2. Identifies the frequency of all GSM base stations in a given band. + */ + +#ifdef HAVE_CONFIG_H +#include "config.h" +#else +#define PACKAGE_VERSION "custom build" +#endif /* HAVE_CONFIG_H */ + +#include +#include +#include +#ifdef D_HOST_OSX +#include +#endif /* D_HOST_OSX */ +#include +#include +#include + +#include "usrp_source.h" +#include "fcch_detector.h" +#include "arfcn_freq.h" +#include "offset.h" +#include "c0_detect.h" +#include "version.h" + +static const double GSM_RATE = 1625000.0 / 6.0; + + +int g_verbosity = 0; +int g_debug = 0; + +void usage(char *prog) { + + printf("kalibrate v%s, Copyright (c) 2010, Joshua Lackey\n", kal_version_string); + printf("\nUsage:\n"); + printf("\tGSM Base Station Scan:\n"); + printf("\t\t%s <-s band indicator> [options]\n", basename(prog)); + printf("\n"); + printf("\tClock Offset Calculation:\n"); + printf("\t\t%s <-f frequency | -c channel> [options]\n", basename(prog)); + printf("\n"); + printf("Where options are:\n"); + printf("\t-s\tband to scan (GSM850, GSM900, EGSM, DCS, PCS)\n"); + printf("\t-f\tfrequency of nearby GSM base station\n"); + printf("\t-c\tchannel of nearby GSM base station\n"); + printf("\t-b\tband indicator (GSM850, GSM900, EGSM, DCS, PCS)\n"); + printf("\t-R\tside A (0) or B (1), defaults to B\n"); + printf("\t-A\tantenna TX/RX (0) or RX2 (1), defaults to RX2\n"); + printf("\t-g\tgain as %% of range, defaults to 45%%\n"); + printf("\t-F\tFPGA master clock frequency, defaults to 52MHz\n"); + printf("\t-v\tverbose\n"); + printf("\t-D\tenable debug messages\n"); + printf("\t-h\thelp\n"); + exit(-1); +} + + +int main(int argc, char **argv) { + + char *endptr; + int c, antenna = 1, bi = BI_NOT_DEFINED, chan = -1, bts_scan = 0; + unsigned int subdev = 1, decimation = 192; + long int fpga_master_clock_freq = 52000000; + float gain = 0.45; + double freq = -1.0, fd; + usrp_source *u; + + while((c = getopt(argc, argv, "f:c:s:b:R:A:g:F:vDh?")) != EOF) { + switch(c) { + case 'f': + freq = strtod(optarg, 0); + break; + + case 'c': + chan = strtoul(optarg, 0, 0); + break; + + case 's': + if((bi = str_to_bi(optarg)) == -1) { + fprintf(stderr, "error: bad band " + "indicator: ``%s''\n", optarg); + usage(argv[0]); + } + bts_scan = 1; + break; + + case 'b': + if((bi = str_to_bi(optarg)) == -1) { + fprintf(stderr, "error: bad band " + "indicator: ``%s''\n", optarg); + usage(argv[0]); + } + break; + + case 'R': + errno = 0; + subdev = strtoul(optarg, &endptr, 0); + if((!errno) && (endptr != optarg)) + break; + if(tolower(*optarg) == 'a') { + subdev = 0; + } else if(tolower(*optarg) == 'b') { + subdev = 1; + } else { + fprintf(stderr, "error: bad side: " + "``%s''\n", + optarg); + usage(argv[0]); + } + break; + + case 'A': + if(!strcmp(optarg, "RX2")) { + antenna = 1; + } else if(!strcmp(optarg, "TX/RX")) { + antenna = 0; + } else { + errno = 0; + antenna = strtoul(optarg, &endptr, 0); + if(errno || (endptr == optarg)) { + fprintf(stderr, "error: bad " + "antenna: ``%s''\n", + optarg); + usage(argv[0]); + } + } + break; + + case 'g': + gain = strtod(optarg, 0); + if((gain > 1.0) && (gain <= 100.0)) + gain /= 100.0; + if((gain < 0.0) || (1.0 < gain)) + usage(argv[0]); + break; + + case 'F': + fpga_master_clock_freq = strtol(optarg, 0, 0); + if(!fpga_master_clock_freq) + fpga_master_clock_freq = (long int)strtod(optarg, 0); + + // was answer in MHz? + if(fpga_master_clock_freq < 1000) { + fpga_master_clock_freq *= 1000000; + } + break; + + case 'v': + g_verbosity++; + break; + + case 'D': + g_debug = 1; + break; + + case 'h': + case '?': + default: + usage(argv[0]); + break; + } + + } + + // sanity check frequency / channel + if(bts_scan) { + if(bi == BI_NOT_DEFINED) { + fprintf(stderr, "error: scaning requires band\n"); + usage(argv[0]); + } + } else { + if(freq < 0.0) { + if(chan < 0) { + fprintf(stderr, "error: must enter channel or " + "frequency\n"); + usage(argv[0]); + } + if((freq = arfcn_to_freq(chan, &bi)) < 869e6) + usage(argv[0]); + } + if((freq < 869e6) || (2e9 < freq)) { + fprintf(stderr, "error: bad frequency: %lf\n", freq); + usage(argv[0]); + } + chan = freq_to_arfcn(freq, &bi); + } + + // sanity check clock + if(fpga_master_clock_freq < 48000000) { + fprintf(stderr, "error: FPGA master clock too slow: %li\n", fpga_master_clock_freq); + usage(argv[0]); + } + + // calculate decimation -- get as close to GSM rate as we can + fd = (double)fpga_master_clock_freq / GSM_RATE; + decimation = (unsigned int)fd; + + if(g_debug) { +#ifdef D_HOST_OSX + printf("debug: Mac OS X version\n"); +#endif + printf("debug: FPGA Master Clock Freq:\t%li\n", fpga_master_clock_freq); + printf("debug: decimation :\t%u\n", decimation); + printf("debug: RX Subdev Spec :\t%s\n", subdev? "B" : "A"); + printf("debug: Antenna :\t%s\n", antenna? "RX2" : "TX/RX"); + printf("debug: Gain :\t%f\n", gain); + } + + u = new usrp_source(decimation, fpga_master_clock_freq); + if(!u) { + fprintf(stderr, "error: usrp_source\n"); + return -1; + } + if(u->open(subdev) == -1) { + fprintf(stderr, "error: usrp_source::open\n"); + return -1; + } + u->set_antenna(antenna); + if(!u->set_gain(gain)) { + fprintf(stderr, "error: usrp_source::set_gain\n"); + return -1; + } + + if(!bts_scan) { + if(!u->tune(freq)) { + fprintf(stderr, "error: usrp_source::tune\n"); + return -1; + } + + fprintf(stderr, "%s: Calculating clock frequency offset.\n", + basename(argv[0])); + fprintf(stderr, "Using %s channel %d (%.1fMHz)\n", + bi_to_str(bi), chan, freq / 1e6); + + return offset_detect(u); + } + + fprintf(stderr, "%s: Scanning for %s base stations.\n", + basename(argv[0]), bi_to_str(bi)); + + return c0_detect(u, bi); +} diff --git a/src/offset.cc b/src/offset.cc new file mode 100644 index 0000000..d091974 --- /dev/null +++ b/src/offset.cc @@ -0,0 +1,122 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "usrp_source.h" +#include "fcch_detector.h" +#include "util.h" + + +static const unsigned int AVG_COUNT = 100; +static const unsigned int AVG_THRESHOLD = (AVG_COUNT / 10); +static const float OFFSET_MAX = 40e3; + +extern int g_verbosity; + + +int offset_detect(usrp_source *u) { + + static const double GSM_RATE = 1625000.0 / 6.0; + + unsigned int new_overruns = 0, overruns = 0; + int notfound = 0; + unsigned int s_len, b_len, consumed, count; + float offset = 0.0, min = 0.0, max = 0.0, avg_offset = 0.0, + stddev = 0.0, sps, offsets[AVG_COUNT]; + complex *cbuf; + fcch_detector *l; + circular_buffer *cb; + + l = new fcch_detector(u->sample_rate()); + + /* + * We deliberately grab 12 frames and 1 burst. We are guaranteed to + * find at least one FCCH burst in this much data. + */ + sps = u->sample_rate() / GSM_RATE; + s_len = (unsigned int)ceil((12 * 8 * 156.25 + 156.25) * sps); + cb = u->get_buffer(); + + u->start(); + u->flush(); + count = 0; + while(count < AVG_COUNT) { + + // ensure at least s_len contiguous samples are read from usrp + do { + if(u->fill(s_len, &new_overruns)) { + return -1; + } + if(new_overruns) { + overruns += new_overruns; + u->flush(); + } + } while(new_overruns); + + // get a pointer to the next samples + cbuf = (complex *)cb->peek(&b_len); + + // search the buffer for a pure tone + if(l->scan(cbuf, b_len, &offset, &consumed)) { + + // FCH is a sine wave at GSM_RATE / 4 + offset = offset - GSM_RATE / 4; + + // sanity check offset + if(fabs(offset) < OFFSET_MAX) { + + offsets[count] = offset; + count += 1; + + if(g_verbosity > 0) { + fprintf(stderr, "\toffset %3u: %.2f\n", count, offset); + } + } + } else { + ++notfound; + } + + // consume used samples + cb->purge(consumed); + } + + u->stop(); + delete l; + + // construct stats + sort(offsets, AVG_COUNT); + avg_offset = avg(offsets + AVG_THRESHOLD, AVG_COUNT - 2 * AVG_THRESHOLD, &stddev); + min = offsets[AVG_THRESHOLD]; + max = offsets[AVG_COUNT - AVG_THRESHOLD - 1]; + + printf("average\t\t[min, max]\t(range, stddev)\n"); + display_freq(avg_offset); + printf("\t\t[%d, %d]\t(%d, %f)\n", (int)round(min), (int)round(max), (int)round(max - min), stddev); + printf("overruns: %u\n", overruns); + printf("not found: %u\n", notfound); + + return 0; +} diff --git a/src/offset.h b/src/offset.h new file mode 100644 index 0000000..8fbca71 --- /dev/null +++ b/src/offset.h @@ -0,0 +1,28 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +int offset_detect(usrp_source *u); diff --git a/src/usrp_complex.h b/src/usrp_complex.h new file mode 100644 index 0000000..eba39f4 --- /dev/null +++ b/src/usrp_complex.h @@ -0,0 +1,31 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +typedef std::complex complex; + diff --git a/src/usrp_source.cc b/src/usrp_source.cc new file mode 100644 index 0000000..2311215 --- /dev/null +++ b/src/usrp_source.cc @@ -0,0 +1,297 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "usrp_source.h" + +extern int g_verbosity; + + +usrp_source::usrp_source(float sample_rate, long int fpga_master_clock_freq) { + + m_fpga_master_clock_freq = fpga_master_clock_freq; + m_desired_sample_rate = sample_rate; + m_sample_rate = 0.0; + m_decimation = 0; + m_u_rx.reset(); + m_db_rx.reset(); + m_cb = new circular_buffer(CB_LEN, sizeof(complex), 0); + + pthread_mutex_init(&m_u_mutex, 0); +} + + +usrp_source::usrp_source(unsigned int decimation, long int fpga_master_clock_freq) { + + m_fpga_master_clock_freq = fpga_master_clock_freq; + m_sample_rate = 0.0; + m_u_rx.reset(); + m_db_rx.reset(); + m_cb = new circular_buffer(CB_LEN, sizeof(complex), 0); + + pthread_mutex_init(&m_u_mutex, 0); + + m_decimation = decimation & ~1; + if(m_decimation < 4) + m_decimation = 4; + if(m_decimation > 256) + m_decimation = 256; +} + + +usrp_source::~usrp_source() { + + stop(); + delete m_cb; + pthread_mutex_destroy(&m_u_mutex); +} + + +void usrp_source::stop() { + + pthread_mutex_lock(&m_u_mutex); + if(m_db_rx) + m_db_rx->set_enable(0); + if(m_u_rx) + m_u_rx->stop(); + pthread_mutex_unlock(&m_u_mutex); +} + + +void usrp_source::start() { + + pthread_mutex_lock(&m_u_mutex); + if(m_db_rx) + m_db_rx->set_enable(1); + if(m_u_rx) + m_u_rx->start(); + pthread_mutex_unlock(&m_u_mutex); +} + + +void usrp_source::calculate_decimation() { + + float decimation_f; + + decimation_f = (float)m_u_rx->fpga_master_clock_freq() / m_desired_sample_rate; + m_decimation = (unsigned int)round(decimation_f) & ~1; + + if(m_decimation < 4) + m_decimation = 4; + if(m_decimation > 256) + m_decimation = 256; +} + + +float usrp_source::sample_rate() { + + return m_sample_rate; + +} + + +int usrp_source::tune(double freq) { + + int r; + usrp_tune_result tr; + + pthread_mutex_lock(&m_u_mutex); + r = m_u_rx->tune(0, m_db_rx, freq, &tr); + pthread_mutex_unlock(&m_u_mutex); + + return r; +} + + +bool usrp_source::set_antenna(int antenna) { + + return m_db_rx->select_rx_antenna(antenna); +} + + +bool usrp_source::set_gain(float gain) { + + float min = m_db_rx->gain_min(), max = m_db_rx->gain_max(); + + if((gain < 0.0) || (1.0 < gain)) + return false; + + return m_db_rx->set_gain(min + gain * (max - min)); +} + + +/* + * open() should be called before multiple threads access usrp_source. + */ +int usrp_source::open(unsigned int subdev) { + + int do_set_decim = 0; + usrp_subdev_spec ss(subdev, 0); + + if(!m_u_rx) { + if(!m_decimation) { + do_set_decim = 1; + m_decimation = 4; + } + if(!(m_u_rx = usrp_standard_rx::make(0, m_decimation, + NCHAN, INITIAL_MUX, usrp_standard_rx::FPGA_MODE_NORMAL, + FUSB_BLOCK_SIZE, FUSB_NBLOCKS, FPGA_FILENAME()))) { + fprintf(stderr, "error: usrp_standard_rx::make: " + "failed!\n"); + return -1; + } + m_u_rx->set_fpga_master_clock_freq(m_fpga_master_clock_freq); + m_u_rx->stop(); + + if(do_set_decim) { + calculate_decimation(); + } + + m_u_rx->set_decim_rate(m_decimation); + m_sample_rate = (double)m_u_rx->fpga_master_clock_freq() / m_decimation; + + if(g_verbosity > 1) { + fprintf(stderr, "FPGA clock : %ld\n", m_u_rx->fpga_master_clock_freq()); + fprintf(stderr, "Decimation : %u\n", m_decimation); + fprintf(stderr, "Sample rate: %f\n", m_sample_rate); + } + } + if(!m_u_rx->is_valid(ss)) { + fprintf(stderr, "error: invalid daughterboard\n"); + return -1; + } + if(!(m_db_rx = m_u_rx->selected_subdev(ss))) { + fprintf(stderr, "error: no daughterboard\n"); + return -1; + } + + m_u_rx->set_mux(m_u_rx->determine_rx_mux_value(ss)); + + set_gain(0.45); + m_db_rx->select_rx_antenna(1); // this is a nop for most db + + return 0; +} + + +#define USB_PACKET_SIZE 512 + +int usrp_source::fill(unsigned int num_samples, unsigned int *overrun_i) { + + bool overrun; + unsigned char ubuf[USB_PACKET_SIZE]; + short *s = (short *)ubuf; + unsigned int i, j, space, overruns = 0; + complex *c; + + while((m_cb->data_available() < num_samples) && (m_cb->space_available() > 0)) { + + // read one usb packet from the usrp + pthread_mutex_lock(&m_u_mutex); + if(m_u_rx->read(ubuf, sizeof(ubuf), &overrun) != sizeof(ubuf)) { + pthread_mutex_unlock(&m_u_mutex); + fprintf(stderr, "error: usrp_standard_rx::read\n"); + return -1; + } + pthread_mutex_unlock(&m_u_mutex); + if(overrun) + overruns++; + + // write complex input to complex output + c = (complex *)m_cb->poke(&space); + + // set space to number of complex items to copy + if(space > (USB_PACKET_SIZE >> 2)) + space = USB_PACKET_SIZE >> 2; + + // write data + for(i = 0, j = 0; i < space; i += 1, j += 2) + c[i] = complex(s[j], s[j + 1]); + + // update cb + m_cb->wrote(i); + } + + // if the cb is full, we left behind data from the usb packet + if(m_cb->space_available() == 0) { + fprintf(stderr, "warning: local overrun\n"); + overruns++; + } + + if(overrun_i) + *overrun_i = overruns; + + return 0; +} + + +int usrp_source::read(complex *buf, unsigned int num_samples, + unsigned int *samples_read) { + + unsigned int n; + + if(fill(num_samples, 0)) + return -1; + + n = m_cb->read(buf, num_samples); + + if(samples_read) + *samples_read = n; + + return 0; +} + + +/* + * Don't hold a lock on this and use the usrp at the same time. + */ +circular_buffer *usrp_source::get_buffer() { + + return m_cb; +} + + +int usrp_source::flush(unsigned int flush_count) { + + m_cb->flush(); + fill(flush_count * USB_PACKET_SIZE, 0); + m_cb->flush(); + + return 0; +} diff --git a/src/usrp_source.h b/src/usrp_source.h new file mode 100644 index 0000000..cf4a2ae --- /dev/null +++ b/src/usrp_source.h @@ -0,0 +1,85 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +#include "usrp_complex.h" +#include "circular_buffer.h" + + +class usrp_source { +public: + usrp_source(float sample_rate, long int fpga_master_clock_freq = 52000000); + usrp_source(unsigned int decimation, long int fpga_master_clock_freq = 52000000); + ~usrp_source(); + + int open(unsigned int subdev); + int read(complex *buf, unsigned int num_samples, unsigned int *samples_read); + int fill(unsigned int num_samples, unsigned int *overrun); + int tune(double freq); + bool set_antenna(int antenna); + bool set_gain(float gain); + void start(); + void stop(); + int flush(unsigned int flush_count = FLUSH_COUNT); + circular_buffer *get_buffer(); + + float sample_rate(); + + static const unsigned int side_A = 0; + static const unsigned int side_B = 1; + +private: + void calculate_decimation(); + + usrp_standard_rx_sptr m_u_rx; + db_base_sptr m_db_rx; + + float m_sample_rate; + float m_desired_sample_rate; + unsigned int m_decimation; + + long int m_fpga_master_clock_freq; + + circular_buffer * m_cb; + + /* + * This mutex protects access to the USRP and daughterboards but not + * necessarily to any fields in this class. + */ + pthread_mutex_t m_u_mutex; + + static const unsigned int FLUSH_COUNT = 10; + static const unsigned int CB_LEN = (1 << 20); + static const int NCHAN = 1; + static const int INITIAL_MUX = -1; + static const int FUSB_BLOCK_SIZE = 1024; + static const int FUSB_NBLOCKS = 16 * 8; + static const char * FPGA_FILENAME() { + return "std_2rxhb_2tx.rbf"; + } +}; diff --git a/src/util.cc b/src/util.cc new file mode 100644 index 0000000..33cb72b --- /dev/null +++ b/src/util.cc @@ -0,0 +1,96 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + + +void display_freq(float f) { + + if(f >= 0) { + printf("+ "); + } else { + printf("- "); + f = -f; + } + if(fabs(f) >= 1e9) { + printf("%.3fGHz", f / 1e9); + return; + } + if(fabs(f) >= 1e6) { + printf("%.1fMHz", f / 1e6); + return; + } + if(fabs(f) >= 1e3) { + printf("%.3fkHz", f / 1e3); + return; + } + if(fabs(f) >= 1e2) { + printf("%.0fHz", f); + return; + } + if(fabs(f) >= 1e1) { + printf(" %.0fHz", f); + return; + } + printf(" %.0fHz", f); +} + + +void sort(float *b, unsigned int len) { + + for(unsigned int i = 0; i < len; i++) { + for(unsigned int j = i + 1; j < len; j++) { + if(b[j] < b[i]) { + float t = b[i]; + b[i] = b[j]; + b[j] = t; + } + } + } +} + + +double avg(float *b, unsigned int len, float *stddev) { + + unsigned int i; + double t = 0.0, a = 0.0, s = 0.0; + + for(i = 0; i < len; i++) + t += b[i]; + a = t / len; + if(stddev) { + for(i = 0; i < len; i++) + s += (b[i] - a) * (b[i] - a); + s /= len; + s = sqrt(s); + *stddev = s; + } + + return a; +} diff --git a/src/util.h b/src/util.h new file mode 100644 index 0000000..8f17175 --- /dev/null +++ b/src/util.h @@ -0,0 +1,30 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +void display_freq(float f); +void sort(float *b, unsigned int len); +double avg(float *b, unsigned int len, float *stddev); diff --git a/src/version.h b/src/version.h new file mode 100644 index 0000000..b79bf5e --- /dev/null +++ b/src/version.h @@ -0,0 +1,29 @@ +/* + * Copyright (c) 2010, Joshua Lackey + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#pragma once +static const char * const kal_version_string = PACKAGE_VERSION;