/*------------------------------------------------------------------------------ -- This file is a part of the SocExplorer Software -- Copyright (C) 2014, Plasma Physics Laboratory - CNRS -- -- This program is free software; you can redistribute it and/or modify -- it under the terms of the GNU General Public License as published by -- the Free Software Foundation; either version 3 of the License, or -- (at your option) any later version. -- -- This program is distributed in the hope that it will be useful, -- but WITHOUT ANY WARRANTY; without even the implied warranty of -- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -- GNU General Public License for more details. -- -- You should have received a copy of the GNU General Public License -- along with this program; if not, write to the Free Software -- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA -------------------------------------------------------------------------------*/ /*-- Author : Alexis Jeandet -- Mail : alexis.jeandet@member.fsf.org ----------------------------------------------------------------------------*/ #include "gr_esb_bridge.h" #include "gr_esb_ui.h" #include #include "spw.h" #include GR_ESB_bridge::GR_ESB_bridge(socexplorerplugin *parent) : abstractSpwBridge(parent) { this->p_GUI = new GR_ESB_ui(); this->manager = new GR_ESB_Manager(parent,this); connect((GR_ESB_ui*)(this->p_GUI),SIGNAL(ipchanged(QString)),this,SLOT(setIP(QString))); connect((GR_ESB_ui*)(this->p_GUI),SIGNAL(vlinkchanged(QString)),this,SLOT(setVirtualLink(QString))); connect((GR_ESB_ui*)(this->p_GUI),SIGNAL(connectClicked()),this,SLOT(toggleBridgeConnection())); this->manager->virtualLinkIndex = 0; this->manager->start(); } GR_ESB_bridge::~GR_ESB_bridge() { this->manager->requestInterruption(); while(this->manager->isRunning()); } void GR_ESB_bridge::toggleBridgeConnection() { if(this->plugin->isConnected()) { this->disconnectBridge(); } else { this->connectBridge(); } } bool GR_ESB_bridge::connectBridge() { if(this->manager->connectBridge()) { ((GR_ESB_ui*)this->p_GUI)->lock(true); emit setConnected(true); return true; } return false; } bool GR_ESB_bridge::disconnectBridge() { if(this->manager->disconnectBridge()) { ((GR_ESB_ui*)this->p_GUI)->lock(false); emit setConnected(false); return true; } return false; } void GR_ESB_bridge::setIP(QString ip) { this->manager->IP = ip; } void GR_ESB_bridge::setVirtualLink(QString vlink) { //vlink = vlink.section(,0,0); vlink.remove("Virtual link"); bool success; int vlinkTmp = vlink.toInt(&success); if(success) { setVirtualLink(vlinkTmp); } } void GR_ESB_bridge::setVirtualLink(qint32 vlink) { if(vlink<6 && vlink>=0) { this->manager->virtualLinkIndex = vlink; } } unsigned int GR_ESB_bridge::Write(unsigned int *Value, unsigned int count, unsigned int address) { char writeBuffer[RMAP_WRITE_PACKET_MIN_SZ((RMAP_MAX_XFER_SIZE*4))]; char *RMAPAckBuff; int transactionID = 0; int written=0; SocExplorerEngine::message(this->plugin,"Enter Write function",2); QProgressBar* progress=NULL; SocExplorerAutoProgressBar autopb; if(count>RMAP_MAX_XFER_SIZE) { progress= SocExplorerEngine::getProgressBar("Writing on SPW @0x"+QString::number(address,16)+" %v of "+QString::number(count)+" words ",count); autopb.setProgressBar(progress); } //Quite stupide loop, I guess that I always get the number of byte I asked for! while(count>=RMAP_MAX_XFER_SIZE) { for(int i=0;i<(RMAP_MAX_XFER_SIZE);i++) { writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+0] = (char)(((unsigned int)Value[i+written]>>24)&0xFF); writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+1] = (char)(((unsigned int)Value[i+written]>>16)&0xFF); writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+2] = (char)(((unsigned int)Value[i+written]>>8)&0xFF); writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+3] = (char)(((unsigned int)Value[i+written])&0xFF); } transactionID=manager->getRMAPtransactionID(); SocExplorerEngine::message(this->plugin,QString("Sending Write request with ID=%1").arg(transactionID),2); RMAP_build_tx_request_header( this->manager->destinationLogicalAddress, this->manager->destinationKey, this->manager->sourceLogicalAddress, transactionID, address+(written*4), RMAP_MAX_XFER_SIZE*4, writeBuffer); manager->sendPacket(writeBuffer,RMAP_WRITE_PACKET_MIN_SZ(RMAP_MAX_XFER_SIZE*4)); manager->getRMAPanswer(transactionID,&RMAPAckBuff); free(RMAPAckBuff); written+=RMAP_MAX_XFER_SIZE; count-=RMAP_MAX_XFER_SIZE; progress->setValue(written); qApp->processEvents(); } if(count>0) { for(int i=0;i<((int)count);i++) { writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+0] = (char)(((unsigned int)Value[i+written]>>24)&0xFF); writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+1] = (char)(((unsigned int)Value[i+written]>>16)&0xFF); writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+2] = (char)(((unsigned int)Value[i+written]>>8)&0xFF); writeBuffer[RMAP_WRITE_HEADER_MIN_SZ+(i*4)+3] = (char)(((unsigned int)Value[i+written])&0xFF); } transactionID=manager->getRMAPtransactionID(); SocExplorerEngine::message(this->plugin,QString("Sending Write request with ID=%1").arg(transactionID),2); RMAP_build_tx_request_header( this->manager->destinationLogicalAddress, this->manager->destinationKey, this->manager->sourceLogicalAddress, transactionID, address+(written*4), count*4, writeBuffer); manager->sendPacket(writeBuffer,RMAP_WRITE_PACKET_MIN_SZ(count*4)); manager->getRMAPanswer(transactionID,&RMAPAckBuff); free(RMAPAckBuff); written+=count; if(progress!=NULL) { progress->setValue(written); qApp->processEvents(); } } return written; } unsigned int GR_ESB_bridge::Read(unsigned int *Value, unsigned int count, unsigned int address) { char requestBuffer[RMAP_READ_HEADER_MIN_SZ]; char* RMAP_AnswerBuffer; requestBuffer[0]=this->manager->linkNumber;//Link number int transactionID = 0; int read=0; QProgressBar* progress=NULL; SocExplorerAutoProgressBar autopb; if(count>RMAP_MAX_XFER_SIZE) { progress= SocExplorerEngine::getProgressBar("Reading on SPW @0x"+QString::number(address,16)+" %v of "+QString::number(count)+" words ",count); autopb.setProgressBar(progress); } SocExplorerEngine::message(this->plugin,QString("Enter read function, count=%1, RMAP_MAX_XFER_SIZE=%2").arg(count).arg(RMAP_MAX_XFER_SIZE),2); //Quite stupide loop, I guess that I always get the number of byte I asked for! while((int)count>=(int)RMAP_MAX_XFER_SIZE) { transactionID = manager->getRMAPtransactionID(); SocExplorerEngine::message(this->plugin,QString("New transactionID:%1").arg(transactionID),2); RMAP_build_rx_request_header( this->manager->destinationLogicalAddress, this->manager->destinationKey, this->manager->sourceLogicalAddress, transactionID, address+(read*4), RMAP_MAX_XFER_SIZE*4, requestBuffer); manager->sendPacket(requestBuffer,RMAP_READ_HEADER_MIN_SZ); int len=manager->getRMAPanswer(transactionID,&RMAP_AnswerBuffer); if(len==-1) { this->toggleBridgeConnection(); return 0; } for(int i=0;i<((len-13)/4);i++) { Value[read+i] = 0x0FF & ((unsigned int)RMAP_AnswerBuffer[(4*i)+12]); Value[read+i] = (Value[read+i]<<8) + (0x0FF & ((unsigned int)RMAP_AnswerBuffer[(4*i)+13])); Value[read+i] = (Value[read+i]<<8) + (0x0FF & ((unsigned int)RMAP_AnswerBuffer[(4*i)+14])); Value[read+i] = (Value[read+i]<<8) + (0x0FF & ((unsigned int)RMAP_AnswerBuffer[(4*i)+15])); } free(RMAP_AnswerBuffer); read+=RMAP_MAX_XFER_SIZE; count-=RMAP_MAX_XFER_SIZE; if(progress!=NULL) { progress->setValue(read); qApp->processEvents(); } } if((int)count>0) { transactionID = manager->getRMAPtransactionID(); SocExplorerEngine::message(this->plugin,QString("New transactionID: %1").arg(transactionID),2); SocExplorerEngine::message(this->plugin,QString("Building request with:"),2); SocExplorerEngine::message(this->plugin,QString("Address = %1").arg(address+(read*4),8,16),2); SocExplorerEngine::message(this->plugin,QString("Size = %1").arg(count*4),2); SocExplorerEngine::message(this->plugin,QString("Size + 13 = %1").arg((count*4)+13),2); RMAP_build_rx_request_header( this->manager->destinationLogicalAddress, this->manager->destinationKey, this->manager->sourceLogicalAddress, transactionID, address+(read*4), count*4, requestBuffer); manager->sendPacket(requestBuffer,RMAP_READ_HEADER_MIN_SZ); int len=manager->getRMAPanswer(transactionID,&RMAP_AnswerBuffer); if(len==-1) { this->toggleBridgeConnection(); return 0; } for(int i=0;i<((len-13)/4);i++) { Value[read+i] = 0x0FF & ((unsigned int)RMAP_AnswerBuffer[(4*i)+12]); Value[read+i] = (Value[read+i]<<8) + (0x0FF & ((unsigned int)RMAP_AnswerBuffer[(4*i)+13])); Value[read+i] = (Value[read+i]<<8) + (0x0FF & ((unsigned int)RMAP_AnswerBuffer[(4*i)+14])); Value[read+i] = (Value[read+i]<<8) + (0x0FF & ((unsigned int)RMAP_AnswerBuffer[(4*i)+15])); } free(RMAP_AnswerBuffer); read+=count; if(progress!=NULL) { progress->setValue(read); qApp->processEvents(); } } return read; } int GR_ESB_bridge::pushRMAPPacket(char *packet, int size) { return this->manager->sendPacket(packet,size); } GR_ESB_Manager::GR_ESB_Manager(socexplorerplugin *plugin, QObject *parent) :abstractSpwManager(plugin, parent) { this->sourceLogicalAddress=32; this->destinationLogicalAddress=254; this->destinationKey=2; connect(&(this->Read_soc),SIGNAL(readyRead()),this,SLOT(readyRead())); } GR_ESB_Manager::~GR_ESB_Manager() { } void GR_ESB_Manager::__processPacket(packetBuffer_t *packet) { if(packet->complete) { if(packet->buffer[1]==(char)SPW_PROTO_ID_RMAP) //RMAP packet { RMAP_Answer* RMapPacket; SocExplorerEngine::message(this->plugin,"Got RMAP packet",2); SocExplorerEngine::message(this->plugin,QString("Rmap packet size %1").arg(packet->PacketLen),2); char* packetbuffer = (char*)malloc(packet->PacketLen); if(packetbuffer) { memcpy(packetbuffer,packet->buffer,packet->PacketLen); this->handleMutex->unlock(); if(packet->PacketLen==8) { RMapPacket=new RMAP_Answer(RMAP_get_transactionID(packetbuffer),packetbuffer,packet->PacketLen); } else { RMapPacket=new RMAP_Answer(RMAP_get_transactionID(packetbuffer),packetbuffer,packet->PacketLen); } RMAP_AnswersMtx->lock(); RMAP_Answers.append(RMapPacket); RMAP_AnswersMtx->unlock(); RMAP_AnswersSem->release(); } } else //any non-rmap packet will be pushed to the network { char* packetbuffer = (char*)malloc(packet->PacketLen); if(packetbuffer) { memcpy(packetbuffer,packet->buffer,packet->PacketLen); emit emitPacket(packetbuffer,packet->PacketLen); SocExplorerEngine::message(this->plugin,"Got SPW packet",2); } } } } QByteArray GR_ESB_Manager::__processData(const QByteArray &data, packetBuffer_t *buffer) { if(buffer->complete) { if(Q_UNLIKELY(data.size()<=4)) return data; buffer->PacketLen= ((int)data.at(3)& 0x0FF) + (((int)data.at(2)& 0x0FF)<<8) + (((int)data.at(1)& 0x0FF)<<16); if(buffer->PacketLen>(data.size()-4)) { memcpy(buffer->buffer,data.data()+4,data.size()-4); buffer->complete=false; buffer->index=data.size()-4; } else { memcpy(buffer->buffer,data.data()+4,data.size()-4); buffer->PacketLen = data.size()-4; buffer->index=data.size()-4; buffer->complete=true; __processPacket(buffer); int len = buffer->PacketLen; buffer->PacketLen = 0; buffer->index=0; if((data.size()-4-len)) { return __processData(data.right((data.size()-4-len)),buffer); } } } else { if(buffer->PacketLen>(data.size()+buffer->index)) { memcpy(buffer->buffer+buffer->index,data.data(),data.size()); buffer->complete=false; buffer->index+=data.size(); } else { memcpy(buffer->buffer+buffer->index,data.data(),buffer->PacketLen-buffer->index); buffer->complete=true; buffer->index+=data.size(); __processPacket(buffer); int len = buffer->PacketLen; buffer->PacketLen = 0; buffer->index=0; if((data.size()-len)) { return __processData(data.right((data.size()-len)),buffer); } } } return QByteArray(); } void GR_ESB_Manager::run() { char bufferData[(RMAP_MAX_XFER_SIZE*4)+50]; packetBuffer_t buffer={bufferData,0,0,true}; SocExplorerEngine::message(this->plugin,"Starting GRESB pooling thread",1); QByteArray data; while (!this->isInterruptionRequested()) { if(this->connected) { SocExplorerEngine::message(this->plugin,"Looking for new RMAP packets",8); if(!incomingPackets.isEmpty()) { incomingPacketsMutex.lock(); data.append(incomingPackets.dequeue()); this->incomingPacketsMutex.unlock(); data = __processData(data,&buffer); } } else { //do some sanity checks! usleep(RMAPtimeout/2); } usleep(1000); } SocExplorerEngine::message(this->plugin,"Exiting Startdundee USB pooling thread",1); } bool GR_ESB_Manager::connectBridge() { int timeout=60; this->connected = false; if(this->Read_soc.state()==QTcpSocket::UnconnectedState) { this->Read_soc.connectToHost(IP,gresb_Conf[virtualLinkIndex].Receive_port); this->Read_soc.waitForConnected(30000); } if(this->Write_soc.state()==QTcpSocket::UnconnectedState) { this->Write_soc.connectToHost(IP,gresb_Conf[virtualLinkIndex].Transmit_port); this->Write_soc.waitForConnected(30000); } while((this->Read_soc.state()!=QTcpSocket::ConnectedState) && (this->Write_soc.state()!=QTcpSocket::ConnectedState)) { usleep(100000); if(timeout--==0)return false; } this->connected = true; return true; } bool GR_ESB_Manager::disconnectBridge() { int timeout=60; if(this->Read_soc.state()!=QTcpSocket::UnconnectedState) { this->Read_soc.disconnectFromHost(); this->Read_soc.waitForDisconnected(30000); } if(this->Write_soc.state()!=QTcpSocket::UnconnectedState) { this->Write_soc.disconnectFromHost(); this->Write_soc.waitForDisconnected(30000); } while((this->Read_soc.state()!=QTcpSocket::UnconnectedState) && (this->Write_soc.state()!=QTcpSocket::UnconnectedState)) { usleep(100000); if(timeout--==0)return false; } return true; } bool GR_ESB_Manager::sendPacket(char *packet, int size) { bool result = false; char protocoleIdentifier; SocExplorerEngine::message(this->plugin,"Sending SPW packet",2); if(Q_UNLIKELY(this->Write_soc.state()!=QAbstractSocket::ConnectedState)) { SocExplorerEngine::message(this->plugin,"Socket closed",2); //TODO handle disconnection } char* SPWpacket = (char*)malloc(size+4); if(SPWpacket!=NULL) { SPWpacket[0]=0; //Protocol = spw memcpy(SPWpacket+4,packet,size); SPWpacket[1]=(size>>16) & 0x0FF; SPWpacket[2]=(size>>8) & 0x0FF; SPWpacket[3]=size & 0x0FF; this->handleMutex->lock(); result = ((size+4) == this->Write_soc.write(SPWpacket,size+4)); this->Write_soc.flush(); this->handleMutex->unlock(); free(SPWpacket); } if (Q_UNLIKELY(!result)) { SocExplorerEngine::message(this->plugin,"ERR sending the READ command ",2); return false; } else { emit bytesTransmittedToSpw( size-1 ); // -1 is for removing the first bytes added to the packet to route to the right link // read the protocole identifier protocoleIdentifier = packet[2]; if (protocoleIdentifier == SPW_PROTO_ID_CCSDS) emit ccsdsPacketTransmittedToSpw(); SocExplorerEngine::message(this->plugin,"Packet sent",2); } return true; } void GR_ESB_Manager::readyRead() { incomingPacketsMutex.lock(); incomingPackets.append(Read_soc.readAll()); incomingPacketsMutex.unlock(); }