IOFWIsochChannel.cpp [plain text]
#include <IOKit/assert.h>
#include <IOKit/firewire/IOFireWireController.h>
#include <IOKit/firewire/IOFWIsochChannel.h>
#include <IOKit/firewire/IOFWLocalIsochPort.h>
#include <IOKit/firewire/IOFWCommand.h>
#include <IOKit/firewire/IOFireWireDevice.h>
#import <IOKit/firewire/IOFWDCLProgram.h>
#import <IOKit/firewire/IOFireWireLink.h>
#include <libkern/c++/OSSet.h>
#include <libkern/c++/OSCollectionIterator.h>
#include "FWDebugging.h"
OSDefineMetaClassAndStructors(IOFWIsochChannel, OSObject)
OSMetaClassDefineReservedUnused(IOFWIsochChannel, 0);
OSMetaClassDefineReservedUnused(IOFWIsochChannel, 1);
OSMetaClassDefineReservedUnused(IOFWIsochChannel, 2);
OSMetaClassDefineReservedUnused(IOFWIsochChannel, 3);
#pragma mark -
bool IOFWIsochChannel::init( IOFireWireController * control,
bool doIRM,
UInt32 packetSize,
IOFWSpeed prefSpeed,
ForceStopNotificationProc * stopProc,
void * stopRefCon )
{
bool success = true;
FWKLOG(( "IOFWIsochChannel::init - doIRM = %d\n", doIRM ));
success = OSObject::init();
if( success )
{
fControl = control;
fDoIRM = doIRM;
fPacketSize = packetSize;
fPrefSpeed = prefSpeed;
fStopProc = stopProc;
fStopRefCon = stopRefCon;
fTalker = NULL;
fChannel = 64; fBandwidth = 0;
fLock = IOLockAlloc();
if( fLock == NULL )
{
success = false;
}
}
if( success )
{
fListeners = OSSet::withCapacity( 1 );
if( fListeners == NULL )
{
success = false;
}
}
if( success )
{
fReadCmd = new IOFWReadQuadCommand;
if( fReadCmd == NULL )
{
success = false;
}
}
if( success )
{
fReadCmd->initAll( fControl, 0, FWAddress(), NULL, 0, NULL, NULL );
}
if( success )
{
fLockCmd = new IOFWCompareAndSwapCommand;
if( fLockCmd == NULL )
{
success = false;
}
}
if( success )
{
fLockCmd->initAll( fControl, 0, FWAddress(), NULL, NULL, 0, NULL, NULL );
}
return success;
}
void IOFWIsochChannel::free()
{
if( fListeners )
{
fListeners->release();
fListeners = NULL;
}
if( fReadCmd )
{
fReadCmd->release();
fReadCmd = NULL;
}
if( fLockCmd )
{
fLockCmd->release();
fLockCmd = NULL;
}
if( fLock )
{
IOLockFree( fLock );
fLock = NULL;
}
OSObject::free();
}
#pragma mark -
IOReturn IOFWIsochChannel::setTalker( IOFWIsochPort *talker )
{
fTalker = talker;
IOFWLocalIsochPort * localIsochPort = OSDynamicCast( IOFWLocalIsochPort, talker ) ;
if ( localIsochPort )
{
IODCLProgram * program = localIsochPort->getProgramRef() ;
program->setForceStopProc( fStopProc, fStopRefCon, this ) ;
program->release() ;
}
return kIOReturnSuccess;
}
IOReturn IOFWIsochChannel::addListener( IOFWIsochPort *listener )
{
IOReturn error = kIOReturnSuccess;
if( !fListeners->setObject( listener ) )
{
error = kIOReturnNoMemory;
}
if ( !error )
{
IOFWLocalIsochPort * localIsochPort = OSDynamicCast( IOFWLocalIsochPort, listener ) ;
if ( localIsochPort )
{
IODCLProgram * program = localIsochPort->getProgramRef() ;
program->setForceStopProc( fStopProc, fStopRefCon, this ) ;
program->release() ;
}
}
return error ;
}
IOReturn IOFWIsochChannel::start()
{
OSIterator *listenIterator;
IOFWIsochPort *listen;
IOReturn error = kIOReturnSuccess ;
listenIterator = OSCollectionIterator::withCollection( fListeners );
if( listenIterator )
{
listenIterator->reset();
while( !error && (listen = (IOFWIsochPort *)listenIterator->getNextObject()) )
{
error = listen->start();
}
listenIterator->release();
}
if( !error && fTalker )
{
error = fTalker->start();
}
return error ;
}
IOReturn IOFWIsochChannel::stop()
{
OSIterator *listenIterator;
IOFWIsochPort *listen;
listenIterator = OSCollectionIterator::withCollection( fListeners );
if( listenIterator )
{
listenIterator->reset();
while( (listen = (IOFWIsochPort *)listenIterator->getNextObject()) )
{
listen->stop();
}
listenIterator->release();
}
if( fTalker )
{
fTalker->stop();
}
return kIOReturnSuccess;
}
#pragma mark -
IOReturn IOFWIsochChannel::allocateChannel()
{
IOReturn status = kIOReturnSuccess;
IOFWIsochPort * listen = NULL;
OSIterator * listenIterator = NULL;
if( status == kIOReturnSuccess )
{
UInt64 portChans;
IOFWSpeed portSpeed;
UInt64 allowedChans;
IOFWSpeed speed;
speed = fPrefSpeed;
allowedChans = ~(UInt64)0;
if( fTalker )
{
fTalker->getSupported( portSpeed, portChans );
if( portSpeed < speed )
{
speed = portSpeed;
}
allowedChans &= portChans;
}
listenIterator = OSCollectionIterator::withCollection( fListeners );
if( listenIterator )
{
while( (listen = (IOFWIsochPort *)listenIterator->getNextObject()) )
{
listen->getSupported( portSpeed, portChans );
if( portSpeed < speed )
{
speed = portSpeed;
}
allowedChans &= portChans;
}
}
status = allocateChannelBegin( speed, allowedChans );
}
if( status == kIOReturnSuccess )
{
if( listenIterator )
{
listenIterator->reset();
while( (listen = (IOFWIsochPort *)listenIterator->getNextObject()) &&
(status == kIOReturnSuccess) )
{
status = listen->allocatePort( fSpeed, fChannel );
}
}
}
if( status == kIOReturnSuccess )
{
if( fTalker )
{
status = fTalker->allocatePort( fSpeed, fChannel );
}
}
if( listenIterator )
{
listenIterator->release();
}
if( status != kIOReturnSuccess )
{
releaseChannel();
}
return status;
}
IOReturn IOFWIsochChannel::allocateChannelBegin( IOFWSpeed inSpeed,
UInt64 inAllowedChans,
UInt32 * outChannel )
{
IOReturn status = kIOReturnSuccess;
UInt32 channel = 0;
FWKLOG(( "IOFWIsochChannel::allocateChannelBegin - entered, inSpeed = %d, fDoIRM = %d, inAllowedChans = 0x%016llx\n", inSpeed, fDoIRM, inAllowedChans ));
IOLockLock( fLock );
fSpeed = ( inSpeed == kFWSpeedMaximum ) ? fControl->getLink()->getPhySpeed() : inSpeed ;
if( fDoIRM )
{
UInt32 generation;
UInt32 newVal;
FWAddress addr(kCSRRegisterSpaceBaseAddressHi, kCSRBandwidthAvailable);
UInt32 oldIRM[3];
do
{
UInt32 bandwidth = (fPacketSize/4 + 3) * 16 / (1 << inSpeed);
fControl->getIRMNodeID( generation, addr.nodeID );
FWKLOG(( "IOFWIsochChannel::allocateChannelBegin - generation %d\n", generation ));
fReadCmd->reinit( generation, addr, oldIRM, 3 );
fReadCmd->setMaxPacket( 4 ); status = fReadCmd->submit();
if( (status == kIOReturnSuccess) && (oldIRM[0] < bandwidth) )
{
status = kIOReturnNoSpace;
}
if( status == kIOReturnSuccess )
{
newVal = oldIRM[0] - bandwidth;
fLockCmd->reinit( generation, addr, &oldIRM[0], &newVal, 1 );
status = fLockCmd->submit();
if ( (status == kIOReturnSuccess) && !fLockCmd->locked( &oldIRM[0] ) )
{
status = kIOReturnCannotLock;
}
if( status == kIOReturnSuccess )
{
FWKLOG(( "IOFWIsochChannel::allocateChannelBegin - allocated bandwidth = %d\n", bandwidth ));
fBandwidth = bandwidth;
}
}
if( status == kIOReturnSuccess )
{
inAllowedChans &= (UInt64)(oldIRM[2]) | ((UInt64)oldIRM[1] << 32);
}
if( status == kIOReturnSuccess )
{
for( channel = 0; channel < 64; channel++ )
{
if( inAllowedChans & ((UInt64)1 << (63 - channel)) )
{
break;
}
}
if( channel == 64 )
{
status = kIOReturnNoResources;
}
}
if( status == kIOReturnSuccess )
{
UInt32* oldPtr;
if( channel < 32 )
{
addr.addressLo = kCSRChannelsAvailable31_0;
oldPtr = &oldIRM[1];
newVal = *oldPtr & ~(1<<(31 - channel));
}
else
{
addr.addressLo = kCSRChannelsAvailable63_32;
oldPtr = &oldIRM[2];
newVal = *oldPtr & ~((UInt64)1 << (63 - channel));
}
fLockCmd->reinit( generation, addr, oldPtr, &newVal, 1 );
status = fLockCmd->submit();
if( !status && !fLockCmd->locked(oldPtr) )
{
status = kIOReturnCannotLock;
}
}
if( status == kIOReturnSuccess )
{
FWKLOG(( "IOFWIsochChannel::allocateChannelBegin - allocated channel = %d\n", channel ));
fChannel = channel;
if( outChannel != NULL )
{
*outChannel = channel;
}
}
if( status == kIOReturnSuccess )
{
fGeneration = generation;
fControl->addAllocatedChannel(this);
}
}
while( (status == kIOFireWireBusReset) || (status == kIOReturnCannotLock) );
}
else
{
if( status == kIOReturnSuccess )
{
for( channel = 0; channel < 64; channel++ )
{
if( inAllowedChans & ((UInt64)1 << (63 - channel)) )
{
break;
}
}
if( channel == 64 )
{
status = kIOReturnNoResources;
}
}
if( status == kIOReturnSuccess )
{
FWKLOG(( "IOFWIsochChannel::allocateChannelBegin - allocated channel = %d\n", channel ));
fChannel = channel;
if( outChannel != NULL )
{
*outChannel = channel;
}
}
}
IOLockUnlock( fLock );
FWKLOG(( "IOFWIsochChannel::allocateChannelBegin - exited with status = 0x%08lx\n", status ));
return status;
}
struct ChannelThreadInfo
{
IOFWIsochChannel * fChannel;
UInt32 fGeneration;
};
void IOFWIsochChannel::handleBusReset()
{
ChannelThreadInfo * threadInfo = (ChannelThreadInfo *)IOMalloc( sizeof(ChannelThreadInfo) );
if( threadInfo )
{
threadInfo->fGeneration = fControl->getGeneration();
threadInfo->fChannel = this;
retain(); IOCreateThread( threadFunc, threadInfo );
}
}
void IOFWIsochChannel::threadFunc( void * arg )
{
ChannelThreadInfo * threadInfo = (ChannelThreadInfo *)arg;
IOFWIsochChannel * channel = threadInfo->fChannel;
UInt32 generation = threadInfo->fGeneration;
channel->reallocBandwidth( generation );
IOFree( threadInfo, sizeof(threadInfo) );
channel->release(); }
void IOFWIsochChannel::reallocBandwidth( UInt32 generation )
{
IOReturn result = kIOReturnSuccess;
FWAddress addr( kCSRRegisterSpaceBaseAddressHi, kCSRBandwidthAvailable );
IOLockLock( fLock );
FWKLOG(( "IOFWIsochChannel::reallocBandwidth - bandwidth %d, channel = %d\n", fBandwidth, fChannel ));
if( result == kIOReturnSuccess )
{
UInt32 current_generation;
UInt16 irm;
fControl->getIRMNodeID( current_generation, irm );
addr.nodeID = irm;
if( current_generation != generation )
{
result = kIOFireWireBusReset;
}
FWKLOG(( "IOFWIsochChannel::reallocBandwidth - realloc generation %d, current generation = %d\n", generation, current_generation ));
}
if( result == kIOReturnSuccess )
{
if( fGeneration == generation )
{
result = kIOReturnError;
}
}
if( fBandwidth != 0 )
{
UInt32 oldVal = 0;
if( result == kIOReturnSuccess )
{
fReadCmd->reinit( generation, addr, &oldVal, 1 );
result = fReadCmd->submit();
}
bool done = false;
while( (result == kIOReturnSuccess) && !done )
{
UInt32 newVal = 0;
if( oldVal < fBandwidth )
{
result = kIOReturnNoSpace;
}
if( result == kIOReturnSuccess )
{
newVal = oldVal - fBandwidth;
fLockCmd->reinit( generation, addr, &oldVal, &newVal, 1 );
result = fLockCmd->submit();
}
if( result == kIOReturnSuccess )
{
done = fLockCmd->locked(&oldVal);
}
}
if( result == kIOReturnNoSpace )
{
FWKLOG(( "IOFWIsochChannel::reallocBandwidth - failed to reallocate bandwidth = %d, channel = %d\n", fBandwidth, fChannel ));
fBandwidth = 0;
fChannel = 64; }
else
{
FWKLOG(( "IOFWIsochChannel::reallocBandwidth - reallocated bandwidth = %d\n", fBandwidth ));
}
}
if( fChannel != 64 )
{
UInt32 mask = 0;
UInt32 oldVal = 0;
if( result == kIOReturnSuccess )
{
if( fChannel <= 31 )
{
addr.addressLo = kCSRChannelsAvailable31_0;
mask = 1 << (31-fChannel);
}
else
{
addr.addressLo = kCSRChannelsAvailable63_32;
mask = 1 << (63-fChannel);
}
fReadCmd->reinit( generation, addr, &oldVal, 1 );
result = fReadCmd->submit();
}
bool done = false;
while( (result == kIOReturnSuccess) && !done )
{
UInt32 newVal = oldVal & ~mask;
if( newVal == oldVal )
{
result = kIOFireWireChannelNotAvailable ;
}
if( result == kIOReturnSuccess )
{
fLockCmd->reinit( generation, addr, &oldVal, &newVal, 1 );
result = fLockCmd->submit();
}
if( result == kIOReturnSuccess )
{
done = fLockCmd->locked( &oldVal );
}
}
if( result == kIOFireWireChannelNotAvailable )
{
FWKLOG(( "IOFWIsochChannel::reallocBandwidth - failed to reallocate channel = %d\n", fChannel ));
fChannel = 64;
}
else
{
FWKLOG(( "IOFWIsochChannel::reallocBandwidth - reallocated channel = %d\n", fChannel ));
}
}
fGeneration = generation;
FWKLOG(( "IOFWIsochChannel::reallocBandwidth - exited with result = 0x%08lx\n", result ));
IOLockUnlock( fLock );
if( result == kIOReturnNoSpace || result == kIOFireWireChannelNotAvailable )
{
stop();
releaseChannel();
if ( fStopProc )
{
(*fStopProc)( fStopRefCon, this, kIOFireWireChannelNotAvailable );
}
}
}
IOReturn IOFWIsochChannel::releaseChannel()
{
OSIterator *listenIterator;
IOFWIsochPort *listen;
if( fTalker )
{
fTalker->releasePort();
}
listenIterator = OSCollectionIterator::withCollection(fListeners);
if( listenIterator )
{
while( (listen = (IOFWIsochPort *)listenIterator->getNextObject()) )
{
listen->releasePort();
}
listenIterator->release();
}
return releaseChannelComplete();
}
IOReturn IOFWIsochChannel::releaseChannelComplete()
{
IOReturn result = kIOReturnSuccess;
FWKLOG(( "IOFWIsochChannel::releaseChannelComplete - entered, fDoIRM = %d\n", fDoIRM ));
if( fDoIRM )
{
FWAddress addr( kCSRRegisterSpaceBaseAddressHi, kCSRBandwidthAvailable );
UInt32 generation = 0;
IOLockLock( fLock );
if( result == kIOReturnSuccess )
{
fControl->removeAllocatedChannel( this );
}
if( result == kIOReturnSuccess )
{
UInt16 irm;
fControl->getIRMNodeID( generation, irm );
addr.nodeID = irm;
FWKLOG(( "IOFWIsochChannel::releaseChannelComplete - generation = %d allocated generation = %d\n", generation, fGeneration ));
if( generation != fGeneration )
{
result = kIOFireWireBusReset;
fBandwidth = 0;
fChannel = 64;
}
}
if( fBandwidth != 0 )
{
UInt32 oldVal = 0;
if( result == kIOReturnSuccess )
{
fReadCmd->reinit( generation, addr, &oldVal, 1 );
result = fReadCmd->submit();
}
bool done = false;
while( (result == kIOReturnSuccess) && !done )
{
UInt32 newVal = newVal = oldVal + fBandwidth;
fLockCmd->reinit( generation, addr, &oldVal, &newVal, 1 );
result = fLockCmd->submit();
if( result == kIOReturnSuccess )
{
done = fLockCmd->locked(&oldVal);
}
}
fBandwidth = 0;
FWKLOG(( "IOFWIsochChannel::releaseChannelComplete - released bandwidth\n" ));
if( result != kIOFireWireBusReset )
{
result = kIOReturnSuccess;
}
}
if( fChannel != 64 )
{
UInt32 mask = 0;
UInt32 oldVal = 0;
if( result == kIOReturnSuccess )
{
if( fChannel <= 31 )
{
addr.addressLo = kCSRChannelsAvailable31_0;
mask = 1 << (31-fChannel);
}
else
{
addr.addressLo = kCSRChannelsAvailable63_32;
mask = 1 << (63-fChannel);
}
fReadCmd->reinit( generation, addr, &oldVal, 1 );
result = fReadCmd->submit();
}
bool done = false;
while( (result == kIOReturnSuccess) && !done )
{
UInt32 newVal = oldVal | mask;
fLockCmd->reinit( generation, addr, &oldVal, &newVal, 1 );
result = fLockCmd->submit();
if( result == kIOReturnSuccess )
{
done = fLockCmd->locked( &oldVal );
}
}
FWKLOG(( "IOFWIsochChannel::releaseChannelComplete - released channel\n" ));
fChannel = 64;
}
fBandwidth = 0;
fChannel = 64;
fGeneration = generation;
IOLockUnlock( fLock );
}
return kIOReturnSuccess;
}
IOReturn IOFWIsochChannel::updateBandwidth( bool )
{
return kIOReturnUnsupported;
}