Eric | a0e1ed3 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 1 | |
| 2 | /* |
| 3 | * (C) 2022 by sysmocom s.f.m.c. GmbH <info@sysmocom.de> |
| 4 | * All Rights Reserved |
| 5 | * |
| 6 | * Author: Eric Wild <ewild@sysmocom.de> |
| 7 | * |
| 8 | * This program is free software; you can redistribute it and/or modify |
| 9 | * it under the terms of the GNU Affero General Public License as published by |
| 10 | * the Free Software Foundation; either version 3 of the License, or |
| 11 | * (at your option) any later version. |
| 12 | * |
| 13 | * This program is distributed in the hope that it will be useful, |
| 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 16 | * GNU Affero General Public License for more details. |
| 17 | * |
| 18 | * You should have received a copy of the GNU Affero General Public License |
| 19 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 20 | * |
| 21 | */ |
| 22 | #include "GSMCommon.h" |
| 23 | #include <atomic> |
| 24 | #include <cassert> |
| 25 | #include <complex> |
| 26 | #include <iostream> |
| 27 | #include <cstdlib> |
| 28 | #include <cstdio> |
| 29 | #include <thread> |
| 30 | #include <fstream> |
| 31 | |
| 32 | #include "sigProcLib.h" |
| 33 | |
| 34 | #include "ms.h" |
| 35 | #include "ms_rx_burst.h" |
| 36 | #include "grgsm_vitac/grgsm_vitac.h" |
| 37 | |
| 38 | extern "C" { |
| 39 | #include "sch.h" |
| 40 | #include "convolve.h" |
| 41 | #include "convert.h" |
| 42 | } |
| 43 | |
| 44 | dummylog ms_trx::dummy_log; |
| 45 | |
| 46 | #ifdef DBGXX |
| 47 | const int offsetrange = 200; |
| 48 | const int offset_start = -15; |
| 49 | static int offset_ctr = 0; |
| 50 | #endif |
| 51 | |
| 52 | void tx_test(ms_trx *t, ts_hitter_q_t *q, unsigned int *tsc) |
| 53 | { |
| 54 | sched_param sch_params; |
| 55 | sch_params.sched_priority = sched_get_priority_max(SCHED_FIFO); |
| 56 | pthread_setschedparam(pthread_self(), SCHED_FIFO, &sch_params); |
| 57 | |
| 58 | auto burst = genRandAccessBurst(0, 4, 0); |
| 59 | scaleVector(*burst, t->txFullScale * 0.7); |
| 60 | |
| 61 | // float -> int16 |
| 62 | blade_sample_type burst_buf[burst->size()]; |
| 63 | convert_and_scale<int16_t, float>(burst_buf, burst->begin(), burst->size() * 2, 1); |
| 64 | |
| 65 | while (1) { |
| 66 | GSM::Time target; |
| 67 | while (!q->spsc_pop(&target)) { |
| 68 | q->spsc_prep_pop(); |
| 69 | } |
| 70 | |
| 71 | std::cerr << std::endl << "\x1B[32m hitting " << target.FN() << "\033[0m" << std::endl; |
| 72 | |
| 73 | int timing_advance = 0; |
| 74 | int64_t now_ts; |
| 75 | GSM::Time now_time; |
| 76 | target.incTN(3); // ul dl offset |
| 77 | int target_fn = target.FN(); |
| 78 | int target_tn = target.TN(); |
| 79 | t->timekeeper.get_both(&now_time, &now_ts); |
| 80 | |
| 81 | auto diff_fn = GSM::FNDelta(target_fn, now_time.FN()); |
| 82 | int diff_tn = (target_tn - (int)now_time.TN()) % 8; |
| 83 | auto tosend = GSM::Time(diff_fn, 0); |
| 84 | |
| 85 | if (diff_tn > 0) |
| 86 | tosend.incTN(diff_tn); |
| 87 | else if (diff_tn < 0) |
| 88 | tosend.decTN(-diff_tn); |
| 89 | |
| 90 | // in thory fn equal and tn+3 equal is also a problem... |
| 91 | if (diff_fn < 0 || (diff_fn == 0 && (now_time.TN() - target_tn < 1))) { |
| 92 | std::cerr << "## TX too late?! fn DIFF:" << diff_fn << " tn LOCAL: " << now_time.TN() |
| 93 | << " tn OTHER: " << target_tn << std::endl; |
| 94 | return; |
| 95 | } |
| 96 | |
| 97 | auto check = now_time + tosend; |
| 98 | int64_t send_ts = |
| 99 | now_ts + tosend.FN() * 8 * ONE_TS_BURST_LEN + tosend.TN() * ONE_TS_BURST_LEN - timing_advance; |
| 100 | |
| 101 | #ifdef DBGXX |
| 102 | std::cerr << "## fn DIFF: " << diff_fn << " ## tn DIFF: " << diff_tn << " tn LOCAL: " << now_time.TN() |
| 103 | << " tn OTHER: " << target_tn << " tndiff" << diff_tn << " tosend:" << tosend.FN() << ":" |
| 104 | << tosend.TN() << " calc: " << check.FN() << ":" << check.TN() << " target: " << target.FN() |
| 105 | << ":" << target.TN() << " ts now: " << now_ts << " target ts:" << send_ts << std::endl; |
| 106 | #endif |
| 107 | |
| 108 | unsigned int pad = 4 * 25; |
| 109 | blade_sample_type buf2[burst->size() + pad]; |
| 110 | memset(buf2, 0, pad * sizeof(blade_sample_type)); |
| 111 | memcpy(&buf2[pad], burst_buf, burst->size() * sizeof(blade_sample_type)); |
| 112 | |
| 113 | assert(target.FN() == check.FN()); |
| 114 | assert(target.TN() == check.TN()); |
| 115 | assert(target.FN() % 51 == 21); |
| 116 | #ifdef DBGXX |
| 117 | auto this_offset = offset_start + (offset_ctr++ % offsetrange); |
| 118 | std::cerr << "-- O " << this_offset << std::endl; |
| 119 | send_ts = now_ts - timing_advance + |
| 120 | ((target.FN() * 8 + (int)target.TN()) - (now_time.FN() * 8 + (int)now_time.TN())) * |
| 121 | ONE_TS_BURST_LEN; |
| 122 | #endif |
| 123 | t->submit_burst_ts(buf2, burst->size() + pad, send_ts - pad); |
| 124 | #ifdef DBGXX |
| 125 | signalVector test(burst->size() + pad); |
| 126 | convert_and_scale<float, int16_t>(test.begin(), buf2, burst->size() * 2 + pad, 1.f / float(scale)); |
| 127 | estim_burst_params ebp; |
| 128 | auto det = detectAnyBurst(test, 0, 4, 4, CorrType::RACH, 40, &ebp); |
| 129 | if (det > 0) |
| 130 | std::cerr << "## Y " << ebp.toa << std::endl; |
| 131 | else |
| 132 | std::cerr << "## NOOOOOOOOO " << ebp.toa << std::endl; |
| 133 | #endif |
| 134 | } |
| 135 | } |
| 136 | #ifdef SYNCTHINGONLY |
| 137 | template <typename A> auto parsec(std::vector<std::string> &v, A &itr, std::string arg, bool *rv) |
| 138 | { |
| 139 | if (*itr == arg) { |
| 140 | *rv = true; |
| 141 | return true; |
| 142 | } |
| 143 | return false; |
| 144 | } |
| 145 | |
| 146 | template <typename A, typename B, typename C> |
| 147 | bool parsec(std::vector<std::string> &v, A &itr, std::string arg, B f, C *rv) |
| 148 | { |
| 149 | if (*itr == arg) { |
| 150 | itr++; |
| 151 | if (itr != v.end()) { |
| 152 | *rv = f(itr->c_str()); |
| 153 | return true; |
| 154 | } |
| 155 | } |
| 156 | return false; |
| 157 | } |
| 158 | template <typename A> bool parsec(std::vector<std::string> &v, A &itr, std::string arg, int scale, int *rv) |
| 159 | { |
| 160 | return parsec( |
| 161 | v, itr, arg, [scale](const char *v) -> auto{ return atoi(v) * scale; }, rv); |
| 162 | } |
| 163 | template <typename A> bool parsec(std::vector<std::string> &v, A &itr, std::string arg, int scale, unsigned int *rv) |
| 164 | { |
| 165 | return parsec( |
| 166 | v, itr, arg, [scale](const char *v) -> auto{ return atoi(v) * scale; }, rv); |
| 167 | } |
| 168 | |
| 169 | int main(int argc, char *argv[]) |
| 170 | { |
| 171 | cpu_set_t cpuset; |
| 172 | |
| 173 | CPU_ZERO(&cpuset); |
| 174 | CPU_SET(2, &cpuset); |
| 175 | |
| 176 | auto rv = pthread_setaffinity_np(pthread_self(), sizeof(cpuset), &cpuset); |
| 177 | if (rv < 0) { |
| 178 | std::cerr << "affinity: errreur! " << std::strerror(errno); |
| 179 | return 0; |
| 180 | } |
| 181 | |
| 182 | unsigned int default_tx_freq(881000 * 1000), default_rx_freq(926000 * 1000); |
| 183 | unsigned int grx = 20, gtx = 20; |
| 184 | bool tx_flag = false; |
| 185 | pthread_setname_np(pthread_self(), "main"); |
| 186 | convolve_init(); |
| 187 | convert_init(); |
| 188 | sigProcLibSetup(); |
| 189 | initvita(); |
| 190 | |
| 191 | int status = 0; |
| 192 | auto trx = new ms_trx(); |
| 193 | trx->do_auto_gain = true; |
| 194 | |
| 195 | std::vector<std::string> args(argv + 1, argv + argc); |
| 196 | for (auto i = args.begin(); i != args.end(); ++i) { |
| 197 | parsec(args, i, "-r", 1000, &default_rx_freq); |
| 198 | parsec(args, i, "-t", 1000, &default_tx_freq); |
| 199 | parsec(args, i, "-gr", 1, &grx); |
| 200 | parsec(args, i, "-gt", 1, >x); |
| 201 | parsec(args, i, "-tx", &tx_flag); |
| 202 | } |
| 203 | |
| 204 | std::cerr << "usage: " << argv[0] << " <rxfreq in khz, i.e. 926000> [txfreq in khz, i.e. 881000] [TX]" |
| 205 | << std::endl |
| 206 | << "rx" << (argc == 1 ? " (default) " : " ") << default_rx_freq << "hz, tx " << default_tx_freq |
| 207 | << "hz" << std::endl |
| 208 | << "gain rx " << grx << " gain tx " << gtx << std::endl |
| 209 | << (tx_flag ? "##!!## RACH TX ACTIVE ##!!##" : "-- no rach tx --") << std::endl; |
| 210 | |
| 211 | status = trx->init_dev_and_streams(); |
| 212 | if (status < 0) |
| 213 | return status; |
| 214 | trx->tuneRx(default_rx_freq); |
| 215 | trx->tuneTx(default_tx_freq); |
| 216 | trx->setRxGain(grx); |
| 217 | trx->setTxGain(gtx); |
| 218 | |
| 219 | if (status == 0) { |
| 220 | // FIXME: hacks! needs exit flag for detached threads! |
| 221 | |
| 222 | std::thread(rcv_bursts_test, &trx->rxqueue, &trx->mTSC, trx->rxFullScale).detach(); |
| 223 | if (tx_flag) |
| 224 | std::thread(tx_test, trx, &trx->ts_hitter_q, &trx->mTSC).detach(); |
| 225 | trx->start(); |
| 226 | do { |
| 227 | sleep(1); |
| 228 | } while (1); |
| 229 | |
| 230 | trx->stop_threads(); |
| 231 | } |
| 232 | delete trx; |
| 233 | |
| 234 | return status; |
| 235 | } |
| 236 | #endif |
| 237 | |
| 238 | int ms_trx::init_dev_and_streams() |
| 239 | { |
| 240 | int status = 0; |
| 241 | status = base::init_device(rx_bh(), tx_bh()); |
| 242 | if (status < 0) { |
| 243 | std::cerr << "failed to init dev!" << std::endl; |
| 244 | return -1; |
| 245 | } |
| 246 | return status; |
| 247 | } |
| 248 | |
| 249 | bh_fn_t ms_trx::rx_bh() |
| 250 | { |
| 251 | return [this](dev_buf_t *rcd) -> int { |
| 252 | if (this->search_for_sch(rcd) == SCH_STATE::FOUND) |
| 253 | this->grab_bursts(rcd); |
| 254 | return 0; |
| 255 | }; |
| 256 | } |
| 257 | |
| 258 | bh_fn_t ms_trx::tx_bh() |
| 259 | { |
| 260 | return [this](dev_buf_t *rcd) -> int { |
| 261 | #pragma unused(rcd) |
| 262 | auto y = this; |
| 263 | #pragma unused(y) |
| 264 | /* nothing to do here */ |
| 265 | return 0; |
| 266 | }; |
| 267 | } |
| 268 | |
| 269 | void ms_trx::start() |
| 270 | { |
| 271 | auto fn = get_rx_burst_handler_fn(rx_bh()); |
| 272 | rx_task = std::thread(fn); |
| 273 | set_name_aff_sched(rx_task.native_handle(), "rxrun", 2, SCHED_FIFO, sched_get_priority_max(SCHED_FIFO) - 2); |
| 274 | |
| 275 | usleep(1000); |
| 276 | auto fn2 = get_tx_burst_handler_fn(tx_bh()); |
| 277 | tx_task = std::thread(fn2); |
| 278 | set_name_aff_sched(tx_task.native_handle(), "txrun", 2, SCHED_FIFO, sched_get_priority_max(SCHED_FIFO) - 1); |
| 279 | } |
| 280 | |
| 281 | void ms_trx::set_upper_ready(bool is_ready) |
| 282 | { |
| 283 | upper_is_ready = is_ready; |
| 284 | } |
| 285 | |
| 286 | void ms_trx::stop_threads() |
| 287 | { |
| 288 | std::cerr << "killing threads...\r\n" << std::endl; |
| 289 | close_device(); |
| 290 | rx_task.join(); |
| 291 | tx_task.join(); |
| 292 | } |
| 293 | |
| 294 | void ms_trx::submit_burst(blade_sample_type *buffer, int len, GSM::Time target) |
| 295 | { |
| 296 | int64_t now_ts; |
| 297 | GSM::Time now_time; |
| 298 | target.incTN(3); // ul dl offset |
| 299 | int target_fn = target.FN(); |
| 300 | int target_tn = target.TN(); |
| 301 | timekeeper.get_both(&now_time, &now_ts); |
| 302 | |
| 303 | auto diff_fn = GSM::FNDelta(target_fn, now_time.FN()); |
| 304 | int diff_tn = (target_tn - (int)now_time.TN()) % 8; |
| 305 | auto tosend = GSM::Time(diff_fn, 0); |
| 306 | |
| 307 | if (diff_tn > 0) |
| 308 | tosend.incTN(diff_tn); |
| 309 | else |
| 310 | tosend.decTN(-diff_tn); |
| 311 | |
| 312 | // in thory fn equal and tn+3 equal is also a problem... |
| 313 | if (diff_fn < 0 || (diff_fn == 0 && (now_time.TN() - target_tn < 1))) { |
| 314 | std::cerr << "## TX too late?! fn DIFF:" << diff_fn << " tn LOCAL: " << now_time.TN() |
| 315 | << " tn OTHER: " << target_tn << std::endl; |
| 316 | return; |
| 317 | } |
| 318 | |
| 319 | auto check = now_time + tosend; |
| 320 | int64_t send_ts = now_ts + tosend.FN() * 8 * ONE_TS_BURST_LEN + tosend.TN() * ONE_TS_BURST_LEN - timing_advance; |
| 321 | #ifdef DBGXX |
| 322 | std::cerr << "## fn DIFF: " << diff_fn << " ## tn DIFF: " << diff_tn << " tn LOCAL/OTHER: " << now_time.TN() |
| 323 | << "/" << target_tn << " tndiff" << diff_tn << " tosend:" << tosend.FN() << ":" << tosend.TN() |
| 324 | << " check: " << check.FN() << ":" << check.TN() << " target: " << target.FN() << ":" << target.TN() |
| 325 | << " ts now: " << now_ts << " target ts:" << send_ts << std::endl; |
| 326 | #endif |
| 327 | #if 1 |
| 328 | unsigned int pad = 4 * 4; |
| 329 | blade_sample_type buf2[len + pad]; |
| 330 | memset(buf2, 0, pad * sizeof(blade_sample_type)); |
| 331 | memcpy(&buf2[pad], buffer, len * sizeof(blade_sample_type)); |
| 332 | |
| 333 | assert(target.FN() == check.FN()); |
| 334 | assert(target.TN() == check.TN()); |
| 335 | submit_burst_ts(buf2, len + pad, send_ts - pad); |
| 336 | #else |
| 337 | submit_burst_ts(buffer, len, send_ts); |
| 338 | #endif |
| 339 | } |