1 files changed, 263 insertions, 96 deletions

diff --git a/drivers/w1/w1_netlink.c b/drivers/w1/w1_netlink.c
index 40788c9..5234964 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)