IOCommandQueue.cpp [plain text]
#include <IOKit/IOCommandQueue.h>
#include <IOKit/IOWorkLoop.h>
#include <IOKit/IOTimeStamp.h>
#include <mach/sync_policy.h>
#define NUM_FIELDS_IN_COMMAND 4
typedef struct commandEntryTag {
void *f[NUM_FIELDS_IN_COMMAND];
} commandEntryT;
#define super IOEventSource
OSDefineMetaClassAndStructors(IOCommandQueue, IOEventSource)
bool IOCommandQueue::init(OSObject *inOwner,
IOCommandQueueAction inAction,
int inSize)
{
if ( !super::init(inOwner, (IOEventSourceAction) inAction) )
return false;
if (KERN_SUCCESS
!= semaphore_create(kernel_task, &producerSema, SYNC_POLICY_FIFO, inSize))
return false;
size = inSize + 1;
queue = (void *)kalloc(size * sizeof(commandEntryT));
if (!queue)
return false;
producerLock = IOLockAlloc();
if (!producerLock)
return false;
producerIndex = consumerIndex = 0;
return true;
}
IOCommandQueue *
IOCommandQueue::commandQueue(OSObject *inOwner,
IOCommandQueueAction inAction,
int inSize)
{
IOCommandQueue *me = new IOCommandQueue;
if (me && !me->init(inOwner, inAction, inSize)) {
me->free();
return 0;
}
return me;
}
void IOCommandQueue::free()
{
if (queue)
kfree(queue, size * sizeof(commandEntryT));
if (producerSema)
semaphore_destroy(kernel_task, producerSema);
if (producerLock)
IOLockFree(producerLock);
super::free();
}
#if NUM_FIELDS_IN_COMMAND != 4
#error IOCommandQueue::checkForWork needs to be updated for new command size
#endif
bool IOCommandQueue::checkForWork()
{
void *field0, *field1, *field2, *field3;
if (!enabled || consumerIndex == producerIndex)
return false;
{
commandEntryT *q = (commandEntryT *) queue;
int localIndex = consumerIndex;
field0 = q[localIndex].f[0]; field1 = q[localIndex].f[1];
field2 = q[localIndex].f[2]; field3 = q[localIndex].f[3];
semaphore_signal(producerSema);
}
if (++consumerIndex >= size)
consumerIndex = 0;
IOTimeStampConstant(IODBG_CMDQ(IOCMDQ_ACTION),
(unsigned int) action, (unsigned int) owner);
(*(IOCommandQueueAction) action)(owner, field0, field1, field2, field3);
return (consumerIndex != producerIndex);
}
#if NUM_FIELDS_IN_COMMAND != 4
#error IOCommandQueue::enqueueCommand needs to be updated
#endif
kern_return_t
IOCommandQueue::enqueueCommand(bool gotoSleep,
void *field0, void *field1,
void *field2, void *field3)
{
kern_return_t rtn = KERN_SUCCESS;
int retry;
if (gotoSleep) {
retry = 0;
do
rtn = semaphore_wait(producerSema);
while( (KERN_SUCCESS != rtn)
&& (KERN_OPERATION_TIMED_OUT != rtn)
&& (KERN_SEMAPHORE_DESTROYED != rtn)
&& (KERN_TERMINATED != rtn)
&& ((retry++) < 4));
} else
rtn = semaphore_timedwait(producerSema, MACH_TIMESPEC_ZERO);
if (KERN_SUCCESS != rtn)
return rtn;
IOTakeLock(producerLock);
{
commandEntryT *q = (commandEntryT *) queue;
int localIndex = producerIndex;
q[localIndex].f[0] = field0; q[localIndex].f[1] = field1;
q[localIndex].f[2] = field2; q[localIndex].f[3] = field3;
}
if (++producerIndex >= size)
producerIndex = 0;
IOUnlock(producerLock);
signalWorkAvailable();
return rtn;
}
int IOCommandQueue::performAndFlush(OSObject *target,
IOCommandQueueAction inAction)
{
int numEntries;
kern_return_t rtn;
if (!target)
target = owner;
if (!inAction)
inAction = (IOCommandQueueAction) action;
do {
rtn = semaphore_timedwait(producerSema, MACH_TIMESPEC_ZERO);
} while (rtn == KERN_SUCCESS);
for (numEntries = 0; consumerIndex != producerIndex; ) {
void *field0, *field1, *field2, *field3;
{
commandEntryT *q = (commandEntryT *) queue;
int localIndex = consumerIndex;
field0 = q[localIndex].f[0]; field1 = q[localIndex].f[1];
field2 = q[localIndex].f[2]; field3 = q[localIndex].f[3];
}
if (++consumerIndex >= size)
consumerIndex = 0;
(*inAction)(target, field0, field1, field2, field3);
}
for (int i = 1; i < size; i++)
semaphore_signal(producerSema);
return numEntries;
}