net/802/mrp.c

237 static struct mrp_attr *mrp_attr_lookup(const struct mrp_applicant *app,
240 	struct rb_node *parent = app->mad.rb_node;
257 static struct mrp_attr *mrp_attr_create(struct mrp_applicant *app,
260 	struct rb_node *parent = NULL, **p = &app->mad.rb_node;
286 	rb_insert_color(&attr->node, &app->mad);
290 static void mrp_attr_destroy(struct mrp_applicant *app, struct mrp_attr *attr)
292 	rb_erase(&attr->node, &app->mad);
296 static void mrp_attr_destroy_all(struct mrp_applicant *app)
301 	for (node = rb_first(&app->mad);
305 		mrp_attr_destroy(app, attr);
309 static int mrp_pdu_init(struct mrp_applicant *app)
314 	skb = alloc_skb(app->dev->mtu + LL_RESERVED_SPACE(app->dev),
319 	skb->dev = app->dev;
320 	skb->protocol = app->app->pkttype.type;
321 	skb_reserve(skb, LL_RESERVED_SPACE(app->dev));
326 	ph->version = app->app->version;
328 	app->pdu = skb;
332 static int mrp_pdu_append_end_mark(struct mrp_applicant *app)
336 	if (skb_tailroom(app->pdu) < sizeof(*endmark))
338 	endmark = __skb_put(app->pdu, sizeof(*endmark));
343 static void mrp_pdu_queue(struct mrp_applicant *app)
345 	if (!app->pdu)
348 	if (mrp_cb(app->pdu)->mh)
349 		mrp_pdu_append_end_mark(app);
350 	mrp_pdu_append_end_mark(app);
352 	dev_hard_header(app->pdu, app->dev, ntohs(app->app->pkttype.type),
353 			app->app->group_address, app->dev->dev_addr,
354 			app->pdu->len);
356 	skb_queue_tail(&app->queue, app->pdu);
357 	app->pdu = NULL;
360 static void mrp_queue_xmit(struct mrp_applicant *app)
364 	while ((skb = skb_dequeue(&app->queue)))
368 static int mrp_pdu_append_msg_hdr(struct mrp_applicant *app,
373 	if (mrp_cb(app->pdu)->mh) {
374 		if (mrp_pdu_append_end_mark(app) < 0)
376 		mrp_cb(app->pdu)->mh = NULL;
377 		mrp_cb(app->pdu)->vah = NULL;
380 	if (skb_tailroom(app->pdu) < sizeof(*mh))
382 	mh = __skb_put(app->pdu, sizeof(*mh));
385 	mrp_cb(app->pdu)->mh = mh;
389 static int mrp_pdu_append_vecattr_hdr(struct mrp_applicant *app,
394 	if (skb_tailroom(app->pdu) < sizeof(*vah) + attrlen)
396 	vah = __skb_put(app->pdu, sizeof(*vah) + attrlen);
399 	mrp_cb(app->pdu)->vah = vah;
400 	memcpy(mrp_cb(app->pdu)->attrvalue, firstattrvalue, attrlen);
404 static int mrp_pdu_append_vecattr_event(struct mrp_applicant *app,
412 	if (!app->pdu) {
413 		err = mrp_pdu_init(app);
422 	if (!mrp_cb(app->pdu)->mh ||
423 	    mrp_cb(app->pdu)->mh->attrtype != attr->type ||
424 	    mrp_cb(app->pdu)->mh->attrlen != attr->len) {
425 		if (mrp_pdu_append_msg_hdr(app, attr->type, attr->len) < 0)
433 	if (!mrp_cb(app->pdu)->vah ||
434 	    memcmp(mrp_cb(app->pdu)->attrvalue, attr->value, attr->len)) {
435 		if (mrp_pdu_append_vecattr_hdr(app, attr->value, attr->len) < 0)
439 	len = be16_to_cpu(get_unaligned(&mrp_cb(app->pdu)->vah->lenflags));
446 		if (skb_tailroom(app->pdu) < sizeof(u8))
448 		vaevents = __skb_put(app->pdu, sizeof(u8));
450 		vaevents = (u8 *)(skb_tail_pointer(app->pdu) - sizeof(u8));
471 	put_unaligned(cpu_to_be16(++len), &mrp_cb(app->pdu)->vah->lenflags);
472 	mrp_attrvalue_inc(mrp_cb(app->pdu)->attrvalue, attr->len);
477 	mrp_pdu_queue(app);
481 static void mrp_attr_event(struct mrp_applicant *app,
504 				    app, attr, MRP_VECATTR_EVENT_NEW) < 0)
509 				    app, attr, MRP_VECATTR_EVENT_JOIN_IN) < 0)
514 				    app, attr, MRP_VECATTR_EVENT_LV) < 0)
520 			mrp_attr_destroy(app, attr);
535 	struct mrp_applicant *app = rtnl_dereference(
543 	spin_lock_bh(&app->lock);
544 	attr = mrp_attr_create(app, value, len, type);
546 		spin_unlock_bh(&app->lock);
549 	mrp_attr_event(app, attr, MRP_EVENT_JOIN);
550 	spin_unlock_bh(&app->lock);
560 	struct mrp_applicant *app = rtnl_dereference(
568 	spin_lock_bh(&app->lock);
569 	attr = mrp_attr_lookup(app, value, len, type);
571 		spin_unlock_bh(&app->lock);
574 	mrp_attr_event(app, attr, MRP_EVENT_LV);
575 	spin_unlock_bh(&app->lock);
579 static void mrp_mad_event(struct mrp_applicant *app, enum mrp_event event)
584 	for (node = rb_first(&app->mad);
588 		mrp_attr_event(app, attr, event);
592 static void mrp_join_timer_arm(struct mrp_applicant *app)
597 	mod_timer(&app->join_timer, jiffies + delay);
602 	struct mrp_applicant *app = from_timer(app, t, join_timer);
604 	spin_lock(&app->lock);
605 	mrp_mad_event(app, MRP_EVENT_TX);
606 	mrp_pdu_queue(app);
607 	spin_unlock(&app->lock);
609 	mrp_queue_xmit(app);
610 	spin_lock(&app->lock);
611 	if (likely(app->active))
612 		mrp_join_timer_arm(app);
613 	spin_unlock(&app->lock);
616 static void mrp_periodic_timer_arm(struct mrp_applicant *app)
618 	mod_timer(&app->periodic_timer,
624 	struct mrp_applicant *app = from_timer(app, t, periodic_timer);
626 	spin_lock(&app->lock);
627 	if (likely(app->active)) {
628 		mrp_mad_event(app, MRP_EVENT_PERIODIC);
629 		mrp_pdu_queue(app);
630 		mrp_periodic_timer_arm(app);
632 	spin_unlock(&app->lock);
648 static void mrp_pdu_parse_vecattr_event(struct mrp_applicant *app,
655 	attr = mrp_attr_lookup(app, mrp_cb(skb)->attrvalue,
684 	mrp_attr_event(app, attr, event);
687 static int mrp_pdu_parse_vecattr(struct mrp_applicant *app,
702 		mrp_mad_event(app, MRP_EVENT_R_LA);
736 		mrp_pdu_parse_vecattr_event(app, skb, vaevent);
746 		mrp_pdu_parse_vecattr_event(app, skb, vaevent);
755 		mrp_pdu_parse_vecattr_event(app, skb, vaevent);
760 static int mrp_pdu_parse_msg(struct mrp_applicant *app, struct sk_buff *skb,
771 	    mrp_cb(skb)->mh->attrtype > app->app->maxattr ||
778 		if (mrp_pdu_parse_vecattr(app, skb, offset) < 0)
790 	struct mrp_applicant *app;
806 	app = rcu_dereference(port->applicants[appl->type]);
807 	if (unlikely(!app))
815 	if (ph->version != app->app->version)
818 	spin_lock(&app->lock);
822 		if (mrp_pdu_parse_msg(app, skb, &offset) < 0)
825 	spin_unlock(&app->lock);
857 	struct mrp_applicant *app;
869 	app = kzalloc(sizeof(*app), GFP_KERNEL);
870 	if (!app)
877 	app->dev = dev;
878 	app->app = appl;
879 	app->mad = RB_ROOT;
880 	app->active = true;
881 	spin_lock_init(&app->lock);
882 	skb_queue_head_init(&app->queue);
883 	rcu_assign_pointer(dev->mrp_port->applicants[appl->type], app);
884 	timer_setup(&app->join_timer, mrp_join_timer, 0);
885 	mrp_join_timer_arm(app);
886 	timer_setup(&app->periodic_timer, mrp_periodic_timer, 0);
887 	mrp_periodic_timer_arm(app);
891 	kfree(app);
902 	struct mrp_applicant *app = rtnl_dereference(
909 	spin_lock_bh(&app->lock);
910 	app->active = false;
911 	spin_unlock_bh(&app->lock);
915 	timer_shutdown_sync(&app->join_timer);
916 	timer_shutdown_sync(&app->periodic_timer);
918 	spin_lock_bh(&app->lock);
919 	mrp_mad_event(app, MRP_EVENT_TX);
920 	mrp_attr_destroy_all(app);
921 	mrp_pdu_queue(app);
922 	spin_unlock_bh(&app->lock);
924 	mrp_queue_xmit(app);
927 	kfree_rcu(app, rcu);