bool AsioDrivers::loadDriver(char *name)
{
char dname[64];
unsigned long newID;
for(long i = 0; i < getNumFragments(); i++)
{
if(getName(i, dname) && !strcmp(name, dname))
{
if(newInstance(i, &newID))
{
if(resolveASIO(newID))
{
if(connID != -1)
removeInstance(curIndex, connID);
curIndex = i;
connID = newID;
return true;
}
}
break;
}
}
return false;
}
int SendTextIMPacket::putContents(unsigned char *buf)
{
int pos=0;
//multi-fragment information
buf[pos++] = numFragments;
buf[pos++] = seqFragments;
memcpy(buf+pos, &messageID, 2);
pos+=2;
bool hasImage = false;
std::string str2send = EvaUtil::convertToSend(message, &hasImage);
buf[pos++] = hasImage?QQ_IM_IMAGE_REPLY:replyType; // auto-reply or not
memcpy(buf+pos, str2send.c_str(), str2send.length());
pos += str2send.length();
if (getNumFragments() == (getSeqOfFragments() + 1))
{
buf[pos++] = 0x20; // a space, witch is needed in the last fragment
setMessageID(getMessageID() + 1);
}
buf[pos++] = 0x00; // C style string terminator
buf[pos++] = fontFlag;
buf[pos++] = red;
buf[pos++] = green;
buf[pos++] = blue;
buf[pos++] = 0;
short tmpEncoding = htons(encoding); // encoding for text
memcpy(buf+pos,&tmpEncoding, 2);
pos+=2;
int len = fontName.length(); // font name
memcpy(buf+pos, fontName.c_str(), len);
pos+=len;
buf[pos++] = 0x0D; // an Enter
return pos;
}
long AsioDrivers::getDriverNames(char **names, long maxDrivers)
{
for(long i = 0; i < getNumFragments() && i < maxDrivers; i++)
getName(i, names[i]);
return getNumFragments() < maxDrivers ? getNumFragments() : maxDrivers;
}
bool LcmTunnel::send_lcm_messages(std::deque<TunnelLcmMessage *> &msgQueue, uint32_t bytesInQueue)
{
if (udp_fd >= 0) {
if (server_udp_port <= 0)
return true; //connection hasn't been setup yet.
udp_send_seqno++;//increment the sequence counter
udp_send_seqno = udp_send_seqno % SEQNO_WRAP_VAL;
if (verbose)
printf("sending %d bytes from %d lcm messages\n", bytesInQueue, static_cast<int> (msgQueue.size()));
uint32_t msgSize = bytesInQueue;
int nfragments = getNumFragments(msgSize);
if ((tunnel_params->fec <= 0 && nfragments > MAX_NUM_FRAGMENTS) || (tunnel_params->fec > 0 && nfragments
> MAX_NUM_FRAGMENTS / tunnel_params->fec)) {
uint32_t maxMsgSize;
if (tunnel_params->fec > 0)
maxMsgSize = MAX_PAYLOAD_BYTES_PER_FRAGMENT * MAX_NUM_FRAGMENTS / tunnel_params->fec;
else
maxMsgSize = MAX_PAYLOAD_BYTES_PER_FRAGMENT * MAX_NUM_FRAGMENTS;
fprintf(stderr,
"WARNING! Queue contains more than the max message size of %d bytes... we're WAY behind, dropping msgs\n",
maxMsgSize);
while (msgSize < maxMsgSize) {
//drop messages
TunnelLcmMessage * drop_msg = msgQueue.front();
msgQueue.pop_front();
msgSize -= drop_msg->encoded_size;
delete drop_msg;
}
}
//put the entire queue into 1 big buffer
uint8_t * msgBuf = (uint8_t *) malloc(msgSize * sizeof(uint8_t));
uint32_t msgBufOffset = 0;
while (!msgQueue.empty()) {
TunnelLcmMessage * msg = msgQueue.front();
msgQueue.pop_front();
lcm_tunnel_sub_msg_t_encode(msgBuf, msgBufOffset, msgSize - msgBufOffset, msg->sub_msg);
msgBufOffset += msg->encoded_size;
delete msg;
}
assert(msgBufOffset==msgSize);
if (tunnel_params->fec < 1 || nfragments < MIN_NUM_FRAGMENTS_FOR_FEC) { //don't use FEC
int sendRepeats = 1;
if (fabs(tunnel_params->fec) > 1) { //fec <0 means always send duplicates
sendRepeats = (int) ceil(fabs(tunnel_params->fec)); //send ceil of the fec rate times
}
for (int r = 0; r < sendRepeats; r++) {
msgBufOffset = 0;
for (int i = 0; i < nfragments; i++) {
lcm_tunnel_udp_msg_t msg;
msg.seqno = udp_send_seqno;
msg.fragno = i;
msg.payload_size = msgSize;
msg.data_size = MIN(MAX_PAYLOAD_BYTES_PER_FRAGMENT, msgSize - msgBufOffset);
msg.data = msgBuf + msgBufOffset;
msgBufOffset += msg.data_size;
int msg_sz = lcm_tunnel_udp_msg_t_encoded_size(&msg);
uint8_t msg_buf[msg_sz];
lcm_tunnel_udp_msg_t_encode(msg_buf, 0, msg_sz, &msg);
// printf("sending: %d, %d / %d\n", msg.seqno, msg.fragment, msg.nfrags);
int send_status = send(udp_fd, msg_buf, msg_sz, 0);
// fprintf(stderr,"sent packet\n");
checkUDPSendStatus(send_status);
}
}
}
else { //use tunnel error correction to send
ldpc_enc_wrapper * ldpc_enc = new ldpc_enc_wrapper(msgBuf, msgSize, MAX_PAYLOAD_BYTES_PER_FRAGMENT,
tunnel_params->fec);
lcm_tunnel_udp_msg_t msg;
msg.seqno = udp_send_seqno;
msg.payload_size = msgSize;
// printf("sending: %d, %d / %d\n", recv_udp_msg->seqno, recv_udp_msg->fragment, recv_udp_msg->nfrags);
int enc_done = 0;
while (!enc_done) {
uint8_t data_buf[MAX_PAYLOAD_BYTES_PER_FRAGMENT];
msg.data_size = MAX_PAYLOAD_BYTES_PER_FRAGMENT;
msg.data = data_buf;
enc_done = ldpc_enc->getNextPacket(msg.data, &msg.fragno);
int msg_sz = lcm_tunnel_udp_msg_t_encoded_size(&msg);
uint8_t msg_buf[msg_sz];
lcm_tunnel_udp_msg_t_encode(msg_buf, 0, msg_sz, &msg);
// printf("sending: %d, %d / %d\n", msg.seqno, msg.fragment, msg.nfrags);
int send_status = send(udp_fd, msg_buf, msg_sz, 0);
checkUDPSendStatus(send_status);
}
// printf("finished encoding and sending packet %d for channel: %s\n",recv_udp_msg->seqno,channel);
delete ldpc_enc;
}
free(msgBuf);
}
else {
int cfd = ssocket_get_fd(tcp_sock);
assert(cfd>0);
while (!msgQueue.empty()) {
TunnelLcmMessage * msg = msgQueue.front();
msgQueue.pop_front();
int64_t now = _timestamp_now();
double age_ms = (now - msg->recv_utime) * 1.0e-3;
if (tunnel_params->tcp_max_age_ms > 0 && age_ms > tunnel_params->tcp_max_age_ms) {
// message has been queued up for too long. Drop it.
if (verbose)
fprintf(stderr, "%s message too old (age = %d, param = %d), dropping.\n", msg->sub_msg->channel,
(int) age_ms, tunnel_params->tcp_max_age_ms);
}
else {
// send channel
int chan_len = strlen(msg->sub_msg->channel);
uint32_t chan_len_n = htonl(chan_len);
if (4 != _fileutils_write_fully(cfd, &chan_len_n, 4)) {
delete msg;
return false;
}
if (chan_len != _fileutils_write_fully(cfd, msg->sub_msg->channel, chan_len)) {
delete msg;
return false;
}
// send data
int data_size_n = htonl(msg->sub_msg->data_size);
if (4 != _fileutils_write_fully(cfd, &data_size_n, 4)) {
delete msg;
return false;
}
if (msg->sub_msg->data_size != _fileutils_write_fully(cfd, msg->sub_msg->data, msg->sub_msg->data_size)) {
delete msg;
return false;
}
}
if (verbose)
printf("Sent \"%s\".\n", msg->sub_msg->channel);
delete msg;
}
}
return true;
}
int LcmTunnel::on_udp_data(GIOChannel * source, GIOCondition cond, void *user_data)
{
LcmTunnel * self = (LcmTunnel*) user_data;
uint8_t recv_buffer[65535];
int recv_status = recv(self->udp_fd, recv_buffer, 65535, 0);
if (recv_status < 0) {
perror("recv error: ");
return TRUE;
// LcmTunnelServer::disconnectClient(self);
}
lcm_tunnel_udp_msg_t * recv_udp_msg = (lcm_tunnel_udp_msg_t *) calloc(1, sizeof(lcm_tunnel_udp_msg_t));
int decode_ret = lcm_tunnel_udp_msg_t_decode(recv_buffer, 0, recv_status, recv_udp_msg);
if (decode_ret < 0) {
lcm_tunnel_disconnect_msg_t disc_msg;
decode_ret = lcm_tunnel_disconnect_msg_t_decode(recv_buffer, 0, recv_status, &disc_msg);
if (decode_ret >= 0) {
fprintf(stderr, "Received a disconnect message... disconnecting!\n");
LcmTunnelServer::disconnectClient(self);
}
else {
fprintf(stderr, "Received Corrupted UDP packet!\n");
}
}
// printf("received: %d, %d / %d\n", recv_udp_msg->seqno, recv_udp_msg->fragment, recv_udp_msg->nfrags);
if (self->verbose, recv_udp_msg->seqno < self->cur_seqno) {
printf("Got Out of order packet!\n");
}
// start of a new message?
if (recv_udp_msg->seqno > self->cur_seqno || recv_udp_msg->seqno < (int32_t) self->cur_seqno - SEQNO_WRAP_GAP) { //handle wrap-around with second part
if (!self->message_complete && self->cur_seqno > 0 || recv_udp_msg->seqno > (self->cur_seqno + 1)) {
printf("packets %d to %d dropped! with %d of %d fragments received, ", self->cur_seqno, recv_udp_msg->seqno - 1,
self->numFragsRec, self->nfrags);
if (self->tunnel_params->fec > 1 && self->nfrags >= MIN_NUM_FRAGMENTS_FOR_FEC) {
printf("was FECed\n");
}
else
printf("not FECed\n");
}
self->cur_seqno = recv_udp_msg->seqno;
self->nfrags = getNumFragments(recv_udp_msg->payload_size);
self->numFragsRec = 0;
//increase the recFlags buffers
if (self->recFlags_sz < self->nfrags) {
self->recFlags_sz = self->nfrags;
self->recFlags = (char *) realloc(self->recFlags, self->recFlags_sz);
}
memset(self->recFlags, 0, self->recFlags_sz); //mark all frags as unreceived
self->completeTo_fragno = 0;
self->fragment_buf_offset = 0;
int messageSize = recv_udp_msg->payload_size;
// increase buffer size if needed, also make enough space for the channel in case we're using FEC
if (self->buf_sz < messageSize) {
self->buf = (char *) realloc(self->buf, messageSize);
self->buf_sz = messageSize;
}
//create a new FEC decoder
if (self->ldpc_dec != NULL) {
delete self->ldpc_dec; //delete the old one if we haven't already
self->ldpc_dec = NULL;
}
if (self->tunnel_params->fec > 1 && self->nfrags >= MIN_NUM_FRAGMENTS_FOR_FEC) {
//allocate the new one
self->ldpc_dec = new ldpc_dec_wrapper(messageSize, MAX_PAYLOAD_BYTES_PER_FRAGMENT, self->tunnel_params->fec);
}
self->message_complete = 0;
}
if (!self->message_complete && recv_udp_msg->seqno == self->cur_seqno && getNumFragments(recv_udp_msg->payload_size)
== self->nfrags) {
self->numFragsRec++;
if (self->tunnel_params->fec < 1 || self->nfrags < MIN_NUM_FRAGMENTS_FOR_FEC) { //we're not using FEC for this message
// have we already received this fragment?
if (recv_udp_msg->fragno < self->nfrags && !self->recFlags[recv_udp_msg->fragno]) {
self->recFlags[recv_udp_msg->fragno] = 1;
//copy everything to the app->buf
int64_t pos_start = recv_udp_msg->fragno * MAX_PAYLOAD_BYTES_PER_FRAGMENT;
int64_t pos_end = MIN(recv_udp_msg->payload_size, (recv_udp_msg->fragno + 1) * MAX_PAYLOAD_BYTES_PER_FRAGMENT);
int64_t curPayloadSize = pos_end - pos_start;
assert(recv_udp_msg->data_size==curPayloadSize);
memcpy(self->buf + pos_start, recv_udp_msg->data, curPayloadSize);
self->message_complete = 1;
for (int i = self->completeTo_fragno; i < self->nfrags; i++) {
if (!self->recFlags[i]) {
self->message_complete = 0;
break;
}
else
self->completeTo_fragno = i;
}
if (self->message_complete) {
//publish all the lcm messages in the buffer
self->publishLcmMessagesInBuf(recv_udp_msg->payload_size);
}
}
else if (self->verbose) {
printf("ignoring udp packet\n");
// printf("seqno: %d (%d) fragment: %d (%d) nfrags: %d (%d)\n",
// recv_udp_msg->seqno, app->cur_seqno,
// recv_udp_msg->fragment, app->expected_fragno,
// recv_udp_msg->nfrags, app->nfrags);
}
}
else { //we're using FEC
int dec_done = self->ldpc_dec->processPacket(recv_udp_msg->data, recv_udp_msg->fragno);
if (dec_done != 0) {
if (dec_done == 1) {
check_ret(self->ldpc_dec->getObject((uint8_t*) self->buf));
//publish all the lcm messages in the buffer
self->publishLcmMessagesInBuf(recv_udp_msg->payload_size);
}
else {
fprintf(stderr, "ldpc got all the sent packets, but couldn't reconstruct... this shouldn't happen!\n");
}
self->message_complete = 1;
delete self->ldpc_dec; //we're all done, so we can delete it
self->ldpc_dec = NULL;
}
}
}
else if (self->verbose && !self->message_complete) {
// if (!self->message_complete && recv_udp_msg->seqno == self->cur_seqno && recv_udp_msg->nfrags == self->nfrags) {
printf("ignoring udp packet seqno=%d, nfrag =%d, \t self-> seqno=%d, nfrags=%d\n", recv_udp_msg->seqno,
getNumFragments(recv_udp_msg->payload_size), self->cur_seqno, self->nfrags);
}
lcm_tunnel_udp_msg_t_destroy(recv_udp_msg);
return TRUE;
}
