bool TFLinkUpdater::getLinkTransforms(const std::string& _link_name, Ogre::Vector3& visual_position, Ogre::Quaternion& visual_orientation,
Ogre::Vector3& collision_position, Ogre::Quaternion& collision_orientation, bool& apply_offset_transforms) const
{
std::string link_name = _link_name;
if (!tf_prefix_.empty())
{
link_name = tf::resolve(tf_prefix_, link_name);
}
Ogre::Vector3 position;
Ogre::Quaternion orientation;
if (!frame_manager_->getTransform(link_name, ros::Time(), position, orientation))
{
std::stringstream ss;
ss << "No transform from [" << link_name << "] to [" << frame_manager_->getFixedFrame() << "]";
setLinkStatus(StatusProperty::Error, link_name, ss.str());
return false;
}
setLinkStatus(StatusProperty::Ok, link_name, "Transform OK");
// Collision/visual transforms are the same in this case
visual_position = position;
visual_orientation = orientation;
collision_position = position;
collision_orientation = orientation;
apply_offset_transforms = true;
return true;
}
void AttansicL2Ethernet::atGetAndUpdateLinkStatus()
{
u16 speed, duplex;
u32 currentMediumIndex = MEDIUM_INDEX_AUTO;
if(at_get_speed_and_duplex(&adapter_.hw, &speed, &duplex) == AT_SUCCESS)
{
DbgPrint("Link is active, speed %d, duplex %d\n", speed, duplex);
if(speed == SPEED_10 && duplex == FULL_DUPLEX) currentMediumIndex = MEDIUM_INDEX_10FD;
if(speed == SPEED_100 && duplex == FULL_DUPLEX) currentMediumIndex = MEDIUM_INDEX_100FD;
if(speed == SPEED_10 && duplex == HALF_DUPLEX) currentMediumIndex = MEDIUM_INDEX_10HD;
if(speed == SPEED_100 && duplex == HALF_DUPLEX) currentMediumIndex = MEDIUM_INDEX_100HD;
setLinkStatus(kIONetworkLinkActive | kIONetworkLinkValid, mediumTable[currentMediumIndex], speed * MBit, NULL);
adapter_.link_speed = speed;
adapter_.link_duplex = duplex;
at_setup_mac_ctrl(&adapter_);
} else
{
DbgPrint("Link is down\n");
setLinkStatus(kIONetworkLinkValid, NULL, 0, NULL);
}
}
IOReturn darwin_iwi3945::disable( IONetworkInterface * netif )
{
IWI_DEBUG("ifconfig down\n");
if ((fNetif->getFlags() & IFF_RUNNING)!=0)
{
IWI_DEBUG("ifconfig going down\n");
//super::disable(fNetif);
//fNetif->setPoweredOnByUser(false);
fTransmitQueue->stop();
setLinkStatus(kIONetworkLinkValid);
//fNetif->setLinkState(kIO80211NetworkLinkDown);
//fNetif->syncSIOCSIFFLAGS( /*IONetworkController * */this);
//(if_flags & ~mask) | (new_flags & mask) if mask has IFF_UP if_updown fires up (kpi_interface.c in xnu)
ifnet_set_flags(fifnet, 0 , IFF_RUNNING );
fTransmitQueue->setCapacity(0);
fTransmitQueue->flush();
//if ((priv->status & STATUS_AUTH)) enable(fNetif);
return kIOReturnSuccess;
}
else
{
IWI_DEBUG("ifconfig already down\n");
return -1;
}
}
bool
MolEnet::resetAndEnable( bool enable )
{
int i;
ready = false;
if( getWorkLoop() )
getWorkLoop()->disableAllInterrupts();
OSI_Enet2Cntrl( kEnet2Reset );
// Initialize the link status.
setLinkStatus( 0, 0 );
// Reset ring buffers
rx_head = tx_tail = 0;
memset( rx_ring, 0, sizeof(enet2_ring_t) * RX_NUM_EL );
memset( tx_ring, 0, sizeof(enet2_ring_t) * TX_NUM_EL );
if( !enable )
return true;
for( i=0; i<RX_NUM_EL; i++ )
updateRXDescriptor( i );
OSI_Enet2Cntrl( kEnet2Start );
if( getWorkLoop() )
getWorkLoop()->enableAllInterrupts();
ready = true;
monitorLinkStatus( true );
return true;
}
IOReturn AtherosL1Ethernet::disable(IONetworkInterface *netif)
{
DbgPrint("disable()\n");
at_adapter *adapter=&adapter_;
setLinkStatus(kIONetworkLinkValid, 0);
transmitQueue_->flush();
transmitQueue_->stop();
transmitQueue_->setCapacity(0);
/* reset MAC to disable all RX/TX */
at_reset_hw(&adapter->hw);
adapter->cmb.cmb->int_stats = 0;
msec_delay(1);
at_irq_disable(adapter);
adapter->link_speed = SPEED_0;
adapter->link_duplex = -1;
at_clean_tx_ring(adapter);
at_clean_rx_ring(adapter);
if (adapter->pdev && adapter->pdev->isOpen () ) adapter->pdev->close(this);
return kIOReturnSuccess;
}
/* IONetworkController methods. */
IOReturn NullEthernet::enable(IONetworkInterface *netif)
{
const IONetworkMedium *selectedMedium;
IOReturn result = kIOReturnError;
DebugLog("enable() ===>\n");
if (m_isEnabled) {
DebugLog("Interface already enabled.\n");
result = kIOReturnSuccess;
goto done;
}
selectedMedium = getSelectedMedium();
if (!selectedMedium) {
DebugLog("No medium selected. Falling back to autonegotiation.\n");
selectedMedium = m_mediumTable[MEDIUM_INDEX_AUTO];
}
selectMedium(selectedMedium);
setLinkStatus(kIONetworkLinkValid);
m_isEnabled = true;
result = kIOReturnSuccess;
DebugLog("enable() <===\n");
done:
return result;
}
IOReturn AttansicL2Ethernet::disable(IONetworkInterface *netif)
{
DbgPrint("disable()\n");
at_adapter *adapter=&adapter_;
setLinkStatus(kIONetworkLinkValid, 0);
transmitQueue_->flush();
transmitQueue_->stop();
transmitQueue_->setCapacity(0);
at_reset_hw(adapter);
IOSleep(1);
at_irq_disable(adapter);
adapter->link_speed = SPEED_0;
adapter->link_duplex = -1;
// pretty wrong place for DMA stuff cleaning, eh?
if (adapter->pdev && adapter->pdev->isOpen () ) adapter->pdev->close(this);
return kIOReturnSuccess;
}
IOReturn HoRNDIS::enable(IONetworkInterface *netif) {
IONetworkMedium *medium;
IOReturn rtn = kIOReturnSuccess;
if (fNetifEnabled) {
LOG(V_ERROR, "already enabled?");
return kIOReturnSuccess;
}
if (!allocateResources())
return kIOReturnNoMemory;
if (!fMediumDict)
if (!createMediumTables()) {
rtn = kIOReturnNoMemory;
goto bailout;
}
setCurrentMedium(IONetworkMedium::medium(kIOMediumEthernetAuto, 480 * 1000000));
/* Kick off the first read. */
inbuf.comp.target = this;
inbuf.comp.action = dataReadComplete;
inbuf.comp.parameter = NULL;
rtn = fInPipe->Read(inbuf.mdp, &inbuf.comp, NULL);
if (rtn != kIOReturnSuccess)
goto bailout;
/* Tell the world that the link is up... */
medium = IONetworkMedium::getMediumWithType(fMediumDict, kIOMediumEthernetAuto);
setLinkStatus(kIONetworkLinkActive | kIONetworkLinkValid, medium, 480 * 1000000);
/* ... and then listen for packets! */
getOutputQueue()->setCapacity(TRANSMIT_QUEUE_SIZE);
getOutputQueue()->start();
LOG(V_DEBUG, "txqueue started");
/* Tell the other end to start transmitting. */
if (!rndisSetPacketFilter(RNDIS_DEFAULT_FILTER))
goto bailout;
/* Now we can say we're alive. */
fNetifEnabled = true;
return kIOReturnSuccess;
bailout:
LOG(V_ERROR, "setting up the pipes failed");
releaseResources();
return rtn;
}
IOReturn AgereET131x::disable(IONetworkInterface * netif)
{
//IOLog("AgereET131x::disable()\n");
if(!enabledForNetif)
return kIOReturnSuccess;
enabledForNetif = false;
/**************************************************************************
First thing is to stop the queue
*************************************************************************/
workLoop->disableAllInterrupts();
if (transmitQueue) {
transmitQueue->stop();
transmitQueue->flush();
}
//stop watchdog timer
watchdogSource->cancelTimeout(); // Stop the timer event source.
/**************************************************************************
Stop the Tx and Rx DMA engines
*************************************************************************/
rx_dma_disable();
tx_dma_disable();
/**************************************************************************
Disable device interrupts
*************************************************************************/
disable_interrupts( );
/**************************************************************************
Deregistering ISR
*************************************************************************/
MP_CLEAR_FLAG( &adapter, fMP_ADAPTER_INTERRUPT_IN_USE );
setLinkStatus( kIONetworkLinkValid ); // Valid sans kIONetworkLinkActive
/**************************************************************************
Stop the error timer
*************************************************************************/
//del_timer_sync( &adapter->ErrorTimer );
pciDevice->close(this);
return kIOReturnSuccess;
}
void AtherosL1Ethernet::atGetAndUpdateLinkStatus()
{
at_adapter *adapter=&adapter_;
struct at_hw *hw = &adapter->hw;
u16 speed, duplex;
u32 currentMediumIndex = MEDIUM_INDEX_AUTO;
if(at_get_speed_and_duplex(hw, &speed, &duplex) == AT_SUCCESS)
{
DbgPrint("Link is active, speed %d, duplex %d\n", speed, duplex);
if(speed == SPEED_10 && duplex == FULL_DUPLEX) currentMediumIndex = MEDIUM_INDEX_10FD;
if(speed == SPEED_100 && duplex == FULL_DUPLEX) currentMediumIndex = MEDIUM_INDEX_100FD;
if(speed == SPEED_1000 && duplex == FULL_DUPLEX) currentMediumIndex = MEDIUM_INDEX_1000FD;
if(speed == SPEED_10 && duplex == HALF_DUPLEX) currentMediumIndex = MEDIUM_INDEX_10HD;
if(speed == SPEED_100 && duplex == HALF_DUPLEX) currentMediumIndex = MEDIUM_INDEX_100HD;
if(speed == SPEED_1000 && duplex == HALF_DUPLEX) currentMediumIndex = MEDIUM_INDEX_1000HD;
setLinkStatus(kIONetworkLinkActive | kIONetworkLinkValid, mediumTable[currentMediumIndex], speed * MBit, NULL);
adapter->link_speed = speed;
adapter->link_duplex = duplex;
at_setup_mac_ctrl(adapter);
} else
{
DbgPrint("Link is down\n");
u32 value;
//disable rx
value = AT_READ_REG(hw, REG_MAC_CTRL);
value &= ~MAC_CTRL_RX_EN;
AT_WRITE_REG(hw, REG_MAC_CTRL, value);
adapter->link_speed = SPEED_0;
setLinkStatus(kIONetworkLinkValid, NULL, 0, NULL);
}
}
bool CLASS::decreaseActivationLevel( UInt32 currentLevel )
{
bool success = true;
switch (currentLevel)
{
case kActivationLevelKDP:
hwStop();
fWatchdogTimer->cancelTimeout();
// Report link valid and down.
setLinkStatus( kIONetworkLinkValid );
// Flush all packets held in the queue and prevent it
// from accumulating any additional packets.
fTransmitQueue->setCapacity(0);
fTransmitQueue->flush();
if (fPCINub) fPCINub->close(this);
// Free all runtime resources. Must make sure DMA
// engine is stopped before this to avoid hard to
// locate mbuf/memory corruptions.
swFree();
break;
case kActivationLevelBSD:
fTransmitQueue->stop();
// Don't need interrupts for KDP, mask all sources.
disableHardwareInterrupts();
break;
}
return success;
}
bool RTL8139::disableAdapter( UInt32 currentLevel )
{
bool success = false;
ELG( 0, currentLevel, 'disA', "RTL8139::disableAdapter" );
DEBUG_LOG( "disableAdapter() ===>\n" );
DEBUG_LOG( "disable currentLevel %ld\n", currentLevel );
switch ( currentLevel )
{
case kActivationLevel1:
timerSrc->cancelTimeout(); // Stop the timer event source.
initAdapter( kResetChip ); // Reset the hardware engine.
phySetMedium( MEDIUM_INDEX_NONE ); // Power down the PHY
if ( pciNub )
pciNub->close( this ); // Close our provider.
success = true;
break;
case kActivationLevel2:
disableHardwareInterrupts(); // KDP doesn't use interrupts.
workLoop->disableAllInterrupts();
// Stop the transmit queue. outputPacket() will not get called
// after this. KDP calls sendPacket() to send a packet in polled
// mode and that is unaffected by the state of the output queue.
fTransmitQueue->stop();
fTransmitQueue->flush();
setLinkStatus( kIONetworkLinkValid ); // Valid sans kIONetworkLinkActive
success = true;
break;
}/* end SWITCH */
if ( false == success )
IOLog( "disable currentLevel %u failed\n", (unsigned int)currentLevel );
DEBUG_LOG( "disableAdapter() <===\n" );
return success;
}/* end disableAdapter */
IOReturn NullEthernet::disable(IONetworkInterface *netif)
{
IOReturn result = kIOReturnSuccess;
DebugLog("disable() ===>\n");
if (!m_isEnabled)
goto done;
setLinkStatus(kIONetworkLinkValid);
m_linkUp = false;
m_isEnabled = false;
DebugLog("disable() <===\n");
done:
return result;
}
IOReturn HoRNDIS::disable(IONetworkInterface * netif) {
/* Disable the queue (no more outputPacket), and then flush everything in the queue. */
getOutputQueue()->stop();
getOutputQueue()->setCapacity(0);
getOutputQueue()->flush();
/* Other end should stop xmitting, too. */
rndisSetPacketFilter(0);
setLinkStatus(0, 0);
/* Release all resources */
releaseResources();
fNetifEnabled = false;
/* Terminates also close the device in 'disable'. */
if (fTerminate) {
fpDevice->close(this);
fpDevice = NULL;
}
return kIOReturnSuccess;
}
/*
* This function is quite brain-dead... eventually we might want to
* be able to fake a link down though.
*/
void
MolEnet::monitorLinkStatus( bool firstPoll )
{
IONetworkMedium *medium;
UInt32 linkSpeed, linkStatus;
IOMediumType mediumType;
linkSpeed = 100;
linkStatus = kIONetworkLinkValid | kIONetworkLinkActive;
mediumType = kIOMediumEthernet100BaseTX | kIOMediumOptionFullDuplex;
if( firstPoll || !linkStatusReported ) {
linkStatusReported = true;
medium = IONetworkMedium::getMediumWithType(mediumDict, mediumType);
setLinkStatus( linkStatus, medium, linkSpeed * 1000000 );
if( linkStatus & kIONetworkLinkActive )
printm( "MolEnet: Link up at %ld Mbps - %s Duplex\n",
linkSpeed, (mediumType & kIOMediumOptionFullDuplex) ? "Full" : "Half" );
else
printm( "MolEnet: Link down\n" );
}
}
bool darwin_iwi3945::start(IOService *provider)
{
UInt16 reg;
//linking the kext control clone to the driver:
clone=this;
firstifup=0;
do {
if ( super::start(provider) == 0) {
IOLog("%s ERR: super::start failed\n", getName());
break;
}
if ( (fPCIDevice = OSDynamicCast(IOPCIDevice, provider)) == 0) {
IOLog("%s ERR: fPCIDevice == 0 :(\n", getName());
break;
}
fPCIDevice->retain();
if (fPCIDevice->open(this) == 0) {
IOLog("%s ERR: fPCIDevice->open(this) failed\n", getName());
break;
}
// Request domain power.
// Without this, the PCIDevice may be in state 0, and the
// PCI config space may be invalid if the machine has been
// sleeping.
if (fPCIDevice->requestPowerDomainState(kIOPMPowerOn,
(IOPowerConnection *) getParentEntry(gIOPowerPlane),
IOPMLowestState ) != IOPMNoErr) {
IOLog("%s Power thingi failed\n", getName());
break;
}
/*UInt16 reg16;
reg16 = fPCIDevice->configRead16(kIOPCIConfigCommand);
reg16 |= (kIOPCICommandBusMaster |
kIOPCICommandMemorySpace |
kIOPCICommandMemWrInvalidate);
reg16 &= ~kIOPCICommandIOSpace; // disable I/O space
fPCIDevice->configWrite16(kIOPCIConfigCommand,reg16);*/
fPCIDevice->setBusMasterEnable(true);
fPCIDevice->setMemoryEnable(true);
irqNumber = fPCIDevice->configRead8(kIOPCIConfigInterruptLine);
vendorID = fPCIDevice->configRead16(kIOPCIConfigVendorID);
deviceID = fPCIDevice->configRead16(kIOPCIConfigDeviceID);
pciReg = fPCIDevice->configRead16(kIOPCIConfigRevisionID);
map = fPCIDevice->mapDeviceMemoryWithRegister(kIOPCIConfigBaseAddress0, kIOMapInhibitCache);
if (map == 0) {
IOLog("%s map is zero\n", getName());
break;
}
ioBase = map->getPhysicalAddress();
memBase = (UInt16 *)map->getVirtualAddress();
//memDes = map->getMemoryDescriptor();
//mem = fPCIDevice->getDeviceMemoryWithRegister(kIOPCIConfigBaseAddress0);
//memDes->initWithPhysicalAddress(ioBase, map->getLength(), kIODirectionOutIn);
/* We disable the RETRY_TIMEOUT register (0x41) to keep
* PCI Tx retries from interfering with C3 CPU state */
reg = fPCIDevice->configRead16(0x40);
if((reg & 0x0000ff00) != 0)
fPCIDevice->configWrite16(0x40, reg & 0xffff00ff);
printf("%s iomemory length: 0x%x @ 0x%x\n", getName(), map->getLength(), ioBase);
printf("%s virt: 0x%x physical: 0x%x\n", getName(), memBase, ioBase);
printf("%s IRQ: %d, Vendor ID: %04x, Product ID: %04x\n", getName(), irqNumber, vendorID, deviceID);
fWorkLoop = (IOWorkLoop *) getWorkLoop();
if (!fWorkLoop) {
//IOLog("%s ERR: start - getWorkLoop failed\n", getName());
break;
}
fInterruptSrc = IOInterruptEventSource::interruptEventSource(
this, (IOInterruptEventAction) &darwin_iwi3945::interruptOccurred,
provider);
if(!fInterruptSrc || (fWorkLoop->addEventSource(fInterruptSrc) != kIOReturnSuccess)) {
//IOLog("%s fInterruptSrc error\n", getName());
break;;
}
// This is important. If the interrupt line is shared with other devices,
// then the interrupt vector will be enabled only if all corresponding
// interrupt event sources are enabled. To avoid masking interrupts for
// other devices that are sharing the interrupt line, the event source
// is enabled immediately.
fInterruptSrc->enable();
//mutex=IOLockAlloc();
fTransmitQueue = createOutputQueue();
if (fTransmitQueue == NULL)
{
IWI_ERR("ERR: getOutputQueue()\n");
break;
}
fTransmitQueue->setCapacity(1024);
iwl_pci_probe();
if (!priv) break;
iwl_hw_nic_init(priv);
iwl_hw_nic_reset(priv);
if (attachInterface((IONetworkInterface **) &fNetif, false) == false) {
//IOLog("%s attach failed\n", getName());
break;
}
setProperty(kIOMinPacketSize,12);
setProperty(kIOMaxPacketSize, IWL_RX_BUF_SIZE);
//setProperty(kIOFeatures, kIONetworkFeatureNoBSDWait|kIONetworkFeatureSoftwareVlan);
fNetif->registerOutputHandler(this,getOutputHandler());
fNetif->registerService();
registerService();
mediumDict = OSDictionary::withCapacity(MEDIUM_TYPE_INVALID + 1);
addMediumType(kIOMediumIEEE80211None, 0, MEDIUM_TYPE_NONE);
addMediumType(kIOMediumIEEE80211Auto, 0, MEDIUM_TYPE_AUTO);
publishMediumDictionary(mediumDict);
setCurrentMedium(mediumTable[MEDIUM_TYPE_AUTO]);
setSelectedMedium(mediumTable[MEDIUM_TYPE_AUTO]);
setLinkStatus(kIONetworkLinkValid, mediumTable[MEDIUM_TYPE_AUTO]);
//kext control registration:
//these functions registers the control which enables
//the user to interact with the driver
struct kern_ctl_reg ep_ctl; // Initialize control
kern_ctl_ref kctlref;
bzero(&ep_ctl, sizeof(ep_ctl));
ep_ctl.ctl_id = 0;
ep_ctl.ctl_unit = 0;
strcpy(ep_ctl.ctl_name,"insanelymac.iwidarwin.control");
ep_ctl.ctl_flags = 0;
ep_ctl.ctl_connect = ConnectClient;
ep_ctl.ctl_disconnect = disconnectClient;
ep_ctl.ctl_send = setSelectedNetwork;
ep_ctl.ctl_setopt = configureConnection;
ep_ctl.ctl_getopt = sendNetworkList;
errno_t error = ctl_register(&ep_ctl, &kctlref);
//queue_te(12,OSMemberFunctionCast(thread_call_func_t,this,&darwin_iwi3945::check_firstup),NULL,NULL,false);
//queue_te(12,OSMemberFunctionCast(thread_call_func_t,this,&darwin_iwi3945::check_firstup),priv,1000,true);
//check_firstup(priv);
return true; // end start successfully
} while (false);
//stop(provider);
//free();
return false; // end start insuccessfully
}