drivers/staging/rts5208/ms.c

   1 /* Driver for Realtek PCI-Express card reader
   2  *
   3  * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
   4  *
   5  * This program is free software; you can redistribute it and/or modify it
   6  * under the terms of the GNU General Public License as published by the
   7  * Free Software Foundation; either version 2, or (at your option) any
   8  * later version.
   9  *
  10  * This program is distributed in the hope that it will be useful, but
  11  * WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13  * General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License along
  16  * with this program; if not, see <http://www.gnu.org/licenses/>.
  17  *
  18  * Author:
  19  *   Wei WANG (wei_wang@realsil.com.cn)
  20  *   Micky Ching (micky_ching@realsil.com.cn)
  21  */
  22
  23 #include <linux/blkdev.h>
  24 #include <linux/kthread.h>
  25 #include <linux/sched.h>
  26 #include <linux/vmalloc.h>
  27
  28 #include "rtsx.h"
  29 #include "ms.h"
  30
  31 static inline void ms_set_err_code(struct rtsx_chip *chip, u8 err_code)
  32 {
  33         struct ms_info *ms_card = &chip->ms_card;
  34
  35         ms_card->err_code = err_code;
  36 }
  37
  38 static inline int ms_check_err_code(struct rtsx_chip *chip, u8 err_code)
  39 {
  40         struct ms_info *ms_card = &chip->ms_card;
  41
  42         return (ms_card->err_code == err_code);
  43 }
  44
  45 static int ms_parse_err_code(struct rtsx_chip *chip)
  46 {
  47         rtsx_trace(chip);
  48         return STATUS_FAIL;
  49 }
  50
  51 static int ms_transfer_tpc(struct rtsx_chip *chip, u8 trans_mode,
  52                            u8 tpc, u8 cnt, u8 cfg)
  53 {
  54         struct ms_info *ms_card = &chip->ms_card;
  55         int retval;
  56         u8 *ptr;
  57
  58         dev_dbg(rtsx_dev(chip), "%s: tpc = 0x%x\n", __func__, tpc);
  59
  60         rtsx_init_cmd(chip);
  61
  62         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
  63         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
  64         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
  65         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
  66                      0x01, PINGPONG_BUFFER);
  67
  68         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER,
  69                      0xFF, MS_TRANSFER_START | trans_mode);
  70         rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
  71                      MS_TRANSFER_END, MS_TRANSFER_END);
  72
  73         rtsx_add_cmd(chip, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
  74
  75         retval = rtsx_send_cmd(chip, MS_CARD, 5000);
  76         if (retval < 0) {
  77                 rtsx_clear_ms_error(chip);
  78                 ms_set_err_code(chip, MS_TO_ERROR);
  79                 rtsx_trace(chip);
  80                 return ms_parse_err_code(chip);
  81         }
  82
  83         ptr = rtsx_get_cmd_data(chip) + 1;
  84
  85         if (!(tpc & 0x08)) {            /* Read Packet */
  86                 if (*ptr & MS_CRC16_ERR) {
  87                         ms_set_err_code(chip, MS_CRC16_ERROR);
  88                         rtsx_trace(chip);
  89                         return ms_parse_err_code(chip);
  90                 }
  91         } else {                        /* Write Packet */
  92                 if (CHK_MSPRO(ms_card) && !(*ptr & 0x80)) {
  93                         if (*ptr & (MS_INT_ERR | MS_INT_CMDNK)) {
  94                                 ms_set_err_code(chip, MS_CMD_NK);
  95                                 rtsx_trace(chip);
  96                                 return ms_parse_err_code(chip);
  97                         }
  98                 }
  99         }
 100
 101         if (*ptr & MS_RDY_TIMEOUT) {
 102                 rtsx_clear_ms_error(chip);
 103                 ms_set_err_code(chip, MS_TO_ERROR);
 104                 rtsx_trace(chip);
 105                 return ms_parse_err_code(chip);
 106         }
 107
 108         return STATUS_SUCCESS;
 109 }
 110
 111 static int ms_transfer_data(struct rtsx_chip *chip, u8 trans_mode,
 112                             u8 tpc, u16 sec_cnt, u8 cfg, bool mode_2k,
 113                             int use_sg, void *buf, int buf_len)
 114 {
 115         int retval;
 116         u8 val, err_code = 0;
 117         enum dma_data_direction dir;
 118
 119         if (!buf || !buf_len) {
 120                 rtsx_trace(chip);
 121                 return STATUS_FAIL;
 122         }
 123
 124         if (trans_mode == MS_TM_AUTO_READ) {
 125                 dir = DMA_FROM_DEVICE;
 126                 err_code = MS_FLASH_READ_ERROR;
 127         } else if (trans_mode == MS_TM_AUTO_WRITE) {
 128                 dir = DMA_TO_DEVICE;
 129                 err_code = MS_FLASH_WRITE_ERROR;
 130         } else {
 131                 rtsx_trace(chip);
 132                 return STATUS_FAIL;
 133         }
 134
 135         rtsx_init_cmd(chip);
 136
 137         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
 138         rtsx_add_cmd(chip, WRITE_REG_CMD,
 139                      MS_SECTOR_CNT_H, 0xFF, (u8)(sec_cnt >> 8));
 140         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_L, 0xFF, (u8)sec_cnt);
 141         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
 142
 143         if (mode_2k) {
 144                 rtsx_add_cmd(chip, WRITE_REG_CMD,
 145                              MS_CFG, MS_2K_SECTOR_MODE, MS_2K_SECTOR_MODE);
 146         } else {
 147                 rtsx_add_cmd(chip, WRITE_REG_CMD, MS_CFG, MS_2K_SECTOR_MODE, 0);
 148         }
 149
 150         trans_dma_enable(dir, chip, sec_cnt * 512, DMA_512);
 151
 152         rtsx_add_cmd(chip, WRITE_REG_CMD,
 153                      MS_TRANSFER, 0xFF, MS_TRANSFER_START | trans_mode);
 154         rtsx_add_cmd(chip, CHECK_REG_CMD,
 155                      MS_TRANSFER, MS_TRANSFER_END, MS_TRANSFER_END);
 156
 157         rtsx_send_cmd_no_wait(chip);
 158
 159         retval = rtsx_transfer_data(chip, MS_CARD, buf, buf_len,
 160                                     use_sg, dir, chip->mspro_timeout);
 161         if (retval < 0) {
 162                 ms_set_err_code(chip, err_code);
 163                 if (retval == -ETIMEDOUT)
 164                         retval = STATUS_TIMEDOUT;
 165                 else
 166                         retval = STATUS_FAIL;
 167
 168                 rtsx_trace(chip);
 169                 return retval;
 170         }
 171
 172         retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
 173         if (retval) {
 174                 rtsx_trace(chip);
 175                 return retval;
 176         }
 177         if (val & (MS_INT_CMDNK | MS_INT_ERR | MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
 178                 rtsx_trace(chip);
 179                 return STATUS_FAIL;
 180         }
 181
 182         return STATUS_SUCCESS;
 183 }
 184
 185 static int ms_write_bytes(struct rtsx_chip *chip,
 186                           u8 tpc, u8 cnt, u8 cfg, u8 *data, int data_len)
 187 {
 188         struct ms_info *ms_card = &chip->ms_card;
 189         int retval, i;
 190
 191         if (!data || (data_len < cnt)) {
 192                 rtsx_trace(chip);
 193                 return STATUS_ERROR;
 194         }
 195
 196         rtsx_init_cmd(chip);
 197
 198         for (i = 0; i < cnt; i++) {
 199                 rtsx_add_cmd(chip, WRITE_REG_CMD,
 200                              PPBUF_BASE2 + i, 0xFF, data[i]);
 201         }
 202         if (cnt % 2)
 203                 rtsx_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i, 0xFF, 0xFF);
 204
 205         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
 206         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
 207         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
 208         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
 209                      0x01, PINGPONG_BUFFER);
 210
 211         rtsx_add_cmd(chip, WRITE_REG_CMD,
 212                      MS_TRANSFER, 0xFF, MS_TRANSFER_START | MS_TM_WRITE_BYTES);
 213         rtsx_add_cmd(chip, CHECK_REG_CMD,
 214                      MS_TRANSFER, MS_TRANSFER_END, MS_TRANSFER_END);
 215
 216         retval = rtsx_send_cmd(chip, MS_CARD, 5000);
 217         if (retval < 0) {
 218                 u8 val = 0;
 219
 220                 rtsx_read_register(chip, MS_TRANS_CFG, &val);
 221                 dev_dbg(rtsx_dev(chip), "MS_TRANS_CFG: 0x%02x\n", val);
 222
 223                 rtsx_clear_ms_error(chip);
 224
 225                 if (!(tpc & 0x08)) {
 226                         if (val & MS_CRC16_ERR) {
 227                                 ms_set_err_code(chip, MS_CRC16_ERROR);
 228                                 rtsx_trace(chip);
 229                                 return ms_parse_err_code(chip);
 230                         }
 231                 } else {
 232                         if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
 233                                 if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
 234                                         ms_set_err_code(chip, MS_CMD_NK);
 235                                         rtsx_trace(chip);
 236                                         return ms_parse_err_code(chip);
 237                                 }
 238                         }
 239                 }
 240
 241                 if (val & MS_RDY_TIMEOUT) {
 242                         ms_set_err_code(chip, MS_TO_ERROR);
 243                         rtsx_trace(chip);
 244                         return ms_parse_err_code(chip);
 245                 }
 246
 247                 ms_set_err_code(chip, MS_TO_ERROR);
 248                 rtsx_trace(chip);
 249                 return ms_parse_err_code(chip);
 250         }
 251
 252         return STATUS_SUCCESS;
 253 }
 254
 255 static int ms_read_bytes(struct rtsx_chip *chip,
 256                          u8 tpc, u8 cnt, u8 cfg, u8 *data, int data_len)
 257 {
 258         struct ms_info *ms_card = &chip->ms_card;
 259         int retval, i;
 260         u8 *ptr;
 261
 262         if (!data) {
 263                 rtsx_trace(chip);
 264                 return STATUS_ERROR;
 265         }
 266
 267         rtsx_init_cmd(chip);
 268
 269         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
 270         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
 271         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
 272         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
 273                      0x01, PINGPONG_BUFFER);
 274
 275         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
 276                      MS_TRANSFER_START | MS_TM_READ_BYTES);
 277         rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
 278                      MS_TRANSFER_END, MS_TRANSFER_END);
 279
 280         for (i = 0; i < data_len - 1; i++)
 281                 rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + i, 0, 0);
 282
 283         if (data_len % 2)
 284                 rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len, 0, 0);
 285         else
 286                 rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len - 1,
 287                              0, 0);
 288
 289         retval = rtsx_send_cmd(chip, MS_CARD, 5000);
 290         if (retval < 0) {
 291                 u8 val = 0;
 292
 293                 rtsx_read_register(chip, MS_TRANS_CFG, &val);
 294                 rtsx_clear_ms_error(chip);
 295
 296                 if (!(tpc & 0x08)) {
 297                         if (val & MS_CRC16_ERR) {
 298                                 ms_set_err_code(chip, MS_CRC16_ERROR);
 299                                 rtsx_trace(chip);
 300                                 return ms_parse_err_code(chip);
 301                         }
 302                 } else {
 303                         if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
 304                                 if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
 305                                         ms_set_err_code(chip, MS_CMD_NK);
 306                                         rtsx_trace(chip);
 307                                         return ms_parse_err_code(chip);
 308                                 }
 309                         }
 310                 }
 311
 312                 if (val & MS_RDY_TIMEOUT) {
 313                         ms_set_err_code(chip, MS_TO_ERROR);
 314                         rtsx_trace(chip);
 315                         return ms_parse_err_code(chip);
 316                 }
 317
 318                 ms_set_err_code(chip, MS_TO_ERROR);
 319                 rtsx_trace(chip);
 320                 return ms_parse_err_code(chip);
 321         }
 322
 323         ptr = rtsx_get_cmd_data(chip) + 1;
 324
 325         for (i = 0; i < data_len; i++)
 326                 data[i] = ptr[i];
 327
 328         if ((tpc == PRO_READ_SHORT_DATA) && (data_len == 8)) {
 329                 dev_dbg(rtsx_dev(chip), "Read format progress:\n");
 330                 print_hex_dump_bytes(KBUILD_MODNAME ": ", DUMP_PREFIX_NONE, ptr,
 331                                      cnt);
 332         }
 333
 334         return STATUS_SUCCESS;
 335 }
 336
 337 static int ms_set_rw_reg_addr(struct rtsx_chip *chip, u8 read_start,
 338                               u8 read_cnt, u8 write_start, u8 write_cnt)
 339 {
 340         int retval, i;
 341         u8 data[4];
 342
 343         data[0] = read_start;
 344         data[1] = read_cnt;
 345         data[2] = write_start;
 346         data[3] = write_cnt;
 347
 348         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 349                 retval = ms_write_bytes(chip, SET_RW_REG_ADRS, 4,
 350                                         NO_WAIT_INT, data, 4);
 351                 if (retval == STATUS_SUCCESS)
 352                         return STATUS_SUCCESS;
 353                 rtsx_clear_ms_error(chip);
 354         }
 355
 356         rtsx_trace(chip);
 357         return STATUS_FAIL;
 358 }
 359
 360 static int ms_send_cmd(struct rtsx_chip *chip, u8 cmd, u8 cfg)
 361 {
 362         u8 data[2];
 363
 364         data[0] = cmd;
 365         data[1] = 0;
 366
 367         return ms_write_bytes(chip, PRO_SET_CMD, 1, cfg, data, 1);
 368 }
 369
 370 static int ms_set_init_para(struct rtsx_chip *chip)
 371 {
 372         struct ms_info *ms_card = &chip->ms_card;
 373         int retval;
 374
 375         if (CHK_HG8BIT(ms_card)) {
 376                 if (chip->asic_code)
 377                         ms_card->ms_clock = chip->asic_ms_hg_clk;
 378                 else
 379                         ms_card->ms_clock = chip->fpga_ms_hg_clk;
 380
 381         } else if (CHK_MSPRO(ms_card) || CHK_MS4BIT(ms_card)) {
 382                 if (chip->asic_code)
 383                         ms_card->ms_clock = chip->asic_ms_4bit_clk;
 384                 else
 385                         ms_card->ms_clock = chip->fpga_ms_4bit_clk;
 386
 387         } else {
 388                 if (chip->asic_code)
 389                         ms_card->ms_clock = chip->asic_ms_1bit_clk;
 390                 else
 391                         ms_card->ms_clock = chip->fpga_ms_1bit_clk;
 392         }
 393
 394         retval = switch_clock(chip, ms_card->ms_clock);
 395         if (retval != STATUS_SUCCESS) {
 396                 rtsx_trace(chip);
 397                 return STATUS_FAIL;
 398         }
 399
 400         retval = select_card(chip, MS_CARD);
 401         if (retval != STATUS_SUCCESS) {
 402                 rtsx_trace(chip);
 403                 return STATUS_FAIL;
 404         }
 405
 406         return STATUS_SUCCESS;
 407 }
 408
 409 static int ms_switch_clock(struct rtsx_chip *chip)
 410 {
 411         struct ms_info *ms_card = &chip->ms_card;
 412         int retval;
 413
 414         retval = select_card(chip, MS_CARD);
 415         if (retval != STATUS_SUCCESS) {
 416                 rtsx_trace(chip);
 417                 return STATUS_FAIL;
 418         }
 419
 420         retval = switch_clock(chip, ms_card->ms_clock);
 421         if (retval != STATUS_SUCCESS) {
 422                 rtsx_trace(chip);
 423                 return STATUS_FAIL;
 424         }
 425
 426         return STATUS_SUCCESS;
 427 }
 428
 429 static int ms_pull_ctl_disable(struct rtsx_chip *chip)
 430 {
 431         int retval;
 432
 433         if (CHECK_PID(chip, 0x5208)) {
 434                 retval = rtsx_write_register(chip, CARD_PULL_CTL1, 0xFF,
 435                                              MS_D1_PD | MS_D2_PD | MS_CLK_PD |
 436                                              MS_D6_PD);
 437                 if (retval) {
 438                         rtsx_trace(chip);
 439                         return retval;
 440                 }
 441                 retval = rtsx_write_register(chip, CARD_PULL_CTL2, 0xFF,
 442                                              MS_D3_PD | MS_D0_PD | MS_BS_PD |
 443                                              XD_D4_PD);
 444                 if (retval) {
 445                         rtsx_trace(chip);
 446                         return retval;
 447                 }
 448                 retval = rtsx_write_register(chip, CARD_PULL_CTL3, 0xFF,
 449                                              MS_D7_PD | XD_CE_PD | XD_CLE_PD |
 450                                              XD_CD_PU);
 451                 if (retval) {
 452                         rtsx_trace(chip);
 453                         return retval;
 454                 }
 455                 retval = rtsx_write_register(chip, CARD_PULL_CTL4, 0xFF,
 456                                              XD_RDY_PD | SD_D3_PD | SD_D2_PD |
 457                                              XD_ALE_PD);
 458                 if (retval) {
 459                         rtsx_trace(chip);
 460                         return retval;
 461                 }
 462                 retval = rtsx_write_register(chip, CARD_PULL_CTL5, 0xFF,
 463                                              MS_INS_PU | SD_WP_PD | SD_CD_PU |
 464                                              SD_CMD_PD);
 465                 if (retval) {
 466                         rtsx_trace(chip);
 467                         return retval;
 468                 }
 469                 retval = rtsx_write_register(chip, CARD_PULL_CTL6, 0xFF,
 470                                              MS_D5_PD | MS_D4_PD);
 471                 if (retval) {
 472                         rtsx_trace(chip);
 473                         return retval;
 474                 }
 475         } else if (CHECK_PID(chip, 0x5288)) {
 476                 if (CHECK_BARO_PKG(chip, QFN)) {
 477                         retval = rtsx_write_register(chip, CARD_PULL_CTL1,
 478                                                      0xFF, 0x55);
 479                         if (retval) {
 480                                 rtsx_trace(chip);
 481                                 return retval;
 482                         }
 483                         retval = rtsx_write_register(chip, CARD_PULL_CTL2,
 484                                                      0xFF, 0x55);
 485                         if (retval) {
 486                                 rtsx_trace(chip);
 487                                 return retval;
 488                         }
 489                         retval = rtsx_write_register(chip, CARD_PULL_CTL3,
 490                                                      0xFF, 0x4B);
 491                         if (retval) {
 492                                 rtsx_trace(chip);
 493                                 return retval;
 494                         }
 495                         retval = rtsx_write_register(chip, CARD_PULL_CTL4,
 496                                                      0xFF, 0x69);
 497                         if (retval) {
 498                                 rtsx_trace(chip);
 499                                 return retval;
 500                         }
 501                 }
 502         }
 503
 504         return STATUS_SUCCESS;
 505 }
 506
 507 static int ms_pull_ctl_enable(struct rtsx_chip *chip)
 508 {
 509         int retval;
 510
 511         rtsx_init_cmd(chip);
 512
 513         if (CHECK_PID(chip, 0x5208)) {
 514                 rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF,
 515                              MS_D1_PD | MS_D2_PD | MS_CLK_NP | MS_D6_PD);
 516                 rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF,
 517                              MS_D3_PD | MS_D0_PD | MS_BS_NP | XD_D4_PD);
 518                 rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF,
 519                              MS_D7_PD | XD_CE_PD | XD_CLE_PD | XD_CD_PU);
 520                 rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF,
 521                              XD_RDY_PD | SD_D3_PD | SD_D2_PD | XD_ALE_PD);
 522                 rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF,
 523                              MS_INS_PU | SD_WP_PD | SD_CD_PU | SD_CMD_PD);
 524                 rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF,
 525                              MS_D5_PD | MS_D4_PD);
 526         } else if (CHECK_PID(chip, 0x5288)) {
 527                 if (CHECK_BARO_PKG(chip, QFN)) {
 528                         rtsx_add_cmd(chip, WRITE_REG_CMD,
 529                                      CARD_PULL_CTL1, 0xFF, 0x55);
 530                         rtsx_add_cmd(chip, WRITE_REG_CMD,
 531                                      CARD_PULL_CTL2, 0xFF, 0x45);
 532                         rtsx_add_cmd(chip, WRITE_REG_CMD,
 533                                      CARD_PULL_CTL3, 0xFF, 0x4B);
 534                         rtsx_add_cmd(chip, WRITE_REG_CMD,
 535                                      CARD_PULL_CTL4, 0xFF, 0x29);
 536                 }
 537         }
 538
 539         retval = rtsx_send_cmd(chip, MS_CARD, 100);
 540         if (retval < 0) {
 541                 rtsx_trace(chip);
 542                 return STATUS_FAIL;
 543         }
 544
 545         return STATUS_SUCCESS;
 546 }
 547
 548 static int ms_prepare_reset(struct rtsx_chip *chip)
 549 {
 550         struct ms_info *ms_card = &chip->ms_card;
 551         int retval;
 552         u8 oc_mask = 0;
 553
 554         ms_card->ms_type = 0;
 555         ms_card->check_ms_flow = 0;
 556         ms_card->switch_8bit_fail = 0;
 557         ms_card->delay_write.delay_write_flag = 0;
 558
 559         ms_card->pro_under_formatting = 0;
 560
 561         retval = ms_power_off_card3v3(chip);
 562         if (retval != STATUS_SUCCESS) {
 563                 rtsx_trace(chip);
 564                 return STATUS_FAIL;
 565         }
 566
 567         if (!chip->ft2_fast_mode)
 568                 wait_timeout(250);
 569
 570         retval = enable_card_clock(chip, MS_CARD);
 571         if (retval != STATUS_SUCCESS) {
 572                 rtsx_trace(chip);
 573                 return STATUS_FAIL;
 574         }
 575
 576         if (chip->asic_code) {
 577                 retval = ms_pull_ctl_enable(chip);
 578                 if (retval != STATUS_SUCCESS) {
 579                         rtsx_trace(chip);
 580                         return STATUS_FAIL;
 581                 }
 582         } else {
 583                 retval = rtsx_write_register(chip, FPGA_PULL_CTL,
 584                                              FPGA_MS_PULL_CTL_BIT | 0x20, 0);
 585                 if (retval) {
 586                         rtsx_trace(chip);
 587                         return retval;
 588                 }
 589         }
 590
 591         if (!chip->ft2_fast_mode) {
 592                 retval = card_power_on(chip, MS_CARD);
 593                 if (retval != STATUS_SUCCESS) {
 594                         rtsx_trace(chip);
 595                         return STATUS_FAIL;
 596                 }
 597
 598                 wait_timeout(150);
 599
 600 #ifdef SUPPORT_OCP
 601                 if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
 602                         oc_mask = MS_OC_NOW | MS_OC_EVER;
 603                 else
 604                         oc_mask = SD_OC_NOW | SD_OC_EVER;
 605
 606                 if (chip->ocp_stat & oc_mask) {
 607                         dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
 608                                 chip->ocp_stat);
 609                         rtsx_trace(chip);
 610                         return STATUS_FAIL;
 611                 }
 612 #endif
 613         }
 614
 615         retval = rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN,
 616                                      MS_OUTPUT_EN);
 617         if (retval) {
 618                 rtsx_trace(chip);
 619                 return retval;
 620         }
 621
 622         if (chip->asic_code) {
 623                 retval = rtsx_write_register(chip, MS_CFG, 0xFF,
 624                                              SAMPLE_TIME_RISING |
 625                                              PUSH_TIME_DEFAULT |
 626                                              NO_EXTEND_TOGGLE |
 627                                              MS_BUS_WIDTH_1);
 628                 if (retval) {
 629                         rtsx_trace(chip);
 630                         return retval;
 631                 }
 632         } else {
 633                 retval = rtsx_write_register(chip, MS_CFG, 0xFF,
 634                                              SAMPLE_TIME_FALLING |
 635                                              PUSH_TIME_DEFAULT |
 636                                              NO_EXTEND_TOGGLE |
 637                                              MS_BUS_WIDTH_1);
 638                 if (retval) {
 639                         rtsx_trace(chip);
 640                         return retval;
 641                 }
 642         }
 643         retval = rtsx_write_register(chip, MS_TRANS_CFG, 0xFF,
 644                                      NO_WAIT_INT | NO_AUTO_READ_INT_REG);
 645         if (retval) {
 646                 rtsx_trace(chip);
 647                 return retval;
 648         }
 649         retval = rtsx_write_register(chip, CARD_STOP, MS_STOP | MS_CLR_ERR,
 650                                      MS_STOP | MS_CLR_ERR);
 651         if (retval) {
 652                 rtsx_trace(chip);
 653                 return retval;
 654         }
 655
 656         retval = ms_set_init_para(chip);
 657         if (retval != STATUS_SUCCESS) {
 658                 rtsx_trace(chip);
 659                 return STATUS_FAIL;
 660         }
 661
 662         return STATUS_SUCCESS;
 663 }
 664
 665 static int ms_identify_media_type(struct rtsx_chip *chip, int switch_8bit_bus)
 666 {
 667         struct ms_info *ms_card = &chip->ms_card;
 668         int retval, i;
 669         u8 val;
 670
 671         retval = ms_set_rw_reg_addr(chip, Pro_StatusReg, 6, SystemParm, 1);
 672         if (retval != STATUS_SUCCESS) {
 673                 rtsx_trace(chip);
 674                 return STATUS_FAIL;
 675         }
 676
 677         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 678                 retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, READ_REG,
 679                                          6, NO_WAIT_INT);
 680                 if (retval == STATUS_SUCCESS)
 681                         break;
 682         }
 683         if (i == MS_MAX_RETRY_COUNT) {
 684                 rtsx_trace(chip);
 685                 return STATUS_FAIL;
 686         }
 687
 688         retval = rtsx_read_register(chip, PPBUF_BASE2 + 2, &val);
 689         if (retval) {
 690                 rtsx_trace(chip);
 691                 return retval;
 692         }
 693         dev_dbg(rtsx_dev(chip), "Type register: 0x%x\n", val);
 694         if (val != 0x01) {
 695                 if (val != 0x02)
 696                         ms_card->check_ms_flow = 1;
 697
 698                 rtsx_trace(chip);
 699                 return STATUS_FAIL;
 700         }
 701
 702         retval = rtsx_read_register(chip, PPBUF_BASE2 + 4, &val);
 703         if (retval) {
 704                 rtsx_trace(chip);
 705                 return retval;
 706         }
 707         dev_dbg(rtsx_dev(chip), "Category register: 0x%x\n", val);
 708         if (val != 0) {
 709                 ms_card->check_ms_flow = 1;
 710                 rtsx_trace(chip);
 711                 return STATUS_FAIL;
 712         }
 713
 714         retval = rtsx_read_register(chip, PPBUF_BASE2 + 5, &val);
 715         if (retval) {
 716                 rtsx_trace(chip);
 717                 return retval;
 718         }
 719         dev_dbg(rtsx_dev(chip), "Class register: 0x%x\n", val);
 720         if (val == 0) {
 721                 retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
 722                 if (retval) {
 723                         rtsx_trace(chip);
 724                         return retval;
 725                 }
 726                 if (val & WRT_PRTCT)
 727                         chip->card_wp |= MS_CARD;
 728                 else
 729                         chip->card_wp &= ~MS_CARD;
 730
 731         } else if ((val == 0x01) || (val == 0x02) || (val == 0x03)) {
 732                 chip->card_wp |= MS_CARD;
 733         } else {
 734                 ms_card->check_ms_flow = 1;
 735                 rtsx_trace(chip);
 736                 return STATUS_FAIL;
 737         }
 738
 739         ms_card->ms_type |= TYPE_MSPRO;
 740
 741         retval = rtsx_read_register(chip, PPBUF_BASE2 + 3, &val);
 742         if (retval) {
 743                 rtsx_trace(chip);
 744                 return retval;
 745         }
 746         dev_dbg(rtsx_dev(chip), "IF Mode register: 0x%x\n", val);
 747         if (val == 0) {
 748                 ms_card->ms_type &= 0x0F;
 749         } else if (val == 7) {
 750                 if (switch_8bit_bus)
 751                         ms_card->ms_type |= MS_HG;
 752                 else
 753                         ms_card->ms_type &= 0x0F;
 754
 755         } else {
 756                 rtsx_trace(chip);
 757                 return STATUS_FAIL;
 758         }
 759
 760         return STATUS_SUCCESS;
 761 }
 762
 763 static int ms_confirm_cpu_startup(struct rtsx_chip *chip)
 764 {
 765         int retval, i, k;
 766         u8 val;
 767
 768         /* Confirm CPU StartUp */
 769         k = 0;
 770         do {
 771                 if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 772                         ms_set_err_code(chip, MS_NO_CARD);
 773                         rtsx_trace(chip);
 774                         return STATUS_FAIL;
 775                 }
 776
 777                 for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 778                         retval = ms_read_bytes(chip, GET_INT, 1,
 779                                                NO_WAIT_INT, &val, 1);
 780                         if (retval == STATUS_SUCCESS)
 781                                 break;
 782                 }
 783                 if (i == MS_MAX_RETRY_COUNT) {
 784                         rtsx_trace(chip);
 785                         return STATUS_FAIL;
 786                 }
 787
 788                 if (k > 100) {
 789                         rtsx_trace(chip);
 790                         return STATUS_FAIL;
 791                 }
 792
 793                 k++;
 794                 wait_timeout(100);
 795         } while (!(val & INT_REG_CED));
 796
 797         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 798                 retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
 799                 if (retval == STATUS_SUCCESS)
 800                         break;
 801         }
 802         if (i == MS_MAX_RETRY_COUNT) {
 803                 rtsx_trace(chip);
 804                 return STATUS_FAIL;
 805         }
 806
 807         if (val & INT_REG_ERR) {
 808                 if (val & INT_REG_CMDNK) {
 809                         chip->card_wp |= (MS_CARD);
 810                 } else {
 811                         rtsx_trace(chip);
 812                         return STATUS_FAIL;
 813                 }
 814         }
 815         /* --  end confirm CPU startup */
 816
 817         return STATUS_SUCCESS;
 818 }
 819
 820 static int ms_switch_parallel_bus(struct rtsx_chip *chip)
 821 {
 822         int retval, i;
 823         u8 data[2];
 824
 825         data[0] = PARALLEL_4BIT_IF;
 826         data[1] = 0;
 827         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 828                 retval = ms_write_bytes(chip, WRITE_REG, 1, NO_WAIT_INT,
 829                                         data, 2);
 830                 if (retval == STATUS_SUCCESS)
 831                         break;
 832         }
 833         if (retval != STATUS_SUCCESS) {
 834                 rtsx_trace(chip);
 835                 return STATUS_FAIL;
 836         }
 837
 838         return STATUS_SUCCESS;
 839 }
 840
 841 static int ms_switch_8bit_bus(struct rtsx_chip *chip)
 842 {
 843         struct ms_info *ms_card = &chip->ms_card;
 844         int retval, i;
 845         u8 data[2];
 846
 847         data[0] = PARALLEL_8BIT_IF;
 848         data[1] = 0;
 849         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 850                 retval = ms_write_bytes(chip, WRITE_REG, 1,
 851                                         NO_WAIT_INT, data, 2);
 852                 if (retval == STATUS_SUCCESS)
 853                         break;
 854         }
 855         if (retval != STATUS_SUCCESS) {
 856                 rtsx_trace(chip);
 857                 return STATUS_FAIL;
 858         }
 859
 860         retval = rtsx_write_register(chip, MS_CFG, 0x98,
 861                                      MS_BUS_WIDTH_8 | SAMPLE_TIME_FALLING);
 862         if (retval) {
 863                 rtsx_trace(chip);
 864                 return retval;
 865         }
 866         ms_card->ms_type |= MS_8BIT;
 867         retval = ms_set_init_para(chip);
 868         if (retval != STATUS_SUCCESS) {
 869                 rtsx_trace(chip);
 870                 return STATUS_FAIL;
 871         }
 872
 873         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
 874                 retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT,
 875                                          1, NO_WAIT_INT);
 876                 if (retval != STATUS_SUCCESS) {
 877                         rtsx_trace(chip);
 878                         return STATUS_FAIL;
 879                 }
 880         }
 881
 882         return STATUS_SUCCESS;
 883 }
 884
 885 static int ms_pro_reset_flow(struct rtsx_chip *chip, int switch_8bit_bus)
 886 {
 887         struct ms_info *ms_card = &chip->ms_card;
 888         int retval, i;
 889
 890         for (i = 0; i < 3; i++) {
 891                 retval = ms_prepare_reset(chip);
 892                 if (retval != STATUS_SUCCESS) {
 893                         rtsx_trace(chip);
 894                         return STATUS_FAIL;
 895                 }
 896
 897                 retval = ms_identify_media_type(chip, switch_8bit_bus);
 898                 if (retval != STATUS_SUCCESS) {
 899                         rtsx_trace(chip);
 900                         return STATUS_FAIL;
 901                 }
 902
 903                 retval = ms_confirm_cpu_startup(chip);
 904                 if (retval != STATUS_SUCCESS) {
 905                         rtsx_trace(chip);
 906                         return STATUS_FAIL;
 907                 }
 908
 909                 retval = ms_switch_parallel_bus(chip);
 910                 if (retval != STATUS_SUCCESS) {
 911                         if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
 912                                 ms_set_err_code(chip, MS_NO_CARD);
 913                                 rtsx_trace(chip);
 914                                 return STATUS_FAIL;
 915                         }
 916                         continue;
 917                 } else {
 918                         break;
 919                 }
 920         }
 921
 922         if (retval != STATUS_SUCCESS) {
 923                 rtsx_trace(chip);
 924                 return STATUS_FAIL;
 925         }
 926
 927         /* Switch MS-PRO into Parallel mode */
 928         retval = rtsx_write_register(chip, MS_CFG, 0x18, MS_BUS_WIDTH_4);
 929         if (retval) {
 930                 rtsx_trace(chip);
 931                 return retval;
 932         }
 933         retval = rtsx_write_register(chip, MS_CFG, PUSH_TIME_ODD,
 934                                      PUSH_TIME_ODD);
 935         if (retval) {
 936                 rtsx_trace(chip);
 937                 return retval;
 938         }
 939
 940         retval = ms_set_init_para(chip);
 941         if (retval != STATUS_SUCCESS) {
 942                 rtsx_trace(chip);
 943                 return STATUS_FAIL;
 944         }
 945
 946         /* If MSPro HG Card, We shall try to switch to 8-bit bus */
 947         if (CHK_MSHG(ms_card) && chip->support_ms_8bit && switch_8bit_bus) {
 948                 retval = ms_switch_8bit_bus(chip);
 949                 if (retval != STATUS_SUCCESS) {
 950                         ms_card->switch_8bit_fail = 1;
 951                         rtsx_trace(chip);
 952                         return STATUS_FAIL;
 953                 }
 954         }
 955
 956         return STATUS_SUCCESS;
 957 }
 958
 959 #ifdef XC_POWERCLASS
 960 static int msxc_change_power(struct rtsx_chip *chip, u8 mode)
 961 {
 962         int retval;
 963         u8 buf[6];
 964
 965         ms_cleanup_work(chip);
 966
 967         retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_DataCount1, 6);
 968         if (retval != STATUS_SUCCESS) {
 969                 rtsx_trace(chip);
 970                 return STATUS_FAIL;
 971         }
 972
 973         buf[0] = 0;
 974         buf[1] = mode;
 975         buf[2] = 0;
 976         buf[3] = 0;
 977         buf[4] = 0;
 978         buf[5] = 0;
 979
 980         retval = ms_write_bytes(chip, PRO_WRITE_REG, 6, NO_WAIT_INT, buf, 6);
 981         if (retval != STATUS_SUCCESS) {
 982                 rtsx_trace(chip);
 983                 return STATUS_FAIL;
 984         }
 985
 986         retval = ms_send_cmd(chip, XC_CHG_POWER, WAIT_INT);
 987         if (retval != STATUS_SUCCESS) {
 988                 rtsx_trace(chip);
 989                 return STATUS_FAIL;
 990         }
 991
 992         retval = rtsx_read_register(chip, MS_TRANS_CFG, buf);
 993         if (retval) {
 994                 rtsx_trace(chip);
 995                 return retval;
 996         }
 997         if (buf[0] & (MS_INT_CMDNK | MS_INT_ERR)) {
 998                 rtsx_trace(chip);
 999                 return STATUS_FAIL;
1000         }
1001
1002         return STATUS_SUCCESS;
1003 }
1004 #endif
1005
1006 static int ms_read_attribute_info(struct rtsx_chip *chip)
1007 {
1008         struct ms_info *ms_card = &chip->ms_card;
1009         int retval, i;
1010         u8 val, *buf, class_code, device_type, sub_class, data[16];
1011         u16 total_blk = 0, blk_size = 0;
1012 #ifdef SUPPORT_MSXC
1013         u32 xc_total_blk = 0, xc_blk_size = 0;
1014 #endif
1015         u32 sys_info_addr = 0, sys_info_size;
1016 #ifdef SUPPORT_PCGL_1P18
1017         u32 model_name_addr = 0, model_name_size;
1018         int found_sys_info = 0, found_model_name = 0;
1019 #endif
1020
1021         retval = ms_set_rw_reg_addr(chip, Pro_IntReg, 2, Pro_SystemParm, 7);
1022         if (retval != STATUS_SUCCESS) {
1023                 rtsx_trace(chip);
1024                 return STATUS_FAIL;
1025         }
1026
1027         if (CHK_MS8BIT(ms_card))
1028                 data[0] = PARALLEL_8BIT_IF;
1029         else
1030                 data[0] = PARALLEL_4BIT_IF;
1031
1032         data[1] = 0;
1033
1034         data[2] = 0x40;
1035         data[3] = 0;
1036         data[4] = 0;
1037         data[5] = 0;
1038         data[6] = 0;
1039         data[7] = 0;
1040
1041         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
1042                 retval = ms_write_bytes(chip, PRO_WRITE_REG, 7, NO_WAIT_INT,
1043                                         data, 8);
1044                 if (retval == STATUS_SUCCESS)
1045                         break;
1046         }
1047         if (retval != STATUS_SUCCESS) {
1048                 rtsx_trace(chip);
1049                 return STATUS_FAIL;
1050         }
1051
1052         buf = kmalloc(64 * 512, GFP_KERNEL);
1053         if (!buf) {
1054                 rtsx_trace(chip);
1055                 return STATUS_ERROR;
1056         }
1057
1058         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
1059                 retval = ms_send_cmd(chip, PRO_READ_ATRB, WAIT_INT);
1060                 if (retval != STATUS_SUCCESS)
1061                         continue;
1062
1063                 retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
1064                 if (retval != STATUS_SUCCESS) {
1065                         kfree(buf);
1066                         rtsx_trace(chip);
1067                         return STATUS_FAIL;
1068                 }
1069                 if (!(val & MS_INT_BREQ)) {
1070                         kfree(buf);
1071                         rtsx_trace(chip);
1072                         return STATUS_FAIL;
1073                 }
1074                 retval = ms_transfer_data(chip, MS_TM_AUTO_READ,
1075                                           PRO_READ_LONG_DATA, 0x40, WAIT_INT,
1076                                           0, 0, buf, 64 * 512);
1077                 if (retval == STATUS_SUCCESS)
1078                         break;
1079
1080                 rtsx_clear_ms_error(chip);
1081         }
1082         if (retval != STATUS_SUCCESS) {
1083                 kfree(buf);
1084                 rtsx_trace(chip);
1085                 return STATUS_FAIL;
1086         }
1087
1088         i = 0;
1089         do {
1090                 retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
1091                 if (retval != STATUS_SUCCESS) {
1092                         kfree(buf);
1093                         rtsx_trace(chip);
1094                         return STATUS_FAIL;
1095                 }
1096
1097                 if ((val & MS_INT_CED) || !(val & MS_INT_BREQ))
1098                         break;
1099
1100                 retval = ms_transfer_tpc(chip, MS_TM_NORMAL_READ,
1101                                          PRO_READ_LONG_DATA, 0, WAIT_INT);
1102                 if (retval != STATUS_SUCCESS) {
1103                         kfree(buf);
1104                         rtsx_trace(chip);
1105                         return STATUS_FAIL;
1106                 }
1107
1108                 i++;
1109         } while (i < 1024);
1110
1111         if ((buf[0] != 0xa5) && (buf[1] != 0xc3)) {
1112                 /* Signature code is wrong */
1113                 kfree(buf);
1114                 rtsx_trace(chip);
1115                 return STATUS_FAIL;
1116         }
1117
1118         if ((buf[4] < 1) || (buf[4] > 12)) {
1119                 kfree(buf);
1120                 rtsx_trace(chip);
1121                 return STATUS_FAIL;
1122         }
1123
1124         for (i = 0; i < buf[4]; i++) {
1125                 int cur_addr_off = 16 + i * 12;
1126
1127 #ifdef SUPPORT_MSXC
1128                 if ((buf[cur_addr_off + 8] == 0x10) ||
1129                     (buf[cur_addr_off + 8] == 0x13)) {
1130 #else
1131                 if (buf[cur_addr_off + 8] == 0x10) {
1132 #endif
1133                         sys_info_addr = ((u32)buf[cur_addr_off + 0] << 24) |
1134                                 ((u32)buf[cur_addr_off + 1] << 16) |
1135                                 ((u32)buf[cur_addr_off + 2] << 8) |
1136                                 buf[cur_addr_off + 3];
1137                         sys_info_size = ((u32)buf[cur_addr_off + 4] << 24) |
1138                                 ((u32)buf[cur_addr_off + 5] << 16) |
1139                                 ((u32)buf[cur_addr_off + 6] << 8) |
1140                                 buf[cur_addr_off + 7];
1141                         dev_dbg(rtsx_dev(chip), "sys_info_addr = 0x%x, sys_info_size = 0x%x\n",
1142                                 sys_info_addr, sys_info_size);
1143                         if (sys_info_size != 96)  {
1144                                 kfree(buf);
1145                                 rtsx_trace(chip);
1146                                 return STATUS_FAIL;
1147                         }
1148                         if (sys_info_addr < 0x1A0) {
1149                                 kfree(buf);
1150                                 rtsx_trace(chip);
1151                                 return STATUS_FAIL;
1152                         }
1153                         if ((sys_info_size + sys_info_addr) > 0x8000) {
1154                                 kfree(buf);
1155                                 rtsx_trace(chip);
1156                                 return STATUS_FAIL;
1157                         }
1158
1159 #ifdef SUPPORT_MSXC
1160                         if (buf[cur_addr_off + 8] == 0x13)
1161                                 ms_card->ms_type |= MS_XC;
1162 #endif
1163 #ifdef SUPPORT_PCGL_1P18
1164                         found_sys_info = 1;
1165 #else
1166                         break;
1167 #endif
1168                 }
1169 #ifdef SUPPORT_PCGL_1P18
1170                 if (buf[cur_addr_off + 8] == 0x15) {
1171                         model_name_addr = ((u32)buf[cur_addr_off + 0] << 24) |
1172                                 ((u32)buf[cur_addr_off + 1] << 16) |
1173                                 ((u32)buf[cur_addr_off + 2] << 8) |
1174                                 buf[cur_addr_off + 3];
1175                         model_name_size = ((u32)buf[cur_addr_off + 4] << 24) |
1176                                 ((u32)buf[cur_addr_off + 5] << 16) |
1177                                 ((u32)buf[cur_addr_off + 6] << 8) |
1178                                 buf[cur_addr_off + 7];
1179                         dev_dbg(rtsx_dev(chip), "model_name_addr = 0x%x, model_name_size = 0x%x\n",
1180                                 model_name_addr, model_name_size);
1181                         if (model_name_size != 48)  {
1182                                 kfree(buf);
1183                                 rtsx_trace(chip);
1184                                 return STATUS_FAIL;
1185                         }
1186                         if (model_name_addr < 0x1A0) {
1187                                 kfree(buf);
1188                                 rtsx_trace(chip);
1189                                 return STATUS_FAIL;
1190                         }
1191                         if ((model_name_size + model_name_addr) > 0x8000) {
1192                                 kfree(buf);
1193                                 rtsx_trace(chip);
1194                                 return STATUS_FAIL;
1195                         }
1196
1197                         found_model_name = 1;
1198                 }
1199
1200                 if (found_sys_info && found_model_name)
1201                         break;
1202 #endif
1203         }
1204
1205         if (i == buf[4]) {
1206                 kfree(buf);
1207                 rtsx_trace(chip);
1208                 return STATUS_FAIL;
1209         }
1210
1211         class_code =  buf[sys_info_addr + 0];
1212         device_type = buf[sys_info_addr + 56];
1213         sub_class = buf[sys_info_addr + 46];
1214 #ifdef SUPPORT_MSXC
1215         if (CHK_MSXC(ms_card)) {
1216                 xc_total_blk = ((u32)buf[sys_info_addr + 6] << 24) |
1217                                 ((u32)buf[sys_info_addr + 7] << 16) |
1218                                 ((u32)buf[sys_info_addr + 8] << 8) |
1219                                 buf[sys_info_addr + 9];
1220                 xc_blk_size = ((u32)buf[sys_info_addr + 32] << 24) |
1221                                 ((u32)buf[sys_info_addr + 33] << 16) |
1222                                 ((u32)buf[sys_info_addr + 34] << 8) |
1223                                 buf[sys_info_addr + 35];
1224                 dev_dbg(rtsx_dev(chip), "xc_total_blk = 0x%x, xc_blk_size = 0x%x\n",
1225                         xc_total_blk, xc_blk_size);
1226         } else {
1227                 total_blk = ((u16)buf[sys_info_addr + 6] << 8) |
1228                         buf[sys_info_addr + 7];
1229                 blk_size = ((u16)buf[sys_info_addr + 2] << 8) |
1230                         buf[sys_info_addr + 3];
1231                 dev_dbg(rtsx_dev(chip), "total_blk = 0x%x, blk_size = 0x%x\n",
1232                         total_blk, blk_size);
1233         }
1234 #else
1235         total_blk = ((u16)buf[sys_info_addr + 6] << 8) | buf[sys_info_addr + 7];
1236         blk_size = ((u16)buf[sys_info_addr + 2] << 8) | buf[sys_info_addr + 3];
1237         dev_dbg(rtsx_dev(chip), "total_blk = 0x%x, blk_size = 0x%x\n",
1238                 total_blk, blk_size);
1239 #endif
1240
1241         dev_dbg(rtsx_dev(chip), "class_code = 0x%x, device_type = 0x%x, sub_class = 0x%x\n",
1242                 class_code, device_type, sub_class);
1243
1244         memcpy(ms_card->raw_sys_info, buf + sys_info_addr, 96);
1245 #ifdef SUPPORT_PCGL_1P18
1246         memcpy(ms_card->raw_model_name, buf + model_name_addr, 48);
1247 #endif
1248
1249         kfree(buf);
1250
1251 #ifdef SUPPORT_MSXC
1252         if (CHK_MSXC(ms_card)) {
1253                 if (class_code != 0x03) {
1254                         rtsx_trace(chip);
1255                         return STATUS_FAIL;
1256                 }
1257         } else {
1258                 if (class_code != 0x02) {
1259                         rtsx_trace(chip);
1260                         return STATUS_FAIL;
1261                 }
1262         }
1263 #else
1264         if (class_code != 0x02) {
1265                 rtsx_trace(chip);
1266                 return STATUS_FAIL;
1267         }
1268 #endif
1269
1270         if (device_type != 0x00) {
1271                 if ((device_type == 0x01) || (device_type == 0x02) ||
1272                     (device_type == 0x03)) {
1273                         chip->card_wp |= MS_CARD;
1274                 } else {
1275                         rtsx_trace(chip);
1276                         return STATUS_FAIL;
1277                 }
1278         }
1279
1280         if (sub_class & 0xC0) {
1281                 rtsx_trace(chip);
1282                 return STATUS_FAIL;
1283         }
1284
1285         dev_dbg(rtsx_dev(chip), "class_code: 0x%x, device_type: 0x%x, sub_class: 0x%x\n",
1286                 class_code, device_type, sub_class);
1287
1288 #ifdef SUPPORT_MSXC
1289         if (CHK_MSXC(ms_card)) {
1290                 chip->capacity[chip->card2lun[MS_CARD]] =
1291                         ms_card->capacity = xc_total_blk * xc_blk_size;
1292         } else {
1293                 chip->capacity[chip->card2lun[MS_CARD]] =
1294                         ms_card->capacity = total_blk * blk_size;
1295         }
1296 #else
1297         ms_card->capacity = total_blk * blk_size;
1298         chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity;
1299 #endif
1300
1301         return STATUS_SUCCESS;
1302 }
1303
1304 #ifdef SUPPORT_MAGIC_GATE
1305 static int mg_set_tpc_para_sub(struct rtsx_chip *chip,
1306                                int type, u8 mg_entry_num);
1307 #endif
1308
1309 static int reset_ms_pro(struct rtsx_chip *chip)
1310 {
1311         struct ms_info *ms_card = &chip->ms_card;
1312         int retval;
1313 #ifdef XC_POWERCLASS
1314         u8 change_power_class;
1315
1316         if (chip->ms_power_class_en & 0x02)
1317                 change_power_class = 2;
1318         else if (chip->ms_power_class_en & 0x01)
1319                 change_power_class = 1;
1320         else
1321                 change_power_class = 0;
1322 #endif
1323
1324 #ifdef XC_POWERCLASS
1325 retry:
1326 #endif
1327         retval = ms_pro_reset_flow(chip, 1);
1328         if (retval != STATUS_SUCCESS) {
1329                 if (ms_card->switch_8bit_fail) {
1330                         retval = ms_pro_reset_flow(chip, 0);
1331                         if (retval != STATUS_SUCCESS) {
1332                                 rtsx_trace(chip);
1333                                 return STATUS_FAIL;
1334                         }
1335                 } else {
1336                         rtsx_trace(chip);
1337                         return STATUS_FAIL;
1338                 }
1339         }
1340
1341         retval = ms_read_attribute_info(chip);
1342         if (retval != STATUS_SUCCESS) {
1343                 rtsx_trace(chip);
1344                 return STATUS_FAIL;
1345         }
1346
1347 #ifdef XC_POWERCLASS
1348         if (CHK_HG8BIT(ms_card))
1349                 change_power_class = 0;
1350
1351         if (change_power_class && CHK_MSXC(ms_card)) {
1352                 u8 power_class_en = chip->ms_power_class_en;
1353
1354                 dev_dbg(rtsx_dev(chip), "power_class_en = 0x%x\n",
1355                         power_class_en);
1356                 dev_dbg(rtsx_dev(chip), "change_power_class = %d\n",
1357                         change_power_class);
1358
1359                 if (change_power_class)
1360                         power_class_en &= (1 << (change_power_class - 1));
1361                 else
1362                         power_class_en = 0;
1363
1364                 if (power_class_en) {
1365                         u8 power_class_mode =
1366                                 (ms_card->raw_sys_info[46] & 0x18) >> 3;
1367                         dev_dbg(rtsx_dev(chip), "power_class_mode = 0x%x",
1368                                 power_class_mode);
1369                         if (change_power_class > power_class_mode)
1370                                 change_power_class = power_class_mode;
1371                         if (change_power_class) {
1372                                 retval = msxc_change_power(chip,
1373                                                            change_power_class);
1374                                 if (retval != STATUS_SUCCESS) {
1375                                         change_power_class--;
1376                                         goto retry;
1377                                 }
1378                         }
1379                 }
1380         }
1381 #endif
1382
1383 #ifdef SUPPORT_MAGIC_GATE
1384         retval = mg_set_tpc_para_sub(chip, 0, 0);
1385         if (retval != STATUS_SUCCESS) {
1386                 rtsx_trace(chip);
1387                 return STATUS_FAIL;
1388         }
1389 #endif
1390
1391         if (CHK_HG8BIT(ms_card))
1392                 chip->card_bus_width[chip->card2lun[MS_CARD]] = 8;
1393         else
1394                 chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
1395
1396         return STATUS_SUCCESS;
1397 }
1398
1399 static int ms_read_status_reg(struct rtsx_chip *chip)
1400 {
1401         int retval;
1402         u8 val[2];
1403
1404         retval = ms_set_rw_reg_addr(chip, StatusReg0, 2, 0, 0);
1405         if (retval != STATUS_SUCCESS) {
1406                 rtsx_trace(chip);
1407                 return STATUS_FAIL;
1408         }
1409
1410         retval = ms_read_bytes(chip, READ_REG, 2, NO_WAIT_INT, val, 2);
1411         if (retval != STATUS_SUCCESS) {
1412                 rtsx_trace(chip);
1413                 return STATUS_FAIL;
1414         }
1415
1416         if (val[1] & (STS_UCDT | STS_UCEX | STS_UCFG)) {
1417                 ms_set_err_code(chip, MS_FLASH_READ_ERROR);
1418                 rtsx_trace(chip);
1419                 return STATUS_FAIL;
1420         }
1421
1422         return STATUS_SUCCESS;
1423 }
1424
1425 static int ms_read_extra_data(struct rtsx_chip *chip,
1426                               u16 block_addr, u8 page_num, u8 *buf, int buf_len)
1427 {
1428         struct ms_info *ms_card = &chip->ms_card;
1429         int retval, i;
1430         u8 val, data[10];
1431
1432         retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
1433                                     SystemParm, 6);
1434         if (retval != STATUS_SUCCESS) {
1435                 rtsx_trace(chip);
1436                 return STATUS_FAIL;
1437         }
1438
1439         if (CHK_MS4BIT(ms_card)) {
1440                 /* Parallel interface */
1441                 data[0] = 0x88;
1442         } else {
1443                 /* Serial interface */
1444                 data[0] = 0x80;
1445         }
1446         data[1] = 0;
1447         data[2] = (u8)(block_addr >> 8);
1448         data[3] = (u8)block_addr;
1449         data[4] = 0x40;
1450         data[5] = page_num;
1451
1452         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
1453                 retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT,
1454                                         data, 6);
1455                 if (retval == STATUS_SUCCESS)
1456                         break;
1457         }
1458         if (i == MS_MAX_RETRY_COUNT) {
1459                 rtsx_trace(chip);
1460                 return STATUS_FAIL;
1461         }
1462
1463         ms_set_err_code(chip, MS_NO_ERROR);
1464
1465         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
1466                 retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
1467                 if (retval == STATUS_SUCCESS)
1468                         break;
1469         }
1470         if (i == MS_MAX_RETRY_COUNT) {
1471                 rtsx_trace(chip);
1472                 return STATUS_FAIL;
1473         }
1474
1475         ms_set_err_code(chip, MS_NO_ERROR);
1476         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1477         if (retval != STATUS_SUCCESS) {
1478                 rtsx_trace(chip);
1479                 return STATUS_FAIL;
1480         }
1481
1482         if (val & INT_REG_CMDNK) {
1483                 ms_set_err_code(chip, MS_CMD_NK);
1484                 rtsx_trace(chip);
1485                 return STATUS_FAIL;
1486         }
1487         if (val & INT_REG_CED) {
1488                 if (val & INT_REG_ERR) {
1489                         retval = ms_read_status_reg(chip);
1490                         if (retval != STATUS_SUCCESS) {
1491                                 rtsx_trace(chip);
1492                                 return STATUS_FAIL;
1493                         }
1494
1495                         retval = ms_set_rw_reg_addr(chip, OverwriteFlag,
1496                                                     MS_EXTRA_SIZE, SystemParm,
1497                                                     6);
1498                         if (retval != STATUS_SUCCESS) {
1499                                 rtsx_trace(chip);
1500                                 return STATUS_FAIL;
1501                         }
1502                 }
1503         }
1504
1505         retval = ms_read_bytes(chip, READ_REG, MS_EXTRA_SIZE, NO_WAIT_INT,
1506                                data, MS_EXTRA_SIZE);
1507         if (retval != STATUS_SUCCESS) {
1508                 rtsx_trace(chip);
1509                 return STATUS_FAIL;
1510         }
1511
1512         if (buf && buf_len) {
1513                 if (buf_len > MS_EXTRA_SIZE)
1514                         buf_len = MS_EXTRA_SIZE;
1515                 memcpy(buf, data, buf_len);
1516         }
1517
1518         return STATUS_SUCCESS;
1519 }
1520
1521 static int ms_write_extra_data(struct rtsx_chip *chip, u16 block_addr,
1522                                u8 page_num, u8 *buf, int buf_len)
1523 {
1524         struct ms_info *ms_card = &chip->ms_card;
1525         int retval, i;
1526         u8 val, data[16];
1527
1528         if (!buf || (buf_len < MS_EXTRA_SIZE)) {
1529                 rtsx_trace(chip);
1530                 return STATUS_FAIL;
1531         }
1532
1533         retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
1534                                     SystemParm, 6 + MS_EXTRA_SIZE);
1535         if (retval != STATUS_SUCCESS) {
1536                 rtsx_trace(chip);
1537                 return STATUS_FAIL;
1538         }
1539
1540         if (CHK_MS4BIT(ms_card))
1541                 data[0] = 0x88;
1542         else
1543                 data[0] = 0x80;
1544
1545         data[1] = 0;
1546         data[2] = (u8)(block_addr >> 8);
1547         data[3] = (u8)block_addr;
1548         data[4] = 0x40;
1549         data[5] = page_num;
1550
1551         for (i = 6; i < MS_EXTRA_SIZE + 6; i++)
1552                 data[i] = buf[i - 6];
1553
1554         retval = ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE),
1555                                 NO_WAIT_INT, data, 16);
1556         if (retval != STATUS_SUCCESS) {
1557                 rtsx_trace(chip);
1558                 return STATUS_FAIL;
1559         }
1560
1561         retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
1562         if (retval != STATUS_SUCCESS) {
1563                 rtsx_trace(chip);
1564                 return STATUS_FAIL;
1565         }
1566
1567         ms_set_err_code(chip, MS_NO_ERROR);
1568         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1569         if (retval != STATUS_SUCCESS) {
1570                 rtsx_trace(chip);
1571                 return STATUS_FAIL;
1572         }
1573
1574         if (val & INT_REG_CMDNK) {
1575                 ms_set_err_code(chip, MS_CMD_NK);
1576                 rtsx_trace(chip);
1577                 return STATUS_FAIL;
1578         }
1579         if (val & INT_REG_CED) {
1580                 if (val & INT_REG_ERR) {
1581                         ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
1582                         rtsx_trace(chip);
1583                         return STATUS_FAIL;
1584                 }
1585         }
1586
1587         return STATUS_SUCCESS;
1588 }
1589
1590 static int ms_read_page(struct rtsx_chip *chip, u16 block_addr, u8 page_num)
1591 {
1592         struct ms_info *ms_card = &chip->ms_card;
1593         int retval;
1594         u8 val, data[6];
1595
1596         retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
1597                                     SystemParm, 6);
1598         if (retval != STATUS_SUCCESS) {
1599                 rtsx_trace(chip);
1600                 return STATUS_FAIL;
1601         }
1602
1603         if (CHK_MS4BIT(ms_card))
1604                 data[0] = 0x88;
1605         else
1606                 data[0] = 0x80;
1607
1608         data[1] = 0;
1609         data[2] = (u8)(block_addr >> 8);
1610         data[3] = (u8)block_addr;
1611         data[4] = 0x20;
1612         data[5] = page_num;
1613
1614         retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
1615         if (retval != STATUS_SUCCESS) {
1616                 rtsx_trace(chip);
1617                 return STATUS_FAIL;
1618         }
1619
1620         retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
1621         if (retval != STATUS_SUCCESS) {
1622                 rtsx_trace(chip);
1623                 return STATUS_FAIL;
1624         }
1625
1626         ms_set_err_code(chip, MS_NO_ERROR);
1627         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1628         if (retval != STATUS_SUCCESS) {
1629                 rtsx_trace(chip);
1630                 return STATUS_FAIL;
1631         }
1632
1633         if (val & INT_REG_CMDNK) {
1634                 ms_set_err_code(chip, MS_CMD_NK);
1635                 rtsx_trace(chip);
1636                 return STATUS_FAIL;
1637         }
1638
1639         if (val & INT_REG_CED) {
1640                 if (val & INT_REG_ERR) {
1641                         if (!(val & INT_REG_BREQ)) {
1642                                 ms_set_err_code(chip,  MS_FLASH_READ_ERROR);
1643                                 rtsx_trace(chip);
1644                                 return STATUS_FAIL;
1645                         }
1646                         retval = ms_read_status_reg(chip);
1647                         if (retval != STATUS_SUCCESS)
1648                                 ms_set_err_code(chip,  MS_FLASH_WRITE_ERROR);
1649
1650                 } else {
1651                         if (!(val & INT_REG_BREQ)) {
1652                                 ms_set_err_code(chip, MS_BREQ_ERROR);
1653                                 rtsx_trace(chip);
1654                                 return STATUS_FAIL;
1655                         }
1656                 }
1657         }
1658
1659         retval = ms_transfer_tpc(chip, MS_TM_NORMAL_READ, READ_PAGE_DATA,
1660                                  0, NO_WAIT_INT);
1661         if (retval != STATUS_SUCCESS) {
1662                 rtsx_trace(chip);
1663                 return STATUS_FAIL;
1664         }
1665
1666         if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR)) {
1667                 rtsx_trace(chip);
1668                 return STATUS_FAIL;
1669         }
1670
1671         return STATUS_SUCCESS;
1672 }
1673
1674 static int ms_set_bad_block(struct rtsx_chip *chip, u16 phy_blk)
1675 {
1676         struct ms_info *ms_card = &chip->ms_card;
1677         int retval;
1678         u8 val, data[8], extra[MS_EXTRA_SIZE];
1679
1680         retval = ms_read_extra_data(chip, phy_blk, 0, extra, MS_EXTRA_SIZE);
1681         if (retval != STATUS_SUCCESS) {
1682                 rtsx_trace(chip);
1683                 return STATUS_FAIL;
1684         }
1685
1686         retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
1687                                     SystemParm, 7);
1688         if (retval != STATUS_SUCCESS) {
1689                 rtsx_trace(chip);
1690                 return STATUS_FAIL;
1691         }
1692
1693         ms_set_err_code(chip, MS_NO_ERROR);
1694
1695         if (CHK_MS4BIT(ms_card))
1696                 data[0] = 0x88;
1697         else
1698                 data[0] = 0x80;
1699
1700         data[1] = 0;
1701         data[2] = (u8)(phy_blk >> 8);
1702         data[3] = (u8)phy_blk;
1703         data[4] = 0x80;
1704         data[5] = 0;
1705         data[6] = extra[0] & 0x7F;
1706         data[7] = 0xFF;
1707
1708         retval = ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT, data, 7);
1709         if (retval != STATUS_SUCCESS) {
1710                 rtsx_trace(chip);
1711                 return STATUS_FAIL;
1712         }
1713
1714         retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
1715         if (retval != STATUS_SUCCESS) {
1716                 rtsx_trace(chip);
1717                 return STATUS_FAIL;
1718         }
1719
1720         ms_set_err_code(chip, MS_NO_ERROR);
1721         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1722         if (retval != STATUS_SUCCESS) {
1723                 rtsx_trace(chip);
1724                 return STATUS_FAIL;
1725         }
1726
1727         if (val & INT_REG_CMDNK) {
1728                 ms_set_err_code(chip, MS_CMD_NK);
1729                 rtsx_trace(chip);
1730                 return STATUS_FAIL;
1731         }
1732
1733         if (val & INT_REG_CED) {
1734                 if (val & INT_REG_ERR) {
1735                         ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
1736                         rtsx_trace(chip);
1737                         return STATUS_FAIL;
1738                 }
1739         }
1740
1741         return STATUS_SUCCESS;
1742 }
1743
1744 static int ms_erase_block(struct rtsx_chip *chip, u16 phy_blk)
1745 {
1746         struct ms_info *ms_card = &chip->ms_card;
1747         int retval, i = 0;
1748         u8 val, data[6];
1749
1750         retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
1751                                     SystemParm, 6);
1752         if (retval != STATUS_SUCCESS) {
1753                 rtsx_trace(chip);
1754                 return STATUS_FAIL;
1755         }
1756
1757         ms_set_err_code(chip, MS_NO_ERROR);
1758
1759         if (CHK_MS4BIT(ms_card))
1760                 data[0] = 0x88;
1761         else
1762                 data[0] = 0x80;
1763
1764         data[1] = 0;
1765         data[2] = (u8)(phy_blk >> 8);
1766         data[3] = (u8)phy_blk;
1767         data[4] = 0;
1768         data[5] = 0;
1769
1770         retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
1771         if (retval != STATUS_SUCCESS) {
1772                 rtsx_trace(chip);
1773                 return STATUS_FAIL;
1774         }
1775
1776 ERASE_RTY:
1777         retval = ms_send_cmd(chip, BLOCK_ERASE, WAIT_INT);
1778         if (retval != STATUS_SUCCESS) {
1779                 rtsx_trace(chip);
1780                 return STATUS_FAIL;
1781         }
1782
1783         ms_set_err_code(chip, MS_NO_ERROR);
1784         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1785         if (retval != STATUS_SUCCESS) {
1786                 rtsx_trace(chip);
1787                 return STATUS_FAIL;
1788         }
1789
1790         if (val & INT_REG_CMDNK) {
1791                 if (i < 3) {
1792                         i++;
1793                         goto ERASE_RTY;
1794                 }
1795
1796                 ms_set_err_code(chip, MS_CMD_NK);
1797                 ms_set_bad_block(chip, phy_blk);
1798                 rtsx_trace(chip);
1799                 return STATUS_FAIL;
1800         }
1801
1802         if (val & INT_REG_CED) {
1803                 if (val & INT_REG_ERR) {
1804                         ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
1805                         rtsx_trace(chip);
1806                         return STATUS_FAIL;
1807                 }
1808         }
1809
1810         return STATUS_SUCCESS;
1811 }
1812
1813 static void ms_set_page_status(u16 log_blk, u8 type, u8 *extra, int extra_len)
1814 {
1815         if (!extra || (extra_len < MS_EXTRA_SIZE))
1816                 return;
1817
1818         memset(extra, 0xFF, MS_EXTRA_SIZE);
1819
1820         if (type == setPS_NG) {
1821                 /* set page status as 1:NG,and block status keep 1:OK */
1822                 extra[0] = 0xB8;
1823         } else {
1824                 /* set page status as 0:Data Error,and block status keep 1:OK */
1825                 extra[0] = 0x98;
1826         }
1827
1828         extra[2] = (u8)(log_blk >> 8);
1829         extra[3] = (u8)log_blk;
1830 }
1831
1832 static int ms_init_page(struct rtsx_chip *chip, u16 phy_blk, u16 log_blk,
1833                         u8 start_page, u8 end_page)
1834 {
1835         int retval;
1836         u8 extra[MS_EXTRA_SIZE], i;
1837
1838         memset(extra, 0xff, MS_EXTRA_SIZE);
1839
1840         extra[0] = 0xf8;        /* Block, page OK, data erased */
1841         extra[1] = 0xff;
1842         extra[2] = (u8)(log_blk >> 8);
1843         extra[3] = (u8)log_blk;
1844
1845         for (i = start_page; i < end_page; i++) {
1846                 if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
1847                         ms_set_err_code(chip, MS_NO_CARD);
1848                         rtsx_trace(chip);
1849                         return STATUS_FAIL;
1850                 }
1851
1852                 retval = ms_write_extra_data(chip, phy_blk, i,
1853                                              extra, MS_EXTRA_SIZE);
1854                 if (retval != STATUS_SUCCESS) {
1855                         rtsx_trace(chip);
1856                         return STATUS_FAIL;
1857                 }
1858         }
1859
1860         return STATUS_SUCCESS;
1861 }
1862
1863 static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
1864                         u16 log_blk, u8 start_page, u8 end_page)
1865 {
1866         struct ms_info *ms_card = &chip->ms_card;
1867         bool uncorrect_flag = false;
1868         int retval, rty_cnt;
1869         u8 extra[MS_EXTRA_SIZE], val, i, j, data[16];
1870
1871         dev_dbg(rtsx_dev(chip), "Copy page from 0x%x to 0x%x, logical block is 0x%x\n",
1872                 old_blk, new_blk, log_blk);
1873         dev_dbg(rtsx_dev(chip), "start_page = %d, end_page = %d\n",
1874                 start_page, end_page);
1875
1876         retval = ms_read_extra_data(chip, new_blk, 0, extra, MS_EXTRA_SIZE);
1877         if (retval != STATUS_SUCCESS) {
1878                 rtsx_trace(chip);
1879                 return STATUS_FAIL;
1880         }
1881
1882         retval = ms_read_status_reg(chip);
1883         if (retval != STATUS_SUCCESS) {
1884                 rtsx_trace(chip);
1885                 return STATUS_FAIL;
1886         }
1887
1888         retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
1889         if (retval) {
1890                 rtsx_trace(chip);
1891                 return retval;
1892         }
1893
1894         if (val & BUF_FULL) {
1895                 retval = ms_send_cmd(chip, CLEAR_BUF, WAIT_INT);
1896                 if (retval != STATUS_SUCCESS) {
1897                         rtsx_trace(chip);
1898                         return STATUS_FAIL;
1899                 }
1900
1901                 retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1902                 if (retval != STATUS_SUCCESS) {
1903                         rtsx_trace(chip);
1904                         return STATUS_FAIL;
1905                 }
1906
1907                 if (!(val & INT_REG_CED)) {
1908                         ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
1909                         rtsx_trace(chip);
1910                         return STATUS_FAIL;
1911                 }
1912         }
1913
1914         for (i = start_page; i < end_page; i++) {
1915                 if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
1916                         ms_set_err_code(chip, MS_NO_CARD);
1917                         rtsx_trace(chip);
1918                         return STATUS_FAIL;
1919                 }
1920
1921                 ms_read_extra_data(chip, old_blk, i, extra, MS_EXTRA_SIZE);
1922
1923                 retval = ms_set_rw_reg_addr(chip, OverwriteFlag,
1924                                             MS_EXTRA_SIZE, SystemParm, 6);
1925                 if (retval != STATUS_SUCCESS) {
1926                         rtsx_trace(chip);
1927                         return STATUS_FAIL;
1928                 }
1929
1930                 ms_set_err_code(chip, MS_NO_ERROR);
1931
1932                 if (CHK_MS4BIT(ms_card))
1933                         data[0] = 0x88;
1934                 else
1935                         data[0] = 0x80;
1936
1937                 data[1] = 0;
1938                 data[2] = (u8)(old_blk >> 8);
1939                 data[3] = (u8)old_blk;
1940                 data[4] = 0x20;
1941                 data[5] = i;
1942
1943                 retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT,
1944                                         data, 6);
1945                 if (retval != STATUS_SUCCESS) {
1946                         rtsx_trace(chip);
1947                         return STATUS_FAIL;
1948                 }
1949
1950                 retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
1951                 if (retval != STATUS_SUCCESS) {
1952                         rtsx_trace(chip);
1953                         return STATUS_FAIL;
1954                 }
1955
1956                 ms_set_err_code(chip, MS_NO_ERROR);
1957                 retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1958                 if (retval != STATUS_SUCCESS) {
1959                         rtsx_trace(chip);
1960                         return STATUS_FAIL;
1961                 }
1962
1963                 if (val & INT_REG_CMDNK) {
1964                         ms_set_err_code(chip, MS_CMD_NK);
1965                         rtsx_trace(chip);
1966                         return STATUS_FAIL;
1967                 }
1968
1969                 if (val & INT_REG_CED) {
1970                         if (val & INT_REG_ERR) {
1971                                 retval = ms_read_status_reg(chip);
1972                                 if (retval != STATUS_SUCCESS) {
1973                                         uncorrect_flag = true;
1974                                         dev_dbg(rtsx_dev(chip), "Uncorrectable error\n");
1975                                 } else {
1976                                         uncorrect_flag = false;
1977                                 }
1978
1979                                 retval = ms_transfer_tpc(chip,
1980                                                          MS_TM_NORMAL_READ,
1981                                                          READ_PAGE_DATA,
1982                                                          0, NO_WAIT_INT);
1983                                 if (retval != STATUS_SUCCESS) {
1984                                         rtsx_trace(chip);
1985                                         return STATUS_FAIL;
1986                                 }
1987
1988                                 if (uncorrect_flag) {
1989                                         ms_set_page_status(log_blk, setPS_NG,
1990                                                            extra,
1991                                                            MS_EXTRA_SIZE);
1992                                         if (i == 0)
1993                                                 extra[0] &= 0xEF;
1994
1995                                         ms_write_extra_data(chip, old_blk, i,
1996                                                             extra,
1997                                                             MS_EXTRA_SIZE);
1998                                         dev_dbg(rtsx_dev(chip), "page %d : extra[0] = 0x%x\n",
1999                                                 i, extra[0]);
2000                                         MS_SET_BAD_BLOCK_FLG(ms_card);
2001
2002                                         ms_set_page_status(log_blk, setPS_Error,
2003                                                            extra,
2004                                                            MS_EXTRA_SIZE);
2005                                         ms_write_extra_data(chip, new_blk, i,
2006                                                             extra,
2007                                                             MS_EXTRA_SIZE);
2008                                         continue;
2009                                 }
2010
2011                                 for (rty_cnt = 0; rty_cnt < MS_MAX_RETRY_COUNT;
2012                                      rty_cnt++) {
2013                                         retval = ms_transfer_tpc(
2014                                                 chip,
2015                                                 MS_TM_NORMAL_WRITE,
2016                                                 WRITE_PAGE_DATA,
2017                                                 0, NO_WAIT_INT);
2018                                         if (retval == STATUS_SUCCESS)
2019                                                 break;
2020                                 }
2021                                 if (rty_cnt == MS_MAX_RETRY_COUNT) {
2022                                         rtsx_trace(chip);
2023                                         return STATUS_FAIL;
2024                                 }
2025                         }
2026
2027                         if (!(val & INT_REG_BREQ)) {
2028                                 ms_set_err_code(chip, MS_BREQ_ERROR);
2029                                 rtsx_trace(chip);
2030                                 return STATUS_FAIL;
2031                         }
2032                 }
2033
2034                 retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
2035                                             SystemParm, (6 + MS_EXTRA_SIZE));
2036
2037                 ms_set_err_code(chip, MS_NO_ERROR);
2038
2039                 if (CHK_MS4BIT(ms_card))
2040                         data[0] = 0x88;
2041                 else
2042                         data[0] = 0x80;
2043
2044                 data[1] = 0;
2045                 data[2] = (u8)(new_blk >> 8);
2046                 data[3] = (u8)new_blk;
2047                 data[4] = 0x20;
2048                 data[5] = i;
2049
2050                 if ((extra[0] & 0x60) != 0x60)
2051                         data[6] = extra[0];
2052                 else
2053                         data[6] = 0xF8;
2054
2055                 data[6 + 1] = 0xFF;
2056                 data[6 + 2] = (u8)(log_blk >> 8);
2057                 data[6 + 3] = (u8)log_blk;
2058
2059                 for (j = 4; j <= MS_EXTRA_SIZE; j++)
2060                         data[6 + j] = 0xFF;
2061
2062                 retval = ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE),
2063                                         NO_WAIT_INT, data, 16);
2064                 if (retval != STATUS_SUCCESS) {
2065                         rtsx_trace(chip);
2066                         return STATUS_FAIL;
2067                 }
2068
2069                 retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
2070                 if (retval != STATUS_SUCCESS) {
2071                         rtsx_trace(chip);
2072                         return STATUS_FAIL;
2073                 }
2074
2075                 ms_set_err_code(chip, MS_NO_ERROR);
2076                 retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
2077                 if (retval != STATUS_SUCCESS) {
2078                         rtsx_trace(chip);
2079                         return STATUS_FAIL;
2080                 }
2081
2082                 if (val & INT_REG_CMDNK) {
2083                         ms_set_err_code(chip, MS_CMD_NK);
2084                         rtsx_trace(chip);
2085                         return STATUS_FAIL;
2086                 }
2087
2088                 if (val & INT_REG_CED) {
2089                         if (val & INT_REG_ERR) {
2090                                 ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
2091                                 rtsx_trace(chip);
2092                                 return STATUS_FAIL;
2093                         }
2094                 }
2095
2096                 if (i == 0) {
2097                         retval = ms_set_rw_reg_addr(chip, OverwriteFlag,
2098                                                     MS_EXTRA_SIZE, SystemParm,
2099                                                     7);
2100                         if (retval != STATUS_SUCCESS) {
2101                                 rtsx_trace(chip);
2102                                 return STATUS_FAIL;
2103                         }
2104
2105                         ms_set_err_code(chip, MS_NO_ERROR);
2106
2107                         if (CHK_MS4BIT(ms_card))
2108                                 data[0] = 0x88;
2109                         else
2110                                 data[0] = 0x80;
2111
2112                         data[1] = 0;
2113                         data[2] = (u8)(old_blk >> 8);
2114                         data[3] = (u8)old_blk;
2115                         data[4] = 0x80;
2116                         data[5] = 0;
2117                         data[6] = 0xEF;
2118                         data[7] = 0xFF;
2119
2120                         retval = ms_write_bytes(chip, WRITE_REG, 7,
2121                                                 NO_WAIT_INT, data, 8);
2122                         if (retval != STATUS_SUCCESS) {
2123                                 rtsx_trace(chip);
2124                                 return STATUS_FAIL;
2125                         }
2126
2127                         retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
2128                         if (retval != STATUS_SUCCESS) {
2129                                 rtsx_trace(chip);
2130                                 return STATUS_FAIL;
2131                         }
2132
2133                         ms_set_err_code(chip, MS_NO_ERROR);
2134                         retval = ms_read_bytes(chip, GET_INT, 1,
2135                                                NO_WAIT_INT, &val, 1);
2136                         if (retval != STATUS_SUCCESS) {
2137                                 rtsx_trace(chip);
2138                                 return STATUS_FAIL;
2139                         }
2140
2141                         if (val & INT_REG_CMDNK) {
2142                                 ms_set_err_code(chip, MS_CMD_NK);
2143                                 rtsx_trace(chip);
2144                                 return STATUS_FAIL;
2145                         }
2146
2147                         if (val & INT_REG_CED) {
2148                                 if (val & INT_REG_ERR) {
2149                                         ms_set_err_code(chip,
2150                                                         MS_FLASH_WRITE_ERROR);
2151                                         rtsx_trace(chip);
2152                                         return STATUS_FAIL;
2153                                 }
2154                         }
2155                 }
2156         }
2157
2158         return STATUS_SUCCESS;
2159 }
2160
2161 static int reset_ms(struct rtsx_chip *chip)
2162 {
2163         struct ms_info *ms_card = &chip->ms_card;
2164         int retval;
2165         u16 i, reg_addr, block_size;
2166         u8 val, extra[MS_EXTRA_SIZE], j, *ptr;
2167 #ifndef SUPPORT_MAGIC_GATE
2168         u16 eblock_cnt;
2169 #endif
2170
2171         retval = ms_prepare_reset(chip);
2172         if (retval != STATUS_SUCCESS) {
2173                 rtsx_trace(chip);
2174                 return STATUS_FAIL;
2175         }
2176
2177         ms_card->ms_type |= TYPE_MS;
2178
2179         retval = ms_send_cmd(chip, MS_RESET, NO_WAIT_INT);
2180         if (retval != STATUS_SUCCESS) {
2181                 rtsx_trace(chip);
2182                 return STATUS_FAIL;
2183         }
2184
2185         retval = ms_read_status_reg(chip);
2186         if (retval != STATUS_SUCCESS) {
2187                 rtsx_trace(chip);
2188                 return STATUS_FAIL;
2189         }
2190
2191         retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
2192         if (retval) {
2193                 rtsx_trace(chip);
2194                 return retval;
2195         }
2196         if (val & WRT_PRTCT)
2197                 chip->card_wp |= MS_CARD;
2198         else
2199                 chip->card_wp &= ~MS_CARD;
2200
2201         i = 0;
2202
2203 RE_SEARCH:
2204         /* Search Boot Block */
2205         while (i < (MAX_DEFECTIVE_BLOCK + 2)) {
2206                 if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
2207                         ms_set_err_code(chip, MS_NO_CARD);
2208                         rtsx_trace(chip);
2209                         return STATUS_FAIL;
2210                 }
2211
2212                 retval = ms_read_extra_data(chip, i, 0, extra, MS_EXTRA_SIZE);
2213                 if (retval != STATUS_SUCCESS) {
2214                         i++;
2215                         continue;
2216                 }
2217
2218                 if (extra[0] & BLOCK_OK) {
2219                         if (!(extra[1] & NOT_BOOT_BLOCK)) {
2220                                 ms_card->boot_block = i;
2221                                 break;
2222                         }
2223                 }
2224                 i++;
2225         }
2226
2227         if (i == (MAX_DEFECTIVE_BLOCK + 2)) {
2228                 dev_dbg(rtsx_dev(chip), "No boot block found!");
2229                 rtsx_trace(chip);
2230                 return STATUS_FAIL;
2231         }
2232
2233         for (j = 0; j < 3; j++) {
2234                 retval = ms_read_page(chip, ms_card->boot_block, j);
2235                 if (retval != STATUS_SUCCESS) {
2236                         if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR)) {
2237                                 i = ms_card->boot_block + 1;
2238                                 ms_set_err_code(chip, MS_NO_ERROR);
2239                                 goto RE_SEARCH;
2240                         }
2241                 }
2242         }
2243
2244         retval = ms_read_page(chip, ms_card->boot_block, 0);
2245         if (retval != STATUS_SUCCESS) {
2246                 rtsx_trace(chip);
2247                 return STATUS_FAIL;
2248         }
2249
2250         /* Read MS system information as sys_info */
2251         rtsx_init_cmd(chip);
2252
2253         for (i = 0; i < 96; i++)
2254                 rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 0x1A0 + i, 0, 0);
2255
2256         retval = rtsx_send_cmd(chip, MS_CARD, 100);
2257         if (retval < 0) {
2258                 rtsx_trace(chip);
2259                 return STATUS_FAIL;
2260         }
2261
2262         ptr = rtsx_get_cmd_data(chip);
2263         memcpy(ms_card->raw_sys_info, ptr, 96);
2264
2265         /* Read useful block contents */
2266         rtsx_init_cmd(chip);
2267
2268         rtsx_add_cmd(chip, READ_REG_CMD, HEADER_ID0, 0, 0);
2269         rtsx_add_cmd(chip, READ_REG_CMD, HEADER_ID1, 0, 0);
2270
2271         for (reg_addr = DISABLED_BLOCK0; reg_addr <= DISABLED_BLOCK3;
2272              reg_addr++)
2273                 rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
2274
2275         for (reg_addr = BLOCK_SIZE_0; reg_addr <= PAGE_SIZE_1; reg_addr++)
2276                 rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
2277
2278         rtsx_add_cmd(chip, READ_REG_CMD, MS_Device_Type, 0, 0);
2279         rtsx_add_cmd(chip, READ_REG_CMD, MS_4bit_Support, 0, 0);
2280
2281         retval = rtsx_send_cmd(chip, MS_CARD, 100);
2282         if (retval < 0) {
2283                 rtsx_trace(chip);
2284                 return STATUS_FAIL;
2285         }
2286
2287         ptr = rtsx_get_cmd_data(chip);
2288
2289         dev_dbg(rtsx_dev(chip), "Boot block data:\n");
2290         dev_dbg(rtsx_dev(chip), "%*ph\n", 16, ptr);
2291
2292         /* Block ID error
2293          * HEADER_ID0, HEADER_ID1
2294          */
2295         if (ptr[0] != 0x00 || ptr[1] != 0x01) {
2296                 i = ms_card->boot_block + 1;
2297                 goto RE_SEARCH;
2298         }
2299
2300         /* Page size error
2301          * PAGE_SIZE_0, PAGE_SIZE_1
2302          */
2303         if (ptr[12] != 0x02 || ptr[13] != 0x00) {
2304                 i = ms_card->boot_block + 1;
2305                 goto RE_SEARCH;
2306         }
2307
2308         if ((ptr[14] == 1) || (ptr[14] == 3))
2309                 chip->card_wp |= MS_CARD;
2310
2311         /* BLOCK_SIZE_0, BLOCK_SIZE_1 */
2312         block_size = ((u16)ptr[6] << 8) | ptr[7];
2313         if (block_size == 0x0010) {
2314                 /* Block size 16KB */
2315                 ms_card->block_shift = 5;
2316                 ms_card->page_off = 0x1F;
2317         } else if (block_size == 0x0008) {
2318                 /* Block size 8KB */
2319                 ms_card->block_shift = 4;
2320                 ms_card->page_off = 0x0F;
2321         }
2322
2323         /* BLOCK_COUNT_0, BLOCK_COUNT_1 */
2324         ms_card->total_block = ((u16)ptr[8] << 8) | ptr[9];
2325
2326 #ifdef SUPPORT_MAGIC_GATE
2327         j = ptr[10];
2328
2329         if (ms_card->block_shift == 4)  { /* 4MB or 8MB */
2330                 if (j < 2)  { /* Effective block for 4MB: 0x1F0 */
2331                         ms_card->capacity = 0x1EE0;
2332                 } else { /* Effective block for 8MB: 0x3E0 */
2333                         ms_card->capacity = 0x3DE0;
2334                 }
2335         } else  { /* 16MB, 32MB, 64MB or 128MB */
2336                 if (j < 5)  { /* Effective block for 16MB: 0x3E0 */
2337                         ms_card->capacity = 0x7BC0;
2338                 } else if (j < 0xA) { /* Effective block for 32MB: 0x7C0 */
2339                         ms_card->capacity = 0xF7C0;
2340                 } else if (j < 0x11) { /* Effective block for 64MB: 0xF80 */
2341                         ms_card->capacity = 0x1EF80;
2342                 } else { /* Effective block for 128MB: 0x1F00 */
2343                         ms_card->capacity = 0x3DF00;
2344                 }
2345         }
2346 #else
2347         /* EBLOCK_COUNT_0, EBLOCK_COUNT_1 */
2348         eblock_cnt = ((u16)ptr[10] << 8) | ptr[11];
2349
2350         ms_card->capacity = ((u32)eblock_cnt - 2) << ms_card->block_shift;
2351 #endif
2352
2353         chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity;
2354
2355         /* Switch I/F Mode */
2356         if (ptr[15]) {
2357                 retval = ms_set_rw_reg_addr(chip, 0, 0, SystemParm, 1);
2358                 if (retval != STATUS_SUCCESS) {
2359                         rtsx_trace(chip);
2360                         return STATUS_FAIL;
2361                 }
2362
2363                 retval = rtsx_write_register(chip, PPBUF_BASE2, 0xFF, 0x88);
2364                 if (retval) {
2365                         rtsx_trace(chip);
2366                         return retval;
2367                 }
2368                 retval = rtsx_write_register(chip, PPBUF_BASE2 + 1, 0xFF, 0);
2369                 if (retval) {
2370                         rtsx_trace(chip);
2371                         return retval;
2372                 }
2373
2374                 retval = ms_transfer_tpc(chip, MS_TM_WRITE_BYTES, WRITE_REG, 1,
2375                                          NO_WAIT_INT);
2376                 if (retval != STATUS_SUCCESS) {
2377                         rtsx_trace(chip);
2378                         return STATUS_FAIL;
2379                 }
2380
2381                 retval = rtsx_write_register(chip, MS_CFG,
2382                                              0x58 | MS_NO_CHECK_INT,
2383                                              MS_BUS_WIDTH_4 |
2384                                              PUSH_TIME_ODD |
2385                                              MS_NO_CHECK_INT);
2386                 if (retval) {
2387                         rtsx_trace(chip);
2388                         return retval;
2389                 }
2390
2391                 ms_card->ms_type |= MS_4BIT;
2392         }
2393
2394         if (CHK_MS4BIT(ms_card))
2395                 chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
2396         else
2397                 chip->card_bus_width[chip->card2lun[MS_CARD]] = 1;
2398
2399         return STATUS_SUCCESS;
2400 }
2401
2402 static int ms_init_l2p_tbl(struct rtsx_chip *chip)
2403 {
2404         struct ms_info *ms_card = &chip->ms_card;
2405         int size, i, seg_no, retval;
2406         u16 defect_block, reg_addr;
2407         u8 val1, val2;
2408
2409         ms_card->segment_cnt = ms_card->total_block >> 9;
2410         dev_dbg(rtsx_dev(chip), "ms_card->segment_cnt = %d\n",
2411                 ms_card->segment_cnt);
2412
2413         size = ms_card->segment_cnt * sizeof(struct zone_entry);
2414         ms_card->segment = vzalloc(size);
2415         if (!ms_card->segment) {
2416                 rtsx_trace(chip);
2417                 return STATUS_FAIL;
2418         }
2419
2420         retval = ms_read_page(chip, ms_card->boot_block, 1);
2421         if (retval != STATUS_SUCCESS) {
2422                 rtsx_trace(chip);
2423                 goto INIT_FAIL;
2424         }
2425
2426         reg_addr = PPBUF_BASE2;
2427         for (i = 0; i < (((ms_card->total_block >> 9) * 10) + 1); i++) {
2428                 int block_no;
2429
2430                 retval = rtsx_read_register(chip, reg_addr++, &val1);
2431                 if (retval != STATUS_SUCCESS) {
2432                         rtsx_trace(chip);
2433                         goto INIT_FAIL;
2434                 }
2435
2436                 retval = rtsx_read_register(chip, reg_addr++, &val2);
2437                 if (retval != STATUS_SUCCESS) {
2438                         rtsx_trace(chip);
2439                         goto INIT_FAIL;
2440                 }
2441
2442                 defect_block = ((u16)val1 << 8) | val2;
2443                 if (defect_block == 0xFFFF)
2444                         break;
2445
2446                 seg_no = defect_block / 512;
2447
2448                 block_no = ms_card->segment[seg_no].disable_count++;
2449                 ms_card->segment[seg_no].defect_list[block_no] = defect_block;
2450         }
2451
2452         for (i = 0; i < ms_card->segment_cnt; i++) {
2453                 ms_card->segment[i].build_flag = 0;
2454                 ms_card->segment[i].l2p_table = NULL;
2455                 ms_card->segment[i].free_table = NULL;
2456                 ms_card->segment[i].get_index = 0;
2457                 ms_card->segment[i].set_index = 0;
2458                 ms_card->segment[i].unused_blk_cnt = 0;
2459
2460                 dev_dbg(rtsx_dev(chip), "defective block count of segment %d is %d\n",
2461                         i, ms_card->segment[i].disable_count);
2462         }
2463
2464         return STATUS_SUCCESS;
2465
2466 INIT_FAIL:
2467         vfree(ms_card->segment);
2468         ms_card->segment = NULL;
2469
2470         return STATUS_FAIL;
2471 }
2472
2473 static u16 ms_get_l2p_tbl(struct rtsx_chip *chip, int seg_no, u16 log_off)
2474 {
2475         struct ms_info *ms_card = &chip->ms_card;
2476         struct zone_entry *segment;
2477
2478         if (!ms_card->segment)
2479                 return 0xFFFF;
2480
2481         segment = &ms_card->segment[seg_no];
2482
2483         if (segment->l2p_table)
2484                 return segment->l2p_table[log_off];
2485
2486         return 0xFFFF;
2487 }
2488
2489 static void ms_set_l2p_tbl(struct rtsx_chip *chip,
2490                            int seg_no, u16 log_off, u16 phy_blk)
2491 {
2492         struct ms_info *ms_card = &chip->ms_card;
2493         struct zone_entry *segment;
2494
2495         if (!ms_card->segment)
2496                 return;
2497
2498         segment = &ms_card->segment[seg_no];
2499         if (segment->l2p_table)
2500                 segment->l2p_table[log_off] = phy_blk;
2501 }
2502
2503 static void ms_set_unused_block(struct rtsx_chip *chip, u16 phy_blk)
2504 {
2505         struct ms_info *ms_card = &chip->ms_card;
2506         struct zone_entry *segment;
2507         int seg_no;
2508
2509         seg_no = (int)phy_blk >> 9;
2510         segment = &ms_card->segment[seg_no];
2511
2512         segment->free_table[segment->set_index++] = phy_blk;
2513         if (segment->set_index >= MS_FREE_TABLE_CNT)
2514                 segment->set_index = 0;
2515
2516         segment->unused_blk_cnt++;
2517 }
2518
2519 static u16 ms_get_unused_block(struct rtsx_chip *chip, int seg_no)
2520 {
2521         struct ms_info *ms_card = &chip->ms_card;
2522         struct zone_entry *segment;
2523         u16 phy_blk;
2524
2525         segment = &ms_card->segment[seg_no];
2526
2527         if (segment->unused_blk_cnt <= 0)
2528                 return 0xFFFF;
2529
2530         phy_blk = segment->free_table[segment->get_index];
2531         segment->free_table[segment->get_index++] = 0xFFFF;
2532         if (segment->get_index >= MS_FREE_TABLE_CNT)
2533                 segment->get_index = 0;
2534
2535         segment->unused_blk_cnt--;
2536
2537         return phy_blk;
2538 }
2539
2540 static const unsigned short ms_start_idx[] = {0, 494, 990, 1486, 1982, 2478,
2541                                               2974, 3470, 3966, 4462, 4958,
2542                                               5454, 5950, 6446, 6942, 7438,
2543                                               7934};
2544
2545 static int ms_arbitrate_l2p(struct rtsx_chip *chip, u16 phy_blk,
2546                             u16 log_off, u8 us1, u8 us2)
2547 {
2548         struct ms_info *ms_card = &chip->ms_card;
2549         struct zone_entry *segment;
2550         int seg_no;
2551         u16 tmp_blk;
2552
2553         seg_no = (int)phy_blk >> 9;
2554         segment = &ms_card->segment[seg_no];
2555         tmp_blk = segment->l2p_table[log_off];
2556
2557         if (us1 != us2) {
2558                 if (us1 == 0) {
2559                         if (!(chip->card_wp & MS_CARD))
2560                                 ms_erase_block(chip, tmp_blk);
2561
2562                         ms_set_unused_block(chip, tmp_blk);
2563                         segment->l2p_table[log_off] = phy_blk;
2564                 } else {
2565                         if (!(chip->card_wp & MS_CARD))
2566                                 ms_erase_block(chip, phy_blk);
2567
2568                         ms_set_unused_block(chip, phy_blk);
2569                 }
2570         } else {
2571                 if (phy_blk < tmp_blk) {
2572                         if (!(chip->card_wp & MS_CARD))
2573                                 ms_erase_block(chip, phy_blk);
2574
2575                         ms_set_unused_block(chip, phy_blk);
2576                 } else {
2577                         if (!(chip->card_wp & MS_CARD))
2578                                 ms_erase_block(chip, tmp_blk);
2579
2580                         ms_set_unused_block(chip, tmp_blk);
2581                         segment->l2p_table[log_off] = phy_blk;
2582                 }
2583         }
2584
2585         return STATUS_SUCCESS;
2586 }
2587
2588 static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
2589 {
2590         struct ms_info *ms_card = &chip->ms_card;
2591         struct zone_entry *segment;
2592         bool defect_flag;
2593         int retval, table_size, disable_cnt, i;
2594         u16 start, end, phy_blk, log_blk, tmp_blk, idx;
2595         u8 extra[MS_EXTRA_SIZE], us1, us2;
2596
2597         dev_dbg(rtsx_dev(chip), "%s: %d\n", __func__, seg_no);
2598
2599         if (!ms_card->segment) {
2600                 retval = ms_init_l2p_tbl(chip);
2601                 if (retval != STATUS_SUCCESS) {
2602                         rtsx_trace(chip);
2603                         return retval;
2604                 }
2605         }
2606
2607         if (ms_card->segment[seg_no].build_flag) {
2608                 dev_dbg(rtsx_dev(chip), "l2p table of segment %d has been built\n",
2609                         seg_no);
2610                 return STATUS_SUCCESS;
2611         }
2612
2613         if (seg_no == 0)
2614                 table_size = 494;
2615         else
2616                 table_size = 496;
2617
2618         segment = &ms_card->segment[seg_no];
2619
2620         if (!segment->l2p_table) {
2621                 segment->l2p_table = vmalloc(array_size(table_size, 2));
2622                 if (!segment->l2p_table) {
2623                         rtsx_trace(chip);
2624                         goto BUILD_FAIL;
2625                 }
2626         }
2627         memset((u8 *)(segment->l2p_table), 0xff, table_size * 2);
2628
2629         if (!segment->free_table) {
2630                 segment->free_table = vmalloc(MS_FREE_TABLE_CNT * 2);
2631                 if (!segment->free_table) {
2632                         rtsx_trace(chip);
2633                         goto BUILD_FAIL;
2634                 }
2635         }
2636         memset((u8 *)(segment->free_table), 0xff, MS_FREE_TABLE_CNT * 2);
2637
2638         start = (u16)seg_no << 9;
2639         end = (u16)(seg_no + 1) << 9;
2640
2641         disable_cnt = segment->disable_count;
2642
2643         segment->get_index = 0;
2644         segment->set_index = 0;
2645         segment->unused_blk_cnt = 0;
2646
2647         for (phy_blk = start; phy_blk < end; phy_blk++) {
2648                 if (disable_cnt) {
2649                         defect_flag = false;
2650                         for (i = 0; i < segment->disable_count; i++) {
2651                                 if (phy_blk == segment->defect_list[i]) {
2652                                         defect_flag = true;
2653                                         break;
2654                                 }
2655                         }
2656                         if (defect_flag) {
2657                                 disable_cnt--;
2658                                 continue;
2659                         }
2660                 }
2661
2662                 retval = ms_read_extra_data(chip, phy_blk, 0,
2663                                             extra, MS_EXTRA_SIZE);
2664                 if (retval != STATUS_SUCCESS) {
2665                         dev_dbg(rtsx_dev(chip), "read extra data fail\n");
2666                         ms_set_bad_block(chip, phy_blk);
2667                         continue;
2668                 }
2669
2670                 if (seg_no == ms_card->segment_cnt - 1) {
2671                         if (!(extra[1] & NOT_TRANSLATION_TABLE)) {
2672                                 if (!(chip->card_wp & MS_CARD)) {
2673                                         retval = ms_erase_block(chip, phy_blk);
2674                                         if (retval != STATUS_SUCCESS)
2675                                                 continue;
2676                                         extra[2] = 0xff;
2677                                         extra[3] = 0xff;
2678                                 }
2679                         }
2680                 }
2681
2682                 if (!(extra[0] & BLOCK_OK))
2683                         continue;
2684                 if (!(extra[1] & NOT_BOOT_BLOCK))
2685                         continue;
2686                 if ((extra[0] & PAGE_OK) != PAGE_OK)
2687                         continue;
2688
2689                 log_blk = ((u16)extra[2] << 8) | extra[3];
2690
2691                 if (log_blk == 0xFFFF) {
2692                         if (!(chip->card_wp & MS_CARD)) {
2693                                 retval = ms_erase_block(chip, phy_blk);
2694                                 if (retval != STATUS_SUCCESS)
2695                                         continue;
2696                         }
2697                         ms_set_unused_block(chip, phy_blk);
2698                         continue;
2699                 }
2700
2701                 if ((log_blk < ms_start_idx[seg_no]) ||
2702                     (log_blk >= ms_start_idx[seg_no + 1])) {
2703                         if (!(chip->card_wp & MS_CARD)) {
2704                                 retval = ms_erase_block(chip, phy_blk);
2705                                 if (retval != STATUS_SUCCESS)
2706                                         continue;
2707                         }
2708                         ms_set_unused_block(chip, phy_blk);
2709                         continue;
2710                 }
2711
2712                 idx = log_blk - ms_start_idx[seg_no];
2713
2714                 if (segment->l2p_table[idx] == 0xFFFF) {
2715                         segment->l2p_table[idx] = phy_blk;
2716                         continue;
2717                 }
2718
2719                 us1 = extra[0] & 0x10;
2720                 tmp_blk = segment->l2p_table[idx];
2721                 retval = ms_read_extra_data(chip, tmp_blk, 0,
2722                                             extra, MS_EXTRA_SIZE);
2723                 if (retval != STATUS_SUCCESS)
2724                         continue;
2725                 us2 = extra[0] & 0x10;
2726
2727                 (void)ms_arbitrate_l2p(chip, phy_blk,
2728                                 log_blk - ms_start_idx[seg_no], us1, us2);
2729                 continue;
2730         }
2731
2732         segment->build_flag = 1;
2733
2734         dev_dbg(rtsx_dev(chip), "unused block count: %d\n",
2735                 segment->unused_blk_cnt);
2736
2737         /* Logical Address Confirmation Process */
2738         if (seg_no == ms_card->segment_cnt - 1) {
2739                 if (segment->unused_blk_cnt < 2)
2740                         chip->card_wp |= MS_CARD;
2741         } else {
2742                 if (segment->unused_blk_cnt < 1)
2743                         chip->card_wp |= MS_CARD;
2744         }
2745
2746         if (chip->card_wp & MS_CARD)
2747                 return STATUS_SUCCESS;
2748
2749         for (log_blk = ms_start_idx[seg_no];
2750              log_blk < ms_start_idx[seg_no + 1]; log_blk++) {
2751                 idx = log_blk - ms_start_idx[seg_no];
2752                 if (segment->l2p_table[idx] == 0xFFFF) {
2753                         phy_blk = ms_get_unused_block(chip, seg_no);
2754                         if (phy_blk == 0xFFFF) {
2755                                 chip->card_wp |= MS_CARD;
2756                                 return STATUS_SUCCESS;
2757                         }
2758                         retval = ms_init_page(chip, phy_blk, log_blk, 0, 1);
2759                         if (retval != STATUS_SUCCESS) {
2760                                 rtsx_trace(chip);
2761                                 goto BUILD_FAIL;
2762                         }
2763
2764                         segment->l2p_table[idx] = phy_blk;
2765                         if (seg_no == ms_card->segment_cnt - 1) {
2766                                 if (segment->unused_blk_cnt < 2) {
2767                                         chip->card_wp |= MS_CARD;
2768                                         return STATUS_SUCCESS;
2769                                 }
2770                         } else {
2771                                 if (segment->unused_blk_cnt < 1) {
2772                                         chip->card_wp |= MS_CARD;
2773                                         return STATUS_SUCCESS;
2774                                 }
2775                         }
2776                 }
2777         }
2778
2779         /* Make boot block be the first normal block */
2780         if (seg_no == 0) {
2781                 for (log_blk = 0; log_blk < 494; log_blk++) {
2782                         tmp_blk = segment->l2p_table[log_blk];
2783                         if (tmp_blk < ms_card->boot_block) {
2784                                 dev_dbg(rtsx_dev(chip), "Boot block is not the first normal block.\n");
2785
2786                                 if (chip->card_wp & MS_CARD)
2787                                         break;
2788
2789                                 phy_blk = ms_get_unused_block(chip, 0);
2790                                 retval = ms_copy_page(chip, tmp_blk, phy_blk,
2791                                                       log_blk, 0,
2792                                                       ms_card->page_off + 1);
2793                                 if (retval != STATUS_SUCCESS) {
2794                                         rtsx_trace(chip);
2795                                         return STATUS_FAIL;
2796                                 }
2797
2798                                 segment->l2p_table[log_blk] = phy_blk;
2799
2800                                 retval = ms_set_bad_block(chip, tmp_blk);
2801                                 if (retval != STATUS_SUCCESS) {
2802                                         rtsx_trace(chip);
2803                                         return STATUS_FAIL;
2804                                 }
2805                         }
2806                 }
2807         }
2808
2809         return STATUS_SUCCESS;
2810
2811 BUILD_FAIL:
2812         segment->build_flag = 0;
2813         vfree(segment->l2p_table);
2814         segment->l2p_table = NULL;
2815         vfree(segment->free_table);
2816         segment->free_table = NULL;
2817
2818         return STATUS_FAIL;
2819 }
2820
2821 int reset_ms_card(struct rtsx_chip *chip)
2822 {
2823         struct ms_info *ms_card = &chip->ms_card;
2824         int seg_no = ms_card->total_block / 512 - 1;
2825         int retval;
2826
2827         memset(ms_card, 0, sizeof(struct ms_info));
2828
2829         retval = enable_card_clock(chip, MS_CARD);
2830         if (retval != STATUS_SUCCESS) {
2831                 rtsx_trace(chip);
2832                 return STATUS_FAIL;
2833         }
2834
2835         retval = select_card(chip, MS_CARD);
2836         if (retval != STATUS_SUCCESS) {
2837                 rtsx_trace(chip);
2838                 return STATUS_FAIL;
2839         }
2840
2841         ms_card->ms_type = 0;
2842
2843         retval = reset_ms_pro(chip);
2844         if (retval != STATUS_SUCCESS) {
2845                 if (ms_card->check_ms_flow) {
2846                         retval = reset_ms(chip);
2847                         if (retval != STATUS_SUCCESS) {
2848                                 rtsx_trace(chip);
2849                                 return STATUS_FAIL;
2850                         }
2851                 } else {
2852                         rtsx_trace(chip);
2853                         return STATUS_FAIL;
2854                 }
2855         }
2856
2857         retval = ms_set_init_para(chip);
2858         if (retval != STATUS_SUCCESS) {
2859                 rtsx_trace(chip);
2860                 return STATUS_FAIL;
2861         }
2862
2863         if (!CHK_MSPRO(ms_card)) {
2864                 /* Build table for the last segment,
2865                  * to check if L2P table block exists, erasing it
2866                  */
2867                 retval = ms_build_l2p_tbl(chip, seg_no);
2868                 if (retval != STATUS_SUCCESS) {
2869                         rtsx_trace(chip);
2870                         return STATUS_FAIL;
2871                 }
2872         }
2873
2874         dev_dbg(rtsx_dev(chip), "ms_card->ms_type = 0x%x\n", ms_card->ms_type);
2875
2876         return STATUS_SUCCESS;
2877 }
2878
2879 static int mspro_set_rw_cmd(struct rtsx_chip *chip,
2880                             u32 start_sec, u16 sec_cnt, u8 cmd)
2881 {
2882         int retval, i;
2883         u8 data[8];
2884
2885         data[0] = cmd;
2886         data[1] = (u8)(sec_cnt >> 8);
2887         data[2] = (u8)sec_cnt;
2888         data[3] = (u8)(start_sec >> 24);
2889         data[4] = (u8)(start_sec >> 16);
2890         data[5] = (u8)(start_sec >> 8);
2891         data[6] = (u8)start_sec;
2892         data[7] = 0;
2893
2894         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
2895                 retval = ms_write_bytes(chip, PRO_EX_SET_CMD, 7,
2896                                         WAIT_INT, data, 8);
2897                 if (retval == STATUS_SUCCESS)
2898                         break;
2899         }
2900         if (i == MS_MAX_RETRY_COUNT) {
2901                 rtsx_trace(chip);
2902                 return STATUS_FAIL;
2903         }
2904
2905         return STATUS_SUCCESS;
2906 }
2907
2908 void mspro_stop_seq_mode(struct rtsx_chip *chip)
2909 {
2910         struct ms_info *ms_card = &chip->ms_card;
2911         int retval;
2912
2913         if (ms_card->seq_mode) {
2914                 retval = ms_switch_clock(chip);
2915                 if (retval != STATUS_SUCCESS)
2916                         return;
2917
2918                 ms_card->seq_mode = 0;
2919                 ms_card->total_sec_cnt = 0;
2920                 ms_send_cmd(chip, PRO_STOP, WAIT_INT);
2921
2922                 rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
2923         }
2924 }
2925
2926 static inline int ms_auto_tune_clock(struct rtsx_chip *chip)
2927 {
2928         struct ms_info *ms_card = &chip->ms_card;
2929         int retval;
2930
2931         if (chip->asic_code) {
2932                 if (ms_card->ms_clock > 30)
2933                         ms_card->ms_clock -= 20;
2934         } else {
2935                 if (ms_card->ms_clock == CLK_80)
2936                         ms_card->ms_clock = CLK_60;
2937                 else if (ms_card->ms_clock == CLK_60)
2938                         ms_card->ms_clock = CLK_40;
2939         }
2940
2941         retval = ms_switch_clock(chip);
2942         if (retval != STATUS_SUCCESS) {
2943                 rtsx_trace(chip);
2944                 return STATUS_FAIL;
2945         }
2946
2947         return STATUS_SUCCESS;
2948 }
2949
2950 static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
2951                                  struct rtsx_chip *chip, u32 start_sector,
2952                                  u16 sector_cnt)
2953 {
2954         struct ms_info *ms_card = &chip->ms_card;
2955         bool mode_2k = false;
2956         int retval;
2957         u16 count;
2958         u8 val, trans_mode, rw_tpc, rw_cmd;
2959
2960         ms_set_err_code(chip, MS_NO_ERROR);
2961
2962         ms_card->cleanup_counter = 0;
2963
2964         if (CHK_MSHG(ms_card)) {
2965                 if ((start_sector % 4) || (sector_cnt % 4)) {
2966                         if (srb->sc_data_direction == DMA_FROM_DEVICE) {
2967                                 rw_tpc = PRO_READ_LONG_DATA;
2968                                 rw_cmd = PRO_READ_DATA;
2969                         } else {
2970                                 rw_tpc = PRO_WRITE_LONG_DATA;
2971                                 rw_cmd = PRO_WRITE_DATA;
2972                         }
2973                 } else {
2974                         if (srb->sc_data_direction == DMA_FROM_DEVICE) {
2975                                 rw_tpc = PRO_READ_QUAD_DATA;
2976                                 rw_cmd = PRO_READ_2K_DATA;
2977                         } else {
2978                                 rw_tpc = PRO_WRITE_QUAD_DATA;
2979                                 rw_cmd = PRO_WRITE_2K_DATA;
2980                         }
2981                         mode_2k = true;
2982                 }
2983         } else {
2984                 if (srb->sc_data_direction == DMA_FROM_DEVICE) {
2985                         rw_tpc = PRO_READ_LONG_DATA;
2986                         rw_cmd = PRO_READ_DATA;
2987                 } else {
2988                         rw_tpc = PRO_WRITE_LONG_DATA;
2989                         rw_cmd = PRO_WRITE_DATA;
2990                 }
2991         }
2992
2993         retval = ms_switch_clock(chip);
2994         if (retval != STATUS_SUCCESS) {
2995                 rtsx_trace(chip);
2996                 return STATUS_FAIL;
2997         }
2998
2999         if (srb->sc_data_direction == DMA_FROM_DEVICE)
3000                 trans_mode = MS_TM_AUTO_READ;
3001         else
3002                 trans_mode = MS_TM_AUTO_WRITE;
3003
3004         retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
3005         if (retval) {
3006                 rtsx_trace(chip);
3007                 return retval;
3008         }
3009
3010         if (ms_card->seq_mode) {
3011                 if ((ms_card->pre_dir != srb->sc_data_direction) ||
3012                     ((ms_card->pre_sec_addr + ms_card->pre_sec_cnt) !=
3013                      start_sector) ||
3014                     (mode_2k && (ms_card->seq_mode & MODE_512_SEQ)) ||
3015                     (!mode_2k && (ms_card->seq_mode & MODE_2K_SEQ)) ||
3016                     !(val & MS_INT_BREQ) ||
3017                     ((ms_card->total_sec_cnt + sector_cnt) > 0xFE00)) {
3018                         ms_card->seq_mode = 0;
3019                         ms_card->total_sec_cnt = 0;
3020                         if (val & MS_INT_BREQ) {
3021                                 retval = ms_send_cmd(chip, PRO_STOP, WAIT_INT);
3022                                 if (retval != STATUS_SUCCESS) {
3023                                         rtsx_trace(chip);
3024                                         return STATUS_FAIL;
3025                                 }
3026
3027                                 rtsx_write_register(chip, RBCTL, RB_FLUSH,
3028                                                     RB_FLUSH);
3029                         }
3030                 }
3031         }
3032
3033         if (!ms_card->seq_mode) {
3034                 ms_card->total_sec_cnt = 0;
3035                 if (sector_cnt >= SEQ_START_CRITERIA) {
3036                         if ((ms_card->capacity - start_sector) > 0xFE00)
3037                                 count = 0xFE00;
3038                         else
3039                                 count = (u16)(ms_card->capacity - start_sector);
3040
3041                         if (count > sector_cnt) {
3042                                 if (mode_2k)
3043                                         ms_card->seq_mode = MODE_2K_SEQ;
3044                                 else
3045                                         ms_card->seq_mode = MODE_512_SEQ;
3046                         }
3047                 } else {
3048                         count = sector_cnt;
3049                 }
3050                 retval = mspro_set_rw_cmd(chip, start_sector, count, rw_cmd);
3051                 if (retval != STATUS_SUCCESS) {
3052                         ms_card->seq_mode = 0;
3053                         rtsx_trace(chip);
3054                         return STATUS_FAIL;
3055                 }
3056         }
3057
3058         retval = ms_transfer_data(chip, trans_mode, rw_tpc, sector_cnt,
3059                                   WAIT_INT, mode_2k, scsi_sg_count(srb),
3060                                   scsi_sglist(srb), scsi_bufflen(srb));
3061         if (retval != STATUS_SUCCESS) {
3062                 ms_card->seq_mode = 0;
3063                 rtsx_read_register(chip, MS_TRANS_CFG, &val);
3064                 rtsx_clear_ms_error(chip);
3065
3066                 if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
3067                         chip->rw_need_retry = 0;
3068                         dev_dbg(rtsx_dev(chip), "No card exist, exit %s\n",
3069                                 __func__);
3070                         rtsx_trace(chip);
3071                         return STATUS_FAIL;
3072                 }
3073
3074                 if (val & MS_INT_BREQ)
3075                         ms_send_cmd(chip, PRO_STOP, WAIT_INT);
3076
3077                 if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
3078                         dev_dbg(rtsx_dev(chip), "MSPro CRC error, tune clock!\n");
3079                         chip->rw_need_retry = 1;
3080                         ms_auto_tune_clock(chip);
3081                 }
3082
3083                 rtsx_trace(chip);
3084                 return retval;
3085         }
3086
3087         if (ms_card->seq_mode) {
3088                 ms_card->pre_sec_addr = start_sector;
3089                 ms_card->pre_sec_cnt = sector_cnt;
3090                 ms_card->pre_dir = srb->sc_data_direction;
3091                 ms_card->total_sec_cnt += sector_cnt;
3092         }
3093
3094         return STATUS_SUCCESS;
3095 }
3096
3097 static int mspro_read_format_progress(struct rtsx_chip *chip,
3098                                       const int short_data_len)
3099 {
3100         struct ms_info *ms_card = &chip->ms_card;
3101         int retval, i;
3102         u32 total_progress, cur_progress;
3103         u8 cnt, tmp;
3104         u8 data[8];
3105
3106         dev_dbg(rtsx_dev(chip), "%s, short_data_len = %d\n", __func__,
3107                 short_data_len);
3108
3109         retval = ms_switch_clock(chip);
3110         if (retval != STATUS_SUCCESS) {
3111                 ms_card->format_status = FORMAT_FAIL;
3112                 rtsx_trace(chip);
3113                 return STATUS_FAIL;
3114         }
3115
3116         retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
3117         if (retval != STATUS_SUCCESS) {
3118                 ms_card->format_status = FORMAT_FAIL;
3119                 rtsx_trace(chip);
3120                 return STATUS_FAIL;
3121         }
3122
3123         if (!(tmp & MS_INT_BREQ)) {
3124                 if ((tmp & (MS_INT_CED | MS_INT_BREQ | MS_INT_CMDNK |
3125                             MS_INT_ERR)) == MS_INT_CED) {
3126                         ms_card->format_status = FORMAT_SUCCESS;
3127                         return STATUS_SUCCESS;
3128                 }
3129                 ms_card->format_status = FORMAT_FAIL;
3130                 rtsx_trace(chip);
3131                 return STATUS_FAIL;
3132         }
3133
3134         if (short_data_len >= 256)
3135                 cnt = 0;
3136         else
3137                 cnt = (u8)short_data_len;
3138
3139         retval = rtsx_write_register(chip, MS_CFG, MS_NO_CHECK_INT,
3140                                      MS_NO_CHECK_INT);
3141         if (retval != STATUS_SUCCESS) {
3142                 ms_card->format_status = FORMAT_FAIL;
3143                 rtsx_trace(chip);
3144                 return STATUS_FAIL;
3145         }
3146
3147         retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, cnt, WAIT_INT,
3148                                data, 8);
3149         if (retval != STATUS_SUCCESS) {
3150                 ms_card->format_status = FORMAT_FAIL;
3151                 rtsx_trace(chip);
3152                 return STATUS_FAIL;
3153         }
3154
3155         total_progress = (data[0] << 24) | (data[1] << 16) |
3156                 (data[2] << 8) | data[3];
3157         cur_progress = (data[4] << 24) | (data[5] << 16) |
3158                 (data[6] << 8) | data[7];
3159
3160         dev_dbg(rtsx_dev(chip), "total_progress = %d, cur_progress = %d\n",
3161                 total_progress, cur_progress);
3162
3163         if (total_progress == 0) {
3164                 ms_card->progress = 0;
3165         } else {
3166                 u64 ulltmp = (u64)cur_progress * (u64)65535;
3167
3168                 do_div(ulltmp, total_progress);
3169                 ms_card->progress = (u16)ulltmp;
3170         }
3171         dev_dbg(rtsx_dev(chip), "progress = %d\n", ms_card->progress);
3172
3173         for (i = 0; i < 5000; i++) {
3174                 retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
3175                 if (retval != STATUS_SUCCESS) {
3176                         ms_card->format_status = FORMAT_FAIL;
3177                         rtsx_trace(chip);
3178                         return STATUS_FAIL;
3179                 }
3180                 if (tmp & (MS_INT_CED | MS_INT_CMDNK |
3181                                 MS_INT_BREQ | MS_INT_ERR))
3182                         break;
3183
3184                 wait_timeout(1);
3185         }
3186
3187         retval = rtsx_write_register(chip, MS_CFG, MS_NO_CHECK_INT, 0);
3188         if (retval != STATUS_SUCCESS) {
3189                 ms_card->format_status = FORMAT_FAIL;
3190                 rtsx_trace(chip);
3191                 return STATUS_FAIL;
3192         }
3193
3194         if (i == 5000) {
3195                 ms_card->format_status = FORMAT_FAIL;
3196                 rtsx_trace(chip);
3197                 return STATUS_FAIL;
3198         }
3199
3200         if (tmp & (MS_INT_CMDNK | MS_INT_ERR)) {
3201                 ms_card->format_status = FORMAT_FAIL;
3202                 rtsx_trace(chip);
3203                 return STATUS_FAIL;
3204         }
3205
3206         if (tmp & MS_INT_CED) {
3207                 ms_card->format_status = FORMAT_SUCCESS;
3208                 ms_card->pro_under_formatting = 0;
3209         } else if (tmp & MS_INT_BREQ) {
3210                 ms_card->format_status = FORMAT_IN_PROGRESS;
3211         } else {
3212                 ms_card->format_status = FORMAT_FAIL;
3213                 ms_card->pro_under_formatting = 0;
3214                 rtsx_trace(chip);
3215                 return STATUS_FAIL;
3216         }
3217
3218         return STATUS_SUCCESS;
3219 }
3220
3221 void mspro_polling_format_status(struct rtsx_chip *chip)
3222 {
3223         struct ms_info *ms_card = &chip->ms_card;
3224         int i;
3225
3226         if (ms_card->pro_under_formatting &&
3227             (rtsx_get_stat(chip) != RTSX_STAT_SS)) {
3228                 rtsx_set_stat(chip, RTSX_STAT_RUN);
3229
3230                 for (i = 0; i < 65535; i++) {
3231                         mspro_read_format_progress(chip, MS_SHORT_DATA_LEN);
3232                         if (ms_card->format_status != FORMAT_IN_PROGRESS)
3233                                 break;
3234                 }
3235         }
3236 }
3237
3238 int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
3239                  int short_data_len, bool quick_format)
3240 {
3241         struct ms_info *ms_card = &chip->ms_card;
3242         int retval, i;
3243         u8 buf[8], tmp;
3244         u16 para;
3245
3246         retval = ms_switch_clock(chip);
3247         if (retval != STATUS_SUCCESS) {
3248                 rtsx_trace(chip);
3249                 return STATUS_FAIL;
3250         }
3251
3252         retval = ms_set_rw_reg_addr(chip, 0x00, 0x00, Pro_TPCParm, 0x01);
3253         if (retval != STATUS_SUCCESS) {
3254                 rtsx_trace(chip);
3255                 return STATUS_FAIL;
3256         }
3257
3258         memset(buf, 0, 2);
3259         switch (short_data_len) {
3260         case 32:
3261                 buf[0] = 0;
3262                 break;
3263         case 64:
3264                 buf[0] = 1;
3265                 break;
3266         case 128:
3267                 buf[0] = 2;
3268                 break;
3269         case 256:
3270         default:
3271                 buf[0] = 3;
3272                 break;
3273         }
3274
3275         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
3276                 retval = ms_write_bytes(chip, PRO_WRITE_REG, 1,
3277                                         NO_WAIT_INT, buf, 2);
3278                 if (retval == STATUS_SUCCESS)
3279                         break;
3280         }
3281         if (i == MS_MAX_RETRY_COUNT) {
3282                 rtsx_trace(chip);
3283                 return STATUS_FAIL;
3284         }
3285
3286         if (quick_format)
3287                 para = 0x0000;
3288         else
3289                 para = 0x0001;
3290
3291         retval = mspro_set_rw_cmd(chip, 0, para, PRO_FORMAT);
3292         if (retval != STATUS_SUCCESS) {
3293                 rtsx_trace(chip);
3294                 return STATUS_FAIL;
3295         }
3296
3297         retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
3298         if (retval) {
3299                 rtsx_trace(chip);
3300                 return retval;
3301         }
3302
3303         if (tmp & (MS_INT_CMDNK | MS_INT_ERR)) {
3304                 rtsx_trace(chip);
3305                 return STATUS_FAIL;
3306         }
3307
3308         if ((tmp & (MS_INT_BREQ | MS_INT_CED)) == MS_INT_BREQ) {
3309                 ms_card->pro_under_formatting = 1;
3310                 ms_card->progress = 0;
3311                 ms_card->format_status = FORMAT_IN_PROGRESS;
3312                 return STATUS_SUCCESS;
3313         }
3314
3315         if (tmp & MS_INT_CED) {
3316                 ms_card->pro_under_formatting = 0;
3317                 ms_card->progress = 0;
3318                 ms_card->format_status = FORMAT_SUCCESS;
3319                 set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_NO_SENSE);
3320                 return STATUS_SUCCESS;
3321         }
3322
3323         rtsx_trace(chip);
3324         return STATUS_FAIL;
3325 }
3326
3327 static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk,
3328                                   u16 log_blk, u8 start_page, u8 end_page,
3329                                   u8 *buf, unsigned int *index,
3330                                   unsigned int *offset)
3331 {
3332         struct ms_info *ms_card = &chip->ms_card;
3333         int retval, i;
3334         u8 extra[MS_EXTRA_SIZE], page_addr, val, trans_cfg, data[6];
3335         u8 *ptr;
3336
3337         retval = ms_read_extra_data(chip, phy_blk, start_page,
3338                                     extra, MS_EXTRA_SIZE);
3339         if (retval == STATUS_SUCCESS) {
3340                 if ((extra[1] & 0x30) != 0x30) {
3341                         ms_set_err_code(chip, MS_FLASH_READ_ERROR);
3342                         rtsx_trace(chip);
3343                         return STATUS_FAIL;
3344                 }
3345         }
3346
3347         retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
3348                                     SystemParm, 6);
3349         if (retval != STATUS_SUCCESS) {
3350                 rtsx_trace(chip);
3351                 return STATUS_FAIL;
3352         }
3353
3354         if (CHK_MS4BIT(ms_card))
3355                 data[0] = 0x88;
3356         else
3357                 data[0] = 0x80;
3358
3359         data[1] = 0;
3360         data[2] = (u8)(phy_blk >> 8);
3361         data[3] = (u8)phy_blk;
3362         data[4] = 0;
3363         data[5] = start_page;
3364
3365         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
3366                 retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT,
3367                                         data, 6);
3368                 if (retval == STATUS_SUCCESS)
3369                         break;
3370         }
3371         if (i == MS_MAX_RETRY_COUNT) {
3372                 rtsx_trace(chip);
3373                 return STATUS_FAIL;
3374         }
3375
3376         ms_set_err_code(chip, MS_NO_ERROR);
3377
3378         retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
3379         if (retval != STATUS_SUCCESS) {
3380                 rtsx_trace(chip);
3381                 return STATUS_FAIL;
3382         }
3383
3384         ptr = buf;
3385
3386         for (page_addr = start_page; page_addr < end_page; page_addr++) {
3387                 ms_set_err_code(chip, MS_NO_ERROR);
3388
3389                 if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
3390                         ms_set_err_code(chip, MS_NO_CARD);
3391                         rtsx_trace(chip);
3392                         return STATUS_FAIL;
3393                 }
3394
3395                 retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
3396                 if (retval != STATUS_SUCCESS) {
3397                         rtsx_trace(chip);
3398                         return STATUS_FAIL;
3399                 }
3400
3401                 if (val & INT_REG_CMDNK) {
3402                         ms_set_err_code(chip, MS_CMD_NK);
3403                         rtsx_trace(chip);
3404                         return STATUS_FAIL;
3405                 }
3406                 if (val & INT_REG_ERR) {
3407                         if (val & INT_REG_BREQ) {
3408                                 retval = ms_read_status_reg(chip);
3409                                 if (retval != STATUS_SUCCESS) {
3410                                         if (!(chip->card_wp & MS_CARD)) {
3411                                                 reset_ms(chip);
3412                                                 ms_set_page_status
3413                                                         (log_blk, setPS_NG,
3414                                                          extra,
3415                                                          MS_EXTRA_SIZE);
3416                                                 ms_write_extra_data
3417                                                         (chip, phy_blk,
3418                                                          page_addr, extra,
3419                                                          MS_EXTRA_SIZE);
3420                                         }
3421                                         ms_set_err_code(chip,
3422                                                         MS_FLASH_READ_ERROR);
3423                                         rtsx_trace(chip);
3424                                         return STATUS_FAIL;
3425                                 }
3426                         } else {
3427                                 ms_set_err_code(chip, MS_FLASH_READ_ERROR);
3428                                 rtsx_trace(chip);
3429                                 return STATUS_FAIL;
3430                         }
3431                 } else {
3432                         if (!(val & INT_REG_BREQ)) {
3433                                 ms_set_err_code(chip, MS_BREQ_ERROR);
3434                                 rtsx_trace(chip);
3435                                 return STATUS_FAIL;
3436                         }
3437                 }
3438
3439                 if (page_addr == (end_page - 1)) {
3440                         if (!(val & INT_REG_CED)) {
3441                                 retval = ms_send_cmd(chip, BLOCK_END, WAIT_INT);
3442                                 if (retval != STATUS_SUCCESS) {
3443                                         rtsx_trace(chip);
3444                                         return STATUS_FAIL;
3445                                 }
3446                         }
3447
3448                         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT,
3449                                                &val, 1);
3450                         if (retval != STATUS_SUCCESS) {
3451                                 rtsx_trace(chip);
3452                                 return STATUS_FAIL;
3453                         }
3454
3455                         if (!(val & INT_REG_CED)) {
3456                                 ms_set_err_code(chip, MS_FLASH_READ_ERROR);
3457                                 rtsx_trace(chip);
3458                                 return STATUS_FAIL;
3459                         }
3460
3461                         trans_cfg = NO_WAIT_INT;
3462                 } else {
3463                         trans_cfg = WAIT_INT;
3464                 }
3465
3466                 rtsx_init_cmd(chip);
3467
3468                 rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, READ_PAGE_DATA);
3469                 rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG,
3470                              0xFF, trans_cfg);
3471                 rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
3472                              0x01, RING_BUFFER);
3473
3474                 trans_dma_enable(DMA_FROM_DEVICE, chip, 512, DMA_512);
3475
3476                 rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
3477                              MS_TRANSFER_START |  MS_TM_NORMAL_READ);
3478                 rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
3479                              MS_TRANSFER_END, MS_TRANSFER_END);
3480
3481                 rtsx_send_cmd_no_wait(chip);
3482
3483                 retval = rtsx_transfer_data_partial(chip, MS_CARD, ptr, 512,
3484                                                     scsi_sg_count(chip->srb),
3485                                                     index, offset,
3486                                                     DMA_FROM_DEVICE,
3487                                                     chip->ms_timeout);
3488                 if (retval < 0) {
3489                         if (retval == -ETIMEDOUT) {
3490                                 ms_set_err_code(chip, MS_TO_ERROR);
3491                                 rtsx_clear_ms_error(chip);
3492                                 rtsx_trace(chip);
3493                                 return STATUS_TIMEDOUT;
3494                         }
3495
3496                         retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
3497                         if (retval != STATUS_SUCCESS) {
3498                                 ms_set_err_code(chip, MS_TO_ERROR);
3499                                 rtsx_clear_ms_error(chip);
3500                                 rtsx_trace(chip);
3501                                 return STATUS_TIMEDOUT;
3502                         }
3503                         if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
3504                                 ms_set_err_code(chip, MS_CRC16_ERROR);
3505                                 rtsx_clear_ms_error(chip);
3506                                 rtsx_trace(chip);
3507                                 return STATUS_FAIL;
3508                         }
3509                 }
3510
3511                 if (scsi_sg_count(chip->srb) == 0)
3512                         ptr += 512;
3513         }
3514
3515         return STATUS_SUCCESS;
3516 }
3517
3518 static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk,
3519                                    u16 new_blk, u16 log_blk, u8 start_page,
3520                                 u8 end_page, u8 *buf, unsigned int *index,
3521                                 unsigned int *offset)
3522 {
3523         struct ms_info *ms_card = &chip->ms_card;
3524         int retval, i;
3525         u8 page_addr, val, data[16];
3526         u8 *ptr;
3527
3528         if (!start_page) {
3529                 retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
3530                                             SystemParm, 7);
3531                 if (retval != STATUS_SUCCESS) {
3532                         rtsx_trace(chip);
3533                         return STATUS_FAIL;
3534                 }
3535
3536                 if (CHK_MS4BIT(ms_card))
3537                         data[0] = 0x88;
3538                 else
3539                         data[0] = 0x80;
3540
3541                 data[1] = 0;
3542                 data[2] = (u8)(old_blk >> 8);
3543                 data[3] = (u8)old_blk;
3544                 data[4] = 0x80;
3545                 data[5] = 0;
3546                 data[6] = 0xEF;
3547                 data[7] = 0xFF;
3548
3549                 retval = ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT,
3550                                         data, 8);
3551                 if (retval != STATUS_SUCCESS) {
3552                         rtsx_trace(chip);
3553                         return STATUS_FAIL;
3554                 }
3555
3556                 retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
3557                 if (retval != STATUS_SUCCESS) {
3558                         rtsx_trace(chip);
3559                         return STATUS_FAIL;
3560                 }
3561
3562                 ms_set_err_code(chip, MS_NO_ERROR);
3563                 retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT, 1,
3564                                          NO_WAIT_INT);
3565                 if (retval != STATUS_SUCCESS) {
3566                         rtsx_trace(chip);
3567                         return STATUS_FAIL;
3568                 }
3569         }
3570
3571         retval = ms_set_rw_reg_addr(chip, OverwriteFlag, MS_EXTRA_SIZE,
3572                                     SystemParm, (6 + MS_EXTRA_SIZE));
3573         if (retval != STATUS_SUCCESS) {
3574                 rtsx_trace(chip);
3575                 return STATUS_FAIL;
3576         }
3577
3578         ms_set_err_code(chip, MS_NO_ERROR);
3579
3580         if (CHK_MS4BIT(ms_card))
3581                 data[0] = 0x88;
3582         else
3583                 data[0] = 0x80;
3584
3585         data[1] = 0;
3586         data[2] = (u8)(new_blk >> 8);
3587         data[3] = (u8)new_blk;
3588         if ((end_page - start_page) == 1)
3589                 data[4] = 0x20;
3590         else
3591                 data[4] = 0;
3592
3593         data[5] = start_page;
3594         data[6] = 0xF8;
3595         data[7] = 0xFF;
3596         data[8] = (u8)(log_blk >> 8);
3597         data[9] = (u8)log_blk;
3598
3599         for (i = 0x0A; i < 0x10; i++)
3600                 data[i] = 0xFF;
3601
3602         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
3603                 retval = ms_write_bytes(chip, WRITE_REG, 6 + MS_EXTRA_SIZE,
3604                                         NO_WAIT_INT, data, 16);
3605                 if (retval == STATUS_SUCCESS)
3606                         break;
3607         }
3608         if (i == MS_MAX_RETRY_COUNT) {
3609                 rtsx_trace(chip);
3610                 return STATUS_FAIL;
3611         }
3612
3613         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
3614                 retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
3615                 if (retval == STATUS_SUCCESS)
3616                         break;
3617         }
3618         if (i == MS_MAX_RETRY_COUNT) {
3619                 rtsx_trace(chip);
3620                 return STATUS_FAIL;
3621         }
3622
3623         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
3624         if (retval != STATUS_SUCCESS) {
3625                 rtsx_trace(chip);
3626                 return STATUS_FAIL;
3627         }
3628
3629         ptr = buf;
3630         for (page_addr = start_page; page_addr < end_page; page_addr++) {
3631                 ms_set_err_code(chip, MS_NO_ERROR);
3632
3633                 if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
3634                         ms_set_err_code(chip, MS_NO_CARD);
3635                         rtsx_trace(chip);
3636                         return STATUS_FAIL;
3637                 }
3638
3639                 if (val & INT_REG_CMDNK) {
3640                         ms_set_err_code(chip, MS_CMD_NK);
3641                         rtsx_trace(chip);
3642                         return STATUS_FAIL;
3643                 }
3644                 if (val & INT_REG_ERR) {
3645                         ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
3646                         rtsx_trace(chip);
3647                         return STATUS_FAIL;
3648                 }
3649                 if (!(val & INT_REG_BREQ)) {
3650                         ms_set_err_code(chip, MS_BREQ_ERROR);
3651                         rtsx_trace(chip);
3652                         return STATUS_FAIL;
3653                 }
3654
3655                 udelay(30);
3656
3657                 rtsx_init_cmd(chip);
3658
3659                 rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC,
3660                              0xFF, WRITE_PAGE_DATA);
3661                 rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG,
3662                              0xFF, WAIT_INT);
3663                 rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
3664                              0x01, RING_BUFFER);
3665
3666                 trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
3667
3668                 rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
3669                              MS_TRANSFER_START |  MS_TM_NORMAL_WRITE);
3670                 rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
3671                              MS_TRANSFER_END, MS_TRANSFER_END);
3672
3673                 rtsx_send_cmd_no_wait(chip);
3674
3675                 retval = rtsx_transfer_data_partial(chip, MS_CARD, ptr, 512,
3676                                                     scsi_sg_count(chip->srb),
3677                                                     index, offset,
3678                                                     DMA_TO_DEVICE,
3679                                                     chip->ms_timeout);
3680                 if (retval < 0) {
3681                         ms_set_err_code(chip, MS_TO_ERROR);
3682                         rtsx_clear_ms_error(chip);
3683
3684                         if (retval == -ETIMEDOUT) {
3685                                 rtsx_trace(chip);
3686                                 return STATUS_TIMEDOUT;
3687                         }
3688                         rtsx_trace(chip);
3689                         return STATUS_FAIL;
3690                 }
3691
3692                 retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
3693                 if (retval != STATUS_SUCCESS) {
3694                         rtsx_trace(chip);
3695                         return STATUS_FAIL;
3696                 }
3697
3698                 if ((end_page - start_page) == 1) {
3699                         if (!(val & INT_REG_CED)) {
3700                                 ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
3701                                 rtsx_trace(chip);
3702                                 return STATUS_FAIL;
3703                         }
3704                 } else {
3705                         if (page_addr == (end_page - 1)) {
3706                                 if (!(val & INT_REG_CED)) {
3707                                         retval = ms_send_cmd(chip, BLOCK_END,
3708                                                              WAIT_INT);
3709                                         if (retval != STATUS_SUCCESS) {
3710                                                 rtsx_trace(chip);
3711                                                 return STATUS_FAIL;
3712                                         }
3713                                 }
3714
3715                                 retval = ms_read_bytes(chip, GET_INT, 1,
3716                                                        NO_WAIT_INT, &val, 1);
3717                                 if (retval != STATUS_SUCCESS) {
3718                                         rtsx_trace(chip);
3719                                         return STATUS_FAIL;
3720                                 }
3721                         }
3722
3723                         if ((page_addr == (end_page - 1)) ||
3724                             (page_addr == ms_card->page_off)) {
3725                                 if (!(val & INT_REG_CED)) {
3726                                         ms_set_err_code(chip,
3727                                                         MS_FLASH_WRITE_ERROR);
3728                                         rtsx_trace(chip);
3729                                         return STATUS_FAIL;
3730                                 }
3731                         }
3732                 }
3733
3734                 if (scsi_sg_count(chip->srb) == 0)
3735                         ptr += 512;
3736         }
3737
3738         return STATUS_SUCCESS;
3739 }
3740
3741 static int ms_finish_write(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
3742                            u16 log_blk, u8 page_off)
3743 {
3744         struct ms_info *ms_card = &chip->ms_card;
3745         int retval, seg_no;
3746
3747         retval = ms_copy_page(chip, old_blk, new_blk, log_blk,
3748                               page_off, ms_card->page_off + 1);
3749         if (retval != STATUS_SUCCESS) {
3750                 rtsx_trace(chip);
3751                 return STATUS_FAIL;
3752         }
3753
3754         seg_no = old_blk >> 9;
3755
3756         if (MS_TST_BAD_BLOCK_FLG(ms_card)) {
3757                 MS_CLR_BAD_BLOCK_FLG(ms_card);
3758                 ms_set_bad_block(chip, old_blk);
3759         } else {
3760                 retval = ms_erase_block(chip, old_blk);
3761                 if (retval == STATUS_SUCCESS)
3762                         ms_set_unused_block(chip, old_blk);
3763         }
3764
3765         ms_set_l2p_tbl(chip, seg_no, log_blk - ms_start_idx[seg_no], new_blk);
3766
3767         return STATUS_SUCCESS;
3768 }
3769
3770 static int ms_prepare_write(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
3771                             u16 log_blk, u8 start_page)
3772 {
3773         int retval;
3774
3775         if (start_page) {
3776                 retval = ms_copy_page(chip, old_blk, new_blk, log_blk,
3777                                       0, start_page);
3778                 if (retval != STATUS_SUCCESS) {
3779                         rtsx_trace(chip);
3780                         return STATUS_FAIL;
3781                 }
3782         }
3783
3784         return STATUS_SUCCESS;
3785 }
3786
3787 #ifdef MS_DELAY_WRITE
3788 int ms_delay_write(struct rtsx_chip *chip)
3789 {
3790         struct ms_info *ms_card = &chip->ms_card;
3791         struct ms_delay_write_tag *delay_write = &ms_card->delay_write;
3792         int retval;
3793
3794         if (delay_write->delay_write_flag) {
3795                 retval = ms_set_init_para(chip);
3796                 if (retval != STATUS_SUCCESS) {
3797                         rtsx_trace(chip);
3798                         return STATUS_FAIL;
3799                 }
3800
3801                 delay_write->delay_write_flag = 0;
3802                 retval = ms_finish_write(chip,
3803                                          delay_write->old_phyblock,
3804                                         delay_write->new_phyblock,
3805                                         delay_write->logblock,
3806                                         delay_write->pageoff);
3807                 if (retval != STATUS_SUCCESS) {
3808                         rtsx_trace(chip);
3809                         return STATUS_FAIL;
3810                 }
3811         }
3812
3813         return STATUS_SUCCESS;
3814 }
3815 #endif
3816
3817 static inline void ms_rw_fail(struct scsi_cmnd *srb, struct rtsx_chip *chip)
3818 {
3819         if (srb->sc_data_direction == DMA_FROM_DEVICE)
3820                 set_sense_type(chip, SCSI_LUN(srb),
3821                                SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
3822         else
3823                 set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
3824 }
3825
3826 static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
3827                               u32 start_sector, u16 sector_cnt)
3828 {
3829         struct ms_info *ms_card = &chip->ms_card;
3830         unsigned int lun = SCSI_LUN(srb);
3831         int retval, seg_no;
3832         unsigned int index = 0, offset = 0;
3833         u16 old_blk = 0, new_blk = 0, log_blk, total_sec_cnt = sector_cnt;
3834         u8 start_page, end_page = 0, page_cnt;
3835         u8 *ptr;
3836 #ifdef MS_DELAY_WRITE
3837         struct ms_delay_write_tag *delay_write = &ms_card->delay_write;
3838 #endif
3839
3840         ms_set_err_code(chip, MS_NO_ERROR);
3841
3842         ms_card->cleanup_counter = 0;
3843
3844         ptr = (u8 *)scsi_sglist(srb);
3845
3846         retval = ms_switch_clock(chip);
3847         if (retval != STATUS_SUCCESS) {
3848                 ms_rw_fail(srb, chip);
3849                 rtsx_trace(chip);
3850                 return STATUS_FAIL;
3851         }
3852
3853         log_blk = (u16)(start_sector >> ms_card->block_shift);
3854         start_page = (u8)(start_sector & ms_card->page_off);
3855
3856         for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1; seg_no++) {
3857                 if (log_blk < ms_start_idx[seg_no + 1])
3858                         break;
3859         }
3860
3861         if (ms_card->segment[seg_no].build_flag == 0) {
3862                 retval = ms_build_l2p_tbl(chip, seg_no);
3863                 if (retval != STATUS_SUCCESS) {
3864                         chip->card_fail |= MS_CARD;
3865                         set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
3866                         rtsx_trace(chip);
3867                         return STATUS_FAIL;
3868                 }
3869         }
3870
3871         if (srb->sc_data_direction == DMA_TO_DEVICE) {
3872 #ifdef MS_DELAY_WRITE
3873                 if (delay_write->delay_write_flag &&
3874                     (delay_write->logblock == log_blk) &&
3875                     (start_page > delay_write->pageoff)) {
3876                         delay_write->delay_write_flag = 0;
3877                         retval = ms_copy_page(chip,
3878                                               delay_write->old_phyblock,
3879                                               delay_write->new_phyblock,
3880                                               log_blk,
3881                                               delay_write->pageoff, start_page);
3882                         if (retval != STATUS_SUCCESS) {
3883                                 set_sense_type(chip, lun,
3884                                                SENSE_TYPE_MEDIA_WRITE_ERR);
3885                                 rtsx_trace(chip);
3886                                 return STATUS_FAIL;
3887                         }
3888                         old_blk = delay_write->old_phyblock;
3889                         new_blk = delay_write->new_phyblock;
3890                 } else if (delay_write->delay_write_flag &&
3891                                 (delay_write->logblock == log_blk) &&
3892                                 (start_page == delay_write->pageoff)) {
3893                         delay_write->delay_write_flag = 0;
3894                         old_blk = delay_write->old_phyblock;
3895                         new_blk = delay_write->new_phyblock;
3896                 } else {
3897                         retval = ms_delay_write(chip);
3898                         if (retval != STATUS_SUCCESS) {
3899                                 set_sense_type(chip, lun,
3900                                                SENSE_TYPE_MEDIA_WRITE_ERR);
3901                                 rtsx_trace(chip);
3902                                 return STATUS_FAIL;
3903                         }
3904 #endif
3905                         old_blk = ms_get_l2p_tbl
3906                                         (chip, seg_no,
3907                                          log_blk - ms_start_idx[seg_no]);
3908                         new_blk  = ms_get_unused_block(chip, seg_no);
3909                         if ((old_blk == 0xFFFF) || (new_blk == 0xFFFF)) {
3910                                 set_sense_type(chip, lun,
3911                                                SENSE_TYPE_MEDIA_WRITE_ERR);
3912                                 rtsx_trace(chip);
3913                                 return STATUS_FAIL;
3914                         }
3915
3916                         retval = ms_prepare_write(chip, old_blk, new_blk,
3917                                                   log_blk, start_page);
3918                         if (retval != STATUS_SUCCESS) {
3919                                 if (detect_card_cd(chip, MS_CARD) !=
3920                                     STATUS_SUCCESS) {
3921                                         set_sense_type
3922                                                 (chip, lun,
3923                                                 SENSE_TYPE_MEDIA_NOT_PRESENT);
3924                                         rtsx_trace(chip);
3925                                         return STATUS_FAIL;
3926                                 }
3927                                 set_sense_type(chip, lun,
3928                                                SENSE_TYPE_MEDIA_WRITE_ERR);
3929                                 rtsx_trace(chip);
3930                                 return STATUS_FAIL;
3931                         }
3932 #ifdef MS_DELAY_WRITE
3933                 }
3934 #endif
3935         } else {
3936 #ifdef MS_DELAY_WRITE
3937                 retval = ms_delay_write(chip);
3938                 if (retval != STATUS_SUCCESS) {
3939                         if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
3940                                 set_sense_type(chip, lun,
3941                                                SENSE_TYPE_MEDIA_NOT_PRESENT);
3942                                 rtsx_trace(chip);
3943                                 return STATUS_FAIL;
3944                         }
3945                         set_sense_type(chip, lun,
3946                                        SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
3947                         rtsx_trace(chip);
3948                         return STATUS_FAIL;
3949                 }
3950 #endif
3951                 old_blk = ms_get_l2p_tbl(chip, seg_no,
3952                                          log_blk - ms_start_idx[seg_no]);
3953                 if (old_blk == 0xFFFF) {
3954                         set_sense_type(chip, lun,
3955                                        SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
3956                         rtsx_trace(chip);
3957                         return STATUS_FAIL;
3958                 }
3959         }
3960
3961         dev_dbg(rtsx_dev(chip), "seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n",
3962                 seg_no, old_blk, new_blk);
3963
3964         while (total_sec_cnt) {
3965                 if ((start_page + total_sec_cnt) > (ms_card->page_off + 1))
3966                         end_page = ms_card->page_off + 1;
3967                 else
3968                         end_page = start_page + (u8)total_sec_cnt;
3969
3970                 page_cnt = end_page - start_page;
3971
3972                 dev_dbg(rtsx_dev(chip), "start_page = %d, end_page = %d, page_cnt = %d\n",
3973                         start_page, end_page, page_cnt);
3974
3975                 if (srb->sc_data_direction == DMA_FROM_DEVICE) {
3976                         retval = ms_read_multiple_pages(chip,
3977                                                         old_blk, log_blk,
3978                                                         start_page, end_page,
3979                                                         ptr, &index, &offset);
3980                 } else {
3981                         retval = ms_write_multiple_pages(chip, old_blk, new_blk,
3982                                                          log_blk, start_page,
3983                                                          end_page, ptr, &index,
3984                                                          &offset);
3985                 }
3986
3987                 if (retval != STATUS_SUCCESS) {
3988                         toggle_gpio(chip, 1);
3989                         if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
3990                                 set_sense_type(chip, lun,
3991                                                SENSE_TYPE_MEDIA_NOT_PRESENT);
3992                                 rtsx_trace(chip);
3993                                 return STATUS_FAIL;
3994                         }
3995                         ms_rw_fail(srb, chip);
3996                         rtsx_trace(chip);
3997                         return STATUS_FAIL;
3998                 }
3999
4000                 if (srb->sc_data_direction == DMA_TO_DEVICE) {
4001                         if (end_page == (ms_card->page_off + 1)) {
4002                                 retval = ms_erase_block(chip, old_blk);
4003                                 if (retval == STATUS_SUCCESS)
4004                                         ms_set_unused_block(chip, old_blk);
4005
4006                                 ms_set_l2p_tbl(chip, seg_no,
4007                                                log_blk - ms_start_idx[seg_no],
4008                                                new_blk);
4009                         }
4010                 }
4011
4012                 total_sec_cnt -= page_cnt;
4013                 if (scsi_sg_count(srb) == 0)
4014                         ptr += page_cnt * 512;
4015
4016                 if (total_sec_cnt == 0)
4017                         break;
4018
4019                 log_blk++;
4020
4021                 for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1;
4022                                 seg_no++) {
4023                         if (log_blk < ms_start_idx[seg_no + 1])
4024                                 break;
4025                 }
4026
4027                 if (ms_card->segment[seg_no].build_flag == 0) {
4028                         retval = ms_build_l2p_tbl(chip, seg_no);
4029                         if (retval != STATUS_SUCCESS) {
4030                                 chip->card_fail |= MS_CARD;
4031                                 set_sense_type(chip, lun,
4032                                                SENSE_TYPE_MEDIA_NOT_PRESENT);
4033                                 rtsx_trace(chip);
4034                                 return STATUS_FAIL;
4035                         }
4036                 }
4037
4038                 old_blk = ms_get_l2p_tbl(chip, seg_no,
4039                                          log_blk - ms_start_idx[seg_no]);
4040                 if (old_blk == 0xFFFF) {
4041                         ms_rw_fail(srb, chip);
4042                         rtsx_trace(chip);
4043                         return STATUS_FAIL;
4044                 }
4045
4046                 if (srb->sc_data_direction == DMA_TO_DEVICE) {
4047                         new_blk = ms_get_unused_block(chip, seg_no);
4048                         if (new_blk == 0xFFFF) {
4049                                 ms_rw_fail(srb, chip);
4050                                 rtsx_trace(chip);
4051                                 return STATUS_FAIL;
4052                         }
4053                 }
4054
4055                 dev_dbg(rtsx_dev(chip), "seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n",
4056                         seg_no, old_blk, new_blk);
4057
4058                 start_page = 0;
4059         }
4060
4061         if (srb->sc_data_direction == DMA_TO_DEVICE) {
4062                 if (end_page < (ms_card->page_off + 1)) {
4063 #ifdef MS_DELAY_WRITE
4064                         delay_write->delay_write_flag = 1;
4065                         delay_write->old_phyblock = old_blk;
4066                         delay_write->new_phyblock = new_blk;
4067                         delay_write->logblock = log_blk;
4068                         delay_write->pageoff = end_page;
4069 #else
4070                         retval = ms_finish_write(chip, old_blk, new_blk,
4071                                                  log_blk, end_page);
4072                         if (retval != STATUS_SUCCESS) {
4073                                 if (detect_card_cd(chip, MS_CARD) !=
4074                                     STATUS_SUCCESS) {
4075                                         set_sense_type
4076                                                 (chip, lun,
4077                                                 SENSE_TYPE_MEDIA_NOT_PRESENT);
4078                                         rtsx_trace(chip);
4079                                         return STATUS_FAIL;
4080                                 }
4081
4082                                 ms_rw_fail(srb, chip);
4083                                 rtsx_trace(chip);
4084                                 return STATUS_FAIL;
4085                         }
4086 #endif
4087                 }
4088         }
4089
4090         scsi_set_resid(srb, 0);
4091
4092         return STATUS_SUCCESS;
4093 }
4094
4095 int ms_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
4096           u32 start_sector, u16 sector_cnt)
4097 {
4098         struct ms_info *ms_card = &chip->ms_card;
4099         int retval;
4100
4101         if (CHK_MSPRO(ms_card))
4102                 retval = mspro_rw_multi_sector(srb, chip, start_sector,
4103                                                sector_cnt);
4104         else
4105                 retval = ms_rw_multi_sector(srb, chip, start_sector,
4106                                             sector_cnt);
4107
4108         return retval;
4109 }
4110
4111 void ms_free_l2p_tbl(struct rtsx_chip *chip)
4112 {
4113         struct ms_info *ms_card = &chip->ms_card;
4114         int i = 0;
4115
4116         if (ms_card->segment) {
4117                 for (i = 0; i < ms_card->segment_cnt; i++) {
4118                         vfree(ms_card->segment[i].l2p_table);
4119                         ms_card->segment[i].l2p_table = NULL;
4120                         vfree(ms_card->segment[i].free_table);
4121                         ms_card->segment[i].free_table = NULL;
4122                 }
4123                 vfree(ms_card->segment);
4124                 ms_card->segment = NULL;
4125         }
4126 }
4127
4128 #ifdef SUPPORT_MAGIC_GATE
4129
4130 #ifdef READ_BYTES_WAIT_INT
4131 static int ms_poll_int(struct rtsx_chip *chip)
4132 {
4133         int retval;
4134         u8 val;
4135
4136         rtsx_init_cmd(chip);
4137
4138         rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANS_CFG, MS_INT_CED, MS_INT_CED);
4139
4140         retval = rtsx_send_cmd(chip, MS_CARD, 5000);
4141         if (retval != STATUS_SUCCESS) {
4142                 rtsx_trace(chip);
4143                 return STATUS_FAIL;
4144         }
4145
4146         val = *rtsx_get_cmd_data(chip);
4147         if (val & MS_INT_ERR) {
4148                 rtsx_trace(chip);
4149                 return STATUS_FAIL;
4150         }
4151
4152         return STATUS_SUCCESS;
4153 }
4154 #endif
4155
4156 #ifdef MS_SAMPLE_INT_ERR
4157 static int check_ms_err(struct rtsx_chip *chip)
4158 {
4159         int retval;
4160         u8 val;
4161
4162         retval = rtsx_read_register(chip, MS_TRANSFER, &val);
4163         if (retval != STATUS_SUCCESS)
4164                 return 1;
4165         if (val & MS_TRANSFER_ERR)
4166                 return 1;
4167
4168         retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
4169         if (retval != STATUS_SUCCESS)
4170                 return 1;
4171
4172         if (val & (MS_INT_ERR | MS_INT_CMDNK))
4173                 return 1;
4174
4175         return 0;
4176 }
4177 #else
4178 static int check_ms_err(struct rtsx_chip *chip)
4179 {
4180         int retval;
4181         u8 val;
4182
4183         retval = rtsx_read_register(chip, MS_TRANSFER, &val);
4184         if (retval != STATUS_SUCCESS)
4185                 return 1;
4186         if (val & MS_TRANSFER_ERR)
4187                 return 1;
4188
4189         return 0;
4190 }
4191 #endif
4192
4193 static int mg_send_ex_cmd(struct rtsx_chip *chip, u8 cmd, u8 entry_num)
4194 {
4195         int retval, i;
4196         u8 data[8];
4197
4198         data[0] = cmd;
4199         data[1] = 0;
4200         data[2] = 0;
4201         data[3] = 0;
4202         data[4] = 0;
4203         data[5] = 0;
4204         data[6] = entry_num;
4205         data[7] = 0;
4206
4207         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
4208                 retval = ms_write_bytes(chip, PRO_EX_SET_CMD, 7, WAIT_INT,
4209                                         data, 8);
4210                 if (retval == STATUS_SUCCESS)
4211                         break;
4212         }
4213         if (i == MS_MAX_RETRY_COUNT) {
4214                 rtsx_trace(chip);
4215                 return STATUS_FAIL;
4216         }
4217
4218         if (check_ms_err(chip)) {
4219                 rtsx_clear_ms_error(chip);
4220                 rtsx_trace(chip);
4221                 return STATUS_FAIL;
4222         }
4223
4224         return STATUS_SUCCESS;
4225 }
4226
4227 static int mg_set_tpc_para_sub(struct rtsx_chip *chip, int type,
4228                                u8 mg_entry_num)
4229 {
4230         int retval;
4231         u8 buf[6];
4232
4233         if (type == 0)
4234                 retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_TPCParm, 1);
4235         else
4236                 retval = ms_set_rw_reg_addr(chip, 0, 0, Pro_DataCount1, 6);
4237
4238         if (retval != STATUS_SUCCESS) {
4239                 rtsx_trace(chip);
4240                 return STATUS_FAIL;
4241         }
4242
4243         buf[0] = 0;
4244         buf[1] = 0;
4245         if (type == 1) {
4246                 buf[2] = 0;
4247                 buf[3] = 0;
4248                 buf[4] = 0;
4249                 buf[5] = mg_entry_num;
4250         }
4251         retval = ms_write_bytes(chip, PRO_WRITE_REG, (type == 0) ? 1 : 6,
4252                                 NO_WAIT_INT, buf, 6);
4253         if (retval != STATUS_SUCCESS) {
4254                 rtsx_trace(chip);
4255                 return STATUS_FAIL;
4256         }
4257
4258         return STATUS_SUCCESS;
4259 }
4260
4261 int mg_set_leaf_id(struct scsi_cmnd *srb, struct rtsx_chip *chip)
4262 {
4263         int retval;
4264         int i;
4265         unsigned int lun = SCSI_LUN(srb);
4266         u8 buf1[32], buf2[12];
4267
4268         if (scsi_bufflen(srb) < 12) {
4269                 set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
4270                 rtsx_trace(chip);
4271                 return STATUS_FAIL;
4272         }
4273
4274         ms_cleanup_work(chip);
4275
4276         retval = ms_switch_clock(chip);
4277         if (retval != STATUS_SUCCESS) {
4278                 rtsx_trace(chip);
4279                 return STATUS_FAIL;
4280         }
4281
4282         retval = mg_send_ex_cmd(chip, MG_SET_LID, 0);
4283         if (retval != STATUS_SUCCESS) {
4284                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
4285                 rtsx_trace(chip);
4286                 return STATUS_FAIL;
4287         }
4288
4289         memset(buf1, 0, 32);
4290         rtsx_stor_get_xfer_buf(buf2, min_t(int, 12, scsi_bufflen(srb)), srb);
4291         for (i = 0; i < 8; i++)
4292                 buf1[8 + i] = buf2[4 + i];
4293
4294         retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT,
4295                                 buf1, 32);
4296         if (retval != STATUS_SUCCESS) {
4297                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
4298                 rtsx_trace(chip);
4299                 return STATUS_FAIL;
4300         }
4301         if (check_ms_err(chip)) {
4302                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
4303                 rtsx_clear_ms_error(chip);
4304                 rtsx_trace(chip);
4305                 return STATUS_FAIL;
4306         }
4307
4308         return STATUS_SUCCESS;
4309 }
4310
4311 int mg_get_local_EKB(struct scsi_cmnd *srb, struct rtsx_chip *chip)
4312 {
4313         int retval = STATUS_FAIL;
4314         int bufflen;
4315         unsigned int lun = SCSI_LUN(srb);
4316         u8 *buf = NULL;
4317
4318         ms_cleanup_work(chip);
4319
4320         retval = ms_switch_clock(chip);
4321         if (retval != STATUS_SUCCESS) {
4322                 rtsx_trace(chip);
4323                 return STATUS_FAIL;
4324         }
4325
4326         buf = kmalloc(1540, GFP_KERNEL);
4327         if (!buf) {
4328                 rtsx_trace(chip);
4329                 return STATUS_ERROR;
4330         }
4331
4332         buf[0] = 0x04;
4333         buf[1] = 0x1A;
4334         buf[2] = 0x00;
4335         buf[3] = 0x00;
4336
4337         retval = mg_send_ex_cmd(chip, MG_GET_LEKB, 0);
4338         if (retval != STATUS_SUCCESS) {
4339                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4340                 rtsx_trace(chip);
4341                 goto free_buffer;
4342         }
4343
4344         retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
4345                                   3, WAIT_INT, 0, 0, buf + 4, 1536);
4346         if (retval != STATUS_SUCCESS) {
4347                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4348                 rtsx_clear_ms_error(chip);
4349                 rtsx_trace(chip);
4350                 goto free_buffer;
4351         }
4352         if (check_ms_err(chip)) {
4353                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4354                 rtsx_clear_ms_error(chip);
4355                 rtsx_trace(chip);
4356                 retval = STATUS_FAIL;
4357                 goto free_buffer;
4358         }
4359
4360         bufflen = min_t(int, 1052, scsi_bufflen(srb));
4361         rtsx_stor_set_xfer_buf(buf, bufflen, srb);
4362
4363 free_buffer:
4364         kfree(buf);
4365         return retval;
4366 }
4367
4368 int mg_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
4369 {
4370         struct ms_info *ms_card = &chip->ms_card;
4371         int retval;
4372         int bufflen;
4373         int i;
4374         unsigned int lun = SCSI_LUN(srb);
4375         u8 buf[32];
4376
4377         ms_cleanup_work(chip);
4378
4379         retval = ms_switch_clock(chip);
4380         if (retval != STATUS_SUCCESS) {
4381                 rtsx_trace(chip);
4382                 return STATUS_FAIL;
4383         }
4384
4385         retval = mg_send_ex_cmd(chip, MG_GET_ID, 0);
4386         if (retval != STATUS_SUCCESS) {
4387                 set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
4388                 rtsx_trace(chip);
4389                 return STATUS_FAIL;
4390         }
4391
4392         retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT,
4393                                buf, 32);
4394         if (retval != STATUS_SUCCESS) {
4395                 set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
4396                 rtsx_trace(chip);
4397                 return STATUS_FAIL;
4398         }
4399         if (check_ms_err(chip)) {
4400                 set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
4401                 rtsx_clear_ms_error(chip);
4402                 rtsx_trace(chip);
4403                 return STATUS_FAIL;
4404         }
4405
4406         memcpy(ms_card->magic_gate_id, buf, 16);
4407
4408 #ifdef READ_BYTES_WAIT_INT
4409         retval = ms_poll_int(chip);
4410         if (retval != STATUS_SUCCESS) {
4411                 set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
4412                 rtsx_trace(chip);
4413                 return STATUS_FAIL;
4414         }
4415 #endif
4416
4417         retval = mg_send_ex_cmd(chip, MG_SET_RD, 0);
4418         if (retval != STATUS_SUCCESS) {
4419                 set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
4420                 rtsx_trace(chip);
4421                 return STATUS_FAIL;
4422         }
4423
4424         bufflen = min_t(int, 12, scsi_bufflen(srb));
4425         rtsx_stor_get_xfer_buf(buf, bufflen, srb);
4426
4427         for (i = 0; i < 8; i++)
4428                 buf[i] = buf[4 + i];
4429
4430         for (i = 0; i < 24; i++)
4431                 buf[8 + i] = 0;
4432
4433         retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA,
4434                                 32, WAIT_INT, buf, 32);
4435         if (retval != STATUS_SUCCESS) {
4436                 set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
4437                 rtsx_trace(chip);
4438                 return STATUS_FAIL;
4439         }
4440         if (check_ms_err(chip)) {
4441                 set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
4442                 rtsx_clear_ms_error(chip);
4443                 rtsx_trace(chip);
4444                 return STATUS_FAIL;
4445         }
4446
4447         ms_card->mg_auth = 0;
4448
4449         return STATUS_SUCCESS;
4450 }
4451
4452 int mg_get_rsp_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
4453 {
4454         struct ms_info *ms_card = &chip->ms_card;
4455         int retval;
4456         int bufflen;
4457         unsigned int lun = SCSI_LUN(srb);
4458         u8 buf1[32], buf2[36];
4459
4460         ms_cleanup_work(chip);
4461
4462         retval = ms_switch_clock(chip);
4463         if (retval != STATUS_SUCCESS) {
4464                 rtsx_trace(chip);
4465                 return STATUS_FAIL;
4466         }
4467
4468         retval = mg_send_ex_cmd(chip, MG_MAKE_RMS, 0);
4469         if (retval != STATUS_SUCCESS) {
4470                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4471                 rtsx_trace(chip);
4472                 return STATUS_FAIL;
4473         }
4474
4475         retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT,
4476                                buf1, 32);
4477         if (retval != STATUS_SUCCESS) {
4478                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4479                 rtsx_trace(chip);
4480                 return STATUS_FAIL;
4481         }
4482         if (check_ms_err(chip)) {
4483                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4484                 rtsx_clear_ms_error(chip);
4485                 rtsx_trace(chip);
4486                 return STATUS_FAIL;
4487         }
4488
4489         buf2[0] = 0x00;
4490         buf2[1] = 0x22;
4491         buf2[2] = 0x00;
4492         buf2[3] = 0x00;
4493
4494         memcpy(buf2 + 4, ms_card->magic_gate_id, 16);
4495         memcpy(buf2 + 20, buf1, 16);
4496
4497         bufflen = min_t(int, 36, scsi_bufflen(srb));
4498         rtsx_stor_set_xfer_buf(buf2, bufflen, srb);
4499
4500 #ifdef READ_BYTES_WAIT_INT
4501         retval = ms_poll_int(chip);
4502         if (retval != STATUS_SUCCESS) {
4503                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4504                 rtsx_trace(chip);
4505                 return STATUS_FAIL;
4506         }
4507 #endif
4508
4509         return STATUS_SUCCESS;
4510 }
4511
4512 int mg_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip)
4513 {
4514         struct ms_info *ms_card = &chip->ms_card;
4515         int retval;
4516         int i;
4517         int bufflen;
4518         unsigned int lun = SCSI_LUN(srb);
4519         u8 buf[32];
4520
4521         ms_cleanup_work(chip);
4522
4523         retval = ms_switch_clock(chip);
4524         if (retval != STATUS_SUCCESS) {
4525                 rtsx_trace(chip);
4526                 return STATUS_FAIL;
4527         }
4528
4529         retval = mg_send_ex_cmd(chip, MG_MAKE_KSE, 0);
4530         if (retval != STATUS_SUCCESS) {
4531                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4532                 rtsx_trace(chip);
4533                 return STATUS_FAIL;
4534         }
4535
4536         bufflen = min_t(int, 12, scsi_bufflen(srb));
4537         rtsx_stor_get_xfer_buf(buf, bufflen, srb);
4538
4539         for (i = 0; i < 8; i++)
4540                 buf[i] = buf[4 + i];
4541
4542         for (i = 0; i < 24; i++)
4543                 buf[8 + i] = 0;
4544
4545         retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT,
4546                                 buf, 32);
4547         if (retval != STATUS_SUCCESS) {
4548                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4549                 rtsx_trace(chip);
4550                 return STATUS_FAIL;
4551         }
4552         if (check_ms_err(chip)) {
4553                 set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4554                 rtsx_clear_ms_error(chip);
4555                 rtsx_trace(chip);
4556                 return STATUS_FAIL;
4557         }
4558
4559         ms_card->mg_auth = 1;
4560
4561         return STATUS_SUCCESS;
4562 }
4563
4564 int mg_get_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
4565 {
4566         struct ms_info *ms_card = &chip->ms_card;
4567         int retval;
4568         int bufflen;
4569         unsigned int lun = SCSI_LUN(srb);
4570         u8 *buf = NULL;
4571
4572         ms_cleanup_work(chip);
4573
4574         retval = ms_switch_clock(chip);
4575         if (retval != STATUS_SUCCESS) {
4576                 rtsx_trace(chip);
4577                 return STATUS_FAIL;
4578         }
4579
4580         buf = kmalloc(1028, GFP_KERNEL);
4581         if (!buf) {
4582                 rtsx_trace(chip);
4583                 return STATUS_ERROR;
4584         }
4585
4586         buf[0] = 0x04;
4587         buf[1] = 0x02;
4588         buf[2] = 0x00;
4589         buf[3] = 0x00;
4590
4591         retval = mg_send_ex_cmd(chip, MG_GET_IBD, ms_card->mg_entry_num);
4592         if (retval != STATUS_SUCCESS) {
4593                 set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
4594                 rtsx_trace(chip);
4595                 goto free_buffer;
4596         }
4597
4598         retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
4599                                   2, WAIT_INT, 0, 0, buf + 4, 1024);
4600         if (retval != STATUS_SUCCESS) {
4601                 set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
4602                 rtsx_clear_ms_error(chip);
4603                 rtsx_trace(chip);
4604                 goto free_buffer;
4605         }
4606         if (check_ms_err(chip)) {
4607                 set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
4608                 rtsx_clear_ms_error(chip);
4609                 rtsx_trace(chip);
4610                 retval = STATUS_FAIL;
4611                 goto free_buffer;
4612         }
4613
4614         bufflen = min_t(int, 1028, scsi_bufflen(srb));
4615         rtsx_stor_set_xfer_buf(buf, bufflen, srb);
4616
4617 free_buffer:
4618         kfree(buf);
4619         return retval;
4620 }
4621
4622 int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
4623 {
4624         struct ms_info *ms_card = &chip->ms_card;
4625         int retval;
4626         int bufflen;
4627 #ifdef MG_SET_ICV_SLOW
4628         int i;
4629 #endif
4630         unsigned int lun = SCSI_LUN(srb);
4631         u8 *buf = NULL;
4632
4633         ms_cleanup_work(chip);
4634
4635         retval = ms_switch_clock(chip);
4636         if (retval != STATUS_SUCCESS) {
4637                 rtsx_trace(chip);
4638                 return STATUS_FAIL;
4639         }
4640
4641         buf = kmalloc(1028, GFP_KERNEL);
4642         if (!buf) {
4643                 rtsx_trace(chip);
4644                 return STATUS_ERROR;
4645         }
4646
4647         bufflen = min_t(int, 1028, scsi_bufflen(srb));
4648         rtsx_stor_get_xfer_buf(buf, bufflen, srb);
4649
4650         retval = mg_send_ex_cmd(chip, MG_SET_IBD, ms_card->mg_entry_num);
4651         if (retval != STATUS_SUCCESS) {
4652                 if (ms_card->mg_auth == 0) {
4653                         if ((buf[5] & 0xC0) != 0)
4654                                 set_sense_type
4655                                         (chip, lun,
4656                                         SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
4657                         else
4658                                 set_sense_type(chip, lun,
4659                                                SENSE_TYPE_MG_WRITE_ERR);
4660                 } else {
4661                         set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
4662                 }
4663                 rtsx_trace(chip);
4664                 goto SetICVFinish;
4665         }
4666
4667 #ifdef MG_SET_ICV_SLOW
4668         for (i = 0; i < 2; i++) {
4669                 udelay(50);
4670
4671                 rtsx_init_cmd(chip);
4672
4673                 rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC,
4674                              0xFF, PRO_WRITE_LONG_DATA);
4675                 rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, WAIT_INT);
4676                 rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
4677                              0x01, RING_BUFFER);
4678
4679                 trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
4680
4681                 rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
4682                              MS_TRANSFER_START |  MS_TM_NORMAL_WRITE);
4683                 rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
4684                              MS_TRANSFER_END, MS_TRANSFER_END);
4685
4686                 rtsx_send_cmd_no_wait(chip);
4687
4688                 retval = rtsx_transfer_data(chip, MS_CARD, buf + 4 + i * 512,
4689                                             512, 0, DMA_TO_DEVICE, 3000);
4690                 if ((retval < 0) || check_ms_err(chip)) {
4691                         rtsx_clear_ms_error(chip);
4692                         if (ms_card->mg_auth == 0) {
4693                                 if ((buf[5] & 0xC0) != 0)
4694                                         set_sense_type
4695                                             (chip, lun,
4696                                              SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
4697                                 else
4698                                         set_sense_type(chip, lun,
4699                                                        SENSE_TYPE_MG_WRITE_ERR);
4700                         } else {
4701                                 set_sense_type(chip, lun,
4702                                                SENSE_TYPE_MG_WRITE_ERR);
4703                         }
4704                         retval = STATUS_FAIL;
4705                         rtsx_trace(chip);
4706                         goto SetICVFinish;
4707                 }
4708         }
4709 #else
4710         retval = ms_transfer_data(chip, MS_TM_AUTO_WRITE, PRO_WRITE_LONG_DATA,
4711                                   2, WAIT_INT, 0, 0, buf + 4, 1024);
4712         if ((retval != STATUS_SUCCESS) || check_ms_err(chip)) {
4713                 rtsx_clear_ms_error(chip);
4714                 if (ms_card->mg_auth == 0) {
4715                         if ((buf[5] & 0xC0) != 0)
4716                                 set_sense_type
4717                                     (chip, lun,
4718                                      SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
4719                         else
4720                                 set_sense_type(chip, lun,
4721                                                SENSE_TYPE_MG_WRITE_ERR);
4722                 } else {
4723                         set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
4724                 }
4725                 rtsx_trace(chip);
4726                 goto SetICVFinish;
4727         }
4728 #endif
4729
4730 SetICVFinish:
4731         kfree(buf);
4732         return retval;
4733 }
4734
4735 #endif /* SUPPORT_MAGIC_GATE */
4736
4737 void ms_cleanup_work(struct rtsx_chip *chip)
4738 {
4739         struct ms_info *ms_card = &chip->ms_card;
4740
4741         if (CHK_MSPRO(ms_card)) {
4742                 if (ms_card->seq_mode) {
4743                         dev_dbg(rtsx_dev(chip), "MS Pro: stop transmission\n");
4744                         mspro_stop_seq_mode(chip);
4745                         ms_card->cleanup_counter = 0;
4746                 }
4747                 if (CHK_MSHG(ms_card)) {
4748                         rtsx_write_register(chip, MS_CFG,
4749                                             MS_2K_SECTOR_MODE, 0x00);
4750                 }
4751         }
4752 #ifdef MS_DELAY_WRITE
4753         else if ((!CHK_MSPRO(ms_card)) &&
4754                  ms_card->delay_write.delay_write_flag) {
4755                 dev_dbg(rtsx_dev(chip), "MS: delay write\n");
4756                 ms_delay_write(chip);
4757                 ms_card->cleanup_counter = 0;
4758         }
4759 #endif
4760 }
4761
4762 int ms_power_off_card3v3(struct rtsx_chip *chip)
4763 {
4764         int retval;
4765
4766         retval = disable_card_clock(chip, MS_CARD);
4767         if (retval != STATUS_SUCCESS) {
4768                 rtsx_trace(chip);
4769                 return STATUS_FAIL;
4770         }
4771
4772         if (chip->asic_code) {
4773                 retval = ms_pull_ctl_disable(chip);
4774                 if (retval != STATUS_SUCCESS) {
4775                         rtsx_trace(chip);
4776                         return STATUS_FAIL;
4777                 }
4778         } else {
4779                 retval = rtsx_write_register(chip, FPGA_PULL_CTL,
4780                                              FPGA_MS_PULL_CTL_BIT | 0x20,
4781                                              FPGA_MS_PULL_CTL_BIT);
4782                 if (retval) {
4783                         rtsx_trace(chip);
4784                         return retval;
4785                 }
4786         }
4787         retval = rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
4788         if (retval) {
4789                 rtsx_trace(chip);
4790                 return retval;
4791         }
4792         if (!chip->ft2_fast_mode) {
4793                 retval = card_power_off(chip, MS_CARD);
4794                 if (retval != STATUS_SUCCESS) {
4795                         rtsx_trace(chip);
4796                         return STATUS_FAIL;
4797                 }
4798         }
4799
4800         return STATUS_SUCCESS;
4801 }
4802
4803 int release_ms_card(struct rtsx_chip *chip)
4804 {
4805         struct ms_info *ms_card = &chip->ms_card;
4806         int retval;
4807
4808 #ifdef MS_DELAY_WRITE
4809         ms_card->delay_write.delay_write_flag = 0;
4810 #endif
4811         ms_card->pro_under_formatting = 0;
4812
4813         chip->card_ready &= ~MS_CARD;
4814         chip->card_fail &= ~MS_CARD;
4815         chip->card_wp &= ~MS_CARD;
4816
4817         ms_free_l2p_tbl(chip);
4818
4819         memset(ms_card->raw_sys_info, 0, 96);
4820 #ifdef SUPPORT_PCGL_1P18
4821         memset(ms_card->raw_model_name, 0, 48);
4822 #endif
4823
4824         retval = ms_power_off_card3v3(chip);
4825         if (retval != STATUS_SUCCESS) {
4826                 rtsx_trace(chip);
4827                 return STATUS_FAIL;
4828         }
4829
4830         return STATUS_SUCCESS;
4831 }