Skip to content

Instantly share code, notes, and snippets.

@hotpaw2

hotpaw2/hl2.emulator.c

Last active February 21, 2024 12:11
Show Gist options
  • Save hotpaw2/5e75e19ecd1fb2d43d6f8f6f44bd2611 to your computer and use it in GitHub Desktop.
Save hotpaw2/5e75e19ecd1fb2d43d6f8f6f44bd2611 to your computer and use it in GitHub Desktop.
Download ZIP
Hermes Lite 2 Emulator
Raw
hl2.emulator.c
// Hermes Lite 2 Basic Metis Protocol 1 Operation Emulator
// emulates Discovery and a 1 slice receiver (for IQ data from a file)
// uses UDP port 1024
//
// cc fhl2e.c -lpthread -lm -o fhl2
//
// ( "usage -n -f0 -g -sr -iq \n" )
// for add noise, add tone frequency & gain, set sample rate, IQ filename
#define FHL2EVERSION ("1.0.102") // 2024-02-05 rhn@hotpaw.com
// #define PI_LINUX 1
#define ECHO_ENABLE (1)
#define NUM_PACKETS (1)
#define DEFAULT_PORT (1024)
#define MAX_SEQ_ERRS_TO_PRINT (8)
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <sys/types.h>
#include <ctype.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <pthread.h>
#include <sys/time.h>
#include <signal.h>
#include <errno.h>
#include <sys/stat.h>
#ifdef PI_LINUX
#include <sys/ioctl.h>
#include <net/if.h>
#endif
static int port = DEFAULT_PORT ;
static int srate = 48000; // fsr
static double ph0 = 0.0;
static unsigned char rcvDataBuf[ 2048]; // 1032 needed
static unsigned char sendDataBuf[2048]; // 1032 needed
struct sockaddr_in6 client_addr; // client/sender address
static int fd;
struct sigaction sigact, sigign;
static void sighandler(int signum);
int udp_running = 0; // udp receive ready state
int dpkt_cnt = 0; // count of received udp data packets
int udp_tstate = 0; // udp send data thread state
static float *rrTest = NULL; //
static float *iiTest = NULL; //
int loadFileSkip = 0;
char *loadFilePath = NULL;
int iq_file_flag = 0;
int iqFileSamples = 0; // length in samples
float gF0test = 656.25;
float gAmp = 1.0;
float gNoise = 0.0;
long int seqNumR = 0; // received sequence number
int seqErrCount = 0;
int received_r0_f0 = 0;
int received_fsr = 0;
int received_gain = 0;
static void udp_send_discovery_reply();
static void udp_thread_control(int flag);
static void fill_samples();
static int udp_send_data();
static double timer();
void normal_rand_test();
double normal_rand(double sd);
// returns a time in seconds
static double timer() {
struct timespec now;
clock_gettime( CLOCK_MONOTONIC_RAW, &now );
uint64_t t1 = ( (uint64_t)now.tv_sec * 1000000000U
+ (uint64_t)now.tv_nsec );
double t2 = t1 / 1.0e9;
return(t2);
}
static double timer2() {
struct timeval tvalue2;
gettimeofday(&tvalue2, NULL);
double t2 = ( (double)(tvalue2.tv_usec) / 1000000.0 )
+ (double)(tvalue2.tv_sec);
return(t2);
}
double normal_rand(double sd)
{
long int r1b = random();
long int r2b = random();
while (r1b == 0) { r1b = random(); }
while (r2b == 0) { r2b = random(); }
double r1 = (double)(0x00007fffL & r1b) / 32768.0 ;
double r2 = (double)(0x7fffffffL & r2b) / 1073741824.0 ;
double pi = M_PI;
double r = sd * sqrt(-2.0*log(r1))*cos(2.0*pi*r2) ;
return(r);
}
void normal_rand_test()
{
int d[64]; for (int i=0; i<64; i++) { d[i] = 0; }
double t1 = timer();
long int s = (long int)(100000000.0 * (t1 - floor(t1)));
srandom(s);
printf("seed %ld \n", s);
for (int i=0;i<1000;i++) {
float r = normal_rand(10.0);
int j = (int)fabsf(roundf(r));
// printf("%3d %f %d \n", i, r, j);
if (j < 64) { d[j] += 1; }
}
for (int i=0; i<35; i++) {
printf("%3d %d \n", i, d[i]);
}
}
// load_iq
// if f4fwrite flag -> fft whole file
// if iq_file_flag == 1
long int read_iq_file(char *fpath, float scale)
{
int nfft = 2*1024; // 16*1024;
long int n = 0;
long int sz = 0;
char s[32];
struct stat st;
if (fpath == NULL) {
printf("null IQ file name \n");
exit(-1);
return(0);
}
// printf("fpath >%s<\n", fpath);
if (stat(fpath, &st) == 0) {
sz = (long)st.st_size / 4;
if (sz <= 0) { exit(-1); }
iqFileSamples = sz; // length in samples
// printf("IQ file samples = %ld\n", sz);
} else {
printf("file stat error on >%s< \n", fpath);
exit(-1);
}
int n3 = 1024 * (1 + sz/1024) + 2 * 32768 + 4; // round up
if (1) {
if (rrTest != NULL) { free(rrTest); }
if (iiTest != NULL) { free(iiTest); }
rrTest = (float *)malloc(sizeof(float) * n3 + 4);
if (rrTest == NULL) { exit(-1); }
iiTest = (float *)malloc(sizeof(float) * n3 + 4);
if (iiTest == NULL) { exit(-1); }
bzero(rrTest, 4+4*n3);
bzero(iiTest, 4+4*n3);
}
FILE *f;
f = fopen(fpath, "r"); // read_iq_file
if (f == NULL) { return(0); }
int c0 = fgetc(f);
if (c0 == EOF) {
printf("empty file \n");
exit(0);
}
char c1 = fgetc(f);
char c2 = fgetc(f);
char c3 = fgetc(f);
int skip = loadFileSkip;
float tsum = 0.0;
n = 0;
while (1) {
int x0 = (0x00ff & c0) + 256 * (0x00ff & c1); // LE
if (x0 > 32767) { x0 -= 2 * 32768; }
float x = (float)x0 / 32768.0;
if (x > 1.0) { x = 1.0; }
if (x < -1.0) { x = -1.0; }
int y0 = (0x00ff & c2) + 256 * (0x00ff & c3); // LE
if (y0 > 32767) { y0 -= 2 * 32768; }
float y = (float)y0 / 32768.0;
if (y > 1.0) { y = 1.0; }
if (y < -1.0) { y = -1.0; }
if (1) {
rrTest[n] += scale * x;
iiTest[n] += scale * y; // yyy 0217
if (1) {
if (skip > 0) {
skip -= 1;
} else {
n += 1;
}
// printf("%4d 0x%08x 0x%08x %d %d\n", n,x0,y0,c0,(c0 == EOF));
if ((n + 1) >= sz) { break; }
tsum += x*x+y*y;
}
}
c0 = fgetc(f);
if (c0 == EOF) { break; }
c1 = fgetc(f);
c2 = fgetc(f);
c3 = fgetc(f);
} // while
fclose(f);
printf("%ld samples loaded\n", n);
return(n);
} // read_iq_file()
#ifdef PI_LINUX
#include <sys/ioctl.h>
#include <net/if.h>
// get MAC address of eth0 on Raspberry Pi
void getmacaddr(unsigned char *replyBuf)
{
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(&replyBuf[3],(unsigned char *)buffer.ifr_hwaddr.sa_data,6);
}
#else
void getmacaddr(void *x) { return; }
#endif
static void udp_send_discovery_reply()
{
unsigned char replyBuf[64]; // 60 needed
float t1 = timer();
int length = 60;
int status = 0;
int gver = 1;
int prot = 6;
bzero(replyBuf, 64);
replyBuf[ 0] = 0xef;
replyBuf[ 1] = 0xfe;
replyBuf[ 2] = 0x02;
replyBuf[ 9] = gver;
getmacaddr(replyBuf);
replyBuf[10] = prot;
socklen_t addr6Len = sizeof(client_addr);
// discovery
status = sendto( fd, &replyBuf[0], length,
0,
(struct sockaddr *)&client_addr,
addr6Len );
if ( status < 0 ) {
perror("discovery reply error ");
} else {
printf( "successful discovery reply %d \n", status );
}
}
long int total_bytesSent = 0;
long int total_samplesSent = 0;
long int seqNumS = 0; // sent sequence number
int iqFileIndex = 0; // length in samples
void fill_samples() // fill_buffer fill buffer
{
// sendDataBuf
double sd = 1.4142; // about S1
double r1 = 0.0;
double r2 = 0.0;
double f0 = gF0test ; // default = 656.25
double g1 = 64.0 * gAmp; // about S9 for gAmp = 1
double g2 = 256.0; // IQ file gain
double dph = 2.0 * M_PI * f0 / (double)srate;
int im16 = 0;
int re16 = 0;
for (int i=0; i<63; i++) {
if (iq_file_flag == 1) {
int k = iqFileIndex + i ;
im16 = g2 * iiTest[k];
re16 = g2 * rrTest[k];
} else {
r1 = gNoise * normal_rand(sd);
r2 = gNoise * normal_rand(sd);
im16 = (int)round(g1 * 1.0 * sin(ph0) + r1);
re16 = (int)round(g1 * 1.0 * cos(ph0) + r2);
}
sendDataBuf[8 +8+8*i + 0] = 0x00ff & (im16 >> 8); // big
sendDataBuf[8 +8+8*i + 1] = 0x00ff & (im16);
sendDataBuf[8 +8+8*i + 2] = 0;
sendDataBuf[8 +8+8*i + 3] = 0x00ff & (re16 >> 8); // big
sendDataBuf[8 +8+8*i + 4] = 0x00ff & (re16);
sendDataBuf[8 +8+8*i + 5] = 0;
sendDataBuf[8 +8+8*i + 6] = 0; // zero mic data
sendDataBuf[8 +8+8*i + 7] = 0;
ph0 += dph;
}
for (int i=0; i<63; i++) {
if (iq_file_flag == 1) {
int k = iqFileIndex + 63 + i ;
im16 = g2 * iiTest[k];
re16 = g2 * rrTest[k];
} else {
r1 = gNoise * normal_rand(sd);
r2 = gNoise * normal_rand(sd);
im16 = (int)round(g1 * 1.0 * sin(ph0) + r1);
re16 = (int)round(g1 * 1.0 * cos(ph0) + r2);
}
sendDataBuf[8+512+8+8*i + 0] = 0x00ff & (im16 >> 8); // big
sendDataBuf[8+512+8+8*i + 1] = 0x00ff & (im16);
sendDataBuf[8+512+8+8*i + 2] = 0;
sendDataBuf[8+512+8+8*i + 3] = 0x00ff & (re16 >> 8); // big
sendDataBuf[8+512+8+8*i + 4] = 0x00ff & (re16);
sendDataBuf[8+512+8+8*i + 5] = 0;
sendDataBuf[8+512+8+8*i + 6] = 0; // zero mic data
sendDataBuf[8+512+8+8*i + 7] = 0;
ph0 += dph;
}
if (iqFileIndex + (2*63) < iqFileSamples) {
iqFileIndex += 2*63;
} else {
iqFileIndex = 0;
}
}
int udp_send_data()
{
int C0 = 0;
int status = -1;
int length = 1032;
bzero(sendDataBuf, length);
sendDataBuf[ 0] = 0xef;
sendDataBuf[ 1] = 0xfe;
sendDataBuf[ 2] = 0x01;
sendDataBuf[ 3] = 0x06;
sendDataBuf[ 4] = seqNumS >> 24 & 0xFF;
sendDataBuf[ 5] = seqNumS >> 16 & 0xFF;
sendDataBuf[ 6] = seqNumS >> 8 & 0xFF;
sendDataBuf[ 7] = seqNumS & 0xFF;
sendDataBuf[ 8] = 0x7f;
sendDataBuf[ 9] = 0x7f;
sendDataBuf[10] = 0x7f;
sendDataBuf[11] = C0;
fill_samples();
socklen_t addr6Len = sizeof(client_addr);
// 1032 bytes of data
status = sendto( fd, &sendDataBuf[0], length,
0,
(struct sockaddr *)&client_addr,
addr6Len );
return(status);
}
int status = 0;
// udp_send_data_loop() is inside pthread udp_send_thread
void *udp_send_data_loop(void *param)
{
total_bytesSent = 0;
total_samplesSent = 0;
seqNumS = 0;
struct timeval tvalue1;
gettimeofday(&tvalue1, NULL);
double t0 = ( (double)(tvalue1.tv_usec) / 1000000.0
+ (double)(tvalue1.tv_sec ) );
double t1 = 0.0; // time past t0
double t2 = 0.0; // proper time for number of samples sent
while (udp_tstate == 1) {
// send 1032 bytes
int bytesSent = 0;
int samplesSent = 0;
status = udp_send_data();
if ( status < 0 ) {
perror("upd send data error ");
udp_tstate = 0;
break;
} else {
bytesSent = 1032;
samplesSent = 2 * 63;
total_bytesSent += bytesSent;
total_samplesSent += samplesSent;
if (seqNumS < 1) {
printf( "successful send data %ld %d %ld %lf %lf\n",
seqNumS, status, total_samplesSent, t1, t2);
}
seqNumS += 1;
}
// dt as fraction of a second
double dt = 1.0 * (double)samplesSent/ (double)srate;
if (dt > 0.5) { dt = 0.5; } // safety
t2 += dt; // proper duration for srate
struct timeval tvalue2;
gettimeofday(&tvalue2, NULL);
// actual streaming duration
t1 = ( (double)(tvalue2.tv_usec) / 1000000.0
+ (double)(tvalue2.tv_sec )
- t0 );
// delta proper vs. current
double ahead = t2 - t1; // if current time is less
if (ahead > 0.0) { // then it's ahead of proper time
usleep(1.0 * 1.0e6 * dt); // so slow down
// usleep(2625); // so slow down
}
}
if (1) {
printf( "exiting send data, %d %ld udp, %ld bytes in %.3lf seconds",
status, seqNumS, total_samplesSent, t1);
if ( seqErrCount > 0) {
printf( " %d sequence errors\n", seqErrCount);
}
}
return(param);
}
// starts pthread udp_send_thread
void udp_thread_control(int flag)
{
long int *param = NULL;
if (flag == 1) {
udp_tstate = 1;
pthread_t udp_send_thread;
if ( pthread_create( &udp_send_thread,
NULL ,
udp_send_data_loop,
(void *)param ) < 0 ) {
printf("could not create udp streaming thread");
} else {
// printf("udp streaming thread started \n");
}
} else {
udp_tstate = 0;
}
}
int handle_receive_sequence(unsigned char *db)
{
int seqNum = ( (db[ 4] << 24)
| (db[ 5] << 16)
| (db[ 6] << 8)
| (db[ 7] ) );
if (seqNum != seqNumR + 1) {
// sequence error
if (seqErrCount < MAX_SEQ_ERRS_TO_PRINT) {
printf("sequence error: received %d, expected %ld\n",
seqNum, seqNumR+1);
}
seqErrCount += 1;
}
seqNumR = seqNum;
return(seqNum);
}
/*
*/
int handle_receive_command(int u, unsigned char *usb_db)
{
int syncOK = 0;
if ( usb_db[ 0] == 0x7f
&& usb_db[ 1] == 0x7f
&& usb_db[ 2] == 0x7f ) {
syncOK = 1;
} else {
// sync error ???
return(-1);
}
int C0 = usb_db[ 3];
if (C0 == 0) { // sample rate
int t = usb_db[ 4];
int r = 48000;
if (t == 0) { r = 48000; }
if (t == 1) { r = 96000; }
if (t == 2) { r = 192000; }
if (t == 3) { r = 384000; }
if (r != received_fsr) {
printf("received fsr %d\n", r);
received_fsr = r;
}
}
if (C0 == ( 2 << 1)) { // slice 0 receiver frequency
int f0 = ( (usb_db[ 4] << 24)
| (usb_db[ 5] << 16)
| (usb_db[ 6] << 8)
| (usb_db[ 7] ) );
if (f0 != received_r0_f0) {
printf("received new slice 0 f0 %d\n", f0 );
received_r0_f0 = f0;
}
}
if (C0 == (10 << 1)) { // receiver LNA gain
int g = usb_db[ 7] & 0x3f;
if (g != received_gain) {
printf("received new LNA gain %d\n", g );
received_gain = g;
}
}
return(0);
}
int handle_receive_data(unsigned char *db, int length)
{
int err = -1;
if (1) {
// unsigned char *db = rcvDataBuf;
if (length == 63 && db[2] == 2) {
// printf("Received %d bytes \n", length);
if (db[0] == 0xef && db[1] == 0xfe && db[2] == 2) {
char client_name_buf[128];
inet_ntop( AF_INET6,
&(client_addr.sin6_addr),
client_name_buf, 127 );
client_name_buf[127] = 0;
printf("Received Discovery msg from: %s #%d \n",
&client_name_buf[0],
ntohs(client_addr.sin6_port));
udp_send_discovery_reply();
}
err = 0;
}
if (length == 64 && db[2] == 4) {
// printf("Received %d bytes \n", length);
if (db[0] == 0xef && db[1] == 0xfe) {
if (db[3] == 1) {
if (udp_tstate == 0) {
char client_name_buf[128];
bzero(client_name_buf, 128);
inet_ntop(AF_INET6, &(client_addr.sin6_addr),
client_name_buf, 127);
client_name_buf[127] = 0;
printf("Received Start msg from: %s #%d \n",
&client_name_buf[0],
ntohs(client_addr.sin6_port));
// seqNumR = -1;
seqErrCount = 0;
received_r0_f0 = 0;
received_fsr = 0;
received_gain = -1;
}
udp_thread_control(1);
} else {
if (udp_tstate == 1) {
printf("Received Stop msg \n");
}
udp_thread_control(0);
}
}
err = 0;
}
if (length == 1032 && db[2] == 1) {
/// ToDo: command handler for f0, fsr, gain, etc.
if (dpkt_cnt < 1) {
// printf("Received %d bytes \n", length);
printf("Received %d bytes UDP data, #%d\n", length, dpkt_cnt);
}
if ( db[ 0] == 0xef && db[1] == 0xfe
&& db[ 2] == 1 && db[3] == 2
) {
handle_receive_sequence(db);
handle_receive_command(0, &db[8 ]);
handle_receive_command(1, &db[8+512]);
}
dpkt_cnt += 1;
err = 0;
}
}
return(err);
}
static void sighandler(int signum)
{
fprintf(stderr, "Signal caught, exiting!\n");
fflush(stderr);
udp_running = -1;
exit(-1);
}
void handle_args(int argc, char **argv)
{
if (argc > 1) {
if (strcmp(argv[1], "-h") == 0) {
// printf("mdec05 version %s\n", MC05_VERSION );
printf("usage -n -f0 -g -sr -iq \n" );
// printUsage();
exit(0);
}
if (strcmp(argv[1], "-v") == 0) {
printf("fhl2e version %s\n", (FHL2EVERSION) );
exit(0);
}
int arg = 3;
for (arg=3; arg<=argc; arg+=2) {
// printf("%d >%s<\n", arg, argv[arg-2]);
if (strcmp(argv[arg-2], "-g") == 0) {
float x = atof(argv[arg-1]);
gAmp = x ;
printf("amp = %f \n", x);
}
if (strcmp(argv[arg-2], "-n") == 0) {
float x = atof(argv[arg-1]);
gNoise = x ;
printf("noise = %f \n", x);
}
if (strcmp(argv[arg-2], "-f0") == 0) {
float f = atof(argv[arg-1]);
if (fabsf(f) < 0.5*srate) {
gF0test = f ;
}
printf("f0 = %f \n", f);
}
if (strcmp(argv[arg-2], "-sr") == 0) {
float r = atof(argv[arg-1]);
int ok = 0;
if (r == 48000) { srate = r; ok = 1; }
if (r == 96000) { srate = r; ok = 1; }
if (r == 192000) { srate = r; ok = 1; }
if (r == 384000) { srate = r; ok = 1; }
if (ok == 0) {
printf("unsupported sample rate, try 48000\n");
} else {
printf("fsr = %f \n", r);
}
}
if (strcmp(argv[arg-2], "-iq") == 0) {
loadFilePath = argv[arg-1];
iq_file_flag = 1;
if (loadFilePath != NULL && loadFilePath[0] != 0) {
printf("iq_file = %c%s%c \n", 34,loadFilePath,34);
}
}
}
}
}
int main( int argc, char **argv )
{
struct sockaddr_in6 my_addr;
handle_args(argc, argv);
if (iq_file_flag == 1) {
float scale = 1.0;
char *fpath = loadFilePath ;
read_iq_file(fpath, scale);
}
/*
if (0) {
normal_rand_test();
exit(0);
}
*/
/*
if (argc > 1) {
char *s = argv[2];
if (s != NULL && s[0] != 0) {
sscanf(s, "%d", &port);
printf("port = %d \n", port);
}
}
*/
sigact.sa_handler = sighandler;
sigemptyset(&sigact.sa_mask);
sigact.sa_flags = 0;
sigaction(SIGINT, &sigact, NULL);
sigaction(SIGTERM, &sigact, NULL);
sigaction(SIGQUIT, &sigact, NULL);
#ifdef __APPLE__
signal(SIGPIPE, SIG_IGN);
#else
sigign.sa_handler = SIG_IGN;
sigaction(SIGPIPE, &sigign, NULL);
#endif
memset( &my_addr, 0, sizeof(my_addr) );
bzero((char *) &my_addr, sizeof(my_addr));
if ( (fd = socket(AF_INET6, SOCK_DGRAM, 0)) < 0 ) {
perror( "socket failed" );
return 1;
}
int rr = 1;
setsockopt(fd, SOL_SOCKET, SO_REUSEADDR,
(char *)&rr, 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 messages on port %d \n", port);
dpkt_cnt = 0;
udp_running = 1;
while (udp_running > 0) {
int flags = 0;
int length = 0;
socklen_t addr6Len = sizeof(client_addr);
bzero(rcvDataBuf, 1032);
length = recvfrom( fd, rcvDataBuf, 1032,
flags,
(struct sockaddr *)&client_addr, &addr6Len);
if ( length < 0 ) {
perror( "recvfrom failed" );
break;
}
unsigned char *db = rcvDataBuf;
handle_receive_data( db, length);
}
close( fd );
printf("port %d closed \n", port);
} // main
// Copyright 2024 Ronald H Nicholson, Jr
// (re)Distribution allowed under MPL 1.1
// Distribution under MPL 1.1 is Incompatible With Secondary Licenses
// eof
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment