[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
IMPL(IODataQueueDispatchSource, CheckForWork)
{
	IOReturn ret = kIOReturnNotReady;

	return ret;
}

#if KERNEL

kern_return_t
IMPL(IODataQueueDispatchSource, Create)
{
	IODataQueueDispatchSource * inst;
	IOBufferMemoryDescriptor  * bmd;

	if (3 & queueByteCount) {
		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 = queueByteCount;
	inst->ivars->options        = 0;
	inst->ivars->dataQueue      = (typeof(inst->ivars->dataQueue))bmd->getBytesNoCopy();

	*source = inst;

	return kIOReturnSuccess;
}

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