src/flash/mflash.c

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