Introduce experimental MPLS over ethernet support. Add 'options MPLS'
[dragonfly.git] / sys / netproto / mpls / mpls_output.c
1 /*
2  * Copyright (c) 2007 The DragonFly Project.  All rights reserved.
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  *    notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  *    notice, this list of conditions and the following disclaimer in
12  *    the documentation and/or other materials provided with the
13  *    distribution.
14  * 3. Neither the name of The DragonFly Project nor the names of its
15  *    contributors may be used to endorse or promote products derived
16  *    from this software without specific, prior written permission.
17  *
18  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
19  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
20  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
21  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE
22  * COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
23  * INCIDENTAL, SPECIAL, EXEMPLARY OR CONSEQUENTIAL DAMAGES (INCLUDING,
24  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
25  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
26  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
27  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
28  * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
29  * SUCH DAMAGE.
30  *
31  * $DragonFly: src/sys/netproto/mpls/mpls_output.c,v 1.1 2008/07/07 22:02:10 nant Exp $
32  */
33
34 #include <sys/param.h>
35 #include <sys/mbuf.h>
36 #include <sys/systm.h>
37
38 #include <net/if_var.h>
39
40 #include <netinet/ip.h>
41
42 #include <netproto/mpls/mpls.h>
43 #include <netproto/mpls/mpls_var.h>
44
45 static int mpls_push(struct mbuf **, mpls_label_t,
46                      mpls_s_t, mpls_exp_t, mpls_ttl_t);
47 static int mpls_swap(struct mbuf *, mpls_label_t);
48 static int mpls_pop(struct mbuf *, mpls_s_t *);
49
50 int
51 mpls_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *dst,
52              struct rtentry *rt)
53 {
54         struct sockaddr_mpls *smpls = NULL;
55         int error=0, i;
56         struct sockaddr *sa = NULL;
57         mpls_s_t stackempty;
58         mpls_ttl_t ttl = 255;
59         struct ip *ip;
60
61         M_ASSERTPKTHDR(m);
62
63         KASSERT(ifp != NULL, ("mpls_output: ifp can't be NULL"));
64
65         /* Check if we are coming from an MPLS routing table lookup */
66         stackempty = rt_key(rt)->sa_family != AF_MPLS ? 1 : 0;
67         if (stackempty) {
68                 switch (rt_key(rt)->sa_family) {
69                 case AF_INET:
70                         ip = mtod(m, struct ip *);
71                         ttl = ip->ip_ttl;
72                         break;
73                 }
74         }
75
76         for (i=0; i < MPLS_MAXLOPS && rt->rt_shim[i] != NULL; ++i) {
77                 smpls = (struct sockaddr_mpls *)rt->rt_shim[i];
78                 switch (smpls->smpls_op) {
79                 case MPLSLOP_PUSH:
80                         error = mpls_push(&m,
81                                   ntohl(smpls->smpls_label),
82                                   (i==0 && dst->sa_family != AF_MPLS) ? 1 : 0,
83                                   0,
84                                   ttl);
85                         if (error)
86                                 return (error);
87                         stackempty = 0;
88                         sa = (struct sockaddr *)smpls;
89                         break;
90                 case MPLSLOP_SWAP:
91                         /*
92                          * Operation is only permmited if label stack
93                          * is not empty.
94                          */
95                         if (stackempty)
96                                 return (ENOTSUP);
97                         error = mpls_swap(m, ntohl(smpls->smpls_label));
98                         if (error)
99                                 return (error);
100                         sa = (struct sockaddr *)smpls;
101                         break;
102                 case MPLSLOP_POP:
103                         /*
104                          * Operation is only permmited if label stack
105                          * is not empty.
106                          */
107                         if (stackempty)
108                                 return (ENOTSUP);
109                         mpls_pop(m, &stackempty);
110                         /* If not bottom label */
111                         if (!stackempty)
112                                 sa = (struct sockaddr *)smpls;
113                         else
114                                 sa = dst;
115                         break;
116                 default:
117                         /* Unknown label operation */
118                         return (ENOTSUP);
119                 }
120         }
121         
122         error = (*ifp->if_output)(ifp, m, sa, rt);
123
124         return (error);
125 }
126
127 /*
128  * Returns FALSE if no further output processing required.
129  */
130 boolean_t
131 mpls_output_process(struct ifnet *ifp, struct mbuf *m, struct sockaddr *dst,
132                  struct rtentry *rt)
133 {
134         int error;
135
136         if (!(rt->rt_flags & RTF_MPLSOPS))
137                 return TRUE;
138
139         error = mpls_output(ifp, m,
140                         (struct sockaddr *)dst,
141                         rt);
142         if (error)
143                 return FALSE;
144
145         return TRUE;
146 }
147
148 static int
149 mpls_push(struct mbuf **m, mpls_label_t label, mpls_s_t s, mpls_exp_t exp, mpls_ttl_t ttl) {
150         struct mpls *mpls;
151         u_int32_t buf = 0;      /* Silence warning */
152
153         M_PREPEND(*m, sizeof(struct mpls), MB_DONTWAIT);
154         if (*m == NULL)
155                 return (ENOBUFS);
156
157         MPLS_SET_LABEL(buf, label);
158         MPLS_SET_STACK(buf, s);
159         MPLS_SET_EXP(buf, exp);
160         MPLS_SET_TTL(buf, ttl);
161         mpls = mtod(*m, struct mpls *);
162         mpls->mpls_shim = htonl(buf);
163         
164         return (0);
165 }
166
167 static int
168 mpls_swap(struct mbuf *m, mpls_label_t label) {
169         struct mpls *mpls;
170         u_int32_t buf;
171         mpls_ttl_t ttl;
172
173         if (m->m_len < sizeof(struct mpls) &&
174            (m = m_pullup(m, sizeof(struct mpls))) == NULL)
175                 return (ENOBUFS);
176
177         mpls = mtod(m, struct mpls *);
178         buf = ntohl(mpls->mpls_shim);
179         MPLS_SET_LABEL(buf, label);
180         ttl = MPLS_TTL(buf);
181         MPLS_SET_TTL(buf, --ttl); /* XXX tunnel mode: uniform, pipe, short pipe */
182         mpls->mpls_shim = htonl(buf);
183         
184         return (0);
185 }
186
187 static int
188 mpls_pop(struct mbuf *m, mpls_s_t *sbit) {
189         struct mpls *mpls;
190         u_int32_t buf;
191
192         if (m->m_len < sizeof(struct mpls)) {
193                 m = m_pullup(m, sizeof(struct mpls));
194                 if (m == NULL)
195                         return (ENOBUFS);
196         }
197         mpls = mtod(m, struct mpls *);
198         buf = ntohl(mpls->mpls_shim);
199         *sbit = MPLS_STACK(buf);
200
201         m_adj(m, sizeof(struct mpls));
202
203         return (0);
204 }