<commit_before>//===- BreakCriticalEdges.cpp - Critical Edge Elimination Pass ------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// BreakCriticalEdges pass - Break all of the critical edges in the CFG by
// inserting a dummy basic block.  This pass may be "required" by passes that
// cannot deal with critical edges.  For this usage, the structure type is
// forward declared.  This pass obviously invalidates the CFG, but can update
// dominator trees.
//
//===----------------------------------------------------------------------===//

#include "llvm/Transforms/Scalar.h"
#include "llvm/ADT/SmallVector.h"
#include "llvm/ADT/Statistic.h"
#include "llvm/Analysis/AliasAnalysis.h"
#include "llvm/Analysis/CFG.h"
#include "llvm/Analysis/LoopInfo.h"
#include "llvm/IR/CFG.h"
#include "llvm/IR/Dominators.h"
#include "llvm/IR/Function.h"
#include "llvm/IR/Instructions.h"
#include "llvm/IR/Type.h"
#include "llvm/Support/ErrorHandling.h"
#include "llvm/Transforms/Utils/BasicBlockUtils.h"
using namespace llvm;

#define DEBUG_TYPE "break-crit-edges"

STATISTIC(NumBroken, "Number of blocks inserted");

namespace {
  struct BreakCriticalEdges : public FunctionPass {
    static char ID; // Pass identification, replacement for typeid
    BreakCriticalEdges() : FunctionPass(ID) {
      initializeBreakCriticalEdgesPass(*PassRegistry::getPassRegistry());
    }

    bool runOnFunction(Function &F) override {
      unsigned N = SplitAllCriticalEdges(F, this);
      NumBroken += N;
      return N > 0;
    }

    void getAnalysisUsage(AnalysisUsage &AU) const override {
      AU.addPreserved<DominatorTreeWrapperPass>();
      AU.addPreserved<LoopInfoWrapperPass>();

      // No loop canonicalization guarantees are broken by this pass.
      AU.addPreservedID(LoopSimplifyID);
    }
  };
}

char BreakCriticalEdges::ID = 0;
INITIALIZE_PASS(BreakCriticalEdges, "break-crit-edges",
                "Break critical edges in CFG", false, false)

// Publicly exposed interface to pass...
char &llvm::BreakCriticalEdgesID = BreakCriticalEdges::ID;
FunctionPass *llvm::createBreakCriticalEdgesPass() {
  return new BreakCriticalEdges();
}

//===----------------------------------------------------------------------===//
//    Implementation of the external critical edge manipulation functions
//===----------------------------------------------------------------------===//

/// createPHIsForSplitLoopExit - When a loop exit edge is split, LCSSA form
/// may require new PHIs in the new exit block. This function inserts the
/// new PHIs, as needed. Preds is a list of preds inside the loop, SplitBB
/// is the new loop exit block, and DestBB is the old loop exit, now the
/// successor of SplitBB.
static void createPHIsForSplitLoopExit(ArrayRef<BasicBlock *> Preds,
                                       BasicBlock *SplitBB,
                                       BasicBlock *DestBB) {
  // SplitBB shouldn't have anything non-trivial in it yet.
  assert((SplitBB->getFirstNonPHI() == SplitBB->getTerminator() ||
          SplitBB->isLandingPad()) && "SplitBB has non-PHI nodes!");

  // For each PHI in the destination block.
  for (BasicBlock::iterator I = DestBB->begin();
       PHINode *PN = dyn_cast<PHINode>(I); ++I) {
    unsigned Idx = PN->getBasicBlockIndex(SplitBB);
    Value *V = PN->getIncomingValue(Idx);

    // If the input is a PHI which already satisfies LCSSA, don't create
    // a new one.
    if (const PHINode *VP = dyn_cast<PHINode>(V))
      if (VP->getParent() == SplitBB)
        continue;

    // Otherwise a new PHI is needed. Create one and populate it.
    PHINode *NewPN =
      PHINode::Create(PN->getType(), Preds.size(), "split",
                      SplitBB->isLandingPad() ?
                      SplitBB->begin() : SplitBB->getTerminator());
    for (unsigned i = 0, e = Preds.size(); i != e; ++i)
      NewPN->addIncoming(V, Preds[i]);

    // Update the original PHI.
    PN->setIncomingValue(Idx, NewPN);
  }
}

/// SplitCriticalEdge - If this edge is a critical edge, insert a new node to
/// split the critical edge.  This will update DominatorTree information if it
/// is available, thus calling this pass will not invalidate either of them.
/// This returns the new block if the edge was split, null otherwise.
///
/// If MergeIdenticalEdges is true (not the default), *all* edges from TI to the
/// specified successor will be merged into the same critical edge block.
/// This is most commonly interesting with switch instructions, which may
/// have many edges to any one destination.  This ensures that all edges to that
/// dest go to one block instead of each going to a different block, but isn't
/// the standard definition of a "critical edge".
///
/// It is invalid to call this function on a critical edge that starts at an
/// IndirectBrInst.  Splitting these edges will almost always create an invalid
/// program because the address of the new block won't be the one that is jumped
/// to.
///
BasicBlock *llvm::SplitCriticalEdge(TerminatorInst *TI, unsigned SuccNum,
                                    Pass *P, bool MergeIdenticalEdges,
                                    bool DontDeleteUselessPhis,
                                    bool SplitLandingPads) {
  if (!isCriticalEdge(TI, SuccNum, MergeIdenticalEdges)) return nullptr;

  assert(!isa<IndirectBrInst>(TI) &&
         "Cannot split critical edge from IndirectBrInst");

  BasicBlock *TIBB = TI->getParent();
  BasicBlock *DestBB = TI->getSuccessor(SuccNum);

  // Splitting the critical edge to a landing pad block is non-trivial. Don't do
  // it in this generic function.
  if (DestBB->isLandingPad()) return nullptr;

  // Create a new basic block, linking it into the CFG.
  BasicBlock *NewBB = BasicBlock::Create(TI->getContext(),
                      TIBB->getName() + "." + DestBB->getName() + "_crit_edge");
  // Create our unconditional branch.
  BranchInst *NewBI = BranchInst::Create(DestBB, NewBB);
  NewBI->setDebugLoc(TI->getDebugLoc());

  // Branch to the new block, breaking the edge.
  TI->setSuccessor(SuccNum, NewBB);

  // Insert the block into the function... right after the block TI lives in.
  Function &F = *TIBB->getParent();
  Function::iterator FBBI = TIBB;
  F.getBasicBlockList().insert(++FBBI, NewBB);

  // If there are any PHI nodes in DestBB, we need to update them so that they
  // merge incoming values from NewBB instead of from TIBB.
  {
    unsigned BBIdx = 0;
    for (BasicBlock::iterator I = DestBB->begin(); isa<PHINode>(I); ++I) {
      // We no longer enter through TIBB, now we come in through NewBB.
      // Revector exactly one entry in the PHI node that used to come from
      // TIBB to come from NewBB.
      PHINode *PN = cast<PHINode>(I);

      // Reuse the previous value of BBIdx if it lines up.  In cases where we
      // have multiple phi nodes with *lots* of predecessors, this is a speed
      // win because we don't have to scan the PHI looking for TIBB.  This
      // happens because the BB list of PHI nodes are usually in the same
      // order.
      if (PN->getIncomingBlock(BBIdx) != TIBB)
        BBIdx = PN->getBasicBlockIndex(TIBB);
      PN->setIncomingBlock(BBIdx, NewBB);
    }
  }

  // If there are any other edges from TIBB to DestBB, update those to go
  // through the split block, making those edges non-critical as well (and
  // reducing the number of phi entries in the DestBB if relevant).
  if (MergeIdenticalEdges) {
    for (unsigned i = SuccNum+1, e = TI->getNumSuccessors(); i != e; ++i) {
      if (TI->getSuccessor(i) != DestBB) continue;

      // Remove an entry for TIBB from DestBB phi nodes.
      DestBB->removePredecessor(TIBB, DontDeleteUselessPhis);

      // We found another edge to DestBB, go to NewBB instead.
      TI->setSuccessor(i, NewBB);
    }
  }

  // If we don't have a pass object, we can't update anything...
  if (!P) return NewBB;

  auto *AA = P->getAnalysisIfAvailable<AliasAnalysis>();
  DominatorTreeWrapperPass *DTWP =
      P->getAnalysisIfAvailable<DominatorTreeWrapperPass>();
  DominatorTree *DT = DTWP ? &DTWP->getDomTree() : nullptr;
  auto *LIWP = P->getAnalysisIfAvailable<LoopInfoWrapperPass>();
  LoopInfo *LI = LIWP ? &LIWP->getLoopInfo() : nullptr;
  bool PreserveLCSSA = P->mustPreserveAnalysisID(LCSSAID);

  // If we have nothing to update, just return.
  if (!DT && !LI)
    return NewBB;

  // Now update analysis information.  Since the only predecessor of NewBB is
  // the TIBB, TIBB clearly dominates NewBB.  TIBB usually doesn't dominate
  // anything, as there are other successors of DestBB.  However, if all other
  // predecessors of DestBB are already dominated by DestBB (e.g. DestBB is a
  // loop header) then NewBB dominates DestBB.
  SmallVector<BasicBlock*, 8> OtherPreds;

  // If there is a PHI in the block, loop over predecessors with it, which is
  // faster than iterating pred_begin/end.
  if (PHINode *PN = dyn_cast<PHINode>(DestBB->begin())) {
    for (unsigned i = 0, e = PN->getNumIncomingValues(); i != e; ++i)
      if (PN->getIncomingBlock(i) != NewBB)
        OtherPreds.push_back(PN->getIncomingBlock(i));
  } else {
    for (pred_iterator I = pred_begin(DestBB), E = pred_end(DestBB);
         I != E; ++I) {
      BasicBlock *P = *I;
      if (P != NewBB)
        OtherPreds.push_back(P);
    }
  }

  bool NewBBDominatesDestBB = true;

  // Should we update DominatorTree information?
  if (DT) {
    DomTreeNode *TINode = DT->getNode(TIBB);

    // The new block is not the immediate dominator for any other nodes, but
    // TINode is the immediate dominator for the new node.
    //
    if (TINode) {       // Don't break unreachable code!
      DomTreeNode *NewBBNode = DT->addNewBlock(NewBB, TIBB);
      DomTreeNode *DestBBNode = nullptr;

      // If NewBBDominatesDestBB hasn't been computed yet, do so with DT.
      if (!OtherPreds.empty()) {
        DestBBNode = DT->getNode(DestBB);
        while (!OtherPreds.empty() && NewBBDominatesDestBB) {
          if (DomTreeNode *OPNode = DT->getNode(OtherPreds.back()))
            NewBBDominatesDestBB = DT->dominates(DestBBNode, OPNode);
          OtherPreds.pop_back();
        }
        OtherPreds.clear();
      }

      // If NewBBDominatesDestBB, then NewBB dominates DestBB, otherwise it
      // doesn't dominate anything.
      if (NewBBDominatesDestBB) {
        if (!DestBBNode) DestBBNode = DT->getNode(DestBB);
        DT->changeImmediateDominator(DestBBNode, NewBBNode);
      }
    }
  }

  // Update LoopInfo if it is around.
  if (LI) {
    if (Loop *TIL = LI->getLoopFor(TIBB)) {
      // If one or the other blocks were not in a loop, the new block is not
      // either, and thus LI doesn't need to be updated.
      if (Loop *DestLoop = LI->getLoopFor(DestBB)) {
        if (TIL == DestLoop) {
          // Both in the same loop, the NewBB joins loop.
          DestLoop->addBasicBlockToLoop(NewBB, *LI);
        } else if (TIL->contains(DestLoop)) {
          // Edge from an outer loop to an inner loop.  Add to the outer loop.
          TIL->addBasicBlockToLoop(NewBB, *LI);
        } else if (DestLoop->contains(TIL)) {
          // Edge from an inner loop to an outer loop.  Add to the outer loop.
          DestLoop->addBasicBlockToLoop(NewBB, *LI);
        } else {
          // Edge from two loops with no containment relation.  Because these
          // are natural loops, we know that the destination block must be the
          // header of its loop (adding a branch into a loop elsewhere would
          // create an irreducible loop).
          assert(DestLoop->getHeader() == DestBB &&
                 "Should not create irreducible loops!");
          if (Loop *P = DestLoop->getParentLoop())
            P->addBasicBlockToLoop(NewBB, *LI);
        }
      }
      // If TIBB is in a loop and DestBB is outside of that loop, we may need
      // to update LoopSimplify form and LCSSA form.
      if (!TIL->contains(DestBB) &&
          P->mustPreserveAnalysisID(LoopSimplifyID)) {
        assert(!TIL->contains(NewBB) &&
               "Split point for loop exit is contained in loop!");

        // Update LCSSA form in the newly created exit block.
        if (PreserveLCSSA)
          createPHIsForSplitLoopExit(TIBB, NewBB, DestBB);

        // The only that we can break LoopSimplify form by splitting a critical
        // edge is if after the split there exists some edge from TIL to DestBB
        // *and* the only edge into DestBB from outside of TIL is that of
        // NewBB. If the first isn't true, then LoopSimplify still holds, NewBB
        // is the new exit block and it has no non-loop predecessors. If the
        // second isn't true, then DestBB was not in LoopSimplify form prior to
        // the split as it had a non-loop predecessor. In both of these cases,
        // the predecessor must be directly in TIL, not in a subloop, or again
        // LoopSimplify doesn't hold.
        SmallVector<BasicBlock *, 4> LoopPreds;
        for (pred_iterator I = pred_begin(DestBB), E = pred_end(DestBB); I != E;
             ++I) {
          BasicBlock *P = *I;
          if (P == NewBB)
            continue; // The new block is known.
          if (LI->getLoopFor(P) != TIL) {
            // No need to re-simplify, it wasn't to start with.
            LoopPreds.clear();
            break;
          }
          LoopPreds.push_back(P);
        }
        if (!LoopPreds.empty()) {
          assert(!DestBB->isLandingPad() &&
                 "We don't split edges to landing pads!");
          BasicBlock *NewExitBB = SplitBlockPredecessors(
              DestBB, LoopPreds, "split", AA, DT, LI, PreserveLCSSA);
          if (PreserveLCSSA)
            createPHIsForSplitLoopExit(LoopPreds, NewExitBB, DestBB);
        }
      }
      // LCSSA form was updated above for the case where LoopSimplify is
      // available, which means that all predecessors of loop exit blocks
      // are within the loop. Without LoopSimplify form, it would be
      // necessary to insert a new phi.
      assert((!PreserveLCSSA || P->mustPreserveAnalysisID(LoopSimplifyID)) &&
             "SplitCriticalEdge doesn't know how to update LCCSA form "
             "without LoopSimplify!");
    }
  }

  return NewBB;
}
<commit_msg>[PM] Relax asserts and always try to reconstruct loop simplify form when we can while splitting critical edges.<commit_after>//===- BreakCriticalEdges.cpp - Critical Edge Elimination Pass ------------===//
//
//                     The LLVM Compiler Infrastructure
//
// This file is distributed under the University of Illinois Open Source
// License. See LICENSE.TXT for details.
//
//===----------------------------------------------------------------------===//
//
// BreakCriticalEdges pass - Break all of the critical edges in the CFG by
// inserting a dummy basic block.  This pass may be "required" by passes that
// cannot deal with critical edges.  For this usage, the structure type is
// forward declared.  This pass obviously invalidates the CFG, but can update
// dominator trees.
//
//===----------------------------------------------------------------------===//

#include "llvm/Transforms/Scalar.h"
#include "llvm/ADT/SmallVector.h"
#include "llvm/ADT/Statistic.h"
#include "llvm/Analysis/AliasAnalysis.h"
#include "llvm/Analysis/CFG.h"
#include "llvm/Analysis/LoopInfo.h"
#include "llvm/IR/CFG.h"
#include "llvm/IR/Dominators.h"
#include "llvm/IR/Function.h"
#include "llvm/IR/Instructions.h"
#include "llvm/IR/Type.h"
#include "llvm/Support/ErrorHandling.h"
#include "llvm/Transforms/Utils/BasicBlockUtils.h"
using namespace llvm;

#define DEBUG_TYPE "break-crit-edges"

STATISTIC(NumBroken, "Number of blocks inserted");

namespace {
  struct BreakCriticalEdges : public FunctionPass {
    static char ID; // Pass identification, replacement for typeid
    BreakCriticalEdges() : FunctionPass(ID) {
      initializeBreakCriticalEdgesPass(*PassRegistry::getPassRegistry());
    }

    bool runOnFunction(Function &F) override {
      unsigned N = SplitAllCriticalEdges(F, this);
      NumBroken += N;
      return N > 0;
    }

    void getAnalysisUsage(AnalysisUsage &AU) const override {
      AU.addPreserved<DominatorTreeWrapperPass>();
      AU.addPreserved<LoopInfoWrapperPass>();

      // No loop canonicalization guarantees are broken by this pass.
      AU.addPreservedID(LoopSimplifyID);
    }
  };
}

char BreakCriticalEdges::ID = 0;
INITIALIZE_PASS(BreakCriticalEdges, "break-crit-edges",
                "Break critical edges in CFG", false, false)

// Publicly exposed interface to pass...
char &llvm::BreakCriticalEdgesID = BreakCriticalEdges::ID;
FunctionPass *llvm::createBreakCriticalEdgesPass() {
  return new BreakCriticalEdges();
}

//===----------------------------------------------------------------------===//
//    Implementation of the external critical edge manipulation functions
//===----------------------------------------------------------------------===//

/// createPHIsForSplitLoopExit - When a loop exit edge is split, LCSSA form
/// may require new PHIs in the new exit block. This function inserts the
/// new PHIs, as needed. Preds is a list of preds inside the loop, SplitBB
/// is the new loop exit block, and DestBB is the old loop exit, now the
/// successor of SplitBB.
static void createPHIsForSplitLoopExit(ArrayRef<BasicBlock *> Preds,
                                       BasicBlock *SplitBB,
                                       BasicBlock *DestBB) {
  // SplitBB shouldn't have anything non-trivial in it yet.
  assert((SplitBB->getFirstNonPHI() == SplitBB->getTerminator() ||
          SplitBB->isLandingPad()) && "SplitBB has non-PHI nodes!");

  // For each PHI in the destination block.
  for (BasicBlock::iterator I = DestBB->begin();
       PHINode *PN = dyn_cast<PHINode>(I); ++I) {
    unsigned Idx = PN->getBasicBlockIndex(SplitBB);
    Value *V = PN->getIncomingValue(Idx);

    // If the input is a PHI which already satisfies LCSSA, don't create
    // a new one.
    if (const PHINode *VP = dyn_cast<PHINode>(V))
      if (VP->getParent() == SplitBB)
        continue;

    // Otherwise a new PHI is needed. Create one and populate it.
    PHINode *NewPN =
      PHINode::Create(PN->getType(), Preds.size(), "split",
                      SplitBB->isLandingPad() ?
                      SplitBB->begin() : SplitBB->getTerminator());
    for (unsigned i = 0, e = Preds.size(); i != e; ++i)
      NewPN->addIncoming(V, Preds[i]);

    // Update the original PHI.
    PN->setIncomingValue(Idx, NewPN);
  }
}

/// SplitCriticalEdge - If this edge is a critical edge, insert a new node to
/// split the critical edge.  This will update DominatorTree information if it
/// is available, thus calling this pass will not invalidate either of them.
/// This returns the new block if the edge was split, null otherwise.
///
/// If MergeIdenticalEdges is true (not the default), *all* edges from TI to the
/// specified successor will be merged into the same critical edge block.
/// This is most commonly interesting with switch instructions, which may
/// have many edges to any one destination.  This ensures that all edges to that
/// dest go to one block instead of each going to a different block, but isn't
/// the standard definition of a "critical edge".
///
/// It is invalid to call this function on a critical edge that starts at an
/// IndirectBrInst.  Splitting these edges will almost always create an invalid
/// program because the address of the new block won't be the one that is jumped
/// to.
///
BasicBlock *llvm::SplitCriticalEdge(TerminatorInst *TI, unsigned SuccNum,
                                    Pass *P, bool MergeIdenticalEdges,
                                    bool DontDeleteUselessPhis,
                                    bool SplitLandingPads) {
  if (!isCriticalEdge(TI, SuccNum, MergeIdenticalEdges)) return nullptr;

  assert(!isa<IndirectBrInst>(TI) &&
         "Cannot split critical edge from IndirectBrInst");

  BasicBlock *TIBB = TI->getParent();
  BasicBlock *DestBB = TI->getSuccessor(SuccNum);

  // Splitting the critical edge to a landing pad block is non-trivial. Don't do
  // it in this generic function.
  if (DestBB->isLandingPad()) return nullptr;

  // Create a new basic block, linking it into the CFG.
  BasicBlock *NewBB = BasicBlock::Create(TI->getContext(),
                      TIBB->getName() + "." + DestBB->getName() + "_crit_edge");
  // Create our unconditional branch.
  BranchInst *NewBI = BranchInst::Create(DestBB, NewBB);
  NewBI->setDebugLoc(TI->getDebugLoc());

  // Branch to the new block, breaking the edge.
  TI->setSuccessor(SuccNum, NewBB);

  // Insert the block into the function... right after the block TI lives in.
  Function &F = *TIBB->getParent();
  Function::iterator FBBI = TIBB;
  F.getBasicBlockList().insert(++FBBI, NewBB);

  // If there are any PHI nodes in DestBB, we need to update them so that they
  // merge incoming values from NewBB instead of from TIBB.
  {
    unsigned BBIdx = 0;
    for (BasicBlock::iterator I = DestBB->begin(); isa<PHINode>(I); ++I) {
      // We no longer enter through TIBB, now we come in through NewBB.
      // Revector exactly one entry in the PHI node that used to come from
      // TIBB to come from NewBB.
      PHINode *PN = cast<PHINode>(I);

      // Reuse the previous value of BBIdx if it lines up.  In cases where we
      // have multiple phi nodes with *lots* of predecessors, this is a speed
      // win because we don't have to scan the PHI looking for TIBB.  This
      // happens because the BB list of PHI nodes are usually in the same
      // order.
      if (PN->getIncomingBlock(BBIdx) != TIBB)
        BBIdx = PN->getBasicBlockIndex(TIBB);
      PN->setIncomingBlock(BBIdx, NewBB);
    }
  }

  // If there are any other edges from TIBB to DestBB, update those to go
  // through the split block, making those edges non-critical as well (and
  // reducing the number of phi entries in the DestBB if relevant).
  if (MergeIdenticalEdges) {
    for (unsigned i = SuccNum+1, e = TI->getNumSuccessors(); i != e; ++i) {
      if (TI->getSuccessor(i) != DestBB) continue;

      // Remove an entry for TIBB from DestBB phi nodes.
      DestBB->removePredecessor(TIBB, DontDeleteUselessPhis);

      // We found another edge to DestBB, go to NewBB instead.
      TI->setSuccessor(i, NewBB);
    }
  }

  // If we don't have a pass object, we can't update anything...
  if (!P) return NewBB;

  auto *AA = P->getAnalysisIfAvailable<AliasAnalysis>();
  DominatorTreeWrapperPass *DTWP =
      P->getAnalysisIfAvailable<DominatorTreeWrapperPass>();
  DominatorTree *DT = DTWP ? &DTWP->getDomTree() : nullptr;
  auto *LIWP = P->getAnalysisIfAvailable<LoopInfoWrapperPass>();
  LoopInfo *LI = LIWP ? &LIWP->getLoopInfo() : nullptr;
  bool PreserveLCSSA = P->mustPreserveAnalysisID(LCSSAID);

  // If we have nothing to update, just return.
  if (!DT && !LI)
    return NewBB;

  // Now update analysis information.  Since the only predecessor of NewBB is
  // the TIBB, TIBB clearly dominates NewBB.  TIBB usually doesn't dominate
  // anything, as there are other successors of DestBB.  However, if all other
  // predecessors of DestBB are already dominated by DestBB (e.g. DestBB is a
  // loop header) then NewBB dominates DestBB.
  SmallVector<BasicBlock*, 8> OtherPreds;

  // If there is a PHI in the block, loop over predecessors with it, which is
  // faster than iterating pred_begin/end.
  if (PHINode *PN = dyn_cast<PHINode>(DestBB->begin())) {
    for (unsigned i = 0, e = PN->getNumIncomingValues(); i != e; ++i)
      if (PN->getIncomingBlock(i) != NewBB)
        OtherPreds.push_back(PN->getIncomingBlock(i));
  } else {
    for (pred_iterator I = pred_begin(DestBB), E = pred_end(DestBB);
         I != E; ++I) {
      BasicBlock *P = *I;
      if (P != NewBB)
        OtherPreds.push_back(P);
    }
  }

  bool NewBBDominatesDestBB = true;

  // Should we update DominatorTree information?
  if (DT) {
    DomTreeNode *TINode = DT->getNode(TIBB);

    // The new block is not the immediate dominator for any other nodes, but
    // TINode is the immediate dominator for the new node.
    //
    if (TINode) {       // Don't break unreachable code!
      DomTreeNode *NewBBNode = DT->addNewBlock(NewBB, TIBB);
      DomTreeNode *DestBBNode = nullptr;

      // If NewBBDominatesDestBB hasn't been computed yet, do so with DT.
      if (!OtherPreds.empty()) {
        DestBBNode = DT->getNode(DestBB);
        while (!OtherPreds.empty() && NewBBDominatesDestBB) {
          if (DomTreeNode *OPNode = DT->getNode(OtherPreds.back()))
            NewBBDominatesDestBB = DT->dominates(DestBBNode, OPNode);
          OtherPreds.pop_back();
        }
        OtherPreds.clear();
      }

      // If NewBBDominatesDestBB, then NewBB dominates DestBB, otherwise it
      // doesn't dominate anything.
      if (NewBBDominatesDestBB) {
        if (!DestBBNode) DestBBNode = DT->getNode(DestBB);
        DT->changeImmediateDominator(DestBBNode, NewBBNode);
      }
    }
  }

  // Update LoopInfo if it is around.
  if (LI) {
    if (Loop *TIL = LI->getLoopFor(TIBB)) {
      // If one or the other blocks were not in a loop, the new block is not
      // either, and thus LI doesn't need to be updated.
      if (Loop *DestLoop = LI->getLoopFor(DestBB)) {
        if (TIL == DestLoop) {
          // Both in the same loop, the NewBB joins loop.
          DestLoop->addBasicBlockToLoop(NewBB, *LI);
        } else if (TIL->contains(DestLoop)) {
          // Edge from an outer loop to an inner loop.  Add to the outer loop.
          TIL->addBasicBlockToLoop(NewBB, *LI);
        } else if (DestLoop->contains(TIL)) {
          // Edge from an inner loop to an outer loop.  Add to the outer loop.
          DestLoop->addBasicBlockToLoop(NewBB, *LI);
        } else {
          // Edge from two loops with no containment relation.  Because these
          // are natural loops, we know that the destination block must be the
          // header of its loop (adding a branch into a loop elsewhere would
          // create an irreducible loop).
          assert(DestLoop->getHeader() == DestBB &&
                 "Should not create irreducible loops!");
          if (Loop *P = DestLoop->getParentLoop())
            P->addBasicBlockToLoop(NewBB, *LI);
        }
      }

      // If TIBB is in a loop and DestBB is outside of that loop, we may need
      // to update LoopSimplify form and LCSSA form.
      if (!TIL->contains(DestBB)) {
        assert(!TIL->contains(NewBB) &&
               "Split point for loop exit is contained in loop!");

        // Update LCSSA form in the newly created exit block.
        if (PreserveLCSSA) {
          createPHIsForSplitLoopExit(TIBB, NewBB, DestBB);
        }

        // The only that we can break LoopSimplify form by splitting a critical
        // edge is if after the split there exists some edge from TIL to DestBB
        // *and* the only edge into DestBB from outside of TIL is that of
        // NewBB. If the first isn't true, then LoopSimplify still holds, NewBB
        // is the new exit block and it has no non-loop predecessors. If the
        // second isn't true, then DestBB was not in LoopSimplify form prior to
        // the split as it had a non-loop predecessor. In both of these cases,
        // the predecessor must be directly in TIL, not in a subloop, or again
        // LoopSimplify doesn't hold.
        SmallVector<BasicBlock *, 4> LoopPreds;
        for (pred_iterator I = pred_begin(DestBB), E = pred_end(DestBB); I != E;
             ++I) {
          BasicBlock *P = *I;
          if (P == NewBB)
            continue; // The new block is known.
          if (LI->getLoopFor(P) != TIL) {
            // No need to re-simplify, it wasn't to start with.
            LoopPreds.clear();
            break;
          }
          LoopPreds.push_back(P);
        }
        if (!LoopPreds.empty()) {
          assert(!DestBB->isLandingPad() &&
                 "We don't split edges to landing pads!");
          BasicBlock *NewExitBB = SplitBlockPredecessors(
              DestBB, LoopPreds, "split", AA, DT, LI, PreserveLCSSA);
          if (PreserveLCSSA)
            createPHIsForSplitLoopExit(LoopPreds, NewExitBB, DestBB);
        }
      }
    }
  }

  return NewBB;
}
<|endoftext|>


<commit_before>// recycle result vector
on_all_cores([=] {
    swap_refs({{symsrc}}.data, {{symsink}}.data);
    {{symsink}}.data->vector.clear();
});
<commit_msg>newline<commit_after>// recycle result vector
on_all_cores([=] {
    swap_refs({{symsrc}}.data, {{symsink}}.data);
    {{symsink}}.data->vector.clear();
});

<|endoftext|>


<commit_before>#include "Engine.hpp"

int ProcessMudFunctions() {

  // -- Handle our server protocols
  if(!System.Server->ResetPollDescriptors()) { return PROCESS_FAILED; }
  if(!System.Server->ResetFDControllers()) { return PROCESS_FAILED; }
  if(!System.Server->SelectAndPoll()) { return PROCESS_FAILED; }

  System.EventManager->processEvents(EV_BEFORE_INPUT);
  // -- TODO: add input handling Server call here

  System.EventManager->processEvents(EV_BEFORE_OUTPUT);
  // -- TODO: add output handling Server call here

  return PROCESS_SUCCESS;
}

int MainLoop() {
  struct timeval last_time;
  int processRetValue;

  signal( SIGPIPE, SIG_IGN );

  gettimeofday( &last_time, NULL );
  System.CurrentSystemTime = (time_t) last_time.tv_sec;

  while((processRetValue = ProcessMudFunctions()) != PROCESS_FAILED) {

    // --------------------------------------------------------------------------------------------
    // -- process End of Process functions
    System.EventManager->processEvents(EV_POST_OUTPUT);
    // -- TODO: add output handling Server call here (so we send any updates that have occurred before input)

    // --------------------------------------------------------------------------------------------
    // -- Process time management and system speed adjustments
    // -- Need to figure out a better way to do this
    struct timeval now_time;
    long secDelta;
    long usecDelta;
    gettimeofday( &now_time, NULL );
    usecDelta	= ((int) last_time.tv_usec) - ((int) now_time.tv_usec)			+ 1000000 / 4;
    secDelta	= ((int) last_time.tv_sec ) - ((int) now_time.tv_sec );

    while ( usecDelta < 0 )
    {
      usecDelta += 1000000;
      secDelta  -= 1;
    }
    while ( usecDelta >= 1000000 )
    {
      usecDelta -= 1000000;
      secDelta  += 1;
    }

    if ( secDelta > 0 || ( secDelta == 0 && usecDelta > 0 ) )
    {
      struct timeval stall_time;

      stall_time.tv_usec = usecDelta;
      stall_time.tv_sec  = secDelta;
      if ( select( 0, NULL, NULL, NULL, &stall_time ) < 0 )
      {
        // -- TODO: insert logging mechanism
        kill(getpid(), SIGSEGV);
      }
    }
    gettimeofday( &last_time, NULL );
    System.CurrentSystemTime = (time_t) last_time.tv_sec;
    // --------------------------------------------------------------------------------------------
  } // -- end while loop

  // -- generate a core file
  if(processRetValue == PROCESS_FAILED) {
    kill(getpid(), SIGSEGV);
  }

  return processRetValue;
}

// -- EOF
<commit_msg>Update Engine.cpp<commit_after>#include "Engine.hpp"

int ProcessMudFunctions() {

  // -- Handle our server protocols
  if(!System.mServer->ResetPollDescriptors()) { return PROCESS_FAILED; }
  if(!System.mServer->ResetFDControllers()) { return PROCESS_FAILED; }
  if(!System.mServer->SelectAndPoll()) { return PROCESS_FAILED; }

  System.mEventManager->processEvents(EV_BEFORE_INPUT);
  // -- TODO: add input handling Server call here

  System.mEventManager->processEvents(EV_BEFORE_OUTPUT);
  // -- TODO: add output handling Server call here

  return PROCESS_SUCCESS;
}

int MainLoop() {
  struct timeval last_time;
  int processRetValue;

  signal( SIGPIPE, SIG_IGN );

  gettimeofday( &last_time, NULL );
  System.CurrentSystemTime = (time_t) last_time.tv_sec;

  while((processRetValue = ProcessMudFunctions()) != PROCESS_FAILED) {

    // --------------------------------------------------------------------------------------------
    // -- process End of Process functions
    System.mEventManager->processEvents(EV_POST_OUTPUT);
    // -- TODO: add output handling Server call here (so we send any updates that have occurred before input)

    // --------------------------------------------------------------------------------------------
    // -- Process time management and system speed adjustments
    // -- Need to figure out a better way to do this
    struct timeval now_time;
    long secDelta;
    long usecDelta;
    gettimeofday( &now_time, NULL );
    usecDelta	= ((int) last_time.tv_usec) - ((int) now_time.tv_usec)			+ 1000000 / 4;
    secDelta	= ((int) last_time.tv_sec ) - ((int) now_time.tv_sec );

    while ( usecDelta < 0 )
    {
      usecDelta += 1000000;
      secDelta  -= 1;
    }
    while ( usecDelta >= 1000000 )
    {
      usecDelta -= 1000000;
      secDelta  += 1;
    }

    if ( secDelta > 0 || ( secDelta == 0 && usecDelta > 0 ) )
    {
      struct timeval stall_time;

      stall_time.tv_usec = usecDelta;
      stall_time.tv_sec  = secDelta;
      if ( select( 0, NULL, NULL, NULL, &stall_time ) < 0 )
      {
        // -- TODO: insert logging mechanism
        kill(getpid(), SIGSEGV);
      }
    }
    gettimeofday( &last_time, NULL );
    System.CurrentSystemTime = (time_t) last_time.tv_sec;
    // --------------------------------------------------------------------------------------------
  } // -- end while loop

  // -- generate a core file
  if(processRetValue == PROCESS_FAILED) {
    kill(getpid(), SIGSEGV);
  }

  return processRetValue;
}

// -- EOF
<|endoftext|>


<commit_before>#include "Engine.hpp"

Server::Server() {

};

Server::~Server() {

}

bool Server::InitiatePort(int portNum) {
	static struct sockaddr_in sa_zero;
	struct sockaddr_in sa;
	int x = 1;
	int fd;

	if ( ( fd = socket( AF_INET, SOCK_STREAM, 0 ) ) < 0 )
	{
		// -- TODO: insert logging mechanism
		return false;
	}

	if ( setsockopt( fd, SOL_SOCKET, SO_REUSEADDR,    (char *) &x, sizeof(x) ) < 0 )
	{
		// -- TODO: insert logging mechanism
		close(fd);
		exit( 1 );
	}

	#if defined(SO_DONTLINGER) && !defined(SYSV)
	{
		struct	linger	ld;
		ld.l_onoff  = 1;
		ld.l_linger = 1000;

		if ( setsockopt( fd, SOL_SOCKET, SO_DONTLINGER,	(char *) &ld, sizeof(ld) ) < 0 ) {
			// -- TODO: insert logging mechanism
			close(fd);
			return false;
		}
	}
	#endif

	sa		    = sa_zero;
	sa.sin_family	    = AF_INET;
	sa.sin_port	    = htons( port );

	if ( bind( fd, (struct sockaddr *) &sa, sizeof(sa) ) < 0 )
	{
		// -- TODO: insert logging mechanism
		close(fd);
		return false;
	}
	if ( listen( fd, 3 ) < 0 )
	{
		// -- TODO: insert logging mechanism
		close(fd);
		exit(1);
	}
	mControl = fd;
	return true;
}

bool Server::ResetPollDescriptors() {
	FD_ZERO( &mRead  );
	FD_ZERO( &mWrite );
	FD_ZERO( &mError );
	FD_SET( mControl, &mRead );

	maxedConnection	= mControl;
	return true;
}

bool Server::ResetFDControllers() {
  std::list<Socket *>::iterator iter, itern;
  for(iter = System.mSocketList.begin(); iter != System.mSocketList.end(); iter = itern) {
    Socket *s = (*iter);
    itern = ++iter;

	   maxedConnection = Maximum<int>( maxedConnection, s->getFileDescriptor() );
	   FD_SET( s->getFileDescriptor(), &mRead  );
	   FD_SET( s->getFileDescriptor(), &mWrite );
	   FD_SET( s->getFileDescriptor(), &mError );
  }
  return;
}

bool Server::SelectAndPoll() {
  static struct timeval base_time;
	if ( select( maxedConnection+1, &mRead, &mWrite, &mError, &base_time ) < 0 )
	{
	    return false;
	}

	if ( FD_ISSET( mControl, &mRead ) )
	  startNewSocket( mControl );
	return true;
}

void Server::startNewSocket(int ctrl) {
    	Socket *s;
    	struct sockaddr_in sock;

    	int desc;

    	socklen_t size;
    	size = sizeof(sock);

    	getsockname( ctrl, (struct sockaddr *) &sock, &size );

    	if ( ( desc = accept( ctrl, (struct sockaddr *) &sock, &size) ) < 0 )
    	{
		// -- TODO: insert log mechanism here
    		return;
    	}

    	#if !defined(FNDELAY)
    		#define FNDELAY O_NDELAY
    	#endif

    	if ( fcntl( desc, F_SETFL, FNDELAY ) == -1 )
    	{
    		// -- TODO: insert log mechanism here
    		return;
    	}

    	s = new Socket();

	// -- attempt to glean the host address/ip
    	size = sizeof(sock);
    	if ( getpeername( desc, (struct sockaddr *) &sock, &size ) < 0 )
    	{
    		// -- TODO: insert logging mechanism
    		s->setHost("UNKNOWN");
    		s->setIpAddress("UNKNOWN");
    	} else {
    		struct hostent *from = gethostbyaddr( (char *) &sock.sin_addr, sizeof(sock.sin_addr), AF_INET );

    		s->setHost(from ? from->h_name : "UNKNOWN");
    		h->setIpAddress(inet_ntoa(sock.sin_addr));
    	}

    	System.mSocketList.push_back(s);
    	s->sendGreeting();
    	return;
}

bool Server::ReadFromSockets() {
  std::list<Socket *>::iterator iter, itern;

  for(iter = System.mSocketList.begin(); iter != System.mSocketList.end(); iter = itern) {
  	Socket *s = (*iter);
  	itern = ++iter;

	if(FD_ISSET(s->getFileDescriptor(), &mRead)) {
		if(!s->Read()) {
			CloseSocket(s, true);
		}
	}

  }
}

bool Server::ProcessInput() {

  	std::list<Socket *>::iterator iter, itern;

  	for(iter = System.mSocketList.begin(); iter != System.mSocketList.end(); iter = itern) {
	  	Socket *s = (*iter);
	  	itern = ++iter;

		if(!s->getInputBuffer().empty()) {
			// -- handle processing of the input buffer here.
			// -- here we will attempt to process commands/functions
		}

		s->clearInputBuffer();

  	}

	return true;
}

bool Server::FlushSockets() {
  std::list<Socket *>::iterator iter, itern;

  for(iter = System.mSocketList.begin(); iter != System.mSocketList.end(); iter = itern) {
  	Socket *s = (*iter);
  	itern = ++iter;

	if(!s->Flush()) {
		CloseSocket(s, true);
	}
  }

  return true;
}

void Server::CloseSocket(Socket *s, bool forced) {

	// -- attempt to dump any remaining data to the socket
	if(!forced) {
		// -- we don't attempt to trap errors here, if it returns false we just don't
		// -- care as they were disconnecting anyways.
		s->Flush();
	}

	// -- strip the socket from the socket-list
	System.mSocketList.remove(s);

	// -- reset the control values for said socket to CLR status.
	FD_CLR(s->getFileDescriptor(), &mRead);
	FD_CLR(s->getFileDescriptor(), &mWrite);
	FD_CLR(s->getFileDescriptor(), &mError);

	// -- handle accounts/players here

	// -- close out the descriptor
	close(s->getFileDescriptor());

	// -- delete the actual socket-data (kill any lingering data)
	delete s;
}
<commit_msg>Update Server.cpp<commit_after>#include "Engine.hpp"

Server::Server() {

};

Server::~Server() {

}

bool Server::InitiatePort(int portNum) {
	static struct sockaddr_in sa_zero;
	struct sockaddr_in sa;
	int x = 1;
	int fd;

	if ( ( fd = socket( AF_INET, SOCK_STREAM, 0 ) ) < 0 )
	{
		// -- TODO: insert logging mechanism
		return false;
	}

	if ( setsockopt( fd, SOL_SOCKET, SO_REUSEADDR,    (char *) &x, sizeof(x) ) < 0 )
	{
		// -- TODO: insert logging mechanism
		close(fd);
		exit( 1 );
	}

	#if defined(SO_DONTLINGER) && !defined(SYSV)
	{
		struct	linger	ld;
		ld.l_onoff  = 1;
		ld.l_linger = 1000;

		if ( setsockopt( fd, SOL_SOCKET, SO_DONTLINGER,	(char *) &ld, sizeof(ld) ) < 0 ) {
			// -- TODO: insert logging mechanism
			close(fd);
			return false;
		}
	}
	#endif

	sa		    = sa_zero;
	sa.sin_family	    = AF_INET;
	sa.sin_port	    = htons( port );

	if ( bind( fd, (struct sockaddr *) &sa, sizeof(sa) ) < 0 )
	{
		// -- TODO: insert logging mechanism
		close(fd);
		return false;
	}
	if ( listen( fd, 3 ) < 0 )
	{
		// -- TODO: insert logging mechanism
		close(fd);
		return false;
	}
	mControl = fd;
	return true;
}

bool Server::ResetPollDescriptors() {
	FD_ZERO( &mRead  );
	FD_ZERO( &mWrite );
	FD_ZERO( &mError );
	FD_SET( mControl, &mRead );

	maxedConnection	= mControl;
	return true;
}

bool Server::ResetFDControllers() {
  std::list<Socket *>::iterator iter, itern;
  for(iter = System.mSocketList.begin(); iter != System.mSocketList.end(); iter = itern) {
    Socket *s = (*iter);
    itern = ++iter;

	   maxedConnection = Maximum<int>( maxedConnection, s->getFileDescriptor() );
	   FD_SET( s->getFileDescriptor(), &mRead  );
	   FD_SET( s->getFileDescriptor(), &mWrite );
	   FD_SET( s->getFileDescriptor(), &mError );
  }
  return;
}

bool Server::SelectAndPoll() {
  static struct timeval base_time;
	if ( select( maxedConnection+1, &mRead, &mWrite, &mError, &base_time ) < 0 )
	{
	    return false;
	}

	if ( FD_ISSET( mControl, &mRead ) )
	  startNewSocket( mControl );
	return true;
}

void Server::startNewSocket(int ctrl) {
    	Socket *s;
    	struct sockaddr_in sock;

    	int desc;

    	socklen_t size;
    	size = sizeof(sock);

    	getsockname( ctrl, (struct sockaddr *) &sock, &size );

    	if ( ( desc = accept( ctrl, (struct sockaddr *) &sock, &size) ) < 0 )
    	{
		// -- TODO: insert log mechanism here
    		return;
    	}

    	#if !defined(FNDELAY)
    		#define FNDELAY O_NDELAY
    	#endif

    	if ( fcntl( desc, F_SETFL, FNDELAY ) == -1 )
    	{
    		// -- TODO: insert log mechanism here
    		return;
    	}

    	s = new Socket();

	// -- attempt to glean the host address/ip
    	size = sizeof(sock);
    	if ( getpeername( desc, (struct sockaddr *) &sock, &size ) < 0 )
    	{
    		// -- TODO: insert logging mechanism
    		s->setHost("UNKNOWN");
    		s->setIpAddress("UNKNOWN");
    	} else {
    		struct hostent *from = gethostbyaddr( (char *) &sock.sin_addr, sizeof(sock.sin_addr), AF_INET );

    		s->setHost(from ? from->h_name : "UNKNOWN");
    		h->setIpAddress(inet_ntoa(sock.sin_addr));
    	}

    	System.mSocketList.push_back(s);
    	s->sendGreeting();
    	return;
}

bool Server::ReadFromSockets() {
  std::list<Socket *>::iterator iter, itern;

  for(iter = System.mSocketList.begin(); iter != System.mSocketList.end(); iter = itern) {
  	Socket *s = (*iter);
  	itern = ++iter;

	if(FD_ISSET(s->getFileDescriptor(), &mRead)) {
		if(!s->Read()) {
			CloseSocket(s, true);
		}
	}

  }
}

bool Server::ProcessInput() {

  	std::list<Socket *>::iterator iter, itern;

  	for(iter = System.mSocketList.begin(); iter != System.mSocketList.end(); iter = itern) {
	  	Socket *s = (*iter);
	  	itern = ++iter;

		if(!s->getInputBuffer().empty()) {
			// -- handle processing of the input buffer here.
			// -- here we will attempt to process commands/functions
		}

		s->clearInputBuffer();

  	}

	return true;
}

bool Server::FlushSockets() {
  std::list<Socket *>::iterator iter, itern;

  for(iter = System.mSocketList.begin(); iter != System.mSocketList.end(); iter = itern) {
  	Socket *s = (*iter);
  	itern = ++iter;

	if(!s->Flush()) {
		CloseSocket(s, true);
	}
  }

  return true;
}

void Server::CloseSocket(Socket *s, bool forced) {

	// -- attempt to dump any remaining data to the socket
	if(!forced) {
		// -- we don't attempt to trap errors here, if it returns false we just don't
		// -- care as they were disconnecting anyways.
		s->Flush();
	}

	// -- strip the socket from the socket-list
	System.mSocketList.remove(s);

	// -- reset the control values for said socket to CLR status.
	FD_CLR(s->getFileDescriptor(), &mRead);
	FD_CLR(s->getFileDescriptor(), &mWrite);
	FD_CLR(s->getFileDescriptor(), &mError);

	// -- handle accounts/players here

	// -- close out the descriptor
	close(s->getFileDescriptor());

	// -- delete the actual socket-data (kill any lingering data)
	delete s;
}
<|endoftext|>


<commit_before>#ifndef ECL_ISTREAM_HPP
#define ECL_ISTREAM_HPP

#include <cstddef>
#include <cstdint>

namespace ecl {
template< class IO_device >
class istream
{
public:
    // Provides type information of the underlaying device
    using device_type = IO_device;

    // Initializes a stream with given device
    // NOTE: device must be initialized and opened already
    istream(IO_device *device);
    ~istream();

    istream& operator>>(int& value);
    istream& operator>>(unsigned int& value);
    istream& operator>>(char& character);
    istream& operator>>(char *string);
    // For I\O manipulators
    istream& operator>>(istream& (*func)(istream< IO_device >&));
    // NOTE: this will be used later with different manipulators,
    // such that used for hex or octal output of integers

    // unformatted input
    int get();
    istream& get(char& c);

private:
    // Simply, a device driver object
    IO_device *m_device;
};

//------------------------------------------------------------------------------

template< class IO_device >
istream< IO_device >::istream(IO_device *device)
    :m_device{device}
{
}

template< class IO_device >
istream< IO_device >::~istream()
{
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::operator>>(int& value)
{
    value = 0;
    char in_character;
    char in_char_for_spaces;
    int symbol_code;
    bool sign = false; //TRUE is used to indicate negative number
    bool first = true; //TRUE indicates the first round of cycle

    do{
        if(m_device->read((uint8_t*) &in_char_for_spaces, 1) < 0) {
            break;
        }
    } while(in_char_for_spaces == ' ');

    in_character = in_char_for_spaces;

    do{
        symbol_code = in_character;
        if(symbol_code >= 48 && symbol_code <= 58) {

            value = value * 10 + (symbol_code - 48);
        }

        else if(symbol_code == 45 && first == true) {
            sign = true;
            first = false;
        }

        else if (in_character != '\n') {
            break; //FIXME: add error handling
        }

        if(m_device->read((uint8_t*) &in_character, 1) < 0) {
            break;
        }
    } while(in_character != '\n' && in_character != ' ');

    if(sign == true) {
        value *= -1;
    }

    return *this;
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::operator>>(unsigned int& value)
{
    value = 0;
    char in_character;
    char in_char_for_spaces;
    int symbol_code;

    do{
        if(m_device->read((uint8_t*) &in_char_for_spaces, 1) < 0) {
            break;
        }
    } while(in_char_for_spaces == ' ');

    in_character = in_char_for_spaces;

    do{
        symbol_code = in_character;
        if(symbol_code >= 48 && symbol_code <= 58) {

            value = value * 10 + (symbol_code - 48);
        }

        else if (in_character != '\n') {
            break; //FIXME: add error handling
        }

        if(m_device->read((uint8_t*) &in_character, 1) < 0) {
            break; //FIXME: add error handling
        }

    } while(in_character != '\n' && in_character != ' ');

    return *this;
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::operator>>(char& character)
{
    char in_char_for_spaces;

    do{
        if(m_device->read((uint8_t*) &in_char_for_spaces, 1) < 0) {
            break;
        }
    } while(in_char_for_spaces == ' ');

    character = in_char_for_spaces;

    return *this;
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::operator>>(char *string)
{
    size_t i = 1;
    char in_char_for_spaces;

    do{
        if(m_device->read((uint8_t*) &in_char_for_spaces, 1) < 0) {
            break;
        }
    } while(in_char_for_spaces == ' ');

    string[0] = in_char_for_spaces;

    do {
        if(m_device->read((uint8_t *) &string[i], 1) < 0) {
            break; //FIXME: add error handling
        }
    }
    while(string[i] != '\n' && string[i++] != ' ');
    return *this;
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::operator>>(
        istream& (*func)(istream< IO_device >&))
{
    return func(*this);
}

template< class IO_device >
int istream< IO_device >::get()
{
	int c = 0;
	if(m_device->read((uint8_t *) &c, 1) < 0) {
		return -1;
	}

    return c;
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::get(char& character)
{
	m_device->read((uint8_t *) &character, 1);

	return *this;
}

}

#endif // ECL_istream_HPP
<commit_msg>Tabs substituted with sapces<commit_after>#ifndef ECL_ISTREAM_HPP
#define ECL_ISTREAM_HPP

#include <cstddef>
#include <cstdint>

namespace ecl {
template< class IO_device >
class istream
{
public:
    // Provides type information of the underlaying device
    using device_type = IO_device;

    // Initializes a stream with given device
    // NOTE: device must be initialized and opened already
    istream(IO_device *device);
    ~istream();

    istream& operator>>(int& value);
    istream& operator>>(unsigned int& value);
    istream& operator>>(char& character);
    istream& operator>>(char *string);
    // For I\O manipulators
    istream& operator>>(istream& (*func)(istream< IO_device >&));
    // NOTE: this will be used later with different manipulators,
    // such that used for hex or octal output of integers

    // unformatted input
    int get();
    istream& get(char& c);

private:
    // Simply, a device driver object
    IO_device *m_device;
};

//------------------------------------------------------------------------------

template< class IO_device >
istream< IO_device >::istream(IO_device *device)
    :m_device{device}
{
}

template< class IO_device >
istream< IO_device >::~istream()
{
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::operator>>(int& value)
{
    value = 0;
    char in_character;
    char in_char_for_spaces;
    int symbol_code;
    bool sign = false; //TRUE is used to indicate negative number
    bool first = true; //TRUE indicates the first round of cycle

    do{
        if(m_device->read((uint8_t*) &in_char_for_spaces, 1) < 0) {
            break;
        }
    } while(in_char_for_spaces == ' ');

    in_character = in_char_for_spaces;

    do{
        symbol_code = in_character;
        if(symbol_code >= 48 && symbol_code <= 58) {

            value = value * 10 + (symbol_code - 48);
        }

        else if(symbol_code == 45 && first == true) {
            sign = true;
            first = false;
        }

        else if (in_character != '\n') {
            break; //FIXME: add error handling
        }

        if(m_device->read((uint8_t*) &in_character, 1) < 0) {
            break;
        }
    } while(in_character != '\n' && in_character != ' ');

    if(sign == true) {
        value *= -1;
    }

    return *this;
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::operator>>(unsigned int& value)
{
    value = 0;
    char in_character;
    char in_char_for_spaces;
    int symbol_code;

    do{
        if(m_device->read((uint8_t*) &in_char_for_spaces, 1) < 0) {
            break;
        }
    } while(in_char_for_spaces == ' ');

    in_character = in_char_for_spaces;

    do{
        symbol_code = in_character;
        if(symbol_code >= 48 && symbol_code <= 58) {

            value = value * 10 + (symbol_code - 48);
        }

        else if (in_character != '\n') {
            break; //FIXME: add error handling
        }

        if(m_device->read((uint8_t*) &in_character, 1) < 0) {
            break; //FIXME: add error handling
        }

    } while(in_character != '\n' && in_character != ' ');

    return *this;
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::operator>>(char& character)
{
    char in_char_for_spaces;

    do{
        if(m_device->read((uint8_t*) &in_char_for_spaces, 1) < 0) {
            break;
        }
    } while(in_char_for_spaces == ' ');

    character = in_char_for_spaces;

    return *this;
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::operator>>(char *string)
{
    size_t i = 1;
    char in_char_for_spaces;

    do{
        if(m_device->read((uint8_t*) &in_char_for_spaces, 1) < 0) {
            break;
        }
    } while(in_char_for_spaces == ' ');

    string[0] = in_char_for_spaces;

    do {
        if(m_device->read((uint8_t *) &string[i], 1) < 0) {
            break; //FIXME: add error handling
        }
    }
    while(string[i] != '\n' && string[i++] != ' ');
    return *this;
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::operator>>(
        istream& (*func)(istream< IO_device >&))
{
    return func(*this);
}

template< class IO_device >
int istream< IO_device >::get()
{
	int c = 0;
    if(m_device->read((uint8_t *) &c, 1) < 0) {
        return -1;
    }

    return c;
}

template< class IO_device >
istream<IO_device>& istream< IO_device >::get(char& character)
{
    m_device->read((uint8_t *) &character, 1);

    return *this;
}

}

#endif // ECL_istream_HPP
<|endoftext|>


<commit_before>/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC 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 of the copyright holder 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 HOLDER 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 tasks.c
 *
 * \author MAV'RIC Team
 *
 * \brief Definition of the tasks executed on the autopilot
 *
 ******************************************************************************/

#include "tasks.hpp"
#include "central_data.hpp"

extern "C"
{
	#include "led.h"
	#include "pwm_servos.h"
}

void tasks_run_imu_update(Central_data* central_data)
{
	central_data->imu.update();
	qfilter_update(&central_data->attitude_filter);
	position_estimation_update(&central_data->position_estimation);
}

bool tasks_run_stabilisation(Central_data* central_data)
{
	tasks_run_imu_update(central_data);

	mav_mode_t mode = central_data->state.mav_mode;

	if  ( mav_modes_is_armed(mode) )
	{
		if ( mav_modes_is_auto(mode) )
		{
			central_data->controls = central_data->controls_nav;
			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;

			// if no waypoints are set, we do position hold therefore the yaw mode is absolute
			if (((central_data->state.nav_plan_active&&(!central_data->navigation.auto_takeoff)&&(!central_data->navigation.auto_landing)&&(!central_data->navigation.stop_nav)))||((central_data->state.mav_state == MAV_STATE_CRITICAL)&&(central_data->navigation.critical_behavior == FLY_TO_HOME_WP)))
			//if (((central_data->state.nav_plan_active&&(!central_data->navigation.auto_takeoff)&&(!central_data->navigation.auto_landing)))||((central_data->state.mav_state == MAV_STATE_CRITICAL)&&(central_data->navigation.critical_behavior == FLY_TO_HOME_WP)))
			{
				central_data->controls.yaw_mode = YAW_RELATIVE;
			}
			else
			{
				central_data->controls.yaw_mode = YAW_ABSOLUTE;
			}

			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
				servos_mix_quadcopter_diag_update( &central_data->servo_mix );
			}
		}
		else if ( mav_modes_is_guided(mode) )
		{
			central_data->controls = central_data->controls_nav;
			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;

			if ((central_data->state.mav_state == MAV_STATE_CRITICAL) && (central_data->navigation.critical_behavior == FLY_TO_HOME_WP))
			{
				central_data->controls.yaw_mode = YAW_RELATIVE;
			}
			else
			{
				central_data->controls.yaw_mode = YAW_ABSOLUTE;
			}

			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
				servos_mix_quadcopter_diag_update( &central_data->servo_mix );
			}
		}
		else if ( mav_modes_is_stabilise(mode) )
		{
			manual_control_get_velocity_vector(&central_data->manual_control, &central_data->controls);

			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;
			central_data->controls.yaw_mode = YAW_RELATIVE;

			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
				servos_mix_quadcopter_diag_update( &central_data->servo_mix );
			}
		}
		else if ( mav_modes_is_manual(mode) )
		{
			manual_control_get_control_command(&central_data->manual_control, &central_data->controls);

			central_data->controls.control_mode = ATTITUDE_COMMAND_MODE;
			central_data->controls.yaw_mode=YAW_RELATIVE;

			stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
			servos_mix_quadcopter_diag_update( &central_data->servo_mix );
		}
		else
		{
			servos_set_value_failsafe( &central_data->servos );
		}
	}
	else
	{
		servos_set_value_failsafe( &central_data->servos );
	}

	return true;
}

bool tasks_run_stabilisation_quaternion(Central_data* central_data);
bool tasks_run_stabilisation_quaternion(Central_data* central_data)
{
	tasks_run_imu_update(central_data);

	mav_mode_t mode = central_data->state.mav_mode;

	if( mav_modes_is_armed(mode) == false )
	{
		// Set command to current heading
		central_data->command.attitude.rpy[2] = coord_conventions_quat_to_aero(central_data->ahrs.qe).rpy[2];
		servos_set_value_failsafe( &central_data->servos );
	}
	else if( mav_modes_is_auto(mode) )
	{
		vector_field_waypoint_update( &central_data->vector_field_waypoint );
		velocity_controller_copter_update( &central_data->velocity_controller );
		attitude_controller_update( &central_data->attitude_controller );
		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	else if( mav_modes_is_manual(mode) && mav_modes_is_guided(mode) )
	{
		manual_control_get_velocity_command(&central_data->manual_control, &central_data->command.velocity, 1.0f);
		velocity_controller_copter_update( &central_data->velocity_controller );
		attitude_controller_update( &central_data->attitude_controller );
		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	else if( mav_modes_is_manual(mode) && mav_modes_is_stabilise(mode) )
	{
		manual_control_get_attitude_command(&central_data->manual_control, 0.02f, &central_data->command.attitude, 1.0f);
		manual_control_get_thrust_command(&central_data->manual_control, &central_data->command.thrust);
		attitude_controller_update( &central_data->attitude_controller );

		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	else
	{
		servos_set_value_failsafe( &central_data->servos );
	}

	return true;
}

bool tasks_run_gps_update(Central_data* central_data)
{
	central_data->gps.update();

	return true;
}

bool tasks_run_barometer_update(Central_data* central_data)
{

	central_data->barometer.update();

	return true;
}

bool tasks_run_sonar_update(Central_data* central_data)
{

	central_data->sonar.update();

	return true;
}

#include <unistd.h>
bool tasks_sleep(Central_data* central_data)
{
	usleep(3900);
	return true;
}

bool tasks_create_tasks(Central_data* central_data)
{
	bool init_success = true;

	scheduler_t* scheduler = &central_data->scheduler;

	// init_success &= scheduler_add_task(scheduler, 4000,		RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGHEST, (task_function_t)&tasks_run_stabilisation							, (task_argument_t)central_data 						, 0);
	init_success &= scheduler_add_task(scheduler, 4000,		RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGHEST, (task_function_t)&tasks_run_stabilisation_quaternion				, (task_argument_t)central_data 						, 0);

	init_success &= scheduler_add_task(scheduler, 15000, 	RUN_REGULAR, PERIODIC_RELATIVE, PRIORITY_HIGH   , (task_function_t)&tasks_run_barometer_update                      , (task_argument_t)central_data						, 2);
	init_success &= scheduler_add_task(scheduler, 100000, 	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGH   , (task_function_t)&tasks_run_gps_update                            , (task_argument_t)central_data						, 3);

	// init_success &= scheduler_add_task(scheduler, 10000, 	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGH   , (task_function_t)&navigation_update								, (task_argument_t)&central_data->navigation			, 4);
	init_success &= scheduler_add_task(scheduler, 10000, 	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGH   , (task_function_t)&vector_field_waypoint_update					, (task_argument_t)&central_data->vector_field_waypoint	, 4);

	init_success &= scheduler_add_task(scheduler, 200000,   RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_NORMAL , (task_function_t)&state_machine_update              				, (task_argument_t)&central_data->state_machine         , 5);

	init_success &= scheduler_add_task(scheduler, 4000, 	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_NORMAL , (task_function_t)&mavlink_communication_update                    , (task_argument_t)&central_data->mavlink_communication , 6);
	init_success &= scheduler_add_task(scheduler, 10000, 	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_LOW    , (task_function_t)&waypoint_handler_control_time_out_waypoint_msg  , (task_argument_t)&central_data->waypoint_handler 		, 8);

	init_success &= scheduler_add_task(scheduler, 20000,	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGH , (task_function_t)&remote_update 									, (task_argument_t)&central_data->manual_control.remote	, 12);

	init_success &= scheduler_add_task(scheduler, 500000,	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_LOW	, (task_function_t)&tasks_run_sonar_update							, (task_argument_t)central_data							, 13);

	init_success &= scheduler_add_task(scheduler, 4000,		RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_LOWEST	, (task_function_t)&tasks_sleep										, (task_argument_t)central_data							, 14);

	scheduler_sort_tasks(scheduler);

	return init_success;
}
<commit_msg>Roll back to gps navigation for test<commit_after>/*******************************************************************************
 * Copyright (c) 2009-2014, MAV'RIC 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 of the copyright holder 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 HOLDER 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 tasks.c
 *
 * \author MAV'RIC Team
 *
 * \brief Definition of the tasks executed on the autopilot
 *
 ******************************************************************************/

#include "tasks.hpp"
#include "central_data.hpp"

extern "C"
{
	#include "led.h"
	#include "pwm_servos.h"
}

void tasks_run_imu_update(Central_data* central_data)
{
	central_data->imu.update();
	qfilter_update(&central_data->attitude_filter);
	position_estimation_update(&central_data->position_estimation);
}

bool tasks_run_stabilisation(Central_data* central_data)
{
	tasks_run_imu_update(central_data);

	mav_mode_t mode = central_data->state.mav_mode;

	if  ( mav_modes_is_armed(mode) )
	{
		if ( mav_modes_is_auto(mode) )
		{
			central_data->controls = central_data->controls_nav;
			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;

			// if no waypoints are set, we do position hold therefore the yaw mode is absolute
			if (((central_data->state.nav_plan_active&&(!central_data->navigation.auto_takeoff)&&(!central_data->navigation.auto_landing)&&(!central_data->navigation.stop_nav)))||((central_data->state.mav_state == MAV_STATE_CRITICAL)&&(central_data->navigation.critical_behavior == FLY_TO_HOME_WP)))
			//if (((central_data->state.nav_plan_active&&(!central_data->navigation.auto_takeoff)&&(!central_data->navigation.auto_landing)))||((central_data->state.mav_state == MAV_STATE_CRITICAL)&&(central_data->navigation.critical_behavior == FLY_TO_HOME_WP)))
			{
				central_data->controls.yaw_mode = YAW_RELATIVE;
			}
			else
			{
				central_data->controls.yaw_mode = YAW_ABSOLUTE;
			}

			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
				servos_mix_quadcopter_diag_update( &central_data->servo_mix );
			}
		}
		else if ( mav_modes_is_guided(mode) )
		{
			central_data->controls = central_data->controls_nav;
			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;

			if ((central_data->state.mav_state == MAV_STATE_CRITICAL) && (central_data->navigation.critical_behavior == FLY_TO_HOME_WP))
			{
				central_data->controls.yaw_mode = YAW_RELATIVE;
			}
			else
			{
				central_data->controls.yaw_mode = YAW_ABSOLUTE;
			}

			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
				servos_mix_quadcopter_diag_update( &central_data->servo_mix );
			}
		}
		else if ( mav_modes_is_stabilise(mode) )
		{
			manual_control_get_velocity_vector(&central_data->manual_control, &central_data->controls);

			central_data->controls.control_mode = VELOCITY_COMMAND_MODE;
			central_data->controls.yaw_mode = YAW_RELATIVE;

			if (central_data->state.in_the_air || central_data->navigation.auto_takeoff)
			{
				stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
				servos_mix_quadcopter_diag_update( &central_data->servo_mix );
			}
		}
		else if ( mav_modes_is_manual(mode) )
		{
			manual_control_get_control_command(&central_data->manual_control, &central_data->controls);

			central_data->controls.control_mode = ATTITUDE_COMMAND_MODE;
			central_data->controls.yaw_mode=YAW_RELATIVE;

			stabilisation_copter_cascade_stabilise(&central_data->stabilisation_copter);
			servos_mix_quadcopter_diag_update( &central_data->servo_mix );
		}
		else
		{
			servos_set_value_failsafe( &central_data->servos );
		}
	}
	else
	{
		servos_set_value_failsafe( &central_data->servos );
	}

	return true;
}

bool tasks_run_stabilisation_quaternion(Central_data* central_data);
bool tasks_run_stabilisation_quaternion(Central_data* central_data)
{
	tasks_run_imu_update(central_data);

	mav_mode_t mode = central_data->state.mav_mode;

	if( mav_modes_is_armed(mode) == false )
	{
		// Set command to current heading
		central_data->command.attitude.rpy[2] = coord_conventions_quat_to_aero(central_data->ahrs.qe).rpy[2];
		servos_set_value_failsafe( &central_data->servos );
	}
	else if( mav_modes_is_auto(mode) )
	{
		vector_field_waypoint_update( &central_data->vector_field_waypoint );
		velocity_controller_copter_update( &central_data->velocity_controller );
		attitude_controller_update( &central_data->attitude_controller );
		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	else if( mav_modes_is_manual(mode) && mav_modes_is_guided(mode) )
	{
		manual_control_get_velocity_command(&central_data->manual_control, &central_data->command.velocity, 1.0f);
		velocity_controller_copter_update( &central_data->velocity_controller );
		attitude_controller_update( &central_data->attitude_controller );
		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	else if( mav_modes_is_manual(mode) && mav_modes_is_stabilise(mode) )
	{
		manual_control_get_attitude_command(&central_data->manual_control, 0.02f, &central_data->command.attitude, 1.0f);
		manual_control_get_thrust_command(&central_data->manual_control, &central_data->command.thrust);
		attitude_controller_update( &central_data->attitude_controller );

		servos_mix_quadcopter_diag_update( &central_data->servo_mix );
	}
	else
	{
		servos_set_value_failsafe( &central_data->servos );
	}

	return true;
}

bool tasks_run_gps_update(Central_data* central_data)
{
	central_data->gps.update();

	return true;
}

bool tasks_run_barometer_update(Central_data* central_data)
{

	central_data->barometer.update();

	return true;
}

bool tasks_run_sonar_update(Central_data* central_data)
{

	central_data->sonar.update();

	return true;
}

#include <unistd.h>
bool tasks_sleep(Central_data* central_data)
{
	usleep(3900);
	return true;
}

bool tasks_create_tasks(Central_data* central_data)
{
	bool init_success = true;

	scheduler_t* scheduler = &central_data->scheduler;

	init_success &= scheduler_add_task(scheduler, 4000,		RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGHEST, (task_function_t)&tasks_run_stabilisation							, (task_argument_t)central_data 						, 0);
	// init_success &= scheduler_add_task(scheduler, 4000,		RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGHEST, (task_function_t)&tasks_run_stabilisation_quaternion				, (task_argument_t)central_data 						, 0);

	init_success &= scheduler_add_task(scheduler, 15000, 	RUN_REGULAR, PERIODIC_RELATIVE, PRIORITY_HIGH   , (task_function_t)&tasks_run_barometer_update                      , (task_argument_t)central_data						, 2);
	init_success &= scheduler_add_task(scheduler, 100000, 	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGH   , (task_function_t)&tasks_run_gps_update                            , (task_argument_t)central_data						, 3);

	init_success &= scheduler_add_task(scheduler, 10000, 	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGH   , (task_function_t)&navigation_update								, (task_argument_t)&central_data->navigation			, 4);
	// init_success &= scheduler_add_task(scheduler, 10000, 	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGH   , (task_function_t)&vector_field_waypoint_update					, (task_argument_t)&central_data->vector_field_waypoint	, 4);

	init_success &= scheduler_add_task(scheduler, 200000,   RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_NORMAL , (task_function_t)&state_machine_update              				, (task_argument_t)&central_data->state_machine         , 5);

	init_success &= scheduler_add_task(scheduler, 4000, 	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_NORMAL , (task_function_t)&mavlink_communication_update                    , (task_argument_t)&central_data->mavlink_communication , 6);
	init_success &= scheduler_add_task(scheduler, 10000, 	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_LOW    , (task_function_t)&waypoint_handler_control_time_out_waypoint_msg  , (task_argument_t)&central_data->waypoint_handler 		, 8);

	init_success &= scheduler_add_task(scheduler, 20000,	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_HIGH , (task_function_t)&remote_update 									, (task_argument_t)&central_data->manual_control.remote	, 12);

	init_success &= scheduler_add_task(scheduler, 500000,	RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_LOW	, (task_function_t)&tasks_run_sonar_update							, (task_argument_t)central_data							, 13);

	init_success &= scheduler_add_task(scheduler, 4000,		RUN_REGULAR, PERIODIC_ABSOLUTE, PRIORITY_LOWEST	, (task_function_t)&tasks_sleep										, (task_argument_t)central_data							, 14);

	scheduler_sort_tasks(scheduler);

	return init_success;
}
<|endoftext|>


<commit_before>/*
 * Copyright (C) 2018 ScyllaDB
 */

/*
 * This file is part of Scylla.
 *
 * Scylla is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Scylla is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Scylla.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <boost/range/adaptor/map.hpp>
#include "view_update_generator.hh"
#include "service/priority_manager.hh"
#include "utils/error_injection.hh"

static logging::logger vug_logger("view_update_generator");

static inline void inject_failure(std::string_view operation) {
    utils::get_local_injector().inject(operation,
            [operation] { throw std::runtime_error(std::string(operation)); });
}

namespace db::view {

future<> view_update_generator::start() {
    thread_attributes attr;
    attr.sched_group = _db.get_streaming_scheduling_group();
    _started = seastar::async(std::move(attr), [this]() mutable {
        auto drop_sstable_references = defer([&] {
            // Clear sstable references so sstables_manager::stop() doesn't hang.
            vug_logger.info("leaving {} unstaged sstables unprocessed",
                    _sstables_to_move.size(), _sstables_with_tables.size());
            _sstables_to_move.clear();
            _sstables_with_tables.clear();
        });
        while (!_as.abort_requested()) {
            if (_sstables_with_tables.empty()) {
                _pending_sstables.wait().get();
            }

            // To ensure we don't race with updates, move the entire content
            // into a local variable.
            auto sstables_with_tables = std::exchange(_sstables_with_tables, {});

            // If we got here, we will process all tables we know about so far eventually so there
            // is no starvation
            for (auto table_it = sstables_with_tables.begin(); table_it != sstables_with_tables.end(); table_it = sstables_with_tables.erase(table_it)) {
                auto& [t, sstables] = *table_it;
                schema_ptr s = t->schema();

                vug_logger.trace("Processing {}.{}: {} sstables", s->ks_name(), s->cf_name(), sstables.size());

                const auto num_sstables = sstables.size();

                try {
                    // Exploit the fact that sstables in the staging directory
                    // are usually non-overlapping and use a partitioned set for
                    // the read.
                    auto ssts = make_lw_shared<sstables::sstable_set>(sstables::make_partitioned_sstable_set(s, make_lw_shared<sstable_list>(sstable_list{}), false));
                    for (auto& sst : sstables) {
                        ssts->insert(sst);
                    }

                    auto permit = _db.get_reader_concurrency_semaphore().make_permit(s.get(), "view_update_generator");
                    auto ms = mutation_source([this, ssts] (
                                schema_ptr s,
                                reader_permit permit,
                                const dht::partition_range& pr,
                                const query::partition_slice& ps,
                                const io_priority_class& pc,
                                tracing::trace_state_ptr ts,
                                streamed_mutation::forwarding fwd_ms,
                                mutation_reader::forwarding fwd_mr) {
                        return make_restricted_range_sstable_reader(std::move(ssts), s, std::move(permit), pr, ps, pc, std::move(ts), fwd_ms, fwd_mr);
                    });
                    auto [staging_sstable_reader, staging_sstable_reader_handle] = make_manually_paused_evictable_reader(
                            std::move(ms),
                            s,
                            permit,
                            query::full_partition_range,
                            s->full_slice(),
                            service::get_local_streaming_priority(),
                            nullptr,
                            ::mutation_reader::forwarding::no);

                    inject_failure("view_update_generator_consume_staging_sstable");
                    auto result = staging_sstable_reader.consume_in_thread(view_updating_consumer(s, std::move(permit), *t, sstables, _as, staging_sstable_reader_handle), db::no_timeout);
                    if (result == stop_iteration::yes) {
                        break;
                    }
                } catch (...) {
                    vug_logger.warn("Processing {} failed for table {}:{}. Will retry...", s->ks_name(), s->cf_name(), std::current_exception());
                    // Need to add sstables back to the set so we can retry later. By now it may
                    // have had other updates.
                    std::move(sstables.begin(), sstables.end(), std::back_inserter(_sstables_with_tables[t]));
                    break;
                }
                try {
                    inject_failure("view_update_generator_collect_consumed_sstables");
                    // collect all staging sstables to move in a map, grouped by table.
                    std::move(sstables.begin(), sstables.end(), std::back_inserter(_sstables_to_move[t]));
                } catch (...) {
                    // Move from staging will be retried upon restart.
                    vug_logger.warn("Moving {} from staging failed: {}:{}. Ignoring...", s->ks_name(), s->cf_name(), std::current_exception());
                }
                _registration_sem.signal(num_sstables);
            }
            // For each table, move the processed staging sstables into the table's base dir.
            for (auto it = _sstables_to_move.begin(); it != _sstables_to_move.end(); ) {
                auto& [t, sstables] = *it;
                try {
                    inject_failure("view_update_generator_move_staging_sstable");
                    t->move_sstables_from_staging(sstables).get();
                } catch (...) {
                    // Move from staging will be retried upon restart.
                    vug_logger.warn("Moving some sstable from staging failed: {}. Ignoring...", std::current_exception());
                }
                it = _sstables_to_move.erase(it);
            }
        }
    });
    return make_ready_future<>();
}

future<> view_update_generator::stop() {
    _as.request_abort();
    _pending_sstables.signal();
    return std::move(_started).then([this] {
        _registration_sem.broken();
    });
}

bool view_update_generator::should_throttle() const {
    return !_started.available();
}

future<> view_update_generator::register_staging_sstable(sstables::shared_sstable sst, lw_shared_ptr<table> table) {
    if (_as.abort_requested()) {
        return make_ready_future<>();
    }
    inject_failure("view_update_generator_registering_staging_sstable");
    _sstables_with_tables[table].push_back(std::move(sst));

    _pending_sstables.signal();
    if (should_throttle()) {
        return _registration_sem.wait(1);
    } else {
        _registration_sem.consume(1);
        return make_ready_future<>();
    }
}

void view_update_generator::setup_metrics() {
    namespace sm = seastar::metrics;

    _metrics.add_group("view_update_generator", {
        sm::make_gauge("pending_registrations", sm::description("Number of tasks waiting to register staging sstables"),
                [this] { return _registration_sem.waiters(); }),

        sm::make_gauge("queued_batches_count",
                sm::description("Number of sets of sstables queued for view update generation"),
                [this] { return _sstables_with_tables.size(); }),

        sm::make_gauge("sstables_to_move_count",
                sm::description("Number of sets of sstables which are already processed and wait to be moved from their staging directory"),
                [this] { return _sstables_to_move.size(); })
    });
}

}
<commit_msg>view_update_generator: start: close staging_sstable_reader when done<commit_after>/*
 * Copyright (C) 2018 ScyllaDB
 */

/*
 * This file is part of Scylla.
 *
 * Scylla is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Scylla is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Scylla.  If not, see <http://www.gnu.org/licenses/>.
 */

#include <boost/range/adaptor/map.hpp>
#include "view_update_generator.hh"
#include "service/priority_manager.hh"
#include "utils/error_injection.hh"

static logging::logger vug_logger("view_update_generator");

static inline void inject_failure(std::string_view operation) {
    utils::get_local_injector().inject(operation,
            [operation] { throw std::runtime_error(std::string(operation)); });
}

namespace db::view {

future<> view_update_generator::start() {
    thread_attributes attr;
    attr.sched_group = _db.get_streaming_scheduling_group();
    _started = seastar::async(std::move(attr), [this]() mutable {
        auto drop_sstable_references = defer([&] {
            // Clear sstable references so sstables_manager::stop() doesn't hang.
            vug_logger.info("leaving {} unstaged sstables unprocessed",
                    _sstables_to_move.size(), _sstables_with_tables.size());
            _sstables_to_move.clear();
            _sstables_with_tables.clear();
        });
        while (!_as.abort_requested()) {
            if (_sstables_with_tables.empty()) {
                _pending_sstables.wait().get();
            }

            // To ensure we don't race with updates, move the entire content
            // into a local variable.
            auto sstables_with_tables = std::exchange(_sstables_with_tables, {});

            // If we got here, we will process all tables we know about so far eventually so there
            // is no starvation
            for (auto table_it = sstables_with_tables.begin(); table_it != sstables_with_tables.end(); table_it = sstables_with_tables.erase(table_it)) {
                auto& [t, sstables] = *table_it;
                schema_ptr s = t->schema();

                vug_logger.trace("Processing {}.{}: {} sstables", s->ks_name(), s->cf_name(), sstables.size());

                const auto num_sstables = sstables.size();

                try {
                    // Exploit the fact that sstables in the staging directory
                    // are usually non-overlapping and use a partitioned set for
                    // the read.
                    auto ssts = make_lw_shared<sstables::sstable_set>(sstables::make_partitioned_sstable_set(s, make_lw_shared<sstable_list>(sstable_list{}), false));
                    for (auto& sst : sstables) {
                        ssts->insert(sst);
                    }

                    auto permit = _db.get_reader_concurrency_semaphore().make_permit(s.get(), "view_update_generator");
                    auto ms = mutation_source([this, ssts] (
                                schema_ptr s,
                                reader_permit permit,
                                const dht::partition_range& pr,
                                const query::partition_slice& ps,
                                const io_priority_class& pc,
                                tracing::trace_state_ptr ts,
                                streamed_mutation::forwarding fwd_ms,
                                mutation_reader::forwarding fwd_mr) {
                        return make_restricted_range_sstable_reader(std::move(ssts), s, std::move(permit), pr, ps, pc, std::move(ts), fwd_ms, fwd_mr);
                    });
                    auto [staging_sstable_reader, staging_sstable_reader_handle] = make_manually_paused_evictable_reader(
                            std::move(ms),
                            s,
                            permit,
                            query::full_partition_range,
                            s->full_slice(),
                            service::get_local_streaming_priority(),
                            nullptr,
                            ::mutation_reader::forwarding::no);

                    inject_failure("view_update_generator_consume_staging_sstable");
                    auto result = staging_sstable_reader.consume_in_thread(view_updating_consumer(s, std::move(permit), *t, sstables, _as, staging_sstable_reader_handle), db::no_timeout);
                    staging_sstable_reader.close().get();
                    if (result == stop_iteration::yes) {
                        break;
                    }
                } catch (...) {
                    vug_logger.warn("Processing {} failed for table {}:{}. Will retry...", s->ks_name(), s->cf_name(), std::current_exception());
                    // Need to add sstables back to the set so we can retry later. By now it may
                    // have had other updates.
                    std::move(sstables.begin(), sstables.end(), std::back_inserter(_sstables_with_tables[t]));
                    break;
                }
                try {
                    inject_failure("view_update_generator_collect_consumed_sstables");
                    // collect all staging sstables to move in a map, grouped by table.
                    std::move(sstables.begin(), sstables.end(), std::back_inserter(_sstables_to_move[t]));
                } catch (...) {
                    // Move from staging will be retried upon restart.
                    vug_logger.warn("Moving {} from staging failed: {}:{}. Ignoring...", s->ks_name(), s->cf_name(), std::current_exception());
                }
                _registration_sem.signal(num_sstables);
            }
            // For each table, move the processed staging sstables into the table's base dir.
            for (auto it = _sstables_to_move.begin(); it != _sstables_to_move.end(); ) {
                auto& [t, sstables] = *it;
                try {
                    inject_failure("view_update_generator_move_staging_sstable");
                    t->move_sstables_from_staging(sstables).get();
                } catch (...) {
                    // Move from staging will be retried upon restart.
                    vug_logger.warn("Moving some sstable from staging failed: {}. Ignoring...", std::current_exception());
                }
                it = _sstables_to_move.erase(it);
            }
        }
    });
    return make_ready_future<>();
}

future<> view_update_generator::stop() {
    _as.request_abort();
    _pending_sstables.signal();
    return std::move(_started).then([this] {
        _registration_sem.broken();
    });
}

bool view_update_generator::should_throttle() const {
    return !_started.available();
}

future<> view_update_generator::register_staging_sstable(sstables::shared_sstable sst, lw_shared_ptr<table> table) {
    if (_as.abort_requested()) {
        return make_ready_future<>();
    }
    inject_failure("view_update_generator_registering_staging_sstable");
    _sstables_with_tables[table].push_back(std::move(sst));

    _pending_sstables.signal();
    if (should_throttle()) {
        return _registration_sem.wait(1);
    } else {
        _registration_sem.consume(1);
        return make_ready_future<>();
    }
}

void view_update_generator::setup_metrics() {
    namespace sm = seastar::metrics;

    _metrics.add_group("view_update_generator", {
        sm::make_gauge("pending_registrations", sm::description("Number of tasks waiting to register staging sstables"),
                [this] { return _registration_sem.waiters(); }),

        sm::make_gauge("queued_batches_count",
                sm::description("Number of sets of sstables queued for view update generation"),
                [this] { return _sstables_with_tables.size(); }),

        sm::make_gauge("sstables_to_move_count",
                sm::description("Number of sets of sstables which are already processed and wait to be moved from their staging directory"),
                [this] { return _sstables_to_move.size(); })
    });
}

}
<|endoftext|>