From dab8e0029ee5eaf0d2ced57f3fef2136d0616d90 Mon Sep 17 00:00:00 2001
From: PgSocks <pgsocks@pm.me>
Date: Mon, 26 Dec 2022 23:44:26 -0600
Subject: [PATCH] Make a struct for connections

---
 include/robomaster.h |  2 ++
 src/client.h         |  4 +++
 src/connection.c     | 78 ++++++++++++++++++++++++++++++++++++++++++++
 src/connection.h     |  8 +++++
 src/message.h        | 20 ++++++++++++
 5 files changed, 112 insertions(+)
 create mode 100644 src/connection.c
 create mode 100644 src/connection.h

diff --git a/include/robomaster.h b/include/robomaster.h
index 369b234..96729e0 100644
--- a/include/robomaster.h
+++ b/include/robomaster.h
@@ -12,4 +12,6 @@ typedef struct Client* Client;
 typedef struct Message* Message;
 
 Client client_new(void* buffer, size_t size);
+void client_connect(Client client);
+Message poll_message(Client client);
 
diff --git a/src/client.h b/src/client.h
index 47008e2..5507151 100644
--- a/src/client.h
+++ b/src/client.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include "connection.h"
+
 struct Client {
 
    void* buffer;
@@ -10,6 +12,8 @@ struct Client {
 
    int16_t seq;
 
+   struct Connection* connection;
+
 };
 
 static inline uint8_t host2byte(uint8_t host, uint8_t index) {
diff --git a/src/connection.c b/src/connection.c
new file mode 100644
index 0000000..ca75962
--- /dev/null
+++ b/src/connection.c
@@ -0,0 +1,78 @@
+
+#include "robomaster.h"
+#include "client.h"
+#include "connection.h"
+#include "message.h"
+
+static struct Connection* connection_new()
+{
+    struct Connection* conn = calloc(sizeof(struct Connection));
+
+    // Request a UDP socket
+    conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+ 
+    // Make the socket non-blocking
+    int flags = fcntl(conn->sockfd, F_GETFL);
+    fcntl(conn->sockfd, F_SETFL, flags | O_NONBLOCK);
+
+    // Set the address of the drone
+    conn->addrlen = sizeof(conn->remote_addr);
+    conn->dest_addr.sin_family = AF_INET;
+    conn->dest_addr.sin_port = htons(30030);
+    conn->dest_addr.sin_addr.s_addr = inet_addr("192.168.2.1");
+
+    return conn;
+}
+
+void client_connect(Client client) {
+    client->connection = connection_new();
+
+    sendto(client->connection->sockfd, client->buffer, session_size(session), 0, (struct sockaddr*)&client->connection->dest_addr, client->connection->addrlen);
+}
+
+// TODO: Use union to make all messages same size
+void send_message(Client client, Message message, int length) {
+    sendto(client->connection->sockfd, message, length, 0, (struct sockaddr*)&client->connection->dest_addr, client->connection->addrlen);
+}
+
+Message poll_message(Client client) {
+
+    // Poll for messages
+    static const struct timeval timeout = {-1, 0};
+    fd_set read_fds;
+    FD_ZERO(&read_fds);
+    FD_SET(client->connection->sockfd, &read_fds);
+    int result = select(client->connection->sockfd + 1, &read_fds, NULL, NULL, &timeout);
+
+    // Check for socket polling errors
+    if(result < 0) {
+        perror("message polling failed");
+	exit(EXIT_FAILURE);
+    }
+
+    // Skip if nothing was received yet
+    // TODO: Make a static "empty" message or something
+    if (result == 0) {
+        return NULL;
+    }
+
+    // Read a message from the socket
+    // TODO: Use union to make all messages same size
+    // NOTE: This is never freed, so it's a memory leak
+    void* buffer = malloc(1024);
+    int recvb = recvfrom(client->connection->sockfd, buffer, sizeof(client->max_size), 0, (struct sockaddr*)&client->connection->dest_addr, &client->connection->addrlen);
+
+    // Check for socket read errors
+    if(recvb < 0) {
+        perror("reading socket failed");
+	exit(EXIT_FAILURE);
+    }
+
+    // Check for message errors
+    if(message_validate(buffer)) {
+        perror("invalid message");
+	exit(EXIT_FAILURE);
+    }
+
+    return (Message)buffer;
+}
diff --git a/src/connection.h b/src/connection.h
new file mode 100644
index 0000000..de824ba
--- /dev/null
+++ b/src/connection.h
@@ -0,0 +1,8 @@
+#pragma once
+
+struct Connection {
+    int sockfd;
+    socklen_t addrlen;
+    sockaddr_in remote_addr;
+};
+
diff --git a/src/message.h b/src/message.h
index e1a4f08..612fb8f 100644
--- a/src/message.h
+++ b/src/message.h
@@ -95,3 +95,23 @@ message_new(struct Client* session, uint8_t cmdset, uint8_t cmdid, size_t length
 
 }
 
+enum MESSAGEERR {
+    MESSAGEERR_NONE,
+    MESSAGEERR_HEADERCRC,
+    MESSAGEERR_FOOTERCRC
+}
+
+static
+inline
+enum MESSAGEERR
+message_validate(const struct Message* message) {
+
+    uint16_t length = (message->header->length_h[2] & 0x3) * 0xFF + message->header->length_l[1];
+
+    if(message->header.crc != crc8(message, 3))
+        return MESSAGEERR_HEADERCRC;
+
+    if(message->header.crc != crc16(message, length - sizeof(struct Footer)))
+        return MESSAGEERR_FOOTERCRC;
+
+}