static int pn_raw_send(const void *data, int len, struct net_device *dev,
u16 dst, u16 src, u8 res)
{
struct sk_buff *skb = alloc_skb(MAX_PHONET_HEADER + len, GFP_ATOMIC);
if (skb == NULL)
return -ENOMEM;
if (phonet_address_lookup(dev_net(dev), pn_addr(dst)) == 0)
skb->pkt_type = PACKET_LOOPBACK;
skb_reserve(skb, MAX_PHONET_HEADER);
__skb_put(skb, len);
skb_copy_to_linear_data(skb, data, len);
/*
[P120208-3952]
For preventing the situation that some ISI message go into FMT channel.
*/
if ((len == 4) && ((pn_dev(dst) != 0x60) && (pn_dev(dst) != 0x64))) {
pr_info("dst 0x%x, res: 0x%x\n", dst, res);
/*
To prevent VT call disconnection.
*/
if (skb != NULL)
kfree_skb(skb);
return 1;
}
return pn_send(skb, dev, dst, src, res, 1);
}
/*
* Prepends an ISI header and sends a datagram.
*/
static int pn_send(struct sk_buff *skb, struct net_device *dev,
u16 dst, u16 src, u8 res, u8 irq)
{
struct phonethdr *ph;
int err;
if (skb->len + 2 > 0xffff /* Phonet length field limit */ ||
skb->len + sizeof(struct phonethdr) > dev->mtu) {
err = -EMSGSIZE;
goto drop;
}
/* Broadcast sending is not implemented */
if (pn_addr(dst) == PNADDR_BROADCAST) {
err = -EOPNOTSUPP;
goto drop;
}
skb_reset_transport_header(skb);
WARN_ON(skb_headroom(skb) & 1); /* HW assumes word alignment */
skb_push(skb, sizeof(struct phonethdr));
skb_reset_network_header(skb);
ph = pn_hdr(skb);
ph->pn_rdev = pn_dev(dst);
ph->pn_sdev = pn_dev(src);
ph->pn_res = res;
ph->pn_length = __cpu_to_be16(skb->len + 2 - sizeof(*ph));
ph->pn_robj = pn_obj(dst);
ph->pn_sobj = pn_obj(src);
skb->protocol = htons(ETH_P_PHONET);
skb->priority = 0;
skb->dev = dev;
if (pn_addr(src) == pn_addr(dst)) {
skb_reset_mac_header(skb);
skb->pkt_type = PACKET_LOOPBACK;
skb_orphan(skb);
if (irq)
netif_rx(skb);
else
netif_rx_ni(skb);
err = 0;
} else {
err = dev_hard_header(skb, dev, ntohs(skb->protocol),
NULL, NULL, skb->len);
if (err < 0) {
err = -EHOSTUNREACH;
goto drop;
}
err = dev_queue_xmit(skb);
}
return err;
drop:
kfree_skb(skb);
return err;
}
/*
* Prepends an ISI header and sends a datagram.
*/
static int pn_send(struct sk_buff *skb, struct net_device *dev,
u16 dst, u16 src, u8 res, u8 irq)
{
struct phonethdr *ph;
int err, i;
if (skb->len + 2 > 0xffff /* Phonet length field limit */ ||
skb->len + sizeof(struct phonethdr) > dev->mtu) {
err = -EMSGSIZE;
goto drop;
}
/* Broadcast sending is not implemented */
if (pn_addr(dst) == PNADDR_BROADCAST) {
err = -EOPNOTSUPP;
goto drop;
}
skb_reset_transport_header(skb);
WARN_ON(skb_headroom(skb) & 1); /* HW assumes word alignment */
skb_push(skb, sizeof(struct phonethdr));
skb_reset_network_header(skb);
ph = pn_hdr(skb);
ph->pn_rdev = pn_dev(dst);
ph->pn_sdev = pn_dev(src);
ph->pn_res = res;
ph->pn_length = __cpu_to_be16(skb->len + 2 - sizeof(*ph));
ph->pn_robj = pn_obj(dst);
ph->pn_sobj = pn_obj(src);
skb->protocol = htons(ETH_P_PHONET);
skb->priority = 0;
skb->dev = dev;
PN_PRINTK("pn_send rdev %x sdev %x res %x robj %x sobj %x netdev=%s\n",
ph->pn_rdev, ph->pn_sdev, ph->pn_res,
ph->pn_robj, ph->pn_sobj, dev->name);
PN_DATA_PRINTK("PHONET : skb data = %d\nPHONET :", skb->len);
for (i = 1; i <= skb->len; i++) {
PN_DATA_PRINTK(" %02x", skb->data[i-1]);
if ((i%8) == 0)
PN_DATA_PRINTK("\n");
}
if (skb->pkt_type == PACKET_LOOPBACK) {
skb_reset_mac_header(skb);
skb_orphan(skb);
err = (irq ? netif_rx(skb) : netif_rx_ni(skb)) ? -ENOBUFS : 0;
} else {
err = dev_hard_header(skb, dev, ntohs(skb->protocol),
NULL, NULL, skb->len);
if (err < 0) {
err = -EHOSTUNREACH;
goto drop;
}
err = dev_queue_xmit(skb);
if (unlikely(err > 0))
err = net_xmit_errno(err);
}
return err;
drop:
printk(KERN_DEBUG "pn_send DROP\n");
kfree_skb(skb);
return err;
}
/*
* Stuff received packets to associated sockets.
* On error, returns non-zero and releases the skb.
*/
static int phonet_rcv(struct sk_buff *skb, struct net_device *dev,
struct packet_type *pkttype,
struct net_device *orig_dev)
{
struct net *net = dev_net(dev);
struct phonethdr *ph;
struct sockaddr_pn sa;
u16 len;
/* check we have at least a full Phonet header */
if (!pskb_pull(skb, sizeof(struct phonethdr)))
goto out;
/* check that the advertised length is correct */
ph = pn_hdr(skb);
len = get_unaligned_be16(&ph->pn_length);
if (len < 2)
goto out;
len -= 2;
if ((len > skb->len) || pskb_trim(skb, len))
goto out;
skb_reset_transport_header(skb);
pn_skb_get_dst_sockaddr(skb, &sa);
/* check if this is broadcasted */
if (pn_sockaddr_get_addr(&sa) == PNADDR_BROADCAST) {
pn_deliver_sock_broadcast(net, skb);
goto out;
}
/* resource routing */
if (pn_sockaddr_get_object(&sa) == 0) {
struct sock *sk = pn_find_sock_by_res(net, sa.spn_resource);
if (sk)
return sk_receive_skb(sk, skb, 0);
}
/* check if we are the destination */
if (phonet_address_lookup(net, pn_sockaddr_get_addr(&sa)) == 0) {
/* Phonet packet input */
struct sock *sk = pn_find_sock_by_sa(net, &sa);
u16 valueCheck = 0;
if (sk)
return sk_receive_skb(sk, skb, 0);
if (can_respond(skb)) {
/*
Gavini issue WR code: [P120507-3743]
For preventing the garbage data go into SVNET.
Janice issue WR code: [P120208-3952]
For preventing the situation that some
ISI message go into FMT channel.
*/
valueCheck = pn_object(ph->pn_sdev, 0x00);
if ((pn_dev(valueCheck) != 0x60) &&
(pn_dev(valueCheck) != 0x64))
pr_info("[Phonet] Unreachable dst: 0x%x\n",
pn_dev(valueCheck));
else {
send_obj_unreachable(skb);
send_reset_indications(skb);
}
}
} else if (unlikely(skb->pkt_type == PACKET_LOOPBACK))
goto out; /* Race between address deletion and loopback */
else {
/* Phonet packet routing */
struct net_device *out_dev;
out_dev = phonet_route_output(net, pn_sockaddr_get_addr(&sa));
if (!out_dev) {
LIMIT_NETDEBUG(KERN_WARNING"No Phonet route to %02X\n",
pn_sockaddr_get_addr(&sa));
goto out;
}
__skb_push(skb, sizeof(struct phonethdr));
skb->dev = out_dev;
if (out_dev == dev) {
LIMIT_NETDEBUG(KERN_ERR"Phonet loop to %02X on %s\n",
pn_sockaddr_get_addr(&sa), dev->name);
goto out_dev;
}
/* Some drivers (e.g. TUN) do not allocate HW header space */
if (skb_cow_head(skb, out_dev->hard_header_len))
goto out_dev;
if (dev_hard_header(skb, out_dev, ETH_P_PHONET, NULL, NULL,
skb->len) < 0)
goto out_dev;
dev_queue_xmit(skb);
dev_put(out_dev);
return NET_RX_SUCCESS;
out_dev:
dev_put(out_dev);
}
out:
kfree_skb(skb);
return NET_RX_DROP;
}
