MQTT-Gateway/network.c

#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include <sys/socket.h>
#include <sys/types.h> // BSD compatible
#include <arpa/inet.h>
#include <unistd.h>

#include <errno.h>
#include <netdb.h>

#include "network.h"

const int DGRAM_MSG_START = 0;
const int DGRAM_PAYLOAD_START = DGRAM_MSG_START + DGRAM_MSG_LENGTH + DGRAM_NULL_LENGTH;

int sendToXPlane(int sock, char *msg, void *payload, int lengthOfPayload) {
	char bytesToSend[SEND_BUFFER];
	memset(bytesToSend, 0, SEND_BUFFER);

	// msg is always 4 bytes long
	if (strlen(msg) != DGRAM_MSG_LENGTH) {
		return -10;
	}

	// payload shouldn't exceed buffer
	if (lengthOfPayload > (SEND_BUFFER - DGRAM_MSG_LENGTH - DGRAM_NULL_LENGTH)) {
		return -11;
	}

	strcpy(&bytesToSend[DGRAM_MSG_START], msg);
	memcpy(&bytesToSend[DGRAM_PAYLOAD_START], payload, lengthOfPayload);

	int numBytesSend = send(sock, bytesToSend, DGRAM_MSG_LENGTH + DGRAM_NULL_LENGTH + lengthOfPayload, 0);
	if (numBytesSend >= 0) {
		return numBytesSend;
	} else {
		fprintf(stderr, "Failed to send message (%d): %s", errno, strerror(errno));
		return -1;
	}
}

int recvFromXPlane(int sock, char msg[DGRAM_MSG_LENGTH], void *payload, int maxLength) {
	char buf[maxLength + DGRAM_MSG_LENGTH];
	memset(buf, 0, sizeof(buf));
	int recvBytes = recv(sock, buf, maxLength + DGRAM_MSG_LENGTH, 0);
	if (recvBytes >= 0) {
			printf("received (%d): ", recvBytes);
			for (int i = 0; i < recvBytes; i++) {
				printf("%02x ", buf[i]);
			}
			printf("\n");
		if (recvBytes >= DGRAM_MSG_LENGTH) {
			memcpy(msg, &buf[0], DGRAM_MSG_LENGTH);
			memcpy(payload, &buf[DGRAM_MSG_LENGTH], maxLength);
			return recvBytes - DGRAM_MSG_LENGTH;
		} else {
			fprintf(stderr, "Received data without message header?!");
			return -11;
		}
	} else {
		fprintf(stderr, "Error receiving data (%d): %s", errno, strerror(errno));
		return -1;
	}

}

int connectXPlane(char *destAddr, int destPort, int srcPort) {
	int xPlaneSocket;
	struct sockaddr_in6 xPlaneAddress, localAddress;

	int err;

	// create socket
	xPlaneSocket = socket(AF_INET6, SOCK_DGRAM, 0);
	if (xPlaneSocket == -1) {
		fprintf(stderr, "Error creating socket (%d): %s\n", errno, strerror(errno));
		return -1;
	}

	// prepare source address struct
	localAddress.sin6_family = AF_INET6;
	localAddress.sin6_port = htons(srcPort);
	//inet_pton(AF_INET6, in6addr_any, &localAddress.sin6_addr.s6_addr);
	localAddress.sin6_addr = in6addr_any;

	// bind to local port
	err = bind(xPlaneSocket, (struct sockaddr *)&localAddress, sizeof(localAddress));
	if (err) {
		fprintf(stderr, "Error binding [::]:%d (%d): %s\n", srcPort, errno, gai_strerror(errno));
		return -1;
	}

	// prepare destination address struct
	xPlaneAddress.sin6_family = AF_INET6;
	xPlaneAddress.sin6_port = htons(destPort);
	inet_pton(AF_INET6, destAddr, &xPlaneAddress.sin6_addr.s6_addr);

	// connect to xPlane
	//err = connect(xPlaneSocket, (struct sockaddr *)&xPlaneAddress, sizeof(xPlaneAddress));
	if (err) {
		fprintf(stderr, "Error connecting %s:%d (%d): %s\n", destAddr, destPort, errno, gai_strerror(errno));
		return -1;
	}

	return xPlaneSocket;
}

int disconnectXPlane(int sock) {
	// bye bye (and no, I don't care about error anymore; I'll exit anyway?!)
	close(sock);
}