mirror of
https://github.com/servalproject/serval-dna.git
synced 2024-12-25 07:41:10 +00:00
348 lines
9.7 KiB
C
348 lines
9.7 KiB
C
#ifndef __SERVAL_DNA__MSP_COMMON_H
|
|
#define __SERVAL_DNA__MSP_COMMON_H
|
|
|
|
|
|
#define FLAG_SHUTDOWN (1<<0)
|
|
#define FLAG_ACK (1<<1)
|
|
#define FLAG_FIRST (1<<2)
|
|
#define FLAG_STOP (1<<3)
|
|
#define RETRANSMIT_TIME 1500
|
|
#define HANDLER_KEEPALIVE 1000
|
|
|
|
typedef uint16_t msp_state_t;
|
|
|
|
struct msp_packet{
|
|
struct msp_packet *_next;
|
|
uint16_t seq;
|
|
uint8_t flags;
|
|
time_ms_t added;
|
|
time_ms_t sent;
|
|
size_t len;
|
|
size_t offset;
|
|
uint8_t payload[];
|
|
};
|
|
|
|
#define MAX_WINDOW_SIZE 4
|
|
struct msp_window{
|
|
unsigned packet_count;
|
|
uint32_t base_rtt;
|
|
uint32_t rtt;
|
|
uint16_t next_seq; // seq of next expected TX or RX packet.
|
|
time_ms_t last_activity;
|
|
time_ms_t last_packet;
|
|
struct msp_packet *_head, *_tail;
|
|
};
|
|
|
|
struct msp_stream{
|
|
msp_state_t state;
|
|
struct msp_window tx;
|
|
struct msp_window rx;
|
|
uint16_t previous_ack;
|
|
time_ms_t next_ack;
|
|
time_ms_t timeout;
|
|
time_ms_t next_action;
|
|
};
|
|
|
|
static void msp_stream_init(struct msp_stream *stream)
|
|
{
|
|
stream->state = MSP_STATE_UNINITIALISED;
|
|
// TODO set base rtt to ensure that we send the first packet a few times before giving up
|
|
stream->tx.base_rtt = stream->tx.rtt = 0xFFFFFFFF;
|
|
stream->tx.last_activity = TIME_MS_NEVER_HAS;
|
|
stream->tx.last_packet = TIME_MS_NEVER_HAS;
|
|
stream->rx.last_activity = TIME_MS_NEVER_HAS;
|
|
stream->rx.last_packet = TIME_MS_NEVER_HAS;
|
|
stream->next_action = TIME_MS_NEVER_WILL;
|
|
stream->timeout = gettime_ms() + 10000;
|
|
stream->previous_ack = 0x7FFF;
|
|
}
|
|
|
|
static void free_all_packets(struct msp_window *window)
|
|
{
|
|
struct msp_packet *p = window->_head;
|
|
while(p){
|
|
struct msp_packet *free_me=p;
|
|
p=p->_next;
|
|
free(free_me);
|
|
}
|
|
window->_head = NULL;
|
|
window->packet_count=0;
|
|
}
|
|
|
|
static void free_acked_packets(struct msp_window *window, uint16_t seq)
|
|
{
|
|
if (!window->_head)
|
|
return;
|
|
struct msp_packet *p = window->_head;
|
|
uint32_t rtt=0xFFFFFFFF, rtt_max=0;
|
|
time_ms_t now = gettime_ms();
|
|
|
|
while(p && compare_wrapped_uint16(p->seq, seq)<=0){
|
|
if (p->sent!=TIME_MS_NEVER_HAS){
|
|
uint32_t this_rtt=now - p->sent;
|
|
if (rtt > this_rtt)
|
|
rtt = this_rtt;
|
|
if (rtt_max < this_rtt)
|
|
rtt_max = this_rtt;
|
|
}
|
|
struct msp_packet *free_me=p;
|
|
p=p->_next;
|
|
free(free_me);
|
|
window->packet_count--;
|
|
}
|
|
window->_head = p;
|
|
if (rtt!=0xFFFFFFFF){
|
|
if (rtt < 10)
|
|
rtt=10;
|
|
window->rtt = rtt;
|
|
if (window->base_rtt > rtt)
|
|
window->base_rtt = rtt;
|
|
DEBUGF(msp, "ACK %x, RTT %u-%u, base %u", seq, rtt, rtt_max, window->base_rtt);
|
|
}
|
|
if (!p)
|
|
window->_tail = NULL;
|
|
}
|
|
|
|
static int add_packet(struct msp_window *window, uint16_t seq, uint8_t flags, const uint8_t *payload, size_t len)
|
|
{
|
|
assert(payload || len==0);
|
|
struct msp_packet **insert_pos=NULL;
|
|
|
|
if (!window->_head){
|
|
insert_pos = &window->_head;
|
|
}else{
|
|
if (window->_tail->seq == seq){
|
|
// ignore duplicate packets
|
|
DEBUGF(msp, "Ignore duplicate packet %02x", seq);
|
|
return 0;
|
|
}else if (compare_wrapped_uint16(window->_tail->seq, seq)<0){
|
|
if (compare_wrapped_uint16(window->_head->seq, seq)>0){
|
|
// this is ambiguous
|
|
return WHYF("%04x is both < tail (%04x) and > head (%04x)", seq, window->_tail->seq, window->_head->seq);
|
|
}
|
|
insert_pos = &window->_tail->_next;
|
|
}else{
|
|
insert_pos = &window->_head;
|
|
while(compare_wrapped_uint16((*insert_pos)->seq, seq)<0)
|
|
insert_pos = &(*insert_pos)->_next;
|
|
if ((*insert_pos)->seq == seq){
|
|
// ignore duplicate packets
|
|
DEBUGF(msp, "Ignore duplicate packet %02x", seq);
|
|
return 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
struct msp_packet *packet = emalloc_zero(sizeof(struct msp_packet) + len);
|
|
if (!packet)
|
|
return -1;
|
|
|
|
packet->_next = (*insert_pos);
|
|
*insert_pos = packet;
|
|
if (!packet->_next)
|
|
window->_tail = packet;
|
|
packet->added = gettime_ms();
|
|
packet->seq = seq;
|
|
packet->flags = flags;
|
|
packet->len = len;
|
|
packet->offset = 0;
|
|
packet->sent = TIME_MS_NEVER_HAS;
|
|
|
|
if (len)
|
|
bcopy(payload, packet->payload, len);
|
|
window->packet_count++;
|
|
DEBUGF(msp, "Add packet %02x", seq);
|
|
return 1;
|
|
}
|
|
|
|
static size_t msp_write_ack_header(uint8_t *header, struct msp_stream *stream)
|
|
{
|
|
header[0]=0;
|
|
// if we haven't heard a sequence number, we can't ack data
|
|
// (but we can indicate the existence of the connection)
|
|
if (stream->state & MSP_STATE_RECEIVED_DATA)
|
|
header[0]|=FLAG_ACK;
|
|
|
|
// never received anything? set the connect flag
|
|
if (!(stream->state & MSP_STATE_RECEIVED_PACKET))
|
|
header[0]|=FLAG_FIRST;
|
|
|
|
write_uint16(&header[1], stream->rx.next_seq -1);
|
|
|
|
stream->previous_ack = stream->rx.next_seq -1;
|
|
stream->tx.last_activity = gettime_ms();
|
|
stream->next_ack = stream->tx.last_activity + RETRANSMIT_TIME;
|
|
|
|
DEBUGF(msp, "Sending packet flags %02x (acked %02x)",
|
|
header[0], stream->rx.next_seq -1);
|
|
return 3;
|
|
}
|
|
|
|
static size_t msp_write_preamble(uint8_t *header, struct msp_stream *stream, struct msp_packet *packet)
|
|
{
|
|
msp_write_ack_header(header, stream);
|
|
header[0]|=packet->flags;
|
|
|
|
write_uint16(&header[3], packet->seq);
|
|
|
|
DEBUGF(msp, "With packet flags %02x seq %02x len %zd",
|
|
header[0], packet->seq, packet->len);
|
|
packet->sent = stream->tx.last_packet = stream->tx.last_activity;
|
|
return MSP_PAYLOAD_PREAMBLE_SIZE;
|
|
}
|
|
|
|
static ssize_t msp_stream_send(struct msp_stream *stream, const uint8_t *payload, size_t len)
|
|
{
|
|
assert(!(stream->state & MSP_STATE_LISTENING));
|
|
assert((stream->state & MSP_STATE_SHUTDOWN_LOCAL)==0);
|
|
|
|
if ((stream->state & MSP_STATE_CLOSED) || stream->tx.packet_count > MAX_WINDOW_SIZE)
|
|
return -1;
|
|
if (add_packet(&stream->tx, stream->tx.next_seq, 0, payload, len)==-1)
|
|
return -1;
|
|
|
|
stream->tx.next_seq++;
|
|
if (stream->tx.packet_count>=MAX_WINDOW_SIZE)
|
|
stream->state&=~MSP_STATE_DATAOUT;
|
|
// make sure we attempt to process packets from this sock soon
|
|
// TODO calculate based on congestion window
|
|
stream->next_action = gettime_ms();
|
|
|
|
return len;
|
|
}
|
|
|
|
static int msp_stream_shutdown(struct msp_stream *stream)
|
|
{
|
|
assert(!(stream->state&MSP_STATE_LISTENING));
|
|
assert(!(stream->state&MSP_STATE_SHUTDOWN_LOCAL));
|
|
if (stream->tx._tail && stream->tx._tail->sent==TIME_MS_NEVER_HAS){
|
|
stream->tx._tail->flags |= FLAG_SHUTDOWN;
|
|
}else{
|
|
if (add_packet(&stream->tx, stream->tx.next_seq, FLAG_SHUTDOWN, NULL, 0)==-1)
|
|
return -1;
|
|
stream->tx.next_seq++;
|
|
}
|
|
stream->state|=MSP_STATE_SHUTDOWN_LOCAL;
|
|
stream->state&=~MSP_STATE_DATAOUT;
|
|
// make sure we send a packet soon
|
|
stream->next_action = gettime_ms();
|
|
return 0;
|
|
}
|
|
|
|
static int msp_stream_process(struct msp_stream *stream)
|
|
{
|
|
time_ms_t now = gettime_ms();
|
|
if (stream->timeout < now){
|
|
stream->state |= (MSP_STATE_CLOSED|MSP_STATE_ERROR);
|
|
return WHY("MSP socket timed out");
|
|
}
|
|
|
|
if (stream->state & MSP_STATE_LISTENING)
|
|
return 1;
|
|
|
|
// when we've delivered all local packets
|
|
// and all our data packets have been acked, close.
|
|
if ( (stream->state & MSP_STATE_SHUTDOWN_LOCAL)
|
|
&& (stream->state & MSP_STATE_SHUTDOWN_REMOTE)
|
|
&& stream->tx.packet_count == 0
|
|
&& stream->rx.packet_count == 0
|
|
&& stream->previous_ack == stream->rx.next_seq -1
|
|
)
|
|
stream->state |= MSP_STATE_CLOSED;
|
|
|
|
return 0;
|
|
}
|
|
|
|
// return the next in-order packet
|
|
static struct msp_packet *msp_stream_next(struct msp_stream *stream)
|
|
{
|
|
struct msp_packet *packet = stream->rx._head;
|
|
if (!packet || packet->seq != stream->rx.next_seq)
|
|
return NULL;
|
|
|
|
assert(packet->offset <= packet->len);
|
|
|
|
if (packet->flags & FLAG_SHUTDOWN)
|
|
stream->state|=MSP_STATE_SHUTDOWN_REMOTE;
|
|
|
|
return packet;
|
|
}
|
|
|
|
static void msp_consume_packet(struct msp_stream *stream, struct msp_packet *packet, size_t consumed)
|
|
{
|
|
assert(packet->seq==stream->rx.next_seq);
|
|
packet->offset += consumed;
|
|
if (packet->offset < packet->len)
|
|
return;
|
|
|
|
free_acked_packets(&stream->rx, stream->rx.next_seq);
|
|
stream->rx.next_seq++;
|
|
|
|
// when we've delivered all local packets
|
|
// and all our data packets have been acked, close.
|
|
if ( (stream->state & MSP_STATE_SHUTDOWN_LOCAL)
|
|
&& (stream->state & MSP_STATE_SHUTDOWN_REMOTE)
|
|
&& stream->tx.packet_count == 0
|
|
&& stream->rx.packet_count == 0
|
|
&& stream->previous_ack == stream->rx.next_seq -1
|
|
)
|
|
stream->state |= MSP_STATE_CLOSED;
|
|
}
|
|
|
|
static int msp_process_packet(struct msp_stream *stream, const uint8_t *payload, size_t len)
|
|
{
|
|
if (len<1)
|
|
return WHY("Expected at least 1 byte");
|
|
|
|
uint8_t flags = payload[0];
|
|
time_ms_t now = gettime_ms();
|
|
stream->rx.last_activity = now;
|
|
stream->timeout = stream->rx.last_activity + MSP_TIMEOUT;
|
|
stream->state |= MSP_STATE_RECEIVED_PACKET;
|
|
|
|
if (flags & FLAG_STOP){
|
|
DEBUGF(msp, "Closing socket due to STOP packet");
|
|
stream->state |= (MSP_STATE_CLOSED|MSP_STATE_STOPPED);
|
|
stream->state &= ~MSP_STATE_DATAOUT;
|
|
free_all_packets(&stream->tx);
|
|
stream->next_action = now;
|
|
return 0;
|
|
}
|
|
|
|
if (len<3)
|
|
return 0;
|
|
|
|
if (flags & FLAG_ACK){
|
|
uint16_t ack_seq = read_uint16(&payload[1]);
|
|
// release acknowledged packets
|
|
free_acked_packets(&stream->tx, ack_seq);
|
|
|
|
// TODO if their ack seq has not advanced, we may need to hurry up and retransmit a packet
|
|
}
|
|
|
|
// Do we have space for more data now?
|
|
if (stream->tx.packet_count < MAX_WINDOW_SIZE
|
|
&& !(stream->state & MSP_STATE_SHUTDOWN_LOCAL)
|
|
&& !(stream->state & MSP_STATE_CLOSED)){
|
|
stream->state|=MSP_STATE_DATAOUT;
|
|
}
|
|
|
|
// make sure we attempt to process packets from this sock soon
|
|
// TODO calculate based on congestion window
|
|
stream->next_action = now;
|
|
|
|
if (len<MSP_PAYLOAD_PREAMBLE_SIZE)
|
|
return 0;
|
|
|
|
stream->state |= MSP_STATE_RECEIVED_DATA;
|
|
uint16_t seq = read_uint16(&payload[3]);
|
|
stream->rx.last_packet = stream->rx.last_activity;
|
|
if (compare_wrapped_uint16(seq, stream->rx.next_seq)>=0){
|
|
if (add_packet(&stream->rx, seq, flags, &payload[MSP_PAYLOAD_PREAMBLE_SIZE], len - MSP_PAYLOAD_PREAMBLE_SIZE)==1)
|
|
stream->next_ack = now;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
#endif |