robomaster-sdk/src/connection.c

#include "message.h"
#include "client.h"
#include "crc.h"
#include "connection.h"

#include <stdlib.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <string.h>
#include <fcntl.h>
#include <stdio.h>

// The greated file descriptor is needed for polling the sockets.
// It needs to be global for the whole process.
int max_fd = -1;

// TODO: Close the socket and return NULL on error
struct Connection*
connection_new(unsigned int source_port, const char* source_ip, unsigned int dest_port, const char* dest_ip)
{
    struct Connection* conn = malloc(sizeof(struct Connection));
    memset(conn, 0, sizeof(struct Connection));

    // Request a UDP socket
    conn->sockfd = socket(AF_INET, SOCK_DGRAM, 0);

    // Set the source address and port if they are provided
    if(source_port && source_ip) {
       struct sockaddr_in loc_addr;
       loc_addr.sin_family = AF_INET;
       loc_addr.sin_port = htons(source_port);
       loc_addr.sin_addr.s_addr = inet_addr(source_ip);
       if(bind(conn->sockfd, (struct sockaddr*)&loc_addr, sizeof(loc_addr)) < 0)
       {
           perror("unable to bind local port");
           exit(EXIT_FAILURE);
       }
    }
 
    // 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
    memset(&conn->remote_addr, 0, sizeof(conn->remote_addr));
    conn->addrlen = sizeof(conn->remote_addr);
    conn->remote_addr.sin_family = AF_INET;
    conn->remote_addr.sin_port = htons(dest_port);
    conn->remote_addr.sin_addr.s_addr = inet_addr(dest_ip);

    // File descriptors are numbers that count up sequentially,
    // so save the last one as the greatest file descriptor.
    // This is needed for polling the sockets later.
    max_fd = conn->sockfd;

    return conn;
}

struct Connection*
connection_poll_ready(struct Client* client) {

    // Return a null connection if no sockets have been opened
    if(max_fd < 0)
        return NULL;

    // Add all the connections' socket file descriptors to a watch list
    fd_set read_fds;
    FD_ZERO(&read_fds);
    for(int i = 0; i < 2; i++)
        if(client->conns[i])
            FD_SET(client->conns[i]->sockfd, &read_fds);

    //struct timeval timeout = {0, 0};
    int result = select(max_fd + 1, &read_fds, NULL, NULL, NULL);

    // Check for socket polling errors
    if(result < 0) {
        perror("message polling failed");
	exit(EXIT_FAILURE);
    }

    // Return a null connection if nothing was received on any of them
    if (result == 0)
        return NULL;

    // Return the first connection with something to read
    for(int i = 0; i < 2; i++)
        if(FD_ISSET(client->conns[i]->sockfd, &read_fds))
            return client->conns[i];

    // Return a null connection if somehow none of them have anything to read
    return NULL;

}

void
connection_read(struct Connection* conn, union Message* resp) {

    memset(resp, 0, sizeof(union Message));

    if(!conn) return;

    int recvb = recvfrom(conn->sockfd, resp, sizeof(union Message), 0, (struct sockaddr*)&conn->remote_addr, &conn->addrlen);

    // Check for socket read errors
    if(recvb < 0) {
        perror("reading socket failed");
        exit(EXIT_FAILURE);
    }

    // Check for message errors
    if(message_validate(resp) != MESSAGEERR_NONE) {
        perror("invalid message");
        exit(EXIT_FAILURE);
    }
}

void
req_send(struct Connection* conn, union Request* req, size_t length) {
    if(!conn || !req) return;
    sendto(conn->sockfd, req, length, 0, (struct sockaddr*)&conn->remote_addr, conn->addrlen);
}

void
req_finalize(struct Client* client, uint8_t cmdset, uint8_t cmdid, size_t length, union Request* req) {

    req->header.preamble = 0x55;
    req->header.length_l = length & 0xFF;
    req->header.length_h = ((length >> 8) & 0x3) | 4;
    req->header.crc = crc8(req, 3);
    req->header.seq_id = client->seq++;
    req->header.sender = client->hostbyte;
    // TODO: Figure out what this is supposed to be
    req->header.receiver = host2byte(DEFAULT_CLIENT_HOST, DEFAULT_ROBOT_INDEX);
    req->header.ack_needed = true;
    req->header.cmdset = cmdset;
    req->header.cmdid = cmdid;

    struct Footer* footer = (void*)req + length - sizeof(struct Footer);
    uint16_t crc = crc16(req, length - sizeof(struct Footer));
    footer->crc = crc;

}