diff --git a/Controller.cpp b/Controller.cpp index 5984dce..e3b17f1 100644 --- a/Controller.cpp +++ b/Controller.cpp @@ -22,7 +22,7 @@ bool CController::Start() { - if (InitDevices() || reader.Open("urfd2tcd")) + if (InitDevices() || reader.Open(REF2TC)) { keep_running = false; return true; @@ -90,11 +90,6 @@ bool CController::InitDevices() type = (type == Encoding::dstar) ? Encoding::dmr : Encoding::dstar; } - dmr_audio_block.resize(dmr_vocoder_count); - dmr_packet_queue.resize(dmr_vocoder_count); - dstar_audio_block.resize(dstar_vocoder_count); - dstar_packet_queue.resize(dstar_vocoder_count); - return false; } @@ -125,120 +120,215 @@ void CController::IncrementDStarVocoder() void CController::ReadReflector() { + // local audio storage + static int16_t audio[160]; + while (keep_running) { STCPacket tcpack; //wait up to 40 ms to read something on the unix port if (reader.Receive(&tcpack, 40)) { //create a std::shared_ptr to a new packet - auto packet = std::unique_ptr(new CTranscoderPacket(tcpack)); + auto packet = std::make_shared(tcpack); switch (packet->GetCodecIn()) { case ECodecType::dstar: + unsigned int devnum = current_dstar_vocoder / 3; //send it to the next available dstar vocoder - dstar_device[current_dstar_vocoder/3]->SendData(current_dstar_vocoder%3, packet->GetDStarData()); + dstar_device[devnum]->SendData(current_dstar_vocoder%3, packet->GetDStarData()); //push the packet onto that vocoder's queue - dstar_packet_queue[current_dstar_vocoder].push(packet); + dstar_device[devnum]->packet_queue.push(packet); //increment the dstar vocoder index IncrementDStarVocoder(); break; case ECodecType::dmr: + unsigned int devnum = current_dmr_vocoder / 3; //send it to the next avaiable dmr vocoder - dmr_device[current_dmr_vocoder/3]->SendData(current_dmr_vocoder%3, packet->GetDMRData()); + dmr_device[devnum]->SendData(current_dmr_vocoder%3, packet->GetDMRData()); //push the packet onto that vocoder's queue - dmr_packet_queue[current_dmr_vocoder].push(packet); + dmr_device[devnum]->packet_queue.push(packet); //increment the dmr vocoder index IncrementDMRVocoder(); break; case ECodecType::c2_1600: case ECodecType::c2_3200: if (packet->IsSecond()) { - if (packet->GetCodecIn() == ECodecType::c2_3200) { + if (packet->GetCodecIn() == ECodecType::c2_1600) { + //copy the audio from local storage + memcpy(packet->GetAudio(), audio, 320); + } else /* codec_in is ECodecType::c2_3200 */ { //decode the second 8 data bytes //move the 160 audio samples to the packet - } else /* the codec is C2_1600 */ { - //copy the audio from local storage + c2_32.codec2_decode(packet->GetAudio(), packet->GetM17Data()+8); } - // encode the audio to dstar - //create a 3003 audio packet - //save the dstar vocoder index in the packet - //push the packet onto the dstar vocoder's queue - //send the 3003 audio packet to the dstar vocoder specified in the packet - //increment the dstar vocoder index - // encode the audio to dmr - //save the dmr vocoder index in the packet - //push the packet onto the dmr vocoder's queue - //send the same 3003 audio packet to the dmr vocoder specified in the packet - //increment the dmr vocoder index } else /* it's a "first packet" */ { - //decode the first 8 bytes of data to get the up to 320 audio samples - //move the first 160 audio samples to the packet - // encode the audio to dstar - //create a 3003 audio packet - //save the dstar vocoder index in the packet - //push the packet onto the vocoder's queue - //send it to the dstar vocoder specified in the packet - //increment the dstar vocoder index - // encode the audio to dmr - //save the dmr vocoder index in the packet - //push the packet onto the vocoder's queue - //send the same audio packet to the dmr vocoder specified in the packet - //increment the dmr vocoder index - // save the second half of the audio if the m17 packet was c2_1600 if (packet->GetCodecIn() == ECodecType::c2_1600) { - //put the second 160 audio samples into local storage + //c2_1600 encodes 40 ms of audio, 320 points, so... + //we need some temprary audio storage: + int16_t tmp[320]; + //decode it + c2_16.codec2_decode(tmp, packet->GetM17Data()); // 8 bytes input produces 320 audio points + //move the first and second half + memcpy(packet->GetAudio(), tmp, 160*sizeof(int16_t)); + memcpy(audio, tmp+160, 160*sizeof(int16_t)); + } else /* codec_in is ECodecType::c2_3200 */ { + c2_32.codec2_decode(packet->GetAudio(), packet->GetM17Data()); } } + // encode the audio to dstar + unsigned int devnum = current_dstar_vocoder / 3; + //send the audio to the current dstar vocoder + dstar_device[devnum]->SendAudio(current_dstar_vocoder%3, packet->GetAudio()); + //push the packet onto the vocoder's queue + dstar_device[devnum]->packet_queue.push(packet); + //increment the dstar vocoder index + IncrementDStarVocoder(); + // encode the audio to dmr + devnum = current_dmr_vocoder / 3; + //send the audio to the corrent dmr vocoder + dmr_device[devnum]->SendAudio(current_dmr_vocoder%3, packet->GetAudio()); + //push the packet onto the dmr vocoder's queue + dmr_device[devnum]->packet_queue.push(packet); + //increment the dmr vocoder index + IncrementDMRVocoder(); } } } } +void CController::AddFDSet(int &max, int newfd, fd_set *set) const +{ + if (newfd > max) + max = newfd; + FD_SET(newfd, set); +} void CController::ReadAmbeDevices() { while (keep_running) { + int maxfd = -1; + fd_set FdSet; + FD_ZERO(&FdSet); + for (const auto &it : dstar_device) + { + AddFDSet(maxfd, it->GetFd(), &FdSet); + } + for (const auto &it : dmr_device) + { + AddFDSet(maxfd, it->GetFd(), &FdSet); + } + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 40000; + auto rval = select(maxfd, &FdSet, nullptr, nullptr, &tv); + if (rval < 1) + { + std::cerr << "select() ERROR reading AMBE devices: " << strerror(errno) << std::endl; + } //wait for up to 40 ms to read anthing from all devices - if (something is ready to be read) { + if (rval > 0) { // from the device file descriptor, we'll know if it's dstar or dmr - //save the dmr/dstar type - //read the response from the vocoder - //get the packet from either the dstar or dmr vocoder's queue - if (the response is audio) { - // if the response is audio, this means that packet.codec_in is an ambe codec - // so we need to encode the m17 - //encode the audio to c2_3200 - if (packet.IsSecond()) { - //move the c2_3200 data to the second half of the M17 packet - } else /* the packet is first */ { - //move the c2_3200 data do the first half of the M17 packet - if (packet->IsLast()) { - // we have an odd number of packets, so we have to do finish up the m17 packet - const uint8_t silence[] = {0x00, 0x01, 0x43, 0x09, 0xe4, 0x9c, 0x08, 0x21 }; - //put codec silence in the second half of the codec - } - } - //create a 3003 audio packet - //move the response audio to the packet - if (the dmr/dstar type == dstar) { - //send the audio packet to the next available dmr vocoder - //push the packet onto the dmr vocoder's queue - //increment the dmr vocoder index - } else /* the dmr/dstar type is dmr */ { - //send the audio packet to the next available dstar vocoder - //push the packet onto the dstar vocoder's queue - //increment the dstar vocoder index + for (unsigned int i=0 ; iGetFd(), &FdSet)) + { + ReadDevice(dstar_device[i], EAmbeType::dstar); } - } else /* the response is ambe */ { - if (the dmr/dstar type == dstar) { - //move the ambe to the packet.dstar - } else { - //move the ambe to the packet.dmr - } - if (packet.AllCodecsAreSet()) { - //open a socket to the reflector channel - //send the packet over the socket - //close the socket + } + for (unsigned int i=0 ; iGetFd(), &FdSet)) + { + ReadDevice(dmr_device[i], EAmbeType::dmr); } } } } } + +void CController::ReadDevice(std::shared_ptr device, EAmbeType type) +{ + //save the dmr/dstar type + const char *device_type = (EAmbeType::dstar==type) ? "D-Star" : "DMR"; + + //read the response from the vocoder + SDV3003_Packet devpacket; + if (device->GetResponse(devpacket)) + { + std::cerr << "ERROR: could not get response from " << device_type << " device at " << device->GetDevicePath() << std::endl; + return; + } + + // get the response type + bool is_audio; + if (2U == devpacket.header.packet_type) + is_audio = true; + else if (1U == devpacket.header.packet_type) + is_audio = false; + else + { + std::string title("Unexpected "); + title.append(device_type); + title.append(" response packet"); + device->dump(title.c_str(), &devpacket, packet_size(devpacket)); + return; + } + + //get the packet from either the dstar or dmr vocoder's queue + auto spPacket = device->packet_queue.pop(); + + if (is_audio) { + //move the audio to the CTranscoderPacket + for (unsigned int i=0; i<160; i++) + spPacket->GetAudio()[i] = ntohs(devpacket.payload.audio.samples[i]); + // we need to encode the m17 + //encode the audio to c2_3200 (all ambe input vocodes to ECodecType::c2_3200) + uint8_t m17data[8]; + if (spPacket->IsSecond()) { + c2_32.codec2_encode(m17data, spPacket->GetAudio()); + //move the c2_3200 data to the second half of the M17 packet + spPacket->SetM17Data(m17data, 8, 8); + } else /* the packet is first */ { + c2_32.codec2_encode(m17data, spPacket->GetAudio()); + // move it into the packet + spPacket->SetM17Data(m17data, 0, 8); + if (spPacket->IsLast()) { + // we have an odd number of packets, so we have to do finish up the m17 packet + const uint8_t silence[] = {0x00, 0x01, 0x43, 0x09, 0xe4, 0x9c, 0x08, 0x21 }; + //put codec silence in the second half of the codec + spPacket->SetM17Data(silence, 8, 8); + } + } + // we've received the audio and we've calculated the m17 data, now we just need to + // calculate the other ambe data + if (type == EAmbeType::dmr) { + //send the audio packet to the next available dmr vocoder + device->SendAudio(current_dmr_vocoder % 3, spPacket->GetAudio()); + //push the packet onto the dmr vocoder's queue + device->packet_queue.push(spPacket); + //increment the dmr vocoder index + IncrementDMRVocoder(); + } else /* the dmr/dstar type is dstar */ { + //send the audio packet to the next available dstar vocoder + device->SendAudio(current_dstar_vocoder % 3, spPacket->GetAudio()); + //push the packet onto the dstar vocoder's queue + device->packet_queue.push(spPacket); + //increment the dstar vocoder index + IncrementDStarVocoder(); + } + } else /* the response is ambe */ { + if (type == EAmbeType::dmr) { + spPacket->SetDMRData(devpacket.payload.ambe.data); + } else { + spPacket->SetDStarData(devpacket.payload.ambe.data); + } + if (spPacket->AllCodecsAreSet()) { + // open a socket to the reflector channel + CUnixDgramWriter socket; + std::string name(TC2REF); + name.append(1, spPacket->GetModule()); + socket.SetUp(name.c_str()); + // send the packet over the socket + socket.Send(spPacket->GetTCPacket()); + // the socket will automatically close after sending + } + } +} diff --git a/Controller.h b/Controller.h index f9c95f5..63864c2 100644 --- a/Controller.h +++ b/Controller.h @@ -21,23 +21,14 @@ #include #include #include +#include -#include "PacketQueue.h" +#include "codec2.h" #include "DV3003.h" #include "configure.h" #include "UnixDgramSocket.h" -// local audio storage -class CAudioBlock -{ -public: - int16_t operator [](int i) const {return audio[i];} - int16_t & operator [](int i) {return audio[i];} - int16_t *data() { return audio; } - const int16_t *cdata() const { return audio; } -protected: - int16_t audio[160]; -}; +enum class EAmbeType { dstar, dmr }; class CController { @@ -54,8 +45,8 @@ protected: std::vector> dmr_device, dstar_device; CUnixDgramReader reader; CUnixDgramWriter writer; - std::vector dmr_audio_block, dstar_audio_block; - std::vector dmr_packet_queue, dstar_packet_queue; + CCodec2 c2_16{false}; + CCodec2 c2_32{true}; bool InitDevices(); void IncrementDMRVocoder(void); @@ -63,6 +54,8 @@ protected: // processing threads void ReadReflector(); void ReadAmbeDevices(); + void ReadDevice(std::shared_ptr dv3003, EAmbeType type); + void AddFDSet(int &max, int newfd, fd_set *set) const; void CSVtoSet(const std::string &str, std::set &set, const std::string &delimiters = ","); }; diff --git a/DV3003.cpp b/DV3003.cpp index 37d335b..ea032b9 100644 --- a/DV3003.cpp +++ b/DV3003.cpp @@ -31,8 +31,6 @@ #include #include -#include - #include "DV3003.h" CDV3003::CDV3003(Encoding t) : type(t), fd(-1) @@ -152,7 +150,7 @@ bool CDV3003::InitDV3003() return true; } - if (getresponse(responsePacket)) { + if (GetResponse(responsePacket)) { std::cerr << "InitDV3003: error receiving response to reset" << std::endl; return true; } @@ -174,7 +172,7 @@ bool CDV3003::InitDV3003() } memset(&responsePacket, 0, sizeof(responsePacket)); - if (getresponse(responsePacket)) { + if (GetResponse(responsePacket)) { std::cerr << "InitDV3003: error receiving response to parity set" << std::endl; dump("Parity Ctrl Response Packet", &responsePacket, 4+ntohs(responsePacket.header.payload_length)); return true; @@ -194,7 +192,7 @@ bool CDV3003::InitDV3003() return true; } - if (getresponse(responsePacket)) { + if (GetResponse(responsePacket)) { std::cerr << "InitDV3003: error receiving response to product id request" << std::endl; return true; } @@ -212,7 +210,7 @@ bool CDV3003::InitDV3003() return true; } - if (getresponse(responsePacket)) { + if (GetResponse(responsePacket)) { std::cerr << "InitDV3003: error receiving response to version request" << std::endl; return true; } @@ -250,7 +248,7 @@ bool CDV3003::ConfigureCodec(uint8_t pkt_ch, Encoding type) } memset(&responsePacket, 0, sizeof(SDV3003_Packet)); - if (getresponse(responsePacket)) { + if (GetResponse(responsePacket)) { std::cerr << "error reading codec config response packet" << std::endl; return true; } @@ -275,7 +273,7 @@ void CDV3003::CloseDevice() } } -bool CDV3003::getresponse(SDV3003_Packet &packet) +bool CDV3003::GetResponse(SDV3003_Packet &packet) { ssize_t bytesRead; @@ -353,27 +351,6 @@ bool CDV3003::SendAudio(const uint8_t channel, const int16_t *audio) const return false; } -bool CDV3003::GetData(uint8_t *data) -{ - SDV3003_Packet p; - // read data packet from DV3000 - p.start_byte = 0U; - if (getresponse(p)) - return true; - if (p.start_byte!=PKT_HEADER || htons(p.header.payload_length)!=12 || - p.header.packet_type!=PKT_CHANNEL || p.payload.ambe.chand!=1U || - p.payload.ambe.num_bits!=72U) { - std::cerr << "Error receiving audio packet response" << std::endl; - dump("Received AMBE", &p, packet_size(p)); - return true; - } - - // copy it to the output - memcpy(data, p.payload.ambe.data, 9); - - return false; -} - bool CDV3003::SendData(const uint8_t channel, const uint8_t *data) const { // Create data packet @@ -396,28 +373,6 @@ bool CDV3003::SendData(const uint8_t channel, const uint8_t *data) const return false; } -bool CDV3003::GetAudio(int16_t *audio) -{ - SDV3003_Packet p; - // read audio packet from DV3000 - p.start_byte = 0U; - if (getresponse(p)) - return true; - if (p.start_byte!=PKT_HEADER || htons(p.header.payload_length)!=323 || - p.header.packet_type!=PKT_SPEECH || p.payload.audio.speechd!=0U || - p.payload.audio.num_samples!=160U) { - std::cerr << "GetAudio: unexpected audio packet response" << std::endl; - int size = packet_size(p); - dump("Received Audio", &p, size); - return true; - } - - for (int i=0; i<160; i++) - audio[i] = ntohs(p.payload.audio.samples[i]); - - return false; -} - void CDV3003::dump(const char *title, void *pointer, int length) const { assert(title != NULL); diff --git a/DV3003.h b/DV3003.h index 2cf2b76..b14c60d 100644 --- a/DV3003.h +++ b/DV3003.h @@ -16,9 +16,12 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . +#include #include #include +#include "PacketQueue.h" + #define USB3XXX_MAXPACKETSIZE 1024 // must be multiple of 64 #define PKT_HEADER 0x61 @@ -93,20 +96,21 @@ public: bool InitDV3003(); bool ConfigureCodec(uint8_t pkt_ch, Encoding type); bool SendAudio(const uint8_t channel, const int16_t *audio) const; - bool GetData(uint8_t *data); bool SendData(const uint8_t channel, const uint8_t *data) const; - bool GetAudio(int16_t *audio); + bool GetResponse(SDV3003_Packet &packet); + int GetFd() const { return fd; } void CloseDevice(); bool IsOpen() const; void dump(const char *title, void *data, int length) const; std::string GetDevicePath() const; std::string GetProductID() const; std::string GetVersion() const; + + CPacketQueue packet_queue; private: const Encoding type; int fd; std::string devicepath, productid, version; bool SetBaudRate(int baudrate); - bool getresponse(SDV3003_Packet &packet); bool checkResponse(SDV3003_Packet &responsePacket, uint8_t response) const; }; diff --git a/PacketQueue.h b/PacketQueue.h index 452f438..2b7f63e 100644 --- a/PacketQueue.h +++ b/PacketQueue.h @@ -29,12 +29,12 @@ class CPacketQueue { public: // pass thru - std::unique_ptr pop() + std::shared_ptr pop() { - std::unique_ptr pack; + std::shared_ptr pack; mutex.lock(); if (! queue.empty()) { - pack = std::move(queue.front()); + pack = queue.front(); queue.pop(); } mutex.unlock(); @@ -49,14 +49,14 @@ public: return rval; } - void push(std::unique_ptr &packet) + void push(std::shared_ptr packet) { mutex.lock(); - queue.push(std::move(packet)); + queue.push(packet); mutex.unlock(); } protected: std::mutex mutex; - std::queue> queue; + std::queue> queue; }; diff --git a/TranscoderPacket.cpp b/TranscoderPacket.cpp index 724db24..f460568 100644 --- a/TranscoderPacket.cpp +++ b/TranscoderPacket.cpp @@ -35,7 +35,7 @@ CTranscoderPacket::CTranscoderPacket(const STCPacket &tcp) : dstar_set(false), d break; case ECodecType::c2_1600: case ECodecType::c2_3200: - SetM17Data(tcp.m17); + SetM17Data(tcp.m17, 0, 16); break; default: std::cerr << "Trying to allocate CTranscoderPacket with an unknown codec type!" << std::endl; @@ -48,21 +48,40 @@ char CTranscoderPacket::GetModule() const return tcpacket.module; } -const uint8_t *CTranscoderPacket::GetDStarData() +const uint8_t *CTranscoderPacket::GetDStarData() const { return tcpacket.dstar; } -const uint8_t *CTranscoderPacket::GetDMRData() +const uint8_t *CTranscoderPacket::GetDMRData() const { return tcpacket.dmr; } -const uint8_t *CTranscoderPacket::GetM17Data() +const uint8_t *CTranscoderPacket::GetM17Data() const { return tcpacket.m17; } +const STCPacket *CTranscoderPacket::GetTCPacket() const +{ + return &tcpacket; +} + +void CTranscoderPacket::SetM17Data(const uint8_t *data, unsigned int offset, unsigned int size) +{ + if (offset) + { + if (8 != offset) + offset = 8; + } + if (offset + size != 16) + size = 16U - offset; + memcpy(tcpacket.m17+offset, data, size); + if (offset>0 || size==16) + m17_set = true; +} + void CTranscoderPacket::SetDStarData(const uint8_t *dstar) { memcpy(tcpacket.dstar, dstar, 9); @@ -74,12 +93,6 @@ void CTranscoderPacket::SetDMRData(const uint8_t *dmr ) dmr_set = true; } -void CTranscoderPacket::SetM17Data(const uint8_t *m17) -{ - memcpy(tcpacket.m17, m17, 16); - m17_set = true; -} - int16_t *CTranscoderPacket::GetAudio() { return audio; diff --git a/TranscoderPacket.h b/TranscoderPacket.h index d33caa4..9a6752a 100644 --- a/TranscoderPacket.h +++ b/TranscoderPacket.h @@ -34,12 +34,12 @@ public: char GetModule() const; // codec - const uint8_t *GetDStarData(); - const uint8_t *GetDMRData(); - const uint8_t *GetM17Data(); + const uint8_t *GetDStarData() const; + const uint8_t *GetDMRData() const; + const uint8_t *GetM17Data() const; void SetDStarData(const uint8_t *dstar); void SetDMRData(const uint8_t *dmr); - void SetM17Data(const uint8_t *m17); + void SetM17Data(const uint8_t *m17, unsigned int offset, unsigned int size); // audio int16_t *GetAudio(); @@ -54,6 +54,9 @@ public: bool M17IsSet() const; bool AllCodecsAreSet() const; + // the all important packet + const STCPacket *GetTCPacket() const; + private: STCPacket tcpacket; int16_t audio[160];