test_common.c (16172B)
1 /* See LICENSE for license details. */ 2 #define BASE_EXPORT function 3 #define BASE_IMPORT function 4 #define BEAMFORMER_LIB_EXPORT function 5 #include "base_platform.h" 6 #include "ogl_beamformer_lib.c" 7 8 #include <signal.h> 9 #include <stdarg.h> 10 #include <stdio.h> 11 #include <stdlib.h> 12 #include <zstd.h> 13 14 #include "external/zemp_bp.h" 15 16 #define RXOrientation(tx_rx) ((ZBP_RCAOrientation)((tx_rx) >> 0 & 0x0F)) 17 #define TXOrientation(tx_rx) ((ZBP_RCAOrientation)((tx_rx) >> 4 & 0x0F)) 18 19 typedef struct { 20 ZBP_DataKind kind; 21 ZBP_DataCompressionKind compression_kind; 22 str8 bytes; 23 } ZBP_Data; 24 25 #define shift_n(v, c, n) v += n, c -= n 26 #define shift(v, c) shift_n(v, c, 1) 27 28 #define die(...) die_((char *)__func__, __VA_ARGS__) 29 function no_return void 30 die_(char *function_name, char *format, ...) 31 { 32 if (function_name) 33 fprintf(stderr, "%s: ", function_name); 34 35 va_list ap; 36 37 va_start(ap, format); 38 vfprintf(stderr, format, ap); 39 va_end(ap); 40 41 os_exit(1); 42 } 43 44 function void 45 beamformer_simple_parameters_equip_channel_mapping(BeamformerSimpleParameters *bp, u8 *raw, u32 channel_count, i32 channel_mapping_offset) 46 { 47 if (channel_mapping_offset != -1) { 48 memory_copy(bp->channel_mapping, raw + channel_mapping_offset, sizeof(*bp->channel_mapping) * channel_count); 49 } else { 50 for EachIndex(channel_count, it) 51 bp->channel_mapping[it] = it; 52 } 53 } 54 55 function void 56 beamformer_simple_parameters_equip_emission(BeamformerSimpleParameters *bp, u8 *raw, i32 emission_descriptors_offset) 57 { 58 // NOTE/TODO(rnp): only considering the first data frame right now 59 ZBP_EmissionDescriptor *ed = (ZBP_EmissionDescriptor *)(raw + emission_descriptors_offset); 60 switch (ed->emission_kind) { 61 62 case ZBP_EmissionKind_Sine:{ 63 ZBP_EmissionSineParameters *ep = (ZBP_EmissionSineParameters *)(raw + ed->parameters_offset); 64 bp->emission_parameters.kind = BeamformerEmissionKind_Sine; 65 bp->emission_parameters.sine.cycles = ep->cycles; 66 bp->emission_parameters.sine.frequency = ep->frequency; 67 }break; 68 69 case ZBP_EmissionKind_Chirp:{ 70 ZBP_EmissionChirpParameters *ep = (ZBP_EmissionChirpParameters *)(raw + ed->parameters_offset); 71 bp->emission_parameters.kind = BeamformerEmissionKind_Chirp; 72 bp->emission_parameters.chirp.duration = ep->duration; 73 bp->emission_parameters.chirp.min_frequency = ep->min_frequency; 74 bp->emission_parameters.chirp.max_frequency = ep->max_frequency; 75 }break; 76 77 InvalidDefaultCase; 78 static_assert(ZBP_EmissionKind_Count == (ZBP_EmissionKind_Chirp + 1), ""); 79 } 80 } 81 82 function void 83 beamformer_simple_parameters_equip_acquisition(BeamformerSimpleParameters *bp, u8 *raw, 84 BeamformerAcquisitionKind acquisition_kind, 85 i32 acquisition_parameters_offset) 86 { 87 switch (acquisition_kind) { 88 case ZBP_AcquisitionKind_FORCES:{ 89 ZBP_FORCESParameters *p = (ZBP_FORCESParameters *)(raw + acquisition_parameters_offset); 90 bp->transmit_receive_orientation = p->transmit_focus.transmit_receive_orientation; 91 bp->focal_vector.E[0] = p->transmit_focus.steering_angle; 92 bp->focal_vector.E[1] = p->transmit_focus.focal_depth; 93 94 bp->single_focus = 1; 95 bp->single_orientation = 1; 96 }break; 97 98 case ZBP_AcquisitionKind_HERCULES:{ 99 ZBP_HERCULESParameters *p = (ZBP_HERCULESParameters *)(raw + acquisition_parameters_offset); 100 bp->transmit_receive_orientation = p->transmit_focus.transmit_receive_orientation; 101 bp->focal_vector.E[0] = p->transmit_focus.steering_angle; 102 bp->focal_vector.E[1] = p->transmit_focus.focal_depth; 103 104 bp->single_focus = 1; 105 bp->single_orientation = 1; 106 }break; 107 108 case ZBP_AcquisitionKind_UFORCES:{ 109 ZBP_uFORCESParameters *p = (ZBP_uFORCESParameters *)(raw + acquisition_parameters_offset); 110 bp->transmit_receive_orientation = p->transmit_focus.transmit_receive_orientation; 111 bp->focal_vector.E[0] = p->transmit_focus.steering_angle; 112 bp->focal_vector.E[1] = p->transmit_focus.focal_depth; 113 114 bp->single_focus = 1; 115 bp->single_orientation = 1; 116 117 memory_copy(bp->sparse_elements, raw + p->sparse_elements_offset, 118 sizeof(*bp->sparse_elements) * bp->acquisition_count); 119 }break; 120 121 case ZBP_AcquisitionKind_UHERCULES:{ 122 ZBP_uHERCULESParameters *p = (ZBP_uHERCULESParameters *)(raw + acquisition_parameters_offset); 123 bp->transmit_receive_orientation = p->transmit_focus.transmit_receive_orientation; 124 bp->focal_vector.E[0] = p->transmit_focus.steering_angle; 125 bp->focal_vector.E[1] = p->transmit_focus.focal_depth; 126 127 bp->single_focus = 1; 128 bp->single_orientation = 1; 129 130 memory_copy(bp->sparse_elements, raw + p->sparse_elements_offset, 131 sizeof(*bp->sparse_elements) * bp->acquisition_count); 132 }break; 133 134 case ZBP_AcquisitionKind_RCA_TPW:{ 135 ZBP_TPWParameters *p = (ZBP_TPWParameters *)(raw + acquisition_parameters_offset); 136 137 memory_copy(bp->transmit_receive_orientations, raw + p->transmit_receive_orientations_offset, 138 sizeof(*bp->transmit_receive_orientations) * bp->acquisition_count); 139 memory_copy(bp->steering_angles, raw + p->tilting_angles_offset, 140 sizeof(*bp->steering_angles) * bp->acquisition_count); 141 142 for EachIndex(bp->acquisition_count, it) 143 bp->focal_depths[it] = inf32(); 144 }break; 145 146 case ZBP_AcquisitionKind_RCA_VLS:{ 147 ZBP_VLSParameters *p = (ZBP_VLSParameters *)(raw + acquisition_parameters_offset); 148 149 memory_copy(bp->transmit_receive_orientations, raw + p->transmit_receive_orientations_offset, 150 sizeof(*bp->transmit_receive_orientations) * bp->acquisition_count); 151 152 f32 *focal_depths = (f32 *)(raw + p->focal_depths_offset); 153 f32 *origin_offsets = (f32 *)(raw + p->origin_offsets_offset); 154 155 for EachIndex(bp->acquisition_count, it) { 156 f32 sign = Sign(focal_depths[it]); 157 f32 depth = focal_depths[it]; 158 f32 origin = origin_offsets[it]; 159 bp->steering_angles[it] = atan2_f32(origin, -depth) * 180.0f / PI; 160 bp->focal_depths[it] = sign * sqrt_f32(depth * depth + origin * origin); 161 } 162 }break; 163 164 InvalidDefaultCase; 165 } 166 } 167 168 function b32 169 beamformer_simple_parameters_from_zbp_file(Arena *arena, BeamformerSimpleParameters *bp, char *path, ZBP_Data *raw_data, u32 *data_frame_count) 170 { 171 str8 raw = os_read_entire_file(arena, path); 172 if (raw.length < (i64)sizeof(ZBP_BaseHeader) || ((ZBP_BaseHeader *)raw.data)->magic != ZBP_HeaderMagic) 173 return 0; 174 175 read_only BeamformerSamplingMode zbp_sampling_mode_to_beamformer[] = { 176 [ZBP_SamplingMode_Standard] = BeamformerSamplingMode_4X, 177 [ZBP_SamplingMode_Bandpass] = BeamformerSamplingMode_2X, 178 }; 179 180 switch (((ZBP_BaseHeader *)raw.data)->major) { 181 182 case 1:{ 183 ZBP_HeaderV1 *header = (ZBP_HeaderV1 *)raw.data; 184 185 bp->sample_count = header->sample_count; 186 bp->channel_count = header->channel_count; 187 bp->acquisition_count = header->receive_event_count; 188 189 bp->sampling_mode = BeamformerSamplingMode_4X; 190 bp->acquisition_kind = header->beamform_mode; 191 bp->decode_mode = header->decode_mode; 192 bp->sampling_frequency = header->sampling_frequency; 193 bp->demodulation_frequency = header->sampling_frequency / 4; 194 bp->speed_of_sound = header->speed_of_sound; 195 bp->time_offset = header->time_offset; 196 197 memory_copy(bp->channel_mapping, header->channel_mapping, sizeof(*bp->channel_mapping) * bp->channel_count); 198 memory_copy(bp->xdc_transform.E, header->transducer_transform_matrix, sizeof(bp->xdc_transform)); 199 memory_copy(bp->xdc_element_pitch.E, header->transducer_element_pitch, sizeof(bp->xdc_element_pitch)); 200 // NOTE(rnp): ignores emission count and ensemble count 201 memory_copy(bp->raw_data_dimensions.E, header->raw_data_dimension, sizeof(bp->raw_data_dimensions)); 202 if (data_frame_count) *data_frame_count = header->raw_data_dimension[2]; 203 //if (data_frame_count) *data_frame_count = header->raw_data_dimension[3]; 204 205 bp->data_kind = (BeamformerDataKind)ZBP_DataKind_Int16; 206 raw_data->kind = ZBP_DataKind_Int16; 207 raw_data->compression_kind = ZBP_DataCompressionKind_ZSTD; 208 209 read_only u8 transmit_mode_to_orientation[] = { 210 [0] = (ZBP_RCAOrientation_Rows << 4) | ZBP_RCAOrientation_Rows, 211 [1] = (ZBP_RCAOrientation_Rows << 4) | ZBP_RCAOrientation_Columns, 212 [2] = (ZBP_RCAOrientation_Columns << 4) | ZBP_RCAOrientation_Rows, 213 [3] = (ZBP_RCAOrientation_Columns << 4) | ZBP_RCAOrientation_Columns, 214 }; 215 if (header->transmit_mode >= countof(transmit_mode_to_orientation)) 216 return 0; 217 218 bp->transmit_receive_orientation = transmit_mode_to_orientation[header->transmit_mode]; 219 220 ZBP_AcquisitionKind acquisition_kind = header->beamform_mode; 221 if (acquisition_kind == ZBP_AcquisitionKind_FORCES || 222 acquisition_kind == ZBP_AcquisitionKind_HERCULES || 223 acquisition_kind == ZBP_AcquisitionKind_UFORCES || 224 acquisition_kind == ZBP_AcquisitionKind_UHERCULES) 225 { 226 bp->single_focus = 1; 227 bp->single_orientation = 1; 228 bp->focal_vector.E[0] = header->steering_angles[0]; 229 bp->focal_vector.E[1] = header->focal_depths[0]; 230 } 231 232 if (acquisition_kind == ZBP_AcquisitionKind_UFORCES || 233 acquisition_kind == ZBP_AcquisitionKind_UHERCULES) 234 { 235 memory_copy(bp->sparse_elements, header->sparse_elements, sizeof(*bp->sparse_elements) * bp->acquisition_count); 236 } 237 238 if (acquisition_kind == ZBP_AcquisitionKind_RCA_TPW || 239 acquisition_kind == ZBP_AcquisitionKind_RCA_VLS) 240 { 241 memory_copy(bp->focal_depths, header->focal_depths, sizeof(*bp->focal_depths) * bp->acquisition_count); 242 memory_copy(bp->steering_angles, header->steering_angles, sizeof(*bp->steering_angles) * bp->acquisition_count); 243 for EachIndex(bp->acquisition_count, it) 244 bp->transmit_receive_orientations[it] = bp->transmit_receive_orientation; 245 } 246 247 bp->emission_parameters.kind = BeamformerEmissionKind_Sine; 248 bp->emission_parameters.sine.cycles = 2; 249 bp->emission_parameters.sine.frequency = bp->demodulation_frequency; 250 }break; 251 252 case 2:{ 253 ZBP_HeaderV2 *header = (ZBP_HeaderV2 *)raw.data; 254 bp->sample_count = header->sample_count; 255 bp->channel_count = header->channel_count; 256 bp->acquisition_count = header->receive_event_count; 257 bp->sampling_mode = zbp_sampling_mode_to_beamformer[header->sampling_mode]; 258 bp->acquisition_kind = (BeamformerAcquisitionKind)header->acquisition_mode; 259 bp->decode_mode = (BeamformerDecodeMode)header->decode_mode; 260 bp->sampling_frequency = header->sampling_frequency; 261 bp->demodulation_frequency = header->demodulation_frequency; 262 bp->speed_of_sound = header->speed_of_sound; 263 bp->time_offset = header->time_offset; 264 bp->contrast_mode = (BeamformerContrastMode)header->contrast_mode; 265 bp->data_kind = (BeamformerDataKind)header->raw_data_kind; 266 267 beamformer_simple_parameters_equip_channel_mapping(bp, raw.data, bp->channel_count, header->channel_mapping_offset); 268 beamformer_simple_parameters_equip_emission(bp, raw.data, header->emission_descriptors_offset); 269 beamformer_simple_parameters_equip_acquisition(bp, raw.data, bp->acquisition_kind, header->acquisition_parameters_offset); 270 271 memory_copy(bp->xdc_transform.E, header->transducer_transform_matrix, sizeof(bp->xdc_transform)); 272 memory_copy(bp->xdc_element_pitch.E, header->transducer_element_pitch, sizeof(bp->xdc_element_pitch)); 273 // NOTE(rnp): ignores group count and ensemble count 274 memory_copy(bp->raw_data_dimensions.E, header->raw_data_dimension, sizeof(bp->raw_data_dimensions)); 275 if (data_frame_count) *data_frame_count = header->raw_data_dimension[2]; 276 //if (data_frame_count) *data_frame_count = header->raw_data_dimension[3]; 277 278 raw_data->kind = header->raw_data_kind; 279 raw_data->compression_kind = header->raw_data_compression_kind; 280 281 if (header->raw_data_offset != -1) { 282 raw_data->bytes.data = raw.data + header->raw_data_offset; 283 if (raw_data->compression_kind == ZBP_DataCompressionKind_ZSTD) { 284 // NOTE(rnp): limitation in the header format 285 raw_data->bytes.length = raw.length - header->raw_data_offset; 286 } else { 287 raw_data->bytes.length = header->raw_data_dimension[0] * header->raw_data_dimension[1] * 288 header->raw_data_dimension[2] * header->raw_data_dimension[3]; 289 raw_data->bytes.length *= beamformer_data_kind_byte_size[header->raw_data_kind]; 290 } 291 } 292 }break; 293 294 case 3:{ 295 ZBP_HeaderV3 *header = (ZBP_HeaderV3 *)raw.data; 296 bp->sample_count = header->sample_count; 297 bp->channel_count = header->channel_count; 298 bp->acquisition_count = header->receive_event_count; 299 bp->sampling_mode = zbp_sampling_mode_to_beamformer[header->sampling_mode]; 300 bp->acquisition_kind = (BeamformerAcquisitionKind)header->acquisition_mode; 301 bp->decode_mode = (BeamformerDecodeMode)header->decode_mode; 302 bp->sampling_frequency = header->sampling_frequency; 303 bp->speed_of_sound = header->speed_of_sound; 304 bp->contrast_mode = (BeamformerContrastMode)header->contrast_mode; 305 bp->data_kind = (BeamformerDataKind)header->raw_data_kind; 306 307 memory_copy(bp->xdc_element_pitch.E, header->transducer_element_pitch, sizeof(bp->xdc_element_pitch)); 308 memory_copy(bp->raw_data_dimensions.E, header->raw_data_dimension, sizeof(bp->raw_data_dimensions)); 309 if (data_frame_count) *data_frame_count = header->raw_data_dimension[2]; 310 311 beamformer_simple_parameters_equip_channel_mapping(bp, raw.data, bp->channel_count, header->channel_mapping_offset); 312 beamformer_simple_parameters_equip_emission(bp, raw.data, header->emission_descriptors_offset); 313 beamformer_simple_parameters_equip_acquisition(bp, raw.data, bp->acquisition_kind, header->acquisition_parameters_offset); 314 315 // TODO(rnp): for now we only consider first data frame 316 memory_copy(&bp->time_offset, raw.data + header->time_delays_offset, sizeof(f32)); 317 memory_copy(&bp->demodulation_frequency , raw.data + header->demodulation_frequencies_offset, sizeof(f32)); 318 319 // TODO(rnp): for now we only consider first tile 320 memory_copy(&bp->xdc_transform, raw.data + header->transducer_transforms_offset, sizeof(m4)); 321 322 raw_data->kind = header->raw_data_kind; 323 raw_data->compression_kind = header->raw_data_compression_kind; 324 if (header->raw_data_offset != -1) { 325 raw_data->bytes.length = header->raw_data_size; 326 raw_data->bytes.data = raw.data + header->raw_data_offset; 327 } 328 }break; 329 330 default:{return 0;}break; 331 } 332 333 return 1; 334 } 335 336 function void 337 stream_ensure_termination(Stream *s, u8 byte) 338 { 339 b32 found = 0; 340 if (!s->errors && s->widx > 0) 341 found = s->data[s->widx - 1] == byte; 342 if (!found) { 343 s->errors |= s->cap - 1 < s->widx; 344 if (!s->errors) 345 s->data[s->widx++] = byte; 346 } 347 } 348 349 function str8 350 zstd_decompress_data(Arena *arena, str8 raw) 351 { 352 u64 requested_size = ZSTD_getFrameContentSize(raw.data, (u64)raw.length); 353 void *out = push_array_no_zero(arena, u8, requested_size); 354 str8 result = {.data = out}; 355 u64 decompressed = ZSTD_decompress(out, requested_size, raw.data, (u64)raw.length); 356 if (decompressed == requested_size) result.length = requested_size; 357 return result; 358 } 359 360 function void * 361 zbp_data_pointer(Arena *arena, ZBP_Data *raw, Stream path, u32 frame_number) 362 { 363 void *result = 0; 364 if (raw->bytes.length == 0) { 365 // NOTE(rnp): strip ".bp" 366 stream_reset(&path, path.widx - 3); 367 368 b32 compressed = raw->compression_kind == ZBP_DataCompressionKind_ZSTD; 369 stream_append_byte(&path, '_'); 370 stream_append_u64_width(&path, frame_number, 2); 371 stream_append_str8(&path, compressed ? str8(".zst") : str8(".bin")); 372 stream_ensure_termination(&path, 0); 373 str8 compressed_data = os_read_entire_file(arena, (char *)path.data); 374 375 str8 bytes = compressed_data; 376 if (compressed) { 377 bytes = zstd_decompress_data(arena, compressed_data); 378 if (!bytes.length) 379 die("failed to decompress data: %s\n", path.data); 380 } 381 result = bytes.data; 382 } else { 383 if (raw->compression_kind == ZBP_DataCompressionKind_ZSTD) { 384 str8 bytes = zstd_decompress_data(arena, raw->bytes); 385 result = bytes.data; 386 } else { 387 result = raw->bytes.data; 388 } 389 } 390 return result; 391 }