// 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 #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef PI_LINUX #include #include #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 #include // 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