ogl_beamforming

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

util.c (28895B)


      1 /* See LICENSE for license details. */
      2 #if   COMPILER_CLANG
      3   #pragma GCC diagnostic ignored "-Winitializer-overrides"
      4 #elif COMPILER_GCC
      5   #pragma GCC diagnostic ignored "-Woverride-init"
      6 #endif
      7 
      8 #define zero_struct(s) memory_clear((s), 0, sizeof(*(s)))
      9 function void *
     10 memory_clear(void *restrict p_, u8 c, u64 size)
     11 {
     12 	u8 *p = p_;
     13 	while (size > 0) p[--size] = c;
     14 	return p;
     15 }
     16 
     17 function b32
     18 memory_equal(void *restrict left, void *restrict right, u64 n)
     19 {
     20 	u8 *a = left, *b = right;
     21 	b32 result = 1;
     22 	for (; result && n; n--)
     23 		result &= *a++ == *b++;
     24 	return result;
     25 }
     26 
     27 function void
     28 memory_copy(void *restrict dest, void *restrict src, u64 n)
     29 {
     30 	u8 *s = src, *d = dest;
     31 	#ifdef __AVX512BW__
     32 	{
     33 		for (; n >= 64; n -= 64, s += 64, d += 64)
     34 			_mm512_storeu_epi8(d, _mm512_loadu_epi8(s));
     35 		__mmask64 k = _cvtu64_mask64(_bzhi_u64(-1ULL, n));
     36 		_mm512_mask_storeu_epi8(d, k, _mm512_maskz_loadu_epi8(k, s));
     37 	}
     38 	#else
     39 		for (; n; n--) *d++ = *s++;
     40 	#endif
     41 }
     42 
     43 /* IMPORTANT: this function may fault if dest, src, and n are not multiples of 64 */
     44 function void
     45 memory_copy_non_temporal(void *restrict dest, void *restrict src, u64 n)
     46 {
     47 	assume(((u64)dest & 63) == 0);
     48 	assume(((u64)src  & 63) == 0);
     49 	assume(((u64)n    & 63) == 0);
     50 	u8 *s = src, *d = dest;
     51 
     52 	#if defined(__AVX512BW__)
     53 	{
     54 		for (; n >= 64; n -= 64, s += 64, d += 64)
     55 			_mm512_stream_si512((__m512i *)d, _mm512_stream_load_si512((__m512i *)s));
     56 	}
     57 	#elif defined(__AVX2__)
     58 	{
     59 		for (; n >= 32; n -= 32, s += 32, d += 32)
     60 			_mm256_stream_si256((__m256i *)d, _mm256_stream_load_si256((__m256i *)s));
     61 	}
     62 	#elif ARCH_ARM64 && !COMPILER_MSVC
     63 	{
     64 		asm volatile (
     65 			"cbz  %2, 2f\n"
     66 			"1: ldnp q0, q1, [%1]\n"
     67 			"subs %2, %2, #32\n"
     68 			"add  %1, %1, #32\n"
     69 			"stnp q0, q1, [%0]\n"
     70 			"add  %0, %0, #32\n"
     71 			"b.ne 1b\n"
     72 			"2:"
     73 			:  "+r"(d), "+r"(s), "+r"(n)
     74 			:: "memory", "v0", "v1"
     75 		);
     76 	}
     77 	#else
     78 		memory_copy(d, s, n);
     79 	#endif
     80 }
     81 
     82 function void
     83 memory_move(void *dest, void *src, u64 n)
     84 {
     85 	u8 *d = dest, *s = src;
     86 	if (d < s) memory_copy(d, s, n);
     87 	else            while (n) { n--; d[n] = s[n]; }
     88 }
     89 
     90 function void *
     91 memory_scan_backwards(void *memory, u8 byte, i64 n)
     92 {
     93 	void *result = 0;
     94 	u8   *s      = memory;
     95 	while (n > 0) if (s[--n] == byte) { result = s + n; break; }
     96 	return result;
     97 }
     98 
     99 /* NOTE(rnp): from Hacker's Delight */
    100 function force_inline u64
    101 round_down_power_of_two(u64 a)
    102 {
    103 	u64 result = 0x8000000000000000ULL >> clz_u64(a);
    104 	return result;
    105 }
    106 
    107 function force_inline u64
    108 round_up_power_of_two(u64 a)
    109 {
    110 	u64 result = 0x8000000000000000ULL >> (clz_u64(a - 1) - 1);
    111 	return result;
    112 }
    113 
    114 function force_inline i64
    115 round_up_to(i64 value, i64 multiple)
    116 {
    117 	i64 result = value;
    118 	if (value % multiple != 0)
    119 		result += multiple - value % multiple;
    120 	return result;
    121 }
    122 
    123 function BumpArena
    124 bump_arena_from_buffer(void *base, i64 size)
    125 {
    126 	BumpArena result = {.start = base};
    127 	result.end = result.start + size;
    128 	return result;
    129 }
    130 
    131 function void *
    132 bump_arena_aligned_start(BumpArena a, u64 alignment)
    133 {
    134 	u64 padding = -(u64)a.start & (alignment - 1);
    135 	u8 *result = a.start + padding;
    136 	return result;
    137 }
    138 
    139 typedef enum {
    140 	ArenaAllocateFlags_NoZero = 1 << 0,
    141 } ArenaAllocateFlags;
    142 
    143 typedef struct {
    144 	i64 size;
    145 	u64 align;
    146 	i64 count;
    147 	ArenaAllocateFlags flags;
    148 } ArenaAllocateInfo;
    149 
    150 #define bump_arena_alloc(a, ...)         bump_arena_alloc_(a, (ArenaAllocateInfo){.align = 8, .count = 1, ##__VA_ARGS__})
    151 #define gpu_arena_alloc(a, ...)          bump_arena_alloc_(a, (ArenaAllocateInfo){.flags = ArenaAllocateFlags_NoZero, .align = 8, .count = 1, ##__VA_ARGS__})
    152 #define bump_push_array(a, t, n)         (t *)bump_arena_alloc(a, .size = sizeof(t), .align = alignof(t), .count = n)
    153 #define bump_push_array_no_zero(a, t, n) (t *)bump_arena_alloc(a, .size = sizeof(t), .align = alignof(t), .count = n, .flags = ArenaAllocateFlags_NoZero)
    154 #define bump_push_struct(a, t)           bump_push_array(a, t, 1)
    155 #define bump_push_struct_no_zero(a, t)   bump_push_array_no_zero(a, t, 1)
    156 
    157 function void *
    158 bump_arena_alloc_(BumpArena *a, ArenaAllocateInfo info)
    159 {
    160 	void *result = 0;
    161 	if (a->start) {
    162 		u8 *start = bump_arena_aligned_start(*a, info.align);
    163 		i64 available = a->end - start;
    164 		assert((available >= 0 && info.count <= available / info.size));
    165 		a->start = start + info.count * info.size;
    166 		result = start;
    167 		if ((info.flags & ArenaAllocateFlags_NoZero) == 0)
    168 			result = memory_clear(start, 0, info.count * info.size);
    169 	}
    170 	return result;
    171 }
    172 
    173 function u8 *
    174 arena_commit(Arena *a, i64 size)
    175 {
    176 	Arena *current = a->current;
    177 	assert(current->committed - current->position >= (u64)size);
    178 	u8 *result = (u8 *)current + current->position;
    179 	current->position += size;
    180 	return result;
    181 }
    182 
    183 #define arena_alloc(a, ...)         arena_alloc_(a, (ArenaAllocateInfo){.align = 8, .count = 1, ##__VA_ARGS__})
    184 #define push_array(a, t, n)         (t *)arena_alloc(a, .size = sizeof(t), .align = alignof(t), .count = n)
    185 #define push_array_no_zero(a, t, n) (t *)arena_alloc(a, .size = sizeof(t), .align = alignof(t), .count = n, .flags = ArenaAllocateFlags_NoZero)
    186 #define push_struct(a, t)           push_array(a, t, 1)
    187 #define push_struct_no_zero(a, t)   push_array_no_zero(a, t, 1)
    188 
    189 #define arena_create(...) arena_create_((ArenaParameters){\
    190 	.reserve_size = MB(64),\
    191 	.commit_size  = KB(64),\
    192 	.flags        = 0,\
    193 	.allocation_site_file = __FILE__,\
    194 	.allocation_site_line = __LINE__,\
    195 	__VA_ARGS__})
    196 
    197 function Arena *
    198 arena_create_(ArenaParameters ap)
    199 {
    200 	void *base = ap.optional_backing_store;
    201 	if (base == 0) {
    202 		ap.commit_size  = round_up_to(ap.commit_size,  os_system_info()->page_size);
    203 		ap.reserve_size = round_up_to(ap.reserve_size, os_system_info()->page_size);
    204 
    205 		base = os_memory_reserve(ap.reserve_size);
    206 		os_memory_commit(base, ap.commit_size);
    207 	}
    208 
    209 	Arena *result     = base;
    210 	result->current   = result;
    211 	result->position  = sizeof(*result);
    212 	result->reserved  = ap.reserve_size;
    213 	result->committed = ap.commit_size;
    214 	result->flags     = ap.flags;
    215 
    216 	result->reserve_size = ap.reserve_size;
    217 	result->commit_size  = ap.commit_size;
    218 
    219 	result->name                 = ap.name;
    220 	result->allocation_site_file = ap.allocation_site_file;
    221 	result->allocation_site_line = ap.allocation_site_line;
    222 
    223 	return result;
    224 }
    225 
    226 function void
    227 arena_destroy(Arena *arena)
    228 {
    229 	for (Arena *a = arena->current, *prev = 0; a; a = prev) {
    230 		prev = a->prev;
    231 		os_memory_release(a, a->reserved);
    232 	}
    233 }
    234 
    235 function void
    236 arena_seal(Arena *arena)
    237 {
    238 	assert(arena == arena->current);
    239 	u64 position = round_up_to(arena->position, os_system_info()->page_size);
    240 	if (arena->committed > position) {
    241 		os_memory_uncommit((u8 *)arena + position, arena->committed - position);
    242 		arena->committed = position;
    243 	}
    244 	if (arena->reserved > arena->committed) {
    245 		os_memory_release((u8 *)arena + arena->committed, arena->reserved - arena->committed);
    246 		arena->reserved = arena->committed;
    247 	}
    248 	os_memory_seal(arena, arena->reserved);
    249 }
    250 
    251 #define arena_alloc(a, ...)         arena_alloc_(a, (ArenaAllocateInfo){.align = 8, .count = 1, ##__VA_ARGS__})
    252 #define push_array(a, t, n)         (t *)arena_alloc(a, .size = sizeof(t), .align = alignof(t), .count = n)
    253 #define push_array_no_zero(a, t, n) (t *)arena_alloc(a, .size = sizeof(t), .align = alignof(t), .count = n, .flags = ArenaAllocateFlags_NoZero)
    254 #define push_struct(a, t)           push_array(a, t, 1)
    255 #define push_struct_no_zero(a, t)   push_array_no_zero(a, t, 1)
    256 
    257 function void *
    258 arena_alloc_(Arena *arena, ArenaAllocateInfo info)
    259 {
    260 	Arena *current = arena->current;
    261 	u64 size          = info.count * info.size;
    262 	u64 pre_position  = AlignUpPowerOfTwo(current->position, info.align);
    263 	u64 post_position = pre_position + size;
    264 	u64 zero_size     = Min(current->committed, post_position) - pre_position;
    265 
    266 	if (current->reserved < post_position && (current->flags & ArenaFlag_NoChain) == 0) {
    267 		u64 reserve_size = current->reserve_size;
    268 		u64 commit_size  = current->commit_size;
    269 		if (size + AlignUpPowerOfTwo(sizeof(*arena), info.align) > reserve_size) {
    270 			reserve_size = size + AlignUpPowerOfTwo(sizeof(*arena), info.align);
    271 			commit_size  = size + AlignUpPowerOfTwo(sizeof(*arena), info.align);
    272 		}
    273 		Arena *new_arena = arena_create(.reserve_size         = reserve_size,
    274 		                                .commit_size          = commit_size,
    275 		                                .flags                = current->flags,
    276 		                                .allocation_site_file = current->allocation_site_file,
    277 		                                .allocation_site_line = current->allocation_site_line,
    278 		                                .name                 = current->name);
    279 		zero_size = 0;
    280 
    281 		new_arena->base_position = current->base_position + current->reserved;
    282 		SLLStackPush(arena->current, new_arena, prev);
    283 		current = new_arena;
    284 		pre_position  = AlignUpPowerOfTwo(current->position, info.align);
    285 		post_position = pre_position + size;
    286 	}
    287 
    288 	if (current->committed < post_position) {
    289 		u64 commit_post = post_position + current->commit_size - 1;
    290 		commit_post -= commit_post % current->commit_size;
    291 		commit_post  = Min(commit_post, current->reserved);
    292 		os_memory_commit((u8 *)current + current->committed, commit_post - current->committed);
    293 		current->committed = commit_post;
    294 	}
    295 
    296 	void *result = 0;
    297 	if (current->committed >= post_position) {
    298 		result = (u8 *)current + pre_position;
    299 		current->position = post_position;
    300 		if ((info.flags & ArenaAllocateFlags_NoZero) == 0)
    301 			result = memory_clear(result, 0, zero_size);
    302 	}
    303 
    304 	assert(result);
    305 
    306 	return result;
    307 }
    308 
    309 function u64
    310 arena_position(Arena *arena)
    311 {
    312 	Arena *current = arena->current;
    313 	u64 result = current->base_position + current->position;
    314 	return result;
    315 }
    316 
    317 function void
    318 arena_pop_to(Arena *arena, u64 position)
    319 {
    320 	position = Max(position, sizeof(*arena));
    321 	Arena *current = arena->current;
    322 	for (Arena *prev = 0; current->base_position >= position; current = prev) {
    323 		prev = current->prev;
    324 		os_memory_release(current, current->reserved);
    325 	}
    326 	arena->current = current;
    327 	u64 new_position = position - current->base_position;
    328 	assert(new_position <= current->position);
    329 	current->position = new_position;
    330 }
    331 
    332 function void
    333 arena_pop(Arena *arena, u64 size)
    334 {
    335 	u64 old_position = arena_position(arena);
    336 	u64 new_position = old_position;
    337 	if (size < old_position)
    338 		new_position = old_position - size;
    339 	arena_pop_to(arena, new_position);
    340 }
    341 
    342 function void
    343 arena_clear(Arena *arena)
    344 {
    345 	arena_pop_to(arena, 0);
    346 }
    347 
    348 function void
    349 arena_pre_align(Arena *arena, u64 align)
    350 {
    351 	assert(IsPowerOfTwo(align));
    352 	Arena *current = arena->current;
    353 	u8 *start = (u8 *)current + current->position;
    354 	u8 *desired_start = (u8 *)AlignUpPowerOfTwo((u64)start, align);
    355 	current->position += (u64)(desired_start - start);
    356 }
    357 
    358 function Temp
    359 temp_begin(Arena *arena)
    360 {
    361 	Temp result = {.arena = arena, .position = arena_position(arena)};
    362 	return result;
    363 }
    364 
    365 function void
    366 temp_end(Temp t)
    367 {
    368 	arena_pop_to(t.arena, t.position);
    369 }
    370 
    371 enum { DA_INITIAL_CAP = 16 };
    372 
    373 #define da_index(it, s) ((it) - (s)->data)
    374 #define da_reserve(a, s, n) \
    375   (s)->data = da_reserve_((a), (s)->data, &(s)->capacity, (s)->count + n, \
    376                           _Alignof(typeof(*(s)->data)), sizeof(*(s)->data))
    377 
    378 #define da_append_count(a, s, items, item_count) do { \
    379 	da_reserve((a), (s), (item_count)); \
    380 	memory_copy((s)->data + (s)->count, (items), sizeof(*(items)) * (u64)(item_count)); \
    381 	(s)->count += (item_count); \
    382 } while (0)
    383 
    384 #define da_push(a, s) \
    385   ((typeof((s)->data))memory_clear((s)->count == (s)->capacity  \
    386     ? da_reserve(a, s, 1),      \
    387       (s)->data + (s)->count++  \
    388     : (s)->data + (s)->count++, 0, sizeof(*(s)->data)))
    389 
    390 
    391 /* NOTE(rnp): handles both 0 initialized DAs and DAs that need to be moved (they started
    392  * on the stack or someone allocated something in the middle of the arena during usage) */
    393 function void *
    394 da_reserve_(Arena *a, void *data, da_count *capacity, da_count needed, u64 align, i64 size)
    395 {
    396 	da_count cap = *capacity;
    397 	if (!cap) cap = DA_INITIAL_CAP;
    398 	while (cap < needed) cap *= 2;
    399 
    400 	Arena *current = a->current;
    401 	u64 needed_size = cap * size;
    402 	u64 old_size    = *capacity * size;
    403 	b32 can_extend  = data && (u8 *)current + current->position == (u8 *)data + old_size &&
    404 	                  (current->reserved - current->position) >= (needed_size - old_size);
    405 	b32 needs_copy  = data && !can_extend;
    406 
    407 	u64 alloc_cap = cap;
    408 	if (can_extend) alloc_cap -= *capacity;
    409 
    410 	void *new = arena_alloc(a, .size = size, .align = align, .count = alloc_cap);
    411 
    412 	if (needs_copy)
    413 		memory_copy(new, data, (u64)(*capacity * size));
    414 
    415 	if (!can_extend)
    416 		data = new;
    417 
    418 	*capacity = cap;
    419 
    420 	return data;
    421 }
    422 
    423 function u32
    424 utf8_encode(u8 *out, u32 cp)
    425 {
    426 	u32 result = 1;
    427 	if (cp <= 0x7F) {
    428 		out[0] = cp & 0x7F;
    429 	} else if (cp <= 0x7FF) {
    430 		result = 2;
    431 		out[0] = ((cp >>  6) & 0x1F) | 0xC0;
    432 		out[1] = ((cp >>  0) & 0x3F) | 0x80;
    433 	} else if (cp <= 0xFFFF) {
    434 		result = 3;
    435 		out[0] = ((cp >> 12) & 0x0F) | 0xE0;
    436 		out[1] = ((cp >>  6) & 0x3F) | 0x80;
    437 		out[2] = ((cp >>  0) & 0x3F) | 0x80;
    438 	} else if (cp <= 0x10FFFF) {
    439 		result = 4;
    440 		out[0] = ((cp >> 18) & 0x07) | 0xF0;
    441 		out[1] = ((cp >> 12) & 0x3F) | 0x80;
    442 		out[2] = ((cp >>  6) & 0x3F) | 0x80;
    443 		out[3] = ((cp >>  0) & 0x3F) | 0x80;
    444 	} else {
    445 		out[0] = '?';
    446 	}
    447 	return result;
    448 }
    449 
    450 function UnicodeDecode
    451 utf16_decode(u16 *data, i64 length)
    452 {
    453 	UnicodeDecode result = {.cp = U32_MAX};
    454 	if (length) {
    455 		result.consumed = 1;
    456 		result.cp = data[0];
    457 		if (length > 1 && Between(data[0], 0xD800u, 0xDBFFu)
    458 		               && Between(data[1], 0xDC00u, 0xDFFFu))
    459 		{
    460 			result.consumed = 2;
    461 			result.cp = ((data[0] - 0xD800u) << 10u) | ((data[1] - 0xDC00u) + 0x10000u);
    462 		}
    463 	}
    464 	return result;
    465 }
    466 
    467 function u32
    468 utf16_encode(u16 *out, u32 cp)
    469 {
    470 	u32 result = 1;
    471 	if (cp == U32_MAX) {
    472 		out[0] = '?';
    473 	} else if (cp < 0x10000u) {
    474 		out[0] = (u16)cp;
    475 	} else {
    476 		u32 value = cp - 0x10000u;
    477 		out[0] = (u16)(0xD800u + (value >> 10u));
    478 		out[1] = (u16)(0xDC00u + (value & 0x3FFu));
    479 		result = 2;
    480 	}
    481 	return result;
    482 }
    483 
    484 function Stream
    485 stream_from_buffer(u8 *buffer, u32 capacity)
    486 {
    487 	Stream result = {.data = buffer, .cap = (i32)capacity};
    488 	return result;
    489 }
    490 
    491 function Stream
    492 stream_alloc(Arena *a, i32 cap)
    493 {
    494 	Stream result = stream_from_buffer(push_array_no_zero(a, u8, cap), (u32)cap);
    495 	return result;
    496 }
    497 
    498 function str8
    499 stream_to_str8(Stream *s)
    500 {
    501 	str8 result = str8("");
    502 	if (!s->errors) result = (str8){.length = s->widx, .data = s->data};
    503 	return result;
    504 }
    505 
    506 function void
    507 stream_reset(Stream *s, i32 index)
    508 {
    509 	s->errors = s->cap <= index;
    510 	if (!s->errors)
    511 		s->widx = index;
    512 }
    513 
    514 function void
    515 stream_append(Stream *s, void *data, i64 count)
    516 {
    517 	s->errors |= (s->cap - s->widx) < count;
    518 	if (!s->errors) {
    519 		memory_copy(s->data + s->widx, data, (u64)count);
    520 		s->widx += (i32)count;
    521 	}
    522 }
    523 
    524 function void
    525 stream_append_codepoint(Stream *s, u32 codepoint)
    526 {
    527 	u8 buffer[4];
    528 	stream_append(s, buffer, utf8_encode(buffer, codepoint));
    529 }
    530 
    531 // TODO(rnp): replace with handwritten version
    532 #include <stdarg.h>
    533 #include <stdio.h>
    534 function void
    535 stream_appendfv(Stream *s, const char *format, va_list args)
    536 {
    537 	i32 written = vsnprintf((char *)s->data + s->widx, s->cap - s->widx, format, args);
    538 	s->errors |= written > (s->cap - s->widx);
    539 	if (!s->errors) s->widx += written;
    540 }
    541 
    542 function print_format(2, 3) void
    543 stream_appendf(Stream *s, const char *format, ...)
    544 {
    545 	va_list args;
    546 	va_start(args, format);
    547 	stream_appendfv(s, format, args);
    548 	va_end(args);
    549 }
    550 
    551 function void
    552 stream_append_byte(Stream *s, u8 b)
    553 {
    554 	stream_append(s, &b, 1);
    555 }
    556 
    557 function void
    558 stream_pad(Stream *s, u8 b, i32 n)
    559 {
    560 	while (n > 0) stream_append_byte(s, b), n--;
    561 }
    562 
    563 function void
    564 stream_append_str8(Stream *s, str8 str)
    565 {
    566 	stream_append(s, str.data, str.length);
    567 }
    568 
    569 #define stream_append_str8s(s, ...) stream_append_str8s_(s, arg_list(str8, ##__VA_ARGS__))
    570 function void
    571 stream_append_str8s_(Stream *s, str8 *strs, i64 count)
    572 {
    573 	for (i64 i = 0; i < count; i++)
    574 		stream_append(s, strs[i].data, strs[i].length);
    575 }
    576 
    577 function void
    578 stream_append_u64_width(Stream *s, u64 n, u64 min_width)
    579 {
    580 	u8 tmp[64];
    581 	u8 *end = tmp + sizeof(tmp);
    582 	u8 *beg = end;
    583 	min_width = Min(sizeof(tmp), min_width);
    584 
    585 	do { *--beg = (u8)('0' + (n % 10)); } while (n /= 10);
    586 	while (end - beg > 0 && (u64)(end - beg) < min_width)
    587 		*--beg = '0';
    588 
    589 	stream_append(s, beg, end - beg);
    590 }
    591 
    592 function void
    593 stream_append_u64(Stream *s, u64 n)
    594 {
    595 	stream_append_u64_width(s, n, 0);
    596 }
    597 
    598 function void
    599 stream_append_hex_u64_width(Stream *s, u64 n, i64 width)
    600 {
    601 	assert(width <= 16);
    602 	if (!s->errors) {
    603 		u8  buf[16];
    604 		u8 *end = buf + sizeof(buf);
    605 		u8 *beg = end;
    606 		while (n) {
    607 			*--beg = (u8)"0123456789abcdef"[n & 0x0F];
    608 			n >>= 4;
    609 		}
    610 		while (end - beg < width)
    611 			*--beg = '0';
    612 		stream_append(s, beg, end - beg);
    613 	}
    614 }
    615 
    616 function void
    617 stream_append_hex_u64(Stream *s, u64 n)
    618 {
    619 	stream_append_hex_u64_width(s, n, 2);
    620 }
    621 
    622 function void
    623 stream_append_i64(Stream *s, i64 n)
    624 {
    625 	if (n < 0) {
    626 		stream_append_byte(s, '-');
    627 		n *= -1;
    628 	}
    629 	stream_append_u64(s, (u64)n);
    630 }
    631 
    632 function void
    633 stream_append_f64(Stream *s, f64 f, u64 prec)
    634 {
    635 	if (f < 0) {
    636 		stream_append_byte(s, '-');
    637 		f *= -1;
    638 	}
    639 
    640 	/* NOTE: round last digit */
    641 	f += 0.5f / (f64)prec;
    642 
    643 	if (f >= (f64)(-1UL >> 1)) {
    644 		stream_append_str8(s, str8("inf"));
    645 	} else {
    646 		u64 integral = (u64)f;
    647 		u64 fraction = (u64)((f - (f64)integral) * (f64)prec);
    648 		stream_append_u64(s, integral);
    649 		stream_append_byte(s, '.');
    650 		for (u64 i = prec / 10; i > 1; i /= 10) {
    651 			if (i > fraction)
    652 				stream_append_byte(s, '0');
    653 		}
    654 		stream_append_u64(s, fraction);
    655 	}
    656 }
    657 
    658 function void
    659 stream_append_f64_e(Stream *s, f64 f)
    660 {
    661 	/* TODO: there should be a better way of doing this */
    662 	#if 0
    663 	/* NOTE: we ignore subnormal numbers for now */
    664 	union { f64 f; u64 u; } u = {.f = f};
    665 	i32 exponent = ((u.u >> 52) & 0x7ff) - 1023;
    666 	f32 log_10_of_2 = 0.301f;
    667 	i32 scale       = (exponent * log_10_of_2);
    668 	/* NOTE: normalize f */
    669 	for (i32 i = ABS(scale); i > 0; i--)
    670 		f *= (scale > 0)? 0.1f : 10.0f;
    671 	#else
    672 	f32 sign = Sign(f);
    673 	f *= sign;
    674 	i32 scale = 0;
    675 	if (f != 0) {
    676 		while (f > 1) {
    677 			f *= 0.1f;
    678 			scale++;
    679 		}
    680 		while (f < 1) {
    681 			f *= 10.0f;
    682 			scale--;
    683 		}
    684 	}
    685 	#endif
    686 
    687 	u32 prec = 100;
    688 	stream_append_f64(s, sign * f, prec);
    689 	stream_append_byte(s, 'e');
    690 	stream_append_byte(s, scale >= 0? '+' : '-');
    691 	for (u32 i = prec / 10; i > 1; i /= 10)
    692 		stream_append_byte(s, '0');
    693 	stream_append_u64(s, (u64)Abs(scale));
    694 }
    695 
    696 function void
    697 stream_append_struct_member(Stream *s, MetaStructMember *m, void *struct_base)
    698 {
    699 	switch (m->type_id) {
    700 	InvalidDefaultCase;
    701 	case MetaKind_F32:{
    702 		f32 value;
    703 		memory_copy(&value, ((u8 *)struct_base + m->offset), sizeof(value));
    704 		stream_append_f64_e(s, value);
    705 	}break;
    706 	case MetaKind_B32:{
    707 		b32 value;
    708 		memory_copy(&value, ((u8 *)struct_base + m->offset), sizeof(value));
    709 		stream_append_str8(s, value ? str8("True") : str8("False"));
    710 	}break;
    711 	case MetaKind_U32:{
    712 		u32 value;
    713 		memory_copy(&value, ((u8 *)struct_base + m->offset), sizeof(value));
    714 		stream_append_u64(s, value);
    715 	}break;
    716 	case MetaKind_U64:{
    717 		u64 value;
    718 		memory_copy(&value, ((u8 *)struct_base + m->offset), sizeof(value));
    719 		stream_append_str8(s, str8("0x"));
    720 		stream_append_hex_u64(s, value);
    721 	}break;
    722 	case MetaKind_S32:{
    723 		i32 value;
    724 		memory_copy(&value, ((u8 *)struct_base + m->offset), sizeof(value));
    725 		stream_append_i64(s, value);
    726 	}break;
    727 	}
    728 }
    729 
    730 function Stream
    731 arena_stream(Arena *a)
    732 {
    733 	Arena *current = a->current;
    734 	Stream result = {0};
    735 	result.data   = (u8 *)current + current->position;
    736 	result.cap    = (i32)(current->committed - current->position);
    737 
    738 	/* TODO(rnp): no idea what to do here if we want to maintain the ergonomics */
    739 	asan_unpoison_region(result.data, result.cap);
    740 
    741 	return result;
    742 }
    743 
    744 function str8
    745 arena_stream_commit(Arena *a, Stream *s)
    746 {
    747 	Arena *current = a->current;
    748 	assert(s->data == (u8 *)current + current->position);
    749 	str8 result = stream_to_str8(s);
    750 	arena_commit(a, result.length);
    751 	return result;
    752 }
    753 
    754 function str8
    755 arena_stream_commit_zero(Arena *a, Stream *s)
    756 {
    757 	b32 error = s->errors || s->widx == s->cap;
    758 	if (!error)
    759 		s->data[s->widx] = 0;
    760 	str8 result = stream_to_str8(s);
    761 	arena_commit(a, result.length + 1);
    762 	return result;
    763 }
    764 
    765 function str8
    766 arena_stream_commit_and_reset(Arena *arena, Stream *s)
    767 {
    768 	str8 result = arena_stream_commit_zero(arena, s);
    769 	*s = arena_stream(arena);
    770 	return result;
    771 }
    772 
    773 #if !defined(XXH_IMPLEMENTATION)
    774 # define XXH_INLINE_ALL
    775 # define XXH_IMPLEMENTATION
    776 # define XXH_STATIC_LINKING_ONLY
    777 # include "external/xxhash.h"
    778 #endif
    779 
    780 function u128
    781 u128_hash_from_data(void *data, u64 size)
    782 {
    783 	u128 result = {0};
    784 	XXH128_hash_t hash = XXH3_128bits_withSeed(data, size, 4969);
    785 	memory_copy(&result, &hash, sizeof(result));
    786 	return result;
    787 }
    788 
    789 function u64
    790 u64_hash_from_str8_seed(str8 string, u64 seed)
    791 {
    792 	u64 result = XXH3_64bits_withSeed(string.data, (u64)string.length, seed);
    793 	return result;
    794 }
    795 
    796 function u64
    797 u64_hash_from_str8(str8 v)
    798 {
    799 	u64 result = u64_hash_from_str8_seed(v, 4969);
    800 	return result;
    801 }
    802 
    803 function str8
    804 str8_from_c_str(char *cstr)
    805 {
    806 	str8 result = {.data = (u8 *)cstr};
    807 	if (cstr) while (*cstr) cstr++;
    808 	result.length = (u8 *)cstr - result.data;
    809 	return result;
    810 }
    811 
    812 function str8
    813 str8_range(u8 *start, u8 *one_past_last)
    814 {
    815 	str8 result;
    816 	result.data   = start;
    817 	result.length = one_past_last - start;
    818 	return result;
    819 }
    820 
    821 function str8
    822 str8_skip(str8 s, i64 count)
    823 {
    824 	str8 result = s;
    825 	if (count > 0) {
    826 		result.data   += count;
    827 		result.length -= count;
    828 	}
    829 	return result;
    830 }
    831 
    832 function b32
    833 str8_equal(str8 a, str8 b)
    834 {
    835 	b32 result = a.length == b.length;
    836 	for (i64 i = 0; result && i < a.length; i++)
    837 		result = a.data[i] == b.data[i];
    838 	return result;
    839 }
    840 
    841 /* NOTE(rnp): returns < 0 if byte is not found */
    842 function i64
    843 str8_scan_backwards(str8 s, u8 byte)
    844 {
    845 	i64 result = (u8 *)memory_scan_backwards(s.data, byte, s.length) - s.data;
    846 	return result;
    847 }
    848 
    849 function str8
    850 str8_cut_head(str8 s, i64 cut)
    851 {
    852 	str8 result = s;
    853 	if (cut > 0) {
    854 		result.data   += cut;
    855 		result.length -= cut;
    856 	}
    857 	result.length = Max(0, result.length);
    858 	return result;
    859 }
    860 
    861 function i64
    862 str8_word_boundary_before(str8 s, i64 p)
    863 {
    864 	p = Max(p - 1, 0);
    865 	b32 negate = IsWordBoundary(s.data[p]);
    866 	while (p >= 0 && (negate ^ !IsWordBoundary(s.data[p])))
    867 		p -= 1;
    868 	p++;
    869 	return p;
    870 }
    871 
    872 function i64
    873 str8_word_boundary_after(str8 s, i64 p)
    874 {
    875 	b32 negate = IsWordBoundary(s.data[p]);
    876 	while (p != s.length && (negate ^ !IsWordBoundary(s.data[p])))
    877 		p += 1;
    878 	return p;
    879 }
    880 
    881 function i64
    882 str8_word_boundary(str8 s, i64 p, i64 count)
    883 {
    884 	i64 sign  = Sign(count);
    885 	i64 end_p = sign > 0 ? s.length : 0;
    886 	count = Abs(count);
    887 	p     = Clamp(p, 0, s.length);
    888 
    889 	for (i64 word_count = 0; word_count < count && p != end_p; word_count++)
    890 		p = sign > 0 ? str8_word_boundary_after(s, p) : str8_word_boundary_before(s, p);
    891 
    892 	return p;
    893 }
    894 
    895 function b32
    896 str8_match(str8 a, str8 b, StringMatchFlags flags)
    897 {
    898 	b32 result = 0;
    899 	if (flags == 0) {
    900 		result = str8_equal(a, b);
    901 	} else if (a.length == b.length || (flags & StringMatchFlag_SloppySize)) {
    902 		result = 1;
    903 		i64 length = Min(a.length, b.length);
    904 		for (i64 it = 0; it < length && result; it++) {
    905 			u8 ab = a.data[it], bb = b.data[it];
    906 			if (flags & StringMatchFlag_CaseInsensitive) {
    907 				ab |= 0x20;
    908 				bb |= 0x20;
    909 			}
    910 			result &= ab == bb;
    911 		}
    912 	}
    913 	return result;
    914 }
    915 
    916 function i64
    917 str8_find_needle(str8 string, str8 needle, StringMatchFlags flags)
    918 {
    919 	u8 *s  = string.data;
    920 	u8 *se = string.data + Max(string.length + 1, needle.length) - needle.length;
    921 	if (needle.length > 0) {
    922 		flags |= StringMatchFlag_SloppySize;
    923 
    924 		u8 nb = needle.data[0];
    925 		if (flags & StringMatchFlag_CaseInsensitive)
    926 			nb |= 0x20;
    927 
    928 		str8 needle_tail = str8_skip(needle, 1);
    929 		u8 *s_opl = string.data + string.length;
    930 		for (; s < se; s++) {
    931 			u8 sb = *s;
    932 			if (flags & StringMatchFlag_CaseInsensitive)
    933 				sb |= 0x20;
    934 
    935 			if (sb == nb && str8_match(str8_range(s + 1, s_opl), needle_tail, flags))
    936 				break;
    937 		}
    938 	}
    939 
    940 	i64 result = string.length;
    941 	if (s < se)
    942 		result = s - string.data;
    943 	return result;
    944 }
    945 
    946 
    947 function str8
    948 str8_alloc(Arena *a, i64 length)
    949 {
    950 	str8 result = {.data = push_array(a, u8, length), .length = length};
    951 	return result;
    952 }
    953 
    954 function str8
    955 str8_from_str16(Arena *a, str16 in)
    956 {
    957 	str8 result = str8("");
    958 	if (in.length) {
    959 		i64 commit = in.length * 4;
    960 		i64 length = 0;
    961 		u8  *data = push_array_no_zero(a, u8, commit + 1);
    962 		u16 *beg  = in.data;
    963 		u16 *end  = in.data + in.length;
    964 		while (beg < end) {
    965 			UnicodeDecode decode = utf16_decode(beg, end - beg);
    966 			length += utf8_encode(data + length, decode.cp);
    967 			beg    += decode.consumed;
    968 		}
    969 		data[length] = 0;
    970 		result = (str8){.length = length, .data = data};
    971 		arena_pop(a, commit - length);
    972 	}
    973 	return result;
    974 }
    975 
    976 function str16
    977 str16_from_str8(Arena *a, str8 in)
    978 {
    979 	str16 result = {0};
    980 	if (in.length) {
    981 		i64  length   = 0;
    982 		i64  required = 2 * in.length + 1;
    983 		u16 *data     = push_array(a, u16, required);
    984 		/* TODO(rnp): utf8_decode */
    985 		for (i64 i = 0; i < in.length; i++) {
    986 			u32 cp  = in.data[i];
    987 			length += utf16_encode(data + length, cp);
    988 		}
    989 		result = (str16){.length = length, .data = data};
    990 		arena_pop(a, required - length);
    991 	}
    992 	return result;
    993 }
    994 
    995 #define push_str8_from_parts(a, j, ...) push_str8_from_parts_((a), (j), arg_list(str8, __VA_ARGS__))
    996 function str8
    997 push_str8_from_parts_(Arena *arena, str8 joiner, str8 *parts, i64 count)
    998 {
    999 	i64 length = joiner.length * (count - 1);
   1000 	for (i64 i = 0; i < count; i++)
   1001 		length += parts[i].length;
   1002 
   1003 	str8 result = {.length = length, .data = push_array_no_zero(arena, u8, length + 1)};
   1004 
   1005 	i64 offset = 0;
   1006 	for (i64 i = 0; i < count; i++) {
   1007 		if (i != 0) {
   1008 			memory_copy(result.data + offset, joiner.data, (u64)joiner.length);
   1009 			offset += joiner.length;
   1010 		}
   1011 		memory_copy(result.data + offset, parts[i].data, (u64)parts[i].length);
   1012 		offset += parts[i].length;
   1013 	}
   1014 	result.data[result.length] = 0;
   1015 
   1016 	return result;
   1017 }
   1018 
   1019 function str8
   1020 push_str8(Arena *a, str8 str)
   1021 {
   1022 	str8 result    = str8_alloc(a, str.length + 1);
   1023 	result.length -= 1;
   1024 	memory_copy(result.data, str.data, (u64)result.length);
   1025 	return result;
   1026 }
   1027 
   1028 // TODO(rnp): replace with handwritten version
   1029 function str8
   1030 push_str8_fv(Arena *arena, const char *format, va_list args)
   1031 {
   1032 	va_list args2;
   1033 	va_copy(args2, args);
   1034 	i64  size    = vsnprintf(0, 0, format, args2);
   1035 	va_end(args2);
   1036 	str8 result  = str8_alloc(arena, size + 1);
   1037 	i64  written = vsnprintf((char *)result.data, result.length, format, args);
   1038 	assert(written == size);
   1039 	result.length -= 1;
   1040 	return result;
   1041 }
   1042 
   1043 function print_format(2, 3) str8
   1044 push_str8_f(Arena *arena, const char *format, ...)
   1045 {
   1046 	va_list args;
   1047 	va_start(args, format);
   1048 	str8 result = push_str8_fv(arena, format, args);
   1049 	va_end(args);
   1050 	return result;
   1051 }
   1052 
   1053 function NumberConversion
   1054 integer_from_str8(str8 raw)
   1055 {
   1056 	read_only local_persist alignas(64) i8 lut[64] = {
   1057 		 0,  1,  2,  3,  4,  5,  6,  7,  8,  9, -1, -1, -1, -1, -1, -1,
   1058 		-1, 10, 11, 12, 13, 14, 15, -1, -1, -1, -1, -1, -1, -1, -1, -1,
   1059 		-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
   1060 		-1, 10, 11, 12, 13, 14, 15, -1, -1, -1, -1, -1, -1, -1, -1, -1,
   1061 	};
   1062 
   1063 	NumberConversion result = {.unparsed = raw};
   1064 
   1065 	i64 i     = 0;
   1066 	i64 scale = 1;
   1067 	if (raw.length > 0 && raw.data[0] == '-') {
   1068 		scale = -1;
   1069 		i     =  1;
   1070 	}
   1071 
   1072 	b32 hex = 0;
   1073 	if (raw.length - i > 2 && raw.data[i] == '0' && (raw.data[1] == 'x' || raw.data[1] == 'X')) {
   1074 		hex = 1;
   1075 		i += 2;
   1076 	}
   1077 
   1078 	#define integer_conversion_body(radix, clamp) do {\
   1079 		for (; i < raw.length; i++) {\
   1080 			i64 value = lut[Min((u8)(raw.data[i] - (u8)'0'), clamp)];\
   1081 			if (value >= 0) {\
   1082 				if (result.U64 > (U64_MAX - (u64)value) / radix) {\
   1083 					result.result = NumberConversionResult_OutOfRange;\
   1084 					result.U64    = U64_MAX;\
   1085 					return result;\
   1086 				} else {\
   1087 					result.U64 = radix * result.U64 + (u64)value;\
   1088 				}\
   1089 			} else {\
   1090 				break;\
   1091 			}\
   1092 		}\
   1093 	} while (0)
   1094 
   1095 	if (hex) integer_conversion_body(16u, 63u);
   1096 	else     integer_conversion_body(10u, 15u);
   1097 
   1098 	#undef integer_conversion_body
   1099 
   1100 	result.unparsed = (str8){.length = raw.length - i, .data = raw.data + i};
   1101 	result.result   = i > 0 ? NumberConversionResult_Success : NumberConversionResult_Invalid;
   1102 	result.kind     = NumberConversionKind_Integer;
   1103 	if (scale < 0) result.U64 = 0 - result.U64;
   1104 
   1105 	return result;
   1106 }
   1107 
   1108 function NumberConversion
   1109 number_from_str8(str8 s)
   1110 {
   1111 	NumberConversion result  = {.unparsed = s};
   1112 	NumberConversion integer = integer_from_str8(s);
   1113 	if (integer.result == NumberConversionResult_Success) {
   1114 		if (integer.unparsed.length != 0 && integer.unparsed.data[0] == '.') {
   1115 			s = integer.unparsed;
   1116 			s.data++;
   1117 			s.length--;
   1118 
   1119 			while (s.length > 0 && s.data[s.length - 1] == '0') s.length--;
   1120 
   1121 			NumberConversion fractional = integer_from_str8(s);
   1122 			if (fractional.result == NumberConversionResult_Success || s.length == 0) {
   1123 				result.F64 = (f64)fractional.U64;
   1124 
   1125 				u64 divisor = (u64)(fractional.unparsed.data - s.data);
   1126 				while (divisor > 0) { result.F64 /= 10.0; divisor--; }
   1127 
   1128 				result.F64 += (f64)integer.S64;
   1129 
   1130 				result.result   = NumberConversionResult_Success;
   1131 				result.kind     = NumberConversionKind_Float;
   1132 				result.unparsed = fractional.unparsed;
   1133 			}
   1134 		} else {
   1135 			result = integer;
   1136 		}
   1137 	}
   1138 	return result;
   1139 }