diff --git a/drivers/w1/w1_netlink.c b/drivers/w1/w1_netlink.c index 8a7021577f32..a94336be7654 100644 --- a/drivers/w1/w1_netlink.c +++ b/drivers/w1/w1_netlink.c @@ -95,6 +95,70 @@ static int w1_process_search_command(struct w1_master *dev, struct cn_msg *msg, return 0; } +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; + struct w1_netlink_cmd *c; + struct cn_msg *cm; + int err; + + data = kzalloc(sizeof(struct cn_msg) + + sizeof(struct w1_netlink_msg) + + sizeof(struct w1_netlink_cmd) + + cmd->len, GFP_KERNEL); + if (!data) + return -ENOMEM; + + cm = (struct cn_msg *)(data); + h = (struct w1_netlink_msg *)(cm + 1); + c = (struct w1_netlink_cmd *)(h + 1); + + memcpy(cm, msg, sizeof(struct cn_msg)); + memcpy(h, hdr, sizeof(struct w1_netlink_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; + + h->len = sizeof(struct w1_netlink_cmd) + cmd->len; + + memcpy(c->data, cmd->data, c->len); + + err = cn_netlink_send(cm, 0, GFP_KERNEL); + + kfree(data); + + return err; +} + +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; + + 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) { @@ -123,86 +187,34 @@ static int w1_process_command_master(struct w1_master *dev, struct cn_msg *req_m cmd->len = 0; switch (cmd->cmd) { - case W1_CMD_SEARCH: - case W1_CMD_ALARM_SEARCH: - err = w1_process_search_command(dev, msg, + 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; + 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_send_read_reply(struct w1_slave *sl, struct cn_msg *msg, - struct w1_netlink_msg *hdr, struct w1_netlink_cmd *cmd) -{ - void *data; - struct w1_netlink_msg *h; - struct w1_netlink_cmd *c; - struct cn_msg *cm; - int err; - - data = kzalloc(sizeof(struct cn_msg) + - sizeof(struct w1_netlink_msg) + - sizeof(struct w1_netlink_cmd) + - cmd->len, GFP_KERNEL); - if (!data) - return -ENOMEM; - - cm = (struct cn_msg *)(data); - h = (struct w1_netlink_msg *)(cm + 1); - c = (struct w1_netlink_cmd *)(h + 1); - - memcpy(cm, msg, sizeof(struct cn_msg)); - memcpy(h, hdr, sizeof(struct w1_netlink_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; - - h->len = sizeof(struct w1_netlink_cmd) + cmd->len; - - memcpy(c->data, cmd->data, c->len); - - err = cn_netlink_send(cm, 0, GFP_KERNEL); - - kfree(data); - - 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) { - 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); + __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(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; - } - - return err; + 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) @@ -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; }