tor_multipath_voip/src/packet.c

168 lines
5.2 KiB
C
Raw Normal View History

#include "packet.h"
2019-05-09 17:53:47 +00:00
size_t get_full_size(struct buffer_packet* bp) {
2019-05-14 09:23:23 +00:00
union abstract_packet* ap = &bp->ip.ap;
2019-05-09 17:53:47 +00:00
for (int i = 0; i < bp->ap_count; i++) {
2019-05-14 09:23:23 +00:00
ap = (union abstract_packet*)(&ap->raw + ap->fmt.headers.size);
2019-05-09 17:53:47 +00:00
}
return &ap->raw - &bp->ip.ap.raw;
}
enum FD_STATE read_packet_from_tcp(int fd, struct buffer_packet* bp) {
2019-02-14 14:40:05 +00:00
ssize_t nread;
2019-05-14 09:23:23 +00:00
size_t pkt_size_size = sizeof(bp->ip.ap.fmt.headers.size);
2019-02-18 16:51:13 +00:00
if (bp->mode != BP_READING) return FDS_ERR;
while (bp->aread < pkt_size_size) {
nread = read(fd, &(bp->ip.ap.raw) + bp->aread, pkt_size_size - bp->aread);
if (nread == 0) return FDS_AGAIN;
if (nread == -1 && errno == EAGAIN) return FDS_AGAIN;
if (nread == -1) return FDS_ERR;
bp->aread += nread;
}
2019-05-14 09:23:23 +00:00
while (bp->aread < bp->ip.ap.fmt.headers.size) {
nread = read(fd, &(bp->ip.ap.raw) + bp->aread, bp->ip.ap.fmt.headers.size - bp->aread);
if (nread == 0) return FDS_AGAIN;
if (nread == -1 && errno == EAGAIN) return FDS_AGAIN;
if (nread == -1) return FDS_ERR;
bp->aread += nread;
}
2019-02-18 20:55:53 +00:00
bp->mode = BP_WRITING;
bp->awrite = 0;
2019-05-09 17:53:47 +00:00
bp->ap_count = 1;
return FDS_READY;
}
enum FD_STATE write_packet_to_tcp(int fd, struct buffer_packet* bp) {
2019-02-14 14:40:05 +00:00
ssize_t nwrite;
2019-06-03 13:14:12 +00:00
//dump_buffer_packet (bp);
2019-02-18 16:51:13 +00:00
if (bp->mode != BP_WRITING) return FDS_ERR;
2019-05-09 17:53:47 +00:00
while (bp->awrite < get_full_size(bp)) {
nwrite = send(fd, &(bp->ip.ap.raw) + bp->awrite, get_full_size(bp) - bp->awrite, 0);
if (nwrite == -1 && errno == EAGAIN) return FDS_AGAIN;
if (nwrite == -1) return FDS_ERR;
bp->awrite += nwrite;
}
2019-02-18 20:55:53 +00:00
bp->mode = BP_READING;
bp->aread = 0;
2019-05-09 17:53:47 +00:00
bp->ap_count = 0;
return FDS_READY;
}
2019-02-19 13:49:44 +00:00
enum FD_STATE write_packet_to_udp(int fd, struct buffer_packet* bp, struct udp_target* udp_t) {
2019-02-14 16:15:13 +00:00
ssize_t nwrite;
size_t bytes_to_send;
2019-05-14 12:48:40 +00:00
assert(bp->ip.ap.fmt.headers.cmd == CMD_CLEAR);
2019-05-14 09:23:23 +00:00
size_t pkt_header_size = sizeof(bp->ip.ap.fmt.headers) + sizeof(bp->ip.ap.fmt.content.clear) - sizeof(char);
2019-02-19 13:54:42 +00:00
struct sockaddr* addr = NULL;
socklen_t addrlen = 0;
if (udp_t->set) {
addr = (struct sockaddr*) &udp_t->addr;
addrlen = sizeof(struct sockaddr_in);
}
2019-02-18 16:51:13 +00:00
if (bp->mode != BP_WRITING) return FDS_ERR;
2019-05-14 09:23:23 +00:00
bytes_to_send = bp->ip.ap.fmt.headers.size - pkt_header_size;
nwrite = sendto(fd,
2019-05-14 09:23:23 +00:00
&(bp->ip.ap.fmt.content.clear.payload),
bytes_to_send,
0,
2019-02-19 13:54:42 +00:00
addr,
addrlen);
2019-02-14 16:15:13 +00:00
if (nwrite == -1 && errno == EAGAIN) return FDS_AGAIN;
if (nwrite != bytes_to_send) return FDS_ERR;
2019-02-18 20:55:53 +00:00
bp->mode = BP_READING;
bp->aread = 0;
2019-05-09 17:53:47 +00:00
bp->ap_count = 0;
return FDS_READY;
}
2019-02-19 13:49:44 +00:00
enum FD_STATE read_packet_from_udp (int fd, struct buffer_packet* bp, struct udp_target* udp_t) {
2019-02-14 14:40:05 +00:00
ssize_t nread;
2019-05-23 20:53:00 +00:00
if (bp->mode != BP_READING) {
2019-05-24 08:52:49 +00:00
fprintf(stderr, "Buffer packet is not in reading mode (mode: %d)\n", bp->mode);
2019-05-23 20:53:00 +00:00
return FDS_ERR;
}
2019-05-14 09:23:23 +00:00
size_t pkt_header_size = sizeof(bp->ip.ap.fmt.headers) + sizeof(bp->ip.ap.fmt.content.clear) - sizeof(char); // We remove the payload
2019-02-18 14:11:12 +00:00
size_t udp_packet_size = sizeof(struct internet_packet) - pkt_header_size;
2019-02-19 13:49:44 +00:00
socklen_t addrlen = sizeof(struct sockaddr_in);
nread = recvfrom(fd,
2019-05-14 09:23:23 +00:00
&(bp->ip.ap.fmt.content.clear.payload),
udp_packet_size,
MSG_TRUNC,
2019-02-19 13:49:44 +00:00
(struct sockaddr*)&udp_t->addr,
&addrlen);
2019-05-24 08:52:49 +00:00
if ((int)nread > (int)udp_packet_size) {
fprintf(stderr, "Packet has been truncated (%ld instead of %d)\n", nread, (int)udp_packet_size);
return FDS_ERR;
}
if (nread == -1 && errno == EAGAIN) return FDS_AGAIN;
2019-05-23 20:36:26 +00:00
if (nread == -1 && errno == EAGAIN) return FDS_AGAIN;
2019-05-24 08:52:49 +00:00
if (nread == -1) {
fprintf(stderr, "A system error occured\n");
return FDS_ERR;
}
2019-02-19 13:49:44 +00:00
udp_t->set = 1;
udp_t->addrlen = addrlen;
2019-05-14 09:23:23 +00:00
bp->ip.ap.fmt.headers.size = nread + pkt_header_size;
bp->ip.ap.fmt.headers.cmd = CMD_CLEAR;
2019-02-18 20:55:53 +00:00
bp->mode = BP_WRITING;
bp->awrite = 0;
2019-05-09 17:53:47 +00:00
bp->ap_count = 1;
return FDS_READY;
}
2019-05-24 09:46:23 +00:00
void dump_buffer_packet(struct buffer_packet* bp) {
printf("<Buffer Packet>\n");
2019-05-24 13:07:21 +00:00
printf(" mode=%d, aread=%d, awrite=%d, ap_count=%d, usage=%ld/%ld\n", bp->mode, bp->aread, bp->awrite, bp->ap_count, get_full_size (bp), sizeof(struct internet_packet));
2019-05-24 09:46:23 +00:00
union abstract_packet* ap = &bp->ip.ap;
for (int i = 0; i < bp->ap_count; i++) {
dump_abstract_packet(ap);
ap = (union abstract_packet*)(&ap->raw + ap->fmt.headers.size);
}
printf("</Buffer Packet>\n");
}
void dump_abstract_packet(union abstract_packet* ap) {
2019-05-24 09:49:00 +00:00
printf(" <Abstract Packet>\n");
printf(" size=%d, cmd=%d\n", ap->fmt.headers.size, ap->fmt.headers.cmd);
2019-05-24 09:46:23 +00:00
switch (ap->fmt.headers.cmd) {
case CMD_HEALTH:
printf(" <Health>id=%d, deltat=%d, prevlink=%d, min_blocked_pkt=%d, bitfield=%02x</Health>\n",
2019-05-24 09:46:23 +00:00
ap->fmt.content.health.id,
ap->fmt.content.health.deltat,
ap->fmt.content.health.prevlink,
ap->fmt.content.health.min_blocked_pkt,
ap->fmt.content.health.bitfield);
break;
case CMD_CLEAR:
2019-05-24 09:49:00 +00:00
printf(" <Clear>id=%d, port=%d</Clear>\n",
2019-05-24 09:46:23 +00:00
ap->fmt.content.clear.id,
ap->fmt.content.clear.port);
break;
case CMD_XOR:
2019-05-24 09:49:00 +00:00
printf(" <Xor>Unimplemented</Xor>\n");
2019-05-24 09:46:23 +00:00
break;
default:
2019-05-24 09:49:00 +00:00
printf(" <Unknown/>\n");
2019-05-24 09:46:23 +00:00
break;
}
2019-05-24 09:49:00 +00:00
printf(" </Abstract Packet>\n");
2019-05-24 09:46:23 +00:00
}