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