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 mflash_rst(u8 level)
 195 {
 196         return mflash_bank->gpio_drv->set_gpio_output_val(mflash_bank->rst_pin, level);
 197 }
 198
 199 static int mflash_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         if (mflash_bank->wp_pin.num != -1) {
 207                 gpio_drv->set_gpio_to_output(mflash_bank->wp_pin);
 208                 gpio_drv->set_gpio_output_val(mflash_bank->wp_pin, 1);
 209         }
 210
 211         if (mflash_bank->dpd_pin.num != -1) {
 212                 gpio_drv->set_gpio_to_output(mflash_bank->dpd_pin);
 213                 gpio_drv->set_gpio_output_val(mflash_bank->dpd_pin, 1);
 214         }
 215
 216         return ERROR_OK;
 217 }
 218
 219 static int mg_dsk_wait(mg_io_type_wait wait, u32 time)
 220 {
 221         u8 status, error;
 222         target_t *target = mflash_bank->target;
 223         u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 224         duration_t duration;
 225         long long t=0;
 226
 227         duration_start_measure(&duration);
 228
 229         while (time) {
 230
 231                 target_read_u8(target, mg_task_reg + MG_REG_STATUS, &status);
 232
 233                 if (status & mg_io_rbit_status_busy)
 234                 {
 235                         if (wait == mg_io_wait_bsy)
 236                                 return ERROR_OK;
 237                 } else {
 238                         switch(wait)
 239                         {
 240                                 case mg_io_wait_not_bsy:
 241                                         return ERROR_OK;
 242                                 case mg_io_wait_rdy_noerr:
 243                                         if (status & mg_io_rbit_status_ready)
 244                                                 return ERROR_OK;
 245                                         break;
 246                                 case mg_io_wait_drq_noerr:
 247                                         if (status & mg_io_rbit_status_data_req)
 248                                                 return ERROR_OK;
 249                                         break;
 250                                 default:
 251                                         break;
 252                         }
 253
 254                         /* Now we check the error condition! */
 255                         if (status & mg_io_rbit_status_error)
 256                         {
 257                                 target_read_u8(target, mg_task_reg + MG_REG_ERROR, &error);
 258
 259                                 if (error & mg_io_rbit_err_bad_sect_num) {
 260                                         LOG_ERROR("sector not found");
 261                                         return ERROR_FAIL;
 262                                 }
 263                                 else if (error & (mg_io_rbit_err_bad_block | mg_io_rbit_err_uncorrectable)) {
 264                                         LOG_ERROR("bad block");
 265                                         return ERROR_FAIL;
 266                                 } else {
 267                                         LOG_ERROR("disk operation fail");
 268                                         return ERROR_FAIL;
 269                                 }
 270                         }
 271
 272                         switch (wait)
 273                         {
 274                                 case mg_io_wait_rdy:
 275                                         if (status & mg_io_rbit_status_ready)
 276                                                 return ERROR_OK;
 277
 278                                 case mg_io_wait_drq:
 279                                         if (status & mg_io_rbit_status_data_req)
 280                                                 return ERROR_OK;
 281
 282                                 default:
 283                                         break;
 284                         }
 285                 }
 286
 287                 duration_stop_measure(&duration, NULL);
 288
 289                 t=duration.duration.tv_usec/1000;
 290                 t+=duration.duration.tv_sec*1000;
 291
 292                 if (t > time)
 293                         break;
 294         }
 295
 296         LOG_ERROR("timeout occured");
 297         return ERROR_FAIL;
 298 }
 299
 300 static int mg_dsk_srst(u8 on)
 301 {
 302         target_t *target = mflash_bank->target;
 303         u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 304         u8 value;
 305         int ret;
 306
 307         if ((ret = target_read_u8(target, mg_task_reg + MG_REG_DRV_CTRL, &value)) != ERROR_OK)
 308                 return ret;
 309
 310         if(on) {
 311                 value |= (mg_io_rbit_devc_srst);
 312         } else {
 313                 value &= ~mg_io_rbit_devc_srst;
 314         }
 315
 316         ret = target_write_u8(target, mg_task_reg + MG_REG_DRV_CTRL, value);
 317         return ret;
 318 }
 319
 320 static int mg_dsk_io_cmd(u32 sect_num, u32 cnt, u8 cmd)
 321 {
 322         target_t *target = mflash_bank->target;
 323         u32 mg_task_reg = mflash_bank->base + MG_REG_OFFSET;
 324         u8 value;
 325
 326         if (mg_dsk_wait(mg_io_wait_rdy_noerr, MG_OEM_DISK_WAIT_TIME_NORMAL) != ERROR_OK)
 327                 return ERROR_FAIL;
 328
 329         value = mg_io_rval_dev_drv_master | mg_io_rval_dev_lba_mode |((sect_num >> 24) & 0xf);
 330
 331         target_write_u8(target, mg_task_reg + MG_REG_DRV_HEAD, value);
 332         target_write_u8(target, mg_task_reg + MG_REG_SECT_CNT, (u8)cnt);
 333         target_write_u8(target, mg_task_reg + MG_REG_SECT_NUM, (u8)sect_num);
 334         target_write_u8(target, mg_task_reg + MG_REG_CYL_LOW, (u8)(sect_num >> 8));
 335         target_write_u8(target, mg_task_reg + MG_REG_CYL_HIGH, (u8)(sect_num >> 16));
 336
 337         target_write_u8(target, mg_task_reg + MG_REG_COMMAND, cmd);
 338
 339         return ERROR_OK;
 340 }
 341
 342 static int mg_dsk_drv_info(void)
 343 {
 344         target_t *target = mflash_bank->target;
 345         u32 mg_buff = mflash_bank->base + MG_BUFFER_OFFSET;
 346
 347         if ( mg_dsk_io_cmd(0, 1, mg_io_cmd_identify) != ERROR_OK)
 348                 return ERROR_FAIL;
 349
 350         if ( mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL) != ERROR_OK)
 351                 return ERROR_FAIL;
 352
 353         LOG_INFO("read drive info.");
 354
 355         if (! mflash_bank->drv_info)
 356                 mflash_bank->drv_info = malloc(sizeof(mg_drv_info_t));
 357
 358         target->type->read_memory(target, mg_buff, 2, sizeof(mg_io_type_drv_info) >> 1,
 359                         (u8 *)&mflash_bank->drv_info->drv_id);
 360
 361         mflash_bank->drv_info->tot_sects = (u32)(mflash_bank->drv_info->drv_id.total_user_addressable_sectors_hi << 16)
 362                                                                         + mflash_bank->drv_info->drv_id.total_user_addressable_sectors_lo;
 363
 364         target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_read);
 365
 366         return ERROR_OK;
 367 }
 368
 369 static int mg_mflash_probe(void)
 370 {
 371         mflash_bank->proved = 0;
 372
 373         mflash_init_gpio();
 374
 375         LOG_INFO("reset mflash");
 376
 377         mflash_rst(0);
 378
 379         if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 380                 return ERROR_FAIL;
 381
 382         mflash_rst(1);
 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         mg_dsk_srst(1);
 388
 389         if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 390                 return ERROR_FAIL;
 391
 392         mg_dsk_srst(0);
 393
 394         if (mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 395                 return ERROR_FAIL;
 396
 397         if (mg_dsk_drv_info() != ERROR_OK)
 398                 return ERROR_FAIL;
 399
 400         mflash_bank->proved = 1;
 401
 402         return ERROR_OK;
 403 }
 404
 405 static int mflash_probe_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 406 {
 407         int ret;
 408
 409         ret = mg_mflash_probe();
 410
 411         if (ret == ERROR_OK) {
 412                 command_print(cmd_ctx, "mflash (total %u sectors) found at 0x%8.8x",
 413                                 mflash_bank->drv_info->tot_sects, mflash_bank->base );
 414         }
 415
 416         return ret;
 417 }
 418
 419 static int mg_mflash_do_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
 420 {
 421         u32 i, address;
 422         int ret;
 423         target_t *target = mflash_bank->target;
 424         u8 *buff_ptr = buff;
 425         duration_t duration;
 426
 427         if ( mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_read) != ERROR_OK )
 428                 return ERROR_FAIL;
 429
 430         address = mflash_bank->base + MG_BUFFER_OFFSET;
 431
 432         duration_start_measure(&duration);
 433
 434         for (i = 0; i < sect_cnt; i++) {
 435                 mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 436
 437                 target->type->read_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
 438                 buff_ptr += MG_MFLASH_SECTOR_SIZE;
 439
 440                 target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_read);
 441
 442                 LOG_DEBUG("%u (0x%8.8x) sector read", sect_num + i, (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 443
 444                 duration_stop_measure(&duration, NULL);
 445
 446                 if ((duration.duration.tv_sec * 1000 + duration.duration.tv_usec / 1000) > 3000) {
 447                         LOG_INFO("read %u'th sectors", sect_num + i);
 448                         duration_start_measure(&duration);
 449                 }
 450         }
 451
 452         ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
 453
 454         return ret;
 455 }
 456
 457 static int mg_mflash_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
 458 {
 459         u32 quotient, residue, i;
 460         u8 *buff_ptr = buff;
 461
 462         quotient = sect_cnt >> 8;
 463         residue = sect_cnt % 256;
 464
 465         for (i = 0; i < quotient; i++) {
 466                 LOG_DEBUG("sect num : %u buff : 0x%0lx", sect_num,
 467                         (unsigned long)buff_ptr);
 468                 mg_mflash_do_read_sects(buff_ptr, sect_num, 256);
 469                 sect_num += 256;
 470                 buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 471         }
 472
 473         if (residue) {
 474                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 475                         (unsigned long)buff_ptr);
 476                 mg_mflash_do_read_sects(buff_ptr, sect_num, residue);
 477         }
 478
 479         return ERROR_OK;
 480 }
 481
 482 static int mg_mflash_do_write_sects(void *buff, u32 sect_num, u32 sect_cnt)
 483 {
 484         u32 i, address;
 485         int ret;
 486         target_t *target = mflash_bank->target;
 487         u8 *buff_ptr = buff;
 488         duration_t duration;
 489
 490         if ( mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_write) != ERROR_OK ) {
 491                 LOG_ERROR("mg_io_cmd_write fail");
 492                 return ERROR_FAIL;
 493         }
 494
 495         address = mflash_bank->base + MG_BUFFER_OFFSET;
 496
 497         duration_start_measure(&duration);
 498
 499         for (i = 0; i < sect_cnt; i++) {
 500                 ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 501                 if (ret != ERROR_OK)
 502                         LOG_ERROR("mg_io_wait_drq time out");
 503
 504                 ret = target->type->write_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
 505                 if (ret != ERROR_OK)
 506                         LOG_ERROR("mem write error");
 507                 buff_ptr += MG_MFLASH_SECTOR_SIZE;
 508
 509                 ret = target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_write);
 510                 if (ret != ERROR_OK)
 511                         LOG_ERROR("mg_io_cmd_confirm_write error");
 512
 513                 LOG_DEBUG("%u (0x%8.8x) sector write", sect_num + i, (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 514
 515                 duration_stop_measure(&duration, NULL);
 516
 517                 if ((duration.duration.tv_sec * 1000 + duration.duration.tv_usec / 1000) > 3000) {
 518                         LOG_INFO("wrote %u'th sectors", sect_num + i);
 519                         duration_start_measure(&duration);
 520                 }
 521         }
 522
 523         ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
 524
 525         return ret;
 526 }
 527
 528 static int mg_mflash_write_sects(void *buff, u32 sect_num, u32 sect_cnt)
 529 {
 530         u32 quotient, residue, i;
 531         u8 *buff_ptr = buff;
 532
 533         quotient = sect_cnt >> 8;
 534         residue = sect_cnt % 256;
 535
 536         for (i = 0; i < quotient; i++) {
 537                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 538                         (unsigned long)buff_ptr);
 539                 mg_mflash_do_write_sects(buff_ptr, sect_num, 256);
 540                 sect_num += 256;
 541                 buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 542         }
 543
 544         if (residue) {
 545                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 546                         (unsigned long)buff_ptr);
 547                 mg_mflash_do_write_sects(buff_ptr, sect_num, residue);
 548         }
 549
 550         return ERROR_OK;
 551 }
 552
 553 static int mg_mflash_read (u32 addr, u8 *buff, u32 len)
 554 {
 555         u8 *sect_buff, *buff_ptr = buff;
 556         u32 cur_addr, next_sec_addr, end_addr, cnt, sect_num;
 557
 558         cnt = 0;
 559         cur_addr = addr;
 560         end_addr = addr + len;
 561
 562         sect_buff = malloc(MG_MFLASH_SECTOR_SIZE);
 563
 564         if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 565
 566                 next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 567                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 568                 mg_mflash_read_sects(sect_buff, sect_num, 1);
 569
 570                 if (end_addr < next_sec_addr) {
 571                         memcpy(buff_ptr, sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), end_addr - cur_addr);
 572                         LOG_DEBUG("copies %u byte from sector offset 0x%8.8x", end_addr - cur_addr, cur_addr);
 573                         cur_addr = end_addr;
 574                 } else {
 575                         memcpy(buff_ptr, sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), next_sec_addr - cur_addr);
 576                         LOG_DEBUG("copies %u byte from sector offset 0x%8.8x", next_sec_addr - cur_addr, cur_addr);
 577                         buff_ptr += (next_sec_addr - cur_addr);
 578                         cur_addr = next_sec_addr;
 579                 }
 580         }
 581
 582         if (cur_addr < end_addr) {
 583
 584                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 585                 next_sec_addr = cur_addr + MG_MFLASH_SECTOR_SIZE;
 586
 587                 while (next_sec_addr <= end_addr) {
 588                         cnt++;
 589                         next_sec_addr += MG_MFLASH_SECTOR_SIZE;
 590                 }
 591
 592                 if (cnt)
 593                         mg_mflash_read_sects(buff_ptr, sect_num, cnt);
 594
 595                 buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 596                 cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
 597
 598                 if (cur_addr < end_addr) {
 599
 600                         sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 601                         mg_mflash_read_sects(sect_buff, sect_num, 1);
 602                         memcpy(buff_ptr, sect_buff, end_addr - cur_addr);
 603                         LOG_DEBUG("copies %u byte", end_addr - cur_addr);
 604
 605                 }
 606         }
 607
 608         free(sect_buff);
 609
 610         return ERROR_OK;
 611 }
 612
 613 static int mg_mflash_write(u32 addr, u8 *buff, u32 len)
 614 {
 615         u8 *sect_buff, *buff_ptr = buff;
 616         u32 cur_addr, next_sec_addr, end_addr, cnt, sect_num;
 617
 618         cnt = 0;
 619         cur_addr = addr;
 620         end_addr = addr + len;
 621
 622         sect_buff = malloc(MG_MFLASH_SECTOR_SIZE);
 623
 624         if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 625
 626                 next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 627                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 628                 mg_mflash_read_sects(sect_buff, sect_num, 1);
 629
 630                 if (end_addr < next_sec_addr) {
 631                         memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), buff_ptr, end_addr - cur_addr);
 632                         LOG_DEBUG("copies %u byte to sector offset 0x%8.8x", end_addr - cur_addr, cur_addr);
 633                         cur_addr = end_addr;
 634                 } else {
 635                         memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), buff_ptr, next_sec_addr - cur_addr);
 636                         LOG_DEBUG("copies %u byte to sector offset 0x%8.8x", next_sec_addr - cur_addr, cur_addr);
 637                         buff_ptr += (next_sec_addr - cur_addr);
 638                         cur_addr = next_sec_addr;
 639                 }
 640
 641                 mg_mflash_write_sects(sect_buff, sect_num, 1);
 642         }
 643
 644         if (cur_addr < end_addr) {
 645
 646                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 647                 next_sec_addr = cur_addr + MG_MFLASH_SECTOR_SIZE;
 648
 649                 while (next_sec_addr <= end_addr) {
 650                         cnt++;
 651                         next_sec_addr += MG_MFLASH_SECTOR_SIZE;
 652                 }
 653
 654                 if (cnt)
 655                         mg_mflash_write_sects(buff_ptr, sect_num, cnt);
 656
 657                 buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 658                 cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
 659
 660                 if (cur_addr < end_addr) {
 661
 662                         sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 663                         mg_mflash_read_sects(sect_buff, sect_num, 1);
 664                         memcpy(sect_buff, buff_ptr, end_addr - cur_addr);
 665                         LOG_DEBUG("copies %u byte", end_addr - cur_addr);
 666                         mg_mflash_write_sects(sect_buff, sect_num, 1);
 667                 }
 668         }
 669
 670         free(sect_buff);
 671
 672         return ERROR_OK;
 673 }
 674
 675 static int mflash_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 676 {
 677         u32 address, buf_cnt;
 678         u8 *buffer;
 679         /* TODO : multi-bank support, large file support */
 680         fileio_t fileio;
 681         duration_t duration;
 682         char *duration_text;
 683         int ret;
 684
 685         if (argc != 3) {
 686                 return ERROR_COMMAND_SYNTAX_ERROR;
 687         }
 688
 689         address = strtoul(args[2], NULL, 0);
 690
 691         if (! mflash_bank->proved ) {
 692                 mg_mflash_probe();
 693         }
 694
 695         if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK) {
 696                 return ERROR_FAIL;
 697         }
 698
 699         buffer = malloc(fileio.size);
 700
 701         if (fileio_read(&fileio, fileio.size, buffer, &buf_cnt) != ERROR_OK)
 702         {
 703                 free(buffer);
 704                 fileio_close(&fileio);
 705                 return ERROR_FAIL;
 706         }
 707
 708         duration_start_measure(&duration);
 709
 710         ret = mg_mflash_write(address, buffer, (u32)fileio.size);
 711
 712         duration_stop_measure(&duration, &duration_text);
 713
 714         command_print(cmd_ctx, "wrote %lli byte from file %s in %s (%f kB/s)",
 715                 fileio.size, args[1], duration_text,
 716                 (float)fileio.size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
 717
 718         free(duration_text);
 719
 720         fileio_close(&fileio);
 721
 722         free(buffer);
 723
 724         return ERROR_OK;
 725 }
 726
 727 static int mflash_dump_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 728 {
 729         u32 address, size_written, size;
 730         u8 *buffer;
 731         /* TODO : multi-bank support */
 732         fileio_t fileio;
 733         duration_t duration;
 734         char *duration_text;
 735
 736         if (argc != 4) {
 737                 return ERROR_COMMAND_SYNTAX_ERROR;
 738         }
 739
 740         address = strtoul(args[2], NULL, 0);
 741         size = strtoul(args[3], NULL, 0);
 742
 743         if (! mflash_bank->proved ) {
 744                         mg_mflash_probe();
 745         }
 746
 747         if (fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK) {
 748                 return ERROR_FAIL;
 749         }
 750
 751         buffer = malloc(size);
 752
 753         duration_start_measure(&duration);
 754
 755         mg_mflash_read(address, buffer, size);
 756
 757         duration_stop_measure(&duration, &duration_text);
 758
 759         fileio_write(&fileio, size, buffer, &size_written);
 760
 761         command_print(cmd_ctx, "dump image (address 0x%8.8x size %u) to file %s in %s (%f kB/s)",
 762                                 address, size, args[1], duration_text,
 763                                 (float)size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
 764
 765         free(duration_text);
 766
 767         fileio_close(&fileio);
 768
 769         free(buffer);
 770
 771         return ERROR_OK;
 772 }
 773
 774 int mflash_init_drivers(struct command_context_s *cmd_ctx)
 775 {
 776         if (mflash_bank) {
 777                 register_command(cmd_ctx, mflash_cmd, "probe", mflash_probe_command, COMMAND_EXEC, NULL);
 778                 register_command(cmd_ctx, mflash_cmd, "write", mflash_write_command, COMMAND_EXEC,
 779                                 "mflash write <num> <file> <address>");
 780                 register_command(cmd_ctx, mflash_cmd, "dump", mflash_dump_command, COMMAND_EXEC,
 781                                                 "mflash dump <num> <file> <address> <size>");
 782         }
 783
 784         return ERROR_OK;
 785 }
 786
 787 static int mflash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 788 {
 789         target_t *target;
 790         char *str;
 791         int i;
 792
 793         if (argc < 8)
 794         {
 795                 return ERROR_COMMAND_SYNTAX_ERROR;
 796         }
 797
 798         if ((target = get_target_by_num(strtoul(args[7], NULL, 0))) == NULL)
 799         {
 800                 LOG_ERROR("target %lu not defined", strtoul(args[7], NULL, 0));
 801                 return ERROR_FAIL;
 802         }
 803
 804         mflash_bank = calloc(sizeof(mflash_bank_t), 1);
 805         mflash_bank->base = strtoul(args[1], NULL, 0);
 806         mflash_bank->chip_width = strtoul(args[2], NULL, 0);
 807         mflash_bank->bus_width = strtoul(args[3], NULL, 0);
 808         mflash_bank->rst_pin.num = strtoul(args[4], &str, 0);
 809         if (*str)
 810                 mflash_bank->rst_pin.port[0] = (u16)tolower(str[0]);
 811         mflash_bank->wp_pin.num = strtol(args[5], &str, 0);
 812         if (*str)
 813                 mflash_bank->wp_pin.port[0] = (u16)tolower(str[0]);
 814         mflash_bank->dpd_pin.num = strtol(args[6], &str, 0);
 815         if (*str)
 816                 mflash_bank->dpd_pin.port[0] = (u16)tolower(str[0]);
 817
 818         mflash_bank->target = target;
 819
 820         for (i = 0; mflash_gpio[i] ; i++) {
 821                 if (! strcmp(mflash_gpio[i]->name, args[0])) {
 822                         mflash_bank->gpio_drv = mflash_gpio[i];
 823                 }
 824         }
 825
 826         if (! mflash_bank->gpio_drv) {
 827                 LOG_ERROR("%s is unsupported soc", args[0]);
 828                 return ERROR_INVALID_ARGUMENTS;
 829         }
 830
 831         return ERROR_OK;
 832 }
 833
 834 int mflash_register_commands(struct command_context_s *cmd_ctx)
 835 {
 836         mflash_cmd = register_command(cmd_ctx, NULL, "mflash", NULL, COMMAND_ANY, NULL);
 837         register_command(cmd_ctx, mflash_cmd, "bank", mflash_bank_command, COMMAND_CONFIG,
 838                         "mflash bank <soc> <base> <chip_width> <bus_width> <RST pin> <WP pin> <DPD pin> <target #>");
 839         return ERROR_OK;
 840 }