ide: Implement disk shock protection support (v4)

[firefly-linux-kernel-4.4.55.git] / drivers / ide / ide-io.c

diff --git a/drivers/ide/ide-io.c b/drivers/ide/ide-io.c
index e205f46c3c7aa4de4ff944dc1a7b22f7c401f4cb..77c6eaeacefafaf987f0d7500eb82f394e189284 100644 (file)
--- a/drivers/ide/ide-io.c
+++ b/drivers/ide/ide-io.c
@@ -672,7 +672,32 @@ EXPORT_SYMBOL_GPL(ide_devset_execute);
 
 static ide_startstop_t ide_special_rq(ide_drive_t *drive, struct request *rq)
 {
-       switch (rq->cmd[0]) {
+       u8 cmd = rq->cmd[0];
+
+       if (cmd == REQ_PARK_HEADS || cmd == REQ_UNPARK_HEADS) {
+               ide_task_t task;
+               struct ide_taskfile *tf = &task.tf;
+
+               memset(&task, 0, sizeof(task));
+               if (cmd == REQ_PARK_HEADS) {
+                       drive->sleep = *(unsigned long *)rq->special;
+                       drive->dev_flags |= IDE_DFLAG_SLEEPING;
+                       tf->command = ATA_CMD_IDLEIMMEDIATE;
+                       tf->feature = 0x44;
+                       tf->lbal = 0x4c;
+                       tf->lbam = 0x4e;
+                       tf->lbah = 0x55;
+                       task.tf_flags |= IDE_TFLAG_CUSTOM_HANDLER;
+               } else          /* cmd == REQ_UNPARK_HEADS */
+                       tf->command = ATA_CMD_CHK_POWER;
+
+               task.tf_flags |= IDE_TFLAG_TF | IDE_TFLAG_DEVICE;
+               task.rq = rq;
+               drive->hwif->data_phase = task.data_phase = TASKFILE_NO_DATA;
+               return do_rw_taskfile(drive, &task);
+       }
+
+       switch (cmd) {
        case REQ_DEVSET_EXEC:
        {
                int err, (*setfunc)(ide_drive_t *, int) = rq->special;
@@ -1008,7 +1033,7 @@ static void ide_do_request (ide_hwgroup_t *hwgroup, int masked_irq)
                }
                hwgroup->hwif = hwif;
                hwgroup->drive = drive;
-               drive->dev_flags &= ~IDE_DFLAG_SLEEPING;
+               drive->dev_flags &= ~(IDE_DFLAG_SLEEPING | IDE_DFLAG_PARKED);
                drive->service_start = jiffies;
 
                if (blk_queue_plugged(drive->queue)) {