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; }