Hermes Lite 2 Discovery emulator

discovery.emulator.c

// Hermes Lite 2 Protocol 1 Discovery emulator
// emulates the behavior of the FPGA inside an OpenHPSDR radio.
// waits for 1 63-byte broadcast packets on port 1024 ,
// replies with a 60 byte UDP packet
// Version  2020/11/03 (b)  rhn@hotpaw.com  
//

// #define PI_LINUX
#define REPLY_ENABLE 	(1)
#define DEFAULT_PORT	(1024)

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <sys/ioctl.h>
#include <net/if.h>

int 	port =  DEFAULT_PORT ;
char 	dataBuf[1024];

unsigned char replyBuffer[64] = {
	0xEF,0xFE,0x02,0x00,0x1C,0xC0,0xA2,0x13,
	// 0xDD,0x46,0x06,0x00,0x00,0x00,0x00,0x00,
	0xDD,0x01,0x01,0x00,0x00,0x00,0x00,0x00,
	0x00,0x00,0x00,0x04,0x45,0x00,0x06,0x06,
	0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,
	0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,
	0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,
	0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,
	0x06,0x06,0x06,0x00,0x00,0x00,0x00,0x00
};

#ifdef PI_LINUX
// get MAC address of eth0 on Raspberry Pi
void getmacaddr()
{
    int           s;
    struct ifreq  buffer;

    s = socket(PF_INET, SOCK_DGRAM, 0);
    memset(&buffer, 0x00, sizeof(buffer));
    strcpy(buffer.ifr_name, "eth0");
    ioctl(s, SIOCGIFHWADDR, &buffer);
    close(s);
    memcpy(&replyBuffer[3],(unsigned char *)buffer.ifr_hwaddr.sa_data,6);
}    
#else
void getmacaddr() { return; }
#endif

int main( int argc, char **argv )
{
    int 		fd;
    struct sockaddr_in6 my_addr;

    memset( &my_addr, 0, sizeof(my_addr) );
    bzero((char *) &my_addr, sizeof(my_addr));
    bzero((char *)dataBuf, sizeof(dataBuf));

    if ( (fd = socket(AF_INET6, SOCK_DGRAM, 0)) < 0 ) {
        perror( "socket failed" );
        return 1;
    }
    int rua = 1;
    setsockopt( fd, SOL_SOCKET, SO_REUSEADDR,
                (char *)&rua, sizeof(int) );

    my_addr.sin6_flowinfo    =  0;
    my_addr.sin6_family      =  AF_INET6;
    my_addr.sin6_port        =  htons( port );
    my_addr.sin6_addr        =  in6addr_any;

    if ( bind(fd, (struct sockaddr *)&my_addr, sizeof(my_addr)) < 0 ) {
        perror( "bind failed" );
        return 1;
    }

    printf("waiting for message on port %d \n", port);

    if (1) {
        int     flags    =  0;
	int	length   =  0;
	int	i;
	struct sockaddr_in6 sender;	// client/sender address
	socklen_t len6      =  sizeof(sender);
	bzero(dataBuf, 1024);
	length = recvfrom( fd, dataBuf, sizeof(dataBuf) - 1, 
			flags,
			(struct sockaddr *)&sender, &len6);
        if ( length < 0 ) {
            perror( "recvfrom failed" );
	    exit(-1);
        }
	char client_name_buf[128];
	inet_ntop(AF_INET6, &(sender.sin6_addr), client_name_buf, 127);
	client_name_buf[127] = 0;
	printf("Received UDP packet from: %s #%d \n", 
			&client_name_buf[0],
			ntohs(sender.sin6_port));
        printf("received %d bytes \n", length);
        for (i=0;i<length;i++) {
            printf("%02X ",dataBuf[i]);
        } printf("\n");
#ifdef REPLY_ENABLE 
        getmacaddr();
	length = 60;
	int status = 0;
	status = sendto( fd, &replyBuffer[0], length,
	                 0,
	                 (struct sockaddr *)&sender, 
		         len6 );
	if ( status < 0 ) {
       	    perror("sendto error ");
	    // if (errno == ENOBUFS) { continue; }
	} else {
	    printf( "succesfully sent UDP reply of %d bytes\n", status );
	}
#endif
    }

    close( fd );
    printf("port %d closed \n", port);
}
// eof