git.sur5r.net Git - openocd/blob - src/flash/mflash.c

   1 /***************************************************************************
   2  *   Copyright (C) 2007-2008 by unsik Kim <donari75@gmail.com>             *
   3  *                                                                         *
   4  *   This program is free software; you can redistribute it and/or modify  *
   5  *   it under the terms of the GNU General Public License as published by  *
   6  *   the Free Software Foundation; either version 2 of the License, or     *
   7  *   (at your option) any later version.                                   *
   8  *                                                                         *
   9  *   This program is distributed in the hope that it will be useful,       *
  10  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  11  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  12  *   GNU General Public License for more details.                          *
  13  *                                                                         *
  14  *   You should have received a copy of the GNU General Public License     *
  15  *   along with this program; if not, write to the                         *
  16  *   Free Software Foundation, Inc.,                                       *
  17  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  18  ***************************************************************************/
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include "mflash.h"
  25 #include "time_support.h"
  26 #include "fileio.h"
  27 #include "log.h"
  28
  29
  30 static int s3c2440_set_gpio_to_output (mflash_gpio_num_t gpio);
  31 static int s3c2440_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val);
  32 static int pxa270_set_gpio_to_output (mflash_gpio_num_t gpio);
  33 static int pxa270_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val);
  34
  35 static command_t *mflash_cmd;
  36
  37 static mflash_bank_t *mflash_bank;
  38
  39 static mflash_gpio_drv_t pxa270_gpio = {
  40         .name = "pxa270",
  41         .set_gpio_to_output = pxa270_set_gpio_to_output,
  42         .set_gpio_output_val = pxa270_set_gpio_output_val
  43 };
  44
  45 static mflash_gpio_drv_t s3c2440_gpio = {
  46         .name = "s3c2440",
  47         .set_gpio_to_output = s3c2440_set_gpio_to_output,
  48         .set_gpio_output_val = s3c2440_set_gpio_output_val
  49 };
  50
  51 static mflash_gpio_drv_t *mflash_gpio[] =
  52 {
  53                 &pxa270_gpio,
  54                 &s3c2440_gpio,
  55                 NULL
  56 };
  57
  58 #define PXA270_GAFR0_L 0x40E00054
  59 #define PXA270_GAFR3_U 0x40E00070
  60 #define PXA270_GAFR3_U_RESERVED_BITS  0xfffc0000u
  61 #define PXA270_GPDR0 0x40E0000C
  62 #define PXA270_GPDR3 0x40E0010C
  63 #define PXA270_GPDR3_RESERVED_BITS  0xfe000000u
  64 #define PXA270_GPSR0 0x40E00018
  65 #define PXA270_GPCR0 0x40E00024
  66
  67 static int pxa270_set_gpio_to_output (mflash_gpio_num_t gpio)
  68 {
  69         u32 addr, value, mask;
  70         target_t *target = mflash_bank->target;
  71         int ret;
  72
  73         /* remove alternate function. */
  74         mask = 0x3u << (gpio.num & 0xF)*2;
  75
  76         addr = PXA270_GAFR0_L + (gpio.num >> 4) * 4;
  77
  78         if ((ret = target_read_u32(target, addr, &value)) != ERROR_OK)
  79                 return ret;
  80
  81         value &= ~mask;
  82         if (addr == PXA270_GAFR3_U)
  83                 value &= ~PXA270_GAFR3_U_RESERVED_BITS;
  84
  85         if ((ret = target_write_u32(target, addr, value)) != ERROR_OK)
  86                 return ret;
  87
  88         /* set direction to output */
  89         mask = 0x1u << (gpio.num & 0x1F);
  90
  91         addr = PXA270_GPDR0 + (gpio.num >> 5) * 4;
  92
  93         if ((ret = target_read_u32(target, addr, &value)) != ERROR_OK)
  94                 return ret;
  95
  96         value |= mask;
  97         if (addr == PXA270_GPDR3)
  98                 value &= ~PXA270_GPDR3_RESERVED_BITS;
  99
 100         ret = target_write_u32(target, addr, value);
 101         return ret;
 102 }
 103
 104 static int pxa270_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val)
 105 {
 106         u32 addr, value, mask;
 107         target_t *target = mflash_bank->target;
 108         int ret;
 109
 110         mask = 0x1u << (gpio.num & 0x1F);
 111
 112         if (val) {
 113                 addr = PXA270_GPSR0 + (gpio.num >> 5) * 4;
 114         } else {
 115                 addr = PXA270_GPCR0 + (gpio.num >> 5) * 4;
 116         }
 117
 118         if ((ret = target_read_u32(target, addr, &value)) != ERROR_OK)
 119                 return ret;
 120
 121         value |= mask;
 122
 123         ret = target_write_u32(target, addr, value);
 124
 125         return ret;
 126 }
 127
 128 #define S3C2440_GPACON 0x56000000
 129 #define S3C2440_GPADAT 0x56000004
 130 #define S3C2440_GPJCON 0x560000d0
 131 #define S3C2440_GPJDAT 0x560000d4
 132
 133 static int s3c2440_set_gpio_to_output (mflash_gpio_num_t gpio)
 134 {
 135         u32 data, mask, gpio_con;
 136         target_t *target = mflash_bank->target;
 137         int ret;
 138
 139         if (gpio.port[0] >= 'a' && gpio.port[0] <= 'h') {
 140                 gpio_con = S3C2440_GPACON + (gpio.port[0] - 'a') * 0x10;
 141         } else if (gpio.port[0] == 'j') {
 142                 gpio_con = S3C2440_GPJCON;
 143         } else {
 144                 LOG_ERROR("invalid port %d%s", gpio.num, gpio.port);
 145                 return ERROR_INVALID_ARGUMENTS;
 146         }
 147
 148         ret = target_read_u32(target, gpio_con, &data);
 149
 150         if (ret == ERROR_OK) {
 151                 if (gpio.port[0] == 'a') {
 152                         mask = 1 << gpio.num;
 153                         data &= ~mask;
 154                 } else {
 155                         mask = 3 << gpio.num * 2;
 156                         data &= ~mask;
 157                         data |= (1 << gpio.num * 2);
 158                 }
 159
 160                 ret = target_write_u32(target, gpio_con, data);
 161         }
 162         return ret;
 163 }
 164
 165 static int s3c2440_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val)
 166 {
 167         u32 data, mask, gpio_dat;
 168         target_t *target = mflash_bank->target;
 169         int ret;
 170
 171         if (gpio.port[0] >= 'a' && gpio.port[0] <= 'h') {
 172                 gpio_dat = S3C2440_GPADAT + (gpio.port[0] - 'a') * 0x10;
 173         } else if (gpio.port[0] == 'j') {
 174                 gpio_dat = S3C2440_GPJDAT;
 175         } else {
 176                 LOG_ERROR("invalid port %d%s", gpio.num, gpio.port);
 177                 return ERROR_INVALID_ARGUMENTS;
 178         }
 179
 180         ret = target_read_u32(target, gpio_dat, &data);
 181
 182         if (ret == ERROR_OK) {
 183                 mask = 1 << gpio.num;
 184                 if (val)
 185                         data |= mask;
 186                 else
 187                         data &= ~mask;
 188
 189                 ret = target_write_u32(target, gpio_dat, data);
 190         }
 191         return ret;
 192 }
 193
 194 static int mg_hdrst(u8 level)
 195 {
 196         return mflash_bank->gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, level);
 197 }
 198
 199 static int mg_init_gpio (void)
 200 {
 201         mflash_gpio_drv_t *gpio_drv = mflash_bank->gpio_drv;
 202
 203         gpio_drv->set_gpio_to_output(mflash_bank->rst_pin);
 204         gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, 1);
 205
 206         return ERROR_OK;
 207 }
 208
 209 static int mg_dsk_wait(mg_io_type_wait wait, u32 time)
 210 {
 211         u8 status, error;
 212         target_t *target = mflash_bank->target;
 213         u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 214         duration_t duration;
 215         long long t=0;
 216
 217         duration_start_measure(&duration);
 218
 219         while (time) {
 220
 221                 target_read_u8(target, mg_task_reg + MG_REG_STATUS, &status);
 222
 223                 if (status & mg_io_rbit_status_busy)
 224                 {
 225                         if (wait == mg_io_wait_bsy)
 226                                 return ERROR_OK;
 227                 } else {
 228                         switch(wait)
 229                         {
 230                                 case mg_io_wait_not_bsy:
 231                                         return ERROR_OK;
 232                                 case mg_io_wait_rdy_noerr:
 233                                         if (status & mg_io_rbit_status_ready)
 234                                                 return ERROR_OK;
 235                                         break;
 236                                 case mg_io_wait_drq_noerr:
 237                                         if (status & mg_io_rbit_status_data_req)
 238                                                 return ERROR_OK;
 239                                         break;
 240                                 default:
 241                                         break;
 242                         }
 243
 244                         /* Now we check the error condition! */
 245                         if (status & mg_io_rbit_status_error)
 246                         {
 247                                 target_read_u8(target, mg_task_reg + MG_REG_ERROR, &error);
 248
 249                                 if (error & mg_io_rbit_err_bad_sect_num) {
 250                                         LOG_ERROR("sector not found");
 251                                         return ERROR_FAIL;
 252                                 }
 253                                 else if (error & (mg_io_rbit_err_bad_block | mg_io_rbit_err_uncorrectable)) {
 254                                         LOG_ERROR("bad block");
 255                                         return ERROR_FAIL;
 256                                 } else {
 257                                         LOG_ERROR("disk operation fail");
 258                                         return ERROR_FAIL;
 259                                 }
 260                         }
 261
 262                         switch (wait)
 263                         {
 264                                 case mg_io_wait_rdy:
 265                                         if (status & mg_io_rbit_status_ready)
 266                                                 return ERROR_OK;
 267
 268                                 case mg_io_wait_drq:
 269                                         if (status & mg_io_rbit_status_data_req)
 270                                                 return ERROR_OK;
 271
 272                                 default:
 273                                         break;
 274                         }
 275                 }
 276
 277                 duration_stop_measure(&duration, NULL);
 278
 279                 t=duration.duration.tv_usec/1000;
 280                 t+=duration.duration.tv_sec*1000;
 281
 282                 if (t > time)
 283                         break;
 284         }
 285
 286         LOG_ERROR("timeout occured");
 287         return ERROR_FAIL;
 288 }
 289
 290 static int mg_dsk_srst(u8 on)
 291 {
 292         target_t *target = mflash_bank->target;
 293         u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 294         u8 value;
 295         int ret;
 296
 297         if ((ret = target_read_u8(target, mg_task_reg + MG_REG_DRV_CTRL, &value)) != ERROR_OK)
 298                 return ret;
 299
 300         if(on) {
 301                 value |= (mg_io_rbit_devc_srst);
 302         } else {
 303                 value &= ~mg_io_rbit_devc_srst;
 304         }
 305
 306         ret = target_write_u8(target, mg_task_reg + MG_REG_DRV_CTRL, value);
 307         return ret;
 308 }
 309
 310 static int mg_dsk_io_cmd(u32 sect_num, u32 cnt, u8 cmd)
 311 {
 312         target_t *target = mflash_bank->target;
 313         u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 314         u8 value;
 315
 316         if (mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL) != ERROR_OK)
 317                 return ERROR_FAIL;
 318
 319         value = mg_io_rval_dev_drv_master | mg_io_rval_dev_lba_mode |((sect_num >> 24) & 0xf);
 320
 321         target_write_u8(target, mg_task_reg + MG_REG_DRV_HEAD, value);
 322         target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, (u8)cnt);
 323         target_write_u8(target, mg_task_reg + MG_REG_SECT_NUM, (u8)sect_num);
 324         target_write_u8(target, mg_task_reg + MG_REG_CYL_LOW, (u8)(sect_num >> 8));
 325         target_write_u8(target, mg_task_reg + MG_REG_CYL_HIGH, (u8)(sect_num >> 16));
 326
 327         target_write_u8(target, mg_task_reg + MG_REG_COMMAND, cmd);
 328
 329         return ERROR_OK;
 330 }
 331
 332 static int mg_dsk_drv_info(void)
 333 {
 334         target_t *target = mflash_bank->target;
 335         u32 mg_buff = mflash_bank->base + MG_BUFFER_OFFSET;
 336
 337         if ( mg_dsk_io_cmd(0, 1, mg_io_cmd_identify) != ERROR_OK)
 338                 return ERROR_FAIL;
 339
 340         if ( mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL) != ERROR_OK)
 341                 return ERROR_FAIL;
 342
 343         LOG_INFO("read drive info.");
 344
 345         if (! mflash_bank->drv_info)
 346                 mflash_bank->drv_info = malloc(sizeof(mg_drv_info_t));
 347
 348         target_read_memory(target, mg_buff, 2, sizeof(mg_io_type_drv_info) >> 1,
 349                         (u8 *)&mflash_bank->drv_info->drv_id);
 350
 351         mflash_bank->drv_info->tot_sects = (u32)(mflash_bank->drv_info->drv_id.total_user_addressable_sectors_hi << 16)
 352                                                                         + mflash_bank->drv_info->drv_id.total_user_addressable_sectors_lo;
 353
 354         target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_read);
 355
 356         return ERROR_OK;
 357 }
 358
 359 static int mg_mflash_rst(void)
 360 {
 361         mg_init_gpio();
 362
 363         mg_hdrst(0);
 364
 365         if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 366                 return ERROR_FAIL;
 367
 368         mg_hdrst(1);
 369
 370         if (mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 371                 return ERROR_FAIL;
 372
 373         mg_dsk_srst(1);
 374
 375         if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 376                 return ERROR_FAIL;
 377
 378         mg_dsk_srst(0);
 379
 380         if (mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 381                 return ERROR_FAIL;
 382
 383         LOG_INFO("mflash: reset ok");
 384
 385         return ERROR_OK;
 386 }
 387
 388 static int mg_mflash_probe(void)
 389 {
 390         if (mg_mflash_rst() != ERROR_OK)
 391                 return ERROR_FAIL;
 392
 393         if (mg_dsk_drv_info() != ERROR_OK)
 394                 return ERROR_FAIL;
 395
 396         return ERROR_OK;
 397 }
 398
 399 static int mg_probe_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 400 {
 401         int ret;
 402
 403         ret = mg_mflash_probe();
 404
 405         if (ret == ERROR_OK) {
 406                 command_print(cmd_ctx, "mflash (total %u sectors) found at 0x%8.8x",
 407                                 mflash_bank->drv_info->tot_sects, mflash_bank->base );
 408         }
 409
 410         return ret;
 411 }
 412
 413 static int mg_mflash_do_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
 414 {
 415         u32 i, address;
 416         int ret;
 417         target_t *target = mflash_bank->target;
 418         u8 *buff_ptr = buff;
 419         duration_t duration;
 420
 421         if ( mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_read) != ERROR_OK )
 422                 return ERROR_FAIL;
 423
 424         address = mflash_bank->base + MG_BUFFER_OFFSET;
 425
 426         duration_start_measure(&duration);
 427
 428         for (i = 0; i < sect_cnt; i++) {
 429                 mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 430
 431                 target_read_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
 432                 buff_ptr += MG_MFLASH_SECTOR_SIZE;
 433
 434                 target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_read);
 435
 436                 LOG_DEBUG("%u (0x%8.8x) sector read", sect_num + i, (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 437
 438                 duration_stop_measure(&duration, NULL);
 439
 440                 if ((duration.duration.tv_sec * 1000 + duration.duration.tv_usec / 1000) > 3000) {
 441                         LOG_INFO("read %u'th sectors", sect_num + i);
 442                         duration_start_measure(&duration);
 443                 }
 444         }
 445
 446         ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
 447
 448         return ret;
 449 }
 450
 451 static int mg_mflash_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
 452 {
 453         u32 quotient, residue, i;
 454         u8 *buff_ptr = buff;
 455
 456         quotient = sect_cnt >> 8;
 457         residue = sect_cnt % 256;
 458
 459         for (i = 0; i < quotient; i++) {
 460                 LOG_DEBUG("sect num : %u buff : 0x%0lx", sect_num,
 461                         (unsigned long)buff_ptr);
 462                 mg_mflash_do_read_sects(buff_ptr, sect_num, 256);
 463                 sect_num += 256;
 464                 buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 465         }
 466
 467         if (residue) {
 468                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 469                         (unsigned long)buff_ptr);
 470                 mg_mflash_do_read_sects(buff_ptr, sect_num, residue);
 471         }
 472
 473         return ERROR_OK;
 474 }
 475
 476 static int mg_mflash_do_write_sects(void *buff, u32 sect_num, u32 sect_cnt,
 477                 mg_io_type_cmd cmd)
 478 {
 479         u32 i, address;
 480         int ret;
 481         target_t *target = mflash_bank->target;
 482         u8 *buff_ptr = buff;
 483         duration_t duration;
 484
 485         if ( mg_dsk_io_cmd(sect_num, sect_cnt, cmd) != ERROR_OK )
 486                 return ERROR_FAIL;
 487
 488         address = mflash_bank->base + MG_BUFFER_OFFSET;
 489
 490         duration_start_measure(&duration);
 491
 492         for (i = 0; i < sect_cnt; i++) {
 493                 ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 494                 if (ret != ERROR_OK)
 495                         LOG_ERROR("mg_io_wait_drq time out");
 496
 497                 ret = target_write_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
 498                 if (ret != ERROR_OK)
 499                         LOG_ERROR("mem write error");
 500                 buff_ptr += MG_MFLASH_SECTOR_SIZE;
 501
 502                 ret = target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_write);
 503                 if (ret != ERROR_OK)
 504                         LOG_ERROR("mg_io_cmd_confirm_write error");
 505
 506                 LOG_DEBUG("%u (0x%8.8x) sector write", sect_num + i, (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 507
 508                 duration_stop_measure(&duration, NULL);
 509
 510                 if ((duration.duration.tv_sec * 1000 + duration.duration.tv_usec / 1000) > 3000) {
 511                         LOG_INFO("wrote %u'th sectors", sect_num + i);
 512                         duration_start_measure(&duration);
 513                 }
 514         }
 515
 516         if (cmd == mg_io_cmd_write)
 517                 ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
 518         else
 519                 ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_LONG);
 520
 521         return ret;
 522 }
 523
 524 static int mg_mflash_write_sects(void *buff, u32 sect_num, u32 sect_cnt)
 525 {
 526         u32 quotient, residue, i;
 527         u8 *buff_ptr = buff;
 528
 529         quotient = sect_cnt >> 8;
 530         residue = sect_cnt % 256;
 531
 532         for (i = 0; i < quotient; i++) {
 533                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 534                         (unsigned long)buff_ptr);
 535                 mg_mflash_do_write_sects(buff_ptr, sect_num, 256, mg_io_cmd_write);
 536                 sect_num += 256;
 537                 buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 538         }
 539
 540         if (residue) {
 541                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 542                         (unsigned long)buff_ptr);
 543                 mg_mflash_do_write_sects(buff_ptr, sect_num, residue, mg_io_cmd_write);
 544         }
 545
 546         return ERROR_OK;
 547 }
 548
 549 static int mg_mflash_read (u32 addr, u8 *buff, u32 len)
 550 {
 551         u8 *sect_buff, *buff_ptr = buff;
 552         u32 cur_addr, next_sec_addr, end_addr, cnt, sect_num;
 553
 554         cnt = 0;
 555         cur_addr = addr;
 556         end_addr = addr + len;
 557
 558         sect_buff = malloc(MG_MFLASH_SECTOR_SIZE);
 559
 560         if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 561
 562                 next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 563                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 564                 mg_mflash_read_sects(sect_buff, sect_num, 1);
 565
 566                 if (end_addr < next_sec_addr) {
 567                         memcpy(buff_ptr, sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), end_addr - cur_addr);
 568                         LOG_DEBUG("copies %u byte from sector offset 0x%8.8x", end_addr - cur_addr, cur_addr);
 569                         cur_addr = end_addr;
 570                 } else {
 571                         memcpy(buff_ptr, sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), next_sec_addr - cur_addr);
 572                         LOG_DEBUG("copies %u byte from sector offset 0x%8.8x", next_sec_addr - cur_addr, cur_addr);
 573                         buff_ptr += (next_sec_addr - cur_addr);
 574                         cur_addr = next_sec_addr;
 575                 }
 576         }
 577
 578         if (cur_addr < end_addr) {
 579
 580                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 581                 next_sec_addr = cur_addr + MG_MFLASH_SECTOR_SIZE;
 582
 583                 while (next_sec_addr <= end_addr) {
 584                         cnt++;
 585                         next_sec_addr += MG_MFLASH_SECTOR_SIZE;
 586                 }
 587
 588                 if (cnt)
 589                         mg_mflash_read_sects(buff_ptr, sect_num, cnt);
 590
 591                 buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 592                 cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
 593
 594                 if (cur_addr < end_addr) {
 595
 596                         sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 597                         mg_mflash_read_sects(sect_buff, sect_num, 1);
 598                         memcpy(buff_ptr, sect_buff, end_addr - cur_addr);
 599                         LOG_DEBUG("copies %u byte", end_addr - cur_addr);
 600
 601                 }
 602         }
 603
 604         free(sect_buff);
 605
 606         return ERROR_OK;
 607 }
 608
 609 static int mg_mflash_write(u32 addr, u8 *buff, u32 len)
 610 {
 611         u8 *sect_buff, *buff_ptr = buff;
 612         u32 cur_addr, next_sec_addr, end_addr, cnt, sect_num;
 613
 614         cnt = 0;
 615         cur_addr = addr;
 616         end_addr = addr + len;
 617
 618         sect_buff = malloc(MG_MFLASH_SECTOR_SIZE);
 619
 620         if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 621
 622                 next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 623                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 624                 mg_mflash_read_sects(sect_buff, sect_num, 1);
 625
 626                 if (end_addr < next_sec_addr) {
 627                         memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), buff_ptr, end_addr - cur_addr);
 628                         LOG_DEBUG("copies %u byte to sector offset 0x%8.8x", end_addr - cur_addr, cur_addr);
 629                         cur_addr = end_addr;
 630                 } else {
 631                         memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), buff_ptr, next_sec_addr - cur_addr);
 632                         LOG_DEBUG("copies %u byte to sector offset 0x%8.8x", next_sec_addr - cur_addr, cur_addr);
 633                         buff_ptr += (next_sec_addr - cur_addr);
 634                         cur_addr = next_sec_addr;
 635                 }
 636
 637                 mg_mflash_write_sects(sect_buff, sect_num, 1);
 638         }
 639
 640         if (cur_addr < end_addr) {
 641
 642                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 643                 next_sec_addr = cur_addr + MG_MFLASH_SECTOR_SIZE;
 644
 645                 while (next_sec_addr <= end_addr) {
 646                         cnt++;
 647                         next_sec_addr += MG_MFLASH_SECTOR_SIZE;
 648                 }
 649
 650                 if (cnt)
 651                         mg_mflash_write_sects(buff_ptr, sect_num, cnt);
 652
 653                 buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 654                 cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
 655
 656                 if (cur_addr < end_addr) {
 657
 658                         sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 659                         mg_mflash_read_sects(sect_buff, sect_num, 1);
 660                         memcpy(sect_buff, buff_ptr, end_addr - cur_addr);
 661                         LOG_DEBUG("copies %u byte", end_addr - cur_addr);
 662                         mg_mflash_write_sects(sect_buff, sect_num, 1);
 663                 }
 664         }
 665
 666         free(sect_buff);
 667
 668         return ERROR_OK;
 669 }
 670
 671 static int mg_write_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 672 {
 673         u32 address, buf_cnt, cnt, res, i;
 674         u8 *buffer;
 675         fileio_t fileio;
 676         duration_t duration;
 677         char *duration_text;
 678
 679         if (argc != 3) {
 680                 return ERROR_COMMAND_SYNTAX_ERROR;
 681         }
 682
 683         address = strtoul(args[2], NULL, 0);
 684
 685         if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK) {
 686                 return ERROR_FAIL;
 687         }
 688
 689         buffer = malloc(MG_FILEIO_CHUNK);
 690         if (!buffer) {
 691                 fileio_close(&fileio);
 692                 return ERROR_FAIL;
 693         }
 694
 695         cnt = fileio.size / MG_FILEIO_CHUNK;
 696         res = fileio.size % MG_FILEIO_CHUNK;
 697
 698         duration_start_measure(&duration);
 699
 700         for (i = 0; i < cnt; i++) {
 701                 if (fileio_read(&fileio, MG_FILEIO_CHUNK, buffer, &buf_cnt) !=
 702                                 ERROR_OK)
 703                         goto mg_write_cmd_err;
 704                 if (mg_mflash_write(address, buffer, MG_FILEIO_CHUNK) != ERROR_OK)
 705                         goto mg_write_cmd_err;
 706                 address += MG_FILEIO_CHUNK;
 707         }
 708
 709         if (res) {
 710                 if (fileio_read(&fileio, res, buffer, &buf_cnt) != ERROR_OK)
 711                         goto mg_write_cmd_err;
 712                 if (mg_mflash_write(address, buffer, res) != ERROR_OK)
 713                         goto mg_write_cmd_err;
 714         }
 715
 716         duration_stop_measure(&duration, &duration_text);
 717
 718         command_print(cmd_ctx, "wrote %lli byte from file %s in %s (%f kB/s)",
 719                 fileio.size, args[1], duration_text,
 720                 (float)fileio.size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
 721
 722         free(duration_text);
 723         free(buffer);
 724         fileio_close(&fileio);
 725
 726         return ERROR_OK;
 727
 728 mg_write_cmd_err:
 729         duration_stop_measure(&duration, &duration_text);
 730         free(duration_text);
 731         free(buffer);
 732         fileio_close(&fileio);
 733
 734         return ERROR_FAIL;
 735 }
 736
 737 static int mg_dump_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 738 {
 739         u32 address, size_written, size, cnt, res, i;
 740         u8 *buffer;
 741         fileio_t fileio;
 742         duration_t duration;
 743         char *duration_text;
 744
 745         if (argc != 4) {
 746                 return ERROR_COMMAND_SYNTAX_ERROR;
 747         }
 748
 749         address = strtoul(args[2], NULL, 0);
 750         size = strtoul(args[3], NULL, 0);
 751
 752         if (fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK) {
 753                 return ERROR_FAIL;
 754         }
 755
 756         buffer = malloc(MG_FILEIO_CHUNK);
 757         if (!buffer) {
 758                 fileio_close(&fileio);
 759                 return ERROR_FAIL;
 760         }
 761
 762         cnt = size / MG_FILEIO_CHUNK;
 763         res = size % MG_FILEIO_CHUNK;
 764
 765         duration_start_measure(&duration);
 766
 767         for (i = 0; i < cnt; i++) {
 768                 if (mg_mflash_read(address, buffer, MG_FILEIO_CHUNK) != ERROR_OK)
 769                         goto mg_dump_cmd_err;
 770                 if (fileio_write(&fileio, MG_FILEIO_CHUNK, buffer, &size_written)
 771                                 != ERROR_OK)
 772                         goto mg_dump_cmd_err;
 773                 address += MG_FILEIO_CHUNK;
 774         }
 775
 776         if (res) {
 777                 if (mg_mflash_read(address, buffer, res) != ERROR_OK)
 778                         goto mg_dump_cmd_err;
 779                 if (fileio_write(&fileio, res, buffer, &size_written) != ERROR_OK)
 780                         goto mg_dump_cmd_err;
 781         }
 782
 783         duration_stop_measure(&duration, &duration_text);
 784
 785         command_print(cmd_ctx, "dump image (address 0x%8.8x size %u) to file %s in %s (%f kB/s)",
 786                                 address, size, args[1], duration_text,
 787                                 (float)size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
 788
 789         free(duration_text);
 790         free(buffer);
 791         fileio_close(&fileio);
 792
 793         return ERROR_OK;
 794
 795 mg_dump_cmd_err:
 796         duration_stop_measure(&duration, &duration_text);
 797         free(duration_text);
 798         free(buffer);
 799         fileio_close(&fileio);
 800
 801         return ERROR_FAIL;
 802 }
 803
 804 static int mg_set_feature(mg_feature_id feature, mg_feature_val config)
 805 {
 806         target_t *target = mflash_bank->target;
 807         u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 808
 809         if (mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL)
 810                         != ERROR_OK)
 811                 return ERROR_FAIL;
 812
 813         target_write_u8(target, mg_task_reg + MG_REG_FEATURE, feature);
 814         target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, config);
 815         target_write_u8(target, mg_task_reg + MG_REG_COMMAND,
 816                         mg_io_cmd_set_feature);
 817
 818         return ERROR_OK;
 819 }
 820
 821 static int mg_is_valid_pll(double XIN, int N, double CLK_OUT, int NO)
 822 {
 823         double v1 = XIN / N;
 824         double v2 = CLK_OUT * NO;
 825
 826         if (v1 <1000000 || v1 > 15000000 || v2 < 100000000 || v2 > 500000000)
 827                 return ERROR_FAIL;
 828
 829         return ERROR_OK;
 830 }
 831
 832 static int mg_pll_get_M(unsigned short feedback_div)
 833 {
 834         int i, M;
 835
 836         for (i = 1, M=0; i < 512; i <<= 1, feedback_div >>= 1)
 837                 M += (feedback_div & 1) * i;
 838
 839         return M + 2;
 840 }
 841
 842 static int mg_pll_get_N(unsigned char input_div)
 843 {
 844         int i, N;
 845
 846         for (i = 1, N = 0; i < 32; i <<= 1, input_div >>= 1)
 847                 N += (input_div & 1) * i;
 848
 849         return N + 2;
 850 }
 851
 852 static int mg_pll_get_NO(unsigned char  output_div)
 853 {
 854         int i, NO;
 855
 856         for (i = 0, NO = 1; i < 2; ++i, output_div >>= 1)
 857                 if(output_div & 1)
 858                         NO = NO << 1;
 859
 860         return NO;
 861 }
 862
 863 static double mg_do_calc_pll(double XIN, mg_pll_t * p_pll_val, int is_approximate)
 864 {
 865         unsigned short i;
 866         unsigned char  j, k;
 867         int            M, N, NO;
 868         double CLK_OUT;
 869         double DIV = 1;
 870         double ROUND = 0;
 871
 872         if (is_approximate) {
 873                 DIV   = 1000000;
 874                 ROUND = 500000;
 875         }
 876
 877         for (i = 0; i < MG_PLL_MAX_FEEDBACKDIV_VAL ; ++i) {
 878                 M  = mg_pll_get_M(i);
 879
 880                 for (j = 0; j < MG_PLL_MAX_INPUTDIV_VAL ; ++j) {
 881                         N  = mg_pll_get_N(j);
 882
 883                         for (k = 0; k < MG_PLL_MAX_OUTPUTDIV_VAL ; ++k) {
 884                                 NO = mg_pll_get_NO(k);
 885
 886                                 CLK_OUT = XIN * ((double)M / N) / NO;
 887
 888                                 if ((int)((CLK_OUT+ROUND) / DIV)
 889                                                 == (int)(MG_PLL_CLK_OUT / DIV)) {
 890                                         if (mg_is_valid_pll(XIN, N, CLK_OUT, NO) == ERROR_OK)
 891                                         {
 892                                                 p_pll_val->lock_cyc = (int)(XIN * MG_PLL_STD_LOCKCYCLE / MG_PLL_STD_INPUTCLK);
 893                                                 p_pll_val->feedback_div = i;
 894                                                 p_pll_val->input_div = j;
 895                                                 p_pll_val->output_div = k;
 896
 897                                                 return CLK_OUT;
 898                                         }
 899                                 }
 900                         }
 901                 }
 902         }
 903
 904         return 0;
 905 }
 906
 907 static double mg_calc_pll(double XIN, mg_pll_t *p_pll_val)
 908 {
 909         double CLK_OUT;
 910
 911         CLK_OUT = mg_do_calc_pll(XIN, p_pll_val, 0);
 912
 913         if (!CLK_OUT)
 914                 return mg_do_calc_pll(XIN, p_pll_val, 1);
 915         else
 916                 return CLK_OUT;
 917 }
 918
 919 static int mg_verify_interface(void)
 920 {
 921         u16 buff[MG_MFLASH_SECTOR_SIZE >> 1];
 922         u16 i, j;
 923         u32 address = mflash_bank->base + MG_BUFFER_OFFSET;
 924         target_t *target = mflash_bank->target;
 925
 926         for (j = 0; j < 10; j++) {
 927                 for (i = 0; i < MG_MFLASH_SECTOR_SIZE >> 1; i++)
 928                         buff[i] = i;
 929
 930                 target_write_memory(target, address, 2,
 931                                 MG_MFLASH_SECTOR_SIZE / 2, (u8 *)buff);
 932
 933                 memset(buff, 0xff, MG_MFLASH_SECTOR_SIZE);
 934
 935                 target_read_memory(target, address, 2,
 936                                 MG_MFLASH_SECTOR_SIZE / 2, (u8 *)buff);
 937
 938                 for (i = 0; i < MG_MFLASH_SECTOR_SIZE >> 1; i++) {
 939                         if (buff[i] != i) {
 940                                 LOG_ERROR("mflash: verify interface fail");
 941                                 return ERROR_FAIL;
 942                         }
 943                 }
 944         }
 945
 946         LOG_INFO("mflash: verify interface ok");
 947         return ERROR_OK;
 948 }
 949
 950 static const char g_strSEG_SerialNum[20] = {
 951         'G','m','n','i','-','e','e','S','g','a','e','l',
 952         0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20
 953 };
 954
 955 static const char g_strSEG_FWRev[8] = {
 956         'F','X','L','T','2','v','0','.'
 957 };
 958
 959 static const char g_strSEG_ModelNum[40] = {
 960         'F','X','A','L','H','S','2',0x20,'0','0','s','7',
 961         0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,
 962         0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,
 963         0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20
 964 };
 965
 966 static void mg_gen_ataid(mg_io_type_drv_info *pSegIdDrvInfo)
 967 {
 968         /* b15 is ATA device(0) , b7 is Removable Media Device */
 969         pSegIdDrvInfo->general_configuration            = 0x045A;
 970         /* 128MB :   Cylinder=> 977 , Heads=> 8 ,  Sectors=> 32
 971          * 256MB :   Cylinder=> 980 , Heads=> 16 , Sectors=> 32
 972          * 384MB :   Cylinder=> 745 , Heads=> 16 , Sectors=> 63
 973          */
 974         pSegIdDrvInfo->number_of_cylinders              = 0x02E9;
 975         pSegIdDrvInfo->reserved1                        = 0x0;
 976         pSegIdDrvInfo->number_of_heads                  = 0x10;
 977         pSegIdDrvInfo->unformatted_bytes_per_track      = 0x0;
 978         pSegIdDrvInfo->unformatted_bytes_per_sector     = 0x0;
 979         pSegIdDrvInfo->sectors_per_track                = 0x3F;
 980         pSegIdDrvInfo->vendor_unique1[0]                = 0x000B;
 981         pSegIdDrvInfo->vendor_unique1[1]                = 0x7570;
 982         pSegIdDrvInfo->vendor_unique1[2]                = 0x8888;
 983
 984         memcpy(pSegIdDrvInfo->serial_number, (void *)g_strSEG_SerialNum,20);
 985         /* 0x2 : dual buffer */
 986         pSegIdDrvInfo->buffer_type                      = 0x2;
 987         /* buffer size : 2KB */
 988         pSegIdDrvInfo->buffer_sector_size               = 0x800;
 989         pSegIdDrvInfo->number_of_ecc_bytes              = 0;
 990
 991         memcpy(pSegIdDrvInfo->firmware_revision, (void *)g_strSEG_FWRev,8);
 992
 993         memcpy(pSegIdDrvInfo->model_number, (void *)g_strSEG_ModelNum,40);
 994
 995         pSegIdDrvInfo->maximum_block_transfer           = 0x4;
 996         pSegIdDrvInfo->vendor_unique2                   = 0x0;
 997         pSegIdDrvInfo->dword_io                         = 0x00;
 998         /* b11 : IORDY support(PIO Mode 4), b10 : Disable/Enbale IORDY
 999          * b9  : LBA support, b8 : DMA mode support
1000          */
1001         pSegIdDrvInfo->capabilities                     = 0x1 << 9;
1002
1003         pSegIdDrvInfo->reserved2                        = 0x4000;
1004         pSegIdDrvInfo->vendor_unique3                   = 0x00;
1005         /* PIOMode-2 support */
1006         pSegIdDrvInfo->pio_cycle_timing_mode            = 0x02;
1007         pSegIdDrvInfo->vendor_unique4                   = 0x00;
1008         /* MultiWord-2 support */
1009         pSegIdDrvInfo->dma_cycle_timing_mode            = 0x00;
1010         /* b1 : word64~70 is valid
1011          * b0 : word54~58 are valid and reflect the current numofcyls,heads,sectors
1012          * b2 : If device supports Ultra DMA , set to one to vaildate word88
1013          */
1014         pSegIdDrvInfo->translation_fields_valid         = (0x1 << 1) | (0x1 << 0);
1015         pSegIdDrvInfo->number_of_current_cylinders      = 0x02E9;
1016         pSegIdDrvInfo->number_of_current_heads          = 0x10;
1017         pSegIdDrvInfo->current_sectors_per_track        = 0x3F;
1018         pSegIdDrvInfo->current_sector_capacity_lo       = 0x7570;
1019         pSegIdDrvInfo->current_sector_capacity_hi       = 0x000B;
1020
1021         pSegIdDrvInfo->multi_sector_count                       = 0x04;
1022         /* b8 : Multiple secotr setting valid , b[7:0] num of secotrs per block */
1023         pSegIdDrvInfo->multi_sector_setting_valid               = 0x01;
1024         pSegIdDrvInfo->total_user_addressable_sectors_lo        = 0x7570;
1025         pSegIdDrvInfo->total_user_addressable_sectors_hi        = 0x000B;
1026         pSegIdDrvInfo->single_dma_modes_supported               = 0x00;
1027         pSegIdDrvInfo->single_dma_transfer_active               = 0x00;
1028         /* b2 :Multi-word DMA mode 2, b1 : Multi-word DMA mode 1 */
1029         pSegIdDrvInfo->multi_dma_modes_supported                = (0x1 << 0);
1030         /* b2 :Multi-word DMA mode 2, b1 : Multi-word DMA mode 1 */
1031         pSegIdDrvInfo->multi_dma_transfer_active                = (0x1 << 0);
1032         /* b0 : PIO Mode-3 support, b1 : PIO Mode-4 support */
1033         pSegIdDrvInfo->adv_pio_mode                             = 0x00;
1034         /* 480(0x1E0)nsec for Multi-word DMA mode0
1035          * 150(0x96) nsec for Multi-word DMA mode1
1036          * 120(0x78) nsec for Multi-word DMA mode2
1037          */
1038         pSegIdDrvInfo->min_dma_cyc                      = 0x1E0;
1039         pSegIdDrvInfo->recommend_dma_cyc                = 0x1E0;
1040         pSegIdDrvInfo->min_pio_cyc_no_iordy             = 0x1E0;
1041         pSegIdDrvInfo->min_pio_cyc_with_iordy           = 0x1E0;
1042         memset((void *)pSegIdDrvInfo->reserved3, 0x00, 22);
1043         /* b7 : ATA/ATAPI-7 ,b6 : ATA/ATAPI-6 ,b5 : ATA/ATAPI-5,b4 : ATA/ATAPI-4 */
1044         pSegIdDrvInfo->major_ver_num                    = 0x7E;
1045         /* 0x1C : ATA/ATAPI-6 T13 1532D revision1 */
1046         pSegIdDrvInfo->minor_ver_num                    = 0x19;
1047         /* NOP/READ BUFFER/WRITE BUFFER/Power management feature set support */
1048         pSegIdDrvInfo->feature_cmd_set_suprt0           = 0x7068;
1049         /* Features/command set is valid/Advanced Pwr management/CFA feature set
1050          * not support
1051          */
1052         pSegIdDrvInfo->feature_cmd_set_suprt1           = 0x400C;
1053         pSegIdDrvInfo->feature_cmd_set_suprt2           = 0x4000;
1054         /* READ/WRITE BUFFER/PWR Management enable */
1055         pSegIdDrvInfo->feature_cmd_set_en0              = 0x7000;
1056         /* CFA feature is disabled / Advancde power management disable */
1057         pSegIdDrvInfo->feature_cmd_set_en1              = 0x0;
1058         pSegIdDrvInfo->feature_cmd_set_en2              = 0x4000;
1059         pSegIdDrvInfo->reserved4                        = 0x0;
1060         /* 0x1 * 2minutes */
1061         pSegIdDrvInfo->req_time_for_security_er_done    = 0x19;
1062         pSegIdDrvInfo->req_time_for_enhan_security_er_done      = 0x19;
1063         /* Advanced power management level 1 */
1064         pSegIdDrvInfo->adv_pwr_mgm_lvl_val                      = 0x0;
1065         pSegIdDrvInfo->reserved5                        = 0x0;
1066         memset((void *)pSegIdDrvInfo->reserved6, 0x00, 68);
1067         /* Security mode feature is disabled */
1068         pSegIdDrvInfo->security_stas                    = 0x0;
1069         memset((void *)pSegIdDrvInfo->vendor_uniq_bytes, 0x00, 62);
1070         /* CFA power mode 1 support in maximum 200mA */
1071         pSegIdDrvInfo->cfa_pwr_mode                     = 0x0100;
1072         memset((void *)pSegIdDrvInfo->reserved7, 0x00, 190);
1073 }
1074
1075 static int mg_storage_config(void)
1076 {
1077         u8 buff[512];
1078
1079         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
1080                         != ERROR_OK)
1081                 return ERROR_FAIL;
1082
1083         mg_gen_ataid((mg_io_type_drv_info *)buff);
1084
1085         if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_stgdrvinfo)
1086                         != ERROR_OK)
1087                 return ERROR_FAIL;
1088
1089         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
1090                         != ERROR_OK)
1091                 return ERROR_FAIL;
1092
1093         LOG_INFO("mflash: storage config ok");
1094         return ERROR_OK;
1095 }
1096
1097 static int mg_boot_config(void)
1098 {
1099         u8 buff[512];
1100
1101         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
1102                         != ERROR_OK)
1103                 return ERROR_FAIL;
1104
1105         memset(buff, 0xff, 512);
1106
1107         buff[0] = mg_op_mode_snd;               /* operation mode */
1108         buff[1] = MG_UNLOCK_OTP_AREA;
1109         buff[2] = 4;                            /* boot size */
1110         *((u32 *)(buff + 4)) = 0;               /* XIP size */
1111
1112         if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_xipinfo)
1113                         != ERROR_OK)
1114                 return ERROR_FAIL;
1115
1116         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
1117                         != ERROR_OK)
1118                 return ERROR_FAIL;
1119
1120         LOG_INFO("mflash: boot config ok");
1121         return ERROR_OK;
1122 }
1123
1124 static int mg_set_pll(mg_pll_t *pll)
1125 {
1126         u8 buff[512];
1127
1128         memset(buff, 0xff, 512);
1129         /* PLL Lock cycle and Feedback 9bit Divider */
1130         memcpy(buff, &pll->lock_cyc, sizeof(u32));
1131         memcpy(buff + 4, &pll->feedback_div, sizeof(u16));
1132         buff[6] = pll->input_div;               /* PLL Input 5bit Divider */
1133         buff[7] = pll->output_div;              /* PLL Output Divider */
1134
1135         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
1136                         != ERROR_OK)
1137                 return ERROR_FAIL;
1138
1139         if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_wr_pll)
1140                         != ERROR_OK)
1141                 return ERROR_FAIL;
1142
1143         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
1144                         != ERROR_OK)
1145                 return ERROR_FAIL;
1146
1147         LOG_INFO("mflash: set pll ok");
1148         return ERROR_OK;
1149 }
1150
1151 static int mg_erase_nand(void)
1152 {
1153         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
1154                         != ERROR_OK)
1155                 return ERROR_FAIL;
1156
1157         if (mg_mflash_do_write_sects(NULL, 0, 0, mg_vcmd_purge_nand)
1158                         != ERROR_OK)
1159                 return ERROR_FAIL;
1160
1161         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
1162                         != ERROR_OK)
1163                 return ERROR_FAIL;
1164
1165         LOG_INFO("mflash: erase nand ok");
1166         return ERROR_OK;
1167 }
1168
1169 int mg_config_cmd(struct command_context_s *cmd_ctx, char *cmd,
1170                 char **args, int argc)
1171 {
1172         double fin, fout;
1173         mg_pll_t pll;
1174
1175         if (mg_verify_interface() != ERROR_OK)
1176                 return ERROR_FAIL;
1177
1178         if (mg_mflash_rst() != ERROR_OK)
1179                 return ERROR_FAIL;
1180
1181         switch (argc) {
1182                 case 2:
1183                         if (!strcmp(args[1], "boot")) {
1184                                 if (mg_boot_config() != ERROR_OK)
1185                                         return ERROR_FAIL;
1186
1187                                 return ERROR_OK;
1188                         } else if (!strcmp(args[1], "storage")) {
1189                                 if (mg_storage_config() != ERROR_OK)
1190                                         return ERROR_FAIL;
1191
1192                                 return ERROR_OK;
1193                         } else
1194                                 return ERROR_COMMAND_NOTFOUND;
1195                         break;
1196                 case 3:
1197                         if (!strcmp(args[1], "pll")) {
1198                                 fin = strtoul(args[2], NULL, 0);
1199
1200                                 if (fin > MG_PLL_CLK_OUT)
1201                                         return ERROR_FAIL;
1202
1203                                 fout = mg_calc_pll(fin, &pll);
1204
1205                                 if (!fout)
1206                                         return ERROR_FAIL;
1207
1208                                 LOG_INFO("mflash: Fout=%u Hz, feedback=%u,"
1209                                                 "indiv=%u, outdiv=%u, lock=%u",
1210                                                 (u32)fout, pll.feedback_div,
1211                                                 pll.input_div, pll.output_div,
1212                                                 pll.lock_cyc);
1213
1214                                 if (mg_erase_nand() != ERROR_OK)
1215                                         return ERROR_FAIL;
1216
1217                                 if (mg_set_pll(&pll) != ERROR_OK)
1218                                         return ERROR_FAIL;
1219
1220                                 return ERROR_OK;
1221                         } else
1222                                 return ERROR_COMMAND_NOTFOUND;
1223                         break;
1224                 default:
1225                         return ERROR_COMMAND_SYNTAX_ERROR;
1226         }
1227
1228         return ERROR_OK;
1229 }
1230
1231 int mflash_init_drivers(struct command_context_s *cmd_ctx)
1232 {
1233         if (mflash_bank) {
1234                 register_command(cmd_ctx, mflash_cmd, "probe", mg_probe_cmd, COMMAND_EXEC, NULL);
1235                 register_command(cmd_ctx, mflash_cmd, "write", mg_write_cmd, COMMAND_EXEC,
1236                                 "mflash write <num> <file> <address>");
1237                 register_command(cmd_ctx, mflash_cmd, "dump", mg_dump_cmd, COMMAND_EXEC,
1238                                                 "mflash dump <num> <file> <address> <size>");
1239                 register_command(cmd_ctx, mflash_cmd, "config", mg_config_cmd,
1240                                 COMMAND_EXEC, "mflash config <num> <stage>");
1241         }
1242
1243         return ERROR_OK;
1244 }
1245
1246 static int mg_bank_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1247 {
1248         target_t *target;
1249         char *str;
1250         int i;
1251
1252         if (argc < 4)
1253         {
1254                 return ERROR_COMMAND_SYNTAX_ERROR;
1255         }
1256
1257         if ((target = get_target(args[3])) == NULL)
1258         {
1259                 LOG_ERROR("target '%s' not defined", args[3]);
1260                 return ERROR_FAIL;
1261         }
1262
1263         mflash_bank = calloc(sizeof(mflash_bank_t), 1);
1264         mflash_bank->base = strtoul(args[1], NULL, 0);
1265         mflash_bank->rst_pin.num = strtoul(args[2], &str, 0);
1266         if (*str)
1267                 mflash_bank->rst_pin.port[0] = (u16)tolower(str[0]);
1268
1269         mflash_bank->target = target;
1270
1271         for (i = 0; mflash_gpio[i] ; i++) {
1272                 if (! strcmp(mflash_gpio[i]->name, args[0])) {
1273                         mflash_bank->gpio_drv = mflash_gpio[i];
1274                 }
1275         }
1276
1277         if (! mflash_bank->gpio_drv) {
1278                 LOG_ERROR("%s is unsupported soc", args[0]);
1279                 return ERROR_INVALID_ARGUMENTS;
1280         }
1281
1282         return ERROR_OK;
1283 }
1284
1285 int mflash_register_commands(struct command_context_s *cmd_ctx)
1286 {
1287         mflash_cmd = register_command(cmd_ctx, NULL, "mflash", NULL, COMMAND_ANY, NULL);
1288         register_command(cmd_ctx, mflash_cmd, "bank", mg_bank_cmd, COMMAND_CONFIG,
1289                         "mflash bank <soc> <base> <RST pin> <target #>");
1290         return ERROR_OK;
1291 }