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

   1 /***************************************************************************
   2  *   Copyright (C) 2007-2008 by unsik Kim <donari75@gmail.com>             *
   3  *                                                                         *
   4  *   This program is free software; you can redistribute it and/or modify  *
   5  *   it under the terms of the GNU General Public License as published by  *
   6  *   the Free Software Foundation; either version 2 of the License, or     *
   7  *   (at your option) any later version.                                   *
   8  *                                                                         *
   9  *   This program is distributed in the hope that it will be useful,       *
  10  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  11  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  12  *   GNU General Public License for more details.                          *
  13  *                                                                         *
  14  *   You should have received a copy of the GNU General Public License     *
  15  *   along with this program; if not, 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         mg_init_gpio();
 362
 363         LOG_INFO("reset mflash");
 364
 365         mg_hdrst(0);
 366
 367         if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 368                 return ERROR_FAIL;
 369
 370         mg_hdrst(1);
 371
 372         if (mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 373                 return ERROR_FAIL;
 374
 375         mg_dsk_srst(1);
 376
 377         if (mg_dsk_wait(mg_io_wait_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 378                 return ERROR_FAIL;
 379
 380         mg_dsk_srst(0);
 381
 382         if (mg_dsk_wait(mg_io_wait_not_bsy, MG_OEM_DISK_WAIT_TIME_LONG) != ERROR_OK)
 383                 return ERROR_FAIL;
 384
 385         if (mg_dsk_drv_info() != ERROR_OK)
 386                 return ERROR_FAIL;
 387
 388         return ERROR_OK;
 389 }
 390
 391 static int mg_probe_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 392 {
 393         int ret;
 394
 395         ret = mg_mflash_probe();
 396
 397         if (ret == ERROR_OK) {
 398                 command_print(cmd_ctx, "mflash (total %u sectors) found at 0x%8.8x",
 399                                 mflash_bank->drv_info->tot_sects, mflash_bank->base );
 400         }
 401
 402         return ret;
 403 }
 404
 405 static int mg_mflash_do_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
 406 {
 407         u32 i, address;
 408         int ret;
 409         target_t *target = mflash_bank->target;
 410         u8 *buff_ptr = buff;
 411         duration_t duration;
 412
 413         if ( mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_read) != ERROR_OK )
 414                 return ERROR_FAIL;
 415
 416         address = mflash_bank->base + MG_BUFFER_OFFSET;
 417
 418         duration_start_measure(&duration);
 419
 420         for (i = 0; i < sect_cnt; i++) {
 421                 mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 422
 423                 target->type->read_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
 424                 buff_ptr += MG_MFLASH_SECTOR_SIZE;
 425
 426                 target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_read);
 427
 428                 LOG_DEBUG("%u (0x%8.8x) sector read", sect_num + i, (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 429
 430                 duration_stop_measure(&duration, NULL);
 431
 432                 if ((duration.duration.tv_sec * 1000 + duration.duration.tv_usec / 1000) > 3000) {
 433                         LOG_INFO("read %u'th sectors", sect_num + i);
 434                         duration_start_measure(&duration);
 435                 }
 436         }
 437
 438         ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
 439
 440         return ret;
 441 }
 442
 443 static int mg_mflash_read_sects(void *buff, u32 sect_num, u32 sect_cnt)
 444 {
 445         u32 quotient, residue, i;
 446         u8 *buff_ptr = buff;
 447
 448         quotient = sect_cnt >> 8;
 449         residue = sect_cnt % 256;
 450
 451         for (i = 0; i < quotient; i++) {
 452                 LOG_DEBUG("sect num : %u buff : 0x%0lx", sect_num,
 453                         (unsigned long)buff_ptr);
 454                 mg_mflash_do_read_sects(buff_ptr, sect_num, 256);
 455                 sect_num += 256;
 456                 buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 457         }
 458
 459         if (residue) {
 460                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 461                         (unsigned long)buff_ptr);
 462                 mg_mflash_do_read_sects(buff_ptr, sect_num, residue);
 463         }
 464
 465         return ERROR_OK;
 466 }
 467
 468 static int mg_mflash_do_write_sects(void *buff, u32 sect_num, u32 sect_cnt)
 469 {
 470         u32 i, address;
 471         int ret;
 472         target_t *target = mflash_bank->target;
 473         u8 *buff_ptr = buff;
 474         duration_t duration;
 475
 476         if ( mg_dsk_io_cmd(sect_num, sect_cnt, mg_io_cmd_write) != ERROR_OK ) {
 477                 LOG_ERROR("mg_io_cmd_write fail");
 478                 return ERROR_FAIL;
 479         }
 480
 481         address = mflash_bank->base + MG_BUFFER_OFFSET;
 482
 483         duration_start_measure(&duration);
 484
 485         for (i = 0; i < sect_cnt; i++) {
 486                 ret = mg_dsk_wait(mg_io_wait_drq, MG_OEM_DISK_WAIT_TIME_NORMAL);
 487                 if (ret != ERROR_OK)
 488                         LOG_ERROR("mg_io_wait_drq time out");
 489
 490                 ret = target->type->write_memory(target, address, 2, MG_MFLASH_SECTOR_SIZE / 2, buff_ptr);
 491                 if (ret != ERROR_OK)
 492                         LOG_ERROR("mem write error");
 493                 buff_ptr += MG_MFLASH_SECTOR_SIZE;
 494
 495                 ret = target_write_u8(target, mflash_bank->base + MG_REG_OFFSET + MG_REG_COMMAND, mg_io_cmd_confirm_write);
 496                 if (ret != ERROR_OK)
 497                         LOG_ERROR("mg_io_cmd_confirm_write error");
 498
 499                 LOG_DEBUG("%u (0x%8.8x) sector write", sect_num + i, (sect_num + i) * MG_MFLASH_SECTOR_SIZE);
 500
 501                 duration_stop_measure(&duration, NULL);
 502
 503                 if ((duration.duration.tv_sec * 1000 + duration.duration.tv_usec / 1000) > 3000) {
 504                         LOG_INFO("wrote %u'th sectors", sect_num + i);
 505                         duration_start_measure(&duration);
 506                 }
 507         }
 508
 509         ret = mg_dsk_wait(mg_io_wait_rdy, MG_OEM_DISK_WAIT_TIME_NORMAL);
 510
 511         return ret;
 512 }
 513
 514 static int mg_mflash_write_sects(void *buff, u32 sect_num, u32 sect_cnt)
 515 {
 516         u32 quotient, residue, i;
 517         u8 *buff_ptr = buff;
 518
 519         quotient = sect_cnt >> 8;
 520         residue = sect_cnt % 256;
 521
 522         for (i = 0; i < quotient; i++) {
 523                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 524                         (unsigned long)buff_ptr);
 525                 mg_mflash_do_write_sects(buff_ptr, sect_num, 256);
 526                 sect_num += 256;
 527                 buff_ptr += 256 * MG_MFLASH_SECTOR_SIZE;
 528         }
 529
 530         if (residue) {
 531                 LOG_DEBUG("sect num : %u buff : %0lx", sect_num,
 532                         (unsigned long)buff_ptr);
 533                 mg_mflash_do_write_sects(buff_ptr, sect_num, residue);
 534         }
 535
 536         return ERROR_OK;
 537 }
 538
 539 static int mg_mflash_read (u32 addr, u8 *buff, u32 len)
 540 {
 541         u8 *sect_buff, *buff_ptr = buff;
 542         u32 cur_addr, next_sec_addr, end_addr, cnt, sect_num;
 543
 544         cnt = 0;
 545         cur_addr = addr;
 546         end_addr = addr + len;
 547
 548         sect_buff = malloc(MG_MFLASH_SECTOR_SIZE);
 549
 550         if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 551
 552                 next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 553                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 554                 mg_mflash_read_sects(sect_buff, sect_num, 1);
 555
 556                 if (end_addr < next_sec_addr) {
 557                         memcpy(buff_ptr, sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), end_addr - cur_addr);
 558                         LOG_DEBUG("copies %u byte from sector offset 0x%8.8x", end_addr - cur_addr, cur_addr);
 559                         cur_addr = end_addr;
 560                 } else {
 561                         memcpy(buff_ptr, sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), next_sec_addr - cur_addr);
 562                         LOG_DEBUG("copies %u byte from sector offset 0x%8.8x", next_sec_addr - cur_addr, cur_addr);
 563                         buff_ptr += (next_sec_addr - cur_addr);
 564                         cur_addr = next_sec_addr;
 565                 }
 566         }
 567
 568         if (cur_addr < end_addr) {
 569
 570                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 571                 next_sec_addr = cur_addr + MG_MFLASH_SECTOR_SIZE;
 572
 573                 while (next_sec_addr <= end_addr) {
 574                         cnt++;
 575                         next_sec_addr += MG_MFLASH_SECTOR_SIZE;
 576                 }
 577
 578                 if (cnt)
 579                         mg_mflash_read_sects(buff_ptr, sect_num, cnt);
 580
 581                 buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 582                 cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
 583
 584                 if (cur_addr < end_addr) {
 585
 586                         sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 587                         mg_mflash_read_sects(sect_buff, sect_num, 1);
 588                         memcpy(buff_ptr, sect_buff, end_addr - cur_addr);
 589                         LOG_DEBUG("copies %u byte", end_addr - cur_addr);
 590
 591                 }
 592         }
 593
 594         free(sect_buff);
 595
 596         return ERROR_OK;
 597 }
 598
 599 static int mg_mflash_write(u32 addr, u8 *buff, u32 len)
 600 {
 601         u8 *sect_buff, *buff_ptr = buff;
 602         u32 cur_addr, next_sec_addr, end_addr, cnt, sect_num;
 603
 604         cnt = 0;
 605         cur_addr = addr;
 606         end_addr = addr + len;
 607
 608         sect_buff = malloc(MG_MFLASH_SECTOR_SIZE);
 609
 610         if (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK) {
 611
 612                 next_sec_addr = (cur_addr + MG_MFLASH_SECTOR_SIZE) & ~MG_MFLASH_SECTOR_SIZE_MASK;
 613                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 614                 mg_mflash_read_sects(sect_buff, sect_num, 1);
 615
 616                 if (end_addr < next_sec_addr) {
 617                         memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), buff_ptr, end_addr - cur_addr);
 618                         LOG_DEBUG("copies %u byte to sector offset 0x%8.8x", end_addr - cur_addr, cur_addr);
 619                         cur_addr = end_addr;
 620                 } else {
 621                         memcpy(sect_buff + (cur_addr & MG_MFLASH_SECTOR_SIZE_MASK), buff_ptr, next_sec_addr - cur_addr);
 622                         LOG_DEBUG("copies %u byte to sector offset 0x%8.8x", next_sec_addr - cur_addr, cur_addr);
 623                         buff_ptr += (next_sec_addr - cur_addr);
 624                         cur_addr = next_sec_addr;
 625                 }
 626
 627                 mg_mflash_write_sects(sect_buff, sect_num, 1);
 628         }
 629
 630         if (cur_addr < end_addr) {
 631
 632                 sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 633                 next_sec_addr = cur_addr + MG_MFLASH_SECTOR_SIZE;
 634
 635                 while (next_sec_addr <= end_addr) {
 636                         cnt++;
 637                         next_sec_addr += MG_MFLASH_SECTOR_SIZE;
 638                 }
 639
 640                 if (cnt)
 641                         mg_mflash_write_sects(buff_ptr, sect_num, cnt);
 642
 643                 buff_ptr += cnt * MG_MFLASH_SECTOR_SIZE;
 644                 cur_addr += cnt * MG_MFLASH_SECTOR_SIZE;
 645
 646                 if (cur_addr < end_addr) {
 647
 648                         sect_num = cur_addr >> MG_MFLASH_SECTOR_SIZE_SHIFT;
 649                         mg_mflash_read_sects(sect_buff, sect_num, 1);
 650                         memcpy(sect_buff, buff_ptr, end_addr - cur_addr);
 651                         LOG_DEBUG("copies %u byte", end_addr - cur_addr);
 652                         mg_mflash_write_sects(sect_buff, sect_num, 1);
 653                 }
 654         }
 655
 656         free(sect_buff);
 657
 658         return ERROR_OK;
 659 }
 660
 661 static int mg_write_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 662 {
 663         u32 address, buf_cnt;
 664         u8 *buffer;
 665         /* TODO : multi-bank support, large file support */
 666         fileio_t fileio;
 667         duration_t duration;
 668         char *duration_text;
 669         int ret;
 670
 671         if (argc != 3) {
 672                 return ERROR_COMMAND_SYNTAX_ERROR;
 673         }
 674
 675         address = strtoul(args[2], NULL, 0);
 676
 677         if (fileio_open(&fileio, args[1], FILEIO_READ, FILEIO_BINARY) != ERROR_OK) {
 678                 return ERROR_FAIL;
 679         }
 680
 681         buffer = malloc(fileio.size);
 682
 683         if (fileio_read(&fileio, fileio.size, buffer, &buf_cnt) != ERROR_OK)
 684         {
 685                 free(buffer);
 686                 fileio_close(&fileio);
 687                 return ERROR_FAIL;
 688         }
 689
 690         duration_start_measure(&duration);
 691
 692         ret = mg_mflash_write(address, buffer, (u32)fileio.size);
 693
 694         duration_stop_measure(&duration, &duration_text);
 695
 696         command_print(cmd_ctx, "wrote %lli byte from file %s in %s (%f kB/s)",
 697                 fileio.size, args[1], duration_text,
 698                 (float)fileio.size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
 699
 700         free(duration_text);
 701
 702         fileio_close(&fileio);
 703
 704         free(buffer);
 705
 706         return ERROR_OK;
 707 }
 708
 709 static int mg_dump_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 710 {
 711         u32 address, size_written, size;
 712         u8 *buffer;
 713         /* TODO : multi-bank support */
 714         fileio_t fileio;
 715         duration_t duration;
 716         char *duration_text;
 717
 718         if (argc != 4) {
 719                 return ERROR_COMMAND_SYNTAX_ERROR;
 720         }
 721
 722         address = strtoul(args[2], NULL, 0);
 723         size = strtoul(args[3], NULL, 0);
 724
 725         if (fileio_open(&fileio, args[1], FILEIO_WRITE, FILEIO_BINARY) != ERROR_OK) {
 726                 return ERROR_FAIL;
 727         }
 728
 729         buffer = malloc(size);
 730
 731         duration_start_measure(&duration);
 732
 733         mg_mflash_read(address, buffer, size);
 734
 735         duration_stop_measure(&duration, &duration_text);
 736
 737         fileio_write(&fileio, size, buffer, &size_written);
 738
 739         command_print(cmd_ctx, "dump image (address 0x%8.8x size %u) to file %s in %s (%f kB/s)",
 740                                 address, size, args[1], duration_text,
 741                                 (float)size / 1024.0 / ((float)duration.duration.tv_sec + ((float)duration.duration.tv_usec / 1000000.0)));
 742
 743         free(duration_text);
 744
 745         fileio_close(&fileio);
 746
 747         free(buffer);
 748
 749         return ERROR_OK;
 750 }
 751
 752 int mflash_init_drivers(struct command_context_s *cmd_ctx)
 753 {
 754         if (mflash_bank) {
 755                 register_command(cmd_ctx, mflash_cmd, "probe", mg_probe_cmd, COMMAND_EXEC, NULL);
 756                 register_command(cmd_ctx, mflash_cmd, "write", mg_write_cmd, COMMAND_EXEC,
 757                                 "mflash write <num> <file> <address>");
 758                 register_command(cmd_ctx, mflash_cmd, "dump", mg_dump_cmd, COMMAND_EXEC,
 759                                                 "mflash dump <num> <file> <address> <size>");
 760         }
 761
 762         return ERROR_OK;
 763 }
 764
 765 static int mg_bank_cmd(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 766 {
 767         target_t *target;
 768         char *str;
 769         int i;
 770
 771         if (argc < 4)
 772         {
 773                 return ERROR_COMMAND_SYNTAX_ERROR;
 774         }
 775
 776         if ((target = get_target(args[3])) == NULL)
 777         {
 778                 LOG_ERROR("target '%s' not defined", args[3]);
 779                 return ERROR_FAIL;
 780         }
 781
 782         mflash_bank = calloc(sizeof(mflash_bank_t), 1);
 783         mflash_bank->base = strtoul(args[1], NULL, 0);
 784         mflash_bank->rst_pin.num = strtoul(args[2], &str, 0);
 785         if (*str)
 786                 mflash_bank->rst_pin.port[0] = (u16)tolower(str[0]);
 787
 788         mflash_bank->target = target;
 789
 790         for (i = 0; mflash_gpio[i] ; i++) {
 791                 if (! strcmp(mflash_gpio[i]->name, args[0])) {
 792                         mflash_bank->gpio_drv = mflash_gpio[i];
 793                 }
 794         }
 795
 796         if (! mflash_bank->gpio_drv) {
 797                 LOG_ERROR("%s is unsupported soc", args[0]);
 798                 return ERROR_INVALID_ARGUMENTS;
 799         }
 800
 801         return ERROR_OK;
 802 }
 803
 804 int mflash_register_commands(struct command_context_s *cmd_ctx)
 805 {
 806         mflash_cmd = register_command(cmd_ctx, NULL, "mflash", NULL, COMMAND_ANY, NULL);
 807         register_command(cmd_ctx, mflash_cmd, "bank", mg_bank_cmd, COMMAND_CONFIG,
 808                         "mflash bank <soc> <base> <RST pin> <target #>");
 809         return ERROR_OK;
 810 }