packages/hal/arm/mx53/var/v2_0/src/cmds.c

   1 //==========================================================================
   2 //
   3 //      cmds.c
   4 //
   5 //      SoC [platform] specific RedBoot commands
   6 //
   7 //==========================================================================
   8 //####ECOSGPLCOPYRIGHTBEGIN####
   9 // -------------------------------------------
  10 // This file is part of eCos, the Embedded Configurable Operating System.
  11 // Copyright (C) 1998, 1999, 2000, 2001, 2002 Red Hat, Inc.
  12 //
  13 // eCos is free software; you can redistribute it and/or modify it under
  14 // the terms of the GNU General Public License as published by the Free
  15 // Software Foundation; either version 2 or (at your option) any later version.
  16 //
  17 // eCos is distributed in the hope that it will be useful, but WITHOUT ANY
  18 // WARRANTY; without even the implied warranty of MERCHANTABILITY or
  19 // FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  20 // for more details.
  21 //
  22 // You should have received a copy of the GNU General Public License along
  23 // with eCos; if not, write to the Free Software Foundation, Inc.,
  24 // 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  25 //
  26 // As a special exception, if other files instantiate templates or use macros
  27 // or inline functions from this file, or you compile this file and link it
  28 // with other works to produce a work based on this file, this file does not
  29 // by itself cause the resulting work to be covered by the GNU General Public
  30 // License. However the source code for this file must still be made available
  31 // in accordance with section (3) of the GNU General Public License.
  32 //
  33 // This exception does not invalidate any other reasons why a work based on
  34 // this file might be covered by the GNU General Public License.
  35 //
  36 // Alternative licenses for eCos may be arranged by contacting Red Hat, Inc.
  37 // at http://sources.redhat.com/ecos/ecos-license/
  38 // -------------------------------------------
  39 //####ECOSGPLCOPYRIGHTEND####
  40 //==========================================================================
  41 #include <redboot.h>
  42 #include <cyg/hal/hal_intr.h>
  43 #include <cyg/hal/plf_mmap.h>
  44 #include <cyg/hal/hal_soc.h>             // Hardware definitions
  45 #include <cyg/hal/hal_cache.h>
  46 #include CYGBLD_HAL_PLF_DEFS_H
  47
  48 #include "hab_super_root.h"
  49
  50 #ifndef FUSE_PROG_START
  51 #define FUSE_PROG_START()               CYG_EMPTY_STATEMENT
  52 #define FUSE_PROG_DONE()                CYG_EMPTY_STATEMENT
  53 #endif
  54
  55 //#define IIM_FUSE_DEBUG
  56 //#define CMD_CLOCK_DEBUG
  57 #ifdef CMD_CLOCK_DEBUG
  58 static int dbg_enable;
  59
  60 #define enable_dbg()            \
  61   CYG_MACRO_START                       \
  62   dbg_enable = 1;                       \
  63   CYG_MACRO_END
  64
  65 #define dbg(fmt...)                                                             \
  66   CYG_MACRO_START                                                               \
  67   if (dbg_enable)                                                               \
  68           diag_printf(fmt);                                                     \
  69   CYG_MACRO_END
  70 #else
  71 #define dbg_enable 0
  72 #define enable_dbg()            CYG_EMPTY_STATEMENT
  73 #define dbg(fmt...)             CYG_EMPTY_STATEMENT
  74 #endif
  75
  76 static int gcd(int m, int n);
  77
  78 typedef unsigned long long      u64;
  79 typedef unsigned int            u32;
  80 typedef unsigned short          u16;
  81 typedef unsigned char           u8;
  82
  83 #define SZ_DEC_1M               1000000
  84 #define PLL_PD_MAX              16              //actual pd+1
  85 #define PLL_MFI_MAX             15
  86 #define PLL_MFI_MIN             5
  87 #define ARM_DIV_MAX             8
  88 #define IPG_DIV_MAX             4
  89 #define AHB_DIV_MAX             8
  90 #define EMI_DIV_MAX             8
  91 #define NFC_DIV_MAX             8
  92
  93 struct pll_param {
  94         u32 pd;
  95         u32 mfi;
  96         u32 mfn;
  97         u32 mfd;
  98 };
  99
 100 #define PLL_FREQ_MAX(_ref_clk_)    (4 * _ref_clk_ * PLL_MFI_MAX)
 101 #define PLL_FREQ_MIN(_ref_clk_)    ((2 * _ref_clk_ * (PLL_MFI_MIN - 1)) / PLL_PD_MAX)
 102 #define MAX_DDR_CLK             400000000
 103 #define AHB_CLK_MAX             133333333
 104 #define IPG_CLK_MAX             (AHB_CLK_MAX / 2)
 105 #define NFC_CLK_MAX             25000000
 106 // IPU-HSP clock is independent of the HCLK and can go up to 177MHz but requires
 107 // higher voltage support. For simplicity, limit it to 133MHz
 108 #define HSP_CLK_MAX             133333333
 109
 110 #define ERR_WRONG_CLK   (-1)
 111 #define ERR_NO_MFI              (-2)
 112 #define ERR_NO_MFN              (-3)
 113 #define ERR_NO_PD               (-4)
 114 #define ERR_NO_PRESC    (-5)
 115 #define ERR_NO_AHB_DIV  (-6)
 116
 117 u32 pll_clock(enum plls pll);
 118 u32 get_main_clock(enum main_clocks clk);
 119 u32 get_peri_clock(enum peri_clocks clk);
 120
 121 static volatile u32 *pll_base[] =
 122 {
 123         &REG32(PLL1_BASE_ADDR),
 124         &REG32(PLL2_BASE_ADDR),
 125         &REG32(PLL3_BASE_ADDR),
 126         &REG32(PLL4_BASE_ADDR),
 127 };
 128
 129 static void clock_setup(int argc, char *argv[]);
 130
 131 RedBoot_cmd("clock",
 132                         "Setup/Display clock\nSyntax:",
 133                         "[<core clock in MHz> :<DDR clock in MHz>]\n\n"
 134                         "   Examples:\n"
 135                         "   [clock]         -> Show various clocks\n"
 136                         "   [clock 665]     -> Core=665\n"
 137                         "   [clock 800:133] -> Core=800 DDR=133\n"
 138                         "   [clock :166]    -> Core=no change DDR=166\n",
 139                         clock_setup
 140         );
 141
 142 /*!
 143  * This is to calculate various parameters based on reference clock and
 144  * targeted clock based on the equation:
 145  *      t_clk = 2*ref_freq*(mfi + mfn/(mfd+1))/(pd+1)
 146  * This calculation is based on a fixed MFD value for simplicity.
 147  *
 148  * @param ref       reference clock freq in Hz
 149  * @param target    targeted clock in Hz
 150  * @param p_pd      calculated pd value (pd value from register + 1) upon return
 151  * @param p_mfi     calculated actual mfi value upon return
 152  * @param p_mfn     calculated actual mfn value upon return
 153  * @param p_mfd     fixed mfd value (mfd value from register + 1) upon return
 154  *
 155  * @return          0 if successful; non-zero otherwise.
 156  */
 157 int calc_pll_params(u32 ref, u32 target, struct pll_param *pll)
 158 {
 159         int pd, mfi = 1, mfn, mfd, i;
 160         u64 n_target = target, n_ref = ref;
 161
 162         // make sure targeted freq is in the valid range. Otherwise the
 163         // following calculation might be wrong!!!
 164         if (n_target < PLL_FREQ_MIN(ref) || n_target > PLL_FREQ_MAX(ref))
 165                 return ERR_WRONG_CLK;
 166
 167         mfd = 24 * 16;
 168
 169         // Use n_target and n_ref to avoid overflow
 170         for (pd = 1; pd <= PLL_PD_MAX; pd++) {
 171                 mfi = (n_target * pd) / (4 * n_ref);
 172                 if (mfi > PLL_MFI_MAX) {
 173                         return ERR_NO_MFI;
 174                 } else if (mfi < 5) {
 175                         continue;
 176                 }
 177                 break;
 178         }
 179         // Now got pd and mfi already
 180         mfn = (((n_target * pd) / 4 - n_ref * mfi) * mfd) / n_ref;
 181
 182         dbg("%d: ref=%d, target=%d, pd=%d, mfi=%d,mfn=%d, mfd=%d\n",
 183                 __LINE__, ref, target, pd, mfi, mfn, mfd);
 184
 185         i = 1;
 186         if (mfn != 0)
 187                 i = gcd(mfd, mfn);
 188         pll->pd = pd;
 189         pll->mfi = mfi;
 190         pll->mfn = mfn / i;
 191         pll->mfd = mfd / i;
 192         return 0;
 193 }
 194
 195 /*!
 196  * This function returns the low power audio clock.
 197  */
 198 u32 get_lp_apm(void)
 199 {
 200         u32 ret_val;
 201         u32 ccsr = readl(CCM_BASE_ADDR + CLKCTL_CCSR);
 202
 203         if (((ccsr >> 9) & 1) == 0) {
 204                 ret_val = FREQ_24MHZ;
 205         } else {
 206                 ret_val = FREQ_32768HZ;
 207         }
 208         dbg("%s: CCSR[%08lx]=%08x freq=%u.%03uMHz\n", __FUNCTION__,
 209                 CCM_BASE_ADDR + CLKCTL_CCSR, ccsr, ret_val / 1000000, ret_val / 1000 % 1000);
 210         return ret_val;
 211 }
 212
 213 /*!
 214  * This function returns the periph_clk.
 215  */
 216 u32 get_periph_clk(void)
 217 {
 218         u32 ret_val, clk_sel;
 219
 220         u32 cbcdr = readl(CCM_BASE_ADDR + CLKCTL_CBCDR);
 221         u32 cbcmr = readl(CCM_BASE_ADDR + CLKCTL_CBCMR);
 222
 223         if (!(cbcdr & (1 << 25))) {
 224                 ret_val = pll_clock(PLL2);
 225                 dbg("%s: CBCDR[%08lx]=%08x CBCMR[%08lx]=%08x freq=%u.%03uMHz\n", __FUNCTION__,
 226                         CCM_BASE_ADDR + CLKCTL_CBCDR, cbcdr,
 227                         CCM_BASE_ADDR + CLKCTL_CBCMR, cbcmr,
 228                         ret_val / 1000000, ret_val / 1000 % 1000);
 229         } else {
 230                 clk_sel = (cbcmr >> 12) & 3;
 231                 if (clk_sel == 0) {
 232                         ret_val = pll_clock(PLL1);
 233                 } else if (clk_sel == 1) {
 234                         ret_val = pll_clock(PLL3);
 235                 } else if (clk_sel == 2) {
 236                         ret_val = get_lp_apm();
 237                 } else {
 238                         diag_printf("Invalid CBCMR[CLK_SEL]: %d\n", clk_sel);
 239                         return ERR_WRONG_CLK;
 240                 }
 241                 dbg("%s: CBCDR[%08lx]=%08x CBCMR[%08lx]=%08x clk_sel=%d freq=%u.%03uMHz\n", __FUNCTION__,
 242                         CCM_BASE_ADDR + CLKCTL_CBCDR, cbcdr,
 243                         CCM_BASE_ADDR + CLKCTL_CBCMR, cbcmr,
 244                         clk_sel, ret_val / 1000000, ret_val / 1000 % 1000);
 245         }
 246         return ret_val;
 247 }
 248
 249 /*!
 250  * This function assumes the expected core clock has to be changed by
 251  * modifying the PLL. This is NOT true always but for most of the times,
 252  * it is. So it assumes the PLL output freq is the same as the expected
 253  * core clock (presc=1) unless the core clock is less than PLL_FREQ_MIN.
 254  * In the latter case, it will try to increase the presc value until
 255  * (presc*core_clk) is greater than PLL_FREQ_MIN. It then makes call to
 256  * calc_pll_params() and obtains the values of PD, MFI,MFN, MFD based
 257  * on the targeted PLL and reference input clock to the PLL. Lastly,
 258  * it sets the register based on these values along with the dividers.
 259  * Note 1) There is no value checking for the passed-in divider values
 260  *         so the caller has to make sure those values are sensible.
 261  *      2) Also adjust the NFC divider such that the NFC clock doesn't
 262  *         exceed NFC_CLK_MAX.
 263  *      3) IPU HSP clock is independent of AHB clock. Even it can go up to
 264  *         177MHz for higher voltage, this function fixes the max to 133MHz.
 265  *      4) This function should not have allowed diag_printf() calls since
 266  *         the serial driver has been stopped. But leave then here to allow
 267  *         easy debugging by NOT calling the cyg_hal_plf_serial_stop().
 268  *
 269  * @param ref       pll input reference clock (24MHz)
 270  * @param core_clk  core clock in Hz
 271  * @param emi_clk   emi clock in Hz
 272  # @return          0 if successful; non-zero otherwise
 273  */
 274 int configure_clock(u32 ref, u32 core_clk, u32 emi_clk)
 275 {
 276         u32 pll, clk_src;
 277         struct pll_param pll_param;
 278         int ret, clk_sel, div = 1, div_core = 1, div_per = 1, shift = 0;
 279         u32 cbcdr = readl(CCM_BASE_ADDR + CLKCTL_CBCDR);
 280         u32 cbcmr = readl(CCM_BASE_ADDR + CLKCTL_CBCMR);
 281         u32 ccsr = readl(CCM_BASE_ADDR + CLKCTL_CCSR);
 282         u32 icgc = readl(PLATFORM_BASE_ADDR + PLATFORM_ICGC);
 283
 284         dbg("%s: cbcdr[%08lx]=%08x\n", __FUNCTION__,
 285                 CCM_BASE_ADDR + CLKCTL_CBCDR, cbcdr);
 286         dbg("%s: cbcmr[%08lx]=%08x\n", __FUNCTION__,
 287                 CCM_BASE_ADDR + CLKCTL_CBCMR, cbcdr);
 288         dbg("%s: ccsr[%08lx]=%08x\n", __FUNCTION__,
 289                 CCM_BASE_ADDR + CLKCTL_CCSR, cbcdr);
 290         dbg("%s: icgc[%08lx]=%08x\n", __FUNCTION__,
 291                 PLATFORM_BASE_ADDR + PLATFORM_ICGC, icgc);
 292
 293         if (core_clk != 0) {
 294                 // assume pll default to core clock first
 295                 pll = core_clk;
 296                 if ((ret = calc_pll_params(ref, pll, &pll_param)) != 0) {
 297                         diag_printf("can't find pll parameters: %d\n", ret);
 298                         return ret;
 299                 }
 300
 301                 dbg("%s: ref=%d, pll=%d, pd=%d, mfi=%d,mfn=%d, mfd=%d\n", __FUNCTION__,
 302                         ref, pll, pll_param.pd, pll_param.mfi, pll_param.mfn, pll_param.mfd);
 303
 304                 /* Applies for TO 2 only */
 305                 if (((cbcdr >> 30) & 0x1) == 0x1) {
 306                         /* Disable IPU and HSC dividers */
 307                         writel(0x60000, CCM_BASE_ADDR + CLKCTL_CCDR);
 308                         /* Switch DDR to different source */
 309                         writel(cbcdr & ~0x40000000, CCM_BASE_ADDR + CLKCTL_CBCDR);
 310                         while (readl(CCM_BASE_ADDR + CLKCTL_CDHIPR) != 0);
 311                         writel(0x0, CCM_BASE_ADDR + CLKCTL_CCDR);
 312                 }
 313
 314                 /* Switch ARM to PLL2 clock */
 315                 writel(ccsr | 0x4, CCM_BASE_ADDR + CLKCTL_CCSR);
 316
 317                 if ((core_clk > 665000000) && (core_clk <= 800000000)) {
 318                         div_per = 5;
 319                 } else if (core_clk > 800000000) {
 320                         div_per = 6;
 321                 } else {
 322                         div_per = 4;
 323                 }
 324
 325                 if (core_clk > 800000000) {
 326                         div_core = 3;
 327                 } else {
 328                         div_core = 2;
 329                 }
 330                 ret = adjust_core_voltage(core_clk / 1000000);
 331                 if (ret) {
 332                         diag_printf("Failed to adjust core voltage for %u MHz\n",
 333                                                 core_clk / 1000000);
 334                         return ret;
 335                 }
 336                 cyg_hal_plf_serial_stop();
 337
 338                 // adjust pll settings
 339                 writel(((pll_param.pd - 1) << 0) | (pll_param.mfi << 4),
 340                         PLL1_BASE_ADDR + PLL_DP_OP);
 341                 writel(pll_param.mfn, PLL1_BASE_ADDR + PLL_DP_MFN);
 342                 writel(pll_param.mfd - 1, PLL1_BASE_ADDR + PLL_DP_MFD);
 343                 writel(((pll_param.pd - 1) << 0) | (pll_param.mfi << 4),
 344                         PLL1_BASE_ADDR + PLL_DP_HFS_OP);
 345                 writel(pll_param.mfn, PLL1_BASE_ADDR + PLL_DP_HFS_MFN);
 346                 writel(pll_param.mfd - 1, PLL1_BASE_ADDR + PLL_DP_HFS_MFD);
 347
 348                 icgc &= ~0x77;
 349                 icgc |= div_core << 4;
 350                 icgc |= div_per;
 351                 /* Set the platform clock dividers */
 352                 writel(icgc, PLATFORM_BASE_ADDR + PLATFORM_ICGC);
 353                 /* Switch ARM back to PLL1 */
 354                 writel((ccsr & ~0x4), CCM_BASE_ADDR + CLKCTL_CCSR);
 355                 /* Applies for TO 2 only */
 356                 if (((cbcdr >> 30) & 0x1) == 0x1) {
 357                         /* Disable IPU and HSC dividers */
 358                         writel(0x60000, CCM_BASE_ADDR + CLKCTL_CCDR);
 359                         /* Switch DDR back to PLL1 */
 360                         writel(cbcdr | 0x40000000, CCM_BASE_ADDR + CLKCTL_CBCDR);
 361                         while (readl(CCM_BASE_ADDR + CLKCTL_CDHIPR) != 0);
 362                         writel(0x0, CCM_BASE_ADDR + CLKCTL_CCDR);
 363                         if (emi_clk == 0) {
 364                                 /* Keep EMI clock to the max if not specified */
 365                                 emi_clk = 200000000;
 366                         }
 367                 }
 368                 cyg_hal_plf_serial_init();
 369         }
 370
 371         if (emi_clk != 0) {
 372                 /* Applies for TO 2 only */
 373                 if (((cbcdr >> 30) & 0x1) == 0x1) {
 374                         clk_src = pll_clock(PLL1);
 375                         shift = 27;
 376                 } else {
 377                         clk_src = get_periph_clk();
 378                         /* Find DDR clock input */
 379                         clk_sel = (cbcmr >> 10) & 0x3;
 380                         if (clk_sel == 0) {
 381                                 shift = 16;
 382                         } else if (clk_sel == 1) {
 383                                 shift = 19;
 384                         } else if (clk_sel == 2) {
 385                                 shift = 22;
 386                         } else if (clk_sel == 3) {
 387                                 shift = 10;
 388                         }
 389                 }
 390                 if ((clk_src % emi_clk) == 0)
 391                         div = clk_src / emi_clk;
 392                 else
 393                         div = (clk_src / emi_clk) + 1;
 394                 if (div > 8)
 395                         div = 8;
 396
 397                 cbcdr &= ~(0x7 << shift);
 398                 cbcdr |= (div - 1) << shift;
 399
 400                 dbg("%s@%d: \n", __FUNCTION__, __LINE__);
 401
 402                 /* Disable IPU and HSC dividers */
 403                 writel(0x60000, CCM_BASE_ADDR + CLKCTL_CCDR);
 404                 writel(cbcdr, CCM_BASE_ADDR + CLKCTL_CBCDR);
 405                 while (readl(CCM_BASE_ADDR + CLKCTL_CDHIPR) != 0);
 406                 writel(0x0, CCM_BASE_ADDR + CLKCTL_CCDR);
 407         }
 408         return 0;
 409 }
 410
 411 static void clock_setup(int argc,char *argv[])
 412 {
 413         u32 i, core_clk, ddr_clk, data[3];
 414         unsigned long temp;
 415         int ret;
 416
 417         if (argc == 1)
 418                 goto print_clock;
 419
 420         enable_dbg();
 421         for (i = 0; i < 2; i++) {
 422                 if (!parse_num(argv[1], &temp, &argv[1], ":")) {
 423                         diag_printf("Error: Invalid parameter\n");
 424                         return;
 425                 }
 426                 data[i] = temp;
 427         }
 428
 429         core_clk = data[0] * SZ_DEC_1M;
 430         ddr_clk = data[1] * SZ_DEC_1M;
 431
 432         if (core_clk != 0) {
 433                 if ((core_clk < PLL_FREQ_MIN(PLL_REF_CLK)) || (core_clk > PLL_FREQ_MAX(PLL_REF_CLK))) {
 434                         diag_printf("Targeted core clock should be within [%d - %d]\n",
 435                                                 PLL_FREQ_MIN(PLL_REF_CLK), PLL_FREQ_MAX(PLL_REF_CLK));
 436                         return;
 437                 }
 438         }
 439
 440         if (ddr_clk != 0) {
 441                 if (ddr_clk > MAX_DDR_CLK) {
 442                         diag_printf("DDR clock should be less than %d MHz, assuming max value\n",
 443                                                 MAX_DDR_CLK / SZ_DEC_1M);
 444                         ddr_clk = MAX_DDR_CLK;
 445                 }
 446         }
 447
 448         // adjust the clock
 449         ret = configure_clock(PLL_REF_CLK, core_clk, ddr_clk);
 450         if (ret != 0) {
 451                 diag_printf("Failed to setup clock: %d\n", ret);
 452                 return;
 453         }
 454         diag_printf("\n<<<New clock setting>>>\n");
 455
 456         // Now printing clocks
 457 print_clock:
 458
 459         diag_printf("\nPLL1\t\tPLL2\t\tPLL3\t\tPLL4\n");
 460         diag_printf("========================================================\n");
 461         diag_printf("%-16d%-16d%-16d%-16d\n\n", pll_clock(PLL1), pll_clock(PLL2),
 462                                 pll_clock(PLL3), pll_clock(PLL4));
 463         diag_printf("AXI_A\t\tAXI_B\t\tEMI_SLOW_CLK\n");
 464         diag_printf("========================================================\n");
 465         diag_printf("%-16d%-16d%-16d\n\n",
 466                                 get_main_clock(AXI_A_CLK),
 467                                 get_main_clock(AXI_B_CLK),
 468                                 get_main_clock(EMI_SLOW_CLK));
 469         diag_printf("CPU\t\tAHB\t\tIPG\t\tDDR_CLK\n");
 470         diag_printf("========================================================\n");
 471         diag_printf("%-16d%-16d%-16d%-16d\n\n",
 472                                 get_main_clock(CPU_CLK),
 473                                 get_main_clock(AHB_CLK),
 474                                 get_main_clock(IPG_CLK),
 475                                 get_main_clock(DDR_CLK));
 476
 477         diag_printf("NFC\t\tUSB\t\tIPG_PER_CLK\n");
 478         diag_printf("========================================\n");
 479         diag_printf("%-16d%-16d%-16d\n\n",
 480                                 get_main_clock(NFC_CLK),
 481                                 get_main_clock(USB_CLK),
 482                                 get_main_clock(IPG_PER_CLK));
 483
 484         diag_printf("UART1-3\t\tSSI1\t\tSSI2\t\tSPI\n");
 485         diag_printf("===========================================");
 486         diag_printf("=============\n");
 487
 488         diag_printf("%-16d%-16d%-16d%-16d\n\n",
 489                                 get_peri_clock(UART1_BAUD),
 490                                 get_peri_clock(SSI1_BAUD),
 491                                 get_peri_clock(SSI2_BAUD),
 492                                 get_peri_clock(SPI1_CLK));
 493
 494 #if 0
 495         diag_printf("IPG_PERCLK as baud clock for: UART1-5, I2C, OWIRE, SDHC");
 496         if (((readl(EPIT1_BASE_ADDR) >> 24) & 0x3) == 0x2) {
 497                 diag_printf(", EPIT");
 498         }
 499         if (((readl(GPT1_BASE_ADDR) >> 6) & 0x7) == 0x2) {
 500                 diag_printf(", GPT");
 501         }
 502 #endif
 503         diag_printf("\n");
 504
 505 }
 506
 507 /*!
 508  * This function returns the PLL output value in Hz based on pll.
 509  */
 510 u32 pll_clock(enum plls pll)
 511 {
 512         u64 ref_clk;
 513         u32 mfi, mfn, mfd, pdf, pll_out;
 514         int sign;
 515         u32 dp_ctrl, dp_op, dp_mfd, dp_mfn;
 516         int clk_sel;
 517         int dbl;
 518
 519         dp_ctrl = pll_base[pll][PLL_DP_CTL >> 2];
 520         clk_sel = MXC_GET_FIELD(dp_ctrl, 2, 8);
 521         ref_clk = PLL_REF_CLK;
 522
 523         dbg("clk_sel=%d\n", clk_sel);
 524
 525         if ((pll_base[pll][PLL_DP_CTL >> 2] & 0x80) == 0) {
 526                 dp_op = pll_base[pll][PLL_DP_OP >> 2];
 527                 dp_mfd = pll_base[pll][PLL_DP_MFD >> 2];
 528                 dp_mfn = pll_base[pll][PLL_DP_MFN >> 2];
 529         } else {
 530                 dp_op = pll_base[pll][PLL_DP_HFS_OP >> 2];
 531                 dp_mfd = pll_base[pll][PLL_DP_HFS_MFD >> 2];
 532                 dp_mfn = pll_base[pll][PLL_DP_HFS_MFN >> 2];
 533         }
 534         pdf = dp_op & 0xF;
 535         mfi = (dp_op >> 4) & 0xF;
 536         mfi = (mfi <= 5) ? 5: mfi;
 537         mfd = dp_mfd & 0x07FFFFFF;
 538         mfn = dp_mfn & 0x07FFFFFF;
 539
 540         sign = (mfn < 0x4000000) ? 1 : -1;
 541         mfn = (mfn < 0x4000000) ? mfn : (0x8000000 - mfn);
 542
 543         dbl = 2 * (((dp_ctrl >> 12) & 0x1) + 1);
 544
 545         dbg("%s: ref=%llu.%03lluMHz, dbl=%d, pd=%d, mfi=%d, mfn=%d, mfd=%d\n",
 546                 __FUNCTION__, ref_clk / 1000000, ref_clk / 1000 % 1000,
 547                 dbl, pdf + 1, mfi, sign * mfn, mfd + 1);
 548
 549         pll_out = (dbl * ref_clk * mfi + dbl * ref_clk * sign * mfn / (mfd + 1)) /
 550                 (pdf + 1);
 551
 552         return pll_out;
 553 }
 554
 555 /*!
 556  * This function returns the emi_core_clk_root clock.
 557  */
 558 u32 get_emi_core_clk(void)
 559 {
 560         u32 cbcdr = readl(CCM_BASE_ADDR + CLKCTL_CBCDR);
 561         u32 clk_sel, max_pdf, peri_clk, ahb_clk;
 562         u32 ret_val;
 563
 564         max_pdf = (cbcdr >> 10) & 0x7;
 565         peri_clk = get_periph_clk();
 566         ahb_clk = peri_clk / (max_pdf + 1);
 567
 568         clk_sel = (cbcdr >> 26) & 1;
 569         if (clk_sel == 0) {
 570                 ret_val = peri_clk;
 571         } else {
 572                 ret_val = ahb_clk ;
 573         }
 574         dbg("%s: CBCDR[%08lx]=%08x freq=%u.%03uMHz\n", __FUNCTION__,
 575                 CCM_BASE_ADDR + CLKCTL_CBCDR, cbcdr, ret_val / 1000000, ret_val / 1000 % 1000);
 576         return ret_val;
 577 }
 578
 579 /*!
 580  * This function returns the main clock value in Hz.
 581  */
 582 u32 get_main_clock(enum main_clocks clk)
 583 {
 584         u32 pdf, max_pdf, ipg_pdf, nfc_pdf, clk_sel;
 585         u32 pll, ret_val;
 586         u32 cacrr = readl(CCM_BASE_ADDR + CLKCTL_CACRR);
 587         u32 cbcdr = readl(CCM_BASE_ADDR + CLKCTL_CBCDR);
 588         u32 cbcmr = readl(CCM_BASE_ADDR + CLKCTL_CBCMR);
 589         u32 cscmr1 = readl(CCM_BASE_ADDR + CLKCTL_CSCMR1);
 590         u32 cscdr1 = readl(CCM_BASE_ADDR + CLKCTL_CSCDR1);
 591
 592         dbg("%s: \n", __FUNCTION__);
 593         switch (clk) {
 594         case CPU_CLK:
 595                 pdf = cacrr & 0x7;
 596                 pll = pll_clock(PLL1);
 597                 ret_val = pll / (pdf + 1);
 598                 break;
 599
 600         case AHB_CLK:
 601                 max_pdf = (cbcdr >> 10) & 0x7;
 602                 pll = get_periph_clk();
 603                 ret_val = pll / (max_pdf + 1);
 604                 break;
 605
 606         case AXI_A_CLK:
 607                 pdf = (cbcdr >> 16) & 0x7;
 608                 pll = get_periph_clk();
 609                 ret_val = pll / (pdf + 1);
 610                 break;
 611
 612         case AXI_B_CLK:
 613                 pdf = (cbcdr >> 19) & 0x7;
 614                 pll = get_periph_clk();
 615                 ret_val = pll / (pdf + 1);
 616                 break;
 617
 618         case EMI_SLOW_CLK:
 619                 pll = get_emi_core_clk();
 620                 pdf = (cbcdr >> 22) & 0x7;
 621                 ret_val = pll / (pdf + 1);
 622                 break;
 623
 624         case IPG_CLK:
 625                 max_pdf = (cbcdr >> 10) & 0x7;
 626                 ipg_pdf = (cbcdr >> 8) & 0x3;
 627                 pll = get_periph_clk();
 628                 ret_val = pll / ((max_pdf + 1) * (ipg_pdf + 1));
 629                 break;
 630
 631         case IPG_PER_CLK:
 632                 clk_sel = cbcmr & 1;
 633                 if (clk_sel == 0) {
 634                         clk_sel = (cbcmr >> 1) & 1;
 635                         pdf = (((cbcdr >> 6) & 3) + 1) * (((cbcdr >> 3) & 7) + 1) * ((cbcdr & 7) + 1);
 636                         if (clk_sel == 0) {
 637                                 ret_val = get_periph_clk() / pdf;
 638                         } else {
 639                                 ret_val = get_lp_apm();
 640                         }
 641                 } else {
 642                         /* Same as IPG_CLK */
 643                         max_pdf = (cbcdr >> 10) & 0x7;
 644                         ipg_pdf = (cbcdr >> 8) & 0x3;
 645                         pll = get_periph_clk();
 646                         ret_val = pll / ((max_pdf + 1) * (ipg_pdf + 1));
 647                 }
 648                 break;
 649
 650         case DDR_CLK:
 651                 clk_sel = (cbcmr >> 10) & 3;
 652                 pll = get_periph_clk();
 653                 if (clk_sel == 0) {
 654                         /* AXI A */
 655                         pdf = (cbcdr >> 16) & 0x7;
 656                 } else if (clk_sel == 1) {
 657                         /* AXI B */
 658                         pdf = (cbcdr >> 19) & 0x7;
 659                 } else if (clk_sel == 2) {
 660                         /* EMI SLOW CLOCK ROOT */
 661                         pll = get_emi_core_clk();
 662                         pdf = (cbcdr >> 22) & 0x7;
 663                 } else if (clk_sel == 3) {
 664                         /* AHB CLOCK */
 665                         pdf = (cbcdr >> 10) & 0x7;
 666                 }
 667
 668                 ret_val = pll / (pdf + 1);
 669                 break;
 670
 671         case NFC_CLK:
 672                 pdf = (cbcdr >> 22) & 0x7;
 673                 nfc_pdf = (cbcdr >> 13) & 0x7;
 674                 pll = get_emi_core_clk();
 675                 ret_val = pll / ((pdf + 1) * (nfc_pdf + 1));
 676                 break;
 677
 678         case USB_CLK:
 679                 clk_sel = (cscmr1 >> 22) & 3;
 680                 if (clk_sel == 0) {
 681                         pll = pll_clock(PLL1);
 682                 } else if (clk_sel == 1) {
 683                         pll = pll_clock(PLL2);
 684                 } else if (clk_sel == 2) {
 685                         pll = pll_clock(PLL3);
 686                 } else if (clk_sel == 3) {
 687                         pll = get_lp_apm();
 688                 }
 689                 pdf = (cscdr1 >> 8) & 0x7;
 690                 max_pdf = (cscdr1 >> 6) & 0x3;
 691                 ret_val = pll / ((pdf + 1) * (max_pdf + 1));
 692                 break;
 693
 694         default:
 695                 diag_printf("Unknown clock: %d\n", clk);
 696                 return ERR_WRONG_CLK;
 697         }
 698
 699         return ret_val;
 700 }
 701
 702 /*!
 703  * This function returns the peripheral clock value in Hz.
 704  */
 705 u32 get_peri_clock(enum peri_clocks clk)
 706 {
 707         u32 ret_val = 0, pdf, pre_pdf, clk_sel;
 708         u32 cscmr1 = readl(CCM_BASE_ADDR + CLKCTL_CSCMR1);
 709         u32 cscdr1 = readl(CCM_BASE_ADDR + CLKCTL_CSCDR1);
 710         u32 cscdr2 = readl(CCM_BASE_ADDR + CLKCTL_CSCDR2);
 711         u32 cs1cdr = readl(CCM_BASE_ADDR + CLKCTL_CS1CDR);
 712         u32 cs2cdr = readl(CCM_BASE_ADDR + CLKCTL_CS2CDR);
 713
 714         dbg("%s: \n", __FUNCTION__);
 715         switch (clk) {
 716         case UART1_BAUD:
 717         case UART2_BAUD:
 718         case UART3_BAUD:
 719                 pre_pdf = (cscdr1 >> 3) & 0x7;
 720                 pdf = cscdr1 & 0x7;
 721                 clk_sel = (cscmr1 >> 24) & 3;
 722                 if (clk_sel == 0) {
 723                         ret_val = pll_clock(PLL1) / ((pre_pdf + 1) * (pdf + 1));
 724                 } else if (clk_sel == 1) {
 725                         ret_val = pll_clock(PLL2) / ((pre_pdf + 1) * (pdf + 1));
 726                 } else if (clk_sel == 2) {
 727                         ret_val = pll_clock(PLL3) / ((pre_pdf + 1) * (pdf + 1));
 728                 } else {
 729                         ret_val = get_lp_apm() / ((pre_pdf + 1) * (pdf + 1));
 730                 }
 731                 break;
 732         case SSI1_BAUD:
 733                 pre_pdf = (cs1cdr >> 6) & 0x7;
 734                 pdf = cs1cdr & 0x3F;
 735                 clk_sel = (cscmr1 >> 14) & 3;
 736                 if (clk_sel == 0) {
 737                         ret_val = pll_clock(PLL1) / ((pre_pdf + 1) * (pdf + 1));
 738                 } else if (clk_sel == 0x1) {
 739                         ret_val = pll_clock(PLL2) / ((pre_pdf + 1) * (pdf + 1));
 740                 } else if (clk_sel == 0x2) {
 741                         ret_val = pll_clock(PLL3) / ((pre_pdf + 1) * (pdf + 1));
 742                 } else {
 743                         ret_val = CKIH /((pre_pdf + 1) * (pdf + 1));
 744                 }
 745                 break;
 746         case SSI2_BAUD:
 747                 pre_pdf = (cs2cdr >> 6) & 0x7;
 748                 pdf = cs2cdr & 0x3F;
 749                 clk_sel = (cscmr1 >> 12) & 3;
 750                 if (clk_sel == 0) {
 751                         ret_val = pll_clock(PLL1) / ((pre_pdf + 1) * (pdf + 1));
 752                 } else if (clk_sel == 0x1) {
 753                         ret_val = pll_clock(PLL2) / ((pre_pdf + 1) * (pdf + 1));
 754                 } else if (clk_sel == 0x2) {
 755                         ret_val = pll_clock(PLL3) / ((pre_pdf + 1) * (pdf + 1));
 756                 } else {
 757                         ret_val = CKIH /((pre_pdf + 1) * (pdf + 1));
 758                 }
 759                 break;
 760         case SPI1_CLK:
 761         case SPI2_CLK:
 762                 pre_pdf = (cscdr2 >> 25) & 0x7;
 763                 pdf = (cscdr2 >> 19) & 0x3F;
 764                 clk_sel = (cscmr1 >> 4) & 3;
 765                 if (clk_sel == 0) {
 766                         ret_val = pll_clock(PLL1) / ((pre_pdf + 1) * (pdf + 1));
 767                 } else if (clk_sel == 1) {
 768                         ret_val = pll_clock(PLL2) / ((pre_pdf + 1) * (pdf + 1));
 769                 } else if (clk_sel == 2) {
 770                         ret_val = pll_clock(PLL3) / ((pre_pdf + 1) * (pdf + 1));
 771                 } else {
 772                         ret_val = get_lp_apm() / ((pre_pdf + 1) * (pdf + 1));
 773                 }
 774                 break;
 775         default:
 776                 diag_printf("%s(): This clock: %d not supported yet\n",
 777                                         __FUNCTION__, clk);
 778         }
 779
 780         return ret_val;
 781 }
 782
 783 #ifdef L2CC_ENABLED
 784 /*
 785  * This command is added for some simple testing only. It turns on/off
 786  * L2 cache regardless of L1 cache state. The side effect of this is
 787  * when doing any flash operations such as "fis init", the L2
 788  * will be turned back on along with L1 caches even though it is off
 789  * by using this command.
 790  */
 791 RedBoot_cmd("L2",
 792                         "L2 cache",
 793                         "[ON | OFF]",
 794                         do_L2_caches
 795         );
 796
 797 void do_L2_caches(int argc, char *argv[])
 798 {
 799         u32 oldints;
 800
 801         if (argc == 2) {
 802                 if (strcasecmp(argv[1], "on") == 0) {
 803                         HAL_DISABLE_INTERRUPTS(oldints);
 804                         HAL_ENABLE_L2();
 805                         HAL_RESTORE_INTERRUPTS(oldints);
 806                 } else if (strcasecmp(argv[1], "off") == 0) {
 807                         HAL_DISABLE_INTERRUPTS(oldints);
 808                         HAL_DCACHE_DISABLE_L1();
 809                         HAL_CACHE_FLUSH_ALL();
 810                         HAL_DISABLE_L2();
 811                         HAL_DCACHE_ENABLE_L1();
 812                         HAL_RESTORE_INTERRUPTS(oldints);
 813                 } else {
 814                         diag_printf("Invalid L2 cache mode: %s\n", argv[1]);
 815                 }
 816         } else {
 817                 int L2cache_on;
 818
 819                 HAL_L2CACHE_IS_ENABLED(L2cache_on);
 820                 diag_printf("L2 cache: %s\n", L2cache_on ? "On" : "Off");
 821         }
 822 }
 823 #endif //L2CC_ENABLED
 824
 825 #define IIM_ERR_SHIFT           8
 826 #define POLL_FUSE_PRGD          (IIM_STAT_PRGD | (IIM_ERR_PRGE << IIM_ERR_SHIFT))
 827 #define POLL_FUSE_SNSD          (IIM_STAT_SNSD | (IIM_ERR_SNSE << IIM_ERR_SHIFT))
 828
 829 static void fuse_op_start(void)
 830 {
 831         /* Do not generate interrupt */
 832         writel(0, IIM_BASE_ADDR + IIM_STATM_OFF);
 833         // clear the status bits and error bits
 834         writel(0x3, IIM_BASE_ADDR + IIM_STAT_OFF);
 835         writel(0xFE, IIM_BASE_ADDR + IIM_ERR_OFF);
 836 }
 837
 838 /*
 839  * The action should be either:
 840  *          POLL_FUSE_PRGD
 841  * or:
 842  *          POLL_FUSE_SNSD
 843  */
 844 static int poll_fuse_op_done(int action)
 845 {
 846         u32 status, error;
 847
 848         if (action != POLL_FUSE_PRGD && action != POLL_FUSE_SNSD) {
 849                 diag_printf("%s(%d) invalid operation\n", __FUNCTION__, action);
 850                 return -1;
 851         }
 852
 853         /* Poll busy bit till it is NOT set */
 854         while ((readl(IIM_BASE_ADDR + IIM_STAT_OFF) & IIM_STAT_BUSY) != 0 ) {
 855         }
 856
 857         /* Test for successful write */
 858         status = readl(IIM_BASE_ADDR + IIM_STAT_OFF);
 859         error = readl(IIM_BASE_ADDR + IIM_ERR_OFF);
 860
 861         if ((status & action) != 0 && (error & (action >> IIM_ERR_SHIFT)) == 0) {
 862                 if (error) {
 863                         diag_printf("Even though the operation seems successful...\n");
 864                         diag_printf("There are some error(s) at addr=0x%08lx: 0x%08x\n",
 865                                                 (IIM_BASE_ADDR + IIM_ERR_OFF), error);
 866                 }
 867                 return 0;
 868         }
 869         diag_printf("%s(%d) failed\n", __FUNCTION__, action);
 870         diag_printf("status address=0x%08lx, value=0x%08x\n",
 871                                 (IIM_BASE_ADDR + IIM_STAT_OFF), status);
 872         diag_printf("There are some error(s) at addr=0x%08lx: 0x%08x\n",
 873                                 (IIM_BASE_ADDR + IIM_ERR_OFF), error);
 874         return -1;
 875 }
 876
 877 unsigned int sense_fuse(int bank, int row, int bit)
 878 {
 879         int addr, addr_l, addr_h, reg_addr;
 880
 881         fuse_op_start();
 882
 883         addr = ((bank << 11) | (row << 3) | (bit & 0x7));
 884         /* Set IIM Program Upper Address */
 885         addr_h = (addr >> 8) & 0x000000FF;
 886         /* Set IIM Program Lower Address */
 887         addr_l = (addr & 0x000000FF);
 888
 889 #ifdef IIM_FUSE_DEBUG
 890         diag_printf("%s: addr_h=0x%02x, addr_l=0x%02x\n",
 891                                 __FUNCTION__, addr_h, addr_l);
 892 #endif
 893         writel(addr_h, IIM_BASE_ADDR + IIM_UA_OFF);
 894         writel(addr_l, IIM_BASE_ADDR + IIM_LA_OFF);
 895         /* Start sensing */
 896         writel(0x8, IIM_BASE_ADDR + IIM_FCTL_OFF);
 897         if (poll_fuse_op_done(POLL_FUSE_SNSD) != 0) {
 898                 diag_printf("%s(bank: %d, row: %d, bit: %d failed\n",
 899                                         __FUNCTION__, bank, row, bit);
 900         }
 901         reg_addr = IIM_BASE_ADDR + IIM_SDAT_OFF;
 902         return readl(reg_addr);
 903 }
 904
 905 void do_fuse_read(int argc, char *argv[])
 906 {
 907         unsigned long bank, row;
 908         unsigned long fuse_val;
 909
 910         if (argc == 1) {
 911                 diag_printf("Usage: fuse_read <bank> <row>\n");
 912                 return;
 913         } else if (argc == 3) {
 914                 if (!parse_num(argv[1], &bank, &argv[1], " ")) {
 915                         diag_printf("Error: Invalid parameter\n");
 916                         return;
 917                 }
 918                 if (!parse_num(argv[2], &row, &argv[2], " ")) {
 919                         diag_printf("Error: Invalid parameter\n");
 920                         return;
 921                 }
 922
 923                 diag_printf("Read fuse at bank:%ld row:%ld\n", bank, row);
 924                 fuse_val = sense_fuse(bank, row, 0);
 925                 diag_printf("fuses at (bank:%ld, row:%ld) = 0x%02lx\n", bank, row, fuse_val);
 926         } else {
 927                 diag_printf("Passing in wrong arguments: %d\n", argc);
 928                 diag_printf("Usage: fuse_read <bank> <row>\n");
 929         }
 930 }
 931
 932 /* Blow fuses based on the bank, row and bit positions (all 0-based)
 933 */
 934 static int fuse_blow(int bank, int row, int bit)
 935 {
 936         int addr, addr_l, addr_h, ret = -1;
 937
 938         fuse_op_start();
 939
 940         /* Disable IIM Program Protect */
 941         writel(0xAA, IIM_BASE_ADDR + IIM_PREG_P_OFF);
 942
 943         addr = ((bank << 11) | (row << 3) | (bit & 0x7));
 944         /* Set IIM Program Upper Address */
 945         addr_h = (addr >> 8) & 0x000000FF;
 946         /* Set IIM Program Lower Address */
 947         addr_l = (addr & 0x000000FF);
 948
 949 #ifdef IIM_FUSE_DEBUG
 950         diag_printf("blowing fuse %d %d bit %d addr_h=0x%02x, addr_l=0x%02x\n",
 951                                 bank, row, bit, addr_h, addr_l);
 952 #endif
 953
 954         writel(addr_h, IIM_BASE_ADDR + IIM_UA_OFF);
 955         writel(addr_l, IIM_BASE_ADDR + IIM_LA_OFF);
 956         /* Start Programming */
 957         writel(0x71, IIM_BASE_ADDR + IIM_FCTL_OFF);
 958         if (poll_fuse_op_done(POLL_FUSE_PRGD) == 0) {
 959                 ret = 0;
 960         }
 961
 962         /* Enable IIM Program Protect */
 963         writel(0x0, IIM_BASE_ADDR + IIM_PREG_P_OFF);
 964         return ret;
 965 }
 966
 967 /*
 968  * This command is added for burning IIM fuses
 969  */
 970 RedBoot_cmd("fuse_read",
 971                         "read some fuses",
 972                         "<bank> <row>",
 973                         do_fuse_read
 974         );
 975
 976 RedBoot_cmd("fuse_blow",
 977                         "blow some fuses",
 978                         "<bank> <row> <value>",
 979                         do_fuse_blow
 980         );
 981
 982 void quick_itoa(u32 num, char *a)
 983 {
 984         int i, j, k;
 985         for (i = 0; i <= 7; i++) {
 986                 j = (num >> (4 * i)) & 0xF;
 987                 k = (j < 10) ? '0' : ('a' - 0xa);
 988                 a[i] = j + k;
 989         }
 990 }
 991
 992 // slen - streng length, e.g.: 23 -> slen=2; abcd -> slen=4
 993 // only convert hex value as string input. so "12" is 0x12.
 994 u32 quick_atoi(char *a, u32 slen)
 995 {
 996         u32 i, num = 0, digit;
 997
 998         for (i = 0; i < slen; i++) {
 999                 if (a[i] >= '0' && a[i] <= '9') {
1000                         digit = a[i] - '0';
1001                 } else if (a[i] >= 'a' && a[i] <= 'f') {
1002                         digit = a[i] - 'a' + 10;
1003                 } else if (a[i] >= 'A' && a[i] <= 'F') {
1004                         digit = a[i] - 'A' + 10;
1005                 } else {
1006                         diag_printf("ERROR: %c\n", a[i]);
1007                         return -1;
1008                 }
1009                 num = (num * 16) + digit;
1010         }
1011         return num;
1012 }
1013
1014 void fuse_blow_row(int bank, int row, int value)
1015 {
1016         unsigned int reg, i;
1017
1018         FUSE_PROG_START();
1019
1020         // enable fuse blown
1021         reg = readl(CCM_BASE_ADDR + 0x64);
1022         reg |= 0x10;
1023         writel(reg, CCM_BASE_ADDR + 0x64);
1024
1025         for (i = 0; i < 8; i++) {
1026                 if (((value >> i) & 0x1) == 0) {
1027                         continue;
1028                 }
1029                 if (fuse_blow(bank, row, i) != 0) {
1030                         diag_printf("fuse_blow(bank: %d, row: %d, bit: %d failed\n",
1031                                                 bank, row, i);
1032                 }
1033         }
1034         reg &= ~0x10;
1035         writel(reg, CCM_BASE_ADDR + 0x64);
1036
1037         FUSE_PROG_DONE();
1038 }
1039
1040 void do_fuse_blow(int argc, char *argv[])
1041 {
1042         unsigned long bank, row, value, i;
1043         unsigned int fuse_val;
1044         char *s;
1045         char val[3];
1046
1047         if (argc == 1) {
1048                 diag_printf("It is too dangeous for you to use this command.\n");
1049                 return;
1050         }
1051
1052         if (argc == 3) {
1053                 if (strcasecmp(argv[1], "scc") == 0) {
1054                         // fuse_blow scc C3D153EDFD2EA9982226EF5047D3B9A0B9C7138EA87C028401D28C2C2C0B9AA2
1055                         diag_printf("Ready to burn SCC fuses\n");
1056                         s=argv[2];
1057                         for (i = 0; ;i++) {
1058                                 memcpy(val, s, 2);
1059                                 val[2]='\0';
1060                                 value = quick_atoi(val, 2);
1061                                 //    diag_printf("fuse_blow_row(2, %d, value=0x%02x)\n", i, value);
1062                                 fuse_blow_row(2, i, value);
1063
1064                                 if ((++s)[0] == '\0') {
1065                                         diag_printf("ERROR: Odd string input\n");
1066                                         break;
1067                                 }
1068                                 if ((++s)[0] == '\0') {
1069                                         diag_printf("Successful\n");
1070                                         break;
1071                                 }
1072                         }
1073                 } else if (strcasecmp(argv[1], "srk") == 0) {
1074                         // fuse_blow srk 418bccd09b53bee1ab59e2662b3c7877bc0094caee201052add49be8780dff95
1075                         diag_printf("Ready to burn SRK key fuses\n");
1076                         s=argv[2];
1077                         for (i = 0; ;i++) {
1078                                 memcpy(val, s, 2);
1079                                 val[2]='\0';
1080                                 value = quick_atoi(val, 2);
1081                                 if (i == 0) {
1082                                         fuse_blow_row(1, 1, value); // 0x41 goes to SRK_HASH[255:248], bank 1, row 1
1083                                 } else
1084                                         fuse_blow_row(3, i, value);  // 0x8b in SRK_HASH[247:240] bank 3, row 1
1085                                                                                                  // 0xcc in SRK_HASH[239:232] bank 3, row 2
1086                                                                                                  // ...
1087                                 if ((++s)[0] == '\0') {
1088                                         diag_printf("ERROR: Odd string input\n");
1089                                         break;
1090                                 }
1091                                 if ((++s)[0] == '\0') {
1092                                         diag_printf("Successful\n");
1093                                         break;
1094                                 }
1095                         }
1096                 } else
1097                         diag_printf("This command is not supported\n");
1098
1099                 return;
1100         } else if (argc == 4) {
1101                 if (!parse_num(argv[1], &bank, &argv[1], " ")) {
1102                         diag_printf("Error: Invalid parameter\n");
1103                         return;
1104                 }
1105                 if (!parse_num(argv[2], &row, &argv[2], " ")) {
1106                         diag_printf("Error: Invalid parameter\n");
1107                         return;
1108                 }
1109                 if (!parse_num(argv[3], &value, &argv[3], " ")) {
1110                         diag_printf("Error: Invalid parameter\n");
1111                         return;
1112                 }
1113
1114                 diag_printf("Blowing fuse at bank:%ld row:%ld value:%ld\n",
1115                                         bank, row, value);
1116                 fuse_blow_row(bank, row, value);
1117                 fuse_val = sense_fuse(bank, row, 0);
1118                 diag_printf("fuses at (bank:%ld, row:%ld) = 0x%02x\n", bank, row, fuse_val);
1119
1120         } else {
1121                 diag_printf("Passing in wrong arguments: %d\n", argc);
1122         }
1123 }
1124
1125 /* precondition: m>0 and n>0.  Let g=gcd(m,n). */
1126 static int gcd(int m, int n)
1127 {
1128         while (m > 0) {
1129                 if (n > m) {
1130                         /* swap */
1131                         m ^= n;
1132                         n ^= m;
1133                         m ^= n;
1134                 }
1135                 m -= n;
1136         }
1137         return n;
1138 }
1139
1140 int read_mac_addr_from_fuse(unsigned char* data)
1141 {
1142         data[0] = sense_fuse(1, 9, 0) ;
1143         data[1] = sense_fuse(1, 10, 0) ;
1144         data[2] = sense_fuse(1, 11, 0) ;
1145         data[3] = sense_fuse(1, 12, 0) ;
1146         data[4] = sense_fuse(1, 13, 0) ;
1147         data[5] = sense_fuse(1, 14, 0) ;
1148
1149         if ((data[0] == 0) && (data[1] == 0) && (data[2] == 0) &&
1150                 (data[3] == 0) && (data[4] == 0) && (data[5] == 0)) {
1151                 return 0;
1152         }
1153
1154         return 1;
1155 }
1156
1157 #if 0
1158 void imx_power_mode(int mode)
1159 {
1160         volatile unsigned int val;
1161         switch (mode) {
1162         case 2:
1163                 writel(0x0000030f, GPC_PGR);
1164                 writel(0x1, SRPGCR_EMI);
1165                 writel(0x1, SRPGCR_ARM);
1166                 writel(0x1, PGC_PGCR_VPU);
1167                 writel(0x1, PGC_PGCR_IPU);
1168
1169
1170         case 1:
1171                 // stop mode - from validation code
1172                 // Set DSM_INT_HOLDOFF bit in TZIC
1173                 // If the TZIC didn't write the bit then there was interrupt pending
1174                 // It will be serviced while we're in the loop
1175                 // So we write to this bit again
1176                 while (readl(INTC_BASE_ADDR + 0x14) == 0) {
1177                         writel(1, INTC_BASE_ADDR + 0x14);
1178                         // Wait few cycles
1179                         __asm("nop");
1180                         __asm("nop");
1181                         __asm("nop");
1182                         __asm("nop");
1183                         __asm("nop");
1184                         __asm("nop");
1185                         __asm("nop");
1186                 }
1187                 diag_printf("Entering stop mode\n");
1188                 val = readl(CCM_BASE_ADDR + 0x74);
1189                 val = (val & 0xfffffffc) | 0x2; // set STOP mode
1190                 writel(val, CCM_BASE_ADDR + 0x74);
1191                 val = readl(PLATFORM_LPC_REG);
1192                 writel(val | (1 << 16), PLATFORM_LPC_REG);// ENABLE DSM in ELBOW submodule of ARM platform
1193                 writel(val | (1 << 17), PLATFORM_LPC_REG);// ENABLE DSM in ELBOW submodule of ARM platform
1194                 break;
1195         }
1196
1197         hal_delay_us(50);
1198
1199         asm("mov r1, #0");
1200         asm("mcr p15, 0, r1, c7, c0, 4");
1201 }
1202
1203 void do_power_mode(int argc, char *argv[])
1204 {
1205         unsigned long mode;
1206
1207         if (argc == 1) {
1208                 diag_printf("Usage: power_mode <mode>\n");
1209                 return;
1210         } else if (argc == 2) {
1211                 if (!parse_num(argv[1], &mode, &argv[1], " ")) {
1212                         diag_printf("Error: Invalid parameter\n");
1213                         return;
1214                 }
1215                 diag_printf("Entering power mode: %lu\n", mode);
1216                 imx_power_mode(mode);
1217
1218         } else {
1219                 diag_printf("Passing in wrong arguments: %d\n", argc);
1220                 diag_printf("Usage: power_mode <mode>\n");
1221         }
1222 }
1223
1224 /*
1225  * This command is added for burning IIM fuses
1226  */
1227 RedBoot_cmd("power_mode",
1228                         "Enter various power modes:",
1229                         "\n"
1230                         "           <0> - WAIT\n"
1231                         "           <1> - SRPG\n"
1232                         "           <2> - STOP\n"
1233                         "           <3> - STOP with Power-Gating\n"
1234                         "           -- need reset after issuing the command",
1235                         do_power_mode
1236         );
1237 #endif
1238
1239 /* Super Root key moduli */
1240 static const UINT8 hab_super_root_moduli[] = {
1241         /* modulus data */
1242         0xb9, 0x84, 0xc8, 0x8a, 0xd3, 0x7e, 0xcc, 0xc0, 0xe7, 0x3e, 0x11, 0x53,
1243         0x6b, 0x5e, 0xea, 0xf4, 0xd9, 0xac, 0x5a, 0x63, 0x8a, 0x79, 0x96, 0x83,
1244         0xb1, 0x39, 0xb2, 0x6f, 0x9c, 0x54, 0x87, 0xf4, 0x3b, 0x9e, 0xd8, 0x0f,
1245         0x89, 0xf5, 0x01, 0x53, 0xb8, 0xe2, 0xcc, 0x75, 0x0d, 0xe1, 0x13, 0xfa,
1246         0xa7, 0xb9, 0x1e, 0xff, 0x6a, 0x05, 0xdb, 0x58, 0x10, 0xbf, 0x2b, 0xf4,
1247         0xe7, 0x0a, 0x63, 0x82, 0x2c, 0xa3, 0xb5, 0x0a, 0x72, 0x1c, 0xdc, 0x29,
1248         0xc1, 0x81, 0xb5, 0x9a, 0xf0, 0x25, 0x7d, 0xd6, 0xee, 0x01, 0x64, 0xc7,
1249         0x07, 0x2d, 0xcb, 0x31, 0x4c, 0x8d, 0x82, 0xf6, 0x44, 0x95, 0x4a, 0xbc,
1250         0xae, 0xe8, 0x2a, 0x89, 0xd4, 0xf2, 0x66, 0x72, 0x2b, 0x09, 0x4e, 0x56,
1251         0xe9, 0xbf, 0x5e, 0x38, 0x5c, 0xd5, 0x7e, 0x15, 0x55, 0x86, 0x0f, 0x19,
1252         0xf6, 0x00, 0xee, 0xa1, 0x92, 0x78, 0xef, 0x93, 0xcb, 0xfa, 0xb4, 0x98,
1253         0x19, 0xef, 0x10, 0x70, 0xde, 0x36, 0x1c, 0x12, 0x2e, 0xd2, 0x09, 0xc7,
1254         0x7b, 0xd1, 0xaa, 0xd3, 0x46, 0x65, 0xa1, 0x5b, 0xee, 0xa5, 0x96, 0x97,
1255         0x98, 0x3e, 0xfc, 0xf8, 0x74, 0x22, 0x51, 0xe7, 0xf1, 0x2f, 0x30, 0x79,
1256         0x13, 0xe5, 0x42, 0xc6, 0x7c, 0x18, 0x76, 0xd3, 0x7f, 0x5a, 0x13, 0xde,
1257         0x2f, 0x51, 0x07, 0xfa, 0x93, 0xfe, 0x10, 0x8a, 0x0c, 0x18, 0x60, 0x3c,
1258         0xff, 0x6a, 0x9b, 0xe7, 0x10, 0x2d, 0x71, 0xd2, 0x34, 0xc0, 0xdf, 0xbe,
1259         0x17, 0x4e, 0x75, 0x40, 0x83, 0xaa, 0x90, 0xd1, 0xed, 0xbd, 0xbf, 0xac,
1260         0x9a, 0x30, 0xbd, 0x69, 0x4d, 0xd8, 0x00, 0x63, 0x92, 0x69, 0x98, 0xf8,
1261         0x89, 0xdc, 0x7b, 0xe3, 0x66, 0x7e, 0xdd, 0xfa, 0x8c, 0x74, 0xe2, 0xb1,
1262         0xeb, 0x94, 0xf7, 0xab, 0x0e, 0x92, 0x06, 0xab, 0x60, 0xe5, 0x00, 0x43,
1263         0xb2, 0x5e, 0x6e, 0xeb
1264 };
1265
1266 /* Super Root key */
1267 const hab_rsa_public_key hab_super_root_key[] = {
1268         {
1269                 {
1270                         /* RSA public exponent, right-padded */
1271                         0x01, 0x00, 0x01, 0x00,
1272                 },
1273                 /* pointer to modulus data */
1274                 hab_super_root_moduli,
1275                 /* Exponent size in bytes */
1276                 0x03,
1277                 /* Modulus size in bytes */
1278                 0x100,
1279                 /* Key data valid */
1280                 TRUE
1281         }
1282 };