commit 24aa6fb407c8874c873911850f44fd0973da0d2b Author: bkiedinger@gmail.com Date: Mon May 25 22:36:52 2026 -0500 first commit diff --git a/cpp/.gitignore b/cpp/.gitignore new file mode 100644 index 0000000..deb0fb5 --- /dev/null +++ b/cpp/.gitignore @@ -0,0 +1,5 @@ +*.so +*.o +*.bin* +.vscode/ +test_data_recorder \ No newline at end of file diff --git a/cpp/Makefile b/cpp/Makefile new file mode 100755 index 0000000..ac6bf41 --- /dev/null +++ b/cpp/Makefile @@ -0,0 +1,25 @@ +CC = g++ +FLAGS = -std=c++17 -pthread +# CFLAGS = +CFLAGS = -fPIC +LDFLAGS = -shared +# LDFLAGS = -L/usr/lib/aarch64-linux-gnu +DEBUGFLAGS = -O0 +RELEASEFLAGS = -O2 + +all: test_data_recorder + +PROGS=test_data_recorder library +all: $(PROGS) + +library: data_recorder.o + $(CC) $(LDFLAGS) $(CFLAGS) -o data_recorder.so $^ + +test_data_recorder: data_recorder.o test_data_recorder.o + $(CC) -pthread -o $@ $^ + +clean: + rm -f $(PROGS) *.o *.a *.d + +%.o: %.cpp + $(CC) -c $(FLAGS) $(CFLAGS) -o $@ $< diff --git a/cpp/data_recorder.cpp b/cpp/data_recorder.cpp new file mode 100755 index 0000000..cf71bfb --- /dev/null +++ b/cpp/data_recorder.cpp @@ -0,0 +1,417 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "data_recorder.h" + +double timespec_to_double(struct timespec t) +{ + return t.tv_sec + t.tv_nsec * 1e-9; +} + +double timespec_sub(struct timespec t1, struct timespec t2) +{ + // Get seconds part + t1.tv_sec -= t2.tv_sec; + + // Nanoseconds, need to check for negative condition + t1.tv_nsec -= t2.tv_nsec; + if (t1.tv_nsec >= 1000000000) { + t1.tv_sec++; + t1.tv_nsec -= 1000000000; + } else if (t1.tv_nsec < 0) { + t1.tv_sec--; + t1.tv_nsec += 1000000000; + } + + return timespec_to_double(t1); +} + +timespec timespec_add(struct timespec t1, struct timespec t2) +{ + // Get seconds part + t1.tv_sec += t2.tv_sec; + + // Nanoseconds, need to check for negative condition + t1.tv_nsec += t2.tv_nsec; + if (t1.tv_nsec >= 1000000000) { + t1.tv_sec++; + t1.tv_nsec -= 1000000000; + } + + return t1; +} + +DataRecorder::DataRecorder() { + printf("Data Recorder\n"); + recording_active = false; + allocate_memory(); + + // Prep access to PL registers + plfd = open("/dev/wsrpl0", O_RDWR); + if (plfd < 0){ + printf("Failed to open %s\n", "/dev/wsrpl0"); + } + + plmmap = (uint32_t *)mmap(NULL, 0x1000000, PROT_WRITE | PROT_READ, MAP_SHARED, plfd, 0); + + if (plmmap < (uint32_t *)0){ + printf("Failed to mmap %s\n", "/dev/wsrpl0"); + close(plfd); + } + + validate_cnt_data = false; + + return; +} + +void DataRecorder::allocate_memory() { + for (int ch_ind = 0; ch_ind < NUM_DMA_CH; ch_ind++) { + // Memory to buffer data into + data_buffer[ch_ind] = (char *)aligned_alloc(4096, OVERALL_BUFFER_SIZE); + memset(data_buffer[ch_ind], 0, OVERALL_BUFFER_SIZE); + sem_init(&buffer_ready_sem[ch_ind], 0, 0); + recording_rate[ch_ind] = 0; + total_bytes[ch_ind] = 0; + } +} + +void DataRecorder::write_reg(uint32_t addr, uint32_t data) { + plmmap[addr >> 2] = data; +} + +uint32_t DataRecorder::read_reg(uint32_t addr) { + return plmmap[addr >> 2]; +} + +DataRecorder::~DataRecorder() { + printf("~Data Recorder\n"); + for (int ch_ind = 0; ch_ind < NUM_DMA_CH; ch_ind++) { + free(data_buffer[ch_ind]); + sem_destroy(&buffer_ready_sem[ch_ind]); + } + return; +} + +int DataRecorder::start_recording(const char* filename, int save_to_disk) { + + recording_active = true; + + int ret; + for (int ch_ind = 0; ch_ind < NUM_DMA_CH; ch_ind++) { + while (true) { + if (ret = sem_trywait(&buffer_ready_sem[ch_ind]) == -1) { + printf("Ch %d Semaphore Cleared\n", ch_ind); + break; + } + printf("Ch %d Semaphore Was Still Available!!!!\n", ch_ind); + } + } + + // Make sure old thread is done + for (int ch_ind = 0; ch_ind < NUM_DMA_CH; ch_ind++) { + if (recorder[ch_ind].joinable()) { + printf("Thread was still joinable!!!! %d\n", ch_ind); + DataRecorder::stop_recording(); + } + } + + exit_thread = false; + + // Open Output files + for (int i = 0; i < NUM_DMA_CH; i++) { + char* file; + asprintf(&file, "%s_%d", filename, i); + printf("Opening File: %s", file); + out_fd[i] = open(file, O_WRONLY | O_CREAT | O_TRUNC | O_DIRECT, 0666); + if(out_fd[i] < 0) + { + printf("- FAILED!!!\n"); + return -1; + } + + printf(" - SUCCESS FD: %d\n", out_fd[i]); + } + + for (int ch_ind = 0; ch_ind < NUM_DMA_CH; ch_ind++) { + recorder[ch_ind] = std::thread(&DataRecorder::get_data, this, ch_ind, save_to_disk); + } + + sleep(1); + + printf("Recording Started\n"); + + return 1; +} + +int DataRecorder::stop_recording() { + exit_thread = true; + // Wait for recorder thread to finish + for (int ch_ind = 0; ch_ind < NUM_DMA_CH; ch_ind++) { + if (recorder[ch_ind].joinable()) { + recorder[ch_ind].join(); + } + } + printf("Thread Joined!\n"); + recording_active = false; + return 1; +} + +std::vector DataRecorder::get_recording_rate() { + std::vector rate; + for (int ch_ind = 0; ch_ind < NUM_DMA_CH; ch_ind++) { + rate.push_back(recording_rate[ch_ind]); + } + return rate; +} + +std::vector DataRecorder::get_filesize() { + std::vector bytes; + for (int ch_ind = 0; ch_ind < NUM_DMA_CH; ch_ind++) { + bytes.push_back(total_bytes[ch_ind]); + } + return bytes; +} + +void DataRecorder::write_data(int ch_ind) { + printf("Opening Write Data Thread\n"); + + int buffer_ind = 0; + + struct timespec ts; + struct timespec timeout; + timeout.tv_sec = 0; + timeout.tv_nsec = 0.1e9; + + int ret; + + int write_chunk_size = BUFFER_SIZE; + + int bytes_avail = 0; + int write_ind = 0; + int sem_value; + + while (!exit_thread.load()) { + + clock_gettime(CLOCK_REALTIME, &ts); + ts = timespec_add(ts, timeout); + + if (ret = sem_timedwait(&buffer_ready_sem[ch_ind], &ts) == -1) { + // Error + if (errno == ETIMEDOUT) { + // printf("Writer sem timeout\n"); + } + else { + printf("sem_wait error %d\n", errno); + } + continue; + } + + if (num_bytes_to_write[ch_ind][buffer_ind] != BUFFER_SIZE) { + printf("hmmmmmmmmm %d", num_bytes_to_write[ch_ind][buffer_ind]); + } + + bytes_avail += num_bytes_to_write[ch_ind][buffer_ind]; + + if (bytes_avail >= write_chunk_size) { + sem_getvalue(&buffer_ready_sem[ch_ind], &sem_value); + int cnt = write(out_fd[ch_ind], &(data_buffer[ch_ind][write_ind]), bytes_avail); + if (cnt < 0) + { + printf("File write error!\n"); + } + total_bytes[ch_ind] += cnt; + write_ind += bytes_avail; + bytes_avail = 0; + write_ind = write_ind % OVERALL_BUFFER_SIZE; + } + + buffer_ind++; + buffer_ind = buffer_ind % NUM_BUFFERS; + + // int cnt = write(out_fd[ch_ind], &(data_buffer[ch_ind][buffer_ind*BUFFER_SIZE]), num_bytes_to_write[ch_ind][buffer_ind]); + // if (cnt < 0) + // { + // printf("File write error! %d, %d, %d, %p\n", ch_ind, buffer_ind, errno, &(data_buffer[ch_ind][buffer_ind*BUFFER_SIZE])); + // } + // total_bytes[ch_ind] += cnt; + // buffer_ind++; + // buffer_ind = buffer_ind % NUM_USERSPACE_BUFFERS; + } + + sem_getvalue(&buffer_ready_sem[ch_ind], &sem_value); + + printf("Exiting Write Data Thread %d, %d\n", ch_ind, sem_value); +} + +void DataRecorder::set_validate_cnt_data(bool enable, uint32_t pri, uint32_t inter, uint32_t count) { + validate_cnt_data = enable; + cnt_pri = pri; + cnt_num_pulse = count; + cnt_inter_pri = inter; +} + +void DataRecorder::get_data(int ch_ind, int save_to_disk) { + + char* file; + asprintf(&file, "/dev/wsrdma%d", ch_ind); + + // Start write to disk thread + if (save_to_disk) { + writer[ch_ind] = std::thread(&DataRecorder::write_data, this, ch_ind); + } + + int fd = open(file, O_RDWR); + if (fd < 0) { + printf(" open failed\n"); + return; + } + + wsrpcie_ioctl_t wsrpcie_ioctl; + + wsrpcie_ioctl.cmd = WSRDMA_SET_NUM_BYTES; + wsrpcie_ioctl.offset = 0; + wsrpcie_ioctl.value = BUFFER_SIZE; + ioctl(fd, 0, &wsrpcie_ioctl); + + wsrpcie_ioctl.cmd = WSRDMA_SET_NUM_BUFS; + wsrpcie_ioctl.offset = 0; + wsrpcie_ioctl.value = NUM_BUFFERS; + ioctl(fd, 0, &wsrpcie_ioctl); + + wsrpcie_ioctl.cmd = WSRDMA_DMA_INIT; + wsrpcie_ioctl.offset = 0; + wsrpcie_ioctl.value = 0; + ioctl(fd, 0, &wsrpcie_ioctl); + + wsrpcie_ioctl.cmd = WSRDMA_DMA_START; + wsrpcie_ioctl.offset = 0; + wsrpcie_ioctl.value = 0; + ioctl(fd, 0, &wsrpcie_ioctl); + + + struct timespec ts_now; + struct timespec ts_last_print; + struct timespec ts_begin; + + total_bytes[ch_ind] = 0; + long int bytes_since_last_update = 0; + + // For timing info + clock_gettime(CLOCK_MONOTONIC, &ts_begin); + ts_last_print = ts_begin; + double last_print_time = 0; + double last_irq_elapsed = 0; + double print_period = 1; + + printf("Waiting for data\n"); + uint32_t buffer_ind = 0; + uint32_t write_cnt = 0; + bool init_cnt = true; + uint64_t current_cnt; + uint64_t pulse_cnt = 0; + + while (!exit_thread.load()) { + int read_count = read(fd, &(data_buffer[ch_ind][buffer_ind*BUFFER_SIZE]), BUFFER_SIZE); + // reference to current data buffer + char * data_buf = &(data_buffer[ch_ind][buffer_ind*BUFFER_SIZE]); + bytes_since_last_update += read_count; + clock_gettime(CLOCK_MONOTONIC, &ts_now); + + if (read_count) { + // printf("read count %d\n", read_count); + + if (save_to_disk) { + num_bytes_to_write[ch_ind][buffer_ind] = read_count; + if (sem_post(&buffer_ready_sem[ch_ind]) == -1) { + printf("sem_post error\n"); + } + } + + if (validate_cnt_data) { + + uint64_t * data = (uint64_t *)&(data_buffer[ch_ind][buffer_ind*BUFFER_SIZE]); + int num_samp = read_count / (sizeof(uint64_t) * 2); + if (init_cnt) { + current_cnt = data[0]; + init_cnt = false; + } + uint64_t first_sample_cnt = data[0]; + for (int i = 0; i < num_samp; i++) { + if (data[i*2] != current_cnt) { + printf("Data Mismatch!!! CH %d, Got 0x%lx, Expected 0x%lx, Diff %lu, Pulse %lu\n", + ch_ind, data[i*2], current_cnt, data[i*2] - current_cnt, pulse_cnt); + break; + } + current_cnt++; + } + // count data is determined from a freerunning counter in the FPGA, so there will be expected gaps in count values between + // pulses. Set the current count to the next expected value at the end of a pulse + current_cnt = first_sample_cnt + cnt_pri; + pulse_cnt++; + // Handle inter cpi delay for count data + if ((pulse_cnt % cnt_num_pulse) == 0) { + current_cnt += cnt_inter_pri; + } + } + + buffer_ind++; + buffer_ind = buffer_ind % NUM_BUFFERS; + } else { + // Small sleep if no data is available, without this each thread will peg a CPU core + usleep(1); + } + + if (timespec_sub(ts_now, ts_last_print) > 1) { + double elapsed = timespec_sub(ts_now, ts_begin); + double rate = (double)total_bytes[ch_ind] / elapsed; + elapsed = timespec_sub(ts_now, ts_last_print); + double rate_last = (double)bytes_since_last_update / elapsed; + clock_gettime(CLOCK_MONOTONIC, &ts_last_print); + bytes_since_last_update = 0; + printf("Ch %d, Data Rate (MB/s) %0.2f, Data Rate last update (MB/s) %0.2f, Total Recorded (MB) %0.2f\n", ch_ind, rate/1e6, rate_last/1e6, total_bytes[ch_ind]/1e6); + + recording_rate[ch_ind] = rate; + } + + } + + wsrpcie_ioctl.cmd = WSRDMA_DMA_STOP; + wsrpcie_ioctl.offset = 0; + wsrpcie_ioctl.value = 0; + + ioctl(fd, 0, &wsrpcie_ioctl); + + wsrpcie_ioctl.cmd = WSRDMA_DMA_CLEAR; + wsrpcie_ioctl.offset = 0; + wsrpcie_ioctl.value = 0; + + ioctl(fd, 0, &wsrpcie_ioctl); + + // Make sure write thread is done before closing file handle + if (writer[ch_ind].joinable()) { + writer[ch_ind].join(); + } + + // Want to write out that last little bit of data if we didn't make it to a full chunk + + + close(fd); + close(out_fd[ch_ind]); + + recording_rate[ch_ind] = 0; + + printf("DMA %s exiting\n", file); + +} diff --git a/cpp/data_recorder.h b/cpp/data_recorder.h new file mode 100755 index 0000000..90b6f7c --- /dev/null +++ b/cpp/data_recorder.h @@ -0,0 +1,79 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#define UTIL_REG_BASE 0x100000 +#define TIMING_REG_BASE 0x110000 +#define DATA_GEN_REG_BASE 0x120000 + +#define WSRDMA_DMA_INIT 0 +#define WSRDMA_DMA_CLEAR 1 +#define WSRDMA_DMA_START 2 +#define WSRDMA_DMA_STOP 3 +#define WSRDMA_SET_NUM_BUFS 4 +#define WSRDMA_SET_NUM_BYTES 5 +#define WSRDMA_GET_NUM_BUFS 6 +#define WSRDMA_GET_NUM_BYTES 7 +#define WSRDMA_GET_FREE_BUFS 8 + +typedef struct { + unsigned int cmd; + unsigned int offset; + unsigned int value; +} wsrpcie_ioctl_t; + +#define NUM_DMA_CH 2 +#define BUFFER_SIZE (4 * 1024 * 1024) +// #define BUFFER_SIZE (8192) +#define NUM_BUFFERS 128 +#define OVERALL_BUFFER_SIZE (BUFFER_SIZE * NUM_BUFFERS) + +#define CLK_RATE 250e6 + +class DataRecorder { + public: + DataRecorder(); + ~DataRecorder(); + + int start_recording(const char* filename, int save_to_disk); + int stop_recording(); + std::vector get_recording_rate(); + std::vector get_filesize(); + void set_validate_cnt_data(bool enable, uint32_t pri, uint32_t inter, uint32_t count); + void write_reg(uint32_t addr, uint32_t data); + uint32_t read_reg(uint32_t addr); + + bool recording_active; + + private: + void allocate_memory(); + void get_data(int ch_ind, int save_to_disk); + void write_data(int ch_ind); + + int plfd; + volatile uint32_t * plmmap; + + std::thread recorder[NUM_DMA_CH]; + std::thread writer[NUM_DMA_CH]; + std::queue buffer_queue[NUM_DMA_CH]; + sem_t buffer_ready_sem[NUM_DMA_CH]; + int num_bytes_to_write[NUM_DMA_CH][NUM_BUFFERS]; + int out_fd[NUM_DMA_CH]; + char * data_buffer[NUM_DMA_CH]; + std::atomic exit_thread; + + float recording_rate[NUM_DMA_CH]; + uint64_t total_bytes[NUM_DMA_CH]; + + bool validate_cnt_data; + uint32_t cnt_pri; + uint32_t cnt_inter_pri; + uint32_t cnt_num_pulse; + +}; \ No newline at end of file diff --git a/cpp/test_data_recorder.cpp b/cpp/test_data_recorder.cpp new file mode 100755 index 0000000..fca6ae4 --- /dev/null +++ b/cpp/test_data_recorder.cpp @@ -0,0 +1,47 @@ +#include "data_recorder.h" +int main() { + // Instantiate the class + DataRecorder dr; + + printf("Test Reg Read - 0x%x\n", dr.read_reg(UTIL_REG_BASE + 0x0)); + + // Setup Timing Engine and data generator to test + float pri = 90e-6; + uint32_t n_pulses = 128; + float inter_cpi = 100e-6; + uint32_t n_samples = 8192; + + float cpi_time = n_pulses * pri + inter_cpi; + float cpi_num_bytes = n_pulses * n_samples * 16; + + float expected_data_rate = cpi_num_bytes / cpi_time / 1e6; + // PCIe Gen3 x4 Therotecial Max is 4GBPS + printf("Expected Data Rate - %.2f MBps Per Channel, Total %.2f\n", expected_data_rate, expected_data_rate * NUM_DMA_CH); + + dr.write_reg(TIMING_REG_BASE + 0x0, 1); + dr.write_reg(TIMING_REG_BASE + 0x4, (uint32_t)(pri * CLK_RATE + 0.5) - 1); + dr.write_reg(TIMING_REG_BASE + 0x8, n_pulses); + dr.write_reg(TIMING_REG_BASE + 0x10, inter_cpi * CLK_RATE); + + for (int i = 0; i < 4; i++) { + printf("Timing Reg 0x%x - 0x%x\n", i*4, dr.read_reg(TIMING_REG_BASE + i*4)); + } + + dr.write_reg(DATA_GEN_REG_BASE, n_samples - 1); + + dr.set_validate_cnt_data(true, dr.read_reg(TIMING_REG_BASE + 0x4) + 1, dr.read_reg(TIMING_REG_BASE + 0x10) + 1, n_pulses); + + // Start listening for data + dr.start_recording("test.bin", 0); + // Start the timing engine so data starts flowing + dr.write_reg(TIMING_REG_BASE + 0x0, 0); + // Wait a while + std::this_thread::sleep_for(std::chrono::milliseconds(6000)); + + dr.stop_recording(); + dr.write_reg(TIMING_REG_BASE + 0x0, 1); + + printf("Expected Data Rate - %.2f MBps Per Channel, Total %.2f\n", expected_data_rate, expected_data_rate * NUM_DMA_CH); + + +} \ No newline at end of file diff --git a/top.xsa b/top.xsa new file mode 100644 index 0000000..8f554c3 Binary files /dev/null and b/top.xsa differ