diff --git a/groundStation/src/backend/backend.c b/groundStation/src/backend/backend.c index 5e43f9188c5efe71c920a205e8e5b4324fb28f99..6cb684e0bb3a9e97f4802ba449fef244e236f44a 100644 --- a/groundStation/src/backend/backend.c +++ b/groundStation/src/backend/backend.c @@ -57,6 +57,10 @@ static int new_client(int fd); static ssize_t get_client_index(int fd); /* Returns pointer to client buffer, or -1 */ static char * get_client_buffer(int fd); +/* Return pointer to client pending responses, or -1*/ +static int * get_client_pend_responses(int fd); +/* Return positive integer if successful, -1 otherwise */ +static int clientAddPendResponses(int fd, unsigned char *packet); /* Returns -1 on error */ static int remove_client(int fd); /* Receive data from client */ @@ -86,8 +90,10 @@ const char *logHeader = "";//"#\n#\tDefault log header\n#\tEverything after '#'` #define MAX_CLIENTS 32 #define CLIENT_BUFFER_SIZE 1024 +#define CLIENT_MAX_PENDING_RESPONSES 5 static char client_buffers[MAX_CLIENTS][CLIENT_BUFFER_SIZE]; static int client_fds[MAX_CLIENTS]; +static int client_pending_responses[MAX_CLIENTS][CLIENT_MAX_PENDING_RESPONSES]; fd_set rfds_master; int max_fd = 0; @@ -156,6 +162,9 @@ int main(int argc, char **argv) for (int i = 0; i < MAX_CLIENTS; i++) { client_fds[i] = -1; client_buffers[i][0] = '\n'; + for(int j = 0; j < CLIENT_MAX_PENDING_RESPONSES; j++) { + client_pending_responses[i][j] = -1; + } } if (pthread_mutex_lock(&quadSocketMutex)) { @@ -529,6 +538,27 @@ static char * get_client_buffer(int fd) { } } +static int * get_client_pend_responses(int fd) { + ssize_t slot = get_client_index(fd); + if (slot == -1) { + return NULL; + } else { + return client_pending_responses[slot]; + } +} + +static int clientAddPendResponses(int fd, unsigned char *packet) { + int *pendingResponses = get_client_pend_responses(fd); + int packetID = (packet[4] << 8) | (packet[3]); + for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) { + if(pendingResponses[i] == -1) { + pendingResponses[i] = packetID; + return i; + } + } + return -1; +} + static int remove_client(int fd) { ssize_t slot = get_client_index(fd); if(slot == -1) @@ -537,6 +567,12 @@ static int remove_client(int fd) { if(clientBuffer == NULL) return -1; clientBuffer[0] = '\0'; + int *pendingResponses = get_client_pend_responses(fd); + if(pendingResponses == NULL) + return -1; + for(int i = 0; i < CLIENT_MAX_PENDING_RESPONSES; i++) { + pendingResponses[i] = -1; + } client_fds[slot] = -1; return 0; } @@ -591,8 +627,12 @@ static void client_recv(int fd) { if(formatCommand(buffer, &packet) == -1) { printf("Could not recognize command '%s'\n", buffer); } else { - int datalen = (packet[6] << 8) | (packet[5]); - writeQuad((char *) packet, datalen +8); + if(clientAddPendResponses(fd, packet) == -1) { + warnx("Ran out of room! Consider increasing CLIENT_MAX_PENDING_RESPONSES!"); + } else { + int datalen = (packet[6] << 8) | (packet[5]); + writeQuad((char *) packet, datalen +8); + } } char * rest = &buffer[newline] + 1;