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