]> git.kernelconcepts.de Git - karo-tx-uboot.git/blobdiff - drivers/ddr/altera/sequencer.c
ddr: altera: Clean up rw_mgr_mem_calibrate_vfifo_center() part 10
[karo-tx-uboot.git] / drivers / ddr / altera / sequencer.c
index 451e141854bac5da61b8e5159d1d653af213b03d..efcf28302e6032612ceb5fecededc5c7febc0caa 100644 (file)
@@ -1169,28 +1169,39 @@ static void rw_mgr_mem_calibrate_read_load_patterns(const u32 rank_bgn,
        set_rank_and_odt_mask(0, RW_MGR_ODT_MODE_OFF);
 }
 
-/*
- * try a read and see if it returns correct data back. has dummy reads
- * inserted into the mix used to align dqs enable. has more thorough checks
- * than the regular read test.
+/**
+ * rw_mgr_mem_calibrate_read_test() - Perform READ test on single rank
+ * @rank_bgn:          Rank number
+ * @group:             Read/Write group
+ * @num_tries:         Number of retries of the test
+ * @all_correct:       All bits must be correct in the mask
+ * @bit_chk:           Resulting bit mask after the test
+ * @all_groups:                Test all R/W groups
+ * @all_ranks:         Test all ranks
+ *
+ * Try a read and see if it returns correct data back. Test has dummy reads
+ * inserted into the mix used to align DQS enable. Test has more thorough
+ * checks than the regular read test.
  */
-static uint32_t rw_mgr_mem_calibrate_read_test(uint32_t rank_bgn, uint32_t group,
-       uint32_t num_tries, uint32_t all_correct, uint32_t *bit_chk,
-       uint32_t all_groups, uint32_t all_ranks)
+static int
+rw_mgr_mem_calibrate_read_test(const u32 rank_bgn, const u32 group,
+                              const u32 num_tries, const u32 all_correct,
+                              u32 *bit_chk,
+                              const u32 all_groups, const u32 all_ranks)
 {
-       uint32_t r, vg;
-       uint32_t correct_mask_vg;
-       uint32_t tmp_bit_chk;
-       uint32_t rank_end = all_ranks ? RW_MGR_MEM_NUMBER_OF_RANKS :
+       const u32 rank_end = all_ranks ? RW_MGR_MEM_NUMBER_OF_RANKS :
                (rank_bgn + NUM_RANKS_PER_SHADOW_REG);
-       uint32_t addr;
-       uint32_t base_rw_mgr;
+       const u32 quick_read_mode =
+               ((STATIC_CALIB_STEPS & CALIB_SKIP_DELAY_SWEEPS) &&
+                ENABLE_SUPER_QUICK_CALIBRATION);
+       u32 correct_mask_vg = param->read_correct_mask_vg;
+       u32 tmp_bit_chk;
+       u32 base_rw_mgr;
+       u32 addr;
 
-       *bit_chk = param->read_correct_mask;
-       correct_mask_vg = param->read_correct_mask_vg;
+       int r, vg, ret;
 
-       uint32_t quick_read_mode = (((STATIC_CALIB_STEPS) &
-               CALIB_SKIP_DELAY_SWEEPS) && ENABLE_SUPER_QUICK_CALIBRATION);
+       *bit_chk = param->read_correct_mask;
 
        for (r = rank_bgn; r < rank_end; r++) {
                if (param->skip_ranks[r])
@@ -1230,254 +1241,313 @@ static uint32_t rw_mgr_mem_calibrate_read_test(uint32_t rank_bgn, uint32_t group
                        &sdr_rw_load_jump_mgr_regs->load_jump_add3);
 
                tmp_bit_chk = 0;
-               for (vg = RW_MGR_MEM_VIRTUAL_GROUPS_PER_READ_DQS-1; ; vg--) {
-                       /* reset the fifos to get pointers to known state */
+               for (vg = RW_MGR_MEM_VIRTUAL_GROUPS_PER_READ_DQS - 1; vg >= 0;
+                    vg--) {
+                       /* Reset the FIFOs to get pointers to known state. */
                        writel(0, &phy_mgr_cmd->fifo_reset);
                        writel(0, SDR_PHYGRP_RWMGRGRP_ADDRESS |
                                  RW_MGR_RESET_READ_DATAPATH_OFFSET);
 
-                       tmp_bit_chk = tmp_bit_chk << (RW_MGR_MEM_DQ_PER_READ_DQS
-                               / RW_MGR_MEM_VIRTUAL_GROUPS_PER_READ_DQS);
-
-                       if (all_groups)
-                               addr = SDR_PHYGRP_RWMGRGRP_ADDRESS | RW_MGR_RUN_ALL_GROUPS_OFFSET;
-                       else
-                               addr = SDR_PHYGRP_RWMGRGRP_ADDRESS | RW_MGR_RUN_SINGLE_GROUP_OFFSET;
+                       if (all_groups) {
+                               addr = SDR_PHYGRP_RWMGRGRP_ADDRESS |
+                                      RW_MGR_RUN_ALL_GROUPS_OFFSET;
+                       } else {
+                               addr = SDR_PHYGRP_RWMGRGRP_ADDRESS |
+                                      RW_MGR_RUN_SINGLE_GROUP_OFFSET;
+                       }
 
                        writel(RW_MGR_READ_B2B, addr +
                               ((group * RW_MGR_MEM_VIRTUAL_GROUPS_PER_READ_DQS +
                               vg) << 2));
 
                        base_rw_mgr = readl(SDR_PHYGRP_RWMGRGRP_ADDRESS);
-                       tmp_bit_chk = tmp_bit_chk | (correct_mask_vg & ~(base_rw_mgr));
-
-                       if (vg == 0)
-                               break;
+                       tmp_bit_chk <<= RW_MGR_MEM_DQ_PER_READ_DQS /
+                                       RW_MGR_MEM_VIRTUAL_GROUPS_PER_READ_DQS;
+                       tmp_bit_chk |= correct_mask_vg & ~(base_rw_mgr);
                }
+
                *bit_chk &= tmp_bit_chk;
        }
 
        addr = SDR_PHYGRP_RWMGRGRP_ADDRESS | RW_MGR_RUN_SINGLE_GROUP_OFFSET;
        writel(RW_MGR_CLEAR_DQS_ENABLE, addr + (group << 2));
 
+       set_rank_and_odt_mask(0, RW_MGR_ODT_MODE_OFF);
+
        if (all_correct) {
-               set_rank_and_odt_mask(0, RW_MGR_ODT_MODE_OFF);
-               debug_cond(DLEVEL == 2, "%s:%d read_test(%u,ALL,%u) =>\
-                          (%u == %u) => %lu", __func__, __LINE__, group,
-                          all_groups, *bit_chk, param->read_correct_mask,
-                          (long unsigned int)(*bit_chk ==
-                          param->read_correct_mask));
-               return *bit_chk == param->read_correct_mask;
+               ret = (*bit_chk == param->read_correct_mask);
+               debug_cond(DLEVEL == 2,
+                          "%s:%d read_test(%u,ALL,%u) => (%u == %u) => %i\n",
+                          __func__, __LINE__, group, all_groups, *bit_chk,
+                          param->read_correct_mask, ret);
        } else  {
-               set_rank_and_odt_mask(0, RW_MGR_ODT_MODE_OFF);
-               debug_cond(DLEVEL == 2, "%s:%d read_test(%u,ONE,%u) =>\
-                          (%u != %lu) => %lu\n", __func__, __LINE__,
-                          group, all_groups, *bit_chk, (long unsigned int)0,
-                          (long unsigned int)(*bit_chk != 0x00));
-               return *bit_chk != 0x00;
+               ret = (*bit_chk != 0x00);
+               debug_cond(DLEVEL == 2,
+                          "%s:%d read_test(%u,ONE,%u) => (%u != %u) => %i\n",
+                          __func__, __LINE__, group, all_groups, *bit_chk,
+                          0, ret);
        }
+
+       return ret;
 }
 
-static uint32_t rw_mgr_mem_calibrate_read_test_all_ranks(uint32_t group,
-       uint32_t num_tries, uint32_t all_correct, uint32_t *bit_chk,
-       uint32_t all_groups)
+/**
+ * rw_mgr_mem_calibrate_read_test_all_ranks() - Perform READ test on all ranks
+ * @grp:               Read/Write group
+ * @num_tries:         Number of retries of the test
+ * @all_correct:       All bits must be correct in the mask
+ * @all_groups:                Test all R/W groups
+ *
+ * Perform a READ test across all memory ranks.
+ */
+static int
+rw_mgr_mem_calibrate_read_test_all_ranks(const u32 grp, const u32 num_tries,
+                                        const u32 all_correct,
+                                        const u32 all_groups)
 {
-       return rw_mgr_mem_calibrate_read_test(0, group, num_tries, all_correct,
-                                             bit_chk, all_groups, 1);
+       u32 bit_chk;
+       return rw_mgr_mem_calibrate_read_test(0, grp, num_tries, all_correct,
+                                             &bit_chk, all_groups, 1);
 }
 
-static void rw_mgr_incr_vfifo(uint32_t grp, uint32_t *v)
+/**
+ * rw_mgr_incr_vfifo() - Increase VFIFO value
+ * @grp:       Read/Write group
+ *
+ * Increase VFIFO value.
+ */
+static void rw_mgr_incr_vfifo(const u32 grp)
 {
        writel(grp, &phy_mgr_cmd->inc_vfifo_hard_phy);
-       (*v)++;
 }
 
-static void rw_mgr_decr_vfifo(uint32_t grp, uint32_t *v)
+/**
+ * rw_mgr_decr_vfifo() - Decrease VFIFO value
+ * @grp:       Read/Write group
+ *
+ * Decrease VFIFO value.
+ */
+static void rw_mgr_decr_vfifo(const u32 grp)
 {
-       uint32_t i;
+       u32 i;
 
-       for (i = 0; i < VFIFO_SIZE-1; i++)
-               rw_mgr_incr_vfifo(grp, v);
+       for (i = 0; i < VFIFO_SIZE - 1; i++)
+               rw_mgr_incr_vfifo(grp);
 }
 
-static int find_vfifo_read(uint32_t grp, uint32_t *bit_chk)
+/**
+ * find_vfifo_failing_read() - Push VFIFO to get a failing read
+ * @grp:       Read/Write group
+ *
+ * Push VFIFO until a failing read happens.
+ */
+static int find_vfifo_failing_read(const u32 grp)
 {
-       uint32_t  v;
-       uint32_t fail_cnt = 0;
-       uint32_t test_status;
+       u32 v, ret, fail_cnt = 0;
 
-       for (v = 0; v < VFIFO_SIZE; ) {
-               debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: vfifo %u\n",
+       for (v = 0; v < VFIFO_SIZE; v++) {
+               debug_cond(DLEVEL == 2, "%s:%d: vfifo %u\n",
                           __func__, __LINE__, v);
-               test_status = rw_mgr_mem_calibrate_read_test_all_ranks
-                       (grp, 1, PASS_ONE_BIT, bit_chk, 0);
-               if (!test_status) {
+               ret = rw_mgr_mem_calibrate_read_test_all_ranks(grp, 1,
+                                               PASS_ONE_BIT, 0);
+               if (!ret) {
                        fail_cnt++;
 
                        if (fail_cnt == 2)
-                               break;
+                               return v;
                }
 
-               /* fiddle with FIFO */
-               rw_mgr_incr_vfifo(grp, &v);
+               /* Fiddle with FIFO. */
+               rw_mgr_incr_vfifo(grp);
        }
 
-       if (v >= VFIFO_SIZE) {
-               /* no failing read found!! Something must have gone wrong */
-               debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: vfifo failed\n",
-                          __func__, __LINE__);
-               return 0;
-       } else {
-               return v;
-       }
+       /* No failing read found! Something must have gone wrong. */
+       debug_cond(DLEVEL == 2, "%s:%d: vfifo failed\n", __func__, __LINE__);
+       return 0;
 }
 
-static int find_working_phase(uint32_t *grp, uint32_t *bit_chk,
-                             uint32_t dtaps_per_ptap, uint32_t *work_bgn,
-                             uint32_t *v, uint32_t *d, uint32_t *p,
-                             uint32_t *i, uint32_t *max_working_cnt)
+/**
+ * sdr_find_phase_delay() - Find DQS enable phase or delay
+ * @working:   If 1, look for working phase/delay, if 0, look for non-working
+ * @delay:     If 1, look for delay, if 0, look for phase
+ * @grp:       Read/Write group
+ * @work:      Working window position
+ * @work_inc:  Working window increment
+ * @pd:                DQS Phase/Delay Iterator
+ *
+ * Find working or non-working DQS enable phase setting.
+ */
+static int sdr_find_phase_delay(int working, int delay, const u32 grp,
+                               u32 *work, const u32 work_inc, u32 *pd)
 {
-       uint32_t found_begin = 0;
-       uint32_t tmp_delay = 0;
-       uint32_t test_status;
+       const u32 max = delay ? IO_DQS_EN_DELAY_MAX : IO_DQS_EN_PHASE_MAX;
+       u32 ret;
 
-       for (*d = 0; *d <= dtaps_per_ptap; (*d)++, tmp_delay +=
-               IO_DELAY_PER_DQS_EN_DCHAIN_TAP) {
-               *work_bgn = tmp_delay;
-               scc_mgr_set_dqs_en_delay_all_ranks(*grp, *d);
+       for (; *pd <= max; (*pd)++) {
+               if (delay)
+                       scc_mgr_set_dqs_en_delay_all_ranks(grp, *pd);
+               else
+                       scc_mgr_set_dqs_en_phase_all_ranks(grp, *pd);
 
-               for (*i = 0; *i < VFIFO_SIZE; (*i)++) {
-                       for (*p = 0; *p <= IO_DQS_EN_PHASE_MAX; (*p)++, *work_bgn +=
-                               IO_DELAY_PER_OPA_TAP) {
-                               scc_mgr_set_dqs_en_phase_all_ranks(*grp, *p);
+               ret = rw_mgr_mem_calibrate_read_test_all_ranks(grp, 1,
+                                       PASS_ONE_BIT, 0);
+               if (!working)
+                       ret = !ret;
 
-                               test_status =
-                               rw_mgr_mem_calibrate_read_test_all_ranks
-                               (*grp, 1, PASS_ONE_BIT, bit_chk, 0);
+               if (ret)
+                       return 0;
 
-                               if (test_status) {
-                                       *max_working_cnt = 1;
-                                       found_begin = 1;
-                                       break;
-                               }
-                       }
+               if (work)
+                       *work += work_inc;
+       }
 
-                       if (found_begin)
-                               break;
+       return -EINVAL;
+}
+/**
+ * sdr_find_phase() - Find DQS enable phase
+ * @working:   If 1, look for working phase, if 0, look for non-working phase
+ * @grp:       Read/Write group
+ * @work:      Working window position
+ * @i:         Iterator
+ * @p:         DQS Phase Iterator
+ *
+ * Find working or non-working DQS enable phase setting.
+ */
+static int sdr_find_phase(int working, const u32 grp, u32 *work,
+                         u32 *i, u32 *p)
+{
+       const u32 end = VFIFO_SIZE + (working ? 0 : 1);
+       int ret;
 
-                       if (*p > IO_DQS_EN_PHASE_MAX)
-                               /* fiddle with FIFO */
-                               rw_mgr_incr_vfifo(*grp, v);
-               }
+       for (; *i < end; (*i)++) {
+               if (working)
+                       *p = 0;
 
-               if (found_begin)
-                       break;
+               ret = sdr_find_phase_delay(working, 0, grp, work,
+                                          IO_DELAY_PER_OPA_TAP, p);
+               if (!ret)
+                       return 0;
+
+               if (*p > IO_DQS_EN_PHASE_MAX) {
+                       /* Fiddle with FIFO. */
+                       rw_mgr_incr_vfifo(grp);
+                       if (!working)
+                               *p = 0;
+               }
        }
 
-       if (*i >= VFIFO_SIZE) {
-               /* cannot find working solution */
-               debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: no vfifo/\
-                          ptap/dtap\n", __func__, __LINE__);
-               return 0;
-       } else {
-               return 1;
+       return -EINVAL;
+}
+
+/**
+ * sdr_working_phase() - Find working DQS enable phase
+ * @grp:       Read/Write group
+ * @work_bgn:  Working window start position
+ * @d:         dtaps output value
+ * @p:         DQS Phase Iterator
+ * @i:         Iterator
+ *
+ * Find working DQS enable phase setting.
+ */
+static int sdr_working_phase(const u32 grp, u32 *work_bgn, u32 *d,
+                            u32 *p, u32 *i)
+{
+       const u32 dtaps_per_ptap = IO_DELAY_PER_OPA_TAP /
+                                  IO_DELAY_PER_DQS_EN_DCHAIN_TAP;
+       int ret;
+
+       *work_bgn = 0;
+
+       for (*d = 0; *d <= dtaps_per_ptap; (*d)++) {
+               *i = 0;
+               scc_mgr_set_dqs_en_delay_all_ranks(grp, *d);
+               ret = sdr_find_phase(1, grp, work_bgn, i, p);
+               if (!ret)
+                       return 0;
+               *work_bgn += IO_DELAY_PER_DQS_EN_DCHAIN_TAP;
        }
+
+       /* Cannot find working solution */
+       debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: no vfifo/ptap/dtap\n",
+                  __func__, __LINE__);
+       return -EINVAL;
 }
 
-static void sdr_backup_phase(uint32_t *grp, uint32_t *bit_chk,
-                            uint32_t *work_bgn, uint32_t *v, uint32_t *d,
-                            uint32_t *p, uint32_t *max_working_cnt)
+/**
+ * sdr_backup_phase() - Find DQS enable backup phase
+ * @grp:       Read/Write group
+ * @work_bgn:  Working window start position
+ * @p:         DQS Phase Iterator
+ *
+ * Find DQS enable backup phase setting.
+ */
+static void sdr_backup_phase(const u32 grp, u32 *work_bgn, u32 *p)
 {
-       uint32_t found_begin = 0;
-       uint32_t tmp_delay;
+       u32 tmp_delay, d;
+       int ret;
 
        /* Special case code for backing up a phase */
        if (*p == 0) {
                *p = IO_DQS_EN_PHASE_MAX;
-               rw_mgr_decr_vfifo(*grp, v);
+               rw_mgr_decr_vfifo(grp);
        } else {
                (*p)--;
        }
        tmp_delay = *work_bgn - IO_DELAY_PER_OPA_TAP;
-       scc_mgr_set_dqs_en_phase_all_ranks(*grp, *p);
+       scc_mgr_set_dqs_en_phase_all_ranks(grp, *p);
 
-       for (*d = 0; *d <= IO_DQS_EN_DELAY_MAX && tmp_delay < *work_bgn;
-               (*d)++, tmp_delay += IO_DELAY_PER_DQS_EN_DCHAIN_TAP) {
-               scc_mgr_set_dqs_en_delay_all_ranks(*grp, *d);
+       for (d = 0; d <= IO_DQS_EN_DELAY_MAX && tmp_delay < *work_bgn; d++) {
+               scc_mgr_set_dqs_en_delay_all_ranks(grp, d);
 
-               if (rw_mgr_mem_calibrate_read_test_all_ranks(*grp, 1,
-                                                            PASS_ONE_BIT,
-                                                            bit_chk, 0)) {
-                       found_begin = 1;
+               ret = rw_mgr_mem_calibrate_read_test_all_ranks(grp, 1,
+                                       PASS_ONE_BIT, 0);
+               if (ret) {
                        *work_bgn = tmp_delay;
                        break;
                }
-       }
 
-       /* We have found a working dtap before the ptap found above */
-       if (found_begin == 1)
-               (*max_working_cnt)++;
+               tmp_delay += IO_DELAY_PER_DQS_EN_DCHAIN_TAP;
+       }
 
-       /*
-        * Restore VFIFO to old state before we decremented it
-        * (if needed).
-        */
+       /* Restore VFIFO to old state before we decremented it (if needed). */
        (*p)++;
        if (*p > IO_DQS_EN_PHASE_MAX) {
                *p = 0;
-               rw_mgr_incr_vfifo(*grp, v);
+               rw_mgr_incr_vfifo(grp);
        }
 
-       scc_mgr_set_dqs_en_delay_all_ranks(*grp, 0);
+       scc_mgr_set_dqs_en_delay_all_ranks(grp, 0);
 }
 
-static int sdr_nonworking_phase(uint32_t *grp, uint32_t *bit_chk,
-                            uint32_t *work_bgn, uint32_t *v, uint32_t *d,
-                            uint32_t *p, uint32_t *i, uint32_t *max_working_cnt,
-                            uint32_t *work_end)
+/**
+ * sdr_nonworking_phase() - Find non-working DQS enable phase
+ * @grp:       Read/Write group
+ * @work_end:  Working window end position
+ * @p:         DQS Phase Iterator
+ * @i:         Iterator
+ *
+ * Find non-working DQS enable phase setting.
+ */
+static int sdr_nonworking_phase(const u32 grp, u32 *work_end, u32 *p, u32 *i)
 {
-       uint32_t found_end = 0;
+       int ret;
 
        (*p)++;
        *work_end += IO_DELAY_PER_OPA_TAP;
        if (*p > IO_DQS_EN_PHASE_MAX) {
-               /* fiddle with FIFO */
+               /* Fiddle with FIFO. */
                *p = 0;
-               rw_mgr_incr_vfifo(*grp, v);
+               rw_mgr_incr_vfifo(grp);
        }
 
-       for (; *i < VFIFO_SIZE + 1; (*i)++) {
-               for (; *p <= IO_DQS_EN_PHASE_MAX; (*p)++, *work_end
-                       += IO_DELAY_PER_OPA_TAP) {
-                       scc_mgr_set_dqs_en_phase_all_ranks(*grp, *p);
-
-                       if (!rw_mgr_mem_calibrate_read_test_all_ranks
-                               (*grp, 1, PASS_ONE_BIT, bit_chk, 0)) {
-                               found_end = 1;
-                               break;
-                       } else {
-                               (*max_working_cnt)++;
-                       }
-               }
-
-               if (found_end)
-                       break;
-
-               if (*p > IO_DQS_EN_PHASE_MAX) {
-                       /* fiddle with FIFO */
-                       rw_mgr_incr_vfifo(*grp, v);
-                       *p = 0;
-               }
+       ret = sdr_find_phase(0, grp, work_end, i, p);
+       if (ret) {
+               /* Cannot see edge of failing read. */
+               debug_cond(DLEVEL == 2, "%s:%d: end: failed\n",
+                          __func__, __LINE__);
        }
 
-       if (*i >= VFIFO_SIZE + 1) {
-               /* cannot see edge of failing read */
-               debug_cond(DLEVEL == 2, "%s:%d sdr_nonworking_phase: end:\
-                          failed\n", __func__, __LINE__);
-               return 0;
-       } else {
-               return 1;
-       }
+       return ret;
 }
 
 /**
@@ -1485,14 +1555,13 @@ static int sdr_nonworking_phase(uint32_t *grp, uint32_t *bit_chk,
  * @grp:       Read/Write group
  * @work_bgn:  First working settings
  * @work_end:  Last working settings
- * @val:       VFIFO value
  *
  * Find center of the working DQS enable window.
  */
 static int sdr_find_window_center(const u32 grp, const u32 work_bgn,
-                                 const u32 work_end, const u32 val)
+                                 const u32 work_end)
 {
-       u32 bit_chk, work_mid, v = val;
+       u32 work_mid;
        int tmp_delay = 0;
        int i, p, d;
 
@@ -1529,19 +1598,18 @@ static int sdr_find_window_center(const u32 grp, const u32 work_bgn,
         * because the largest possible margin in 1 VFIFO cycle.
         */
        for (i = 0; i < VFIFO_SIZE; i++) {
-               debug_cond(DLEVEL == 2, "find_dqs_en_phase: center: vfifo=%u\n",
-                          v);
+               debug_cond(DLEVEL == 2, "find_dqs_en_phase: center\n");
                if (rw_mgr_mem_calibrate_read_test_all_ranks(grp, 1,
                                                             PASS_ONE_BIT,
-                                                            &bit_chk, 0)) {
+                                                            0)) {
                        debug_cond(DLEVEL == 2,
-                                  "%s:%d center: found: vfifo=%u ptap=%u dtap=%u\n",
-                                  __func__, __LINE__, v, p, d);
+                                  "%s:%d center: found: ptap=%u dtap=%u\n",
+                                  __func__, __LINE__, p, d);
                        return 0;
                }
 
                /* Fiddle with FIFO. */
-               rw_mgr_incr_vfifo(grp, &v);
+               rw_mgr_incr_vfifo(grp);
        }
 
        debug_cond(DLEVEL == 2, "%s:%d center: failed.\n",
@@ -1549,15 +1617,19 @@ static int sdr_find_window_center(const u32 grp, const u32 work_bgn,
        return -EINVAL;
 }
 
-/* find a good dqs enable to use */
-static uint32_t rw_mgr_mem_calibrate_vfifo_find_dqs_en_phase(uint32_t grp)
+/**
+ * rw_mgr_mem_calibrate_vfifo_find_dqs_en_phase() - Find a good DQS enable to use
+ * @grp:       Read/Write Group
+ *
+ * Find a good DQS enable to use.
+ */
+static int rw_mgr_mem_calibrate_vfifo_find_dqs_en_phase(const u32 grp)
 {
-       uint32_t v, d, p, i;
-       uint32_t max_working_cnt;
-       uint32_t bit_chk;
-       uint32_t dtaps_per_ptap;
-       uint32_t work_bgn, work_end;
-       uint32_t found_passing_read, found_failing_read, initial_failing_dtap;
+       u32 d, p, i;
+       u32 dtaps_per_ptap;
+       u32 work_bgn, work_end;
+       u32 found_passing_read, found_failing_read, initial_failing_dtap;
+       int ret;
 
        debug("%s:%d %u\n", __func__, __LINE__, grp);
 
@@ -1566,52 +1638,46 @@ static uint32_t rw_mgr_mem_calibrate_vfifo_find_dqs_en_phase(uint32_t grp)
        scc_mgr_set_dqs_en_delay_all_ranks(grp, 0);
        scc_mgr_set_dqs_en_phase_all_ranks(grp, 0);
 
-       /* ************************************************************** */
-       /* * Step 0 : Determine number of delay taps for each phase tap * */
-       dtaps_per_ptap = IO_DELAY_PER_OPA_TAP/IO_DELAY_PER_DQS_EN_DCHAIN_TAP;
+       /* Step 0: Determine number of delay taps for each phase tap. */
+       dtaps_per_ptap = IO_DELAY_PER_OPA_TAP / IO_DELAY_PER_DQS_EN_DCHAIN_TAP;
 
-       /* ********************************************************* */
-       /* * Step 1 : First push vfifo until we get a failing read * */
-       v = find_vfifo_read(grp, &bit_chk);
+       /* Step 1: First push vfifo until we get a failing read. */
+       find_vfifo_failing_read(grp);
 
-       max_working_cnt = 0;
-
-       /* ******************************************************** */
-       /* * step 2: find first working phase, increment in ptaps * */
+       /* Step 2: Find first working phase, increment in ptaps. */
        work_bgn = 0;
-       if (find_working_phase(&grp, &bit_chk, dtaps_per_ptap, &work_bgn, &v, &d,
-                               &p, &i, &max_working_cnt) == 0)
-               return 0;
+       ret = sdr_working_phase(grp, &work_bgn, &d, &p, &i);
+       if (ret)
+               return ret;
 
        work_end = work_bgn;
 
        /*
-        * If d is 0 then the working window covers a phase tap and
-        * we can follow the old procedure otherwise, we've found the beginning,
+        * If d is 0 then the working window covers a phase tap and we can
+        * follow the old procedure. Otherwise, we've found the beginning
         * and we need to increment the dtaps until we find the end.
         */
        if (d == 0) {
-               /* ********************************************************* */
-               /* * step 3a: if we have room, back off by one and
-               increment in dtaps * */
-
-               sdr_backup_phase(&grp, &bit_chk, &work_bgn, &v, &d, &p,
-                                &max_working_cnt);
-
-               /* ********************************************************* */
-               /* * step 4a: go forward from working phase to non working
-               phase, increment in ptaps * */
-               if (sdr_nonworking_phase(&grp, &bit_chk, &work_bgn, &v, &d, &p,
-                                        &i, &max_working_cnt, &work_end) == 0)
-                       return 0;
+               /*
+                * Step 3a: If we have room, back off by one and
+                *          increment in dtaps.
+                */
+               sdr_backup_phase(grp, &work_bgn, &p);
+
+               /*
+                * Step 4a: go forward from working phase to non working
+                * phase, increment in ptaps.
+                */
+               ret = sdr_nonworking_phase(grp, &work_end, &p, &i);
+               if (ret)
+                       return ret;
 
-               /* ********************************************************* */
-               /* * step 5a:  back off one from last, increment in dtaps  * */
+               /* Step 5a: Back off one from last, increment in dtaps. */
 
                /* Special case code for backing up a phase */
                if (p == 0) {
                        p = IO_DQS_EN_PHASE_MAX;
-                       rw_mgr_decr_vfifo(grp, &v);
+                       rw_mgr_decr_vfifo(grp);
                } else {
                        p = p - 1;
                }
@@ -1619,133 +1685,80 @@ static uint32_t rw_mgr_mem_calibrate_vfifo_find_dqs_en_phase(uint32_t grp)
                work_end -= IO_DELAY_PER_OPA_TAP;
                scc_mgr_set_dqs_en_phase_all_ranks(grp, p);
 
-               /* * The actual increment of dtaps is done outside of
-               the if/else loop to share code */
                d = 0;
 
-               debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: v/p: \
-                          vfifo=%u ptap=%u\n", __func__, __LINE__,
-                          v, p);
-       } else {
-               /* ******************************************************* */
-               /* * step 3-5b:  Find the right edge of the window using
-               delay taps   * */
-               debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase:vfifo=%u \
-                          ptap=%u dtap=%u bgn=%u\n", __func__, __LINE__,
-                          v, p, d, work_bgn);
-
-               work_end = work_bgn;
-
-               /* * The actual increment of dtaps is done outside of the
-               if/else loop to share code */
-
-               /* Only here to counterbalance a subtract later on which is
-               not needed if this branch of the algorithm is taken */
-               max_working_cnt++;
+               debug_cond(DLEVEL == 2, "%s:%d p: ptap=%u\n",
+                          __func__, __LINE__, p);
        }
 
-       /* The dtap increment to find the failing edge is done here */
-       for (; d <= IO_DQS_EN_DELAY_MAX; d++, work_end +=
-               IO_DELAY_PER_DQS_EN_DCHAIN_TAP) {
-                       debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: \
-                                  end-2: dtap=%u\n", __func__, __LINE__, d);
-                       scc_mgr_set_dqs_en_delay_all_ranks(grp, d);
-
-                       if (!rw_mgr_mem_calibrate_read_test_all_ranks(grp, 1,
-                                                                     PASS_ONE_BIT,
-                                                                     &bit_chk, 0)) {
-                               break;
-                       }
-       }
+       /* The dtap increment to find the failing edge is done here. */
+       sdr_find_phase_delay(0, 1, grp, &work_end,
+                            IO_DELAY_PER_DQS_EN_DCHAIN_TAP, &d);
 
        /* Go back to working dtap */
        if (d != 0)
                work_end -= IO_DELAY_PER_DQS_EN_DCHAIN_TAP;
 
-       debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: v/p/d: vfifo=%u \
-                  ptap=%u dtap=%u end=%u\n", __func__, __LINE__,
-                  v, p, d-1, work_end);
+       debug_cond(DLEVEL == 2,
+                  "%s:%d p/d: ptap=%u dtap=%u end=%u\n",
+                  __func__, __LINE__, p, d - 1, work_end);
 
        if (work_end < work_bgn) {
                /* nil range */
-               debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: end-2: \
-                          failed\n", __func__, __LINE__);
-               return 0;
+               debug_cond(DLEVEL == 2, "%s:%d end-2: failed\n",
+                          __func__, __LINE__);
+               return -EINVAL;
        }
 
-       debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: found range [%u,%u]\n",
+       debug_cond(DLEVEL == 2, "%s:%d found range [%u,%u]\n",
                   __func__, __LINE__, work_bgn, work_end);
 
-       /* *************************************************************** */
        /*
-        * * We need to calculate the number of dtaps that equal a ptap
-        * To do that we'll back up a ptap and re-find the edge of the
-        * window using dtaps
+        * We need to calculate the number of dtaps that equal a ptap.
+        * To do that we'll back up a ptap and re-find the edge of the
+        * window using dtaps
         */
-
-       debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: calculate dtaps_per_ptap \
-                  for tracking\n", __func__, __LINE__);
+       debug_cond(DLEVEL == 2, "%s:%d calculate dtaps_per_ptap for tracking\n",
+                  __func__, __LINE__);
 
        /* Special case code for backing up a phase */
        if (p == 0) {
                p = IO_DQS_EN_PHASE_MAX;
-               rw_mgr_decr_vfifo(grp, &v);
-               debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: backedup \
-                          cycle/phase: v=%u p=%u\n", __func__, __LINE__,
-                          v, p);
+               rw_mgr_decr_vfifo(grp);
+               debug_cond(DLEVEL == 2, "%s:%d backedup cycle/phase: p=%u\n",
+                          __func__, __LINE__, p);
        } else {
                p = p - 1;
-               debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: backedup \
-                          phase only: v=%u p=%u", __func__, __LINE__,
-                          v, p);
+               debug_cond(DLEVEL == 2, "%s:%d backedup phase only: p=%u",
+                          __func__, __LINE__, p);
        }
 
        scc_mgr_set_dqs_en_phase_all_ranks(grp, p);
 
        /*
         * Increase dtap until we first see a passing read (in case the
-        * window is smaller than a ptap),
-        * and then a failing read to mark the edge of the window again
+        * window is smaller than a ptap), and then a failing read to
+        * mark the edge of the window again.
         */
 
-       /* Find a passing read */
-       debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: find passing read\n",
+       /* Find a passing read. */
+       debug_cond(DLEVEL == 2, "%s:%d find passing read\n",
                   __func__, __LINE__);
-       found_passing_read = 0;
-       found_failing_read = 0;
-       initial_failing_dtap = d;
-       for (; d <= IO_DQS_EN_DELAY_MAX; d++) {
-               debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: testing \
-                          read d=%u\n", __func__, __LINE__, d);
-               scc_mgr_set_dqs_en_delay_all_ranks(grp, d);
 
-               if (rw_mgr_mem_calibrate_read_test_all_ranks(grp, 1,
-                                                            PASS_ONE_BIT,
-                                                            &bit_chk, 0)) {
-                       found_passing_read = 1;
-                       break;
-               }
-       }
+       initial_failing_dtap = d;
 
+       found_passing_read = !sdr_find_phase_delay(1, 1, grp, NULL, 0, &d);
        if (found_passing_read) {
-               /* Find a failing read */
-               debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: find failing \
-                          read\n", __func__, __LINE__);
-               for (d = d + 1; d <= IO_DQS_EN_DELAY_MAX; d++) {
-                       debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: \
-                                  testing read d=%u\n", __func__, __LINE__, d);
-                       scc_mgr_set_dqs_en_delay_all_ranks(grp, d);
-
-                       if (!rw_mgr_mem_calibrate_read_test_all_ranks
-                               (grp, 1, PASS_ONE_BIT, &bit_chk, 0)) {
-                               found_failing_read = 1;
-                               break;
-                       }
-               }
+               /* Find a failing read. */
+               debug_cond(DLEVEL == 2, "%s:%d find failing read\n",
+                          __func__, __LINE__);
+               d++;
+               found_failing_read = !sdr_find_phase_delay(0, 1, grp, NULL, 0,
+                                                          &d);
        } else {
-               debug_cond(DLEVEL == 1, "%s:%d find_dqs_en_phase: failed to \
-                          calculate dtaps", __func__, __LINE__);
-               debug_cond(DLEVEL == 1, "per ptap. Fall back on static value\n");
+               debug_cond(DLEVEL == 1,
+                          "%s:%d failed to calculate dtaps per ptap. Fall back on static value\n",
+                          __func__, __LINE__);
        }
 
        /*
@@ -1758,299 +1771,503 @@ static uint32_t rw_mgr_mem_calibrate_vfifo_find_dqs_en_phase(uint32_t grp)
                dtaps_per_ptap = d - initial_failing_dtap;
 
        writel(dtaps_per_ptap, &sdr_reg_file->dtaps_per_ptap);
-       debug_cond(DLEVEL == 2, "%s:%d find_dqs_en_phase: dtaps_per_ptap=%u \
-                  - %u = %u",  __func__, __LINE__, d,
-                  initial_failing_dtap, dtaps_per_ptap);
+       debug_cond(DLEVEL == 2, "%s:%d dtaps_per_ptap=%u - %u = %u",
+                  __func__, __LINE__, d, initial_failing_dtap, dtaps_per_ptap);
 
-       /* ******************************************** */
-       /* * step 6:  Find the centre of the window   * */
-       if (sdr_find_window_centre(grp, work_bgn, work_end, v))
-               return 0; /* FIXME: Old code, return 0 means failure :-( */
+       /* Step 6: Find the centre of the window. */
+       ret = sdr_find_window_center(grp, work_bgn, work_end);
 
-       return 1;
+       return ret;
 }
 
-/* per-bit deskew DQ and center */
-static uint32_t rw_mgr_mem_calibrate_vfifo_center(uint32_t rank_bgn,
-       uint32_t write_group, uint32_t read_group, uint32_t test_bgn,
-       uint32_t use_read_test, uint32_t update_fom)
+/**
+ * search_stop_check() - Check if the detected edge is valid
+ * @write:             Perform read (Stage 2) or write (Stage 3) calibration
+ * @d:                 DQS delay
+ * @rank_bgn:          Rank number
+ * @write_group:       Write Group
+ * @read_group:                Read Group
+ * @bit_chk:           Resulting bit mask after the test
+ * @sticky_bit_chk:    Resulting sticky bit mask after the test
+ * @use_read_test:     Perform read test
+ *
+ * Test if the found edge is valid.
+ */
+static u32 search_stop_check(const int write, const int d, const int rank_bgn,
+                            const u32 write_group, const u32 read_group,
+                            u32 *bit_chk, u32 *sticky_bit_chk,
+                            const u32 use_read_test)
 {
-       uint32_t i, p, d, min_index;
+       const u32 ratio = RW_MGR_MEM_IF_READ_DQS_WIDTH /
+                         RW_MGR_MEM_IF_WRITE_DQS_WIDTH;
+       const u32 correct_mask = write ? param->write_correct_mask :
+                                        param->read_correct_mask;
+       const u32 per_dqs = write ? RW_MGR_MEM_DQ_PER_WRITE_DQS :
+                                   RW_MGR_MEM_DQ_PER_READ_DQS;
+       u32 ret;
        /*
-        * Store these as signed since there are comparisons with
-        * signed numbers.
+        * Stop searching when the read test doesn't pass AND when
+        * we've seen a passing read on every bit.
         */
-       uint32_t bit_chk;
-       uint32_t sticky_bit_chk;
-       int32_t left_edge[RW_MGR_MEM_DQ_PER_READ_DQS];
-       int32_t right_edge[RW_MGR_MEM_DQ_PER_READ_DQS];
-       int32_t final_dq[RW_MGR_MEM_DQ_PER_READ_DQS];
-       int32_t mid;
-       int32_t orig_mid_min, mid_min;
-       int32_t new_dqs, start_dqs, start_dqs_en, shift_dq, final_dqs,
-               final_dqs_en;
-       int32_t dq_margin, dqs_margin;
-       uint32_t stop;
-       uint32_t temp_dq_in_delay1, temp_dq_in_delay2;
-       uint32_t addr;
-
-       debug("%s:%d: %u %u", __func__, __LINE__, read_group, test_bgn);
-
-       addr = SDR_PHYGRP_SCCGRP_ADDRESS | SCC_MGR_DQS_IN_DELAY_OFFSET;
-       start_dqs = readl(addr + (read_group << 2));
-       if (IO_SHIFT_DQS_EN_WHEN_SHIFT_DQS)
-               start_dqs_en = readl(addr + ((read_group << 2)
-                                    - IO_DQS_EN_DELAY_OFFSET));
-
-       /* set the left and right edge of each bit to an illegal value */
-       /* use (IO_IO_IN_DELAY_MAX + 1) as an illegal value */
-       sticky_bit_chk = 0;
-       for (i = 0; i < RW_MGR_MEM_DQ_PER_READ_DQS; i++) {
-               left_edge[i]  = IO_IO_IN_DELAY_MAX + 1;
-               right_edge[i] = IO_IO_IN_DELAY_MAX + 1;
-       }
+       if (write) {                    /* WRITE-ONLY */
+               ret = !rw_mgr_mem_calibrate_write_test(rank_bgn, write_group,
+                                                        0, PASS_ONE_BIT,
+                                                        bit_chk, 0);
+       } else if (use_read_test) {     /* READ-ONLY */
+               ret = !rw_mgr_mem_calibrate_read_test(rank_bgn, read_group,
+                                                       NUM_READ_PB_TESTS,
+                                                       PASS_ONE_BIT, bit_chk,
+                                                       0, 0);
+       } else {                        /* READ-ONLY */
+               rw_mgr_mem_calibrate_write_test(rank_bgn, write_group, 0,
+                                               PASS_ONE_BIT, bit_chk, 0);
+               *bit_chk = *bit_chk >> (per_dqs *
+                       (read_group - (write_group * ratio)));
+               ret = (*bit_chk == 0);
+       }
+       *sticky_bit_chk = *sticky_bit_chk | *bit_chk;
+       ret = ret && (*sticky_bit_chk == correct_mask);
+       debug_cond(DLEVEL == 2,
+                  "%s:%d center(left): dtap=%u => %u == %u && %u",
+                  __func__, __LINE__, d,
+                  *sticky_bit_chk, correct_mask, ret);
+       return ret;
+}
 
-       /* Search for the left edge of the window for each bit */
-       for (d = 0; d <= IO_IO_IN_DELAY_MAX; d++) {
-               scc_mgr_apply_group_dq_in_delay(write_group, test_bgn, d);
+/**
+ * search_left_edge() - Find left edge of DQ/DQS working phase
+ * @write:             Perform read (Stage 2) or write (Stage 3) calibration
+ * @rank_bgn:          Rank number
+ * @write_group:       Write Group
+ * @read_group:                Read Group
+ * @test_bgn:          Rank number to begin the test
+ * @sticky_bit_chk:    Resulting sticky bit mask after the test
+ * @left_edge:         Left edge of the DQ/DQS phase
+ * @right_edge:                Right edge of the DQ/DQS phase
+ * @use_read_test:     Perform read test
+ *
+ * Find left edge of DQ/DQS working phase.
+ */
+static void search_left_edge(const int write, const int rank_bgn,
+       const u32 write_group, const u32 read_group, const u32 test_bgn,
+       u32 *sticky_bit_chk,
+       int *left_edge, int *right_edge, const u32 use_read_test)
+{
+       const u32 delay_max = write ? IO_IO_OUT1_DELAY_MAX : IO_IO_IN_DELAY_MAX;
+       const u32 dqs_max = write ? IO_IO_OUT1_DELAY_MAX : IO_DQS_IN_DELAY_MAX;
+       const u32 per_dqs = write ? RW_MGR_MEM_DQ_PER_WRITE_DQS :
+                                   RW_MGR_MEM_DQ_PER_READ_DQS;
+       u32 stop, bit_chk;
+       int i, d;
+
+       for (d = 0; d <= dqs_max; d++) {
+               if (write)
+                       scc_mgr_apply_group_dq_out1_delay(d);
+               else
+                       scc_mgr_apply_group_dq_in_delay(test_bgn, d);
 
                writel(0, &sdr_scc_mgr->update);
 
-               /*
-                * Stop searching when the read test doesn't pass AND when
-                * we've seen a passing read on every bit.
-                */
-               if (use_read_test) {
-                       stop = !rw_mgr_mem_calibrate_read_test(rank_bgn,
-                               read_group, NUM_READ_PB_TESTS, PASS_ONE_BIT,
-                               &bit_chk, 0, 0);
-               } else {
-                       rw_mgr_mem_calibrate_write_test(rank_bgn, write_group,
-                                                       0, PASS_ONE_BIT,
-                                                       &bit_chk, 0);
-                       bit_chk = bit_chk >> (RW_MGR_MEM_DQ_PER_READ_DQS *
-                               (read_group - (write_group *
-                                       RW_MGR_MEM_IF_READ_DQS_WIDTH /
-                                       RW_MGR_MEM_IF_WRITE_DQS_WIDTH)));
-                       stop = (bit_chk == 0);
-               }
-               sticky_bit_chk = sticky_bit_chk | bit_chk;
-               stop = stop && (sticky_bit_chk == param->read_correct_mask);
-               debug_cond(DLEVEL == 2, "%s:%d vfifo_center(left): dtap=%u => %u == %u \
-                          && %u", __func__, __LINE__, d,
-                          sticky_bit_chk,
-                       param->read_correct_mask, stop);
-
-               if (stop == 1) {
+               stop = search_stop_check(write, d, rank_bgn, write_group,
+                                        read_group, &bit_chk, sticky_bit_chk,
+                                        use_read_test);
+               if (stop == 1)
                        break;
-               } else {
-                       for (i = 0; i < RW_MGR_MEM_DQ_PER_READ_DQS; i++) {
-                               if (bit_chk & 1) {
-                                       /* Remember a passing test as the
-                                       left_edge */
-                                       left_edge[i] = d;
-                               } else {
-                                       /* If a left edge has not been seen yet,
-                                       then a future passing test will mark
-                                       this edge as the right edge */
-                                       if (left_edge[i] ==
-                                               IO_IO_IN_DELAY_MAX + 1) {
-                                               right_edge[i] = -(d + 1);
-                                       }
-                               }
-                               bit_chk = bit_chk >> 1;
+
+               /* stop != 1 */
+               for (i = 0; i < per_dqs; i++) {
+                       if (bit_chk & 1) {
+                               /*
+                                * Remember a passing test as
+                                * the left_edge.
+                                */
+                               left_edge[i] = d;
+                       } else {
+                               /*
+                                * If a left edge has not been seen
+                                * yet, then a future passing test
+                                * will mark this edge as the right
+                                * edge.
+                                */
+                               if (left_edge[i] == delay_max + 1)
+                                       right_edge[i] = -(d + 1);
                        }
+                       bit_chk >>= 1;
                }
        }
 
        /* Reset DQ delay chains to 0 */
-       scc_mgr_apply_group_dq_in_delay(test_bgn, 0);
-       sticky_bit_chk = 0;
-       for (i = RW_MGR_MEM_DQ_PER_READ_DQS - 1;; i--) {
-               debug_cond(DLEVEL == 2, "%s:%d vfifo_center: left_edge[%u]: \
-                          %d right_edge[%u]: %d\n", __func__, __LINE__,
-                          i, left_edge[i], i, right_edge[i]);
+       if (write)
+               scc_mgr_apply_group_dq_out1_delay(0);
+       else
+               scc_mgr_apply_group_dq_in_delay(test_bgn, 0);
+
+       *sticky_bit_chk = 0;
+       for (i = per_dqs - 1; i >= 0; i--) {
+               debug_cond(DLEVEL == 2,
+                          "%s:%d vfifo_center: left_edge[%u]: %d right_edge[%u]: %d\n",
+                          __func__, __LINE__, i, left_edge[i],
+                          i, right_edge[i]);
 
                /*
                 * Check for cases where we haven't found the left edge,
                 * which makes our assignment of the the right edge invalid.
                 * Reset it to the illegal value.
                 */
-               if ((left_edge[i] == IO_IO_IN_DELAY_MAX + 1) && (
-                       right_edge[i] != IO_IO_IN_DELAY_MAX + 1)) {
-                       right_edge[i] = IO_IO_IN_DELAY_MAX + 1;
-                       debug_cond(DLEVEL == 2, "%s:%d vfifo_center: reset \
-                                  right_edge[%u]: %d\n", __func__, __LINE__,
-                                  i, right_edge[i]);
+               if ((left_edge[i] == delay_max + 1) &&
+                   (right_edge[i] != delay_max + 1)) {
+                       right_edge[i] = delay_max + 1;
+                       debug_cond(DLEVEL == 2,
+                                  "%s:%d vfifo_center: reset right_edge[%u]: %d\n",
+                                  __func__, __LINE__, i, right_edge[i]);
                }
 
                /*
-                * Reset sticky bit (except for bits where we have seen
-                * both the left and right edge).
+                * Reset sticky bit
+                * READ: except for bits where we have seen both
+                *       the left and right edge.
+                * WRITE: except for bits where we have seen the
+                *        left edge.
                 */
-               sticky_bit_chk = sticky_bit_chk << 1;
-               if ((left_edge[i] != IO_IO_IN_DELAY_MAX + 1) &&
-                   (right_edge[i] != IO_IO_IN_DELAY_MAX + 1)) {
-                       sticky_bit_chk = sticky_bit_chk | 1;
+               *sticky_bit_chk <<= 1;
+               if (write) {
+                       if (left_edge[i] != delay_max + 1)
+                               *sticky_bit_chk |= 1;
+               } else {
+                       if ((left_edge[i] != delay_max + 1) &&
+                           (right_edge[i] != delay_max + 1))
+                               *sticky_bit_chk |= 1;
                }
-
-               if (i == 0)
-                       break;
        }
 
-       /* Search for the right edge of the window for each bit */
-       for (d = 0; d <= IO_DQS_IN_DELAY_MAX - start_dqs; d++) {
-               scc_mgr_set_dqs_bus_in_delay(read_group, d + start_dqs);
-               if (IO_SHIFT_DQS_EN_WHEN_SHIFT_DQS) {
-                       uint32_t delay = d + start_dqs_en;
-                       if (delay > IO_DQS_EN_DELAY_MAX)
-                               delay = IO_DQS_EN_DELAY_MAX;
-                       scc_mgr_set_dqs_en_delay(read_group, delay);
-               }
-               scc_mgr_load_dqs(read_group);
 
-               writel(0, &sdr_scc_mgr->update);
+}
 
-               /*
-                * Stop searching when the read test doesn't pass AND when
-                * we've seen a passing read on every bit.
-                */
-               if (use_read_test) {
-                       stop = !rw_mgr_mem_calibrate_read_test(rank_bgn,
-                               read_group, NUM_READ_PB_TESTS, PASS_ONE_BIT,
-                               &bit_chk, 0, 0);
-               } else {
-                       rw_mgr_mem_calibrate_write_test(rank_bgn, write_group,
-                                                       0, PASS_ONE_BIT,
-                                                       &bit_chk, 0);
-                       bit_chk = bit_chk >> (RW_MGR_MEM_DQ_PER_READ_DQS *
-                               (read_group - (write_group *
-                                       RW_MGR_MEM_IF_READ_DQS_WIDTH /
-                                       RW_MGR_MEM_IF_WRITE_DQS_WIDTH)));
-                       stop = (bit_chk == 0);
+/**
+ * search_right_edge() - Find right edge of DQ/DQS working phase
+ * @write:             Perform read (Stage 2) or write (Stage 3) calibration
+ * @rank_bgn:          Rank number
+ * @write_group:       Write Group
+ * @read_group:                Read Group
+ * @start_dqs:         DQS start phase
+ * @start_dqs_en:      DQS enable start phase
+ * @sticky_bit_chk:    Resulting sticky bit mask after the test
+ * @left_edge:         Left edge of the DQ/DQS phase
+ * @right_edge:                Right edge of the DQ/DQS phase
+ * @use_read_test:     Perform read test
+ *
+ * Find right edge of DQ/DQS working phase.
+ */
+static int search_right_edge(const int write, const int rank_bgn,
+       const u32 write_group, const u32 read_group,
+       const int start_dqs, const int start_dqs_en,
+       u32 *sticky_bit_chk,
+       int *left_edge, int *right_edge, const u32 use_read_test)
+{
+       const u32 delay_max = write ? IO_IO_OUT1_DELAY_MAX : IO_IO_IN_DELAY_MAX;
+       const u32 dqs_max = write ? IO_IO_OUT1_DELAY_MAX : IO_DQS_IN_DELAY_MAX;
+       const u32 per_dqs = write ? RW_MGR_MEM_DQ_PER_WRITE_DQS :
+                                   RW_MGR_MEM_DQ_PER_READ_DQS;
+       u32 stop, bit_chk;
+       int i, d;
+
+       for (d = 0; d <= dqs_max - start_dqs; d++) {
+               if (write) {    /* WRITE-ONLY */
+                       scc_mgr_apply_group_dqs_io_and_oct_out1(write_group,
+                                                               d + start_dqs);
+               } else {        /* READ-ONLY */
+                       scc_mgr_set_dqs_bus_in_delay(read_group, d + start_dqs);
+                       if (IO_SHIFT_DQS_EN_WHEN_SHIFT_DQS) {
+                               uint32_t delay = d + start_dqs_en;
+                               if (delay > IO_DQS_EN_DELAY_MAX)
+                                       delay = IO_DQS_EN_DELAY_MAX;
+                               scc_mgr_set_dqs_en_delay(read_group, delay);
+                       }
+                       scc_mgr_load_dqs(read_group);
                }
-               sticky_bit_chk = sticky_bit_chk | bit_chk;
-               stop = stop && (sticky_bit_chk == param->read_correct_mask);
 
-               debug_cond(DLEVEL == 2, "%s:%d vfifo_center(right): dtap=%u => %u == \
-                          %u && %u", __func__, __LINE__, d,
-                          sticky_bit_chk, param->read_correct_mask, stop);
+               writel(0, &sdr_scc_mgr->update);
 
+               stop = search_stop_check(write, d, rank_bgn, write_group,
+                                        read_group, &bit_chk, sticky_bit_chk,
+                                        use_read_test);
                if (stop == 1) {
+                       if (write && (d == 0)) {        /* WRITE-ONLY */
+                               for (i = 0; i < RW_MGR_MEM_DQ_PER_WRITE_DQS; i++) {
+                                       /*
+                                        * d = 0 failed, but it passed when
+                                        * testing the left edge, so it must be
+                                        * marginal, set it to -1
+                                        */
+                                       if (right_edge[i] == delay_max + 1 &&
+                                           left_edge[i] != delay_max + 1)
+                                               right_edge[i] = -1;
+                               }
+                       }
                        break;
-               } else {
-                       for (i = 0; i < RW_MGR_MEM_DQ_PER_READ_DQS; i++) {
-                               if (bit_chk & 1) {
-                                       /* Remember a passing test as
-                                       the right_edge */
-                                       right_edge[i] = d;
+               }
+
+               /* stop != 1 */
+               for (i = 0; i < per_dqs; i++) {
+                       if (bit_chk & 1) {
+                               /*
+                                * Remember a passing test as
+                                * the right_edge.
+                                */
+                               right_edge[i] = d;
+                       } else {
+                               if (d != 0) {
+                                       /*
+                                        * If a right edge has not
+                                        * been seen yet, then a future
+                                        * passing test will mark this
+                                        * edge as the left edge.
+                                        */
+                                       if (right_edge[i] == delay_max + 1)
+                                               left_edge[i] = -(d + 1);
                                } else {
-                                       if (d != 0) {
-                                               /* If a right edge has not been
-                                               seen yet, then a future passing
-                                               test will mark this edge as the
-                                               left edge */
-                                               if (right_edge[i] ==
-                                               IO_IO_IN_DELAY_MAX + 1) {
-                                                       left_edge[i] = -(d + 1);
-                                               }
-                                       } else {
-                                               /* d = 0 failed, but it passed
-                                               when testing the left edge,
-                                               so it must be marginal,
-                                               set it to -1 */
-                                               if (right_edge[i] ==
-                                                       IO_IO_IN_DELAY_MAX + 1 &&
-                                                       left_edge[i] !=
-                                                       IO_IO_IN_DELAY_MAX
-                                                       + 1) {
-                                                       right_edge[i] = -1;
-                                               }
-                                               /* If a right edge has not been
-                                               seen yet, then a future passing
-                                               test will mark this edge as the
-                                               left edge */
-                                               else if (right_edge[i] ==
-                                                       IO_IO_IN_DELAY_MAX +
-                                                       1) {
-                                                       left_edge[i] = -(d + 1);
-                                               }
-                                       }
+                                       /*
+                                        * d = 0 failed, but it passed
+                                        * when testing the left edge,
+                                        * so it must be marginal, set
+                                        * it to -1
+                                        */
+                                       if (right_edge[i] == delay_max + 1 &&
+                                           left_edge[i] != delay_max + 1)
+                                               right_edge[i] = -1;
+                                       /*
+                                        * If a right edge has not been
+                                        * seen yet, then a future
+                                        * passing test will mark this
+                                        * edge as the left edge.
+                                        */
+                                       else if (right_edge[i] == delay_max + 1)
+                                               left_edge[i] = -(d + 1);
                                }
-
-                               debug_cond(DLEVEL == 2, "%s:%d vfifo_center[r,\
-                                          d=%u]: ", __func__, __LINE__, d);
-                               debug_cond(DLEVEL == 2, "bit_chk_test=%d left_edge[%u]: %d ",
-                                          (int)(bit_chk & 1), i, left_edge[i]);
-                               debug_cond(DLEVEL == 2, "right_edge[%u]: %d\n", i,
-                                          right_edge[i]);
-                               bit_chk = bit_chk >> 1;
                        }
+
+                       debug_cond(DLEVEL == 2, "%s:%d center[r,d=%u]: ",
+                                  __func__, __LINE__, d);
+                       debug_cond(DLEVEL == 2,
+                                  "bit_chk_test=%i left_edge[%u]: %d ",
+                                  bit_chk & 1, i, left_edge[i]);
+                       debug_cond(DLEVEL == 2, "right_edge[%u]: %d\n", i,
+                                  right_edge[i]);
+                       bit_chk >>= 1;
                }
        }
 
        /* Check that all bits have a window */
-       for (i = 0; i < RW_MGR_MEM_DQ_PER_READ_DQS; i++) {
-               debug_cond(DLEVEL == 2, "%s:%d vfifo_center: left_edge[%u]: \
-                          %d right_edge[%u]: %d", __func__, __LINE__,
-                          i, left_edge[i], i, right_edge[i]);
-               if ((left_edge[i] == IO_IO_IN_DELAY_MAX + 1) || (right_edge[i]
-                       == IO_IO_IN_DELAY_MAX + 1)) {
-                       /*
-                        * Restore delay chain settings before letting the loop
-                        * in rw_mgr_mem_calibrate_vfifo to retry different
-                        * dqs/ck relationships.
-                        */
-                       scc_mgr_set_dqs_bus_in_delay(read_group, start_dqs);
-                       if (IO_SHIFT_DQS_EN_WHEN_SHIFT_DQS) {
-                               scc_mgr_set_dqs_en_delay(read_group,
-                                                        start_dqs_en);
-                       }
-                       scc_mgr_load_dqs(read_group);
-                       writel(0, &sdr_scc_mgr->update);
-
-                       debug_cond(DLEVEL == 1, "%s:%d vfifo_center: failed to \
-                                  find edge [%u]: %d %d", __func__, __LINE__,
-                                  i, left_edge[i], right_edge[i]);
-                       if (use_read_test) {
-                               set_failing_group_stage(read_group *
-                                       RW_MGR_MEM_DQ_PER_READ_DQS + i,
-                                       CAL_STAGE_VFIFO,
-                                       CAL_SUBSTAGE_VFIFO_CENTER);
-                       } else {
-                               set_failing_group_stage(read_group *
-                                       RW_MGR_MEM_DQ_PER_READ_DQS + i,
-                                       CAL_STAGE_VFIFO_AFTER_WRITES,
-                                       CAL_SUBSTAGE_VFIFO_CENTER);
-                       }
-                       return 0;
-               }
+       for (i = 0; i < per_dqs; i++) {
+               debug_cond(DLEVEL == 2,
+                          "%s:%d write_center: left_edge[%u]: %d right_edge[%u]: %d",
+                          __func__, __LINE__, i, left_edge[i],
+                          i, right_edge[i]);
+               if ((left_edge[i] == dqs_max + 1) ||
+                   (right_edge[i] == dqs_max + 1))
+                       return i + 1;   /* FIXME: If we fail, retval > 0 */
        }
 
+       return 0;
+}
+
+/**
+ * get_window_mid_index() - Find the best middle setting of DQ/DQS phase
+ * @write:             Perform read (Stage 2) or write (Stage 3) calibration
+ * @left_edge:         Left edge of the DQ/DQS phase
+ * @right_edge:                Right edge of the DQ/DQS phase
+ * @mid_min:           Best DQ/DQS phase middle setting
+ *
+ * Find index and value of the middle of the DQ/DQS working phase.
+ */
+static int get_window_mid_index(const int write, int *left_edge,
+                               int *right_edge, int *mid_min)
+{
+       const u32 per_dqs = write ? RW_MGR_MEM_DQ_PER_WRITE_DQS :
+                                   RW_MGR_MEM_DQ_PER_READ_DQS;
+       int i, mid, min_index;
+
        /* Find middle of window for each DQ bit */
-       mid_min = left_edge[0] - right_edge[0];
+       *mid_min = left_edge[0] - right_edge[0];
        min_index = 0;
-       for (i = 1; i < RW_MGR_MEM_DQ_PER_READ_DQS; i++) {
+       for (i = 1; i < per_dqs; i++) {
                mid = left_edge[i] - right_edge[i];
-               if (mid < mid_min) {
-                       mid_min = mid;
+               if (mid < *mid_min) {
+                       *mid_min = mid;
                        min_index = i;
                }
        }
 
        /*
         * -mid_min/2 represents the amount that we need to move DQS.
-        * If mid_min is odd and positive we'll need to add one to
-        * make sure the rounding in further calculations is correct
-        * (always bias to the right), so just add 1 for all positive values.
+        * If mid_min is odd and positive we'll need to add one to make
+        * sure the rounding in further calculations is correct (always
+        * bias to the right), so just add 1 for all positive values.
         */
-       if (mid_min > 0)
-               mid_min++;
+       if (*mid_min > 0)
+               (*mid_min)++;
+       *mid_min = *mid_min / 2;
+
+       debug_cond(DLEVEL == 1, "%s:%d vfifo_center: *mid_min=%d (index=%u)\n",
+                  __func__, __LINE__, *mid_min, min_index);
+       return min_index;
+}
+
+/**
+ * center_dq_windows() - Center the DQ/DQS windows
+ * @write:             Perform read (Stage 2) or write (Stage 3) calibration
+ * @left_edge:         Left edge of the DQ/DQS phase
+ * @right_edge:                Right edge of the DQ/DQS phase
+ * @mid_min:           Adjusted DQ/DQS phase middle setting
+ * @orig_mid_min:      Original DQ/DQS phase middle setting
+ * @min_index:         DQ/DQS phase middle setting index
+ * @test_bgn:          Rank number to begin the test
+ * @dq_margin:         Amount of shift for the DQ
+ * @dqs_margin:                Amount of shift for the DQS
+ *
+ * Align the DQ/DQS windows in each group.
+ */
+static void center_dq_windows(const int write, int *left_edge, int *right_edge,
+                             const int mid_min, const int orig_mid_min,
+                             const int min_index, const int test_bgn,
+                             int *dq_margin, int *dqs_margin)
+{
+       const u32 delay_max = write ? IO_IO_OUT1_DELAY_MAX : IO_IO_IN_DELAY_MAX;
+       const u32 per_dqs = write ? RW_MGR_MEM_DQ_PER_WRITE_DQS :
+                                   RW_MGR_MEM_DQ_PER_READ_DQS;
+       const u32 delay_off = write ? SCC_MGR_IO_OUT1_DELAY_OFFSET :
+                                     SCC_MGR_IO_IN_DELAY_OFFSET;
+       const u32 addr = SDR_PHYGRP_SCCGRP_ADDRESS | delay_off;
 
-       mid_min = mid_min / 2;
+       u32 temp_dq_io_delay1, temp_dq_io_delay2;
+       int shift_dq, i, p;
 
-       debug_cond(DLEVEL == 1, "%s:%d vfifo_center: mid_min=%d (index=%u)\n",
-                  __func__, __LINE__, mid_min, min_index);
+       /* Initialize data for export structures */
+       *dqs_margin = delay_max + 1;
+       *dq_margin  = delay_max + 1;
+
+       /* add delay to bring centre of all DQ windows to the same "level" */
+       for (i = 0, p = test_bgn; i < per_dqs; i++, p++) {
+               /* Use values before divide by 2 to reduce round off error */
+               shift_dq = (left_edge[i] - right_edge[i] -
+                       (left_edge[min_index] - right_edge[min_index]))/2  +
+                       (orig_mid_min - mid_min);
+
+               debug_cond(DLEVEL == 2,
+                          "vfifo_center: before: shift_dq[%u]=%d\n",
+                          i, shift_dq);
+
+               temp_dq_io_delay1 = readl(addr + (p << 2));
+               temp_dq_io_delay2 = readl(addr + (i << 2));
+
+               if (shift_dq + temp_dq_io_delay1 > delay_max)
+                       shift_dq = delay_max - temp_dq_io_delay2;
+               else if (shift_dq + temp_dq_io_delay1 < 0)
+                       shift_dq = -temp_dq_io_delay1;
+
+               debug_cond(DLEVEL == 2,
+                          "vfifo_center: after: shift_dq[%u]=%d\n",
+                          i, shift_dq);
+
+               if (write)
+                       scc_mgr_set_dq_out1_delay(i, temp_dq_io_delay1 + shift_dq);
+               else
+                       scc_mgr_set_dq_in_delay(p, temp_dq_io_delay1 + shift_dq);
+
+               scc_mgr_load_dq(p);
+
+               debug_cond(DLEVEL == 2,
+                          "vfifo_center: margin[%u]=[%d,%d]\n", i,
+                          left_edge[i] - shift_dq + (-mid_min),
+                          right_edge[i] + shift_dq - (-mid_min));
+
+               /* To determine values for export structures */
+               if (left_edge[i] - shift_dq + (-mid_min) < *dq_margin)
+                       *dq_margin = left_edge[i] - shift_dq + (-mid_min);
+
+               if (right_edge[i] + shift_dq - (-mid_min) < *dqs_margin)
+                       *dqs_margin = right_edge[i] + shift_dq - (-mid_min);
+       }
+
+}
+
+/* per-bit deskew DQ and center */
+static int rw_mgr_mem_calibrate_vfifo_center(const u32 rank_bgn,
+                       const u32 rw_group, const u32 test_bgn,
+                       const int use_read_test, const int update_fom)
+{
+       const u32 addr =
+               SDR_PHYGRP_SCCGRP_ADDRESS + SCC_MGR_DQS_IN_DELAY_OFFSET +
+               (rw_group << 2);
+       /*
+        * Store these as signed since there are comparisons with
+        * signed numbers.
+        */
+       uint32_t sticky_bit_chk;
+       int32_t left_edge[RW_MGR_MEM_DQ_PER_READ_DQS];
+       int32_t right_edge[RW_MGR_MEM_DQ_PER_READ_DQS];
+       int32_t orig_mid_min, mid_min;
+       int32_t new_dqs, start_dqs, start_dqs_en, final_dqs_en;
+       int32_t dq_margin, dqs_margin;
+       int i, min_index;
+       int ret;
+
+       debug("%s:%d: %u %u", __func__, __LINE__, rw_group, test_bgn);
+
+       start_dqs = readl(addr);
+       if (IO_SHIFT_DQS_EN_WHEN_SHIFT_DQS)
+               start_dqs_en = readl(addr - IO_DQS_EN_DELAY_OFFSET);
+
+       /* set the left and right edge of each bit to an illegal value */
+       /* use (IO_IO_IN_DELAY_MAX + 1) as an illegal value */
+       sticky_bit_chk = 0;
+       for (i = 0; i < RW_MGR_MEM_DQ_PER_READ_DQS; i++) {
+               left_edge[i]  = IO_IO_IN_DELAY_MAX + 1;
+               right_edge[i] = IO_IO_IN_DELAY_MAX + 1;
+       }
+
+       /* Search for the left edge of the window for each bit */
+       search_left_edge(0, rank_bgn, rw_group, rw_group, test_bgn,
+                        &sticky_bit_chk,
+                        left_edge, right_edge, use_read_test);
+
+
+       /* Search for the right edge of the window for each bit */
+       ret = search_right_edge(0, rank_bgn, rw_group, rw_group,
+                               start_dqs, start_dqs_en,
+                               &sticky_bit_chk,
+                               left_edge, right_edge, use_read_test);
+       if (ret) {
+               /*
+                * Restore delay chain settings before letting the loop
+                * in rw_mgr_mem_calibrate_vfifo to retry different
+                * dqs/ck relationships.
+                */
+               scc_mgr_set_dqs_bus_in_delay(rw_group, start_dqs);
+               if (IO_SHIFT_DQS_EN_WHEN_SHIFT_DQS)
+                       scc_mgr_set_dqs_en_delay(rw_group, start_dqs_en);
+
+               scc_mgr_load_dqs(rw_group);
+               writel(0, &sdr_scc_mgr->update);
+
+               debug_cond(DLEVEL == 1,
+                          "%s:%d vfifo_center: failed to find edge [%u]: %d %d",
+                          __func__, __LINE__, i, left_edge[i], right_edge[i]);
+               if (use_read_test) {
+                       set_failing_group_stage(rw_group *
+                               RW_MGR_MEM_DQ_PER_READ_DQS + i,
+                               CAL_STAGE_VFIFO,
+                               CAL_SUBSTAGE_VFIFO_CENTER);
+               } else {
+                       set_failing_group_stage(rw_group *
+                               RW_MGR_MEM_DQ_PER_READ_DQS + i,
+                               CAL_STAGE_VFIFO_AFTER_WRITES,
+                               CAL_SUBSTAGE_VFIFO_CENTER);
+               }
+               return -EIO;
+       }
+
+       min_index = get_window_mid_index(0, left_edge, right_edge, &mid_min);
 
        /* Determine the amount we can change DQS (which is -mid_min) */
        orig_mid_min = mid_min;
@@ -2072,68 +2289,29 @@ static uint32_t rw_mgr_mem_calibrate_vfifo_center(uint32_t rank_bgn,
        }
        new_dqs = start_dqs - mid_min;
 
-       debug_cond(DLEVEL == 1, "vfifo_center: start_dqs=%d start_dqs_en=%d \
-                  new_dqs=%d mid_min=%d\n", start_dqs,
+       debug_cond(DLEVEL == 1,
+                  "vfifo_center: start_dqs=%d start_dqs_en=%d new_dqs=%d mid_min=%d\n",
+                  start_dqs,
                   IO_SHIFT_DQS_EN_WHEN_SHIFT_DQS ? start_dqs_en : -1,
                   new_dqs, mid_min);
 
-       /* Initialize data for export structures */
-       dqs_margin = IO_IO_IN_DELAY_MAX + 1;
-       dq_margin  = IO_IO_IN_DELAY_MAX + 1;
-
-       /* add delay to bring centre of all DQ windows to the same "level" */
-       for (i = 0, p = test_bgn; i < RW_MGR_MEM_DQ_PER_READ_DQS; i++, p++) {
-               /* Use values before divide by 2 to reduce round off error */
-               shift_dq = (left_edge[i] - right_edge[i] -
-                       (left_edge[min_index] - right_edge[min_index]))/2  +
-                       (orig_mid_min - mid_min);
-
-               debug_cond(DLEVEL == 2, "vfifo_center: before: \
-                          shift_dq[%u]=%d\n", i, shift_dq);
-
-               addr = SDR_PHYGRP_SCCGRP_ADDRESS | SCC_MGR_IO_IN_DELAY_OFFSET;
-               temp_dq_in_delay1 = readl(addr + (p << 2));
-               temp_dq_in_delay2 = readl(addr + (i << 2));
-
-               if (shift_dq + (int32_t)temp_dq_in_delay1 >
-                       (int32_t)IO_IO_IN_DELAY_MAX) {
-                       shift_dq = (int32_t)IO_IO_IN_DELAY_MAX - temp_dq_in_delay2;
-               } else if (shift_dq + (int32_t)temp_dq_in_delay1 < 0) {
-                       shift_dq = -(int32_t)temp_dq_in_delay1;
-               }
-               debug_cond(DLEVEL == 2, "vfifo_center: after: \
-                          shift_dq[%u]=%d\n", i, shift_dq);
-               final_dq[i] = temp_dq_in_delay1 + shift_dq;
-               scc_mgr_set_dq_in_delay(p, final_dq[i]);
-               scc_mgr_load_dq(p);
-
-               debug_cond(DLEVEL == 2, "vfifo_center: margin[%u]=[%d,%d]\n", i,
-                          left_edge[i] - shift_dq + (-mid_min),
-                          right_edge[i] + shift_dq - (-mid_min));
-               /* To determine values for export structures */
-               if (left_edge[i] - shift_dq + (-mid_min) < dq_margin)
-                       dq_margin = left_edge[i] - shift_dq + (-mid_min);
-
-               if (right_edge[i] + shift_dq - (-mid_min) < dqs_margin)
-                       dqs_margin = right_edge[i] + shift_dq - (-mid_min);
-       }
-
-       final_dqs = new_dqs;
-       if (IO_SHIFT_DQS_EN_WHEN_SHIFT_DQS)
-               final_dqs_en = start_dqs_en - mid_min;
+       /* Add delay to bring centre of all DQ windows to the same "level". */
+       center_dq_windows(0, left_edge, right_edge, mid_min, orig_mid_min,
+                         min_index, test_bgn, &dq_margin, &dqs_margin);
 
        /* Move DQS-en */
        if (IO_SHIFT_DQS_EN_WHEN_SHIFT_DQS) {
-               scc_mgr_set_dqs_en_delay(read_group, final_dqs_en);
-               scc_mgr_load_dqs(read_group);
+               final_dqs_en = start_dqs_en - mid_min;
+               scc_mgr_set_dqs_en_delay(rw_group, final_dqs_en);
+               scc_mgr_load_dqs(rw_group);
        }
 
        /* Move DQS */
-       scc_mgr_set_dqs_bus_in_delay(read_group, final_dqs);
-       scc_mgr_load_dqs(read_group);
-       debug_cond(DLEVEL == 2, "%s:%d vfifo_center: dq_margin=%d \
-                  dqs_margin=%d", __func__, __LINE__,
-                  dq_margin, dqs_margin);
+       scc_mgr_set_dqs_bus_in_delay(rw_group, new_dqs);
+       scc_mgr_load_dqs(rw_group);
+       debug_cond(DLEVEL == 2,
+                  "%s:%d vfifo_center: dq_margin=%d dqs_margin=%d",
+                  __func__, __LINE__, dq_margin, dqs_margin);
 
        /*
         * Do not remove this line as it makes sure all of our decisions
@@ -2141,7 +2319,10 @@ static uint32_t rw_mgr_mem_calibrate_vfifo_center(uint32_t rank_bgn,
         */
        writel(0, &sdr_scc_mgr->update);
 
-       return (dq_margin >= 0) && (dqs_margin >= 0);
+       if ((dq_margin < 0) || (dqs_margin < 0))
+               return -EINVAL;
+
+       return 0;
 }
 
 /**
@@ -2205,7 +2386,7 @@ static int rw_mgr_mem_calibrate_dqs_enable_calibration(const u32 rw_group,
        /* We start at zero, so have one less dq to devide among */
        const u32 delay_step = IO_IO_IN_DELAY_MAX /
                               (RW_MGR_MEM_DQ_PER_READ_DQS - 1);
-       int found;
+       int ret;
        u32 i, p, d, r;
 
        debug("%s:%d (%u,%u)\n", __func__, __LINE__, rw_group, test_bgn);
@@ -2231,11 +2412,11 @@ static int rw_mgr_mem_calibrate_dqs_enable_calibration(const u32 rw_group,
         * Try rw_mgr_mem_calibrate_vfifo_find_dqs_en_phase across different
         * dq_in_delay values
         */
-       found = rw_mgr_mem_calibrate_vfifo_find_dqs_en_phase(rw_group);
+       ret = rw_mgr_mem_calibrate_vfifo_find_dqs_en_phase(rw_group);
 
        debug_cond(DLEVEL == 1,
                   "%s:%d: g=%u found=%u; Reseting delay chain to zero\n",
-                  __func__, __LINE__, rw_group, found);
+                  __func__, __LINE__, rw_group, !ret);
 
        for (r = 0; r < RW_MGR_MEM_NUMBER_OF_RANKS;
             r += NUM_RANKS_PER_SHADOW_REG) {
@@ -2243,11 +2424,7 @@ static int rw_mgr_mem_calibrate_dqs_enable_calibration(const u32 rw_group,
                writel(0, &sdr_scc_mgr->update);
        }
 
-       if (!found)
-               return -EINVAL;
-
-       return 0;
-
+       return ret;
 }
 
 /**
@@ -2282,10 +2459,10 @@ rw_mgr_mem_calibrate_dq_dqs_centering(const u32 rw_group, const u32 test_bgn,
                        continue;
 
                ret = rw_mgr_mem_calibrate_vfifo_center(rank_bgn, rw_group,
-                                                       rw_group, test_bgn,
+                                                       test_bgn,
                                                        use_read_test,
                                                        update_fom);
-               if (ret)
+               if (!ret)
                        continue;
 
                grp_calibrated = 0;
@@ -2420,8 +2597,7 @@ static uint32_t rw_mgr_mem_calibrate_vfifo_end(uint32_t read_group,
                /* Determine if this set of ranks should be skipped entirely */
                if (!param->skip_shadow_regs[sr]) {
                /* This is the last calibration round, update FOM here */
-                       if (!rw_mgr_mem_calibrate_vfifo_center(rank_bgn,
-                                                               write_group,
+                       if (rw_mgr_mem_calibrate_vfifo_center(rank_bgn,
                                                                read_group,
                                                                test_bgn, 0,
                                                                1)) {
@@ -2445,7 +2621,6 @@ static uint32_t rw_mgr_mem_calibrate_vfifo_end(uint32_t read_group,
 static uint32_t rw_mgr_mem_calibrate_lfifo(void)
 {
        uint32_t found_one;
-       uint32_t bit_chk;
 
        debug("%s:%d\n", __func__, __LINE__);
 
@@ -2465,7 +2640,7 @@ static uint32_t rw_mgr_mem_calibrate_lfifo(void)
                if (!rw_mgr_mem_calibrate_read_test_all_ranks(0,
                                                              NUM_READ_TESTS,
                                                              PASS_ALL_BITS,
-                                                             &bit_chk, 1)) {
+                                                             1)) {
                        break;
                }
 
@@ -2706,7 +2881,7 @@ static uint32_t rw_mgr_mem_calibrate_write_test(uint32_t rank_bgn,
 static uint32_t rw_mgr_mem_calibrate_writes_center(uint32_t rank_bgn,
        uint32_t write_group, uint32_t test_bgn)
 {
-       uint32_t i, p, min_index;
+       uint32_t i, min_index;
        int32_t d;
        /*
         * Store these as signed since there are comparisons with
@@ -2718,12 +2893,12 @@ static uint32_t rw_mgr_mem_calibrate_writes_center(uint32_t rank_bgn,
        int32_t right_edge[RW_MGR_MEM_DQ_PER_WRITE_DQS];
        int32_t mid;
        int32_t mid_min, orig_mid_min;
-       int32_t new_dqs, start_dqs, shift_dq;
+       int32_t new_dqs, start_dqs;
        int32_t dq_margin, dqs_margin, dm_margin;
-       uint32_t stop;
-       uint32_t temp_dq_out1_delay;
        uint32_t addr;
 
+       int ret;
+
        debug("%s:%d %u %u", __func__, __LINE__, write_group, test_bgn);
 
        dm_margin = 0;
@@ -2745,214 +2920,22 @@ static uint32_t rw_mgr_mem_calibrate_writes_center(uint32_t rank_bgn,
        }
 
        /* Search for the left edge of the window for each bit */
-       for (d = 0; d <= IO_IO_OUT1_DELAY_MAX; d++) {
-               scc_mgr_apply_group_dq_out1_delay(write_group, d);
-
-               writel(0, &sdr_scc_mgr->update);
-
-               /*
-                * Stop searching when the read test doesn't pass AND when
-                * we've seen a passing read on every bit.
-                */
-               stop = !rw_mgr_mem_calibrate_write_test(rank_bgn, write_group,
-                       0, PASS_ONE_BIT, &bit_chk, 0);
-               sticky_bit_chk = sticky_bit_chk | bit_chk;
-               stop = stop && (sticky_bit_chk == param->write_correct_mask);
-               debug_cond(DLEVEL == 2, "write_center(left): dtap=%d => %u \
-                          == %u && %u [bit_chk= %u ]\n",
-                       d, sticky_bit_chk, param->write_correct_mask,
-                       stop, bit_chk);
-
-               if (stop == 1) {
-                       break;
-               } else {
-                       for (i = 0; i < RW_MGR_MEM_DQ_PER_WRITE_DQS; i++) {
-                               if (bit_chk & 1) {
-                                       /*
-                                        * Remember a passing test as the
-                                        * left_edge.
-                                        */
-                                       left_edge[i] = d;
-                               } else {
-                                       /*
-                                        * If a left edge has not been seen
-                                        * yet, then a future passing test will
-                                        * mark this edge as the right edge.
-                                        */
-                                       if (left_edge[i] ==
-                                               IO_IO_OUT1_DELAY_MAX + 1) {
-                                               right_edge[i] = -(d + 1);
-                                       }
-                               }
-                               debug_cond(DLEVEL == 2, "write_center[l,d=%d):", d);
-                               debug_cond(DLEVEL == 2, "bit_chk_test=%d left_edge[%u]: %d",
-                                          (int)(bit_chk & 1), i, left_edge[i]);
-                               debug_cond(DLEVEL == 2, "right_edge[%u]: %d\n", i,
-                                      right_edge[i]);
-                               bit_chk = bit_chk >> 1;
-                       }
-               }
-       }
-
-       /* Reset DQ delay chains to 0 */
-       scc_mgr_apply_group_dq_out1_delay(0);
-       sticky_bit_chk = 0;
-       for (i = RW_MGR_MEM_DQ_PER_WRITE_DQS - 1;; i--) {
-               debug_cond(DLEVEL == 2, "%s:%d write_center: left_edge[%u]: \
-                          %d right_edge[%u]: %d\n", __func__, __LINE__,
-                          i, left_edge[i], i, right_edge[i]);
-
-               /*
-                * Check for cases where we haven't found the left edge,
-                * which makes our assignment of the the right edge invalid.
-                * Reset it to the illegal value.
-                */
-               if ((left_edge[i] == IO_IO_OUT1_DELAY_MAX + 1) &&
-                   (right_edge[i] != IO_IO_OUT1_DELAY_MAX + 1)) {
-                       right_edge[i] = IO_IO_OUT1_DELAY_MAX + 1;
-                       debug_cond(DLEVEL == 2, "%s:%d write_center: reset \
-                                  right_edge[%u]: %d\n", __func__, __LINE__,
-                                  i, right_edge[i]);
-               }
-
-               /*
-                * Reset sticky bit (except for bits where we have
-                * seen the left edge).
-                */
-               sticky_bit_chk = sticky_bit_chk << 1;
-               if ((left_edge[i] != IO_IO_OUT1_DELAY_MAX + 1))
-                       sticky_bit_chk = sticky_bit_chk | 1;
-
-               if (i == 0)
-                       break;
-       }
+       search_left_edge(1, rank_bgn, write_group, 0, test_bgn,
+                        &sticky_bit_chk,
+                        left_edge, right_edge, 0);
 
        /* Search for the right edge of the window for each bit */
-       for (d = 0; d <= IO_IO_OUT1_DELAY_MAX - start_dqs; d++) {
-               scc_mgr_apply_group_dqs_io_and_oct_out1(write_group,
-                                                       d + start_dqs);
-
-               writel(0, &sdr_scc_mgr->update);
-
-               /*
-                * Stop searching when the read test doesn't pass AND when
-                * we've seen a passing read on every bit.
-                */
-               stop = !rw_mgr_mem_calibrate_write_test(rank_bgn, write_group,
-                       0, PASS_ONE_BIT, &bit_chk, 0);
-
-               sticky_bit_chk = sticky_bit_chk | bit_chk;
-               stop = stop && (sticky_bit_chk == param->write_correct_mask);
-
-               debug_cond(DLEVEL == 2, "write_center (right): dtap=%u => %u == \
-                          %u && %u\n", d, sticky_bit_chk,
-                          param->write_correct_mask, stop);
-
-               if (stop == 1) {
-                       if (d == 0) {
-                               for (i = 0; i < RW_MGR_MEM_DQ_PER_WRITE_DQS;
-                                       i++) {
-                                       /* d = 0 failed, but it passed when
-                                       testing the left edge, so it must be
-                                       marginal, set it to -1 */
-                                       if (right_edge[i] ==
-                                               IO_IO_OUT1_DELAY_MAX + 1 &&
-                                               left_edge[i] !=
-                                               IO_IO_OUT1_DELAY_MAX + 1) {
-                                               right_edge[i] = -1;
-                                       }
-                               }
-                       }
-                       break;
-               } else {
-                       for (i = 0; i < RW_MGR_MEM_DQ_PER_WRITE_DQS; i++) {
-                               if (bit_chk & 1) {
-                                       /*
-                                        * Remember a passing test as
-                                        * the right_edge.
-                                        */
-                                       right_edge[i] = d;
-                               } else {
-                                       if (d != 0) {
-                                               /*
-                                                * If a right edge has not
-                                                * been seen yet, then a future
-                                                * passing test will mark this
-                                                * edge as the left edge.
-                                                */
-                                               if (right_edge[i] ==
-                                                   IO_IO_OUT1_DELAY_MAX + 1)
-                                                       left_edge[i] = -(d + 1);
-                                       } else {
-                                               /*
-                                                * d = 0 failed, but it passed
-                                                * when testing the left edge,
-                                                * so it must be marginal, set
-                                                * it to -1.
-                                                */
-                                               if (right_edge[i] ==
-                                                   IO_IO_OUT1_DELAY_MAX + 1 &&
-                                                   left_edge[i] !=
-                                                   IO_IO_OUT1_DELAY_MAX + 1)
-                                                       right_edge[i] = -1;
-                                               /*
-                                                * If a right edge has not been
-                                                * seen yet, then a future
-                                                * passing test will mark this
-                                                * edge as the left edge.
-                                                */
-                                               else if (right_edge[i] ==
-                                                       IO_IO_OUT1_DELAY_MAX +
-                                                       1)
-                                                       left_edge[i] = -(d + 1);
-                                       }
-                               }
-                               debug_cond(DLEVEL == 2, "write_center[r,d=%d):", d);
-                               debug_cond(DLEVEL == 2, "bit_chk_test=%d left_edge[%u]: %d",
-                                          (int)(bit_chk & 1), i, left_edge[i]);
-                               debug_cond(DLEVEL == 2, "right_edge[%u]: %d\n", i,
-                                          right_edge[i]);
-                               bit_chk = bit_chk >> 1;
-                       }
-               }
-       }
-
-       /* Check that all bits have a window */
-       for (i = 0; i < RW_MGR_MEM_DQ_PER_WRITE_DQS; i++) {
-               debug_cond(DLEVEL == 2, "%s:%d write_center: left_edge[%u]: \
-                          %d right_edge[%u]: %d", __func__, __LINE__,
-                          i, left_edge[i], i, right_edge[i]);
-               if ((left_edge[i] == IO_IO_OUT1_DELAY_MAX + 1) ||
-                   (right_edge[i] == IO_IO_OUT1_DELAY_MAX + 1)) {
-                       set_failing_group_stage(test_bgn + i,
-                                               CAL_STAGE_WRITES,
-                                               CAL_SUBSTAGE_WRITES_CENTER);
-                       return 0;
-               }
+       ret = search_right_edge(1, rank_bgn, write_group, 0,
+                               start_dqs, 0,
+                               &sticky_bit_chk,
+                               left_edge, right_edge, 0);
+       if (ret) {
+               set_failing_group_stage(test_bgn + ret - 1, CAL_STAGE_WRITES,
+                                       CAL_SUBSTAGE_WRITES_CENTER);
+               return 0;
        }
 
-       /* Find middle of window for each DQ bit */
-       mid_min = left_edge[0] - right_edge[0];
-       min_index = 0;
-       for (i = 1; i < RW_MGR_MEM_DQ_PER_WRITE_DQS; i++) {
-               mid = left_edge[i] - right_edge[i];
-               if (mid < mid_min) {
-                       mid_min = mid;
-                       min_index = i;
-               }
-       }
-
-       /*
-        * -mid_min/2 represents the amount that we need to move DQS.
-        * If mid_min is odd and positive we'll need to add one to
-        * make sure the rounding in further calculations is correct
-        * (always bias to the right), so just add 1 for all positive values.
-        */
-       if (mid_min > 0)
-               mid_min++;
-       mid_min = mid_min / 2;
-       debug_cond(DLEVEL == 1, "%s:%d write_center: mid_min=%d\n", __func__,
-                  __LINE__, mid_min);
+       min_index = get_window_mid_index(1, left_edge, right_edge, &mid_min);
 
        /* Determine the amount we can change DQS (which is -mid_min) */
        orig_mid_min = mid_min;
@@ -2960,43 +2943,10 @@ static uint32_t rw_mgr_mem_calibrate_writes_center(uint32_t rank_bgn,
        mid_min = 0;
        debug_cond(DLEVEL == 1, "%s:%d write_center: start_dqs=%d new_dqs=%d \
                   mid_min=%d\n", __func__, __LINE__, start_dqs, new_dqs, mid_min);
-       /* Initialize data for export structures */
-       dqs_margin = IO_IO_OUT1_DELAY_MAX + 1;
-       dq_margin  = IO_IO_OUT1_DELAY_MAX + 1;
-
-       /* add delay to bring centre of all DQ windows to the same "level" */
-       for (i = 0, p = test_bgn; i < RW_MGR_MEM_DQ_PER_WRITE_DQS; i++, p++) {
-               /* Use values before divide by 2 to reduce round off error */
-               shift_dq = (left_edge[i] - right_edge[i] -
-                       (left_edge[min_index] - right_edge[min_index]))/2  +
-               (orig_mid_min - mid_min);
-
-               debug_cond(DLEVEL == 2, "%s:%d write_center: before: shift_dq \
-                          [%u]=%d\n", __func__, __LINE__, i, shift_dq);
-
-               addr = SDR_PHYGRP_SCCGRP_ADDRESS | SCC_MGR_IO_OUT1_DELAY_OFFSET;
-               temp_dq_out1_delay = readl(addr + (i << 2));
-               if (shift_dq + (int32_t)temp_dq_out1_delay >
-                       (int32_t)IO_IO_OUT1_DELAY_MAX) {
-                       shift_dq = (int32_t)IO_IO_OUT1_DELAY_MAX - temp_dq_out1_delay;
-               } else if (shift_dq + (int32_t)temp_dq_out1_delay < 0) {
-                       shift_dq = -(int32_t)temp_dq_out1_delay;
-               }
-               debug_cond(DLEVEL == 2, "write_center: after: shift_dq[%u]=%d\n",
-                          i, shift_dq);
-               scc_mgr_set_dq_out1_delay(i, temp_dq_out1_delay + shift_dq);
-               scc_mgr_load_dq(i);
-
-               debug_cond(DLEVEL == 2, "write_center: margin[%u]=[%d,%d]\n", i,
-                          left_edge[i] - shift_dq + (-mid_min),
-                          right_edge[i] + shift_dq - (-mid_min));
-               /* To determine values for export structures */
-               if (left_edge[i] - shift_dq + (-mid_min) < dq_margin)
-                       dq_margin = left_edge[i] - shift_dq + (-mid_min);
 
-               if (right_edge[i] + shift_dq - (-mid_min) < dqs_margin)
-                       dqs_margin = right_edge[i] + shift_dq - (-mid_min);
-       }
+       /* Add delay to bring centre of all DQ windows to the same "level". */
+       center_dq_windows(1, left_edge, right_edge, mid_min, orig_mid_min,
+                         min_index, 0, &dq_margin, &dqs_margin);
 
        /* Move DQS */
        scc_mgr_apply_group_dqs_io_and_oct_out1(write_group, new_dqs);
@@ -3155,25 +3105,37 @@ static uint32_t rw_mgr_mem_calibrate_writes_center(uint32_t rank_bgn,
        return (dq_margin >= 0) && (dqs_margin >= 0) && (dm_margin >= 0);
 }
 
-/* calibrate the write operations */
-static uint32_t rw_mgr_mem_calibrate_writes(uint32_t rank_bgn, uint32_t g,
-       uint32_t test_bgn)
+/**
+ * rw_mgr_mem_calibrate_writes() - Write Calibration Part One
+ * @rank_bgn:          Rank number
+ * @group:             Read/Write Group
+ * @test_bgn:          Rank at which the test begins
+ *
+ * Stage 2: Write Calibration Part One.
+ *
+ * This function implements UniPHY calibration Stage 2, as explained in
+ * detail in Altera EMI_RM 2015.05.04 , "UniPHY Calibration Stages".
+ */
+static int rw_mgr_mem_calibrate_writes(const u32 rank_bgn, const u32 group,
+                                      const u32 test_bgn)
 {
-       /* update info for sims */
-       debug("%s:%d %u %u\n", __func__, __LINE__, g, test_bgn);
+       int ret;
 
+       /* Update info for sims */
+       debug("%s:%d %u %u\n", __func__, __LINE__, group, test_bgn);
+
+       reg_file_set_group(group);
        reg_file_set_stage(CAL_STAGE_WRITES);
        reg_file_set_sub_stage(CAL_SUBSTAGE_WRITES_CENTER);
 
-       reg_file_set_group(g);
-
-       if (!rw_mgr_mem_calibrate_writes_center(rank_bgn, g, test_bgn)) {
-               set_failing_group_stage(g, CAL_STAGE_WRITES,
+       ret = rw_mgr_mem_calibrate_writes_center(rank_bgn, group, test_bgn);
+       if (!ret) {
+               set_failing_group_stage(group, CAL_STAGE_WRITES,
                                        CAL_SUBSTAGE_WRITES_CENTER);
-               return 0;
+               return -EIO;
        }
 
-       return 1;
+       return 0;
 }
 
 /**
@@ -3467,7 +3429,7 @@ static uint32_t mem_calibrate(void)
                                        continue;
 
                                /* Calibrate WRITEs */
-                               if (rw_mgr_mem_calibrate_writes(rank_bgn,
+                               if (!rw_mgr_mem_calibrate_writes(rank_bgn,
                                                write_group, write_test_bgn))
                                        continue;