git.gag.com Git - fw/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->type->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->type->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->type->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;
 674         u8 *buffer;
 675         /* TODO : multi-bank support, large file support */
 676         fileio_t fileio;
 677         duration_t duration;
 678         char *duration_text;
 679         int ret;
 680
 681         if (argc != 3) {
 682                 return ERROR_COMMAND_SYNTAX_ERROR;
 683         }
 684
 685         address = strtoul(args[2], NULL, 0);
 686
 687         if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK) {
 688                 return ERROR_FAIL;
 689         }
 690
 691         buffer = malloc(fileio.size);
 692
 693         if (fileio_read(&fileio, fileio.size, buffer, &buf_cnt) != ERROR_OK)
 694         {
 695                 free(buffer);
 696                 fileio_close(&fileio);
 697                 return ERROR_FAIL;
 698         }
 699
 700         duration_start_measure(&duration);
 701
 702         ret = mg_mflash_write(address, buffer, (u32)fileio.size);
 703
 704         duration_stop_measure(&duration, &duration_text);
 705
 706         command_print(cmd_ctx, "wrote %lli byte from file %s in %s (%f kB/s)",
 707                 fileio.size, args[1], duration_text,
 708                 (float)fileio.size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
 709
 710         free(duration_text);
 711
 712         fileio_close(&fileio);
 713
 714         free(buffer);
 715
 716         return ERROR_OK;
 717 }
 718
 719 static int mg_dump_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 720 {
 721         u32 address, size_written, size;
 722         u8 *buffer;
 723         /* TODO : multi-bank support */
 724         fileio_t fileio;
 725         duration_t duration;
 726         char *duration_text;
 727
 728         if (argc != 4) {
 729                 return ERROR_COMMAND_SYNTAX_ERROR;
 730         }
 731
 732         address = strtoul(args[2], NULL, 0);
 733         size = strtoul(args[3], NULL, 0);
 734
 735         if (fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK) {
 736                 return ERROR_FAIL;
 737         }
 738
 739         buffer = malloc(size);
 740
 741         duration_start_measure(&duration);
 742
 743         mg_mflash_read(address, buffer, size);
 744
 745         duration_stop_measure(&duration, &duration_text);
 746
 747         fileio_write(&fileio, size, buffer, &size_written);
 748
 749         command_print(cmd_ctx, "dump image (address 0x%8.8x size %u) to file %s in %s (%f kB/s)",
 750                                 address, size, args[1], duration_text,
 751                                 (float)size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
 752
 753         free(duration_text);
 754
 755         fileio_close(&fileio);
 756
 757         free(buffer);
 758
 759         return ERROR_OK;
 760 }
 761
 762
 763 static int mg_set_feature(mg_feature_id feature, mg_feature_val config)
 764 {
 765         target_t *target = mflash_bank->target;
 766         u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 767
 768         if (mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL)
 769                         != ERROR_OK)
 770                 return ERROR_FAIL;
 771
 772         target_write_u8(target, mg_task_reg + MG_REG_FEATURE, feature);
 773         target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, config);
 774         target_write_u8(target, mg_task_reg + MG_REG_COMMAND,
 775                         mg_io_cmd_set_feature);
 776
 777         return ERROR_OK;
 778 }
 779
 780 static int mg_is_valid_pll(double XIN, int N, double CLK_OUT, int NO)
 781 {
 782         double v1 = XIN / N;
 783         double v2 = CLK_OUT * NO;
 784
 785         if (v1 <1000000 || v1 > 15000000 || v2 < 100000000 || v2 > 500000000)
 786                 return ERROR_FAIL;
 787
 788         return ERROR_OK;
 789 }
 790
 791 static int mg_pll_get_M(unsigned short feedback_div)
 792 {
 793         int i, M;
 794
 795         for (i = 1, M=0; i < 512; i <<= 1, feedback_div >>= 1)
 796                 M += (feedback_div & 1) * i;
 797
 798         return M + 2;
 799 }
 800
 801 static int mg_pll_get_N(unsigned char input_div)
 802 {
 803         int i, N;
 804
 805         for (i = 1, N = 0; i < 32; i <<= 1, input_div >>= 1)
 806                 N += (input_div & 1) * i;
 807
 808         return N + 2;
 809 }
 810
 811 static int mg_pll_get_NO(unsigned char  output_div)
 812 {
 813         int i, NO;
 814
 815         for (i = 0, NO = 1; i < 2; ++i, output_div >>= 1)
 816                 if(output_div & 1)
 817                         NO = NO << 1;
 818
 819         return NO;
 820 }
 821
 822 static double mg_do_calc_pll(double XIN, mg_pll_t * p_pll_val, int is_approximate)
 823 {
 824         unsigned short i;
 825         unsigned char  j, k;
 826         int            M, N, NO;
 827         double CLK_OUT;
 828         double DIV = 1;
 829         double ROUND = 0;
 830
 831         if (is_approximate) {
 832                 DIV   = 1000000;
 833                 ROUND = 500000;
 834         }
 835
 836         for (i = 0; i < MG_PLL_MAX_FEEDBACKDIV_VAL ; ++i) {
 837                 M  = mg_pll_get_M(i);
 838
 839                 for (j = 0; j < MG_PLL_MAX_INPUTDIV_VAL ; ++j) {
 840                         N  = mg_pll_get_N(j);
 841
 842                         for (k = 0; k < MG_PLL_MAX_OUTPUTDIV_VAL ; ++k) {
 843                                 NO = mg_pll_get_NO(k);
 844
 845                                 CLK_OUT = XIN * ((double)M / N) / NO;
 846
 847                                 if ((int)((CLK_OUT+ROUND) / DIV)
 848                                                 == (int)(MG_PLL_CLK_OUT / DIV)) {
 849                                         if (mg_is_valid_pll(XIN, N, CLK_OUT, NO) == ERROR_OK)
 850                                         {
 851                                                 p_pll_val->lock_cyc = (int)(XIN * MG_PLL_STD_LOCKCYCLE / MG_PLL_STD_INPUTCLK);
 852                                                 p_pll_val->feedback_div = i;
 853                                                 p_pll_val->input_div = j;
 854                                                 p_pll_val->output_div = k;
 855
 856                                                 return CLK_OUT;
 857                                         }
 858                                 }
 859                         }
 860                 }
 861         }
 862
 863         return 0;
 864 }
 865
 866 static double mg_calc_pll(double XIN, mg_pll_t *p_pll_val)
 867 {
 868         double CLK_OUT;
 869
 870         CLK_OUT = mg_do_calc_pll(XIN, p_pll_val, 0);
 871
 872         if (!CLK_OUT)
 873                 return mg_do_calc_pll(XIN, p_pll_val, 1);
 874         else
 875                 return CLK_OUT;
 876 }
 877
 878 static int mg_verify_interface(void)
 879 {
 880         u16 buff[MG_MFLASH_SECTOR_SIZE >> 1];
 881         u16 i, j;
 882         u32 address = mflash_bank->base + MG_BUFFER_OFFSET;
 883         target_t *target = mflash_bank->target;
 884
 885         for (j = 0; j < 10; j++) {
 886                 for (i = 0; i < MG_MFLASH_SECTOR_SIZE >> 1; i++)
 887                         buff[i] = i;
 888
 889                 target->type->write_memory(target, address, 2,
 890                                 MG_MFLASH_SECTOR_SIZE / 2, (u8 *)buff);
 891
 892                 memset(buff, 0xff, MG_MFLASH_SECTOR_SIZE);
 893
 894                 target->type->read_memory(target, address, 2,
 895                                 MG_MFLASH_SECTOR_SIZE / 2, (u8 *)buff);
 896
 897                 for (i = 0; i < MG_MFLASH_SECTOR_SIZE >> 1; i++) {
 898                         if (buff[i] != i) {
 899                                 LOG_ERROR("mflash: verify interface fail");
 900                                 return ERROR_FAIL;
 901                         }
 902                 }
 903         }
 904
 905         LOG_INFO("mflash: verify interface ok");
 906         return ERROR_OK;
 907 }
 908
 909 static const char g_strSEG_SerialNum[20] = {
 910         'G','m','n','i','-','e','e','S','g','a','e','l',
 911         0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20
 912 };
 913
 914 static const char g_strSEG_FWRev[8] = {
 915         'F','X','L','T','2','v','0','.'
 916 };
 917
 918 static const char g_strSEG_ModelNum[40] = {
 919         'F','X','A','L','H','S','2',0x20,'0','0','s','7',
 920         0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,
 921         0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20,
 922         0x20,0x20,0x20,0x20,0x20,0x20,0x20,0x20
 923 };
 924
 925 static void mg_gen_ataid(mg_io_type_drv_info *pSegIdDrvInfo)
 926 {
 927         /* b15 is ATA device(0) , b7 is Removable Media Device */
 928         pSegIdDrvInfo->general_configuration            = 0x045A;
 929         /* 128MB :   Cylinder=> 977 , Heads=> 8 ,  Sectors=> 32
 930          * 256MB :   Cylinder=> 980 , Heads=> 16 , Sectors=> 32
 931          * 384MB :   Cylinder=> 745 , Heads=> 16 , Sectors=> 63
 932          */
 933         pSegIdDrvInfo->number_of_cylinders              = 0x02E9;
 934         pSegIdDrvInfo->reserved1                        = 0x0;
 935         pSegIdDrvInfo->number_of_heads                  = 0x10;
 936         pSegIdDrvInfo->unformatted_bytes_per_track      = 0x0;
 937         pSegIdDrvInfo->unformatted_bytes_per_sector     = 0x0;
 938         pSegIdDrvInfo->sectors_per_track                = 0x3F;
 939         pSegIdDrvInfo->vendor_unique1[0]                = 0x000B;
 940         pSegIdDrvInfo->vendor_unique1[1]                = 0x7570;
 941         pSegIdDrvInfo->vendor_unique1[2]                = 0x8888;
 942
 943         memcpy(pSegIdDrvInfo->serial_number, (void *)g_strSEG_SerialNum,20);
 944         /* 0x2 : dual buffer */
 945         pSegIdDrvInfo->buffer_type                      = 0x2;
 946         /* buffer size : 2KB */
 947         pSegIdDrvInfo->buffer_sector_size               = 0x800;
 948         pSegIdDrvInfo->number_of_ecc_bytes              = 0;
 949
 950         memcpy(pSegIdDrvInfo->firmware_revision, (void *)g_strSEG_FWRev,8);
 951
 952         memcpy(pSegIdDrvInfo->model_number, (void *)g_strSEG_ModelNum,40);
 953
 954         pSegIdDrvInfo->maximum_block_transfer           = 0x4;
 955         pSegIdDrvInfo->vendor_unique2                   = 0x0;
 956         pSegIdDrvInfo->dword_io                         = 0x00;
 957         /* b11 : IORDY support(PIO Mode 4), b10 : Disable/Enbale IORDY
 958          * b9  : LBA support, b8 : DMA mode support
 959          */
 960         pSegIdDrvInfo->capabilities                     = 0x1 << 9;
 961
 962         pSegIdDrvInfo->reserved2                        = 0x4000;
 963         pSegIdDrvInfo->vendor_unique3                   = 0x00;
 964         /* PIOMode-2 support */
 965         pSegIdDrvInfo->pio_cycle_timing_mode            = 0x02;
 966         pSegIdDrvInfo->vendor_unique4                   = 0x00;
 967         /* MultiWord-2 support */
 968         pSegIdDrvInfo->dma_cycle_timing_mode            = 0x00;
 969         /* b1 : word64~70 is valid
 970          * b0 : word54~58 are valid and reflect the current numofcyls,heads,sectors
 971          * b2 : If device supports Ultra DMA , set to one to vaildate word88
 972          */
 973         pSegIdDrvInfo->translation_fields_valid         = (0x1 << 1) | (0x1 << 0);
 974         pSegIdDrvInfo->number_of_current_cylinders      = 0x02E9;
 975         pSegIdDrvInfo->number_of_current_heads          = 0x10;
 976         pSegIdDrvInfo->current_sectors_per_track        = 0x3F;
 977         pSegIdDrvInfo->current_sector_capacity_lo       = 0x7570;
 978         pSegIdDrvInfo->current_sector_capacity_hi       = 0x000B;
 979
 980         pSegIdDrvInfo->multi_sector_count                       = 0x04;
 981         /* b8 : Multiple secotr setting valid , b[7:0] num of secotrs per block */
 982         pSegIdDrvInfo->multi_sector_setting_valid               = 0x01;
 983         pSegIdDrvInfo->total_user_addressable_sectors_lo        = 0x7570;
 984         pSegIdDrvInfo->total_user_addressable_sectors_hi        = 0x000B;
 985         pSegIdDrvInfo->single_dma_modes_supported               = 0x00;
 986         pSegIdDrvInfo->single_dma_transfer_active               = 0x00;
 987         /* b2 :Multi-word DMA mode 2, b1 : Multi-word DMA mode 1 */
 988         pSegIdDrvInfo->multi_dma_modes_supported                = (0x1 << 0);
 989         /* b2 :Multi-word DMA mode 2, b1 : Multi-word DMA mode 1 */
 990         pSegIdDrvInfo->multi_dma_transfer_active                = (0x1 << 0);
 991         /* b0 : PIO Mode-3 support, b1 : PIO Mode-4 support */
 992         pSegIdDrvInfo->adv_pio_mode                             = 0x00;
 993         /* 480(0x1E0)nsec for Multi-word DMA mode0
 994          * 150(0x96) nsec for Multi-word DMA mode1
 995          * 120(0x78) nsec for Multi-word DMA mode2
 996          */
 997         pSegIdDrvInfo->min_dma_cyc                      = 0x1E0;
 998         pSegIdDrvInfo->recommend_dma_cyc                = 0x1E0;
 999         pSegIdDrvInfo->min_pio_cyc_no_iordy             = 0x1E0;
1000         pSegIdDrvInfo->min_pio_cyc_with_iordy           = 0x1E0;
1001         memset((void *)pSegIdDrvInfo->reserved3, 0x00, 22);
1002         /* b7 : ATA/ATAPI-7 ,b6 : ATA/ATAPI-6 ,b5 : ATA/ATAPI-5,b4 : ATA/ATAPI-4 */
1003         pSegIdDrvInfo->major_ver_num                    = 0x7E;
1004         /* 0x1C : ATA/ATAPI-6 T13 1532D revision1 */
1005         pSegIdDrvInfo->minor_ver_num                    = 0x19;
1006         /* NOP/READ BUFFER/WRITE BUFFER/Power management feature set support */
1007         pSegIdDrvInfo->feature_cmd_set_suprt0           = 0x7068;
1008         /* Features/command set is valid/Advanced Pwr management/CFA feature set
1009          * not support
1010          */
1011         pSegIdDrvInfo->feature_cmd_set_suprt1           = 0x400C;
1012         pSegIdDrvInfo->feature_cmd_set_suprt2           = 0x4000;
1013         /* READ/WRITE BUFFER/PWR Management enable */
1014         pSegIdDrvInfo->feature_cmd_set_en0              = 0x7000;
1015         /* CFA feature is disabled / Advancde power management disable */
1016         pSegIdDrvInfo->feature_cmd_set_en1              = 0x0;
1017         pSegIdDrvInfo->feature_cmd_set_en2              = 0x4000;
1018         pSegIdDrvInfo->reserved4                        = 0x0;
1019         /* 0x1 * 2minutes */
1020         pSegIdDrvInfo->req_time_for_security_er_done    = 0x19;
1021         pSegIdDrvInfo->req_time_for_enhan_security_er_done      = 0x19;
1022         /* Advanced power management level 1 */
1023         pSegIdDrvInfo->adv_pwr_mgm_lvl_val                      = 0x0;
1024         pSegIdDrvInfo->reserved5                        = 0x0;
1025         memset((void *)pSegIdDrvInfo->reserved6, 0x00, 68);
1026         /* Security mode feature is disabled */
1027         pSegIdDrvInfo->security_stas                    = 0x0;
1028         memset((void *)pSegIdDrvInfo->vendor_uniq_bytes, 0x00, 62);
1029         /* CFA power mode 1 support in maximum 200mA */
1030         pSegIdDrvInfo->cfa_pwr_mode                     = 0x0100;
1031         memset((void *)pSegIdDrvInfo->reserved7, 0x00, 190);
1032 }
1033
1034 static int mg_storage_config(void)
1035 {
1036         u8 buff[512];
1037
1038         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
1039                         != ERROR_OK)
1040                 return ERROR_FAIL;
1041
1042         mg_gen_ataid((mg_io_type_drv_info *)buff);
1043
1044         if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_stgdrvinfo)
1045                         != ERROR_OK)
1046                 return ERROR_FAIL;
1047
1048         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
1049                         != ERROR_OK)
1050                 return ERROR_FAIL;
1051
1052         LOG_INFO("mflash: storage config ok");
1053         return ERROR_OK;
1054 }
1055
1056 static int mg_boot_config(void)
1057 {
1058         u8 buff[512];
1059
1060         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
1061                         != ERROR_OK)
1062                 return ERROR_FAIL;
1063
1064         memset(buff, 0xff, 512);
1065
1066         buff[0] = mg_op_mode_snd;               /* operation mode */
1067         buff[1] = MG_UNLOCK_OTP_AREA;
1068         buff[2] = 4;                            /* boot size */
1069         *((u32 *)(buff + 4)) = 0;               /* XIP size */
1070
1071         if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_xipinfo)
1072                         != ERROR_OK)
1073                 return ERROR_FAIL;
1074
1075         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
1076                         != ERROR_OK)
1077                 return ERROR_FAIL;
1078
1079         LOG_INFO("mflash: boot config ok");
1080         return ERROR_OK;
1081 }
1082
1083 static int mg_set_pll(mg_pll_t *pll)
1084 {
1085         u8 buff[512];
1086
1087         memset(buff, 0xff, 512);
1088         *((u32 *)&buff[0]) = pll->lock_cyc;     /* PLL Lock cycle */
1089         *((u16 *)&buff[4]) = pll->feedback_div; /* PLL Feedback 9bit Divider */
1090         buff[6] = pll->input_div;               /* PLL Input 5bit Divider */
1091         buff[7] = pll->output_div;              /* PLL Output Divider */
1092
1093         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
1094                         != ERROR_OK)
1095                 return ERROR_FAIL;
1096
1097         if (mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_wr_pll)
1098                         != ERROR_OK)
1099                 return ERROR_FAIL;
1100
1101         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
1102                         != ERROR_OK)
1103                 return ERROR_FAIL;
1104
1105         LOG_INFO("mflash: set pll ok");
1106         return ERROR_OK;
1107 }
1108
1109 static int mg_erase_nand(void)
1110 {
1111         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd)
1112                         != ERROR_OK)
1113                 return ERROR_FAIL;
1114
1115         if (mg_mflash_do_write_sects(NULL, 0, 0, mg_vcmd_purge_nand)
1116                         != ERROR_OK)
1117                 return ERROR_FAIL;
1118
1119         if (mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default)
1120                         != ERROR_OK)
1121                 return ERROR_FAIL;
1122
1123         LOG_INFO("mflash: erase nand ok");
1124         return ERROR_OK;
1125 }
1126
1127 int mg_config_cmd(struct command_context_s *cmd_ctx, char *cmd,
1128                 char **args, int argc)
1129 {
1130         double fin, fout;
1131         mg_pll_t pll;
1132
1133         if (mg_verify_interface() != ERROR_OK)
1134                 return ERROR_FAIL;
1135
1136         if (mg_mflash_rst() != ERROR_OK)
1137                 return ERROR_FAIL;
1138
1139         switch (argc) {
1140                 case 2:
1141                         if (!strcmp(args[1], "boot")) {
1142                                 if (mg_boot_config() != ERROR_OK)
1143                                         return ERROR_FAIL;
1144
1145                                 return ERROR_OK;
1146                         } else if (!strcmp(args[1], "storage")) {
1147                                 if (mg_storage_config() != ERROR_OK)
1148                                         return ERROR_FAIL;
1149
1150                                 return ERROR_OK;
1151                         } else
1152                                 return ERROR_COMMAND_NOTFOUND;
1153                         break;
1154                 case 3:
1155                         if (!strcmp(args[1], "pll")) {
1156                                 fin = strtoul(args[2], NULL, 0);
1157
1158                                 if (fin > MG_PLL_CLK_OUT)
1159                                         return ERROR_FAIL;
1160
1161                                 fout = mg_calc_pll(fin, &pll);
1162
1163                                 if (!fout)
1164                                         return ERROR_FAIL;
1165
1166                                 LOG_INFO("mflash: Fout=%u Hz, feedback=%u,"
1167                                                 "indiv=%u, outdiv=%u, lock=%u",
1168                                                 (u32)fout, pll.feedback_div,
1169                                                 pll.input_div, pll.output_div,
1170                                                 pll.lock_cyc);
1171
1172                                 if (mg_erase_nand() != ERROR_OK)
1173                                         return ERROR_FAIL;
1174
1175                                 if (mg_set_pll(&pll) != ERROR_OK)
1176                                         return ERROR_FAIL;
1177
1178                                 return ERROR_OK;
1179                         } else
1180                                 return ERROR_COMMAND_NOTFOUND;
1181                         break;
1182                 default:
1183                         return ERROR_COMMAND_SYNTAX_ERROR;
1184         }
1185
1186         return ERROR_OK;
1187 }
1188
1189 int mflash_init_drivers(struct command_context_s *cmd_ctx)
1190 {
1191         if (mflash_bank) {
1192                 register_command(cmd_ctx, mflash_cmd, "probe", mg_probe_cmd, COMMAND_EXEC, NULL);
1193                 register_command(cmd_ctx, mflash_cmd, "write", mg_write_cmd, COMMAND_EXEC,
1194                                 "mflash write <num> <file> <address>");
1195                 register_command(cmd_ctx, mflash_cmd, "dump", mg_dump_cmd, COMMAND_EXEC,
1196                                                 "mflash dump <num> <file> <address> <size>");
1197                 register_command(cmd_ctx, mflash_cmd, "config", mg_config_cmd,
1198                                 COMMAND_EXEC, "mflash config <num> <stage>");
1199         }
1200
1201         return ERROR_OK;
1202 }
1203
1204 static int mg_bank_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1205 {
1206         target_t *target;
1207         char *str;
1208         int i;
1209
1210         if (argc < 4)
1211         {
1212                 return ERROR_COMMAND_SYNTAX_ERROR;
1213         }
1214
1215         if ((target = get_target(args[3])) == NULL)
1216         {
1217                 LOG_ERROR("target '%s' not defined", args[3]);
1218                 return ERROR_FAIL;
1219         }
1220
1221         mflash_bank = calloc(sizeof(mflash_bank_t), 1);
1222         mflash_bank->base = strtoul(args[1], NULL, 0);
1223         mflash_bank->rst_pin.num = strtoul(args[2], &str, 0);
1224         if (*str)
1225                 mflash_bank->rst_pin.port[0] = (u16)tolower(str[0]);
1226
1227         mflash_bank->target = target;
1228
1229         for (i = 0; mflash_gpio[i] ; i++) {
1230                 if (! strcmp(mflash_gpio[i]->name, args[0])) {
1231                         mflash_bank->gpio_drv = mflash_gpio[i];
1232                 }
1233         }
1234
1235         if (! mflash_bank->gpio_drv) {
1236                 LOG_ERROR("%s is unsupported soc", args[0]);
1237                 return ERROR_INVALID_ARGUMENTS;
1238         }
1239
1240         return ERROR_OK;
1241 }
1242
1243 int mflash_register_commands(struct command_context_s *cmd_ctx)
1244 {
1245         mflash_cmd = register_command(cmd_ctx, NULL, "mflash", NULL, COMMAND_ANY, NULL);
1246         register_command(cmd_ctx, mflash_cmd, "bank", mg_bank_cmd, COMMAND_CONFIG,
1247                         "mflash bank <soc> <base> <RST pin> <target #>");
1248         return ERROR_OK;
1249 }