Hallo,
I have written this small patch for RTnet during a research project, so if anyone is interested, here it is.
It allows to register multiple packet handlers for a single protocol and to have separate handlers depending on the interface. For this reason I also added shared SKB support (inspiration by the newer kernels).
The changes should be transparent for most code, though I didn't test it with the more advanced features of RTNet (e.g. TDMA).
Best regards, Petr Grillinger
diff -urNp -x '\.*' -x '*.[ao]' rtnet-0.8.0/stack/include/rtskb.h
rtnet-0.8.0-pg/stack/include/rtskb.h
--- rtnet-0.8.0/stack/include/rtskb.h 2004-12-09 10:09:38.000000000 +0100
+++ rtnet-0.8.0-pg/stack/include/rtskb.h 2004-12-28 10:17:31.000000000
+0100
@@ -28,6 +28,7 @@
#ifdef __KERNEL__
#include <linux/skbuff.h>
+#include <asm/atomic.h>
#include <rtnet_internal.h>
@@ -197,6 +199,7 @@ struct rtskb {
unsigned char *raw;
} mac;
+ atomic_t users; // number of active users of shared SKB
unsigned short protocol;
unsigned char pkt_type;
@@ -262,7 +265,7 @@ struct rtskb_prio_queue {
#define DEFAULT_SOCKET_RTSKBS 16 /* default number of rtskb's in
socket pools */
#define ALIGN_RTSKB_STRUCT_LEN SKB_DATA_ALIGN(sizeof(struct rtskb))
-#define RTSKB_SIZE 1544 /* maximum needed by pcnet32-rt */
+#define RTSKB_SIZE (1544 + sizeof(atomic_t)) /* maximum
needed by pcnet32-rt */
extern unsigned int socket_rtskbs; /* default number of rtskb's in socket
pools */
diff -urNp -x '\.*' -x '*.[ao]' rtnet-0.8.0/stack/include/stack_mgr.h
rtnet-0.8.0-pg/stack/include/stack_mgr.h
--- rtnet-0.8.0/stack/include/stack_mgr.h 2004-10-28 18:05:41.000000000
+0200
+++ rtnet-0.8.0-pg/stack/include/stack_mgr.h 2004-12-27 11:50:33.000000000
+0100
@@ -40,6 +40,8 @@ struct rtpacket_type {
int (*handler)(struct rtskb *, struct rtpacket_type *);
int (*err_handler)(struct rtskb *, struct rtnet_device *,
struct rtpacket_type *);
+ int ifindex;
+ struct rtpacket_type *next;
};
diff -urNp -x '\.*' -x '*.[ao]' rtnet-0.8.0/stack/packet/af_packet.c
rtnet-0.8.0-pg/stack/packet/af_packet.c
--- rtnet-0.8.0/stack/packet/af_packet.c 2004-10-28 18:05:41.000000000
+0200
+++ rtnet-0.8.0-pg/stack/packet/af_packet.c 2004-12-28 10:20:21.000000000
+0100
@@ -36,16 +36,15 @@ int rt_packet_rcv(struct rtskb *skb, str
{
struct rtsocket *sock = (struct rtsocket *)(((u8 *)pt) -
((u8 *)&((struct rtsocket *)0)->prot.packet));
- int ifindex = sock->prot.packet.ifindex;
void (*callback_func)(struct rtdm_dev_context *, void *);
void *callback_arg;
unsigned long flags;
- if (((ifindex != 0) && (ifindex != skb->rtdev->ifindex)) ||
- (rtskb_acquire(skb, &sock->skb_pool) != 0))
+ if (rtskb_acquire(skb, &sock->skb_pool) != 0) {
kfree_rtskb(skb);
- else {
+ rtos_print("Cannot acquire rtskb\n");
+ } else {
rtdev_reference(skb->rtdev);
rtskb_queue_tail(&sock->incoming, skb);
rtos_event_sem_signal(&sock->wakeup_event);
@@ -91,6 +90,7 @@ int rt_packet_bind(struct rtsocket *sock
}
pt->type = new_type;
+ pt->ifindex = sll->sll_ifindex;
sock->prot.packet.ifindex = sll->sll_ifindex;
/* if protocol is non-zero, register the packet type */
diff -urNp -x '\.*' -x '*.[ao]' rtnet-0.8.0/stack/rtskb.c
rtnet-0.8.0-pg/stack/rtskb.c
--- rtnet-0.8.0/stack/rtskb.c 2004-12-09 10:09:39.000000000 +0100
+++ rtnet-0.8.0-pg/stack/rtskb.c 2004-12-27 11:50:33.000000000 +0100
@@ -189,6 +189,7 @@ struct rtskb *alloc_rtskb(unsigned int s
skb->len = 0;
skb->pkt_type = PACKET_HOST;
skb->xmit_stamp = NULL;
+ atomic_set(&skb->users, 1);
#ifdef CONFIG_RTNET_RTCAP
skb->cap_flags = 0;
@@ -215,6 +216,11 @@ void kfree_rtskb(struct rtskb *skb)
RTNET_ASSERT(skb != NULL, return;);
RTNET_ASSERT(skb->pool != NULL, return;);
+ RTNET_ASSERT(atomic_read(&skb->users) > 0, return;);
+
+ /* Do not free SKB with active users */
+ if (!atomic_dec_and_test(&skb->users))
+ return;
#ifdef CONFIG_RTNET_RTCAP
next_skb = skb;
diff -urNp -x '\.*' -x '*.[ao]' rtnet-0.8.0/stack/stack_mgr.c
rtnet-0.8.0-pg/stack/stack_mgr.c
--- rtnet-0.8.0/stack/stack_mgr.c 2004-10-28 18:05:41.000000000 +0200
+++ rtnet-0.8.0-pg/stack/stack_mgr.c 2004-12-27 13:18:42.000000000 +0100
@@ -46,20 +46,12 @@ int rtdev_add_pack(struct rtpacket_type
rtos_spin_lock_irqsave(&rt_packets_lock, flags);
- if (rt_packets[hash] == NULL) {
- rt_packets[hash] = pt;
-
- pt->refcount = 0;
-
- rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
-
- return 0;
- }
- else {
- rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
-
- return -EADDRNOTAVAIL;
- }
+ pt->next = rt_packets[hash];
+ rt_packets[hash] = pt;
+ pt->refcount = 0;
+
+ rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
+ return 0;
}
@@ -84,22 +76,22 @@ int rtdev_remove_pack(struct rtpacket_ty
rtos_spin_lock_irqsave(&rt_packets_lock, flags);
- if ((rt_packets[hash] != NULL) &&
- (rt_packets[hash]->type == pt->type)) {
- rt_packets[hash] = NULL;
-
- if (pt->refcount > 0)
- ret = -EAGAIN;
-
- rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
-
- return ret;
+ struct rtpacket_type **rtp = &(rt_packets[hash]);
+
+ while ((*rtp) != NULL) {
+ if (*rtp == pt) {
+ *rtp = pt->next;
+ if (pt->refcount > 0)
+ ret = -EAGAIN;
+ goto ende;
+ }
+ rtp = &((*rtp)->next);
}
- else {
- rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
+ ret = -ENOENT;
- return -ENOENT;
- }
+ende:
+ rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
+ return ret;
}
@@ -185,28 +177,38 @@ static void do_stacktask(int mgr_id)
hash = ntohs(skb->protocol) & (MAX_RT_PROTOCOLS-1);
rtos_spin_lock_irqsave(&rt_packets_lock, flags);
-
pt = rt_packets[hash];
- if ((pt != NULL) && (pt->type == skb->protocol)) {
- pt->refcount++;
- rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
-
- pt->handler(skb, pt);
-
- rtos_spin_lock_irqsave(&rt_packets_lock, flags);
- pt->refcount--;
- rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
- } else {
- rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
-
- /* don't warn if running in promiscuous mode (RTcap...?) */
- if ((rtdev->flags & IFF_PROMISC) == 0)
- rtos_print("RTnet: unknown layer-3 protocol\n");
-
- kfree_rtskb(skb);
+ int proto_cnt = 0,
+ if_cnt = 0;
+ for ( ; pt; pt = pt->next) {
+ if (pt->type == skb->protocol) {
+ if (!pt->ifindex || pt->ifindex == skb->rtdev->ifindex) {
+ pt->refcount++;
+ rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
+
+ atomic_inc(&skb->users);
+ pt->handler(skb, pt);
+
+ rtos_spin_lock_irqsave(&rt_packets_lock, flags);
+ pt->refcount--;
+ if_cnt++;
+ }
+ proto_cnt++;
+ }
}
-
+ rtos_spin_unlock_irqrestore(&rt_packets_lock, flags);
+
+ /* don't warn if running in promiscuous mode (RTcap...?) */
+ if ((rtdev->flags & IFF_PROMISC) == 0) {
+ if (proto_cnt == 0)
+ rtos_print("RTnet: missing handler for protocol 0x%04x\n",
+ ntohs(skb->protocol));
+ else if (if_cnt == 0)
+ rtos_print("RTnet: missing handler for protocol 0x%04x,
interface %d\n",
+ ntohs(skb->protocol), skb->rtdev->ifindex);
+ }
+ kfree_rtskb(skb);
rtdev_dereference(rtdev);
}
}
