1/*
2 * Copyright (c) 1998-2014 Apple Computer, Inc. All rights reserved.
3 *
4 * @APPLE_LICENSE_HEADER_START@
5 *
6 * The contents of this file constitute Original Code as defined in and
7 * are subject to the Apple Public Source License Version 1.1 (the
8 * "License").  You may not use this file except in compliance with the
9 * License.  Please obtain a copy of the License at
10 * http://www.apple.com/publicsource and read it before using this file.
11 *
12 * This Original Code and all software distributed under the License are
13 * distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY KIND, EITHER
14 * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
15 * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
16 * FITNESS FOR A PARTICULAR PURPOSE OR NON-INFRINGEMENT.  Please see the
17 * License for the specific language governing rights and limitations
18 * under the License.
19 *
20 * @APPLE_LICENSE_HEADER_END@
21 */
22
23#include <IOKit/IOLib.h>
24#include <libkern/c++/OSObject.h>
25#include "IOAudioTimeIntervalFilter.h"
26
27#define super OSObject
28
29OSDefineMetaClassAndAbstractStructors(IOAudioTimeIntervalFilter, OSObject)
30
31OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 0);
32OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 1);
33OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 2);
34OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 3);
35OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 4);
36OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 5);
37OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 6);
38OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 7);
39OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 8);
40OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 9);
41OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 10);
42OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 11);
43OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 12);
44OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 13);
45OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 14);
46OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilter, 15);
47
48bool IOAudioTimeIntervalFilter::initFilter(uint32_t expectedInterval, uint32_t multiIntervalCount /* =1 */)
49{
50	bool result = false;
51	mExpectedInterval = expectedInterval;
52	mMultiIntervalCount = multiIntervalCount+1;
53	mIntervalTimeHistoryPointer = 0;
54	mFilterCount = 0;
55
56	if ( super::init() )
57	{
58		mIntervalTimeHistory = (uint64_t*) IOMalloc ( mMultiIntervalCount * sizeof(uint64_t) );
59		if ( NULL == mIntervalTimeHistory ) goto Exit;
60
61		timeIntervalLock = IOLockAlloc();
62		if ( NULL == timeIntervalLock ) goto Exit;
63	}
64
65	result = true;
66Exit:
67	return result;
68}
69
70
71void IOAudioTimeIntervalFilter::free()
72{
73	if ( timeIntervalLock )
74	{
75		IOLockFree ( timeIntervalLock );
76		timeIntervalLock = NULL;
77	}
78	if ( mIntervalTimeHistory )
79	{
80		IOFree ( mIntervalTimeHistory, mMultiIntervalCount * sizeof(uint64_t) );
81	}
82
83	super::free();
84}
85
86IOReturn IOAudioTimeIntervalFilter::reInitialiseFilter(uint32_t expectedInterval  /* =0 */, uint32_t multiIntervalCount /* =1 */)
87{
88	IOReturn result = kIOReturnError;
89
90	if ( NULL == timeIntervalLock ) goto Exit;
91	IOLockLock ( timeIntervalLock );
92
93	if ( expectedInterval )
94	{
95		mExpectedInterval = expectedInterval;
96	}
97	else
98	{
99		// If the user didn't supply an expected interval, they are assuming the interval
100		// has remained unchanged since the last time the filter was run
101
102		// We can get our last interval through the history buffer
103
104		if ( mFilterCount > 1 )
105		{
106			mExpectedInterval = mIntervalTimeHistory [ decCircularBufferPosition ( mIntervalTimeHistoryPointer ) ] - mIntervalTimeHistory [ decCircularBufferPosition ( mIntervalTimeHistoryPointer, 2 ) ];
107		}
108	}
109
110	mIntervalTimeHistoryPointer = 0;
111	mFilterCount = 0;
112
113	if ( mIntervalTimeHistory )
114	{
115		IOFree ( mIntervalTimeHistory, mMultiIntervalCount * sizeof(uint64_t));
116		mIntervalTimeHistory = NULL;
117	}
118
119	mMultiIntervalCount = multiIntervalCount + 1;
120
121	mIntervalTimeHistory = (uint64_t*) IOMalloc ( mMultiIntervalCount * sizeof(uint64_t) );
122	if ( NULL == mIntervalTimeHistory ) goto Exit;
123
124	result = kIOReturnSuccess;
125
126Exit:
127	if ( timeIntervalLock )
128	{
129		IOLockUnlock ( timeIntervalLock );
130	}
131
132	return result;
133}
134
135
136
137AbsoluteTime IOAudioTimeIntervalFilter::newTimePosition(AbsoluteTime rawSnapshotAT)
138{
139	int			n;
140	uint64_t	rawSnapshot = __OSAbsoluteTime(rawSnapshotAT);
141	uint64_t	filteredSnapshot = 0;
142	int			prevPointer;
143
144	if ( NULL == timeIntervalLock ) goto Exit;
145	if ( NULL == mIntervalTimeHistory ) goto Exit;
146
147	IOLockLock ( timeIntervalLock );
148
149	prevPointer = mIntervalTimeHistoryPointer;
150
151	if ( 0 == mFilterCount )
152	{
153		// The first iteration requires priming of the history
154		mIntervalTimeHistory[mIntervalTimeHistoryPointer] = rawSnapshot;
155		prevPointer = mIntervalTimeHistoryPointer;
156
157		for (n=0; n<mMultiIntervalCount-1; n++)
158		{
159			int prevPrevPointer = decCircularBufferPosition(prevPointer);
160
161			mIntervalTimeHistory[prevPrevPointer] = mIntervalTimeHistory[prevPointer] - mExpectedInterval;
162			prevPointer = prevPrevPointer;
163		}
164	}
165
166
167	filteredSnapshot = calculateNewTimePosition ( rawSnapshot );
168
169	// Save the data in our history buffer
170	mIntervalTimeHistory[mIntervalTimeHistoryPointer] = filteredSnapshot;
171
172	// Increment our pointer
173	mIntervalTimeHistoryPointer = incCircularBufferPosition( mIntervalTimeHistoryPointer );
174
175	mFilterCount++;
176
177	IOLockUnlock ( timeIntervalLock );
178
179Exit:
180	return *((AbsoluteTime*) &filteredSnapshot);
181}
182
183
184uint64_t IOAudioTimeIntervalFilter::getMultiIntervalTime(void)
185{
186	uint64_t value = 0;
187
188	if ( NULL == timeIntervalLock ) goto Exit;
189	if ( NULL == mIntervalTimeHistory ) goto Exit;
190
191	IOLockLock ( timeIntervalLock );
192
193	value = mIntervalTimeHistory [ decCircularBufferPosition ( mIntervalTimeHistoryPointer ) ] - mIntervalTimeHistory [ mIntervalTimeHistoryPointer ];
194
195	IOLockUnlock ( timeIntervalLock );
196
197Exit:
198	return value;
199}
200
201
202
203
204
205
206#pragma mark --
207#pragma mark IIR
208
209#undef super
210#define super IOAudioTimeIntervalFilter
211
212OSDefineMetaClassAndStructors(IOAudioTimeIntervalFilterIIR, IOAudioTimeIntervalFilter)
213
214OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 0);
215OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 1);
216OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 2);
217OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 3);
218OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 4);
219OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 5);
220OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 6);
221OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 7);
222OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 8);
223OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 9);
224OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 10);
225OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 11);
226OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 12);
227OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 13);
228OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 14);
229OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterIIR, 15);
230
231bool IOAudioTimeIntervalFilterIIR::initFilter(uint32_t expectedInterval, uint32_t multiIntervalCount /* =1 */, uint16_t filterCoef /* =4 */)
232{
233	bool result = IOAudioTimeIntervalFilter::initFilter(expectedInterval, multiIntervalCount);
234
235	if ( result )
236	{
237		mIIRCoef = filterCoef;
238	}
239
240	return result;
241}
242
243
244uint64_t IOAudioTimeIntervalFilterIIR::calculateNewTimePosition(uint64_t rawSnapshot)
245{
246	const uint64_t	offset = uint64_t(mExpectedInterval) << mIIRCoef;
247	uint64_t		filteredSnapshot;
248
249	// Because our filter is initialised with a value prior to the rawSnapshot, there is a possibility that
250	// there will be a negative number in our filter. The present math library does not support
251	// signed numbers, we add an offset to our snapshot, and remove it from the resulting calculations
252
253	rawSnapshot += offset;
254
255	if ( 0 == mFilterCount )
256	{
257		// Initialise the filtered snapshot filter.
258		mFilteredSnapshot = (U128(rawSnapshot) -  ( U128( mExpectedInterval ) << mIIRCoef )) << mIIRCoef;
259		IIR( &mFilteredSnapshot, U128(rawSnapshot) << mIIRCoef, mIIRCoef );
260
261		U128 raw_offset = ( U128(rawSnapshot) << mIIRCoef ) - mFilteredSnapshot;
262
263		// Intialise the filtered offset
264		mFilteredOffset = UInt64mult(mExpectedInterval, ( 1 << mIIRCoef ) - 1 ) << mIIRCoef;
265
266		IIR( &mFilteredOffset, raw_offset, mIIRCoef );
267		filteredSnapshot = ((mFilteredSnapshot + mFilteredOffset) >> mIIRCoef).lo;
268	}
269	else
270	{
271		IIR( &mFilteredSnapshot, U128(rawSnapshot) << mIIRCoef, mIIRCoef );
272
273		U128 raw_offset = ( U128(rawSnapshot) << mIIRCoef ) - mFilteredSnapshot;
274
275		IIR( &mFilteredOffset, raw_offset, mIIRCoef );
276		filteredSnapshot = ( (mFilteredSnapshot + mFilteredOffset) >> mIIRCoef ).lo;
277	}
278
279	filteredSnapshot -= offset;
280
281	return filteredSnapshot;
282}
283
284
285void IOAudioTimeIntervalFilterIIR::IIR(U128* filterVal, U128 input, int shift)
286{
287	U128 x, y;
288
289	// IIR of the form:
290	//
291	// filterVal = ( (2^shiftAmount - 1) / 2^shiftAmount) * filterVal + (1 / 2^shiftAmount) * input
292	//
293
294	x =  *filterVal >> shift;
295	y = input >> shift;
296	*filterVal = *filterVal - x + y;
297}
298
299
300#pragma mark --
301#pragma mark FIR
302
303#undef super
304#define super IOAudioTimeIntervalFilter
305
306
307OSDefineMetaClassAndStructors(IOAudioTimeIntervalFilterFIR, IOAudioTimeIntervalFilter)
308
309OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 0);
310OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 1);
311OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 2);
312OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 3);
313OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 4);
314OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 5);
315OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 6);
316OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 7);
317OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 8);
318OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 9);
319OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 10);
320OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 11);
321OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 12);
322OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 13);
323OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 14);
324OSMetaClassDefineReservedUnused(IOAudioTimeIntervalFilterFIR, 15);
325
326
327bool IOAudioTimeIntervalFilterFIR::initFilter(uint32_t expectedInterval, uint32_t multiIntervalCount /* =1 */)
328{
329	bool result;
330	static const uint64_t filterCoefficients[] = {1, 2, 4, 7, 10, 14, 19, 25, 31, 37, 43, 49, 54, 58, 62, 64, 64, 64, 62, 58, 54, 49, 43, 37, 31, 25, 19, 14, 10, 7, 4, 2, 1};
331
332	mDataHistory = NULL;
333	mDataOffsetHistory = NULL;
334	mNumCoeffs = NULL;
335
336	result = IOAudioTimeIntervalFilter::initFilter(expectedInterval, multiIntervalCount);
337
338	if ( result )
339	{
340		// For now, initialise the filter to our default
341		mNumCoeffs = sizeof(filterCoefficients)/sizeof(filterCoefficients[0]);
342
343		if ( kIOReturnSuccess != setNewFilter( mNumCoeffs, filterCoefficients, 10) )
344			result = FALSE;
345
346		mFilterWritePointer = 0;
347	}
348
349	return result;
350}
351
352IOReturn IOAudioTimeIntervalFilterFIR::setNewFilter(uint32_t numCoeffs, const uint64_t* filterCoefficients, uint32_t scale)
353{
354	IOReturn result = kIOReturnError;
355
356	// Free up the previous buffers
357	if ( mDataHistory )
358	{
359		IOFree ( mDataHistory, mNumCoeffs * sizeof(uint64_t) );
360		mDataHistory = NULL;
361	}
362	if ( mDataOffsetHistory )
363	{
364		IOFree ( mDataOffsetHistory, mNumCoeffs * sizeof(uint64_t) );
365		mDataOffsetHistory = NULL;
366	}
367	if ( mCoeffs )
368	{
369		IOFree ( mCoeffs, mNumCoeffs * sizeof(uint64_t) );
370		mCoeffs = NULL;
371	}
372
373	mNumCoeffs = numCoeffs;
374
375	mCoeffs = (uint64_t*) IOMalloc ( mNumCoeffs * sizeof(uint64_t) );
376	if ( NULL == mCoeffs) goto Exit;
377
378	memcpy(mCoeffs, filterCoefficients,  mNumCoeffs * sizeof(uint64_t));
379	mFilterScale = scale;
380
381	mDataHistory = (uint64_t*) IOMalloc ( mNumCoeffs * sizeof(uint64_t) );
382	if ( NULL == mDataHistory) goto Exit;
383
384	mDataOffsetHistory = (uint64_t*) IOMalloc ( mNumCoeffs * sizeof(uint64_t) );
385	if ( NULL == mDataHistory) goto Exit;
386
387	reInitialiseFilter ( mExpectedInterval, mMultiIntervalCount );
388
389	result = kIOReturnSuccess;
390Exit:
391	return result;
392}
393
394IOReturn IOAudioTimeIntervalFilterFIR::reInitialiseFilter(uint32_t expectedInterval /* =0 */, uint32_t multiIntervalCount /* =1 */)
395{
396	IOReturn result = IOAudioTimeIntervalFilter::reInitialiseFilter ( expectedInterval, multiIntervalCount );
397
398	mFilterWritePointer = 0;
399
400	return result;
401}
402
403
404void IOAudioTimeIntervalFilterFIR::free()
405{
406	if ( mDataHistory )
407	{
408		IOFree ( mDataHistory, mNumCoeffs * sizeof(uint64_t) );
409		mDataHistory = NULL;
410	}
411	if ( mDataOffsetHistory )
412	{
413		IOFree ( mDataOffsetHistory, mNumCoeffs * sizeof(uint64_t) );
414		mDataOffsetHistory = NULL;
415	}
416	if ( mCoeffs )
417	{
418		IOFree ( mCoeffs, mNumCoeffs * sizeof(uint64_t) );
419		mCoeffs = NULL;
420	}
421
422	IOAudioTimeIntervalFilter::free();
423}
424
425
426uint64_t IOAudioTimeIntervalFilterFIR::calculateNewTimePosition(uint64_t rawSnapshot)
427{
428	unsigned int	n;
429	U128			filteredSnapshot;
430	U128			filteredInterval;
431	uint64_t		filteredSnapshotFinal;
432
433	if ( 0 == mFilterCount )
434	{
435		// Initialise the filtered snapshot filter.
436		for ( n = 0; n < mNumCoeffs; n++ )
437		{
438			mDataOffsetHistory [ ( mNumCoeffs - n ) % mNumCoeffs ] = (rawSnapshot - UInt64mult(n, mExpectedInterval)).lo;
439		}
440
441		filteredSnapshot = FIR( mDataOffsetHistory, rawSnapshot );
442
443		U128 raw_offset = rawSnapshot - filteredSnapshot;
444
445		// Intialise the filtered offset
446		for ( n = 0; n < mNumCoeffs; n++ )
447		{
448			mDataHistory [ ( mNumCoeffs - n ) % mNumCoeffs ] = uint64_t(mExpectedInterval) * ( mNumCoeffs / 2);
449		}
450
451		filteredInterval = FIR( mDataHistory, raw_offset.lo );
452		filteredSnapshotFinal = (filteredSnapshot + filteredInterval).lo;
453	}
454	else
455	{
456		filteredSnapshot = FIR( mDataOffsetHistory, rawSnapshot );
457
458		U128 raw_offset = rawSnapshot - filteredSnapshot;
459
460		filteredInterval = FIR( mDataHistory, raw_offset.lo );
461		filteredSnapshotFinal = (filteredSnapshot + filteredInterval).lo;
462	}
463
464	// Update the write pointer for the next iteration
465	mFilterWritePointer = ( mFilterWritePointer + mNumCoeffs + 1 ) % mNumCoeffs;
466
467	return filteredSnapshotFinal;
468}
469
470
471U128 IOAudioTimeIntervalFilterFIR::FIR(uint64_t *history, uint64_t input)
472{
473	U128 result128(0);
474	unsigned int n;
475
476	history [ mFilterWritePointer ] = input;
477
478	for ( n = 0; n < mNumCoeffs; n++ )
479	{
480		result128 += UInt64mult ( mCoeffs [ n ] , history [ ( mNumCoeffs + mFilterWritePointer - n ) % mNumCoeffs ] );
481	}
482
483	return result128 >> mFilterScale;
484}
485
486
487
488