src/flash/mflash.c

   1 /***************************************************************************
   2  *   Copyright (C) 2007-2008 by unsik Kim <donari75@gmail.com>             *
   3  *                                                                         *
   4  *   This program is free software; you can redistribute it and/or modify  *
   5  *   it under the terms of the GNU General Public License as published by  *
   6  *   the Free Software Foundation; either version 2 of the License, or     *
   7  *   (at your option) any later version.                                   *
   8  *                                                                         *
   9  *   This program is distributed in the hope that it will be useful,       *
  10  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  11  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  12  *   GNU General Public License for more details.                          *
  13  *                                                                         *
  14  *   You should have received a copy of the GNU General Public License     *
  15  *   along with this program; if not, write to the                         *
  16  *   Free Software Foundation, Inc.,                                       *
  17  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  18  ***************************************************************************/
  19
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include "mflash.h"
  25 #include "time_support.h"
  26 #include "fileio.h"
  27 #include "log.h"
  28
  29
  30 static int s3c2440_set_gpio_to_output (mflash_gpio_num_t gpio);
  31 static int s3c2440_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val);
  32 static int pxa270_set_gpio_to_output (mflash_gpio_num_t gpio);
  33 static int pxa270_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val);
  34
  35 static command_t *mflash_cmd;
  36
  37 static mflash_bank_t *mflash_bank;
  38
  39 static mflash_gpio_drv_t pxa270_gpio = {
  40         .name = "pxa270",
  41         .set_gpio_to_output = pxa270_set_gpio_to_output,
  42         .set_gpio_output_val = pxa270_set_gpio_output_val
  43 };
  44
  45 static mflash_gpio_drv_t s3c2440_gpio = {
  46         .name = "s3c2440",
  47         .set_gpio_to_output = s3c2440_set_gpio_to_output,
  48         .set_gpio_output_val = s3c2440_set_gpio_output_val
  49 };
  50
  51 static mflash_gpio_drv_t *mflash_gpio[] =
  52 {
  53                 &pxa270_gpio,
  54                 &s3c2440_gpio,
  55                 NULL
  56 };
  57
  58 #define PXA270_GAFR0_L 0x40E00054
  59 #define PXA270_GAFR3_U 0x40E00070
  60 #define PXA270_GAFR3_U_RESERVED_BITS  0xfffc0000u
  61 #define PXA270_GPDR0 0x40E0000C
  62 #define PXA270_GPDR3 0x40E0010C
  63 #define PXA270_GPDR3_RESERVED_BITS  0xfe000000u
  64 #define PXA270_GPSR0 0x40E00018
  65 #define PXA270_GPCR0 0x40E00024
  66
  67 static int pxa270_set_gpio_to_output (mflash_gpio_num_t gpio)
  68 {
  69         u32 addr, value, mask;
  70         target_t *target = mflash_bank->target;
  71         int ret;
  72
  73         /* remove alternate function. */
  74         mask = 0x3u << (gpio.num & 0xF)*2;
  75
  76         addr = PXA270_GAFR0_L + (gpio.num >> 4) * 4;
  77
  78         if ((ret = target_read_u32(target, addr, &value)) != ERROR_OK)
  79                 return ret;
  80
  81         value &= ~mask;
  82         if (addr == PXA270_GAFR3_U)
  83                 value &= ~PXA270_GAFR3_U_RESERVED_BITS;
  84
  85         if ((ret = target_write_u32(target, addr, value)) != ERROR_OK)
  86                 return ret;
  87
  88         /* set direction to output */
  89         mask = 0x1u << (gpio.num & 0x1F);
  90
  91         addr = PXA270_GPDR0 + (gpio.num >> 5) * 4;
  92
  93         if ((ret = target_read_u32(target, addr, &value)) != ERROR_OK)
  94                 return ret;
  95
  96         value |= mask;
  97         if (addr == PXA270_GPDR3)
  98                 value &= ~PXA270_GPDR3_RESERVED_BITS;
  99
 100         ret = target_write_u32(target, addr, value);
 101         return ret;
 102 }
 103
 104 static int pxa270_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val)
 105 {
 106         u32 addr, value, mask;
 107         target_t *target = mflash_bank->target;
 108         int ret;
 109
 110         mask = 0x1u << (gpio.num & 0x1F);
 111
 112         if (val) {
 113                 addr = PXA270_GPSR0 + (gpio.num >> 5) * 4;
 114         } else {
 115                 addr = PXA270_GPCR0 + (gpio.num >> 5) * 4;
 116         }
 117
 118         if ((ret = target_read_u32(target, addr, &value)) != ERROR_OK)
 119                 return ret;
 120
 121         value |= mask;
 122
 123         ret = target_write_u32(target, addr, value);
 124
 125         return ret;
 126 }
 127
 128 #define S3C2440_GPACON 0x56000000
 129 #define S3C2440_GPADAT 0x56000004
 130 #define S3C2440_GPJCON 0x560000d0
 131 #define S3C2440_GPJDAT 0x560000d4
 132
 133 static int s3c2440_set_gpio_to_output (mflash_gpio_num_t gpio)
 134 {
 135         u32 data, mask, gpio_con;
 136         target_t *target = mflash_bank->target;
 137         int ret;
 138
 139         if (gpio.port[0] >= 'a' && gpio.port[0] <= 'h') {
 140                 gpio_con = S3C2440_GPACON + (gpio.port[0] - 'a') * 0x10;
 141         } else if (gpio.port[0] == 'j') {
 142                 gpio_con = S3C2440_GPJCON;
 143         } else {
 144                 LOG_ERROR("invalid port %d%s", gpio.num, gpio.port);
 145                 return ERROR_INVALID_ARGUMENTS;
 146         }
 147
 148         ret = target_read_u32(target, gpio_con, &data);
 149
 150         if (ret == ERROR_OK) {
 151                 if (gpio.port[0] == 'a') {
 152                         mask = 1 << gpio.num;
 153                         data &= ~mask;
 154                 } else {
 155                         mask = 3 << gpio.num * 2;
 156                         data &= ~mask;
 157                         data |= (1 << gpio.num * 2);
 158                 }
 159
 160                 ret = target_write_u32(target, gpio_con, data);
 161         }
 162         return ret;
 163 }
 164
 165 static int s3c2440_set_gpio_output_val (mflash_gpio_num_t gpio, u8 val)
 166 {
 167         u32 data, mask, gpio_dat;
 168         target_t *target = mflash_bank->target;
 169         int ret;
 170
 171         if (gpio.port[0] >= 'a' && gpio.port[0] <= 'h') {
 172                 gpio_dat = S3C2440_GPADAT + (gpio.port[0] - 'a') * 0x10;
 173         } else if (gpio.port[0] == 'j') {
 174                 gpio_dat = S3C2440_GPJDAT;
 175         } else {
 176                 LOG_ERROR("invalid port %d%s", gpio.num, gpio.port);
 177                 return ERROR_INVALID_ARGUMENTS;
 178         }
 179
 180         ret = target_read_u32(target, gpio_dat, &data);
 181
 182         if (ret == ERROR_OK) {
 183                 mask = 1 << gpio.num;
 184                 if (val)
 185                         data |= mask;
 186                 else
 187                         data &= ~mask;
 188
 189                 ret = target_write_u32(target, gpio_dat, data);
 190         }
 191         return ret;
 192 }
 193
 194 static int mg_hdrst(u8 level)
 195 {
 196         return mflash_bank->gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, level);
 197 }
 198
 199 static int mg_init_gpio (void)
 200 {
 201         mflash_gpio_drv_t *gpio_drv = mflash_bank->gpio_drv;
 202
 203         gpio_drv->set_gpio_to_output(mflash_bank->rst_pin);
 204         gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, 1);
 205
 206         return ERROR_OK;
 207 }
 208
 209 static int mg_dsk_wait(mg_io_type_wait wait, u32 time)
 210 {
 211         u8 status, error;
 212         target_t *target = mflash_bank->target;
 213         u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 214         duration_t duration;
 215         long long t=0;
 216
 217         duration_start_measure(&duration);
 218
 219         while (time) {
 220
 221                 target_read_u8(target, mg_task_reg + MG_REG_STATUS, &status);
 222
 223                 if (status & mg_io_rbit_status_busy)
 224                 {
 225                         if (wait == mg_io_wait_bsy)
 226                                 return ERROR_OK;
 227                 } else {
 228                         switch(wait)
 229                         {
 230                                 case mg_io_wait_not_bsy:
 231                                         return ERROR_OK;
 232                                 case mg_io_wait_rdy_noerr:
 233                                         if (status & mg_io_rbit_status_ready)
 234                                                 return ERROR_OK;
 235                                         break;
 236                                 case mg_io_wait_drq_noerr:
 237                                         if (status & mg_io_rbit_status_data_req)
 238                                                 return ERROR_OK;
 239                                         break;
 240                                 default:
 241                                         break;
 242                         }
 243
 244                         /* Now we check the error condition! */
 245                         if (status & mg_io_rbit_status_error)
 246                         {
 247                                 target_read_u8(target, mg_task_reg + MG_REG_ERROR, &error);
 248
 249                                 if (error & mg_io_rbit_err_bad_sect_num) {
 250                                         LOG_ERROR("sector not found");
 251                                         return ERROR_FAIL;
 252                                 }
 253                                 else if (error & (mg_io_rbit_err_bad_block | mg_io_rbit_err_uncorrectable)) {
 254                                         LOG_ERROR("bad block");
 255                                         return ERROR_FAIL;
 256                                 } else {
 257                                         LOG_ERROR("disk operation fail");
 258                                         return ERROR_FAIL;
 259                                 }
 260                         }
 261
 262                         switch (wait)
 263                         {
 264                                 case mg_io_wait_rdy:
 265                                         if (status & mg_io_rbit_status_ready)
 266                                                 return ERROR_OK;
 267
 268                                 case mg_io_wait_drq:
 269                                         if (status & mg_io_rbit_status_data_req)
 270                                                 return ERROR_OK;
 271
 272                                 default:
 273                                         break;
 274                         }
 275                 }
 276
 277                 duration_stop_measure(&duration, NULL);
 278
 279                 t=duration.duration.tv_usec/1000;
 280                 t+=duration.duration.tv_sec*1000;
 281
 282                 if (t > time)
 283                         break;
 284         }
 285
 286         LOG_ERROR("timeout occured");
 287         return ERROR_FAIL;
 288 }
 289
 290 static int mg_dsk_srst(u8 on)
 291 {
 292         target_t *target = mflash_bank->target;
 293         u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 294         u8 value;
 295         int ret;
 296
 297         if ((ret = target_read_u8(target, mg_task_reg + MG_REG_DRV_CTRL, &value)) != ERROR_OK)
 298                 return ret;
 299
 300         if(on) {
 301                 value |= (mg_io_rbit_devc_srst);
 302         } else {
 303                 value &= ~mg_io_rbit_devc_srst;
 304         }
 305
 306         ret = target_write_u8(target, mg_task_reg + MG_REG_DRV_CTRL, value);
 307         return ret;
 308 }
 309
 310 static int mg_dsk_io_cmd(u32 sect_num, u32 cnt, u8 cmd)
 311 {
 312         target_t *target = mflash_bank->target;
 313         u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 314         u8 value;
 315
 316         if (mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL) != ERROR_OK)
 317                 return ERROR_FAIL;
 318
 319         value = mg_io_rval_dev_drv_master | mg_io_rval_dev_lba_mode |((sect_num >> 24) & 0xf);
 320
 321         target_write_u8(target, mg_task_reg + MG_REG_DRV_HEAD, value);
 322         target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, (u8)cnt);
 323         target_write_u8(target, mg_task_reg + MG_REG_SECT_NUM, (u8)sect_num);
 324         target_write_u8(target, mg_task_reg + MG_REG_CYL_LOW, (u8)(sect_num >> 8));
 325         target_write_u8(target, mg_task_reg + MG_REG_CYL_HIGH, (u8)(sect_num >> 16));
 326
 327         target_write_u8(target, mg_task_reg + MG_REG_COMMAND, cmd);
 328
 329         return ERROR_OK;
 330 }
 331
 332 static int mg_dsk_drv_info(void)
 333 {
 334         target_t *target = mflash_bank->target;
 335         u32 mg_buff = mflash_bank->base + MG_BUFFER_OFFSET;
 336
 337         if ( mg_dsk_io_cmd(0, 1, mg_io_cmd_identify) != ERROR_OK)
 338                 return ERROR_FAIL;
 339
 340         if ( mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL) != ERROR_OK)
 341                 return ERROR_FAIL;
 342
 343         LOG_INFO("read drive info.");
 344
 345         if (! mflash_bank->drv_info)
 346                 mflash_bank->drv_info = malloc(sizeof(mg_drv_info_t));
 347
 348         target->type->read_memory(target, mg_buff, 2, sizeof(mg_io_type_drv_info) >> 1,
 349                         (u8 *)&mflash_bank->drv_info->drv_id);
 350
 351         mflash_bank->drv_info->tot_sects = (u32)(mflash_bank->drv_info->drv_id.total_user_addressable_sectors_hi << 16)
 352                                                                         + mflash_bank->drv_info->drv_id.total_user_addressable_sectors_lo;
 353
 354         target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_read);
 355
 356         return ERROR_OK;
 357 }
 358
 359 static int mg_mflash_probe(void)
 360 {
 361         mflash_bank->proved = 0;
 362
 363         mg_init_gpio();
 364
 365         LOG_INFO("reset mflash");
 366
 367         mg_hdrst(0);
 368
 369         if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 370                 return ERROR_FAIL;
 371
 372         mg_hdrst(1);
 373
 374         if (mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 375                 return ERROR_FAIL;
 376
 377         mg_dsk_srst(1);
 378
 379         if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 380                 return ERROR_FAIL;
 381
 382         mg_dsk_srst(0);
 383
 384         if (mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 385                 return ERROR_FAIL;
 386
 387         if (mg_dsk_drv_info() != ERROR_OK)
 388                 return ERROR_FAIL;
 389
 390         mflash_bank->proved = 1;
 391
 392         return ERROR_OK;
 393 }
 394
 395 static int mg_probe_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 396 {
 397         int ret;
 398
 399         ret = mg_mflash_probe();
 400
 401         if (ret == ERROR_OK) {
 402                 command_print(cmd_ctx, "mflash (total %u sectors) found at 0x%8.8x",
 403                                 mflash_bank->drv_info->tot_sects, mflash_bank->base );
 404         }
 405
 406         return ret;
 407 }
 408
 409 static int mg_mflash_do_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
 410 {
 411         u32 i, address;
 412         int ret;
 413         target_t *target = mflash_bank->target;
 414         u8 *buff_ptr = buff;
 415         duration_t duration;
 416
 417         if ( mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_read) != ERROR_OK )
 418                 return ERROR_FAIL;
 419
 420         address = mflash_bank->base + MG_BUFFER_OFFSET;
 421
 422         duration_start_measure(&duration);
 423
 424         for (i = 0; i < sect_cnt; i++) {
 425                 mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 426
 427                 target->type->read_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
 428                 buff_ptr += MG_MFLASH_SECTOR_SIZE;
 429
 430                 target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_read);
 431
 432                 LOG_DEBUG("%u (0x%8.8x) sector read", sect_num + i, (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 433
 434                 duration_stop_measure(&duration, NULL);
 435
 436                 if ((duration.duration.tv_sec * 1000 + duration.duration.tv_usec / 1000) > 3000) {
 437                         LOG_INFO("read %u'th sectors", sect_num + i);
 438                         duration_start_measure(&duration);
 439                 }
 440         }
 441
 442         ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
 443
 444         return ret;
 445 }
 446
 447 static int mg_mflash_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
 448 {
 449         u32 quotient, residue, i;
 450         u8 *buff_ptr = buff;
 451
 452         quotient = sect_cnt >> 8;
 453         residue = sect_cnt % 256;
 454
 455         for (i = 0; i < quotient; i++) {
 456                 LOG_DEBUG("sect num : %u buff : 0x%0lx", sect_num,
 457                         (unsigned long)buff_ptr);
 458                 mg_mflash_do_read_sects(buff_ptr, sect_num, 256);
 459                 sect_num += 256;
 460                 buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 461         }
 462
 463         if (residue) {
 464                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 465                         (unsigned long)buff_ptr);
 466                 mg_mflash_do_read_sects(buff_ptr, sect_num, residue);
 467         }
 468
 469         return ERROR_OK;
 470 }
 471
 472 static int mg_mflash_do_write_sects(void *buff, u32 sect_num, u32 sect_cnt)
 473 {
 474         u32 i, address;
 475         int ret;
 476         target_t *target = mflash_bank->target;
 477         u8 *buff_ptr = buff;
 478         duration_t duration;
 479
 480         if ( mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_write) != ERROR_OK ) {
 481                 LOG_ERROR("mg_io_cmd_write fail");
 482                 return ERROR_FAIL;
 483         }
 484
 485         address = mflash_bank->base + MG_BUFFER_OFFSET;
 486
 487         duration_start_measure(&duration);
 488
 489         for (i = 0; i < sect_cnt; i++) {
 490                 ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 491                 if (ret != ERROR_OK)
 492                         LOG_ERROR("mg_io_wait_drq time out");
 493
 494                 ret = target->type->write_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
 495                 if (ret != ERROR_OK)
 496                         LOG_ERROR("mem write error");
 497                 buff_ptr += MG_MFLASH_SECTOR_SIZE;
 498
 499                 ret = target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_write);
 500                 if (ret != ERROR_OK)
 501                         LOG_ERROR("mg_io_cmd_confirm_write error");
 502
 503                 LOG_DEBUG("%u (0x%8.8x) sector write", sect_num + i, (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 504
 505                 duration_stop_measure(&duration, NULL);
 506
 507                 if ((duration.duration.tv_sec * 1000 + duration.duration.tv_usec / 1000) > 3000) {
 508                         LOG_INFO("wrote %u'th sectors", sect_num + i);
 509                         duration_start_measure(&duration);
 510                 }
 511         }
 512
 513         ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
 514
 515         return ret;
 516 }
 517
 518 static int mg_mflash_write_sects(void *buff, u32 sect_num, u32 sect_cnt)
 519 {
 520         u32 quotient, residue, i;
 521         u8 *buff_ptr = buff;
 522
 523         quotient = sect_cnt >> 8;
 524         residue = sect_cnt % 256;
 525
 526         for (i = 0; i < quotient; i++) {
 527                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 528                         (unsigned long)buff_ptr);
 529                 mg_mflash_do_write_sects(buff_ptr, sect_num, 256);
 530                 sect_num += 256;
 531                 buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 532         }
 533
 534         if (residue) {
 535                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 536                         (unsigned long)buff_ptr);
 537                 mg_mflash_do_write_sects(buff_ptr, sect_num, residue);
 538         }
 539
 540         return ERROR_OK;
 541 }
 542
 543 static int mg_mflash_read (u32 addr, u8 *buff, u32 len)
 544 {
 545         u8 *sect_buff, *buff_ptr = buff;
 546         u32 cur_addr, next_sec_addr, end_addr, cnt, sect_num;
 547
 548         cnt = 0;
 549         cur_addr = addr;
 550         end_addr = addr + len;
 551
 552         sect_buff = malloc(MG_MFLASH_SECTOR_SIZE);
 553
 554         if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 555
 556                 next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 557                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 558                 mg_mflash_read_sects(sect_buff, sect_num, 1);
 559
 560                 if (end_addr < next_sec_addr) {
 561                         memcpy(buff_ptr, sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), end_addr - cur_addr);
 562                         LOG_DEBUG("copies %u byte from sector offset 0x%8.8x", end_addr - cur_addr, cur_addr);
 563                         cur_addr = end_addr;
 564                 } else {
 565                         memcpy(buff_ptr, sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), next_sec_addr - cur_addr);
 566                         LOG_DEBUG("copies %u byte from sector offset 0x%8.8x", next_sec_addr - cur_addr, cur_addr);
 567                         buff_ptr += (next_sec_addr - cur_addr);
 568                         cur_addr = next_sec_addr;
 569                 }
 570         }
 571
 572         if (cur_addr < end_addr) {
 573
 574                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 575                 next_sec_addr = cur_addr + MG_MFLASH_SECTOR_SIZE;
 576
 577                 while (next_sec_addr <= end_addr) {
 578                         cnt++;
 579                         next_sec_addr += MG_MFLASH_SECTOR_SIZE;
 580                 }
 581
 582                 if (cnt)
 583                         mg_mflash_read_sects(buff_ptr, sect_num, cnt);
 584
 585                 buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 586                 cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
 587
 588                 if (cur_addr < end_addr) {
 589
 590                         sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 591                         mg_mflash_read_sects(sect_buff, sect_num, 1);
 592                         memcpy(buff_ptr, sect_buff, end_addr - cur_addr);
 593                         LOG_DEBUG("copies %u byte", end_addr - cur_addr);
 594
 595                 }
 596         }
 597
 598         free(sect_buff);
 599
 600         return ERROR_OK;
 601 }
 602
 603 static int mg_mflash_write(u32 addr, u8 *buff, u32 len)
 604 {
 605         u8 *sect_buff, *buff_ptr = buff;
 606         u32 cur_addr, next_sec_addr, end_addr, cnt, sect_num;
 607
 608         cnt = 0;
 609         cur_addr = addr;
 610         end_addr = addr + len;
 611
 612         sect_buff = malloc(MG_MFLASH_SECTOR_SIZE);
 613
 614         if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 615
 616                 next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 617                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 618                 mg_mflash_read_sects(sect_buff, sect_num, 1);
 619
 620                 if (end_addr < next_sec_addr) {
 621                         memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), buff_ptr, end_addr - cur_addr);
 622                         LOG_DEBUG("copies %u byte to sector offset 0x%8.8x", end_addr - cur_addr, cur_addr);
 623                         cur_addr = end_addr;
 624                 } else {
 625                         memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), buff_ptr, next_sec_addr - cur_addr);
 626                         LOG_DEBUG("copies %u byte to sector offset 0x%8.8x", next_sec_addr - cur_addr, cur_addr);
 627                         buff_ptr += (next_sec_addr - cur_addr);
 628                         cur_addr = next_sec_addr;
 629                 }
 630
 631                 mg_mflash_write_sects(sect_buff, sect_num, 1);
 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                         mg_mflash_write_sects(buff_ptr, sect_num, cnt);
 646
 647                 buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 648                 cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
 649
 650                 if (cur_addr < end_addr) {
 651
 652                         sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 653                         mg_mflash_read_sects(sect_buff, sect_num, 1);
 654                         memcpy(sect_buff, buff_ptr, end_addr - cur_addr);
 655                         LOG_DEBUG("copies %u byte", end_addr - cur_addr);
 656                         mg_mflash_write_sects(sect_buff, sect_num, 1);
 657                 }
 658         }
 659
 660         free(sect_buff);
 661
 662         return ERROR_OK;
 663 }
 664
 665 static int mg_write_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 666 {
 667         u32 address, buf_cnt;
 668         u8 *buffer;
 669         /* TODO : multi-bank support, large file support */
 670         fileio_t fileio;
 671         duration_t duration;
 672         char *duration_text;
 673         int ret;
 674
 675         if (argc != 3) {
 676                 return ERROR_COMMAND_SYNTAX_ERROR;
 677         }
 678
 679         address = strtoul(args[2], NULL, 0);
 680
 681         if (! mflash_bank->proved ) {
 682                 mg_mflash_probe();
 683         }
 684
 685         if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK) {
 686                 return ERROR_FAIL;
 687         }
 688
 689         buffer = malloc(fileio.size);
 690
 691         if (fileio_read(&fileio, fileio.size, buffer, &buf_cnt) != ERROR_OK)
 692         {
 693                 free(buffer);
 694                 fileio_close(&fileio);
 695                 return ERROR_FAIL;
 696         }
 697
 698         duration_start_measure(&duration);
 699
 700         ret = mg_mflash_write(address, buffer, (u32)fileio.size);
 701
 702         duration_stop_measure(&duration, &duration_text);
 703
 704         command_print(cmd_ctx, "wrote %lli byte from file %s in %s (%f kB/s)",
 705                 fileio.size, args[1], duration_text,
 706                 (float)fileio.size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
 707
 708         free(duration_text);
 709
 710         fileio_close(&fileio);
 711
 712         free(buffer);
 713
 714         return ERROR_OK;
 715 }
 716
 717 static int mg_dump_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 718 {
 719         u32 address, size_written, size;
 720         u8 *buffer;
 721         /* TODO : multi-bank support */
 722         fileio_t fileio;
 723         duration_t duration;
 724         char *duration_text;
 725
 726         if (argc != 4) {
 727                 return ERROR_COMMAND_SYNTAX_ERROR;
 728         }
 729
 730         address = strtoul(args[2], NULL, 0);
 731         size = strtoul(args[3], NULL, 0);
 732
 733         if (! mflash_bank->proved ) {
 734                         mg_mflash_probe();
 735         }
 736
 737         if (fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK) {
 738                 return ERROR_FAIL;
 739         }
 740
 741         buffer = malloc(size);
 742
 743         duration_start_measure(&duration);
 744
 745         mg_mflash_read(address, buffer, size);
 746
 747         duration_stop_measure(&duration, &duration_text);
 748
 749         fileio_write(&fileio, size, buffer, &size_written);
 750
 751         command_print(cmd_ctx, "dump image (address 0x%8.8x size %u) to file %s in %s (%f kB/s)",
 752                                 address, size, args[1], duration_text,
 753                                 (float)size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
 754
 755         free(duration_text);
 756
 757         fileio_close(&fileio);
 758
 759         free(buffer);
 760
 761         return ERROR_OK;
 762 }
 763
 764 int mflash_init_drivers(struct command_context_s *cmd_ctx)
 765 {
 766         if (mflash_bank) {
 767                 register_command(cmd_ctx, mflash_cmd, "probe", mg_probe_cmd, COMMAND_EXEC, NULL);
 768                 register_command(cmd_ctx, mflash_cmd, "write", mg_write_cmd, COMMAND_EXEC,
 769                                 "mflash write <num> <file> <address>");
 770                 register_command(cmd_ctx, mflash_cmd, "dump", mg_dump_cmd, COMMAND_EXEC,
 771                                                 "mflash dump <num> <file> <address> <size>");
 772         }
 773
 774         return ERROR_OK;
 775 }
 776
 777 static int mg_bank_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 778 {
 779         target_t *target;
 780         char *str;
 781         int i;
 782
 783         if (argc < 4)
 784         {
 785                 return ERROR_COMMAND_SYNTAX_ERROR;
 786         }
 787
 788         if ((target = get_target(args[3])) == NULL)
 789         {
 790                 LOG_ERROR("target '%s' not defined", args[3]);
 791                 return ERROR_FAIL;
 792         }
 793
 794         mflash_bank = calloc(sizeof(mflash_bank_t), 1);
 795         mflash_bank->base = strtoul(args[1], NULL, 0);
 796         mflash_bank->rst_pin.num = strtoul(args[2], &str, 0);
 797         if (*str)
 798                 mflash_bank->rst_pin.port[0] = (u16)tolower(str[0]);
 799
 800         mflash_bank->target = target;
 801
 802         for (i = 0; mflash_gpio[i] ; i++) {
 803                 if (! strcmp(mflash_gpio[i]->name, args[0])) {
 804                         mflash_bank->gpio_drv = mflash_gpio[i];
 805                 }
 806         }
 807
 808         if (! mflash_bank->gpio_drv) {
 809                 LOG_ERROR("%s is unsupported soc", args[0]);
 810                 return ERROR_INVALID_ARGUMENTS;
 811         }
 812
 813         return ERROR_OK;
 814 }
 815
 816 int mflash_register_commands(struct command_context_s *cmd_ctx)
 817 {
 818         mflash_cmd = register_command(cmd_ctx, NULL, "mflash", NULL, COMMAND_ANY, NULL);
 819         register_command(cmd_ctx, mflash_cmd, "bank", mg_bank_cmd, COMMAND_CONFIG,
 820                         "mflash bank <soc> <base> <RST pin> <target #>");
 821         return ERROR_OK;
 822 }