static int rmnet_usb_data_dmux(struct sk_buff *skb, struct urb *rx_urb)
{
struct mux_hdr *hdr;
size_t pad_len;
size_t total_len;
unsigned int mux_id;
hdr = (struct mux_hdr *)skb->data;
mux_id = hdr->mux_id;
if (!mux_id || mux_id > no_rmnet_insts_per_dev) {
pr_err_ratelimited("%s: Invalid data channel id %u.\n",
__func__, mux_id);
return -EINVAL;
}
pad_len = hdr->padding_info >> MUX_PAD_SHIFT;
if (pad_len > MAX_PAD_BYTES(4)) {
pr_err_ratelimited("%s: Invalid pad len %d\n",
__func__, pad_len);
return -EINVAL;
}
total_len = le16_to_cpu(hdr->pkt_len_w_padding);
if (!total_len || !(total_len - pad_len)) {
pr_err_ratelimited("%s: Invalid pkt length %d\n", __func__,
total_len);
return -EINVAL;
}
skb->data = (unsigned char *)(hdr + 1);
skb_reset_tail_pointer(skb);
rx_urb->actual_length = total_len - pad_len;
return mux_id - 1;
}
static int rmnet_usb_ctrl_dmux(struct ctrl_pkt_list_elem *clist)
{
struct mux_hdr *hdr;
size_t pad_len;
size_t total_len;
unsigned int mux_id;
hdr = (struct mux_hdr *)clist->cpkt.data;
pad_len = hdr->padding_info & MUX_CTRL_PADLEN_MASK;
if (pad_len > MAX_PAD_BYTES(4)) {
pr_err_ratelimited("%s: Invalid pad len %d\n", __func__,
pad_len);
return -EINVAL;
}
mux_id = hdr->mux_id;
if (!mux_id || mux_id > insts_per_dev) {
pr_err_ratelimited("%s: Invalid mux id %d\n", __func__, mux_id);
return -EINVAL;
}
total_len = ntohs(hdr->pkt_len_w_padding);
if (!total_len || !(total_len - pad_len)) {
pr_err_ratelimited("%s: Invalid pkt length %d\n", __func__,
total_len);
return -EINVAL;
}
clist->cpkt.data_size = total_len - pad_len;
return mux_id - 1;
}
static int rmnet_usb_bind(struct usbnet *usbnet, struct usb_interface *iface)
{
struct usb_host_endpoint *endpoint = NULL;
struct usb_host_endpoint *bulk_in = NULL;
struct usb_host_endpoint *bulk_out = NULL;
struct usb_host_endpoint *int_in = NULL;
struct driver_info *info = usbnet->driver_info;
int status = 0;
int i;
int numends;
bool mux;
mux = test_bit(info->data, &mux_enabled);
mux = mux_enabled_per_pid;
numends = iface->cur_altsetting->desc.bNumEndpoints;
for (i = 0; i < numends; i++) {
endpoint = iface->cur_altsetting->endpoint + i;
if (!endpoint) {
dev_err(&iface->dev, "%s: invalid endpoint %u\n",
__func__, i);
status = -EINVAL;
goto out;
}
if (usb_endpoint_is_bulk_in(&endpoint->desc))
bulk_in = endpoint;
else if (usb_endpoint_is_bulk_out(&endpoint->desc))
bulk_out = endpoint;
else if (usb_endpoint_is_int_in(&endpoint->desc))
int_in = endpoint;
}
if (!bulk_in || !bulk_out || !int_in) {
dev_err(&iface->dev, "%s: invalid endpoints\n", __func__);
status = -EINVAL;
goto out;
}
usbnet->in = usb_rcvbulkpipe(usbnet->udev,
bulk_in->desc.bEndpointAddress & USB_ENDPOINT_NUMBER_MASK);
usbnet->out = usb_sndbulkpipe(usbnet->udev,
bulk_out->desc.bEndpointAddress & USB_ENDPOINT_NUMBER_MASK);
usbnet->status = int_in;
if (mux && (info->in > no_fwd_rmnet_links))
strlcpy(usbnet->net->name, rev_rmnet_names[info->data],
IFNAMSIZ);
else
strlcpy(usbnet->net->name, rmnet_names[info->data],
IFNAMSIZ);
if (mux)
usbnet->rx_urb_size = usbnet->hard_mtu + sizeof(struct mux_hdr)
+ MAX_PAD_BYTES(4);
out:
return status;
}
static void rmnet_usb_ctrl_mux(unsigned int id, struct ctrl_pkt *cpkt)
{
struct mux_hdr *hdr;
size_t len;
size_t pad_len = 0;
hdr = (struct mux_hdr *)cpkt->data;
hdr->mux_id = id + 1;
len = cpkt->data_size - sizeof(struct mux_hdr) - MAX_PAD_BYTES(4);
/*add padding if len is not 4 byte aligned*/
pad_len = ALIGN(len, 4) - len;
hdr->pkt_len_w_padding = htons(len + pad_len);
hdr->padding_info = (pad_len & MUX_CTRL_PADLEN_MASK) | MUX_CTRL_MASK;
cpkt->data_size = sizeof(struct mux_hdr) +
ntohs(hdr->pkt_len_w_padding);
}
static ssize_t rmnet_ctl_write(struct file *file, const char __user * buf,
size_t size, loff_t *pos)
{
int status;
size_t total_len;
void *wbuf;
void *actual_data;
struct ctrl_pkt *cpkt;
struct rmnet_ctrl_dev *dev = file->private_data;
rewrite:
if (!dev)
return -ENODEV;
if (!size)
return -EINVAL;
if (!test_bit(RMNET_CTRL_DEV_READY, &dev->status))
return -ENETRESET;
DBG("%s: Writing %i bytes on %s\n", __func__, size, dev->name);
total_len = size;
if (test_bit(RMNET_CTRL_DEV_MUX_EN, &dev->status))
total_len += sizeof(struct mux_hdr) + MAX_PAD_BYTES(4);
wbuf = kmalloc(total_len , GFP_KERNEL);
if (!wbuf)
return -ENOMEM;
cpkt = kmalloc(sizeof(struct ctrl_pkt), GFP_KERNEL);
if (!cpkt) {
kfree(wbuf);
return -ENOMEM;
}
actual_data = cpkt->data = wbuf;
cpkt->data_size = total_len;
cpkt->ctxt = dev;
if (test_bit(RMNET_CTRL_DEV_MUX_EN, &dev->status)) {
actual_data = wbuf + sizeof(struct mux_hdr);
rmnet_usb_ctrl_mux(dev->ch_id, cpkt);
}
status = copy_from_user(actual_data, buf, size);
if (status) {
dev_err(dev->devicep,
"%s: Unable to copy data from userspace %d\n",
__func__, status);
kfree(wbuf);
kfree(cpkt);
return status;
}
DUMP_BUFFER("Write: ", size, buf);
status = rmnet_usb_ctrl_write(dev, cpkt, size);
if (status == size)
return size;
else if (status == -EAGAIN) {
dev_err(dev->devicep, "%s: rewrite\n", __func__);
goto rewrite;
}
return status;
}
static ssize_t rmnet_ctl_write(struct file *file, const char __user * buf,
size_t size, loff_t *pos)
{
int status;
size_t total_len;
void *wbuf;
void *actual_data;
struct ctrl_pkt *cpkt;
struct rmnet_ctrl_dev *dev = file->private_data;
if (!dev)
return -ENODEV;
if (size <= 0)
return -EINVAL;
if (!test_bit(RMNET_CTRL_DEV_READY, &dev->cudev->status))
return -ENETRESET;
DBG("%s: Writing %i bytes on %s\n", __func__, size, dev->name);
#ifdef CONFIG_QCT_9K_MODEM
if (get_radio_flag() & RADIO_FLAG_MORE_LOG)
pr_info("[RMNET] W: %i\n", size);
#endif
total_len = size;
if (test_bit(RMNET_CTRL_DEV_MUX_EN, &dev->status))
total_len += sizeof(struct mux_hdr) + MAX_PAD_BYTES(4);
wbuf = kmalloc(total_len , GFP_KERNEL);
if (!wbuf)
return -ENOMEM;
cpkt = kmalloc(sizeof(struct ctrl_pkt), GFP_KERNEL);
if (!cpkt) {
kfree(wbuf);
return -ENOMEM;
}
actual_data = cpkt->data = wbuf;
cpkt->data_size = total_len;
cpkt->ctxt = dev;
if (test_bit(RMNET_CTRL_DEV_MUX_EN, &dev->status)) {
actual_data = wbuf + sizeof(struct mux_hdr);
rmnet_usb_ctrl_mux(dev->ch_id, cpkt);
}
status = copy_from_user(actual_data, buf, size);
if (status) {
dev_err(dev->devicep,
"%s: Unable to copy data from userspace %d\n",
__func__, status);
kfree(wbuf);
kfree(cpkt);
return status;
}
DUMP_BUFFER("Write: ", size, buf);
#ifdef CONFIG_QCT_9K_MODEM
status = rmnet_usb_ctrl_write(dev, cpkt, size);
if (status == size)
return size;
else
pr_err("[%s] status %d\n", __func__, status);
#else
status = rmnet_usb_ctrl_write_cmd(dev->cudev,
USB_CDC_SEND_ENCAPSULATED_COMMAND, 0, cpkt->data,
cpkt->data_size);
if (status > 0)
dev->cudev->snd_encap_cmd_cnt++;
kfree(cpkt->data);
kfree(cpkt);
#endif
return status;
}
