[PATCH 3/3] nvme: Check if drive is locked using ATA Security

[Jethro Beekman]

Signed-off-by: Jethro Beekman <ker...@jbeekman.nl>
---
 drivers/nvme/host/core.c | 49 +++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 48 insertions(+), 1 deletion(-)

diff --git a/drivers/nvme/host/core.c b/drivers/nvme/host/core.c
index da027ed..0164122 100644
--- a/drivers/nvme/host/core.c
+++ b/drivers/nvme/host/core.c
@@ -1389,10 +1389,57 @@ int nvme_security_recv(struct nvme_ctrl *dev, u8 
protocol, void *buf,
        return nvme_submit_sync_cmd(dev->admin_q, &c, buf, len);
 }
 
+#define OACS_SECURITY (1<<0)
+#define SCSI_SECURITY_PROTOCOL_ATA_SECURITY 0xef
+#define ATA_SECURITY_LOCKED 0x4
+
 static bool nvme_security_is_locked(struct nvme_ctrl *ctrl,
                struct nvme_id_ctrl *id)
 {
-       return false;
+       int err;
+       unsigned int i;
+       bool found;
+       u8 protocols[256+8]; /* 8 byte hdr + max number of possible protocols */
+       u8 ata_security[16];
+       u16 n;
+
+       /* security commands supported? */
+       if (!(le16_to_cpu(id->oacs) & OACS_SECURITY))
+               return false;
+
+       /* list security protocols */
+       err = nvme_security_recv(ctrl, 0, protocols, sizeof(protocols));
+       if (err) {
+               dev_warn(ctrl->device, "nvme_security_recv returned error 
%xh\n",
+                                       err);
+               return false;
+       }
+
+       /* find ata security protocol */
+       n = be16_to_cpup((__be16 *)(protocols+6));
+       if (n >= 256) {
+               dev_warn(ctrl->device, "security info protocol returned more 
than 256 protocols\n");
+               return false;
+       }
+       found = false;
+       for (i = 0; i <= n; i++) {
+               if (protocols[8+i] == SCSI_SECURITY_PROTOCOL_ATA_SECURITY) {
+                       found = true;
+                       break;
+               }
+       }
+       if (!found)
+               return false;
+
+       /* do ata security identify */
+       err = nvme_security_recv(ctrl, SCSI_SECURITY_PROTOCOL_ATA_SECURITY,
+                       ata_security, sizeof(ata_security))
+       if (err) {
+               dev_warn(ctrl->device, "nvme_security_recv returned error 
%xh\n",
+                                       err);
+               return false;
+       }
+       return ata_security[1] == 0xe && (ata_security[9]&ATA_SECURITY_LOCKED);
 }
 
 void nvme_scan_namespaces(struct nvme_ctrl *ctrl)
-- 
2.9.0