/*
* Create a Phonet header for the skb and send it out. Returns
* non-zero error code if failed. The skb is freed then.
*/
int pn_skb_send(struct sock *sk, struct sk_buff *skb,
const struct sockaddr_pn *target)
{
struct net *net = sock_net(sk);
struct net_device *dev;
struct pn_sock *pn = pn_sk(sk);
int err;
u16 src, dst;
u8 daddr, saddr, res;
src = pn->sobject;
if (target != NULL) {
dst = pn_sockaddr_get_object(target);
res = pn_sockaddr_get_resource(target);
} else {
dst = pn->dobject;
res = pn->resource;
}
daddr = pn_addr(dst);
err = -EHOSTUNREACH;
if (sk->sk_bound_dev_if)
dev = dev_get_by_index(net, sk->sk_bound_dev_if);
else if (phonet_address_lookup(net, daddr) == 0) {
dev = phonet_device_get(net);
skb->pkt_type = PACKET_LOOPBACK;
} else if (dst == 0) {
/* Resource routing (small race until phonet_rcv()) */
struct sock *sk = pn_find_sock_by_res(net, res);
if (sk) {
sock_put(sk);
dev = phonet_device_get(net);
skb->pkt_type = PACKET_LOOPBACK;
} else
dev = phonet_route_output(net, daddr);
} else
dev = phonet_route_output(net, daddr);
if (!dev || !(dev->flags & IFF_UP))
goto drop;
saddr = phonet_address_get(dev, daddr);
if (saddr == PN_NO_ADDR)
goto drop;
if (!pn_addr(src))
src = pn_object(saddr, pn_obj(src));
err = pn_send(skb, dev, dst, src, res, 0);
dev_put(dev);
return err;
drop:
printk(KERN_DEBUG "pn_skb_send DROP\n");
kfree_skb(skb);
if (dev)
dev_put(dev);
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;
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;
}
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);
}
/*
* Find address based on socket address, match only certain fields.
* Also grab sock if it was found. Remember to sock_put it later.
*/
struct sock *pn_find_sock_by_sa(struct net *net, const struct sockaddr_pn *spn)
{
struct hlist_node *node;
struct sock *sknode;
struct sock *rval = NULL;
u16 obj = pn_sockaddr_get_object(spn);
u8 res = spn->spn_resource;
struct hlist_head *hlist = pn_hash_list(obj);
spin_lock_bh(&pnsocks.lock);
sk_for_each(sknode, node, hlist) {
struct pn_sock *pn = pn_sk(sknode);
BUG_ON(!pn->sobject); /* unbound socket */
if (!net_eq(sock_net(sknode), net))
continue;
if (pn_port(obj)) {
/* Look up socket by port */
if (pn_port(pn->sobject) != pn_port(obj))
continue;
} else {
/* If port is zero, look up by resource */
if (pn->resource != res)
continue;
}
if (pn_addr(pn->sobject) &&
pn_addr(pn->sobject) != pn_addr(obj))
continue;
rval = sknode;
sock_hold(sknode);
break;
}
spin_unlock_bh(&pnsocks.lock);
return rval;
}
/* allocate port for a socket */
int pn_sock_get_port(struct sock *sk, unsigned short sport)
{
static int port_cur;
struct net *net = sock_net(sk);
struct pn_sock *pn = pn_sk(sk);
struct sockaddr_pn try_sa;
struct sock *tmpsk;
memset(&try_sa, 0, sizeof(struct sockaddr_pn));
try_sa.spn_family = AF_PHONET;
WARN_ON(!mutex_is_locked(&port_mutex));
if (!sport) {
/* search free port */
int port, pmin, pmax;
phonet_get_local_port_range(&pmin, &pmax);
for (port = pmin; port <= pmax; port++) {
#if defined(CONFIG_SAMSUNG_PHONE_SVNET)
port_cur += PN_HASHSIZE;
#else
port_cur++;
#endif
if (port_cur < pmin || port_cur > pmax)
port_cur = pmin;
pn_sockaddr_set_port(&try_sa, port_cur);
tmpsk = pn_find_sock_by_sa(net, &try_sa);
if (tmpsk == NULL) {
sport = port_cur;
goto found;
} else
sock_put(tmpsk);
}
} else {
/* try to find specific port */
pn_sockaddr_set_port(&try_sa, sport);
tmpsk = pn_find_sock_by_sa(net, &try_sa);
if (tmpsk == NULL)
/* No sock there! We can use that port... */
goto found;
else
sock_put(tmpsk);
}
/* the port must be in use already */
return -EADDRINUSE;
found:
pn->sobject = pn_object(pn_addr(pn->sobject), sport);
return 0;
}
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);
return pn_send(skb, dev, dst, src, res, 1);
}
static int pn_socket_bind(struct socket *sock, struct sockaddr *addr, int len)
{
struct sock *sk = sock->sk;
struct pn_sock *pn = pn_sk(sk);
struct sockaddr_pn *spn = (struct sockaddr_pn *)addr;
int err;
u16 handle;
u8 saddr;
if (sk->sk_prot->bind)
return sk->sk_prot->bind(sk, addr, len);
if (len < sizeof(struct sockaddr_pn))
return -EINVAL;
if (spn->spn_family != AF_PHONET)
return -EAFNOSUPPORT;
handle = pn_sockaddr_get_object((struct sockaddr_pn *)addr);
saddr = pn_addr(handle);
if (saddr && phonet_address_lookup(sock_net(sk), saddr))
return -EADDRNOTAVAIL;
lock_sock(sk);
if (sk->sk_state != TCP_CLOSE || pn_port(pn->sobject)) {
err = -EINVAL; /* attempt to rebind */
goto out;
}
WARN_ON(sk_hashed(sk));
mutex_lock(&port_mutex);
err = sk->sk_prot->get_port(sk, pn_port(handle));
if (err)
goto out_port;
/* get_port() sets the port, bind() sets the address if applicable */
pn->sobject = pn_object(saddr, pn_port(pn->sobject));
pn->resource = spn->spn_resource;
/* Enable RX on the socket */
sk->sk_prot->hash(sk);
out_port:
mutex_unlock(&port_mutex);
out:
release_sock(sk);
return err;
}
/*
* Create a Phonet header for the skb and send it out. Returns
* non-zero error code if failed. The skb is freed then.
*/
int pn_skb_send(struct sock *sk, struct sk_buff *skb,
const struct sockaddr_pn *target)
{
struct net *net = sock_net(sk);
struct net_device *dev;
struct pn_sock *pn = pn_sk(sk);
int err;
u16 src;
u8 daddr = pn_sockaddr_get_addr(target), saddr = PN_NO_ADDR;
err = -EHOSTUNREACH;
if (sk->sk_bound_dev_if)
dev = dev_get_by_index(net, sk->sk_bound_dev_if);
else if (phonet_address_lookup(net, daddr) == 0) {
dev = phonet_device_get(net);
skb->pkt_type = PACKET_LOOPBACK;
} else
dev = phonet_route_output(net, daddr);
if (!dev || !(dev->flags & IFF_UP))
goto drop;
saddr = phonet_address_get(dev, daddr);
if (saddr == PN_NO_ADDR)
goto drop;
src = pn->sobject;
if (!pn_addr(src))
src = pn_object(saddr, pn_obj(src));
err = pn_send(skb, dev, pn_sockaddr_get_object(target),
src, pn_sockaddr_get_resource(target), 0);
dev_put(dev);
return err;
drop:
kfree_skb(skb);
if (dev)
dev_put(dev);
return err;
}
static int pn_socket_ioctl(struct socket *sock, unsigned int cmd,
unsigned long arg)
{
struct sock *sk = sock->sk;
struct pn_sock *pn = pn_sk(sk);
if (cmd == SIOCPNGETOBJECT) {
struct net_device *dev;
u16 handle;
u8 saddr;
if (get_user(handle, (__u16 __user *)arg))
return -EFAULT;
lock_sock(sk);
if (sk->sk_bound_dev_if)
dev = dev_get_by_index(sock_net(sk),
sk->sk_bound_dev_if);
else
dev = phonet_device_get(sock_net(sk));
if (dev && (dev->flags & IFF_UP))
saddr = phonet_address_get(dev, pn_addr(handle));
else
saddr = PN_NO_ADDR;
release_sock(sk);
if (dev)
dev_put(dev);
if (saddr == PN_NO_ADDR)
return -EHOSTUNREACH;
handle = pn_object(saddr, pn_port(pn->sobject));
return put_user(handle, (__u16 __user *)arg);
}
return sk->sk_prot->ioctl(sk, cmd, arg);
}
/*
* 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;
}
