ogl_beamforming

Ultrasound Beamforming Implemented with OpenGL
git clone anongit@rnpnr.xyz:ogl_beamforming.git
Log | Files | Refs | Feed | Submodules | LICENSE

ogl_beamformer_lib.c (9581B)


      1 /* See LICENSE for license details. */
      2 #include "ogl_beamformer_lib.h"
      3 
      4 typedef struct {
      5 	BeamformerParameters raw;
      6 	ComputeShaderID compute_stages[16];
      7 	u32             compute_stages_count;
      8 	b32             upload;
      9 	u32             raw_data_size;
     10 	b32             export_next_frame;
     11 	c8              export_pipe_name[1024];
     12 } BeamformerParametersFull;
     13 
     14 typedef struct {
     15 	iptr  file;
     16 	char *name;
     17 } Pipe;
     18 
     19 typedef struct { size len; u8 *data; } s8;
     20 #define s8(s) (s8){.len = ARRAY_COUNT(s) - 1, .data = (u8 *)s}
     21 
     22 #define ARRAY_COUNT(a) (sizeof(a) / sizeof(*a))
     23 
     24 #define U32_MAX (0xFFFFFFFFUL)
     25 
     26 #define INVALID_FILE (-1)
     27 
     28 #define PIPE_RETRY_PERIOD_MS (100ULL)
     29 
     30 static volatile BeamformerParametersFull *g_bp;
     31 static Pipe g_pipe = {.file = INVALID_FILE};
     32 
     33 #if defined(__unix__)
     34 #include <fcntl.h>
     35 #include <poll.h>
     36 #include <sys/mman.h>
     37 #include <sys/stat.h>
     38 #include <time.h>
     39 #include <unistd.h>
     40 
     41 #define OS_EXPORT_PIPE_NAME "/tmp/beamformer_output_pipe"
     42 
     43 #elif defined(_WIN32)
     44 
     45 #define OS_EXPORT_PIPE_NAME "\\\\.\\pipe\\beamformer_output_fifo"
     46 
     47 #define OPEN_EXISTING        3
     48 #define GENERIC_WRITE        0x40000000
     49 #define FILE_MAP_ALL_ACCESS  0x000F001F
     50 
     51 #define PIPE_TYPE_BYTE      0x00
     52 #define PIPE_ACCESS_INBOUND 0x01
     53 
     54 #define PIPE_WAIT   0x00
     55 #define PIPE_NOWAIT 0x01
     56 
     57 #define ERROR_NO_DATA            232L
     58 #define ERROR_PIPE_NOT_CONNECTED 233L
     59 #define ERROR_PIPE_LISTENING     536L
     60 
     61 #define W32(r) __declspec(dllimport) r __stdcall
     62 W32(b32)  CloseHandle(iptr);
     63 W32(iptr) CreateFileA(c8 *, u32, u32, void *, u32, u32, void *);
     64 W32(iptr) CreateNamedPipeA(c8 *, u32, u32, u32, u32, u32, u32, void *);
     65 W32(b32)  DisconnectNamedPipe(iptr);
     66 W32(i32)  GetLastError(void);
     67 W32(iptr) MapViewOfFile(iptr, u32, u32, u32, u64);
     68 W32(iptr) OpenFileMappingA(u32, b32, c8 *);
     69 W32(b32)  ReadFile(iptr, u8 *, i32, i32 *, void *);
     70 W32(void) Sleep(u32);
     71 W32(void) UnmapViewOfFile(iptr);
     72 W32(b32)  WriteFile(iptr, u8 *, i32, i32 *, void *);
     73 
     74 #else
     75 #error Unsupported Platform
     76 #endif
     77 
     78 #if defined(MATLAB_CONSOLE)
     79 #define mexErrMsgIdAndTxt  mexErrMsgIdAndTxt_800
     80 #define mexWarnMsgIdAndTxt mexWarnMsgIdAndTxt_800
     81 void mexErrMsgIdAndTxt(const c8*, c8*, ...);
     82 void mexWarnMsgIdAndTxt(const c8*, c8*, ...);
     83 #define error_tag "ogl_beamformer_lib:error"
     84 #define error_msg(...)   mexErrMsgIdAndTxt(error_tag, __VA_ARGS__)
     85 #define warning_msg(...) mexWarnMsgIdAndTxt(error_tag, __VA_ARGS__)
     86 #else
     87 #define error_msg(...)
     88 #define warning_msg(...)
     89 #endif
     90 
     91 #if defined(__unix__)
     92 static Pipe
     93 os_open_named_pipe(char *name)
     94 {
     95 	return (Pipe){.file = open(name, O_WRONLY), .name = name};
     96 }
     97 
     98 static Pipe
     99 os_open_read_pipe(char *name)
    100 {
    101 	mkfifo(name, 0660);
    102 	return (Pipe){.file = open(name, O_RDONLY|O_NONBLOCK), .name = name};
    103 }
    104 
    105 static void
    106 os_disconnect_pipe(Pipe p)
    107 {
    108 }
    109 
    110 static void
    111 os_close_pipe(iptr *file, char *name)
    112 {
    113 	if (file) close(*file);
    114 	if (name) unlink(name);
    115 	*file = INVALID_FILE;
    116 }
    117 
    118 static b32
    119 os_wait_read_pipe(Pipe p, void *buf, size read_size, u32 timeout_ms)
    120 {
    121 	struct pollfd pfd = {.fd = p.file, .events = POLLIN};
    122 	size total_read = 0;
    123 	if (poll(&pfd, 1, timeout_ms) > 0) {
    124 		size r;
    125 		do {
    126 			 r = read(p.file, (u8 *)buf + total_read, read_size - total_read);
    127 			 if (r > 0) total_read += r;
    128 		} while (r != 0);
    129 	}
    130 	return total_read == read_size;
    131 }
    132 
    133 static size
    134 os_write(iptr f, void *data, size data_size)
    135 {
    136 	size written = 0, w = 0;
    137 	do {
    138 		w = write(f, (u8 *)data + written, data_size - written);
    139 		if (w != -1) written += w;
    140 	} while (written != data_size && w != 0);
    141 	return written;
    142 }
    143 
    144 static BeamformerParametersFull *
    145 os_open_shared_memory_area(char *name)
    146 {
    147 	i32 fd = shm_open(name, O_RDWR, S_IRUSR|S_IWUSR);
    148 	if (fd == -1)
    149 		return NULL;
    150 
    151 	BeamformerParametersFull *new;
    152 	new = mmap(NULL, sizeof(*new), PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0);
    153 	close(fd);
    154 
    155 	if (new == MAP_FAILED)
    156 		return NULL;
    157 
    158 	return new;
    159 }
    160 
    161 static void
    162 os_release_shared_memory(iptr memory, u64 size)
    163 {
    164 	munmap((void *)memory, size);
    165 }
    166 
    167 #elif defined(_WIN32)
    168 
    169 static Pipe
    170 os_open_named_pipe(char *name)
    171 {
    172 	iptr pipe = CreateFileA(name, GENERIC_WRITE, 0, 0, OPEN_EXISTING, 0, 0);
    173 	return (Pipe){.file = pipe, .name = name};
    174 }
    175 
    176 static Pipe
    177 os_open_read_pipe(char *name)
    178 {
    179 	iptr file = CreateNamedPipeA(name, PIPE_ACCESS_INBOUND, PIPE_TYPE_BYTE|PIPE_NOWAIT, 1,
    180 	                             0, 1024UL * 1024UL, 0, 0);
    181 	return (Pipe){.file = file, .name = name};
    182 }
    183 
    184 static void
    185 os_disconnect_pipe(Pipe p)
    186 {
    187 	DisconnectNamedPipe(p.file);
    188 }
    189 
    190 static void
    191 os_close_pipe(iptr *file, char *name)
    192 {
    193 	if (file) CloseHandle(*file);
    194 	*file = INVALID_FILE;
    195 }
    196 
    197 static b32
    198 os_wait_read_pipe(Pipe p, void *buf, size read_size, u32 timeout_ms)
    199 {
    200 	size elapsed_ms = 0, total_read = 0;
    201 	while (elapsed_ms <= timeout_ms && read_size != total_read) {
    202 		u8 data;
    203 		i32 read;
    204 		b32 result = ReadFile(p.file, &data, 0, &read, 0);
    205 		if (!result) {
    206 			i32 error = GetLastError();
    207 			if (error != ERROR_NO_DATA &&
    208 			    error != ERROR_PIPE_LISTENING &&
    209 			    error != ERROR_PIPE_NOT_CONNECTED)
    210 			{
    211 				/* NOTE: pipe is in a bad state; we will never read anything */
    212 				break;
    213 			}
    214 			Sleep(PIPE_RETRY_PERIOD_MS);
    215 			elapsed_ms += PIPE_RETRY_PERIOD_MS;
    216 		} else {
    217 			ReadFile(p.file, (u8 *)buf + total_read, read_size - total_read, &read, 0);
    218 			total_read += read;
    219 		}
    220 	}
    221 	return total_read == read_size;
    222 }
    223 
    224 static size
    225 os_write(iptr f, void *data, size data_size)
    226 {
    227 	i32 written = 0;
    228 	b32 result = WriteFile(f, (u8 *)data, data_size, &written, 0);
    229 	if (!result) {
    230 		i32 error = GetLastError();
    231 		warning_msg("os_write(data_size = %td): error: %d", data_size, error);
    232 	}
    233 	return written;
    234 }
    235 
    236 static BeamformerParametersFull *
    237 os_open_shared_memory_area(char *name)
    238 {
    239 	iptr h = OpenFileMappingA(FILE_MAP_ALL_ACCESS, 0, name);
    240 	if (h == INVALID_FILE)
    241 		return 0;
    242 
    243 	BeamformerParametersFull *new;
    244 	iptr view = MapViewOfFile(h, FILE_MAP_ALL_ACCESS, 0, 0, sizeof(*new));
    245 	new = (BeamformerParametersFull *)view;
    246 	CloseHandle(h);
    247 
    248 	return new;
    249 }
    250 
    251 static void
    252 os_release_shared_memory(iptr memory, u64 size)
    253 {
    254 	UnmapViewOfFile(memory);
    255 }
    256 
    257 #endif
    258 
    259 static b32
    260 check_shared_memory(char *name)
    261 {
    262 	if (!g_bp) {
    263 		g_bp = os_open_shared_memory_area(name);
    264 		if (!g_bp) {
    265 			error_msg("failed to open shared memory area");
    266 			return 0;
    267 		}
    268 	}
    269 	return 1;
    270 }
    271 
    272 b32
    273 set_beamformer_pipeline(char *shm_name, i32 *stages, i32 stages_count)
    274 {
    275 	if (stages_count > ARRAY_COUNT(g_bp->compute_stages)) {
    276 		error_msg("maximum stage count is %lu", ARRAY_COUNT(g_bp->compute_stages));
    277 		return 0;
    278 	}
    279 
    280 	if (!check_shared_memory(shm_name))
    281 		return 0;
    282 
    283 	for (i32 i = 0; i < stages_count; i++) {
    284 		b32 valid = 0;
    285 		#define X(en, number, sfn, nh, pn) if (number == stages[i]) valid = 1;
    286 		COMPUTE_SHADERS
    287 		#undef X
    288 
    289 		if (!valid) {
    290 			error_msg("invalid shader stage: %d", stages[i]);
    291 			return 0;
    292 		}
    293 
    294 		g_bp->compute_stages[i] = stages[i];
    295 	}
    296 	g_bp->compute_stages_count = stages_count;
    297 
    298 	return 1;
    299 }
    300 
    301 b32
    302 set_beamformer_parameters(char *shm_name, BeamformerParameters *new_bp)
    303 {
    304 	if (!check_shared_memory(shm_name))
    305 		return 0;
    306 
    307 	u8 *src = (u8 *)new_bp, *dest = (u8 *)&g_bp->raw;
    308 	for (size i = 0; i < sizeof(BeamformerParameters); i++)
    309 		dest[i] = src[i];
    310 	g_bp->upload = 1;
    311 
    312 	return 1;
    313 }
    314 
    315 b32
    316 send_data(char *pipe_name, char *shm_name, void *data, u32 data_size)
    317 {
    318 	b32 result = g_pipe.file != INVALID_FILE;
    319 	if (!result) {
    320 		g_pipe = os_open_named_pipe(pipe_name);
    321 		result = g_pipe.file != INVALID_FILE;
    322 		if (!result)
    323 			error_msg("failed to open pipe");
    324 	}
    325 	result &= check_shared_memory(shm_name);
    326 
    327 	if (result) {
    328 		g_bp->raw_data_size = data_size;
    329 		g_bp->upload        = 1;
    330 
    331 		size written = os_write(g_pipe.file, data, data_size);
    332 		result = written == data_size;
    333 		if (!result) {
    334 			warning_msg("failed to write data to pipe: retrying...");
    335 			os_close_pipe(&g_pipe.file, 0);
    336 			os_release_shared_memory((iptr)g_bp, sizeof(*g_bp));
    337 			g_bp   = 0;
    338 			g_pipe = os_open_named_pipe(pipe_name);
    339 			result = g_pipe.file != INVALID_FILE && check_shared_memory(shm_name);
    340 			if (result)
    341 				written = os_write(g_pipe.file, data, data_size);
    342 			result = written == data_size;
    343 			if (!result)
    344 				warning_msg("failed again, wrote %ld/%u\ngiving up",
    345 				            written, data_size);
    346 		}
    347 	}
    348 
    349 	return result;
    350 }
    351 
    352 b32
    353 beamform_data_synchronized(char *pipe_name, char *shm_name, void *data, u32 data_size,
    354                            uv4 output_points, f32 *out_data, i32 timeout_ms)
    355 {
    356 	if (!check_shared_memory(shm_name))
    357 		return 0;
    358 
    359 	if (output_points.x == 0) output_points.x = 1;
    360 	if (output_points.y == 0) output_points.y = 1;
    361 	if (output_points.z == 0) output_points.z = 1;
    362 	output_points.w = 1;
    363 
    364 	g_bp->raw.output_points.x = output_points.x;
    365 	g_bp->raw.output_points.y = output_points.y;
    366 	g_bp->raw.output_points.z = output_points.z;
    367 	g_bp->export_next_frame   = 1;
    368 
    369 	s8 export_name = s8(OS_EXPORT_PIPE_NAME);
    370 	if (export_name.len > ARRAY_COUNT(g_bp->export_pipe_name)) {
    371 		error_msg("export pipe name too long");
    372 		return 0;
    373 	}
    374 
    375 	Pipe export_pipe = os_open_read_pipe(OS_EXPORT_PIPE_NAME);
    376 	if (export_pipe.file == INVALID_FILE) {
    377 		error_msg("failed to open export pipe");
    378 		return 0;
    379 	}
    380 
    381 	for (u32 i = 0; i < export_name.len; i++)
    382 		g_bp->export_pipe_name[i] = export_name.data[i];
    383 
    384 	b32 result = send_data(pipe_name, shm_name, data, data_size);
    385 	if (result) {
    386 		size output_size = output_points.x * output_points.y * output_points.z * sizeof(f32) * 2;
    387 		result = os_wait_read_pipe(export_pipe, out_data, output_size, timeout_ms);
    388 		if (!result)
    389 			warning_msg("failed to read full export data from pipe");
    390 	}
    391 
    392 	os_disconnect_pipe(export_pipe);
    393 	os_close_pipe(&export_pipe.file, export_pipe.name);
    394 	os_close_pipe(&g_pipe.file, 0);
    395 
    396 	return result;
    397 }