diff options
| -rw-r--r-- | drivers/w1/w1_netlink.c | 142 |
1 files changed, 77 insertions, 65 deletions
diff --git a/drivers/w1/w1_netlink.c b/drivers/w1/w1_netlink.c index 8a70215..a94336b 100644 --- a/drivers/w1/w1_netlink.c +++ b/drivers/w1/w1_netlink.c @@ -95,51 +95,8 @@ static int w1_process_search_command(struct w1_master *dev, struct cn_msg *msg, return 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; - struct cn_msg *msg; - struct w1_netlink_msg *hdr; - struct w1_netlink_cmd *cmd; - - 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; - - switch (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)); - break; - default: - cmd->res = EINVAL; - cn_netlink_send(msg, 0, GFP_KERNEL); - break; - } - - kfree(msg); - return err; -} - -static int w1_send_read_reply(struct w1_slave *sl, struct cn_msg *msg, - struct w1_netlink_msg *hdr, struct w1_netlink_cmd *cmd) +static int w1_send_read_reply(struct cn_msg *msg, struct w1_netlink_msg *hdr, + struct w1_netlink_cmd *cmd) { void *data; struct w1_netlink_msg *h; @@ -163,7 +120,8 @@ static int w1_send_read_reply(struct w1_slave *sl, struct cn_msg *msg, memcpy(c, cmd, sizeof(struct w1_netlink_cmd)); cm->ack = msg->seq+1; - cm->len = sizeof(struct w1_netlink_msg) + sizeof(struct w1_netlink_cmd) + cmd->len; + cm->len = sizeof(struct w1_netlink_msg) + + sizeof(struct w1_netlink_cmd) + cmd->len; h->len = sizeof(struct w1_netlink_cmd) + cmd->len; @@ -176,35 +134,89 @@ static int w1_send_read_reply(struct w1_slave *sl, struct cn_msg *msg, return err; } -static int w1_process_command_slave(struct w1_slave *sl, struct cn_msg *msg, +static int w1_process_command_io(struct w1_master *dev, struct cn_msg *msg, struct w1_netlink_msg *hdr, struct w1_netlink_cmd *cmd) { int err = 0; - 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); + switch (cmd->cmd) { + case W1_CMD_TOUCH: + w1_touch_block(dev, cmd->data, cmd->len); + w1_send_read_reply(msg, hdr, cmd); + break; + case W1_CMD_READ: + w1_read_block(dev, cmd->data, cmd->len); + w1_send_read_reply(msg, hdr, cmd); + break; + case W1_CMD_WRITE: + w1_write_block(dev, cmd->data, cmd->len); + break; + default: + err = -1; + break; + } + + 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) +{ + int err = -EINVAL; + struct cn_msg *msg; + struct w1_netlink_msg *hdr; + struct w1_netlink_cmd *cmd; + + 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; switch (cmd->cmd) { - case W1_CMD_TOUCH: - w1_touch_block(sl->master, cmd->data, cmd->len); - w1_send_read_reply(sl, msg, hdr, cmd); - break; - case W1_CMD_READ: - w1_read_block(sl->master, cmd->data, cmd->len); - w1_send_read_reply(sl, msg, hdr, cmd); - break; - case W1_CMD_WRITE: - w1_write_block(sl->master, cmd->data, cmd->len); - break; - default: - err = -1; - break; + case W1_CMD_SEARCH: + case W1_CMD_ALARM_SEARCH: + err = w1_process_search_command(dev, msg, + PAGE_SIZE - msg->len - sizeof(struct cn_msg)); + break; + case W1_CMD_READ: + case W1_CMD_WRITE: + case W1_CMD_TOUCH: + err = w1_process_command_io(dev, msg, hdr, cmd); + break; + default: + cmd->res = EINVAL; + cn_netlink_send(msg, 0, GFP_KERNEL); + break; } + kfree(msg); return err; } +static int w1_process_command_slave(struct w1_slave *sl, struct cn_msg *msg, + struct w1_netlink_msg *hdr, 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, msg, hdr, cmd); +} + static int w1_process_command_root(struct cn_msg *msg, struct w1_netlink_msg *mcmd) { struct w1_master *m; @@ -214,7 +226,7 @@ static int w1_process_command_root(struct cn_msg *msg, struct w1_netlink_msg *mc if (mcmd->type != W1_LIST_MASTERS) { printk(KERN_NOTICE "%s: msg: %x.%x, wrong type: %u, len: %u.\n", - __func__, msg->id.idx, msg->id.val, mcmd->type, mcmd->len); + __func__, msg->id.idx, msg->id.val, mcmd->type, mcmd->len); return -EPROTO; } |