static int bnep_ctrl_set_mcfilter(struct bnep_session *s, u8 *data, int len)
{
int n;
if (len < 2)
return -EILSEQ;
n = get_unaligned_be16(data);
data += 2;
len -= 2;
if (len < n)
return -EILSEQ;
BT_DBG("filter len %d", n);
#ifdef CONFIG_BT_BNEP_MC_FILTER
n /= (ETH_ALEN * 2);
if (n > 0) {
int i;
s->mc_filter = 0;
/* Always send broadcast */
set_bit(bnep_mc_hash(s->dev->broadcast), (ulong *) &s->mc_filter);
/* Add address ranges to the multicast hash */
for (; n > 0; n--) {
u8 a1[6], *a2;
memcpy(a1, data, ETH_ALEN);
data += ETH_ALEN;
a2 = data;
data += ETH_ALEN;
BT_DBG("mc filter %pMR -> %pMR", a1, a2);
/* Iterate from a1 to a2 */
set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
while (memcmp(a1, a2, 6) < 0 && s->mc_filter != ~0LL) {
/* Increment a1 */
i = 5;
while (i >= 0 && ++a1[i--] == 0)
;
set_bit(bnep_mc_hash(a1), (ulong *) &s->mc_filter);
}
}
}
BT_DBG("mc filter hash 0x%llx", s->mc_filter);
bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_SUCCESS);
#else
bnep_send_rsp(s, BNEP_FILTER_MULTI_ADDR_RSP, BNEP_FILTER_UNSUPPORTED_REQ);
#endif
return 0;
}
static inline int bnep_net_mc_filter(struct sk_buff *skb, struct bnep_session *s)
{
struct ethhdr *eh = (void *) skb->data;
if ((eh->h_dest[0] & 1) && !test_bit(bnep_mc_hash(eh->h_dest), (ulong *) &s->mc_filter))
return 1;
return 0;
}
static inline int bnep_net_mc_filter(struct sk_buff *skb, struct bnep_session *s)
{
struct ethhdr *eh = (void *) skb->data;
if ((eh->h_dest[0] & 1) && !test_bit(bnep_mc_hash(eh->h_dest), &s->mc_filter)) {
BT_DBG("BNEP: filtered skb %p, dst %.2x:%.2x:%.2x:%.2x:%.2x:%.2x", skb,
eh->h_dest[0], eh->h_dest[1], eh->h_dest[2],
eh->h_dest[3], eh->h_dest[4], eh->h_dest[5]);
return 1;
}
return 0;
}
