1 /*
2 * Copyright (c) 2015 South Silicon Valley Microelectronics Inc.
3 * Copyright (c) 2015 iComm Corporation
4 *
5 * This program is free software: you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation, either version 3 of the License, or
8 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12 * See the GNU General Public License for more details.
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 */
16
17 #include <linux/err.h>
18 #include <linux/module.h>
19 #include <linux/init.h>
20 #include <linux/slab.h>
21 #include <linux/random.h>
22 #include <linux/scatterlist.h>
23 #include <linux/skbuff.h>
24 #include <linux/mm.h>
25 #include <asm/string.h>
26 #include <net/lib80211.h>
27 #include <linux/crypto.h>
28 #include <linux/crc32.h>
29 #include "sec.h"
30 struct lib80211_wep_data
31 {
32 u32 iv;
33 u8 key[WEP_KEY_LEN + 1];
34 u8 key_len;
35 u8 key_idx;
36 struct crypto_blkcipher *tx_tfm;
37 struct crypto_blkcipher *rx_tfm;
38 };
lib80211_wep_init(int keyidx)39 static void *lib80211_wep_init (int keyidx)
40 {
41 struct lib80211_wep_data *priv;
42 priv = kzalloc (sizeof (*priv), GFP_ATOMIC);
43 if (priv == NULL)
44 goto fail;
45 priv->key_idx = keyidx;
46 priv->tx_tfm = crypto_alloc_blkcipher ("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
47 if (IS_ERR (priv->tx_tfm))
48 {
49 priv->tx_tfm = NULL;
50 goto fail;
51 }
52 priv->rx_tfm = crypto_alloc_blkcipher ("ecb(arc4)", 0, CRYPTO_ALG_ASYNC);
53 if (IS_ERR (priv->rx_tfm))
54 {
55 priv->rx_tfm = NULL;
56 goto fail;
57 }
58 get_random_bytes (&priv->iv, 4);
59 return priv;
60 fail:
61 if (priv)
62 {
63 if (priv->tx_tfm)
64 crypto_free_blkcipher (priv->tx_tfm);
65 if (priv->rx_tfm)
66 crypto_free_blkcipher (priv->rx_tfm);
67 kfree (priv);
68 }
69 return NULL;
70 }
lib80211_wep_deinit(void * priv)71 static void lib80211_wep_deinit (void *priv)
72 {
73 struct lib80211_wep_data *_priv = priv;
74 if (_priv)
75 {
76 if (_priv->tx_tfm)
77 crypto_free_blkcipher (_priv->tx_tfm);
78 if (_priv->rx_tfm)
79 crypto_free_blkcipher (_priv->rx_tfm);
80 }
81 kfree (priv);
82 }
lib80211_wep_build_iv(struct sk_buff * skb,int hdr_len,u8 * key,int keylen,void * priv)83 static int lib80211_wep_build_iv (struct sk_buff *skb, int hdr_len,
84 u8 *key, int keylen, void *priv)
85 {
86 struct lib80211_wep_data *wep = priv;
87 u32 klen;
88 u8 *pos;
89 if (skb_headroom (skb) < 4 || skb->len < hdr_len)
90 return -1;
91 pos = skb_push (skb, 4);
92 memmove (pos, pos + 4, hdr_len);
93 pos += hdr_len;
94 klen = 3 + wep->key_len;
95 wep->iv++;
96 if ((wep->iv & 0xff00) == 0xff00)
97 {
98 u8 B = (wep->iv >> 16) & 0xff;
99 if (B >= 3 && B < klen)
100 wep->iv += 0x0100;
101 }
102 *pos++ = (wep->iv >> 16) & 0xff;
103 *pos++ = (wep->iv >> 8) & 0xff;
104 *pos++ = wep->iv & 0xff;
105 *pos++ = wep->key_idx << 6;
106 return 0;
107 }
lib80211_wep_encrypt(struct sk_buff * skb,int hdr_len,void * priv)108 static int lib80211_wep_encrypt (struct sk_buff *skb, int hdr_len, void *priv)
109 {
110 struct lib80211_wep_data *wep = priv;
111 struct blkcipher_desc desc = {.tfm = wep->tx_tfm};
112 u32 crc, klen, len;
113 u8 *pos, *icv;
114 struct scatterlist sg;
115 u8 key[WEP_KEY_LEN + 3];
116 if (skb_tailroom (skb) < 4)
117 {
118 printk("####%s: too few tailroom\n", __FUNCTION__);
119 return -1;
120 }
121 if (lib80211_wep_build_iv (skb, hdr_len, NULL, 0, priv))
122 {
123 printk("####%s: build iv failure\n", __FUNCTION__);
124 return -1;
125 }
126 skb_copy_from_linear_data_offset (skb, hdr_len, key, 3);
127 memcpy (key + 3, wep->key, wep->key_len);
128 len = skb->len - hdr_len - 4;
129 pos = skb->data + hdr_len + 4;
130 klen = 3 + wep->key_len;
131 crc = ~crc32_le (~0, pos, len);
132 icv = skb_put (skb, 4);
133 icv[0] = crc;
134 icv[1] = crc >> 8;
135 icv[2] = crc >> 16;
136 icv[3] = crc >> 24;
137 crypto_blkcipher_setkey (wep->tx_tfm, key, klen);
138 sg_init_one (&sg, pos, len + 4);
139 return crypto_blkcipher_encrypt (&desc, &sg, &sg, len + 4);
140 }
lib80211_wep_decrypt(struct sk_buff * skb,int hdr_len,void * priv)141 static int lib80211_wep_decrypt (struct sk_buff *skb, int hdr_len, void *priv)
142 {
143 struct lib80211_wep_data *wep = priv;
144 struct blkcipher_desc desc = {.tfm = wep->rx_tfm};
145 u32 crc, klen, plen;
146 u8 key[WEP_KEY_LEN + 3];
147 u8 keyidx, *pos, icv[4], *pos2;
148 struct scatterlist sg;
149 if (skb->len < hdr_len + 8)
150 {
151 printk ("%s::skb->len = %d\n", __FUNCTION__, skb->len);
152 return -1;
153 }
154 pos = skb->data + hdr_len;
155 key[0] = *pos++;
156 key[1] = *pos++;
157 key[2] = *pos++;
158 keyidx = *pos++ >> 6;
159 if (keyidx != wep->key_idx)
160 return -1;
161 klen = 3 + wep->key_len;
162 memcpy (key + 3, wep->key, wep->key_len);
163 plen = skb->len - hdr_len - 8;
164 crypto_blkcipher_setkey (wep->rx_tfm, key, klen);
165 sg_init_one (&sg, pos, plen + 4);
166 if (crypto_blkcipher_decrypt (&desc, &sg, &sg, plen + 4))
167 return -7;
168 crc = ~crc32_le (~0, pos, plen);
169 icv[0] = crc;
170 icv[1] = crc >> 8;
171 icv[2] = crc >> 16;
172 icv[3] = crc >> 24;
173 pos2 = (pos + plen);
174 if (memcmp (icv, pos + plen, 4) != 0)
175 {
176 return -2;
177 }
178 memmove (skb->data + 4, skb->data, hdr_len);
179 skb_pull (skb, 4);
180 skb_trim (skb, skb->len - 4);
181 return 0;
182 }
lib80211_wep_set_key(void * key,int len,u8 * seq,void * priv)183 static int lib80211_wep_set_key (void *key, int len, u8 * seq, void *priv)
184 {
185 struct lib80211_wep_data *wep = priv;
186 if (len < 0 || len > WEP_KEY_LEN)
187 return -1;
188 memcpy (wep->key, key, len);
189 wep->key_len = len;
190 return 0;
191 }
lib80211_wep_get_key(void * key,int len,u8 * seq,void * priv)192 static int lib80211_wep_get_key (void *key, int len, u8 * seq, void *priv)
193 {
194 struct lib80211_wep_data *wep = priv;
195 if (len < wep->key_len)
196 return -1;
197 memcpy (key, wep->key, wep->key_len);
198 return wep->key_len;
199 }
lib80211_wep_print_stats(char * p,void * priv)200 static char *lib80211_wep_print_stats (char *p, void *priv)
201 {
202 struct lib80211_wep_data *wep = priv;
203 p += sprintf (p, "key[%d] alg=WEP len=%d\n", wep->key_idx, wep->key_len);
204 return p;
205 }
206 static struct ssv_crypto_ops ssv_crypt_wep = {
207 .name = "WEP",
208 .init = lib80211_wep_init,
209 .deinit = lib80211_wep_deinit,
210 .encrypt_mpdu = lib80211_wep_encrypt,
211 .decrypt_mpdu = lib80211_wep_decrypt,
212 .encrypt_msdu = NULL,
213 .decrypt_msdu = NULL,
214 .set_key = lib80211_wep_set_key,
215 .get_key = lib80211_wep_get_key,
216 .print_stats = lib80211_wep_print_stats,
217 .extra_mpdu_prefix_len = 4,
218 .extra_mpdu_postfix_len = 4,
219 #ifdef MULTI_THREAD_ENCRYPT
220 .encrypt_prepare = NULL,
221 .decrypt_prepare = NULL,
222 #endif
223 };
get_crypto_wep_ops(void)224 struct ssv_crypto_ops *get_crypto_wep_ops (void)
225 {
226 return &ssv_crypt_wep;
227 }
228