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