<commit_before>/**************************************************************************

 The MIT License (MIT)

 Copyright (c) 2015 Dmitry Sovetov

 https://github.com/dmsovetov

 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:
 The above copyright notice and this permission notice shall be included in all
 copies or substantial portions of the Software.
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.

 **************************************************************************/

#include "Entity/Aspect.h"
#include "Ecs.h"

#include "Entity/Entity.h"
#include "Entity/Index.h"
#include "Entity/Archetype.h"
#include "System/SystemGroup.h"

DC_BEGIN_DREEMCHEST

namespace Ecs {

IMPLEMENT_LOGGER( log )

// ** Ecs::Ecs
Ecs::Ecs( const EntityIdGeneratorPtr& entityIdGenerator ) : m_entityId( entityIdGenerator )
{
}

// ** Ecs::create
EcsPtr Ecs::create( const EntityIdGeneratorPtr& entityIdGenerator )
{
	DC_BREAK_IF( entityIdGenerator == EntityIdGeneratorPtr() );
	return DC_NEW Ecs( entityIdGenerator );
}

// ** Ecs::registerEntity
void Ecs::registerEntity( EntityPtr entity, const EntityId& id )
{
	DC_BREAK_IF( isUsedId( id ) );

	// Setup entity
	entity->setEcs( this );
	entity->setId( id );

	// Register the entity
	m_entities[id] = entity;
}

// ** Ecs::createArchetypeByName
ArchetypePtr Ecs::createArchetypeByName( const String& name, const EntityId& id )
{
	// Create archetype instance by name
	ArchetypePtr instance = m_archetypeFactory.construct( name );

	// Ensure we found the archetype type
	if( !instance.valid() ) {
		log::error( "Ecs::createArchetypeByName : unknown archetype '%s'\n", name.c_str() );
		return ArchetypePtr();
	}

	// Initialize the entity id
	EntityId eid = id.isNull() ? m_entityId->generate() : id;

	// Register the entity
	registerEntity( instance, eid );

	// Construct the archetype
	instance->construct();

	return instance;
}

// ** Ecs::createComponentByName
ComponentPtr Ecs::createComponentByName( const String& name ) const
{
	return m_componentFactory.construct( name );
}

// ** Ecs::createEntity
EntityPtr Ecs::createEntity( const EntityId& id )
{
	DC_BREAK_IF( isUsedId( id ) );

	EntityPtr entity( DC_NEW Entity );
	registerEntity( entity, id );

	return entity;
}

// ** Ecs::createEntity
EntityPtr Ecs::createEntity( void )
{
	EntityId id = m_entityId->generate();
	return createEntity( id );
}

// ** Ecs::findEntity
EntityPtr Ecs::findEntity( const EntityId& id ) const
{
	Entities::const_iterator i = m_entities.find( id );
	return i != m_entities.end() ? i->second : EntityPtr();
}

// ** Ecs::findByAspect
EntitySet Ecs::findByAspect( const Aspect& aspect ) const
{
	EntitySet result;

	for( Entities::const_iterator i = m_entities.begin(), end = m_entities.end(); i != end; ++i ) {
		if( aspect.hasIntersection( i->second ) ) {
			result.insert( i->second );
		}
	}

	return result;
}

// ** Ecs::removeEntity
void Ecs::removeEntity( const EntityId& id )
{
	DC_BREAK_IF( !isUsedId( id ) );

	Entities::iterator i = m_entities.find( id );

	if( i == m_entities.end() ) {
		return;
	}

	i->second->markAsRemoved();

	m_changed.insert( i->second );
	m_removed.insert( i->second );
}

// ** Ecs::isUsedId
bool Ecs::isUsedId( const EntityId& id ) const
{
	return m_entities.find( id ) != m_entities.end();
}

// ** Ecs::requestIndex
IndexPtr Ecs::requestIndex( const String& name, const Aspect& aspect )
{
	Indices::iterator i = m_indices.find( aspect );

	if( i != m_indices.end() ) {
		return i->second;
	}

	IndexPtr index( DC_NEW Index( this, name, aspect ) );
	m_indices[aspect] = index;

	return index;
}

// ** Ecs::createGroup
SystemGroupPtr Ecs::createGroup( const String& name, u32 mask )
{
	SystemGroupPtr group( DC_NEW SystemGroup( this, name, mask ) );
	m_systems.push_back( group );
	return group;
}

// ** Ecs::notifyEntityChanged
void Ecs::notifyEntityChanged( const EntityId& id )
{
	DC_BREAK_IF( !isUsedId( id ) );

	Entities::iterator i = m_entities.find( id );

	if( i != m_entities.end() ) {
		m_changed.insert( i->second );
	}
}

// ** Ecs::update
void Ecs::update( u32 currentTime, f32 dt, u32 systems )
{
	// Process all changed entities.
	while( m_changed.size() ) {
		EntitySet changed = m_changed;
		m_changed.clear();

		for( EntitySet::iterator i = changed.begin(), end = changed.end(); i != end; ++i ) {
			for( Indices::iterator j = m_indices.begin(), jend = m_indices.end(); j != jend; j++ ) {
				j->second->notifyEntityChanged( *i );
			}
		}
	}

	// Remove all queued entities
	while( m_removed.size() ) {
		EntitySet removed = m_removed;
		m_removed.clear();

		for( EntitySet::iterator i = removed.begin(), end = removed.end(); i != end; ++i ) {
			m_entities.erase( (*i)->id() );
		}
	}

	// Update all system groups.
	for( u32 i = 0, n = ( u32 )m_systems.size(); i < n; i++ ) {
		SystemGroupPtr& group = m_systems[i];

		if( group->mask() & systems ) {
			group->update( currentTime, dt );
		}
	}
}

// ** EntityIdGenerator::EntityIdGenerator
EntityIdGenerator::EntityIdGenerator( void )
{
#if DC_ECS_INTEGER_IDS
	m_nextId = 1;
#endif
}

// ** EntityIdGenerator::generate
EntityId EntityIdGenerator::generate( void )
{
#if DC_ECS_INTEGER_IDS
	return m_nextId++;
#else
	return Guid::generate();
#endif
}

} // namespace Ecs

DC_END_DREEMCHEST<commit_msg>Added GUID randomization to EntityIdGenerator<commit_after>/**************************************************************************

 The MIT License (MIT)

 Copyright (c) 2015 Dmitry Sovetov

 https://github.com/dmsovetov

 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
 in the Software without restriction, including without limitation the rights
 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 copies of the Software, and to permit persons to whom the Software is
 furnished to do so, subject to the following conditions:
 The above copyright notice and this permission notice shall be included in all
 copies or substantial portions of the Software.
 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 SOFTWARE.

 **************************************************************************/

#include "Entity/Aspect.h"
#include "Ecs.h"

#include "Entity/Entity.h"
#include "Entity/Index.h"
#include "Entity/Archetype.h"
#include "System/SystemGroup.h"

#include <time.h>

DC_BEGIN_DREEMCHEST

namespace Ecs {

IMPLEMENT_LOGGER( log )

// ** Ecs::Ecs
Ecs::Ecs( const EntityIdGeneratorPtr& entityIdGenerator ) : m_entityId( entityIdGenerator )
{
}

// ** Ecs::create
EcsPtr Ecs::create( const EntityIdGeneratorPtr& entityIdGenerator )
{
	DC_BREAK_IF( entityIdGenerator == EntityIdGeneratorPtr() );
	return DC_NEW Ecs( entityIdGenerator );
}

// ** Ecs::registerEntity
void Ecs::registerEntity( EntityPtr entity, const EntityId& id )
{
	DC_BREAK_IF( isUsedId( id ) );

	// Setup entity
	entity->setEcs( this );
	entity->setId( id );

	// Register the entity
	m_entities[id] = entity;
}

// ** Ecs::createArchetypeByName
ArchetypePtr Ecs::createArchetypeByName( const String& name, const EntityId& id )
{
	// Create archetype instance by name
	ArchetypePtr instance = m_archetypeFactory.construct( name );

	// Ensure we found the archetype type
	if( !instance.valid() ) {
		log::error( "Ecs::createArchetypeByName : unknown archetype '%s'\n", name.c_str() );
		return ArchetypePtr();
	}

	// Initialize the entity id
	EntityId eid = id.isNull() ? m_entityId->generate() : id;

	// Register the entity
	registerEntity( instance, eid );

	// Construct the archetype
	instance->construct();

	return instance;
}

// ** Ecs::createComponentByName
ComponentPtr Ecs::createComponentByName( const String& name ) const
{
	return m_componentFactory.construct( name );
}

// ** Ecs::createEntity
EntityPtr Ecs::createEntity( const EntityId& id )
{
	DC_BREAK_IF( isUsedId( id ) );

	EntityPtr entity( DC_NEW Entity );
	registerEntity( entity, id );

	return entity;
}

// ** Ecs::createEntity
EntityPtr Ecs::createEntity( void )
{
	EntityId id = m_entityId->generate();
	return createEntity( id );
}

// ** Ecs::findEntity
EntityPtr Ecs::findEntity( const EntityId& id ) const
{
	Entities::const_iterator i = m_entities.find( id );
	return i != m_entities.end() ? i->second : EntityPtr();
}

// ** Ecs::findByAspect
EntitySet Ecs::findByAspect( const Aspect& aspect ) const
{
	EntitySet result;

	for( Entities::const_iterator i = m_entities.begin(), end = m_entities.end(); i != end; ++i ) {
		if( aspect.hasIntersection( i->second ) ) {
			result.insert( i->second );
		}
	}

	return result;
}

// ** Ecs::removeEntity
void Ecs::removeEntity( const EntityId& id )
{
	DC_BREAK_IF( !isUsedId( id ) );

	Entities::iterator i = m_entities.find( id );

	if( i == m_entities.end() ) {
		return;
	}

	i->second->markAsRemoved();

	m_changed.insert( i->second );
	m_removed.insert( i->second );
}

// ** Ecs::isUsedId
bool Ecs::isUsedId( const EntityId& id ) const
{
	return m_entities.find( id ) != m_entities.end();
}

// ** Ecs::requestIndex
IndexPtr Ecs::requestIndex( const String& name, const Aspect& aspect )
{
	Indices::iterator i = m_indices.find( aspect );

	if( i != m_indices.end() ) {
		return i->second;
	}

	IndexPtr index( DC_NEW Index( this, name, aspect ) );
	m_indices[aspect] = index;

	return index;
}

// ** Ecs::createGroup
SystemGroupPtr Ecs::createGroup( const String& name, u32 mask )
{
	SystemGroupPtr group( DC_NEW SystemGroup( this, name, mask ) );
	m_systems.push_back( group );
	return group;
}

// ** Ecs::notifyEntityChanged
void Ecs::notifyEntityChanged( const EntityId& id )
{
	DC_BREAK_IF( !isUsedId( id ) );

	Entities::iterator i = m_entities.find( id );

	if( i != m_entities.end() ) {
		m_changed.insert( i->second );
	}
}

// ** Ecs::update
void Ecs::update( u32 currentTime, f32 dt, u32 systems )
{
	// Process all changed entities.
	while( m_changed.size() ) {
		EntitySet changed = m_changed;
		m_changed.clear();

		for( EntitySet::iterator i = changed.begin(), end = changed.end(); i != end; ++i ) {
			for( Indices::iterator j = m_indices.begin(), jend = m_indices.end(); j != jend; j++ ) {
				j->second->notifyEntityChanged( *i );
			}
		}
	}

	// Remove all queued entities
	while( m_removed.size() ) {
		EntitySet removed = m_removed;
		m_removed.clear();

		for( EntitySet::iterator i = removed.begin(), end = removed.end(); i != end; ++i ) {
			m_entities.erase( (*i)->id() );
		}
	}

	// Update all system groups.
	for( u32 i = 0, n = ( u32 )m_systems.size(); i < n; i++ ) {
		SystemGroupPtr& group = m_systems[i];

		if( group->mask() & systems ) {
			group->update( currentTime, dt );
		}
	}
}

// ** EntityIdGenerator::EntityIdGenerator
EntityIdGenerator::EntityIdGenerator( void )
{
#if DC_ECS_INTEGER_IDS
	m_nextId = 1;
#else
	srand( static_cast<u32>( time( NULL ) ) );
#endif
}

// ** EntityIdGenerator::generate
EntityId EntityIdGenerator::generate( void )
{
#if DC_ECS_INTEGER_IDS
	return m_nextId++;
#else
	return Guid::generate();
#endif
}

} // namespace Ecs

DC_END_DREEMCHEST<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/common/include/p9_misc_scom_addresses_fld_fixes.H $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
///
/// @file misc_scom_addresses_fld_fixes.H
/// @brief The *scom_addresses_fld.H files are generated form figtree,
///        but the figree can be wrong.  This file is included in
///        *_scom_addresses_fld.H and allows incorrect constants to be
///        fixed manually.
///
// *HWP HWP Owner: Ben Gass <bgass@us.ibm.com>
// *HWP FW Owner: ? <?>
// *HWP Team: SAO
// *HWP Level: 1
// *HWP Consumed by: FSP:HB:HS:OCC:SBE:CME:SGPE:PGPE:FPPE:IPPE

#ifndef __P9_MISC_SCOM_ADDRESSES_FLD_FIXES_H
#define __P9_MISC_SCOM_ADDRESSES_FLD_FIXES_H

// N2 Chiplet Config Register1
REG64_FLD(PERV_N2_CPLT_CONF1_TC_PSI_IOVALID_DC,  10, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// PSI Receive Status Register
REG64_FLD(PU_RX_PSI_STATUS_CHECK_PASS,            0, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);
REG64_FLD(PU_RX_PSI_STATUS_CHECK_FAIL,            1, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// PSI Transmit Control Register
REG64_FLD(PU_TX_PSI_CNTL_IORESET,                 0, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// PSI Receive Control Register
REG64_FLD(PU_RX_PSI_CNTL_IORESET,                 0, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// PSI Transmit Channel Internal Address Register
REG64_FLD(PU_TX_CH_INTADDR_IAR_ADDR,              0, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);
REG64_FLD(PU_TX_CH_INTADDR_IAR_ADDR_LEN,          8, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// PSI Transmit Channel Misc Register
REG64_FLD(PU_TX_CH_MISC_WEN0,                    12, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);
REG64_FLD(PU_TX_CH_MISC_WEN1,                    13, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);
REG64_FLD(PU_TX_CH_MISC_WEN2,                    14, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// RX VGA Control Register1
REG64_FLD(PEC_SCOM0X0B_EDMOD,                    52, SH_UNT_PEC, SH_ACS_SCOM, SH_FLD_UNUSED);
REG64_FLD(PEC_SCOM0X0B_EDMOD_LEN,                 2, SH_UNT_PEC, SH_ACS_SCOM, SH_FLD_UNUSED);

//Example
//Copy the whole line from the *scom_addresses_fld.H file.  Then add FIX in front of REG
//and add another paramter that is the new value you want.
//
//FIXREG64_FLD( PU_ALTD_CMD_REG_FBC_WITH_TM_QUIESCE, 24, SH_UNT, SH_ACS_SCOM, SH_FLD_FBC_WITH_TM_QUIESCE,
//              12);

#endif
<commit_msg>update HWP level metadata for nest, common files<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/common/include/p9_misc_scom_addresses_fld_fixes.H $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
///
/// @file misc_scom_addresses_fld_fixes.H
/// @brief The *scom_addresses_fld.H files are generated form figtree,
///        but the figree can be wrong.  This file is included in
///        *_scom_addresses_fld.H and allows incorrect constants to be
///        fixed manually.
///
// *HWP HWP Owner: Ben Gass <bgass@us.ibm.com>
// *HWP FW Owner: ? <?>
// *HWP Team: SAO
// *HWP Level: 3
// *HWP Consumed by: FSP:HB:HS:OCC:SBE:CME:SGPE:PGPE:FPPE:IPPE

#ifndef __P9_MISC_SCOM_ADDRESSES_FLD_FIXES_H
#define __P9_MISC_SCOM_ADDRESSES_FLD_FIXES_H

// N2 Chiplet Config Register1
REG64_FLD(PERV_N2_CPLT_CONF1_TC_PSI_IOVALID_DC,  10, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// PSI Receive Status Register
REG64_FLD(PU_RX_PSI_STATUS_CHECK_PASS,            0, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);
REG64_FLD(PU_RX_PSI_STATUS_CHECK_FAIL,            1, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// PSI Transmit Control Register
REG64_FLD(PU_TX_PSI_CNTL_IORESET,                 0, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// PSI Receive Control Register
REG64_FLD(PU_RX_PSI_CNTL_IORESET,                 0, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// PSI Transmit Channel Internal Address Register
REG64_FLD(PU_TX_CH_INTADDR_IAR_ADDR,              0, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);
REG64_FLD(PU_TX_CH_INTADDR_IAR_ADDR_LEN,          8, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// PSI Transmit Channel Misc Register
REG64_FLD(PU_TX_CH_MISC_WEN0,                    12, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);
REG64_FLD(PU_TX_CH_MISC_WEN1,                    13, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);
REG64_FLD(PU_TX_CH_MISC_WEN2,                    14, SH_UNT, SH_ACS_SCOM, SH_FLD_UNUSED);

// RX VGA Control Register1
REG64_FLD(PEC_SCOM0X0B_EDMOD,                    52, SH_UNT_PEC, SH_ACS_SCOM, SH_FLD_UNUSED);
REG64_FLD(PEC_SCOM0X0B_EDMOD_LEN,                 2, SH_UNT_PEC, SH_ACS_SCOM, SH_FLD_UNUSED);

//Example
//Copy the whole line from the *scom_addresses_fld.H file.  Then add FIX in front of REG
//and add another paramter that is the new value you want.
//
//FIXREG64_FLD( PU_ALTD_CMD_REG_FBC_WITH_TM_QUIESCE, 24, SH_UNT, SH_ACS_SCOM, SH_FLD_FBC_WITH_TM_QUIESCE,
//              12);

#endif
<|endoftext|>


<commit_before>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/io/p9_io_erepairAccessorHwpFuncs.C $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
///
/// @file p9_io_erepairAccessorHwpFuncs.C
/// @brief FW Team utility functions that access fabric and memory eRepair data.
///
//----------------------------------------------------------------------------
#include <fapi2.H>
#include <string.h>
#include <p9_io_erepairConsts.H>

using namespace EREPAIR;
using namespace fapi2;

/***** Function definitions *****/

/**
 * @brief This function checks to see if the passed vectors have matching
 *        fail lane numbers. If no matching lane number is found, such lane
 *        value will be invalidated in the vector
 *
 * @param [in]  io_endp1_txFaillanes        Reference to vector which has fail
 *                                          lane numbers of Tx side
 * @param [in]  io_endp2_rxFaillanes        Reference to vector which has fail
 *                                          lane numbers of Rx side
 * @param [out] o_invalidFails_inTx_Ofendp1 If TRUE, indicates that Tx has fail
 *                                          lane numbers for which there is no
 *                                          matching entry on Rx side
 * @param [out] o_invalidFails_inRx_Ofendp2 If TRUE, indicates that Tx has fail
 *                                          lane numbers for which there is no
 *                                          matching entry on Tx side
 *
 * @return void
 */
void invalidateNonMatchingFailLanes(std::vector<uint8_t>& io_endp1_txFaillanes,
                                    std::vector<uint8_t>& io_endp2_rxFaillanes,
                                    bool& o_invalidFails_inTx_Ofendp1,
                                    bool& o_invalidFails_inRx_Ofendp2)
{
    std::vector<uint8_t>::iterator l_it;
    std::vector<uint8_t>::iterator l_itTmp;
    std::vector<uint8_t>::iterator l_itDrv;
    std::vector<uint8_t>::iterator l_itRcv;

    o_invalidFails_inTx_Ofendp1 = false;
    o_invalidFails_inRx_Ofendp2 = false;

    std::sort(io_endp1_txFaillanes.begin(), io_endp1_txFaillanes.end());
    std::sort(io_endp2_rxFaillanes.begin(), io_endp2_rxFaillanes.end());

    // Start with drive side fail lanes and check for matching lanes
    // on the recieve side
    l_itTmp = io_endp2_rxFaillanes.begin();

    for(l_itDrv = io_endp1_txFaillanes.begin();
        l_itDrv != io_endp1_txFaillanes.end();
        l_itDrv++)
    {
        l_it = std::lower_bound(io_endp2_rxFaillanes.begin(),
                                io_endp2_rxFaillanes.end(),
                                *l_itDrv);

        // If matching fail lane is not found on the receive side,
        // invalidate the drive side fail lane number
        if((l_it == io_endp2_rxFaillanes.end()) || (*l_it > *l_itDrv))
        {
            *l_itDrv = INVALID_FAIL_LANE_NUMBER;
            o_invalidFails_inTx_Ofendp1 = true;
        }
        else
        {
            // save the iterator for the next search
            l_itTmp = l_it;
        }
    }

    // Sort again as we might have invalidated some lanes
    std::sort(io_endp1_txFaillanes.begin(), io_endp1_txFaillanes.end());

    // Now, traverse through the receive side fail lanes and
    // check for matching lanes on the drive side
    for(l_itRcv = io_endp2_rxFaillanes.begin();
        ((l_itRcv <= l_itTmp) && (l_itRcv != io_endp2_rxFaillanes.end()));
        l_itRcv++)
    {
        l_it = std::lower_bound(io_endp1_txFaillanes.begin(),
                                io_endp1_txFaillanes.end(),
                                *l_itRcv);

        // If matching lane is not found on the driver side,
        // invalidate the receive side fail lane number
        if((l_it == io_endp1_txFaillanes.end()) || (*l_it > *l_itRcv))
        {
            *l_itRcv = INVALID_FAIL_LANE_NUMBER;
            o_invalidFails_inRx_Ofendp2 = true;
        }
    }

    // Need to invalidate all the entries beyond the last
    // lower bound of first search
    if(l_itTmp != io_endp2_rxFaillanes.end())
    {
        for(l_itTmp++; l_itTmp != io_endp2_rxFaillanes.end(); l_itTmp++)
        {
            *l_itTmp = INVALID_FAIL_LANE_NUMBER;
        }
    }
}

/**
 * @brief This function combines the eRepair lane numbers read from
 *        Manufacturing VPD and Field VPD
 *
 * @param [in]  i_mnfgFaillanes  The eRepair lane numbers read from the
 *                               Manufacturing VPD
 * @param [in]  i_fieldFaillanes The eRepair lane numbers read from the
 *                               Field VPD
 * @param [out] o_allFaillanes   The eRepair lane numbers which is the union
 *                               of the Field and Manufacturing eRepair lanes
 *                               passed as first iand second params
 *
 * @return void
 */
void combineFieldandMnfgLanes(std::vector<uint8_t>& i_mnfgFaillanes,
                              std::vector<uint8_t>& i_fieldFaillanes,
                              std::vector<uint8_t>& o_allFaillanes)
{
    std::vector<uint8_t>::iterator l_it;

    // Merge the Field and Mnfg fail lanes
    l_it = o_allFaillanes.begin();
    o_allFaillanes.insert(l_it,
                          i_mnfgFaillanes.begin(),
                          i_mnfgFaillanes.end());

    l_it = o_allFaillanes.end();
    o_allFaillanes.insert(l_it,
                          i_fieldFaillanes.begin(),
                          i_fieldFaillanes.end());

    // Check if Mfg VPD and Field VPD have same fail lanes.
    // If found, erase them
    std::sort(o_allFaillanes.begin(), o_allFaillanes.end());

    o_allFaillanes.erase(std::unique(o_allFaillanes.begin(),
                                     o_allFaillanes.end()),
                         o_allFaillanes.end());

}
<commit_msg>eRepair: Fix to invalidate vpd records correctly<commit_after>/* IBM_PROLOG_BEGIN_TAG                                                   */
/* This is an automatically generated prolog.                             */
/*                                                                        */
/* $Source: src/import/chips/p9/procedures/hwp/io/p9_io_erepairAccessorHwpFuncs.C $ */
/*                                                                        */
/* OpenPOWER HostBoot Project                                             */
/*                                                                        */
/* Contributors Listed Below - COPYRIGHT 2015,2019                        */
/* [+] International Business Machines Corp.                              */
/*                                                                        */
/*                                                                        */
/* Licensed under the Apache License, Version 2.0 (the "License");        */
/* you may not use this file except in compliance with the License.       */
/* You may obtain a copy of the License at                                */
/*                                                                        */
/*     http://www.apache.org/licenses/LICENSE-2.0                         */
/*                                                                        */
/* Unless required by applicable law or agreed to in writing, software    */
/* distributed under the License is distributed on an "AS IS" BASIS,      */
/* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or        */
/* implied. See the License for the specific language governing           */
/* permissions and limitations under the License.                         */
/*                                                                        */
/* IBM_PROLOG_END_TAG                                                     */
///
/// @file p9_io_erepairAccessorHwpFuncs.C
/// @brief FW Team utility functions that access fabric and memory eRepair data.
///
//----------------------------------------------------------------------------
#include <fapi2.H>
#include <string.h>
#include <p9_io_erepairConsts.H>

using namespace EREPAIR;
using namespace fapi2;

/***** Function definitions *****/

/**
 * @brief This function checks to see if the passed vectors have matching
 *        fail lane numbers. If no matching lane number is found, such lane
 *        value will be invalidated in the vector
 *
 * @param [in]  io_endp1_txFaillanes        Reference to vector which has fail
 *                                          lane numbers of Tx side
 * @param [in]  io_endp2_rxFaillanes        Reference to vector which has fail
 *                                          lane numbers of Rx side
 * @param [out] o_invalidFails_inTx_Ofendp1 If TRUE, indicates that Tx has fail
 *                                          lane numbers for which there is no
 *                                          matching entry on Rx side
 * @param [out] o_invalidFails_inRx_Ofendp2 If TRUE, indicates that Tx has fail
 *                                          lane numbers for which there is no
 *                                          matching entry on Tx side
 *
 * @return void
 */
void invalidateNonMatchingFailLanes(std::vector<uint8_t>& io_endp1_txFaillanes,
                                    std::vector<uint8_t>& io_endp2_rxFaillanes,
                                    bool& o_invalidFails_inTx_Ofendp1,
                                    bool& o_invalidFails_inRx_Ofendp2)
{
    std::vector<uint8_t>::iterator l_it;
    std::vector<uint8_t>::iterator l_itTmp;
    std::vector<uint8_t>::iterator l_itDrv;
    std::vector<uint8_t>::iterator l_itRcv;

    FAPI_INF("invalidateNonMatchingFailLanes - endp1Tx size:%llu endp2Rx size:%llu",
             io_endp1_txFaillanes.size(), io_endp2_rxFaillanes.size());

    for(unsigned long i = 0; (i < io_endp1_txFaillanes.size() && io_endp1_txFaillanes.size() != 0); i++)
    {
        FAPI_INF("Endp1Tx(%lu):%llu ", i, io_endp1_txFaillanes[i]);
    }

    for(unsigned long i = 0; (i < io_endp2_rxFaillanes.size() && io_endp2_rxFaillanes.size() != 0); i++)
    {
        FAPI_INF("Endp2Rx(%lu):%llu ", i, io_endp2_rxFaillanes[i]);
    }

    o_invalidFails_inTx_Ofendp1 = false;
    o_invalidFails_inRx_Ofendp2 = false;

    std::sort(io_endp1_txFaillanes.begin(), io_endp1_txFaillanes.end());
    std::sort(io_endp2_rxFaillanes.begin(), io_endp2_rxFaillanes.end());

    // Start with drive side fail lanes and check for matching lanes
    // on the recieve side
    l_itTmp = io_endp2_rxFaillanes.begin();

    for(l_itDrv = io_endp1_txFaillanes.begin();
        l_itDrv != io_endp1_txFaillanes.end();
        l_itDrv++)
    {
        l_it = std::lower_bound(io_endp2_rxFaillanes.begin(),
                                io_endp2_rxFaillanes.end(),
                                *l_itDrv);

        // If matching fail lane is not found on the receive side,
        // invalidate the drive side fail lane number
        if((l_it == io_endp2_rxFaillanes.end()) || (*l_it > *l_itDrv))
        {
            *l_itDrv = INVALID_FAIL_LANE_NUMBER;
            o_invalidFails_inTx_Ofendp1 = true;
        }
        else
        {
            // save the iterator for the next search
            l_itTmp = l_it;
        }
    }

    // Sort again as we might have invalidated some lanes
    std::sort(io_endp1_txFaillanes.begin(), io_endp1_txFaillanes.end());

    // Now, traverse through the receive side fail lanes and
    // check for matching lanes on the drive side
    for(l_itRcv = io_endp2_rxFaillanes.begin();
        ((l_itRcv <= l_itTmp) && (l_itRcv != io_endp2_rxFaillanes.end()));
        l_itRcv++)
    {
        l_it = std::lower_bound(io_endp1_txFaillanes.begin(),
                                io_endp1_txFaillanes.end(),
                                *l_itRcv);

        // If matching lane is not found on the driver side,
        // invalidate the receive side fail lane number
        if((l_it == io_endp1_txFaillanes.end()) || (*l_it > *l_itRcv))
        {
            *l_itRcv = INVALID_FAIL_LANE_NUMBER;
            o_invalidFails_inRx_Ofendp2 = true;
        }
    }

    // Need to invalidate all the entries beyond the last
    // lower bound of first search
    if(l_itTmp != io_endp2_rxFaillanes.end())
    {
        for(l_itTmp++; l_itTmp != io_endp2_rxFaillanes.end(); l_itTmp++)
        {
            *l_itTmp = INVALID_FAIL_LANE_NUMBER;
        }
    }
}

/**
 * @brief This function combines the eRepair lane numbers read from
 *        Manufacturing VPD and Field VPD
 *
 * @param [in]  i_mnfgFaillanes  The eRepair lane numbers read from the
 *                               Manufacturing VPD
 * @param [in]  i_fieldFaillanes The eRepair lane numbers read from the
 *                               Field VPD
 * @param [out] o_allFaillanes   The eRepair lane numbers which is the union
 *                               of the Field and Manufacturing eRepair lanes
 *                               passed as first iand second params
 *
 * @return void
 */
void combineFieldandMnfgLanes(std::vector<uint8_t>& i_mnfgFaillanes,
                              std::vector<uint8_t>& i_fieldFaillanes,
                              std::vector<uint8_t>& o_allFaillanes)
{
    std::vector<uint8_t>::iterator l_it;

    FAPI_INF("combineFieldandMnfgLanes - mnfgFailLanes size:%llu fieldFailLanes size:%llu",
             i_mnfgFaillanes.size(), i_fieldFaillanes.size());

    // Merge the Field and Mnfg fail lanes
    l_it = o_allFaillanes.begin();
    o_allFaillanes.insert(l_it,
                          i_mnfgFaillanes.begin(),
                          i_mnfgFaillanes.end());

    l_it = o_allFaillanes.end();
    o_allFaillanes.insert(l_it,
                          i_fieldFaillanes.begin(),
                          i_fieldFaillanes.end());

    // Check if Mfg VPD and Field VPD have same fail lanes.
    // If found, erase them
    std::sort(o_allFaillanes.begin(), o_allFaillanes.end());

    o_allFaillanes.erase(std::unique(o_allFaillanes.begin(),
                                     o_allFaillanes.end()),
                         o_allFaillanes.end());

}
<|endoftext|>


<commit_before>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
/**
 * @file navigator_rtl.cpp
 * Helper class to access RTL
 * @author Julian Oes <julian@oes.ch>
 * @author Anton Babushkin <anton.babushkin@me.com>
 */

#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>
#include <float.h>

#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>

#include <uORB/uORB.h>
#include <navigator/navigation.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vtol_vehicle_status.h>

#include "navigator.h"
#include "rtl.h"

#define DELAY_SIGMA	0.01f

RTL::RTL(Navigator *navigator, const char *name) :
	MissionBlock(navigator, name),
	_rtl_state(RTL_STATE_NONE),
	_param_return_alt(this, "RTL_RETURN_ALT", false),
	_param_min_loiter_alt(this, "MIS_LTRMIN_ALT", false),
	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
	_param_land_delay(this, "RTL_LAND_DELAY", false),
	_param_rtl_min_dist(this, "RTL_MIN_DIST", false)
{
	/* load initial params */
	updateParams();
	/* initial reset */
	on_inactive();
}

RTL::~RTL()
{
}

void
RTL::on_inactive()
{
	// reset RTL state
	_rtl_state = RTL_STATE_NONE;
}

float
RTL::get_rtl_altitude()
{
	return (_param_return_alt.get() < _navigator->get_land_detected()->alt_max) ? _param_return_alt.get() :
	       _navigator->get_land_detected()->alt_max;
}

void
RTL::on_activation()
{
	set_current_position_item(&_mission_item);
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->previous.valid = false;
	pos_sp_triplet->next.valid = false;

	/* for safety reasons don't go into RTL if landed */
	if (_navigator->get_land_detected()->landed) {
		_rtl_state = RTL_STATE_LANDED;
		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");

		/* if lower than return altitude, climb up first */

	} else if (_navigator->get_global_position()->alt < (_navigator->get_home_position()->alt
			+ get_rtl_altitude())) {
		_rtl_state = RTL_STATE_CLIMB;

		/* otherwise go straight to return */

	} else {
		/* set altitude setpoint to current altitude */
		_rtl_state = RTL_STATE_RETURN;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_global_position()->alt;
	}

	set_rtl_item();
}

void
RTL::on_active()
{
	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
		advance_rtl();
		set_rtl_item();
	}
}

void
RTL::set_rtl_item()
{
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	_navigator->set_can_loiter_at_sp(false);

	switch (_rtl_state) {
	case RTL_STATE_CLIMB: {

			// check if we are pretty close to home already
			float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
					  _navigator->get_home_position()->lon,
					  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

			// if we are close to home we do not climb as high, otherwise we climb to return alt
			float climb_alt = _navigator->get_home_position()->alt + get_rtl_altitude();

			// we are close to home, limit climb to min
			if (home_dist < _param_rtl_min_dist.get()) {
				climb_alt = _navigator->get_home_position()->alt + _param_descend_alt.get();
			}

			_mission_item.lat = _navigator->get_global_position()->lat;
			_mission_item.lon = _navigator->get_global_position()->lon;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = climb_alt;
			_mission_item.yaw = NAN;
			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
					 (int)(climb_alt),
					 (int)(climb_alt - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_RETURN: {
			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			// don't change altitude

			// use home yaw if close to home
			/* check if we are pretty close to home already */
			float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
					  _navigator->get_home_position()->lon,
					  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

			if (home_dist < _param_rtl_min_dist.get()) {
				_mission_item.yaw = _navigator->get_home_position()->yaw;

			} else {
				// use current heading to home
				_mission_item.yaw = get_bearing_to_next_waypoint(
							    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
							    _navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
			}

			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
					 (int)(_mission_item.altitude),
					 (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_TRANSITION_TO_MC: {
			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
			break;
		}

	case RTL_STATE_DESCEND: {
			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();

			// check if we are already lower - then we will just stay there
			if (_mission_item.altitude > _navigator->get_global_position()->alt) {
				_mission_item.altitude = _navigator->get_global_position()->alt;
			}

			_mission_item.yaw = _navigator->get_home_position()->yaw;

			// except for vtol which might be still off here and should point towards this location
			float d_current = get_distance_to_next_waypoint(
						  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
						  _mission_item.lat, _mission_item.lon);

			if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
				_mission_item.yaw = get_bearing_to_next_waypoint(
							    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
							    _mission_item.lat, _mission_item.lon);
			}

			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = false;
			_mission_item.origin = ORIGIN_ONBOARD;

			/* disable previous setpoint to prevent drift */
			pos_sp_triplet->previous.valid = false;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
					 (int)(_mission_item.altitude),
					 (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_LOITER: {
			bool autoland = _param_land_delay.get() > -DELAY_SIGMA;

			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			// don't change altitude
			_mission_item.yaw = _navigator->get_home_position()->yaw;
			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
			_mission_item.autocontinue = autoland;
			_mission_item.origin = ORIGIN_ONBOARD;

			_navigator->set_can_loiter_at_sp(true);

			if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
						 (double)Navigator::get_time_inside(_mission_item));

			} else {
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
			}

			break;
		}

	case RTL_STATE_LAND: {
			set_land_item(&_mission_item, false);
			_mission_item.yaw = _navigator->get_home_position()->yaw;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
			break;
		}

	case RTL_STATE_LANDED: {
			set_idle_item(&_mission_item);

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
			break;
		}

	default:
		break;
	}

	reset_mission_item_reached();

	/* execute command if set */
	if (!item_contains_position(&_mission_item)) {
		issue_command(&_mission_item);
	}

	/* convert mission item to current position setpoint and make it valid */
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->next.valid = false;

	_navigator->set_position_setpoint_triplet_updated();
}

void
RTL::advance_rtl()
{
	switch (_rtl_state) {
	case RTL_STATE_CLIMB:
		_rtl_state = RTL_STATE_RETURN;
		break;

	case RTL_STATE_RETURN:
		_rtl_state = RTL_STATE_DESCEND;

		if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) {
			_rtl_state = RTL_STATE_TRANSITION_TO_MC;
		}

		break;

	case RTL_STATE_TRANSITION_TO_MC:
		_rtl_state = RTL_STATE_RETURN;
		break;

	case RTL_STATE_DESCEND:

		/* only go to land if autoland is enabled */
		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
			_rtl_state = RTL_STATE_LOITER;

		} else {
			_rtl_state = RTL_STATE_LAND;
		}

		break;

	case RTL_STATE_LOITER:
		_rtl_state = RTL_STATE_LAND;
		break;

	case RTL_STATE_LAND:
		_rtl_state = RTL_STATE_LANDED;
		break;

	default:
		break;
	}
}
<commit_msg>Navigator: Fix RTL state handling by enabling auto-continue after descend<commit_after>/****************************************************************************
 *
 *   Copyright (c) 2013-2016 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
/**
 * @file navigator_rtl.cpp
 * Helper class to access RTL
 * @author Julian Oes <julian@oes.ch>
 * @author Anton Babushkin <anton.babushkin@me.com>
 */

#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <fcntl.h>
#include <float.h>

#include <systemlib/mavlink_log.h>
#include <systemlib/err.h>
#include <geo/geo.h>

#include <uORB/uORB.h>
#include <navigator/navigation.h>
#include <uORB/topics/home_position.h>
#include <uORB/topics/vtol_vehicle_status.h>

#include "navigator.h"
#include "rtl.h"

#define DELAY_SIGMA	0.01f

RTL::RTL(Navigator *navigator, const char *name) :
	MissionBlock(navigator, name),
	_rtl_state(RTL_STATE_NONE),
	_param_return_alt(this, "RTL_RETURN_ALT", false),
	_param_min_loiter_alt(this, "MIS_LTRMIN_ALT", false),
	_param_descend_alt(this, "RTL_DESCEND_ALT", false),
	_param_land_delay(this, "RTL_LAND_DELAY", false),
	_param_rtl_min_dist(this, "RTL_MIN_DIST", false)
{
	/* load initial params */
	updateParams();
	/* initial reset */
	on_inactive();
}

RTL::~RTL()
{
}

void
RTL::on_inactive()
{
	// reset RTL state
	_rtl_state = RTL_STATE_NONE;
}

float
RTL::get_rtl_altitude()
{
	return (_param_return_alt.get() < _navigator->get_land_detected()->alt_max) ? _param_return_alt.get() :
	       _navigator->get_land_detected()->alt_max;
}

void
RTL::on_activation()
{
	set_current_position_item(&_mission_item);
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->previous.valid = false;
	pos_sp_triplet->next.valid = false;

	/* for safety reasons don't go into RTL if landed */
	if (_navigator->get_land_detected()->landed) {
		_rtl_state = RTL_STATE_LANDED;
		mavlink_log_critical(_navigator->get_mavlink_log_pub(), "Already landed, not executing RTL");

		/* if lower than return altitude, climb up first */

	} else if (_navigator->get_global_position()->alt < (_navigator->get_home_position()->alt
			+ get_rtl_altitude())) {
		_rtl_state = RTL_STATE_CLIMB;

		/* otherwise go straight to return */

	} else {
		/* set altitude setpoint to current altitude */
		_rtl_state = RTL_STATE_RETURN;
		_mission_item.altitude_is_relative = false;
		_mission_item.altitude = _navigator->get_global_position()->alt;
	}

	set_rtl_item();
}

void
RTL::on_active()
{
	if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) {
		advance_rtl();
		set_rtl_item();
	}
}

void
RTL::set_rtl_item()
{
	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();

	_navigator->set_can_loiter_at_sp(false);

	switch (_rtl_state) {
	case RTL_STATE_CLIMB: {

			// check if we are pretty close to home already
			float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
					  _navigator->get_home_position()->lon,
					  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

			// if we are close to home we do not climb as high, otherwise we climb to return alt
			float climb_alt = _navigator->get_home_position()->alt + get_rtl_altitude();

			// we are close to home, limit climb to min
			if (home_dist < _param_rtl_min_dist.get()) {
				climb_alt = _navigator->get_home_position()->alt + _param_descend_alt.get();
			}

			_mission_item.lat = _navigator->get_global_position()->lat;
			_mission_item.lon = _navigator->get_global_position()->lon;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = climb_alt;
			_mission_item.yaw = NAN;
			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above home)",
					 (int)(climb_alt),
					 (int)(climb_alt - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_RETURN: {
			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			// don't change altitude

			// use home yaw if close to home
			/* check if we are pretty close to home already */
			float home_dist = get_distance_to_next_waypoint(_navigator->get_home_position()->lat,
					  _navigator->get_home_position()->lon,
					  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);

			if (home_dist < _param_rtl_min_dist.get()) {
				_mission_item.yaw = _navigator->get_home_position()->yaw;

			} else {
				// use current heading to home
				_mission_item.yaw = get_bearing_to_next_waypoint(
							    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
							    _navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
			}

			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above home)",
					 (int)(_mission_item.altitude),
					 (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_TRANSITION_TO_MC: {
			_mission_item.nav_cmd = NAV_CMD_DO_VTOL_TRANSITION;
			_mission_item.params[0] = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
			break;
		}

	case RTL_STATE_DESCEND: {
			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			_mission_item.altitude_is_relative = false;
			_mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get();

			// check if we are already lower - then we will just stay there
			if (_mission_item.altitude > _navigator->get_global_position()->alt) {
				_mission_item.altitude = _navigator->get_global_position()->alt;
			}

			_mission_item.yaw = _navigator->get_home_position()->yaw;

			// except for vtol which might be still off here and should point towards this location
			float d_current = get_distance_to_next_waypoint(
						  _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
						  _mission_item.lat, _mission_item.lon);

			if (_navigator->get_vstatus()->is_vtol && d_current > _navigator->get_acceptance_radius()) {
				_mission_item.yaw = get_bearing_to_next_waypoint(
							    _navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
							    _mission_item.lat, _mission_item.lon);
			}

			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = 0.0f;
			_mission_item.autocontinue = true;
			_mission_item.origin = ORIGIN_ONBOARD;

			/* disable previous setpoint to prevent drift */
			pos_sp_triplet->previous.valid = false;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above home)",
					 (int)(_mission_item.altitude),
					 (int)(_mission_item.altitude - _navigator->get_home_position()->alt));
			break;
		}

	case RTL_STATE_LOITER: {
			bool autoland = _param_land_delay.get() > -DELAY_SIGMA;

			_mission_item.lat = _navigator->get_home_position()->lat;
			_mission_item.lon = _navigator->get_home_position()->lon;
			// don't change altitude
			_mission_item.yaw = _navigator->get_home_position()->yaw;
			_mission_item.loiter_radius = _navigator->get_loiter_radius();
			_mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED;
			_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
			_mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get();
			_mission_item.autocontinue = autoland;
			_mission_item.origin = ORIGIN_ONBOARD;

			_navigator->set_can_loiter_at_sp(true);

			if (autoland && (Navigator::get_time_inside(_mission_item) > FLT_EPSILON)) {
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs",
						 (double)Navigator::get_time_inside(_mission_item));

			} else {
				mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loiter");
			}

			break;
		}

	case RTL_STATE_LAND: {
			set_land_item(&_mission_item, false);
			_mission_item.yaw = _navigator->get_home_position()->yaw;

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at home");
			break;
		}

	case RTL_STATE_LANDED: {
			set_idle_item(&_mission_item);

			mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, landed");
			break;
		}

	default:
		break;
	}

	reset_mission_item_reached();

	/* execute command if set */
	if (!item_contains_position(&_mission_item)) {
		issue_command(&_mission_item);
	}

	/* convert mission item to current position setpoint and make it valid */
	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
	pos_sp_triplet->next.valid = false;

	_navigator->set_position_setpoint_triplet_updated();
}

void
RTL::advance_rtl()
{
	switch (_rtl_state) {
	case RTL_STATE_CLIMB:
		_rtl_state = RTL_STATE_RETURN;
		break;

	case RTL_STATE_RETURN:
		_rtl_state = RTL_STATE_DESCEND;

		if (_navigator->get_vstatus()->is_vtol && !_navigator->get_vstatus()->is_rotary_wing) {
			_rtl_state = RTL_STATE_TRANSITION_TO_MC;
		}

		break;

	case RTL_STATE_TRANSITION_TO_MC:
		_rtl_state = RTL_STATE_RETURN;
		break;

	case RTL_STATE_DESCEND:

		/* only go to land if autoland is enabled */
		if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) {
			_rtl_state = RTL_STATE_LOITER;

		} else {
			_rtl_state = RTL_STATE_LAND;
		}

		break;

	case RTL_STATE_LOITER:
		_rtl_state = RTL_STATE_LAND;
		break;

	case RTL_STATE_LAND:
		_rtl_state = RTL_STATE_LANDED;
		break;

	default:
		break;
	}
}
<|endoftext|>


<commit_before>//
//  inputSystem.cpp
//  YorkshireTea
//
//  Created by Karl Jacques on 24/05/2014.
//
//

#include <SDL.h>
#include "inputSystem.h"
#include "../event/eventSystem.h"

InputSystem::InputSystem( EventSystem* eventSys, SDL_Window* window )
{
	m_EventSystem = eventSys;
	mWindow = window;
	mWindowActive = true;

	SDL_ShowCursor( false );

}

void InputSystem::update()
{
	while( SDL_PollEvent( &m_inputEvent ) )
	{
		switch(m_inputEvent.type )
		{
			case SDL_KEYDOWN:
			{
				// Key down, create and dispatch event
				KeyboardEvent*	e = new KeyboardEvent(EV_CORE_KEY_PRESS );
				e->mPressed = true;
				e->mReleased =false;
				e->mKeycode = m_inputEvent.key.keysym.scancode;
				m_EventSystem->dispatchEvent( e );

			}
			break;

			case SDL_KEYUP:
			{
				// Key down, create and dispatch event
				KeyboardEvent*	e = new KeyboardEvent(EV_CORE_KEY_RELEASE );
				e->mPressed = false;
				e->mReleased = true;
				e->mKeycode = m_inputEvent.key.keysym.scancode;
				m_EventSystem->dispatchEvent( e );

				//TODO Hack, so what.
				if( e->mKeycode == SDL_SCANCODE_COMMA )
					mWindowActive=!mWindowActive;

			}
			break;

			case SDL_TEXTINPUT:
				{
					KeyboardEvent*	e = new KeyboardEvent(EV_CORE_TEXT_INPUT);
					e->mKey = *m_inputEvent.text.text;
					m_EventSystem->dispatchEvent(e);
					break;
				}

		}
	}
	// TODO make this take half window values
	if( mWindowActive)
	{
		int MouseX,MouseY;
		SDL_GetMouseState(&MouseX,&MouseY);
		int DeltaX = MouseX - 400;
		int DeltaY = MouseY - 300;
		MouseEvent* e = new MouseEvent( EV_CORE_MOUSE_MOVEMENT );
		e->mMouseMoveX = DeltaX;
		e->mMouseMoveY = DeltaY;

		m_EventSystem->dispatchEvent( e );
		SDL_WarpMouseInWindow(  mWindow, 400, 300 );
	}
}
<commit_msg>Window focus taken into account when grabbing mouse<commit_after>//
//  inputSystem.cpp
//  YorkshireTea
//
//  Created by Karl Jacques on 24/05/2014.
//
//

#include <SDL.h>
#include "inputSystem.h"
#include "../event/eventSystem.h"

InputSystem::InputSystem( EventSystem* eventSys, SDL_Window* window )
{
	m_EventSystem = eventSys;
	mWindow = window;
	mWindowActive = true;

	SDL_ShowCursor( false );

}

void InputSystem::update()
{
	while( SDL_PollEvent( &m_inputEvent ) )
	{
		switch(m_inputEvent.type )
		{
			case SDL_WINDOWEVENT_FOCUS_GAINED:
				mWindowActive = true;
				break;

			case SDL_WINDOWEVENT_HIDDEN:
				mWindowActive = false;
				break;

			case SDL_KEYDOWN:
			{
				// Key down, create and dispatch event
				KeyboardEvent*	e = new KeyboardEvent(EV_CORE_KEY_PRESS );
				e->mPressed = true;
				e->mReleased =false;
				e->mKeycode = m_inputEvent.key.keysym.scancode;
				m_EventSystem->dispatchEvent( e );

			}
			break;

			case SDL_KEYUP:
				{
					// Key down, create and dispatch event
					KeyboardEvent*	e = new KeyboardEvent(EV_CORE_KEY_RELEASE );
					e->mPressed = false;
					e->mReleased = true;
					e->mKeycode = m_inputEvent.key.keysym.scancode;
					m_EventSystem->dispatchEvent( e );

				}
			break;

			case SDL_TEXTINPUT:
				{
					KeyboardEvent*	e = new KeyboardEvent(EV_CORE_TEXT_INPUT);
					e->mKey = *m_inputEvent.text.text;
					m_EventSystem->dispatchEvent(e);
					break;
				}

			case SDL_WINDOWEVENT:
				switch( m_inputEvent.window.event )
				{
				case SDL_WINDOWEVENT_FOCUS_GAINED:
					mWindowActive = true;
					break;

				case SDL_WINDOWEVENT_FOCUS_LOST:
					mWindowActive = false;
					break;
				}
				break;

		}
	}
	// TODO make this take half window values
	if( mWindowActive)
	{
		int MouseX,MouseY;
		SDL_GetMouseState(&MouseX,&MouseY);
		int DeltaX = MouseX - 400;
		int DeltaY = MouseY - 300;
		MouseEvent* e = new MouseEvent( EV_CORE_MOUSE_MOVEMENT );
		e->mMouseMoveX = DeltaX;
		e->mMouseMoveY = DeltaY;

		m_EventSystem->dispatchEvent( e );
		SDL_WarpMouseInWindow(  mWindow, 400, 300 );
	}
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2009, Gostai S.A.S.
 *
 * This software is provided "as is" without warranty of any kind,
 * either expressed or implied, including but not limited to the
 * implied warranties of fitness for a particular purpose.
 *
 * See the LICENSE file for more information.
 */
/**
 ** \file sched/job.hh
 ** \brief Definition of Job.
 */

#ifndef SCHED_JOB_HH
# define SCHED_JOB_HH

# include <iosfwd>
# include <list>

# include <boost/any.hpp>

# include <libport/symbol.hh>
# include <libport/utime.hh>

# include <sched/coroutine.hh>
# include <sched/export.hh>
# include <sched/fwd.hh>
# include <sched/tag.hh>

namespace sched
{

  enum job_state
  {
    to_start,            ///< Job needs to be started
    running,             ///< Job is waiting for the CPU
    sleeping,            ///< Job is sleeping until a specified deadline
    waiting,             ///< Job is waiting for changes to happen
    joining,             ///< Job is waiting for another job to terminate
    zombie,              ///< Job wants to be dead but isn't really yet
  };

  /// State names to string, for debugging purpose.
  const char* name(job_state);

  /// Dump \a s on \a o for debugging purpose.
  std::ostream& operator<<(std::ostream& o, job_state s);

  /// A Job represents a thread of control, implemented using coroutines.
  /// The scheduler decides which job to launch and resumes its execution.
  /// The lifetime of a job is the following
  ///
  /// \li   Job()                  : job creation
  ///
  /// \li   start_job()            : add job to the scheduler; the scheduler
  ///                                will call run(), and the job will
  ///                                yield() itself into the scheduler run
  ///                                queue during the course of work()
  ///
  /// \li   work()                 : this method, which must be overridden,
  ///                                does the real work
  ///
  /// Due to the way coroutines work, a job may not delete itself
  /// as once the coroutine structure has been freed, it is illegal to
  /// continue its execution, even only to switch to another coroutine.
  /// And if a job switches back to the scheduler before terminating its
  /// destructor, the coroutine structure will not be freed. Because of
  /// that, it is necessary for a job to be deleted by another one or
  /// by the scheduler.
  ///
  /// An additional constraint comes from the fact that even when a job
  /// has terminated its work, its structure may not always be deleted.
  /// For example, other jobs may have kept a reference to this job and
  /// expect to retrieve some information (such as a result) after the
  /// job termination.
  ///
  /// We have several ways of dealing with those constraints and not leaking
  /// memory upon a job termination:
  ///
  /// \li 1. Make sure the job spits out all the useful information concerning
  ///        its state by the way of inter-job communication before asking to
  ///        be deleted.
  ///
  /// \li  2. Make sure the job is kept alive as long as there is at least one
  ///         reference onto it, and that it gets deleted from another
  ///         coroutine, or from the main one.

  class SCHED_API Job: public libport::RefCounted
  {
  public:
    /// Create a job from another one.
    ///
    /// \param model The parent job. The scheduler and tags will be inherited
    //         from it.
    ///
    /// \param name The name of the new job, or a name derived from \a model
    ///        if none is provided.
    Job(const Job& model, const libport::Symbol& name);

    /// Create a new job.
    ///
    /// \param scheduler The scheduler to which this job will be attached.
    ///
    /// \param name The name of the new job, or an automatically created
    ///        one if none is provided.
    Job(Scheduler& scheduler, const libport::Symbol& name);

    /// Job destructor.
    ///
    /// The destructor is in charge of unscheduling the job in the
    /// scheduler.
    virtual ~Job();

    /// Get this job scheduler.
    ///
    /// \return A reference on the job scheduler.
    Scheduler& scheduler_get() const;

    /// Dump internal data (for the developper).
    ///
    /// \return \a o
    std::ostream& dump(std::ostream& o) const;

    /// Get the underlying coroutine corresponding to this job.
    ///
    /// \return The coroutine structure.
    Coro* coro_get() const;

    /// Has this job terminated?
    ///
    /// \return True if this job is in the \c zombie state.
    bool terminated() const;

    /// Start job by adding it to the scheduler.
    void start_job();

    /// Run the job. This function is called from the scheduler.
    void run();

    /// Terminate the job. The job will execute its cleanup method
    /// and inform the scheduler that it is ready to be destroyed.
    /// Must not be called by a child to an ancester of his.
    void terminate_now();

    /// Asynchronous termination.  Allows a child to kill its parent.
    /// Will be killed at the next cycle.
    void terminate_asap();

    /// Register this Job on its Scheduler so that it is rescheduled
    /// during the next cycle. This should be called from the
    /// currently scheduled job only but must be kept visible to be
    /// callable from the primitives.
    /// \sa yield_until(), yield_until_terminated(),
    /// yield_until_things_changed()
    void yield();

    /// As yield(), but ask not to be woken up before the deadline.
    /// \sa yield(), yield_until_terminated(), yield_until_things_changed()
    void yield_until(libport::utime_t deadline);

    /// Wait for another job to terminate before resuming execution of
    /// the current one. If the other job has already terminated, the
    /// caller will continue its execution.
    ///
    /// \param other The job to wait for. It is allowed to specify the
    ///        same Job as the target and the waiting job, in which case
    ///        the Job will only be woken up by an asynchronous exception.
    ///
    /// \sa yield(), yield_until(), yield_until_things_changed()
    void yield_until_terminated(Job& other);

    /// Same as \p yield_until_terminated above(), but wait for every
    /// job in the collection.
    void yield_until_terminated(const jobs_type& jobs);

    /// Wait for any other task to be scheduled.
    /// \sa yield(), yield_until_terminated(), yield_until()
    void yield_until_things_changed();

    /// Mark the current job as side-effect free.
    ///
    /// \param s True if the job is now side-effect free.
    ///
    /// Indicate whether the current state of a job may influence other
    /// parts of the system. This is used by the scheduler to choose
    /// whether other jobs need scheduling or not. The default value
    /// for \c side_effect_free is false.
    void side_effect_free_set(bool s);

    /// Is the current job side-effect free?
    ///
    /// \return True if the job is currently side-effect free.
    bool side_effect_free_get() const;

    /// Raise an exception next time this job will be resumed.
    ///
    /// \param e The exception to throw when the job will be scheduled
    ///        again.
    void async_throw(const exception& e);

    /// Maybe raise a deferred exception. Must be called from the scheduler
    /// while resuming the job execution. For example, StopException
    /// may be raised from here if the job has been blocked by a tag.
    void check_for_pending_exception();

    /// Get the job name
    ///
    /// \return The job name as set from the constructor.
    const libport::Symbol& name_get() const;

    /// Throw an exception if the stack space for this job is near
    /// exhaustion.
    void check_stack_space();

    /// Is the job frozen?
    ///
    /// \return This depends from the job tags state.
    virtual bool frozen() const = 0;

    /// Check if the job holds a tag.
    ///
    /// \return 0 if the job does not hold the tag,
    ///         the position of the tag in the tag stack
    ///         (starting from 1) otherwise.
    virtual size_t has_tag(const Tag& tag, size_t max_depth = (size_t)-1)
      const = 0;

    /// Get the current job state.
    ///
    /// \return The current job state.
    job_state state_get() const;

    /// Set the current job state
    ///
    /// \param state The new job state.
    void state_set(job_state state);

    /// Get this job deadline if it is sleeping.
    ///
    /// \return The date on which this job needs to be awoken.
    ///
    /// This function must not be called unless the job is in the
    /// \c sleeping state.
    libport::utime_t deadline_get() const;

    /// Check if the job can be interrupted.
    ///
    /// \return True if the job cannot be interrupted right now because
    ///         it is executing an atomic operation.
    bool non_interruptible_get() const;

    /// Indicate if the job can be interrupted or not
    ///
    /// \param ni True if the job must not be interrupted until further
    ///        notice.
    void non_interruptible_set(bool ni);

    /// Remember the time we have been frozen since if not remembered
    /// yet.
    ///
    /// \param current_time The current time.
    void notice_frozen(libport::utime_t current_time);

    /// Note that we are not frozen anymore.
    ///
    /// \param current_time The current time.
    void notice_not_frozen(libport::utime_t current_time);

    /// Return the origin since we have been frozen.
    ///
    /// \return 0 if we have not been frozen, the date since we have
    ///         been frozen otherwise.
    libport::utime_t frozen_since_get() const;

    /// Return the current time shift.
    ///
    /// \return The value to subtract from the system time to get the
    ///         unfrozen time of this runner.
    libport::utime_t time_shift_get() const;

    /// Set the current time shift.
    ///
    /// \param ts The new time shift. This should probably only be used
    ///           at runner creation time.
    void time_shift_set(libport::utime_t ts);

    /// Check and maybe register the fact that a tag has been stopped.
    ///
    /// \param tag The tag that has been stopped.
    ///
    /// \param payload The data to embed in the StopException.
    virtual void register_stopped_tag
      (const Tag& tag, const boost::any& payload);

    /// Check whether the job has a pending exception.
    ///
    /// \return true if the job has a pending exception.
    bool has_pending_exception() const;

    /// Get the current job priority.
    ///
    /// \return The job priority, as previously computed.
    virtual prio_type prio_get() const = 0;

    /// Ensure proper cleanup.
    virtual void terminate_cleanup();

    /// Number of jobs created and not yet destroyed.
    ///
    /// \return The number of jobs.
    static unsigned int alive_jobs();

    /// Helper to unregister all children upon destruction.
    class Collector: public jobs_type
    {
    public:
      typedef jobs_type super_type;
      /// Construct a child collector.
      ///
      /// \param parent Parent of the child to collect
      ///
      /// \param size Approximative previsional number of children.
      Collector(rJob parent, size_t);
      /// Terminate all children.
      ~Collector();

      /// If there are terminated children, collect them.
      void collect();

      /// Print, for debugging.
      std::ostream& dump(std::ostream& o) const;

    private:
      rJob parent_;
    };

    /// Register a child.
    ///
    /// \param child The child job.
    ///
    /// \param collector Children will be automatically terminated
    ///                  when this object is destroyed.
    void register_child(const rJob& child, Collector& collector);

    /// Terminate child and remove it from our children list.
    void terminate_child(const rJob& child);

    /// Do we have a parent?
    ///
    /// \return True if we have a parent.
    bool child_job() const;

    /// Whether \a this is an ancester of \a that.
    bool ancester_of(const rJob& that) const;

  protected:

    /// Must be implemented to do something useful. If an exception is
    /// raised, it will be propagated to our parent if we have one or
    /// lost otherwise.
    virtual void work() = 0;

    /// Must be overriden. Called if a scheduling error is detected
    /// during the execution course of this job.
    ///
    /// \param msg The explanation of the scheduling error.
    virtual void scheduling_error(const std::string& msg = "") = 0;

  private:
    /// Current job state, to be manipulated only from the job and the
    /// scheduler.
    job_state state_;

    /// Current job deadline. The deadline is only meaningful when the
    /// job state is \c sleeping.
    libport::utime_t deadline_;

    /// The last time we have been frozen (in system time), or 0 if we
    /// are not currently frozen.
    libport::utime_t frozen_since_;

    /// The value we have to deduce from system time because we have
    /// been frozen.
    libport::utime_t time_shift_;

    /// Scheduler in charge of this job. Do not delete.
    Scheduler& scheduler_;

    /// This job name.
    libport::Symbol name_;

    /// Other jobs to wake up when we terminate.
    jobs_type to_wake_up_;

    /// Coro structure corresponding to this job.
    Coro* coro_;

  protected:

    /// Is the current job non-interruptible? If yes, yielding will
    /// do nothing and blocking operations may raise an exception.
    bool non_interruptible_;

  private:

    /// Helper functions for constructors.
    void init_common(const libport::Symbol& name);

    /// Is the current job side-effect free?
    bool side_effect_free_;

    /// The next exception to be propagated if any.
    exception_ptr pending_exception_;

    /// Our parent if any.
    rJob parent_;

    /// Our children.
    jobs_type children_;

    /// Check stack space from time to time.
    bool check_stack_space_;

    /// Number of jobs created and not yet destroyed.
    static unsigned int alive_jobs_;

  protected:
    /// Exception used to terminate a job.
    struct SCHED_API TerminateException : public SchedulerException
    {
      COMPLETE_EXCEPTION(TerminateException)
    };

  };

  SCHED_API
  std::ostream& operator<< (std::ostream& o, const Job& j);

  SCHED_API
  std::ostream& operator<< (std::ostream& o, const jobs_type& js);

  SCHED_API
  std::ostream& operator<< (std::ostream& o, const Job::Collector& c);

  /// This exception will be raised to tell the job that it is currently
  /// stopped and must try to unwind tags from its tag stack until it
  /// is either dead or not stopped anymore.
  struct SCHED_API StopException : public SchedulerException
  {
    StopException(unsigned int, boost::any);
    ~StopException() throw() {}
    ADD_FIELD(unsigned int, depth)
    ADD_FIELD(boost::any, payload)
    COMPLETE_EXCEPTION(StopException)
  };

  /// This exception encapsulates another one, sent by a child job.
  struct SCHED_API ChildException : public SchedulerException
  {
    ChildException(exception_ptr);
    ChildException(const ChildException&);
    ~ChildException() throw() {}
    virtual exception_ptr clone() const;
    ATTRIBUTE_NORETURN void rethrow_child_exception() const;
  protected:
    ATTRIBUTE_NORETURN virtual void rethrow_() const;
  private:
    mutable exception_ptr child_exception_;
  };

  /// Terminate all jobs present in container and empty it.
  SCHED_API
  void terminate_jobs(jobs_type& jobs);

} // namespace sched

# include <sched/job.hxx>

#endif // !SCHED_JOB_HH
<commit_msg>sched: Job::Collector: better ctor.<commit_after>/*
 * Copyright (C) 2009, Gostai S.A.S.
 *
 * This software is provided "as is" without warranty of any kind,
 * either expressed or implied, including but not limited to the
 * implied warranties of fitness for a particular purpose.
 *
 * See the LICENSE file for more information.
 */
/**
 ** \file sched/job.hh
 ** \brief Definition of Job.
 */

#ifndef SCHED_JOB_HH
# define SCHED_JOB_HH

# include <iosfwd>
# include <list>

# include <boost/any.hpp>

# include <libport/symbol.hh>
# include <libport/utime.hh>

# include <sched/coroutine.hh>
# include <sched/export.hh>
# include <sched/fwd.hh>
# include <sched/tag.hh>

namespace sched
{

  enum job_state
  {
    to_start,            ///< Job needs to be started
    running,             ///< Job is waiting for the CPU
    sleeping,            ///< Job is sleeping until a specified deadline
    waiting,             ///< Job is waiting for changes to happen
    joining,             ///< Job is waiting for another job to terminate
    zombie,              ///< Job wants to be dead but isn't really yet
  };

  /// State names to string, for debugging purpose.
  const char* name(job_state);

  /// Dump \a s on \a o for debugging purpose.
  std::ostream& operator<<(std::ostream& o, job_state s);

  /// A Job represents a thread of control, implemented using coroutines.
  /// The scheduler decides which job to launch and resumes its execution.
  /// The lifetime of a job is the following
  ///
  /// \li   Job()                  : job creation
  ///
  /// \li   start_job()            : add job to the scheduler; the scheduler
  ///                                will call run(), and the job will
  ///                                yield() itself into the scheduler run
  ///                                queue during the course of work()
  ///
  /// \li   work()                 : this method, which must be overridden,
  ///                                does the real work
  ///
  /// Due to the way coroutines work, a job may not delete itself
  /// as once the coroutine structure has been freed, it is illegal to
  /// continue its execution, even only to switch to another coroutine.
  /// And if a job switches back to the scheduler before terminating its
  /// destructor, the coroutine structure will not be freed. Because of
  /// that, it is necessary for a job to be deleted by another one or
  /// by the scheduler.
  ///
  /// An additional constraint comes from the fact that even when a job
  /// has terminated its work, its structure may not always be deleted.
  /// For example, other jobs may have kept a reference to this job and
  /// expect to retrieve some information (such as a result) after the
  /// job termination.
  ///
  /// We have several ways of dealing with those constraints and not leaking
  /// memory upon a job termination:
  ///
  /// \li 1. Make sure the job spits out all the useful information concerning
  ///        its state by the way of inter-job communication before asking to
  ///        be deleted.
  ///
  /// \li  2. Make sure the job is kept alive as long as there is at least one
  ///         reference onto it, and that it gets deleted from another
  ///         coroutine, or from the main one.

  class SCHED_API Job: public libport::RefCounted
  {
  public:
    /// Create a job from another one.
    ///
    /// \param model The parent job. The scheduler and tags will be inherited
    //         from it.
    ///
    /// \param name The name of the new job, or a name derived from \a model
    ///        if none is provided.
    Job(const Job& model, const libport::Symbol& name);

    /// Create a new job.
    ///
    /// \param scheduler The scheduler to which this job will be attached.
    ///
    /// \param name The name of the new job, or an automatically created
    ///        one if none is provided.
    Job(Scheduler& scheduler, const libport::Symbol& name);

    /// Job destructor.
    ///
    /// The destructor is in charge of unscheduling the job in the
    /// scheduler.
    virtual ~Job();

    /// Get this job scheduler.
    ///
    /// \return A reference on the job scheduler.
    Scheduler& scheduler_get() const;

    /// Dump internal data (for the developper).
    ///
    /// \return \a o
    std::ostream& dump(std::ostream& o) const;

    /// Get the underlying coroutine corresponding to this job.
    ///
    /// \return The coroutine structure.
    Coro* coro_get() const;

    /// Has this job terminated?
    ///
    /// \return True if this job is in the \c zombie state.
    bool terminated() const;

    /// Start job by adding it to the scheduler.
    void start_job();

    /// Run the job. This function is called from the scheduler.
    void run();

    /// Terminate the job. The job will execute its cleanup method
    /// and inform the scheduler that it is ready to be destroyed.
    /// Must not be called by a child to an ancester of his.
    void terminate_now();

    /// Asynchronous termination.  Allows a child to kill its parent.
    /// Will be killed at the next cycle.
    void terminate_asap();

    /// Register this Job on its Scheduler so that it is rescheduled
    /// during the next cycle. This should be called from the
    /// currently scheduled job only but must be kept visible to be
    /// callable from the primitives.
    /// \sa yield_until(), yield_until_terminated(),
    /// yield_until_things_changed()
    void yield();

    /// As yield(), but ask not to be woken up before the deadline.
    /// \sa yield(), yield_until_terminated(), yield_until_things_changed()
    void yield_until(libport::utime_t deadline);

    /// Wait for another job to terminate before resuming execution of
    /// the current one. If the other job has already terminated, the
    /// caller will continue its execution.
    ///
    /// \param other The job to wait for. It is allowed to specify the
    ///        same Job as the target and the waiting job, in which case
    ///        the Job will only be woken up by an asynchronous exception.
    ///
    /// \sa yield(), yield_until(), yield_until_things_changed()
    void yield_until_terminated(Job& other);

    /// Same as \p yield_until_terminated above(), but wait for every
    /// job in the collection.
    void yield_until_terminated(const jobs_type& jobs);

    /// Wait for any other task to be scheduled.
    /// \sa yield(), yield_until_terminated(), yield_until()
    void yield_until_things_changed();

    /// Mark the current job as side-effect free.
    ///
    /// \param s True if the job is now side-effect free.
    ///
    /// Indicate whether the current state of a job may influence other
    /// parts of the system. This is used by the scheduler to choose
    /// whether other jobs need scheduling or not. The default value
    /// for \c side_effect_free is false.
    void side_effect_free_set(bool s);

    /// Is the current job side-effect free?
    ///
    /// \return True if the job is currently side-effect free.
    bool side_effect_free_get() const;

    /// Raise an exception next time this job will be resumed.
    ///
    /// \param e The exception to throw when the job will be scheduled
    ///        again.
    void async_throw(const exception& e);

    /// Maybe raise a deferred exception. Must be called from the scheduler
    /// while resuming the job execution. For example, StopException
    /// may be raised from here if the job has been blocked by a tag.
    void check_for_pending_exception();

    /// Get the job name
    ///
    /// \return The job name as set from the constructor.
    const libport::Symbol& name_get() const;

    /// Throw an exception if the stack space for this job is near
    /// exhaustion.
    void check_stack_space();

    /// Is the job frozen?
    ///
    /// \return This depends from the job tags state.
    virtual bool frozen() const = 0;

    /// Check if the job holds a tag.
    ///
    /// \return 0 if the job does not hold the tag,
    ///         the position of the tag in the tag stack
    ///         (starting from 1) otherwise.
    virtual size_t has_tag(const Tag& tag, size_t max_depth = (size_t)-1)
      const = 0;

    /// Get the current job state.
    ///
    /// \return The current job state.
    job_state state_get() const;

    /// Set the current job state
    ///
    /// \param state The new job state.
    void state_set(job_state state);

    /// Get this job deadline if it is sleeping.
    ///
    /// \return The date on which this job needs to be awoken.
    ///
    /// This function must not be called unless the job is in the
    /// \c sleeping state.
    libport::utime_t deadline_get() const;

    /// Check if the job can be interrupted.
    ///
    /// \return True if the job cannot be interrupted right now because
    ///         it is executing an atomic operation.
    bool non_interruptible_get() const;

    /// Indicate if the job can be interrupted or not
    ///
    /// \param ni True if the job must not be interrupted until further
    ///        notice.
    void non_interruptible_set(bool ni);

    /// Remember the time we have been frozen since if not remembered
    /// yet.
    ///
    /// \param current_time The current time.
    void notice_frozen(libport::utime_t current_time);

    /// Note that we are not frozen anymore.
    ///
    /// \param current_time The current time.
    void notice_not_frozen(libport::utime_t current_time);

    /// Return the origin since we have been frozen.
    ///
    /// \return 0 if we have not been frozen, the date since we have
    ///         been frozen otherwise.
    libport::utime_t frozen_since_get() const;

    /// Return the current time shift.
    ///
    /// \return The value to subtract from the system time to get the
    ///         unfrozen time of this runner.
    libport::utime_t time_shift_get() const;

    /// Set the current time shift.
    ///
    /// \param ts The new time shift. This should probably only be used
    ///           at runner creation time.
    void time_shift_set(libport::utime_t ts);

    /// Check and maybe register the fact that a tag has been stopped.
    ///
    /// \param tag The tag that has been stopped.
    ///
    /// \param payload The data to embed in the StopException.
    virtual void register_stopped_tag
      (const Tag& tag, const boost::any& payload);

    /// Check whether the job has a pending exception.
    ///
    /// \return true if the job has a pending exception.
    bool has_pending_exception() const;

    /// Get the current job priority.
    ///
    /// \return The job priority, as previously computed.
    virtual prio_type prio_get() const = 0;

    /// Ensure proper cleanup.
    virtual void terminate_cleanup();

    /// Number of jobs created and not yet destroyed.
    ///
    /// \return The number of jobs.
    static unsigned int alive_jobs();

    /// Helper to unregister all children upon destruction.
    class Collector: public jobs_type
    {
    public:
      typedef jobs_type super_type;
      /// Construct a child collector.
      ///
      /// \param parent Parent of the child to collect
      ///
      /// \param size Approximative previsional number of children.
      Collector(rJob parent, size_t = 0);
      /// Terminate all children.
      ~Collector();

      /// If there are terminated children, collect them.
      void collect();

      /// Print, for debugging.
      std::ostream& dump(std::ostream& o) const;

    private:
      rJob parent_;
    };

    /// Register a child.
    ///
    /// \param child The child job.
    ///
    /// \param collector Children will be automatically terminated
    ///                  when this object is destroyed.
    void register_child(const rJob& child, Collector& collector);

    /// Terminate child and remove it from our children list.
    void terminate_child(const rJob& child);

    /// Do we have a parent?
    ///
    /// \return True if we have a parent.
    bool child_job() const;

    /// Whether \a this is an ancester of \a that.
    bool ancester_of(const rJob& that) const;

  protected:

    /// Must be implemented to do something useful. If an exception is
    /// raised, it will be propagated to our parent if we have one or
    /// lost otherwise.
    virtual void work() = 0;

    /// Must be overriden. Called if a scheduling error is detected
    /// during the execution course of this job.
    ///
    /// \param msg The explanation of the scheduling error.
    virtual void scheduling_error(const std::string& msg = "") = 0;

  private:
    /// Current job state, to be manipulated only from the job and the
    /// scheduler.
    job_state state_;

    /// Current job deadline. The deadline is only meaningful when the
    /// job state is \c sleeping.
    libport::utime_t deadline_;

    /// The last time we have been frozen (in system time), or 0 if we
    /// are not currently frozen.
    libport::utime_t frozen_since_;

    /// The value we have to deduce from system time because we have
    /// been frozen.
    libport::utime_t time_shift_;

    /// Scheduler in charge of this job. Do not delete.
    Scheduler& scheduler_;

    /// This job name.
    libport::Symbol name_;

    /// Other jobs to wake up when we terminate.
    jobs_type to_wake_up_;

    /// Coro structure corresponding to this job.
    Coro* coro_;

  protected:

    /// Is the current job non-interruptible? If yes, yielding will
    /// do nothing and blocking operations may raise an exception.
    bool non_interruptible_;

  private:

    /// Helper functions for constructors.
    void init_common(const libport::Symbol& name);

    /// Is the current job side-effect free?
    bool side_effect_free_;

    /// The next exception to be propagated if any.
    exception_ptr pending_exception_;

    /// Our parent if any.
    rJob parent_;

    /// Our children.
    jobs_type children_;

    /// Check stack space from time to time.
    bool check_stack_space_;

    /// Number of jobs created and not yet destroyed.
    static unsigned int alive_jobs_;

  protected:
    /// Exception used to terminate a job.
    struct SCHED_API TerminateException : public SchedulerException
    {
      COMPLETE_EXCEPTION(TerminateException)
    };

  };

  SCHED_API
  std::ostream& operator<< (std::ostream& o, const Job& j);

  SCHED_API
  std::ostream& operator<< (std::ostream& o, const jobs_type& js);

  SCHED_API
  std::ostream& operator<< (std::ostream& o, const Job::Collector& c);

  /// This exception will be raised to tell the job that it is currently
  /// stopped and must try to unwind tags from its tag stack until it
  /// is either dead or not stopped anymore.
  struct SCHED_API StopException : public SchedulerException
  {
    StopException(unsigned int, boost::any);
    ~StopException() throw() {}
    ADD_FIELD(unsigned int, depth)
    ADD_FIELD(boost::any, payload)
    COMPLETE_EXCEPTION(StopException)
  };

  /// This exception encapsulates another one, sent by a child job.
  struct SCHED_API ChildException : public SchedulerException
  {
    ChildException(exception_ptr);
    ChildException(const ChildException&);
    ~ChildException() throw() {}
    virtual exception_ptr clone() const;
    ATTRIBUTE_NORETURN void rethrow_child_exception() const;
  protected:
    ATTRIBUTE_NORETURN virtual void rethrow_() const;
  private:
    mutable exception_ptr child_exception_;
  };

  /// Terminate all jobs present in container and empty it.
  SCHED_API
  void terminate_jobs(jobs_type& jobs);

} // namespace sched

# include <sched/job.hxx>

#endif // !SCHED_JOB_HH
<|endoftext|>