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,
FWIsochChannelForceStopNotificationProcPtr 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;
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::allocateChannel()
{
UInt64 portChans;
UInt64 allowedChans, savedChans;
UInt16 irm;
UInt32 generation;
UInt32 newVal;
FWAddress addr(kCSRRegisterSpaceBaseAddressHi, kCSRBandwidthAvailable);
OSIterator *listenIterator = NULL;
IOFWIsochPort *listen;
IOFWSpeed portSpeed;
UInt32 old[3];
UInt32 bandwidth;
UInt32 channel;
bool tryAgain; 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;
}
}
if(fDoIRM) {
fControl->getIRMNodeID(generation, irm);
savedChans = allowedChans; bandwidth = (fPacketSize/4 + 3) * 16 / (1 << fSpeed);
addr.nodeID = irm;
fReadCmd->reinit(generation, addr, old, 3);
fReadCmd->setMaxPacket(4);
result = fReadCmd->submit();
if(kIOReturnSuccess != result) {
break;
}
allowedChans &= (UInt64)(old[2]) | ((UInt64)old[1] << 32);
tryAgain = false;
do {
if(old[0] < bandwidth) {
result = kIOReturnNoSpace;
break;
}
newVal = old[0] - bandwidth;
fLockCmd->reinit(generation, addr, &old[0], &newVal, 1);
result = fLockCmd->submit();
if(kIOReturnSuccess != result)
IOLog("bandwidth update result 0x%x\n", result);
tryAgain = !fLockCmd->locked(&old[0]);
} while (tryAgain);
if(kIOReturnSuccess != result)
break;
fBandwidth = bandwidth;
}
tryAgain = false;
do {
for(channel=0; channel<64; channel++) {
if(allowedChans & (1<<(63-channel))) {
break;
}
}
if(channel == 64) {
result = kIOReturnNoResources;
break;
}
if(fDoIRM) {
UInt32 *oldPtr;
if(channel < 32) {
addr.addressLo = kCSRChannelsAvailable31_0;
oldPtr = &old[1];
newVal = *oldPtr & ~(1<<(31-channel));
}
else {
addr.addressLo = kCSRChannelsAvailable63_32;
oldPtr = &old[2];
newVal = *oldPtr & ~(1<<(63-channel));
}
fLockCmd->reinit(generation, addr, oldPtr, &newVal, 1);
result = fLockCmd->submit();
if(kIOReturnSuccess != result)
IOLog("channel update result 0x%x\n", result);
tryAgain = !fLockCmd->locked(oldPtr);
}
else
tryAgain = false;
} while (tryAgain);
if(kIOReturnSuccess != result)
break;
fChannel = channel;
if(fDoIRM) {
fGeneration = generation;
fControl->addAllocatedChannel(this);
}
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();
}
if(fDoIRM) {
fControl->removeAllocatedChannel(this);
updateBandwidth(false);
}
return kIOReturnSuccess;
}
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;
}