void
Comm::handle_comm (struct sockaddr_in &addr, const char *msg, int len)
throw()
{
if (len == static_cast<int>(strlen(TOOL_REQUEST_MSG)) &&
memcmp(msg, TOOL_REQUEST_MSG, TOOL_REQUEST_LEN) == 0) {
std::string robotName = getRobotName();
const char *name = robotName.c_str();
int len = TOOL_ACCEPT_LEN + 3 + strlen(name);
char *response = (char *) malloc(len);
memcpy(&response[0], TOOL_ACCEPT_MSG, TOOL_ACCEPT_LEN);
response[TOOL_ACCEPT_LEN] = ':';
response[TOOL_ACCEPT_LEN+1] = (char)strlen(name);
response[TOOL_ACCEPT_LEN+2] = ':';
memcpy(&response[TOOL_ACCEPT_LEN+3], name, strlen(name));
struct sockaddr_in r_addr = addr;
r_addr.sin_port = htons(TOOL_PORT);
send(&response[0], len, r_addr);
free(response);
}else {
// validate packet format, check packet timestamp, and parse data
CommPacketHeader packet;
if (validate_packet(msg, len, packet) && timer.check_packet(packet))
parse_packet(packet, msg + sizeof(packet), len - sizeof(packet));
}
}
int main(int argc, char *argv[])
{
int sockfd;
int pkt_size = 3000;
int opt;
int addr_family = AF_INET6; /* Default address family */
uint16_t dest_port = PORT;
char *dest_ip;
int len_send, len_recv;
char buf_send[65535], buf_recv[65535];
/* Adding support for both IPv4 and IPv6 */
struct sockaddr_storage dest_addr; /* Can contain both sockaddr_in and sockaddr_in6 */
memset(&dest_addr, 0, sizeof(dest_addr));
while ((opt = getopt(argc, argv, "s:64v:p:")) != -1) {
if (opt == 's') pkt_size = atoi(optarg);
if (opt == '4') addr_family = AF_INET;
if (opt == '6') addr_family = AF_INET6;
if (opt == 'v') verbose = atoi(optarg);
if (opt == 'p') dest_port = atoi(optarg);
}
if (optind >= argc) {
fprintf(stderr, "Expected dest IP-address (IPv6 or IPv4) argument after options\n");
exit(2);
}
dest_ip = argv[optind];
if (verbose > 0)
printf("Destination IP:%s port:%d\n", dest_ip, dest_port);
sockfd = socket(addr_family, SOCK_DGRAM, 0);
/* Socket options, see man-pages ip(7) and ipv6(7) */
//int set_pmtu_disc = IP_PMTUDISC_DO; /* do PMTU = Don't Fragment */
int set_pmtu_disc = IP_PMTUDISC_DONT; /* Allow fragments, dont do PMTU */
Setsockopt(sockfd, IPPROTO_IP, IP_MTU_DISCOVER, &set_pmtu_disc, sizeof(int));
Setsockopt(sockfd, IPPROTO_IPV6, IPV6_MTU_DISCOVER, &set_pmtu_disc, sizeof(int));
/* Setup dest_addr depending on IPv4 or IPv6 address */
setup_sockaddr(addr_family, &dest_addr, dest_ip, dest_port);
/* Connect to recv ICMP error messages */
connect(sockfd, (struct sockaddr *)&dest_addr,
sockaddr_len(&dest_addr));
len_send = send_packet(sockfd, &dest_addr, buf_send, pkt_size);
len_recv = recv_packet(sockfd, &dest_addr, buf_recv, len_send);
validate_packet(len_send, len_recv, buf_send, buf_recv);
}
void ctrl_cb(evutil_socket_t sock, short ev, void *arg) {
UNUSED(ev);
UNUSED(arg);
struct proto_header header;
ssize_t r;
struct sockaddr_in addr;
socklen_t addr_len = sizeof(addr);
TRY_SYS(r = recvfrom(sock, &header, sizeof(header), 0,
(struct sockaddr *) &addr, &addr_len));
if (validate_packet((struct proto_packet *) &header, r) && header_isempty(&header)) {
if (header_flag_isset(&header, PROTO_RETQUERY)) {
/* mark packet in buffer with PROTO_DORETR flag */
seqno_t ask_seqno = header_seqno(&header);
struct proto_packet *pack = sendbuff_getseqno(&packets, ask_seqno);
if (pack) {
header_flag_set(&pack->header, PROTO_DORETR);
} else {
/* invalid retransmission request - response directly to requester */
header_init(&pack_ret_failed.header, ask_seqno, 0, PROTO_FAIL);
EXPECT(sendto(ctrl_sock, &pack_ret_failed, sizeof(pack_ret_failed), 0,
(struct sockaddr *) &addr, addr_len) == sizeof(pack_ret_failed),
"Sending invalid retransmission response failed.");
}
}
/* whatever we've done addr is still valid */
if (header_flag_isset(&header, PROTO_IDQUERY)) {
EXPECT(sendto(sock, &pack_my_ident, sizeof(pack_my_ident), 0, (struct sockaddr *)
&addr, addr_len) == sizeof(pack_my_ident),
"Sending id response failed.");
}
}
/* else: ignore packet */
}
/**
* Request NAKs from all clients.
* Returns the aggregate number of NAKs for the section.
*/
int get_naks(struct finfo_t *finfo, unsigned int pass, unsigned int section,
int *alldone)
{
unsigned char *packet, *decrypted;
struct uftp_h *header;
struct timeval timeout;
struct sockaddr_in receiver;
int resend, attempt, last_pass, gotall, anyerror;
int blocks_this_sec, section_offset;
int rcv_status, len, nakidx, numnaks, i, j;
time_t endtime;
packet = calloc(mtu, 1);
decrypted = calloc(mtu, 1);
if ((packet == NULL) || (decrypted == NULL)) {
syserror(0, 0, "calloc failed!");
exit(1);
}
header = (struct uftp_h *)packet;
section_offset = (blocksize * 8) * (section - 1);
blocks_this_sec = ((section < finfo->sections) ? (blocksize * 8) :
(finfo->blocks % (blocksize * 8)));
if (finfo->sections && !blocks_this_sec) blocks_this_sec = blocksize * 8;
for (i = 0; i < blocks_this_sec; i++) {
nakidx = i + section_offset;
finfo->naklist[nakidx] = 0;
}
for (i = 0; i < destcount; i++) {
if (client_error(i)) {
continue;
}
if (destlist[i].clientcnt != -1) {
destlist[i].status = DEST_ACTIVE;
} else if (destlist[i].status == DEST_ACTIVE) {
destlist[i].status = DEST_STATUS;
}
free(destlist[i].last_status);
destlist[i].last_status = NULL;
for (j = 0; j < destlist[i].last_prstatus_cnt; j++) {
free(destlist[i].last_prstatus[j]);
destlist[i].last_prstatus[j] = NULL;
}
destlist[i].last_prstatus_cnt = 0;
}
endtime = time(NULL) + status_time;
timeout.tv_sec = status_int / 1000;
timeout.tv_usec = (status_int % 1000) * 1000;
resend = 1;
attempt = 1;
gotall = 0;
last_pass = 0;
while ((time(NULL) < endtime) && (!gotall)) {
if (resend) {
if (!send_doneconf(finfo, attempt)) {
continue;
}
if (!send_done(finfo, attempt, pass, section)) {
continue;
}
resend = 0;
}
// See comments in announce_phase regarding timing
if ((rcv_status = read_packet(sock, &receiver, packet, &len,
mtu, &timeout)) == -1) {
continue;
} else if (rcv_status == 0) {
attempt++;
resend = 1;
if (last_pass) break;
continue;
}
if (!validate_packet(packet, len, finfo)) {
continue;
}
if (!handle_transfer_phase(packet, decrypted, &receiver,
blocks_this_sec, section_offset, pass, section, finfo)) {
continue;
}
for (i = 0, gotall = 1, *alldone = 1;
(i < destcount) && (gotall || *alldone); i++) {
gotall = gotall && (destlist[i].status != DEST_STATUS);
*alldone = *alldone && ((destlist[i].status == DEST_DONE) ||
(client_error(i)) || (destlist[i].clientcnt != -1));
}
if (*alldone) {
if (finfo->file_id != 0) break;
// Change the wait interval to the client's done_int * 1.5
// to allow for late completions
timeout.tv_sec = (int)(done_int / 1000 * 1.5);
timeout.tv_usec = (int)((done_int % 1000) * 1000 * 1.5);
if (!last_pass) {
log(0, 0, "Late completions:");
}
last_pass = 1;
gotall = 0;
send_doneconf(finfo, attempt + 1);
}
}
anyerror = 0;
if (*alldone) {
send_doneconf(finfo, attempt + 1);
} else if (!gotall) {
for (i = 0, *alldone = 1; i < destcount; i++) {
if (destlist[i].status == DEST_STATUS) {
log1(0, 0, "Couldn't get STATUS from %s", destlist[i].name);
destlist[i].status = DEST_LOST;
anyerror = 1;
}
*alldone = *alldone && ((destlist[i].status == DEST_DONE) ||
(client_error(i)) || (destlist[i].clientcnt != -1));
}
}
if (anyerror && quit_on_error) {
log0(0, 0, "Aboring all clients");
send_abort(finfo, "A client dropped out, aborting all",
&receive_dest, NULL, (keytype != KEY_NONE), 0);
for (i = 0; i < destcount; i++) {
if (destlist[i].status == DEST_ACTIVE) {
destlist[i].status = DEST_ABORT;
}
}
*alldone = 1;
}
for (i = 0, numnaks = 0; i < blocks_this_sec; i++) {
nakidx = i + section_offset;
if (finfo->naklist[nakidx]) {
numnaks++;
}
}
free(packet);
free(decrypted);
return numnaks;
}
/**
* Perform the Announce/Register phase for a particular group/file
* Group & encryption: ->ANNOUNCE <-REGISTER ->KEYINFO <-INFO_ACK
* Group & no encryption: ->ANNOUNCE <-REGISTER ->REG_CONF
* Files within a group: ->FILEINFO <-INFO_ACK
* If client_key == 1, REGISTER is followed by CLIENT_KEY
* Returns 1 if at least one client responded, 0 if none responded
*/
int announce_phase(struct finfo_t *finfo)
{
time_t endtime;
int attempt, resend, announce, regconf, keyinfo, fileinfo, open, anyerror;
int len, rval, rcv_status, last_pass, gotall, gotone, allreg, regdone, i;
unsigned char *packet, *decrypted;
struct uftp_h *header;
struct timeval timeout;
struct sockaddr_in receiver;
if (finfo->file_id) {
log1(0, 0, "File ID: %04X Name: %s", finfo->file_id, finfo->filename);
log1(0, 0, " sending as: %s", finfo->destfname);
switch (finfo->ftype) {
case FTYPE_REG:
log(0, 0, "Bytes: %s Blocks: %d Sections: %d",
printll(finfo->size), finfo->blocks, finfo->sections);
break;
case FTYPE_DIR:
log(0, 0, "Empty directory");
break;
case FTYPE_LINK:
log(0, 0, "Symbolic link to %s", finfo->linkname);
break;
}
} else {
log(0, 0, "Initializing group");
if (sync_mode) {
log0(0, 0, "- Connect -");
}
}
rval = 1;
packet = calloc(mtu, 1);
decrypted = calloc(mtu, 1);
if ((packet == NULL) || (decrypted == NULL)) {
syserror(0, 0, "calloc failed!");
exit(1);
}
header = (struct uftp_h *)packet;
endtime = time(NULL) + announce_time;
announce = (finfo->file_id == 0);
regconf = (announce && (keytype == KEY_NONE));
keyinfo = (announce && (keytype != KEY_NONE));
fileinfo = (finfo->file_id != 0);
open = (destcount == 0);
for (i = 0; i < destcount; i++) {
// At start of group, initialize all clients/proxies to DEST_MUTE.
// At start of file, initialize proxies to DEST_ACTIVE (since they
// don't respond directly to a FILEINFO) and clients to DEST_REGISTERED.
if (announce) {
destlist[i].status = DEST_MUTE;
} else if (!client_error(i)) {
if (destlist[i].clientcnt != -1) {
destlist[i].status = DEST_ACTIVE;
} else {
destlist[i].status = DEST_REGISTERED;
}
}
}
timeout.tv_sec = announce_int / 1000;
timeout.tv_usec = (announce_int % 1000) * 1000;
resend = 1;
attempt = 1;
last_pass = 0;
regdone = 0;
while (time(NULL) < endtime) {
// On the initial pass, or when the announce timeout trips,
// send any necessary messages.
if (resend) {
if (keyinfo && !send_keyinfo(finfo, attempt)) {
continue;
}
if (announce && !send_regconf(finfo, attempt, regconf)) {
continue;
}
if (fileinfo && !send_fileinfo(finfo, attempt)) {
continue;
}
if (announce && !send_announce(finfo, attempt, open)) {
continue;
}
resend = 0;
}
// TODO: Currently, the interval between sends is really an inactivity
// timer, not the actual time between sends. We might want to change
// it to that, and perhaps add an extra "overage" timer in case we're
// still processing responses when we're due to resend, that way we'll
// always wait some minimum amount of time.
if ((rcv_status = read_packet(sock, &receiver, packet, &len,
mtu, &timeout)) == -1) {
continue;
} else if (rcv_status == 0) {
attempt++;
resend = 1;
if (last_pass) break;
continue;
}
if (!validate_packet(packet, len, finfo)) {
continue;
}
if (!handle_announce_phase(packet, decrypted, &receiver, finfo,
announce, open, regconf)) {
continue;
}
if (!open) {
for (i = 0, gotall = 1, allreg = 1;
(i < destcount) && (gotall || allreg); i++) {
if (announce) {
gotall = gotall && ((destlist[i].status == DEST_ACTIVE) ||
(destlist[i].status == DEST_ABORT));
allreg = allreg && ((destlist[i].status == DEST_ACTIVE) ||
(destlist[i].status == DEST_REGISTERED) ||
(destlist[i].status == DEST_ABORT));
} else {
gotall = gotall && ((destlist[i].status == DEST_ACTIVE) ||
(destlist[i].status == DEST_DONE) ||
(client_error(i)));
}
}
if (gotall) {
// Break out right away if this is a file registration.
// For group registration, do one last wait, even if
// encryption is enabled since we can still send a
// REG_CONF for a client behind a proxy.
// Change the wait interval to the client's register_int * 1.5
// to allow for late registers.
// Be careful not to overrun the phase timeout!
if (finfo->file_id != 0) break;
timeout.tv_sec = (int)(register_int / 1000 * 1.5);
timeout.tv_usec = (int)((register_int % 1000) * 1000 * 1.5);
if (timeout.tv_sec > endtime) {
#ifdef WINDOWS
timeout.tv_sec = (long)endtime;
#else
timeout.tv_sec = endtime;
#endif
timeout.tv_usec = 0;
}
if (!last_pass) {
log(0, 0, "Late registers:");
}
last_pass = 1;
send_regconf(finfo, attempt + 1, regconf);
} else if (announce && allreg && !regdone) {
// All have registered, so don't wait to send the next message
resend = 1;
regdone = 1;
}
}
}
for (i = 0, gotone = 0, anyerror = 0; i < destcount; i++) {
gotone = gotone || (((destlist[i].status == DEST_ACTIVE) ||
(destlist[i].status == DEST_DONE)) &&
(destlist[i].clientcnt == -1));
if (destlist[i].status == DEST_REGISTERED) {
log1(0, 0, "Couldn't get INFO_ACK from %s", destlist[i].name);
destlist[i].status = DEST_LOST;
anyerror = 1;
}
if ((destlist[i].status == DEST_MUTE) ||
(destlist[i].status == DEST_ABORT)) {
anyerror = 1;
}
}
if (anyerror && quit_on_error) {
log0(0, 0, "Aboring all clients");
send_abort(finfo, "A client dropped out, aborting all",
&receive_dest, NULL, (keytype != KEY_NONE), 0);
for (i = 0; i < destcount; i++) {
if (destlist[i].status == DEST_ACTIVE) {
destlist[i].status = DEST_ABORT;
}
}
rval = 0;
}
if (!gotone) {
log0(0, 0, "Announce timed out");
rval = 0;
}
if (open) {
send_regconf(finfo, attempt, regconf);
}
if ((finfo->file_id == 0) && sync_mode) {
for (i = 0; i < destcount; i++) {
if (destlist[i].status == DEST_ACTIVE) {
log0(0, 0, "CONNECT;success;%s", destlist[i].name);
} else {
log0(0, 0, "CONNECT;failed;%s", destlist[i].name);
}
}
log0(0, 0, "- Transfer -");
}
free(packet);
free(decrypted);
return rval;
}
void process_inbound_udp(int sock) {
struct sockaddr_in from;
socklen_t fromlen;
char buf[MAX_PACKET_LENGTH];
fromlen = sizeof(from);
int read_result = spiffy_recvfrom(sock, buf, MAX_PACKET_LENGTH, 0, (struct sockaddr *) &from, &fromlen);
printf("read %d bytes\n", read_result);
//printf("incoming message from %s:%d\n", inet_ntoa(from.sin_addr), ntohs(from.sin_port));
/* we just assume we got one packet everytime
* if there are multiple packets in the buffer (whichi is rare), we just ignore them,
* and everything will just work fine.
*/
struct network_packet_s* network_packet = (struct network_packet_s*)buf;
net_to_host_header(& network_packet->header);
print_packet_header(&network_packet->header);
if(validate_packet(& network_packet->header) < 0){
printf("packet is invalid, skip\n");
return;
}
int packet_type = get_packet_type(& network_packet->header);
/* get the peer who send this packet*/
bt_peer_t* peer = search_bt_peer(&config, (struct sockaddr *)&from);
switch(packet_type) {
case TYPE_WHOHAS: {
struct network_packet_s* ihave_packet = malloc_ihave_packet(network_packet);
/* this node has no such chunk */
if(ihave_packet == NULL){
return;
}
else{
//print_ihave_packet(ihave_packet);
send_packet(ihave_packet, global_socket, (struct sockaddr *) &from);
free(ihave_packet);
}
break;
}
case TYPE_IHAVE: {
if(current_job == NULL){
return;
}
/* receiving a new IHAVE packet */
/* check whether there is alreay a connection with this peer */
struct receiver_connection_s* existing_connection =
search_receiver_connection(receiver_connection_head, peer);
if(existing_connection != NULL){
/* there is alreay a connection */
add_ihave_to_receiver_connection(existing_connection, network_packet);
}
/* there is no such connection */
else{
/* check if the number of connection reached the maximum */
if(get_receiver_connection_number(receiver_connection_head) >=
max_receiver_connection){
return;
}
/* add add the hash whose chunk is in a CHUNK_STATUS_NOT_FOUND status
* to a new connection
*/
else{
struct receiver_connection_s* new_connection =
malloc_receiver_connection(network_packet, peer);
/* if every chunk has a provider, the new_connection is NULL */
if(new_connection != NULL){
add_receiver_connection(new_connection);
/* start the new connection */
struct network_packet_s* get_packet = start_connection(new_connection);
//print_get_packet(get_packet);
free(get_packet);
}
}
}
//print_receiver_connection_list(receiver_connection_head);
break;
}
case TYPE_GET: {
//print_get_packet(network_packet);
/* check whether there is alreay a connection with this peer */
if(search_provider_connection(provider_connection_head, peer) != NULL){
printf("Provider connection already exists with peer %d , ignore GET\n", peer->id);
return;
}
/* do nothing is the number of connection reached the maximum*/
if(get_provider_connection_number(provider_connection_head) >=
max_provider_connection){
printf("Provider connection reached maximum with peer %d \n", peer->id);
return;
}
struct provider_connection_s* new_connection =
malloc_provider_connection(network_packet, peer);
if(new_connection != NULL){
printf("Add new provider connection with peer %d \n", peer->id);
add_provider_connection(new_connection);
//print_provider_connection(new_connection);
}
//print_provider_connection_list(provider_connection_head);
break;
}
case TYPE_DATA: {
print_data_packet(network_packet);
printf("received data packet, seq: %d, ack: %d\n ",
network_packet->header.seq_number,
network_packet->header.ack_number);
/* check whether there is a connection with this peer */
struct receiver_connection_s* receiver_connection =
search_receiver_connection(receiver_connection_head, peer);
/* connection does not exist, ignore the data packet */
if(receiver_connection == NULL){
return;
}
int existing_seq_num = receiver_connection->chunk_list_head->chunk->received_seq_num;
printf("expected data packet, seq: %d \n", 1 + existing_seq_num);
int packet_seq_num = network_packet->header.seq_number;
/* sequence number is illegal */
if(packet_seq_num < 0){
return;
}
/* old packet arrived, do nothing */
else if(packet_seq_num < (existing_seq_num + 1)){
return;
}
/* latter packet arrived first, send duplicate ACK */
else if(packet_seq_num > (existing_seq_num + 1)){
struct network_packet_s* ack_packet = malloc_ack_packet(existing_seq_num);
send_packet(ack_packet, global_socket, (struct sockaddr *) &from);
free(ack_packet);
return;
}
/* the packet expected */
else{
gettimeofday(&receiver_connection->last_data_time, NULL);
receiver_connection->status = RECEIVER_STATUS_RECEIVED_DATA;
struct network_packet_s* ack_packet = malloc_ack_packet(1 + existing_seq_num);
send_packet(ack_packet, global_socket, (struct sockaddr *) &from);
free(ack_packet);
/* save_data_packet */
save_data_packet(network_packet, receiver_connection);
/* save the downloading chunk of the connnection, if it finihsed */
save_chunk(receiver_connection);
/* continue to download next chunk, if finished first one */
reset_receiver_connection(receiver_connection);
}
break;
}
case TYPE_ACK: {
//print_ack_packet(network_packet);
struct provider_connection_s* provider_connection =
search_provider_connection(provider_connection_head, peer);
if(provider_connection == NULL){
return;
}
provider_control_by_ack(provider_connection,
network_packet->header.ack_number);
/* check if the data has been all send to a receiver */
if(provider_connection->last_packet_acked == CHUNK_DATA_NUM){
/* download finished */
finish_provider_connection(provider_connection);
}
break;
}
case TYPE_DENIED: {
break;
}
default:{
break;
}
}
}
int vrpn_Tracker_Crossbow::get_report() {
struct timeval timeout;
timeout.tv_sec = 0;
timeout.tv_usec = 500000; // Half a second
switch (status) {
case vrpn_TRACKER_AWAITING_STATION:
fprintf(stderr, "vrpn_Tracker_Crossbow: sanity: should never enter AWAITING_STATION state\n");
return 0;
case vrpn_TRACKER_SYNCING: {
int rv;
// Quit early if we just got a packet
if (just_read_something) {
just_read_something = 0;
return 0;
}
// Request a packet from the device
unsigned char echo = 'G';
vrpn_write_characters(serial_fd, &echo, 1);
rv = vrpn_read_available_characters(serial_fd, buffer, 1, &timeout);
// Return early if no characters are available
if (!rv)
return 0;
// Return early if we aren't at the start of a packet header
if (*buffer != 0xAA) {
return 0;
}
bufcount = 1;
status = vrpn_TRACKER_PARTIAL;
// Fall through to next state
}
case vrpn_TRACKER_PARTIAL: {
int rv;
if (bufcount == 1) {
rv = vrpn_read_available_characters(serial_fd, buffer + 1, 1, &timeout);
if (!rv || (buffer[1] != 0x55)) {
buffer[0] = 0;
bufcount = 0;
status = vrpn_TRACKER_SYNCING;
return 0;
}
bufcount++;
}
// Try to read the rest of the packet. Return early if we haven't got a full packet yet.
rv = vrpn_read_available_characters(serial_fd, buffer + bufcount, 24 - bufcount, &timeout);
bufcount += rv;
if (bufcount < 24)
return 0;
raw_packet new_data;
unbuffer_packet(new_data, &buffer[0]);
// Ensure the packet is valid
if (validate_packet(new_data)) {
vrpn_flush_input_buffer(serial_fd);
status = vrpn_TRACKER_SYNCING;
return 0;
}
// Prepare the new report
process_packet(new_data);
bufcount = 0;
memset(buffer, 0, 24);
status = vrpn_TRACKER_SYNCING;
just_read_something = 1;
return 1;
}
default:
fprintf(stderr, "vrpn_Tracker_Crossbow: sanity: unknown tracker state\n");
return 0;
}
}
