You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
83 lines
2.5 KiB
C
83 lines
2.5 KiB
C
2 years ago
|
#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>
|
||
|
|
|
||
|
|
int max_fd = -1;
|
||
|
|
|
||
|
|
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);
|
||
|
|
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
|
||
|
|
void
|
||
|
|
req_send(struct Connection* conn, union Request* req, size_t length) {
|
||
|
|
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;
|
||
|
|
|
||
|
|
}
|
||
|
|
|