net: Add MHI support for RMC PegaPCI.
[linux-3.10.git] / net / mhi / l2mux.c
1 /*
2  * File: l2mux.c
3  *
4  * Modem-Host Interface (MHI) L2MUX layer
5  *
6  * Copyright (C) 2011 Renesas Mobile Corporation. All rights reserved.
7  *
8  * Author: Petri Mattila <petri.to.mattila@renesasmobile.com>
9  *
10  * This program is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU General Public License
12  * version 2 as published by the Free Software Foundation.
13  *
14  * This program is distributed in the hope that it will be useful, but
15  * WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. ┬áSee the GNU
17  * General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program; if not, write to the Free Software
21  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA
22  * 02110-1301 USA
23  *
24  */
25
26 #include <linux/kernel.h>
27 #include <linux/module.h>
28 #include <linux/slab.h>
29 #include <linux/if_mhi.h>
30 #include <linux/mhi.h>
31 #include <linux/l2mux.h>
32
33 #include <net/af_mhi.h>
34
35 #ifdef CONFIG_MHI_DEBUG
36 # define DPRINTK(...)    printk(KERN_DEBUG "MHI/L2MUX: " __VA_ARGS__)
37 #else
38 # define DPRINTK(...)
39 #endif
40
41
42 /* Handle ONLY Non DIX types 0x00-0xff */
43 #define ETH_NON_DIX_NPROTO   0x0100
44
45
46 /* L2MUX master lock */
47 static DEFINE_SPINLOCK(l2mux_lock);
48
49 /* L3 ID -> RX function table */
50 static l2mux_skb_fn *l2mux_id2rx_tab[MHI_L3_NPROTO] __read_mostly;
51
52 /* Packet Type -> TX function table */
53 static l2mux_skb_fn *l2mux_pt2tx_tab[ETH_NON_DIX_NPROTO] __read_mostly;
54
55
56 int l2mux_netif_rx_register(int l3, l2mux_skb_fn *fn)
57 {
58         int err = 0;
59
60         DPRINTK("l2mux_netif_rx_register(l3:%d, fn:%p)\n", l3, fn);
61
62         if (l3 < 0 || l3 >= MHI_L3_NPROTO)
63                 return -EINVAL;
64
65         if (!fn)
66                 return -EINVAL;
67
68         spin_lock(&l2mux_lock);
69         {
70                 if (l2mux_id2rx_tab[l3] == NULL)
71                         l2mux_id2rx_tab[l3] = fn;
72                 else
73                         err = -EBUSY;
74         }
75         spin_unlock(&l2mux_lock);
76
77         return err;
78 }
79 EXPORT_SYMBOL(l2mux_netif_rx_register);
80
81 int l2mux_netif_rx_unregister(int l3)
82 {
83         int err = 0;
84
85         DPRINTK("l2mux_netif_rx_unregister(l3:%d)\n", l3);
86
87         if (l3 < 0 || l3 >= MHI_L3_NPROTO)
88                 return -EINVAL;
89
90         spin_lock(&l2mux_lock);
91         {
92                 if (l2mux_id2rx_tab[l3])
93                         l2mux_id2rx_tab[l3] = NULL;
94                 else
95                         err = -EPROTONOSUPPORT;
96         }
97         spin_unlock(&l2mux_lock);
98
99         return err;
100 }
101 EXPORT_SYMBOL(l2mux_netif_rx_unregister);
102
103 int l2mux_netif_tx_register(int pt, l2mux_skb_fn *fn)
104 {
105         int err = 0;
106
107         DPRINTK("l2mux_netif_tx_register(pt:%d, fn:%p)\n", pt, fn);
108
109         if (pt <= 0 || pt >= ETH_NON_DIX_NPROTO)
110                 return -EINVAL;
111
112         if (!fn)
113                 return -EINVAL;
114
115         spin_lock(&l2mux_lock);
116         {
117                 if (l2mux_pt2tx_tab[pt] == NULL)
118                         l2mux_pt2tx_tab[pt] = fn;
119                 else
120                         err = -EBUSY;
121         }
122         spin_unlock(&l2mux_lock);
123
124         return err;
125 }
126 EXPORT_SYMBOL(l2mux_netif_tx_register);
127
128 int l2mux_netif_tx_unregister(int pt)
129 {
130         int err = 0;
131
132         DPRINTK("l2mux_netif_tx_unregister(pt:%d)\n", pt);
133
134         if (pt <= 0 || pt >= ETH_NON_DIX_NPROTO)
135                 return -EINVAL;
136
137         spin_lock(&l2mux_lock);
138         {
139                 if (l2mux_pt2tx_tab[pt])
140                         l2mux_pt2tx_tab[pt] = NULL;
141                 else
142                         err = -EPROTONOSUPPORT;
143         }
144         spin_unlock(&l2mux_lock);
145
146         return err;
147 }
148 EXPORT_SYMBOL(l2mux_netif_tx_unregister);
149
150 int l2mux_skb_rx(struct sk_buff *skb, struct net_device *dev)
151 {
152         struct l2muxhdr  *l2hdr;
153         unsigned          l3pid;
154         unsigned          l3len;
155         l2mux_skb_fn     *rxfn;
156
157         /* Set the device in the skb */
158         skb->dev = dev;
159
160         /* Set MAC header here */
161         skb_reset_mac_header(skb);
162
163         /* L2MUX header */
164         l2hdr = l2mux_hdr(skb);
165
166         /* proto id and length in L2 header */
167         l3pid = l2mux_get_proto(l2hdr);
168         l3len = l2mux_get_length(l2hdr);
169
170         DPRINTK("L2MUX: RX dev:%d skb_len:%d l3_len:%d l3_pid:%d\n",
171                        skb->dev->ifindex, skb->len, l3len, l3pid);
172
173 #ifdef CONFIG_MHI_DUMP_FRAMES
174         {
175                 u8 *ptr = skb->data;
176                 int len = skb_headlen(skb);
177                 int i;
178
179                 printk(KERN_DEBUG "L2MUX: RX dev:%d skb_len:%d l3_len:%d l3_pid:%d\n",
180                        dev->ifindex, skb->len, l3len, l3pid);
181
182                 for (i = 0; i < len; i++) {
183                         if (i%8 == 0)
184                                 printk(KERN_DEBUG "L2MUX: RX [%04X] ", i);
185                         printk(" 0x%02X", ptr[i]);
186                         if (i%8 == 7 || i == len-1)
187                                 printk("\n");
188                 }
189         }
190 #endif
191         /* check that the advertised length is correct */
192         if (l3len != skb->len - L2MUX_HDR_SIZE) {
193                 printk(KERN_WARNING "L2MUX: l2mux_skb_rx: L3_id:%d - skb length mismatch L3:%d (+4) <> SKB:%d",
194                        l3pid, l3len, skb->len);
195                 goto drop;
196         }
197
198         /* get RX function */
199         rxfn = l2mux_id2rx_tab[l3pid];
200
201         /* Not registered */
202         if (!rxfn)
203                 goto drop;
204
205         /* Call the receiver function */
206         return rxfn(skb, dev);
207
208 drop:
209         kfree_skb(skb);
210         return NET_RX_DROP;
211 }
212 EXPORT_SYMBOL(l2mux_skb_rx);
213
214 int l2mux_skb_tx(struct sk_buff *skb, struct net_device *dev)
215 {
216         l2mux_skb_fn *txfn;
217         unsigned type;
218
219         /* Packet type ETH_P_XXX */
220         type = ntohs(skb->protocol);
221
222 #ifdef CONFIG_MHI_DUMP_FRAMES
223         {
224                 u8 *ptr = skb->data;
225                 int len = skb_headlen(skb);
226                 int i;
227
228                 printk(KERN_DEBUG "L2MUX: TX dev:%d skb_len:%d ETH_P:%d\n",
229                        dev->ifindex, skb->len, type);
230
231                 for (i = 0; i < len; i++) {
232                         if (i%8 == 0)
233                                 printk(KERN_DEBUG "L2MUX: TX [%04X] ", i);
234                         printk(" 0x%02X", ptr[i]);
235                         if (i%8 == 7 || i == len-1)
236                                 printk("\n");
237                 }
238         }
239 #endif
240         /* Only handling non DIX types */
241         if (type <= 0 || type >= ETH_NON_DIX_NPROTO)
242                 return -EINVAL;
243
244         /* TX function for this packet type */
245         txfn = l2mux_pt2tx_tab[type];
246
247         if (txfn)
248                 return txfn(skb, dev);
249
250         return 0;
251 }
252 EXPORT_SYMBOL(l2mux_skb_tx);
253
254 static int __init l2mux_init(void)
255 {
256         int i;
257
258         DPRINTK("l2mux_init\n");
259
260         for (i = 0; i < MHI_L3_NPROTO; i++)
261                 l2mux_id2rx_tab[i] = NULL;
262
263         for (i = 0; i < ETH_NON_DIX_NPROTO; i++)
264                 l2mux_pt2tx_tab[i] = NULL;
265
266         return 0;
267 }
268
269 static void __exit l2mux_exit(void)
270 {
271         DPRINTK("l2mux_exit\n");
272 }
273
274 module_init(l2mux_init);
275 module_exit(l2mux_exit);
276
277 MODULE_AUTHOR("Petri Mattila <petri.to.mattila@renesasmobile.com>");
278 MODULE_DESCRIPTION("L2MUX for MHI Protocol Stack");
279 MODULE_LICENSE("GPL");
280