diff --git a/CRC.cpp b/CRC.cpp
index 99e82b2..c99ca6c 100644
--- a/CRC.cpp
+++ b/CRC.cpp
@@ -238,23 +238,3 @@ unsigned char CCRC::crc8(const unsigned char *in, unsigned int length)
 
 	return crc;
 }
-
-bool CCRC::crcFICH(const unsigned char* fich)
-{
-	assert(fich != NULL);
-
-	union {
-		uint16_t crc16;
-		uint8_t  crc8[2U];
-	};
-
-	crc16 = 0U;
-	crc16 = (uint16_t(crc8[0U]) << 8) ^ CCITT16_TABLE2[crc8[1U] ^ fich[0U]];
-	crc16 = (uint16_t(crc8[0U]) << 8) ^ CCITT16_TABLE2[crc8[1U] ^ fich[1U]];
-	crc16 = (uint16_t(crc8[0U]) << 8) ^ CCITT16_TABLE2[crc8[1U] ^ fich[2U]];
-	crc16 = (uint16_t(crc8[0U]) << 8) ^ CCITT16_TABLE2[crc8[1U] ^ fich[3U]];
-
-	crc16 = ~crc16;
-
-	return crc8[0U] == fich[5U] && crc8[1U] == fich[4U];
-}
diff --git a/CRC.h b/CRC.h
index b54240a..e80c3f6 100644
--- a/CRC.h
+++ b/CRC.h
@@ -32,8 +32,6 @@ public:
 	static bool checkCCITT162(const unsigned char* in, unsigned int length);
 
 	static unsigned char crc8(const unsigned char* in, unsigned int length);
-
-	static bool crcFICH(const unsigned char* fich);
 };
 
 #endif
diff --git a/YSFControl.cpp b/YSFControl.cpp
index d2ab7dc..edee01a 100644
--- a/YSFControl.cpp
+++ b/YSFControl.cpp
@@ -12,6 +12,7 @@
  */
 
 #include "YSFControl.h"
+#include "YSFFICH.h"
 #include "Utils.h"
 #include "Sync.h"
 #include "Log.h"
@@ -24,7 +25,6 @@
 /*
  * TODO:
  * AMBE FEC reconstruction.
- * FICH regeneration.
  * Callsign extraction + late entry.
  * Uplink and downlink callsign addition.
  */
@@ -56,6 +56,10 @@ bool CYSFControl::writeModem(unsigned char *data)
 
 	if (type == TAG_LOST && m_state == RS_RELAYING_RF_AUDIO) {
 		LogMessage("YSF, transmission lost, %.1f seconds", float(m_frames) / 10.0F);
+
+		if (m_parrot != NULL)
+			m_parrot->end();
+
 		writeEndOfTransmission();
 		return false;
 	}
@@ -84,15 +88,24 @@ bool CYSFControl::writeModem(unsigned char *data)
 	if (type == TAG_EOT) {
 		CSync::addYSFSync(data + 2U);
 
+		CYSFFICH fich;
+		fich.decode(data + 2U);
+
 		m_frames++;
 
 		if (m_duplex) {
+			fich.setMR(YSF_MR_BUSY);
+			fich.encode(data + 2U);
+
 			data[0U] = TAG_EOT;
 			data[1U] = 0x00U;
 			writeQueue(data);
 		}
 
 		if (m_parrot != NULL) {
+			fich.setMR(YSF_MR_NOT_BUSY);
+			fich.encode(data + 2U);
+
 			data[0U] = TAG_EOT;
 			data[1U] = 0x00U;
 			writeParrot(data);
@@ -109,15 +122,24 @@ bool CYSFControl::writeModem(unsigned char *data)
 	} else {
 		CSync::addYSFSync(data + 2U);
 
+		CYSFFICH fich;
+		fich.decode(data + 2U);
+
 		m_frames++;
 
 		if (m_duplex) {
+			fich.setMR(YSF_MR_BUSY);
+			fich.encode(data + 2U);
+
 			data[0U] = TAG_DATA;
 			data[1U] = 0x00U;
 			writeQueue(data);
 		}
 
 		if (m_parrot != NULL) {
+			fich.setMR(YSF_MR_NOT_BUSY);
+			fich.encode(data + 2U);
+
 			data[0U] = TAG_DATA;
 			data[1U] = 0x00U;
 			writeParrot(data);
@@ -196,6 +218,9 @@ void CYSFControl::writeParrot(const unsigned char *data)
 		return;
 
 	m_parrot->write(data);
+
+	if (data[0U] == TAG_EOT)
+		m_parrot->end();
 }
 
 bool CYSFControl::openFile()
diff --git a/YSFConvolution.cpp b/YSFConvolution.cpp
index 6633ee3..37e55dd 100644
--- a/YSFConvolution.cpp
+++ b/YSFConvolution.cpp
@@ -29,8 +29,8 @@ const unsigned char BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U
 #define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7])
 #define READ_BIT1(p,i)    (p[(i)>>3] & BIT_MASK_TABLE[(i)&7])
 
-const unsigned char BRANCH_TABLE1[] = {0U, 0U, 0U, 0U, 1U, 1U, 1U, 1U};
-const unsigned char BRANCH_TABLE2[] = {0U, 1U, 1U, 0U, 0U, 1U, 1U, 0U};
+const uint8_t BRANCH_TABLE1[] = {0U, 0U, 0U, 0U, 1U, 1U, 1U, 1U};
+const uint8_t BRANCH_TABLE2[] = {0U, 1U, 1U, 0U, 0U, 1U, 1U, 0U};
 
 CYSFConvolution::CYSFConvolution() :
 m_metrics1(NULL),
@@ -40,9 +40,9 @@ m_newMetrics(NULL),
 m_decisions(NULL),
 m_dp(NULL)
 {
-	m_metrics1  = new unsigned short[16U];
-	m_metrics2  = new unsigned short[16U];
-	m_decisions = new unsigned long long[100U];
+	m_metrics1  = new uint16_t[16U];
+	m_metrics2  = new uint16_t[16U];
+	m_decisions = new uint64_t[100U];
 }
 
 CYSFConvolution::~CYSFConvolution()
@@ -54,44 +54,44 @@ CYSFConvolution::~CYSFConvolution()
 
 const unsigned int NUM_OF_STATES_D2 = 8U;
 const unsigned int NUM_OF_STATES = 16U;
-const unsigned int M = 3U;
+const uint32_t     M = 3U;
 const unsigned int K = 5U;
 
 void CYSFConvolution::start()
 {
-	::memset(m_metrics1, 0x00U, NUM_OF_STATES * sizeof(unsigned short));
-	::memset(m_metrics2, 0x00U, NUM_OF_STATES * sizeof(unsigned short));
+	::memset(m_metrics1, 0x00U, NUM_OF_STATES * sizeof(uint16_t));
+	::memset(m_metrics2, 0x00U, NUM_OF_STATES * sizeof(uint16_t));
 
 	m_oldMetrics = m_metrics1;
 	m_newMetrics = m_metrics2;
 	m_dp = m_decisions;
 }
 
-void CYSFConvolution::decode(unsigned char s0, unsigned char s1)
+void CYSFConvolution::decode(uint8_t s0, uint8_t s1)
 {
   *m_dp = 0U;
 
-  for (unsigned int i = 0U; i < NUM_OF_STATES_D2; i++) {
-    unsigned int j = i * 2U;
+  for (uint8_t i = 0U; i < NUM_OF_STATES_D2; i++) {
+    uint8_t j = i * 2U;
 
-    unsigned short metric = (BRANCH_TABLE1[i] ^ s0) + (BRANCH_TABLE2[i] ^ s1);
+    uint16_t metric = (BRANCH_TABLE1[i] ^ s0) + (BRANCH_TABLE2[i] ^ s1);
 
-    unsigned short m0 = m_oldMetrics[i] + metric;
-    unsigned short m1 = m_oldMetrics[i + NUM_OF_STATES_D2] + (M - metric);
-    unsigned char  decision0 = (m0 >= m1) ? 1U : 0U;
+    uint16_t m0 = m_oldMetrics[i] + metric;
+    uint16_t m1 = m_oldMetrics[i + NUM_OF_STATES_D2] + (M - metric);
+    uint8_t decision0 = (m0 >= m1) ? 1U : 0U;
     m_newMetrics[j + 0U] = decision0 != 0U ? m1 : m0;
 
     m0 = m_oldMetrics[i] + (M - metric);
     m1 = m_oldMetrics[i + NUM_OF_STATES_D2] + metric;
-    unsigned char decision1 = (m0 >= m1) ? 1U : 0U;
+    uint8_t decision1 = (m0 >= m1) ? 1U : 0U;
     m_newMetrics[j + 1U] = decision1 != 0U ? m1 : m0;
 
-    *m_dp |= ((unsigned long long)(decision1) << (j + 1U)) | ((unsigned long long)(decision0) << (j + 0U));
+    *m_dp |= (uint64_t(decision1) << (j + 1U)) | (uint64_t(decision0) << (j + 0U));
   }
 
   ++m_dp;
 
-  unsigned short* tmp = m_oldMetrics;
+  uint16_t* tmp = m_oldMetrics;
   m_oldMetrics = m_newMetrics;
   m_newMetrics = tmp;
 }
@@ -100,16 +100,43 @@ void CYSFConvolution::chainback(unsigned char* out)
 {
 	assert(out != NULL);
 
-	unsigned int state = 0U;
+	uint32_t state = 0U;
 
-	unsigned char nbits = 96U;
+	uint8_t nbits = 96U;
 	while (nbits-- > 0) {
 		--m_dp;
 
-		unsigned int  i = state >> (9 - K);
-		unsigned char bit = (unsigned char)(*m_dp >> i) & 1;
+		uint32_t  i = state >> (9 - K);
+		uint8_t bit = uint8_t(*m_dp >> i) & 1;
 		state = (bit << 7) | (state >> 1);
 
 		WRITE_BIT1(out, nbits, bit != 0U);
 	}
 }
+
+void CYSFConvolution::encode(const unsigned char* in, unsigned char* out, unsigned int nBits) const
+{
+	assert(in != NULL);
+	assert(out != NULL);
+	assert(nBits > 0U);
+
+	uint8_t d1 = 0U, d2 = 0U, d3 = 0U, d4 = 0U;
+	uint32_t k = 0U;
+	for (unsigned int i = 0U; i < nBits; i++) {
+		uint8_t d = READ_BIT1(in, i) ? 1U : 0U;
+
+		uint8_t g1 = (d + d3 + d4) & 1;
+		uint8_t g2 = (d + d1 + d2 + d4) & 1;
+
+		d4 = d3;
+		d3 = d2;
+		d2 = d1;
+		d1 = d;
+
+		WRITE_BIT1(out, k, g1 != 0U);
+		k++;
+
+		WRITE_BIT1(out, k, g2 != 0U);
+		k++;
+	}
+}
diff --git a/YSFConvolution.h b/YSFConvolution.h
index 4b805b2..945257e 100644
--- a/YSFConvolution.h
+++ b/YSFConvolution.h
@@ -21,22 +21,26 @@
 
 #include "YSFConvolution.h"
 
+#include <cstdint>
+
 class CYSFConvolution {
 public:
 	CYSFConvolution();
 	~CYSFConvolution();
 
 	void start();
-	void decode(unsigned char s0, unsigned char s1);
+	void decode(uint8_t s0, uint8_t s1);
 	void chainback(unsigned char* out);
 
+	void encode(const unsigned char* in, unsigned char* out, unsigned int nBits) const;
+
 private:
-	unsigned short*     m_metrics1;
-	unsigned short*     m_metrics2;
-	unsigned short*     m_oldMetrics;
-	unsigned short*     m_newMetrics;
-	unsigned long long* m_decisions;
-	unsigned long long* m_dp;
+	uint16_t* m_metrics1;
+	uint16_t* m_metrics2;
+	uint16_t* m_oldMetrics;
+	uint16_t* m_newMetrics;
+	uint64_t* m_decisions;
+	uint64_t* m_dp;
 };
 
 #endif
diff --git a/YSFDefines.h b/YSFDefines.h
index c9d4969..1c83fd2 100644
--- a/YSFDefines.h
+++ b/YSFDefines.h
@@ -32,4 +32,10 @@ const unsigned char YSF_DT_TERMINATOR_CHANNEL = 0x80U;
 
 const unsigned char YSF_CKSUM_OK = 0x01U;
 
+const unsigned char YSF_CM_GROUP      = 0x00U;
+const unsigned char YSF_CM_INDIVIDUAL = 0x0CU;
+
+const unsigned char YSF_MR_NOT_BUSY = 0x08U;
+const unsigned char YSF_MR_BUSY     = 0x10U;
+
 #endif
diff --git a/YSFFICH.cpp b/YSFFICH.cpp
index efc4fcf..474ff25 100644
--- a/YSFFICH.cpp
+++ b/YSFFICH.cpp
@@ -17,9 +17,11 @@
  */
 
 #include "YSFConvolution.h"
+#include "YSFDefines.h"
 #include "Golay24128.h"
 #include "YSFFICH.h"
 #include "CRC.h"
+#include "Log.h"
 
 #include <cstdio>
 #include <cassert>
@@ -30,7 +32,7 @@ const unsigned char BIT_MASK_TABLE[] = {0x80U, 0x40U, 0x20U, 0x10U, 0x08U, 0x04U
 #define WRITE_BIT1(p,i,b) p[(i)>>3] = (b) ? (p[(i)>>3] | BIT_MASK_TABLE[(i)&7]) : (p[(i)>>3] & ~BIT_MASK_TABLE[(i)&7])
 #define READ_BIT1(p,i)    (p[(i)>>3] & BIT_MASK_TABLE[(i)&7])
 
-const unsigned int INTERLEAVE_TABLE_RX[] = {
+const unsigned int INTERLEAVE_TABLE[] = {
    0U, 40U,  80U, 120U, 160U,
    2U, 42U,  82U, 122U, 162U,
    4U, 44U,  84U, 124U, 164U,
@@ -52,32 +54,37 @@ const unsigned int INTERLEAVE_TABLE_RX[] = {
   36U, 76U, 116U, 156U, 196U,
   38U, 78U, 118U, 158U, 198U};
 
-CYSFFICH::CYSFFICH()
+CYSFFICH::CYSFFICH() :
+m_fich(NULL)
 {
+	m_fich = new unsigned char[6U];
 }
 
 CYSFFICH::~CYSFFICH()
 {
+	delete[] m_fich;
 }
 
-bool CYSFFICH::decode(const unsigned char* data, unsigned char* fich) const
+bool CYSFFICH::decode(const unsigned char* bytes)
 {
-	assert(data != NULL);
-	assert(fich != NULL);
+	assert(bytes != NULL);
+
+	// Skip the sync bytes
+	bytes += YSF_SYNC_LENGTH_BYTES;
 
 	CYSFConvolution viterbi;
 	viterbi.start();
 
 	// Deinterleave the FICH and send bits to the Viterbi decoder
 	for (unsigned int i = 0U; i < 100U; i++) {
-		unsigned int n = INTERLEAVE_TABLE_RX[i];
-		unsigned int s0 = READ_BIT1(data, n) ? 1U : 0U;
+		unsigned int n = INTERLEAVE_TABLE[i];
+		uint8_t s0 = READ_BIT1(bytes, n) ? 1U : 0U;
 
 		n++;
-		unsigned int s1 = READ_BIT1(data, n) ? 1U : 0U;
+		uint8_t s1 = READ_BIT1(bytes, n) ? 1U : 0U;
 
 		viterbi.decode(s0, s1);
-  }
+	}
 
 	unsigned char output[13U];
 	viterbi.chainback(output);
@@ -87,12 +94,96 @@ bool CYSFFICH::decode(const unsigned char* data, unsigned char* fich) const
 	unsigned int b2 = CGolay24128::decode24128(output + 6U);
 	unsigned int b3 = CGolay24128::decode24128(output + 9U);
 
-	fich[0U] = (b0 >> 16) & 0xFFU;
-	fich[1U] = ((b0 >> 8) & 0xF0U) | ((b1 >> 20) & 0x0FU);
-	fich[2U] = (b1 >> 12) & 0xFFU;
-	fich[3U] = (b2 >> 16) & 0xFFU;
-	fich[4U] = ((b2 >> 8) & 0xF0U) | ((b3 >> 20) & 0x0FU);
-	fich[5U] = (b3 >> 12) & 0xFFU;
+	m_fich[0U] = (b0 >> 4) & 0xFFU;
+	m_fich[1U] = ((b0 << 4) & 0xF0U) | ((b1 >> 8) & 0x0FU);
+	m_fich[2U] = (b1 >> 0) & 0xFFU;
+	m_fich[3U] = (b2 >> 4) & 0xFFU;
+	m_fich[4U] = ((b2 << 4) & 0xF0U) | ((b3 >> 8) & 0x0FU);
+	m_fich[5U] = (b3 >> 0) & 0xFFU;
 
-	return CCRC::crcFICH(fich);
+	return CCRC::checkCCITT162(m_fich, 6U);
+}
+
+void CYSFFICH::encode(unsigned char* bytes)
+{
+	assert(bytes != NULL);
+
+	// Skip the sync bytes
+	bytes += YSF_SYNC_LENGTH_BYTES;
+
+	CCRC::addCCITT162(m_fich, 6U);
+
+	unsigned int b0 = ((m_fich[0U] << 4) & 0xFF0U) | ((m_fich[1U] >> 4) & 0x00FU);
+	unsigned int b1 = ((m_fich[1U] << 8) & 0xF00U) | ((m_fich[2U] >> 0) & 0x0FFU);
+	unsigned int b2 = ((m_fich[3U] << 4) & 0xFF0U) | ((m_fich[4U] >> 4) & 0x00FU);
+	unsigned int b3 = ((m_fich[4U] << 8) & 0xF00U) | ((m_fich[5U] >> 0) & 0x0FFU);
+
+	unsigned int c0 = CGolay24128::encode24128(b0);
+	unsigned int c1 = CGolay24128::encode24128(b1);
+	unsigned int c2 = CGolay24128::encode24128(b2);
+	unsigned int c3 = CGolay24128::encode24128(b3);
+
+	unsigned char conv[13U];
+	conv[0U]  = (c0 >> 16) & 0xFFU;
+	conv[1U]  = (c0 >> 8) & 0xFFU;
+	conv[2U]  = (c0 >> 0) & 0xFFU;
+	conv[3U]  = (c1 >> 16) & 0xFFU;
+	conv[4U]  = (c1 >> 8) & 0xFFU;
+	conv[5U]  = (c1 >> 0) & 0xFFU;
+	conv[6U]  = (c2 >> 16) & 0xFFU;
+	conv[7U]  = (c2 >> 8) & 0xFFU;
+	conv[8U]  = (c2 >> 0) & 0xFFU;
+	conv[9U]  = (c3 >> 16) & 0xFFU;
+	conv[10U] = (c3 >> 8) & 0xFFU;
+	conv[11U] = (c3 >> 0) & 0xFFU;
+	conv[12U] = 0x00U;
+
+	CYSFConvolution convolution;
+	unsigned char convolved[25U];
+	convolution.encode(conv, convolved, 100U);
+
+	unsigned int j = 0U;
+	for (unsigned int i = 0U; i < 100U; i++) {
+		unsigned int n = INTERLEAVE_TABLE[i];
+
+		bool s0 = READ_BIT1(convolved, j) != 0U;
+		j++;
+
+		bool s1 = READ_BIT1(convolved, j) != 0U;
+		j++;
+
+		WRITE_BIT1(bytes, n, s0);
+
+		n++;
+		WRITE_BIT1(bytes, n, s1);
+	}
+}
+
+unsigned char CYSFFICH::getCM() const
+{
+	return m_fich[0U] & 0x0CU;
+}
+
+unsigned char CYSFFICH::getFT() const
+{
+	return m_fich[1U] & 0x07U;
+}
+
+unsigned char CYSFFICH::getFN() const
+{
+	return (m_fich[1U] & 0x38U) >> 3;
+}
+
+void CYSFFICH::setMR(unsigned char mr)
+{
+	m_fich[2U] &= 0xC7U;
+	m_fich[2U] |= mr;
+}
+
+void CYSFFICH::setVoIP(bool on)
+{
+	if (on)
+		m_fich[2U] |= 0x04U;
+	else
+		m_fich[2U] &= 0xFBU;
 }
diff --git a/YSFFICH.h b/YSFFICH.h
index 28e50a0..2471ef1 100644
--- a/YSFFICH.h
+++ b/YSFFICH.h
@@ -24,9 +24,19 @@ public:
 	CYSFFICH();
 	~CYSFFICH();
 
-	bool decode(const unsigned char* frame, unsigned char* fich) const;
+	bool decode(const unsigned char* bytes);
+
+	void encode(unsigned char* bytes);
+
+	unsigned char getCM() const;
+	unsigned char getFT() const;
+	unsigned char getFN() const;
+
+	void setMR(unsigned char mr);
+	void setVoIP(bool set);
 
 private:
+	unsigned char* m_fich;
 };
 
 #endif
diff --git a/YSFParrot.cpp b/YSFParrot.cpp
index 582d66a..c8729ee 100644
--- a/YSFParrot.cpp
+++ b/YSFParrot.cpp
@@ -51,14 +51,16 @@ bool CYSFParrot::write(const unsigned char* data)
 	::memcpy(m_data + m_used, data, YSF_FRAME_LENGTH_BYTES + 2U);
 	m_used += YSF_FRAME_LENGTH_BYTES + 2U;
 
-	if (data[0U] == TAG_EOT) {
-		m_timer.start();
-		m_ptr = 0U;
-	}
-
 	return true;
 }
 
+void CYSFParrot::end()
+{
+	m_timer.start();
+
+	m_ptr = 0U;
+}
+
 void CYSFParrot::read(unsigned char* data)
 {
 	assert(data != NULL);
diff --git a/YSFParrot.h b/YSFParrot.h
index fd79460..a21b96d 100644
--- a/YSFParrot.h
+++ b/YSFParrot.h
@@ -31,6 +31,8 @@ public:
 
 	void read(unsigned char* data);
 
+	void end();
+
 	bool hasData();
 
 	void clock(unsigned int ms);