1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28#include <linux/module.h>
29
30#include <linux/socket.h>
31#include <linux/netdevice.h>
32#include <linux/etherdevice.h>
33#include <linux/skbuff.h>
34#include <linux/wait.h>
35
36#include <asm/unaligned.h>
37
38#include <net/bluetooth/bluetooth.h>
39#include <net/bluetooth/hci_core.h>
40#include <net/bluetooth/l2cap.h>
41
42#include "bnep.h"
43
44#define BNEP_TX_QUEUE_LEN 20
45
46static int bnep_net_open(struct net_device *dev)
47{
48 netif_start_queue(dev);
49 return 0;
50}
51
52static int bnep_net_close(struct net_device *dev)
53{
54 netif_stop_queue(dev);
55 return 0;
56}
57
58static void bnep_net_set_mc_list(struct net_device *dev)
59{
60#ifdef CONFIG_BT_BNEP_MC_FILTER
61 struct bnep_session *s = netdev_priv(dev);
62 struct sock *sk = s->sock->sk;
63 struct bnep_set_filter_req *r;
64 struct sk_buff *skb;
65 int size;
66
67 BT_DBG("%s mc_count %d", dev->name, dev->mc_count);
68
69 size = sizeof(*r) + (BNEP_MAX_MULTICAST_FILTERS + 1) * ETH_ALEN * 2;
70 skb = alloc_skb(size, GFP_ATOMIC);
71 if (!skb) {
72 BT_ERR("%s Multicast list allocation failed", dev->name);
73 return;
74 }
75
76 r = (void *) skb->data;
77 __skb_put(skb, sizeof(*r));
78
79 r->type = BNEP_CONTROL;
80 r->ctrl = BNEP_FILTER_MULTI_ADDR_SET;
81
82 if (dev->flags & (IFF_PROMISC | IFF_ALLMULTI)) {
83 u8 start[ETH_ALEN] = { 0x01 };
84
85
86 memcpy(__skb_put(skb, ETH_ALEN), start, ETH_ALEN);
87 memcpy(__skb_put(skb, ETH_ALEN), dev->broadcast, ETH_ALEN);
88 r->len = htons(ETH_ALEN * 2);
89 } else {
90 struct dev_mc_list *dmi = dev->mc_list;
91 int i, len = skb->len;
92
93 if (dev->flags & IFF_BROADCAST) {
94 memcpy(__skb_put(skb, ETH_ALEN), dev->broadcast, ETH_ALEN);
95 memcpy(__skb_put(skb, ETH_ALEN), dev->broadcast, ETH_ALEN);
96 }
97
98
99
100 for (i = 0; i < dev->mc_count && i < BNEP_MAX_MULTICAST_FILTERS; i++) {
101 memcpy(__skb_put(skb, ETH_ALEN), dmi->dmi_addr, ETH_ALEN);
102 memcpy(__skb_put(skb, ETH_ALEN), dmi->dmi_addr, ETH_ALEN);
103 dmi = dmi->next;
104 }
105 r->len = htons(skb->len - len);
106 }
107
108 skb_queue_tail(&sk->sk_write_queue, skb);
109 wake_up_interruptible(sk->sk_sleep);
110#endif
111}
112
113static int bnep_net_set_mac_addr(struct net_device *dev, void *arg)
114{
115 BT_DBG("%s", dev->name);
116 return 0;
117}
118
119static void bnep_net_timeout(struct net_device *dev)
120{
121 BT_DBG("net_timeout");
122 netif_wake_queue(dev);
123}
124
125#ifdef CONFIG_BT_BNEP_MC_FILTER
126static inline int bnep_net_mc_filter(struct sk_buff *skb, struct bnep_session *s)
127{
128 struct ethhdr *eh = (void *) skb->data;
129
130 if ((eh->h_dest[0] & 1) && !test_bit(bnep_mc_hash(eh->h_dest), (ulong *) &s->mc_filter))
131 return 1;
132 return 0;
133}
134#endif
135
136#ifdef CONFIG_BT_BNEP_PROTO_FILTER
137
138static inline u16 bnep_net_eth_proto(struct sk_buff *skb)
139{
140 struct ethhdr *eh = (void *) skb->data;
141 u16 proto = ntohs(eh->h_proto);
142
143 if (proto >= 1536)
144 return proto;
145
146 if (get_unaligned((__be16 *) skb->data) == htons(0xFFFF))
147 return ETH_P_802_3;
148
149 return ETH_P_802_2;
150}
151
152static inline int bnep_net_proto_filter(struct sk_buff *skb, struct bnep_session *s)
153{
154 u16 proto = bnep_net_eth_proto(skb);
155 struct bnep_proto_filter *f = s->proto_filter;
156 int i;
157
158 for (i = 0; i < BNEP_MAX_PROTO_FILTERS && f[i].end; i++) {
159 if (proto >= f[i].start && proto <= f[i].end)
160 return 0;
161 }
162
163 BT_DBG("BNEP: filtered skb %p, proto 0x%.4x", skb, proto);
164 return 1;
165}
166#endif
167
168static netdev_tx_t bnep_net_xmit(struct sk_buff *skb,
169 struct net_device *dev)
170{
171 struct bnep_session *s = netdev_priv(dev);
172 struct sock *sk = s->sock->sk;
173
174 BT_DBG("skb %p, dev %p", skb, dev);
175
176#ifdef CONFIG_BT_BNEP_MC_FILTER
177 if (bnep_net_mc_filter(skb, s)) {
178 kfree_skb(skb);
179 return NETDEV_TX_OK;
180 }
181#endif
182
183#ifdef CONFIG_BT_BNEP_PROTO_FILTER
184 if (bnep_net_proto_filter(skb, s)) {
185 kfree_skb(skb);
186 return NETDEV_TX_OK;
187 }
188#endif
189
190
191
192
193
194
195 dev->trans_start = jiffies;
196 skb_queue_tail(&sk->sk_write_queue, skb);
197 wake_up_interruptible(sk->sk_sleep);
198
199 if (skb_queue_len(&sk->sk_write_queue) >= BNEP_TX_QUEUE_LEN) {
200 BT_DBG("tx queue is full");
201
202
203
204 netif_stop_queue(dev);
205 }
206
207 return NETDEV_TX_OK;
208}
209
210static const struct net_device_ops bnep_netdev_ops = {
211 .ndo_open = bnep_net_open,
212 .ndo_stop = bnep_net_close,
213 .ndo_start_xmit = bnep_net_xmit,
214 .ndo_validate_addr = eth_validate_addr,
215 .ndo_set_multicast_list = bnep_net_set_mc_list,
216 .ndo_set_mac_address = bnep_net_set_mac_addr,
217 .ndo_tx_timeout = bnep_net_timeout,
218 .ndo_change_mtu = eth_change_mtu,
219
220};
221
222void bnep_net_setup(struct net_device *dev)
223{
224
225 memset(dev->broadcast, 0xff, ETH_ALEN);
226 dev->addr_len = ETH_ALEN;
227
228 ether_setup(dev);
229 dev->netdev_ops = &bnep_netdev_ops;
230
231 dev->watchdog_timeo = HZ * 2;
232}
233
