1 files changed, 163 insertions, 259 deletions

diff --git a/drivers/scsi/mvsas/mv_sas.c b/drivers/scsi/mvsas/mv_sas.c
index 0ef27425c447..4958fefff365 100644
--- a/drivers/scsi/mvsas/mv_sas.c
+++ b/drivers/scsi/mvsas/mv_sas.c
@@ -38,7 +38,7 @@ static int mvs_find_tag(struct mvs_info *mvi, struct sas_task *task, u32 *tag)
 
 void mvs_tag_clear(struct mvs_info *mvi, u32 tag)
 {
-        void *bitmap = &mvi->tags;
+        void *bitmap = mvi->tags;
         clear_bit(tag, bitmap);
 }
 
@@ -49,14 +49,14 @@ void mvs_tag_free(struct mvs_info *mvi, u32 tag)
 
 void mvs_tag_set(struct mvs_info *mvi, unsigned int tag)
 {
-        void *bitmap = &mvi->tags;
+        void *bitmap = mvi->tags;
         set_bit(tag, bitmap);
 }
 
 inline int mvs_tag_alloc(struct mvs_info *mvi, u32 *tag_out)
 {
         unsigned int index, tag;
-        void *bitmap = &mvi->tags;
+        void *bitmap = mvi->tags;
 
         index = find_first_zero_bit(bitmap, mvi->tags_num);
         tag = index;
@@ -74,126 +74,6 @@ void mvs_tag_init(struct mvs_info *mvi)
                 mvs_tag_clear(mvi, i);
 }
 
-void mvs_hexdump(u32 size, u8 *data, u32 baseaddr)
-{
-        u32 i;
-        u32 run;
-        u32 offset;
-
-        offset = 0;
-        while (size) {
-                printk(KERN_DEBUG"%08X : ", baseaddr + offset);
-                if (size >= 16)
-                        run = 16;
-                else
-                        run = size;
-                size -= run;
-                for (i = 0; i < 16; i++) {
-                        if (i < run)
-                                printk(KERN_DEBUG"%02X ", (u32)data[i]);
-                        else
-                                printk(KERN_DEBUG"   ");
-                }
-                printk(KERN_DEBUG": ");
-                for (i = 0; i < run; i++)
-                        printk(KERN_DEBUG"%c",
-                                isalnum(data[i]) ? data[i] : '.');
-                printk(KERN_DEBUG"\n");
-                data = &data[16];
-                offset += run;
-        }
-        printk(KERN_DEBUG"\n");
-}
-
-#if (_MV_DUMP > 1)
-static void mvs_hba_sb_dump(struct mvs_info *mvi, u32 tag,
-                                   enum sas_protocol proto)
-{
-        u32 offset;
-        struct mvs_slot_info *slot = &mvi->slot_info[tag];
-
-        offset = slot->cmd_size + MVS_OAF_SZ +
-            MVS_CHIP_DISP->prd_size() * slot->n_elem;
-        dev_printk(KERN_DEBUG, mvi->dev, "+---->Status buffer[%d] :\n",
-                        tag);
-        mvs_hexdump(32, (u8 *) slot->response,
-                    (u32) slot->buf_dma + offset);
-}
-#endif
-
-static void mvs_hba_memory_dump(struct mvs_info *mvi, u32 tag,
-                                enum sas_protocol proto)
-{
-#if (_MV_DUMP > 1)
-        u32 sz, w_ptr;
-        u64 addr;
-        struct mvs_slot_info *slot = &mvi->slot_info[tag];
-
-        /*Delivery Queue */
-        sz = MVS_CHIP_SLOT_SZ;
-        w_ptr = slot->tx;
-        addr = mvi->tx_dma;
-        dev_printk(KERN_DEBUG, mvi->dev,
-                "Delivery Queue Size=%04d , WRT_PTR=%04X\n", sz, w_ptr);
-        dev_printk(KERN_DEBUG, mvi->dev,
-                "Delivery Queue Base Address=0x%llX (PA)"
-                "(tx_dma=0x%llX), Entry=%04d\n",
-                addr, (unsigned long long)mvi->tx_dma, w_ptr);
-        mvs_hexdump(sizeof(u32), (u8 *)(&mvi->tx[mvi->tx_prod]),
-                        (u32) mvi->tx_dma + sizeof(u32) * w_ptr);
-        /*Command List */
-        addr = mvi->slot_dma;
-        dev_printk(KERN_DEBUG, mvi->dev,
-                "Command List Base Address=0x%llX (PA)"
-                "(slot_dma=0x%llX), Header=%03d\n",
-                addr, (unsigned long long)slot->buf_dma, tag);
-        dev_printk(KERN_DEBUG, mvi->dev, "Command Header[%03d]:\n", tag);
-        /*mvs_cmd_hdr */
-        mvs_hexdump(sizeof(struct mvs_cmd_hdr), (u8 *)(&mvi->slot[tag]),
-                (u32) mvi->slot_dma + tag * sizeof(struct mvs_cmd_hdr));
-        /*1.command table area */
-        dev_printk(KERN_DEBUG, mvi->dev, "+---->Command Table :\n");
-        mvs_hexdump(slot->cmd_size, (u8 *) slot->buf, (u32) slot->buf_dma);
-        /*2.open address frame area */
-        dev_printk(KERN_DEBUG, mvi->dev, "+---->Open Address Frame :\n");
-        mvs_hexdump(MVS_OAF_SZ, (u8 *) slot->buf + slot->cmd_size,
-                                (u32) slot->buf_dma + slot->cmd_size);
-        /*3.status buffer */
-        mvs_hba_sb_dump(mvi, tag, proto);
-        /*4.PRD table */
-        dev_printk(KERN_DEBUG, mvi->dev, "+---->PRD table :\n");
-        mvs_hexdump(MVS_CHIP_DISP->prd_size() * slot->n_elem,
-                (u8 *) slot->buf + slot->cmd_size + MVS_OAF_SZ,
-                (u32) slot->buf_dma + slot->cmd_size + MVS_OAF_SZ);
-#endif
-}
-
-static void mvs_hba_cq_dump(struct mvs_info *mvi)
-{
-#if (_MV_DUMP > 2)
-        u64 addr;
-        void __iomem *regs = mvi->regs;
-        u32 entry = mvi->rx_cons + 1;
-        u32 rx_desc = le32_to_cpu(mvi->rx[entry]);
-
-        /*Completion Queue */
-        addr = mr32(RX_HI) << 16 << 16 | mr32(RX_LO);
-        dev_printk(KERN_DEBUG, mvi->dev, "Completion Task = 0x%p\n",
-                   mvi->slot_info[rx_desc & RXQ_SLOT_MASK].task);
-        dev_printk(KERN_DEBUG, mvi->dev,
-                "Completion List Base Address=0x%llX (PA), "
-                "CQ_Entry=%04d, CQ_WP=0x%08X\n",
-                addr, entry - 1, mvi->rx[0]);
-        mvs_hexdump(sizeof(u32), (u8 *)(&rx_desc),
-                    mvi->rx_dma + sizeof(u32) * entry);
-#endif
-}
-
-void mvs_get_sas_addr(void *buf, u32 buflen)
-{
-        /*memcpy(buf, "\x50\x05\x04\x30\x11\xab\x64\x40", 8);*/
-}
-
 struct mvs_info *mvs_find_dev_mvi(struct domain_device *dev)
 {
         unsigned long i = 0, j = 0, hi = 0;
@@ -222,7 +102,6 @@ struct mvs_info *mvs_find_dev_mvi(struct domain_device *dev)
 
 }
 
-/* FIXME */
 int mvs_find_dev_phyno(struct domain_device *dev, int *phyno)
 {
         unsigned long i = 0, j = 0, n = 0, num = 0;
@@ -253,6 +132,20 @@ int mvs_find_dev_phyno(struct domain_device *dev, int *phyno)
         return num;
 }
 
+struct mvs_device *mvs_find_dev_by_reg_set(struct mvs_info *mvi,
+                                                u8 reg_set)
+{
+        u32 dev_no;
+        for (dev_no = 0; dev_no < MVS_MAX_DEVICES; dev_no++) {
+                if (mvi->devices[dev_no].taskfileset == MVS_ID_NOT_MAPPED)
+                        continue;
+
+                if (mvi->devices[dev_no].taskfileset == reg_set)
+                        return &mvi->devices[dev_no];
+        }
+        return NULL;
+}
+
 static inline void mvs_free_reg_set(struct mvs_info *mvi,
                                 struct mvs_device *dev)
 {
@@ -283,7 +176,6 @@ void mvs_phys_reset(struct mvs_info *mvi, u32 phy_mask, int hard)
         }
 }
 
-/* FIXME: locking? */
 int mvs_phy_control(struct asd_sas_phy *sas_phy, enum phy_func func,
                         void *funcdata)
 {
@@ -309,12 +201,12 @@ int mvs_phy_control(struct asd_sas_phy *sas_phy, enum phy_func func,
                 tmp = MVS_CHIP_DISP->read_phy_ctl(mvi, phy_id);
                 if (tmp & PHY_RST_HARD)
                         break;
-                MVS_CHIP_DISP->phy_reset(mvi, phy_id, 1);
+                MVS_CHIP_DISP->phy_reset(mvi, phy_id, MVS_HARD_RESET);
                 break;
 
         case PHY_FUNC_LINK_RESET:
                 MVS_CHIP_DISP->phy_enable(mvi, phy_id);
-                MVS_CHIP_DISP->phy_reset(mvi, phy_id, 0);
+                MVS_CHIP_DISP->phy_reset(mvi, phy_id, MVS_SOFT_RESET);
                 break;
 
         case PHY_FUNC_DISABLE:
@@ -406,14 +298,10 @@ int mvs_slave_configure(struct scsi_device *sdev)
 
         if (ret)
                 return ret;
-        if (dev_is_sata(dev)) {
-                /* may set PIO mode */
-        #if MV_DISABLE_NCQ
-                struct ata_port *ap = dev->sata_dev.ap;
-                struct ata_device *adev = ap->link.device;
-                adev->flags |= ATA_DFLAG_NCQ_OFF;
-                scsi_adjust_queue_depth(sdev, MSG_SIMPLE_TAG, 1);
-        #endif
+        if (!dev_is_sata(dev)) {
+                sas_change_queue_depth(sdev,
+                        MVS_QUEUE_SIZE,
+                        SCSI_QDEPTH_DEFAULT);
         }
         return 0;
 }
@@ -424,6 +312,7 @@ void mvs_scan_start(struct Scsi_Host *shost)
         unsigned short core_nr;
         struct mvs_info *mvi;
         struct sas_ha_struct *sha = SHOST_TO_SAS_HA(shost);
+        struct mvs_prv_info *mvs_prv = sha->lldd_ha;
 
         core_nr = ((struct mvs_prv_info *)sha->lldd_ha)->n_host;
 
@@ -432,15 +321,17 @@ void mvs_scan_start(struct Scsi_Host *shost)
                 for (i = 0; i < mvi->chip->n_phy; ++i)
                         mvs_bytes_dmaed(mvi, i);
         }
+        mvs_prv->scan_finished = 1;
 }
 
 int mvs_scan_finished(struct Scsi_Host *shost, unsigned long time)
 {
-        /* give the phy enabling interrupt event time to come in (1s
-         * is empirically about all it takes) */
-        if (time < HZ)
+        struct sas_ha_struct *sha = SHOST_TO_SAS_HA(shost);
+        struct mvs_prv_info *mvs_prv = sha->lldd_ha;
+
+        if (mvs_prv->scan_finished == 0)
                 return 0;
-        /* Wait for discovery to finish */
+
         scsi_flush_work(shost);
         return 1;
 }
@@ -461,10 +352,7 @@ static int mvs_task_prep_smp(struct mvs_info *mvi,
         void *buf_prd;
         struct mvs_slot_info *slot = &mvi->slot_info[tag];
         u32 flags = (tei->n_elem << MCH_PRD_LEN_SHIFT);
-#if _MV_DUMP
-        u8 *buf_cmd;
-        void *from;
-#endif
+
         /*
          * DMA-map SMP request, response buffers
          */
@@ -496,15 +384,7 @@ static int mvs_task_prep_smp(struct mvs_info *mvi,
         buf_tmp = slot->buf;
         buf_tmp_dma = slot->buf_dma;
 
-#if _MV_DUMP
-        buf_cmd = buf_tmp;
-        hdr->cmd_tbl = cpu_to_le64(buf_tmp_dma);
-        buf_tmp += req_len;
-        buf_tmp_dma += req_len;
-        slot->cmd_size = req_len;
-#else
         hdr->cmd_tbl = cpu_to_le64(sg_dma_address(sg_req));
-#endif
 
         /* region 2: open address frame area (MVS_OAF_SZ bytes) ********* */
         buf_oaf = buf_tmp;
@@ -553,12 +433,6 @@ static int mvs_task_prep_smp(struct mvs_info *mvi,
         /* fill in PRD (scatter/gather) table, if any */
         MVS_CHIP_DISP->make_prd(task->scatter, tei->n_elem, buf_prd);
 
-#if _MV_DUMP
-        /* copy cmd table */
-        from = kmap_atomic(sg_page(sg_req), KM_IRQ0);
-        memcpy(buf_cmd, from + sg_req->offset, req_len);
-        kunmap_atomic(from, KM_IRQ0);
-#endif
         return 0;
 
 err_out_2:
@@ -616,14 +490,11 @@ static int mvs_task_prep_ata(struct mvs_info *mvi,
                 (mvi_dev->taskfileset << TXQ_SRS_SHIFT);
         mvi->tx[mvi->tx_prod] = cpu_to_le32(del_q);
 
-#ifndef DISABLE_HOTPLUG_DMA_FIX
         if (task->data_dir == DMA_FROM_DEVICE)
                 flags = (MVS_CHIP_DISP->prd_count() << MCH_PRD_LEN_SHIFT);
         else
                 flags = (tei->n_elem << MCH_PRD_LEN_SHIFT);
-#else
-        flags = (tei->n_elem << MCH_PRD_LEN_SHIFT);
-#endif
+
         if (task->ata_task.use_ncq)
                 flags |= MCH_FPDMA;
         if (dev->sata_dev.command_set == ATAPI_COMMAND_SET) {
@@ -631,11 +502,8 @@ static int mvs_task_prep_ata(struct mvs_info *mvi,
                         flags |= MCH_ATAPI;
         }
 
-        /* FIXME: fill in port multiplier number */
-
         hdr->flags = cpu_to_le32(flags);
 
-        /* FIXME: the low order order 5 bits for the TAG if enable NCQ */
         if (task->ata_task.use_ncq && mvs_get_ncq_tag(task, &hdr_tag))
                 task->ata_task.fis.sector_count |= (u8) (hdr_tag << 3);
         else
@@ -657,9 +525,6 @@ static int mvs_task_prep_ata(struct mvs_info *mvi,
 
         buf_tmp += MVS_ATA_CMD_SZ;
         buf_tmp_dma += MVS_ATA_CMD_SZ;
-#if _MV_DUMP
-        slot->cmd_size = MVS_ATA_CMD_SZ;
-#endif
 
         /* region 2: open address frame area (MVS_OAF_SZ bytes) ********* */
         /* used for STP.  unused for SATA? */
@@ -682,9 +547,6 @@ static int mvs_task_prep_ata(struct mvs_info *mvi,
         buf_tmp_dma += i;
 
         /* region 4: status buffer (larger the PRD, smaller this buf) ****** */
-        /* FIXME: probably unused, for SATA.  kept here just in case
-         * we get a STP/SATA error information record
-         */
         slot->response = buf_tmp;
         hdr->status_buf = cpu_to_le64(buf_tmp_dma);
         if (mvi->flags & MVF_FLAG_SOC)
@@ -715,11 +577,11 @@ static int mvs_task_prep_ata(struct mvs_info *mvi,
 
         /* fill in PRD (scatter/gather) table, if any */
         MVS_CHIP_DISP->make_prd(task->scatter, tei->n_elem, buf_prd);
-#ifndef DISABLE_HOTPLUG_DMA_FIX
+
         if (task->data_dir == DMA_FROM_DEVICE)
-                MVS_CHIP_DISP->dma_fix(mvi->bulk_buffer_dma,
+                MVS_CHIP_DISP->dma_fix(mvi, sas_port->phy_mask,
                                 TRASH_BUCKET_SIZE, tei->n_elem, buf_prd);
-#endif
+
         return 0;
 }
 
@@ -761,6 +623,9 @@ static int mvs_task_prep_ssp(struct mvs_info *mvi,
         }
         if (is_tmf)
                 flags |= (MCH_SSP_FR_TASK << MCH_SSP_FR_TYPE_SHIFT);
+        else
+                flags |= (MCH_SSP_FR_CMD << MCH_SSP_FR_TYPE_SHIFT);
+
         hdr->flags = cpu_to_le32(flags | (tei->n_elem << MCH_PRD_LEN_SHIFT));
         hdr->tags = cpu_to_le32(tag);
         hdr->data_len = cpu_to_le32(task->total_xfer_len);
@@ -777,9 +642,6 @@ static int mvs_task_prep_ssp(struct mvs_info *mvi,
 
         buf_tmp += MVS_SSP_CMD_SZ;
         buf_tmp_dma += MVS_SSP_CMD_SZ;
-#if _MV_DUMP
-        slot->cmd_size = MVS_SSP_CMD_SZ;
-#endif
 
         /* region 2: open address frame area (MVS_OAF_SZ bytes) ********* */
         buf_oaf = buf_tmp;
@@ -986,7 +848,6 @@ static int mvs_task_prep(struct sas_task *task, struct mvs_info *mvi, int is_tmf
         task->task_state_flags |= SAS_TASK_AT_INITIATOR;
         spin_unlock(&task->task_state_lock);
 
-        mvs_hba_memory_dump(mvi, tag, task->task_proto);
         mvi_dev->running_req++;
         ++(*pass);
         mvi->tx_prod = (mvi->tx_prod + 1) & (MVS_CHIP_SLOT_SZ - 1);
@@ -1189,9 +1050,9 @@ static void mvs_slot_task_free(struct mvs_info *mvi, struct sas_task *task,
         mvs_slot_free(mvi, slot_idx);
 }
 
-static void mvs_update_wideport(struct mvs_info *mvi, int i)
+static void mvs_update_wideport(struct mvs_info *mvi, int phy_no)
 {
-        struct mvs_phy *phy = &mvi->phy[i];
+        struct mvs_phy *phy = &mvi->phy[phy_no];
         struct mvs_port *port = phy->port;
         int j, no;
 
@@ -1246,18 +1107,17 @@ static void *mvs_get_d2h_reg(struct mvs_info *mvi, int i, void *buf)
                 return NULL;
 
         MVS_CHIP_DISP->write_port_cfg_addr(mvi, i, PHYR_SATA_SIG3);
-        s[3] = MVS_CHIP_DISP->read_port_cfg_data(mvi, i);
+        s[3] = cpu_to_le32(MVS_CHIP_DISP->read_port_cfg_data(mvi, i));
 
         MVS_CHIP_DISP->write_port_cfg_addr(mvi, i, PHYR_SATA_SIG2);
-        s[2] = MVS_CHIP_DISP->read_port_cfg_data(mvi, i);
+        s[2] = cpu_to_le32(MVS_CHIP_DISP->read_port_cfg_data(mvi, i));
 
         MVS_CHIP_DISP->write_port_cfg_addr(mvi, i, PHYR_SATA_SIG1);
-        s[1] = MVS_CHIP_DISP->read_port_cfg_data(mvi, i);
+        s[1] = cpu_to_le32(MVS_CHIP_DISP->read_port_cfg_data(mvi, i));
 
         MVS_CHIP_DISP->write_port_cfg_addr(mvi, i, PHYR_SATA_SIG0);
-        s[0] = MVS_CHIP_DISP->read_port_cfg_data(mvi, i);
+        s[0] = cpu_to_le32(MVS_CHIP_DISP->read_port_cfg_data(mvi, i));
 
-        /* Workaround: take some ATAPI devices for ATA */
         if (((s[1] & 0x00FFFFFF) == 0x00EB1401) && (*(u8 *)&s[3] == 0x01))
                 s[1] = 0x00EB1401 | (*((u8 *)&s[1] + 3) & 0x10);
 
@@ -1269,6 +1129,13 @@ static u32 mvs_is_sig_fis_received(u32 irq_status)
         return irq_status & PHYEV_SIG_FIS;
 }
 
+static void mvs_sig_remove_timer(struct mvs_phy *phy)
+{
+        if (phy->timer.function)
+                del_timer(&phy->timer);
+        phy->timer.function = NULL;
+}
+
 void mvs_update_phyinfo(struct mvs_info *mvi, int i, int get_st)
 {
         struct mvs_phy *phy = &mvi->phy[i];
@@ -1291,6 +1158,7 @@ void mvs_update_phyinfo(struct mvs_info *mvi, int i, int get_st)
                 if (phy->phy_type & PORT_TYPE_SATA) {
                         phy->identify.target_port_protocols = SAS_PROTOCOL_STP;
                         if (mvs_is_sig_fis_received(phy->irq_status)) {
+                                mvs_sig_remove_timer(phy);
                                 phy->phy_attached = 1;
                                 phy->att_dev_sas_addr =
                                         i + mvi->id * mvi->chip->n_phy;
@@ -1308,7 +1176,6 @@ void mvs_update_phyinfo(struct mvs_info *mvi, int i, int get_st)
                                                 tmp | PHYEV_SIG_FIS);
                                 phy->phy_attached = 0;
                                 phy->phy_type &= ~PORT_TYPE_SATA;
-                                MVS_CHIP_DISP->phy_reset(mvi, i, 0);
                                 goto out_done;
                         }
                 }       else if (phy->phy_type & PORT_TYPE_SAS
@@ -1334,9 +1201,9 @@ void mvs_update_phyinfo(struct mvs_info *mvi, int i, int get_st)
                 if (MVS_CHIP_DISP->phy_work_around)
                         MVS_CHIP_DISP->phy_work_around(mvi, i);
         }
-        mv_dprintk("port %d attach dev info is %x\n",
+        mv_dprintk("phy %d attach dev info is %x\n",
                 i + mvi->id * mvi->chip->n_phy, phy->att_dev_info);
-        mv_dprintk("port %d attach sas addr is %llx\n",
+        mv_dprintk("phy %d attach sas addr is %llx\n",
                 i + mvi->id * mvi->chip->n_phy, phy->att_dev_sas_addr);
 out_done:
         if (get_st)
@@ -1361,10 +1228,10 @@ static void mvs_port_notify_formed(struct asd_sas_phy *sas_phy, int lock)
         }
         hi = i/((struct mvs_prv_info *)sas_ha->lldd_ha)->n_phy;
         mvi = ((struct mvs_prv_info *)sas_ha->lldd_ha)->mvi[hi];
-        if (sas_port->id >= mvi->chip->n_phy)
-                port = &mvi->port[sas_port->id - mvi->chip->n_phy];
+        if (i >= mvi->chip->n_phy)
+                port = &mvi->port[i - mvi->chip->n_phy];
         else
-                port = &mvi->port[sas_port->id];
+                port = &mvi->port[i];
         if (lock)
                 spin_lock_irqsave(&mvi->lock, flags);
         port->port_attached = 1;
@@ -1393,7 +1260,7 @@ static void mvs_port_notify_deformed(struct asd_sas_phy *sas_phy, int lock)
                         return;
         }
         list_for_each_entry(dev, &port->dev_list, dev_list_node)
-                mvs_do_release_task(phy->mvi, phy_no, NULL);
+                mvs_do_release_task(phy->mvi, phy_no, dev);
 
 }
 
@@ -1457,6 +1324,7 @@ int mvs_dev_found_notify(struct domain_device *dev, int lock)
         mvi_device->dev_status = MVS_DEV_NORMAL;
         mvi_device->dev_type = dev->dev_type;
         mvi_device->mvi_info = mvi;
+        mvi_device->sas_device = dev;
         if (parent_dev && DEV_IS_EXPANDER(parent_dev->dev_type)) {
                 int phy_id;
                 u8 phy_num = parent_dev->ex_dev.num_phys;
@@ -1508,6 +1376,7 @@ void mvs_dev_gone_notify(struct domain_device *dev)
                 mv_dprintk("found dev has gone.\n");
         }
         dev->lldd_dev = NULL;
+        mvi_dev->sas_device = NULL;
 
         spin_unlock_irqrestore(&mvi->lock, flags);
 }
@@ -1555,7 +1424,6 @@ static void mvs_tmf_timedout(unsigned long data)
         complete(&task->completion);
 }
 
-/* XXX */
 #define MVS_TASK_TIMEOUT 20
 static int mvs_exec_internal_tmf_task(struct domain_device *dev,
                         void *parameter, u32 para_len, struct mvs_tmf_task *tmf)
@@ -1588,7 +1456,7 @@ static int mvs_exec_internal_tmf_task(struct domain_device *dev,
                 }
 
                 wait_for_completion(&task->completion);
-                res = -TMF_RESP_FUNC_FAILED;
+                res = TMF_RESP_FUNC_FAILED;
                 /* Even TMF timed out, return direct. */
                 if ((task->task_state_flags & SAS_TASK_STATE_ABORTED)) {
                         if (!(task->task_state_flags & SAS_TASK_STATE_DONE)) {
@@ -1638,11 +1506,10 @@ static int mvs_debug_issue_ssp_tmf(struct domain_device *dev,
                                 u8 *lun, struct mvs_tmf_task *tmf)
 {
         struct sas_ssp_task ssp_task;
-        DECLARE_COMPLETION_ONSTACK(completion);
         if (!(dev->tproto & SAS_PROTOCOL_SSP))
                 return TMF_RESP_FUNC_ESUPP;
 
-        strncpy((u8 *)&ssp_task.LUN, lun, 8);
+        memcpy(ssp_task.LUN, lun, 8);
 
         return mvs_exec_internal_tmf_task(dev, &ssp_task,
                                 sizeof(ssp_task), tmf);
@@ -1666,7 +1533,7 @@ static int mvs_debug_I_T_nexus_reset(struct domain_device *dev)
 int mvs_lu_reset(struct domain_device *dev, u8 *lun)
 {
         unsigned long flags;
-        int i, phyno[WIDE_PORT_MAX_PHY], num , rc = TMF_RESP_FUNC_FAILED;
+        int rc = TMF_RESP_FUNC_FAILED;
         struct mvs_tmf_task tmf_task;
         struct mvs_device * mvi_dev = dev->lldd_dev;
         struct mvs_info *mvi = mvi_dev->mvi_info;
@@ -1675,10 +1542,8 @@ int mvs_lu_reset(struct domain_device *dev, u8 *lun)
         mvi_dev->dev_status = MVS_DEV_EH;
         rc = mvs_debug_issue_ssp_tmf(dev, lun, &tmf_task);
         if (rc == TMF_RESP_FUNC_COMPLETE) {
-                num = mvs_find_dev_phyno(dev, phyno);
                 spin_lock_irqsave(&mvi->lock, flags);
-                for (i = 0; i < num; i++)
-                        mvs_release_task(mvi, dev);
+                mvs_release_task(mvi, dev);
                 spin_unlock_irqrestore(&mvi->lock, flags);
         }
         /* If failed, fall-through I_T_Nexus reset */
@@ -1696,11 +1561,12 @@ int mvs_I_T_nexus_reset(struct domain_device *dev)
 
         if (mvi_dev->dev_status != MVS_DEV_EH)
                 return TMF_RESP_FUNC_COMPLETE;
+        else
+                mvi_dev->dev_status = MVS_DEV_NORMAL;
         rc = mvs_debug_I_T_nexus_reset(dev);
         mv_printk("%s for device[%x]:rc= %d\n",
                 __func__, mvi_dev->device_id, rc);
 
-        /* housekeeper */
         spin_lock_irqsave(&mvi->lock, flags);
         mvs_release_task(mvi, dev);
         spin_unlock_irqrestore(&mvi->lock, flags);
@@ -1739,9 +1605,6 @@ int mvs_query_task(struct sas_task *task)
                 case TMF_RESP_FUNC_FAILED:
                 case TMF_RESP_FUNC_COMPLETE:
                         break;
-                default:
-                        rc = TMF_RESP_FUNC_COMPLETE;
-                        break;
                 }
         }
         mv_printk("%s:rc= %d\n", __func__, rc);
@@ -1761,8 +1624,8 @@ int mvs_abort_task(struct sas_task *task)
         u32 tag;
 
         if (!mvi_dev) {
-                mv_printk("%s:%d TMF_RESP_FUNC_FAILED\n", __func__, __LINE__);
-                rc = TMF_RESP_FUNC_FAILED;
+                mv_printk("Device has removed\n");
+                return TMF_RESP_FUNC_FAILED;
         }
 
         mvi = mvi_dev->mvi_info;
@@ -1807,25 +1670,17 @@ int mvs_abort_task(struct sas_task *task)
 
         } else if (task->task_proto & SAS_PROTOCOL_SATA ||
                 task->task_proto & SAS_PROTOCOL_STP) {
-                /* to do free register_set */
                 if (SATA_DEV == dev->dev_type) {
                         struct mvs_slot_info *slot = task->lldd_task;
-                        struct task_status_struct *tstat;
                         u32 slot_idx = (u32)(slot - mvi->slot_info);
-                        tstat = &task->task_status;
-                        mv_dprintk(KERN_DEBUG "mv_abort_task() mvi=%p task=%p "
+                        mv_dprintk("mvs_abort_task() mvi=%p task=%p "
                                    "slot=%p slot_idx=x%x\n",
                                    mvi, task, slot, slot_idx);
-                        tstat->stat = SAS_ABORTED_TASK;
-                        if (mvi_dev && mvi_dev->running_req)
-                                mvi_dev->running_req--;
-                        if (sas_protocol_ata(task->task_proto))
-                                mvs_free_reg_set(mvi, mvi_dev);
+                        mvs_tmf_timedout((unsigned long)task);
                         mvs_slot_task_free(mvi, task, slot, slot_idx);
-                        return -1;
+                        rc = TMF_RESP_FUNC_COMPLETE;
+                        goto out;
                 }
-        } else {
-                /* SMP */
 
         }
 out:
@@ -1891,12 +1746,63 @@ static int mvs_sata_done(struct mvs_info *mvi, struct sas_task *task,
         return stat;
 }
 
+void mvs_set_sense(u8 *buffer, int len, int d_sense,
+                int key, int asc, int ascq)
+{
+        memset(buffer, 0, len);
+
+        if (d_sense) {
+                /* Descriptor format */
+                if (len < 4) {
+                        mv_printk("Length %d of sense buffer too small to "
+                                "fit sense %x:%x:%x", len, key, asc, ascq);
+                }
+
+                buffer[0] = 0x72;               /* Response Code        */
+                if (len > 1)
+                        buffer[1] = key;        /* Sense Key */
+                if (len > 2)
+                        buffer[2] = asc;        /* ASC  */
+                if (len > 3)
+                        buffer[3] = ascq;       /* ASCQ */
+        } else {
+                if (len < 14) {
+                        mv_printk("Length %d of sense buffer too small to "
+                                "fit sense %x:%x:%x", len, key, asc, ascq);
+                }
+
+                buffer[0] = 0x70;               /* Response Code        */
+                if (len > 2)
+                        buffer[2] = key;        /* Sense Key */
+                if (len > 7)
+                        buffer[7] = 0x0a;       /* Additional Sense Length */
+                if (len > 12)
+                        buffer[12] = asc;       /* ASC */
+                if (len > 13)
+                        buffer[13] = ascq; /* ASCQ */
+        }
+
+        return;
+}
+
+void mvs_fill_ssp_resp_iu(struct ssp_response_iu *iu,
+                                u8 key, u8 asc, u8 asc_q)
+{
+        iu->datapres = 2;
+        iu->response_data_len = 0;
+        iu->sense_data_len = 17;
+        iu->status = 02;
+        mvs_set_sense(iu->sense_data, 17, 0,
+                        key, asc, asc_q);
+}
+
 static int mvs_slot_err(struct mvs_info *mvi, struct sas_task *task,
                          u32 slot_idx)
 {
         struct mvs_slot_info *slot = &mvi->slot_info[slot_idx];
         int stat;
-        u32 err_dw0 = le32_to_cpu(*(u32 *) (slot->response));
+        u32 err_dw0 = le32_to_cpu(*(u32 *)slot->response);
+        u32 err_dw1 = le32_to_cpu(*((u32 *)slot->response + 1));
         u32 tfs = 0;
         enum mvs_port_type type = PORT_TYPE_SAS;
 
@@ -1908,8 +1814,19 @@ static int mvs_slot_err(struct mvs_info *mvi, struct sas_task *task,
         stat = SAM_STAT_CHECK_CONDITION;
         switch (task->task_proto) {
         case SAS_PROTOCOL_SSP:
+        {
                 stat = SAS_ABORTED_TASK;
+                if ((err_dw0 & NO_DEST) || err_dw1 & bit(31)) {
+                        struct ssp_response_iu *iu = slot->response +
+                                sizeof(struct mvs_err_info);
+                        mvs_fill_ssp_resp_iu(iu, NOT_READY, 0x04, 01);
+                        sas_ssp_task_response(mvi->dev, task, iu);
+                        stat = SAM_STAT_CHECK_CONDITION;
+                }
+                if (err_dw1 & bit(31))
+                        mv_printk("reuse same slot, retry command.\n");
                 break;
+        }
         case SAS_PROTOCOL_SMP:
                 stat = SAM_STAT_CHECK_CONDITION;
                 break;
@@ -1918,10 +1835,8 @@ static int mvs_slot_err(struct mvs_info *mvi, struct sas_task *task,
         case SAS_PROTOCOL_STP:
         case SAS_PROTOCOL_SATA | SAS_PROTOCOL_STP:
         {
-                if (err_dw0 == 0x80400002)
-                        mv_printk("find reserved error, why?\n");
-
                 task->ata_task.use_ncq = 0;
+                stat = SAS_PROTO_RESPONSE;
                 mvs_sata_done(mvi, task, slot_idx, err_dw0);
         }
                 break;
@@ -1945,8 +1860,6 @@ int mvs_slot_complete(struct mvs_info *mvi, u32 rx_desc, u32 flags)
         void *to;
         enum exec_status sts;
 
-        if (mvi->exp_req)
-                mvi->exp_req--;
         if (unlikely(!task || !task->lldd_task || !task->dev))
                 return -1;
 
@@ -1954,8 +1867,6 @@ int mvs_slot_complete(struct mvs_info *mvi, u32 rx_desc, u32 flags)
         dev = task->dev;
         mvi_dev = dev->lldd_dev;
 
-        mvs_hba_cq_dump(mvi);
-
         spin_lock(&task->task_state_lock);
         task->task_state_flags &=
                 ~(SAS_TASK_STATE_PENDING | SAS_TASK_AT_INITIATOR);
@@ -1978,6 +1889,7 @@ int mvs_slot_complete(struct mvs_info *mvi, u32 rx_desc, u32 flags)
                 return -1;
         }
 
+        /* when no device attaching, go ahead and complete by error handling*/
         if (unlikely(!mvi_dev || flags)) {
                 if (!mvi_dev)
                         mv_dprintk("port has not device.\n");
@@ -1987,6 +1899,9 @@ int mvs_slot_complete(struct mvs_info *mvi, u32 rx_desc, u32 flags)
 
         /* error info record present */
         if (unlikely((rx_desc & RXQ_ERR) && (*(u64 *) slot->response))) {
+                mv_dprintk("port %d slot %d rx_desc %X has error info"
+                        "%016llX.\n", slot->port->sas_port.id, slot_idx,
+                         rx_desc, (u64)(*(u64 *)slot->response));
                 tstat->stat = mvs_slot_err(mvi, task, slot_idx);
                 tstat->resp = SAS_TASK_COMPLETE;
                 goto out;
@@ -2048,8 +1963,7 @@ out:
         spin_unlock(&mvi->lock);
         if (task->task_done)
                 task->task_done(task);
-        else
-                mv_dprintk("why has not task_done.\n");
+
         spin_lock(&mvi->lock);
 
         return sts;
@@ -2092,7 +2006,6 @@ void mvs_release_task(struct mvs_info *mvi,
                       struct domain_device *dev)
 {
         int i, phyno[WIDE_PORT_MAX_PHY], num;
-        /* housekeeper */
         num = mvs_find_dev_phyno(dev, phyno);
         for (i = 0; i < num; i++)
                 mvs_do_release_task(mvi, phyno[i], dev);
@@ -2111,13 +2024,13 @@ static void mvs_work_queue(struct work_struct *work)
         struct mvs_wq *mwq = container_of(dw, struct mvs_wq, work_q);
         struct mvs_info *mvi = mwq->mvi;
         unsigned long flags;
+        u32 phy_no = (unsigned long) mwq->data;
+        struct sas_ha_struct *sas_ha = mvi->sas;
+        struct mvs_phy *phy = &mvi->phy[phy_no];
+        struct asd_sas_phy *sas_phy = &phy->sas_phy;
 
         spin_lock_irqsave(&mvi->lock, flags);
         if (mwq->handler & PHY_PLUG_EVENT) {
-                u32 phy_no = (unsigned long) mwq->data;
-                struct sas_ha_struct *sas_ha = mvi->sas;
-                struct mvs_phy *phy = &mvi->phy[phy_no];
-                struct asd_sas_phy *sas_phy = &phy->sas_phy;
 
                 if (phy->phy_event & PHY_PLUG_OUT) {
                         u32 tmp;
@@ -2139,6 +2052,11 @@ static void mvs_work_queue(struct work_struct *work)
                                 mv_dprintk("phy%d Attached Device\n", phy_no);
                         }
                 }
+        } else if (mwq->handler & EXP_BRCT_CHG) {
+                phy->phy_event &= ~EXP_BRCT_CHG;
+                sas_ha->notify_port_event(sas_phy,
+                                PORTE_BROADCAST_RCVD);
+                mv_dprintk("phy%d Got Broadcast Change\n", phy_no);
         }
         list_del(&mwq->entry);
         spin_unlock_irqrestore(&mvi->lock, flags);
@@ -2174,29 +2092,21 @@ static void mvs_sig_time_out(unsigned long tphy)
                 if (&mvi->phy[phy_no] == phy) {
                         mv_dprintk("Get signature time out, reset phy %d\n",
                                 phy_no+mvi->id*mvi->chip->n_phy);
-                        MVS_CHIP_DISP->phy_reset(mvi, phy_no, 1);
+                        MVS_CHIP_DISP->phy_reset(mvi, phy_no, MVS_HARD_RESET);
                 }
         }
 }
 
-static void mvs_sig_remove_timer(struct mvs_phy *phy)
-{
-        if (phy->timer.function)
-                del_timer(&phy->timer);
-        phy->timer.function = NULL;
-}
-
 void mvs_int_port(struct mvs_info *mvi, int phy_no, u32 events)
 {
         u32 tmp;
-        struct sas_ha_struct *sas_ha = mvi->sas;
         struct mvs_phy *phy = &mvi->phy[phy_no];
-        struct asd_sas_phy *sas_phy = &phy->sas_phy;
 
         phy->irq_status = MVS_CHIP_DISP->read_port_irq_stat(mvi, phy_no);
-        mv_dprintk("port %d ctrl sts=0x%X.\n", phy_no+mvi->id*mvi->chip->n_phy,
+        MVS_CHIP_DISP->write_port_irq_stat(mvi, phy_no, phy->irq_status);
+        mv_dprintk("phy %d ctrl sts=0x%08X.\n", phy_no+mvi->id*mvi->chip->n_phy,
                 MVS_CHIP_DISP->read_phy_ctl(mvi, phy_no));
-        mv_dprintk("Port %d irq sts = 0x%X\n", phy_no+mvi->id*mvi->chip->n_phy,
+        mv_dprintk("phy %d irq sts = 0x%08X\n", phy_no+mvi->id*mvi->chip->n_phy,
                 phy->irq_status);
 
         /*
@@ -2205,11 +2115,12 @@ void mvs_int_port(struct mvs_info *mvi, int phy_no, u32 events)
         */
 
         if (phy->irq_status & PHYEV_DCDR_ERR) {
-                mv_dprintk("port %d STP decoding error.\n",
+                mv_dprintk("phy %d STP decoding error.\n",
                 phy_no + mvi->id*mvi->chip->n_phy);
         }
 
         if (phy->irq_status & PHYEV_POOF) {
+                mdelay(500);
                 if (!(phy->phy_event & PHY_PLUG_OUT)) {
                         int dev_sata = phy->phy_type & PORT_TYPE_SATA;
                         int ready;
@@ -2220,17 +2131,13 @@ void mvs_int_port(struct mvs_info *mvi, int phy_no, u32 events)
                                 (void *)(unsigned long)phy_no,
                                 PHY_PLUG_EVENT);
                         ready = mvs_is_phy_ready(mvi, phy_no);
-                        if (!ready)
-                                mv_dprintk("phy%d Unplug Notice\n",
-                                        phy_no +
-                                        mvi->id * mvi->chip->n_phy);
                         if (ready || dev_sata) {
                                 if (MVS_CHIP_DISP->stp_reset)
                                         MVS_CHIP_DISP->stp_reset(mvi,
                                                         phy_no);
                                 else
                                         MVS_CHIP_DISP->phy_reset(mvi,
-                                                        phy_no, 0);
+                                                        phy_no, MVS_SOFT_RESET);
                                 return;
                         }
                 }
@@ -2243,13 +2150,12 @@ void mvs_int_port(struct mvs_info *mvi, int phy_no, u32 events)
                 if (phy->timer.function == NULL) {
                         phy->timer.data = (unsigned long)phy;
                         phy->timer.function = mvs_sig_time_out;
-                        phy->timer.expires = jiffies + 10*HZ;
+                        phy->timer.expires = jiffies + 5*HZ;
                         add_timer(&phy->timer);
                 }
         }
         if (phy->irq_status & (PHYEV_SIG_FIS | PHYEV_ID_DONE)) {
                 phy->phy_status = mvs_is_phy_ready(mvi, phy_no);
-                mvs_sig_remove_timer(phy);
                 mv_dprintk("notify plug in on phy[%d]\n", phy_no);
                 if (phy->phy_status) {
                         mdelay(10);
@@ -2263,14 +2169,14 @@ void mvs_int_port(struct mvs_info *mvi, int phy_no, u32 events)
                         }
                         mvs_update_phyinfo(mvi, phy_no, 0);
                         if (phy->phy_type & PORT_TYPE_SAS) {
-                                MVS_CHIP_DISP->phy_reset(mvi, phy_no, 2);
+                                MVS_CHIP_DISP->phy_reset(mvi, phy_no, MVS_PHY_TUNE);
                                 mdelay(10);
                         }
 
                         mvs_bytes_dmaed(mvi, phy_no);
                         /* whether driver is going to handle hot plug */
                         if (phy->phy_event & PHY_PLUG_OUT) {
-                                mvs_port_notify_formed(sas_phy, 0);
+                                mvs_port_notify_formed(&phy->sas_phy, 0);
                                 phy->phy_event &= ~PHY_PLUG_OUT;
                         }
                 } else {
@@ -2278,13 +2184,11 @@ void mvs_int_port(struct mvs_info *mvi, int phy_no, u32 events)
                                 phy_no + mvi->id*mvi->chip->n_phy);
                 }
         } else if (phy->irq_status & PHYEV_BROAD_CH) {
-                mv_dprintk("port %d broadcast change.\n",
+                mv_dprintk("phy %d broadcast change.\n",
                         phy_no + mvi->id*mvi->chip->n_phy);
-                /* exception for Samsung disk drive*/
-                mdelay(1000);
-                sas_ha->notify_port_event(sas_phy, PORTE_BROADCAST_RCVD);
+                mvs_handle_event(mvi, (void *)(unsigned long)phy_no,
+                                EXP_BRCT_CHG);
         }
-        MVS_CHIP_DISP->write_port_irq_stat(mvi, phy_no, phy->irq_status);
 }
 
 int mvs_int_rx(struct mvs_info *mvi, bool self_clear)