int LcmTunnel::connectToClient(lcm_t * lcm_, introspect_t *introspect_, GMainLoop * mainloop_, ssocket_t * sock_,
tunnel_server_params_t * server_params_)
{ //for a client that connected to this server
//parameters will be passed over from the client
server_params = server_params_;
lcm = lcm_;
introspect = introspect_;
mainloop = mainloop_;
tcp_sock = sock_;
struct sockaddr_in client_addr;
socklen_t addrlen = sizeof(client_addr);
getpeername(tcp_sock->socket, (struct sockaddr*) &client_addr, &addrlen);
uint32_t client_port = ntohs(client_addr.sin_port);
snprintf(name, sizeof(name), "%s:%d", inet_ntoa(client_addr.sin_addr), client_port);
printf("Accepted connection from %s\n", name);
tcp_ioc = g_io_channel_unix_new(ssocket_get_fd(tcp_sock));
tcp_sid = g_io_add_watch(tcp_ioc, G_IO_IN, on_tcp_data, this);
bytes_to_read = 4;
bytes_read = 0;
tunnel_state = CLIENT_MSG_SZ; //we're waiting for the client connect message
return 1;
}
char supla_client_connect(void *_suplaclient) {
TSuplaClientData *suplaclient = (TSuplaClientData *)_suplaclient;
supla_client_disconnect(_suplaclient);
supla_client_clean(_suplaclient);
if ( ssocket_client_connect(suplaclient->ssd, NULL) == 1 ) {
suplaclient->eh = eh_init();
TsrpcParams srpc_params;
srpc_params_init(&srpc_params);
srpc_params.user_params = _suplaclient;
srpc_params.data_read = &supla_client_socket_read;
srpc_params.data_write = &supla_client_socket_write;
srpc_params.on_remote_call_received = &supla_client_on_remote_call_received;
srpc_params.before_async_call = &supla_client_before_async_call;
srpc_params.eh = suplaclient->eh;
suplaclient->srpc = srpc_init(&srpc_params);
eh_add_fd(suplaclient->eh, ssocket_get_fd(suplaclient->ssd));
suplaclient->connected = 1;
supla_client_set_registered(_suplaclient, 0);
if ( suplaclient->cfg.cb_on_connected )
suplaclient->cfg.cb_on_connected(_suplaclient, suplaclient->cfg.user_data);
return 1;
}
return 0;
}
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_tcp_data(GIOChannel * source, GIOCondition cond, void *user_data)
{
int ret = TRUE;
LcmTunnel * self = (LcmTunnel*) user_data;
// increase buffer size if needed
if (self->buf_sz < self->bytes_to_read) {
assert(self->bytes_read == 0);
self->buf = (char *) realloc(self->buf, self->bytes_to_read);
self->buf_sz = self->bytes_to_read;
}
ssize_t nread = read(ssocket_get_fd(self->tcp_sock), self->buf + self->bytes_read, self->bytes_to_read
- self->bytes_read);
if (nread <= 0) {
perror("tcp receive error: ");
LcmTunnelServer::disconnectClient(self);
return FALSE;
}
self->bytes_read += nread;
assert(self->bytes_read <= self->bytes_to_read);
if (self->bytes_read != self->bytes_to_read)
return TRUE;
switch (self->tunnel_state) {
case CLIENT_MSG_SZ:
self->bytes_to_read = ntohl(*(uint32_t*) self->buf);
self->tunnel_state = CLIENT_MSG_DATA;
break;
case CLIENT_MSG_DATA:
{
lcm_tunnel_params_t tp_rec;
int decode_status = lcm_tunnel_params_t_decode(self->buf, 0, self->bytes_read, &tp_rec);
if (decode_status <= 0) {
fprintf(stdout, "invalid request (%d)\n", decode_status);
return FALSE;
}
self->tunnel_params = lcm_tunnel_params_t_copy(&tp_rec);
if (self->udp_fd >= 0) {
close(self->udp_fd);
}
self->udp_fd = -1;
if (self->tunnel_params->udp) {
//setup our UDP socket, and send info to client
struct sockaddr_in client_addr;
socklen_t addrlen = sizeof(client_addr);
getpeername(self->tcp_sock->socket, (struct sockaddr*) &client_addr, &addrlen);
self->server_udp_port = ntohs(client_addr.sin_port);
client_addr.sin_port = htons(self->tunnel_params->udp_port);
// allocate UDP socket
self->udp_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (self->udp_fd < 0) {
perror("allocating UDP socket");
LcmTunnelServer::disconnectClient(self);
return FALSE;
}
connect(self->udp_fd, (struct sockaddr*) &client_addr, sizeof(client_addr));
// transmit the udp port info
struct sockaddr_in udp_addr;
socklen_t udp_addr_len = sizeof(udp_addr);
memset(&udp_addr, 0, sizeof(udp_addr));
udp_addr.sin_family = AF_INET;
udp_addr.sin_addr.s_addr = INADDR_ANY;
udp_addr.sin_port = 0;
getsockname(self->udp_fd, (struct sockaddr*) &udp_addr, &udp_addr_len);
lcm_tunnel_params_t tp_port_msg;
tp_port_msg.channels = (char *) " ";
tp_port_msg.udp_port = ntohs(udp_addr.sin_port);
int msg_sz = lcm_tunnel_params_t_encoded_size(&tp_port_msg);
uint8_t msg[msg_sz];
lcm_tunnel_params_t_encode(msg, 0, msg_sz, &tp_port_msg);
uint32_t msg_sz_n = htonl(msg_sz);
if (4 != _fileutils_write_fully(ssocket_get_fd(self->tcp_sock), &msg_sz_n, 4)) {
perror("sending subscription data");
LcmTunnelServer::disconnectClient(self);
return FALSE;
}
if (msg_sz != _fileutils_write_fully(ssocket_get_fd(self->tcp_sock), msg, msg_sz)) {
perror("sending subscription data");
LcmTunnelServer::disconnectClient(self);
return FALSE;
}
self->udp_ioc = g_io_channel_unix_new(self->udp_fd);
self->udp_sid = g_io_add_watch(self->udp_ioc, G_IO_IN, LcmTunnel::on_udp_data, self);
//we're done setting up the UDP connection...Disconnect tcp socket
self->closeTCPSocket();
ret = false;
}
//get ready to receive
self->tunnel_state = RECV_CHAN_SZ;
self->bytes_to_read = 4;
// if (self->server_params->verbose)
fprintf(stderr, "%s subscribed to \"%s\" -- ", self->name, self->tunnel_params->channels);
if (self->udp_fd >= 0) {
if (self->tunnel_params->fec > 1)
fprintf(stderr, "UDP with FEC rate of %.2f and max_delay of %dms\n", self->tunnel_params->fec,
self->tunnel_params->max_delay_ms);
else if (self->tunnel_params->fec < -1)
fprintf(stderr, "UDP with DUP rate of %d and max_delay of %dms\n", (int) -self->tunnel_params->fec,
self->tunnel_params->max_delay_ms);
else
fprintf(stderr, "UDP with a max_delay of %dms\n", self->tunnel_params->max_delay_ms);
}
else {
fprintf(stderr, "TCP with max_delay of %dms and tcp_max_age_ms of %d\n", self->tunnel_params->max_delay_ms,
self->tunnel_params->tcp_max_age_ms);
}
self->init_regex(self->tunnel_params->channels);
//subscribe to the LCM channels
if (self->subscription) {
lcm_unsubscribe(self->lcm, self->subscription);
}
self->subscription = lcm_subscribe(self->lcm, self->tunnel_params->channels, on_lcm_message, self);
}
break;
case SERVER_MSG_SZ:
self->bytes_to_read = ntohl(*(uint32_t*) self->buf);
self->tunnel_state = SERVER_MSG_DATA;
break;
case SERVER_MSG_DATA:
{
lcm_tunnel_params_t tp_rec;
int decode_status = lcm_tunnel_params_t_decode(self->buf, 0, self->bytes_read, &tp_rec);
if (decode_status <= 0) {
fprintf(stderr, "invalid request (%d)\n", decode_status);
return FALSE;
}
assert(self->udp_fd>0);
struct sockaddr_in client_addr;
socklen_t addrlen = sizeof(client_addr);
getpeername(self->tcp_sock->socket, (struct sockaddr*) &client_addr, &addrlen);
self->server_udp_port = tp_rec.udp_port;
client_addr.sin_port = htons(tp_rec.udp_port);
//connect the udp socket
connect(self->udp_fd, (struct sockaddr*) &client_addr, sizeof(client_addr));
//now we can subscribe to LCM
fprintf(stderr, "%s subscribed to \"%s\" \n", self->name, self->tunnel_params->channels);
self->subscription = lcm_subscribe(self->lcm, self->tunnel_params->channels, on_lcm_message, self);
//we're done setting up the UDP connection...Disconnect tcp socket
self->closeTCPSocket();
ret = FALSE; //don't want the TCP handler to be run again
}
break;
case RECV_CHAN_SZ:
self->bytes_to_read = ntohl(*(uint32_t*) self->buf);
self->tunnel_state = RECV_CHAN;
if (self->channel_sz < self->bytes_to_read + 1) {
self->channel = (char *) realloc(self->channel, self->bytes_to_read + 1);
self->channel_sz = self->bytes_to_read + 1;
}
break;
case RECV_CHAN:
memcpy(self->channel, self->buf, self->bytes_read);
self->channel[self->bytes_read] = 0;
self->bytes_to_read = 4;
self->tunnel_state = RECV_DATA_SZ;
break;
case RECV_DATA_SZ:
self->bytes_to_read = ntohl(*(uint32_t*) self->buf);
self->tunnel_state = RECV_DATA;
break;
case RECV_DATA:
if (self->verbose)
printf("Recieved TCP message on channel \"%s\"\n", self->channel);
LcmTunnelServer::check_and_send_to_tunnels(self->channel, self->buf, self->bytes_read, self);
lcm_publish(self->lcm, self->channel, (uint8_t*) self->buf, self->bytes_read);
self->bytes_to_read = 4;
self->tunnel_state = RECV_CHAN_SZ;
break;
}
self->bytes_read = 0;
return ret;
}
int LcmTunnel::connectToServer(lcm_t * lcm_, introspect_t *introspect_, GMainLoop * mainloop_, char * server_addr_str,
int port, char * channels_to_recv, lcm_tunnel_params_t * tunnel_params_, tunnel_server_params_t * server_params_)
{ //for a client that should initiate a connection with a server
tunnel_params = lcm_tunnel_params_t_copy(tunnel_params_);
server_params = server_params_;
lcm = lcm_;
introspect = introspect_;
mainloop = mainloop_;
if (tunnel_params->udp) {
// allocate UDP socket
udp_fd = socket(AF_INET, SOCK_DGRAM, 0);
if (udp_fd < 0) {
perror("allocating UDP socket");
return 0;
}
struct sockaddr_in udp_addr;
socklen_t udp_addr_len = sizeof(udp_addr);
memset(&udp_addr, 0, sizeof(udp_addr));
udp_addr.sin_family = AF_INET;
udp_addr.sin_addr.s_addr = INADDR_ANY;
udp_addr.sin_port = 0;
if (bind(udp_fd, (struct sockaddr*) &udp_addr, sizeof(udp_addr)) < 0) {
perror("binding UDP socket");
return 0;
}
getsockname(udp_fd, (struct sockaddr*) &udp_addr, &udp_addr_len);
tunnel_params->udp_port = ntohs(udp_addr.sin_port);
udp_ioc = g_io_channel_unix_new(udp_fd);
udp_sid = g_io_add_watch(udp_ioc, G_IO_IN, LcmTunnel::on_udp_data, this);
}
else {
udp_fd = -1;
}
// connect
tcp_sock = ssocket_create();
if (0 != ssocket_connect(tcp_sock, server_addr_str, port)) {
perror("connecting");
return 0;
}
tcp_ioc = g_io_channel_unix_new(ssocket_get_fd(tcp_sock));
tcp_sid = g_io_add_watch(tcp_ioc, G_IO_IN, on_tcp_data, this);
//fill out the name info
struct sockaddr_in server_addr;
socklen_t addrlen = sizeof(server_addr);
getpeername(tcp_sock->socket, (struct sockaddr*) &server_addr, &addrlen);
uint32_t server_port = ntohs(server_addr.sin_port);
snprintf(name, sizeof(name), "%s:%d", inet_ntoa(server_addr.sin_addr), server_port);
fprintf(stderr, "Connected to %s\n", name);
// transmit subscription information
lcm_tunnel_params_t * tun_params_to_send = lcm_tunnel_params_t_copy(tunnel_params);
//put the channels the server should send in the params we're sending it.
free(tun_params_to_send->channels);
tun_params_to_send->channels = strdup(channels_to_recv);
int msg_sz = lcm_tunnel_params_t_encoded_size(tun_params_to_send);
uint8_t * msg = (uint8_t *) calloc(msg_sz, sizeof(uint8_t));
lcm_tunnel_params_t_encode(msg, 0, msg_sz, tun_params_to_send);
uint32_t msg_sz_n = htonl(msg_sz);
if (4 != _fileutils_write_fully(ssocket_get_fd(tcp_sock), &msg_sz_n, 4)) {
perror("sending subscription data");
ssocket_destroy(tcp_sock);
free(msg);
lcm_tunnel_params_t_destroy(tun_params_to_send);
return 0;
}
if (msg_sz != _fileutils_write_fully(ssocket_get_fd(tcp_sock), msg, msg_sz)) {
perror("sending subscription data");
ssocket_destroy(tcp_sock);
free(msg);
lcm_tunnel_params_t_destroy(tun_params_to_send);
return 0;
}
lcm_tunnel_params_t_destroy(tun_params_to_send);
free(msg);
//set state for tcp receptions
bytes_to_read = 4;
bytes_read = 0;
if (tunnel_params->udp) {
tunnel_state = SERVER_MSG_SZ; //wait for udp port from server
}
else {
tunnel_state = RECV_CHAN_SZ;
//subscribe to the channels we want to send out
//only subscribe if we're doing TCP, since UDP socket hasn't been setup yet
subscription = lcm_subscribe(lcm, tunnel_params->channels, on_lcm_message, this);
}
return 1;
}
