Eric | b7253c6 | 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 | |
Eric | 2e6c362 | 2023-05-02 14:34:49 +0200 | [diff] [blame] | 52 | template <> |
Eric | 4097804 | 2023-05-23 10:59:58 +0200 | [diff] [blame^] | 53 | std::atomic<bool> ms_trx::base::stop_lower_threads_flag(false); |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 54 | |
| 55 | void tx_test(ms_trx *t, ts_hitter_q_t *q, unsigned int *tsc) |
| 56 | { |
| 57 | sched_param sch_params; |
| 58 | sch_params.sched_priority = sched_get_priority_max(SCHED_FIFO); |
| 59 | pthread_setschedparam(pthread_self(), SCHED_FIFO, &sch_params); |
| 60 | |
| 61 | auto burst = genRandAccessBurst(0, 4, 0); |
Eric | 097a16e | 2023-03-02 17:46:49 +0100 | [diff] [blame] | 62 | scaleVector(*burst, t->txFullScale); |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 63 | |
| 64 | // float -> int16 |
| 65 | blade_sample_type burst_buf[burst->size()]; |
Eric Wild | 621a49e | 2023-01-12 16:21:58 +0100 | [diff] [blame] | 66 | convert_and_scale(burst_buf, burst->begin(), burst->size() * 2, 1); |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 67 | |
| 68 | while (1) { |
| 69 | GSM::Time target; |
| 70 | while (!q->spsc_pop(&target)) { |
| 71 | q->spsc_prep_pop(); |
| 72 | } |
| 73 | |
| 74 | std::cerr << std::endl << "\x1B[32m hitting " << target.FN() << "\033[0m" << std::endl; |
| 75 | |
| 76 | int timing_advance = 0; |
| 77 | int64_t now_ts; |
| 78 | GSM::Time now_time; |
| 79 | target.incTN(3); // ul dl offset |
| 80 | int target_fn = target.FN(); |
| 81 | int target_tn = target.TN(); |
| 82 | t->timekeeper.get_both(&now_time, &now_ts); |
| 83 | |
| 84 | auto diff_fn = GSM::FNDelta(target_fn, now_time.FN()); |
| 85 | int diff_tn = (target_tn - (int)now_time.TN()) % 8; |
| 86 | auto tosend = GSM::Time(diff_fn, 0); |
| 87 | |
| 88 | if (diff_tn > 0) |
| 89 | tosend.incTN(diff_tn); |
| 90 | else if (diff_tn < 0) |
| 91 | tosend.decTN(-diff_tn); |
| 92 | |
| 93 | // in thory fn equal and tn+3 equal is also a problem... |
| 94 | if (diff_fn < 0 || (diff_fn == 0 && (now_time.TN() - target_tn < 1))) { |
| 95 | std::cerr << "## TX too late?! fn DIFF:" << diff_fn << " tn LOCAL: " << now_time.TN() |
| 96 | << " tn OTHER: " << target_tn << std::endl; |
| 97 | return; |
| 98 | } |
| 99 | |
| 100 | auto check = now_time + tosend; |
| 101 | int64_t send_ts = |
| 102 | now_ts + tosend.FN() * 8 * ONE_TS_BURST_LEN + tosend.TN() * ONE_TS_BURST_LEN - timing_advance; |
| 103 | |
| 104 | #ifdef DBGXX |
| 105 | std::cerr << "## fn DIFF: " << diff_fn << " ## tn DIFF: " << diff_tn << " tn LOCAL: " << now_time.TN() |
| 106 | << " tn OTHER: " << target_tn << " tndiff" << diff_tn << " tosend:" << tosend.FN() << ":" |
| 107 | << tosend.TN() << " calc: " << check.FN() << ":" << check.TN() << " target: " << target.FN() |
| 108 | << ":" << target.TN() << " ts now: " << now_ts << " target ts:" << send_ts << std::endl; |
| 109 | #endif |
| 110 | |
| 111 | unsigned int pad = 4 * 25; |
| 112 | blade_sample_type buf2[burst->size() + pad]; |
| 113 | std::fill(buf2, buf2 + pad, 0); |
| 114 | |
| 115 | memcpy(&buf2[pad], burst_buf, burst->size() * sizeof(blade_sample_type)); |
| 116 | |
| 117 | assert(target.FN() == check.FN()); |
| 118 | assert(target.TN() == check.TN()); |
| 119 | assert(target.FN() % 51 == 21); |
| 120 | #ifdef DBGXX |
| 121 | auto this_offset = offset_start + (offset_ctr++ % offsetrange); |
| 122 | std::cerr << "-- O " << this_offset << std::endl; |
| 123 | send_ts = now_ts - timing_advance + |
| 124 | ((target.FN() * 8 + (int)target.TN()) - (now_time.FN() * 8 + (int)now_time.TN())) * |
| 125 | ONE_TS_BURST_LEN; |
| 126 | #endif |
| 127 | t->submit_burst_ts(buf2, burst->size() + pad, send_ts - pad); |
| 128 | #ifdef DBGXX |
| 129 | signalVector test(burst->size() + pad); |
Eric Wild | 621a49e | 2023-01-12 16:21:58 +0100 | [diff] [blame] | 130 | convert_and_scale(test.begin(), buf2, burst->size() * 2 + pad, 1.f / float(scale)); |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 131 | estim_burst_params ebp; |
| 132 | auto det = detectAnyBurst(test, 0, 4, 4, CorrType::RACH, 40, &ebp); |
| 133 | if (det > 0) |
| 134 | std::cerr << "## Y " << ebp.toa << std::endl; |
| 135 | else |
| 136 | std::cerr << "## NOOOOOOOOO " << ebp.toa << std::endl; |
| 137 | #endif |
| 138 | } |
| 139 | } |
| 140 | #ifdef SYNCTHINGONLY |
Eric | 2e6c362 | 2023-05-02 14:34:49 +0200 | [diff] [blame] | 141 | template <typename A> |
| 142 | auto parsec(std::vector<std::string> &v, A &itr, std::string arg, bool *rv) |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 143 | { |
| 144 | if (*itr == arg) { |
| 145 | *rv = true; |
| 146 | return true; |
| 147 | } |
| 148 | return false; |
| 149 | } |
| 150 | |
| 151 | template <typename A, typename B, typename C> |
| 152 | bool parsec(std::vector<std::string> &v, A &itr, std::string arg, B f, C *rv) |
| 153 | { |
| 154 | if (*itr == arg) { |
| 155 | itr++; |
| 156 | if (itr != v.end()) { |
| 157 | *rv = f(itr->c_str()); |
| 158 | return true; |
| 159 | } |
| 160 | } |
| 161 | return false; |
| 162 | } |
Eric | 2e6c362 | 2023-05-02 14:34:49 +0200 | [diff] [blame] | 163 | template <typename A> |
| 164 | bool parsec(std::vector<std::string> &v, A &itr, std::string arg, int scale, int *rv) |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 165 | { |
| 166 | return parsec( |
Eric | 2e6c362 | 2023-05-02 14:34:49 +0200 | [diff] [blame] | 167 | v, itr, arg, [scale](const char *v) -> auto { return atoi(v) * scale; }, rv); |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 168 | } |
Eric | 2e6c362 | 2023-05-02 14:34:49 +0200 | [diff] [blame] | 169 | template <typename A> |
| 170 | bool parsec(std::vector<std::string> &v, A &itr, std::string arg, int scale, unsigned int *rv) |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 171 | { |
| 172 | return parsec( |
Eric | 2e6c362 | 2023-05-02 14:34:49 +0200 | [diff] [blame] | 173 | v, itr, arg, [scale](const char *v) -> auto { return atoi(v) * scale; }, rv); |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 174 | } |
| 175 | |
| 176 | int main(int argc, char *argv[]) |
| 177 | { |
| 178 | cpu_set_t cpuset; |
| 179 | |
| 180 | CPU_ZERO(&cpuset); |
| 181 | CPU_SET(2, &cpuset); |
| 182 | |
| 183 | auto rv = pthread_setaffinity_np(pthread_self(), sizeof(cpuset), &cpuset); |
| 184 | if (rv < 0) { |
| 185 | std::cerr << "affinity: errreur! " << std::strerror(errno); |
| 186 | return 0; |
| 187 | } |
| 188 | |
| 189 | unsigned int default_tx_freq(881000 * 1000), default_rx_freq(926000 * 1000); |
| 190 | unsigned int grx = 20, gtx = 20; |
| 191 | bool tx_flag = false; |
| 192 | pthread_setname_np(pthread_self(), "main"); |
| 193 | convolve_init(); |
| 194 | convert_init(); |
| 195 | sigProcLibSetup(); |
| 196 | initvita(); |
| 197 | |
| 198 | int status = 0; |
| 199 | auto trx = new ms_trx(); |
| 200 | trx->do_auto_gain = true; |
| 201 | |
| 202 | std::vector<std::string> args(argv + 1, argv + argc); |
| 203 | for (auto i = args.begin(); i != args.end(); ++i) { |
| 204 | parsec(args, i, "-r", 1000, &default_rx_freq); |
| 205 | parsec(args, i, "-t", 1000, &default_tx_freq); |
| 206 | parsec(args, i, "-gr", 1, &grx); |
| 207 | parsec(args, i, "-gt", 1, >x); |
| 208 | parsec(args, i, "-tx", &tx_flag); |
| 209 | } |
| 210 | |
| 211 | std::cerr << "usage: " << argv[0] << " <rxfreq in khz, i.e. 926000> [txfreq in khz, i.e. 881000] [TX]" |
| 212 | << std::endl |
| 213 | << "rx" << (argc == 1 ? " (default) " : " ") << default_rx_freq << "hz, tx " << default_tx_freq |
| 214 | << "hz" << std::endl |
| 215 | << "gain rx " << grx << " gain tx " << gtx << std::endl |
| 216 | << (tx_flag ? "##!!## RACH TX ACTIVE ##!!##" : "-- no rach tx --") << std::endl; |
| 217 | |
| 218 | status = trx->init_dev_and_streams(); |
| 219 | if (status < 0) |
| 220 | return status; |
| 221 | trx->tuneRx(default_rx_freq); |
| 222 | trx->tuneTx(default_tx_freq); |
| 223 | trx->setRxGain(grx); |
| 224 | trx->setTxGain(gtx); |
| 225 | |
| 226 | if (status == 0) { |
| 227 | // FIXME: hacks! needs exit flag for detached threads! |
| 228 | |
| 229 | std::thread(rcv_bursts_test, &trx->rxqueue, &trx->mTSC, trx->rxFullScale).detach(); |
| 230 | if (tx_flag) |
| 231 | std::thread(tx_test, trx, &trx->ts_hitter_q, &trx->mTSC).detach(); |
| 232 | trx->start(); |
| 233 | do { |
| 234 | sleep(1); |
| 235 | } while (1); |
| 236 | |
| 237 | trx->stop_threads(); |
| 238 | } |
| 239 | delete trx; |
| 240 | |
| 241 | return status; |
| 242 | } |
| 243 | #endif |
| 244 | |
| 245 | int ms_trx::init_dev_and_streams() |
| 246 | { |
| 247 | int status = 0; |
| 248 | status = base::init_device(rx_bh(), tx_bh()); |
| 249 | if (status < 0) { |
| 250 | std::cerr << "failed to init dev!" << std::endl; |
| 251 | return -1; |
| 252 | } |
| 253 | return status; |
| 254 | } |
| 255 | |
| 256 | bh_fn_t ms_trx::rx_bh() |
| 257 | { |
| 258 | return [this](dev_buf_t *rcd) -> int { |
| 259 | if (this->search_for_sch(rcd) == SCH_STATE::FOUND) |
| 260 | this->grab_bursts(rcd); |
| 261 | return 0; |
| 262 | }; |
| 263 | } |
| 264 | |
| 265 | bh_fn_t ms_trx::tx_bh() |
| 266 | { |
| 267 | return [this](dev_buf_t *rcd) -> int { |
| 268 | #pragma GCC diagnostic push |
| 269 | #pragma GCC diagnostic ignored "-Wunused-variable" |
| 270 | auto y = this; |
| 271 | #pragma GCC diagnostic pop |
| 272 | /* nothing to do here */ |
| 273 | return 0; |
| 274 | }; |
| 275 | } |
| 276 | |
| 277 | void ms_trx::start() |
| 278 | { |
Eric | 4097804 | 2023-05-23 10:59:58 +0200 | [diff] [blame^] | 279 | if (stop_lower_threads_flag) |
Eric | b3157b9 | 2023-05-23 11:32:34 +0200 | [diff] [blame] | 280 | return; |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 281 | auto fn = get_rx_burst_handler_fn(rx_bh()); |
| 282 | rx_task = std::thread(fn); |
Eric | 805e0d9 | 2023-05-23 11:29:18 +0200 | [diff] [blame] | 283 | set_name_aff_sched(rx_task.native_handle(), sched_params::thread_names::RXRUN); |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 284 | |
| 285 | usleep(1000); |
| 286 | auto fn2 = get_tx_burst_handler_fn(tx_bh()); |
| 287 | tx_task = std::thread(fn2); |
Eric | 805e0d9 | 2023-05-23 11:29:18 +0200 | [diff] [blame] | 288 | set_name_aff_sched(tx_task.native_handle(), sched_params::thread_names::TXRUN); |
| 289 | |
Eric | b3157b9 | 2023-05-23 11:32:34 +0200 | [diff] [blame] | 290 | actually_enable_streams(); |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 291 | } |
| 292 | |
| 293 | void ms_trx::set_upper_ready(bool is_ready) |
| 294 | { |
| 295 | upper_is_ready = is_ready; |
| 296 | } |
| 297 | |
| 298 | void ms_trx::stop_threads() |
| 299 | { |
Eric | b3157b9 | 2023-05-23 11:32:34 +0200 | [diff] [blame] | 300 | std::cerr << "killing threads..." << std::endl; |
Eric | 4097804 | 2023-05-23 10:59:58 +0200 | [diff] [blame^] | 301 | stop_lower_threads_flag = true; |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 302 | close_device(); |
Eric | b3157b9 | 2023-05-23 11:32:34 +0200 | [diff] [blame] | 303 | std::cerr << "dev closed..." << std::endl; |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 304 | rx_task.join(); |
Eric | b3157b9 | 2023-05-23 11:32:34 +0200 | [diff] [blame] | 305 | std::cerr << "L rx dead..." << std::endl; |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 306 | tx_task.join(); |
Eric | b3157b9 | 2023-05-23 11:32:34 +0200 | [diff] [blame] | 307 | std::cerr << "L tx dead..." << std::endl; |
Eric | b7253c6 | 2022-11-28 19:21:08 +0100 | [diff] [blame] | 308 | } |
| 309 | |
| 310 | void ms_trx::submit_burst(blade_sample_type *buffer, int len, GSM::Time target) |
| 311 | { |
| 312 | int64_t now_ts; |
| 313 | GSM::Time now_time; |
| 314 | target.incTN(3); // ul dl offset |
| 315 | int target_fn = target.FN(); |
| 316 | int target_tn = target.TN(); |
| 317 | timekeeper.get_both(&now_time, &now_ts); |
| 318 | |
| 319 | auto diff_fn = GSM::FNDelta(target_fn, now_time.FN()); |
| 320 | int diff_tn = (target_tn - (int)now_time.TN()) % 8; |
| 321 | auto tosend = GSM::Time(diff_fn, 0); |
| 322 | |
| 323 | if (diff_tn > 0) |
| 324 | tosend.incTN(diff_tn); |
| 325 | else |
| 326 | tosend.decTN(-diff_tn); |
| 327 | |
| 328 | // in theory fn equal and tn+3 equal is also a problem... |
| 329 | if (diff_fn < 0 || (diff_fn == 0 && (now_time.TN() - target_tn < 1))) { |
| 330 | std::cerr << "## TX too late?! fn DIFF:" << diff_fn << " tn LOCAL: " << now_time.TN() |
| 331 | << " tn OTHER: " << target_tn << std::endl; |
| 332 | return; |
| 333 | } |
| 334 | |
| 335 | auto check = now_time + tosend; |
| 336 | int64_t send_ts = now_ts + tosend.FN() * 8 * ONE_TS_BURST_LEN + tosend.TN() * ONE_TS_BURST_LEN - timing_advance; |
| 337 | #ifdef DBGXX |
| 338 | std::cerr << "## fn DIFF: " << diff_fn << " ## tn DIFF: " << diff_tn << " tn LOCAL/OTHER: " << now_time.TN() |
| 339 | << "/" << target_tn << " tndiff" << diff_tn << " tosend:" << tosend.FN() << ":" << tosend.TN() |
| 340 | << " check: " << check.FN() << ":" << check.TN() << " target: " << target.FN() << ":" << target.TN() |
| 341 | << " ts now: " << now_ts << " target ts:" << send_ts << std::endl; |
| 342 | #endif |
| 343 | #if 1 |
| 344 | unsigned int pad = 4 * 4; |
| 345 | blade_sample_type buf2[len + pad]; |
| 346 | std::fill(buf2, buf2 + pad, 0); |
| 347 | memcpy(&buf2[pad], buffer, len * sizeof(blade_sample_type)); |
| 348 | |
| 349 | assert(target.FN() == check.FN()); |
| 350 | assert(target.TN() == check.TN()); |
| 351 | submit_burst_ts(buf2, len + pad, send_ts - pad); |
| 352 | #else |
| 353 | submit_burst_ts(buffer, len, send_ts); |
| 354 | #endif |
| 355 | } |