diff --git a/DMRDMOTX.cpp b/DMRDMOTX.cpp index 3ba6830..63bc78f 100644 --- a/DMRDMOTX.cpp +++ b/DMRDMOTX.cpp @@ -104,8 +104,22 @@ uint8_t CDMRDMOTX::writeData(const uint8_t* data, uint8_t length) if (space < DMR_FRAME_LENGTH_BYTES) return 5U; + uint8_t buffer[DMR_FRAME_LENGTH_BYTES]; + ::memcpy(buffer, data + 1U, DMR_FRAME_LENGTH_BYTES); + + // Swap the sync bytes from BS to MS + if (::memcmp(buffer + 14U, DMR_BS_DATA_SYNC_BYTES + 1U, 5U) == 0) { + ::memcpy(buffer + 14U, DMR_MS_DATA_SYNC_BYTES + 1U, 5U); + buffer[13U] &= 0xF0U; buffer[13U] |= DMR_MS_DATA_SYNC_BYTES[0U]; + buffer[19U] &= 0x0FU; buffer[19U] |= DMR_MS_DATA_SYNC_BYTES[6U]; + } else if (::memcmp(buffer + 14U, DMR_BS_VOICE_SYNC_BYTES + 1U, 5U) == 0) { + ::memcpy(buffer + 14U, DMR_MS_VOICE_SYNC_BYTES + 1U, 5U); + buffer[13U] &= 0xF0U; buffer[13U] |= DMR_MS_VOICE_SYNC_BYTES[0U]; + buffer[19U] &= 0x0FU; buffer[19U] |= DMR_MS_VOICE_SYNC_BYTES[6U]; + } + for (uint8_t i = 0U; i < DMR_FRAME_LENGTH_BYTES; i++) - m_fifo.put(data[i + 1U]); + m_fifo.put(buffer[i]); return 0U; }