IOFWIsochChannel.cpp [plain text]
#include <IOKit/assert.h>
#include <IOKit/firewire/IOFireWireController.h>
#include <IOKit/firewire/IOFWIsochChannel.h>
#include <IOKit/firewire/IOFWIsochPort.h>
#include <libkern/c++/OSSet.h>
#include <libkern/c++/OSCollectionIterator.h>
#include <IOKit/firewire/IOFWCommand.h>
#include <IOKit/firewire/IOFireWireDevice.h>
OSDefineMetaClassAndStructors(IOFWIsochChannel, OSObject)
OSMetaClassDefineReservedUnused(IOFWIsochChannel, 0);
OSMetaClassDefineReservedUnused(IOFWIsochChannel, 1);
OSMetaClassDefineReservedUnused(IOFWIsochChannel, 2);
OSMetaClassDefineReservedUnused(IOFWIsochChannel, 3);
void IOFWIsochChannel::threadFunc(thread_call_param_t arg, thread_call_param_t)
{
((IOFWIsochChannel *)arg)->reallocBandwidth();
}
bool IOFWIsochChannel::init(IOFireWireController *control, bool doIRM,
UInt32 packetSize, IOFWSpeed prefSpeed, ForceStopNotificationProc* stopProc, void *stopRefCon)
{
if( !OSObject::init() )
return false;
fControl = control;
fDoIRM = doIRM;
fPacketSize = packetSize;
fPrefSpeed = prefSpeed;
fStopProc = stopProc;
fStopRefCon = stopRefCon;
fTalker = NULL;
fListeners = OSSet::withCapacity(1);
fChannel = 64; fReadCmd = new IOFWReadQuadCommand;
if(fReadCmd)
fReadCmd->initAll(fControl, 0, FWAddress(), NULL, 0, NULL, NULL);
fLockCmd = new IOFWCompareAndSwapCommand;
if(fLockCmd)
fLockCmd->initAll(fControl, 0, FWAddress(), NULL, NULL, 0, NULL, NULL);
return fListeners != NULL && fReadCmd != NULL && fLockCmd != NULL;
}
void IOFWIsochChannel::free()
{
if(fListeners)
fListeners->release();
if(fReadCmd)
fReadCmd->release();
if(fLockCmd)
fLockCmd->release();
OSObject::free();
}
IOReturn IOFWIsochChannel::setTalker(IOFWIsochPort *talker)
{
fTalker = talker;
return kIOReturnSuccess;
}
IOReturn IOFWIsochChannel::addListener(IOFWIsochPort *listener)
{
if(fListeners->setObject(listener))
return kIOReturnSuccess;
else
return kIOReturnNoMemory;
}
IOReturn IOFWIsochChannel::updateBandwidth(bool claim)
{
FWAddress addr(kCSRRegisterSpaceBaseAddressHi, kCSRBandwidthAvailable);
UInt16 irm;
UInt32 generation;
UInt32 newVal;
UInt32 oldVal;
IOReturn result = kIOReturnSuccess;
bool tryAgain;
do {
fControl->getIRMNodeID(generation, irm);
if(claim && fGeneration == generation)
return kIOReturnSuccess;
if (!claim && fGeneration != generation)
{
fChannel = 64 ;
fBandwidth = 0 ;
return kIOReturnSuccess ;
}
addr.nodeID = irm;
if(fBandwidth != 0) {
fReadCmd->reinit(generation, addr, &oldVal, 1);
result = fReadCmd->submit();
if(kIOReturnSuccess != result) {
IOLog("IRM read result 0x%x\n", result);
break;
}
do {
if(claim) {
if(oldVal < fBandwidth) {
result = kIOReturnNoSpace;
break;
}
newVal = oldVal - fBandwidth;
}
else
newVal = oldVal + fBandwidth;
fLockCmd->reinit(generation, addr, &oldVal, &newVal, 1);
result = fLockCmd->submit();
if(kIOReturnSuccess != result) {
IOLog("bandwidth update result 0x%x\n", result);
break;
}
tryAgain = !fLockCmd->locked(&oldVal);
} while (tryAgain);
if(claim && kIOReturnNoSpace == result) {
fBandwidth = 0;
fChannel = 64;
}
if(!claim)
fBandwidth = 0;
}
if(fChannel != 64) {
UInt32 mask;
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();
if(kIOReturnSuccess != result) {
break;
}
do {
if(claim) {
newVal = oldVal & ~mask;
if(newVal == oldVal) {
result = kIOReturnNoSpace;
break;
}
}
else {
newVal = oldVal | mask;
}
fLockCmd->reinit(generation, addr, &oldVal, &newVal, 1);
result = fLockCmd->submit();
if(kIOReturnSuccess != result) {
IOLog("channel update result 0x%x\n", result);
break;
}
tryAgain = !fLockCmd->locked(&oldVal);
} while (tryAgain);
if(!claim)
fChannel = 64;
}
} while (false);
if(claim && kIOReturnNoSpace == result) {
stop();
releaseChannel();
}
fGeneration = generation;
return result;
}
IOReturn
IOFWIsochChannel::allocateChannelBegin(
IOFWSpeed inSpeed, UInt64 inAllowedChans,
UInt32& outChannel )
{
UInt32 generation ;
UInt32 newVal ;
FWAddress addr(kCSRRegisterSpaceBaseAddressHi, kCSRBandwidthAvailable) ;
UInt32 oldIRM[3] ;
UInt32 channel ;
IOReturn err = kIOReturnSuccess ;
if(fDoIRM)
{
UInt32 bandwidth = (fPacketSize/4 + 3) * 16 / (1 << inSpeed);
fControl->getIRMNodeID(generation, addr.nodeID);
fReadCmd->reinit(generation, addr, oldIRM, 3);
fReadCmd->setMaxPacket(4); err = fReadCmd->submit();
if ( !err && (oldIRM[0] < bandwidth) )
err = kIOReturnNoSpace;
if (!err)
{
newVal = oldIRM[0] - bandwidth;
fLockCmd->reinit(generation, addr, &oldIRM[0], &newVal, 1);
err = fLockCmd->submit();
if ( !err && !fLockCmd->locked(& oldIRM[0]) )
err = kIOReturnCannotLock ;
if (!err)
fBandwidth = bandwidth;
}
}
if ( !err && fDoIRM )
{
inAllowedChans &= (UInt64)(oldIRM[2]) | ((UInt64)oldIRM[1] << 32);
}
if (!err)
{
for(channel=0; channel<64; channel++)
{
if( inAllowedChans & ((UInt64)1 << ( 63 - channel )) )
break;
}
if(channel == 64)
err = kIOReturnNoResources;
}
if (!err && fDoIRM)
{
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);
err = fLockCmd->submit();
if (!err && !fLockCmd->locked(oldPtr))
err = kIOReturnCannotLock ;
}
if (!err)
outChannel = channel;
if(!err && fDoIRM)
{
fGeneration = generation;
fControl->addAllocatedChannel(this);
}
return err ;
}
IOReturn
IOFWIsochChannel::releaseChannelComplete()
{
if(fDoIRM) {
fControl->removeAllocatedChannel(this);
updateBandwidth(false);
}
return kIOReturnSuccess;
}
IOReturn IOFWIsochChannel::allocateChannel()
{
UInt64 portChans;
UInt64 allowedChans ;
IOFWIsochPort* listen;
IOFWSpeed portSpeed;
OSIterator * listenIterator = NULL;
IOReturn result = kIOReturnSuccess;
fSpeed = fPrefSpeed;
do {
allowedChans = ~(UInt64)0;
if(fTalker) {
fTalker->getSupported(portSpeed, portChans);
if(portSpeed < fSpeed)
fSpeed = portSpeed;
allowedChans &= portChans;
}
listenIterator = OSCollectionIterator::withCollection(fListeners);
if(listenIterator) {
while( (listen = (IOFWIsochPort *) listenIterator->getNextObject())) {
listen->getSupported(portSpeed, portChans);
if(portSpeed < fSpeed)
fSpeed = portSpeed;
allowedChans &= portChans;
}
}
do {
result = allocateChannelBegin( fSpeed, allowedChans, fChannel ) ;
} while ( result == kIOFireWireBusReset || result == kIOReturnCannotLock ) ;
if ( kIOReturnSuccess != result )
break ;
if(listenIterator) {
listenIterator->reset();
while( (listen = (IOFWIsochPort *) listenIterator->getNextObject()) && (result == kIOReturnSuccess)) {
result = listen->allocatePort(fSpeed, fChannel);
}
if(result != kIOReturnSuccess)
break;
}
if(fTalker)
result = fTalker->allocatePort(fSpeed, fChannel);
} while (false);
if(listenIterator)
listenIterator->release();
if(result != kIOReturnSuccess) {
releaseChannel();
}
return result;
}
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() ;
}
void IOFWIsochChannel::reallocBandwidth()
{
updateBandwidth(true);
}
void IOFWIsochChannel::handleBusReset()
{
thread_call_func(threadFunc, this, true);
}
IOReturn IOFWIsochChannel::start()
{
OSIterator *listenIterator;
IOFWIsochPort *listen;
listenIterator = OSCollectionIterator::withCollection(fListeners);
if(listenIterator) {
listenIterator->reset();
while( (listen = (IOFWIsochPort *) listenIterator->getNextObject())) {
listen->start();
}
listenIterator->release();
}
if(fTalker)
fTalker->start();
return kIOReturnSuccess;
}
IOReturn IOFWIsochChannel::stop()
{
OSIterator *listenIterator;
IOFWIsochPort *listen;
listenIterator = OSCollectionIterator::withCollection(fListeners);
if(listenIterator) {
while( (listen = (IOFWIsochPort *) listenIterator->getNextObject())) {
listen->stop();
}
listenIterator->release();
}
if(fTalker)
fTalker->stop();
return kIOReturnSuccess;
}