/// <summary>
/// Submit item in queue
/// </summary>
/// <param name="queue">Queue</param>
/// <param name="item">Item</param>
private int SubmitCMD(NVMe_Queue queue, NVMe_Submission_Item *item)
{
int tail = queue.Tail;
queue.mSubmissionMutex.Lock();
item->CommandID = CurrentCID;
Memory.Memcpy((void *)((int)queue.SubmissionQueue + (sizeof(NVMe_Submission_Item) * tail)), item, sizeof(NVMe_Submission_Item));
var itemm = (NVMe_Submission_Item *)((int)queue.SubmissionQueue + (sizeof(NVMe_Submission_Item) * tail));
tail = tail + 1;
if (tail == queue.Max)
{
tail = 0;
}
queue.Tail = tail;
*queue.TailPtr = tail;
if (++CurrentCID >= 65535)
{
item->CommandID = 0;
}
queue.mSubmissionMutex.Unlock();
return(CurrentCID - 1);
}
private NVMe_Queue CreateQueue(int qid)
{
NVMe_Queue queue = AllocQueue(qid);
int status = CreateCompletionQueue(queue);
int statusCode = (status >> COMP_STATUS_SC_SHIFT) & COMP_STATUS_SC_MASK;
if (statusCode != COMP_STATUS_SC_SUC)
{
return(null);
}
status = CreateSubmissionQueue(queue);
statusCode = (status >> COMP_STATUS_SC_SHIFT) & COMP_STATUS_SC_MASK;
if (statusCode != COMP_STATUS_SC_SUC)
{
return(null);
}
return(queue);
}
/// <summary>
/// Configure admin queue
/// </summary>
private void ConfigureAdminQueue()
{
mAdminQueue = AllocQueue(0);
mRegs->ControllerStatus = 0x00;
mRegs->AdminQueueAttribs = (uint)((uint)(mQueueSize - 1) & AQA_ASQS_MASK);
mRegs->AdminQueueAttribs |= (uint)(((mQueueSize - 1) & AQA_ACQS_SHIFT) << AQA_ACQS_SHIFT);
mRegs->AdminSubmissionQueueAddress = (ulong)Paging.GetPhysicalFromVirtual(mAdminQueue.SubmissionQueue);
mRegs->AdminCompletionQueueAddress = (ulong)Paging.GetPhysicalFromVirtual(mAdminQueue.CompletionQueue);
}
/// <summary>
/// Create queues (for now just 1..)
/// </summary>
private void CreateQueues()
{
var queue = CreateQueue(1);
if (queue == null)
{
Panic.DoPanic("[NVMe] Couldn't make queue");
}
mIOQueue = queue;
}
private NVMe_Completion_Item *PollAndWait(NVMe_Queue queue, int cid)
{
while (true)
{
for (int i = 0; i < mQueueSize; i++)
{
var item = (NVMe_Completion_Item *)((int)queue.CompletionQueue + (sizeof(NVMe_Completion_Item) * i));
if (item->CommandID == cid)
{
return(item);
}
}
Tasking.CurrentTask.CurrentThread.Sleep(0, 100);
}
}
private int CreateCompletionQueue(NVMe_Queue queue)
{
NVMe_Create_Cq_Cmd *item = (NVMe_Create_Cq_Cmd *)Heap.Alloc(sizeof(NVMe_Create_Cq_Cmd));
Memory.Memclear(item, sizeof(NVMe_Create_Cq_Cmd));
item->Opcode = ADMIN_OPCODE_CREATE_CQ;
item->PRP1 = (uint)Paging.GetPhysicalFromVirtual(queue.CompletionQueue);
item->CqID = (ushort)queue.SQID;
item->CqSize = (ushort)(mQueueSize - 1);
item->CqFlags = (ushort)(QUEUE_PHYS_CONFIG | SQ_PRIO_MEDIUM);
int cid = SubmitCMD(mAdminQueue, (NVMe_Submission_Item *)item);
Heap.Free(item);
NVMe_Completion_Item *compItem = PollAndWait(mAdminQueue, cid);
return(compItem->Status);
}
/// <summary>
/// Alocate queue
/// </summary>
/// <param name="sqID">Submission Queue ID</param>
/// <returns>Queue</returns>
private unsafe NVMe_Queue AllocQueue(int sqID)
{
NVMe_Queue queue = new NVMe_Queue();
queue.SQID = sqID;
queue.SubmissionQueue = (NVMe_Submission_Item *)Heap.AlignedAlloc(0x1000, sizeof(NVMe_Submission_Item) * mQueueSize);
queue.CompletionQueue = (NVMe_Completion_Item *)Heap.AlignedAlloc(0x1000, sizeof(NVMe_Completion_Item) * mQueueSize);
Memory.Memclear(queue.SubmissionQueue, sizeof(NVMe_Submission_Item) * mQueueSize);
Memory.Memclear(queue.CompletionQueue, sizeof(NVMe_Completion_Item) * mQueueSize);
queue.mSubmissionMutex = new Mutex();
queue.Max = mQueueSize;
queue.Tail = 0;
uint tail = (uint)mTailsAndHeads + (uint)((2 * sqID) * (sizeof(int) << mStride));
uint head = (uint)mTailsAndHeads + (uint)((2 * sqID + 1) * (sizeof(int) << mStride));
queue.TailPtr = (int *)tail;
queue.HeadPtr = (int *)head;
return(queue);
}
