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