staging/rts5208/ms.c

0001 // SPDX-License-Identifier: GPL-2.0+
0002 /*
0003  * Driver for Realtek PCI-Express card reader
0004  *
0005  * Copyright(c) 2009-2013 Realtek Semiconductor Corp. All rights reserved.
0006  *
0007  * Author:
0008  *   Wei WANG (wei_wang@realsil.com.cn)
0009  *   Micky Ching (micky_ching@realsil.com.cn)
0010  */
0011
0012 #include <linux/blkdev.h>
0013 #include <linux/kthread.h>
0014 #include <linux/sched.h>
0015 #include <linux/vmalloc.h>
0016
0017 #include "rtsx.h"
0018 #include "ms.h"
0019
0020 static inline void ms_set_err_code(struct rtsx_chip *chip, u8 err_code)
0021 {
0022     struct ms_info *ms_card = &chip->ms_card;
0023
0024     ms_card->err_code = err_code;
0025 }
0026
0027 static inline int ms_check_err_code(struct rtsx_chip *chip, u8 err_code)
0028 {
0029     struct ms_info *ms_card = &chip->ms_card;
0030
0031     return (ms_card->err_code == err_code);
0032 }
0033
0034 static int ms_parse_err_code(struct rtsx_chip *chip)
0035 {
0036     return STATUS_FAIL;
0037 }
0038
0039 static int ms_transfer_tpc(struct rtsx_chip *chip, u8 trans_mode,
0040                u8 tpc, u8 cnt, u8 cfg)
0041 {
0042     struct ms_info *ms_card = &chip->ms_card;
0043     int retval;
0044     u8 *ptr;
0045
0046     dev_dbg(rtsx_dev(chip), "%s: tpc = 0x%x\n", __func__, tpc);
0047
0048     rtsx_init_cmd(chip);
0049
0050     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
0051     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
0052     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
0053     rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
0054              0x01, PINGPONG_BUFFER);
0055
0056     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER,
0057              0xFF, MS_TRANSFER_START | trans_mode);
0058     rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
0059              MS_TRANSFER_END, MS_TRANSFER_END);
0060
0061     rtsx_add_cmd(chip, READ_REG_CMD, MS_TRANS_CFG, 0, 0);
0062
0063     retval = rtsx_send_cmd(chip, MS_CARD, 5000);
0064     if (retval < 0) {
0065         rtsx_clear_ms_error(chip);
0066         ms_set_err_code(chip, MS_TO_ERROR);
0067         return ms_parse_err_code(chip);
0068     }
0069
0070     ptr = rtsx_get_cmd_data(chip) + 1;
0071
0072     if (!(tpc & 0x08)) {        /* Read Packet */
0073         if (*ptr & MS_CRC16_ERR) {
0074             ms_set_err_code(chip, MS_CRC16_ERROR);
0075             return ms_parse_err_code(chip);
0076         }
0077     } else {            /* Write Packet */
0078         if (CHK_MSPRO(ms_card) && !(*ptr & 0x80)) {
0079             if (*ptr & (MS_INT_ERR | MS_INT_CMDNK)) {
0080                 ms_set_err_code(chip, MS_CMD_NK);
0081                 return ms_parse_err_code(chip);
0082             }
0083         }
0084     }
0085
0086     if (*ptr & MS_RDY_TIMEOUT) {
0087         rtsx_clear_ms_error(chip);
0088         ms_set_err_code(chip, MS_TO_ERROR);
0089         return ms_parse_err_code(chip);
0090     }
0091
0092     return STATUS_SUCCESS;
0093 }
0094
0095 static int ms_transfer_data(struct rtsx_chip *chip, u8 trans_mode,
0096                 u8 tpc, u16 sec_cnt, u8 cfg, bool mode_2k,
0097                 int use_sg, void *buf, int buf_len)
0098 {
0099     int retval;
0100     u8 val, err_code = 0;
0101     enum dma_data_direction dir;
0102
0103     if (!buf || !buf_len)
0104         return STATUS_FAIL;
0105
0106     if (trans_mode == MS_TM_AUTO_READ) {
0107         dir = DMA_FROM_DEVICE;
0108         err_code = MS_FLASH_READ_ERROR;
0109     } else if (trans_mode == MS_TM_AUTO_WRITE) {
0110         dir = DMA_TO_DEVICE;
0111         err_code = MS_FLASH_WRITE_ERROR;
0112     } else {
0113         return STATUS_FAIL;
0114     }
0115
0116     rtsx_init_cmd(chip);
0117
0118     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
0119     rtsx_add_cmd(chip, WRITE_REG_CMD,
0120              MS_SECTOR_CNT_H, 0xFF, (u8)(sec_cnt >> 8));
0121     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_SECTOR_CNT_L, 0xFF, (u8)sec_cnt);
0122     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
0123
0124     if (mode_2k) {
0125         rtsx_add_cmd(chip, WRITE_REG_CMD,
0126                  MS_CFG, MS_2K_SECTOR_MODE, MS_2K_SECTOR_MODE);
0127     } else {
0128         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_CFG, MS_2K_SECTOR_MODE, 0);
0129     }
0130
0131     trans_dma_enable(dir, chip, sec_cnt * 512, DMA_512);
0132
0133     rtsx_add_cmd(chip, WRITE_REG_CMD,
0134              MS_TRANSFER, 0xFF, MS_TRANSFER_START | trans_mode);
0135     rtsx_add_cmd(chip, CHECK_REG_CMD,
0136              MS_TRANSFER, MS_TRANSFER_END, MS_TRANSFER_END);
0137
0138     rtsx_send_cmd_no_wait(chip);
0139
0140     retval = rtsx_transfer_data(chip, MS_CARD, buf, buf_len,
0141                     use_sg, dir, chip->mspro_timeout);
0142     if (retval < 0) {
0143         ms_set_err_code(chip, err_code);
0144         if (retval == -ETIMEDOUT)
0145             retval = STATUS_TIMEDOUT;
0146         else
0147             retval = STATUS_FAIL;
0148
0149         return retval;
0150     }
0151
0152     retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
0153     if (retval)
0154         return retval;
0155
0156     if (val & (MS_INT_CMDNK | MS_INT_ERR | MS_CRC16_ERR | MS_RDY_TIMEOUT))
0157         return STATUS_FAIL;
0158
0159     return STATUS_SUCCESS;
0160 }
0161
0162 static int ms_write_bytes(struct rtsx_chip *chip,
0163               u8 tpc, u8 cnt, u8 cfg, u8 *data, int data_len)
0164 {
0165     struct ms_info *ms_card = &chip->ms_card;
0166     int retval, i;
0167
0168     if (!data || data_len < cnt)
0169         return STATUS_ERROR;
0170
0171     rtsx_init_cmd(chip);
0172
0173     for (i = 0; i < cnt; i++) {
0174         rtsx_add_cmd(chip, WRITE_REG_CMD,
0175                  PPBUF_BASE2 + i, 0xFF, data[i]);
0176     }
0177     if (cnt % 2)
0178         rtsx_add_cmd(chip, WRITE_REG_CMD, PPBUF_BASE2 + i, 0xFF, 0xFF);
0179
0180     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
0181     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
0182     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
0183     rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
0184              0x01, PINGPONG_BUFFER);
0185
0186     rtsx_add_cmd(chip, WRITE_REG_CMD,
0187              MS_TRANSFER, 0xFF, MS_TRANSFER_START | MS_TM_WRITE_BYTES);
0188     rtsx_add_cmd(chip, CHECK_REG_CMD,
0189              MS_TRANSFER, MS_TRANSFER_END, MS_TRANSFER_END);
0190
0191     retval = rtsx_send_cmd(chip, MS_CARD, 5000);
0192     if (retval < 0) {
0193         u8 val = 0;
0194
0195         rtsx_read_register(chip, MS_TRANS_CFG, &val);
0196         dev_dbg(rtsx_dev(chip), "MS_TRANS_CFG: 0x%02x\n", val);
0197
0198         rtsx_clear_ms_error(chip);
0199
0200         if (!(tpc & 0x08)) {
0201             if (val & MS_CRC16_ERR) {
0202                 ms_set_err_code(chip, MS_CRC16_ERROR);
0203                 return ms_parse_err_code(chip);
0204             }
0205         } else {
0206             if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
0207                 if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
0208                     ms_set_err_code(chip, MS_CMD_NK);
0209                     return ms_parse_err_code(chip);
0210                 }
0211             }
0212         }
0213
0214         if (val & MS_RDY_TIMEOUT) {
0215             ms_set_err_code(chip, MS_TO_ERROR);
0216             return ms_parse_err_code(chip);
0217         }
0218
0219         ms_set_err_code(chip, MS_TO_ERROR);
0220         return ms_parse_err_code(chip);
0221     }
0222
0223     return STATUS_SUCCESS;
0224 }
0225
0226 static int ms_read_bytes(struct rtsx_chip *chip,
0227              u8 tpc, u8 cnt, u8 cfg, u8 *data, int data_len)
0228 {
0229     struct ms_info *ms_card = &chip->ms_card;
0230     int retval, i;
0231     u8 *ptr;
0232
0233     if (!data)
0234         return STATUS_ERROR;
0235
0236     rtsx_init_cmd(chip);
0237
0238     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, tpc);
0239     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_BYTE_CNT, 0xFF, cnt);
0240     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, cfg);
0241     rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
0242              0x01, PINGPONG_BUFFER);
0243
0244     rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
0245              MS_TRANSFER_START | MS_TM_READ_BYTES);
0246     rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
0247              MS_TRANSFER_END, MS_TRANSFER_END);
0248
0249     for (i = 0; i < data_len - 1; i++)
0250         rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + i, 0, 0);
0251
0252     if (data_len % 2)
0253         rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len, 0, 0);
0254     else
0255         rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + data_len - 1,
0256                  0, 0);
0257
0258     retval = rtsx_send_cmd(chip, MS_CARD, 5000);
0259     if (retval < 0) {
0260         u8 val = 0;
0261
0262         rtsx_read_register(chip, MS_TRANS_CFG, &val);
0263         rtsx_clear_ms_error(chip);
0264
0265         if (!(tpc & 0x08)) {
0266             if (val & MS_CRC16_ERR) {
0267                 ms_set_err_code(chip, MS_CRC16_ERROR);
0268                 return ms_parse_err_code(chip);
0269             }
0270         } else {
0271             if (CHK_MSPRO(ms_card) && !(val & 0x80)) {
0272                 if (val & (MS_INT_ERR | MS_INT_CMDNK)) {
0273                     ms_set_err_code(chip, MS_CMD_NK);
0274                     return ms_parse_err_code(chip);
0275                 }
0276             }
0277         }
0278
0279         if (val & MS_RDY_TIMEOUT) {
0280             ms_set_err_code(chip, MS_TO_ERROR);
0281             return ms_parse_err_code(chip);
0282         }
0283
0284         ms_set_err_code(chip, MS_TO_ERROR);
0285         return ms_parse_err_code(chip);
0286     }
0287
0288     ptr = rtsx_get_cmd_data(chip) + 1;
0289
0290     for (i = 0; i < data_len; i++)
0291         data[i] = ptr[i];
0292
0293     if (tpc == PRO_READ_SHORT_DATA && data_len == 8) {
0294         dev_dbg(rtsx_dev(chip), "Read format progress:\n");
0295         print_hex_dump_bytes(KBUILD_MODNAME ": ", DUMP_PREFIX_NONE, ptr,
0296                      cnt);
0297     }
0298
0299     return STATUS_SUCCESS;
0300 }
0301
0302 static int ms_set_rw_reg_addr(struct rtsx_chip *chip, u8 read_start,
0303                   u8 read_cnt, u8 write_start, u8 write_cnt)
0304 {
0305     int retval, i;
0306     u8 data[4];
0307
0308     data[0] = read_start;
0309     data[1] = read_cnt;
0310     data[2] = write_start;
0311     data[3] = write_cnt;
0312
0313     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
0314         retval = ms_write_bytes(chip, SET_RW_REG_ADRS, 4,
0315                     NO_WAIT_INT, data, 4);
0316         if (retval == STATUS_SUCCESS)
0317             return STATUS_SUCCESS;
0318         rtsx_clear_ms_error(chip);
0319     }
0320
0321     return STATUS_FAIL;
0322 }
0323
0324 static int ms_send_cmd(struct rtsx_chip *chip, u8 cmd, u8 cfg)
0325 {
0326     u8 data[2];
0327
0328     data[0] = cmd;
0329     data[1] = 0;
0330
0331     return ms_write_bytes(chip, PRO_SET_CMD, 1, cfg, data, 1);
0332 }
0333
0334 static int ms_set_init_para(struct rtsx_chip *chip)
0335 {
0336     struct ms_info *ms_card = &chip->ms_card;
0337     int retval;
0338
0339     if (CHK_HG8BIT(ms_card)) {
0340         if (chip->asic_code)
0341             ms_card->ms_clock = chip->asic_ms_hg_clk;
0342         else
0343             ms_card->ms_clock = chip->fpga_ms_hg_clk;
0344
0345     } else if (CHK_MSPRO(ms_card) || CHK_MS4BIT(ms_card)) {
0346         if (chip->asic_code)
0347             ms_card->ms_clock = chip->asic_ms_4bit_clk;
0348         else
0349             ms_card->ms_clock = chip->fpga_ms_4bit_clk;
0350
0351     } else {
0352         if (chip->asic_code)
0353             ms_card->ms_clock = chip->asic_ms_1bit_clk;
0354         else
0355             ms_card->ms_clock = chip->fpga_ms_1bit_clk;
0356     }
0357
0358     retval = switch_clock(chip, ms_card->ms_clock);
0359     if (retval != STATUS_SUCCESS)
0360         return STATUS_FAIL;
0361
0362     retval = select_card(chip, MS_CARD);
0363     if (retval != STATUS_SUCCESS)
0364         return STATUS_FAIL;
0365
0366     return STATUS_SUCCESS;
0367 }
0368
0369 static int ms_switch_clock(struct rtsx_chip *chip)
0370 {
0371     struct ms_info *ms_card = &chip->ms_card;
0372     int retval;
0373
0374     retval = select_card(chip, MS_CARD);
0375     if (retval != STATUS_SUCCESS)
0376         return STATUS_FAIL;
0377
0378     retval = switch_clock(chip, ms_card->ms_clock);
0379     if (retval != STATUS_SUCCESS)
0380         return STATUS_FAIL;
0381
0382     return STATUS_SUCCESS;
0383 }
0384
0385 static int ms_pull_ctl_disable(struct rtsx_chip *chip)
0386 {
0387     int retval;
0388
0389     if (CHECK_PID(chip, 0x5208)) {
0390         retval = rtsx_write_register(chip, CARD_PULL_CTL1, 0xFF,
0391                          MS_D1_PD | MS_D2_PD | MS_CLK_PD |
0392                          MS_D6_PD);
0393         if (retval)
0394             return retval;
0395
0396         retval = rtsx_write_register(chip, CARD_PULL_CTL2, 0xFF,
0397                          MS_D3_PD | MS_D0_PD | MS_BS_PD |
0398                          XD_D4_PD);
0399         if (retval)
0400             return retval;
0401
0402         retval = rtsx_write_register(chip, CARD_PULL_CTL3, 0xFF,
0403                          MS_D7_PD | XD_CE_PD | XD_CLE_PD |
0404                          XD_CD_PU);
0405         if (retval)
0406             return retval;
0407
0408         retval = rtsx_write_register(chip, CARD_PULL_CTL4, 0xFF,
0409                          XD_RDY_PD | SD_D3_PD | SD_D2_PD |
0410                          XD_ALE_PD);
0411         if (retval)
0412             return retval;
0413
0414         retval = rtsx_write_register(chip, CARD_PULL_CTL5, 0xFF,
0415                          MS_INS_PU | SD_WP_PD | SD_CD_PU |
0416                          SD_CMD_PD);
0417         if (retval)
0418             return retval;
0419
0420         retval = rtsx_write_register(chip, CARD_PULL_CTL6, 0xFF,
0421                          MS_D5_PD | MS_D4_PD);
0422         if (retval)
0423             return retval;
0424
0425     } else if (CHECK_PID(chip, 0x5288)) {
0426         if (CHECK_BARO_PKG(chip, QFN)) {
0427             retval = rtsx_write_register(chip, CARD_PULL_CTL1,
0428                              0xFF, 0x55);
0429             if (retval)
0430                 return retval;
0431
0432             retval = rtsx_write_register(chip, CARD_PULL_CTL2,
0433                              0xFF, 0x55);
0434             if (retval)
0435                 return retval;
0436
0437             retval = rtsx_write_register(chip, CARD_PULL_CTL3,
0438                              0xFF, 0x4B);
0439             if (retval)
0440                 return retval;
0441
0442             retval = rtsx_write_register(chip, CARD_PULL_CTL4,
0443                              0xFF, 0x69);
0444             if (retval)
0445                 return retval;
0446         }
0447     }
0448
0449     return STATUS_SUCCESS;
0450 }
0451
0452 static int ms_pull_ctl_enable(struct rtsx_chip *chip)
0453 {
0454     int retval;
0455
0456     rtsx_init_cmd(chip);
0457
0458     if (CHECK_PID(chip, 0x5208)) {
0459         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL1, 0xFF,
0460                  MS_D1_PD | MS_D2_PD | MS_CLK_NP | MS_D6_PD);
0461         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL2, 0xFF,
0462                  MS_D3_PD | MS_D0_PD | MS_BS_NP | XD_D4_PD);
0463         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL3, 0xFF,
0464                  MS_D7_PD | XD_CE_PD | XD_CLE_PD | XD_CD_PU);
0465         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL4, 0xFF,
0466                  XD_RDY_PD | SD_D3_PD | SD_D2_PD | XD_ALE_PD);
0467         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL5, 0xFF,
0468                  MS_INS_PU | SD_WP_PD | SD_CD_PU | SD_CMD_PD);
0469         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_PULL_CTL6, 0xFF,
0470                  MS_D5_PD | MS_D4_PD);
0471     } else if (CHECK_PID(chip, 0x5288)) {
0472         if (CHECK_BARO_PKG(chip, QFN)) {
0473             rtsx_add_cmd(chip, WRITE_REG_CMD,
0474                      CARD_PULL_CTL1, 0xFF, 0x55);
0475             rtsx_add_cmd(chip, WRITE_REG_CMD,
0476                      CARD_PULL_CTL2, 0xFF, 0x45);
0477             rtsx_add_cmd(chip, WRITE_REG_CMD,
0478                      CARD_PULL_CTL3, 0xFF, 0x4B);
0479             rtsx_add_cmd(chip, WRITE_REG_CMD,
0480                      CARD_PULL_CTL4, 0xFF, 0x29);
0481         }
0482     }
0483
0484     retval = rtsx_send_cmd(chip, MS_CARD, 100);
0485     if (retval < 0)
0486         return STATUS_FAIL;
0487
0488     return STATUS_SUCCESS;
0489 }
0490
0491 static int ms_prepare_reset(struct rtsx_chip *chip)
0492 {
0493     struct ms_info *ms_card = &chip->ms_card;
0494     int retval;
0495     u8 oc_mask = 0;
0496
0497     ms_card->ms_type = 0;
0498     ms_card->check_ms_flow = 0;
0499     ms_card->switch_8bit_fail = 0;
0500     ms_card->delay_write.delay_write_flag = 0;
0501
0502     ms_card->pro_under_formatting = 0;
0503
0504     retval = ms_power_off_card3v3(chip);
0505     if (retval != STATUS_SUCCESS)
0506         return STATUS_FAIL;
0507
0508     if (!chip->ft2_fast_mode)
0509         wait_timeout(250);
0510
0511     retval = enable_card_clock(chip, MS_CARD);
0512     if (retval != STATUS_SUCCESS)
0513         return STATUS_FAIL;
0514
0515     if (chip->asic_code) {
0516         retval = ms_pull_ctl_enable(chip);
0517         if (retval != STATUS_SUCCESS)
0518             return STATUS_FAIL;
0519     } else {
0520         retval = rtsx_write_register(chip, FPGA_PULL_CTL,
0521                          FPGA_MS_PULL_CTL_BIT | 0x20, 0);
0522         if (retval)
0523             return retval;
0524     }
0525
0526     if (!chip->ft2_fast_mode) {
0527         retval = card_power_on(chip, MS_CARD);
0528         if (retval != STATUS_SUCCESS)
0529             return STATUS_FAIL;
0530
0531         wait_timeout(150);
0532
0533 #ifdef SUPPORT_OCP
0534         if (CHECK_LUN_MODE(chip, SD_MS_2LUN))
0535             oc_mask = MS_OC_NOW | MS_OC_EVER;
0536         else
0537             oc_mask = SD_OC_NOW | SD_OC_EVER;
0538
0539         if (chip->ocp_stat & oc_mask) {
0540             dev_dbg(rtsx_dev(chip), "Over current, OCPSTAT is 0x%x\n",
0541                 chip->ocp_stat);
0542             return STATUS_FAIL;
0543         }
0544 #endif
0545     }
0546
0547     retval = rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN,
0548                      MS_OUTPUT_EN);
0549     if (retval)
0550         return retval;
0551
0552     if (chip->asic_code) {
0553         retval = rtsx_write_register(chip, MS_CFG, 0xFF,
0554                          SAMPLE_TIME_RISING |
0555                          PUSH_TIME_DEFAULT |
0556                          NO_EXTEND_TOGGLE |
0557                          MS_BUS_WIDTH_1);
0558         if (retval)
0559             return retval;
0560
0561     } else {
0562         retval = rtsx_write_register(chip, MS_CFG, 0xFF,
0563                          SAMPLE_TIME_FALLING |
0564                          PUSH_TIME_DEFAULT |
0565                          NO_EXTEND_TOGGLE |
0566                          MS_BUS_WIDTH_1);
0567         if (retval)
0568             return retval;
0569     }
0570     retval = rtsx_write_register(chip, MS_TRANS_CFG, 0xFF,
0571                      NO_WAIT_INT | NO_AUTO_READ_INT_REG);
0572     if (retval)
0573         return retval;
0574
0575     retval = rtsx_write_register(chip, CARD_STOP, MS_STOP | MS_CLR_ERR,
0576                      MS_STOP | MS_CLR_ERR);
0577     if (retval)
0578         return retval;
0579
0580     retval = ms_set_init_para(chip);
0581     if (retval != STATUS_SUCCESS)
0582         return STATUS_FAIL;
0583
0584     return STATUS_SUCCESS;
0585 }
0586
0587 static int ms_identify_media_type(struct rtsx_chip *chip, int switch_8bit_bus)
0588 {
0589     struct ms_info *ms_card = &chip->ms_card;
0590     int retval, i;
0591     u8 val;
0592
0593     retval = ms_set_rw_reg_addr(chip, PRO_STATUS_REG, 6, SYSTEM_PARAM, 1);
0594     if (retval != STATUS_SUCCESS)
0595         return STATUS_FAIL;
0596
0597     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
0598         retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, READ_REG,
0599                      6, NO_WAIT_INT);
0600         if (retval == STATUS_SUCCESS)
0601             break;
0602     }
0603     if (i == MS_MAX_RETRY_COUNT)
0604         return STATUS_FAIL;
0605
0606     retval = rtsx_read_register(chip, PPBUF_BASE2 + 2, &val);
0607     if (retval)
0608         return retval;
0609
0610     dev_dbg(rtsx_dev(chip), "Type register: 0x%x\n", val);
0611     if (val != 0x01) {
0612         if (val != 0x02)
0613             ms_card->check_ms_flow = 1;
0614
0615         return STATUS_FAIL;
0616     }
0617
0618     retval = rtsx_read_register(chip, PPBUF_BASE2 + 4, &val);
0619     if (retval)
0620         return retval;
0621
0622     dev_dbg(rtsx_dev(chip), "Category register: 0x%x\n", val);
0623     if (val != 0) {
0624         ms_card->check_ms_flow = 1;
0625         return STATUS_FAIL;
0626     }
0627
0628     retval = rtsx_read_register(chip, PPBUF_BASE2 + 5, &val);
0629     if (retval)
0630         return retval;
0631
0632     dev_dbg(rtsx_dev(chip), "Class register: 0x%x\n", val);
0633     if (val == 0) {
0634         retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
0635         if (retval)
0636             return retval;
0637
0638         if (val & WRT_PRTCT)
0639             chip->card_wp |= MS_CARD;
0640         else
0641             chip->card_wp &= ~MS_CARD;
0642
0643     } else if ((val == 0x01) || (val == 0x02) || (val == 0x03)) {
0644         chip->card_wp |= MS_CARD;
0645     } else {
0646         ms_card->check_ms_flow = 1;
0647         return STATUS_FAIL;
0648     }
0649
0650     ms_card->ms_type |= TYPE_MSPRO;
0651
0652     retval = rtsx_read_register(chip, PPBUF_BASE2 + 3, &val);
0653     if (retval)
0654         return retval;
0655
0656     dev_dbg(rtsx_dev(chip), "IF Mode register: 0x%x\n", val);
0657     if (val == 0) {
0658         ms_card->ms_type &= 0x0F;
0659     } else if (val == 7) {
0660         if (switch_8bit_bus)
0661             ms_card->ms_type |= MS_HG;
0662         else
0663             ms_card->ms_type &= 0x0F;
0664
0665     } else {
0666         return STATUS_FAIL;
0667     }
0668
0669     return STATUS_SUCCESS;
0670 }
0671
0672 static int ms_confirm_cpu_startup(struct rtsx_chip *chip)
0673 {
0674     int retval, i, k;
0675     u8 val;
0676
0677     /* Confirm CPU StartUp */
0678     k = 0;
0679     do {
0680         if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
0681             ms_set_err_code(chip, MS_NO_CARD);
0682             return STATUS_FAIL;
0683         }
0684
0685         for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
0686             retval = ms_read_bytes(chip, GET_INT, 1,
0687                            NO_WAIT_INT, &val, 1);
0688             if (retval == STATUS_SUCCESS)
0689                 break;
0690         }
0691         if (i == MS_MAX_RETRY_COUNT)
0692             return STATUS_FAIL;
0693
0694         if (k > 100)
0695             return STATUS_FAIL;
0696
0697         k++;
0698         wait_timeout(100);
0699     } while (!(val & INT_REG_CED));
0700
0701     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
0702         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
0703         if (retval == STATUS_SUCCESS)
0704             break;
0705     }
0706     if (i == MS_MAX_RETRY_COUNT)
0707         return STATUS_FAIL;
0708
0709     if (val & INT_REG_ERR) {
0710         if (val & INT_REG_CMDNK)
0711             chip->card_wp |= (MS_CARD);
0712         else
0713             return STATUS_FAIL;
0714     }
0715     /* --  end confirm CPU startup */
0716
0717     return STATUS_SUCCESS;
0718 }
0719
0720 static int ms_switch_parallel_bus(struct rtsx_chip *chip)
0721 {
0722     int retval, i;
0723     u8 data[2];
0724
0725     data[0] = PARALLEL_4BIT_IF;
0726     data[1] = 0;
0727     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
0728         retval = ms_write_bytes(chip, WRITE_REG, 1, NO_WAIT_INT,
0729                     data, 2);
0730         if (retval == STATUS_SUCCESS)
0731             break;
0732     }
0733     if (retval != STATUS_SUCCESS)
0734         return STATUS_FAIL;
0735
0736     return STATUS_SUCCESS;
0737 }
0738
0739 static int ms_switch_8bit_bus(struct rtsx_chip *chip)
0740 {
0741     struct ms_info *ms_card = &chip->ms_card;
0742     int retval, i;
0743     u8 data[2];
0744
0745     data[0] = PARALLEL_8BIT_IF;
0746     data[1] = 0;
0747     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
0748         retval = ms_write_bytes(chip, WRITE_REG, 1,
0749                     NO_WAIT_INT, data, 2);
0750         if (retval == STATUS_SUCCESS)
0751             break;
0752     }
0753     if (retval != STATUS_SUCCESS)
0754         return STATUS_FAIL;
0755
0756     retval = rtsx_write_register(chip, MS_CFG, 0x98,
0757                      MS_BUS_WIDTH_8 | SAMPLE_TIME_FALLING);
0758     if (retval)
0759         return retval;
0760
0761     ms_card->ms_type |= MS_8BIT;
0762     retval = ms_set_init_para(chip);
0763     if (retval != STATUS_SUCCESS)
0764         return STATUS_FAIL;
0765
0766     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
0767         retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT,
0768                      1, NO_WAIT_INT);
0769         if (retval != STATUS_SUCCESS)
0770             return STATUS_FAIL;
0771     }
0772
0773     return STATUS_SUCCESS;
0774 }
0775
0776 static int ms_pro_reset_flow(struct rtsx_chip *chip, int switch_8bit_bus)
0777 {
0778     struct ms_info *ms_card = &chip->ms_card;
0779     int retval, i;
0780
0781     for (i = 0; i < 3; i++) {
0782         retval = ms_prepare_reset(chip);
0783         if (retval != STATUS_SUCCESS)
0784             return STATUS_FAIL;
0785
0786         retval = ms_identify_media_type(chip, switch_8bit_bus);
0787         if (retval != STATUS_SUCCESS)
0788             return STATUS_FAIL;
0789
0790         retval = ms_confirm_cpu_startup(chip);
0791         if (retval != STATUS_SUCCESS)
0792             return STATUS_FAIL;
0793
0794         retval = ms_switch_parallel_bus(chip);
0795         if (retval != STATUS_SUCCESS) {
0796             if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
0797                 ms_set_err_code(chip, MS_NO_CARD);
0798                 return STATUS_FAIL;
0799             }
0800             continue;
0801         } else {
0802             break;
0803         }
0804     }
0805
0806     if (retval != STATUS_SUCCESS)
0807         return STATUS_FAIL;
0808
0809     /* Switch MS-PRO into Parallel mode */
0810     retval = rtsx_write_register(chip, MS_CFG, 0x18, MS_BUS_WIDTH_4);
0811     if (retval)
0812         return retval;
0813
0814     retval = rtsx_write_register(chip, MS_CFG, PUSH_TIME_ODD,
0815                      PUSH_TIME_ODD);
0816     if (retval)
0817         return retval;
0818
0819     retval = ms_set_init_para(chip);
0820     if (retval != STATUS_SUCCESS)
0821         return STATUS_FAIL;
0822
0823     /* If MSPro HG Card, We shall try to switch to 8-bit bus */
0824     if (CHK_MSHG(ms_card) && chip->support_ms_8bit && switch_8bit_bus) {
0825         retval = ms_switch_8bit_bus(chip);
0826         if (retval != STATUS_SUCCESS) {
0827             ms_card->switch_8bit_fail = 1;
0828             return STATUS_FAIL;
0829         }
0830     }
0831
0832     return STATUS_SUCCESS;
0833 }
0834
0835 #ifdef XC_POWERCLASS
0836 static int msxc_change_power(struct rtsx_chip *chip, u8 mode)
0837 {
0838     int retval;
0839     u8 buf[6];
0840
0841     ms_cleanup_work(chip);
0842
0843     retval = ms_set_rw_reg_addr(chip, 0, 0, PRO_DATA_COUNT1, 6);
0844     if (retval != STATUS_SUCCESS)
0845         return STATUS_FAIL;
0846
0847     buf[0] = 0;
0848     buf[1] = mode;
0849     buf[2] = 0;
0850     buf[3] = 0;
0851     buf[4] = 0;
0852     buf[5] = 0;
0853
0854     retval = ms_write_bytes(chip, PRO_WRITE_REG, 6, NO_WAIT_INT, buf, 6);
0855     if (retval != STATUS_SUCCESS)
0856         return STATUS_FAIL;
0857
0858     retval = ms_send_cmd(chip, XC_CHG_POWER, WAIT_INT);
0859     if (retval != STATUS_SUCCESS)
0860         return STATUS_FAIL;
0861
0862     retval = rtsx_read_register(chip, MS_TRANS_CFG, buf);
0863     if (retval)
0864         return retval;
0865
0866     if (buf[0] & (MS_INT_CMDNK | MS_INT_ERR))
0867         return STATUS_FAIL;
0868
0869     return STATUS_SUCCESS;
0870 }
0871 #endif
0872
0873 static int ms_read_attribute_info(struct rtsx_chip *chip)
0874 {
0875     struct ms_info *ms_card = &chip->ms_card;
0876     int retval, i;
0877     u8 val, *buf, class_code, device_type, sub_class, data[16];
0878     u16 total_blk = 0, blk_size = 0;
0879 #ifdef SUPPORT_MSXC
0880     u32 xc_total_blk = 0, xc_blk_size = 0;
0881 #endif
0882     u32 sys_info_addr = 0, sys_info_size;
0883 #ifdef SUPPORT_PCGL_1P18
0884     u32 model_name_addr = 0, model_name_size;
0885     int found_sys_info = 0, found_model_name = 0;
0886 #endif
0887
0888     retval = ms_set_rw_reg_addr(chip, PRO_INT_REG, 2, PRO_SYSTEM_PARAM, 7);
0889     if (retval != STATUS_SUCCESS)
0890         return STATUS_FAIL;
0891
0892     if (CHK_MS8BIT(ms_card))
0893         data[0] = PARALLEL_8BIT_IF;
0894     else
0895         data[0] = PARALLEL_4BIT_IF;
0896
0897     data[1] = 0;
0898
0899     data[2] = 0x40;
0900     data[3] = 0;
0901     data[4] = 0;
0902     data[5] = 0;
0903     data[6] = 0;
0904     data[7] = 0;
0905
0906     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
0907         retval = ms_write_bytes(chip, PRO_WRITE_REG, 7, NO_WAIT_INT,
0908                     data, 8);
0909         if (retval == STATUS_SUCCESS)
0910             break;
0911     }
0912     if (retval != STATUS_SUCCESS)
0913         return STATUS_FAIL;
0914
0915     buf = kmalloc(64 * 512, GFP_KERNEL);
0916     if (!buf)
0917         return STATUS_ERROR;
0918
0919     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
0920         retval = ms_send_cmd(chip, PRO_READ_ATRB, WAIT_INT);
0921         if (retval != STATUS_SUCCESS)
0922             continue;
0923
0924         retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
0925         if (retval != STATUS_SUCCESS) {
0926             kfree(buf);
0927             return STATUS_FAIL;
0928         }
0929         if (!(val & MS_INT_BREQ)) {
0930             kfree(buf);
0931             return STATUS_FAIL;
0932         }
0933         retval = ms_transfer_data(chip, MS_TM_AUTO_READ,
0934                       PRO_READ_LONG_DATA, 0x40, WAIT_INT,
0935                       0, 0, buf, 64 * 512);
0936         if (retval == STATUS_SUCCESS)
0937             break;
0938
0939         rtsx_clear_ms_error(chip);
0940     }
0941     if (retval != STATUS_SUCCESS) {
0942         kfree(buf);
0943         return STATUS_FAIL;
0944     }
0945
0946     i = 0;
0947     do {
0948         retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
0949         if (retval != STATUS_SUCCESS) {
0950             kfree(buf);
0951             return STATUS_FAIL;
0952         }
0953
0954         if ((val & MS_INT_CED) || !(val & MS_INT_BREQ))
0955             break;
0956
0957         retval = ms_transfer_tpc(chip, MS_TM_NORMAL_READ,
0958                      PRO_READ_LONG_DATA, 0, WAIT_INT);
0959         if (retval != STATUS_SUCCESS) {
0960             kfree(buf);
0961             return STATUS_FAIL;
0962         }
0963
0964         i++;
0965     } while (i < 1024);
0966
0967     if (buf[0] != 0xa5 && buf[1] != 0xc3) {
0968         /* Signature code is wrong */
0969         kfree(buf);
0970         return STATUS_FAIL;
0971     }
0972
0973     if (buf[4] < 1 || buf[4] > 12) {
0974         kfree(buf);
0975         return STATUS_FAIL;
0976     }
0977
0978     for (i = 0; i < buf[4]; i++) {
0979         int cur_addr_off = 16 + i * 12;
0980
0981 #ifdef SUPPORT_MSXC
0982         if (buf[cur_addr_off + 8] == 0x10 ||
0983             buf[cur_addr_off + 8] == 0x13) {
0984 #else
0985         if (buf[cur_addr_off + 8] == 0x10) {
0986 #endif
0987             sys_info_addr = ((u32)buf[cur_addr_off + 0] << 24) |
0988                 ((u32)buf[cur_addr_off + 1] << 16) |
0989                 ((u32)buf[cur_addr_off + 2] << 8) |
0990                 buf[cur_addr_off + 3];
0991             sys_info_size = ((u32)buf[cur_addr_off + 4] << 24) |
0992                 ((u32)buf[cur_addr_off + 5] << 16) |
0993                 ((u32)buf[cur_addr_off + 6] << 8) |
0994                 buf[cur_addr_off + 7];
0995             dev_dbg(rtsx_dev(chip), "sys_info_addr = 0x%x, sys_info_size = 0x%x\n",
0996                 sys_info_addr, sys_info_size);
0997             if (sys_info_size != 96)  {
0998                 kfree(buf);
0999                 return STATUS_FAIL;
1000             }
1001             if (sys_info_addr < 0x1A0) {
1002                 kfree(buf);
1003                 return STATUS_FAIL;
1004             }
1005             if ((sys_info_size + sys_info_addr) > 0x8000) {
1006                 kfree(buf);
1007                 return STATUS_FAIL;
1008             }
1009
1010 #ifdef SUPPORT_MSXC
1011             if (buf[cur_addr_off + 8] == 0x13)
1012                 ms_card->ms_type |= MS_XC;
1013 #endif
1014 #ifdef SUPPORT_PCGL_1P18
1015             found_sys_info = 1;
1016 #else
1017             break;
1018 #endif
1019         }
1020 #ifdef SUPPORT_PCGL_1P18
1021         if (buf[cur_addr_off + 8] == 0x15) {
1022             model_name_addr = ((u32)buf[cur_addr_off + 0] << 24) |
1023                 ((u32)buf[cur_addr_off + 1] << 16) |
1024                 ((u32)buf[cur_addr_off + 2] << 8) |
1025                 buf[cur_addr_off + 3];
1026             model_name_size = ((u32)buf[cur_addr_off + 4] << 24) |
1027                 ((u32)buf[cur_addr_off + 5] << 16) |
1028                 ((u32)buf[cur_addr_off + 6] << 8) |
1029                 buf[cur_addr_off + 7];
1030             dev_dbg(rtsx_dev(chip), "model_name_addr = 0x%x, model_name_size = 0x%x\n",
1031                 model_name_addr, model_name_size);
1032             if (model_name_size != 48)  {
1033                 kfree(buf);
1034                 return STATUS_FAIL;
1035             }
1036             if (model_name_addr < 0x1A0) {
1037                 kfree(buf);
1038                 return STATUS_FAIL;
1039             }
1040             if ((model_name_size + model_name_addr) > 0x8000) {
1041                 kfree(buf);
1042                 return STATUS_FAIL;
1043             }
1044
1045             found_model_name = 1;
1046         }
1047
1048         if (found_sys_info && found_model_name)
1049             break;
1050 #endif
1051     }
1052
1053     if (i == buf[4]) {
1054         kfree(buf);
1055         return STATUS_FAIL;
1056     }
1057
1058     class_code =  buf[sys_info_addr + 0];
1059     device_type = buf[sys_info_addr + 56];
1060     sub_class = buf[sys_info_addr + 46];
1061 #ifdef SUPPORT_MSXC
1062     if (CHK_MSXC(ms_card)) {
1063         xc_total_blk = ((u32)buf[sys_info_addr + 6] << 24) |
1064                 ((u32)buf[sys_info_addr + 7] << 16) |
1065                 ((u32)buf[sys_info_addr + 8] << 8) |
1066                 buf[sys_info_addr + 9];
1067         xc_blk_size = ((u32)buf[sys_info_addr + 32] << 24) |
1068                 ((u32)buf[sys_info_addr + 33] << 16) |
1069                 ((u32)buf[sys_info_addr + 34] << 8) |
1070                 buf[sys_info_addr + 35];
1071         dev_dbg(rtsx_dev(chip), "xc_total_blk = 0x%x, xc_blk_size = 0x%x\n",
1072             xc_total_blk, xc_blk_size);
1073     } else {
1074         total_blk = ((u16)buf[sys_info_addr + 6] << 8) |
1075             buf[sys_info_addr + 7];
1076         blk_size = ((u16)buf[sys_info_addr + 2] << 8) |
1077             buf[sys_info_addr + 3];
1078         dev_dbg(rtsx_dev(chip), "total_blk = 0x%x, blk_size = 0x%x\n",
1079             total_blk, blk_size);
1080     }
1081 #else
1082     total_blk = ((u16)buf[sys_info_addr + 6] << 8) | buf[sys_info_addr + 7];
1083     blk_size = ((u16)buf[sys_info_addr + 2] << 8) | buf[sys_info_addr + 3];
1084     dev_dbg(rtsx_dev(chip), "total_blk = 0x%x, blk_size = 0x%x\n",
1085         total_blk, blk_size);
1086 #endif
1087
1088     dev_dbg(rtsx_dev(chip), "class_code = 0x%x, device_type = 0x%x, sub_class = 0x%x\n",
1089         class_code, device_type, sub_class);
1090
1091     memcpy(ms_card->raw_sys_info, buf + sys_info_addr, 96);
1092 #ifdef SUPPORT_PCGL_1P18
1093     memcpy(ms_card->raw_model_name, buf + model_name_addr, 48);
1094 #endif
1095
1096     kfree(buf);
1097
1098 #ifdef SUPPORT_MSXC
1099     if (CHK_MSXC(ms_card)) {
1100         if (class_code != 0x03)
1101             return STATUS_FAIL;
1102     } else {
1103         if (class_code != 0x02)
1104             return STATUS_FAIL;
1105     }
1106 #else
1107     if (class_code != 0x02)
1108         return STATUS_FAIL;
1109 #endif
1110
1111     if (device_type != 0x00) {
1112         if (device_type == 0x01 || device_type == 0x02 ||
1113             device_type == 0x03) {
1114             chip->card_wp |= MS_CARD;
1115         } else {
1116             return STATUS_FAIL;
1117         }
1118     }
1119
1120     if (sub_class & 0xC0)
1121         return STATUS_FAIL;
1122
1123     dev_dbg(rtsx_dev(chip), "class_code: 0x%x, device_type: 0x%x, sub_class: 0x%x\n",
1124         class_code, device_type, sub_class);
1125
1126 #ifdef SUPPORT_MSXC
1127     if (CHK_MSXC(ms_card)) {
1128         chip->capacity[chip->card2lun[MS_CARD]] =
1129             ms_card->capacity = xc_total_blk * xc_blk_size;
1130     } else {
1131         chip->capacity[chip->card2lun[MS_CARD]] =
1132             ms_card->capacity = total_blk * blk_size;
1133     }
1134 #else
1135     ms_card->capacity = total_blk * blk_size;
1136     chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity;
1137 #endif
1138
1139     return STATUS_SUCCESS;
1140 }
1141
1142 #ifdef SUPPORT_MAGIC_GATE
1143 static int mg_set_tpc_para_sub(struct rtsx_chip *chip,
1144                    int type, u8 mg_entry_num);
1145 #endif
1146
1147 static int reset_ms_pro(struct rtsx_chip *chip)
1148 {
1149     struct ms_info *ms_card = &chip->ms_card;
1150     int retval;
1151 #ifdef XC_POWERCLASS
1152     u8 change_power_class;
1153
1154     if (chip->ms_power_class_en & 0x02)
1155         change_power_class = 2;
1156     else if (chip->ms_power_class_en & 0x01)
1157         change_power_class = 1;
1158     else
1159         change_power_class = 0;
1160 #endif
1161
1162 #ifdef XC_POWERCLASS
1163 retry:
1164 #endif
1165     retval = ms_pro_reset_flow(chip, 1);
1166     if (retval != STATUS_SUCCESS) {
1167         if (ms_card->switch_8bit_fail) {
1168             retval = ms_pro_reset_flow(chip, 0);
1169             if (retval != STATUS_SUCCESS)
1170                 return STATUS_FAIL;
1171         } else {
1172             return STATUS_FAIL;
1173         }
1174     }
1175
1176     retval = ms_read_attribute_info(chip);
1177     if (retval != STATUS_SUCCESS)
1178         return STATUS_FAIL;
1179
1180 #ifdef XC_POWERCLASS
1181     if (CHK_HG8BIT(ms_card))
1182         change_power_class = 0;
1183
1184     if (change_power_class && CHK_MSXC(ms_card)) {
1185         u8 power_class_en = chip->ms_power_class_en;
1186
1187         dev_dbg(rtsx_dev(chip), "power_class_en = 0x%x\n",
1188             power_class_en);
1189         dev_dbg(rtsx_dev(chip), "change_power_class = %d\n",
1190             change_power_class);
1191
1192         if (change_power_class)
1193             power_class_en &= (1 << (change_power_class - 1));
1194         else
1195             power_class_en = 0;
1196
1197         if (power_class_en) {
1198             u8 power_class_mode =
1199                 (ms_card->raw_sys_info[46] & 0x18) >> 3;
1200             dev_dbg(rtsx_dev(chip), "power_class_mode = 0x%x",
1201                 power_class_mode);
1202             if (change_power_class > power_class_mode)
1203                 change_power_class = power_class_mode;
1204             if (change_power_class) {
1205                 retval = msxc_change_power(chip,
1206                                change_power_class);
1207                 if (retval != STATUS_SUCCESS) {
1208                     change_power_class--;
1209                     goto retry;
1210                 }
1211             }
1212         }
1213     }
1214 #endif
1215
1216 #ifdef SUPPORT_MAGIC_GATE
1217     retval = mg_set_tpc_para_sub(chip, 0, 0);
1218     if (retval != STATUS_SUCCESS)
1219         return STATUS_FAIL;
1220 #endif
1221
1222     if (CHK_HG8BIT(ms_card))
1223         chip->card_bus_width[chip->card2lun[MS_CARD]] = 8;
1224     else
1225         chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
1226
1227     return STATUS_SUCCESS;
1228 }
1229
1230 static int ms_read_status_reg(struct rtsx_chip *chip)
1231 {
1232     int retval;
1233     u8 val[2];
1234
1235     retval = ms_set_rw_reg_addr(chip, STATUS_REG0, 2, 0, 0);
1236     if (retval != STATUS_SUCCESS)
1237         return STATUS_FAIL;
1238
1239     retval = ms_read_bytes(chip, READ_REG, 2, NO_WAIT_INT, val, 2);
1240     if (retval != STATUS_SUCCESS)
1241         return STATUS_FAIL;
1242
1243     if (val[1] & (STS_UCDT | STS_UCEX | STS_UCFG)) {
1244         ms_set_err_code(chip, MS_FLASH_READ_ERROR);
1245         return STATUS_FAIL;
1246     }
1247
1248     return STATUS_SUCCESS;
1249 }
1250
1251 static int ms_read_extra_data(struct rtsx_chip *chip,
1252                   u16 block_addr, u8 page_num, u8 *buf, int buf_len)
1253 {
1254     struct ms_info *ms_card = &chip->ms_card;
1255     int retval, i;
1256     u8 val, data[10];
1257
1258     retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
1259                     SYSTEM_PARAM, 6);
1260     if (retval != STATUS_SUCCESS)
1261         return STATUS_FAIL;
1262
1263     if (CHK_MS4BIT(ms_card)) {
1264         /* Parallel interface */
1265         data[0] = 0x88;
1266     } else {
1267         /* Serial interface */
1268         data[0] = 0x80;
1269     }
1270     data[1] = 0;
1271     data[2] = (u8)(block_addr >> 8);
1272     data[3] = (u8)block_addr;
1273     data[4] = 0x40;
1274     data[5] = page_num;
1275
1276     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
1277         retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT,
1278                     data, 6);
1279         if (retval == STATUS_SUCCESS)
1280             break;
1281     }
1282     if (i == MS_MAX_RETRY_COUNT)
1283         return STATUS_FAIL;
1284
1285     ms_set_err_code(chip, MS_NO_ERROR);
1286
1287     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
1288         retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
1289         if (retval == STATUS_SUCCESS)
1290             break;
1291     }
1292     if (i == MS_MAX_RETRY_COUNT)
1293         return STATUS_FAIL;
1294
1295     ms_set_err_code(chip, MS_NO_ERROR);
1296     retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1297     if (retval != STATUS_SUCCESS)
1298         return STATUS_FAIL;
1299
1300     if (val & INT_REG_CMDNK) {
1301         ms_set_err_code(chip, MS_CMD_NK);
1302         return STATUS_FAIL;
1303     }
1304     if (val & INT_REG_CED) {
1305         if (val & INT_REG_ERR) {
1306             retval = ms_read_status_reg(chip);
1307             if (retval != STATUS_SUCCESS)
1308                 return STATUS_FAIL;
1309
1310             retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG,
1311                             MS_EXTRA_SIZE, SYSTEM_PARAM,
1312                             6);
1313             if (retval != STATUS_SUCCESS)
1314                 return STATUS_FAIL;
1315         }
1316     }
1317
1318     retval = ms_read_bytes(chip, READ_REG, MS_EXTRA_SIZE, NO_WAIT_INT,
1319                    data, MS_EXTRA_SIZE);
1320     if (retval != STATUS_SUCCESS)
1321         return STATUS_FAIL;
1322
1323     if (buf && buf_len) {
1324         if (buf_len > MS_EXTRA_SIZE)
1325             buf_len = MS_EXTRA_SIZE;
1326         memcpy(buf, data, buf_len);
1327     }
1328
1329     return STATUS_SUCCESS;
1330 }
1331
1332 static int ms_write_extra_data(struct rtsx_chip *chip, u16 block_addr,
1333                    u8 page_num, u8 *buf, int buf_len)
1334 {
1335     struct ms_info *ms_card = &chip->ms_card;
1336     int retval, i;
1337     u8 val, data[16];
1338
1339     if (!buf || buf_len < MS_EXTRA_SIZE)
1340         return STATUS_FAIL;
1341
1342     retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
1343                     SYSTEM_PARAM, 6 + MS_EXTRA_SIZE);
1344     if (retval != STATUS_SUCCESS)
1345         return STATUS_FAIL;
1346
1347     if (CHK_MS4BIT(ms_card))
1348         data[0] = 0x88;
1349     else
1350         data[0] = 0x80;
1351
1352     data[1] = 0;
1353     data[2] = (u8)(block_addr >> 8);
1354     data[3] = (u8)block_addr;
1355     data[4] = 0x40;
1356     data[5] = page_num;
1357
1358     for (i = 6; i < MS_EXTRA_SIZE + 6; i++)
1359         data[i] = buf[i - 6];
1360
1361     retval = ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE),
1362                 NO_WAIT_INT, data, 16);
1363     if (retval != STATUS_SUCCESS)
1364         return STATUS_FAIL;
1365
1366     retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
1367     if (retval != STATUS_SUCCESS)
1368         return STATUS_FAIL;
1369
1370     ms_set_err_code(chip, MS_NO_ERROR);
1371     retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1372     if (retval != STATUS_SUCCESS)
1373         return STATUS_FAIL;
1374
1375     if (val & INT_REG_CMDNK) {
1376         ms_set_err_code(chip, MS_CMD_NK);
1377         return STATUS_FAIL;
1378     }
1379     if (val & INT_REG_CED) {
1380         if (val & INT_REG_ERR) {
1381             ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
1382             return STATUS_FAIL;
1383         }
1384     }
1385
1386     return STATUS_SUCCESS;
1387 }
1388
1389 static int ms_read_page(struct rtsx_chip *chip, u16 block_addr, u8 page_num)
1390 {
1391     struct ms_info *ms_card = &chip->ms_card;
1392     int retval;
1393     u8 val, data[6];
1394
1395     retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
1396                     SYSTEM_PARAM, 6);
1397     if (retval != STATUS_SUCCESS)
1398         return STATUS_FAIL;
1399
1400     if (CHK_MS4BIT(ms_card))
1401         data[0] = 0x88;
1402     else
1403         data[0] = 0x80;
1404
1405     data[1] = 0;
1406     data[2] = (u8)(block_addr >> 8);
1407     data[3] = (u8)block_addr;
1408     data[4] = 0x20;
1409     data[5] = page_num;
1410
1411     retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
1412     if (retval != STATUS_SUCCESS)
1413         return STATUS_FAIL;
1414
1415     retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
1416     if (retval != STATUS_SUCCESS)
1417         return STATUS_FAIL;
1418
1419     ms_set_err_code(chip, MS_NO_ERROR);
1420     retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1421     if (retval != STATUS_SUCCESS)
1422         return STATUS_FAIL;
1423
1424     if (val & INT_REG_CMDNK) {
1425         ms_set_err_code(chip, MS_CMD_NK);
1426         return STATUS_FAIL;
1427     }
1428
1429     if (val & INT_REG_CED) {
1430         if (val & INT_REG_ERR) {
1431             if (!(val & INT_REG_BREQ)) {
1432                 ms_set_err_code(chip,  MS_FLASH_READ_ERROR);
1433                 return STATUS_FAIL;
1434             }
1435             retval = ms_read_status_reg(chip);
1436             if (retval != STATUS_SUCCESS)
1437                 ms_set_err_code(chip,  MS_FLASH_WRITE_ERROR);
1438
1439         } else {
1440             if (!(val & INT_REG_BREQ)) {
1441                 ms_set_err_code(chip, MS_BREQ_ERROR);
1442                 return STATUS_FAIL;
1443             }
1444         }
1445     }
1446
1447     retval = ms_transfer_tpc(chip, MS_TM_NORMAL_READ, READ_PAGE_DATA,
1448                  0, NO_WAIT_INT);
1449     if (retval != STATUS_SUCCESS)
1450         return STATUS_FAIL;
1451
1452     if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR))
1453         return STATUS_FAIL;
1454
1455     return STATUS_SUCCESS;
1456 }
1457
1458 static int ms_set_bad_block(struct rtsx_chip *chip, u16 phy_blk)
1459 {
1460     struct ms_info *ms_card = &chip->ms_card;
1461     int retval;
1462     u8 val, data[8], extra[MS_EXTRA_SIZE];
1463
1464     retval = ms_read_extra_data(chip, phy_blk, 0, extra, MS_EXTRA_SIZE);
1465     if (retval != STATUS_SUCCESS)
1466         return STATUS_FAIL;
1467
1468     retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
1469                     SYSTEM_PARAM, 7);
1470     if (retval != STATUS_SUCCESS)
1471         return STATUS_FAIL;
1472
1473     ms_set_err_code(chip, MS_NO_ERROR);
1474
1475     if (CHK_MS4BIT(ms_card))
1476         data[0] = 0x88;
1477     else
1478         data[0] = 0x80;
1479
1480     data[1] = 0;
1481     data[2] = (u8)(phy_blk >> 8);
1482     data[3] = (u8)phy_blk;
1483     data[4] = 0x80;
1484     data[5] = 0;
1485     data[6] = extra[0] & 0x7F;
1486     data[7] = 0xFF;
1487
1488     retval = ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT, data, 7);
1489     if (retval != STATUS_SUCCESS)
1490         return STATUS_FAIL;
1491
1492     retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
1493     if (retval != STATUS_SUCCESS)
1494         return STATUS_FAIL;
1495
1496     ms_set_err_code(chip, MS_NO_ERROR);
1497     retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1498     if (retval != STATUS_SUCCESS)
1499         return STATUS_FAIL;
1500
1501     if (val & INT_REG_CMDNK) {
1502         ms_set_err_code(chip, MS_CMD_NK);
1503         return STATUS_FAIL;
1504     }
1505
1506     if (val & INT_REG_CED) {
1507         if (val & INT_REG_ERR) {
1508             ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
1509             return STATUS_FAIL;
1510         }
1511     }
1512
1513     return STATUS_SUCCESS;
1514 }
1515
1516 static int ms_erase_block(struct rtsx_chip *chip, u16 phy_blk)
1517 {
1518     struct ms_info *ms_card = &chip->ms_card;
1519     int retval, i = 0;
1520     u8 val, data[6];
1521
1522     retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
1523                     SYSTEM_PARAM, 6);
1524     if (retval != STATUS_SUCCESS)
1525         return STATUS_FAIL;
1526
1527     ms_set_err_code(chip, MS_NO_ERROR);
1528
1529     if (CHK_MS4BIT(ms_card))
1530         data[0] = 0x88;
1531     else
1532         data[0] = 0x80;
1533
1534     data[1] = 0;
1535     data[2] = (u8)(phy_blk >> 8);
1536     data[3] = (u8)phy_blk;
1537     data[4] = 0;
1538     data[5] = 0;
1539
1540     retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT, data, 6);
1541     if (retval != STATUS_SUCCESS)
1542         return STATUS_FAIL;
1543
1544 ERASE_RTY:
1545     retval = ms_send_cmd(chip, BLOCK_ERASE, WAIT_INT);
1546     if (retval != STATUS_SUCCESS)
1547         return STATUS_FAIL;
1548
1549     ms_set_err_code(chip, MS_NO_ERROR);
1550     retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1551     if (retval != STATUS_SUCCESS)
1552         return STATUS_FAIL;
1553
1554     if (val & INT_REG_CMDNK) {
1555         if (i < 3) {
1556             i++;
1557             goto ERASE_RTY;
1558         }
1559
1560         ms_set_err_code(chip, MS_CMD_NK);
1561         ms_set_bad_block(chip, phy_blk);
1562         return STATUS_FAIL;
1563     }
1564
1565     if (val & INT_REG_CED) {
1566         if (val & INT_REG_ERR) {
1567             ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
1568             return STATUS_FAIL;
1569         }
1570     }
1571
1572     return STATUS_SUCCESS;
1573 }
1574
1575 static void ms_set_page_status(u16 log_blk, u8 type, u8 *extra, int extra_len)
1576 {
1577     if (!extra || extra_len < MS_EXTRA_SIZE)
1578         return;
1579
1580     memset(extra, 0xFF, MS_EXTRA_SIZE);
1581
1582     if (type == set_PS_NG) {
1583         /* set page status as 1:NG,and block status keep 1:OK */
1584         extra[0] = 0xB8;
1585     } else {
1586         /* set page status as 0:Data Error,and block status keep 1:OK */
1587         extra[0] = 0x98;
1588     }
1589
1590     extra[2] = (u8)(log_blk >> 8);
1591     extra[3] = (u8)log_blk;
1592 }
1593
1594 static int ms_init_page(struct rtsx_chip *chip, u16 phy_blk, u16 log_blk,
1595             u8 start_page, u8 end_page)
1596 {
1597     int retval;
1598     u8 extra[MS_EXTRA_SIZE], i;
1599
1600     memset(extra, 0xff, MS_EXTRA_SIZE);
1601
1602     extra[0] = 0xf8;    /* Block, page OK, data erased */
1603     extra[1] = 0xff;
1604     extra[2] = (u8)(log_blk >> 8);
1605     extra[3] = (u8)log_blk;
1606
1607     for (i = start_page; i < end_page; i++) {
1608         if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
1609             ms_set_err_code(chip, MS_NO_CARD);
1610             return STATUS_FAIL;
1611         }
1612
1613         retval = ms_write_extra_data(chip, phy_blk, i,
1614                          extra, MS_EXTRA_SIZE);
1615         if (retval != STATUS_SUCCESS)
1616             return STATUS_FAIL;
1617     }
1618
1619     return STATUS_SUCCESS;
1620 }
1621
1622 static int ms_copy_page(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
1623             u16 log_blk, u8 start_page, u8 end_page)
1624 {
1625     struct ms_info *ms_card = &chip->ms_card;
1626     bool uncorrect_flag = false;
1627     int retval, rty_cnt;
1628     u8 extra[MS_EXTRA_SIZE], val, i, j, data[16];
1629
1630     dev_dbg(rtsx_dev(chip), "Copy page from 0x%x to 0x%x, logical block is 0x%x\n",
1631         old_blk, new_blk, log_blk);
1632     dev_dbg(rtsx_dev(chip), "start_page = %d, end_page = %d\n",
1633         start_page, end_page);
1634
1635     retval = ms_read_extra_data(chip, new_blk, 0, extra, MS_EXTRA_SIZE);
1636     if (retval != STATUS_SUCCESS)
1637         return STATUS_FAIL;
1638
1639     retval = ms_read_status_reg(chip);
1640     if (retval != STATUS_SUCCESS)
1641         return STATUS_FAIL;
1642
1643     retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
1644     if (retval)
1645         return retval;
1646
1647     if (val & BUF_FULL) {
1648         retval = ms_send_cmd(chip, CLEAR_BUF, WAIT_INT);
1649         if (retval != STATUS_SUCCESS)
1650             return STATUS_FAIL;
1651
1652         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1653         if (retval != STATUS_SUCCESS)
1654             return STATUS_FAIL;
1655
1656         if (!(val & INT_REG_CED)) {
1657             ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
1658             return STATUS_FAIL;
1659         }
1660     }
1661
1662     for (i = start_page; i < end_page; i++) {
1663         if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
1664             ms_set_err_code(chip, MS_NO_CARD);
1665             return STATUS_FAIL;
1666         }
1667
1668         retval = ms_read_extra_data(chip, old_blk, i, extra,
1669                         MS_EXTRA_SIZE);
1670         if (retval != STATUS_SUCCESS)
1671             return STATUS_FAIL;
1672
1673         retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG,
1674                         MS_EXTRA_SIZE, SYSTEM_PARAM, 6);
1675         if (retval != STATUS_SUCCESS)
1676             return STATUS_FAIL;
1677
1678         ms_set_err_code(chip, MS_NO_ERROR);
1679
1680         if (CHK_MS4BIT(ms_card))
1681             data[0] = 0x88;
1682         else
1683             data[0] = 0x80;
1684
1685         data[1] = 0;
1686         data[2] = (u8)(old_blk >> 8);
1687         data[3] = (u8)old_blk;
1688         data[4] = 0x20;
1689         data[5] = i;
1690
1691         retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT,
1692                     data, 6);
1693         if (retval != STATUS_SUCCESS)
1694             return STATUS_FAIL;
1695
1696         retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
1697         if (retval != STATUS_SUCCESS)
1698             return STATUS_FAIL;
1699
1700         ms_set_err_code(chip, MS_NO_ERROR);
1701         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1702         if (retval != STATUS_SUCCESS)
1703             return STATUS_FAIL;
1704
1705         if (val & INT_REG_CMDNK) {
1706             ms_set_err_code(chip, MS_CMD_NK);
1707             return STATUS_FAIL;
1708         }
1709
1710         if (val & INT_REG_CED) {
1711             if (val & INT_REG_ERR) {
1712                 retval = ms_read_status_reg(chip);
1713                 if (retval != STATUS_SUCCESS) {
1714                     uncorrect_flag = true;
1715                     dev_dbg(rtsx_dev(chip), "Uncorrectable error\n");
1716                 } else {
1717                     uncorrect_flag = false;
1718                 }
1719
1720                 retval = ms_transfer_tpc(chip,
1721                              MS_TM_NORMAL_READ,
1722                              READ_PAGE_DATA,
1723                              0, NO_WAIT_INT);
1724                 if (retval != STATUS_SUCCESS)
1725                     return STATUS_FAIL;
1726
1727                 if (uncorrect_flag) {
1728                     ms_set_page_status(log_blk, set_PS_NG,
1729                                extra,
1730                                MS_EXTRA_SIZE);
1731                     if (i == 0)
1732                         extra[0] &= 0xEF;
1733
1734                     ms_write_extra_data(chip, old_blk, i,
1735                                 extra,
1736                                 MS_EXTRA_SIZE);
1737                     dev_dbg(rtsx_dev(chip), "page %d : extra[0] = 0x%x\n",
1738                         i, extra[0]);
1739                     MS_SET_BAD_BLOCK_FLG(ms_card);
1740
1741                     ms_set_page_status(log_blk,
1742                                set_PS_error, extra,
1743                                MS_EXTRA_SIZE);
1744                     ms_write_extra_data(chip, new_blk, i,
1745                                 extra,
1746                                 MS_EXTRA_SIZE);
1747                     continue;
1748                 }
1749
1750                 for (rty_cnt = 0; rty_cnt < MS_MAX_RETRY_COUNT;
1751                      rty_cnt++) {
1752                     retval = ms_transfer_tpc(chip,
1753                                  MS_TM_NORMAL_WRITE,
1754                                  WRITE_PAGE_DATA,
1755                                  0, NO_WAIT_INT);
1756                     if (retval == STATUS_SUCCESS)
1757                         break;
1758                 }
1759                 if (rty_cnt == MS_MAX_RETRY_COUNT)
1760                     return STATUS_FAIL;
1761             }
1762
1763             if (!(val & INT_REG_BREQ)) {
1764                 ms_set_err_code(chip, MS_BREQ_ERROR);
1765                 return STATUS_FAIL;
1766             }
1767         }
1768
1769         retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
1770                         SYSTEM_PARAM, (6 + MS_EXTRA_SIZE));
1771
1772         ms_set_err_code(chip, MS_NO_ERROR);
1773
1774         if (CHK_MS4BIT(ms_card))
1775             data[0] = 0x88;
1776         else
1777             data[0] = 0x80;
1778
1779         data[1] = 0;
1780         data[2] = (u8)(new_blk >> 8);
1781         data[3] = (u8)new_blk;
1782         data[4] = 0x20;
1783         data[5] = i;
1784
1785         if ((extra[0] & 0x60) != 0x60)
1786             data[6] = extra[0];
1787         else
1788             data[6] = 0xF8;
1789
1790         data[6 + 1] = 0xFF;
1791         data[6 + 2] = (u8)(log_blk >> 8);
1792         data[6 + 3] = (u8)log_blk;
1793
1794         for (j = 4; j <= MS_EXTRA_SIZE; j++)
1795             data[6 + j] = 0xFF;
1796
1797         retval = ms_write_bytes(chip, WRITE_REG, (6 + MS_EXTRA_SIZE),
1798                     NO_WAIT_INT, data, 16);
1799         if (retval != STATUS_SUCCESS)
1800             return STATUS_FAIL;
1801
1802         retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
1803         if (retval != STATUS_SUCCESS)
1804             return STATUS_FAIL;
1805
1806         ms_set_err_code(chip, MS_NO_ERROR);
1807         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
1808         if (retval != STATUS_SUCCESS)
1809             return STATUS_FAIL;
1810
1811         if (val & INT_REG_CMDNK) {
1812             ms_set_err_code(chip, MS_CMD_NK);
1813             return STATUS_FAIL;
1814         }
1815
1816         if (val & INT_REG_CED) {
1817             if (val & INT_REG_ERR) {
1818                 ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
1819                 return STATUS_FAIL;
1820             }
1821         }
1822
1823         if (i == 0) {
1824             retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG,
1825                             MS_EXTRA_SIZE, SYSTEM_PARAM,
1826                             7);
1827             if (retval != STATUS_SUCCESS)
1828                 return STATUS_FAIL;
1829
1830             ms_set_err_code(chip, MS_NO_ERROR);
1831
1832             if (CHK_MS4BIT(ms_card))
1833                 data[0] = 0x88;
1834             else
1835                 data[0] = 0x80;
1836
1837             data[1] = 0;
1838             data[2] = (u8)(old_blk >> 8);
1839             data[3] = (u8)old_blk;
1840             data[4] = 0x80;
1841             data[5] = 0;
1842             data[6] = 0xEF;
1843             data[7] = 0xFF;
1844
1845             retval = ms_write_bytes(chip, WRITE_REG, 7,
1846                         NO_WAIT_INT, data, 8);
1847             if (retval != STATUS_SUCCESS)
1848                 return STATUS_FAIL;
1849
1850             retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
1851             if (retval != STATUS_SUCCESS)
1852                 return STATUS_FAIL;
1853
1854             ms_set_err_code(chip, MS_NO_ERROR);
1855             retval = ms_read_bytes(chip, GET_INT, 1,
1856                            NO_WAIT_INT, &val, 1);
1857             if (retval != STATUS_SUCCESS)
1858                 return STATUS_FAIL;
1859
1860             if (val & INT_REG_CMDNK) {
1861                 ms_set_err_code(chip, MS_CMD_NK);
1862                 return STATUS_FAIL;
1863             }
1864
1865             if (val & INT_REG_CED) {
1866                 if (val & INT_REG_ERR) {
1867                     ms_set_err_code(chip,
1868                             MS_FLASH_WRITE_ERROR);
1869                     return STATUS_FAIL;
1870                 }
1871             }
1872         }
1873     }
1874
1875     return STATUS_SUCCESS;
1876 }
1877
1878 static int reset_ms(struct rtsx_chip *chip)
1879 {
1880     struct ms_info *ms_card = &chip->ms_card;
1881     int retval;
1882     u16 i, reg_addr, block_size;
1883     u8 val, extra[MS_EXTRA_SIZE], j, *ptr;
1884 #ifndef SUPPORT_MAGIC_GATE
1885     u16 eblock_cnt;
1886 #endif
1887
1888     retval = ms_prepare_reset(chip);
1889     if (retval != STATUS_SUCCESS)
1890         return STATUS_FAIL;
1891
1892     ms_card->ms_type |= TYPE_MS;
1893
1894     retval = ms_send_cmd(chip, MS_RESET, NO_WAIT_INT);
1895     if (retval != STATUS_SUCCESS)
1896         return STATUS_FAIL;
1897
1898     retval = ms_read_status_reg(chip);
1899     if (retval != STATUS_SUCCESS)
1900         return STATUS_FAIL;
1901
1902     retval = rtsx_read_register(chip, PPBUF_BASE2, &val);
1903     if (retval)
1904         return retval;
1905
1906     if (val & WRT_PRTCT)
1907         chip->card_wp |= MS_CARD;
1908     else
1909         chip->card_wp &= ~MS_CARD;
1910
1911     i = 0;
1912
1913 RE_SEARCH:
1914     /* Search Boot Block */
1915     while (i < (MAX_DEFECTIVE_BLOCK + 2)) {
1916         if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
1917             ms_set_err_code(chip, MS_NO_CARD);
1918             return STATUS_FAIL;
1919         }
1920
1921         retval = ms_read_extra_data(chip, i, 0, extra, MS_EXTRA_SIZE);
1922         if (retval != STATUS_SUCCESS) {
1923             i++;
1924             continue;
1925         }
1926
1927         if (extra[0] & BLOCK_OK) {
1928             if (!(extra[1] & NOT_BOOT_BLOCK)) {
1929                 ms_card->boot_block = i;
1930                 break;
1931             }
1932         }
1933         i++;
1934     }
1935
1936     if (i == (MAX_DEFECTIVE_BLOCK + 2)) {
1937         dev_dbg(rtsx_dev(chip), "No boot block found!");
1938         return STATUS_FAIL;
1939     }
1940
1941     for (j = 0; j < 3; j++) {
1942         retval = ms_read_page(chip, ms_card->boot_block, j);
1943         if (retval != STATUS_SUCCESS) {
1944             if (ms_check_err_code(chip, MS_FLASH_WRITE_ERROR)) {
1945                 i = ms_card->boot_block + 1;
1946                 ms_set_err_code(chip, MS_NO_ERROR);
1947                 goto RE_SEARCH;
1948             }
1949         }
1950     }
1951
1952     retval = ms_read_page(chip, ms_card->boot_block, 0);
1953     if (retval != STATUS_SUCCESS)
1954         return STATUS_FAIL;
1955
1956     /* Read MS system information as sys_info */
1957     rtsx_init_cmd(chip);
1958
1959     for (i = 0; i < 96; i++)
1960         rtsx_add_cmd(chip, READ_REG_CMD, PPBUF_BASE2 + 0x1A0 + i, 0, 0);
1961
1962     retval = rtsx_send_cmd(chip, MS_CARD, 100);
1963     if (retval < 0)
1964         return STATUS_FAIL;
1965
1966     ptr = rtsx_get_cmd_data(chip);
1967     memcpy(ms_card->raw_sys_info, ptr, 96);
1968
1969     /* Read useful block contents */
1970     rtsx_init_cmd(chip);
1971
1972     rtsx_add_cmd(chip, READ_REG_CMD, HEADER_ID0, 0, 0);
1973     rtsx_add_cmd(chip, READ_REG_CMD, HEADER_ID1, 0, 0);
1974
1975     for (reg_addr = DISABLED_BLOCK0; reg_addr <= DISABLED_BLOCK3;
1976          reg_addr++)
1977         rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
1978
1979     for (reg_addr = BLOCK_SIZE_0; reg_addr <= PAGE_SIZE_1; reg_addr++)
1980         rtsx_add_cmd(chip, READ_REG_CMD, reg_addr, 0, 0);
1981
1982     rtsx_add_cmd(chip, READ_REG_CMD, MS_device_type, 0, 0);
1983     rtsx_add_cmd(chip, READ_REG_CMD, MS_4bit_support, 0, 0);
1984
1985     retval = rtsx_send_cmd(chip, MS_CARD, 100);
1986     if (retval < 0)
1987         return STATUS_FAIL;
1988
1989     ptr = rtsx_get_cmd_data(chip);
1990
1991     dev_dbg(rtsx_dev(chip), "Boot block data:\n");
1992     dev_dbg(rtsx_dev(chip), "%*ph\n", 16, ptr);
1993
1994     /* Block ID error
1995      * HEADER_ID0, HEADER_ID1
1996      */
1997     if (ptr[0] != 0x00 || ptr[1] != 0x01) {
1998         i = ms_card->boot_block + 1;
1999         goto RE_SEARCH;
2000     }
2001
2002     /* Page size error
2003      * PAGE_SIZE_0, PAGE_SIZE_1
2004      */
2005     if (ptr[12] != 0x02 || ptr[13] != 0x00) {
2006         i = ms_card->boot_block + 1;
2007         goto RE_SEARCH;
2008     }
2009
2010     if (ptr[14] == 1 || ptr[14] == 3)
2011         chip->card_wp |= MS_CARD;
2012
2013     /* BLOCK_SIZE_0, BLOCK_SIZE_1 */
2014     block_size = ((u16)ptr[6] << 8) | ptr[7];
2015     if (block_size == 0x0010) {
2016         /* Block size 16KB */
2017         ms_card->block_shift = 5;
2018         ms_card->page_off = 0x1F;
2019     } else if (block_size == 0x0008) {
2020         /* Block size 8KB */
2021         ms_card->block_shift = 4;
2022         ms_card->page_off = 0x0F;
2023     }
2024
2025     /* BLOCK_COUNT_0, BLOCK_COUNT_1 */
2026     ms_card->total_block = ((u16)ptr[8] << 8) | ptr[9];
2027
2028 #ifdef SUPPORT_MAGIC_GATE
2029     j = ptr[10];
2030
2031     if (ms_card->block_shift == 4)  { /* 4MB or 8MB */
2032         if (j < 2)  { /* Effective block for 4MB: 0x1F0 */
2033             ms_card->capacity = 0x1EE0;
2034         } else { /* Effective block for 8MB: 0x3E0 */
2035             ms_card->capacity = 0x3DE0;
2036         }
2037     } else  { /* 16MB, 32MB, 64MB or 128MB */
2038         if (j < 5)  { /* Effective block for 16MB: 0x3E0 */
2039             ms_card->capacity = 0x7BC0;
2040         } else if (j < 0xA) { /* Effective block for 32MB: 0x7C0 */
2041             ms_card->capacity = 0xF7C0;
2042         } else if (j < 0x11) { /* Effective block for 64MB: 0xF80 */
2043             ms_card->capacity = 0x1EF80;
2044         } else { /* Effective block for 128MB: 0x1F00 */
2045             ms_card->capacity = 0x3DF00;
2046         }
2047     }
2048 #else
2049     /* EBLOCK_COUNT_0, EBLOCK_COUNT_1 */
2050     eblock_cnt = ((u16)ptr[10] << 8) | ptr[11];
2051
2052     ms_card->capacity = ((u32)eblock_cnt - 2) << ms_card->block_shift;
2053 #endif
2054
2055     chip->capacity[chip->card2lun[MS_CARD]] = ms_card->capacity;
2056
2057     /* Switch I/F Mode */
2058     if (ptr[15]) {
2059         retval = ms_set_rw_reg_addr(chip, 0, 0, SYSTEM_PARAM, 1);
2060         if (retval != STATUS_SUCCESS)
2061             return STATUS_FAIL;
2062
2063         retval = rtsx_write_register(chip, PPBUF_BASE2, 0xFF, 0x88);
2064         if (retval)
2065             return retval;
2066
2067         retval = rtsx_write_register(chip, PPBUF_BASE2 + 1, 0xFF, 0);
2068         if (retval)
2069             return retval;
2070
2071         retval = ms_transfer_tpc(chip, MS_TM_WRITE_BYTES, WRITE_REG, 1,
2072                      NO_WAIT_INT);
2073         if (retval != STATUS_SUCCESS)
2074             return STATUS_FAIL;
2075
2076         retval = rtsx_write_register(chip, MS_CFG,
2077                          0x58 | MS_NO_CHECK_INT,
2078                          MS_BUS_WIDTH_4 |
2079                          PUSH_TIME_ODD |
2080                          MS_NO_CHECK_INT);
2081         if (retval)
2082             return retval;
2083
2084         ms_card->ms_type |= MS_4BIT;
2085     }
2086
2087     if (CHK_MS4BIT(ms_card))
2088         chip->card_bus_width[chip->card2lun[MS_CARD]] = 4;
2089     else
2090         chip->card_bus_width[chip->card2lun[MS_CARD]] = 1;
2091
2092     return STATUS_SUCCESS;
2093 }
2094
2095 static int ms_init_l2p_tbl(struct rtsx_chip *chip)
2096 {
2097     struct ms_info *ms_card = &chip->ms_card;
2098     int size, i, seg_no, retval;
2099     u16 defect_block, reg_addr;
2100     u8 val1, val2;
2101
2102     ms_card->segment_cnt = ms_card->total_block >> 9;
2103     dev_dbg(rtsx_dev(chip), "ms_card->segment_cnt = %d\n",
2104         ms_card->segment_cnt);
2105
2106     size = ms_card->segment_cnt * sizeof(struct zone_entry);
2107     ms_card->segment = vzalloc(size);
2108     if (!ms_card->segment)
2109         return STATUS_FAIL;
2110
2111     retval = ms_read_page(chip, ms_card->boot_block, 1);
2112     if (retval != STATUS_SUCCESS)
2113         goto INIT_FAIL;
2114
2115     reg_addr = PPBUF_BASE2;
2116     for (i = 0; i < (((ms_card->total_block >> 9) * 10) + 1); i++) {
2117         int block_no;
2118
2119         retval = rtsx_read_register(chip, reg_addr++, &val1);
2120         if (retval != STATUS_SUCCESS)
2121             goto INIT_FAIL;
2122
2123         retval = rtsx_read_register(chip, reg_addr++, &val2);
2124         if (retval != STATUS_SUCCESS)
2125             goto INIT_FAIL;
2126
2127         defect_block = ((u16)val1 << 8) | val2;
2128         if (defect_block == 0xFFFF)
2129             break;
2130
2131         seg_no = defect_block / 512;
2132
2133         block_no = ms_card->segment[seg_no].disable_count++;
2134         ms_card->segment[seg_no].defect_list[block_no] = defect_block;
2135     }
2136
2137     for (i = 0; i < ms_card->segment_cnt; i++) {
2138         ms_card->segment[i].build_flag = 0;
2139         ms_card->segment[i].l2p_table = NULL;
2140         ms_card->segment[i].free_table = NULL;
2141         ms_card->segment[i].get_index = 0;
2142         ms_card->segment[i].set_index = 0;
2143         ms_card->segment[i].unused_blk_cnt = 0;
2144
2145         dev_dbg(rtsx_dev(chip), "defective block count of segment %d is %d\n",
2146             i, ms_card->segment[i].disable_count);
2147     }
2148
2149     return STATUS_SUCCESS;
2150
2151 INIT_FAIL:
2152     vfree(ms_card->segment);
2153     ms_card->segment = NULL;
2154
2155     return STATUS_FAIL;
2156 }
2157
2158 static u16 ms_get_l2p_tbl(struct rtsx_chip *chip, int seg_no, u16 log_off)
2159 {
2160     struct ms_info *ms_card = &chip->ms_card;
2161     struct zone_entry *segment;
2162
2163     if (!ms_card->segment)
2164         return 0xFFFF;
2165
2166     segment = &ms_card->segment[seg_no];
2167
2168     if (segment->l2p_table)
2169         return segment->l2p_table[log_off];
2170
2171     return 0xFFFF;
2172 }
2173
2174 static void ms_set_l2p_tbl(struct rtsx_chip *chip,
2175                int seg_no, u16 log_off, u16 phy_blk)
2176 {
2177     struct ms_info *ms_card = &chip->ms_card;
2178     struct zone_entry *segment;
2179
2180     if (!ms_card->segment)
2181         return;
2182
2183     segment = &ms_card->segment[seg_no];
2184     if (segment->l2p_table)
2185         segment->l2p_table[log_off] = phy_blk;
2186 }
2187
2188 static void ms_set_unused_block(struct rtsx_chip *chip, u16 phy_blk)
2189 {
2190     struct ms_info *ms_card = &chip->ms_card;
2191     struct zone_entry *segment;
2192     int seg_no;
2193
2194     seg_no = (int)phy_blk >> 9;
2195     segment = &ms_card->segment[seg_no];
2196
2197     segment->free_table[segment->set_index++] = phy_blk;
2198     if (segment->set_index >= MS_FREE_TABLE_CNT)
2199         segment->set_index = 0;
2200
2201     segment->unused_blk_cnt++;
2202 }
2203
2204 static u16 ms_get_unused_block(struct rtsx_chip *chip, int seg_no)
2205 {
2206     struct ms_info *ms_card = &chip->ms_card;
2207     struct zone_entry *segment;
2208     u16 phy_blk;
2209
2210     segment = &ms_card->segment[seg_no];
2211
2212     if (segment->unused_blk_cnt <= 0)
2213         return 0xFFFF;
2214
2215     phy_blk = segment->free_table[segment->get_index];
2216     segment->free_table[segment->get_index++] = 0xFFFF;
2217     if (segment->get_index >= MS_FREE_TABLE_CNT)
2218         segment->get_index = 0;
2219
2220     segment->unused_blk_cnt--;
2221
2222     return phy_blk;
2223 }
2224
2225 static const unsigned short ms_start_idx[] = {0, 494, 990, 1486, 1982, 2478,
2226                           2974, 3470, 3966, 4462, 4958,
2227                           5454, 5950, 6446, 6942, 7438,
2228                           7934};
2229
2230 static int ms_arbitrate_l2p(struct rtsx_chip *chip, u16 phy_blk,
2231                 u16 log_off, u8 us1, u8 us2)
2232 {
2233     struct ms_info *ms_card = &chip->ms_card;
2234     struct zone_entry *segment;
2235     int seg_no;
2236     u16 tmp_blk;
2237
2238     seg_no = (int)phy_blk >> 9;
2239     segment = &ms_card->segment[seg_no];
2240     tmp_blk = segment->l2p_table[log_off];
2241
2242     if (us1 != us2) {
2243         if (us1 == 0) {
2244             if (!(chip->card_wp & MS_CARD))
2245                 ms_erase_block(chip, tmp_blk);
2246
2247             ms_set_unused_block(chip, tmp_blk);
2248             segment->l2p_table[log_off] = phy_blk;
2249         } else {
2250             if (!(chip->card_wp & MS_CARD))
2251                 ms_erase_block(chip, phy_blk);
2252
2253             ms_set_unused_block(chip, phy_blk);
2254         }
2255     } else {
2256         if (phy_blk < tmp_blk) {
2257             if (!(chip->card_wp & MS_CARD))
2258                 ms_erase_block(chip, phy_blk);
2259
2260             ms_set_unused_block(chip, phy_blk);
2261         } else {
2262             if (!(chip->card_wp & MS_CARD))
2263                 ms_erase_block(chip, tmp_blk);
2264
2265             ms_set_unused_block(chip, tmp_blk);
2266             segment->l2p_table[log_off] = phy_blk;
2267         }
2268     }
2269
2270     return STATUS_SUCCESS;
2271 }
2272
2273 static int ms_build_l2p_tbl(struct rtsx_chip *chip, int seg_no)
2274 {
2275     struct ms_info *ms_card = &chip->ms_card;
2276     struct zone_entry *segment;
2277     bool defect_flag;
2278     int retval, table_size, disable_cnt, i;
2279     u16 start, end, phy_blk, log_blk, tmp_blk, idx;
2280     u8 extra[MS_EXTRA_SIZE], us1, us2;
2281
2282     dev_dbg(rtsx_dev(chip), "%s: %d\n", __func__, seg_no);
2283
2284     if (!ms_card->segment) {
2285         retval = ms_init_l2p_tbl(chip);
2286         if (retval != STATUS_SUCCESS)
2287             return retval;
2288     }
2289
2290     if (ms_card->segment[seg_no].build_flag) {
2291         dev_dbg(rtsx_dev(chip), "l2p table of segment %d has been built\n",
2292             seg_no);
2293         return STATUS_SUCCESS;
2294     }
2295
2296     if (seg_no == 0)
2297         table_size = 494;
2298     else
2299         table_size = 496;
2300
2301     segment = &ms_card->segment[seg_no];
2302
2303     if (!segment->l2p_table) {
2304         segment->l2p_table = vmalloc(array_size(table_size, 2));
2305         if (!segment->l2p_table)
2306             goto BUILD_FAIL;
2307     }
2308     memset((u8 *)(segment->l2p_table), 0xff, array_size(table_size, 2));
2309
2310     if (!segment->free_table) {
2311         segment->free_table = vmalloc(array_size(MS_FREE_TABLE_CNT, 2));
2312         if (!segment->free_table)
2313             goto BUILD_FAIL;
2314     }
2315     memset((u8 *)(segment->free_table), 0xff, array_size(MS_FREE_TABLE_CNT, 2));
2316
2317     start = (u16)seg_no << 9;
2318     end = (u16)(seg_no + 1) << 9;
2319
2320     disable_cnt = segment->disable_count;
2321
2322     segment->get_index = 0;
2323     segment->set_index = 0;
2324     segment->unused_blk_cnt = 0;
2325
2326     for (phy_blk = start; phy_blk < end; phy_blk++) {
2327         if (disable_cnt) {
2328             defect_flag = false;
2329             for (i = 0; i < segment->disable_count; i++) {
2330                 if (phy_blk == segment->defect_list[i]) {
2331                     defect_flag = true;
2332                     break;
2333                 }
2334             }
2335             if (defect_flag) {
2336                 disable_cnt--;
2337                 continue;
2338             }
2339         }
2340
2341         retval = ms_read_extra_data(chip, phy_blk, 0,
2342                         extra, MS_EXTRA_SIZE);
2343         if (retval != STATUS_SUCCESS) {
2344             dev_dbg(rtsx_dev(chip), "read extra data fail\n");
2345             ms_set_bad_block(chip, phy_blk);
2346             continue;
2347         }
2348
2349         if (seg_no == ms_card->segment_cnt - 1) {
2350             if (!(extra[1] & NOT_TRANSLATION_TABLE)) {
2351                 if (!(chip->card_wp & MS_CARD)) {
2352                     retval = ms_erase_block(chip, phy_blk);
2353                     if (retval != STATUS_SUCCESS)
2354                         continue;
2355                     extra[2] = 0xff;
2356                     extra[3] = 0xff;
2357                 }
2358             }
2359         }
2360
2361         if (!(extra[0] & BLOCK_OK))
2362             continue;
2363         if (!(extra[1] & NOT_BOOT_BLOCK))
2364             continue;
2365         if ((extra[0] & PAGE_OK) != PAGE_OK)
2366             continue;
2367
2368         log_blk = ((u16)extra[2] << 8) | extra[3];
2369
2370         if (log_blk == 0xFFFF) {
2371             if (!(chip->card_wp & MS_CARD)) {
2372                 retval = ms_erase_block(chip, phy_blk);
2373                 if (retval != STATUS_SUCCESS)
2374                     continue;
2375             }
2376             ms_set_unused_block(chip, phy_blk);
2377             continue;
2378         }
2379
2380         if (log_blk < ms_start_idx[seg_no] ||
2381             log_blk >= ms_start_idx[seg_no + 1]) {
2382             if (!(chip->card_wp & MS_CARD)) {
2383                 retval = ms_erase_block(chip, phy_blk);
2384                 if (retval != STATUS_SUCCESS)
2385                     continue;
2386             }
2387             ms_set_unused_block(chip, phy_blk);
2388             continue;
2389         }
2390
2391         idx = log_blk - ms_start_idx[seg_no];
2392
2393         if (segment->l2p_table[idx] == 0xFFFF) {
2394             segment->l2p_table[idx] = phy_blk;
2395             continue;
2396         }
2397
2398         us1 = extra[0] & 0x10;
2399         tmp_blk = segment->l2p_table[idx];
2400         retval = ms_read_extra_data(chip, tmp_blk, 0,
2401                         extra, MS_EXTRA_SIZE);
2402         if (retval != STATUS_SUCCESS)
2403             continue;
2404         us2 = extra[0] & 0x10;
2405
2406         (void)ms_arbitrate_l2p(chip, phy_blk,
2407                 log_blk - ms_start_idx[seg_no], us1, us2);
2408     }
2409
2410     segment->build_flag = 1;
2411
2412     dev_dbg(rtsx_dev(chip), "unused block count: %d\n",
2413         segment->unused_blk_cnt);
2414
2415     /* Logical Address Confirmation Process */
2416     if (seg_no == ms_card->segment_cnt - 1) {
2417         if (segment->unused_blk_cnt < 2)
2418             chip->card_wp |= MS_CARD;
2419     } else {
2420         if (segment->unused_blk_cnt < 1)
2421             chip->card_wp |= MS_CARD;
2422     }
2423
2424     if (chip->card_wp & MS_CARD)
2425         return STATUS_SUCCESS;
2426
2427     for (log_blk = ms_start_idx[seg_no];
2428          log_blk < ms_start_idx[seg_no + 1]; log_blk++) {
2429         idx = log_blk - ms_start_idx[seg_no];
2430         if (segment->l2p_table[idx] == 0xFFFF) {
2431             phy_blk = ms_get_unused_block(chip, seg_no);
2432             if (phy_blk == 0xFFFF) {
2433                 chip->card_wp |= MS_CARD;
2434                 return STATUS_SUCCESS;
2435             }
2436             retval = ms_init_page(chip, phy_blk, log_blk, 0, 1);
2437             if (retval != STATUS_SUCCESS)
2438                 goto BUILD_FAIL;
2439
2440             segment->l2p_table[idx] = phy_blk;
2441             if (seg_no == ms_card->segment_cnt - 1) {
2442                 if (segment->unused_blk_cnt < 2) {
2443                     chip->card_wp |= MS_CARD;
2444                     return STATUS_SUCCESS;
2445                 }
2446             } else {
2447                 if (segment->unused_blk_cnt < 1) {
2448                     chip->card_wp |= MS_CARD;
2449                     return STATUS_SUCCESS;
2450                 }
2451             }
2452         }
2453     }
2454
2455     /* Make boot block be the first normal block */
2456     if (seg_no == 0) {
2457         for (log_blk = 0; log_blk < 494; log_blk++) {
2458             tmp_blk = segment->l2p_table[log_blk];
2459             if (tmp_blk < ms_card->boot_block) {
2460                 dev_dbg(rtsx_dev(chip), "Boot block is not the first normal block.\n");
2461
2462                 if (chip->card_wp & MS_CARD)
2463                     break;
2464
2465                 phy_blk = ms_get_unused_block(chip, 0);
2466                 retval = ms_copy_page(chip, tmp_blk, phy_blk,
2467                               log_blk, 0,
2468                               ms_card->page_off + 1);
2469                 if (retval != STATUS_SUCCESS)
2470                     return STATUS_FAIL;
2471
2472                 segment->l2p_table[log_blk] = phy_blk;
2473
2474                 retval = ms_set_bad_block(chip, tmp_blk);
2475                 if (retval != STATUS_SUCCESS)
2476                     return STATUS_FAIL;
2477             }
2478         }
2479     }
2480
2481     return STATUS_SUCCESS;
2482
2483 BUILD_FAIL:
2484     segment->build_flag = 0;
2485     vfree(segment->l2p_table);
2486     segment->l2p_table = NULL;
2487     vfree(segment->free_table);
2488     segment->free_table = NULL;
2489
2490     return STATUS_FAIL;
2491 }
2492
2493 int reset_ms_card(struct rtsx_chip *chip)
2494 {
2495     struct ms_info *ms_card = &chip->ms_card;
2496     int seg_no = ms_card->total_block / 512 - 1;
2497     int retval;
2498
2499     memset(ms_card, 0, sizeof(struct ms_info));
2500
2501     retval = enable_card_clock(chip, MS_CARD);
2502     if (retval != STATUS_SUCCESS)
2503         return STATUS_FAIL;
2504
2505     retval = select_card(chip, MS_CARD);
2506     if (retval != STATUS_SUCCESS)
2507         return STATUS_FAIL;
2508
2509     ms_card->ms_type = 0;
2510
2511     retval = reset_ms_pro(chip);
2512     if (retval != STATUS_SUCCESS) {
2513         if (ms_card->check_ms_flow) {
2514             retval = reset_ms(chip);
2515             if (retval != STATUS_SUCCESS)
2516                 return STATUS_FAIL;
2517         } else {
2518             return STATUS_FAIL;
2519         }
2520     }
2521
2522     retval = ms_set_init_para(chip);
2523     if (retval != STATUS_SUCCESS)
2524         return STATUS_FAIL;
2525
2526     if (!CHK_MSPRO(ms_card)) {
2527         /* Build table for the last segment,
2528          * to check if L2P table block exists, erasing it
2529          */
2530         retval = ms_build_l2p_tbl(chip, seg_no);
2531         if (retval != STATUS_SUCCESS)
2532             return STATUS_FAIL;
2533     }
2534
2535     dev_dbg(rtsx_dev(chip), "ms_card->ms_type = 0x%x\n", ms_card->ms_type);
2536
2537     return STATUS_SUCCESS;
2538 }
2539
2540 static int mspro_set_rw_cmd(struct rtsx_chip *chip,
2541                 u32 start_sec, u16 sec_cnt, u8 cmd)
2542 {
2543     int retval, i;
2544     u8 data[8];
2545
2546     data[0] = cmd;
2547     data[1] = (u8)(sec_cnt >> 8);
2548     data[2] = (u8)sec_cnt;
2549     data[3] = (u8)(start_sec >> 24);
2550     data[4] = (u8)(start_sec >> 16);
2551     data[5] = (u8)(start_sec >> 8);
2552     data[6] = (u8)start_sec;
2553     data[7] = 0;
2554
2555     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
2556         retval = ms_write_bytes(chip, PRO_EX_SET_CMD, 7,
2557                     WAIT_INT, data, 8);
2558         if (retval == STATUS_SUCCESS)
2559             break;
2560     }
2561     if (i == MS_MAX_RETRY_COUNT)
2562         return STATUS_FAIL;
2563
2564     return STATUS_SUCCESS;
2565 }
2566
2567 void mspro_stop_seq_mode(struct rtsx_chip *chip)
2568 {
2569     struct ms_info *ms_card = &chip->ms_card;
2570     int retval;
2571
2572     if (ms_card->seq_mode) {
2573         retval = ms_switch_clock(chip);
2574         if (retval != STATUS_SUCCESS)
2575             return;
2576
2577         ms_card->seq_mode = 0;
2578         ms_card->total_sec_cnt = 0;
2579         ms_send_cmd(chip, PRO_STOP, WAIT_INT);
2580
2581         rtsx_write_register(chip, RBCTL, RB_FLUSH, RB_FLUSH);
2582     }
2583 }
2584
2585 static inline int ms_auto_tune_clock(struct rtsx_chip *chip)
2586 {
2587     struct ms_info *ms_card = &chip->ms_card;
2588     int retval;
2589
2590     if (chip->asic_code) {
2591         if (ms_card->ms_clock > 30)
2592             ms_card->ms_clock -= 20;
2593     } else {
2594         if (ms_card->ms_clock == CLK_80)
2595             ms_card->ms_clock = CLK_60;
2596         else if (ms_card->ms_clock == CLK_60)
2597             ms_card->ms_clock = CLK_40;
2598     }
2599
2600     retval = ms_switch_clock(chip);
2601     if (retval != STATUS_SUCCESS)
2602         return STATUS_FAIL;
2603
2604     return STATUS_SUCCESS;
2605 }
2606
2607 static int mspro_rw_multi_sector(struct scsi_cmnd *srb,
2608                  struct rtsx_chip *chip, u32 start_sector,
2609                  u16 sector_cnt)
2610 {
2611     struct ms_info *ms_card = &chip->ms_card;
2612     bool mode_2k = false;
2613     int retval;
2614     u16 count;
2615     u8 val, trans_mode, rw_tpc, rw_cmd;
2616
2617     ms_set_err_code(chip, MS_NO_ERROR);
2618
2619     ms_card->cleanup_counter = 0;
2620
2621     if (CHK_MSHG(ms_card)) {
2622         if ((start_sector % 4) || (sector_cnt % 4)) {
2623             if (srb->sc_data_direction == DMA_FROM_DEVICE) {
2624                 rw_tpc = PRO_READ_LONG_DATA;
2625                 rw_cmd = PRO_READ_DATA;
2626             } else {
2627                 rw_tpc = PRO_WRITE_LONG_DATA;
2628                 rw_cmd = PRO_WRITE_DATA;
2629             }
2630         } else {
2631             if (srb->sc_data_direction == DMA_FROM_DEVICE) {
2632                 rw_tpc = PRO_READ_QUAD_DATA;
2633                 rw_cmd = PRO_READ_2K_DATA;
2634             } else {
2635                 rw_tpc = PRO_WRITE_QUAD_DATA;
2636                 rw_cmd = PRO_WRITE_2K_DATA;
2637             }
2638             mode_2k = true;
2639         }
2640     } else {
2641         if (srb->sc_data_direction == DMA_FROM_DEVICE) {
2642             rw_tpc = PRO_READ_LONG_DATA;
2643             rw_cmd = PRO_READ_DATA;
2644         } else {
2645             rw_tpc = PRO_WRITE_LONG_DATA;
2646             rw_cmd = PRO_WRITE_DATA;
2647         }
2648     }
2649
2650     retval = ms_switch_clock(chip);
2651     if (retval != STATUS_SUCCESS)
2652         return STATUS_FAIL;
2653
2654     if (srb->sc_data_direction == DMA_FROM_DEVICE)
2655         trans_mode = MS_TM_AUTO_READ;
2656     else
2657         trans_mode = MS_TM_AUTO_WRITE;
2658
2659     retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
2660     if (retval)
2661         return retval;
2662
2663     if (ms_card->seq_mode) {
2664         if (ms_card->pre_dir != srb->sc_data_direction ||
2665             ((ms_card->pre_sec_addr + ms_card->pre_sec_cnt) !=
2666              start_sector) ||
2667             (mode_2k && (ms_card->seq_mode & MODE_512_SEQ)) ||
2668             (!mode_2k && (ms_card->seq_mode & MODE_2K_SEQ)) ||
2669             !(val & MS_INT_BREQ) ||
2670             ((ms_card->total_sec_cnt + sector_cnt) > 0xFE00)) {
2671             ms_card->seq_mode = 0;
2672             ms_card->total_sec_cnt = 0;
2673             if (val & MS_INT_BREQ) {
2674                 retval = ms_send_cmd(chip, PRO_STOP, WAIT_INT);
2675                 if (retval != STATUS_SUCCESS)
2676                     return STATUS_FAIL;
2677
2678                 rtsx_write_register(chip, RBCTL, RB_FLUSH,
2679                             RB_FLUSH);
2680             }
2681         }
2682     }
2683
2684     if (!ms_card->seq_mode) {
2685         ms_card->total_sec_cnt = 0;
2686         if (sector_cnt >= SEQ_START_CRITERIA) {
2687             if ((ms_card->capacity - start_sector) > 0xFE00)
2688                 count = 0xFE00;
2689             else
2690                 count = (u16)(ms_card->capacity - start_sector);
2691
2692             if (count > sector_cnt) {
2693                 if (mode_2k)
2694                     ms_card->seq_mode = MODE_2K_SEQ;
2695                 else
2696                     ms_card->seq_mode = MODE_512_SEQ;
2697             }
2698         } else {
2699             count = sector_cnt;
2700         }
2701         retval = mspro_set_rw_cmd(chip, start_sector, count, rw_cmd);
2702         if (retval != STATUS_SUCCESS) {
2703             ms_card->seq_mode = 0;
2704             return STATUS_FAIL;
2705         }
2706     }
2707
2708     retval = ms_transfer_data(chip, trans_mode, rw_tpc, sector_cnt,
2709                   WAIT_INT, mode_2k, scsi_sg_count(srb),
2710                   scsi_sglist(srb), scsi_bufflen(srb));
2711     if (retval != STATUS_SUCCESS) {
2712         ms_card->seq_mode = 0;
2713         rtsx_read_register(chip, MS_TRANS_CFG, &val);
2714         rtsx_clear_ms_error(chip);
2715
2716         if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
2717             chip->rw_need_retry = 0;
2718             dev_dbg(rtsx_dev(chip), "No card exist, exit %s\n",
2719                 __func__);
2720             return STATUS_FAIL;
2721         }
2722
2723         if (val & MS_INT_BREQ)
2724             ms_send_cmd(chip, PRO_STOP, WAIT_INT);
2725
2726         if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
2727             dev_dbg(rtsx_dev(chip), "MSPro CRC error, tune clock!\n");
2728             chip->rw_need_retry = 1;
2729             ms_auto_tune_clock(chip);
2730         }
2731
2732         return retval;
2733     }
2734
2735     if (ms_card->seq_mode) {
2736         ms_card->pre_sec_addr = start_sector;
2737         ms_card->pre_sec_cnt = sector_cnt;
2738         ms_card->pre_dir = srb->sc_data_direction;
2739         ms_card->total_sec_cnt += sector_cnt;
2740     }
2741
2742     return STATUS_SUCCESS;
2743 }
2744
2745 static int mspro_read_format_progress(struct rtsx_chip *chip,
2746                       const int short_data_len)
2747 {
2748     struct ms_info *ms_card = &chip->ms_card;
2749     int retval, i;
2750     u32 total_progress, cur_progress;
2751     u8 cnt, tmp;
2752     u8 data[8];
2753
2754     dev_dbg(rtsx_dev(chip), "%s, short_data_len = %d\n", __func__,
2755         short_data_len);
2756
2757     retval = ms_switch_clock(chip);
2758     if (retval != STATUS_SUCCESS) {
2759         ms_card->format_status = FORMAT_FAIL;
2760         return STATUS_FAIL;
2761     }
2762
2763     retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
2764     if (retval != STATUS_SUCCESS) {
2765         ms_card->format_status = FORMAT_FAIL;
2766         return STATUS_FAIL;
2767     }
2768
2769     if (!(tmp & MS_INT_BREQ)) {
2770         if ((tmp & (MS_INT_CED | MS_INT_BREQ | MS_INT_CMDNK |
2771                 MS_INT_ERR)) == MS_INT_CED) {
2772             ms_card->format_status = FORMAT_SUCCESS;
2773             return STATUS_SUCCESS;
2774         }
2775         ms_card->format_status = FORMAT_FAIL;
2776         return STATUS_FAIL;
2777     }
2778
2779     if (short_data_len >= 256)
2780         cnt = 0;
2781     else
2782         cnt = (u8)short_data_len;
2783
2784     retval = rtsx_write_register(chip, MS_CFG, MS_NO_CHECK_INT,
2785                      MS_NO_CHECK_INT);
2786     if (retval != STATUS_SUCCESS) {
2787         ms_card->format_status = FORMAT_FAIL;
2788         return STATUS_FAIL;
2789     }
2790
2791     retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, cnt, WAIT_INT,
2792                    data, 8);
2793     if (retval != STATUS_SUCCESS) {
2794         ms_card->format_status = FORMAT_FAIL;
2795         return STATUS_FAIL;
2796     }
2797
2798     total_progress = (data[0] << 24) | (data[1] << 16) |
2799         (data[2] << 8) | data[3];
2800     cur_progress = (data[4] << 24) | (data[5] << 16) |
2801         (data[6] << 8) | data[7];
2802
2803     dev_dbg(rtsx_dev(chip), "total_progress = %d, cur_progress = %d\n",
2804         total_progress, cur_progress);
2805
2806     if (total_progress == 0) {
2807         ms_card->progress = 0;
2808     } else {
2809         u64 ulltmp = (u64)cur_progress * (u64)65535;
2810
2811         do_div(ulltmp, total_progress);
2812         ms_card->progress = (u16)ulltmp;
2813     }
2814     dev_dbg(rtsx_dev(chip), "progress = %d\n", ms_card->progress);
2815
2816     for (i = 0; i < 5000; i++) {
2817         retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
2818         if (retval != STATUS_SUCCESS) {
2819             ms_card->format_status = FORMAT_FAIL;
2820             return STATUS_FAIL;
2821         }
2822         if (tmp & (MS_INT_CED | MS_INT_CMDNK |
2823                 MS_INT_BREQ | MS_INT_ERR))
2824             break;
2825
2826         wait_timeout(1);
2827     }
2828
2829     retval = rtsx_write_register(chip, MS_CFG, MS_NO_CHECK_INT, 0);
2830     if (retval != STATUS_SUCCESS) {
2831         ms_card->format_status = FORMAT_FAIL;
2832         return STATUS_FAIL;
2833     }
2834
2835     if (i == 5000) {
2836         ms_card->format_status = FORMAT_FAIL;
2837         return STATUS_FAIL;
2838     }
2839
2840     if (tmp & (MS_INT_CMDNK | MS_INT_ERR)) {
2841         ms_card->format_status = FORMAT_FAIL;
2842         return STATUS_FAIL;
2843     }
2844
2845     if (tmp & MS_INT_CED) {
2846         ms_card->format_status = FORMAT_SUCCESS;
2847         ms_card->pro_under_formatting = 0;
2848     } else if (tmp & MS_INT_BREQ) {
2849         ms_card->format_status = FORMAT_IN_PROGRESS;
2850     } else {
2851         ms_card->format_status = FORMAT_FAIL;
2852         ms_card->pro_under_formatting = 0;
2853         return STATUS_FAIL;
2854     }
2855
2856     return STATUS_SUCCESS;
2857 }
2858
2859 void mspro_polling_format_status(struct rtsx_chip *chip)
2860 {
2861     struct ms_info *ms_card = &chip->ms_card;
2862     int i;
2863
2864     if (ms_card->pro_under_formatting &&
2865         (rtsx_get_stat(chip) != RTSX_STAT_SS)) {
2866         rtsx_set_stat(chip, RTSX_STAT_RUN);
2867
2868         for (i = 0; i < 65535; i++) {
2869             mspro_read_format_progress(chip, MS_SHORT_DATA_LEN);
2870             if (ms_card->format_status != FORMAT_IN_PROGRESS)
2871                 break;
2872         }
2873     }
2874 }
2875
2876 int mspro_format(struct scsi_cmnd *srb, struct rtsx_chip *chip,
2877          int short_data_len, bool quick_format)
2878 {
2879     struct ms_info *ms_card = &chip->ms_card;
2880     int retval, i;
2881     u8 buf[8], tmp;
2882     u16 para;
2883
2884     retval = ms_switch_clock(chip);
2885     if (retval != STATUS_SUCCESS)
2886         return STATUS_FAIL;
2887
2888     retval = ms_set_rw_reg_addr(chip, 0x00, 0x00, PRO_TPC_PARM, 0x01);
2889     if (retval != STATUS_SUCCESS)
2890         return STATUS_FAIL;
2891
2892     memset(buf, 0, 2);
2893     switch (short_data_len) {
2894     case 32:
2895         buf[0] = 0;
2896         break;
2897     case 64:
2898         buf[0] = 1;
2899         break;
2900     case 128:
2901         buf[0] = 2;
2902         break;
2903     case 256:
2904     default:
2905         buf[0] = 3;
2906         break;
2907     }
2908
2909     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
2910         retval = ms_write_bytes(chip, PRO_WRITE_REG, 1,
2911                     NO_WAIT_INT, buf, 2);
2912         if (retval == STATUS_SUCCESS)
2913             break;
2914     }
2915     if (i == MS_MAX_RETRY_COUNT)
2916         return STATUS_FAIL;
2917
2918     if (quick_format)
2919         para = 0x0000;
2920     else
2921         para = 0x0001;
2922
2923     retval = mspro_set_rw_cmd(chip, 0, para, PRO_FORMAT);
2924     if (retval != STATUS_SUCCESS)
2925         return STATUS_FAIL;
2926
2927     retval = rtsx_read_register(chip, MS_TRANS_CFG, &tmp);
2928     if (retval)
2929         return retval;
2930
2931     if (tmp & (MS_INT_CMDNK | MS_INT_ERR))
2932         return STATUS_FAIL;
2933
2934     if ((tmp & (MS_INT_BREQ | MS_INT_CED)) == MS_INT_BREQ) {
2935         ms_card->pro_under_formatting = 1;
2936         ms_card->progress = 0;
2937         ms_card->format_status = FORMAT_IN_PROGRESS;
2938         return STATUS_SUCCESS;
2939     }
2940
2941     if (tmp & MS_INT_CED) {
2942         ms_card->pro_under_formatting = 0;
2943         ms_card->progress = 0;
2944         ms_card->format_status = FORMAT_SUCCESS;
2945         set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_NO_SENSE);
2946         return STATUS_SUCCESS;
2947     }
2948
2949     return STATUS_FAIL;
2950 }
2951
2952 static int ms_read_multiple_pages(struct rtsx_chip *chip, u16 phy_blk,
2953                   u16 log_blk, u8 start_page, u8 end_page,
2954                   u8 *buf, unsigned int *index,
2955                   unsigned int *offset)
2956 {
2957     struct ms_info *ms_card = &chip->ms_card;
2958     int retval, i;
2959     u8 extra[MS_EXTRA_SIZE], page_addr, val, trans_cfg, data[6];
2960     u8 *ptr;
2961
2962     retval = ms_read_extra_data(chip, phy_blk, start_page,
2963                     extra, MS_EXTRA_SIZE);
2964     if (retval == STATUS_SUCCESS) {
2965         if ((extra[1] & 0x30) != 0x30) {
2966             ms_set_err_code(chip, MS_FLASH_READ_ERROR);
2967             return STATUS_FAIL;
2968         }
2969     }
2970
2971     retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
2972                     SYSTEM_PARAM, 6);
2973     if (retval != STATUS_SUCCESS)
2974         return STATUS_FAIL;
2975
2976     if (CHK_MS4BIT(ms_card))
2977         data[0] = 0x88;
2978     else
2979         data[0] = 0x80;
2980
2981     data[1] = 0;
2982     data[2] = (u8)(phy_blk >> 8);
2983     data[3] = (u8)phy_blk;
2984     data[4] = 0;
2985     data[5] = start_page;
2986
2987     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
2988         retval = ms_write_bytes(chip, WRITE_REG, 6, NO_WAIT_INT,
2989                     data, 6);
2990         if (retval == STATUS_SUCCESS)
2991             break;
2992     }
2993     if (i == MS_MAX_RETRY_COUNT)
2994         return STATUS_FAIL;
2995
2996     ms_set_err_code(chip, MS_NO_ERROR);
2997
2998     retval = ms_send_cmd(chip, BLOCK_READ, WAIT_INT);
2999     if (retval != STATUS_SUCCESS)
3000         return STATUS_FAIL;
3001
3002     ptr = buf;
3003
3004     for (page_addr = start_page; page_addr < end_page; page_addr++) {
3005         ms_set_err_code(chip, MS_NO_ERROR);
3006
3007         if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
3008             ms_set_err_code(chip, MS_NO_CARD);
3009             return STATUS_FAIL;
3010         }
3011
3012         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
3013         if (retval != STATUS_SUCCESS)
3014             return STATUS_FAIL;
3015
3016         if (val & INT_REG_CMDNK) {
3017             ms_set_err_code(chip, MS_CMD_NK);
3018             return STATUS_FAIL;
3019         }
3020         if (val & INT_REG_ERR) {
3021             if (val & INT_REG_BREQ) {
3022                 retval = ms_read_status_reg(chip);
3023                 if (retval != STATUS_SUCCESS) {
3024                     if (!(chip->card_wp & MS_CARD)) {
3025                         reset_ms(chip);
3026                         ms_set_page_status
3027                             (log_blk, set_PS_NG,
3028                              extra,
3029                              MS_EXTRA_SIZE);
3030                         ms_write_extra_data
3031                             (chip, phy_blk,
3032                              page_addr, extra,
3033                              MS_EXTRA_SIZE);
3034                     }
3035                     ms_set_err_code(chip,
3036                             MS_FLASH_READ_ERROR);
3037                     return STATUS_FAIL;
3038                 }
3039             } else {
3040                 ms_set_err_code(chip, MS_FLASH_READ_ERROR);
3041                 return STATUS_FAIL;
3042             }
3043         } else {
3044             if (!(val & INT_REG_BREQ)) {
3045                 ms_set_err_code(chip, MS_BREQ_ERROR);
3046                 return STATUS_FAIL;
3047             }
3048         }
3049
3050         if (page_addr == (end_page - 1)) {
3051             if (!(val & INT_REG_CED)) {
3052                 retval = ms_send_cmd(chip, BLOCK_END, WAIT_INT);
3053                 if (retval != STATUS_SUCCESS)
3054                     return STATUS_FAIL;
3055             }
3056
3057             retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT,
3058                            &val, 1);
3059             if (retval != STATUS_SUCCESS)
3060                 return STATUS_FAIL;
3061
3062             if (!(val & INT_REG_CED)) {
3063                 ms_set_err_code(chip, MS_FLASH_READ_ERROR);
3064                 return STATUS_FAIL;
3065             }
3066
3067             trans_cfg = NO_WAIT_INT;
3068         } else {
3069             trans_cfg = WAIT_INT;
3070         }
3071
3072         rtsx_init_cmd(chip);
3073
3074         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC, 0xFF, READ_PAGE_DATA);
3075         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG,
3076                  0xFF, trans_cfg);
3077         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
3078                  0x01, RING_BUFFER);
3079
3080         trans_dma_enable(DMA_FROM_DEVICE, chip, 512, DMA_512);
3081
3082         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
3083                  MS_TRANSFER_START |  MS_TM_NORMAL_READ);
3084         rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
3085                  MS_TRANSFER_END, MS_TRANSFER_END);
3086
3087         rtsx_send_cmd_no_wait(chip);
3088
3089         retval = rtsx_transfer_data_partial(chip, MS_CARD, ptr, 512,
3090                             scsi_sg_count(chip->srb),
3091                             index, offset,
3092                             DMA_FROM_DEVICE,
3093                             chip->ms_timeout);
3094         if (retval < 0) {
3095             if (retval == -ETIMEDOUT) {
3096                 ms_set_err_code(chip, MS_TO_ERROR);
3097                 rtsx_clear_ms_error(chip);
3098                 return STATUS_TIMEDOUT;
3099             }
3100
3101             retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
3102             if (retval != STATUS_SUCCESS) {
3103                 ms_set_err_code(chip, MS_TO_ERROR);
3104                 rtsx_clear_ms_error(chip);
3105                 return STATUS_TIMEDOUT;
3106             }
3107             if (val & (MS_CRC16_ERR | MS_RDY_TIMEOUT)) {
3108                 ms_set_err_code(chip, MS_CRC16_ERROR);
3109                 rtsx_clear_ms_error(chip);
3110                 return STATUS_FAIL;
3111             }
3112         }
3113
3114         if (scsi_sg_count(chip->srb) == 0)
3115             ptr += 512;
3116     }
3117
3118     return STATUS_SUCCESS;
3119 }
3120
3121 static int ms_write_multiple_pages(struct rtsx_chip *chip, u16 old_blk,
3122                    u16 new_blk, u16 log_blk, u8 start_page,
3123                 u8 end_page, u8 *buf, unsigned int *index,
3124                 unsigned int *offset)
3125 {
3126     struct ms_info *ms_card = &chip->ms_card;
3127     int retval, i;
3128     u8 page_addr, val, data[16];
3129     u8 *ptr;
3130
3131     if (!start_page) {
3132         retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
3133                         SYSTEM_PARAM, 7);
3134         if (retval != STATUS_SUCCESS)
3135             return STATUS_FAIL;
3136
3137         if (CHK_MS4BIT(ms_card))
3138             data[0] = 0x88;
3139         else
3140             data[0] = 0x80;
3141
3142         data[1] = 0;
3143         data[2] = (u8)(old_blk >> 8);
3144         data[3] = (u8)old_blk;
3145         data[4] = 0x80;
3146         data[5] = 0;
3147         data[6] = 0xEF;
3148         data[7] = 0xFF;
3149
3150         retval = ms_write_bytes(chip, WRITE_REG, 7, NO_WAIT_INT,
3151                     data, 8);
3152         if (retval != STATUS_SUCCESS)
3153             return STATUS_FAIL;
3154
3155         retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
3156         if (retval != STATUS_SUCCESS)
3157             return STATUS_FAIL;
3158
3159         ms_set_err_code(chip, MS_NO_ERROR);
3160         retval = ms_transfer_tpc(chip, MS_TM_READ_BYTES, GET_INT, 1,
3161                      NO_WAIT_INT);
3162         if (retval != STATUS_SUCCESS)
3163             return STATUS_FAIL;
3164     }
3165
3166     retval = ms_set_rw_reg_addr(chip, OVERWRITE_FLAG, MS_EXTRA_SIZE,
3167                     SYSTEM_PARAM, (6 + MS_EXTRA_SIZE));
3168     if (retval != STATUS_SUCCESS)
3169         return STATUS_FAIL;
3170
3171     ms_set_err_code(chip, MS_NO_ERROR);
3172
3173     if (CHK_MS4BIT(ms_card))
3174         data[0] = 0x88;
3175     else
3176         data[0] = 0x80;
3177
3178     data[1] = 0;
3179     data[2] = (u8)(new_blk >> 8);
3180     data[3] = (u8)new_blk;
3181     if ((end_page - start_page) == 1)
3182         data[4] = 0x20;
3183     else
3184         data[4] = 0;
3185
3186     data[5] = start_page;
3187     data[6] = 0xF8;
3188     data[7] = 0xFF;
3189     data[8] = (u8)(log_blk >> 8);
3190     data[9] = (u8)log_blk;
3191
3192     for (i = 0x0A; i < 0x10; i++)
3193         data[i] = 0xFF;
3194
3195     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
3196         retval = ms_write_bytes(chip, WRITE_REG, 6 + MS_EXTRA_SIZE,
3197                     NO_WAIT_INT, data, 16);
3198         if (retval == STATUS_SUCCESS)
3199             break;
3200     }
3201     if (i == MS_MAX_RETRY_COUNT)
3202         return STATUS_FAIL;
3203
3204     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
3205         retval = ms_send_cmd(chip, BLOCK_WRITE, WAIT_INT);
3206         if (retval == STATUS_SUCCESS)
3207             break;
3208     }
3209     if (i == MS_MAX_RETRY_COUNT)
3210         return STATUS_FAIL;
3211
3212     retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
3213     if (retval != STATUS_SUCCESS)
3214         return STATUS_FAIL;
3215
3216     ptr = buf;
3217     for (page_addr = start_page; page_addr < end_page; page_addr++) {
3218         ms_set_err_code(chip, MS_NO_ERROR);
3219
3220         if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
3221             ms_set_err_code(chip, MS_NO_CARD);
3222             return STATUS_FAIL;
3223         }
3224
3225         if (val & INT_REG_CMDNK) {
3226             ms_set_err_code(chip, MS_CMD_NK);
3227             return STATUS_FAIL;
3228         }
3229         if (val & INT_REG_ERR) {
3230             ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
3231             return STATUS_FAIL;
3232         }
3233         if (!(val & INT_REG_BREQ)) {
3234             ms_set_err_code(chip, MS_BREQ_ERROR);
3235             return STATUS_FAIL;
3236         }
3237
3238         udelay(30);
3239
3240         rtsx_init_cmd(chip);
3241
3242         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC,
3243                  0xFF, WRITE_PAGE_DATA);
3244         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG,
3245                  0xFF, WAIT_INT);
3246         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
3247                  0x01, RING_BUFFER);
3248
3249         trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
3250
3251         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
3252                  MS_TRANSFER_START |  MS_TM_NORMAL_WRITE);
3253         rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
3254                  MS_TRANSFER_END, MS_TRANSFER_END);
3255
3256         rtsx_send_cmd_no_wait(chip);
3257
3258         retval = rtsx_transfer_data_partial(chip, MS_CARD, ptr, 512,
3259                             scsi_sg_count(chip->srb),
3260                             index, offset,
3261                             DMA_TO_DEVICE,
3262                             chip->ms_timeout);
3263         if (retval < 0) {
3264             ms_set_err_code(chip, MS_TO_ERROR);
3265             rtsx_clear_ms_error(chip);
3266
3267             if (retval == -ETIMEDOUT)
3268                 return STATUS_TIMEDOUT;
3269             return STATUS_FAIL;
3270         }
3271
3272         retval = ms_read_bytes(chip, GET_INT, 1, NO_WAIT_INT, &val, 1);
3273         if (retval != STATUS_SUCCESS)
3274             return STATUS_FAIL;
3275
3276         if ((end_page - start_page) == 1) {
3277             if (!(val & INT_REG_CED)) {
3278                 ms_set_err_code(chip, MS_FLASH_WRITE_ERROR);
3279                 return STATUS_FAIL;
3280             }
3281         } else {
3282             if (page_addr == (end_page - 1)) {
3283                 if (!(val & INT_REG_CED)) {
3284                     retval = ms_send_cmd(chip, BLOCK_END,
3285                                  WAIT_INT);
3286                     if (retval != STATUS_SUCCESS)
3287                         return STATUS_FAIL;
3288                 }
3289
3290                 retval = ms_read_bytes(chip, GET_INT, 1,
3291                                NO_WAIT_INT, &val, 1);
3292                 if (retval != STATUS_SUCCESS)
3293                     return STATUS_FAIL;
3294             }
3295
3296             if (page_addr == (end_page - 1) ||
3297                 page_addr == ms_card->page_off) {
3298                 if (!(val & INT_REG_CED)) {
3299                     ms_set_err_code(chip,
3300                             MS_FLASH_WRITE_ERROR);
3301                     return STATUS_FAIL;
3302                 }
3303             }
3304         }
3305
3306         if (scsi_sg_count(chip->srb) == 0)
3307             ptr += 512;
3308     }
3309
3310     return STATUS_SUCCESS;
3311 }
3312
3313 static int ms_finish_write(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
3314                u16 log_blk, u8 page_off)
3315 {
3316     struct ms_info *ms_card = &chip->ms_card;
3317     int retval, seg_no;
3318
3319     retval = ms_copy_page(chip, old_blk, new_blk, log_blk,
3320                   page_off, ms_card->page_off + 1);
3321     if (retval != STATUS_SUCCESS)
3322         return STATUS_FAIL;
3323
3324     seg_no = old_blk >> 9;
3325
3326     if (MS_TST_BAD_BLOCK_FLG(ms_card)) {
3327         MS_CLR_BAD_BLOCK_FLG(ms_card);
3328         ms_set_bad_block(chip, old_blk);
3329     } else {
3330         retval = ms_erase_block(chip, old_blk);
3331         if (retval == STATUS_SUCCESS)
3332             ms_set_unused_block(chip, old_blk);
3333     }
3334
3335     ms_set_l2p_tbl(chip, seg_no, log_blk - ms_start_idx[seg_no], new_blk);
3336
3337     return STATUS_SUCCESS;
3338 }
3339
3340 static int ms_prepare_write(struct rtsx_chip *chip, u16 old_blk, u16 new_blk,
3341                 u16 log_blk, u8 start_page)
3342 {
3343     int retval;
3344
3345     if (start_page) {
3346         retval = ms_copy_page(chip, old_blk, new_blk, log_blk,
3347                       0, start_page);
3348         if (retval != STATUS_SUCCESS)
3349             return STATUS_FAIL;
3350     }
3351
3352     return STATUS_SUCCESS;
3353 }
3354
3355 #ifdef MS_DELAY_WRITE
3356 int ms_delay_write(struct rtsx_chip *chip)
3357 {
3358     struct ms_info *ms_card = &chip->ms_card;
3359     struct ms_delay_write_tag *delay_write = &ms_card->delay_write;
3360     int retval;
3361
3362     if (delay_write->delay_write_flag) {
3363         retval = ms_set_init_para(chip);
3364         if (retval != STATUS_SUCCESS)
3365             return STATUS_FAIL;
3366
3367         delay_write->delay_write_flag = 0;
3368         retval = ms_finish_write(chip,
3369                      delay_write->old_phyblock,
3370                     delay_write->new_phyblock,
3371                     delay_write->logblock,
3372                     delay_write->pageoff);
3373         if (retval != STATUS_SUCCESS)
3374             return STATUS_FAIL;
3375     }
3376
3377     return STATUS_SUCCESS;
3378 }
3379 #endif
3380
3381 static inline void ms_rw_fail(struct scsi_cmnd *srb, struct rtsx_chip *chip)
3382 {
3383     if (srb->sc_data_direction == DMA_FROM_DEVICE)
3384         set_sense_type(chip, SCSI_LUN(srb),
3385                    SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
3386     else
3387         set_sense_type(chip, SCSI_LUN(srb), SENSE_TYPE_MEDIA_WRITE_ERR);
3388 }
3389
3390 static int ms_rw_multi_sector(struct scsi_cmnd *srb, struct rtsx_chip *chip,
3391                   u32 start_sector, u16 sector_cnt)
3392 {
3393     struct ms_info *ms_card = &chip->ms_card;
3394     unsigned int lun = SCSI_LUN(srb);
3395     int retval, seg_no;
3396     unsigned int index = 0, offset = 0;
3397     u16 old_blk = 0, new_blk = 0, log_blk, total_sec_cnt = sector_cnt;
3398     u8 start_page, end_page = 0, page_cnt;
3399     u8 *ptr;
3400 #ifdef MS_DELAY_WRITE
3401     struct ms_delay_write_tag *delay_write = &ms_card->delay_write;
3402 #endif
3403
3404     ms_set_err_code(chip, MS_NO_ERROR);
3405
3406     ms_card->cleanup_counter = 0;
3407
3408     ptr = (u8 *)scsi_sglist(srb);
3409
3410     retval = ms_switch_clock(chip);
3411     if (retval != STATUS_SUCCESS) {
3412         ms_rw_fail(srb, chip);
3413         return STATUS_FAIL;
3414     }
3415
3416     log_blk = (u16)(start_sector >> ms_card->block_shift);
3417     start_page = (u8)(start_sector & ms_card->page_off);
3418
3419     for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1; seg_no++) {
3420         if (log_blk < ms_start_idx[seg_no + 1])
3421             break;
3422     }
3423
3424     if (ms_card->segment[seg_no].build_flag == 0) {
3425         retval = ms_build_l2p_tbl(chip, seg_no);
3426         if (retval != STATUS_SUCCESS) {
3427             chip->card_fail |= MS_CARD;
3428             set_sense_type(chip, lun, SENSE_TYPE_MEDIA_NOT_PRESENT);
3429             return STATUS_FAIL;
3430         }
3431     }
3432
3433     if (srb->sc_data_direction == DMA_TO_DEVICE) {
3434 #ifdef MS_DELAY_WRITE
3435         if (delay_write->delay_write_flag &&
3436             delay_write->logblock == log_blk &&
3437             start_page > delay_write->pageoff) {
3438             delay_write->delay_write_flag = 0;
3439             retval = ms_copy_page(chip,
3440                           delay_write->old_phyblock,
3441                           delay_write->new_phyblock,
3442                           log_blk,
3443                           delay_write->pageoff, start_page);
3444             if (retval != STATUS_SUCCESS) {
3445                 set_sense_type(chip, lun,
3446                            SENSE_TYPE_MEDIA_WRITE_ERR);
3447                 return STATUS_FAIL;
3448             }
3449             old_blk = delay_write->old_phyblock;
3450             new_blk = delay_write->new_phyblock;
3451         } else if (delay_write->delay_write_flag &&
3452                 (delay_write->logblock == log_blk) &&
3453                 (start_page == delay_write->pageoff)) {
3454             delay_write->delay_write_flag = 0;
3455             old_blk = delay_write->old_phyblock;
3456             new_blk = delay_write->new_phyblock;
3457         } else {
3458             retval = ms_delay_write(chip);
3459             if (retval != STATUS_SUCCESS) {
3460                 set_sense_type(chip, lun,
3461                            SENSE_TYPE_MEDIA_WRITE_ERR);
3462                 return STATUS_FAIL;
3463             }
3464 #endif
3465             old_blk = ms_get_l2p_tbl
3466                     (chip, seg_no,
3467                      log_blk - ms_start_idx[seg_no]);
3468             new_blk  = ms_get_unused_block(chip, seg_no);
3469             if (old_blk == 0xFFFF || new_blk == 0xFFFF) {
3470                 set_sense_type(chip, lun,
3471                            SENSE_TYPE_MEDIA_WRITE_ERR);
3472                 return STATUS_FAIL;
3473             }
3474
3475             retval = ms_prepare_write(chip, old_blk, new_blk,
3476                           log_blk, start_page);
3477             if (retval != STATUS_SUCCESS) {
3478                 if (detect_card_cd(chip, MS_CARD) !=
3479                     STATUS_SUCCESS) {
3480                     set_sense_type
3481                         (chip, lun,
3482                         SENSE_TYPE_MEDIA_NOT_PRESENT);
3483                     return STATUS_FAIL;
3484                 }
3485                 set_sense_type(chip, lun,
3486                            SENSE_TYPE_MEDIA_WRITE_ERR);
3487                 return STATUS_FAIL;
3488             }
3489 #ifdef MS_DELAY_WRITE
3490         }
3491 #endif
3492     } else {
3493 #ifdef MS_DELAY_WRITE
3494         retval = ms_delay_write(chip);
3495         if (retval != STATUS_SUCCESS) {
3496             if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
3497                 set_sense_type(chip, lun,
3498                            SENSE_TYPE_MEDIA_NOT_PRESENT);
3499                 return STATUS_FAIL;
3500             }
3501             set_sense_type(chip, lun,
3502                        SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
3503             return STATUS_FAIL;
3504         }
3505 #endif
3506         old_blk = ms_get_l2p_tbl(chip, seg_no,
3507                      log_blk - ms_start_idx[seg_no]);
3508         if (old_blk == 0xFFFF) {
3509             set_sense_type(chip, lun,
3510                        SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
3511             return STATUS_FAIL;
3512         }
3513     }
3514
3515     dev_dbg(rtsx_dev(chip), "seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n",
3516         seg_no, old_blk, new_blk);
3517
3518     while (total_sec_cnt) {
3519         if ((start_page + total_sec_cnt) > (ms_card->page_off + 1))
3520             end_page = ms_card->page_off + 1;
3521         else
3522             end_page = start_page + (u8)total_sec_cnt;
3523
3524         page_cnt = end_page - start_page;
3525
3526         dev_dbg(rtsx_dev(chip), "start_page = %d, end_page = %d, page_cnt = %d\n",
3527             start_page, end_page, page_cnt);
3528
3529         if (srb->sc_data_direction == DMA_FROM_DEVICE) {
3530             retval = ms_read_multiple_pages(chip,
3531                             old_blk, log_blk,
3532                             start_page, end_page,
3533                             ptr, &index, &offset);
3534         } else {
3535             retval = ms_write_multiple_pages(chip, old_blk, new_blk,
3536                              log_blk, start_page,
3537                              end_page, ptr, &index,
3538                              &offset);
3539         }
3540
3541         if (retval != STATUS_SUCCESS) {
3542             toggle_gpio(chip, 1);
3543             if (detect_card_cd(chip, MS_CARD) != STATUS_SUCCESS) {
3544                 set_sense_type(chip, lun,
3545                            SENSE_TYPE_MEDIA_NOT_PRESENT);
3546                 return STATUS_FAIL;
3547             }
3548             ms_rw_fail(srb, chip);
3549             return STATUS_FAIL;
3550         }
3551
3552         if (srb->sc_data_direction == DMA_TO_DEVICE) {
3553             if (end_page == (ms_card->page_off + 1)) {
3554                 retval = ms_erase_block(chip, old_blk);
3555                 if (retval == STATUS_SUCCESS)
3556                     ms_set_unused_block(chip, old_blk);
3557
3558                 ms_set_l2p_tbl(chip, seg_no,
3559                            log_blk - ms_start_idx[seg_no],
3560                            new_blk);
3561             }
3562         }
3563
3564         total_sec_cnt -= page_cnt;
3565         if (scsi_sg_count(srb) == 0)
3566             ptr += page_cnt * 512;
3567
3568         if (total_sec_cnt == 0)
3569             break;
3570
3571         log_blk++;
3572
3573         for (seg_no = 0; seg_no < ARRAY_SIZE(ms_start_idx) - 1;
3574                 seg_no++) {
3575             if (log_blk < ms_start_idx[seg_no + 1])
3576                 break;
3577         }
3578
3579         if (ms_card->segment[seg_no].build_flag == 0) {
3580             retval = ms_build_l2p_tbl(chip, seg_no);
3581             if (retval != STATUS_SUCCESS) {
3582                 chip->card_fail |= MS_CARD;
3583                 set_sense_type(chip, lun,
3584                            SENSE_TYPE_MEDIA_NOT_PRESENT);
3585                 return STATUS_FAIL;
3586             }
3587         }
3588
3589         old_blk = ms_get_l2p_tbl(chip, seg_no,
3590                      log_blk - ms_start_idx[seg_no]);
3591         if (old_blk == 0xFFFF) {
3592             ms_rw_fail(srb, chip);
3593             return STATUS_FAIL;
3594         }
3595
3596         if (srb->sc_data_direction == DMA_TO_DEVICE) {
3597             new_blk = ms_get_unused_block(chip, seg_no);
3598             if (new_blk == 0xFFFF) {
3599                 ms_rw_fail(srb, chip);
3600                 return STATUS_FAIL;
3601             }
3602         }
3603
3604         dev_dbg(rtsx_dev(chip), "seg_no = %d, old_blk = 0x%x, new_blk = 0x%x\n",
3605             seg_no, old_blk, new_blk);
3606
3607         start_page = 0;
3608     }
3609
3610     if (srb->sc_data_direction == DMA_TO_DEVICE) {
3611         if (end_page < (ms_card->page_off + 1)) {
3612 #ifdef MS_DELAY_WRITE
3613             delay_write->delay_write_flag = 1;
3614             delay_write->old_phyblock = old_blk;
3615             delay_write->new_phyblock = new_blk;
3616             delay_write->logblock = log_blk;
3617             delay_write->pageoff = end_page;
3618 #else
3619             retval = ms_finish_write(chip, old_blk, new_blk,
3620                          log_blk, end_page);
3621             if (retval != STATUS_SUCCESS) {
3622                 if (detect_card_cd(chip, MS_CARD) !=
3623                     STATUS_SUCCESS) {
3624                     set_sense_type
3625                         (chip, lun,
3626                         SENSE_TYPE_MEDIA_NOT_PRESENT);
3627                     return STATUS_FAIL;
3628                 }
3629
3630                 ms_rw_fail(srb, chip);
3631                 return STATUS_FAIL;
3632             }
3633 #endif
3634         }
3635     }
3636
3637     scsi_set_resid(srb, 0);
3638
3639     return STATUS_SUCCESS;
3640 }
3641
3642 int ms_rw(struct scsi_cmnd *srb, struct rtsx_chip *chip,
3643       u32 start_sector, u16 sector_cnt)
3644 {
3645     struct ms_info *ms_card = &chip->ms_card;
3646     int retval;
3647
3648     if (CHK_MSPRO(ms_card))
3649         retval = mspro_rw_multi_sector(srb, chip, start_sector,
3650                            sector_cnt);
3651     else
3652         retval = ms_rw_multi_sector(srb, chip, start_sector,
3653                         sector_cnt);
3654
3655     return retval;
3656 }
3657
3658 void ms_free_l2p_tbl(struct rtsx_chip *chip)
3659 {
3660     struct ms_info *ms_card = &chip->ms_card;
3661     int i = 0;
3662
3663     if (ms_card->segment) {
3664         for (i = 0; i < ms_card->segment_cnt; i++) {
3665             vfree(ms_card->segment[i].l2p_table);
3666             ms_card->segment[i].l2p_table = NULL;
3667             vfree(ms_card->segment[i].free_table);
3668             ms_card->segment[i].free_table = NULL;
3669         }
3670         vfree(ms_card->segment);
3671         ms_card->segment = NULL;
3672     }
3673 }
3674
3675 #ifdef SUPPORT_MAGIC_GATE
3676
3677 #ifdef READ_BYTES_WAIT_INT
3678 static int ms_poll_int(struct rtsx_chip *chip)
3679 {
3680     int retval;
3681     u8 val;
3682
3683     rtsx_init_cmd(chip);
3684
3685     rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANS_CFG, MS_INT_CED, MS_INT_CED);
3686
3687     retval = rtsx_send_cmd(chip, MS_CARD, 5000);
3688     if (retval != STATUS_SUCCESS)
3689         return STATUS_FAIL;
3690
3691     val = *rtsx_get_cmd_data(chip);
3692     if (val & MS_INT_ERR)
3693         return STATUS_FAIL;
3694
3695     return STATUS_SUCCESS;
3696 }
3697 #endif
3698
3699 #ifdef MS_SAMPLE_INT_ERR
3700 static int check_ms_err(struct rtsx_chip *chip)
3701 {
3702     int retval;
3703     u8 val;
3704
3705     retval = rtsx_read_register(chip, MS_TRANSFER, &val);
3706     if (retval != STATUS_SUCCESS)
3707         return 1;
3708     if (val & MS_TRANSFER_ERR)
3709         return 1;
3710
3711     retval = rtsx_read_register(chip, MS_TRANS_CFG, &val);
3712     if (retval != STATUS_SUCCESS)
3713         return 1;
3714
3715     if (val & (MS_INT_ERR | MS_INT_CMDNK))
3716         return 1;
3717
3718     return 0;
3719 }
3720 #else
3721 static int check_ms_err(struct rtsx_chip *chip)
3722 {
3723     int retval;
3724     u8 val;
3725
3726     retval = rtsx_read_register(chip, MS_TRANSFER, &val);
3727     if (retval != STATUS_SUCCESS)
3728         return 1;
3729     if (val & MS_TRANSFER_ERR)
3730         return 1;
3731
3732     return 0;
3733 }
3734 #endif
3735
3736 static int mg_send_ex_cmd(struct rtsx_chip *chip, u8 cmd, u8 entry_num)
3737 {
3738     int retval, i;
3739     u8 data[8];
3740
3741     data[0] = cmd;
3742     data[1] = 0;
3743     data[2] = 0;
3744     data[3] = 0;
3745     data[4] = 0;
3746     data[5] = 0;
3747     data[6] = entry_num;
3748     data[7] = 0;
3749
3750     for (i = 0; i < MS_MAX_RETRY_COUNT; i++) {
3751         retval = ms_write_bytes(chip, PRO_EX_SET_CMD, 7, WAIT_INT,
3752                     data, 8);
3753         if (retval == STATUS_SUCCESS)
3754             break;
3755     }
3756     if (i == MS_MAX_RETRY_COUNT)
3757         return STATUS_FAIL;
3758
3759     if (check_ms_err(chip)) {
3760         rtsx_clear_ms_error(chip);
3761         return STATUS_FAIL;
3762     }
3763
3764     return STATUS_SUCCESS;
3765 }
3766
3767 static int mg_set_tpc_para_sub(struct rtsx_chip *chip, int type,
3768                    u8 mg_entry_num)
3769 {
3770     int retval;
3771     u8 buf[6];
3772
3773     if (type == 0)
3774         retval = ms_set_rw_reg_addr(chip, 0, 0, PRO_TPC_PARM, 1);
3775     else
3776         retval = ms_set_rw_reg_addr(chip, 0, 0, PRO_DATA_COUNT1, 6);
3777
3778     if (retval != STATUS_SUCCESS)
3779         return STATUS_FAIL;
3780
3781     buf[0] = 0;
3782     buf[1] = 0;
3783     if (type == 1) {
3784         buf[2] = 0;
3785         buf[3] = 0;
3786         buf[4] = 0;
3787         buf[5] = mg_entry_num;
3788     }
3789     retval = ms_write_bytes(chip, PRO_WRITE_REG, (type == 0) ? 1 : 6,
3790                 NO_WAIT_INT, buf, 6);
3791     if (retval != STATUS_SUCCESS)
3792         return STATUS_FAIL;
3793
3794     return STATUS_SUCCESS;
3795 }
3796
3797 int mg_set_leaf_id(struct scsi_cmnd *srb, struct rtsx_chip *chip)
3798 {
3799     int retval;
3800     int i;
3801     unsigned int lun = SCSI_LUN(srb);
3802     u8 buf1[32], buf2[12];
3803
3804     if (scsi_bufflen(srb) < 12) {
3805         set_sense_type(chip, lun, SENSE_TYPE_MEDIA_INVALID_CMD_FIELD);
3806         return STATUS_FAIL;
3807     }
3808
3809     ms_cleanup_work(chip);
3810
3811     retval = ms_switch_clock(chip);
3812     if (retval != STATUS_SUCCESS)
3813         return STATUS_FAIL;
3814
3815     retval = mg_send_ex_cmd(chip, MG_SET_LID, 0);
3816     if (retval != STATUS_SUCCESS) {
3817         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
3818         return STATUS_FAIL;
3819     }
3820
3821     memset(buf1, 0, 32);
3822     rtsx_stor_get_xfer_buf(buf2, min_t(int, 12, scsi_bufflen(srb)), srb);
3823     for (i = 0; i < 8; i++)
3824         buf1[8 + i] = buf2[4 + i];
3825
3826     retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT,
3827                 buf1, 32);
3828     if (retval != STATUS_SUCCESS) {
3829         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
3830         return STATUS_FAIL;
3831     }
3832     if (check_ms_err(chip)) {
3833         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
3834         rtsx_clear_ms_error(chip);
3835         return STATUS_FAIL;
3836     }
3837
3838     return STATUS_SUCCESS;
3839 }
3840
3841 int mg_get_local_EKB(struct scsi_cmnd *srb, struct rtsx_chip *chip)
3842 {
3843     int retval;
3844     int bufflen;
3845     unsigned int lun = SCSI_LUN(srb);
3846     u8 *buf = NULL;
3847
3848     ms_cleanup_work(chip);
3849
3850     retval = ms_switch_clock(chip);
3851     if (retval != STATUS_SUCCESS)
3852         return STATUS_FAIL;
3853
3854     buf = kmalloc(1540, GFP_KERNEL);
3855     if (!buf)
3856         return STATUS_ERROR;
3857
3858     buf[0] = 0x04;
3859     buf[1] = 0x1A;
3860     buf[2] = 0x00;
3861     buf[3] = 0x00;
3862
3863     retval = mg_send_ex_cmd(chip, MG_GET_LEKB, 0);
3864     if (retval != STATUS_SUCCESS) {
3865         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
3866         goto free_buffer;
3867     }
3868
3869     retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
3870                   3, WAIT_INT, 0, 0, buf + 4, 1536);
3871     if (retval != STATUS_SUCCESS) {
3872         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
3873         rtsx_clear_ms_error(chip);
3874         goto free_buffer;
3875     }
3876     if (check_ms_err(chip)) {
3877         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
3878         rtsx_clear_ms_error(chip);
3879         retval = STATUS_FAIL;
3880         goto free_buffer;
3881     }
3882
3883     bufflen = min_t(int, 1052, scsi_bufflen(srb));
3884     rtsx_stor_set_xfer_buf(buf, bufflen, srb);
3885
3886 free_buffer:
3887     kfree(buf);
3888     return retval;
3889 }
3890
3891 int mg_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
3892 {
3893     struct ms_info *ms_card = &chip->ms_card;
3894     int retval;
3895     int bufflen;
3896     int i;
3897     unsigned int lun = SCSI_LUN(srb);
3898     u8 buf[32];
3899
3900     ms_cleanup_work(chip);
3901
3902     retval = ms_switch_clock(chip);
3903     if (retval != STATUS_SUCCESS)
3904         return STATUS_FAIL;
3905
3906     retval = mg_send_ex_cmd(chip, MG_GET_ID, 0);
3907     if (retval != STATUS_SUCCESS) {
3908         set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
3909         return STATUS_FAIL;
3910     }
3911
3912     retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT,
3913                    buf, 32);
3914     if (retval != STATUS_SUCCESS) {
3915         set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
3916         return STATUS_FAIL;
3917     }
3918     if (check_ms_err(chip)) {
3919         set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
3920         rtsx_clear_ms_error(chip);
3921         return STATUS_FAIL;
3922     }
3923
3924     memcpy(ms_card->magic_gate_id, buf, 16);
3925
3926 #ifdef READ_BYTES_WAIT_INT
3927     retval = ms_poll_int(chip);
3928     if (retval != STATUS_SUCCESS) {
3929         set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
3930         return STATUS_FAIL;
3931     }
3932 #endif
3933
3934     retval = mg_send_ex_cmd(chip, MG_SET_RD, 0);
3935     if (retval != STATUS_SUCCESS) {
3936         set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
3937         return STATUS_FAIL;
3938     }
3939
3940     bufflen = min_t(int, 12, scsi_bufflen(srb));
3941     rtsx_stor_get_xfer_buf(buf, bufflen, srb);
3942
3943     for (i = 0; i < 8; i++)
3944         buf[i] = buf[4 + i];
3945
3946     for (i = 0; i < 24; i++)
3947         buf[8 + i] = 0;
3948
3949     retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA,
3950                 32, WAIT_INT, buf, 32);
3951     if (retval != STATUS_SUCCESS) {
3952         set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
3953         return STATUS_FAIL;
3954     }
3955     if (check_ms_err(chip)) {
3956         set_sense_type(chip, lun, SENSE_TYPE_MG_INCOMPATIBLE_MEDIUM);
3957         rtsx_clear_ms_error(chip);
3958         return STATUS_FAIL;
3959     }
3960
3961     ms_card->mg_auth = 0;
3962
3963     return STATUS_SUCCESS;
3964 }
3965
3966 int mg_get_rsp_chg(struct scsi_cmnd *srb, struct rtsx_chip *chip)
3967 {
3968     struct ms_info *ms_card = &chip->ms_card;
3969     int retval;
3970     int bufflen;
3971     unsigned int lun = SCSI_LUN(srb);
3972     u8 buf1[32], buf2[36];
3973
3974     ms_cleanup_work(chip);
3975
3976     retval = ms_switch_clock(chip);
3977     if (retval != STATUS_SUCCESS)
3978         return STATUS_FAIL;
3979
3980     retval = mg_send_ex_cmd(chip, MG_MAKE_RMS, 0);
3981     if (retval != STATUS_SUCCESS) {
3982         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
3983         return STATUS_FAIL;
3984     }
3985
3986     retval = ms_read_bytes(chip, PRO_READ_SHORT_DATA, 32, WAIT_INT,
3987                    buf1, 32);
3988     if (retval != STATUS_SUCCESS) {
3989         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
3990         return STATUS_FAIL;
3991     }
3992     if (check_ms_err(chip)) {
3993         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
3994         rtsx_clear_ms_error(chip);
3995         return STATUS_FAIL;
3996     }
3997
3998     buf2[0] = 0x00;
3999     buf2[1] = 0x22;
4000     buf2[2] = 0x00;
4001     buf2[3] = 0x00;
4002
4003     memcpy(buf2 + 4, ms_card->magic_gate_id, 16);
4004     memcpy(buf2 + 20, buf1, 16);
4005
4006     bufflen = min_t(int, 36, scsi_bufflen(srb));
4007     rtsx_stor_set_xfer_buf(buf2, bufflen, srb);
4008
4009 #ifdef READ_BYTES_WAIT_INT
4010     retval = ms_poll_int(chip);
4011     if (retval != STATUS_SUCCESS) {
4012         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4013         return STATUS_FAIL;
4014     }
4015 #endif
4016
4017     return STATUS_SUCCESS;
4018 }
4019
4020 int mg_rsp(struct scsi_cmnd *srb, struct rtsx_chip *chip)
4021 {
4022     struct ms_info *ms_card = &chip->ms_card;
4023     int retval;
4024     int i;
4025     int bufflen;
4026     unsigned int lun = SCSI_LUN(srb);
4027     u8 buf[32];
4028
4029     ms_cleanup_work(chip);
4030
4031     retval = ms_switch_clock(chip);
4032     if (retval != STATUS_SUCCESS)
4033         return STATUS_FAIL;
4034
4035     retval = mg_send_ex_cmd(chip, MG_MAKE_KSE, 0);
4036     if (retval != STATUS_SUCCESS) {
4037         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4038         return STATUS_FAIL;
4039     }
4040
4041     bufflen = min_t(int, 12, scsi_bufflen(srb));
4042     rtsx_stor_get_xfer_buf(buf, bufflen, srb);
4043
4044     for (i = 0; i < 8; i++)
4045         buf[i] = buf[4 + i];
4046
4047     for (i = 0; i < 24; i++)
4048         buf[8 + i] = 0;
4049
4050     retval = ms_write_bytes(chip, PRO_WRITE_SHORT_DATA, 32, WAIT_INT,
4051                 buf, 32);
4052     if (retval != STATUS_SUCCESS) {
4053         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4054         return STATUS_FAIL;
4055     }
4056     if (check_ms_err(chip)) {
4057         set_sense_type(chip, lun, SENSE_TYPE_MG_KEY_FAIL_NOT_AUTHEN);
4058         rtsx_clear_ms_error(chip);
4059         return STATUS_FAIL;
4060     }
4061
4062     ms_card->mg_auth = 1;
4063
4064     return STATUS_SUCCESS;
4065 }
4066
4067 int mg_get_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
4068 {
4069     struct ms_info *ms_card = &chip->ms_card;
4070     int retval;
4071     int bufflen;
4072     unsigned int lun = SCSI_LUN(srb);
4073     u8 *buf = NULL;
4074
4075     ms_cleanup_work(chip);
4076
4077     retval = ms_switch_clock(chip);
4078     if (retval != STATUS_SUCCESS)
4079         return STATUS_FAIL;
4080
4081     buf = kmalloc(1028, GFP_KERNEL);
4082     if (!buf)
4083         return STATUS_ERROR;
4084
4085     buf[0] = 0x04;
4086     buf[1] = 0x02;
4087     buf[2] = 0x00;
4088     buf[3] = 0x00;
4089
4090     retval = mg_send_ex_cmd(chip, MG_GET_IBD, ms_card->mg_entry_num);
4091     if (retval != STATUS_SUCCESS) {
4092         set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
4093         goto free_buffer;
4094     }
4095
4096     retval = ms_transfer_data(chip, MS_TM_AUTO_READ, PRO_READ_LONG_DATA,
4097                   2, WAIT_INT, 0, 0, buf + 4, 1024);
4098     if (retval != STATUS_SUCCESS) {
4099         set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
4100         rtsx_clear_ms_error(chip);
4101         goto free_buffer;
4102     }
4103     if (check_ms_err(chip)) {
4104         set_sense_type(chip, lun, SENSE_TYPE_MEDIA_UNRECOVER_READ_ERR);
4105         rtsx_clear_ms_error(chip);
4106         retval = STATUS_FAIL;
4107         goto free_buffer;
4108     }
4109
4110     bufflen = min_t(int, 1028, scsi_bufflen(srb));
4111     rtsx_stor_set_xfer_buf(buf, bufflen, srb);
4112
4113 free_buffer:
4114     kfree(buf);
4115     return retval;
4116 }
4117
4118 int mg_set_ICV(struct scsi_cmnd *srb, struct rtsx_chip *chip)
4119 {
4120     struct ms_info *ms_card = &chip->ms_card;
4121     int retval;
4122     int bufflen;
4123 #ifdef MG_SET_ICV_SLOW
4124     int i;
4125 #endif
4126     unsigned int lun = SCSI_LUN(srb);
4127     u8 *buf = NULL;
4128
4129     ms_cleanup_work(chip);
4130
4131     retval = ms_switch_clock(chip);
4132     if (retval != STATUS_SUCCESS)
4133         return STATUS_FAIL;
4134
4135     buf = kmalloc(1028, GFP_KERNEL);
4136     if (!buf)
4137         return STATUS_ERROR;
4138
4139     bufflen = min_t(int, 1028, scsi_bufflen(srb));
4140     rtsx_stor_get_xfer_buf(buf, bufflen, srb);
4141
4142     retval = mg_send_ex_cmd(chip, MG_SET_IBD, ms_card->mg_entry_num);
4143     if (retval != STATUS_SUCCESS) {
4144         if (ms_card->mg_auth == 0) {
4145             if ((buf[5] & 0xC0) != 0)
4146                 set_sense_type
4147                     (chip, lun,
4148                     SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
4149             else
4150                 set_sense_type(chip, lun,
4151                            SENSE_TYPE_MG_WRITE_ERR);
4152         } else {
4153             set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
4154         }
4155         goto set_ICV_finish;
4156     }
4157
4158 #ifdef MG_SET_ICV_SLOW
4159     for (i = 0; i < 2; i++) {
4160         udelay(50);
4161
4162         rtsx_init_cmd(chip);
4163
4164         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TPC,
4165                  0xFF, PRO_WRITE_LONG_DATA);
4166         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANS_CFG, 0xFF, WAIT_INT);
4167         rtsx_add_cmd(chip, WRITE_REG_CMD, CARD_DATA_SOURCE,
4168                  0x01, RING_BUFFER);
4169
4170         trans_dma_enable(DMA_TO_DEVICE, chip, 512, DMA_512);
4171
4172         rtsx_add_cmd(chip, WRITE_REG_CMD, MS_TRANSFER, 0xFF,
4173                  MS_TRANSFER_START |  MS_TM_NORMAL_WRITE);
4174         rtsx_add_cmd(chip, CHECK_REG_CMD, MS_TRANSFER,
4175                  MS_TRANSFER_END, MS_TRANSFER_END);
4176
4177         rtsx_send_cmd_no_wait(chip);
4178
4179         retval = rtsx_transfer_data(chip, MS_CARD, buf + 4 + i * 512,
4180                         512, 0, DMA_TO_DEVICE, 3000);
4181         if (retval < 0 || check_ms_err(chip)) {
4182             rtsx_clear_ms_error(chip);
4183             if (ms_card->mg_auth == 0) {
4184                 if ((buf[5] & 0xC0) != 0)
4185                     set_sense_type
4186                         (chip, lun,
4187                          SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
4188                 else
4189                     set_sense_type(chip, lun,
4190                                SENSE_TYPE_MG_WRITE_ERR);
4191             } else {
4192                 set_sense_type(chip, lun,
4193                            SENSE_TYPE_MG_WRITE_ERR);
4194             }
4195             retval = STATUS_FAIL;
4196             goto set_ICV_finish;
4197         }
4198     }
4199 #else
4200     retval = ms_transfer_data(chip, MS_TM_AUTO_WRITE, PRO_WRITE_LONG_DATA,
4201                   2, WAIT_INT, 0, 0, buf + 4, 1024);
4202     if (retval != STATUS_SUCCESS || check_ms_err(chip)) {
4203         rtsx_clear_ms_error(chip);
4204         if (ms_card->mg_auth == 0) {
4205             if ((buf[5] & 0xC0) != 0)
4206                 set_sense_type
4207                     (chip, lun,
4208                      SENSE_TYPE_MG_KEY_FAIL_NOT_ESTAB);
4209             else
4210                 set_sense_type(chip, lun,
4211                            SENSE_TYPE_MG_WRITE_ERR);
4212         } else {
4213             set_sense_type(chip, lun, SENSE_TYPE_MG_WRITE_ERR);
4214         }
4215         goto set_ICV_finish;
4216     }
4217 #endif
4218
4219 set_ICV_finish:
4220     kfree(buf);
4221     return retval;
4222 }
4223
4224 #endif /* SUPPORT_MAGIC_GATE */
4225
4226 void ms_cleanup_work(struct rtsx_chip *chip)
4227 {
4228     struct ms_info *ms_card = &chip->ms_card;
4229
4230     if (CHK_MSPRO(ms_card)) {
4231         if (ms_card->seq_mode) {
4232             dev_dbg(rtsx_dev(chip), "MS Pro: stop transmission\n");
4233             mspro_stop_seq_mode(chip);
4234             ms_card->cleanup_counter = 0;
4235         }
4236         if (CHK_MSHG(ms_card)) {
4237             rtsx_write_register(chip, MS_CFG,
4238                         MS_2K_SECTOR_MODE, 0x00);
4239         }
4240     }
4241 #ifdef MS_DELAY_WRITE
4242     else if ((!CHK_MSPRO(ms_card)) &&
4243          ms_card->delay_write.delay_write_flag) {
4244         dev_dbg(rtsx_dev(chip), "MS: delay write\n");
4245         ms_delay_write(chip);
4246         ms_card->cleanup_counter = 0;
4247     }
4248 #endif
4249 }
4250
4251 int ms_power_off_card3v3(struct rtsx_chip *chip)
4252 {
4253     int retval;
4254
4255     retval = disable_card_clock(chip, MS_CARD);
4256     if (retval != STATUS_SUCCESS)
4257         return STATUS_FAIL;
4258
4259     if (chip->asic_code) {
4260         retval = ms_pull_ctl_disable(chip);
4261         if (retval != STATUS_SUCCESS)
4262             return STATUS_FAIL;
4263     } else {
4264         retval = rtsx_write_register(chip, FPGA_PULL_CTL,
4265                          FPGA_MS_PULL_CTL_BIT | 0x20,
4266                          FPGA_MS_PULL_CTL_BIT);
4267         if (retval)
4268             return retval;
4269     }
4270     retval = rtsx_write_register(chip, CARD_OE, MS_OUTPUT_EN, 0);
4271     if (retval)
4272         return retval;
4273
4274     if (!chip->ft2_fast_mode) {
4275         retval = card_power_off(chip, MS_CARD);
4276         if (retval != STATUS_SUCCESS)
4277             return STATUS_FAIL;
4278     }
4279
4280     return STATUS_SUCCESS;
4281 }
4282
4283 int release_ms_card(struct rtsx_chip *chip)
4284 {
4285     struct ms_info *ms_card = &chip->ms_card;
4286     int retval;
4287
4288 #ifdef MS_DELAY_WRITE
4289     ms_card->delay_write.delay_write_flag = 0;
4290 #endif
4291     ms_card->pro_under_formatting = 0;
4292
4293     chip->card_ready &= ~MS_CARD;
4294     chip->card_fail &= ~MS_CARD;
4295     chip->card_wp &= ~MS_CARD;
4296
4297     ms_free_l2p_tbl(chip);
4298
4299     memset(ms_card->raw_sys_info, 0, 96);
4300 #ifdef SUPPORT_PCGL_1P18
4301     memset(ms_card->raw_model_name, 0, 48);
4302 #endif
4303
4304     retval = ms_power_off_card3v3(chip);
4305     if (retval != STATUS_SUCCESS)
4306         return STATUS_FAIL;
4307
4308     return STATUS_SUCCESS;
4309 }