struct sk_buff *alloc_canfd_skb(struct net_device *dev,
struct canfd_frame **cfd)
{
struct sk_buff *skb;
skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
sizeof(struct canfd_frame));
if (unlikely(!skb))
return NULL;
skb->protocol = htons(ETH_P_CANFD);
skb->pkt_type = PACKET_BROADCAST;
skb->ip_summed = CHECKSUM_UNNECESSARY;
skb_reset_mac_header(skb);
skb_reset_network_header(skb);
skb_reset_transport_header(skb);
can_skb_reserve(skb);
can_skb_prv(skb)->ifindex = dev->ifindex;
can_skb_prv(skb)->skbcnt = 0;
*cfd = (struct canfd_frame *)skb_put(skb, sizeof(struct canfd_frame));
memset(*cfd, 0, sizeof(struct canfd_frame));
return skb;
}
struct sk_buff *alloc_can_skb(struct net_device *dev, struct can_frame **cf)
{
struct sk_buff *skb;
skb = netdev_alloc_skb(dev, sizeof(struct can_skb_priv) +
sizeof(struct can_frame));
if (unlikely(!skb))
return NULL;
skb->protocol = htons(ETH_P_CAN);
skb->pkt_type = PACKET_BROADCAST;
skb->ip_summed = CHECKSUM_UNNECESSARY;
can_skb_reserve(skb);
can_skb_prv(skb)->ifindex = dev->ifindex;
*cf = (struct can_frame *)skb_put(skb, sizeof(struct can_frame));
memset(*cf, 0, sizeof(struct can_frame));
return skb;
}
/* Send one completely decapsulated can_frame to the network layer */
static void slc_bump(struct slcan *sl)
{
struct sk_buff *skb;
struct can_frame cf;
int i, dlc_pos, tmp;
unsigned long ultmp;
char cmd = sl->rbuff[0];
if ((cmd != 't') && (cmd != 'T') && (cmd != 'r') && (cmd != 'R'))
return;
if (cmd & 0x20) /* tiny chars 'r' 't' => standard frame format */
dlc_pos = 4; /* dlc position tiiid */
else
dlc_pos = 9; /* dlc position Tiiiiiiiid */
if (!((sl->rbuff[dlc_pos] >= '0') && (sl->rbuff[dlc_pos] < '9')))
return;
cf.can_dlc = sl->rbuff[dlc_pos] - '0'; /* get can_dlc from ASCII val */
sl->rbuff[dlc_pos] = 0; /* terminate can_id string */
if (kstrtoul(sl->rbuff+1, 16, &ultmp))
return;
cf.can_id = ultmp;
if (!(cmd & 0x20)) /* NO tiny chars => extended frame format */
cf.can_id |= CAN_EFF_FLAG;
if ((cmd | 0x20) == 'r') /* RTR frame */
cf.can_id |= CAN_RTR_FLAG;
*(u64 *) (&cf.data) = 0; /* clear payload */
for (i = 0, dlc_pos++; i < cf.can_dlc; i++) {
tmp = hex_to_bin(sl->rbuff[dlc_pos++]);
if (tmp < 0)
return;
cf.data[i] = (tmp << 4);
tmp = hex_to_bin(sl->rbuff[dlc_pos++]);
if (tmp < 0)
return;
cf.data[i] |= tmp;
}
skb = dev_alloc_skb(sizeof(struct can_frame) +
sizeof(struct can_skb_priv));
if (!skb)
return;
skb->dev = sl->dev;
skb->protocol = htons(ETH_P_CAN);
skb->pkt_type = PACKET_BROADCAST;
skb->ip_summed = CHECKSUM_UNNECESSARY;
can_skb_reserve(skb);
can_skb_prv(skb)->ifindex = sl->dev->ifindex;
memcpy(skb_put(skb, sizeof(struct can_frame)),
&cf, sizeof(struct can_frame));
netif_rx_ni(skb);
sl->dev->stats.rx_packets++;
sl->dev->stats.rx_bytes += cf.can_dlc;
}
/* Send one completely decapsulated can_frame to the network layer */
static void slc_bump(struct slcan *sl)
{
struct sk_buff *skb;
struct can_frame cf;
int i, tmp;
u32 tmpid;
char *cmd = sl->rbuff;
cf.can_id = 0;
switch (*cmd) {
case 'r':
cf.can_id = CAN_RTR_FLAG;
/* fallthrough */
case 't':
/* store dlc ASCII value and terminate SFF CAN ID string */
cf.can_dlc = sl->rbuff[SLC_CMD_LEN + SLC_SFF_ID_LEN];
sl->rbuff[SLC_CMD_LEN + SLC_SFF_ID_LEN] = 0;
/* point to payload data behind the dlc */
cmd += SLC_CMD_LEN + SLC_SFF_ID_LEN + 1;
break;
case 'R':
cf.can_id = CAN_RTR_FLAG;
/* fallthrough */
case 'T':
cf.can_id |= CAN_EFF_FLAG;
/* store dlc ASCII value and terminate EFF CAN ID string */
cf.can_dlc = sl->rbuff[SLC_CMD_LEN + SLC_EFF_ID_LEN];
sl->rbuff[SLC_CMD_LEN + SLC_EFF_ID_LEN] = 0;
/* point to payload data behind the dlc */
cmd += SLC_CMD_LEN + SLC_EFF_ID_LEN + 1;
break;
default:
return;
}
if (kstrtou32(sl->rbuff + SLC_CMD_LEN, 16, &tmpid))
return;
cf.can_id |= tmpid;
/* get can_dlc from sanitized ASCII value */
if (cf.can_dlc >= '0' && cf.can_dlc < '9')
cf.can_dlc -= '0';
else
return;
*(u64 *) (&cf.data) = 0; /* clear payload */
/* RTR frames may have a dlc > 0 but they never have any data bytes */
if (!(cf.can_id & CAN_RTR_FLAG)) {
for (i = 0; i < cf.can_dlc; i++) {
tmp = hex_to_bin(*cmd++);
if (tmp < 0)
return;
cf.data[i] = (tmp << 4);
tmp = hex_to_bin(*cmd++);
if (tmp < 0)
return;
cf.data[i] |= tmp;
}
}
skb = dev_alloc_skb(sizeof(struct can_frame) +
sizeof(struct can_skb_priv));
if (!skb)
return;
skb->dev = sl->dev;
skb->protocol = htons(ETH_P_CAN);
skb->pkt_type = PACKET_BROADCAST;
skb->ip_summed = CHECKSUM_UNNECESSARY;
can_skb_reserve(skb);
can_skb_prv(skb)->ifindex = sl->dev->ifindex;
can_skb_prv(skb)->skbcnt = 0;
memcpy(skb_put(skb, sizeof(struct can_frame)),
&cf, sizeof(struct can_frame));
sl->dev->stats.rx_packets++;
sl->dev->stats.rx_bytes += cf.can_dlc;
netif_rx_ni(skb);
}
static void raw_rcv(struct sk_buff *oskb, void *data)
{
struct sock *sk = (struct sock *)data;
struct raw_sock *ro = raw_sk(sk);
struct sockaddr_can *addr;
struct sk_buff *skb;
unsigned int *pflags;
/* check the received tx sock reference */
if (!ro->recv_own_msgs && oskb->sk == sk)
return;
/* do not pass non-CAN2.0 frames to a legacy socket */
if (!ro->fd_frames && oskb->len != CAN_MTU)
return;
/* eliminate multiple filter matches for the same skb */
if (this_cpu_ptr(ro->uniq)->skb == oskb &&
this_cpu_ptr(ro->uniq)->skbcnt == can_skb_prv(oskb)->skbcnt) {
if (ro->join_filters) {
this_cpu_inc(ro->uniq->join_rx_count);
/* drop frame until all enabled filters matched */
if (this_cpu_ptr(ro->uniq)->join_rx_count < ro->count)
return;
} else {
return;
}
} else {
this_cpu_ptr(ro->uniq)->skb = oskb;
this_cpu_ptr(ro->uniq)->skbcnt = can_skb_prv(oskb)->skbcnt;
this_cpu_ptr(ro->uniq)->join_rx_count = 1;
/* drop first frame to check all enabled filters? */
if (ro->join_filters && ro->count > 1)
return;
}
/* clone the given skb to be able to enqueue it into the rcv queue */
skb = skb_clone(oskb, GFP_ATOMIC);
if (!skb)
return;
/*
* Put the datagram to the queue so that raw_recvmsg() can
* get it from there. We need to pass the interface index to
* raw_recvmsg(). We pass a whole struct sockaddr_can in skb->cb
* containing the interface index.
*/
sock_skb_cb_check_size(sizeof(struct sockaddr_can));
addr = (struct sockaddr_can *)skb->cb;
memset(addr, 0, sizeof(*addr));
addr->can_family = AF_CAN;
addr->can_ifindex = skb->dev->ifindex;
/* add CAN specific message flags for raw_recvmsg() */
pflags = raw_flags(skb);
*pflags = 0;
if (oskb->sk)
*pflags |= MSG_DONTROUTE;
if (oskb->sk == sk)
*pflags |= MSG_CONFIRM;
if (sock_queue_rcv_skb(sk, skb) < 0)
kfree_skb(skb);
}
static int raw_sendmsg(struct kiocb *iocb, struct socket *sock,
struct msghdr *msg, size_t size)
{
struct sock *sk = sock->sk;
struct raw_sock *ro = raw_sk(sk);
struct sk_buff *skb;
struct net_device *dev;
int ifindex;
int err;
if (msg->msg_name) {
struct sockaddr_can *addr =
(struct sockaddr_can *)msg->msg_name;
if (msg->msg_namelen < sizeof(*addr))
return -EINVAL;
if (addr->can_family != AF_CAN)
return -EINVAL;
ifindex = addr->can_ifindex;
} else
ifindex = ro->ifindex;
if (ro->fd_frames) {
if (unlikely(size != CANFD_MTU && size != CAN_MTU))
return -EINVAL;
} else {
if (unlikely(size != CAN_MTU))
return -EINVAL;
}
dev = dev_get_by_index(&init_net, ifindex);
if (!dev)
return -ENXIO;
skb = sock_alloc_send_skb(sk, size + sizeof(struct can_skb_priv),
msg->msg_flags & MSG_DONTWAIT, &err);
if (!skb)
goto put_dev;
can_skb_reserve(skb);
can_skb_prv(skb)->ifindex = dev->ifindex;
err = memcpy_fromiovec(skb_put(skb, size), msg->msg_iov, size);
if (err < 0)
goto free_skb;
err = sock_tx_timestamp(sk, &skb_shinfo(skb)->tx_flags);
if (err < 0)
goto free_skb;
skb->dev = dev;
skb->sk = sk;
err = can_send(skb, ro->loopback);
dev_put(dev);
if (err)
goto send_failed;
return size;
free_skb:
kfree_skb(skb);
put_dev:
dev_put(dev);
send_failed:
return err;
}
