2 files changed, 111 insertions, 3 deletions

diff --git a/drivers/ata/ahci.h b/drivers/ata/ahci.h
index 5edce4447d8..7113c572447 100644
--- a/drivers/ata/ahci.h
+++ b/drivers/ata/ahci.h
@@ -224,9 +224,12 @@ enum {
         EM_MAX_RETRY                    = 5,
 
         /* em_ctl bits */
-        EM_CTL_RST                      = (1 << 9), /* Reset */
-        EM_CTL_TM                       = (1 << 8), /* Transmit Message */
-        EM_CTL_ALHD                     = (1 << 26), /* Activity LED */
+        EM_CTL_RST              = (1 << 9), /* Reset */
+        EM_CTL_TM               = (1 << 8), /* Transmit Message */
+        EM_CTL_MR               = (1 << 0), /* Message Recieved */
+        EM_CTL_ALHD             = (1 << 26), /* Activity LED */
+        EM_CTL_XMT              = (1 << 25), /* Transmit Only */
+        EM_CTL_SMB              = (1 << 24), /* Single Message Buffer */
 
         /* em message type */
         EM_MSG_TYPE_LED         = (1 << 0), /* LED */
@@ -288,6 +291,7 @@ struct ahci_host_priv {
         u32                     saved_cap2;     /* saved initial cap2 */
         u32                     saved_port_map; /* saved initial port_map */
         u32                     em_loc; /* enclosure management location */
+        u32                     em_buf_sz;      /* EM buffer size in byte */
         u32                     em_msg_type;    /* EM message type */
 };
 
diff --git a/drivers/ata/libahci.c b/drivers/ata/libahci.c
index 817bcd023cc..1984a6e89e8 100644
--- a/drivers/ata/libahci.c
+++ b/drivers/ata/libahci.c
@@ -108,11 +108,18 @@ static ssize_t ahci_show_host_version(struct device *dev,
                                       struct device_attribute *attr, char *buf);
 static ssize_t ahci_show_port_cmd(struct device *dev,
                                   struct device_attribute *attr, char *buf);
+static ssize_t ahci_read_em_buffer(struct device *dev,
+                                   struct device_attribute *attr, char *buf);
+static ssize_t ahci_store_em_buffer(struct device *dev,
+                                    struct device_attribute *attr,
+                                    const char *buf, size_t size);
 
 static DEVICE_ATTR(ahci_host_caps, S_IRUGO, ahci_show_host_caps, NULL);
 static DEVICE_ATTR(ahci_host_cap2, S_IRUGO, ahci_show_host_cap2, NULL);
 static DEVICE_ATTR(ahci_host_version, S_IRUGO, ahci_show_host_version, NULL);
 static DEVICE_ATTR(ahci_port_cmd, S_IRUGO, ahci_show_port_cmd, NULL);
+static DEVICE_ATTR(em_buffer, S_IWUSR | S_IRUGO,
+                   ahci_read_em_buffer, ahci_store_em_buffer);
 
 static struct device_attribute *ahci_shost_attrs[] = {
         &dev_attr_link_power_management_policy,
@@ -122,6 +129,7 @@ static struct device_attribute *ahci_shost_attrs[] = {
         &dev_attr_ahci_host_cap2,
         &dev_attr_ahci_host_version,
         &dev_attr_ahci_port_cmd,
+        &dev_attr_em_buffer,
         NULL
 };
 
@@ -252,6 +260,101 @@ static ssize_t ahci_show_port_cmd(struct device *dev,
         return sprintf(buf, "%x\n", readl(port_mmio + PORT_CMD));
 }
 
+static ssize_t ahci_read_em_buffer(struct device *dev,
+                                   struct device_attribute *attr, char *buf)
+{
+        struct Scsi_Host *shost = class_to_shost(dev);
+        struct ata_port *ap = ata_shost_to_port(shost);
+        struct ahci_host_priv *hpriv = ap->host->private_data;
+        void __iomem *mmio = hpriv->mmio;
+        void __iomem *em_mmio = mmio + hpriv->em_loc;
+        u32 em_ctl, msg;
+        unsigned long flags;
+        size_t count;
+        int i;
+
+        spin_lock_irqsave(ap->lock, flags);
+
+        em_ctl = readl(mmio + HOST_EM_CTL);
+        if (!(ap->flags & ATA_FLAG_EM) || em_ctl & EM_CTL_XMT ||
+            !(hpriv->em_msg_type & EM_MSG_TYPE_SGPIO)) {
+                spin_unlock_irqrestore(ap->lock, flags);
+                return -EINVAL;
+        }
+
+        if (!(em_ctl & EM_CTL_MR)) {
+                spin_unlock_irqrestore(ap->lock, flags);
+                return -EAGAIN;
+        }
+
+        if (!(em_ctl & EM_CTL_SMB))
+                em_mmio += hpriv->em_buf_sz;
+
+        count = hpriv->em_buf_sz;
+
+        /* the count should not be larger than PAGE_SIZE */
+        if (count > PAGE_SIZE) {
+                if (printk_ratelimit())
+                        ata_port_printk(ap, KERN_WARNING,
+                                        "EM read buffer size too large: "
+                                        "buffer size %u, page size %lu\n",
+                                        hpriv->em_buf_sz, PAGE_SIZE);
+                count = PAGE_SIZE;
+        }
+
+        for (i = 0; i < count; i += 4) {
+                msg = readl(em_mmio + i);
+                buf[i] = msg & 0xff;
+                buf[i + 1] = (msg >> 8) & 0xff;
+                buf[i + 2] = (msg >> 16) & 0xff;
+                buf[i + 3] = (msg >> 24) & 0xff;
+        }
+
+        spin_unlock_irqrestore(ap->lock, flags);
+
+        return i;
+}
+
+static ssize_t ahci_store_em_buffer(struct device *dev,
+                                    struct device_attribute *attr,
+                                    const char *buf, size_t size)
+{
+        struct Scsi_Host *shost = class_to_shost(dev);
+        struct ata_port *ap = ata_shost_to_port(shost);
+        struct ahci_host_priv *hpriv = ap->host->private_data;
+        void __iomem *mmio = hpriv->mmio;
+        void __iomem *em_mmio = mmio + hpriv->em_loc;
+        u32 em_ctl, msg;
+        unsigned long flags;
+        int i;
+
+        /* check size validity */
+        if (!(ap->flags & ATA_FLAG_EM) ||
+            !(hpriv->em_msg_type & EM_MSG_TYPE_SGPIO) ||
+            size % 4 || size > hpriv->em_buf_sz)
+                return -EINVAL;
+
+        spin_lock_irqsave(ap->lock, flags);
+
+        em_ctl = readl(mmio + HOST_EM_CTL);
+        if (em_ctl & EM_CTL_TM) {
+                spin_unlock_irqrestore(ap->lock, flags);
+                return -EBUSY;
+        }
+
+        for (i = 0; i < size; i += 4) {
+                msg = buf[i] | buf[i + 1] << 8 |
+                      buf[i + 2] << 16 | buf[i + 3] << 24;
+                writel(msg, em_mmio + i);
+        }
+
+        writel(em_ctl | EM_CTL_TM, mmio + HOST_EM_CTL);
+
+        spin_unlock_irqrestore(ap->lock, flags);
+
+        return size;
+}
+
 /**
  *      ahci_save_initial_config - Save and fixup initial config values
  *      @dev: target AHCI device
@@ -2099,6 +2202,7 @@ void ahci_set_em_messages(struct ahci_host_priv *hpriv,
         if (messages) {
                 /* store em_loc */
                 hpriv->em_loc = ((em_loc >> 16) * 4);
+                hpriv->em_buf_sz = ((em_loc & 0xff) * 4);
                 hpriv->em_msg_type = messages;
                 pi->flags |= ATA_FLAG_EM;
                 if (!(em_ctl & EM_CTL_ALHD))