[apple/xnu.git] / iokit / System / IODataQueueDispatchSourceShared.h

typedef struct _IODataQueueEntry {
	uint32_t  size;
	uint8_t   data[0];
} IODataQueueEntry;

#define DATA_QUEUE_ENTRY_HEADER_SIZE sizeof(IODataQueueEntry)

typedef struct _IODataQueueMemory {
	volatile uint32_t   head;
	volatile uint32_t   tail;
	volatile uint8_t    needServicedCallback;
	volatile uint8_t    _resv[31];
	IODataQueueEntry  queue[0];
} IODataQueueMemory;

struct IODataQueueDispatchSource_IVars {
	IODataQueueMemory         * dataQueue;
	IODataQueueDispatchSource * source;
//    IODispatchQueue           * queue;
	IOMemoryDescriptor        * memory;
	OSAction                  * dataAvailableAction;
	OSAction                  * dataServicedAction;
	uint64_t                    options;
	uint32_t                    queueByteCount;

#if !KERNEL
	bool                        enable;
	bool                        canceled;
#endif
};

bool
IODataQueueDispatchSource::init()
{
	if (!super::init()) {
		return false;
	}

	ivars = IONewZero(IODataQueueDispatchSource_IVars, 1);
	ivars->source = this;

#if !KERNEL
	kern_return_t ret;

	ret = CopyMemory(&ivars->memory);
	assert(kIOReturnSuccess == ret);

	uint64_t address;
	uint64_t length;

	ret = ivars->memory->Map(0, 0, 0, 0, &address, &length);
	assert(kIOReturnSuccess == ret);
	ivars->dataQueue = (typeof(ivars->dataQueue))(uintptr_t) address;
	ivars->queueByteCount = length;
#endif

	return true;
}

kern_return_t
IODataQueueDispatchSource::CheckForWork_Impl(
	const IORPC rpc,
	bool synchronous)
{
	IOReturn ret = kIOReturnNotReady;

	return ret;
}

#if KERNEL

kern_return_t
IODataQueueDispatchSource::Create_Impl(
	uint64_t queueByteCount,
	IODispatchQueue * queue,
	IODataQueueDispatchSource ** source)
{
	IODataQueueDispatchSource * inst;
	IOBufferMemoryDescriptor  * bmd;

	if (3 & queueByteCount) {
		return kIOReturnBadArgument;
	}
	if (queueByteCount > UINT_MAX) {
		return kIOReturnBadArgument;
	}
	inst = OSTypeAlloc(IODataQueueDispatchSource);
	if (!inst) {
		return kIOReturnNoMemory;
	}
	if (!inst->init()) {
		inst->release();
		return kIOReturnError;
	}

	bmd = IOBufferMemoryDescriptor::withOptions(
		kIODirectionOutIn | kIOMemoryKernelUserShared,
		queueByteCount, page_size);
	if (!bmd) {
		inst->release();
		return kIOReturnNoMemory;
	}
	inst->ivars->memory         = bmd;
	inst->ivars->queueByteCount = ((uint32_t) queueByteCount);
	inst->ivars->options        = 0;
	inst->ivars->dataQueue      = (typeof(inst->ivars->dataQueue))bmd->getBytesNoCopy();

	*source = inst;

	return kIOReturnSuccess;
}

kern_return_t
IODataQueueDispatchSource::CopyMemory_Impl(
	IOMemoryDescriptor ** memory)
{
	kern_return_t ret;
	IOMemoryDescriptor * result;

	result = ivars->memory;
	if (result) {
		result->retain();
		ret = kIOReturnSuccess;
	} else {
		ret = kIOReturnNotReady;
	}
	*memory = result;

	return ret;
}

kern_return_t
IODataQueueDispatchSource::CopyDataAvailableHandler_Impl(
	OSAction ** action)
{
	kern_return_t ret;
	OSAction    * result;

	result = ivars->dataAvailableAction;
	if (result) {
		result->retain();
		ret = kIOReturnSuccess;
	} else {
		ret = kIOReturnNotReady;
	}
	*action = result;

	return ret;
}

kern_return_t
IODataQueueDispatchSource::CopyDataServicedHandler_Impl(
	OSAction ** action)
{
	kern_return_t ret;
	OSAction    * result;

	result = ivars->dataServicedAction;
	if (result) {
		result->retain();
		ret = kIOReturnSuccess;
	} else {
		ret = kIOReturnNotReady;
	}
	*action = result;
	return ret;
}

kern_return_t
IODataQueueDispatchSource::SetDataAvailableHandler_Impl(
	OSAction * action)
{
	IOReturn ret;
	OSAction * oldAction;

	oldAction = ivars->dataAvailableAction;
	if (oldAction && OSCompareAndSwapPtr(oldAction, NULL, &ivars->dataAvailableAction)) {
		oldAction->release();
	}
	if (action) {
		action->retain();
		ivars->dataAvailableAction = action;
		if (IsDataAvailable()) {
			DataAvailable(ivars->dataAvailableAction);
		}
	}
	ret = kIOReturnSuccess;

	return ret;
}

kern_return_t
IODataQueueDispatchSource::SetDataServicedHandler_Impl(
	OSAction * action)
{
	IOReturn ret;
	OSAction * oldAction;

	oldAction = ivars->dataServicedAction;
	if (oldAction && OSCompareAndSwapPtr(oldAction, NULL, &ivars->dataServicedAction)) {
		oldAction->release();
	}
	if (action) {
		action->retain();
		ivars->dataServicedAction = action;
	}
	ret = kIOReturnSuccess;

	return ret;
}

#endif /* KERNEL */

void
IODataQueueDispatchSource::SendDataAvailable(void)
{
	IOReturn ret;

	if (!ivars->dataAvailableAction) {
		ret = CopyDataAvailableHandler(&ivars->dataAvailableAction);
		if (kIOReturnSuccess != ret) {
			ivars->dataAvailableAction = NULL;
		}
	}
	if (ivars->dataAvailableAction) {
		DataAvailable(ivars->dataAvailableAction);
	}
}

void
IODataQueueDispatchSource::SendDataServiced(void)
{
	IOReturn ret;

	if (!ivars->dataServicedAction) {
		ret = CopyDataServicedHandler(&ivars->dataServicedAction);
		if (kIOReturnSuccess != ret) {
			ivars->dataServicedAction = NULL;
		}
	}
	if (ivars->dataServicedAction) {
		ivars->dataQueue->needServicedCallback = false;
		DataServiced(ivars->dataServicedAction);
	}
}

kern_return_t
IODataQueueDispatchSource::SetEnableWithCompletion_Impl(
	bool enable,
	IODispatchSourceCancelHandler handler)
{
	IOReturn ret;

#if !KERNEL
	ivars->enable = enable;
#endif

	ret = kIOReturnSuccess;
	return ret;
}

void
IODataQueueDispatchSource::free()
{
	OSSafeReleaseNULL(ivars->memory);
	OSSafeReleaseNULL(ivars->dataAvailableAction);
	OSSafeReleaseNULL(ivars->dataServicedAction);
	IOSafeDeleteNULL(ivars, IODataQueueDispatchSource_IVars, 1);
	super::free();
}

kern_return_t
IODataQueueDispatchSource::Cancel_Impl(
	IODispatchSourceCancelHandler handler)
{
	return kIOReturnSuccess;
}

bool
IODataQueueDispatchSource::IsDataAvailable(void)
{
	IODataQueueMemory *dataQueue = ivars->dataQueue;

	return dataQueue && (dataQueue->head != dataQueue->tail);
}

kern_return_t
IODataQueueDispatchSource::Peek(IODataQueueClientDequeueEntryBlock callback)
{
	IODataQueueEntry *  entry = NULL;
	IODataQueueMemory * dataQueue;
	uint32_t            callerDataSize;
	uint32_t            dataSize;
	uint32_t            headOffset;
	uint32_t            tailOffset;

	dataQueue = ivars->dataQueue;
	if (!dataQueue) {
		return kIOReturnNoMemory;
	}

	// Read head and tail with acquire barrier
	headOffset = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->head, __ATOMIC_RELAXED);
	tailOffset = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->tail, __ATOMIC_ACQUIRE);

	if (headOffset != tailOffset) {
		IODataQueueEntry *  head        = NULL;
		uint32_t            headSize    = 0;
		uint32_t            queueSize   = ivars->queueByteCount;

		if (headOffset > queueSize) {
			return kIOReturnError;
		}

		head     = (IODataQueueEntry *)((uintptr_t)dataQueue->queue + headOffset);
		callerDataSize = head->size;
		if (os_add_overflow(3, callerDataSize, &headSize)) {
			return kIOReturnError;
		}
		headSize &= ~3U;

		// Check if there's enough room before the end of the queue for a header.
		// If there is room, check if there's enough room to hold the header and
		// the data.

		if ((headOffset > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) ||
		    (headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize) ||
		    (headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > UINT32_MAX - headSize) ||
		    (headOffset + headSize + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize)) {
			// No room for the header or the data, wrap to the beginning of the queue.
			// Note: wrapping even with the UINT32_MAX checks, as we have to support
			// queueSize of UINT32_MAX
			entry = dataQueue->queue;
			callerDataSize  = entry->size;
			dataSize = entry->size;
			if (os_add_overflow(3, callerDataSize, &dataSize)) {
				return kIOReturnError;
			}
			dataSize &= ~3U;

			if ((dataSize > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) ||
			    (dataSize + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize)) {
				return kIOReturnError;
			}

			callback(&entry->data, callerDataSize);
			return kIOReturnSuccess;
		} else {
			callback(&head->data, callerDataSize);
			return kIOReturnSuccess;
		}
	}

	return kIOReturnUnderrun;
}

kern_return_t
IODataQueueDispatchSource::Dequeue(IODataQueueClientDequeueEntryBlock callback)
{
	kern_return_t ret;
	bool          sendDataServiced;

	sendDataServiced = false;
	ret = DequeueWithCoalesce(&sendDataServiced, callback);
	if (sendDataServiced) {
		SendDataServiced();
	}
	return ret;
}

kern_return_t
IODataQueueDispatchSource::DequeueWithCoalesce(bool * sendDataServiced,
    IODataQueueClientDequeueEntryBlock callback)
{
	IOReturn            retVal          = kIOReturnSuccess;
	IODataQueueEntry *  entry           = NULL;
	IODataQueueMemory * dataQueue;
	uint32_t            callerDataSize;
	uint32_t            dataSize        = 0;
	uint32_t            headOffset      = 0;
	uint32_t            tailOffset      = 0;
	uint32_t            newHeadOffset   = 0;

	dataQueue = ivars->dataQueue;
	if (!dataQueue) {
		return kIOReturnNoMemory;
	}

	// Read head and tail with acquire barrier
	headOffset = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->head, __ATOMIC_RELAXED);
	tailOffset = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->tail, __ATOMIC_ACQUIRE);

	if (headOffset != tailOffset) {
		IODataQueueEntry *  head        = NULL;
		uint32_t            headSize    = 0;
		uint32_t            queueSize   = ivars->queueByteCount;

		if (headOffset > queueSize) {
			return kIOReturnError;
		}

		head = (IODataQueueEntry *)((uintptr_t)dataQueue->queue + headOffset);
		callerDataSize = head->size;
		if (os_add_overflow(3, callerDataSize, &headSize)) {
			return kIOReturnError;
		}
		headSize &= ~3U;

		// we wrapped around to beginning, so read from there
		// either there was not even room for the header
		if ((headOffset > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) ||
		    (headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize) ||
		    // or there was room for the header, but not for the data
		    (headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > UINT32_MAX - headSize) ||
		    (headOffset + headSize + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize)) {
			// Note: we have to wrap to the beginning even with the UINT32_MAX checks
			// because we have to support a queueSize of UINT32_MAX.
			entry           = dataQueue->queue;
			callerDataSize  = entry->size;

			if (os_add_overflow(callerDataSize, 3, &dataSize)) {
				return kIOReturnError;
			}
			dataSize &= ~3U;
			if ((dataSize > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) ||
			    (dataSize + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize)) {
				return kIOReturnError;
			}
			newHeadOffset   = dataSize + DATA_QUEUE_ENTRY_HEADER_SIZE;
			// else it is at the end
		} else {
			entry = head;

			if ((headSize > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) ||
			    (headSize + DATA_QUEUE_ENTRY_HEADER_SIZE > UINT32_MAX - headOffset) ||
			    (headSize + DATA_QUEUE_ENTRY_HEADER_SIZE + headOffset > queueSize)) {
				return kIOReturnError;
			}
			newHeadOffset   = headOffset + headSize + DATA_QUEUE_ENTRY_HEADER_SIZE;
		}
	} else {
		// empty queue
		if (dataQueue->needServicedCallback) {
			*sendDataServiced = true;
		}
		return kIOReturnUnderrun;
	}

	callback(&entry->data, callerDataSize);
	if (dataQueue->needServicedCallback) {
		*sendDataServiced = true;
	}

	__c11_atomic_store((_Atomic uint32_t *)&dataQueue->head, newHeadOffset, __ATOMIC_RELEASE);

	if (newHeadOffset == tailOffset) {
		//
		// If we are making the queue empty, then we need to make sure
		// that either the enqueuer notices, or we notice the enqueue
		// that raced with our making of the queue empty.
		//
		__c11_atomic_thread_fence(__ATOMIC_SEQ_CST);
	}

	return retVal;
}

kern_return_t
IODataQueueDispatchSource::Enqueue(uint32_t callerDataSize,
    IODataQueueClientEnqueueEntryBlock callback)
{
	kern_return_t ret;
	bool          sendDataAvailable;

	sendDataAvailable = false;
	ret = EnqueueWithCoalesce(callerDataSize, &sendDataAvailable, callback);
	if (sendDataAvailable) {
		SendDataAvailable();
	}
	return ret;
}

kern_return_t
IODataQueueDispatchSource::EnqueueWithCoalesce(uint32_t callerDataSize,
    bool * sendDataAvailable,
    IODataQueueClientEnqueueEntryBlock callback)
{
	IODataQueueMemory * dataQueue;
	IODataQueueEntry *  entry;
	uint32_t            head;
	uint32_t            tail;
	uint32_t            newTail;
	uint32_t                        dataSize;
	uint32_t            queueSize;
	uint32_t            entrySize;
	IOReturn            retVal = kIOReturnSuccess;

	dataQueue = ivars->dataQueue;
	if (!dataQueue) {
		return kIOReturnNoMemory;
	}
	queueSize = ivars->queueByteCount;

	// Force a single read of head and tail
	tail = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->tail, __ATOMIC_RELAXED);
	head = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->head, __ATOMIC_ACQUIRE);

	if (os_add_overflow(callerDataSize, 3, &dataSize)) {
		return kIOReturnOverrun;
	}
	dataSize &= ~3U;

	// Check for overflow of entrySize
	if (os_add_overflow(DATA_QUEUE_ENTRY_HEADER_SIZE, dataSize, &entrySize)) {
		return kIOReturnOverrun;
	}

	// Check for underflow of (getQueueSize() - tail)
	if (queueSize < tail || queueSize < head) {
		return kIOReturnUnderrun;
	}

	newTail = tail;
	if (tail >= head) {
		// Is there enough room at the end for the entry?
		if ((entrySize <= (UINT32_MAX - tail)) &&
		    ((tail + entrySize) <= queueSize)) {
			entry = (IODataQueueEntry *)((uintptr_t)dataQueue->queue + tail);

			callback(&entry->data, callerDataSize);

			entry->size = callerDataSize;

			// The tail can be out of bound when the size of the new entry
			// exactly matches the available space at the end of the queue.
			// The tail can range from 0 to queueSize inclusive.

			newTail = tail + entrySize;
		} else if (head > entrySize) { // Is there enough room at the beginning?
			entry = (IODataQueueEntry *)((uintptr_t)dataQueue->queue);

			callback(&entry->data, callerDataSize);

			// Wrap around to the beginning, but do not allow the tail to catch
			// up to the head.

			entry->size = callerDataSize;

			// We need to make sure that there is enough room to set the size before
			// doing this. The user client checks for this and will look for the size
			// at the beginning if there isn't room for it at the end.

			if ((queueSize - tail) >= DATA_QUEUE_ENTRY_HEADER_SIZE) {
				((IODataQueueEntry *)((uintptr_t)dataQueue->queue + tail))->size = dataSize;
			}

			newTail = entrySize;
		} else {
			retVal = kIOReturnOverrun; // queue is full
		}
	} else {
		// Do not allow the tail to catch up to the head when the queue is full.
		// That's why the comparison uses a '>' rather than '>='.

		if ((head - tail) > entrySize) {
			entry = (IODataQueueEntry *)((uintptr_t)dataQueue->queue + tail);

			callback(&entry->data, callerDataSize);

			entry->size = callerDataSize;

			newTail = tail + entrySize;
		} else {
			retVal = kIOReturnOverrun; // queue is full
		}
	}

	// Send notification (via mach message) that data is available.

	if (retVal == kIOReturnSuccess) {
		// Publish the data we just enqueued
		__c11_atomic_store((_Atomic uint32_t *)&dataQueue->tail, newTail, __ATOMIC_RELEASE);

		if (tail != head) {
			//
			// The memory barrier below pairs with the one in dequeue
			// so that either our store to the tail cannot be missed by
			// the next dequeue attempt, or we will observe the dequeuer
			// making the queue empty.
			//
			// Of course, if we already think the queue is empty,
			// there's no point paying this extra cost.
			//
			__c11_atomic_thread_fence(__ATOMIC_SEQ_CST);
			head = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->head, __ATOMIC_RELAXED);
		}

		if (tail == head) {
			// Send notification that data is now available.
			*sendDataAvailable = true;
			retVal = kIOReturnSuccess;
		}
	} else if (retVal == kIOReturnOverrun) {
		// ask to be notified of Dequeue()
		dataQueue->needServicedCallback = true;
		*sendDataAvailable = true;
	}

	return retVal;
}
Commit	Line	Data
cb323159 A	1	typedef struct _IODataQueueEntry {
	2	uint32_t size;
	3	uint8_t data[0];
	4	} IODataQueueEntry;
	5
	6	#define DATA_QUEUE_ENTRY_HEADER_SIZE sizeof(IODataQueueEntry)
	7
	8	typedef struct _IODataQueueMemory {
	9	volatile uint32_t head;
	10	volatile uint32_t tail;
	11	volatile uint8_t needServicedCallback;
	12	volatile uint8_t _resv[31];
	13	IODataQueueEntry queue[0];
	14	} IODataQueueMemory;
	15
	16	struct IODataQueueDispatchSource_IVars {
	17	IODataQueueMemory * dataQueue;
	18	IODataQueueDispatchSource * source;
	19	// IODispatchQueue * queue;
	20	IOMemoryDescriptor * memory;
	21	OSAction * dataAvailableAction;
	22	OSAction * dataServicedAction;
	23	uint64_t options;
	24	uint32_t queueByteCount;
	25
	26	#if !KERNEL
	27	bool enable;
	28	bool canceled;
	29	#endif
	30	};
	31
	32	bool
	33	IODataQueueDispatchSource::init()
	34	{
	35	if (!super::init()) {
	36	return false;
	37	}
	38
	39	ivars = IONewZero(IODataQueueDispatchSource_IVars, 1);
	40	ivars->source = this;
	41
	42	#if !KERNEL
	43	kern_return_t ret;
	44
	45	ret = CopyMemory(&ivars->memory);
	46	assert(kIOReturnSuccess == ret);
	47
	48	uint64_t address;
	49	uint64_t length;
	50
	51	ret = ivars->memory->Map(0, 0, 0, 0, &address, &length);
	52	assert(kIOReturnSuccess == ret);
	53	ivars->dataQueue = (typeof(ivars->dataQueue))(uintptr_t) address;
	54	ivars->queueByteCount = length;
	55	#endif
	56
	57	return true;
	58	}
	59
	60	kern_return_t
f427ee49 A	61	IODataQueueDispatchSource::CheckForWork_Impl(
	62	const IORPC rpc,
	63	bool synchronous)
cb323159 A	64	{
	65	IOReturn ret = kIOReturnNotReady;
	66
	67	return ret;
	68	}
	69
	70	#if KERNEL
	71
	72	kern_return_t
f427ee49 A	73	IODataQueueDispatchSource::Create_Impl(
	74	uint64_t queueByteCount,
	75	IODispatchQueue * queue,
	76	IODataQueueDispatchSource ** source)
cb323159 A	77	{
	78	IODataQueueDispatchSource * inst;
	79	IOBufferMemoryDescriptor * bmd;
	80
	81	if (3 & queueByteCount) {
	82	return kIOReturnBadArgument;
	83	}
f427ee49 A	84	if (queueByteCount > UINT_MAX) {
	85	return kIOReturnBadArgument;
	86	}
cb323159 A	87	inst = OSTypeAlloc(IODataQueueDispatchSource);
	88	if (!inst) {
	89	return kIOReturnNoMemory;
	90	}
	91	if (!inst->init()) {
	92	inst->release();
	93	return kIOReturnError;
	94	}
	95
	96	bmd = IOBufferMemoryDescriptor::withOptions(
	97	kIODirectionOutIn \| kIOMemoryKernelUserShared,
	98	queueByteCount, page_size);
	99	if (!bmd) {
	100	inst->release();
	101	return kIOReturnNoMemory;
	102	}
	103	inst->ivars->memory = bmd;
f427ee49	104	inst->ivars->queueByteCount = ((uint32_t) queueByteCount);
cb323159 A	105	inst->ivars->options = 0;
	106	inst->ivars->dataQueue = (typeof(inst->ivars->dataQueue))bmd->getBytesNoCopy();
	107
	108	*source = inst;
	109
	110	return kIOReturnSuccess;
	111	}
	112
	113	kern_return_t
f427ee49 A	114	IODataQueueDispatchSource::CopyMemory_Impl(
f427ee49 A	115	IOMemoryDescriptor ** memory)
cb323159 A	116	{
	117	kern_return_t ret;
	118	IOMemoryDescriptor * result;
	119
	120	result = ivars->memory;
	121	if (result) {
	122	result->retain();
	123	ret = kIOReturnSuccess;
	124	} else {
	125	ret = kIOReturnNotReady;
	126	}
	127	*memory = result;
	128
	129	return ret;
	130	}
	131
	132	kern_return_t
f427ee49 A	133	IODataQueueDispatchSource::CopyDataAvailableHandler_Impl(
f427ee49 A	134	OSAction ** action)
cb323159 A	135	{
	136	kern_return_t ret;
	137	OSAction * result;
	138
	139	result = ivars->dataAvailableAction;
	140	if (result) {
	141	result->retain();
	142	ret = kIOReturnSuccess;
	143	} else {
	144	ret = kIOReturnNotReady;
	145	}
	146	*action = result;
	147
	148	return ret;
	149	}
	150
	151	kern_return_t
f427ee49 A	152	IODataQueueDispatchSource::CopyDataServicedHandler_Impl(
f427ee49 A	153	OSAction ** action)
cb323159 A	154	{
	155	kern_return_t ret;
	156	OSAction * result;
	157
	158	result = ivars->dataServicedAction;
	159	if (result) {
	160	result->retain();
	161	ret = kIOReturnSuccess;
	162	} else {
	163	ret = kIOReturnNotReady;
	164	}
	165	*action = result;
	166	return ret;
	167	}
	168
	169	kern_return_t
f427ee49 A	170	IODataQueueDispatchSource::SetDataAvailableHandler_Impl(
f427ee49 A	171	OSAction * action)
cb323159 A	172	{
	173	IOReturn ret;
	174	OSAction * oldAction;
	175
	176	oldAction = ivars->dataAvailableAction;
	177	if (oldAction && OSCompareAndSwapPtr(oldAction, NULL, &ivars->dataAvailableAction)) {
	178	oldAction->release();
	179	}
	180	if (action) {
	181	action->retain();
	182	ivars->dataAvailableAction = action;
	183	if (IsDataAvailable()) {
	184	DataAvailable(ivars->dataAvailableAction);
	185	}
	186	}
	187	ret = kIOReturnSuccess;
	188
	189	return ret;
	190	}
	191
	192	kern_return_t
f427ee49 A	193	IODataQueueDispatchSource::SetDataServicedHandler_Impl(
f427ee49 A	194	OSAction * action)
cb323159 A	195	{
	196	IOReturn ret;
	197	OSAction * oldAction;
	198
	199	oldAction = ivars->dataServicedAction;
	200	if (oldAction && OSCompareAndSwapPtr(oldAction, NULL, &ivars->dataServicedAction)) {
	201	oldAction->release();
	202	}
	203	if (action) {
	204	action->retain();
	205	ivars->dataServicedAction = action;
	206	}
	207	ret = kIOReturnSuccess;
	208
	209	return ret;
	210	}
	211
	212	#endif /* KERNEL */
	213
	214	void
	215	IODataQueueDispatchSource::SendDataAvailable(void)
	216	{
	217	IOReturn ret;
	218
	219	if (!ivars->dataAvailableAction) {
	220	ret = CopyDataAvailableHandler(&ivars->dataAvailableAction);
	221	if (kIOReturnSuccess != ret) {
	222	ivars->dataAvailableAction = NULL;
	223	}
	224	}
	225	if (ivars->dataAvailableAction) {
	226	DataAvailable(ivars->dataAvailableAction);
	227	}
	228	}
	229
	230	void
	231	IODataQueueDispatchSource::SendDataServiced(void)
	232	{
	233	IOReturn ret;
	234
	235	if (!ivars->dataServicedAction) {
	236	ret = CopyDataServicedHandler(&ivars->dataServicedAction);
	237	if (kIOReturnSuccess != ret) {
	238	ivars->dataServicedAction = NULL;
	239	}
	240	}
	241	if (ivars->dataServicedAction) {
	242	ivars->dataQueue->needServicedCallback = false;
	243	DataServiced(ivars->dataServicedAction);
	244	}
	245	}
	246
	247	kern_return_t
f427ee49 A	248	IODataQueueDispatchSource::SetEnableWithCompletion_Impl(
	249	bool enable,
	250	IODispatchSourceCancelHandler handler)
cb323159 A	251	{
	252	IOReturn ret;
	253
	254	#if !KERNEL
	255	ivars->enable = enable;
	256	#endif
	257
	258	ret = kIOReturnSuccess;
	259	return ret;
	260	}
	261
	262	void
	263	IODataQueueDispatchSource::free()
	264	{
	265	OSSafeReleaseNULL(ivars->memory);
	266	OSSafeReleaseNULL(ivars->dataAvailableAction);
	267	OSSafeReleaseNULL(ivars->dataServicedAction);
	268	IOSafeDeleteNULL(ivars, IODataQueueDispatchSource_IVars, 1);
	269	super::free();
	270	}
	271
	272	kern_return_t
f427ee49 A	273	IODataQueueDispatchSource::Cancel_Impl(
f427ee49 A	274	IODispatchSourceCancelHandler handler)
cb323159 A	275	{
	276	return kIOReturnSuccess;
	277	}
	278
	279	bool
	280	IODataQueueDispatchSource::IsDataAvailable(void)
	281	{
	282	IODataQueueMemory *dataQueue = ivars->dataQueue;
	283
	284	return dataQueue && (dataQueue->head != dataQueue->tail);
	285	}
	286
	287	kern_return_t
	288	IODataQueueDispatchSource::Peek(IODataQueueClientDequeueEntryBlock callback)
	289	{
	290	IODataQueueEntry * entry = NULL;
	291	IODataQueueMemory * dataQueue;
	292	uint32_t callerDataSize;
	293	uint32_t dataSize;
	294	uint32_t headOffset;
	295	uint32_t tailOffset;
	296
	297	dataQueue = ivars->dataQueue;
	298	if (!dataQueue) {
	299	return kIOReturnNoMemory;
	300	}
	301
	302	// Read head and tail with acquire barrier
	303	headOffset = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->head, __ATOMIC_RELAXED);
	304	tailOffset = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->tail, __ATOMIC_ACQUIRE);
	305
	306	if (headOffset != tailOffset) {
	307	IODataQueueEntry * head = NULL;
	308	uint32_t headSize = 0;
	309	uint32_t queueSize = ivars->queueByteCount;
	310
	311	if (headOffset > queueSize) {
	312	return kIOReturnError;
	313	}
	314
	315	head = (IODataQueueEntry *)((uintptr_t)dataQueue->queue + headOffset);
	316	callerDataSize = head->size;
	317	if (os_add_overflow(3, callerDataSize, &headSize)) {
	318	return kIOReturnError;
	319	}
	320	headSize &= ~3U;
	321
	322	// Check if there's enough room before the end of the queue for a header.
	323	// If there is room, check if there's enough room to hold the header and
	324	// the data.
	325
	326	if ((headOffset > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) \|\|
	327	(headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize) \|\|
	328	(headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > UINT32_MAX - headSize) \|\|
	329	(headOffset + headSize + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize)) {
	330	// No room for the header or the data, wrap to the beginning of the queue.
	331	// Note: wrapping even with the UINT32_MAX checks, as we have to support
	332	// queueSize of UINT32_MAX
	333	entry = dataQueue->queue;
	334	callerDataSize = entry->size;
	335	dataSize = entry->size;
	336	if (os_add_overflow(3, callerDataSize, &dataSize)) {
	337	return kIOReturnError;
	338	}
339	dataSize &= ~3U;
340
341	if ((dataSize > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) \|\|
342	(dataSize + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize)) {
343	return kIOReturnError;
344	}
345
346	callback(&entry->data, callerDataSize);
347	return kIOReturnSuccess;
348	} else {
349	callback(&head->data, callerDataSize);
350	return kIOReturnSuccess;
351	}
352	}
353
354	return kIOReturnUnderrun;
355	}
356
357	kern_return_t
358	IODataQueueDispatchSource::Dequeue(IODataQueueClientDequeueEntryBlock callback)
359	{
360	kern_return_t ret;
361	bool sendDataServiced;
362
363	sendDataServiced = false;
364	ret = DequeueWithCoalesce(&sendDataServiced, callback);
365	if (sendDataServiced) {
366	SendDataServiced();
367	}
368	return ret;
369	}
370
371	kern_return_t
372	IODataQueueDispatchSource::DequeueWithCoalesce(bool * sendDataServiced,
373	IODataQueueClientDequeueEntryBlock callback)
374	{
375	IOReturn retVal = kIOReturnSuccess;
376	IODataQueueEntry * entry = NULL;
377	IODataQueueMemory * dataQueue;
378	uint32_t callerDataSize;
379	uint32_t dataSize = 0;
380	uint32_t headOffset = 0;
381	uint32_t tailOffset = 0;
382	uint32_t newHeadOffset = 0;
383
384	dataQueue = ivars->dataQueue;
385	if (!dataQueue) {
386	return kIOReturnNoMemory;
387	}
388
389	// Read head and tail with acquire barrier
390	headOffset = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->head, __ATOMIC_RELAXED);
391	tailOffset = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->tail, __ATOMIC_ACQUIRE);
392
393	if (headOffset != tailOffset) {
394	IODataQueueEntry * head = NULL;
395	uint32_t headSize = 0;
396	uint32_t queueSize = ivars->queueByteCount;
397
398	if (headOffset > queueSize) {
399	return kIOReturnError;
400	}
401
402	head = (IODataQueueEntry *)((uintptr_t)dataQueue->queue + headOffset);
403	callerDataSize = head->size;
404	if (os_add_overflow(3, callerDataSize, &headSize)) {
405	return kIOReturnError;
406	}
407	headSize &= ~3U;
408
409	// we wrapped around to beginning, so read from there
410	// either there was not even room for the header
411	if ((headOffset > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) \|\|
412	(headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize) \|\|
413	// or there was room for the header, but not for the data
414	(headOffset + DATA_QUEUE_ENTRY_HEADER_SIZE > UINT32_MAX - headSize) \|\|
415	(headOffset + headSize + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize)) {
416	// Note: we have to wrap to the beginning even with the UINT32_MAX checks
417	// because we have to support a queueSize of UINT32_MAX.
418	entry = dataQueue->queue;
419	callerDataSize = entry->size;
420
421	if (os_add_overflow(callerDataSize, 3, &dataSize)) {
422	return kIOReturnError;
423	}
424	dataSize &= ~3U;
425	if ((dataSize > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) \|\|
426	(dataSize + DATA_QUEUE_ENTRY_HEADER_SIZE > queueSize)) {
427	return kIOReturnError;
428	}
429	newHeadOffset = dataSize + DATA_QUEUE_ENTRY_HEADER_SIZE;
430	// else it is at the end
431	} else {
432	entry = head;
433
434	if ((headSize > UINT32_MAX - DATA_QUEUE_ENTRY_HEADER_SIZE) \|\|
435	(headSize + DATA_QUEUE_ENTRY_HEADER_SIZE > UINT32_MAX - headOffset) \|\|
436	(headSize + DATA_QUEUE_ENTRY_HEADER_SIZE + headOffset > queueSize)) {
437	return kIOReturnError;
438	}
439	newHeadOffset = headOffset + headSize + DATA_QUEUE_ENTRY_HEADER_SIZE;
440	}
441	} else {
442	// empty queue
443	if (dataQueue->needServicedCallback) {
444	*sendDataServiced = true;
445	}
446	return kIOReturnUnderrun;
447	}
448
449	callback(&entry->data, callerDataSize);
450	if (dataQueue->needServicedCallback) {
451	*sendDataServiced = true;
452	}
453
454	__c11_atomic_store((_Atomic uint32_t *)&dataQueue->head, newHeadOffset, __ATOMIC_RELEASE);
455
456	if (newHeadOffset == tailOffset) {
457	//
458	// If we are making the queue empty, then we need to make sure
459	// that either the enqueuer notices, or we notice the enqueue
460	// that raced with our making of the queue empty.
461	//
462	__c11_atomic_thread_fence(__ATOMIC_SEQ_CST);
463	}
464
465	return retVal;
466	}
467
468	kern_return_t
469	IODataQueueDispatchSource::Enqueue(uint32_t callerDataSize,
470	IODataQueueClientEnqueueEntryBlock callback)
471	{
472	kern_return_t ret;
473	bool sendDataAvailable;
474
475	sendDataAvailable = false;
476	ret = EnqueueWithCoalesce(callerDataSize, &sendDataAvailable, callback);
477	if (sendDataAvailable) {
478	SendDataAvailable();
479	}
480	return ret;
481	}
482
483	kern_return_t
484	IODataQueueDispatchSource::EnqueueWithCoalesce(uint32_t callerDataSize,
485	bool * sendDataAvailable,
486	IODataQueueClientEnqueueEntryBlock callback)
487	{
488	IODataQueueMemory * dataQueue;
489	IODataQueueEntry * entry;
490	uint32_t head;
491	uint32_t tail;
492	uint32_t newTail;
493	uint32_t dataSize;
494	uint32_t queueSize;
495	uint32_t entrySize;
496	IOReturn retVal = kIOReturnSuccess;
497
498	dataQueue = ivars->dataQueue;
499	if (!dataQueue) {
500	return kIOReturnNoMemory;
501	}
502	queueSize = ivars->queueByteCount;
503
504	// Force a single read of head and tail
505	tail = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->tail, __ATOMIC_RELAXED);
506	head = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->head, __ATOMIC_ACQUIRE);
507
508	if (os_add_overflow(callerDataSize, 3, &dataSize)) {
509	return kIOReturnOverrun;
510	}
511	dataSize &= ~3U;
512
513	// Check for overflow of entrySize
514	if (os_add_overflow(DATA_QUEUE_ENTRY_HEADER_SIZE, dataSize, &entrySize)) {
515	return kIOReturnOverrun;
516	}
517
518	// Check for underflow of (getQueueSize() - tail)
519	if (queueSize < tail \|\| queueSize < head) {
520	return kIOReturnUnderrun;
521	}
522
523	newTail = tail;
524	if (tail >= head) {
525	// Is there enough room at the end for the entry?
526	if ((entrySize <= (UINT32_MAX - tail)) &&
527	((tail + entrySize) <= queueSize)) {
528	entry = (IODataQueueEntry *)((uintptr_t)dataQueue->queue + tail);
529
530	callback(&entry->data, callerDataSize);
531
532	entry->size = callerDataSize;
533
534	// The tail can be out of bound when the size of the new entry
535	// exactly matches the available space at the end of the queue.
536	// The tail can range from 0 to queueSize inclusive.
537
538	newTail = tail + entrySize;
539	} else if (head > entrySize) { // Is there enough room at the beginning?
540	entry = (IODataQueueEntry *)((uintptr_t)dataQueue->queue);
541
542	callback(&entry->data, callerDataSize);
543
544	// Wrap around to the beginning, but do not allow the tail to catch
545	// up to the head.
546
547	entry->size = callerDataSize;
548
549	// We need to make sure that there is enough room to set the size before
550	// doing this. The user client checks for this and will look for the size
551	// at the beginning if there isn't room for it at the end.
552
553	if ((queueSize - tail) >= DATA_QUEUE_ENTRY_HEADER_SIZE) {
554	((IODataQueueEntry *)((uintptr_t)dataQueue->queue + tail))->size = dataSize;
555	}
556
557	newTail = entrySize;
558	} else {
559	retVal = kIOReturnOverrun; // queue is full
560	}
561	} else {
562	// Do not allow the tail to catch up to the head when the queue is full.
563	// That's why the comparison uses a '>' rather than '>='.
564
565	if ((head - tail) > entrySize) {
566	entry = (IODataQueueEntry *)((uintptr_t)dataQueue->queue + tail);
567
568	callback(&entry->data, callerDataSize);
569
570	entry->size = callerDataSize;
571
572	newTail = tail + entrySize;
573	} else {
574	retVal = kIOReturnOverrun; // queue is full
575	}
576	}
577
578	// Send notification (via mach message) that data is available.
579
580	if (retVal == kIOReturnSuccess) {
581	// Publish the data we just enqueued
582	__c11_atomic_store((_Atomic uint32_t *)&dataQueue->tail, newTail, __ATOMIC_RELEASE);
583
584	if (tail != head) {
585	//
586	// The memory barrier below pairs with the one in dequeue
587	// so that either our store to the tail cannot be missed by
588	// the next dequeue attempt, or we will observe the dequeuer
589	// making the queue empty.
590	//
591	// Of course, if we already think the queue is empty,
592	// there's no point paying this extra cost.
593	//
594	__c11_atomic_thread_fence(__ATOMIC_SEQ_CST);
595	head = __c11_atomic_load((_Atomic uint32_t *)&dataQueue->head, __ATOMIC_RELAXED);
596	}
597
598	if (tail == head) {
599	// Send notification that data is now available.
600	*sendDataAvailable = true;
601	retVal = kIOReturnSuccess;
602	}
603	} else if (retVal == kIOReturnOverrun) {
604	// ask to be notified of Dequeue()
605	dataQueue->needServicedCallback = true;
606	*sendDataAvailable = true;
607	}
608
609	return retVal;
610	}