ip_mroute.c

RTEMS (Real-Time Executive for Multiprocessor Systems) is a free open source real-time operating sys

	return;
    }

    tbf_update_tokens(vifp);

    /* if there are enough tokens, 
     * and the queue is empty,
     * send this packet out
     */

    if (t->tbf_q_len == 0) {
	/* queue empty, send packet if enough tokens */
	if (p_len <= t->tbf_n_tok) {
	    t->tbf_n_tok -= p_len;
	    tbf_send_packet(vifp, m);
	} else {
	    /* queue packet and timeout till later */
	    tbf_queue(vifp, m);
	    timeout(tbf_reprocess_q, (caddr_t)vifp, TBF_REPROCESS);
	}
    } else if (t->tbf_q_len < t->tbf_max_q_len) {
	/* finite queue length, so queue pkts and process queue */
	tbf_queue(vifp, m);
	tbf_process_q(vifp);
    } else {
	/* queue length too much, try to dq and queue and process */
	if (!tbf_dq_sel(vifp, ip)) {
	    mrtstat.mrts_q_overflow++;
	    m_freem(m);
	    return;
	} else {
	    tbf_queue(vifp, m);
	    tbf_process_q(vifp);
	}
    }
    return;
}

/* 
 * adds a packet to the queue at the interface
 */
static void
tbf_queue(vifp, m) 
	register struct vif *vifp;
	register struct mbuf *m;
{
    register int s = splnet();
    register struct tbf *t = vifp->v_tbf;

    if (t->tbf_t == NULL) {
	/* Queue was empty */
	t->tbf_q = m;
    } else {
	/* Insert at tail */
	t->tbf_t->m_act = m;
    }

    /* Set new tail pointer */
    t->tbf_t = m;

#ifdef DIAGNOSTIC
    /* Make sure we didn't get fed a bogus mbuf */
    if (m->m_act)
	panic("tbf_queue: m_act");
#endif
    m->m_act = NULL;

    t->tbf_q_len++;

    splx(s);
}


/* 
 * processes the queue at the interface
 */
static void
tbf_process_q(vifp)
    register struct vif *vifp;
{
    register struct mbuf *m;
    register int len;
    register int s = splnet();
    register struct tbf *t = vifp->v_tbf;

    /* loop through the queue at the interface and send as many packets
     * as possible
     */
    while (t->tbf_q_len > 0) {
	m = t->tbf_q;

	len = mtod(m, struct ip *)->ip_len;

	/* determine if the packet can be sent */
	if (len <= t->tbf_n_tok) {
	    /* if so,
	     * reduce no of tokens, dequeue the packet,
	     * send the packet.
	     */
	    t->tbf_n_tok -= len;

	    t->tbf_q = m->m_act;
	    if (--t->tbf_q_len == 0)
		t->tbf_t = NULL;

	    m->m_act = NULL;
	    tbf_send_packet(vifp, m);

	} else break;
    }
    splx(s);
}

static void
tbf_reprocess_q(xvifp)
	void *xvifp;
{
    register struct vif *vifp = xvifp;
    if (ip_mrouter == NULL) 
	return;

    tbf_update_tokens(vifp);

    tbf_process_q(vifp);

    if (vifp->v_tbf->tbf_q_len)
	timeout(tbf_reprocess_q, (caddr_t)vifp, TBF_REPROCESS);
}

/* function that will selectively discard a member of the queue
 * based on the precedence value and the priority
 */
static int
tbf_dq_sel(vifp, ip)
    register struct vif *vifp;
    register struct ip *ip;
{
    register int s = splnet();
    register u_int p;
    register struct mbuf *m, *last;
    register struct mbuf **np;
    register struct tbf *t = vifp->v_tbf;

    p = priority(vifp, ip);

    np = &t->tbf_q;
    last = NULL;
    while ((m = *np) != NULL) {
	if (p > priority(vifp, mtod(m, struct ip *))) {
	    *np = m->m_act;
	    /* If we're removing the last packet, fix the tail pointer */
	    if (m == t->tbf_t)
		t->tbf_t = last;
	    m_freem(m);
	    /* it's impossible for the queue to be empty, but
	     * we check anyway. */
	    if (--t->tbf_q_len == 0)
		t->tbf_t = NULL;
	    splx(s);
	    mrtstat.mrts_drop_sel++;
	    return(1);
	}
	np = &m->m_act;
	last = m;
    }
    splx(s);
    return(0);
}

static void
tbf_send_packet(vifp, m)
    register struct vif *vifp;
    register struct mbuf *m;
{
    struct ip_moptions imo;
    int error;
    static struct route ro;
    int s = splnet();

    if (vifp->v_flags & VIFF_TUNNEL) {
	/* If tunnel options */
	ip_output(m, (struct mbuf *)0, &vifp->v_route,
		  IP_FORWARDING, (struct ip_moptions *)0);
    } else {
	imo.imo_multicast_ifp  = vifp->v_ifp;
	imo.imo_multicast_ttl  = mtod(m, struct ip *)->ip_ttl - 1;
	imo.imo_multicast_loop = 1;
	imo.imo_multicast_vif  = -1;

	/*
	 * Re-entrancy should not be a problem here, because
	 * the packets that we send out and are looped back at us
	 * should get rejected because they appear to come from
	 * the loopback interface, thus preventing looping.
	 */
	error = ip_output(m, (struct mbuf *)0, &ro,
			  IP_FORWARDING, &imo);

	if (mrtdebug & DEBUG_XMIT)
	    log(LOG_DEBUG, "phyint_send on vif %d err %d\n", 
		vifp - viftable, error);
    }
    splx(s);
}

/* determine the current time and then
 * the elapsed time (between the last time and time now)
 * in milliseconds & update the no. of tokens in the bucket
 */
static void
tbf_update_tokens(vifp)
    register struct vif *vifp;
{
    struct timeval tp;
    register u_long tm;
    register int s = splnet();
    register struct tbf *t = vifp->v_tbf;

    GET_TIME(tp);

    TV_DELTA(tp, t->tbf_last_pkt_t, tm);

    /*
     * This formula is actually
     * "time in seconds" * "bytes/second".
     *
     * (tm / 1000000) * (v_rate_limit * 1000 * (1000/1024) / 8)
     *
     * The (1000/1024) was introduced in add_vif to optimize
     * this divide into a shift.
     */
    t->tbf_n_tok += tm * vifp->v_rate_limit / 1024 / 8;
    t->tbf_last_pkt_t = tp;

    if (t->tbf_n_tok > MAX_BKT_SIZE)
	t->tbf_n_tok = MAX_BKT_SIZE;

    splx(s);
}

static int
priority(vifp, ip)
    register struct vif *vifp;
    register struct ip *ip;
{
    register int prio;

    /* temporary hack; may add general packet classifier some day */

    /*
     * The UDP port space is divided up into four priority ranges:
     * [0, 16384)     : unclassified - lowest priority
     * [16384, 32768) : audio - highest priority
     * [32768, 49152) : whiteboard - medium priority
     * [49152, 65536) : video - low priority
     */
    if (ip->ip_p == IPPROTO_UDP) {
	struct udphdr *udp = (struct udphdr *)(((char *)ip) + (ip->ip_hl << 2));
	switch (ntohs(udp->uh_dport) & 0xc000) {
	    case 0x4000:
		prio = 70;
		break;
	    case 0x8000:
		prio = 60;
		break;
	    case 0xc000:
		prio = 55;
		break;
	    default:
		prio = 50;
		break;
	}
	if (tbfdebug > 1)
		log(LOG_DEBUG, "port %x prio%d\n", ntohs(udp->uh_dport), prio);
    } else {
	    prio = 50;
    }
    return prio;
}

/*
 * End of token bucket filter modifications 
 */

int
ip_rsvp_vif_init(so, m)
    struct socket *so;
    struct mbuf *m;
{
    int i;
    register int s;

    if (rsvpdebug)
	printf("ip_rsvp_vif_init: so_type = %d, pr_protocol = %d\n",
	       so->so_type, so->so_proto->pr_protocol);

    if (so->so_type != SOCK_RAW || so->so_proto->pr_protocol != IPPROTO_RSVP)
	return EOPNOTSUPP;

    /* Check mbuf. */
    if (m == NULL || m->m_len != sizeof(int)) {
	return EINVAL;
    }
    i = *(mtod(m, int *));
 
    if (rsvpdebug)
	printf("ip_rsvp_vif_init: vif = %d rsvp_on = %d\n",i,rsvp_on);
 
    s = splnet();

    /* Check vif. */
    if (!legal_vif_num(i)) {
	splx(s);
	return EADDRNOTAVAIL;
    }

    /* Check if socket is available. */
    if (viftable[i].v_rsvpd != NULL) {
	splx(s);
	return EADDRINUSE;
    }

    viftable[i].v_rsvpd = so;
    /* This may seem silly, but we need to be sure we don't over-increment
     * the RSVP counter, in case something slips up.
     */
    if (!viftable[i].v_rsvp_on) {
	viftable[i].v_rsvp_on = 1;
	rsvp_on++;
    }

    splx(s);
    return 0;
}

int
ip_rsvp_vif_done(so, m)
    struct socket *so;
    struct mbuf *m;
{
	int i;
	register int s;
 
    if (rsvpdebug)
	printf("ip_rsvp_vif_done: so_type = %d, pr_protocol = %d\n",
	       so->so_type, so->so_proto->pr_protocol);
 
    if (so->so_type != SOCK_RAW || so->so_proto->pr_protocol != IPPROTO_RSVP)
	return EOPNOTSUPP;
 
    /* Check mbuf. */
    if (m == NULL || m->m_len != sizeof(int)) {
	    return EINVAL;
    }
    i = *(mtod(m, int *));
 
    s = splnet();
 
    /* Check vif. */
    if (!legal_vif_num(i)) {
	splx(s);
        return EADDRNOTAVAIL;
    }

    if (rsvpdebug)
	printf("ip_rsvp_vif_done: v_rsvpd = %p so = %p\n",
	       viftable[i].v_rsvpd, so);

    viftable[i].v_rsvpd = NULL;
    /* This may seem silly, but we need to be sure we don't over-decrement
     * the RSVP counter, in case something slips up.
     */
    if (viftable[i].v_rsvp_on) {
	viftable[i].v_rsvp_on = 0;
	rsvp_on--;
    }

    splx(s);
    return 0;
}

void
ip_rsvp_force_done(so)
    struct socket *so;
{
    int vifi;
    register int s;

    /* Don't bother if it is not the right type of socket. */
    if (so->so_type != SOCK_RAW || so->so_proto->pr_protocol != IPPROTO_RSVP)
	return;

    s = splnet();

    /* The socket may be attached to more than one vif...this
     * is perfectly legal.
     */
    for (vifi = 0; vifi < numvifs; vifi++) {
	if (viftable[vifi].v_rsvpd == so) {
	    viftable[vifi].v_rsvpd = NULL;
	    /* This may seem silly, but we need to be sure we don't
	     * over-decrement the RSVP counter, in case something slips up.
	     */
	    if (viftable[vifi].v_rsvp_on) {
		viftable[vifi].v_rsvp_on = 0;
		rsvp_on--;
	    }
	}
    }

    splx(s);
    return;
}

void
rsvp_input(m, iphlen)
	struct mbuf *m;
	int iphlen;
{
    int vifi;
    register struct ip *ip = mtod(m, struct ip *);
    static struct sockaddr_in rsvp_src = { sizeof rsvp_src, AF_INET };
    register int s;
    struct ifnet *ifp;

    if (rsvpdebug)
	printf("rsvp_input: rsvp_on %d\n",rsvp_on);

    /* Can still get packets with rsvp_on = 0 if there is a local member
     * of the group to which the RSVP packet is addressed.  But in this
     * case we want to throw the packet away.
     */
    if (!rsvp_on) {
	m_freem(m);
	return;
    }

    /* If the old-style non-vif-associated socket is set, then use
     * it and ignore the new ones.
     */
    if (ip_rsvpd != NULL) {
	if (rsvpdebug)
	    printf("rsvp_input: Sending packet up old-style socket\n");
	rip_input(m, iphlen);
	return;
    }

    s = splnet();

    if (rsvpdebug)
	printf("rsvp_input: check vifs\n");

#ifdef DIAGNOSTIC
    if (!(m->m_flags & M_PKTHDR))
	    panic("rsvp_input no hdr");
#endif

    ifp = m->m_pkthdr.rcvif;
    /* Find which vif the packet arrived on. */
    for (vifi = 0; vifi < numvifs; vifi++) {
	if (viftable[vifi].v_ifp == ifp)
 		break;
 	}
 
    if (vifi == numvifs) {
	/* Can't find vif packet arrived on. Drop packet. */
	if (rsvpdebug)
	    printf("rsvp_input: Can't find vif for packet...dropping it.\n");
	m_freem(m);
	splx(s);
	return;
    }

    if (rsvpdebug)
	printf("rsvp_input: check socket\n");

    if (viftable[vifi].v_rsvpd == NULL) {
	/* drop packet, since there is no specific socket for this
	 * interface */
	    if (rsvpdebug)
		    printf("rsvp_input: No socket defined for vif %d\n",vifi);
	    m_freem(m);
	    splx(s);
	    return;
    }
    rsvp_src.sin_addr = ip->ip_src;

    if (rsvpdebug && m)
	printf("rsvp_input: m->m_len = %d, sbspace() = %ld\n",
	       m->m_len,sbspace(&(viftable[vifi].v_rsvpd->so_rcv)));

    if (socket_send(viftable[vifi].v_rsvpd, m, &rsvp_src) < 0)
	if (rsvpdebug)
	    printf("rsvp_input: Failed to append to socket\n");
    else
	if (rsvpdebug)
	    printf("rsvp_input: send packet up\n");
    
    splx(s);
}

#ifdef MROUTE_LKM
#include <sys/conf.h>
#include <sys/exec.h>
#include <sys/sysent.h>
#include <sys/lkm.h>

MOD_MISC("ip_mroute_mod")

static int
ip_mroute_mod_handle(struct lkm_table *lkmtp, int cmd)
{
	int i;
	struct lkm_misc	*args = lkmtp->private.lkm_misc;
	int err = 0;

	switch(cmd) {
		static int (*old_ip_mrouter_cmd)();
		static int (*old_ip_mrouter_done)();
		static int (*old_ip_mforward)();
		static int (*old_mrt_ioctl)();
		static void (*old_proto4_input)();
		static int (*old_legal_vif_num)();
		extern struct protosw inetsw[];

	case LKM_E_LOAD:
		if(lkmexists(lkmtp) || ip_mrtproto)
		  return(EEXIST);
		old_ip_mrouter_cmd = ip_mrouter_cmd;
		ip_mrouter_cmd = X_ip_mrouter_cmd;
		old_ip_mrouter_done = ip_mrouter_done;
		ip_mrouter_done = X_ip_mrouter_done;
		old_ip_mforward = ip_mforward;
		ip_mforward = X_ip_mforward;
		old_mrt_ioctl = mrt_ioctl;
		mrt_ioctl = X_mrt_ioctl;
              old_proto4_input = inetsw[ip_protox[ENCAP_PROTO]].pr_input;
              inetsw[ip_protox[ENCAP_PROTO]].pr_input = X_ipip_input;
		old_legal_vif_num = legal_vif_num;
		legal_vif_num = X_legal_vif_num;
		ip_mrtproto = IGMP_DVMRP;

		printf("\nIP multicast routing loaded\n");
		break;

	case LKM_E_UNLOAD:
		if (ip_mrouter)
		  return EINVAL;

		ip_mrouter_cmd = old_ip_mrouter_cmd;
		ip_mrouter_done = old_ip_mrouter_done;
		ip_mforward = old_ip_mforward;
		mrt_ioctl = old_mrt_ioctl;
              inetsw[ip_protox[ENCAP_PROTO]].pr_input = old_proto4_input;
		legal_vif_num = old_legal_vif_num;
		ip_mrtproto = 0;
		break;

	default:
		err = EINVAL;
		break;
	}

	return(err);
}

int
ip_mroute_mod(struct lkm_table *lkmtp, int cmd, int ver) {
	DISPATCH(lkmtp, cmd, ver, ip_mroute_mod_handle, ip_mroute_mod_handle,
		 nosys);
}

#endif /* MROUTE_LKM */
#endif /* MROUTING */