Vasil Velichkov | 5968552 | 2018-05-06 02:13:36 +0300 | [diff] [blame] | 1 | /* -*- c++ -*- */ |
| 2 | /* |
| 3 | * @file |
| 4 | * @author (C) 2018 by Vasil Velichkov <vvvelichkov@gmail.com> |
| 5 | * @section LICENSE |
| 6 | * |
| 7 | * Gr-gsm is free software; you can redistribute it and/or modify |
| 8 | * it under the terms of the GNU General Public License as published by |
| 9 | * the Free Software Foundation; either version 3, or (at your option) |
| 10 | * any later version. |
| 11 | * |
| 12 | * Gr-gsm is distributed in the hope that it will be useful, |
| 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 15 | * GNU General Public License for more details. |
| 16 | * |
| 17 | * You should have received a copy of the GNU General Public License |
| 18 | * along with gr-gsm; see the file COPYING. If not, write to |
| 19 | * the Free Software Foundation, Inc., 51 Franklin Street, |
| 20 | * Boston, MA 02110-1301, USA. |
| 21 | */ |
| 22 | |
| 23 | #ifdef HAVE_CONFIG_H |
| 24 | #include "config.h" |
| 25 | #endif |
| 26 | |
| 27 | #include <grgsm/gsmtap.h> |
| 28 | #include "tch_h_decoder_impl.h" |
| 29 | |
| 30 | extern "C" { |
| 31 | #include "osmocom/gsm/protocol/gsm_04_08.h" |
| 32 | #include "osmocom/coding/gsm0503_coding.h" |
| 33 | } |
| 34 | |
| 35 | namespace gr { |
| 36 | namespace gsm { |
| 37 | |
| 38 | static int ubits2sbits(ubit_t *ubits, sbit_t *sbits, int count) |
| 39 | { |
| 40 | int i; |
| 41 | |
| 42 | for (i = 0; i < count; i++) { |
| 43 | if (*ubits == 0x23) { |
| 44 | ubits++; |
| 45 | sbits++; |
| 46 | continue; |
| 47 | } |
| 48 | if ((*ubits++) & 1) |
| 49 | *sbits++ = -127; |
| 50 | else |
| 51 | *sbits++ = 127; |
| 52 | } |
| 53 | |
| 54 | return count; |
| 55 | } |
| 56 | |
| 57 | |
| 58 | tch_h_decoder::sptr |
| 59 | tch_h_decoder::make(unsigned int sub_channel, std::string multi_rate, bool boundary_check) |
| 60 | { |
| 61 | return gnuradio::get_initial_sptr |
| 62 | (new tch_h_decoder_impl(sub_channel, multi_rate, boundary_check)); |
| 63 | } |
| 64 | |
| 65 | /* |
| 66 | * Constructor |
| 67 | */ |
| 68 | tch_h_decoder_impl::tch_h_decoder_impl(unsigned int sub_channel, std::string multi_rate, bool boundary_check) |
| 69 | : gr::block("tch_h_decoder", |
| 70 | gr::io_signature::make(0, 0, 0), |
| 71 | gr::io_signature::make(0, 0, 0)), |
| 72 | d_collected_bursts_num(0), |
| 73 | d_tch_mode(TCH_HS), |
| 74 | d_sub_channel(sub_channel), |
| 75 | d_boundary_check(boundary_check), |
| 76 | d_boundary_decode(false), |
| 77 | d_header_sent(false), |
| 78 | d_ft(0), |
| 79 | d_cmr(0) |
| 80 | { |
| 81 | //setup input/output ports |
| 82 | message_port_register_in(pmt::mp("bursts")); |
| 83 | set_msg_handler(pmt::mp("bursts"), boost::bind(&tch_h_decoder_impl::decode, this, _1)); |
| 84 | message_port_register_out(pmt::mp("msgs")); |
| 85 | message_port_register_out(pmt::mp("voice")); |
| 86 | |
| 87 | if(multi_rate.length()) |
| 88 | { |
| 89 | std::cout<<"multi_rate configuration: "<<multi_rate<<std::endl; |
| 90 | if (multi_rate.length() < 4 || multi_rate.length() % 2) |
| 91 | { |
| 92 | throw std::invalid_argument("Invalid multi_rate hexstring"); |
| 93 | } |
| 94 | |
| 95 | std::vector<uint8_t> binary; |
| 96 | for (std::string::const_iterator it = multi_rate.begin(); |
| 97 | it != multi_rate.end(); it += 2) |
| 98 | { |
| 99 | std::string byte(it, it + 2); |
| 100 | char* end = NULL; |
| 101 | errno = 0; |
| 102 | uint8_t b = strtoul(byte.c_str(), &end, 16); |
| 103 | if (errno != 0 || *end != '\0') |
| 104 | { |
| 105 | throw std::invalid_argument("Invalid multi_rate hexstring"); |
| 106 | } |
| 107 | binary.push_back(b); |
| 108 | } |
| 109 | |
| 110 | if (binary.size() < 2) { |
| 111 | throw std::invalid_argument("The multi_rate is too short"); |
| 112 | } |
| 113 | |
| 114 | //GSM A-I/F DTAP - Assignment Command |
| 115 | // Protocol Discriminator: Radio Resources Management messages (6) |
| 116 | // DTAP Radio Resources Management Message Type: Assignment Command (0x2e) |
| 117 | // Channel Description 2 - Description of the First Channel, after time |
| 118 | // Power Command |
| 119 | // Channel Mode - Mode of the First Channel(Channel Set 1) |
| 120 | // MultiRate configuration |
| 121 | // Element ID: 0x03 |
| 122 | // Length: 4 |
| 123 | // 001. .... = Multirate speech version: Adaptive Multirate speech version 1 (1) |
| 124 | // ...0 .... = NSCB: Noise Suppression Control Bit: Noise Suppression can be used (default) (0) |
| 125 | // .... 1... = ICMI: Initial Codec Mode Indicator: The initial codec mode is defined by the Start Mode field (1) |
| 126 | // .... ..00 = Start Mode: 0 |
| 127 | // 0... .... = 12,2 kbit/s codec rate: is not part of the subset |
| 128 | // .0.. .... = 10,2 kbit/s codec rate: is not part of the subset |
| 129 | // ..0. .... = 7,95 kbit/s codec rate: is not part of the subset |
| 130 | // ...1 .... = 7,40 kbit/s codec rate: is part of the subset |
| 131 | // .... 0... = 6,70 kbit/s codec rate: is not part of the subset |
| 132 | // .... .0.. = 5,90 kbit/s codec rate: is not part of the subset |
| 133 | // .... ..0. = 5,15 kbit/s codec rate: is not part of the subset |
| 134 | // .... ...1 = 4,75 kbit/s codec rate: is part of the subset |
| 135 | // ..01 1010 = AMR Threshold: 13.0 dB (26) |
| 136 | // 0100 .... = AMR Hysteresis: 2.0 dB (4) |
| 137 | |
| 138 | const uint8_t first = binary[0]; |
| 139 | uint8_t multirate_speech_ver = (first >> 5) & 0x07; |
| 140 | if (multirate_speech_ver == 1) |
| 141 | { |
| 142 | d_tch_mode = TCH_AFS4_75; |
| 143 | } |
| 144 | else if (multirate_speech_ver == 2) |
| 145 | { |
| 146 | throw std::invalid_argument("Adaptive Multirate speech version 2 is not supported"); |
| 147 | } |
| 148 | else |
| 149 | { |
| 150 | throw std::invalid_argument("Multirate speech version"); |
| 151 | } |
| 152 | |
| 153 | bool ncsb = (first >> 4) & 0x01; |
| 154 | bool icmi = (first >> 3) & 0x01; |
| 155 | uint8_t start = first & 0x03; |
| 156 | |
| 157 | const uint8_t codecs = binary[1]; |
| 158 | for (int i = 0; i < 8; i++) |
| 159 | { |
| 160 | if ((codecs >> i) & 1) |
| 161 | { |
| 162 | d_multi_rate_codes.push_back(i); |
| 163 | } |
| 164 | } |
| 165 | |
| 166 | std::cout<<"Enabled AMR Codecs:"<<std::endl; |
| 167 | for(std::vector<uint8_t>::const_iterator it = d_multi_rate_codes.begin(); |
| 168 | it != d_multi_rate_codes.end(); |
| 169 | it ++) |
| 170 | { |
| 171 | switch(*it) |
| 172 | { |
| 173 | case 0: |
| 174 | std::cout<<"4,75 kbit/s codec rate: is part of the subset"<<std::endl; |
| 175 | break; |
| 176 | case 1: |
| 177 | std::cout<<"5,15 kbit/s codec rate: is part of the subset"<<std::endl; |
| 178 | break; |
| 179 | case 2: |
| 180 | std::cout<<"6,90 kbit/s codec rate: is part of the subset"<<std::endl; |
| 181 | break; |
| 182 | case 3: |
| 183 | std::cout<<"6,70 kbit/s codec rate: is part of the subset"<<std::endl; |
| 184 | break; |
| 185 | case 4: |
| 186 | std::cout<<"7,40 kbit/s codec rate: is part of the subset"<<std::endl; |
| 187 | break; |
| 188 | case 5: |
| 189 | std::cout<<"7,95 kbit/s codec rate: is part of the subset"<<std::endl; |
| 190 | break; |
| 191 | case 6: |
| 192 | std::cout<<"12,2 kbit/s codec rate: is part of the subset"<<std::endl; |
| 193 | } |
| 194 | } |
| 195 | if (d_multi_rate_codes.size() > 4) { |
| 196 | throw std::invalid_argument("More then 4 multirate codes"); |
| 197 | } |
| 198 | } |
| 199 | } |
| 200 | |
| 201 | tch_h_decoder_impl::~tch_h_decoder_impl() |
| 202 | { |
| 203 | } |
| 204 | |
| 205 | void tch_h_decoder_impl::decode(pmt::pmt_t msg) |
| 206 | { |
| 207 | d_bursts[d_collected_bursts_num++] = msg; |
| 208 | if (d_collected_bursts_num <= 7) |
| 209 | { |
| 210 | return; |
| 211 | } |
| 212 | |
| 213 | gsmtap_hdr* header = (gsmtap_hdr*)(pmt::blob_data(pmt::cdr(msg))); |
| 214 | uint32_t frame_nr = be32toh(header->frame_number); |
| 215 | bool uplink_burst = (be16toh(header->arfcn) & 0x4000) ? true : false; |
| 216 | |
| 217 | //TODO: Check in 3gpp specs which frames could contains facch/h frames |
| 218 | //and replace this ugly formula with table |
| 219 | int fn_is_odd = (((frame_nr - (uplink_burst ? 10 : 15)) % 26) >> 2) & 1; |
| 220 | |
| 221 | ubit_t bursts_u[116 * 6] = {0}; //facch/h is 6 bursts |
| 222 | |
| 223 | //reorganize data |
| 224 | for (int ii = 0; ii < 8; ii++) |
| 225 | { |
| 226 | //skip the 4th and 5th bursts |
| 227 | if (ii == 4 || ii == 5) continue; |
| 228 | |
| 229 | int8_t* burst_bits = (int8_t*)(pmt::blob_data(pmt::cdr(d_bursts[ii])))+sizeof(gsmtap_hdr); |
| 230 | |
| 231 | //copy 6th and 7th burst to 4th and 5th position |
| 232 | int n = ii < 6 ? ii : ii - 2; |
| 233 | |
| 234 | memcpy(&bursts_u[n*116], &burst_bits[3],58); |
| 235 | memcpy(&bursts_u[n*116+58], &burst_bits[3+57+1+26],58); |
| 236 | } |
| 237 | |
| 238 | //Convert to sbits |
| 239 | sbit_t bursts_s[116 * 6] = {0}; |
| 240 | ubits2sbits(bursts_u, bursts_s, 116 * 6); |
| 241 | |
| 242 | //Prepare burst for the next iteration by shifting them by 4 |
| 243 | for (int ii = 0; ii < 4; ii++) { |
| 244 | d_bursts[ii] = d_bursts[ii + 4]; |
| 245 | } |
| 246 | d_collected_bursts_num = 4; |
| 247 | |
| 248 | uint8_t frameBuffer[64]; |
| 249 | int frameLength = -1; |
| 250 | int n_errors, n_bits_total; |
| 251 | |
| 252 | if (d_tch_mode == TCH_HS) |
| 253 | { |
| 254 | frameLength = gsm0503_tch_hr_decode(frameBuffer, bursts_s, fn_is_odd, &n_errors, &n_bits_total); |
| 255 | } |
| 256 | else |
| 257 | { |
| 258 | frameLength = gsm0503_tch_ahs_decode(frameBuffer, bursts_s, fn_is_odd, |
| 259 | fn_is_odd, //int codec_mode_req, |
| 260 | &d_multi_rate_codes.front(), d_multi_rate_codes.size(), |
| 261 | &d_ft, |
| 262 | &d_cmr, |
| 263 | &n_errors, &n_bits_total); |
| 264 | } |
| 265 | |
| 266 | if (frameLength < 12) |
| 267 | { |
| 268 | if (!d_boundary_check || d_boundary_decode) { |
| 269 | std::cerr<<"Error! frame_nr:"<<frame_nr<<" mod26:"<<frame_nr%26 |
| 270 | <<" fn_is_odd:"<<fn_is_odd<<" length:"<<frameLength<<std::endl; |
| 271 | } |
| 272 | return; |
| 273 | } |
| 274 | else if (frameLength == GSM_MACBLOCK_LEN) //FACCH/H |
| 275 | { |
| 276 | pmt::pmt_t first_header_plus_burst = pmt::cdr(d_bursts[0]); |
| 277 | gsmtap_hdr* header = (gsmtap_hdr *)pmt::blob_data(first_header_plus_burst); |
| 278 | int8_t header_plus_data[sizeof(gsmtap_hdr)+frameLength]; |
| 279 | memcpy(header_plus_data, header, sizeof(gsmtap_hdr)); |
| 280 | memcpy(header_plus_data+sizeof(gsmtap_hdr), frameBuffer, frameLength); |
| 281 | ((gsmtap_hdr*)header_plus_data)->type = GSMTAP_TYPE_UM; |
| 282 | |
| 283 | pmt::pmt_t msg_binary_blob = pmt::make_blob(header_plus_data, frameLength + sizeof(gsmtap_hdr)); |
| 284 | pmt::pmt_t msg_out = pmt::cons(pmt::PMT_NIL, msg_binary_blob); |
| 285 | |
| 286 | message_port_pub(pmt::mp("msgs"), msg_out); |
| 287 | |
| 288 | // if d_boundary_check is enabled, we set d_boundary_decode to true, when a |
| 289 | // "Connect" or "Connect Acknowledge" message is received, and |
| 290 | // we set d_boundary_decode back to false, when "Release" message is received |
| 291 | if (d_boundary_check) |
| 292 | { |
| 293 | // check if this is a call control message |
| 294 | if ((frameBuffer[3] & 0x0f) == 0x03) |
| 295 | { |
Vasil Velichkov | 4d300e9 | 2018-05-15 21:36:14 +0300 | [diff] [blame] | 296 | // Alerting |
| 297 | if ((frameBuffer[4] & 0x3f) == 0x01) |
| 298 | { |
| 299 | if ((frameBuffer[5] == 0x1e) && //element id |
| 300 | (frameBuffer[6] == 2) && //length |
| 301 | ((frameBuffer[8] & 0x7f) == 0x08)) |
| 302 | { |
| 303 | //.000 1000 = Progress description: In-band information or appropriate pattern now available (8) |
| 304 | d_boundary_decode = true; |
| 305 | } |
| 306 | } |
| 307 | // Progress |
| 308 | else if ((frameBuffer[4] & 0x3f) == 0x03) |
| 309 | { |
| 310 | if ((frameBuffer[5] == 2) && //length |
| 311 | (frameBuffer[7] & 0x7f) == 0x08) |
| 312 | { |
| 313 | //.000 1000 = Progress description: In-band information or appropriate pattern now available (8) |
| 314 | d_boundary_decode = true; |
| 315 | } |
| 316 | } |
Vasil Velichkov | 5968552 | 2018-05-06 02:13:36 +0300 | [diff] [blame] | 317 | // Connect specified in GSM 04.08, 9.3.5 |
Vasil Velichkov | 4d300e9 | 2018-05-15 21:36:14 +0300 | [diff] [blame] | 318 | else if ((frameBuffer[4] & 0x3f) == 0x07) |
Vasil Velichkov | 5968552 | 2018-05-06 02:13:36 +0300 | [diff] [blame] | 319 | { |
| 320 | d_boundary_decode = true; |
| 321 | } |
| 322 | // Connect Acknowledge specified in GSM 04.08, 9.3.6 |
| 323 | else if ((frameBuffer[4] & 0x3f) == 0x0f) |
| 324 | { |
| 325 | d_boundary_decode = true; |
| 326 | } |
| 327 | // Release specified in GSM 04.08, 9.3.18 |
| 328 | else if ((frameBuffer[4] & 0x3f) == 0x2d) |
| 329 | { |
| 330 | d_boundary_decode = false; |
| 331 | } |
| 332 | } |
| 333 | } |
| 334 | return; |
| 335 | } |
| 336 | |
| 337 | if (!d_header_sent && d_tch_mode != TCH_HS) |
| 338 | { |
| 339 | const unsigned char amr_nb_magic[7] = "#!AMR\n"; |
| 340 | message_port_pub(pmt::mp("voice"), pmt::cons(pmt::PMT_NIL, pmt::make_blob(amr_nb_magic, 6))); |
| 341 | d_header_sent = true; |
| 342 | } |
| 343 | |
| 344 | if (!n_errors && (!d_boundary_check || d_boundary_decode)) |
| 345 | { |
| 346 | //std::cerr<<"Voice frame_nr:"<<frame_nr<<" mod26:"<<frame_nr%26<<" is_odd:"<<fn_is_odd |
| 347 | // <<" type:"<<(uint32_t)d_ft<<" cmr:"<<(uint32_t)d_cmr |
| 348 | // <<" errors:"<<n_errors<<std::endl; |
| 349 | |
| 350 | if (d_tch_mode != TCH_HS) |
| 351 | { |
| 352 | //Move one byte to make space for the header |
| 353 | memmove(frameBuffer + 1, frameBuffer, frameLength); |
| 354 | //Add the AMR header |
| 355 | switch(frameLength) |
| 356 | { |
| 357 | case 12: frameBuffer[0] = (0 << 3); break; //TCH/AHS4.75 |
| 358 | case 13: frameBuffer[0] = (1 << 3); break; //TCH/AHS5.15 |
| 359 | case 15: frameBuffer[0] = (2 << 3); break; //TCH/AHS5.9 |
| 360 | case 17: frameBuffer[0] = (3 << 3); break; //TCH/AHS6.7 |
| 361 | case 19: frameBuffer[0] = (4 << 3); break; //TCH/AHS7.4 |
| 362 | case 20: frameBuffer[0] = (5 << 3); break; //TCH/AHS7.95 |
| 363 | default: std::cerr<<"Unexpected voice frame length:"<<frameLength<<std::endl; return; |
| 364 | } |
| 365 | frameLength += 1; |
| 366 | } |
| 367 | |
| 368 | //std::ostringstream out; |
| 369 | //out << "voice frame: "; |
| 370 | //for (int i = 0; i < frameLength; i++) |
| 371 | // out << " " << (std::hex) << std::setw(2) << std::setfill('0') << (uint32_t)*(frameBuffer + i); |
| 372 | //std::cerr << out.str() << std::endl; |
| 373 | message_port_pub(pmt::mp("voice"), pmt::cons(pmt::PMT_NIL, pmt::make_blob(frameBuffer, frameLength))); |
| 374 | } |
| 375 | } |
| 376 | } /* namespace gsm */ |
| 377 | } /* namespace gr */ |
| 378 | |