Roman Khassraf | 059bab9 | 2015-05-20 12:49:46 +0200 | [diff] [blame] | 1 | /* -*- c++ -*- */ |
| 2 | /* |
| 3 | * @file |
| 4 | * @author Piotr Krysik <ptrkrysik@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 <gnuradio/io_signature.h> |
| 28 | #include <grgsm/gsmtap.h> |
| 29 | #include "stdio.h" |
| 30 | #include "tch_f_decoder_impl.h" |
| 31 | |
| 32 | #define DATA_BYTES 23 |
| 33 | |
| 34 | namespace gr { |
| 35 | namespace gsm { |
| 36 | |
| 37 | tch_f_decoder::sptr |
| 38 | tch_f_decoder::make(tch_mode mode, const std::string &file) |
| 39 | { |
| 40 | return gnuradio::get_initial_sptr |
| 41 | (new tch_f_decoder_impl(mode, file)); |
| 42 | } |
| 43 | |
| 44 | /* |
| 45 | * Constructor |
| 46 | */ |
| 47 | tch_f_decoder_impl::tch_f_decoder_impl(tch_mode mode, const std::string &file) |
| 48 | : gr::block("tch_f_decoder", |
| 49 | gr::io_signature::make(0, 0, 0), |
| 50 | gr::io_signature::make(0, 0, 0)), |
| 51 | d_tch_mode(mode), |
| 52 | d_collected_bursts_num(0) |
| 53 | { |
| 54 | d_speech_file = fopen( file.c_str(), "wb" ); |
| 55 | if (d_speech_file == NULL) |
| 56 | { |
| 57 | throw std::runtime_error("TCH/F Decoder: can't open file\n"); |
| 58 | } |
| 59 | |
Roman Khassraf | 091a80f | 2015-05-22 10:39:16 +0200 | [diff] [blame^] | 60 | if (d_tch_mode == MODE_SPEECH_EFR) |
| 61 | { |
| 62 | fwrite(amr_nb_magic, 1, 6, d_speech_file); |
| 63 | } |
| 64 | |
Roman Khassraf | 059bab9 | 2015-05-20 12:49:46 +0200 | [diff] [blame] | 65 | int j, k, B; |
| 66 | for (k = 0; k < CONV_SIZE; k++) |
| 67 | { |
| 68 | B = k % 8; |
| 69 | j = 2 * ((49 * k) % 57) + ((k % 8) / 4); |
| 70 | interleave_trans[k] = B * 114 + j; |
| 71 | } |
| 72 | |
| 73 | //setup input/output ports |
| 74 | message_port_register_in(pmt::mp("bursts")); |
| 75 | set_msg_handler(pmt::mp("bursts"), boost::bind(&tch_f_decoder_impl::decode, this, _1)); |
| 76 | message_port_register_out(pmt::mp("msgs")); |
| 77 | } |
| 78 | |
| 79 | tch_f_decoder_impl::~tch_f_decoder_impl() |
| 80 | { |
| 81 | } |
| 82 | |
| 83 | void tch_f_decoder_impl::decode(pmt::pmt_t msg) |
| 84 | { |
| 85 | d_bursts[d_collected_bursts_num] = msg; |
| 86 | d_collected_bursts_num++; |
| 87 | |
| 88 | bool stolen = false; |
| 89 | |
| 90 | if (d_collected_bursts_num == 8) |
| 91 | { |
| 92 | unsigned char iBLOCK[2*BLOCKS*iBLOCK_SIZE]; |
| 93 | SoftVector mC(CONV_SIZE); |
| 94 | SoftVector mClass1_c(mC.head(378)); |
| 95 | SoftVector mClass2_c(mC.segment(378, 78)); |
| 96 | BitVector mTCHU(189); |
| 97 | BitVector mTCHD(260); |
| 98 | BitVector mClass1A_d(mTCHD.head(50)); |
| 99 | ViterbiR2O4 mVCoder; |
| 100 | |
| 101 | d_collected_bursts_num = 0; |
| 102 | |
| 103 | // reorganize data |
| 104 | for (int ii = 0; ii < 8; ii++) |
| 105 | { |
| 106 | pmt::pmt_t header_plus_burst = pmt::cdr(d_bursts[ii]); |
| 107 | int8_t * burst_bits = (int8_t *)(pmt::blob_data(header_plus_burst))+sizeof(gsmtap_hdr); |
| 108 | |
| 109 | for (int jj = 0; jj < 57; jj++) |
| 110 | { |
| 111 | iBLOCK[ii*114+jj] = burst_bits[jj + 3]; |
| 112 | iBLOCK[ii*114+jj+57] = burst_bits[jj + 88]; //88 = 3+57+1+26+1 |
| 113 | } |
| 114 | |
| 115 | if ((ii <= 3 && static_cast<int>(burst_bits[87]) == 1) || (ii >= 4 && static_cast<int>(burst_bits[60]) == 1)) |
| 116 | { |
| 117 | stolen = true; |
| 118 | } |
| 119 | } |
| 120 | |
| 121 | // deinterleave |
| 122 | for (int k = 0; k < CONV_SIZE; k++) |
| 123 | { |
| 124 | mC[k] = iBLOCK[interleave_trans[k]]; |
| 125 | } |
| 126 | |
| 127 | // Decode stolen frames as FACCH/F |
| 128 | if (stolen) |
| 129 | { |
| 130 | BitVector mU(228); |
| 131 | BitVector mP(mU.segment(184,40)); |
| 132 | BitVector mD(mU.head(184)); |
| 133 | BitVector mDP(mU.head(224)); |
| 134 | Parity mBlockCoder(0x10004820009ULL, 40, 224); |
| 135 | |
| 136 | mC.decode(mVCoder, mU); |
| 137 | mP.invert(); |
| 138 | |
| 139 | unsigned syndrome = mBlockCoder.syndrome(mDP); |
| 140 | |
| 141 | if (syndrome == 0) |
| 142 | { |
| 143 | unsigned char outmsg[27]; |
| 144 | unsigned char sbuf_len=224; |
| 145 | int i, j, c, pos=0; |
| 146 | for(i = 0; i < sbuf_len; i += 8) { |
| 147 | for(j = 0, c = 0; (j < 8) && (i + j < sbuf_len); j++){ |
| 148 | c |= (!!mU.bit(i + j)) << j; |
| 149 | } |
| 150 | outmsg[pos++] = c & 0xff; |
| 151 | } |
| 152 | |
| 153 | pmt::pmt_t first_header_plus_burst = pmt::cdr(d_bursts[0]); |
| 154 | gsmtap_hdr * header = (gsmtap_hdr *)pmt::blob_data(first_header_plus_burst); |
| 155 | header->type = GSMTAP_TYPE_UM; |
| 156 | int8_t * header_content = (int8_t *)header; |
| 157 | int8_t header_plus_data[sizeof(gsmtap_hdr)+DATA_BYTES]; |
| 158 | memcpy(header_plus_data, header_content, sizeof(gsmtap_hdr)); |
| 159 | memcpy(header_plus_data+sizeof(gsmtap_hdr), outmsg, DATA_BYTES); |
| 160 | |
| 161 | pmt::pmt_t msg_binary_blob = pmt::make_blob(header_plus_data,DATA_BYTES+sizeof(gsmtap_hdr)); |
| 162 | pmt::pmt_t msg_out = pmt::cons(pmt::PMT_NIL, msg_binary_blob); |
| 163 | |
| 164 | message_port_pub(pmt::mp("msgs"), msg_out); |
| 165 | } |
| 166 | } |
| 167 | |
| 168 | mClass1_c.decode(mVCoder, mTCHU); |
| 169 | mClass2_c.sliced().copyToSegment(mTCHD, 182); |
| 170 | |
| 171 | // 3.1.2.1 |
| 172 | // copy class 1 bits u[] to d[] |
| 173 | for (unsigned k = 0; k <= 90; k++) { |
| 174 | mTCHD[2*k] = mTCHU[k]; |
| 175 | mTCHD[2*k+1] = mTCHU[184-k]; |
| 176 | } |
| 177 | |
| 178 | Parity mTCHParity(0x0b, 3, 50); |
| 179 | |
| 180 | // 3.1.2.1 |
| 181 | // check parity of class 1A |
| 182 | unsigned sentParity = (~mTCHU.peekField(91, 3)) & 0x07; |
| 183 | //unsigned calcParity = mTCHD.head(50).parity(mTCHParity) & 0x07; |
| 184 | unsigned calcParity = mClass1A_d.parity(mTCHParity) & 0x07; |
| 185 | |
| 186 | // 3.1.2.2 |
| 187 | // Check the tail bits, too. |
| 188 | unsigned tail = mTCHU.peekField(185, 4); |
| 189 | |
| 190 | bool good = (sentParity == calcParity) && (tail == 0); |
| 191 | |
| 192 | if (good) |
| 193 | { |
| 194 | unsigned char mTCHFrame[33]; |
| 195 | unsigned int mTCHFrameLength; |
| 196 | |
| 197 | if (d_tch_mode == MODE_SPEECH_FR) // GSM-FR |
| 198 | { |
| 199 | // Undo Um's importance-sorted bit ordering. |
| 200 | // See GSM 05.03 3.1 and Tablee 2. |
| 201 | VocoderFrame mVFrame; |
| 202 | |
| 203 | BitVector payload = mVFrame.payload(); |
| 204 | mTCHD.unmap(GSM::g610BitOrder, 260, payload); |
| 205 | mVFrame.pack(mTCHFrame); |
| 206 | mTCHFrameLength = 33; |
| 207 | } |
| 208 | else if (d_tch_mode == MODE_SPEECH_EFR) // GSM-EFR / AMR 12.2 |
| 209 | { |
| 210 | VocoderAMRFrame mVFrameAMR; |
| 211 | |
| 212 | BitVector payload = mVFrameAMR.payload(); |
| 213 | BitVector TCHW(260), EFRBits(244); |
| 214 | |
| 215 | // Undo Um's EFR bit ordering. |
| 216 | mTCHD.unmap(GSM::g660BitOrder, 260, TCHW); |
| 217 | |
| 218 | // Remove repeating bits and CRC to get raw EFR frame (244 bits) |
| 219 | for (unsigned k=0; k<71; k++) |
| 220 | EFRBits[k] = TCHW[k] & 1; |
| 221 | |
| 222 | for (unsigned k=73; k<123; k++) |
| 223 | EFRBits[k-2] = TCHW[k] & 1; |
| 224 | |
| 225 | for (unsigned k=125; k<178; k++) |
| 226 | EFRBits[k-4] = TCHW[k] & 1; |
| 227 | |
| 228 | for (unsigned k=180; k<230; k++) |
| 229 | EFRBits[k-6] = TCHW[k] & 1; |
| 230 | |
| 231 | for (unsigned k=232; k<252; k++) |
| 232 | EFRBits[k-8] = TCHW[k] & 1; |
| 233 | |
| 234 | // Map bits as AMR 12.2k |
| 235 | EFRBits.map(GSM::g690_12_2_BitOrder, 244, payload); |
| 236 | |
| 237 | // Put the whole frame (hdr + payload) |
| 238 | mVFrameAMR.pack(mTCHFrame); |
| 239 | mTCHFrameLength = 32; |
| 240 | } |
| 241 | fwrite(mTCHFrame, 1 , mTCHFrameLength, d_speech_file); |
| 242 | } |
| 243 | } |
| 244 | } |
| 245 | } /* namespace gsm */ |
| 246 | } /* namespace gr */ |
| 247 | |