mirror of https://github.com/Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
935 lines
25 KiB
935 lines
25 KiB
// SPDX-License-Identifier: GPL-2.0-only |
|
/* |
|
* IEEE 802.1Q Multiple Registration Protocol (MRP) |
|
* |
|
* Copyright (c) 2012 Massachusetts Institute of Technology |
|
* |
|
* Adapted from code in net/802/garp.c |
|
* Copyright (c) 2008 Patrick McHardy <[email protected]> |
|
*/ |
|
#include <linux/kernel.h> |
|
#include <linux/timer.h> |
|
#include <linux/skbuff.h> |
|
#include <linux/netdevice.h> |
|
#include <linux/etherdevice.h> |
|
#include <linux/rtnetlink.h> |
|
#include <linux/slab.h> |
|
#include <linux/module.h> |
|
#include <net/mrp.h> |
|
#include <asm/unaligned.h> |
|
|
|
static unsigned int mrp_join_time __read_mostly = 200; |
|
module_param(mrp_join_time, uint, 0644); |
|
MODULE_PARM_DESC(mrp_join_time, "Join time in ms (default 200ms)"); |
|
|
|
static unsigned int mrp_periodic_time __read_mostly = 1000; |
|
module_param(mrp_periodic_time, uint, 0644); |
|
MODULE_PARM_DESC(mrp_periodic_time, "Periodic time in ms (default 1s)"); |
|
|
|
MODULE_LICENSE("GPL"); |
|
|
|
static const u8 |
|
mrp_applicant_state_table[MRP_APPLICANT_MAX + 1][MRP_EVENT_MAX + 1] = { |
|
[MRP_APPLICANT_VO] = { |
|
[MRP_EVENT_NEW] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_JOIN] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_LV] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_TX] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_R_NEW] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AO, |
|
[MRP_EVENT_R_IN] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_R_MT] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_R_LV] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_R_LA] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_PERIODIC] = MRP_APPLICANT_VO, |
|
}, |
|
[MRP_APPLICANT_VP] = { |
|
[MRP_EVENT_NEW] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_JOIN] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_LV] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_TX] = MRP_APPLICANT_AA, |
|
[MRP_EVENT_R_NEW] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AP, |
|
[MRP_EVENT_R_IN] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_R_MT] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_R_LV] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_R_LA] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_PERIODIC] = MRP_APPLICANT_VP, |
|
}, |
|
[MRP_APPLICANT_VN] = { |
|
[MRP_EVENT_NEW] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_JOIN] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_LV] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_TX] = MRP_APPLICANT_AN, |
|
[MRP_EVENT_R_NEW] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_R_IN] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_R_MT] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_R_LV] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_R_LA] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_REDECLARE] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_PERIODIC] = MRP_APPLICANT_VN, |
|
}, |
|
[MRP_APPLICANT_AN] = { |
|
[MRP_EVENT_NEW] = MRP_APPLICANT_AN, |
|
[MRP_EVENT_JOIN] = MRP_APPLICANT_AN, |
|
[MRP_EVENT_LV] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_TX] = MRP_APPLICANT_QA, |
|
[MRP_EVENT_R_NEW] = MRP_APPLICANT_AN, |
|
[MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_AN, |
|
[MRP_EVENT_R_IN] = MRP_APPLICANT_AN, |
|
[MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AN, |
|
[MRP_EVENT_R_MT] = MRP_APPLICANT_AN, |
|
[MRP_EVENT_R_LV] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_R_LA] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_REDECLARE] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_PERIODIC] = MRP_APPLICANT_AN, |
|
}, |
|
[MRP_APPLICANT_AA] = { |
|
[MRP_EVENT_NEW] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_JOIN] = MRP_APPLICANT_AA, |
|
[MRP_EVENT_LV] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_TX] = MRP_APPLICANT_QA, |
|
[MRP_EVENT_R_NEW] = MRP_APPLICANT_AA, |
|
[MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QA, |
|
[MRP_EVENT_R_IN] = MRP_APPLICANT_AA, |
|
[MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AA, |
|
[MRP_EVENT_R_MT] = MRP_APPLICANT_AA, |
|
[MRP_EVENT_R_LV] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_R_LA] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_PERIODIC] = MRP_APPLICANT_AA, |
|
}, |
|
[MRP_APPLICANT_QA] = { |
|
[MRP_EVENT_NEW] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_JOIN] = MRP_APPLICANT_QA, |
|
[MRP_EVENT_LV] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_TX] = MRP_APPLICANT_QA, |
|
[MRP_EVENT_R_NEW] = MRP_APPLICANT_QA, |
|
[MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QA, |
|
[MRP_EVENT_R_IN] = MRP_APPLICANT_QA, |
|
[MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AA, |
|
[MRP_EVENT_R_MT] = MRP_APPLICANT_AA, |
|
[MRP_EVENT_R_LV] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_R_LA] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_PERIODIC] = MRP_APPLICANT_AA, |
|
}, |
|
[MRP_APPLICANT_LA] = { |
|
[MRP_EVENT_NEW] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_JOIN] = MRP_APPLICANT_AA, |
|
[MRP_EVENT_LV] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_TX] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_R_NEW] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_R_IN] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_R_MT] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_R_LV] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_R_LA] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_REDECLARE] = MRP_APPLICANT_LA, |
|
[MRP_EVENT_PERIODIC] = MRP_APPLICANT_LA, |
|
}, |
|
[MRP_APPLICANT_AO] = { |
|
[MRP_EVENT_NEW] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_JOIN] = MRP_APPLICANT_AP, |
|
[MRP_EVENT_LV] = MRP_APPLICANT_AO, |
|
[MRP_EVENT_TX] = MRP_APPLICANT_AO, |
|
[MRP_EVENT_R_NEW] = MRP_APPLICANT_AO, |
|
[MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QO, |
|
[MRP_EVENT_R_IN] = MRP_APPLICANT_AO, |
|
[MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AO, |
|
[MRP_EVENT_R_MT] = MRP_APPLICANT_AO, |
|
[MRP_EVENT_R_LV] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_R_LA] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_PERIODIC] = MRP_APPLICANT_AO, |
|
}, |
|
[MRP_APPLICANT_QO] = { |
|
[MRP_EVENT_NEW] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_JOIN] = MRP_APPLICANT_QP, |
|
[MRP_EVENT_LV] = MRP_APPLICANT_QO, |
|
[MRP_EVENT_TX] = MRP_APPLICANT_QO, |
|
[MRP_EVENT_R_NEW] = MRP_APPLICANT_QO, |
|
[MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QO, |
|
[MRP_EVENT_R_IN] = MRP_APPLICANT_QO, |
|
[MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AO, |
|
[MRP_EVENT_R_MT] = MRP_APPLICANT_AO, |
|
[MRP_EVENT_R_LV] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_R_LA] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_REDECLARE] = MRP_APPLICANT_VO, |
|
[MRP_EVENT_PERIODIC] = MRP_APPLICANT_QO, |
|
}, |
|
[MRP_APPLICANT_AP] = { |
|
[MRP_EVENT_NEW] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_JOIN] = MRP_APPLICANT_AP, |
|
[MRP_EVENT_LV] = MRP_APPLICANT_AO, |
|
[MRP_EVENT_TX] = MRP_APPLICANT_QA, |
|
[MRP_EVENT_R_NEW] = MRP_APPLICANT_AP, |
|
[MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QP, |
|
[MRP_EVENT_R_IN] = MRP_APPLICANT_AP, |
|
[MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AP, |
|
[MRP_EVENT_R_MT] = MRP_APPLICANT_AP, |
|
[MRP_EVENT_R_LV] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_R_LA] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_PERIODIC] = MRP_APPLICANT_AP, |
|
}, |
|
[MRP_APPLICANT_QP] = { |
|
[MRP_EVENT_NEW] = MRP_APPLICANT_VN, |
|
[MRP_EVENT_JOIN] = MRP_APPLICANT_QP, |
|
[MRP_EVENT_LV] = MRP_APPLICANT_QO, |
|
[MRP_EVENT_TX] = MRP_APPLICANT_QP, |
|
[MRP_EVENT_R_NEW] = MRP_APPLICANT_QP, |
|
[MRP_EVENT_R_JOIN_IN] = MRP_APPLICANT_QP, |
|
[MRP_EVENT_R_IN] = MRP_APPLICANT_QP, |
|
[MRP_EVENT_R_JOIN_MT] = MRP_APPLICANT_AP, |
|
[MRP_EVENT_R_MT] = MRP_APPLICANT_AP, |
|
[MRP_EVENT_R_LV] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_R_LA] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_REDECLARE] = MRP_APPLICANT_VP, |
|
[MRP_EVENT_PERIODIC] = MRP_APPLICANT_AP, |
|
}, |
|
}; |
|
|
|
static const u8 |
|
mrp_tx_action_table[MRP_APPLICANT_MAX + 1] = { |
|
[MRP_APPLICANT_VO] = MRP_TX_ACTION_S_IN_OPTIONAL, |
|
[MRP_APPLICANT_VP] = MRP_TX_ACTION_S_JOIN_IN, |
|
[MRP_APPLICANT_VN] = MRP_TX_ACTION_S_NEW, |
|
[MRP_APPLICANT_AN] = MRP_TX_ACTION_S_NEW, |
|
[MRP_APPLICANT_AA] = MRP_TX_ACTION_S_JOIN_IN, |
|
[MRP_APPLICANT_QA] = MRP_TX_ACTION_S_JOIN_IN_OPTIONAL, |
|
[MRP_APPLICANT_LA] = MRP_TX_ACTION_S_LV, |
|
[MRP_APPLICANT_AO] = MRP_TX_ACTION_S_IN_OPTIONAL, |
|
[MRP_APPLICANT_QO] = MRP_TX_ACTION_S_IN_OPTIONAL, |
|
[MRP_APPLICANT_AP] = MRP_TX_ACTION_S_JOIN_IN, |
|
[MRP_APPLICANT_QP] = MRP_TX_ACTION_S_IN_OPTIONAL, |
|
}; |
|
|
|
static void mrp_attrvalue_inc(void *value, u8 len) |
|
{ |
|
u8 *v = (u8 *)value; |
|
|
|
/* Add 1 to the last byte. If it becomes zero, |
|
* go to the previous byte and repeat. |
|
*/ |
|
while (len > 0 && !++v[--len]) |
|
; |
|
} |
|
|
|
static int mrp_attr_cmp(const struct mrp_attr *attr, |
|
const void *value, u8 len, u8 type) |
|
{ |
|
if (attr->type != type) |
|
return attr->type - type; |
|
if (attr->len != len) |
|
return attr->len - len; |
|
return memcmp(attr->value, value, len); |
|
} |
|
|
|
static struct mrp_attr *mrp_attr_lookup(const struct mrp_applicant *app, |
|
const void *value, u8 len, u8 type) |
|
{ |
|
struct rb_node *parent = app->mad.rb_node; |
|
struct mrp_attr *attr; |
|
int d; |
|
|
|
while (parent) { |
|
attr = rb_entry(parent, struct mrp_attr, node); |
|
d = mrp_attr_cmp(attr, value, len, type); |
|
if (d > 0) |
|
parent = parent->rb_left; |
|
else if (d < 0) |
|
parent = parent->rb_right; |
|
else |
|
return attr; |
|
} |
|
return NULL; |
|
} |
|
|
|
static struct mrp_attr *mrp_attr_create(struct mrp_applicant *app, |
|
const void *value, u8 len, u8 type) |
|
{ |
|
struct rb_node *parent = NULL, **p = &app->mad.rb_node; |
|
struct mrp_attr *attr; |
|
int d; |
|
|
|
while (*p) { |
|
parent = *p; |
|
attr = rb_entry(parent, struct mrp_attr, node); |
|
d = mrp_attr_cmp(attr, value, len, type); |
|
if (d > 0) |
|
p = &parent->rb_left; |
|
else if (d < 0) |
|
p = &parent->rb_right; |
|
else { |
|
/* The attribute already exists; re-use it. */ |
|
return attr; |
|
} |
|
} |
|
attr = kmalloc(sizeof(*attr) + len, GFP_ATOMIC); |
|
if (!attr) |
|
return attr; |
|
attr->state = MRP_APPLICANT_VO; |
|
attr->type = type; |
|
attr->len = len; |
|
memcpy(attr->value, value, len); |
|
|
|
rb_link_node(&attr->node, parent, p); |
|
rb_insert_color(&attr->node, &app->mad); |
|
return attr; |
|
} |
|
|
|
static void mrp_attr_destroy(struct mrp_applicant *app, struct mrp_attr *attr) |
|
{ |
|
rb_erase(&attr->node, &app->mad); |
|
kfree(attr); |
|
} |
|
|
|
static void mrp_attr_destroy_all(struct mrp_applicant *app) |
|
{ |
|
struct rb_node *node, *next; |
|
struct mrp_attr *attr; |
|
|
|
for (node = rb_first(&app->mad); |
|
next = node ? rb_next(node) : NULL, node != NULL; |
|
node = next) { |
|
attr = rb_entry(node, struct mrp_attr, node); |
|
mrp_attr_destroy(app, attr); |
|
} |
|
} |
|
|
|
static int mrp_pdu_init(struct mrp_applicant *app) |
|
{ |
|
struct sk_buff *skb; |
|
struct mrp_pdu_hdr *ph; |
|
|
|
skb = alloc_skb(app->dev->mtu + LL_RESERVED_SPACE(app->dev), |
|
GFP_ATOMIC); |
|
if (!skb) |
|
return -ENOMEM; |
|
|
|
skb->dev = app->dev; |
|
skb->protocol = app->app->pkttype.type; |
|
skb_reserve(skb, LL_RESERVED_SPACE(app->dev)); |
|
skb_reset_network_header(skb); |
|
skb_reset_transport_header(skb); |
|
|
|
ph = __skb_put(skb, sizeof(*ph)); |
|
ph->version = app->app->version; |
|
|
|
app->pdu = skb; |
|
return 0; |
|
} |
|
|
|
static int mrp_pdu_append_end_mark(struct mrp_applicant *app) |
|
{ |
|
__be16 *endmark; |
|
|
|
if (skb_tailroom(app->pdu) < sizeof(*endmark)) |
|
return -1; |
|
endmark = __skb_put(app->pdu, sizeof(*endmark)); |
|
put_unaligned(MRP_END_MARK, endmark); |
|
return 0; |
|
} |
|
|
|
static void mrp_pdu_queue(struct mrp_applicant *app) |
|
{ |
|
if (!app->pdu) |
|
return; |
|
|
|
if (mrp_cb(app->pdu)->mh) |
|
mrp_pdu_append_end_mark(app); |
|
mrp_pdu_append_end_mark(app); |
|
|
|
dev_hard_header(app->pdu, app->dev, ntohs(app->app->pkttype.type), |
|
app->app->group_address, app->dev->dev_addr, |
|
app->pdu->len); |
|
|
|
skb_queue_tail(&app->queue, app->pdu); |
|
app->pdu = NULL; |
|
} |
|
|
|
static void mrp_queue_xmit(struct mrp_applicant *app) |
|
{ |
|
struct sk_buff *skb; |
|
|
|
while ((skb = skb_dequeue(&app->queue))) |
|
dev_queue_xmit(skb); |
|
} |
|
|
|
static int mrp_pdu_append_msg_hdr(struct mrp_applicant *app, |
|
u8 attrtype, u8 attrlen) |
|
{ |
|
struct mrp_msg_hdr *mh; |
|
|
|
if (mrp_cb(app->pdu)->mh) { |
|
if (mrp_pdu_append_end_mark(app) < 0) |
|
return -1; |
|
mrp_cb(app->pdu)->mh = NULL; |
|
mrp_cb(app->pdu)->vah = NULL; |
|
} |
|
|
|
if (skb_tailroom(app->pdu) < sizeof(*mh)) |
|
return -1; |
|
mh = __skb_put(app->pdu, sizeof(*mh)); |
|
mh->attrtype = attrtype; |
|
mh->attrlen = attrlen; |
|
mrp_cb(app->pdu)->mh = mh; |
|
return 0; |
|
} |
|
|
|
static int mrp_pdu_append_vecattr_hdr(struct mrp_applicant *app, |
|
const void *firstattrvalue, u8 attrlen) |
|
{ |
|
struct mrp_vecattr_hdr *vah; |
|
|
|
if (skb_tailroom(app->pdu) < sizeof(*vah) + attrlen) |
|
return -1; |
|
vah = __skb_put(app->pdu, sizeof(*vah) + attrlen); |
|
put_unaligned(0, &vah->lenflags); |
|
memcpy(vah->firstattrvalue, firstattrvalue, attrlen); |
|
mrp_cb(app->pdu)->vah = vah; |
|
memcpy(mrp_cb(app->pdu)->attrvalue, firstattrvalue, attrlen); |
|
return 0; |
|
} |
|
|
|
static int mrp_pdu_append_vecattr_event(struct mrp_applicant *app, |
|
const struct mrp_attr *attr, |
|
enum mrp_vecattr_event vaevent) |
|
{ |
|
u16 len, pos; |
|
u8 *vaevents; |
|
int err; |
|
again: |
|
if (!app->pdu) { |
|
err = mrp_pdu_init(app); |
|
if (err < 0) |
|
return err; |
|
} |
|
|
|
/* If there is no Message header in the PDU, or the Message header is |
|
* for a different attribute type, add an EndMark (if necessary) and a |
|
* new Message header to the PDU. |
|
*/ |
|
if (!mrp_cb(app->pdu)->mh || |
|
mrp_cb(app->pdu)->mh->attrtype != attr->type || |
|
mrp_cb(app->pdu)->mh->attrlen != attr->len) { |
|
if (mrp_pdu_append_msg_hdr(app, attr->type, attr->len) < 0) |
|
goto queue; |
|
} |
|
|
|
/* If there is no VectorAttribute header for this Message in the PDU, |
|
* or this attribute's value does not sequentially follow the previous |
|
* attribute's value, add a new VectorAttribute header to the PDU. |
|
*/ |
|
if (!mrp_cb(app->pdu)->vah || |
|
memcmp(mrp_cb(app->pdu)->attrvalue, attr->value, attr->len)) { |
|
if (mrp_pdu_append_vecattr_hdr(app, attr->value, attr->len) < 0) |
|
goto queue; |
|
} |
|
|
|
len = be16_to_cpu(get_unaligned(&mrp_cb(app->pdu)->vah->lenflags)); |
|
pos = len % 3; |
|
|
|
/* Events are packed into Vectors in the PDU, three to a byte. Add a |
|
* byte to the end of the Vector if necessary. |
|
*/ |
|
if (!pos) { |
|
if (skb_tailroom(app->pdu) < sizeof(u8)) |
|
goto queue; |
|
vaevents = __skb_put(app->pdu, sizeof(u8)); |
|
} else { |
|
vaevents = (u8 *)(skb_tail_pointer(app->pdu) - sizeof(u8)); |
|
} |
|
|
|
switch (pos) { |
|
case 0: |
|
*vaevents = vaevent * (__MRP_VECATTR_EVENT_MAX * |
|
__MRP_VECATTR_EVENT_MAX); |
|
break; |
|
case 1: |
|
*vaevents += vaevent * __MRP_VECATTR_EVENT_MAX; |
|
break; |
|
case 2: |
|
*vaevents += vaevent; |
|
break; |
|
default: |
|
WARN_ON(1); |
|
} |
|
|
|
/* Increment the length of the VectorAttribute in the PDU, as well as |
|
* the value of the next attribute that would continue its Vector. |
|
*/ |
|
put_unaligned(cpu_to_be16(++len), &mrp_cb(app->pdu)->vah->lenflags); |
|
mrp_attrvalue_inc(mrp_cb(app->pdu)->attrvalue, attr->len); |
|
|
|
return 0; |
|
|
|
queue: |
|
mrp_pdu_queue(app); |
|
goto again; |
|
} |
|
|
|
static void mrp_attr_event(struct mrp_applicant *app, |
|
struct mrp_attr *attr, enum mrp_event event) |
|
{ |
|
enum mrp_applicant_state state; |
|
|
|
state = mrp_applicant_state_table[attr->state][event]; |
|
if (state == MRP_APPLICANT_INVALID) { |
|
WARN_ON(1); |
|
return; |
|
} |
|
|
|
if (event == MRP_EVENT_TX) { |
|
/* When appending the attribute fails, don't update its state |
|
* in order to retry at the next TX event. |
|
*/ |
|
|
|
switch (mrp_tx_action_table[attr->state]) { |
|
case MRP_TX_ACTION_NONE: |
|
case MRP_TX_ACTION_S_JOIN_IN_OPTIONAL: |
|
case MRP_TX_ACTION_S_IN_OPTIONAL: |
|
break; |
|
case MRP_TX_ACTION_S_NEW: |
|
if (mrp_pdu_append_vecattr_event( |
|
app, attr, MRP_VECATTR_EVENT_NEW) < 0) |
|
return; |
|
break; |
|
case MRP_TX_ACTION_S_JOIN_IN: |
|
if (mrp_pdu_append_vecattr_event( |
|
app, attr, MRP_VECATTR_EVENT_JOIN_IN) < 0) |
|
return; |
|
break; |
|
case MRP_TX_ACTION_S_LV: |
|
if (mrp_pdu_append_vecattr_event( |
|
app, attr, MRP_VECATTR_EVENT_LV) < 0) |
|
return; |
|
/* As a pure applicant, sending a leave message |
|
* implies that the attribute was unregistered and |
|
* can be destroyed. |
|
*/ |
|
mrp_attr_destroy(app, attr); |
|
return; |
|
default: |
|
WARN_ON(1); |
|
} |
|
} |
|
|
|
attr->state = state; |
|
} |
|
|
|
int mrp_request_join(const struct net_device *dev, |
|
const struct mrp_application *appl, |
|
const void *value, u8 len, u8 type) |
|
{ |
|
struct mrp_port *port = rtnl_dereference(dev->mrp_port); |
|
struct mrp_applicant *app = rtnl_dereference( |
|
port->applicants[appl->type]); |
|
struct mrp_attr *attr; |
|
|
|
if (sizeof(struct mrp_skb_cb) + len > |
|
sizeof_field(struct sk_buff, cb)) |
|
return -ENOMEM; |
|
|
|
spin_lock_bh(&app->lock); |
|
attr = mrp_attr_create(app, value, len, type); |
|
if (!attr) { |
|
spin_unlock_bh(&app->lock); |
|
return -ENOMEM; |
|
} |
|
mrp_attr_event(app, attr, MRP_EVENT_JOIN); |
|
spin_unlock_bh(&app->lock); |
|
return 0; |
|
} |
|
EXPORT_SYMBOL_GPL(mrp_request_join); |
|
|
|
void mrp_request_leave(const struct net_device *dev, |
|
const struct mrp_application *appl, |
|
const void *value, u8 len, u8 type) |
|
{ |
|
struct mrp_port *port = rtnl_dereference(dev->mrp_port); |
|
struct mrp_applicant *app = rtnl_dereference( |
|
port->applicants[appl->type]); |
|
struct mrp_attr *attr; |
|
|
|
if (sizeof(struct mrp_skb_cb) + len > |
|
sizeof_field(struct sk_buff, cb)) |
|
return; |
|
|
|
spin_lock_bh(&app->lock); |
|
attr = mrp_attr_lookup(app, value, len, type); |
|
if (!attr) { |
|
spin_unlock_bh(&app->lock); |
|
return; |
|
} |
|
mrp_attr_event(app, attr, MRP_EVENT_LV); |
|
spin_unlock_bh(&app->lock); |
|
} |
|
EXPORT_SYMBOL_GPL(mrp_request_leave); |
|
|
|
static void mrp_mad_event(struct mrp_applicant *app, enum mrp_event event) |
|
{ |
|
struct rb_node *node, *next; |
|
struct mrp_attr *attr; |
|
|
|
for (node = rb_first(&app->mad); |
|
next = node ? rb_next(node) : NULL, node != NULL; |
|
node = next) { |
|
attr = rb_entry(node, struct mrp_attr, node); |
|
mrp_attr_event(app, attr, event); |
|
} |
|
} |
|
|
|
static void mrp_join_timer_arm(struct mrp_applicant *app) |
|
{ |
|
unsigned long delay; |
|
|
|
delay = (u64)msecs_to_jiffies(mrp_join_time) * prandom_u32() >> 32; |
|
mod_timer(&app->join_timer, jiffies + delay); |
|
} |
|
|
|
static void mrp_join_timer(struct timer_list *t) |
|
{ |
|
struct mrp_applicant *app = from_timer(app, t, join_timer); |
|
|
|
spin_lock(&app->lock); |
|
mrp_mad_event(app, MRP_EVENT_TX); |
|
mrp_pdu_queue(app); |
|
spin_unlock(&app->lock); |
|
|
|
mrp_queue_xmit(app); |
|
mrp_join_timer_arm(app); |
|
} |
|
|
|
static void mrp_periodic_timer_arm(struct mrp_applicant *app) |
|
{ |
|
mod_timer(&app->periodic_timer, |
|
jiffies + msecs_to_jiffies(mrp_periodic_time)); |
|
} |
|
|
|
static void mrp_periodic_timer(struct timer_list *t) |
|
{ |
|
struct mrp_applicant *app = from_timer(app, t, periodic_timer); |
|
|
|
spin_lock(&app->lock); |
|
mrp_mad_event(app, MRP_EVENT_PERIODIC); |
|
mrp_pdu_queue(app); |
|
spin_unlock(&app->lock); |
|
|
|
mrp_periodic_timer_arm(app); |
|
} |
|
|
|
static int mrp_pdu_parse_end_mark(struct sk_buff *skb, int *offset) |
|
{ |
|
__be16 endmark; |
|
|
|
if (skb_copy_bits(skb, *offset, &endmark, sizeof(endmark)) < 0) |
|
return -1; |
|
if (endmark == MRP_END_MARK) { |
|
*offset += sizeof(endmark); |
|
return -1; |
|
} |
|
return 0; |
|
} |
|
|
|
static void mrp_pdu_parse_vecattr_event(struct mrp_applicant *app, |
|
struct sk_buff *skb, |
|
enum mrp_vecattr_event vaevent) |
|
{ |
|
struct mrp_attr *attr; |
|
enum mrp_event event; |
|
|
|
attr = mrp_attr_lookup(app, mrp_cb(skb)->attrvalue, |
|
mrp_cb(skb)->mh->attrlen, |
|
mrp_cb(skb)->mh->attrtype); |
|
if (attr == NULL) |
|
return; |
|
|
|
switch (vaevent) { |
|
case MRP_VECATTR_EVENT_NEW: |
|
event = MRP_EVENT_R_NEW; |
|
break; |
|
case MRP_VECATTR_EVENT_JOIN_IN: |
|
event = MRP_EVENT_R_JOIN_IN; |
|
break; |
|
case MRP_VECATTR_EVENT_IN: |
|
event = MRP_EVENT_R_IN; |
|
break; |
|
case MRP_VECATTR_EVENT_JOIN_MT: |
|
event = MRP_EVENT_R_JOIN_MT; |
|
break; |
|
case MRP_VECATTR_EVENT_MT: |
|
event = MRP_EVENT_R_MT; |
|
break; |
|
case MRP_VECATTR_EVENT_LV: |
|
event = MRP_EVENT_R_LV; |
|
break; |
|
default: |
|
return; |
|
} |
|
|
|
mrp_attr_event(app, attr, event); |
|
} |
|
|
|
static int mrp_pdu_parse_vecattr(struct mrp_applicant *app, |
|
struct sk_buff *skb, int *offset) |
|
{ |
|
struct mrp_vecattr_hdr _vah; |
|
u16 valen; |
|
u8 vaevents, vaevent; |
|
|
|
mrp_cb(skb)->vah = skb_header_pointer(skb, *offset, sizeof(_vah), |
|
&_vah); |
|
if (!mrp_cb(skb)->vah) |
|
return -1; |
|
*offset += sizeof(_vah); |
|
|
|
if (get_unaligned(&mrp_cb(skb)->vah->lenflags) & |
|
MRP_VECATTR_HDR_FLAG_LA) |
|
mrp_mad_event(app, MRP_EVENT_R_LA); |
|
valen = be16_to_cpu(get_unaligned(&mrp_cb(skb)->vah->lenflags) & |
|
MRP_VECATTR_HDR_LEN_MASK); |
|
|
|
/* The VectorAttribute structure in a PDU carries event information |
|
* about one or more attributes having consecutive values. Only the |
|
* value for the first attribute is contained in the structure. So |
|
* we make a copy of that value, and then increment it each time we |
|
* advance to the next event in its Vector. |
|
*/ |
|
if (sizeof(struct mrp_skb_cb) + mrp_cb(skb)->mh->attrlen > |
|
sizeof_field(struct sk_buff, cb)) |
|
return -1; |
|
if (skb_copy_bits(skb, *offset, mrp_cb(skb)->attrvalue, |
|
mrp_cb(skb)->mh->attrlen) < 0) |
|
return -1; |
|
*offset += mrp_cb(skb)->mh->attrlen; |
|
|
|
/* In a VectorAttribute, the Vector contains events which are packed |
|
* three to a byte. We process one byte of the Vector at a time. |
|
*/ |
|
while (valen > 0) { |
|
if (skb_copy_bits(skb, *offset, &vaevents, |
|
sizeof(vaevents)) < 0) |
|
return -1; |
|
*offset += sizeof(vaevents); |
|
|
|
/* Extract and process the first event. */ |
|
vaevent = vaevents / (__MRP_VECATTR_EVENT_MAX * |
|
__MRP_VECATTR_EVENT_MAX); |
|
if (vaevent >= __MRP_VECATTR_EVENT_MAX) { |
|
/* The byte is malformed; stop processing. */ |
|
return -1; |
|
} |
|
mrp_pdu_parse_vecattr_event(app, skb, vaevent); |
|
|
|
/* If present, extract and process the second event. */ |
|
if (!--valen) |
|
break; |
|
mrp_attrvalue_inc(mrp_cb(skb)->attrvalue, |
|
mrp_cb(skb)->mh->attrlen); |
|
vaevents %= (__MRP_VECATTR_EVENT_MAX * |
|
__MRP_VECATTR_EVENT_MAX); |
|
vaevent = vaevents / __MRP_VECATTR_EVENT_MAX; |
|
mrp_pdu_parse_vecattr_event(app, skb, vaevent); |
|
|
|
/* If present, extract and process the third event. */ |
|
if (!--valen) |
|
break; |
|
mrp_attrvalue_inc(mrp_cb(skb)->attrvalue, |
|
mrp_cb(skb)->mh->attrlen); |
|
vaevents %= __MRP_VECATTR_EVENT_MAX; |
|
vaevent = vaevents; |
|
mrp_pdu_parse_vecattr_event(app, skb, vaevent); |
|
} |
|
return 0; |
|
} |
|
|
|
static int mrp_pdu_parse_msg(struct mrp_applicant *app, struct sk_buff *skb, |
|
int *offset) |
|
{ |
|
struct mrp_msg_hdr _mh; |
|
|
|
mrp_cb(skb)->mh = skb_header_pointer(skb, *offset, sizeof(_mh), &_mh); |
|
if (!mrp_cb(skb)->mh) |
|
return -1; |
|
*offset += sizeof(_mh); |
|
|
|
if (mrp_cb(skb)->mh->attrtype == 0 || |
|
mrp_cb(skb)->mh->attrtype > app->app->maxattr || |
|
mrp_cb(skb)->mh->attrlen == 0) |
|
return -1; |
|
|
|
while (skb->len > *offset) { |
|
if (mrp_pdu_parse_end_mark(skb, offset) < 0) |
|
break; |
|
if (mrp_pdu_parse_vecattr(app, skb, offset) < 0) |
|
return -1; |
|
} |
|
return 0; |
|
} |
|
|
|
static int mrp_rcv(struct sk_buff *skb, struct net_device *dev, |
|
struct packet_type *pt, struct net_device *orig_dev) |
|
{ |
|
struct mrp_application *appl = container_of(pt, struct mrp_application, |
|
pkttype); |
|
struct mrp_port *port; |
|
struct mrp_applicant *app; |
|
struct mrp_pdu_hdr _ph; |
|
const struct mrp_pdu_hdr *ph; |
|
int offset = skb_network_offset(skb); |
|
|
|
/* If the interface is in promiscuous mode, drop the packet if |
|
* it was unicast to another host. |
|
*/ |
|
if (unlikely(skb->pkt_type == PACKET_OTHERHOST)) |
|
goto out; |
|
skb = skb_share_check(skb, GFP_ATOMIC); |
|
if (unlikely(!skb)) |
|
goto out; |
|
port = rcu_dereference(dev->mrp_port); |
|
if (unlikely(!port)) |
|
goto out; |
|
app = rcu_dereference(port->applicants[appl->type]); |
|
if (unlikely(!app)) |
|
goto out; |
|
|
|
ph = skb_header_pointer(skb, offset, sizeof(_ph), &_ph); |
|
if (!ph) |
|
goto out; |
|
offset += sizeof(_ph); |
|
|
|
if (ph->version != app->app->version) |
|
goto out; |
|
|
|
spin_lock(&app->lock); |
|
while (skb->len > offset) { |
|
if (mrp_pdu_parse_end_mark(skb, &offset) < 0) |
|
break; |
|
if (mrp_pdu_parse_msg(app, skb, &offset) < 0) |
|
break; |
|
} |
|
spin_unlock(&app->lock); |
|
out: |
|
kfree_skb(skb); |
|
return 0; |
|
} |
|
|
|
static int mrp_init_port(struct net_device *dev) |
|
{ |
|
struct mrp_port *port; |
|
|
|
port = kzalloc(sizeof(*port), GFP_KERNEL); |
|
if (!port) |
|
return -ENOMEM; |
|
rcu_assign_pointer(dev->mrp_port, port); |
|
return 0; |
|
} |
|
|
|
static void mrp_release_port(struct net_device *dev) |
|
{ |
|
struct mrp_port *port = rtnl_dereference(dev->mrp_port); |
|
unsigned int i; |
|
|
|
for (i = 0; i <= MRP_APPLICATION_MAX; i++) { |
|
if (rtnl_dereference(port->applicants[i])) |
|
return; |
|
} |
|
RCU_INIT_POINTER(dev->mrp_port, NULL); |
|
kfree_rcu(port, rcu); |
|
} |
|
|
|
int mrp_init_applicant(struct net_device *dev, struct mrp_application *appl) |
|
{ |
|
struct mrp_applicant *app; |
|
int err; |
|
|
|
ASSERT_RTNL(); |
|
|
|
if (!rtnl_dereference(dev->mrp_port)) { |
|
err = mrp_init_port(dev); |
|
if (err < 0) |
|
goto err1; |
|
} |
|
|
|
err = -ENOMEM; |
|
app = kzalloc(sizeof(*app), GFP_KERNEL); |
|
if (!app) |
|
goto err2; |
|
|
|
err = dev_mc_add(dev, appl->group_address); |
|
if (err < 0) |
|
goto err3; |
|
|
|
app->dev = dev; |
|
app->app = appl; |
|
app->mad = RB_ROOT; |
|
spin_lock_init(&app->lock); |
|
skb_queue_head_init(&app->queue); |
|
rcu_assign_pointer(dev->mrp_port->applicants[appl->type], app); |
|
timer_setup(&app->join_timer, mrp_join_timer, 0); |
|
mrp_join_timer_arm(app); |
|
timer_setup(&app->periodic_timer, mrp_periodic_timer, 0); |
|
mrp_periodic_timer_arm(app); |
|
return 0; |
|
|
|
err3: |
|
kfree(app); |
|
err2: |
|
mrp_release_port(dev); |
|
err1: |
|
return err; |
|
} |
|
EXPORT_SYMBOL_GPL(mrp_init_applicant); |
|
|
|
void mrp_uninit_applicant(struct net_device *dev, struct mrp_application *appl) |
|
{ |
|
struct mrp_port *port = rtnl_dereference(dev->mrp_port); |
|
struct mrp_applicant *app = rtnl_dereference( |
|
port->applicants[appl->type]); |
|
|
|
ASSERT_RTNL(); |
|
|
|
RCU_INIT_POINTER(port->applicants[appl->type], NULL); |
|
|
|
/* Delete timer and generate a final TX event to flush out |
|
* all pending messages before the applicant is gone. |
|
*/ |
|
del_timer_sync(&app->join_timer); |
|
del_timer_sync(&app->periodic_timer); |
|
|
|
spin_lock_bh(&app->lock); |
|
mrp_mad_event(app, MRP_EVENT_TX); |
|
mrp_attr_destroy_all(app); |
|
mrp_pdu_queue(app); |
|
spin_unlock_bh(&app->lock); |
|
|
|
mrp_queue_xmit(app); |
|
|
|
dev_mc_del(dev, appl->group_address); |
|
kfree_rcu(app, rcu); |
|
mrp_release_port(dev); |
|
} |
|
EXPORT_SYMBOL_GPL(mrp_uninit_applicant); |
|
|
|
int mrp_register_application(struct mrp_application *appl) |
|
{ |
|
appl->pkttype.func = mrp_rcv; |
|
dev_add_pack(&appl->pkttype); |
|
return 0; |
|
} |
|
EXPORT_SYMBOL_GPL(mrp_register_application); |
|
|
|
void mrp_unregister_application(struct mrp_application *appl) |
|
{ |
|
dev_remove_pack(&appl->pkttype); |
|
} |
|
EXPORT_SYMBOL_GPL(mrp_unregister_application);
|
|
|