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