123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758 |
- #include <linux/slab.h>
- #include <linux/skbuff.h>
- #include <linux/netlink.h>
- #include <linux/connector.h>
- #include "w1.h"
- #include "w1_log.h"
- #include "w1_netlink.h"
- #if defined(CONFIG_W1_CON) && (defined(CONFIG_CONNECTOR) || (defined(CONFIG_CONNECTOR_MODULE) && defined(CONFIG_W1_MODULE)))
- #define MIN(a, b) (((a) < (b)) ? (a) : (b))
- struct w1_cb_block {
- atomic_t refcnt;
- u32 portid;
-
- u16 maxlen;
-
- struct cn_msg *first_cn;
- struct cn_msg *cn;
- struct w1_netlink_msg *msg;
- struct w1_netlink_cmd *cmd;
- struct w1_netlink_msg *cur_msg;
-
- struct cn_msg request_cn;
-
- };
- struct w1_cb_node {
- struct w1_async_cmd async;
-
- struct w1_cb_block *block;
- struct w1_netlink_msg *msg;
- struct w1_slave *sl;
- struct w1_master *dev;
- };
- static u16 w1_reply_len(struct w1_cb_block *block)
- {
- if (!block->cn)
- return 0;
- return (u8 *)block->cn - (u8 *)block->first_cn + block->cn->len;
- }
- static void w1_unref_block(struct w1_cb_block *block)
- {
- if (atomic_sub_return(1, &block->refcnt) == 0) {
- u16 len = w1_reply_len(block);
- if (len) {
- cn_netlink_send_mult(block->first_cn, len,
- block->portid, 0, GFP_KERNEL);
- }
- kfree(block);
- }
- }
- static void w1_reply_make_space(struct w1_cb_block *block, u16 space)
- {
- u16 len = w1_reply_len(block);
- if (len + space >= block->maxlen) {
- cn_netlink_send_mult(block->first_cn, len, block->portid, 0, GFP_KERNEL);
- block->first_cn->len = 0;
- block->cn = NULL;
- block->msg = NULL;
- block->cmd = NULL;
- }
- }
- static void w1_netlink_check_send(struct w1_cb_block *block)
- {
- if (!(block->request_cn.flags & W1_CN_BUNDLE) && block->cn)
- w1_reply_make_space(block, block->maxlen);
- }
- static void w1_netlink_setup_msg(struct w1_cb_block *block, u32 ack)
- {
- if (block->cn && block->cn->ack == ack) {
- block->msg = (struct w1_netlink_msg *)(block->cn->data + block->cn->len);
- } else {
-
- if (block->cn)
- block->cn = (struct cn_msg *)(block->cn->data +
- block->cn->len);
- else
- block->cn = block->first_cn;
- memcpy(block->cn, &block->request_cn, sizeof(*block->cn));
- block->cn->len = 0;
- block->cn->ack = ack;
- block->msg = (struct w1_netlink_msg *)block->cn->data;
- }
- }
- static void w1_netlink_queue_cmd(struct w1_cb_block *block,
- struct w1_netlink_cmd *cmd)
- {
- u32 space;
- w1_reply_make_space(block, sizeof(struct cn_msg) +
- sizeof(struct w1_netlink_msg) + sizeof(*cmd) + cmd->len);
-
- w1_netlink_setup_msg(block, block->request_cn.seq + 1);
- memcpy(block->msg, block->cur_msg, sizeof(*block->msg));
- block->cn->len += sizeof(*block->msg);
- block->msg->len = 0;
- block->cmd = (struct w1_netlink_cmd *)(block->msg->data);
- space = sizeof(*cmd) + cmd->len;
- if (block->cmd != cmd)
- memcpy(block->cmd, cmd, space);
- block->cn->len += space;
- block->msg->len += space;
- }
- static void w1_netlink_queue_status(struct w1_cb_block *block,
- struct w1_netlink_msg *req_msg, struct w1_netlink_cmd *req_cmd,
- int error)
- {
- u16 space = sizeof(struct cn_msg) + sizeof(*req_msg) + sizeof(*req_cmd);
- w1_reply_make_space(block, space);
- w1_netlink_setup_msg(block, block->request_cn.ack);
- memcpy(block->msg, req_msg, sizeof(*req_msg));
- block->cn->len += sizeof(*req_msg);
- block->msg->len = 0;
- block->msg->status = (u8)-error;
- if (req_cmd) {
- struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)block->msg->data;
- memcpy(cmd, req_cmd, sizeof(*cmd));
- block->cn->len += sizeof(*cmd);
- block->msg->len += sizeof(*cmd);
- cmd->len = 0;
- }
- w1_netlink_check_send(block);
- }
- static void w1_netlink_send_error(struct cn_msg *cn, struct w1_netlink_msg *msg,
- int portid, int error)
- {
- struct {
- struct cn_msg cn;
- struct w1_netlink_msg msg;
- } packet;
- memcpy(&packet.cn, cn, sizeof(packet.cn));
- memcpy(&packet.msg, msg, sizeof(packet.msg));
- packet.cn.len = sizeof(packet.msg);
- packet.msg.len = 0;
- packet.msg.status = (u8)-error;
- cn_netlink_send(&packet.cn, portid, 0, GFP_KERNEL);
- }
- void w1_netlink_send(struct w1_master *dev, struct w1_netlink_msg *msg)
- {
- struct {
- struct cn_msg cn;
- struct w1_netlink_msg msg;
- } packet;
- memset(&packet, 0, sizeof(packet));
- packet.cn.id.idx = CN_W1_IDX;
- packet.cn.id.val = CN_W1_VAL;
- packet.cn.seq = dev->seq++;
- packet.cn.len = sizeof(*msg);
- memcpy(&packet.msg, msg, sizeof(*msg));
- packet.msg.len = 0;
- cn_netlink_send(&packet.cn, 0, 0, GFP_KERNEL);
- }
- static void w1_send_slave(struct w1_master *dev, u64 rn)
- {
- struct w1_cb_block *block = dev->priv;
- struct w1_netlink_cmd *cache_cmd = block->cmd;
- u64 *data;
- w1_reply_make_space(block, sizeof(*data));
-
- if (!block->cmd) {
- cache_cmd->len = 0;
- w1_netlink_queue_cmd(block, cache_cmd);
- }
- data = (u64 *)(block->cmd->data + block->cmd->len);
- *data = rn;
- block->cn->len += sizeof(*data);
- block->msg->len += sizeof(*data);
- block->cmd->len += sizeof(*data);
- }
- static void w1_found_send_slave(struct w1_master *dev, u64 rn)
- {
-
- w1_slave_found(dev, rn);
- w1_send_slave(dev, rn);
- }
- static int w1_get_slaves(struct w1_master *dev, struct w1_netlink_cmd *req_cmd)
- {
- struct w1_slave *sl;
- req_cmd->len = 0;
- w1_netlink_queue_cmd(dev->priv, req_cmd);
- 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, req_cmd->cmd == W1_CMD_ALARM_SEARCH ?
- W1_ALARM_SEARCH : W1_SEARCH, w1_found_send_slave);
- }
- return 0;
- }
- static int w1_process_command_io(struct w1_master *dev,
- struct w1_netlink_cmd *cmd)
- {
- int err = 0;
- switch (cmd->cmd) {
- case W1_CMD_TOUCH:
- w1_touch_block(dev, cmd->data, cmd->len);
- w1_netlink_queue_cmd(dev->priv, cmd);
- break;
- case W1_CMD_READ:
- w1_read_block(dev, cmd->data, cmd->len);
- w1_netlink_queue_cmd(dev->priv, cmd);
- break;
- case W1_CMD_WRITE:
- w1_write_block(dev, cmd->data, cmd->len);
- break;
- default:
- err = -EINVAL;
- break;
- }
- return err;
- }
- static int w1_process_command_addremove(struct w1_master *dev,
- struct w1_netlink_cmd *cmd)
- {
- struct w1_slave *sl;
- int err = 0;
- struct w1_reg_num *id;
- if (cmd->len != sizeof(*id))
- return -EINVAL;
- id = (struct w1_reg_num *)cmd->data;
- 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;
- }
- return err;
- }
- static int w1_process_command_master(struct w1_master *dev,
- struct w1_netlink_cmd *req_cmd)
- {
- int err = -EINVAL;
-
- switch (req_cmd->cmd) {
- case W1_CMD_SEARCH:
- case W1_CMD_ALARM_SEARCH:
- case W1_CMD_LIST_SLAVES:
- mutex_unlock(&dev->bus_mutex);
- err = w1_get_slaves(dev, req_cmd);
- mutex_lock(&dev->bus_mutex);
- break;
- case W1_CMD_READ:
- case W1_CMD_WRITE:
- case W1_CMD_TOUCH:
- err = w1_process_command_io(dev, req_cmd);
- break;
- 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_cmd);
- mutex_unlock(&dev->mutex);
- mutex_lock(&dev->bus_mutex);
- break;
- default:
- err = -EINVAL;
- break;
- }
- return err;
- }
- static int w1_process_command_slave(struct w1_slave *sl,
- struct w1_netlink_cmd *cmd)
- {
- dev_dbg(&sl->master->dev, "%s: %02x.%012llx.%02x: cmd=%02x, len=%u.\n",
- __func__, sl->reg_num.family, (unsigned long long)sl->reg_num.id,
- sl->reg_num.crc, cmd->cmd, cmd->len);
- return w1_process_command_io(sl->master, cmd);
- }
- static int w1_process_command_root(struct cn_msg *req_cn, u32 portid)
- {
- struct w1_master *dev;
- struct cn_msg *cn;
- struct w1_netlink_msg *msg;
- u32 *id;
- cn = kmalloc(PAGE_SIZE, GFP_KERNEL);
- if (!cn)
- return -ENOMEM;
- cn->id.idx = CN_W1_IDX;
- cn->id.val = CN_W1_VAL;
- cn->seq = req_cn->seq;
- cn->ack = req_cn->seq + 1;
- cn->len = sizeof(struct w1_netlink_msg);
- msg = (struct w1_netlink_msg *)cn->data;
- msg->type = W1_LIST_MASTERS;
- msg->status = 0;
- msg->len = 0;
- id = (u32 *)msg->data;
- mutex_lock(&w1_mlock);
- list_for_each_entry(dev, &w1_masters, w1_master_entry) {
- if (cn->len + sizeof(*id) > PAGE_SIZE - sizeof(struct cn_msg)) {
- cn_netlink_send(cn, portid, 0, GFP_KERNEL);
- cn->len = sizeof(struct w1_netlink_msg);
- msg->len = 0;
- id = (u32 *)msg->data;
- }
- *id = dev->id;
- msg->len += sizeof(*id);
- cn->len += sizeof(*id);
- id++;
- }
- cn_netlink_send(cn, portid, 0, GFP_KERNEL);
- mutex_unlock(&w1_mlock);
- kfree(cn);
- return 0;
- }
- 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->msg->len;
- u16 len;
- int err = 0;
- struct w1_slave *sl = node->sl;
- struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)node->msg->data;
- mutex_lock(&dev->bus_mutex);
- dev->priv = node->block;
- if (sl && w1_reset_select_slave(sl))
- err = -ENODEV;
- node->block->cur_msg = node->msg;
- while (mlen && !err) {
- if (cmd->len + sizeof(struct w1_netlink_cmd) > mlen) {
- err = -E2BIG;
- break;
- }
- if (sl)
- err = w1_process_command_slave(sl, cmd);
- else
- err = w1_process_command_master(dev, cmd);
- w1_netlink_check_send(node->block);
- w1_netlink_queue_status(node->block, node->msg, cmd, err);
- err = 0;
- len = sizeof(*cmd) + cmd->len;
- cmd = (struct w1_netlink_cmd *)((u8 *)cmd + len);
- mlen -= len;
- }
- if (!cmd || err)
- w1_netlink_queue_status(node->block, node->msg, cmd, err);
-
- if (sl)
- w1_unref_slave(sl);
- else
- atomic_dec(&dev->refcnt);
- dev->priv = NULL;
- mutex_unlock(&dev->bus_mutex);
- mutex_lock(&dev->list_mutex);
- list_del(&async_cmd->async_entry);
- mutex_unlock(&dev->list_mutex);
- w1_unref_block(node->block);
- }
- static void w1_list_count_cmds(struct w1_netlink_msg *msg, int *cmd_count,
- u16 *slave_len)
- {
- struct w1_netlink_cmd *cmd = (struct w1_netlink_cmd *)msg->data;
- u16 mlen = msg->len;
- u16 len;
- int slave_list = 0;
- while (mlen) {
- if (cmd->len + sizeof(struct w1_netlink_cmd) > mlen)
- break;
- switch (cmd->cmd) {
- case W1_CMD_SEARCH:
- case W1_CMD_ALARM_SEARCH:
- case W1_CMD_LIST_SLAVES:
- ++slave_list;
- }
- ++*cmd_count;
- len = sizeof(*cmd) + cmd->len;
- cmd = (struct w1_netlink_cmd *)((u8 *)cmd + len);
- mlen -= len;
- }
- if (slave_list) {
- struct w1_master *dev = w1_search_master_id(msg->id.mst.id);
- if (dev) {
-
- *slave_len += sizeof(struct w1_reg_num) * slave_list *
- (dev->slave_count + dev->max_slave_count);
-
- atomic_dec(&dev->refcnt);
- }
- }
- }
- static void w1_cn_callback(struct cn_msg *cn, struct netlink_skb_parms *nsp)
- {
- struct w1_netlink_msg *msg = (struct w1_netlink_msg *)(cn + 1);
- struct w1_slave *sl;
- struct w1_master *dev;
- u16 msg_len;
- u16 slave_len = 0;
- int err = 0;
- struct w1_cb_block *block = NULL;
- struct w1_cb_node *node = NULL;
- int node_count = 0;
- int cmd_count = 0;
-
- if (cn->flags & ~(W1_CN_BUNDLE)) {
- w1_netlink_send_error(cn, msg, nsp->portid, -EINVAL);
- return;
- }
-
- msg_len = cn->len;
- while (msg_len && !err) {
- if (msg->len + sizeof(struct w1_netlink_msg) > msg_len) {
- err = -E2BIG;
- break;
- }
-
- if (msg->type == W1_MASTER_CMD || msg->type == W1_SLAVE_CMD) {
- ++node_count;
- w1_list_count_cmds(msg, &cmd_count, &slave_len);
- }
- msg_len -= sizeof(struct w1_netlink_msg) + msg->len;
- msg = (struct w1_netlink_msg *)(((u8 *)msg) +
- sizeof(struct w1_netlink_msg) + msg->len);
- }
- msg = (struct w1_netlink_msg *)(cn + 1);
- if (node_count) {
- int size;
- int reply_size = sizeof(*cn) + cn->len + slave_len;
- if (cn->flags & W1_CN_BUNDLE) {
-
- reply_size += 2 * cmd_count * (sizeof(struct cn_msg) +
- sizeof(struct w1_netlink_msg) +
- sizeof(struct w1_netlink_cmd));
- }
- reply_size = MIN(CONNECTOR_MAX_MSG_SIZE, reply_size);
-
- size =
- sizeof(struct w1_cb_block) + sizeof(*cn) + cn->len +
-
- node_count * sizeof(struct w1_cb_node) +
-
- sizeof(struct cn_msg) + reply_size;
- block = kzalloc(size, GFP_KERNEL);
- if (!block) {
-
- w1_netlink_send_error(cn, msg, nsp->portid, -ENOMEM);
- return;
- }
- atomic_set(&block->refcnt, 1);
- block->portid = nsp->portid;
- memcpy(&block->request_cn, cn, sizeof(*cn) + cn->len);
- node = (struct w1_cb_node *)(block->request_cn.data + cn->len);
-
- block->maxlen = reply_size;
- block->first_cn = (struct cn_msg *)(node + node_count);
- memset(block->first_cn, 0, sizeof(*block->first_cn));
- }
- msg_len = cn->len;
- while (msg_len && !err) {
- dev = NULL;
- sl = NULL;
- if (msg->len + sizeof(struct w1_netlink_msg) > msg_len) {
- err = -E2BIG;
- break;
- }
-
- if (msg->type == W1_LIST_MASTERS) {
- err = w1_process_command_root(cn, nsp->portid);
- goto out_cont;
- }
-
- if (!msg->len) {
- err = -EPROTO;
- goto out_cont;
- }
-
- if (msg->type == W1_MASTER_CMD) {
- dev = w1_search_master_id(msg->id.mst.id);
- } else if (msg->type == W1_SLAVE_CMD) {
- sl = w1_search_slave((struct w1_reg_num *)msg->id.id);
- if (sl)
- dev = sl->master;
- } else {
- pr_notice("%s: cn: %x.%x, wrong type: %u, len: %u.\n",
- __func__, cn->id.idx, cn->id.val,
- msg->type, msg->len);
- err = -EPROTO;
- goto out_cont;
- }
- if (!dev) {
- err = -ENODEV;
- goto out_cont;
- }
- err = 0;
- atomic_inc(&block->refcnt);
- node->async.cb = w1_process_cb;
- node->block = block;
- node->msg = (struct w1_netlink_msg *)((u8 *)&block->request_cn +
- (size_t)((u8 *)msg - (u8 *)cn));
- node->sl = sl;
- node->dev = dev;
- 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;
- out_cont:
-
- if (err)
- w1_netlink_send_error(cn, msg, nsp->portid, err);
- msg_len -= sizeof(struct w1_netlink_msg) + msg->len;
- msg = (struct w1_netlink_msg *)(((u8 *)msg) +
- sizeof(struct w1_netlink_msg) + msg->len);
-
- if (err == -ENODEV)
- err = 0;
- }
- if (block)
- w1_unref_block(block);
- }
- int w1_init_netlink(void)
- {
- struct cb_id w1_id = {.idx = CN_W1_IDX, .val = CN_W1_VAL};
- return cn_add_callback(&w1_id, "w1", &w1_cn_callback);
- }
- void w1_fini_netlink(void)
- {
- struct cb_id w1_id = {.idx = CN_W1_IDX, .val = CN_W1_VAL};
- cn_del_callback(&w1_id);
- }
- #else
- void w1_netlink_send(struct w1_master *dev, struct w1_netlink_msg *cn)
- {
- }
- int w1_init_netlink(void)
- {
- return 0;
- }
- void w1_fini_netlink(void)
- {
- }
- #endif
|