ssocket_t *
ssocket_accept (ssocket_t *s)
{
int thesocket;
ssocket_t *cs = ssocket_create ();
s->addrlen = sizeof(struct sockaddr);
if (s->type != SSOCKET_SERVER_TYPE) {
printf ("not server type\n");
ssocket_destroy (cs);
return NULL;
}
/* actually accept the connection */
if ((thesocket = accept (s->socket,&s->addr,&s->addrlen))<0) {
perror ("accept failed");
ssocket_destroy (cs);
return NULL;
}
cs->type = SSOCKET_CLIENT_TYPE;
cs->socket = thesocket;
return cs;
}
void LcmTunnel::closeTCPSocket()
{
// fprintf(stderr, "closing TCP socket\n");
if (tcp_sock != NULL)
ssocket_destroy(tcp_sock);
tcp_sock = NULL;
if (tcp_ioc != NULL)
g_io_channel_unref(tcp_ioc);
tcp_ioc = NULL;
if (tcp_sid > 0)
g_source_remove(tcp_sid);
tcp_sid = -1;
}
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;
}
