mirror of
https://github.com/open-goal/jak-project.git
synced 2024-10-20 11:26:18 -04:00
2313d35800
* IOP: Rename exitThread, too close to ExitThread. * IOP: Implement Semaphores
368 lines
9.2 KiB
C++
368 lines
9.2 KiB
C++
#include "iso_queue.h"
|
|
|
|
#include <cstdio>
|
|
#include <cstring>
|
|
|
|
#include "isocommon.h"
|
|
|
|
#include "common/log/log.h"
|
|
#include "common/util/Assert.h"
|
|
|
|
#include "game/sce/iop.h"
|
|
|
|
using namespace iop;
|
|
|
|
constexpr int N_BUFFERS = 4;
|
|
constexpr int N_STR_BUFFERS = 1;
|
|
constexpr int N_VAG_CMDS = 64;
|
|
|
|
struct IsoBuffer {
|
|
IsoBufferHeader header;
|
|
u8 data[BUFFER_PAGE_SIZE];
|
|
};
|
|
|
|
struct IsoStrBuffer {
|
|
IsoBufferHeader header;
|
|
u8 data[STR_BUFFER_DATA_SIZE];
|
|
};
|
|
|
|
static IsoBuffer sBuffer[N_BUFFERS];
|
|
static IsoStrBuffer sStrBuffer[N_STR_BUFFERS];
|
|
static IsoBuffer* sFreeBuffer;
|
|
static IsoStrBuffer* sFreeStrBuffer;
|
|
PriStackEntry gPriStack[N_PRIORITIES];
|
|
|
|
u32 vag_cmd_cnt;
|
|
u32 vag_cmd_used;
|
|
u32 max_vag_cmd_cnt;
|
|
VagCommand vag_cmds[N_VAG_CMDS];
|
|
|
|
static s32 sSema;
|
|
|
|
void iso_queue_init_globals() {
|
|
memset(sBuffer, 0, sizeof(sBuffer));
|
|
memset(sStrBuffer, 0, sizeof(sStrBuffer));
|
|
sFreeBuffer = nullptr;
|
|
sFreeStrBuffer = nullptr;
|
|
for (auto& e : gPriStack)
|
|
e.reset();
|
|
|
|
vag_cmd_cnt = 0;
|
|
vag_cmd_used = 0;
|
|
max_vag_cmd_cnt = 0;
|
|
memset(vag_cmds, 0, sizeof(vag_cmds));
|
|
sSema = 0;
|
|
}
|
|
|
|
void PriStackEntry::reset() {
|
|
for (auto& c : cmds)
|
|
c = nullptr;
|
|
n = 0;
|
|
for (auto& x : names)
|
|
x.clear();
|
|
}
|
|
|
|
void InitBuffers() {
|
|
// chain all buffers together and set them as free.
|
|
for (uint32_t i = 0; i < N_BUFFERS; i++) {
|
|
sBuffer[i].header.data = nullptr;
|
|
sBuffer[i].header.data_size = 0;
|
|
sBuffer[i].header.buffer_size = BUFFER_PAGE_SIZE;
|
|
sBuffer[i].header.next = &sBuffer[i + 1].header;
|
|
}
|
|
sBuffer[N_BUFFERS - 1].header.next = nullptr;
|
|
sFreeBuffer = &sBuffer[0];
|
|
|
|
for (uint32_t i = 0; i < N_STR_BUFFERS; i++) {
|
|
sStrBuffer[i].header.data = nullptr;
|
|
sStrBuffer[i].header.data_size = 0;
|
|
sStrBuffer[i].header.buffer_size = STR_BUFFER_DATA_SIZE;
|
|
sStrBuffer[i].header.next = &sStrBuffer[i + 1].header;
|
|
}
|
|
sStrBuffer[N_STR_BUFFERS - 1].header.next = nullptr;
|
|
sFreeStrBuffer = &sStrBuffer[0];
|
|
|
|
SemaParam params;
|
|
params.attr = SA_THPRI;
|
|
params.max_count = 1;
|
|
params.init_count = 1;
|
|
params.option = 0;
|
|
sSema = CreateSema(¶ms);
|
|
|
|
if (sSema < 0) {
|
|
for (;;) {
|
|
printf("[OVERLORD] VAG Semaphore creation failed!\n");
|
|
}
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* Allocate a buffer of the given size. If not possible, loop forever. Size must be BUFFER_PAGE_SIZE
|
|
* or STR_BUFFER_DATA_SIZE,
|
|
*/
|
|
IsoBufferHeader* AllocateBuffer(uint32_t size) {
|
|
IsoBufferHeader* buffer = TryAllocateBuffer(size);
|
|
if (buffer) {
|
|
return buffer;
|
|
} else {
|
|
while (true) {
|
|
printf("[OVERLORD ISO QUEUE] Failed to allocate buffer!\n");
|
|
}
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* Allocate a buffer of given size. If the size isn't BUFFER_PAGE_SIZE, you get a streaming buffer
|
|
* (STR_BUFFER_DATA_SIZE). If no allocation can be done, return nullptr.
|
|
*/
|
|
IsoBufferHeader* TryAllocateBuffer(uint32_t size) {
|
|
IsoStrBuffer* top_str = sFreeStrBuffer;
|
|
IsoBuffer* top_buff = sFreeBuffer;
|
|
|
|
if (size == BUFFER_PAGE_SIZE) {
|
|
if (sFreeBuffer) {
|
|
auto next = sFreeBuffer->header.next;
|
|
sFreeBuffer->header.data = nullptr;
|
|
sFreeBuffer = (IsoBuffer*)next;
|
|
top_buff->header.data_size = 0;
|
|
top_buff->header.next = nullptr;
|
|
return (IsoBufferHeader*)top_buff;
|
|
}
|
|
} else {
|
|
if (sFreeStrBuffer) {
|
|
auto next = sFreeStrBuffer->header.next;
|
|
sFreeStrBuffer->header.data = nullptr;
|
|
sFreeStrBuffer = (IsoStrBuffer*)next;
|
|
top_str->header.data_size = 0;
|
|
top_str->header.next = nullptr;
|
|
return (IsoBufferHeader*)top_str;
|
|
}
|
|
}
|
|
printf("[OVERLORD] Failed to allocate buffer (requested size 0x%x)\n", size);
|
|
return nullptr;
|
|
}
|
|
|
|
/*!
|
|
* Return a buffer once you are done using it so somebody else can have a turn
|
|
*/
|
|
void FreeBuffer(IsoBufferHeader* buffer) {
|
|
IsoBufferHeader* b = (IsoBufferHeader*)buffer;
|
|
if (b->buffer_size == BUFFER_PAGE_SIZE) {
|
|
b->next = sFreeBuffer;
|
|
sFreeBuffer = (IsoBuffer*)b;
|
|
} else {
|
|
b->next = sFreeStrBuffer;
|
|
sFreeStrBuffer = (IsoStrBuffer*)b;
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* Display all messages in the priority stack
|
|
* The actual function does nothing.
|
|
*/
|
|
void DisplayQueue() {
|
|
for (int pri = 0; pri < N_PRIORITIES; pri++) {
|
|
for (int cmd = 0; cmd < (int)gPriStack[pri].n; cmd++) {
|
|
lg::debug(" PRI {} elt {} {}", pri, cmd, gPriStack[pri].names[cmd]);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* Add a message to the back of the queue for the given priority.
|
|
* If there is no room left in the queue, ReturnMessage with a CMD_STATUS_FAILED_TO_QUEUE.
|
|
* Return 1 on success.
|
|
*/
|
|
u32 QueueMessage(IsoMessage* cmd, int32_t priority, const char* name) {
|
|
u32 ok = gPriStack[priority].n != PRI_STACK_LENGTH;
|
|
if (ok) {
|
|
gPriStack[priority].cmds[gPriStack[priority].n] = cmd;
|
|
gPriStack[priority].names[gPriStack[priority].n] = name;
|
|
gPriStack[priority].n++;
|
|
lg::debug("[OVERLORD] Queue {} ({}/{}), {}", priority, gPriStack[priority].n, PRI_STACK_LENGTH,
|
|
gPriStack[priority].names[gPriStack[priority].n - 1].c_str());
|
|
DisplayQueue();
|
|
} else {
|
|
lg::warn("[OVERLORD ISO QUEUE] Failed to queue!");
|
|
cmd->status = CMD_STATUS_FAILED_TO_QUEUE;
|
|
ReturnMessage(cmd);
|
|
}
|
|
return ok;
|
|
}
|
|
|
|
/*!
|
|
* Remove a message from the priority stack.
|
|
*/
|
|
void UnqueueMessage(IsoMessage* cmd) {
|
|
int pri = 0;
|
|
u32 idx = 0;
|
|
PriStackEntry* pse;
|
|
|
|
// loop over priorities
|
|
for (pri = 0; pri < N_PRIORITIES; pri++) {
|
|
pse = gPriStack + pri;
|
|
|
|
// loop over entries
|
|
for (idx = 0; idx < gPriStack[pri].n; idx++) {
|
|
if (pse->cmds[idx] == cmd) {
|
|
goto found;
|
|
}
|
|
}
|
|
}
|
|
lg::warn("[OVERLORD ISO QUEUE] Failed to unqueue!");
|
|
|
|
found:
|
|
ASSERT(gPriStack[pri].cmds[idx] == cmd);
|
|
|
|
// pop
|
|
gPriStack[pri].n--;
|
|
// and move other entries up.
|
|
while (idx < gPriStack[pri].n) {
|
|
pse->cmds[idx] = pse->cmds[idx + 1];
|
|
idx++;
|
|
}
|
|
DisplayQueue();
|
|
}
|
|
|
|
/*!
|
|
* Get the highest priority message with an open buffer.
|
|
* (Note - messages with priority less than max priority will be gotten if they have < 2 buffers
|
|
* filled)
|
|
* @return
|
|
*/
|
|
IsoMessage* GetMessage() {
|
|
// loop over all priorities
|
|
for (int pri = (N_PRIORITIES - 1); pri >= 0; pri--) {
|
|
auto pse = gPriStack + pri;
|
|
int idx = gPriStack[pri].n;
|
|
for (idx = idx - 1; idx >= 0; idx--) {
|
|
if (pse->cmds[idx]->fd && pse->cmds[idx]->status == CMD_STATUS_IN_PROGRESS &&
|
|
pse->cmds[idx]->ready_for_data) {
|
|
if (pri == N_PRIORITIES - 1) {
|
|
// return high priority commands only if they don't have any buffers filled
|
|
if (!pse->cmds[idx]->callback_buffer) {
|
|
return pse->cmds[idx];
|
|
}
|
|
} else {
|
|
// return lower priority commands if they don't have 2 buffers filled.
|
|
if (!pse->cmds[idx]->callback_buffer ||
|
|
!(IsoBufferHeader*)(pse->cmds[idx]->callback_buffer)->next) {
|
|
return pse->cmds[idx];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
/*!
|
|
* Execute callbacks and maintain buffers for finished reads in the priority stack
|
|
*/
|
|
void ProcessMessageData() {
|
|
for (s32 pri = N_PRIORITIES - 1; pri >= 0; pri--) {
|
|
for (s32 n = gPriStack[pri].n - 1; n >= 0; n--) {
|
|
IsoMessage* cmd = gPriStack[pri].cmds[n];
|
|
|
|
if (cmd->status == CMD_STATUS_IN_PROGRESS) {
|
|
IsoBufferHeader* callback_buffer = cmd->callback_buffer;
|
|
|
|
if (callback_buffer != nullptr) {
|
|
cmd->status = cmd->callback_function(cmd, callback_buffer);
|
|
|
|
if (callback_buffer->data_size == 0) {
|
|
cmd->callback_buffer = (IsoBufferHeader*)callback_buffer->next;
|
|
FreeBuffer(callback_buffer);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (cmd->status != CMD_STATUS_IN_PROGRESS) {
|
|
ReleaseMessage(cmd);
|
|
ReturnMessage(cmd);
|
|
pri++;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* Wakeup thread/message mbx for a message
|
|
*/
|
|
void ReturnMessage(IsoMessage* cmd) {
|
|
if (!cmd->messagebox_to_reply) {
|
|
if (cmd->thread_id == 0) {
|
|
FreeVAGCommand((VagCommand*)cmd);
|
|
} else {
|
|
WakeupThread(cmd->thread_id);
|
|
}
|
|
} else {
|
|
SendMbx(cmd->messagebox_to_reply, (MsgPacket*)cmd);
|
|
}
|
|
}
|
|
|
|
/*!
|
|
* Free buffers, close files, and remove from priority stack
|
|
*/
|
|
void ReleaseMessage(IsoMessage* cmd) {
|
|
// kill all buffers
|
|
while (cmd->callback_buffer) {
|
|
auto old_head = cmd->callback_buffer;
|
|
cmd->callback_buffer = (IsoBufferHeader*)old_head->next;
|
|
FreeBuffer(old_head);
|
|
}
|
|
|
|
// close file
|
|
if (cmd->fd) {
|
|
isofs->close(cmd->fd);
|
|
}
|
|
|
|
// unqueue message
|
|
UnqueueMessage(cmd);
|
|
}
|
|
|
|
// GetVAGCommand
|
|
VagCommand* GetVAGCommand() {
|
|
for (;;) {
|
|
// wait for command to be available
|
|
while (vag_cmd_cnt == (N_VAG_CMDS - 1)) {
|
|
DelayThread(100);
|
|
}
|
|
|
|
// wait for VAG semaphore
|
|
while (WaitSema(sSema)) {
|
|
}
|
|
|
|
// try to get something.
|
|
for (s32 i = 0; i < N_VAG_CMDS; i++) {
|
|
if (!((vag_cmd_used >> (i & 0x1f)) & 1)) {
|
|
// free!
|
|
vag_cmd_used |= (1 << (i & 0x1f));
|
|
vag_cmd_cnt++;
|
|
if (vag_cmd_cnt > max_vag_cmd_cnt) {
|
|
max_vag_cmd_cnt = vag_cmd_cnt;
|
|
}
|
|
SignalSema(sSema);
|
|
return &vag_cmds[i];
|
|
}
|
|
}
|
|
|
|
SignalSema(sSema);
|
|
}
|
|
}
|
|
|
|
void FreeVAGCommand(VagCommand* cmd) {
|
|
s32 idx = cmd - vag_cmds;
|
|
if (idx >= 0 && idx < N_VAG_CMDS && ((vag_cmd_used >> (idx & 0x1f)) & 1)) {
|
|
while (WaitSema(sSema)) {
|
|
}
|
|
|
|
vag_cmd_used &= ~(1 << (idx & 0x1f));
|
|
vag_cmd_cnt--;
|
|
SignalSema(sSema);
|
|
} else {
|
|
printf("[OVERLORD] Invalid FreeVAGCommand!\n");
|
|
}
|
|
}
|