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 <target/target.h>
  26 #include <helper/time_support.h>
  27 #include <helper/fileio.h>
  28 #include <helper/log.h>
  29
  30 static int s3c2440_set_gpio_to_output(struct mflash_gpio_num gpio);
  31 static int s3c2440_set_gpio_output_val(struct mflash_gpio_num gpio, uint8_t val);
  32 static int pxa270_set_gpio_to_output(struct mflash_gpio_num gpio);
  33 static int pxa270_set_gpio_output_val(struct mflash_gpio_num gpio, uint8_t val);
  34
  35 static struct mflash_bank *mflash_bank;
  36
  37 static struct mflash_gpio_drv pxa270_gpio = {
  38         .name = "pxa270",
  39         .set_gpio_to_output = pxa270_set_gpio_to_output,
  40         .set_gpio_output_val = pxa270_set_gpio_output_val
  41 };
  42
  43 static struct mflash_gpio_drv s3c2440_gpio = {
  44         .name = "s3c2440",
  45         .set_gpio_to_output = s3c2440_set_gpio_to_output,
  46         .set_gpio_output_val = s3c2440_set_gpio_output_val
  47 };
  48
  49 static struct mflash_gpio_drv *mflash_gpio[] = {
  50         &pxa270_gpio,
  51         &s3c2440_gpio,
  52         NULL
  53 };
  54
  55 #define PXA270_GAFR0_L 0x40E00054
  56 #define PXA270_GAFR3_U 0x40E00070
  57 #define PXA270_GAFR3_U_RESERVED_BITS  0xfffc0000u
  58 #define PXA270_GPDR0 0x40E0000C
  59 #define PXA270_GPDR3 0x40E0010C
  60 #define PXA270_GPDR3_RESERVED_BITS  0xfe000000u
  61 #define PXA270_GPSR0 0x40E00018
  62 #define PXA270_GPCR0 0x40E00024
  63
  64 static int pxa270_set_gpio_to_output(struct mflash_gpio_num gpio)
  65 {
  66         uint32_t addr, value, mask;
  67         struct target *target = mflash_bank->target;
  68         int ret;
  69
  70         /* remove alternate function. */
  71         mask = 0x3u << (gpio.num & 0xF)*2;
  72
  73         addr = PXA270_GAFR0_L + (gpio.num >> 4) * 4;
  74
  75         ret = target_read_u32(target, addr, &value);
  76         if (ret != ERROR_OK)
  77                 return ret;
  78
  79         value &= ~mask;
  80         if (addr == PXA270_GAFR3_U)
  81                 value &= ~PXA270_GAFR3_U_RESERVED_BITS;
  82
  83         ret = target_write_u32(target, addr, value);
  84         if (ret != ERROR_OK)
  85                 return ret;
  86
  87         /* set direction to output */
  88         mask = 0x1u << (gpio.num & 0x1F);
  89
  90         addr = PXA270_GPDR0 + (gpio.num >> 5) * 4;
  91
  92         ret = target_read_u32(target, addr, &value);
  93         if (ret != 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(struct mflash_gpio_num gpio, uint8_t val)
 105 {
 106         uint32_t addr, value, mask;
 107         struct target *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         ret = target_read_u32(target, addr, &value);
 118         if (ret != 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(struct mflash_gpio_num gpio)
 134 {
 135         uint32_t data, mask, gpio_con;
 136         struct target *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("mflash: invalid port %d%s", gpio.num, gpio.port);
 145                 return ERROR_COMMAND_SYNTAX_ERROR;
 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(struct mflash_gpio_num gpio, uint8_t val)
 166 {
 167         uint32_t data, mask, gpio_dat;
 168         struct target *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("mflash: invalid port %d%s", gpio.num, gpio.port);
 177                 return ERROR_COMMAND_SYNTAX_ERROR;
 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(uint8_t 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         int ret;
 202         struct mflash_gpio_drv *gpio_drv = mflash_bank->gpio_drv;
 203
 204         ret = gpio_drv->set_gpio_to_output(mflash_bank->rst_pin);
 205         if (ret != ERROR_OK)
 206                 return ret;
 207
 208         ret = gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, 1);
 209
 210         return ret;
 211 }
 212
 213 static int mg_dsk_wait(mg_io_type_wait wait_local, uint32_t time_var)
 214 {
 215         uint8_t status, error;
 216         struct target *target = mflash_bank->target;
 217         uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 218         int ret;
 219         long long t = 0;
 220
 221         struct duration bench;
 222         duration_start(&bench);
 223
 224         while (time_var) {
 225
 226                 ret = target_read_u8(target, mg_task_reg + MG_REG_STATUS, &status);
 227                 if (ret != ERROR_OK)
 228                         return ret;
 229
 230                 if (status & mg_io_rbit_status_busy) {
 231                         if (wait_local == mg_io_wait_bsy)
 232                                 return ERROR_OK;
 233                 } else {
 234                         switch (wait_local) {
 235                                 case mg_io_wait_not_bsy:
 236                                         return ERROR_OK;
 237                                 case mg_io_wait_rdy_noerr:
 238                                         if (status & mg_io_rbit_status_ready)
 239                                                 return ERROR_OK;
 240                                         break;
 241                                 case mg_io_wait_drq_noerr:
 242                                         if (status & mg_io_rbit_status_data_req)
 243                                                 return ERROR_OK;
 244                                         break;
 245                                 default:
 246                                         break;
 247                         }
 248
 249                         /* Now we check the error condition! */
 250                         if (status & mg_io_rbit_status_error) {
 251                                 ret = target_read_u8(target, mg_task_reg + MG_REG_ERROR, &error);
 252                                 if (ret != ERROR_OK)
 253                                         return ret;
 254
 255                                 LOG_ERROR("mflash: io error 0x%02x", error);
 256
 257                                 return ERROR_MG_IO;
 258                         }
 259
 260                         switch (wait_local) {
 261                                 case mg_io_wait_rdy:
 262                                         if (status & mg_io_rbit_status_ready)
 263                                                 return ERROR_OK;
 264
 265                                 case mg_io_wait_drq:
 266                                         if (status & mg_io_rbit_status_data_req)
 267                                                 return ERROR_OK;
 268
 269                                 default:
 270                                         break;
 271                         }
 272                 }
 273
 274                 ret = duration_measure(&bench);
 275                 if (ERROR_OK == ret)
 276                         t = duration_elapsed(&bench) * 1000.0;
 277                 else
 278                         LOG_ERROR("mflash: duration measurement failed: %d", ret);
 279
 280                 if (t > time_var)
 281                         break;
 282         }
 283
 284         LOG_ERROR("mflash: timeout occured");
 285         return ERROR_MG_TIMEOUT;
 286 }
 287
 288 static int mg_dsk_srst(uint8_t on)
 289 {
 290         struct target *target = mflash_bank->target;
 291         uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 292         uint8_t value;
 293         int ret;
 294
 295         ret = target_read_u8(target, mg_task_reg + MG_REG_DRV_CTRL, &value);
 296         if (ret != ERROR_OK)
 297                 return ret;
 298
 299         if (on)
 300                 value |= (mg_io_rbit_devc_srst);
 301         else
 302                 value &= ~mg_io_rbit_devc_srst;
 303
 304         ret = target_write_u8(target, mg_task_reg + MG_REG_DRV_CTRL, value);
 305         return ret;
 306 }
 307
 308 static int mg_dsk_io_cmd(uint32_t sect_num, uint32_t cnt, uint8_t cmd)
 309 {
 310         struct target *target = mflash_bank->target;
 311         uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 312         uint8_t value;
 313         int ret;
 314
 315         ret = mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL);
 316         if (ret != ERROR_OK)
 317                 return ret;
 318
 319         value = mg_io_rval_dev_drv_master | mg_io_rval_dev_lba_mode | ((sect_num >> 24) & 0xf);
 320
 321         ret = target_write_u8(target, mg_task_reg + MG_REG_DRV_HEAD, value);
 322         ret |= target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, (uint8_t)cnt);
 323         ret |= target_write_u8(target, mg_task_reg + MG_REG_SECT_NUM, (uint8_t)sect_num);
 324         ret |= target_write_u8(target, mg_task_reg + MG_REG_CYL_LOW, (uint8_t)(sect_num >> 8));
 325         ret |= target_write_u8(target, mg_task_reg + MG_REG_CYL_HIGH, (uint8_t)(sect_num >> 16));
 326
 327         if (ret != ERROR_OK)
 328                 return ret;
 329
 330         return target_write_u8(target, mg_task_reg + MG_REG_COMMAND, cmd);
 331 }
 332
 333 static int mg_dsk_drv_info(void)
 334 {
 335         struct target *target = mflash_bank->target;
 336         uint32_t mg_buff = mflash_bank->base + MG_BUFFER_OFFSET;
 337         int ret;
 338
 339         ret = mg_dsk_io_cmd(0, 1, mg_io_cmd_identify);
 340         if (ret != ERROR_OK)
 341                 return ret;
 342
 343         ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 344         if (ret != ERROR_OK)
 345                 return ret;
 346
 347         LOG_INFO("mflash: read drive info");
 348
 349         if (!mflash_bank->drv_info)
 350                 mflash_bank->drv_info = malloc(sizeof(struct mg_drv_info));
 351
 352         ret = target_read_memory(target, mg_buff, 2,
 353                         sizeof(mg_io_type_drv_info) >> 1,
 354                         (uint8_t *)&mflash_bank->drv_info->drv_id);
 355         if (ret != ERROR_OK)
 356                 return ret;
 357
 358         mflash_bank->drv_info->tot_sects =
 359                 (uint32_t)(mflash_bank->drv_info->drv_id.total_user_addressable_sectors_hi << 16)
 360                 + mflash_bank->drv_info->drv_id.total_user_addressable_sectors_lo;
 361
 362         return target_write_u8(target,
 363                 mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND,
 364                 mg_io_cmd_confirm_read);
 365 }
 366
 367 static int mg_mflash_rst(void)
 368 {
 369         int ret;
 370
 371         ret = mg_init_gpio();
 372         if (ret != ERROR_OK)
 373                 return ret;
 374
 375         ret = mg_hdrst(0);
 376         if (ret != ERROR_OK)
 377                 return ret;
 378
 379         ret = mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
 380         if (ret != ERROR_OK)
 381                 return ret;
 382
 383         ret = mg_hdrst(1);
 384         if (ret != ERROR_OK)
 385                 return ret;
 386
 387         ret = mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
 388         if (ret != ERROR_OK)
 389                 return ret;
 390
 391         ret = mg_dsk_srst(1);
 392         if (ret != ERROR_OK)
 393                 return ret;
 394
 395         ret = mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
 396         if (ret != ERROR_OK)
 397                 return ret;
 398
 399         ret = mg_dsk_srst(0);
 400         if (ret != ERROR_OK)
 401                 return ret;
 402
 403         ret = mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG);
 404         if (ret != ERROR_OK)
 405                 return ret;
 406
 407         LOG_INFO("mflash: reset ok");
 408
 409         return ERROR_OK;
 410 }
 411
 412 static int mg_mflash_probe(void)
 413 {
 414         int ret = mg_mflash_rst();
 415         if (ret != ERROR_OK)
 416                 return ret;
 417
 418         return mg_dsk_drv_info();
 419 }
 420
 421 COMMAND_HANDLER(mg_probe_cmd)
 422 {
 423         int ret;
 424
 425         ret = mg_mflash_probe();
 426
 427         if (ret == ERROR_OK) {
 428                 command_print(CMD_CTX,
 429                         "mflash (total %" PRIu32 " sectors) found at 0x%8.8" PRIx32 "",
 430                         mflash_bank->drv_info->tot_sects,
 431                         mflash_bank->base);
 432         }
 433
 434         return ret;
 435 }
 436
 437 static int mg_mflash_do_read_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt)
 438 {
 439         uint32_t i, address;
 440         int ret;
 441         struct target *target = mflash_bank->target;
 442         uint8_t *buff_ptr = buff;
 443
 444         ret = mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_read);
 445         if (ret != ERROR_OK)
 446                 return ret;
 447
 448         address = mflash_bank->base + MG_BUFFER_OFFSET;
 449
 450         struct duration bench;
 451         duration_start(&bench);
 452
 453         for (i = 0; i < sect_cnt; i++) {
 454                 ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 455                 if (ret != ERROR_OK)
 456                         return ret;
 457
 458                 ret = target_read_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
 459                 if (ret != ERROR_OK)
 460                         return ret;
 461
 462                 buff_ptr += MG_MFLASH_SECTOR_SIZE;
 463
 464                 ret = target_write_u8(target,
 465                                 mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND,
 466                                 mg_io_cmd_confirm_read);
 467                 if (ret != ERROR_OK)
 468                         return ret;
 469
 470                 LOG_DEBUG("mflash: %" PRIu32 " (0x%8.8" PRIx32 ") sector read", sect_num + i,
 471                         (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 472
 473                 ret = duration_measure(&bench);
 474                 if ((ERROR_OK == ret) && (duration_elapsed(&bench) > 3)) {
 475                         LOG_INFO("mflash: read %" PRIu32 "'th sectors", sect_num + i);
 476                         duration_start(&bench);
 477                 }
 478         }
 479
 480         return mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
 481 }
 482
 483 static int mg_mflash_read_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt)
 484 {
 485         uint32_t quotient, residue, i;
 486         uint8_t *buff_ptr = buff;
 487         int ret = ERROR_OK;
 488
 489         quotient = sect_cnt >> 8;
 490         residue = sect_cnt % 256;
 491
 492         for (i = 0; i < quotient; i++) {
 493                 LOG_DEBUG("mflash: sect num : %" PRIu32 " buff : %p",
 494                         sect_num, buff_ptr);
 495                 ret = mg_mflash_do_read_sects(buff_ptr, sect_num, 256);
 496                 if (ret != ERROR_OK)
 497                         return ret;
 498
 499                 sect_num += 256;
 500                 buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 501         }
 502
 503         if (residue) {
 504                 LOG_DEBUG("mflash: sect num : %" PRIx32 " buff : %p",
 505                         sect_num, buff_ptr);
 506                 return mg_mflash_do_read_sects(buff_ptr, sect_num, residue);
 507         }
 508
 509         return ret;
 510 }
 511
 512 static int mg_mflash_do_write_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt,
 513         mg_io_type_cmd cmd)
 514 {
 515         uint32_t i, address;
 516         int ret;
 517         struct target *target = mflash_bank->target;
 518         uint8_t *buff_ptr = buff;
 519
 520         ret = mg_dsk_io_cmd(sect_num, sect_cnt, cmd);
 521         if (ret != ERROR_OK)
 522                 return ret;
 523
 524         address = mflash_bank->base + MG_BUFFER_OFFSET;
 525
 526         struct duration bench;
 527         duration_start(&bench);
 528
 529         for (i = 0; i < sect_cnt; i++) {
 530                 ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 531                 if (ret != ERROR_OK)
 532                         return ret;
 533
 534                 ret = target_write_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
 535                 if (ret != ERROR_OK)
 536                         return ret;
 537
 538                 buff_ptr += MG_MFLASH_SECTOR_SIZE;
 539
 540                 ret = target_write_u8(target,
 541                                 mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND,
 542                                 mg_io_cmd_confirm_write);
 543                 if (ret != ERROR_OK)
 544                         return ret;
 545
 546                 LOG_DEBUG("mflash: %" PRIu32 " (0x%8.8" PRIx32 ") sector write", sect_num + i,
 547                         (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 548
 549                 ret = duration_measure(&bench);
 550                 if ((ERROR_OK == ret) && (duration_elapsed(&bench) > 3)) {
 551                         LOG_INFO("mflash: wrote %" PRIu32 "'th sectors", sect_num + i);
 552                         duration_start(&bench);
 553                 }
 554         }
 555
 556         if (cmd == mg_io_cmd_write)
 557                 ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
 558         else
 559                 ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_LONG);
 560
 561         return ret;
 562 }
 563
 564 static int mg_mflash_write_sects(void *buff, uint32_t sect_num, uint32_t sect_cnt)
 565 {
 566         uint32_t quotient, residue, i;
 567         uint8_t *buff_ptr = buff;
 568         int ret = ERROR_OK;
 569
 570         quotient = sect_cnt >> 8;
 571         residue = sect_cnt % 256;
 572
 573         for (i = 0; i < quotient; i++) {
 574                 LOG_DEBUG("mflash: sect num : %" PRIu32 "buff : %p", sect_num,
 575                         buff_ptr);
 576                 ret = mg_mflash_do_write_sects(buff_ptr, sect_num, 256, mg_io_cmd_write);
 577                 if (ret != ERROR_OK)
 578                         return ret;
 579
 580                 sect_num += 256;
 581                 buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 582         }
 583
 584         if (residue) {
 585                 LOG_DEBUG("mflash: sect num : %" PRIu32 " buff : %p", sect_num,
 586                         buff_ptr);
 587                 return mg_mflash_do_write_sects(buff_ptr, sect_num, residue, mg_io_cmd_write);
 588         }
 589
 590         return ret;
 591 }
 592
 593 static int mg_mflash_read(uint32_t addr, uint8_t *buff, uint32_t len)
 594 {
 595         uint8_t *buff_ptr = buff;
 596         uint8_t sect_buff[MG_MFLASH_SECTOR_SIZE];
 597         uint32_t cur_addr, next_sec_addr, end_addr, cnt, sect_num;
 598         int ret = ERROR_OK;
 599
 600         cnt = 0;
 601         cur_addr = addr;
 602         end_addr = addr + len;
 603
 604         if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 605
 606                 next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 607                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 608                 ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
 609                 if (ret != ERROR_OK)
 610                         return ret;
 611
 612                 if (end_addr < next_sec_addr) {
 613                         memcpy(buff_ptr,
 614                                 sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
 615                                 end_addr - cur_addr);
 616                         LOG_DEBUG(
 617                                 "mflash: copies %" PRIu32 " byte from sector offset 0x%8.8" PRIx32 "",
 618                                 end_addr - cur_addr,
 619                                 cur_addr);
 620                         cur_addr = end_addr;
 621                 } else {
 622                         memcpy(buff_ptr,
 623                                 sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
 624                                 next_sec_addr - cur_addr);
 625                         LOG_DEBUG(
 626                                 "mflash: copies %" PRIu32 " byte from sector offset 0x%8.8" PRIx32 "",
 627                                 next_sec_addr - cur_addr,
 628                                 cur_addr);
 629                         buff_ptr += (next_sec_addr - cur_addr);
 630                         cur_addr = next_sec_addr;
 631                 }
 632         }
 633
 634         if (cur_addr < end_addr) {
 635
 636                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 637                 next_sec_addr = cur_addr + MG_MFLASH_SECTOR_SIZE;
 638
 639                 while (next_sec_addr <= end_addr) {
 640                         cnt++;
 641                         next_sec_addr += MG_MFLASH_SECTOR_SIZE;
 642                 }
 643
 644                 if (cnt) {
 645                         ret = mg_mflash_read_sects(buff_ptr, sect_num, cnt);
 646                         if (ret != ERROR_OK)
 647                                 return ret;
 648                 }
 649
 650                 buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 651                 cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
 652
 653                 if (cur_addr < end_addr) {
 654
 655                         sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 656                         ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
 657                         if (ret != ERROR_OK)
 658                                 return ret;
 659
 660                         memcpy(buff_ptr, sect_buff, end_addr - cur_addr);
 661                         LOG_DEBUG("mflash: copies %u byte", (unsigned)(end_addr - cur_addr));
 662                 }
 663         }
 664
 665         return ret;
 666 }
 667
 668 static int mg_mflash_write(uint32_t addr, uint8_t *buff, uint32_t len)
 669 {
 670         uint8_t *buff_ptr = buff;
 671         uint8_t sect_buff[MG_MFLASH_SECTOR_SIZE];
 672         uint32_t cur_addr, next_sec_addr, end_addr, cnt, sect_num;
 673         int ret = ERROR_OK;
 674
 675         cnt = 0;
 676         cur_addr = addr;
 677         end_addr = addr + len;
 678
 679         if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 680
 681                 next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 682                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 683                 ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
 684                 if (ret != ERROR_OK)
 685                         return ret;
 686
 687                 if (end_addr < next_sec_addr) {
 688                         memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
 689                                 buff_ptr,
 690                                 end_addr - cur_addr);
 691                         LOG_DEBUG(
 692                                 "mflash: copies %" PRIu32 " byte to sector offset 0x%8.8" PRIx32 "",
 693                                 end_addr - cur_addr,
 694                                 cur_addr);
 695                         cur_addr = end_addr;
 696                 } else {
 697                         memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK),
 698                                 buff_ptr,
 699                                 next_sec_addr - cur_addr);
 700                         LOG_DEBUG(
 701                                 "mflash: copies %" PRIu32 " byte to sector offset 0x%8.8" PRIx32 "",
 702                                 next_sec_addr - cur_addr,
 703                                 cur_addr);
 704                         buff_ptr += (next_sec_addr - cur_addr);
 705                         cur_addr = next_sec_addr;
 706                 }
 707
 708                 ret = mg_mflash_write_sects(sect_buff, sect_num, 1);
 709                 if (ret != ERROR_OK)
 710                         return ret;
 711         }
 712
 713         if (cur_addr < end_addr) {
 714
 715                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 716                 next_sec_addr = cur_addr + MG_MFLASH_SECTOR_SIZE;
 717
 718                 while (next_sec_addr <= end_addr) {
 719                         cnt++;
 720                         next_sec_addr += MG_MFLASH_SECTOR_SIZE;
 721                 }
 722
 723                 if (cnt) {
 724                         ret = mg_mflash_write_sects(buff_ptr, sect_num, cnt);
 725                         if (ret != ERROR_OK)
 726                                 return ret;
 727                 }
 728
 729                 buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 730                 cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
 731
 732                 if (cur_addr < end_addr) {
 733
 734                         sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 735                         ret = mg_mflash_read_sects(sect_buff, sect_num, 1);
 736                         if (ret != ERROR_OK)
 737                                 return ret;
 738
 739                         memcpy(sect_buff, buff_ptr, end_addr - cur_addr);
 740                         LOG_DEBUG("mflash: copies %" PRIu32 " byte", end_addr - cur_addr);
 741                         ret = mg_mflash_write_sects(sect_buff, sect_num, 1);
 742                 }
 743         }
 744
 745         return ret;
 746 }
 747
 748 COMMAND_HANDLER(mg_write_cmd)
 749 {
 750         uint32_t address, cnt, res, i;
 751         uint8_t *buffer;
 752         struct fileio fileio;
 753         int ret;
 754
 755         if (CMD_ARGC != 3)
 756                 return ERROR_COMMAND_SYNTAX_ERROR;
 757
 758         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], address);
 759
 760         ret = fileio_open(&fileio, CMD_ARGV[1], FILEIO_READ, FILEIO_BINARY);
 761         if (ret != ERROR_OK)
 762                 return ret;
 763
 764         int filesize;
 765         buffer = malloc(MG_FILEIO_CHUNK);
 766         if (!buffer) {
 767                 fileio_close(&fileio);
 768                 return ERROR_FAIL;
 769         }
 770         int retval = fileio_size(&fileio, &filesize);
 771         if (retval != ERROR_OK) {
 772                 fileio_close(&fileio);
 773                 return retval;
 774         }
 775
 776         cnt = filesize / MG_FILEIO_CHUNK;
 777         res = filesize % MG_FILEIO_CHUNK;
 778
 779         struct duration bench;
 780         duration_start(&bench);
 781
 782         size_t buf_cnt;
 783         for (i = 0; i < cnt; i++) {
 784                 ret = fileio_read(&fileio, MG_FILEIO_CHUNK, buffer, &buf_cnt);
 785                 if (ret != ERROR_OK)
 786                         goto mg_write_cmd_err;
 787                 ret = mg_mflash_write(address, buffer, MG_FILEIO_CHUNK);
 788                 if (ret != ERROR_OK)
 789                         goto mg_write_cmd_err;
 790                 address += MG_FILEIO_CHUNK;
 791         }
 792
 793         if (res) {
 794                 ret = fileio_read(&fileio, res, buffer, &buf_cnt);
 795                 if (ret != ERROR_OK)
 796                         goto mg_write_cmd_err;
 797                 ret = mg_mflash_write(address, buffer, res);
 798                 if (ret != ERROR_OK)
 799                         goto mg_write_cmd_err;
 800         }
 801
 802         if (duration_measure(&bench) == ERROR_OK) {
 803                 command_print(CMD_CTX, "wrote %ld bytes from file %s "
 804                         "in %fs (%0.3f kB/s)", (long)filesize, CMD_ARGV[1],
 805                         duration_elapsed(&bench), duration_kbps(&bench, filesize));
 806         }
 807
 808         free(buffer);
 809         fileio_close(&fileio);
 810
 811         return ERROR_OK;
 812
 813 mg_write_cmd_err:
 814         free(buffer);
 815         fileio_close(&fileio);
 816
 817         return ret;
 818 }
 819
 820 COMMAND_HANDLER(mg_dump_cmd)
 821 {
 822         uint32_t address, size, cnt, res, i;
 823         uint8_t *buffer;
 824         struct fileio fileio;
 825         int ret;
 826
 827         if (CMD_ARGC != 4)
 828                 return ERROR_COMMAND_SYNTAX_ERROR;
 829
 830         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], address);
 831         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[3], size);
 832
 833         ret = fileio_open(&fileio, CMD_ARGV[1], FILEIO_WRITE, FILEIO_BINARY);
 834         if (ret != ERROR_OK)
 835                 return ret;
 836
 837         buffer = malloc(MG_FILEIO_CHUNK);
 838         if (!buffer) {
 839                 fileio_close(&fileio);
 840                 return ERROR_FAIL;
 841         }
 842
 843         cnt = size / MG_FILEIO_CHUNK;
 844         res = size % MG_FILEIO_CHUNK;
 845
 846         struct duration bench;
 847         duration_start(&bench);
 848
 849         size_t size_written;
 850         for (i = 0; i < cnt; i++) {
 851                 ret = mg_mflash_read(address, buffer, MG_FILEIO_CHUNK);
 852                 if (ret != ERROR_OK)
 853                         goto mg_dump_cmd_err;
 854                 ret = fileio_write(&fileio, MG_FILEIO_CHUNK, buffer, &size_written);
 855                 if (ret != ERROR_OK)
 856                         goto mg_dump_cmd_err;
 857                 address += MG_FILEIO_CHUNK;
 858         }
 859
 860         if (res) {
 861                 ret = mg_mflash_read(address, buffer, res);
 862                 if (ret != ERROR_OK)
 863                         goto mg_dump_cmd_err;
 864                 ret = fileio_write(&fileio, res, buffer, &size_written);
 865                 if (ret != ERROR_OK)
 866                         goto mg_dump_cmd_err;
 867         }
 868
 869         if (duration_measure(&bench) == ERROR_OK) {
 870                 command_print(CMD_CTX, "dump image (address 0x%8.8" PRIx32 " "
 871                         "size %" PRIu32 ") to file %s in %fs (%0.3f kB/s)",
 872                         address, size, CMD_ARGV[1],
 873                         duration_elapsed(&bench), duration_kbps(&bench, size));
 874         }
 875
 876         free(buffer);
 877         fileio_close(&fileio);
 878
 879         return ERROR_OK;
 880
 881 mg_dump_cmd_err:
 882         free(buffer);
 883         fileio_close(&fileio);
 884
 885         return ret;
 886 }
 887
 888 static int mg_set_feature(mg_feature_id feature, mg_feature_val config)
 889 {
 890         struct target *target = mflash_bank->target;
 891         uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 892         int ret;
 893
 894         ret = mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL);
 895         if (ret != ERROR_OK)
 896                 return ret;
 897
 898         ret = target_write_u8(target, mg_task_reg + MG_REG_FEATURE, feature);
 899         ret |= target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, config);
 900         ret |= target_write_u8(target, mg_task_reg + MG_REG_COMMAND,
 901                         mg_io_cmd_set_feature);
 902
 903         return ret;
 904 }
 905
 906 static int mg_is_valid_pll(double XIN, int N, double CLK_OUT, int NO)
 907 {
 908         double v1 = XIN / N;
 909         double v2 = CLK_OUT * NO;
 910
 911         if (v1 < 1000000 || v1 > 15000000 || v2 < 100000000 || v2 > 500000000)
 912                 return ERROR_MG_INVALID_PLL;
 913
 914         return ERROR_OK;
 915 }
 916
 917 static int mg_pll_get_M(unsigned short feedback_div)
 918 {
 919         int i, M;
 920
 921         for (i = 1, M = 0; i < 512; i <<= 1, feedback_div >>= 1)
 922                 M += (feedback_div & 1) * i;
 923
 924         return M + 2;
 925 }
 926
 927 static int mg_pll_get_N(unsigned char input_div)
 928 {
 929         int i, N;
 930
 931         for (i = 1, N = 0; i < 32; i <<= 1, input_div >>= 1)
 932                 N += (input_div & 1) * i;
 933
 934         return N + 2;
 935 }
 936
 937 static int mg_pll_get_NO(unsigned char output_div)
 938 {
 939         int i, NO;
 940
 941         for (i = 0, NO = 1; i < 2; ++i, output_div >>= 1)
 942                 if (output_div & 1)
 943                         NO = NO << 1;
 944
 945         return NO;
 946 }
 947
 948 static double mg_do_calc_pll(double XIN, mg_pll_t *p_pll_val, int is_approximate)
 949 {
 950         unsigned short i;
 951         unsigned char j, k;
 952         int M, N, NO;
 953         double CLK_OUT;
 954         double DIV = 1;
 955         double ROUND = 0;
 956
 957         if (is_approximate) {
 958                 DIV   = 1000000;
 959                 ROUND = 500000;
 960         }
 961
 962         for (i = 0; i < MG_PLL_MAX_FEEDBACKDIV_VAL; ++i) {
 963                 M  = mg_pll_get_M(i);
 964
 965                 for (j = 0; j < MG_PLL_MAX_INPUTDIV_VAL; ++j) {
 966                         N  = mg_pll_get_N(j);
 967
 968                         for (k = 0; k < MG_PLL_MAX_OUTPUTDIV_VAL; ++k) {
 969                                 NO = mg_pll_get_NO(k);
 970
 971                                 CLK_OUT = XIN * ((double)M / N) / NO;
 972
 973                                 if ((int)((CLK_OUT + ROUND) / DIV)
 974                                     == (int)(MG_PLL_CLK_OUT / DIV)) {
 975                                         if (mg_is_valid_pll(XIN, N, CLK_OUT, NO) == ERROR_OK) {
 976                                                 p_pll_val->lock_cyc =
 977                                                         (int)(XIN * MG_PLL_STD_LOCKCYCLE /
 978                                                               MG_PLL_STD_INPUTCLK);
 979                                                 p_pll_val->feedback_div = i;
 980                                                 p_pll_val->input_div = j;
 981                                                 p_pll_val->output_div = k;
 982
 983                                                 return CLK_OUT;
 984                                         }
 985                                 }
 986                         }
 987                 }
 988         }
 989
 990         return 0;
 991 }
 992
 993 static double mg_calc_pll(double XIN, mg_pll_t *p_pll_val)
 994 {
 995         double CLK_OUT;
 996
 997         CLK_OUT = mg_do_calc_pll(XIN, p_pll_val, 0);
 998
 999         if (!CLK_OUT)
1000                 return mg_do_calc_pll(XIN, p_pll_val, 1);
1001         else
1002                 return CLK_OUT;
1003 }
1004
1005 static int mg_verify_interface(void)
1006 {
1007         uint16_t buff[MG_MFLASH_SECTOR_SIZE >> 1];
1008         uint16_t i, j;
1009         uint32_t address = mflash_bank->base + MG_BUFFER_OFFSET;
1010         struct target *target = mflash_bank->target;
1011         int ret;
1012
1013         for (j = 0; j < 10; j++) {
1014                 for (i = 0; i < MG_MFLASH_SECTOR_SIZE >> 1; i++)
1015                         buff[i] = i;
1016
1017                 ret = target_write_memory(target, address, 2,
1018                                 MG_MFLASH_SECTOR_SIZE / 2, (uint8_t *)buff);
1019                 if (ret != ERROR_OK)
1020                         return ret;
1021
1022                 memset(buff, 0xff, MG_MFLASH_SECTOR_SIZE);
1023
1024                 ret = target_read_memory(target, address, 2,
1025                                 MG_MFLASH_SECTOR_SIZE / 2, (uint8_t *)buff);
1026                 if (ret != ERROR_OK)
1027                         return ret;
1028
1029                 for (i = 0; i < MG_MFLASH_SECTOR_SIZE >> 1; i++) {
1030                         if (buff[i] != i) {
1031                                 LOG_ERROR("mflash: verify interface fail");
1032                                 return ERROR_MG_INTERFACE;
1033                         }
1034                 }
1035         }
1036
1037         LOG_INFO("mflash: verify interface ok");
1038         return ret;
1039 }
1040
1041 static const char g_strSEG_SerialNum[20] = {
1042         'G', 'm', 'n', 'i', '-', 'e', 'e', 'S', 'g', 'a', 'e', 'l',
1043         0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20
1044 };
1045
1046 static const char g_strSEG_FWRev[8] = {
1047         'F', 'X', 'L', 'T', '2', 'v', '0', '.'
1048 };
1049
1050 static const char g_strSEG_ModelNum[40] = {
1051         'F', 'X', 'A', 'L', 'H', 'S', '2', 0x20, '0', '0', 's', '7',
1052         0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
1053         0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
1054         0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20
1055 };
1056
1057 static void mg_gen_ataid(mg_io_type_drv_info *pSegIdDrvInfo)
1058 {
1059         /* b15 is ATA device(0) , b7 is Removable Media Device */
1060         pSegIdDrvInfo->general_configuration            = 0x045A;
1061         /* 128MB :   Cylinder=> 977 , Heads=> 8 ,  Sectors=> 32
1062          * 256MB :   Cylinder=> 980 , Heads=> 16 , Sectors=> 32
1063          * 384MB :   Cylinder=> 745 , Heads=> 16 , Sectors=> 63
1064          */
1065         pSegIdDrvInfo->number_of_cylinders              = 0x02E9;
1066         pSegIdDrvInfo->reserved1                        = 0x0;
1067         pSegIdDrvInfo->number_of_heads                  = 0x10;
1068         pSegIdDrvInfo->unformatted_bytes_per_track      = 0x0;
1069         pSegIdDrvInfo->unformatted_bytes_per_sector     = 0x0;
1070         pSegIdDrvInfo->sectors_per_track                = 0x3F;
1071         pSegIdDrvInfo->vendor_unique1[0]                = 0x000B;
1072         pSegIdDrvInfo->vendor_unique1[1]                = 0x7570;
1073         pSegIdDrvInfo->vendor_unique1[2]                = 0x8888;
1074
1075         memcpy(pSegIdDrvInfo->serial_number, (void *)g_strSEG_SerialNum, 20);
1076         /* 0x2 : dual buffer */
1077         pSegIdDrvInfo->buffer_type                      = 0x2;
1078         /* buffer size : 2KB */
1079         pSegIdDrvInfo->buffer_sector_size               = 0x800;
1080         pSegIdDrvInfo->number_of_ecc_bytes              = 0;
1081
1082         memcpy(pSegIdDrvInfo->firmware_revision, (void *)g_strSEG_FWRev, 8);
1083
1084         memcpy(pSegIdDrvInfo->model_number, (void *)g_strSEG_ModelNum, 40);
1085
1086         pSegIdDrvInfo->maximum_block_transfer           = 0x4;
1087         pSegIdDrvInfo->vendor_unique2                   = 0x0;
1088         pSegIdDrvInfo->dword_io                         = 0x00;
1089         /* b11 : IORDY support(PIO Mode 4), b10 : Disable/Enbale IORDY
1090          * b9  : LBA support, b8 : DMA mode support
1091          */
1092         pSegIdDrvInfo->capabilities                     = 0x1 << 9;
1093
1094         pSegIdDrvInfo->reserved2                        = 0x4000;
1095         pSegIdDrvInfo->vendor_unique3                   = 0x00;
1096         /* PIOMode-2 support */
1097         pSegIdDrvInfo->pio_cycle_timing_mode            = 0x02;
1098         pSegIdDrvInfo->vendor_unique4                   = 0x00;
1099         /* MultiWord-2 support */
1100         pSegIdDrvInfo->dma_cycle_timing_mode            = 0x00;
1101         /* b1 : word64~70 is valid
1102          * b0 : word54~58 are valid and reflect the current numofcyls,heads,sectors
1103          * b2 : If device supports Ultra DMA , set to one to vaildate word88
1104          */
1105         pSegIdDrvInfo->translation_fields_valid         = (0x1 << 1) | (0x1 << 0);
1106         pSegIdDrvInfo->number_of_current_cylinders      = 0x02E9;
1107         pSegIdDrvInfo->number_of_current_heads          = 0x10;
1108         pSegIdDrvInfo->current_sectors_per_track        = 0x3F;
1109         pSegIdDrvInfo->current_sector_capacity_lo       = 0x7570;
1110         pSegIdDrvInfo->current_sector_capacity_hi       = 0x000B;
1111
1112         pSegIdDrvInfo->multi_sector_count                       = 0x04;
1113         /* b8 : Multiple secotr setting valid , b[7:0] num of secotrs per block */
1114         pSegIdDrvInfo->multi_sector_setting_valid               = 0x01;
1115         pSegIdDrvInfo->total_user_addressable_sectors_lo        = 0x7570;
1116         pSegIdDrvInfo->total_user_addressable_sectors_hi        = 0x000B;
1117         pSegIdDrvInfo->single_dma_modes_supported               = 0x00;
1118         pSegIdDrvInfo->single_dma_transfer_active               = 0x00;
1119         /* b2 :Multi-word DMA mode 2, b1 : Multi-word DMA mode 1 */
1120         pSegIdDrvInfo->multi_dma_modes_supported                = (0x1 << 0);
1121         /* b2 :Multi-word DMA mode 2, b1 : Multi-word DMA mode 1 */
1122         pSegIdDrvInfo->multi_dma_transfer_active                = (0x1 << 0);
1123         /* b0 : PIO Mode-3 support, b1 : PIO Mode-4 support */
1124         pSegIdDrvInfo->adv_pio_mode                             = 0x00;
1125         /* 480(0x1E0)nsec for Multi-word DMA mode0
1126          * 150(0x96) nsec for Multi-word DMA mode1
1127          * 120(0x78) nsec for Multi-word DMA mode2
1128          */
1129         pSegIdDrvInfo->min_dma_cyc                      = 0x1E0;
1130         pSegIdDrvInfo->recommend_dma_cyc                = 0x1E0;
1131         pSegIdDrvInfo->min_pio_cyc_no_iordy             = 0x1E0;
1132         pSegIdDrvInfo->min_pio_cyc_with_iordy           = 0x1E0;
1133         memset((void *)pSegIdDrvInfo->reserved3, 0x00, 22);
1134         /* b7 : ATA/ATAPI-7 ,b6 : ATA/ATAPI-6 ,b5 : ATA/ATAPI-5,b4 : ATA/ATAPI-4 */
1135         pSegIdDrvInfo->major_ver_num                    = 0x7E;
1136         /* 0x1C : ATA/ATAPI-6 T13 1532D revision1 */
1137         pSegIdDrvInfo->minor_ver_num                    = 0x19;
1138         /* NOP/READ BUFFER/WRITE BUFFER/Power management feature set support */
1139         pSegIdDrvInfo->feature_cmd_set_suprt0           = 0x7068;
1140         /* Features/command set is valid/Advanced Pwr management/CFA feature set
1141          * not support
1142          */
1143         pSegIdDrvInfo->feature_cmd_set_suprt1           = 0x400C;
1144         pSegIdDrvInfo->feature_cmd_set_suprt2           = 0x4000;
1145         /* READ/WRITE BUFFER/PWR Management enable */
1146         pSegIdDrvInfo->feature_cmd_set_en0              = 0x7000;
1147         /* CFA feature is disabled / Advancde power management disable */
1148         pSegIdDrvInfo->feature_cmd_set_en1              = 0x0;
1149         pSegIdDrvInfo->feature_cmd_set_en2              = 0x4000;
1150         pSegIdDrvInfo->reserved4                        = 0x0;
1151         /* 0x1 * 2minutes */
1152         pSegIdDrvInfo->req_time_for_security_er_done    = 0x19;
1153         pSegIdDrvInfo->req_time_for_enhan_security_er_done      = 0x19;
1154         /* Advanced power management level 1 */
1155         pSegIdDrvInfo->adv_pwr_mgm_lvl_val                      = 0x0;
1156         pSegIdDrvInfo->reserved5                        = 0x0;
1157         memset((void *)pSegIdDrvInfo->reserved6, 0x00, 68);
1158         /* Security mode feature is disabled */
1159         pSegIdDrvInfo->security_stas                    = 0x0;
1160         memset((void *)pSegIdDrvInfo->vendor_uniq_bytes, 0x00, 62);
1161         /* CFA power mode 1 support in maximum 200mA */
1162         pSegIdDrvInfo->cfa_pwr_mode                     = 0x0100;
1163         memset((void *)pSegIdDrvInfo->reserved7, 0x00, 190);
1164 }
1165
1166 static int mg_storage_config(void)
1167 {
1168         uint8_t buff[512];
1169         int ret;
1170
1171         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
1172         if (ret != ERROR_OK)
1173                 return ret;
1174
1175         mg_gen_ataid((mg_io_type_drv_info *)(void *)buff);
1176
1177         ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_stgdrvinfo);
1178         if (ret != ERROR_OK)
1179                 return ret;
1180
1181         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
1182         if (ret != ERROR_OK)
1183                 return ret;
1184
1185         LOG_INFO("mflash: storage config ok");
1186         return ret;
1187 }
1188
1189 static int mg_boot_config(void)
1190 {
1191         uint8_t buff[512];
1192         int ret;
1193
1194         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
1195         if (ret != ERROR_OK)
1196                 return ret;
1197
1198         memset(buff, 0xff, 512);
1199
1200         buff[0] = mg_op_mode_snd;               /* operation mode */
1201         buff[1] = MG_UNLOCK_OTP_AREA;
1202         buff[2] = 4;                            /* boot size */
1203         *((uint32_t *)(void *)(buff + 4)) = 0;          /* XIP size */
1204
1205         ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_xipinfo);
1206         if (ret != ERROR_OK)
1207                 return ret;
1208
1209         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
1210         if (ret != ERROR_OK)
1211                 return ret;
1212
1213         LOG_INFO("mflash: boot config ok");
1214         return ret;
1215 }
1216
1217 static int mg_set_pll(mg_pll_t *pll)
1218 {
1219         uint8_t buff[512];
1220         int ret;
1221
1222         memset(buff, 0xff, 512);
1223         /* PLL Lock cycle and Feedback 9bit Divider */
1224         memcpy(buff, &pll->lock_cyc, sizeof(uint32_t));
1225         memcpy(buff + 4, &pll->feedback_div, sizeof(uint16_t));
1226         buff[6] = pll->input_div;               /* PLL Input 5bit Divider */
1227         buff[7] = pll->output_div;              /* PLL Output Divider */
1228
1229         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
1230         if (ret != ERROR_OK)
1231                 return ret;
1232
1233         ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_wr_pll);
1234         if (ret != ERROR_OK)
1235                 return ret;
1236
1237         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
1238         if (ret != ERROR_OK)
1239                 return ret;
1240
1241         LOG_INFO("mflash: set pll ok");
1242         return ret;
1243 }
1244
1245 static int mg_erase_nand(void)
1246 {
1247         int ret;
1248
1249         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
1250         if (ret != ERROR_OK)
1251                 return ret;
1252
1253         ret = mg_mflash_do_write_sects(NULL, 0, 0, mg_vcmd_purge_nand);
1254         if (ret != ERROR_OK)
1255                 return ret;
1256
1257         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
1258         if (ret != ERROR_OK)
1259                 return ret;
1260
1261         LOG_INFO("mflash: erase nand ok");
1262         return ret;
1263 }
1264
1265 COMMAND_HANDLER(mg_config_cmd)
1266 {
1267         double fin, fout;
1268         mg_pll_t pll;
1269         int ret;
1270
1271         ret = mg_verify_interface();
1272         if (ret != ERROR_OK)
1273                 return ret;
1274
1275         ret = mg_mflash_rst();
1276         if (ret != ERROR_OK)
1277                 return ret;
1278
1279         switch (CMD_ARGC) {
1280                 case 2:
1281                         if (!strcmp(CMD_ARGV[1], "boot"))
1282                                 return mg_boot_config();
1283                         else if (!strcmp(CMD_ARGV[1], "storage"))
1284                                 return mg_storage_config();
1285                         else
1286                                 return ERROR_COMMAND_NOTFOUND;
1287                         break;
1288                 case 3:
1289                         if (!strcmp(CMD_ARGV[1], "pll")) {
1290                                 unsigned long freq;
1291                                 COMMAND_PARSE_NUMBER(ulong, CMD_ARGV[2], freq);
1292                                 fin = freq;
1293
1294                                 if (fin > MG_PLL_CLK_OUT) {
1295                                         LOG_ERROR("mflash: input freq. is too large");
1296                                         return ERROR_MG_INVALID_OSC;
1297                                 }
1298
1299                                 fout = mg_calc_pll(fin, &pll);
1300
1301                                 if (!fout) {
1302                                         LOG_ERROR("mflash: cannot generate valid pll");
1303                                         return ERROR_MG_INVALID_PLL;
1304                                 }
1305
1306                                 LOG_INFO("mflash: Fout=%" PRIu32 " Hz, feedback=%u,"
1307                                                 "indiv=%u, outdiv=%u, lock=%u",
1308                                                 (uint32_t)fout, pll.feedback_div,
1309                                                 pll.input_div, pll.output_div,
1310                                                 pll.lock_cyc);
1311
1312                                 ret = mg_erase_nand();
1313                                 if (ret != ERROR_OK)
1314                                         return ret;
1315
1316                                 return mg_set_pll(&pll);
1317                         } else
1318                                 return ERROR_COMMAND_NOTFOUND;
1319                         break;
1320                 default:
1321                         return ERROR_COMMAND_SYNTAX_ERROR;
1322         }
1323 }
1324
1325 static const struct command_registration mflash_exec_command_handlers[] = {
1326         {
1327                 .name = "probe",
1328                 .handler = mg_probe_cmd,
1329                 .mode = COMMAND_EXEC,
1330                 .help = "Detect bank configuration information",
1331         },
1332         {
1333                 .name = "write",
1334                 .handler = mg_write_cmd,
1335                 .mode = COMMAND_EXEC,
1336                 /* FIXME bank_num is unused */
1337                 .usage = "bank_num filename address",
1338                 .help = "Write binary file at the specified address.",
1339         },
1340         {
1341                 .name = "dump",
1342                 .handler = mg_dump_cmd,
1343                 .mode = COMMAND_EXEC,
1344                 /* FIXME bank_num is unused */
1345                 .usage = "bank_num filename address size",
1346                 .help = "Write specified number of bytes from a binary file "
1347                         "to the specified, address.",
1348         },
1349         {
1350                 .name = "config",
1351                 .handler = mg_config_cmd,
1352                 .mode = COMMAND_EXEC,
1353                 .help = "Configure MFLASH options.",
1354                 .usage = "('boot'|'storage'|'pll' frequency)",
1355         },
1356         COMMAND_REGISTRATION_DONE
1357 };
1358
1359 static int mflash_init_drivers(struct command_context *cmd_ctx)
1360 {
1361         if (!mflash_bank)
1362                 return ERROR_OK;
1363         return register_commands(cmd_ctx, NULL, mflash_exec_command_handlers);
1364 }
1365
1366 COMMAND_HANDLER(handle_mflash_init_command)
1367 {
1368         if (CMD_ARGC != 0)
1369                 return ERROR_COMMAND_SYNTAX_ERROR;
1370
1371         static bool mflash_initialized;
1372         if (mflash_initialized) {
1373                 LOG_INFO("'mflash init' has already been called");
1374                 return ERROR_OK;
1375         }
1376         mflash_initialized = true;
1377
1378         LOG_DEBUG("Initializing mflash devices...");
1379         return mflash_init_drivers(CMD_CTX);
1380 }
1381
1382 COMMAND_HANDLER(mg_bank_cmd)
1383 {
1384         struct target *target;
1385         int i;
1386
1387         if (CMD_ARGC < 4)
1388                 return ERROR_COMMAND_SYNTAX_ERROR;
1389
1390         target = get_target(CMD_ARGV[3]);
1391         if (target == NULL) {
1392                 LOG_ERROR("target '%s' not defined", CMD_ARGV[3]);
1393                 return ERROR_FAIL;
1394         }
1395
1396         mflash_bank = calloc(sizeof(struct mflash_bank), 1);
1397         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], mflash_bank->base);
1398         /** @todo Verify how this parsing should work, then document it. */
1399         char *str;
1400         mflash_bank->rst_pin.num = strtoul(CMD_ARGV[2], &str, 0);
1401         if (*str)
1402                 mflash_bank->rst_pin.port[0] = (uint16_t)
1403                         tolower((unsigned)str[0]);
1404
1405         mflash_bank->target = target;
1406
1407         for (i = 0; mflash_gpio[i]; i++) {
1408                 if (!strcmp(mflash_gpio[i]->name, CMD_ARGV[0]))
1409                         mflash_bank->gpio_drv = mflash_gpio[i];
1410         }
1411
1412         if (!mflash_bank->gpio_drv) {
1413                 LOG_ERROR("%s is unsupported soc", CMD_ARGV[0]);
1414                 return ERROR_MG_UNSUPPORTED_SOC;
1415         }
1416
1417         return ERROR_OK;
1418 }
1419
1420 static const struct command_registration mflash_config_command_handlers[] = {
1421         {
1422                 .name = "bank",
1423                 .handler = mg_bank_cmd,
1424                 .mode = COMMAND_CONFIG,
1425                 .help = "configure a mflash device bank",
1426                 .usage = "soc_type base_addr pin_id target",
1427         },
1428         {
1429                 .name = "init",
1430                 .mode = COMMAND_CONFIG,
1431                 .handler = handle_mflash_init_command,
1432                 .help = "initialize mflash devices",
1433                 .usage = ""
1434         },
1435         COMMAND_REGISTRATION_DONE
1436 };
1437 static const struct command_registration mflash_command_handler[] = {
1438         {
1439                 .name = "mflash",
1440                 .mode = COMMAND_ANY,
1441                 .help = "mflash command group",
1442                 .usage = "",
1443                 .chain = mflash_config_command_handlers,
1444         },
1445         COMMAND_REGISTRATION_DONE
1446 };
1447 int mflash_register_commands(struct command_context *cmd_ctx)
1448 {
1449         return register_commands(cmd_ctx, NULL, mflash_command_handler);
1450 }