diff --git a/lib/decoding/tch_f_decoder_impl.cc b/lib/decoding/tch_f_decoder_impl.cc
index 41e5f53..a84c47c 100644
--- a/lib/decoding/tch_f_decoder_impl.cc
+++ b/lib/decoding/tch_f_decoder_impl.cc
@@ -29,11 +29,34 @@
 #include "stdio.h"
 #include "tch_f_decoder_impl.h"
 
+extern "C" {
+    #include <osmocom/coding/gsm0503_coding.h>
+}
+
 #define DATA_BYTES 23
 
 namespace gr {
   namespace gsm {
 
+    static int ubits2sbits(ubit_t *ubits, sbit_t *sbits, int count)
+    {
+	    int i;
+
+	    for (i = 0; i < count; i++) {
+		    if (*ubits == 0x23) {
+			    ubits++;
+			    sbits++;
+			    continue;
+		    }
+		    if ((*ubits++) & 1)
+			    *sbits++ = -127;
+		    else
+			    *sbits++ = 127;
+	    }
+
+	    return count;
+    }
+
     tch_f_decoder::sptr
     tch_f_decoder::make(tch_mode mode, bool boundary_check)
     {
@@ -107,6 +130,7 @@
 
         if (d_collected_bursts_num == 8)
         {
+        	ubit_t bursts_u[116 * 8];
             d_collected_bursts_num = 0;
 
             // reorganize data
@@ -115,6 +139,9 @@
                 pmt::pmt_t header_plus_burst = pmt::cdr(d_bursts[ii]);
                 int8_t * burst_bits = (int8_t *)(pmt::blob_data(header_plus_burst))+sizeof(gsmtap_hdr);
 
+                memcpy(&bursts_u[ii*116], &burst_bits[3],58);
+                memcpy(&bursts_u[ii*116+58], &burst_bits[3+57+1+26],58);
+
                 for (int jj = 0; jj < 57; jj++)
                 {
                     iBLOCK[ii*114+jj] = burst_bits[jj + 3];
@@ -269,65 +296,26 @@
                 unsigned tail = mTCHU.peekField(185, 4);
                 bool good = (sentParity == calcParity) && (tail == 0);
 
+                
                 if (good)
                 {
-                    unsigned char frameBuffer[33];
+                    uint8_t frameBuffer[33];
+                	sbit_t bursts_s[116 * 8];
+	                int n_errors, n_bits_total;
                     unsigned int  mTCHFrameLength;
 
+                	ubits2sbits(bursts_u, bursts_s, 116 * 8);                    
+
                     if (d_tch_mode == TCH_FS) // GSM-FR
                     {
-                        unsigned char mFrameHeader = 0x0d;
                         mTCHFrameLength = 33;
-
-                        // Undo Um's importance-sorted bit ordering.
-                        // See GSM 05.03 3.1 and Table 2.
-                        BitVector frFrame(260 + 4); // FR has a frameheader of 4 bits only
-                        frFrame.fillField(0, mFrameHeader, 4);
-                        BitVector payload = frFrame.tail(4);
-
-                        mTCHD.unmap(GSM::g610BitOrder, 260, payload);
-                        frFrame.pack(frameBuffer);
-
+                        gsm0503_tch_fr_decode(frameBuffer, bursts_s, 1, 0, &n_errors, &n_bits_total);
+                        std::cout << "Errors: " << n_errors << std::endl;
                     }
                     else if (d_tch_mode == TCH_EFR) // GSM-EFR
                     {
-                        unsigned char mFrameHeader = 0x3c;
-
-                        // AMR Frame, consisting of a 8 bit frame header, plus the payload from decoding
-                        BitVector amrFrame(244 + 8); // Same output length as AMR 12.2
-                        BitVector payload = amrFrame.tail(8);
-
-                        BitVector TCHW(260), EFRBits(244);
-
-                        // write frame header
-                        amrFrame.fillField(0, mFrameHeader, 8);
-
-                        // Undo Um's EFR bit ordering.
-                        mTCHD.unmap(GSM::g660BitOrder, 260, TCHW);
-
-                        // Remove repeating bits and CRC to get raw EFR frame (244 bits)
-                        for (unsigned k=0; k<71; k++)
-                          EFRBits[k] = TCHW[k] & 1;
-
-                        for (unsigned k=73; k<123; k++)
-                          EFRBits[k-2] = TCHW[k] & 1;
-
-                        for (unsigned k=125; k<178; k++)
-                          EFRBits[k-4] = TCHW[k] & 1;
-
-                        for (unsigned k=180; k<230; k++)
-                          EFRBits[k-6] = TCHW[k] & 1;
-
-                        for (unsigned k=232; k<252; k++)
-                          EFRBits[k-8] = TCHW[k] & 1;
-
-                        // Map bits as AMR 12.2k
-                        EFRBits.map(GSM::gAMRBitOrderTCH_AFS12_2, 244, payload);
-
-                        // Put the whole frame (hdr + payload)
                         mTCHFrameLength = 32;
-                        amrFrame.pack(frameBuffer);
-
+                        gsm0503_tch_fr_decode(frameBuffer, bursts_s, 1, 1, &n_errors, &n_bits_total);
                     }
                     message_port_pub(pmt::mp("voice"), pmt::cons(pmt::PMT_NIL, pmt::make_blob(frameBuffer,mTCHFrameLength)));
                 }