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                 free(buffer);
 774                 return retval;
 775         }
 776
 777         cnt = filesize / MG_FILEIO_CHUNK;
 778         res = filesize % MG_FILEIO_CHUNK;
 779
 780         struct duration bench;
 781         duration_start(&bench);
 782
 783         size_t buf_cnt;
 784         for (i = 0; i < cnt; i++) {
 785                 ret = fileio_read(&fileio, MG_FILEIO_CHUNK, buffer, &buf_cnt);
 786                 if (ret != ERROR_OK)
 787                         goto mg_write_cmd_err;
 788                 ret = mg_mflash_write(address, buffer, MG_FILEIO_CHUNK);
 789                 if (ret != ERROR_OK)
 790                         goto mg_write_cmd_err;
 791                 address += MG_FILEIO_CHUNK;
 792         }
 793
 794         if (res) {
 795                 ret = fileio_read(&fileio, res, buffer, &buf_cnt);
 796                 if (ret != ERROR_OK)
 797                         goto mg_write_cmd_err;
 798                 ret = mg_mflash_write(address, buffer, res);
 799                 if (ret != ERROR_OK)
 800                         goto mg_write_cmd_err;
 801         }
 802
 803         if (duration_measure(&bench) == ERROR_OK) {
 804                 command_print(CMD_CTX, "wrote %ld bytes from file %s "
 805                         "in %fs (%0.3f kB/s)", (long)filesize, CMD_ARGV[1],
 806                         duration_elapsed(&bench), duration_kbps(&bench, filesize));
 807         }
 808
 809         free(buffer);
 810         fileio_close(&fileio);
 811
 812         return ERROR_OK;
 813
 814 mg_write_cmd_err:
 815         free(buffer);
 816         fileio_close(&fileio);
 817
 818         return ret;
 819 }
 820
 821 COMMAND_HANDLER(mg_dump_cmd)
 822 {
 823         uint32_t address, size, cnt, res, i;
 824         uint8_t *buffer;
 825         struct fileio fileio;
 826         int ret;
 827
 828         if (CMD_ARGC != 4)
 829                 return ERROR_COMMAND_SYNTAX_ERROR;
 830
 831         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], address);
 832         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[3], size);
 833
 834         ret = fileio_open(&fileio, CMD_ARGV[1], FILEIO_WRITE, FILEIO_BINARY);
 835         if (ret != ERROR_OK)
 836                 return ret;
 837
 838         buffer = malloc(MG_FILEIO_CHUNK);
 839         if (!buffer) {
 840                 fileio_close(&fileio);
 841                 return ERROR_FAIL;
 842         }
 843
 844         cnt = size / MG_FILEIO_CHUNK;
 845         res = size % MG_FILEIO_CHUNK;
 846
 847         struct duration bench;
 848         duration_start(&bench);
 849
 850         size_t size_written;
 851         for (i = 0; i < cnt; i++) {
 852                 ret = mg_mflash_read(address, buffer, MG_FILEIO_CHUNK);
 853                 if (ret != ERROR_OK)
 854                         goto mg_dump_cmd_err;
 855                 ret = fileio_write(&fileio, MG_FILEIO_CHUNK, buffer, &size_written);
 856                 if (ret != ERROR_OK)
 857                         goto mg_dump_cmd_err;
 858                 address += MG_FILEIO_CHUNK;
 859         }
 860
 861         if (res) {
 862                 ret = mg_mflash_read(address, buffer, res);
 863                 if (ret != ERROR_OK)
 864                         goto mg_dump_cmd_err;
 865                 ret = fileio_write(&fileio, res, buffer, &size_written);
 866                 if (ret != ERROR_OK)
 867                         goto mg_dump_cmd_err;
 868         }
 869
 870         if (duration_measure(&bench) == ERROR_OK) {
 871                 command_print(CMD_CTX, "dump image (address 0x%8.8" PRIx32 " "
 872                         "size %" PRIu32 ") to file %s in %fs (%0.3f kB/s)",
 873                         address, size, CMD_ARGV[1],
 874                         duration_elapsed(&bench), duration_kbps(&bench, size));
 875         }
 876
 877         free(buffer);
 878         fileio_close(&fileio);
 879
 880         return ERROR_OK;
 881
 882 mg_dump_cmd_err:
 883         free(buffer);
 884         fileio_close(&fileio);
 885
 886         return ret;
 887 }
 888
 889 static int mg_set_feature(mg_feature_id feature, mg_feature_val config)
 890 {
 891         struct target *target = mflash_bank->target;
 892         uint32_t mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 893         int ret;
 894
 895         ret = mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL);
 896         if (ret != ERROR_OK)
 897                 return ret;
 898
 899         ret = target_write_u8(target, mg_task_reg + MG_REG_FEATURE, feature);
 900         ret |= target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, config);
 901         ret |= target_write_u8(target, mg_task_reg + MG_REG_COMMAND,
 902                         mg_io_cmd_set_feature);
 903
 904         return ret;
 905 }
 906
 907 static int mg_is_valid_pll(double XIN, int N, double CLK_OUT, int NO)
 908 {
 909         double v1 = XIN / N;
 910         double v2 = CLK_OUT * NO;
 911
 912         if (v1 < 1000000 || v1 > 15000000 || v2 < 100000000 || v2 > 500000000)
 913                 return ERROR_MG_INVALID_PLL;
 914
 915         return ERROR_OK;
 916 }
 917
 918 static int mg_pll_get_M(unsigned short feedback_div)
 919 {
 920         int i, M;
 921
 922         for (i = 1, M = 0; i < 512; i <<= 1, feedback_div >>= 1)
 923                 M += (feedback_div & 1) * i;
 924
 925         return M + 2;
 926 }
 927
 928 static int mg_pll_get_N(unsigned char input_div)
 929 {
 930         int i, N;
 931
 932         for (i = 1, N = 0; i < 32; i <<= 1, input_div >>= 1)
 933                 N += (input_div & 1) * i;
 934
 935         return N + 2;
 936 }
 937
 938 static int mg_pll_get_NO(unsigned char output_div)
 939 {
 940         int i, NO;
 941
 942         for (i = 0, NO = 1; i < 2; ++i, output_div >>= 1)
 943                 if (output_div & 1)
 944                         NO = NO << 1;
 945
 946         return NO;
 947 }
 948
 949 static double mg_do_calc_pll(double XIN, mg_pll_t *p_pll_val, int is_approximate)
 950 {
 951         unsigned short i;
 952         unsigned char j, k;
 953         int M, N, NO;
 954         double CLK_OUT;
 955         double DIV = 1;
 956         double ROUND = 0;
 957
 958         if (is_approximate) {
 959                 DIV   = 1000000;
 960                 ROUND = 500000;
 961         }
 962
 963         for (i = 0; i < MG_PLL_MAX_FEEDBACKDIV_VAL; ++i) {
 964                 M  = mg_pll_get_M(i);
 965
 966                 for (j = 0; j < MG_PLL_MAX_INPUTDIV_VAL; ++j) {
 967                         N  = mg_pll_get_N(j);
 968
 969                         for (k = 0; k < MG_PLL_MAX_OUTPUTDIV_VAL; ++k) {
 970                                 NO = mg_pll_get_NO(k);
 971
 972                                 CLK_OUT = XIN * ((double)M / N) / NO;
 973
 974                                 if ((int)((CLK_OUT + ROUND) / DIV)
 975                                     == (int)(MG_PLL_CLK_OUT / DIV)) {
 976                                         if (mg_is_valid_pll(XIN, N, CLK_OUT, NO) == ERROR_OK) {
 977                                                 p_pll_val->lock_cyc =
 978                                                         (int)(XIN * MG_PLL_STD_LOCKCYCLE /
 979                                                               MG_PLL_STD_INPUTCLK);
 980                                                 p_pll_val->feedback_div = i;
 981                                                 p_pll_val->input_div = j;
 982                                                 p_pll_val->output_div = k;
 983
 984                                                 return CLK_OUT;
 985                                         }
 986                                 }
 987                         }
 988                 }
 989         }
 990
 991         return 0;
 992 }
 993
 994 static double mg_calc_pll(double XIN, mg_pll_t *p_pll_val)
 995 {
 996         double CLK_OUT;
 997
 998         CLK_OUT = mg_do_calc_pll(XIN, p_pll_val, 0);
 999
1000         if (!CLK_OUT)
1001                 return mg_do_calc_pll(XIN, p_pll_val, 1);
1002         else
1003                 return CLK_OUT;
1004 }
1005
1006 static int mg_verify_interface(void)
1007 {
1008         uint16_t buff[MG_MFLASH_SECTOR_SIZE >> 1];
1009         uint16_t i, j;
1010         uint32_t address = mflash_bank->base + MG_BUFFER_OFFSET;
1011         struct target *target = mflash_bank->target;
1012         int ret;
1013
1014         for (j = 0; j < 10; j++) {
1015                 for (i = 0; i < MG_MFLASH_SECTOR_SIZE >> 1; i++)
1016                         buff[i] = i;
1017
1018                 ret = target_write_memory(target, address, 2,
1019                                 MG_MFLASH_SECTOR_SIZE / 2, (uint8_t *)buff);
1020                 if (ret != ERROR_OK)
1021                         return ret;
1022
1023                 memset(buff, 0xff, MG_MFLASH_SECTOR_SIZE);
1024
1025                 ret = target_read_memory(target, address, 2,
1026                                 MG_MFLASH_SECTOR_SIZE / 2, (uint8_t *)buff);
1027                 if (ret != ERROR_OK)
1028                         return ret;
1029
1030                 for (i = 0; i < MG_MFLASH_SECTOR_SIZE >> 1; i++) {
1031                         if (buff[i] != i) {
1032                                 LOG_ERROR("mflash: verify interface fail");
1033                                 return ERROR_MG_INTERFACE;
1034                         }
1035                 }
1036         }
1037
1038         LOG_INFO("mflash: verify interface ok");
1039         return ret;
1040 }
1041
1042 static const char g_strSEG_SerialNum[20] = {
1043         'G', 'm', 'n', 'i', '-', 'e', 'e', 'S', 'g', 'a', 'e', 'l',
1044         0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20
1045 };
1046
1047 static const char g_strSEG_FWRev[8] = {
1048         'F', 'X', 'L', 'T', '2', 'v', '0', '.'
1049 };
1050
1051 static const char g_strSEG_ModelNum[40] = {
1052         'F', 'X', 'A', 'L', 'H', 'S', '2', 0x20, '0', '0', 's', '7',
1053         0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
1054         0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20,
1055         0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20
1056 };
1057
1058 static void mg_gen_ataid(mg_io_type_drv_info *pSegIdDrvInfo)
1059 {
1060         /* b15 is ATA device(0) , b7 is Removable Media Device */
1061         pSegIdDrvInfo->general_configuration            = 0x045A;
1062         /* 128MB :   Cylinder=> 977 , Heads=> 8 ,  Sectors=> 32
1063          * 256MB :   Cylinder=> 980 , Heads=> 16 , Sectors=> 32
1064          * 384MB :   Cylinder=> 745 , Heads=> 16 , Sectors=> 63
1065          */
1066         pSegIdDrvInfo->number_of_cylinders              = 0x02E9;
1067         pSegIdDrvInfo->reserved1                        = 0x0;
1068         pSegIdDrvInfo->number_of_heads                  = 0x10;
1069         pSegIdDrvInfo->unformatted_bytes_per_track      = 0x0;
1070         pSegIdDrvInfo->unformatted_bytes_per_sector     = 0x0;
1071         pSegIdDrvInfo->sectors_per_track                = 0x3F;
1072         pSegIdDrvInfo->vendor_unique1[0]                = 0x000B;
1073         pSegIdDrvInfo->vendor_unique1[1]                = 0x7570;
1074         pSegIdDrvInfo->vendor_unique1[2]                = 0x8888;
1075
1076         memcpy(pSegIdDrvInfo->serial_number, (void *)g_strSEG_SerialNum, 20);
1077         /* 0x2 : dual buffer */
1078         pSegIdDrvInfo->buffer_type                      = 0x2;
1079         /* buffer size : 2KB */
1080         pSegIdDrvInfo->buffer_sector_size               = 0x800;
1081         pSegIdDrvInfo->number_of_ecc_bytes              = 0;
1082
1083         memcpy(pSegIdDrvInfo->firmware_revision, (void *)g_strSEG_FWRev, 8);
1084
1085         memcpy(pSegIdDrvInfo->model_number, (void *)g_strSEG_ModelNum, 40);
1086
1087         pSegIdDrvInfo->maximum_block_transfer           = 0x4;
1088         pSegIdDrvInfo->vendor_unique2                   = 0x0;
1089         pSegIdDrvInfo->dword_io                         = 0x00;
1090         /* b11 : IORDY support(PIO Mode 4), b10 : Disable/Enbale IORDY
1091          * b9  : LBA support, b8 : DMA mode support
1092          */
1093         pSegIdDrvInfo->capabilities                     = 0x1 << 9;
1094
1095         pSegIdDrvInfo->reserved2                        = 0x4000;
1096         pSegIdDrvInfo->vendor_unique3                   = 0x00;
1097         /* PIOMode-2 support */
1098         pSegIdDrvInfo->pio_cycle_timing_mode            = 0x02;
1099         pSegIdDrvInfo->vendor_unique4                   = 0x00;
1100         /* MultiWord-2 support */
1101         pSegIdDrvInfo->dma_cycle_timing_mode            = 0x00;
1102         /* b1 : word64~70 is valid
1103          * b0 : word54~58 are valid and reflect the current numofcyls,heads,sectors
1104          * b2 : If device supports Ultra DMA , set to one to vaildate word88
1105          */
1106         pSegIdDrvInfo->translation_fields_valid         = (0x1 << 1) | (0x1 << 0);
1107         pSegIdDrvInfo->number_of_current_cylinders      = 0x02E9;
1108         pSegIdDrvInfo->number_of_current_heads          = 0x10;
1109         pSegIdDrvInfo->current_sectors_per_track        = 0x3F;
1110         pSegIdDrvInfo->current_sector_capacity_lo       = 0x7570;
1111         pSegIdDrvInfo->current_sector_capacity_hi       = 0x000B;
1112
1113         pSegIdDrvInfo->multi_sector_count                       = 0x04;
1114         /* b8 : Multiple secotr setting valid , b[7:0] num of secotrs per block */
1115         pSegIdDrvInfo->multi_sector_setting_valid               = 0x01;
1116         pSegIdDrvInfo->total_user_addressable_sectors_lo        = 0x7570;
1117         pSegIdDrvInfo->total_user_addressable_sectors_hi        = 0x000B;
1118         pSegIdDrvInfo->single_dma_modes_supported               = 0x00;
1119         pSegIdDrvInfo->single_dma_transfer_active               = 0x00;
1120         /* b2 :Multi-word DMA mode 2, b1 : Multi-word DMA mode 1 */
1121         pSegIdDrvInfo->multi_dma_modes_supported                = (0x1 << 0);
1122         /* b2 :Multi-word DMA mode 2, b1 : Multi-word DMA mode 1 */
1123         pSegIdDrvInfo->multi_dma_transfer_active                = (0x1 << 0);
1124         /* b0 : PIO Mode-3 support, b1 : PIO Mode-4 support */
1125         pSegIdDrvInfo->adv_pio_mode                             = 0x00;
1126         /* 480(0x1E0)nsec for Multi-word DMA mode0
1127          * 150(0x96) nsec for Multi-word DMA mode1
1128          * 120(0x78) nsec for Multi-word DMA mode2
1129          */
1130         pSegIdDrvInfo->min_dma_cyc                      = 0x1E0;
1131         pSegIdDrvInfo->recommend_dma_cyc                = 0x1E0;
1132         pSegIdDrvInfo->min_pio_cyc_no_iordy             = 0x1E0;
1133         pSegIdDrvInfo->min_pio_cyc_with_iordy           = 0x1E0;
1134         memset((void *)pSegIdDrvInfo->reserved3, 0x00, 22);
1135         /* b7 : ATA/ATAPI-7 ,b6 : ATA/ATAPI-6 ,b5 : ATA/ATAPI-5,b4 : ATA/ATAPI-4 */
1136         pSegIdDrvInfo->major_ver_num                    = 0x7E;
1137         /* 0x1C : ATA/ATAPI-6 T13 1532D revision1 */
1138         pSegIdDrvInfo->minor_ver_num                    = 0x19;
1139         /* NOP/READ BUFFER/WRITE BUFFER/Power management feature set support */
1140         pSegIdDrvInfo->feature_cmd_set_suprt0           = 0x7068;
1141         /* Features/command set is valid/Advanced Pwr management/CFA feature set
1142          * not support
1143          */
1144         pSegIdDrvInfo->feature_cmd_set_suprt1           = 0x400C;
1145         pSegIdDrvInfo->feature_cmd_set_suprt2           = 0x4000;
1146         /* READ/WRITE BUFFER/PWR Management enable */
1147         pSegIdDrvInfo->feature_cmd_set_en0              = 0x7000;
1148         /* CFA feature is disabled / Advancde power management disable */
1149         pSegIdDrvInfo->feature_cmd_set_en1              = 0x0;
1150         pSegIdDrvInfo->feature_cmd_set_en2              = 0x4000;
1151         pSegIdDrvInfo->reserved4                        = 0x0;
1152         /* 0x1 * 2minutes */
1153         pSegIdDrvInfo->req_time_for_security_er_done    = 0x19;
1154         pSegIdDrvInfo->req_time_for_enhan_security_er_done      = 0x19;
1155         /* Advanced power management level 1 */
1156         pSegIdDrvInfo->adv_pwr_mgm_lvl_val                      = 0x0;
1157         pSegIdDrvInfo->reserved5                        = 0x0;
1158         memset((void *)pSegIdDrvInfo->reserved6, 0x00, 68);
1159         /* Security mode feature is disabled */
1160         pSegIdDrvInfo->security_stas                    = 0x0;
1161         memset((void *)pSegIdDrvInfo->vendor_uniq_bytes, 0x00, 62);
1162         /* CFA power mode 1 support in maximum 200mA */
1163         pSegIdDrvInfo->cfa_pwr_mode                     = 0x0100;
1164         memset((void *)pSegIdDrvInfo->reserved7, 0x00, 190);
1165 }
1166
1167 static int mg_storage_config(void)
1168 {
1169         uint8_t buff[512];
1170         int ret;
1171
1172         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
1173         if (ret != ERROR_OK)
1174                 return ret;
1175
1176         mg_gen_ataid((mg_io_type_drv_info *)(void *)buff);
1177
1178         ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_stgdrvinfo);
1179         if (ret != ERROR_OK)
1180                 return ret;
1181
1182         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
1183         if (ret != ERROR_OK)
1184                 return ret;
1185
1186         LOG_INFO("mflash: storage config ok");
1187         return ret;
1188 }
1189
1190 static int mg_boot_config(void)
1191 {
1192         uint8_t buff[512];
1193         int ret;
1194
1195         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
1196         if (ret != ERROR_OK)
1197                 return ret;
1198
1199         memset(buff, 0xff, 512);
1200
1201         buff[0] = mg_op_mode_snd;               /* operation mode */
1202         buff[1] = MG_UNLOCK_OTP_AREA;
1203         buff[2] = 4;                            /* boot size */
1204         *((uint32_t *)(void *)(buff + 4)) = 0;          /* XIP size */
1205
1206         ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_update_xipinfo);
1207         if (ret != ERROR_OK)
1208                 return ret;
1209
1210         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
1211         if (ret != ERROR_OK)
1212                 return ret;
1213
1214         LOG_INFO("mflash: boot config ok");
1215         return ret;
1216 }
1217
1218 static int mg_set_pll(mg_pll_t *pll)
1219 {
1220         uint8_t buff[512];
1221         int ret;
1222
1223         memset(buff, 0xff, 512);
1224         /* PLL Lock cycle and Feedback 9bit Divider */
1225         memcpy(buff, &pll->lock_cyc, sizeof(uint32_t));
1226         memcpy(buff + 4, &pll->feedback_div, sizeof(uint16_t));
1227         buff[6] = pll->input_div;               /* PLL Input 5bit Divider */
1228         buff[7] = pll->output_div;              /* PLL Output Divider */
1229
1230         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
1231         if (ret != ERROR_OK)
1232                 return ret;
1233
1234         ret = mg_mflash_do_write_sects(buff, 0, 1, mg_vcmd_wr_pll);
1235         if (ret != ERROR_OK)
1236                 return ret;
1237
1238         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
1239         if (ret != ERROR_OK)
1240                 return ret;
1241
1242         LOG_INFO("mflash: set pll ok");
1243         return ret;
1244 }
1245
1246 static int mg_erase_nand(void)
1247 {
1248         int ret;
1249
1250         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_vcmd);
1251         if (ret != ERROR_OK)
1252                 return ret;
1253
1254         ret = mg_mflash_do_write_sects(NULL, 0, 0, mg_vcmd_purge_nand);
1255         if (ret != ERROR_OK)
1256                 return ret;
1257
1258         ret = mg_set_feature(mg_feature_id_transmode, mg_feature_val_trans_default);
1259         if (ret != ERROR_OK)
1260                 return ret;
1261
1262         LOG_INFO("mflash: erase nand ok");
1263         return ret;
1264 }
1265
1266 COMMAND_HANDLER(mg_config_cmd)
1267 {
1268         double fin, fout;
1269         mg_pll_t pll;
1270         int ret;
1271
1272         ret = mg_verify_interface();
1273         if (ret != ERROR_OK)
1274                 return ret;
1275
1276         ret = mg_mflash_rst();
1277         if (ret != ERROR_OK)
1278                 return ret;
1279
1280         switch (CMD_ARGC) {
1281                 case 2:
1282                         if (!strcmp(CMD_ARGV[1], "boot"))
1283                                 return mg_boot_config();
1284                         else if (!strcmp(CMD_ARGV[1], "storage"))
1285                                 return mg_storage_config();
1286                         else
1287                                 return ERROR_COMMAND_NOTFOUND;
1288                         break;
1289                 case 3:
1290                         if (!strcmp(CMD_ARGV[1], "pll")) {
1291                                 unsigned long freq;
1292                                 COMMAND_PARSE_NUMBER(ulong, CMD_ARGV[2], freq);
1293                                 fin = freq;
1294
1295                                 if (fin > MG_PLL_CLK_OUT) {
1296                                         LOG_ERROR("mflash: input freq. is too large");
1297                                         return ERROR_MG_INVALID_OSC;
1298                                 }
1299
1300                                 fout = mg_calc_pll(fin, &pll);
1301
1302                                 if (!fout) {
1303                                         LOG_ERROR("mflash: cannot generate valid pll");
1304                                         return ERROR_MG_INVALID_PLL;
1305                                 }
1306
1307                                 LOG_INFO("mflash: Fout=%" PRIu32 " Hz, feedback=%u,"
1308                                                 "indiv=%u, outdiv=%u, lock=%u",
1309                                                 (uint32_t)fout, pll.feedback_div,
1310                                                 pll.input_div, pll.output_div,
1311                                                 pll.lock_cyc);
1312
1313                                 ret = mg_erase_nand();
1314                                 if (ret != ERROR_OK)
1315                                         return ret;
1316
1317                                 return mg_set_pll(&pll);
1318                         } else
1319                                 return ERROR_COMMAND_NOTFOUND;
1320                         break;
1321                 default:
1322                         return ERROR_COMMAND_SYNTAX_ERROR;
1323         }
1324 }
1325
1326 static const struct command_registration mflash_exec_command_handlers[] = {
1327         {
1328                 .name = "probe",
1329                 .handler = mg_probe_cmd,
1330                 .mode = COMMAND_EXEC,
1331                 .help = "Detect bank configuration information",
1332         },
1333         {
1334                 .name = "write",
1335                 .handler = mg_write_cmd,
1336                 .mode = COMMAND_EXEC,
1337                 /* FIXME bank_num is unused */
1338                 .usage = "bank_num filename address",
1339                 .help = "Write binary file at the specified address.",
1340         },
1341         {
1342                 .name = "dump",
1343                 .handler = mg_dump_cmd,
1344                 .mode = COMMAND_EXEC,
1345                 /* FIXME bank_num is unused */
1346                 .usage = "bank_num filename address size",
1347                 .help = "Write specified number of bytes from a binary file "
1348                         "to the specified, address.",
1349         },
1350         {
1351                 .name = "config",
1352                 .handler = mg_config_cmd,
1353                 .mode = COMMAND_EXEC,
1354                 .help = "Configure MFLASH options.",
1355                 .usage = "('boot'|'storage'|'pll' frequency)",
1356         },
1357         COMMAND_REGISTRATION_DONE
1358 };
1359
1360 static int mflash_init_drivers(struct command_context *cmd_ctx)
1361 {
1362         if (!mflash_bank)
1363                 return ERROR_OK;
1364         return register_commands(cmd_ctx, NULL, mflash_exec_command_handlers);
1365 }
1366
1367 COMMAND_HANDLER(handle_mflash_init_command)
1368 {
1369         if (CMD_ARGC != 0)
1370                 return ERROR_COMMAND_SYNTAX_ERROR;
1371
1372         static bool mflash_initialized;
1373         if (mflash_initialized) {
1374                 LOG_INFO("'mflash init' has already been called");
1375                 return ERROR_OK;
1376         }
1377         mflash_initialized = true;
1378
1379         LOG_DEBUG("Initializing mflash devices...");
1380         return mflash_init_drivers(CMD_CTX);
1381 }
1382
1383 COMMAND_HANDLER(mg_bank_cmd)
1384 {
1385         struct target *target;
1386         int i;
1387
1388         if (CMD_ARGC < 4)
1389                 return ERROR_COMMAND_SYNTAX_ERROR;
1390
1391         target = get_target(CMD_ARGV[3]);
1392         if (target == NULL) {
1393                 LOG_ERROR("target '%s' not defined", CMD_ARGV[3]);
1394                 return ERROR_FAIL;
1395         }
1396
1397         mflash_bank = calloc(sizeof(struct mflash_bank), 1);
1398         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], mflash_bank->base);
1399         /** @todo Verify how this parsing should work, then document it. */
1400         char *str;
1401         mflash_bank->rst_pin.num = strtoul(CMD_ARGV[2], &str, 0);
1402         if (*str)
1403                 mflash_bank->rst_pin.port[0] = (uint16_t)
1404                         tolower((unsigned)str[0]);
1405
1406         mflash_bank->target = target;
1407
1408         for (i = 0; mflash_gpio[i]; i++) {
1409                 if (!strcmp(mflash_gpio[i]->name, CMD_ARGV[0]))
1410                         mflash_bank->gpio_drv = mflash_gpio[i];
1411         }
1412
1413         if (!mflash_bank->gpio_drv) {
1414                 LOG_ERROR("%s is unsupported soc", CMD_ARGV[0]);
1415                 return ERROR_MG_UNSUPPORTED_SOC;
1416         }
1417
1418         return ERROR_OK;
1419 }
1420
1421 static const struct command_registration mflash_config_command_handlers[] = {
1422         {
1423                 .name = "bank",
1424                 .handler = mg_bank_cmd,
1425                 .mode = COMMAND_CONFIG,
1426                 .help = "configure a mflash device bank",
1427                 .usage = "soc_type base_addr pin_id target",
1428         },
1429         {
1430                 .name = "init",
1431                 .mode = COMMAND_CONFIG,
1432                 .handler = handle_mflash_init_command,
1433                 .help = "initialize mflash devices",
1434                 .usage = ""
1435         },
1436         COMMAND_REGISTRATION_DONE
1437 };
1438 static const struct command_registration mflash_command_handler[] = {
1439         {
1440                 .name = "mflash",
1441                 .mode = COMMAND_ANY,
1442                 .help = "mflash command group",
1443                 .usage = "",
1444                 .chain = mflash_config_command_handlers,
1445         },
1446         COMMAND_REGISTRATION_DONE
1447 };
1448 int mflash_register_commands(struct command_context *cmd_ctx)
1449 {
1450         return register_commands(cmd_ctx, NULL, mflash_command_handler);
1451 }