/***********************license start************************************
 * Copyright (c) 2004-2007 Cavium Networks (support@cavium.com). All rights
 * reserved.
 *
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *     * Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *     * Neither the name of Cavium Networks nor the names of
 *       its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written
 *       permission.
 *
 * TO THE MAXIMUM EXTENT PERMITTED BY LAW, THE SOFTWARE IS PROVIDED "AS IS"
 * AND WITH ALL FAULTS AND CAVIUM NETWORKS MAKES NO PROMISES, REPRESENTATIONS
 * OR WARRANTIES, EITHER EXPRESS, IMPLIED, STATUTORY, OR OTHERWISE, WITH
 * RESPECT TO THE SOFTWARE, INCLUDING ITS CONDITION, ITS CONFORMITY TO ANY
 * REPRESENTATION OR DESCRIPTION, OR THE EXISTENCE OF ANY LATENT OR PATENT
 * DEFECTS, AND CAVIUM SPECIFICALLY DISCLAIMS ALL IMPLIED (IF ANY) WARRANTIES
 * OF TITLE, MERCHANTABILITY, NONINFRINGEMENT, FITNESS FOR A PARTICULAR
 * PURPOSE, LACK OF VIRUSES, ACCURACY OR COMPLETENESS, QUIET ENJOYMENT, QUIET
 * POSSESSION OR CORRESPONDENCE TO DESCRIPTION.  THE ENTIRE RISK ARISING OUT
 * OF USE OR PERFORMANCE OF THE SOFTWARE LIES WITH YOU.
 *
 *
 * For any questions regarding licensing please contact marketing@caviumnetworks.com
 *
 ***********************license end**************************************/

#ifdef __U_BOOT__
#include "cvmx.h"
#include <common.h>
#else
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include "cvmx.h"
#endif

#define error_print  printf

/* Make a define similar to OCTEON_IS_MODEL that will work in this context.  This is runtime only.
** We can't do the normal runtime here since this code can run on the host.
** All functions using this must have a cpu_id variable set to the correct value */
#define octeon_is_cpuid(x)   (__OCTEON_IS_MODEL_COMPILE__(x, cpu_id))

//#define DEBUG 1
#ifdef DEBUG
#define DEBUG_DEFINED (1)
#define debug_print  printf
#else
#define DEBUG_DEFINED (0)
#define debug_print(...)
#endif

int initialize_dfm_clock(uint32_t cpu_id);
uint32_t measure_octeon_dfm_clock(uint32_t cpu_id, uint32_t cpu_hertz);

#ifdef __U_BOOT__
static inline int dfm_verbose(void)
{
    DECLARE_GLOBAL_DATA_PTR;
    return (!!(gd->flags & GD_FLG_DFM_VERBOSE));
}
static inline int dfm_trace_init(void)
{
    DECLARE_GLOBAL_DATA_PTR;
    return (!!(gd->flags & GD_FLG_DFM_TRACE_INIT));
}
#else
static int dfm_verbose(void)
{
    return(getenv("dfm_verbose") != NULL);
}

static int dfm_trace_init(void)
{
    return(getenv("dfm_trace_init") != NULL);
}
#endif

static void dfm_print(const char *format, ...)
{
    if (dfm_verbose() || (DEBUG_DEFINED))
    {
        va_list args;
        va_start(args, format);
        vprintf(format, args);
        va_end(args);
    }
}

static inline void dfm_config_write_csr(uint64_t csr_addr, uint64_t val)
{
    if (dfm_trace_init()) {
        dfm_print("DFM Config %016llx => %016llx\n", csr_addr, val);
        //cvmx_csr_db_decode(cvmx_get_proc_id(), csr_addr, val);
    }
    cvmx_write_csr(csr_addr, val);
}

#ifdef CVMX_BUILD_FOR_UBOOT
extern int do_reset (void *cmdtp, int flag, int argc, char *argv[]);
#endif

#undef min
#define min(X, Y)				\
	({ typeof (X) __x = (X), __y = (Y);	\
		(__x < __y) ? __x : __y; })

#undef max
#define max(X, Y)				\
	({ typeof (X) __x = (X), __y = (Y);	\
		(__x > __y) ? __x : __y; })

static int encode_row_lsb_ddr3(uint32_t cpu_id, int row_lsb)
{
    int encoded_row_lsb;
    int row_lsb_start = 14;

    /*  Decoding for row_lsb             */
    /*       000: row_lsb = mem_adr[14]  */
    /*       001: row_lsb = mem_adr[15]  */
    /*       010: row_lsb = mem_adr[16]  */
    /*       011: row_lsb = mem_adr[17]  */
    /*       100: row_lsb = mem_adr[18]  */
    /*       101: row_lsb = mem_adr[19]  */
    /*       110: row_lsb = mem_adr[20]  */
    /*       111: RESERVED               */

    if (octeon_is_cpuid(OCTEON_CN63XX))
        row_lsb_start = 11;
    else 
        printf("ERROR: Unsupported Octeon model: 0x%x\n", cpu_id);

    encoded_row_lsb      = row_lsb - row_lsb_start ;

    return encoded_row_lsb;
}

static int encode_pbank_lsb_ddr3(uint32_t cpu_id, int pbank_lsb)
{
    int encoded_pbank_lsb;

    /*  Decoding for pbank_lsb        */
    /*  PBANK_LSB                     */                           
    /*       0: rank = mem_adr[24]    */
    /*       1: rank = mem_adr[25]    */
    /*       2: rank = mem_adr[26]    */
    /*       3: rank = mem_adr[27]    */
    /*       4: rank = mem_adr[28]    */
    /*       5: rank = mem_adr[29]    */
    /*       6: rank = mem_adr[30]    */
    /*       7: rank = mem_adr[31]    */
    /*    8-15:  RESERVED             */

    int pbank_lsb_start = 0;
    if (octeon_is_cpuid(OCTEON_CN63XX))
        pbank_lsb_start = 25;
    else 
        printf("ERROR: Unsupported Octeon model: 0x%x\n", cpu_id);

    encoded_pbank_lsb      = pbank_lsb - pbank_lsb_start;

    return encoded_pbank_lsb;
}

static int divide_roundup(long dividend, long divisor)
{
    return ((dividend+divisor-1) / divisor);
}

uint64_t octeon_read_dfm_ddr3_rlevel_dbg(int idx)
{
    cvmx_dfm_rlevel_dbg_t rlevel_dbg;
    cvmx_dfm_rlevel_ctl_t rlevel_ctl;

    rlevel_ctl.u64 = cvmx_read_csr(CVMX_DFM_RLEVEL_CTL);
    rlevel_ctl.s.byte = idx;

    dfm_config_write_csr(CVMX_DFM_RLEVEL_CTL, rlevel_ctl.u64);
    cvmx_read_csr(CVMX_DFM_RLEVEL_CTL);

    rlevel_dbg.u64 = cvmx_read_csr(CVMX_DFM_RLEVEL_DBG);
    return rlevel_dbg.s.bitmask;
}

uint64_t octeon_read_dfm_ddr3_wlevel_dbg(int idx)
{
    cvmx_dfm_wlevel_dbg_t wlevel_dbg;

    wlevel_dbg.u64 = 0;
    wlevel_dbg.s.byte = idx;

    dfm_config_write_csr(CVMX_DFM_WLEVEL_DBG, wlevel_dbg.u64);
    cvmx_read_csr(CVMX_DFM_WLEVEL_DBG);

    wlevel_dbg.u64 = cvmx_read_csr(CVMX_DFM_WLEVEL_DBG);
    return wlevel_dbg.s.bitmask;
}

static int validate_ddr3_rlevel_bitmask(int bitmask)
{
    int mask = 0x3f;
    int shifted_mask;
    int i, leader;

    for (leader=0; leader<32; ++leader) {
        if ((bitmask>>leader)&1)
            break;
    }

    /*
    ** Only accept a mask within 0x3f with contiguous bits set.  Valid
    ** masks are 0x3f, 0x1f, 0x0f, 0x07, 0x03, 0x01.  Anything else
    ** fails.
    */
    for (i=0; i<6; ++i) {
        shifted_mask = (mask>>i)<<leader;

        //debug_print("bitmask=0x06%x, leader=%d, shifted_mask=0x%05x\n", bitmask, leader, shifted_mask);

        /* Accept bitmask that equals valid mask */
        if ((shifted_mask&bitmask) == bitmask)
            return 0;
    }

    return 1;
}

static int test_dfm_byte(uint64_t *p, int count, int byte)
{
    uint64_t *p1, *p2, d1, d2;
    int errors = 0;
    int i;
    uint64_t *p2base = (uint64_t *)((((unsigned long)(p+count)+0x100000)>>20)<<20); /* Round up to next MB boundary */

    p1 = p;
    p2 = p2base;
    for (i=0; i<count; ++i) {
        *p1 = *p2 = (~((uint64_t)i%256)&0xff)<<(8*byte);
        p1+=2;
        p2+=2;
    }

    p1 = p;
    p2 = p2base;
    for (i=0; i<count; ++i) {
        d1 = *p1;
        d2 = *p2;

        if (d1 != d2) {
            ++errors;
            debug_print("%d: [%p] 0x%016lX  [%p] 0x%016lX xor %016lX\n",
                        errors, p1, d1, p2, d2, (d1 ^ d2));
            return errors;      /* Quit on first error */
        }
        p1+=2;
        p2+=2;
    }
    return errors;
}

static unsigned rlevel_to_wlevel(unsigned byte_delay)
{
    return (divide_roundup(byte_delay,2) & 0x1e);
}


#ifdef CVMX_BUILD_FOR_UBOOT
int octeon_dfm_initialize(void)
#else
int main()
#endif
{
#if defined(__U_BOOT__) || defined(unix)
    char *s;
#endif

    uint32_t cpu_id = cvmx_get_proc_id();

    unsigned long tclk_psecs;

    int row_bits, col_bits, num_banks, num_ranks, dram_width;

    int safe_dfm_flag = 0; /* Flag that indicates safe DDR settings should be used */
    uint32_t mem_size_mbytes = 0;
    int bank_bits = 0;
    int bunk_enable = 0;
    int column_bits_start = 1;
    int row_lsb;
    int pbank_lsb;

    uint64_t dfm_hertz;
    uint64_t cpu_hertz;

    /* Samsung K4B2G1646B-HFC8  128M x 16 x 8 banks  */
    num_banks             = 8;
    dram_width            = 16;
    row_bits              = 14;
    col_bits              = 10;
    num_ranks             = 2;

    int wlevel_bitmask_errors = 0;
    cpu_hertz = cvmx_clock_get_rate(CVMX_CLOCK_SCLK);

    dfm_print("\nInitializing DFM interface\n");

    if (((s = getenv("dfm_safe")) != NULL) && (strcmp(s,"yes") == 0)) {
        safe_dfm_flag = 1;
        error_print("Parameter found in environment. dfm_safe = %d\n", safe_dfm_flag);
    }

    initialize_dfm_clock(cpu_id);
    dfm_hertz = measure_octeon_dfm_clock(cpu_id, cpu_hertz);

    tclk_psecs = (unsigned long) (1000*1000*1000)/(dfm_hertz/1000); /* Clock in psecs */
         

    /* Enable the SCLK source and DDR3 interface. */
    {
        cvmx_dfm_fnt_sclk_t dfm_fnt_sclk;
        cvmx_dfm_fnt_ctl_t dfm_fnt_ctl;

        dfm_fnt_sclk.u64 = cvmx_read_csr(CVMX_DFM_FNT_SCLK);
        dfm_fnt_sclk.s.sclkdis = 0; /* Enable the SCLK source. */
        dfm_config_write_csr(CVMX_DFM_FNT_SCLK, dfm_fnt_sclk.u64);

        dfm_fnt_ctl.u64 = cvmx_read_csr(CVMX_DFM_FNT_CTL);
        dfm_fnt_ctl.s.dfr_ena = 1; /* Enable the DDR3 interface */
        dfm_config_write_csr(CVMX_DFM_FNT_CTL, dfm_fnt_ctl.u64);
    }


    if (num_banks == 8)
        bank_bits = 3;
    else if (num_banks == 4)
        bank_bits = 2;

    if (num_ranks > 1)
    	bunk_enable = 1;

    row_lsb = column_bits_start + col_bits + bank_bits;
    debug_print("row_lsb = column_bits_start + col_bits + bank_bits = %d\n", row_lsb);

    pbank_lsb = row_lsb + row_bits + bunk_enable;
    debug_print("pbank_lsb = row_lsb + row_bits + bunk_enable = %d\n", pbank_lsb);

    mem_size_mbytes =  ((1ull << pbank_lsb) >> 20);

    /* Parameters from DDR3 Specifications */
#define DDR3_tREFI 7800000       /* 7.8 us */
#define DDR3_ZQCS_Interval 80000 /* 80 ns */
#define DDR3_tCKE 5000          /* 5 ns */
#define DDR3_tXPR 5000          /* 5 ns */
#define DDR3_tMRD 4             /* 4 nCK */
#define DDR3_tDLLK 512          /* 512 nCK */
#define DDR3_tMPRR 1            /* 1 nCK */
#define DDR3_tRAS 35000         /* 35 ns to 9*tREFI */
#define DDR3_tWLMRD 40          /* 40 nCK */
#define DDR3_tWLDQSEN 25        /* 25 nCK */

    /*
     * 4.8.5 Early LMC Initialization
     *
     * All of DDR PLL, LMC CK, and LMC DRESET initializations must
     * be completed prior to starting this LMC initialization
     * sequence.
     *
     * 1. Software must ensure there are no pending DRAM transactions.
     *
     * 2. Write LMC0_CONFIG, LMC0_CONTROL, LMC0_TIMING_PARAMS0,
     *    LMC0_TIMING_PARAMS1, LMC0_MODEREG_PARAMS0,
     *    LMC0_MODEREG_PARAMS1, LMC0_DUAL_MEMCFG, LMC0_NXM,
     *    LMC0_WODT_MASK, LMC0_RODT_MASK, LMC0_COMP_CTL2,
     *    LMC0_PHY_CTL, LMC0_DIMM0/1_PARAMS, and LMC0_DIMM_CTL
     *    with appropriate values. All sections in this chapter
     *    may be used to derive proper register settings.
     */

    /* LMC0_CONFIG */
    {
        cvmx_dfm_config_t dfm_config;
        dfm_config.u64 = cvmx_read_csr(CVMX_DFM_CONFIG);
        dfm_config.s.init_start      = 0;
        dfm_config.s.row_lsb         = encode_row_lsb_ddr3(cpu_id, row_lsb);
        dfm_config.s.pbank_lsb       = encode_pbank_lsb_ddr3(cpu_id, pbank_lsb);
        dfm_config.s.idlepower       = 0; /* Disabled */
        dfm_config.s.forcewrite      = 0; /* Disabled */
        dfm_config.s.ecc_adr         = 1; /* Include memory reference address in the ECC */
        dfm_config.s.reset           = 0;

        /*
         *  Program LMC0_CONFIG[24:18], ref_zqcs_int(6:0) to
         *  RND-DN(tREFI/clkPeriod/512) Program LMC0_CONFIG[36:25],
         *  ref_zqcs_int(18:7) to
         *  RND-DN(ZQCS_Interval/clkPeriod/(512*64)). Note that this
         *  value should always be greater than 32, to account for
         *  resistor calibration delays.
         */
        dfm_config.s.ref_zqcs_int     = ((DDR3_tREFI/tclk_psecs/512) & 0x7f);
        dfm_config.s.ref_zqcs_int    |= (((DDR3_ZQCS_Interval/tclk_psecs/(512)*64) & 0xfff)<<7);

        dfm_config.s.sequence        = 0; /* Set later */
        dfm_config.s.early_dqx       = 0; /* disabled */
        dfm_config.s.sref_with_dll   = 0; /* disabled */
        dfm_config.s.rank_ena        = bunk_enable;
        dfm_config.s.rankmask        = 0; /* Set later */
        dfm_config.s.mirrmask        = 0; /* disabled */
        dfm_config.s.init_status     = 0; /* Set later */
        dfm_config_write_csr(CVMX_DFM_CONFIG, dfm_config.u64);
    }

    /* LMC0_CONTROL */
    {
        cvmx_dfm_control_t dfm_control;
        dfm_control.u64 = cvmx_read_csr(CVMX_DFM_CONTROL);
        dfm_control.s.bwcnt           = 0; /* Clear counter later */
        dfm_control.s.ddr2t           = safe_dfm_flag ? 1 : 0;
        dfm_control.s.pocas           = 0;
        dfm_control.s.fprch2          = 1;
        dfm_control.s.throttle_rd     = safe_dfm_flag ? 1 : 0;
        dfm_control.s.throttle_wr     = safe_dfm_flag ? 1 : 0;
        dfm_control.s.inorder_rd      = safe_dfm_flag ? 1 : 0;
        dfm_control.s.inorder_wr      = safe_dfm_flag ? 1 : 0;
        dfm_control.s.elev_prio_dis   = safe_dfm_flag ? 1 : 0;
        dfm_control.s.nxm_write_en    = 0; /* discards writes to
                                            addresses that don't exist
                                            in the DRAM */
        dfm_control.s.max_write_batch = 8;
        dfm_control.s.xor_bank        = 1;
        //dfm_control.s.auto_dclkdis    = 1;
        dfm_control.s.int_zqcs_dis    = 1;
        dfm_control.s.ext_zqcs_dis    = 0;
        dfm_control.s.bprch           = 0;
        dfm_control.s.wodt_bprch      = 0;
        dfm_control.s.rodt_bprch      = 0;
        dfm_config_write_csr(CVMX_DFM_CONTROL, dfm_control.u64);
    }

    /* LMC0_TIMING_PARAMS0 */
    {
        cvmx_dfm_timing_params0_t dfm_timing_params0;
        dfm_timing_params0.u64 = cvmx_read_csr(CVMX_DFM_TIMING_PARAMS0);

        if (octeon_is_cpuid(OCTEON_CN63XX_PASS1_0)) {
            /* Reserved. Should be written to zero. */
            dfm_timing_params0.cn63xxp1.tckeon   = 0;
        }
        dfm_timing_params0.s.tzqcs    = 4;
        dfm_timing_params0.s.tcke     = 3;
        dfm_timing_params0.s.txpr     = 5;
        dfm_timing_params0.s.tmrd     = divide_roundup((DDR3_tMRD*tclk_psecs), tclk_psecs) - 1;
        dfm_timing_params0.s.tmod     = 0xb;
        dfm_timing_params0.s.tdllk    = 2;
        dfm_timing_params0.s.tzqinit  = 2;
        dfm_timing_params0.s.trp      = 9;
        dfm_timing_params0.s.tcksre   = 6;

        if (!octeon_is_cpuid(OCTEON_CN63XX_PASS1_0)) {
            dfm_timing_params0.s.trp_ext  = 0;
        }
        dfm_config_write_csr(CVMX_DFM_TIMING_PARAMS0, dfm_timing_params0.u64);
    }

    /* LMC0_TIMING_PARAMS1 */
    {
        cvmx_dfm_timing_params1_t dfm_timing_params1;
        dfm_timing_params1.u64 = cvmx_read_csr(CVMX_DFM_TIMING_PARAMS1);
        dfm_timing_params1.s.tmprr    = 1;

        dfm_timing_params1.s.tras     = 0x17;
        dfm_timing_params1.s.trcd     = 9;
        dfm_timing_params1.s.twtr     = 4;
        dfm_timing_params1.s.trfc     = 0xa;
        dfm_timing_params1.s.trrd     = 2;
        dfm_timing_params1.s.txp      = 3;
        dfm_timing_params1.s.twlmrd   = 0xa;
        dfm_timing_params1.s.twldqsen = 7;
        dfm_timing_params1.s.tfaw     = 5;
        dfm_timing_params1.s.txpdll   = 0xf;
        if (!octeon_is_cpuid(OCTEON_CN63XX_PASS1_0)) {
            dfm_timing_params1.s.tras_ext = 0;
        }
        dfm_config_write_csr(CVMX_DFM_TIMING_PARAMS1, dfm_timing_params1.u64);
    }

    /* LMC0_MODEREG_PARAMS0 */
    {
        cvmx_dfm_modereg_params0_t dfm_modereg_params0;
        dfm_modereg_params0.u64 = cvmx_read_csr(CVMX_DFM_MODEREG_PARAMS0);
        dfm_modereg_params0.s.cwl     = 1;
        dfm_modereg_params0.s.mprloc  = 0;
        dfm_modereg_params0.s.mpr     = 0;
        dfm_modereg_params0.s.dll     = 0;
        dfm_modereg_params0.s.al      = 0;
        dfm_modereg_params0.s.wlev    = 0;
        dfm_modereg_params0.s.tdqs    = 0;
        dfm_modereg_params0.s.qoff    = 0;
        dfm_modereg_params0.s.bl      = 0;
        dfm_modereg_params0.s.cl      = 0x8;
        dfm_modereg_params0.s.rbt     = 0;
        dfm_modereg_params0.s.tm      = 0;
        dfm_modereg_params0.s.dllr    = 0;
        dfm_modereg_params0.s.wrp     = 2; 
        dfm_modereg_params0.s.ppd     = 0;
        dfm_config_write_csr(CVMX_DFM_MODEREG_PARAMS0, dfm_modereg_params0.u64);
    }

    /* LMC0_MODEREG_PARAMS1 */
    {
        cvmx_dfm_modereg_params1_t dfm_modereg_params1;
        dfm_modereg_params1.u64 = cvmx_read_csr(CVMX_DFM_MODEREG_PARAMS1);
        dfm_modereg_params1.s.pasr_00      = 0;
        dfm_modereg_params1.s.asr_00       = 0;
        dfm_modereg_params1.s.srt_00       = 0;
        dfm_modereg_params1.s.rtt_wr_00    = 2; /* 240/2 = 120 ohms? */
        dfm_modereg_params1.s.rtt_wr_00    = 0;
        dfm_modereg_params1.s.dic_00       = 0;
        dfm_modereg_params1.s.rtt_nom_00   = 3; /* 240/6 =  40 ohms? */
        dfm_modereg_params1.s.rtt_nom_00   = 0;
        dfm_modereg_params1.s.pasr_01      = 0;
        dfm_modereg_params1.s.asr_01       = 0;
        dfm_modereg_params1.s.srt_01       = 0;
        dfm_modereg_params1.s.rtt_wr_01    = 0;
        dfm_modereg_params1.s.dic_01       = 0;
        dfm_modereg_params1.s.rtt_nom_01   = 0;
        dfm_modereg_params1.s.pasr_10      = 0;
        dfm_modereg_params1.s.asr_10       = 0;
        dfm_modereg_params1.s.srt_10       = 0;
        dfm_modereg_params1.s.rtt_wr_10    = 0;
        dfm_modereg_params1.s.dic_10       = 0;
        dfm_modereg_params1.s.rtt_nom_10   = 0;
        dfm_modereg_params1.s.pasr_11      = 0;
        dfm_modereg_params1.s.asr_11       = 0;
        dfm_modereg_params1.s.srt_11       = 0;
        dfm_modereg_params1.s.rtt_wr_11    = 0;
        dfm_modereg_params1.s.dic_11       = 0;
        dfm_modereg_params1.s.rtt_nom_11   = 0;
        dfm_config_write_csr(CVMX_DFM_MODEREG_PARAMS1, dfm_modereg_params1.u64);
    }


    /* FIXME: Single-rank only for poweron */
    /* LMC0_WODT_MASK */
    {
        cvmx_dfm_wodt_mask_t dfm_wodt_mask;
        dfm_wodt_mask.u64 = cvmx_read_csr(CVMX_DFM_WODT_MASK);
        dfm_wodt_mask.s.wodt_d0_r0         = 2;
        dfm_wodt_mask.s.wodt_d0_r1         = 1;
        dfm_wodt_mask.s.wodt_d1_r0         = 0x00;
        dfm_wodt_mask.s.wodt_d1_r1         = 0x00;
        dfm_wodt_mask.s.wodt_d2_r0         = 0x00;
        dfm_wodt_mask.s.wodt_d2_r1         = 0x00;
        dfm_wodt_mask.s.wodt_d3_r0         = 0x00;
        dfm_wodt_mask.s.wodt_d3_r1         = 0x00;
        dfm_config_write_csr(CVMX_DFM_WODT_MASK, dfm_wodt_mask.u64);
    }

    /* FIXME: Single-rank only for poweron */
    /* LMC0_RODT_MASK */
    {
        cvmx_dfm_rodt_mask_t dfm_rodt_mask;
        dfm_rodt_mask.u64 = cvmx_read_csr(CVMX_DFM_RODT_MASK);
        dfm_rodt_mask.s.rodt_d0_r0         = 1;
        dfm_rodt_mask.s.rodt_d0_r1         = 0;
        dfm_rodt_mask.s.rodt_d1_r0         = 0x00;
        dfm_rodt_mask.s.rodt_d1_r1         = 0x00;
        dfm_rodt_mask.s.rodt_d2_r0         = 0x00;
        dfm_rodt_mask.s.rodt_d2_r1         = 0x00;
        dfm_rodt_mask.s.rodt_d3_r0         = 0x00;
        dfm_rodt_mask.s.rodt_d3_r1         = 0x00;
        dfm_config_write_csr(CVMX_DFM_RODT_MASK, dfm_rodt_mask.u64);
    }

    /* LMC0_COMP_CTL2 */
    {
        cvmx_dfm_comp_ctl2_t dfm_comp_ctl2;
        dfm_comp_ctl2.u64 = cvmx_read_csr(CVMX_DFM_COMP_CTL2);
        dfm_comp_ctl2.s.dqx_ctl            = 0x4; /* 34.3 ohm */
        dfm_comp_ctl2.s.ck_ctl             = 0x4; /* 34.3 ohm */
        dfm_comp_ctl2.s.cmd_ctl            = 0x4; /* 34.3 ohm */
        dfm_comp_ctl2.s.rodt_ctl           = 0x4; /* 60 ohm */
        dfm_comp_ctl2.s.ntune              = 0;
        dfm_comp_ctl2.s.ptune              = 0;
        dfm_comp_ctl2.s.byp                = 0;
        dfm_comp_ctl2.s.m180               = 0;
        dfm_comp_ctl2.s.ddr__ntune         = 0; /* read only */
        dfm_comp_ctl2.s.ddr__ptune         = 0; /* read only */
        dfm_config_write_csr(CVMX_DFM_COMP_CTL2, dfm_comp_ctl2.u64);
    }

#if 0                           /* Not in HRM */
    /* LMC0_PHY_CTL */
    {
        cvmx_dfm_phy_ctl_t dfm_phy_ctl;
        dfm_phy_ctl.u64 = cvmx_read_csr(CVMX_DFM_PHY_CTL);
        dfm_phy_ctl.s.ts_stagger           =
        dfm_phy_ctl.s.loopback_pos         =
        dfm_phy_ctl.s.loopback             =
        dfm_phy_ctl.s.ck_dlyout0           =
        dfm_phy_ctl.s.ck_tune0             =
        dfm_phy_ctl.s.ck_dlyout1           =
        dfm_phy_ctl.s.ck_tune1             =
        dfm_phy_ctl.s.lv_mode              =
        dfm_config_write_csr(CVMX_DFM_PHY_CTL, dfm_phy_ctl.u64);
    }
#endif

    /*
     * 3. Without changing any other fields in LMC0_CONFIG, write
     *    LMC0_CONFIG[RANKMASK,INIT_STATUS,SEQUENCE] while writing
     *    LMC0_CONFIG[INIT_START] = 1, all with a single CSR
     *    write. Both LMC0_CONFIG[RANKMASK,INIT_STATUS] bits
     *    should be set to the same mask value indicating the
     *    ranks that will participate in the sequence.
     *
     *    The SEQUENCE value should select power-up/init or
     *    self-refresh exit, depending on whether the DRAM parts
     *    are in self-refresh and whether their contents should be
     *    preserved. While LMC performs these sequences, it will
     *    not perform any other DDR3 transactions.
     *
     *    If power-up/init is selected immediately following a
     *    DRESET assertion, LMC executes the sequence described in
     *    the Reset and Initialization Procedure section of the
     *    JEDEC DDR3 specification. This includes activating CKE,
     *    writing all four DDR3 mode registers on all selected
     *    ranks, and issuing the required ZQCL command. The
     *    RANKMASK value should select all ranks with attached
     *    DRAM in this case. If LMC0_CONTROL[RDIMM_ENA]=1, LMC
     *    writes the JEDEC standard SSTE32882 control words
     *    selected by LMC0_DIMM_CTL [DIMM*_WMASK] between DDR_CKE*
     *    signal assertion and the first DDR3 mode register write.
     */

    {
        cvmx_dfm_config_t dfm_config;
        dfm_config.u64 = cvmx_read_csr(CVMX_DFM_CONFIG);
        dfm_config.s.init_start      = 1;
        dfm_config.s.sequence        = 0; /* power-up/init */
        dfm_config.s.rankmask        = 1;
        dfm_config.s.init_status     = 1;
        dfm_config_write_csr(CVMX_DFM_CONFIG, dfm_config.u64);

	dfm_config.u64 = cvmx_read_csr(CVMX_DFM_CONFIG);
        dfm_config.s.init_start      = 1;
        dfm_config.s.sequence        = 0; /* power-up/init */
        dfm_config.s.rankmask        = 2;
        dfm_config.s.init_status     = 2;
        dfm_config_write_csr(CVMX_DFM_CONFIG, dfm_config.u64);
    }

    /*
     *    If self-refresh exit is selected, LMC executes the
     *    required SRX command followed by a refresh and ZQ
     *    calibration. Section 4.5 describes behavior of a REF +
     *    ZQCS.  LMC does not write the DDR3 mode registers as
     *    part of this sequence, and the mode register parameters
     *    must match at self-refresh entry and exit times. The
     *    self-refresh exit sequence does not use LMC0_CONFIG
     *    [RANKMASK]. If the self-refresh exit is preceded by a
     *    self-refresh exit SEQUENCE without an intervening DRESET
     *    sequence, then LMC0_CONFIG [INIT_STATUS] will already
     *    indicate the ranks that should participate, and software
     *    need not assert any LMC0_CONFIG[INIT_STATUS]
     *    bits. Otherwise, the LMC0_CONFIG CSR write that
     *    initiates the self-refresh exit must also set
     *    LMC0_CONFIG[INIT_STATUS] bits for all ranks with
     *    attached DRAM.
     *
     * 4. Read LMC0_CONFIG and wait for the result.
     *
     * 4.8.6 LMC Write Leveling
     *
     * LMC supports an automatic write-leveling like that
     * described in the JEDEC DDR3 specifications separately per
     * byte-lane.
     *
     * All of DDR PLL, LMC CK, and LMC DRESET, and early LMC
     * initializations must be completed prior to starting this
     * LMC write-leveling sequence.
     *
     * There are many possible procedures that will write-level
     * all the attached DDR3 DRAM parts. One possibility is for
     * software to simply write the desired values into
     * LMC0_WLEVEL_RANK(0..3). This section describes one possible
     * sequence that uses LMCs auto-write-leveling capabilities.
     */
#if defined(CONFIG_OCTEON_GEF_63XX_BOARD_GENERAL)
    {
        cvmx_dfm_wlevel_ctl_t wlevel_ctl;
        cvmx_dfm_wlevel_rankx_t dfm_wlevel_rank;
        cvmx_dfm_config_t dfm_config;
        int rankx = 0;

        dfm_print("Setting Write-Leveling\n");

        wlevel_ctl.u64 = cvmx_read_csr(CVMX_DFM_WLEVEL_CTL);
        dfm_config.u64 = cvmx_read_csr(CVMX_DFM_CONFIG);

		/* The auto-leveling values return by the Octeon 63XX are
		** invalid for the second rank, so we hard code them here.
		*/
		for (rankx=0; rankx<num_ranks; rankx++)
		{
			/* Values hard coded to work at 500/533 MHz */
			if (rankx == 0)
			{
				dfm_wlevel_rank.cn63xx.byte1 = 0;
				dfm_wlevel_rank.cn63xx.byte0 = 0;
			}
			else
			{
				dfm_wlevel_rank.cn63xx.byte1 = 0;
				dfm_wlevel_rank.cn63xx.byte0 = 0;
			}
	
			dfm_config_write_csr(CVMX_DFM_WLEVEL_RANKX(rankx), dfm_wlevel_rank.u64);
	
			do {
				dfm_wlevel_rank.u64 = cvmx_read_csr(CVMX_DFM_WLEVEL_RANKX(rankx));
			} while (dfm_wlevel_rank.cn63xx.status != 1);
	
			dfm_print("Wlevel Rank(%d) Status/Deskew Bytes 8:0 %#6x : %4d %4d\n",
					   rankx,
					   dfm_wlevel_rank.cn63xx.status,
					   dfm_wlevel_rank.cn63xx.byte1,
					   dfm_wlevel_rank.cn63xx.byte0
					   );
		}
    }
#else
    {
        cvmx_dfm_wlevel_ctl_t wlevel_ctl;
        cvmx_dfm_wlevel_rankx_t dfm_wlevel_rank;
        cvmx_dfm_config_t dfm_config;
        int rankx = 0;
        int wlevel_bitmask[9] = {0};
        int byte_idx;
        int i;
        int anybitsset;

        dfm_print("Performing Write-Leveling\n");

        wlevel_ctl.u64 = cvmx_read_csr(CVMX_DFM_WLEVEL_CTL);
        dfm_config.u64 = cvmx_read_csr(CVMX_DFM_CONFIG);

		for (rankx=0; rankx < num_ranks; rankx++)
		{
			/* Clear write-level delays */
			dfm_wlevel_rank.u64 = 0;
			dfm_config_write_csr(CVMX_DFM_WLEVEL_RANKX(rankx), dfm_wlevel_rank.u64);
	
			wlevel_bitmask_errors = 0;
			for (i=0; i<2; ++i) {
				wlevel_ctl.s.lanemask = 1 << i; /* lanemask = 0x1, 0x2 */
	
				dfm_config_write_csr(CVMX_DFM_WLEVEL_CTL, wlevel_ctl.u64);
	
				/* Read and write values back in order to update the
				   status field. This insurs that we read the updated
				   values after write-leveling has completed. */
				dfm_config_write_csr(CVMX_DFM_WLEVEL_RANKX(rankx),
									 cvmx_read_csr(CVMX_DFM_WLEVEL_RANKX(rankx)));
	
				dfm_config.s.sequence        = 6; /* write-leveling */
				dfm_config.s.rankmask        = 1 << rankx;  
				dfm_config.s.init_start      = 1;
				dfm_config_write_csr(CVMX_DFM_CONFIG, dfm_config.u64);
	
				do {
					dfm_wlevel_rank.u64 = cvmx_read_csr(CVMX_DFM_WLEVEL_RANKX(rankx));
				} while (dfm_wlevel_rank.cn63xx.status != 3);
	
				anybitsset = 0;
				for (byte_idx=i; byte_idx<2; byte_idx+=2) {
					wlevel_bitmask[byte_idx] = octeon_read_dfm_ddr3_wlevel_dbg(byte_idx);
					anybitsset |= wlevel_bitmask[byte_idx];
					if (wlevel_bitmask[byte_idx] == 0)
						++wlevel_bitmask_errors;
				}
	
				dfm_print("Rank(%d) Wlevel Debug Results   (lanemask %03X) : %04x %04x %s\n",
						  rankx,
						  wlevel_ctl.s.lanemask,
						  wlevel_bitmask[1],
						  wlevel_bitmask[0],
						  (anybitsset ? "*!*!*!*!" : "")
						  );
	
				dfm_print("Wlevel Rank(%d) Status/Deskew Bytes 8:0 %#6x : %4d %4d\n",
						  rankx,
						  dfm_wlevel_rank.cn63xx.status,
						  dfm_wlevel_rank.cn63xx.byte1,
						  dfm_wlevel_rank.cn63xx.byte0
						  );
			}
	
			if (wlevel_bitmask_errors != 0) {
				dfm_print("!!! Write-Leveling Failed. Using fixed delay settings !!!\n");
	
				/* FIXME: Hardcoded values until Wleveling works. These are only good for 400 MHz. */
				/* Estimates based on scope measurements in the lab */
				dfm_wlevel_rank.cn63xx.byte1 = 0;
				dfm_wlevel_rank.cn63xx.byte0 = 0;
	
				dfm_config_write_csr(CVMX_DFM_WLEVEL_RANKX(rankx), dfm_wlevel_rank.u64);
	
				do {
					dfm_wlevel_rank.u64 = cvmx_read_csr(CVMX_DFM_WLEVEL_RANKX(rankx));
				} while (dfm_wlevel_rank.cn63xx.status != 1);
	
				dfm_print("Wlevel Rank(%d) Status/Deskew Bytes 8:0 %#6x : %4d %4d\n",
						  rankx,
						  dfm_wlevel_rank.cn63xx.status,
						  dfm_wlevel_rank.cn63xx.byte1,
						  dfm_wlevel_rank.cn63xx.byte0
						  );
			}
		}
    }
#endif /* CONFIG_OCTEON_GEF_63XX_BOARD_GENERAL */

    /*
     * 1. If the DQS/DQ delays on the board may be more than the
     *    ADD/CMD delays, then ensure that LMC0_CONFIG[EARLY_DQX]
     *    is set at this point.
     *
     * Do the remaining steps 2-7 separately for each rank i with
     * attached DRAM.
     *
     * 2. Write LMC0_WLEVEL_RANKi = 0.
     *
     * 3. For 8 parts:
     *
     *    Without changing any other fields in LMC0_WLEVEL_CTL,
     *    write LMC0_WLEVEL_CTL[LANEMASK] to select all byte lanes
     *    with attached DRAM.
     *
     *    For 16 parts:
     *
     *    Without changing any other fields in LMC0_WLEVEL_CTL,
     *    write LMC0_WLEVEL_CTL[LANEMASK] to select all even byte
     *    lanes with attached DRAM.
     *
     * 4. Without changing any other fields in LMC0_CONFIG,
     *
     *    o write LMC0_CONFIG[SEQUENCE] to select write-leveling, and
     *
     *    o write LMC0_CONFIG[RANKMASK] = (1 << i), and
     *
     *    o write LMC0_CONFIG[INIT_START] = 1
     *
     *    LMC will initiate write-leveling at this point. Assuming
     *    LMC0_WLEVEL_CTL [SSET] = 0, LMC first enables
     *    write-leveling on the selected DRAM rank via a DDR3 MR1
     *    write, then sequences through and accumulates
     *    write-leveling results for 8 different delay settings
     *    twice, starting at a delay of zero in this case since
     *    LMC0_WLEVEL_RANKi[BYTE*<4:3>] = 0, increasing by 1/8 CK
     *    each setting, covering a total distance of one CK, then
     *    disables the write-leveling via another DDR3 MR1 write.
     *
     *    After the sequence through 16 delay settings is complete:
     *
     *    o LMC sets LMC0_WLEVEL_RANKi[STATUS]=3
     *
     *    o LMC sets LMC0_WLEVEL_RANKi[BYTE*<2:0>] (for all ranks
     *      selected by LANEMASK) to indicate the first write
     *      leveling result of 1 that followed result of 0 during
     *      the sequence, except that LMC always writes
     *      LMC0_WLEVEL_RANKi[BYTE*<0>]=0.
     *
     *    o Software can read the 8 write-leveling results from
     *      the first pass through the delay settings by reading
     *      LMC0_WLEVEL_DBG[BITMASK] (after writing
     *      LMC0_WLEVEL_DBG[BYTE]). (LMC does not retain the
     *      write-leveling results from the second pass through
     *      the 8 delay settings. They should often be identical
     *      to the LMC0_WLEVEL_DBG[BITMASK] results, though.)
     *
     * 5. Wait until LMC0_WLEVEL_RANKi[STATUS] != 2.
     *
     *    LMC will have updated LMC0_WLEVEL_RANKi[BYTE*<2:0>] for
     *    all byte lanes selected by LMC0_WLEVEL_CTL[LANEMASK] at
     *    this point.  LMC0_WLEVEL_RANKi[BYTE*<4:3>] will still be
     *    the value that software wrote in step 2 above, which is
     *    zero.
     *
     * 6. For 16 parts:
     *
     *    Without changing any other fields in LMC0_WLEVEL_CTL,
     *    write LMC0_WLEVEL_CTL[LANEMASK] to select all odd byte
     *    lanes with attached DRAM.
     *
     *    Repeat steps 4 and 5 with this new
     *    LMC0_WLEVEL_CTL[LANEMASK] setting. Skip to step 7 if
     *    this has already been done.
     *
     *    For 8 parts:
     *
     *    Skip this step. Go to step 7.
     *
     * 7. Calculate LMC0_WLEVEL_RANKi[BYTE*<4:3>] settings for all
     *    byte lanes on all ranks with attached DRAM.
     *
     *    At this point, all byte lanes on rank i with attached
     *    DRAM should have been write-leveled, and
     *    LMC0_WLEVEL_RANKi[BYTE*<2:0>] has the result for each
     *    byte lane.
     *
     *    But note that the DDR3 write-leveling sequence will only
     *    determine the delay modulo the CK cycle time, and cannot
     *    determine how many additional CKs of delay are
     *    present. Software must calculate the number of CKs, or
     *    equivalently, the LMC0_WLEVEL_RANKi[BYTE*<4:3>]
     *    settings.
     *
     *    This BYTE*<4:3> calculation is system/board specific.
     *
     *    Many techniques may be used to calculate write-leveling
     *    BYTE*<4:3> values, including:
     *
     *    o Known values for some byte lanes.
     *
     *    o Relative values for some byte lanes relative to others.
     *
     *    For example, suppose lane X is likely to require a
     *    larger write-leveling delay than lane Y. A BYTEX<2:0>
     *    value that is much smaller than the BYTEY<2:0> value may
     *    then indicate that the required lane X delay wrapped
     *    into the next CK, so BYTEX<4:3> should be set to
     *    BYTEY<4:3>+1.
     *
     * 8. Initialize LMC0_WLEVEL_RANK* values for all unused ranks.
     *
     *    Let rank i be a rank with attached DRAM.
     *
     *    For all ranks j that do not have attached DRAM, set
     *    LMC0_WLEVEL_RANKj = LMC0_WLEVEL_RANKi.
     *
     * 4.8.7 LMC Read Leveling
     *
     * LMC supports an automatic read-leveling separately per
     * byte-lane using the DDR3 multi-purpose register pre-defined
     * pattern for system calibration defined in the JEDEC DDR3
     * specifications.
     *
     * All of DDR PLL, LMC CK, and LMC DRESET, and early LMC
     * initializations must be completed prior to starting this
     * LMC read-leveling sequence.
     *
     * Software could simply write the desired read-leveling
     * values into LMC0_RLEVEL_RANK(0..3). This section describes
     * a sequence that uses LMCs auto-read-leveling capabilities.
     *
     * When LMC does the read-leveling sequence for a rank, it
     * first enables the DDR3 multi-purpose register predefined
     * pattern for system calibration on the selected DRAM rank
     * via a DDR3 MR3 write, then executes 64 RD operations at
     * different internal delay settings, then disables the
     * predefined pattern via another DDR3 MR3 write. LMC
     * determines the pass or fail of each of the 64 settings
     * independently for each byte lane, then writes appropriate
     * LMC0_RLEVEL_RANK(0..3)[BYTE*] values for the rank.
     *
     * After read-leveling for a rank, software can read the 64
     * pass/fail indications for one byte lane via
     * LMC0_RLEVEL_DBG[BITMASK]. Software can observe all
     * pass/fail results for all byte lanes in a rank via separate
     * read-leveling sequences on the rank with different
     * LMC0_RLEVEL_CTL[BYTE] values.
     *
     * The 64 pass/fail results will typically have failures for
     * the low delays, followed by a run of some passing settings,
     * followed by more failures in the remaining high delays.
     * LMC sets LMC0_RLEVEL_RANK(0..3)[BYTE*] to one of the
     * passing settings. First, LMC selects the longest run of
     * successes in the 64 results. (In the unlikely event that
     * there is more than one longest run, LMC selects the first
     * one.) Then if LMC0_RLEVEL_CTL[OFFSET_EN] = 1 and the
     * selected run has more than LMC0_RLEVEL_CTL[OFFSET]
     * successes, LMC selects the last passing setting in the run
     * minus LMC0_RLEVEL_CTL[OFFSET]. Otherwise LMC selects the
     * middle setting in the run (rounding earlier when
     * necessary). We expect the read-leveling sequence to produce
     * good results with the reset values LMC0_RLEVEL_CTL
     * [OFFSET_EN]=1, LMC0_RLEVEL_CTL[OFFSET] = 2.
     *
     * The read-leveling sequence:
     *
     * 1. Select desired LMC0_RLEVEL_CTL[OFFSET_EN,OFFSET,BYTE]
     *    settings.  Do the remaining steps 2V4 separately for
     *    each rank i with attached DRAM.
     */

/* Auto-leveling working correctly in Pass 2.X, retaining this code
** for older boards.
*/
#if 0 //defined(CONFIG_OCTEON_GEF_63XX_BOARD_GENERAL)
    {
        cvmx_dfm_rlevel_rankx_t dfm_rlevel_rank;
        cvmx_dfm_comp_ctl2_t dfm_comp_ctl2;
        int rankx = 0;


        dfm_print("Setting Read-Leveling\n");
        dfm_comp_ctl2.u64 = cvmx_read_csr(CVMX_DFM_COMP_CTL2);
        dfm_comp_ctl2.s.rodt_ctl = 3; /* 40 ohms */
        dfm_config_write_csr(CVMX_DFM_COMP_CTL2, dfm_comp_ctl2.u64);

		for (rankx=0; rankx<2; rankx++)
		{
			/* Values hard coded to work at 533 MHz */
			if (rankx == 0)
			{
			dfm_rlevel_rank.cn63xx.byte1 = 7;
			dfm_rlevel_rank.cn63xx.byte0 = 7;
			}
			else
			{
			dfm_rlevel_rank.cn63xx.byte1 = 7;
			dfm_rlevel_rank.cn63xx.byte0 = 7;
			}
	
			dfm_config_write_csr(CVMX_DFM_RLEVEL_RANKX(rankx), dfm_rlevel_rank.u64);        do {
				dfm_rlevel_rank.u64 = cvmx_read_csr(CVMX_DFM_RLEVEL_RANKX(rankx));
			} while (dfm_rlevel_rank.cn63xx.status != 1);
	
			dfm_print("Rlevel Rank(%d) Status/Deskew Bytes 8:0 %#6x : %4d %4d\n",
					   rankx,
					   dfm_rlevel_rank.cn63xx.status,
					   dfm_rlevel_rank.cn63xx.byte1,
					   dfm_rlevel_rank.cn63xx.byte0
					   );
		}
    }
#else
    {
        cvmx_dfm_rlevel_ctl_t rlevel_ctl;
        cvmx_dfm_rlevel_rankx_t dfm_rlevel_rank;
        cvmx_dfm_config_t dfm_config;
        cvmx_dfm_comp_ctl2_t dfm_comp_ctl2;
        int rodt_ctl;
        int rankx = 0;
        int rlevel_bitmask_errors;
        int rodt_ohms[] = { -1, 20, 30, 40, 60, 120 };

        dfm_print("Performing Read-Leveling\n");
        for (rodt_ctl=5; rodt_ctl>0; --rodt_ctl) {
            dfm_comp_ctl2.u64 = cvmx_read_csr(CVMX_DFM_COMP_CTL2);
            dfm_comp_ctl2.s.rodt_ctl = rodt_ctl;
            dfm_config_write_csr(CVMX_DFM_COMP_CTL2, dfm_comp_ctl2.u64);
            dfm_comp_ctl2.u64 = cvmx_read_csr(CVMX_DFM_COMP_CTL2);
            dfm_print("Read ODT_CTL                                  : 0x%x (%d ohms)\n",
                      dfm_comp_ctl2.s.rodt_ctl, rodt_ohms[rodt_ctl]);

            rlevel_ctl.u64 = cvmx_read_csr(CVMX_DFM_RLEVEL_CTL);
            dfm_config.u64 = cvmx_read_csr(CVMX_DFM_CONFIG);

			for (rankx=0; rankx < num_ranks; rankx++)
			{
				/* Clear read-level delays */
				dfm_config_write_csr(CVMX_DFM_RLEVEL_RANKX(rankx), 0);
	
				dfm_config.s.sequence        = 1; /* read-leveling */
				dfm_config.s.rankmask        = 1 << rankx;
				dfm_config.s.init_start      = 1;
				dfm_config_write_csr(CVMX_DFM_CONFIG, dfm_config.u64);
	
				do {
					dfm_rlevel_rank.u64 = cvmx_read_csr(CVMX_DFM_RLEVEL_RANKX(rankx));
				} while (dfm_rlevel_rank.cn63xx.status != 3);
	
				{
					int rlevel_bitmask[2];
					int byte_idx;
	
					rlevel_bitmask_errors = 0;
					for (byte_idx=0; byte_idx<2; ++byte_idx) {
						rlevel_bitmask[byte_idx] = octeon_read_dfm_ddr3_rlevel_dbg(byte_idx);
						if (rlevel_bitmask[byte_idx] == 0)
							++rlevel_bitmask_errors;
						rlevel_bitmask_errors += validate_ddr3_rlevel_bitmask(rlevel_bitmask[byte_idx]);
					}
	
					dfm_print("Rank(%d) Rlevel Debug Test Results 8:0         : %04x %04x\n",
							  rankx,
							  rlevel_bitmask[1],
							  rlevel_bitmask[0] 
							  );
				}
	
				dfm_rlevel_rank.u64 = cvmx_read_csr(CVMX_DFM_RLEVEL_RANKX(rankx));
				dfm_print("Rlevel Rank(%d) Status/Deskew Bytes 8:0 %#6x : %4d %4d\n",
						  rankx,
						  dfm_rlevel_rank.cn63xx.status,
						  dfm_rlevel_rank.cn63xx.byte1,
						  dfm_rlevel_rank.cn63xx.byte0
						  );
			}

            if (rlevel_bitmask_errors == 0)
                break;
        }
        if (rlevel_bitmask_errors != 0) {
            dfm_print("!!! Read-Leveling Failed !!!\n");
        }
    }
#endif /* CONFIG_OCTEON_GEF_63XX_BOARD_GENERAL */

    /*
     * 2. Without changing any other fields in LMC0_CONFIG,
     *
     *    o write LMC0_CONFIG[SEQUENCE] to select read-leveling, and
     *
     *    o write LMC0_CONFIG[RANKMASK] = (1 << i), and
     *
     *    o write LMC0_CONFIG[INIT_START] = 1
     *
     *    This initiates the previously-described read-leveling.
     *
     * 3. Wait until LMC0_RLEVEL_RANKi[STATUS] != 2
     *
     *    LMC will have updated LMC0_RLEVEL_RANKi[BYTE*] for all
     *    byte lanes at this point.
     *
     * 4. If desired, consult LMC0_RLEVEL_DBG[BITMASK] and compare
     *    to LMC0_RLEVEL_RANKi[BYTE*] for the lane selected by
     *    LMC0_RLEVEL_CTL[BYTE]. If desired, modify
     *    LMC0_RLEVEL_CTL[BYTE] to a new value and repeat so that
     *    all BITMASKs can be observed.
     *
     * 5. Initialize LMC0_RLEVEL_RANK* values for all unused ranks.
     *
     *    Let rank i be a rank with attached DRAM.
     *
     *    For all ranks j that do not have attached DRAM, set
     *    LMC0_RLEVEL_RANKj = LMC0_RLEVEL_RANKi.
     */


    if (wlevel_bitmask_errors != 0) {
        /* Try to determine write-level delays experimentally. */
        cvmx_dfm_wlevel_ctl_t wlevel_ctl;
        cvmx_dfm_wlevel_rankx_t dfm_wlevel_rank;
        cvmx_dfm_rlevel_rankx_t dfm_rlevel_rank;
        cvmx_dfm_config_t dfm_config;
        int byte;
        int delay;
        int byte_test_status[2] = {0};
        int rankx = 0;
        int max_rlevel_delay;

        dfm_print("Performing software Write-Leveling\n");

        wlevel_ctl.u64 = cvmx_read_csr(CVMX_DFM_WLEVEL_CTL);
        dfm_config.u64 = cvmx_read_csr(CVMX_DFM_CONFIG);

		for (rankx=0; rankx<2; rankx++)
		{
			dfm_rlevel_rank.u64 = cvmx_read_csr(CVMX_DFM_RLEVEL_RANKX(rankx));
			max_rlevel_delay = 0;
			max_rlevel_delay = max(max_rlevel_delay, dfm_rlevel_rank.cn63xx.byte0);
			max_rlevel_delay = max(max_rlevel_delay, dfm_rlevel_rank.cn63xx.byte1);
	
			/* Use Rlevel settings if software write-leveling fails */
			for (byte=0; byte<2; ++byte) {
				int passed = 0;
				for (delay=(max_rlevel_delay&0x1e) + 4; delay>=0; delay-=2) {
					switch(byte) {
					case 0:
						dfm_wlevel_rank.cn63xx.byte0 = delay;
						break;
					case 1:
						dfm_wlevel_rank.cn63xx.byte1 = delay;
						break;
					}
	
					debug_print("Testing byte %d delay %d\n", byte, delay);
					dfm_config_write_csr(CVMX_DFM_WLEVEL_RANKX(rankx), dfm_wlevel_rank.u64);

					if (rankx == 0 )
					{
						if (test_dfm_byte((uint64_t*)0x8001370500000000ULL, 256, byte) == 0) {
							++passed;
							if (passed == 2) { /* Look for two consecutive working settings */
								byte_test_status[byte] = 1;
								break;
							}
						} else {
							passed = 0;
						}
					}
					else
					{
						if (test_dfm_byte((uint64_t*)0x8001370510000000ULL, 256, byte) == 0) {
							++passed;
							if (passed == 2) { /* Look for two consecutive working settings */
								byte_test_status[byte] = 1;
								break;
							}
						} else {
							passed = 0;
						}
					}
				}
				if (passed != 2) {
					debug_print("Estimating Wlevel delay byte %d\n", byte);
					switch(byte) {
					case 0:
						dfm_wlevel_rank.cn63xx.byte0 = rlevel_to_wlevel(dfm_rlevel_rank.cn63xx.byte0);
						break;
					case 1:
						dfm_wlevel_rank.cn63xx.byte1 = rlevel_to_wlevel(dfm_rlevel_rank.cn63xx.byte1);
						break;
					}
				}
			}
	
			dfm_config_write_csr(CVMX_DFM_WLEVEL_RANKX(rankx), dfm_wlevel_rank.u64);
	
			do {
				dfm_wlevel_rank.u64 = cvmx_read_csr(CVMX_DFM_WLEVEL_RANKX(rankx));
			} while (dfm_wlevel_rank.cn63xx.status != 1);
	
			dfm_print("Wlevel Rank(%d) Status/Deskew Bytes 8:0 %#6x : %4d%3s %d%3s\n",
					  rankx,
					  dfm_wlevel_rank.cn63xx.status,
					  dfm_wlevel_rank.cn63xx.byte1,   byte_test_status[1] ? "" : "(e)",
					  dfm_wlevel_rank.cn63xx.byte0,   byte_test_status[0] ? "" : "(e)"
					  );
		}
    }


    /*
     * 4.8.8 Final LMC Initialization
     *
     * Early LMC initialization, LMC write-leveling, and LMC
     * read-leveling must be completed prior to starting this
     * final LMC initialization.
     *
     * LMC hardware updates the LMC0_SLOT_CTL0, LMC0_SLOT_CTL1,
     * LMC0_SLOT_CTL2 CSRs with minimum values based on the
     * selected read-leveling and write-leveling
     * settings. Software should not write the final
     * LMC0_SLOT_CTL0, LMC0_SLOT_CTL1, and LMC0_SLOT_CTL2 values
     * until after the final readleveling and write-leveling
     * settings are written.
     *
     * Software must ensure the LMC0_SLOT_CTL0, LMC0_SLOT_CTL1,
     * and LMC0_SLOT_CTL2 CSR values are appropriate for this
     * step. These CSRs select the minimum gaps between reads and
     * writes of various types.
     *
     * Software must not reduce the values in these CSR fields
     * below the values previously selected by the LMC hardware
     * (during write-leveling and read-leveling steps above).
     *
     * All sections in this chapter may be used to derive proper
     * settings for these registers.
     */

    if (safe_dfm_flag) {
        cvmx_dfm_slot_ctl0_t dfm_slot_ctl0;
        cvmx_dfm_slot_ctl1_t dfm_slot_ctl1;

        dfm_slot_ctl0.u64 = 0;
        dfm_slot_ctl0.cn63xx.r2r_init = 0x3f;
        dfm_slot_ctl0.cn63xx.r2w_init = 0x3f;
        dfm_slot_ctl0.cn63xx.w2r_init = 0x3f;
        dfm_slot_ctl0.cn63xx.w2w_init = 0x3f;
        dfm_config_write_csr(CVMX_DFM_SLOT_CTL0, dfm_slot_ctl0.u64);

        dfm_slot_ctl1.u64 = 0;
        dfm_slot_ctl1.cn63xx.r2r_xrank_init = 0x3f;
        dfm_slot_ctl1.cn63xx.r2w_xrank_init = 0x3f;
        dfm_slot_ctl1.cn63xx.w2r_xrank_init = 0x3f;
        dfm_slot_ctl1.cn63xx.w2w_xrank_init = 0x3f;
        dfm_config_write_csr(CVMX_DFM_SLOT_CTL1, dfm_slot_ctl1.u64);
    }

    cvmx_dfm_config_t dfm_config;
    dfm_config.u64 = cvmx_read_csr(CVMX_DFM_CONFIG);

    dfm_print("DFM interface initialized %d MB\n", mem_size_mbytes);
    return(mem_size_mbytes);
}

static int64_t _abs(int64_t v)
{
    return ((v < 0) ? -v : v);
}

static void set_dfm_clock_initialized(int inited_flag)
{
    DECLARE_GLOBAL_DATA_PTR;
    gd->flags |= GD_FLG_DFM_CLK_INITIALIZED;
}

static int dfm_clock_initialized(void)
{
    DECLARE_GLOBAL_DATA_PTR;
    return(gd->flags & GD_FLG_DFM_CLK_INITIALIZED);
}

int initialize_dfm_clock(uint32_t cpu_id)
{
#ifdef __U_BOOT__
    {
        DECLARE_GLOBAL_DATA_PTR;
        if (getenv("dfm_verbose") != NULL)
        {
            gd->flags |= GD_FLG_DFM_VERBOSE;
        }
        if (getenv("dfm_trace_init") != NULL)
        {
            gd->flags |= GD_FLG_DFM_TRACE_INIT;
            gd->flags |= GD_FLG_DFM_VERBOSE;
        }

        if (gd->flags & GD_FLG_FAILSAFE_MODE)
            gd->flags |= GD_FLG_DFM_VERBOSE;
    }
#endif
    if (dfm_clock_initialized())
        return 0;

    if (octeon_is_cpuid(OCTEON_CN63XX)) {
        cvmx_lmcx_ddr_pll_ctl_t ddr_pll_ctl;
        cvmx_dfm_dll_ctl2_t dll_ctl2;
        cvmx_dfm_reset_ctl_t dfm_reset_ctl;


        {
            uint64_t clkf;
            uint64_t pll_MHz;
            uint64_t calculated_dfm_hertz;
            unsigned en;
            static const unsigned _en[] = {1, 2, 3, 4, 6, 8, 12};

#if 1 /* Temporary until Cavium adds code to set diff clock freqs for ddr and dfm */
            uint32_t ddr_hertz = 533000000;
            unsigned en_idx, save_en_idx, best_en_idx=0;
            uint64_t min_clkf = 32, max_clkf = 64;
            uint64_t best_clkf=0;
            uint64_t best_pll_MHz = 0, highest_pll_MHz = 0;
            uint64_t min_pll_MHz = 1600, max_pll_MHz = 3000;
            int64_t  error = ddr_hertz, best_error = ddr_hertz; /* Init to max error */
            uint64_t best_calculated_ddr_hertz = 0, calculated_ddr_hertz = 0;
            uint64_t orig_ddr_hertz = ddr_hertz;

            ddr_pll_ctl.u64 = cvmx_read_csr(CVMX_LMCX_DDR_PLL_CTL(0));
            clkf = ddr_pll_ctl.cn63xx.clkf;
            pll_MHz = clkf * 50;            while (best_error == ddr_hertz)
            {
                for (en_idx=0; en_idx < sizeof(_en)/sizeof(int); ++en_idx) {
                    save_en_idx = en_idx;
                    pll_MHz = clkf * 50;
                    calculated_ddr_hertz = (pll_MHz * 1000000) / _en[save_en_idx];
                    error = ddr_hertz - calculated_ddr_hertz;

#if defined(__U_BOOT__)
                    debug_print("en: %2d, clkf: %4lu, pll_MHz: %4lu, ddr_hertz: %9lu, error: %10d, best_error: %8d\n",
                              _en[save_en_idx], clkf, pll_MHz, calculated_ddr_hertz, error, best_error);
#else
                    debug_print("en: %2d, clkf: %4llu, pll_MHz: %4llu, ddr_hertz: %9llu, error: %10lld, best_error: %8d\n",
                              _en[save_en_idx], clkf, pll_MHz, calculated_ddr_hertz, error, best_error);
#endif
                    if ((pll_MHz < min_pll_MHz) || (pll_MHz > max_pll_MHz)) continue;
                    if ((clkf < min_clkf) || (clkf > max_clkf)) continue;
                    if (_abs(error) > _abs(best_error)) continue;

                    if ((_abs(error) < _abs(best_error)) || (pll_MHz > highest_pll_MHz)) {
                        highest_pll_MHz = pll_MHz;
                        best_pll_MHz = pll_MHz;
                        best_calculated_ddr_hertz = calculated_ddr_hertz;
                        best_error = error;
                        best_clkf = clkf;
                        best_en_idx = save_en_idx;
                    }
                }
#if defined(__U_BOOT__)
                debug_print("en: %2d, clkf: %4lu, pll_MHz: %4lu, ddr_hertz: %8lu, error: %10d <==\n",
                          _en[best_en_idx], best_clkf, best_pll_MHz, best_calculated_ddr_hertz, best_error);
#else
                debug_print("en: %2d, clkf: %4llu, pll_MHz: %4llu, ddr_hertz: %8llu, error: %10lld <==\n",
                          _en[best_en_idx], best_clkf, best_pll_MHz, best_calculated_ddr_hertz, best_error);
#endif
                /* Try lowering the frequency if we can't get a working configuration */
                if (best_error == ddr_hertz) {
                    if (ddr_hertz < orig_ddr_hertz - 10000000)
                        break;
                    ddr_hertz -= 1000000;
                    best_error = ddr_hertz;
                }
            }
            if (best_error == ddr_hertz) {
                error_print("ERROR: Can not compute a legal DDR clock speed configuration.\n");
                return(-1);
            }

            ddr_pll_ctl.cn63xx.dfm_ps_en = best_en_idx;
            dfm_config_write_csr(CVMX_LMCX_DDR_PLL_CTL(0), ddr_pll_ctl.u64);
#endif

            ddr_pll_ctl.u64 = cvmx_read_csr(CVMX_LMCX_DDR_PLL_CTL(0));

            en = ddr_pll_ctl.cn63xx.dfm_ps_en;
            clkf = ddr_pll_ctl.cn63xx.clkf;
            pll_MHz = clkf * 50;
            calculated_dfm_hertz = (pll_MHz * 1000000) / _en[en];

#if defined(__U_BOOT__)
            dfm_print("en: %2d, clkf: %4lu, pll_MHz: %4lu, dfm_hertz: %8lu\n",
                      _en[en], clkf, pll_MHz, calculated_dfm_hertz);
#else
            dfm_print("en: %2d, clkf: %4llu, pll_MHz: %4llu, dfm_hertz: %8llu\n",
                      _en[en], clkf, pll_MHz, calculated_dfm_hertz);
#endif
        }


        /*
         * 4.8.2 LMC CK Initialization
         *
         * DDR PLL initialization must be completed prior to starting
         * LMC CK initialization.
         *
         * 1. Without changing any other LMC0_DDR_PLL_CTL values,
         *    write LMC0_DDR_PLL_CTL[DDR_DIV_RESET] = 1 and
         *    LMC0_DDR_PLL_CTL [DDR_PS_EN] with the appropriate value
         *    to get the desired LMC CK speed. Section Section 4.13
         *    discusses CLKF and DDR_PS_EN programmings.
         *
         *    The LMC0_DDR_PLL_CTL[DDR_PS_EN] must not change after
         *    this point without restarting this LMC CK initialization
         *    sequence.
         */

        ddr_pll_ctl.u64 = cvmx_read_csr(CVMX_LMCX_DDR_PLL_CTL(0));
        ddr_pll_ctl.cn63xx.dfm_div_reset = 1;
        dfm_config_write_csr(CVMX_LMCX_DDR_PLL_CTL(0), ddr_pll_ctl.u64);


        /*
         * 2. Without changing any other LMC0_COMP_CTL2 values, write
         *    LMC0_COMP_CTL2[CK_CTL] to the desired DDR_CK_*_P/
         *    DDR_DIMM*_CS*_L/DDR_DIMM*_ODT_* drive strength.
         */

        {
            cvmx_dfm_comp_ctl2_t comp_ctl2;
            comp_ctl2.u64 = cvmx_read_csr(CVMX_DFM_COMP_CTL2);
            comp_ctl2.cn63xx.ck_ctl = 6; /* 0110 = 48 ohm */
            dfm_config_write_csr(CVMX_DFM_COMP_CTL2, comp_ctl2.u64);
        }

        /*
         * 3. Read LMC0_DDR_PLL_CTL and wait for the result.
         */

        cvmx_read_csr(CVMX_LMCX_DDR_PLL_CTL(0));

        /*
         * 4. Wait 200 ns.
         */

        cvmx_wait_usec(200000);          /* Wait 200 ns */

        /*
         * 5. Without changing any other LMC0_DDR_PLL_CTL values,
         *    write LMC0_DDR_PLL_CTL[DDR_DIV_RESET] = 0.
         */

        ddr_pll_ctl.cn63xx.dfm_div_reset = 0;

        dfm_config_write_csr(CVMX_LMCX_DDR_PLL_CTL(0), ddr_pll_ctl.u64);

        /*
         * 6. Read LMC0_DDR_PLL_CTL and wait for the result.
         */

        cvmx_read_csr(CVMX_LMCX_DDR_PLL_CTL(0));

        /*
         * 7. Wait 200 ns.
         */

        cvmx_wait_usec(1);      /* Wait 200 ns */

        /*
         * 4.8.3 LMC DRESET Initialization
         *
         * Both DDR PLL and LMC CK initializations must be completed
         * prior to starting this LMC DRESET initialization.
         *
         * 1. If not done already, write LMC0_DLL_CTL2 to its reset
         *    value, including LMC0_DLL_CTL2[DRESET] = 1.
         */

        /* Already done. */

        /*
         * 2. Without changing any other LMC0_DLL_CTL2 fields, write
         *    LMC0_DLL_CTL2 [DLL_BRINGUP] = 1.
         */

        dll_ctl2.u64 = cvmx_read_csr(CVMX_DFM_DLL_CTL2);
        dll_ctl2.cn63xx.dll_bringup = 1;
        dfm_config_write_csr(CVMX_DFM_DLL_CTL2, dll_ctl2.u64);

        /*
         * 3. Read LMC0_DLL_CTL2 and wait for the result.
         */

        cvmx_read_csr(CVMX_LMCX_DDR_PLL_CTL(0));

        /*
         * 4. Wait for 10 LMC CK cycles.
         */

        cvmx_wait_usec(1);

        /*
         * 5. Without changing any other fields in LMC0_DLL_CTL2, write
         *
         *    LMC0_DLL_CTL2[QUAD_DLL_ENA] = 1.
         *
         *    LMC0_DLL_CTL2[QUAD_DLL_ENA] must not change after this
         *    point without restarting the LMC DRESET initialization
         *    sequence.
         */

        dll_ctl2.u64 = cvmx_read_csr(CVMX_DFM_DLL_CTL2);
        dll_ctl2.cn63xx.quad_dll_ena = 1;
        dfm_config_write_csr(CVMX_DFM_DLL_CTL2, dll_ctl2.u64);

        /*
         * 6. Read LMC0_DLL_CTL2 and wait for the result.
         */

        cvmx_read_csr(CVMX_LMCX_DDR_PLL_CTL(0));

        /*
         * 7. Wait 10 usec.
         */

        cvmx_wait_usec(10);

        /*
         * 8. Without changing any other fields in LMC0_DLL_CTL2, write
         *
         *    LMC0_DLL_CTL2[DLL_BRINGUP] = 0.
         *
         *    LMC0_DLL_CTL2[DLL_BRINGUP] must not change after this
         *    point without restarting the LMC DRESET initialization
         *    sequence.
         */

        dll_ctl2.u64 = cvmx_read_csr(CVMX_DFM_DLL_CTL2);
        dll_ctl2.cn63xx.dll_bringup = 0;
        dfm_config_write_csr(CVMX_DFM_DLL_CTL2, dll_ctl2.u64);

        /*
         * 9. Read LMC0_DLL_CTL2 and wait for the result.
         */

        cvmx_read_csr(CVMX_LMCX_DDR_PLL_CTL(0));

        /*
         * 10. Without changing any other fields in LMC0_DLL_CTL2, write
         *
         *     LMC0_DLL_CTL2[DRESET] = 0.
         *
         *     LMC0_DLL_CTL2[DRESET] must not change after this point
         *     without restarting the LMC DRESET initialization
         *     sequence.
         *
         *     After completing LMC DRESET initialization, all LMC
         *     CSRs may be accessed. Prior to completing LMC DRESET
         *     initialization, only LMC0_DDR_PLL_CTL, LMC0_DLL_CTL2,
         *     LMC0_RESET_CTL, and LMC0_COMP_CTL2 LMC CSRs can be
         *     accessed.
         */

        dll_ctl2.u64 = cvmx_read_csr(CVMX_DFM_DLL_CTL2);
        dll_ctl2.cn63xx.dreset = 0;
        dfm_config_write_csr(CVMX_DFM_DLL_CTL2, dll_ctl2.u64);

        /*
         * 4.8.4 LMC RESET Initialization
         *
         * The purpose of this step is to assert/deassert the RESET#
         * pin at the DDR3 parts.
         *
         * It may be appropriate to skip this step if the DDR3 DRAM
         * parts are in self refresh and are currently preserving
         * their contents. (Software can determine this via
         * LMC0_RESET_CTL[DDR3PSV] in some circumstances.)  The
         * remainder of this section assumes that the DRAM contents
         * need not be preserved.
         *
         * The remainder of this section assumes that the CN63XX
         * DDR_RESET_L pin is attached to the RESET# pin of the
         * attached DDR3 parts, as will be appropriate in many
         * systems.
         *
         * (In other systems, such as ones that can preserve DDR3 part
         * contents while CN63XX is powered down, it will not be
         * appropriate to directly attach the CN63XX DDR_RESET_L pin
         * to DRESET# of the DDR3 parts, and this section may not
         * apply.)
         *
         * DRAM Controller (LMC): LMC Initialization Sequence
         *
         * 1. If not done already, assert DDR_RESET_L pin by writing
         *    LMC0_RESET_CTL[DDR3RST] = 0 without modifying any other
         *    LMC0_RESET_CTL fields.
         */

        dfm_reset_ctl.u64 = cvmx_read_csr(CVMX_DFM_RESET_CTL);
        dfm_reset_ctl.cn63xx.ddr3rst = 0;
        dfm_config_write_csr(CVMX_DFM_RESET_CTL, dfm_reset_ctl.u64);

        /*
         * 2. Read LMC0_RESET_CTL and wait for the result.
         */

        cvmx_read_csr(CVMX_DFM_RESET_CTL);

        /*
         * 3. Wait until RESET# assertion time requirement from JEDEC
         *    DDR3 specification is satisfied (200 usec during a
         *    power-on ramp, 100ns when power is already stable).
         */

        cvmx_wait_usec(200);

        /*
         * 4. Deassert DDR_RESET_L pin by writing
         *    LMC0_RESET_CTL[DDR3RST] = 1 without modifying any other
         *    LMC0_RESET_CTL fields.
         */

        dfm_reset_ctl.u64 = cvmx_read_csr(CVMX_DFM_RESET_CTL);
        dfm_reset_ctl.cn63xx.ddr3rst = 1;
        dfm_config_write_csr(CVMX_DFM_RESET_CTL, dfm_reset_ctl.u64);

        /*
         * 5. Read LMC0_RESET_CTL and wait for the result.
         */

        cvmx_read_csr(CVMX_DFM_RESET_CTL);

        /*
         * (Subsequent RESET# deassertion to CKE assertion requirement
         * can be satisfied during LMC initialization via
         * LMC0_TIMING_PARAMS0[TCKEON] timing.)
         */

    } else {
        error_print("Unsupported CPU type\n");
    }

    set_dfm_clock_initialized(1);
    return(0);
}

static void octeon_ipd_delay_cycles(uint64_t cycles)
{
    uint64_t start = cvmx_read_csr(CVMX_IPD_CLK_COUNT);
    while (start + cycles > cvmx_read_csr(CVMX_IPD_CLK_COUNT))
        ;
}

#define CALC_SCALER 10000

uint32_t measure_octeon_dfm_clock(uint32_t cpu_id,
                                  uint32_t cpu_hertz
                                  )
{
    uint32_t core_clocks = 0;
    uint32_t ddr_clocks = 0;
    uint32_t calc_dfm_hertz;

    if (initialize_dfm_clock(cpu_id) != 0)
        return (0);

    /* Dynamically determine the DDR clock speed */
    if (octeon_is_cpuid(OCTEON_CN63XX)) {
        core_clocks = cvmx_read_csr(CVMX_IPD_CLK_COUNT);
        ddr_clocks = cvmx_read_csr(CVMX_DFM_FCLK_CNT);
        octeon_ipd_delay_cycles(100000000); /* How many cpu cycles to measure over */
        core_clocks = cvmx_read_csr(CVMX_IPD_CLK_COUNT) - core_clocks;
        ddr_clocks = cvmx_read_csr(CVMX_DFM_FCLK_CNT) - ddr_clocks;
    } else {
        error_print("Unsupported CPU type\n");
    }

    calc_dfm_hertz = ((ddr_clocks/CALC_SCALER) * (cpu_hertz/CALC_SCALER) / (core_clocks/CALC_SCALER)) * CALC_SCALER;
    dfm_print("Measured DFM clock %d\n", calc_dfm_hertz);

#ifdef CVMX_BUILD_FOR_UBOOT
    /* Check for unreasonable settings. */
    if (calc_dfm_hertz < 10000) {
#ifdef CONFIG_OCTEON_EBB6300
        int twsii_mcu_read(uint8_t twsii_addr);
        error_print("DFM clock misconfigured. Asking MCU to power cycle Octeon.\n");
        twsii_mcu_read(8);  /* MCU from 5/10 required */
#endif
        error_print("DFM clock misconfigured. Hanging.\n");
        while (1)
            ;
    }
#endif
    return (calc_dfm_hertz);
}
