git.gag.com Git - fw/openocd/blob - src/flash/mflash.c

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