1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
|
/*************************************************************************/
/* command_queue_mt.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
/* https://godotengine.org */
/*************************************************************************/
/* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
/* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
/* "Software"), to deal in the Software without restriction, including */
/* without limitation the rights to use, copy, modify, merge, publish, */
/* distribute, sublicense, and/or sell copies of the Software, and to */
/* permit persons to whom the Software is furnished to do so, subject to */
/* the following conditions: */
/* */
/* The above copyright notice and this permission notice shall be */
/* included in all copies or substantial portions of the Software. */
/* */
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
/*************************************************************************/
#ifndef COMMAND_QUEUE_MT_H
#define COMMAND_QUEUE_MT_H
#include "core/os/memory.h"
#include "core/os/mutex.h"
#include "core/os/semaphore.h"
#include "core/simple_type.h"
#include "core/typedefs.h"
#define COMMA(N) _COMMA_##N
#define _COMMA_0
#define _COMMA_1 ,
#define _COMMA_2 ,
#define _COMMA_3 ,
#define _COMMA_4 ,
#define _COMMA_5 ,
#define _COMMA_6 ,
#define _COMMA_7 ,
#define _COMMA_8 ,
#define _COMMA_9 ,
#define _COMMA_10 ,
#define _COMMA_11 ,
#define _COMMA_12 ,
#define _COMMA_13 ,
#define _COMMA_14 ,
#define _COMMA_15 ,
// 1-based comma separated list of ITEMs
#define COMMA_SEP_LIST(ITEM, LENGTH) _COMMA_SEP_LIST_##LENGTH(ITEM)
#define _COMMA_SEP_LIST_15(ITEM) \
_COMMA_SEP_LIST_14(ITEM) \
, ITEM(15)
#define _COMMA_SEP_LIST_14(ITEM) \
_COMMA_SEP_LIST_13(ITEM) \
, ITEM(14)
#define _COMMA_SEP_LIST_13(ITEM) \
_COMMA_SEP_LIST_12(ITEM) \
, ITEM(13)
#define _COMMA_SEP_LIST_12(ITEM) \
_COMMA_SEP_LIST_11(ITEM) \
, ITEM(12)
#define _COMMA_SEP_LIST_11(ITEM) \
_COMMA_SEP_LIST_10(ITEM) \
, ITEM(11)
#define _COMMA_SEP_LIST_10(ITEM) \
_COMMA_SEP_LIST_9(ITEM) \
, ITEM(10)
#define _COMMA_SEP_LIST_9(ITEM) \
_COMMA_SEP_LIST_8(ITEM) \
, ITEM(9)
#define _COMMA_SEP_LIST_8(ITEM) \
_COMMA_SEP_LIST_7(ITEM) \
, ITEM(8)
#define _COMMA_SEP_LIST_7(ITEM) \
_COMMA_SEP_LIST_6(ITEM) \
, ITEM(7)
#define _COMMA_SEP_LIST_6(ITEM) \
_COMMA_SEP_LIST_5(ITEM) \
, ITEM(6)
#define _COMMA_SEP_LIST_5(ITEM) \
_COMMA_SEP_LIST_4(ITEM) \
, ITEM(5)
#define _COMMA_SEP_LIST_4(ITEM) \
_COMMA_SEP_LIST_3(ITEM) \
, ITEM(4)
#define _COMMA_SEP_LIST_3(ITEM) \
_COMMA_SEP_LIST_2(ITEM) \
, ITEM(3)
#define _COMMA_SEP_LIST_2(ITEM) \
_COMMA_SEP_LIST_1(ITEM) \
, ITEM(2)
#define _COMMA_SEP_LIST_1(ITEM) \
_COMMA_SEP_LIST_0(ITEM) \
ITEM(1)
#define _COMMA_SEP_LIST_0(ITEM)
// 1-based semicolon separated list of ITEMs
#define SEMIC_SEP_LIST(ITEM, LENGTH) _SEMIC_SEP_LIST_##LENGTH(ITEM)
#define _SEMIC_SEP_LIST_15(ITEM) \
_SEMIC_SEP_LIST_14(ITEM); \
ITEM(15)
#define _SEMIC_SEP_LIST_14(ITEM) \
_SEMIC_SEP_LIST_13(ITEM); \
ITEM(14)
#define _SEMIC_SEP_LIST_13(ITEM) \
_SEMIC_SEP_LIST_12(ITEM); \
ITEM(13)
#define _SEMIC_SEP_LIST_12(ITEM) \
_SEMIC_SEP_LIST_11(ITEM); \
ITEM(12)
#define _SEMIC_SEP_LIST_11(ITEM) \
_SEMIC_SEP_LIST_10(ITEM); \
ITEM(11)
#define _SEMIC_SEP_LIST_10(ITEM) \
_SEMIC_SEP_LIST_9(ITEM); \
ITEM(10)
#define _SEMIC_SEP_LIST_9(ITEM) \
_SEMIC_SEP_LIST_8(ITEM); \
ITEM(9)
#define _SEMIC_SEP_LIST_8(ITEM) \
_SEMIC_SEP_LIST_7(ITEM); \
ITEM(8)
#define _SEMIC_SEP_LIST_7(ITEM) \
_SEMIC_SEP_LIST_6(ITEM); \
ITEM(7)
#define _SEMIC_SEP_LIST_6(ITEM) \
_SEMIC_SEP_LIST_5(ITEM); \
ITEM(6)
#define _SEMIC_SEP_LIST_5(ITEM) \
_SEMIC_SEP_LIST_4(ITEM); \
ITEM(5)
#define _SEMIC_SEP_LIST_4(ITEM) \
_SEMIC_SEP_LIST_3(ITEM); \
ITEM(4)
#define _SEMIC_SEP_LIST_3(ITEM) \
_SEMIC_SEP_LIST_2(ITEM); \
ITEM(3)
#define _SEMIC_SEP_LIST_2(ITEM) \
_SEMIC_SEP_LIST_1(ITEM); \
ITEM(2)
#define _SEMIC_SEP_LIST_1(ITEM) \
_SEMIC_SEP_LIST_0(ITEM) \
ITEM(1)
#define _SEMIC_SEP_LIST_0(ITEM)
// 1-based space separated list of ITEMs
#define SPACE_SEP_LIST(ITEM, LENGTH) _SPACE_SEP_LIST_##LENGTH(ITEM)
#define _SPACE_SEP_LIST_15(ITEM) \
_SPACE_SEP_LIST_14(ITEM) \
ITEM(15)
#define _SPACE_SEP_LIST_14(ITEM) \
_SPACE_SEP_LIST_13(ITEM) \
ITEM(14)
#define _SPACE_SEP_LIST_13(ITEM) \
_SPACE_SEP_LIST_12(ITEM) \
ITEM(13)
#define _SPACE_SEP_LIST_12(ITEM) \
_SPACE_SEP_LIST_11(ITEM) \
ITEM(12)
#define _SPACE_SEP_LIST_11(ITEM) \
_SPACE_SEP_LIST_10(ITEM) \
ITEM(11)
#define _SPACE_SEP_LIST_10(ITEM) \
_SPACE_SEP_LIST_9(ITEM) \
ITEM(10)
#define _SPACE_SEP_LIST_9(ITEM) \
_SPACE_SEP_LIST_8(ITEM) \
ITEM(9)
#define _SPACE_SEP_LIST_8(ITEM) \
_SPACE_SEP_LIST_7(ITEM) \
ITEM(8)
#define _SPACE_SEP_LIST_7(ITEM) \
_SPACE_SEP_LIST_6(ITEM) \
ITEM(7)
#define _SPACE_SEP_LIST_6(ITEM) \
_SPACE_SEP_LIST_5(ITEM) \
ITEM(6)
#define _SPACE_SEP_LIST_5(ITEM) \
_SPACE_SEP_LIST_4(ITEM) \
ITEM(5)
#define _SPACE_SEP_LIST_4(ITEM) \
_SPACE_SEP_LIST_3(ITEM) \
ITEM(4)
#define _SPACE_SEP_LIST_3(ITEM) \
_SPACE_SEP_LIST_2(ITEM) \
ITEM(3)
#define _SPACE_SEP_LIST_2(ITEM) \
_SPACE_SEP_LIST_1(ITEM) \
ITEM(2)
#define _SPACE_SEP_LIST_1(ITEM) \
_SPACE_SEP_LIST_0(ITEM) \
ITEM(1)
#define _SPACE_SEP_LIST_0(ITEM)
#define ARG(N) p##N
#define PARAM(N) P##N p##N
#define TYPE_PARAM(N) class P##N
#define PARAM_DECL(N) typename GetSimpleTypeT<P##N>::type_t p##N
#define DECL_CMD(N) \
template <class T, class M COMMA(N) COMMA_SEP_LIST(TYPE_PARAM, N)> \
struct Command##N : public CommandBase { \
T *instance; \
M method; \
SEMIC_SEP_LIST(PARAM_DECL, N); \
virtual void call() { \
(instance->*method)(COMMA_SEP_LIST(ARG, N)); \
} \
};
#define DECL_CMD_RET(N) \
template <class T, class M, COMMA_SEP_LIST(TYPE_PARAM, N) COMMA(N) class R> \
struct CommandRet##N : public SyncCommand { \
R *ret; \
T *instance; \
M method; \
SEMIC_SEP_LIST(PARAM_DECL, N); \
virtual void call() { \
*ret = (instance->*method)(COMMA_SEP_LIST(ARG, N)); \
} \
};
#define DECL_CMD_SYNC(N) \
template <class T, class M COMMA(N) COMMA_SEP_LIST(TYPE_PARAM, N)> \
struct CommandSync##N : public SyncCommand { \
T *instance; \
M method; \
SEMIC_SEP_LIST(PARAM_DECL, N); \
virtual void call() { \
(instance->*method)(COMMA_SEP_LIST(ARG, N)); \
} \
};
#define TYPE_ARG(N) P##N
#define CMD_TYPE(N) Command##N<T, M COMMA(N) COMMA_SEP_LIST(TYPE_ARG, N)>
#define CMD_ASSIGN_PARAM(N) cmd->p##N = p##N
#define DECL_PUSH(N) \
template <class T, class M COMMA(N) COMMA_SEP_LIST(TYPE_PARAM, N)> \
void push(T *p_instance, M p_method COMMA(N) COMMA_SEP_LIST(PARAM, N)) { \
CMD_TYPE(N) *cmd = allocate_and_lock<CMD_TYPE(N)>(); \
cmd->instance = p_instance; \
cmd->method = p_method; \
SEMIC_SEP_LIST(CMD_ASSIGN_PARAM, N); \
unlock(); \
if (sync) \
sync->post(); \
}
#define CMD_RET_TYPE(N) CommandRet##N<T, M, COMMA_SEP_LIST(TYPE_ARG, N) COMMA(N) R>
#define DECL_PUSH_AND_RET(N) \
template <class T, class M, COMMA_SEP_LIST(TYPE_PARAM, N) COMMA(N) class R> \
void push_and_ret(T *p_instance, M p_method, COMMA_SEP_LIST(PARAM, N) COMMA(N) R *r_ret) { \
SyncSemaphore *ss = _alloc_sync_sem(); \
CMD_RET_TYPE(N) *cmd = allocate_and_lock<CMD_RET_TYPE(N)>(); \
cmd->instance = p_instance; \
cmd->method = p_method; \
SEMIC_SEP_LIST(CMD_ASSIGN_PARAM, N); \
cmd->ret = r_ret; \
cmd->sync_sem = ss; \
unlock(); \
if (sync) \
sync->post(); \
ss->sem.wait(); \
ss->in_use = false; \
}
#define CMD_SYNC_TYPE(N) CommandSync##N<T, M COMMA(N) COMMA_SEP_LIST(TYPE_ARG, N)>
#define DECL_PUSH_AND_SYNC(N) \
template <class T, class M COMMA(N) COMMA_SEP_LIST(TYPE_PARAM, N)> \
void push_and_sync(T *p_instance, M p_method COMMA(N) COMMA_SEP_LIST(PARAM, N)) { \
SyncSemaphore *ss = _alloc_sync_sem(); \
CMD_SYNC_TYPE(N) *cmd = allocate_and_lock<CMD_SYNC_TYPE(N)>(); \
cmd->instance = p_instance; \
cmd->method = p_method; \
SEMIC_SEP_LIST(CMD_ASSIGN_PARAM, N); \
cmd->sync_sem = ss; \
unlock(); \
if (sync) \
sync->post(); \
ss->sem.wait(); \
ss->in_use = false; \
}
#define MAX_CMD_PARAMS 15
class CommandQueueMT {
struct SyncSemaphore {
Semaphore sem;
bool in_use = false;
};
struct CommandBase {
virtual void call() = 0;
virtual void post() {}
virtual ~CommandBase() {}
};
struct SyncCommand : public CommandBase {
SyncSemaphore *sync_sem;
virtual void post() {
sync_sem->sem.post();
}
};
DECL_CMD(0)
SPACE_SEP_LIST(DECL_CMD, 15)
/* comands that return */
DECL_CMD_RET(0)
SPACE_SEP_LIST(DECL_CMD_RET, 15)
/* commands that don't return but sync */
DECL_CMD_SYNC(0)
SPACE_SEP_LIST(DECL_CMD_SYNC, 15)
/***** BASE *******/
enum {
COMMAND_MEM_SIZE_KB = 256,
COMMAND_MEM_SIZE = COMMAND_MEM_SIZE_KB * 1024,
SYNC_SEMAPHORES = 8
};
uint8_t *command_mem = (uint8_t *)memalloc(COMMAND_MEM_SIZE);
uint32_t read_ptr = 0;
uint32_t write_ptr = 0;
uint32_t dealloc_ptr = 0;
SyncSemaphore sync_sems[SYNC_SEMAPHORES];
Mutex mutex;
Semaphore *sync = nullptr;
template <class T>
T *allocate() {
// alloc size is size+T+safeguard
uint32_t alloc_size = ((sizeof(T) + 8 - 1) & ~(8 - 1)) + 8;
tryagain:
if (write_ptr < dealloc_ptr) {
// behind dealloc_ptr, check that there is room
if ((dealloc_ptr - write_ptr) <= alloc_size) {
// There is no more room, try to deallocate something
if (dealloc_one()) {
goto tryagain;
}
return nullptr;
}
} else {
// ahead of dealloc_ptr, check that there is room
if ((COMMAND_MEM_SIZE - write_ptr) < alloc_size + sizeof(uint32_t)) {
// no room at the end, wrap down;
if (dealloc_ptr == 0) { // don't want write_ptr to become dealloc_ptr
// There is no more room, try to deallocate something
if (dealloc_one()) {
goto tryagain;
}
return nullptr;
}
// if this happens, it's a bug
ERR_FAIL_COND_V((COMMAND_MEM_SIZE - write_ptr) < 8, nullptr);
// zero means, wrap to beginning
uint32_t *p = (uint32_t *)&command_mem[write_ptr];
*p = 0;
write_ptr = 0;
goto tryagain;
}
}
// Allocate the size and the 'in use' bit.
// First bit used to mark if command is still in use (1)
// or if it has been destroyed and can be deallocated (0).
uint32_t size = (sizeof(T) + 8 - 1) & ~(8 - 1);
uint32_t *p = (uint32_t *)&command_mem[write_ptr];
*p = (size << 1) | 1;
write_ptr += 8;
// allocate the command
T *cmd = memnew_placement(&command_mem[write_ptr], T);
write_ptr += size;
return cmd;
}
template <class T>
T *allocate_and_lock() {
lock();
T *ret;
while ((ret = allocate<T>()) == nullptr) {
unlock();
// sleep a little until fetch happened and some room is made
wait_for_flush();
lock();
}
return ret;
}
bool flush_one(bool p_lock = true) {
if (p_lock) {
lock();
}
tryagain:
// tried to read an empty queue
if (read_ptr == write_ptr) {
if (p_lock) {
unlock();
}
return false;
}
uint32_t size_ptr = read_ptr;
uint32_t size = *(uint32_t *)&command_mem[read_ptr] >> 1;
if (size == 0) {
//end of ringbuffer, wrap
read_ptr = 0;
goto tryagain;
}
read_ptr += 8;
CommandBase *cmd = reinterpret_cast<CommandBase *>(&command_mem[read_ptr]);
read_ptr += size;
if (p_lock) {
unlock();
}
cmd->call();
if (p_lock) {
lock();
}
cmd->post();
cmd->~CommandBase();
*(uint32_t *)&command_mem[size_ptr] &= ~1;
if (p_lock) {
unlock();
}
return true;
}
void lock();
void unlock();
void wait_for_flush();
SyncSemaphore *_alloc_sync_sem();
bool dealloc_one();
public:
/* NORMAL PUSH COMMANDS */
DECL_PUSH(0)
SPACE_SEP_LIST(DECL_PUSH, 15)
/* PUSH AND RET COMMANDS */
DECL_PUSH_AND_RET(0)
SPACE_SEP_LIST(DECL_PUSH_AND_RET, 15)
/* PUSH AND RET SYNC COMMANDS*/
DECL_PUSH_AND_SYNC(0)
SPACE_SEP_LIST(DECL_PUSH_AND_SYNC, 15)
void wait_and_flush_one() {
ERR_FAIL_COND(!sync);
sync->wait();
flush_one();
}
void flush_all() {
//ERR_FAIL_COND(sync);
lock();
while (flush_one(false)) {
}
unlock();
}
CommandQueueMT(bool p_sync);
~CommandQueueMT();
};
#undef ARG
#undef PARAM
#undef TYPE_PARAM
#undef PARAM_DECL
#undef DECL_CMD
#undef DECL_CMD_RET
#undef DECL_CMD_SYNC
#undef TYPE_ARG
#undef CMD_TYPE
#undef CMD_ASSIGN_PARAM
#undef DECL_PUSH
#undef CMD_RET_TYPE
#undef DECL_PUSH_AND_RET
#undef CMD_SYNC_TYPE
#undef DECL_CMD_SYNC
#endif // COMMAND_QUEUE_MT_H
|