1 files changed, 129 insertions, 87 deletions

diff --git a/drivers/firmware/arm_scpi.c b/drivers/firmware/arm_scpi.c
index dfb373c8ba2a..7da9f1b83ebe 100644
--- a/drivers/firmware/arm_scpi.c
+++ b/drivers/firmware/arm_scpi.c
@@ -28,7 +28,6 @@
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
 #include <linux/bitmap.h>
-#include <linux/bitfield.h>
 #include <linux/device.h>
 #include <linux/err.h>
 #include <linux/export.h>
@@ -73,13 +72,21 @@
 
 #define MAX_DVFS_DOMAINS        8
 #define MAX_DVFS_OPPS           16
-
-#define PROTO_REV_MAJOR_MASK    GENMASK(31, 16)
-#define PROTO_REV_MINOR_MASK    GENMASK(15, 0)
-
-#define FW_REV_MAJOR_MASK       GENMASK(31, 24)
-#define FW_REV_MINOR_MASK       GENMASK(23, 16)
-#define FW_REV_PATCH_MASK       GENMASK(15, 0)
+#define DVFS_LATENCY(hdr)       (le32_to_cpu(hdr) >> 16)
+#define DVFS_OPP_COUNT(hdr)     ((le32_to_cpu(hdr) >> 8) & 0xff)
+
+#define PROTOCOL_REV_MINOR_BITS 16
+#define PROTOCOL_REV_MINOR_MASK ((1U << PROTOCOL_REV_MINOR_BITS) - 1)
+#define PROTOCOL_REV_MAJOR(x)   ((x) >> PROTOCOL_REV_MINOR_BITS)
+#define PROTOCOL_REV_MINOR(x)   ((x) & PROTOCOL_REV_MINOR_MASK)
+
+#define FW_REV_MAJOR_BITS       24
+#define FW_REV_MINOR_BITS       16
+#define FW_REV_PATCH_MASK       ((1U << FW_REV_MINOR_BITS) - 1)
+#define FW_REV_MINOR_MASK       ((1U << FW_REV_MAJOR_BITS) - 1)
+#define FW_REV_MAJOR(x)         ((x) >> FW_REV_MAJOR_BITS)
+#define FW_REV_MINOR(x)         (((x) & FW_REV_MINOR_MASK) >> FW_REV_MINOR_BITS)
+#define FW_REV_PATCH(x)         ((x) & FW_REV_PATCH_MASK)
 
 #define MAX_RX_TIMEOUT          (msecs_to_jiffies(30))
 
@@ -304,6 +311,10 @@ struct clk_get_info {
         u8 name[20];
 } __packed;
 
+struct clk_get_value {
+        __le32 rate;
+} __packed;
+
 struct clk_set_value {
         __le16 id;
         __le16 reserved;
@@ -317,9 +328,7 @@ struct legacy_clk_set_value {
 } __packed;
 
 struct dvfs_info {
-        u8 domain;
-        u8 opp_count;
-        __le16 latency;
+        __le32 header;
         struct {
                 __le32 freq;
                 __le32 m_volt;
@@ -342,6 +351,11 @@ struct _scpi_sensor_info {
         char name[20];
 };
 
+struct sensor_value {
+        __le32 lo_val;
+        __le32 hi_val;
+} __packed;
+
 struct dev_pstate_set {
         __le16 dev_id;
         u8 pstate;
@@ -405,20 +419,19 @@ static void scpi_process_cmd(struct scpi_chan *ch, u32 cmd)
                 unsigned int len;
 
                 if (scpi_info->is_legacy) {
-                        struct legacy_scpi_shared_mem __iomem *mem =
-                                                        ch->rx_payload;
+                        struct legacy_scpi_shared_mem *mem = ch->rx_payload;
 
                         /* RX Length is not replied by the legacy Firmware */
                         len = match->rx_len;
 
-                        match->status = ioread32(&mem->status);
+                        match->status = le32_to_cpu(mem->status);
                         memcpy_fromio(match->rx_buf, mem->payload, len);
                 } else {
-                        struct scpi_shared_mem __iomem *mem = ch->rx_payload;
+                        struct scpi_shared_mem *mem = ch->rx_payload;
 
                         len = min(match->rx_len, CMD_SIZE(cmd));
 
-                        match->status = ioread32(&mem->status);
+                        match->status = le32_to_cpu(mem->status);
                         memcpy_fromio(match->rx_buf, mem->payload, len);
                 }
 
@@ -432,11 +445,11 @@ static void scpi_process_cmd(struct scpi_chan *ch, u32 cmd)
 static void scpi_handle_remote_msg(struct mbox_client *c, void *msg)
 {
         struct scpi_chan *ch = container_of(c, struct scpi_chan, cl);
-        struct scpi_shared_mem __iomem *mem = ch->rx_payload;
+        struct scpi_shared_mem *mem = ch->rx_payload;
         u32 cmd = 0;
 
         if (!scpi_info->is_legacy)
-                cmd = ioread32(&mem->command);
+                cmd = le32_to_cpu(mem->command);
 
         scpi_process_cmd(ch, cmd);
 }
@@ -446,7 +459,7 @@ static void scpi_tx_prepare(struct mbox_client *c, void *msg)
         unsigned long flags;
         struct scpi_xfer *t = msg;
         struct scpi_chan *ch = container_of(c, struct scpi_chan, cl);
-        struct scpi_shared_mem __iomem *mem = ch->tx_payload;
+        struct scpi_shared_mem *mem = (struct scpi_shared_mem *)ch->tx_payload;
 
         if (t->tx_buf) {
                 if (scpi_info->is_legacy)
@@ -465,7 +478,7 @@ static void scpi_tx_prepare(struct mbox_client *c, void *msg)
         }
 
         if (!scpi_info->is_legacy)
-                iowrite32(t->cmd, &mem->command);
+                mem->command = cpu_to_le32(t->cmd);
 }
 
 static struct scpi_xfer *get_scpi_xfer(struct scpi_chan *ch)
@@ -570,13 +583,13 @@ scpi_clk_get_range(u16 clk_id, unsigned long *min, unsigned long *max)
 static unsigned long scpi_clk_get_val(u16 clk_id)
 {
         int ret;
-        __le32 rate;
+        struct clk_get_value clk;
         __le16 le_clk_id = cpu_to_le16(clk_id);
 
         ret = scpi_send_message(CMD_GET_CLOCK_VALUE, &le_clk_id,
-                                sizeof(le_clk_id), &rate, sizeof(rate));
+                                sizeof(le_clk_id), &clk, sizeof(clk));
 
-        return ret ? ret : le32_to_cpu(rate);
+        return ret ? ret : le32_to_cpu(clk.rate);
 }
 
 static int scpi_clk_set_val(u16 clk_id, unsigned long rate)
@@ -632,34 +645,34 @@ static int opp_cmp_func(const void *opp1, const void *opp2)
 
 static struct scpi_dvfs_info *scpi_dvfs_get_info(u8 domain)
 {
-        if (domain >= MAX_DVFS_DOMAINS)
-                return ERR_PTR(-EINVAL);
-
-        return scpi_info->dvfs[domain] ?: ERR_PTR(-EINVAL);
-}
-
-static int scpi_dvfs_populate_info(struct device *dev, u8 domain)
-{
         struct scpi_dvfs_info *info;
         struct scpi_opp *opp;
         struct dvfs_info buf;
         int ret, i;
 
+        if (domain >= MAX_DVFS_DOMAINS)
+                return ERR_PTR(-EINVAL);
+
+        if (scpi_info->dvfs[domain])    /* data already populated */
+                return scpi_info->dvfs[domain];
+
         ret = scpi_send_message(CMD_GET_DVFS_INFO, &domain, sizeof(domain),
                                 &buf, sizeof(buf));
         if (ret)
-                return ret;
+                return ERR_PTR(ret);
 
-        info = devm_kmalloc(dev, sizeof(*info), GFP_KERNEL);
+        info = kmalloc(sizeof(*info), GFP_KERNEL);
         if (!info)
-                return -ENOMEM;
+                return ERR_PTR(-ENOMEM);
 
-        info->count = buf.opp_count;
-        info->latency = le16_to_cpu(buf.latency) * 1000; /* uS to nS */
+        info->count = DVFS_OPP_COUNT(buf.header);
+        info->latency = DVFS_LATENCY(buf.header) * 1000; /* uS to nS */
 
-        info->opps = devm_kcalloc(dev, info->count, sizeof(*opp), GFP_KERNEL);
-        if (!info->opps)
-                return -ENOMEM;
+        info->opps = kcalloc(info->count, sizeof(*opp), GFP_KERNEL);
+        if (!info->opps) {
+                kfree(info);
+                return ERR_PTR(-ENOMEM);
+        }
 
         for (i = 0, opp = info->opps; i < info->count; i++, opp++) {
                 opp->freq = le32_to_cpu(buf.opps[i].freq);
@@ -669,15 +682,7 @@ static int scpi_dvfs_populate_info(struct device *dev, u8 domain)
         sort(info->opps, info->count, sizeof(*opp), opp_cmp_func, NULL);
 
         scpi_info->dvfs[domain] = info;
-        return 0;
-}
-
-static void scpi_dvfs_populate(struct device *dev)
-{
-        int domain;
-
-        for (domain = 0; domain < MAX_DVFS_DOMAINS; domain++)
-                scpi_dvfs_populate_info(dev, domain);
+        return info;
 }
 
 static int scpi_dev_domain_id(struct device *dev)
@@ -708,6 +713,9 @@ static int scpi_dvfs_get_transition_latency(struct device *dev)
         if (IS_ERR(info))
                 return PTR_ERR(info);
 
+        if (!info->latency)
+                return 0;
+
         return info->latency;
 }
 
@@ -768,19 +776,20 @@ static int scpi_sensor_get_info(u16 sensor_id, struct scpi_sensor_info *info)
 static int scpi_sensor_get_value(u16 sensor, u64 *val)
 {
         __le16 id = cpu_to_le16(sensor);
-        __le64 value;
+        struct sensor_value buf;
         int ret;
 
         ret = scpi_send_message(CMD_SENSOR_VALUE, &id, sizeof(id),
-                                &value, sizeof(value));
+                                &buf, sizeof(buf));
         if (ret)
                 return ret;
 
         if (scpi_info->is_legacy)
-                /* only 32-bits supported, upper 32 bits can be junk */
-                *val = le32_to_cpup((__le32 *)&value);
+                /* only 32-bits supported, hi_val can be junk */
+                *val = le32_to_cpu(buf.lo_val);
         else
-                *val = le64_to_cpu(value);
+                *val = (u64)le32_to_cpu(buf.hi_val) << 32 |
+                        le32_to_cpu(buf.lo_val);
 
         return 0;
 }
@@ -853,19 +862,23 @@ static int scpi_init_versions(struct scpi_drvinfo *info)
 static ssize_t protocol_version_show(struct device *dev,
                                      struct device_attribute *attr, char *buf)
 {
-        return sprintf(buf, "%lu.%lu\n",
-                FIELD_GET(PROTO_REV_MAJOR_MASK, scpi_info->protocol_version),
-                FIELD_GET(PROTO_REV_MINOR_MASK, scpi_info->protocol_version));
+        struct scpi_drvinfo *scpi_info = dev_get_drvdata(dev);
+
+        return sprintf(buf, "%d.%d\n",
+                       PROTOCOL_REV_MAJOR(scpi_info->protocol_version),
+                       PROTOCOL_REV_MINOR(scpi_info->protocol_version));
 }
 static DEVICE_ATTR_RO(protocol_version);
 
 static ssize_t firmware_version_show(struct device *dev,
                                      struct device_attribute *attr, char *buf)
 {
-        return sprintf(buf, "%lu.%lu.%lu\n",
-                     FIELD_GET(FW_REV_MAJOR_MASK, scpi_info->firmware_version),
-                     FIELD_GET(FW_REV_MINOR_MASK, scpi_info->firmware_version),
-                     FIELD_GET(FW_REV_PATCH_MASK, scpi_info->firmware_version));
+        struct scpi_drvinfo *scpi_info = dev_get_drvdata(dev);
+
+        return sprintf(buf, "%d.%d.%d\n",
+                       FW_REV_MAJOR(scpi_info->firmware_version),
+                       FW_REV_MINOR(scpi_info->firmware_version),
+                       FW_REV_PATCH(scpi_info->firmware_version));
 }
 static DEVICE_ATTR_RO(firmware_version);
 
@@ -876,13 +889,39 @@ static struct attribute *versions_attrs[] = {
 };
 ATTRIBUTE_GROUPS(versions);
 
-static void scpi_free_channels(void *data)
+static void
+scpi_free_channels(struct device *dev, struct scpi_chan *pchan, int count)
 {
-        struct scpi_drvinfo *info = data;
         int i;
 
-        for (i = 0; i < info->num_chans; i++)
-                mbox_free_channel(info->channels[i].chan);
+        for (i = 0; i < count && pchan->chan; i++, pchan++) {
+                mbox_free_channel(pchan->chan);
+                devm_kfree(dev, pchan->xfers);
+                devm_iounmap(dev, pchan->rx_payload);
+        }
+}
+
+static int scpi_remove(struct platform_device *pdev)
+{
+        int i;
+        struct device *dev = &pdev->dev;
+        struct scpi_drvinfo *info = platform_get_drvdata(pdev);
+
+        scpi_info = NULL; /* stop exporting SCPI ops through get_scpi_ops */
+
+        of_platform_depopulate(dev);
+        sysfs_remove_groups(&dev->kobj, versions_groups);
+        scpi_free_channels(dev, info->channels, info->num_chans);
+        platform_set_drvdata(pdev, NULL);
+
+        for (i = 0; i < MAX_DVFS_DOMAINS && info->dvfs[i]; i++) {
+                kfree(info->dvfs[i]->opps);
+                kfree(info->dvfs[i]);
+        }
+        devm_kfree(dev, info->channels);
+        devm_kfree(dev, info);
+
+        return 0;
 }
 
 #define MAX_SCPI_XFERS          10
@@ -913,6 +952,7 @@ static int scpi_probe(struct platform_device *pdev)
 {
         int count, idx, ret;
         struct resource res;
+        struct scpi_chan *scpi_chan;
         struct device *dev = &pdev->dev;
         struct device_node *np = dev->of_node;
 
@@ -929,19 +969,13 @@ static int scpi_probe(struct platform_device *pdev)
                 return -ENODEV;
         }
 
-        scpi_info->channels = devm_kcalloc(dev, count, sizeof(struct scpi_chan),
-                                           GFP_KERNEL);
-        if (!scpi_info->channels)
+        scpi_chan = devm_kcalloc(dev, count, sizeof(*scpi_chan), GFP_KERNEL);
+        if (!scpi_chan)
                 return -ENOMEM;
 
-        ret = devm_add_action(dev, scpi_free_channels, scpi_info);
-        if (ret)
-                return ret;
-
-        for (; scpi_info->num_chans < count; scpi_info->num_chans++) {
+        for (idx = 0; idx < count; idx++) {
                 resource_size_t size;
-                int idx = scpi_info->num_chans;
-                struct scpi_chan *pchan = scpi_info->channels + idx;
+                struct scpi_chan *pchan = scpi_chan + idx;
                 struct mbox_client *cl = &pchan->cl;
                 struct device_node *shmem = of_parse_phandle(np, "shmem", idx);
 
@@ -949,14 +983,15 @@ static int scpi_probe(struct platform_device *pdev)
                 of_node_put(shmem);
                 if (ret) {
                         dev_err(dev, "failed to get SCPI payload mem resource\n");
-                        return ret;
+                        goto err;
                 }
 
                 size = resource_size(&res);
                 pchan->rx_payload = devm_ioremap(dev, res.start, size);
                 if (!pchan->rx_payload) {
                         dev_err(dev, "failed to ioremap SCPI payload\n");
-                        return -EADDRNOTAVAIL;
+                        ret = -EADDRNOTAVAIL;
+                        goto err;
                 }
                 pchan->tx_payload = pchan->rx_payload + (size >> 1);
 
@@ -982,11 +1017,17 @@ static int scpi_probe(struct platform_device *pdev)
                                 dev_err(dev, "failed to get channel%d err %d\n",
                                         idx, ret);
                 }
+err:
+                scpi_free_channels(dev, scpi_chan, idx);
+                scpi_info = NULL;
                 return ret;
         }
 
+        scpi_info->channels = scpi_chan;
+        scpi_info->num_chans = count;
         scpi_info->commands = scpi_std_commands;
-        scpi_info->scpi_ops = &scpi_ops;
+
+        platform_set_drvdata(pdev, scpi_info);
 
         if (scpi_info->is_legacy) {
                 /* Replace with legacy variants */
@@ -1002,23 +1043,23 @@ static int scpi_probe(struct platform_device *pdev)
         ret = scpi_init_versions(scpi_info);
         if (ret) {
                 dev_err(dev, "incorrect or no SCP firmware found\n");
+                scpi_remove(pdev);
                 return ret;
         }
 
-        scpi_dvfs_populate(dev);
-
-        _dev_info(dev, "SCP Protocol %lu.%lu Firmware %lu.%lu.%lu version\n",
-                  FIELD_GET(PROTO_REV_MAJOR_MASK, scpi_info->protocol_version),
-                  FIELD_GET(PROTO_REV_MINOR_MASK, scpi_info->protocol_version),
-                  FIELD_GET(FW_REV_MAJOR_MASK, scpi_info->firmware_version),
-                  FIELD_GET(FW_REV_MINOR_MASK, scpi_info->firmware_version),
-                  FIELD_GET(FW_REV_PATCH_MASK, scpi_info->firmware_version));
+        _dev_info(dev, "SCP Protocol %d.%d Firmware %d.%d.%d version\n",
+                  PROTOCOL_REV_MAJOR(scpi_info->protocol_version),
+                  PROTOCOL_REV_MINOR(scpi_info->protocol_version),
+                  FW_REV_MAJOR(scpi_info->firmware_version),
+                  FW_REV_MINOR(scpi_info->firmware_version),
+                  FW_REV_PATCH(scpi_info->firmware_version));
+        scpi_info->scpi_ops = &scpi_ops;
 
-        ret = devm_device_add_groups(dev, versions_groups);
+        ret = sysfs_create_groups(&dev->kobj, versions_groups);
         if (ret)
                 dev_err(dev, "unable to create sysfs version group\n");
 
-        return devm_of_platform_populate(dev);
+        return of_platform_populate(dev->of_node, NULL, NULL, dev);
 }
 
 static const struct of_device_id scpi_of_match[] = {
@@ -1035,6 +1076,7 @@ static struct platform_driver scpi_driver = {
                 .of_match_table = scpi_of_match,
         },
         .probe = scpi_probe,
+        .remove = scpi_remove,
 };
 module_platform_driver(scpi_driver);