M1-nw-project/utils/ihu_replier.c

#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <errno.h>
#include <unistd.h>
#include <string.h>
#include <signal.h>
#include <stdint.h>

const int BUFFER_SIZE = 4096;

int main(int argc, char** argv) {
	int sock = socket(PF_INET6, SOCK_DGRAM, 0);
	if(sock < 0) {
		perror("Could not create socket");
		exit(1);
	}

	int reuseVal = 1;
	if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseVal, sizeof(reuseVal))
			< 0) {
		perror("Could not set reuseaddr");
		exit(1);
	}

	if(argc < 3) {
		fprintf(stderr, "Gimmeh my port and ID!\n");
		exit(1);
	}
	int port = atoi(argv[1]);
	char id = atoi(argv[2]);

	struct sockaddr_in6 sockIn;
	memset(&sockIn, 0, sizeof(sockIn));
	sockIn.sin6_family = AF_INET6;
	sockIn.sin6_port = htons(port);
	if(bind(sock, (struct sockaddr*)&sockIn, sizeof(sockIn))) {
		perror("Could not bind socket to port 8080");
		exit(1);
	}

	char* buffer = (char*)malloc(sizeof(char) * BUFFER_SIZE);
	while(1) {
		struct sockaddr fromAddr;
		socklen_t fromAddr_len = sizeof(struct sockaddr_in6);
		ssize_t readBytes = recvfrom(sock, buffer, BUFFER_SIZE,
				0, &fromAddr, &fromAddr_len);
		if(readBytes < 0) {
			perror("WARNING: Could not read from socket");
			continue;
		}

		uint8_t outPck[24];
		memset(outPck, 0, 24);
		outPck[0] = 57;
		outPck[1] = 0;
		*(uint16_t*)(outPck+2) = htonl(10);
		outPck[11] = id;
		outPck[12] = 2;
		outPck[13] = 8;
		memcpy(outPck+14, buffer+4, 8);

		printf("%d v4=%d v6=%d\n", fromAddr.sa_family, AF_INET, AF_INET6);
		char sAddr[54];
		struct sockaddr_in6* addr6 = (struct sockaddr_in6*)(&fromAddr);
		inet_ntop(AF_INET6, &addr6->sin6_addr, sAddr, 54);
		printf("Sending [%s]:%d\n", sAddr, ntohs(addr6->sin6_port));
		sendto(sock, outPck, 22, 0, &fromAddr, fromAddr_len);
	}

	if(close(sock) < 0) {
		perror("Could not destroy socket");
		exit(1);
	}
	return 0;
}