lpsolver 0.1.0 → 0.2.1

data/ext/lpsolver-highs/highs/ipm/hipo/factorhighs/FormatHandler.cpp

@@ -0,0 +1,46 @@
+#include "FormatHandler.h"
+
+#include "CallAndTimeBlas.h"
+#include "DenseFact.h"
+#include "ipm/hipo/auxiliary/Auxiliary.h"
+
+namespace hipo {
+
+FormatHandler::FormatHandler(const Symbolic& S, Int sn, const Regul& regul,
+                             std::vector<double>& frontal, double* clique_ptr)
+    : S_{&S},
+      regul_{regul},
+      sn_{sn},
+      nb_{S_->blockSize()},
+      sn_size_{S_->snStart(sn_ + 1) - S_->snStart(sn_)},
+      ldf_{(Int)(S_->ptr(sn_ + 1) - S_->ptr(sn_))},
+      ldc_{ldf_ - sn_size_},
+      frontal_{frontal},
+      clique_ptr_{clique_ptr} {
+  local_reg_.resize(sn_size_);
+  swaps_.resize(sn_size_);
+  pivot_2x2_.resize(sn_size_);
+}
+
+void FormatHandler::terminate(std::vector<double>& clique,
+                              std::vector<double>& total_reg,
+                              std::vector<Int>& swaps,
+                              std::vector<double>& pivot_2x2) {
+  // Move local copies of data into their final position.
+  // In this way, the shared objects sn_columns_ and schur_contribution_ are
+  // accessed only here, while a local copy is used for the assembly and dense
+  // factorisation. This should avoid the problem of false sharing.
+
+  clique = std::move(clique_);
+  swaps = std::move(swaps_);
+  pivot_2x2 = std::move(pivot_2x2_);
+
+  // Move local regularisation into total regularisation.
+  for (Int i = 0; i < sn_size_; ++i)
+    total_reg[S_->snStart(sn_) + i] = local_reg_[i];
+
+  // This function should not require a lock, since all threads access different
+  // locations within total_reg, sn_columns and schur_contribution.
+}
+
+}  // namespace hipo

data/ext/lpsolver-highs/highs/ipm/hipo/factorhighs/FormatHandler.h

@@ -0,0 +1,95 @@
+#ifndef FACTORHIGHS_FORMAT_HANDLER_H
+#define FACTORHIGHS_FORMAT_HANDLER_H
+
+#include <vector>
+
+#include "FactorHiGHSSettings.h"
+#include "Symbolic.h"
+#include "ipm/hipo/auxiliary/IntConfig.h"
+
+namespace hipo {
+
+// Interface class to handle different formats of dense matrices during the
+// factorise phase.
+// Any implementation of a specific format needs to define:
+// - initFrontal: to initialise the frontal matrix with the correct number of
+//                elements; the entries should be set to zero.
+// - initClique: to initialise the clique matrix with the correct number of
+//                elements; the entries should be left uninitialised.
+// - assembleFrontal: to set a specific entry of frontal (used to assemble the
+//                original matrix)
+// - assembleFrontalMultiple: to sum a given number of consecutive entries into
+//                frontal (used to assemble the child supernodes)
+// - assembleClique: to sum the contributions of a given child supernode into
+//                clique.
+// - denseFactorise: to perform the dense partial factorisation of frontal,
+//                storing the Schur complement in clique.
+
+class FormatHandler {
+ protected:
+  // symbolic object
+  const Symbolic* S_;
+
+  const Regul& regul_;
+
+  // supernode being processed
+  const Int sn_{};
+
+  // block size
+  const Int nb_{};
+
+  // size of the supernode
+  const Int sn_size_{};
+
+  // size of the front
+  const Int ldf_{};
+
+  // size of the clique
+  const Int ldc_{};
+
+  // local copies to be moved at the end
+  std::vector<double>& frontal_;
+  std::vector<double> clique_{};
+  double* clique_ptr_ = nullptr;
+  std::vector<double> local_reg_{};
+  std::vector<Int> swaps_{};
+  std::vector<double> pivot_2x2_{};
+
+ public:
+  FormatHandler(const Symbolic& S, Int sn, const Regul& regul,
+                std::vector<double>& frontal, double* clique_ptr);
+  void terminate(std::vector<double>& clique, std::vector<double>& total_reg,
+                 std::vector<Int>& swaps, std::vector<double>& pivot_2x2);
+
+  // avoid copies
+  FormatHandler(const FormatHandler&) = delete;
+  FormatHandler& operator=(const FormatHandler&) = delete;
+
+  // virtual destructor
+  virtual ~FormatHandler() = default;
+
+  // =================================================================
+  // Pure virtual functions.
+  // These need to be defined by any derived class.
+  // =================================================================
+  virtual void initFrontal() = 0;
+  virtual void initClique() = 0;
+  virtual void assembleFrontal(Int i, Int j, double val) = 0;
+  virtual void assembleFrontalMultiple(Int& num, const double* child, Int nc,
+                                       Int child_sn, Int row, Int col, Int i,
+                                       Int j) = 0;
+  virtual void assembleClique(const double* child, Int nc, Int child_sn) = 0;
+  virtual Int denseFactorise(double reg_thresh) = 0;
+
+  // =================================================================
+  // Virtual functions.
+  // These may be overridden by derived classes, if needed.
+  // =================================================================
+  virtual void extremeEntries() {}
+};
+
+const Int extra_space_frontal = 10;
+
+}  // namespace hipo
+
+#endif

data/ext/lpsolver-highs/highs/ipm/hipo/factorhighs/HybridHybridFormatHandler.cpp

@@ -0,0 +1,238 @@
+#include "HybridHybridFormatHandler.h"
+
+#include <cassert>
+#include <cstring>
+
+#include "CallAndTimeBlas.h"
+#include "DataCollector.h"
+#include "DenseFact.h"
+#include "ipm/hipo/auxiliary/Auxiliary.h"
+
+namespace hipo {
+
+HybridHybridFormatHandler::HybridHybridFormatHandler(
+    const Symbolic& S, Int sn, const Regul& regul, DataCollector& data,
+    std::vector<double>& frontal, double* clique_ptr)
+    : FormatHandler(S, sn, regul, frontal, clique_ptr), data_{data} {
+  // initialise frontal and clique
+  initFrontal();
+
+  // if CliqueStack is used, clique_ptr already points to a valid region of
+  // memory for the clique. Otherwise, allocate it locally.
+  if (!clique_ptr_) initClique();
+}
+
+void HybridHybridFormatHandler::initFrontal() {
+  const Int n_blocks = (sn_size_ - 1) / nb_ + 1;
+  diag_start_.resize(n_blocks);
+  Int64 frontal_size =
+      getDiagStart(ldf_, sn_size_, nb_, n_blocks, diag_start_) +
+      extra_space_frontal;
+  frontal_.resize(frontal_size);
+  std::memset(frontal_.data(), 0, frontal_size * sizeof(double));
+
+  // NB: extra_space_frontal is not strictly needed. However, it removes some
+  // weird problem on windows in debug. Who knows what's happening...
+
+  // frontal_ is actually allocated just the first time, then the memory is
+  // reused from the previous factorisations and just initialised.
+}
+
+void HybridHybridFormatHandler::initClique() {
+  clique_.resize(S_->cliqueSize(sn_));
+
+  // If the clique size is zero, do not access the underlying pointer. This
+  // causes strange issues on windows. It's not a problem if clique_ptr_ remains
+  // null, because it will never be used in that case.
+  if (!clique_.empty()) clique_ptr_ = clique_.data();
+}
+
+void HybridHybridFormatHandler::assembleFrontal(Int i, Int j, double val) {
+  Int block = j / nb_;
+  Int ldb = ldf_ - block * nb_;
+  Int ii = i - block * nb_;
+  Int jj = j - block * nb_;
+  frontal_[diag_start_[block] + ii + ldb * jj] = val;
+}
+
+void HybridHybridFormatHandler::assembleFrontalMultiple(Int& num,
+                                                        const double* child,
+                                                        Int nc, Int child_sn,
+                                                        Int row, Int col, Int i,
+                                                        Int j) {
+  const Int jblock = col / nb_;
+  const Int jb = std::min(nb_, nc - nb_ * jblock);
+  const Int row_ = row - jblock * nb_;
+  const Int col_ = col - jblock * nb_;
+  const Int64 start_block = S_->cliqueBlockStart(child_sn, jblock);
+
+  Int block = j / nb_;
+  Int ldb = ldf_ - block * nb_;
+  Int ii = i - block * nb_;
+  Int jj = j - block * nb_;
+
+  if (num > kMinConsecutiveSums)
+    callAndTime_daxpy(num, 1.0, &child[start_block + col_ + jb * row_], jb,
+                      &frontal_[diag_start_[block] + ii + ldb * jj], 1, data_);
+  else {
+    frontal_[diag_start_[block] + ii + ldb * jj] +=
+        child[start_block + col_ + jb * row_];
+    num = 1;
+  }
+}
+
+Int HybridHybridFormatHandler::denseFactorise(double reg_thresh) {
+  Int status;
+
+  // either clique is valid, or clique is not needed
+  assert(clique_ptr_ || ldf_ == sn_size_);
+
+  status = denseFactFP2FH(frontal_.data(), ldf_, sn_size_, nb_, data_);
+  if (status) return status;
+
+  // find the position within pivot_sign corresponding to this supernode
+  Int sn_start = S_->snStart(sn_);
+  const Int* pivot_sign = &S_->pivotSign().data()[sn_start];
+
+  status = denseFactFH('H', ldf_, sn_size_, S_->blockSize(), frontal_.data(),
+                       clique_ptr_, pivot_sign, reg_thresh, regul_,
+                       local_reg_.data(), swaps_.data(), pivot_2x2_.data(),
+                       S_->parNode(), data_);
+
+  return status;
+}
+
+void HybridHybridFormatHandler::assembleClique(const double* child, Int nc,
+                                               Int child_sn) {
+  // assemble the child clique into the current clique by blocks of columns.
+  // within a block, assemble by rows.
+
+  const Int n_blocks = (nc - 1) / nb_ + 1;
+
+  Int row_start{};
+
+  // go through the blocks of columns of the child sn
+  for (Int b = 0; b < n_blocks; ++b) {
+    const Int64 b_start = S_->cliqueBlockStart(child_sn, b);
+
+    const Int col_start = row_start;
+    const Int col_end = std::min(col_start + nb_, nc);
+
+    // go through the rows within this block
+    for (Int row = row_start; row < nc; ++row) {
+      const Int i = S_->relindClique(child_sn, row) - sn_size_;
+
+      // already assembled into frontal
+      if (i < 0) continue;
+
+      // go through the columns of the block
+      Int col = col_start;
+      while (col < col_end) {
+        Int j = S_->relindClique(child_sn, col);
+        if (j < sn_size_) {
+          ++col;
+          continue;
+        }
+        j -= sn_size_;
+
+        // information and sizes of child sn
+        const Int jblock_c = b;
+        const Int jb_c = std::min(nb_, nc - nb_ * jblock_c);
+        const Int row_ = row - jblock_c * nb_;
+        const Int col_ = col - jblock_c * nb_;
+        const Int64 start_block_c = b_start;
+
+        // sun consecutive entries in a row.
+        // consecutive need to be reduced, to account for edge of the block
+        const Int zeros_stored_row =
+            std::max((Int)0, jb_c - (row - row_start) - 1);
+        Int consecutive = S_->consecutiveSums(child_sn, col);
+        const Int left_in_child = col_end - col - zeros_stored_row;
+        consecutive = std::min(consecutive, left_in_child);
+
+        // consecutive need to account also for edge of block in parent
+        const Int block_in_parent = j / nb_;
+        const Int col_end_parent = std::min((block_in_parent + 1) * nb_, ldc_);
+        const Int left_in_parent = col_end_parent - j;
+        consecutive = std::min(consecutive, left_in_parent);
+
+        // needed to deal with zeros stored in upper right part of block
+        if (consecutive == 0) break;
+
+        // information and sizes of current sn
+        const Int jblock = block_in_parent;
+        const Int jb = std::min(nb_, ldc_ - nb_ * jblock);
+        const Int i_ = i - jblock * nb_;
+        const Int j_ = j - jblock * nb_;
+        const Int64 start_block = S_->cliqueBlockStart(sn_, jblock);
+
+        if (consecutive > kMinConsecutiveSums) {
+          callAndTime_daxpy(consecutive, 1.0,
+                            &child[start_block_c + col_ + jb_c * row_], 1,
+                            &clique_ptr_[start_block + j_ + jb * i_], 1, data_);
+
+          col += consecutive;
+        } else {
+          clique_ptr_[start_block + j_ + jb * i_] +=
+              child[start_block_c + col_ + jb_c * row_];
+
+          col++;
+        }
+      }
+    }
+
+    row_start += nb_;
+  }
+}
+
+void HybridHybridFormatHandler::extremeEntries() {
+#ifdef HIPO_COLLECT_EXPENSIVE_DATA
+  double minD = 1e100;
+  double maxD = 0.0;
+  double minoffD = 1e100;
+  double maxoffD = 0.0;
+
+  // number of blocks of columns
+  const Int n_blocks = (sn_size_ - 1) / nb_ + 1;
+
+  // index to access frontal
+  Int64 index{};
+
+  // go through blocks of columns for this supernode
+  for (Int j = 0; j < n_blocks; ++j) {
+    // number of columns in the block
+    const Int jb = std::min(nb_, sn_size_ - nb_ * j);
+
+    for (Int k = 0; k < jb; ++k) {
+      // off diagonal entries
+      for (Int i = 0; i < k; ++i) {
+        if (frontal_[index] != 0.0) {
+          minoffD = std::min(minoffD, std::abs(frontal_[index]));
+          maxoffD = std::max(maxoffD, std::abs(frontal_[index]));
+        }
+        index++;
+      }
+
+      // diagonal entry
+      minD = std::min(minD, std::abs(1.0 / frontal_[index]));
+      maxD = std::max(maxD, std::abs(1.0 / frontal_[index]));
+
+      index += jb - k;
+    }
+
+    const Int64 entries_left = (Int64)(ldf_ - nb_ * j - jb) * jb;
+
+    for (Int64 i = 0; i < entries_left; ++i) {
+      if (frontal_[index] != 0.0) {
+        minoffD = std::min(minoffD, std::abs(frontal_[index]));
+        maxoffD = std::max(maxoffD, std::abs(frontal_[index]));
+      }
+      index++;
+    }
+  }
+
+  data_.setExtremeEntries(minD, maxD, minoffD, maxoffD);
+#endif
+}
+
+}  // namespace hipo

data/ext/lpsolver-highs/highs/ipm/hipo/factorhighs/HybridHybridFormatHandler.h

@@ -0,0 +1,31 @@
+#ifndef FACTORHIGHS_HYBRID_HYBRID_FORMAT_H
+#define FACTORHIGHS_HYBRID_HYBRID_FORMAT_H
+
+#include "DataCollector.h"
+#include "FormatHandler.h"
+
+namespace hipo {
+
+class HybridHybridFormatHandler : public FormatHandler {
+  std::vector<Int64> diag_start_;
+  DataCollector& data_;
+
+  void initFrontal() override;
+  void initClique() override;
+  void assembleFrontal(Int i, Int j, double val) override;
+  void assembleFrontalMultiple(Int& num, const double* child, Int nc,
+                               Int child_sn, Int row, Int col, Int i,
+                               Int j) override;
+  Int denseFactorise(double reg_thresh) override;
+  void assembleClique(const double* child, Int nc, Int child_sn) override;
+  void extremeEntries() override;
+
+ public:
+  HybridHybridFormatHandler(const Symbolic& S, Int sn, const Regul& regul,
+                            DataCollector& data, std::vector<double>& frontal,
+                            double* clique_ptr);
+};
+
+}  // namespace hipo
+
+#endif

data/ext/lpsolver-highs/highs/ipm/hipo/factorhighs/HybridSolveHandler.cpp

@@ -0,0 +1,272 @@
+#include "HybridSolveHandler.h"
+
+#include "CallAndTimeBlas.h"
+#include "DataCollector.h"
+#include "FactorHiGHSSettings.h"
+#include "FormatHandler.h"
+#include "Swaps.h"
+#include "ipm/hipo/auxiliary/Auxiliary.h"
+
+namespace hipo {
+
+HybridSolveHandler::HybridSolveHandler(
+    const Symbolic& S, const std::vector<std::vector<double>>& sn_columns,
+    const std::vector<std::vector<Int>>& swaps,
+    const std::vector<std::vector<double>>& pivot_2x2, DataCollector& data)
+    : SolveHandler(S, sn_columns, data), swaps_{swaps}, pivot_2x2_{pivot_2x2} {}
+
+void HybridSolveHandler::forwardSolve(std::vector<double>& x) const {
+  // Forward solve.
+  // Blas calls: dtrsv, dgemv
+
+  // supernode columns in format FH
+
+  HIPO_CLOCK_CREATE;
+
+  const Int nb = S_.blockSize();
+
+  for (Int sn = 0; sn < S_.sn(); ++sn) {
+    // leading size of supernode
+    const Int ldSn = S_.ptr(sn + 1) - S_.ptr(sn);
+
+    // number of columns in the supernode
+    const Int sn_size = S_.snStart(sn + 1) - S_.snStart(sn);
+
+    // first colums of the supernode
+    const Int sn_start = S_.snStart(sn);
+
+    // index to access S->rows for this supernode
+    const Int64 start_row = S_.ptr(sn);
+
+    // number of blocks of columns
+    const Int n_blocks = (sn_size - 1) / nb + 1;
+
+    // index to access snColumns[sn]
+    Int64 SnCol_ind{};
+
+    // go through blocks of columns for this supernode
+    for (Int j = 0; j < n_blocks; ++j) {
+      // number of columns in the block
+      const Int jb = std::min(nb, sn_size - nb * j);
+
+      // number of entries in diagonal part
+      const Int diag_entries = jb * jb;
+
+      // index to access vector x
+      const Int x_start = sn_start + nb * j;
+
+#ifdef HIPO_PIVOTING
+      HIPO_CLOCK_START(2);
+      // apply swaps to portion of rhs that is affected
+      const Int* current_swaps = &swaps_[sn][nb * j];
+      permuteWithSwaps(&x[x_start], current_swaps, jb);
+      HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_swap);
+#endif
+
+      HIPO_CLOCK_START(2);
+      callAndTime_dtrsv('U', 'T', 'U', jb, &sn_columns_[sn][SnCol_ind], jb,
+                        &x[x_start], 1, data_);
+
+      SnCol_ind += diag_entries;
+
+      // temporary space for gemv
+      const Int gemv_space = ldSn - nb * j - jb;
+      std::vector<double> y(gemv_space);
+      if (gemv_space > 0) {
+        callAndTime_dgemv('T', jb, gemv_space, 1.0, &sn_columns_[sn][SnCol_ind],
+                          jb, &x[x_start], 1, 0.0, y.data(), 1, data_);
+        SnCol_ind += jb * gemv_space;
+        HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_dense);
+
+        HIPO_CLOCK_START(2);
+        // scatter solution of gemv
+        for (Int i = 0; i < gemv_space; ++i) {
+          const Int row = S_.rows(start_row + nb * j + jb + i);
+          x[row] -= y[i];
+        }
+        HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_sparse);
+      }
+
+#ifdef HIPO_PIVOTING
+      HIPO_CLOCK_START(2);
+      // apply inverse swaps
+      permuteWithSwaps(&x[x_start], current_swaps, jb, true);
+      HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_swap);
+#endif
+    }
+  }
+}
+
+void HybridSolveHandler::backwardSolve(std::vector<double>& x) const {
+  // Backward solve.
+  // Blas calls: dtrsv, dgemv
+
+  // supernode columns in format FH
+
+  HIPO_CLOCK_CREATE;
+
+  const Int nb = S_.blockSize();
+
+  // go through the sn in reverse order
+  for (Int sn = S_.sn() - 1; sn >= 0; --sn) {
+    // leading size of supernode
+    const Int ldSn = S_.ptr(sn + 1) - S_.ptr(sn);
+
+    // number of columns in the supernode
+    const Int sn_size = S_.snStart(sn + 1) - S_.snStart(sn);
+
+    // first colums of the supernode
+    const Int sn_start = S_.snStart(sn);
+
+    // index to access S->rows for this supernode
+    const Int64 start_row = S_.ptr(sn);
+
+    // number of blocks of columns
+    const Int n_blocks = (sn_size - 1) / nb + 1;
+
+    // index to access snColumns[sn]
+    // initialised with the total number of entries of snColumns[sn]
+    Int64 SnCol_ind = sn_columns_[sn].size() - extra_space_frontal;
+
+    // go through blocks of columns for this supernode in reverse order
+    for (Int j = n_blocks - 1; j >= 0; --j) {
+      // number of columns in the block
+      const Int jb = std::min(nb, sn_size - nb * j);
+
+      // number of entries in diagonal part
+      const Int diag_entries = jb * jb;
+
+      // index to access vector x
+      const Int x_start = sn_start + nb * j;
+
+#ifdef HIPO_PIVOTING
+      HIPO_CLOCK_START(2);
+      // apply swaps to portion of rhs that is affected
+      const Int* current_swaps = &swaps_[sn][nb * j];
+      permuteWithSwaps(&x[x_start], current_swaps, jb);
+      HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_swap);
+#endif
+
+      // temporary space for gemv
+      const Int gemv_space = ldSn - nb * j - jb;
+      std::vector<double> y(gemv_space);
+      if (gemv_space > 0) {
+        HIPO_CLOCK_START(2);
+        // scatter entries into y
+        for (Int i = 0; i < gemv_space; ++i) {
+          const Int row = S_.rows(start_row + nb * j + jb + i);
+          y[i] = x[row];
+        }
+        HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_sparse);
+
+        HIPO_CLOCK_START(2);
+        SnCol_ind -= jb * gemv_space;
+        callAndTime_dgemv('N', jb, gemv_space, -1.0,
+                          &sn_columns_[sn][SnCol_ind], jb, y.data(), 1, 1.0,
+                          &x[x_start], 1, data_);
+        HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_dense);
+      }
+
+      HIPO_CLOCK_START(2);
+      SnCol_ind -= diag_entries;
+      callAndTime_dtrsv('U', 'N', 'U', jb, &sn_columns_[sn][SnCol_ind], jb,
+                        &x[x_start], 1, data_);
+      HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_dense);
+
+#ifdef HIPO_PIVOTING
+      HIPO_CLOCK_START(2);
+      // apply inverse swaps
+      permuteWithSwaps(&x[x_start], current_swaps, jb, true);
+      HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_swap);
+#endif
+    }
+  }
+}
+
+void HybridSolveHandler::diagSolve(std::vector<double>& x) const {
+  // Diagonal solve
+
+  // supernode columns in format FH
+
+  HIPO_CLOCK_CREATE;
+
+  const Int nb = S_.blockSize();
+
+  for (Int sn = 0; sn < S_.sn(); ++sn) {
+    // leading size of supernode
+    const Int ldSn = S_.ptr(sn + 1) - S_.ptr(sn);
+
+    // number of columns in the supernode
+    const Int sn_size = S_.snStart(sn + 1) - S_.snStart(sn);
+
+    // first colums of the supernode
+    const Int sn_start = S_.snStart(sn);
+
+    // number of blocks of columns
+    const Int n_blocks = (sn_size - 1) / nb + 1;
+
+    // index to access diagonal part of block
+    Int diag_start{};
+
+    // go through blocks of columns for this supernode
+    for (Int j = 0; j < n_blocks; ++j) {
+      // number of columns in the block
+      const Int jb = std::min(nb, sn_size - nb * j);
+
+#ifdef HIPO_PIVOTING
+      HIPO_CLOCK_START(2);
+      // apply swaps to portion of rhs that is affected
+      const Int* current_swaps = &swaps_[sn][nb * j];
+      permuteWithSwaps(&x[sn_start + nb * j], current_swaps, jb);
+      HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_swap);
+#endif
+
+      HIPO_CLOCK_START(2);
+
+      const double* current_2x2 = &pivot_2x2_[sn][nb * j];
+      Int step = 1;
+
+      // go through columns of block
+      for (Int col = 0; col < jb; col += step) {
+        if (current_2x2[col] == 0.0) {
+          // 1x1 pivots
+          step = 1;
+          const double inv_d = sn_columns_[sn][diag_start + col + jb * col];
+          x[sn_start + nb * j + col] *= inv_d;
+        } else {
+          // 2x2 pivots
+          step = 2;
+
+          // inverse of 2x2 pivot
+          const double i_d1 = sn_columns_[sn][diag_start + col + jb * col];
+          const double i_d2 =
+              sn_columns_[sn][diag_start + col + 1 + jb * (col + 1)];
+          const double i_off = current_2x2[col];
+
+          double x1 = x[sn_start + nb * j + col];
+          double x2 = x[sn_start + nb * j + col + 1];
+
+          x[sn_start + nb * j + col] = i_d1 * x1 + i_off * x2;
+          x[sn_start + nb * j + col + 1] = i_d2 * x2 + i_off * x1;
+        }
+      }
+
+      HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_dense);
+
+#ifdef HIPO_PIVOTING
+      HIPO_CLOCK_START(2);
+      // apply inverse swaps
+      permuteWithSwaps(&x[sn_start + nb * j], current_swaps, jb, true);
+      HIPO_CLOCK_STOP(2, data_, kTimeSolveSolve_swap);
+#endif
+
+      // move diag_start forward by number of diagonal entries in block
+      diag_start += jb * jb;
+
+      // move diag_start forward by number of sub-diagonal entries in block
+      diag_start += (ldSn - nb * j - jb) * jb;
+    }
+  }
+}
+
+}  // namespace hipo