1 files changed, 263 insertions, 96 deletions

diff --git a/drivers/w1/w1_netlink.c b/drivers/w1/w1_netlink.c
index 40788c925d1c..5234964fe001 100644
--- a/drivers/w1/w1_netlink.c
+++ b/drivers/w1/w1_netlink.c
@@ -45,7 +45,7 @@ void w1_netlink_send(struct w1_master *dev, struct w1_netlink_msg *msg)
 
         memcpy(w, msg, sizeof(struct w1_netlink_msg));
 
-        cn_netlink_send(m, 0, GFP_KERNEL);
+        cn_netlink_send(m, dev->portid, 0, GFP_KERNEL);
 }
 
 static void w1_send_slave(struct w1_master *dev, u64 rn)
@@ -54,53 +54,95 @@ static void w1_send_slave(struct w1_master *dev, u64 rn)
         struct w1_netlink_msg *hdr = (struct w1_netlink_msg *)(msg + 1);
         struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)(hdr + 1);
         int avail;
-
-        /* update kernel slave list */
-        w1_slave_found(dev, rn);
+        u64 *data;
 
         avail = dev->priv_size - cmd->len;
 
-        if (avail > 8) {
-                u64 *data = (void *)(cmd + 1) + cmd->len;
+        if (avail < 8) {
+                msg->ack++;
+                cn_netlink_send(msg, dev->portid, 0, GFP_KERNEL);
 
-                *data = rn;
-                cmd->len += 8;
-                hdr->len += 8;
-                msg->len += 8;
-                return;
+                msg->len = sizeof(struct w1_netlink_msg) +
+                        sizeof(struct w1_netlink_cmd);
+                hdr->len = sizeof(struct w1_netlink_cmd);
+                cmd->len = 0;
         }
 
-        msg->ack++;
-        cn_netlink_send(msg, 0, GFP_KERNEL);
+        data = (void *)(cmd + 1) + cmd->len;
 
-        msg->len = sizeof(struct w1_netlink_msg) + sizeof(struct w1_netlink_cmd);
-        hdr->len = sizeof(struct w1_netlink_cmd);
-        cmd->len = 0;
+        *data = rn;
+        cmd->len += 8;
+        hdr->len += 8;
+        msg->len += 8;
 }
 
-static int w1_process_search_command(struct w1_master *dev, struct cn_msg *msg,
-                unsigned int avail)
+static void w1_found_send_slave(struct w1_master *dev, u64 rn)
 {
-        struct w1_netlink_msg *hdr = (struct w1_netlink_msg *)(msg + 1);
-        struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)(hdr + 1);
-        int search_type = (cmd->cmd == W1_CMD_ALARM_SEARCH)?W1_ALARM_SEARCH:W1_SEARCH;
+        /* update kernel slave list */
+        w1_slave_found(dev, rn);
 
-        dev->priv = msg;
-        dev->priv_size = avail;
+        w1_send_slave(dev, rn);
+}
+
+/* Get the current slave list, or search (with or without alarm) */
+static int w1_get_slaves(struct w1_master *dev,
+                struct cn_msg *req_msg, struct w1_netlink_msg *req_hdr,
+                struct w1_netlink_cmd *req_cmd)
+{
+        struct cn_msg *msg;
+        struct w1_netlink_msg *hdr;
+        struct w1_netlink_cmd *cmd;
+        struct w1_slave *sl;
+
+        msg = kzalloc(PAGE_SIZE, GFP_KERNEL);
+        if (!msg)
+                return -ENOMEM;
+
+        msg->id = req_msg->id;
+        msg->seq = req_msg->seq;
+        msg->ack = 0;
+        msg->len = sizeof(struct w1_netlink_msg) +
+                sizeof(struct w1_netlink_cmd);
+
+        hdr = (struct w1_netlink_msg *)(msg + 1);
+        cmd = (struct w1_netlink_cmd *)(hdr + 1);
+
+        hdr->type = W1_MASTER_CMD;
+        hdr->id = req_hdr->id;
+        hdr->len = sizeof(struct w1_netlink_cmd);
+
+        cmd->cmd = req_cmd->cmd;
+        cmd->len = 0;
 
-        w1_search_process_cb(dev, search_type, w1_send_slave);
+        dev->priv = msg;
+        dev->priv_size = PAGE_SIZE - msg->len - sizeof(struct cn_msg);
+
+        if (req_cmd->cmd == W1_CMD_LIST_SLAVES) {
+                __u64 rn;
+                mutex_lock(&dev->list_mutex);
+                list_for_each_entry(sl, &dev->slist, w1_slave_entry) {
+                        memcpy(&rn, &sl->reg_num, sizeof(rn));
+                        w1_send_slave(dev, rn);
+                }
+                mutex_unlock(&dev->list_mutex);
+        } else {
+                w1_search_process_cb(dev, cmd->cmd == W1_CMD_ALARM_SEARCH ?
+                        W1_ALARM_SEARCH : W1_SEARCH, w1_found_send_slave);
+        }
 
         msg->ack = 0;
-        cn_netlink_send(msg, 0, GFP_KERNEL);
+        cn_netlink_send(msg, dev->portid, 0, GFP_KERNEL);
 
         dev->priv = NULL;
         dev->priv_size = 0;
 
+        kfree(msg);
+
         return 0;
 }
 
 static int w1_send_read_reply(struct cn_msg *msg, struct w1_netlink_msg *hdr,
-                struct w1_netlink_cmd *cmd)
+                struct w1_netlink_cmd *cmd, u32 portid)
 {
         void *data;
         struct w1_netlink_msg *h;
@@ -131,7 +173,7 @@ static int w1_send_read_reply(struct cn_msg *msg, struct w1_netlink_msg *hdr,
 
         memcpy(c->data, cmd->data, c->len);
 
-        err = cn_netlink_send(cm, 0, GFP_KERNEL);
+        err = cn_netlink_send(cm, portid, 0, GFP_KERNEL);
 
         kfree(data);
 
@@ -146,11 +188,11 @@ static int w1_process_command_io(struct w1_master *dev, struct cn_msg *msg,
         switch (cmd->cmd) {
         case W1_CMD_TOUCH:
                 w1_touch_block(dev, cmd->data, cmd->len);
-                w1_send_read_reply(msg, hdr, cmd);
+                w1_send_read_reply(msg, hdr, cmd, dev->portid);
                 break;
         case W1_CMD_READ:
                 w1_read_block(dev, cmd->data, cmd->len);
-                w1_send_read_reply(msg, hdr, cmd);
+                w1_send_read_reply(msg, hdr, cmd, dev->portid);
                 break;
         case W1_CMD_WRITE:
                 w1_write_block(dev, cmd->data, cmd->len);
@@ -163,38 +205,57 @@ static int w1_process_command_io(struct w1_master *dev, struct cn_msg *msg,
         return err;
 }
 
-static int w1_process_command_master(struct w1_master *dev, struct cn_msg *req_msg,
-                struct w1_netlink_msg *req_hdr, struct w1_netlink_cmd *req_cmd)
+static int w1_process_command_addremove(struct w1_master *dev,
+        struct cn_msg *msg, struct w1_netlink_msg *hdr,
+        struct w1_netlink_cmd *cmd)
 {
-        int err = -EINVAL;
-        struct cn_msg *msg;
-        struct w1_netlink_msg *hdr;
-        struct w1_netlink_cmd *cmd;
+        struct w1_slave *sl;
+        int err = 0;
+        struct w1_reg_num *id;
 
-        msg = kzalloc(PAGE_SIZE, GFP_KERNEL);
-        if (!msg)
-                return -ENOMEM;
+        if (cmd->len != 8)
+                return -EINVAL;
 
-        msg->id = req_msg->id;
-        msg->seq = req_msg->seq;
-        msg->ack = 0;
-        msg->len = sizeof(struct w1_netlink_msg) + sizeof(struct w1_netlink_cmd);
+        id = (struct w1_reg_num *)cmd->data;
 
-        hdr = (struct w1_netlink_msg *)(msg + 1);
-        cmd = (struct w1_netlink_cmd *)(hdr + 1);
+        sl = w1_slave_search_device(dev, id);
+        switch (cmd->cmd) {
+        case W1_CMD_SLAVE_ADD:
+                if (sl)
+                        err = -EINVAL;
+                else
+                        err = w1_attach_slave_device(dev, id);
+                break;
+        case W1_CMD_SLAVE_REMOVE:
+                if (sl)
+                        w1_slave_detach(sl);
+                else
+                        err = -EINVAL;
+                break;
+        default:
+                err = -EINVAL;
+                break;
+        }
 
-        hdr->type = W1_MASTER_CMD;
-        hdr->id = req_hdr->id;
-        hdr->len = sizeof(struct w1_netlink_cmd);
+        return err;
+}
 
-        cmd->cmd = req_cmd->cmd;
-        cmd->len = 0;
+static int w1_process_command_master(struct w1_master *dev,
+        struct cn_msg *req_msg, struct w1_netlink_msg *req_hdr,
+        struct w1_netlink_cmd *req_cmd)
+{
+        int err = -EINVAL;
 
-        switch (cmd->cmd) {
+        /* drop bus_mutex for search (does it's own locking), and add/remove
+         * which doesn't use the bus
+         */
+        switch (req_cmd->cmd) {
         case W1_CMD_SEARCH:
         case W1_CMD_ALARM_SEARCH:
-                err = w1_process_search_command(dev, msg,
-                                PAGE_SIZE - msg->len - sizeof(struct cn_msg));
+        case W1_CMD_LIST_SLAVES:
+                mutex_unlock(&dev->bus_mutex);
+                err = w1_get_slaves(dev, req_msg, req_hdr, req_cmd);
+                mutex_lock(&dev->bus_mutex);
                 break;
         case W1_CMD_READ:
         case W1_CMD_WRITE:
@@ -204,12 +265,20 @@ static int w1_process_command_master(struct w1_master *dev, struct cn_msg *req_m
         case W1_CMD_RESET:
                 err = w1_reset_bus(dev);
                 break;
+        case W1_CMD_SLAVE_ADD:
+        case W1_CMD_SLAVE_REMOVE:
+                mutex_unlock(&dev->bus_mutex);
+                mutex_lock(&dev->mutex);
+                err = w1_process_command_addremove(dev, req_msg, req_hdr,
+                        req_cmd);
+                mutex_unlock(&dev->mutex);
+                mutex_lock(&dev->bus_mutex);
+                break;
         default:
                 err = -EINVAL;
                 break;
         }
 
-        kfree(msg);
         return err;
 }
 
@@ -223,7 +292,8 @@ static int w1_process_command_slave(struct w1_slave *sl, struct cn_msg *msg,
         return w1_process_command_io(sl->master, msg, hdr, cmd);
 }
 
-static int w1_process_command_root(struct cn_msg *msg, struct w1_netlink_msg *mcmd)
+static int w1_process_command_root(struct cn_msg *msg,
+        struct w1_netlink_msg *mcmd, u32 portid)
 {
         struct w1_master *m;
         struct cn_msg *cn;
@@ -256,7 +326,7 @@ static int w1_process_command_root(struct cn_msg *msg, struct w1_netlink_msg *mc
         mutex_lock(&w1_mlock);
         list_for_each_entry(m, &w1_masters, w1_master_entry) {
                 if (cn->len + sizeof(*id) > PAGE_SIZE - sizeof(struct cn_msg)) {
-                        cn_netlink_send(cn, 0, GFP_KERNEL);
+                        cn_netlink_send(cn, portid, 0, GFP_KERNEL);
                         cn->ack++;
                         cn->len = sizeof(struct w1_netlink_msg);
                         w->len = 0;
@@ -269,7 +339,7 @@ static int w1_process_command_root(struct cn_msg *msg, struct w1_netlink_msg *mc
                 id++;
         }
         cn->ack = 0;
-        cn_netlink_send(cn, 0, GFP_KERNEL);
+        cn_netlink_send(cn, portid, 0, GFP_KERNEL);
         mutex_unlock(&w1_mlock);
 
         kfree(cn);
@@ -277,7 +347,7 @@ static int w1_process_command_root(struct cn_msg *msg, struct w1_netlink_msg *mc
 }
 
 static int w1_netlink_send_error(struct cn_msg *rcmsg, struct w1_netlink_msg *rmsg,
-                struct w1_netlink_cmd *rcmd, int error)
+                struct w1_netlink_cmd *rcmd, int portid, int error)
 {
         struct cn_msg *cmsg;
         struct w1_netlink_msg *msg;
@@ -304,35 +374,147 @@ static int w1_netlink_send_error(struct cn_msg *rcmsg, struct w1_netlink_msg *rm
                 cmsg->len += sizeof(*cmd);
         }
 
-        error = cn_netlink_send(cmsg, 0, GFP_KERNEL);
+        error = cn_netlink_send(cmsg, portid, 0, GFP_KERNEL);
         kfree(cmsg);
 
         return error;
 }
 
+/* Bundle together a reference count, the full message, and broken out
+ * commands to be executed on each w1 master kthread in one memory allocation.
+ */
+struct w1_cb_block {
+        atomic_t refcnt;
+        u32 portid; /* Sending process port ID */
+        struct cn_msg msg;
+        /* cn_msg data */
+        /* one or more variable length struct w1_cb_node */
+};
+struct w1_cb_node {
+        struct w1_async_cmd async;
+        /* pointers within w1_cb_block and msg data */
+        struct w1_cb_block *block;
+        struct w1_netlink_msg *m;
+        struct w1_slave *sl;
+        struct w1_master *dev;
+};
+
+static void w1_process_cb(struct w1_master *dev, struct w1_async_cmd *async_cmd)
+{
+        struct w1_cb_node *node = container_of(async_cmd, struct w1_cb_node,
+                async);
+        u16 mlen = node->m->len;
+        u8 *cmd_data = node->m->data;
+        int err = 0;
+        struct w1_slave *sl = node->sl;
+        struct w1_netlink_cmd *cmd = NULL;
+
+        mutex_lock(&dev->bus_mutex);
+        dev->portid = node->block->portid;
+        if (sl && w1_reset_select_slave(sl))
+                err = -ENODEV;
+
+        while (mlen && !err) {
+                cmd = (struct w1_netlink_cmd *)cmd_data;
+
+                if (cmd->len + sizeof(struct w1_netlink_cmd) > mlen) {
+                        err = -E2BIG;
+                        break;
+                }
+
+                if (sl)
+                        err = w1_process_command_slave(sl, &node->block->msg,
+                                node->m, cmd);
+                else
+                        err = w1_process_command_master(dev, &node->block->msg,
+                                node->m, cmd);
+
+                w1_netlink_send_error(&node->block->msg, node->m, cmd,
+                        node->block->portid, err);
+                err = 0;
+
+                cmd_data += cmd->len + sizeof(struct w1_netlink_cmd);
+                mlen -= cmd->len + sizeof(struct w1_netlink_cmd);
+        }
+
+        if (!cmd || err)
+                w1_netlink_send_error(&node->block->msg, node->m, cmd,
+                        node->block->portid, err);
+
+        if (sl)
+                w1_unref_slave(sl);
+        else
+                atomic_dec(&dev->refcnt);
+        dev->portid = 0;
+        mutex_unlock(&dev->bus_mutex);
+
+        mutex_lock(&dev->list_mutex);
+        list_del(&async_cmd->async_entry);
+        mutex_unlock(&dev->list_mutex);
+
+        if (atomic_sub_return(1, &node->block->refcnt) == 0)
+                kfree(node->block);
+}
+
 static void w1_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
 {
         struct w1_netlink_msg *m = (struct w1_netlink_msg *)(msg + 1);
-        struct w1_netlink_cmd *cmd;
         struct w1_slave *sl;
         struct w1_master *dev;
+        u16 msg_len;
         int err = 0;
+        struct w1_cb_block *block = NULL;
+        struct w1_cb_node *node = NULL;
+        int node_count = 0;
+
+        /* Count the number of master or slave commands there are to allocate
+         * space for one cb_node each.
+         */
+        msg_len = msg->len;
+        while (msg_len && !err) {
+                if (m->len + sizeof(struct w1_netlink_msg) > msg_len) {
+                        err = -E2BIG;
+                        break;
+                }
+
+                if (m->type == W1_MASTER_CMD || m->type == W1_SLAVE_CMD)
+                        ++node_count;
 
-        while (msg->len && !err) {
+                msg_len -= sizeof(struct w1_netlink_msg) + m->len;
+                m = (struct w1_netlink_msg *)(((u8 *)m) +
+                        sizeof(struct w1_netlink_msg) + m->len);
+        }
+        m = (struct w1_netlink_msg *)(msg + 1);
+        if (node_count) {
+                /* msg->len doesn't include itself */
+                long size = sizeof(struct w1_cb_block) + msg->len +
+                        node_count*sizeof(struct w1_cb_node);
+                block = kmalloc(size, GFP_KERNEL);
+                if (!block) {
+                        w1_netlink_send_error(msg, m, NULL, nsp->portid,
+                                -ENOMEM);
+                        return;
+                }
+                atomic_set(&block->refcnt, 1);
+                block->portid = nsp->portid;
+                memcpy(&block->msg, msg, sizeof(*msg) + msg->len);
+                node = (struct w1_cb_node *)((u8 *)block->msg.data + msg->len);
+        }
+
+        msg_len = msg->len;
+        while (msg_len && !err) {
                 struct w1_reg_num id;
                 u16 mlen = m->len;
-                u8 *cmd_data = m->data;
 
                 dev = NULL;
                 sl = NULL;
-                cmd = NULL;
 
                 memcpy(&id, m->id.id, sizeof(id));
 #if 0
                 printk("%s: %02x.%012llx.%02x: type=%02x, len=%u.\n",
                                 __func__, id.family, (unsigned long long)id.id, id.crc, m->type, m->len);
 #endif
-                if (m->len + sizeof(struct w1_netlink_msg) > msg->len) {
+                if (m->len + sizeof(struct w1_netlink_msg) > msg_len) {
                         err = -E2BIG;
                         break;
                 }
@@ -344,7 +526,7 @@ static void w1_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
                         if (sl)
                                 dev = sl->master;
                 } else {
-                        err = w1_process_command_root(msg, m);
+                        err = w1_process_command_root(msg, m, nsp->portid);
                         goto out_cont;
                 }
 
@@ -357,41 +539,24 @@ static void w1_cn_callback(struct cn_msg *msg, struct netlink_skb_parms *nsp)
                 if (!mlen)
                         goto out_cont;
 
-                mutex_lock(&dev->mutex);
-
-                if (sl && w1_reset_select_slave(sl)) {
-                        err = -ENODEV;
-                        goto out_up;
-                }
+                atomic_inc(&block->refcnt);
+                node->async.cb = w1_process_cb;
+                node->block = block;
+                node->m = (struct w1_netlink_msg *)((u8 *)&block->msg +
+                        (size_t)((u8 *)m - (u8 *)msg));
+                node->sl = sl;
+                node->dev = dev;
 
-                while (mlen) {
-                        cmd = (struct w1_netlink_cmd *)cmd_data;
+                mutex_lock(&dev->list_mutex);
+                list_add_tail(&node->async.async_entry, &dev->async_list);
+                wake_up_process(dev->thread);
+                mutex_unlock(&dev->list_mutex);
+                ++node;
 
-                        if (cmd->len + sizeof(struct w1_netlink_cmd) > mlen) {
-                                err = -E2BIG;
-                                break;
-                        }
-
-                        if (sl)
-                                err = w1_process_command_slave(sl, msg, m, cmd);
-                        else
-                                err = w1_process_command_master(dev, msg, m, cmd);
-
-                        w1_netlink_send_error(msg, m, cmd, err);
-                        err = 0;
-
-                        cmd_data += cmd->len + sizeof(struct w1_netlink_cmd);
-                        mlen -= cmd->len + sizeof(struct w1_netlink_cmd);
-                }
-out_up:
-                atomic_dec(&dev->refcnt);
-                if (sl)
-                        atomic_dec(&sl->refcnt);
-                mutex_unlock(&dev->mutex);
 out_cont:
-                if (!cmd || err)
-                        w1_netlink_send_error(msg, m, cmd, err);
-                msg->len -= sizeof(struct w1_netlink_msg) + m->len;
+                if (err)
+                        w1_netlink_send_error(msg, m, NULL, nsp->portid, err);
+                msg_len -= sizeof(struct w1_netlink_msg) + m->len;
                 m = (struct w1_netlink_msg *)(((u8 *)m) + sizeof(struct w1_netlink_msg) + m->len);
 
                 /*
@@ -400,6 +565,8 @@ out_cont:
                 if (err == -ENODEV)
                         err = 0;
         }
+        if (block && atomic_sub_return(1, &block->refcnt) == 0)
+                kfree(block);
 }
 
 int w1_init_netlink(void)