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 }