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

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