1
2
3
4
5
6
7#include <linux/module.h>
8#include <linux/init.h>
9#include <linux/slab.h>
10#include <linux/random.h>
11#include <linux/skbuff.h>
12#include <linux/netdevice.h>
13#include <linux/if_ether.h>
14#include <linux/if_arp.h>
15#include <linux/string.h>
16#include <linux/wireless.h>
17#include "rtllib.h"
18
19#include <linux/crypto.h>
20#include <crypto/aead.h>
21
22#include <linux/scatterlist.h>
23
24#define AES_BLOCK_LEN 16
25#define CCMP_HDR_LEN 8
26#define CCMP_MIC_LEN 8
27#define CCMP_TK_LEN 16
28#define CCMP_PN_LEN 6
29
30struct rtllib_ccmp_data {
31 u8 key[CCMP_TK_LEN];
32 int key_set;
33
34 u8 tx_pn[CCMP_PN_LEN];
35 u8 rx_pn[CCMP_PN_LEN];
36
37 u32 dot11RSNAStatsCCMPFormatErrors;
38 u32 dot11RSNAStatsCCMPReplays;
39 u32 dot11RSNAStatsCCMPDecryptErrors;
40
41 int key_idx;
42
43 struct crypto_aead *tfm;
44
45
46 u8 tx_aad[2 * AES_BLOCK_LEN];
47 u8 rx_aad[2 * AES_BLOCK_LEN];
48};
49
50static void *rtllib_ccmp_init(int key_idx)
51{
52 struct rtllib_ccmp_data *priv;
53
54 priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
55 if (priv == NULL)
56 goto fail;
57 priv->key_idx = key_idx;
58
59 priv->tfm = crypto_alloc_aead("ccm(aes)", 0, CRYPTO_ALG_ASYNC);
60 if (IS_ERR(priv->tfm)) {
61 pr_debug("Could not allocate crypto API aes\n");
62 priv->tfm = NULL;
63 goto fail;
64 }
65 return priv;
66
67fail:
68 if (priv) {
69 if (priv->tfm)
70 crypto_free_aead(priv->tfm);
71 kfree(priv);
72 }
73
74 return NULL;
75}
76
77
78static void rtllib_ccmp_deinit(void *priv)
79{
80 struct rtllib_ccmp_data *_priv = priv;
81
82 if (_priv && _priv->tfm)
83 crypto_free_aead(_priv->tfm);
84 kfree(priv);
85}
86
87
88static int ccmp_init_iv_and_aad(struct rtllib_hdr_4addr *hdr,
89 u8 *pn, u8 *iv, u8 *aad)
90{
91 u8 *pos, qc = 0;
92 size_t aad_len;
93 u16 fc;
94 int a4_included, qc_included;
95
96 fc = le16_to_cpu(hdr->frame_ctl);
97 a4_included = ((fc & (RTLLIB_FCTL_TODS | RTLLIB_FCTL_FROMDS)) ==
98 (RTLLIB_FCTL_TODS | RTLLIB_FCTL_FROMDS));
99
100 qc_included = ((WLAN_FC_GET_TYPE(fc) == RTLLIB_FTYPE_DATA) &&
101 (WLAN_FC_GET_STYPE(fc) & 0x80));
102 aad_len = 22;
103 if (a4_included)
104 aad_len += 6;
105 if (qc_included) {
106 pos = (u8 *) &hdr->addr4;
107 if (a4_included)
108 pos += 6;
109 qc = *pos & 0x0f;
110 aad_len += 2;
111 }
112
113
114
115
116
117
118
119 iv[0] = 0x1;
120
121
122 iv[1] = qc;
123 memcpy(iv + 2, hdr->addr2, ETH_ALEN);
124 memcpy(iv + 8, pn, CCMP_PN_LEN);
125
126
127
128
129
130
131
132
133 pos = (u8 *) hdr;
134 aad[0] = pos[0] & 0x8f;
135 aad[1] = pos[1] & 0xc7;
136 memcpy(&aad[2], &hdr->addr1, ETH_ALEN);
137 memcpy(&aad[8], &hdr->addr2, ETH_ALEN);
138 memcpy(&aad[14], &hdr->addr3, ETH_ALEN);
139 pos = (u8 *) &hdr->seq_ctl;
140 aad[20] = pos[0] & 0x0f;
141 aad[21] = 0;
142 memset(aad + 22, 0, 8);
143 if (a4_included)
144 memcpy(aad + 22, hdr->addr4, ETH_ALEN);
145 if (qc_included) {
146 aad[a4_included ? 28 : 22] = qc;
147
148 }
149
150 return aad_len;
151}
152
153
154
155static int rtllib_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
156{
157 struct rtllib_ccmp_data *key = priv;
158 int i;
159 u8 *pos;
160 struct rtllib_hdr_4addr *hdr;
161 struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
162 MAX_DEV_ADDR_SIZE);
163 if (skb_headroom(skb) < CCMP_HDR_LEN ||
164 skb_tailroom(skb) < CCMP_MIC_LEN ||
165 skb->len < hdr_len)
166 return -1;
167
168 pos = skb_push(skb, CCMP_HDR_LEN);
169 memmove(pos, pos + CCMP_HDR_LEN, hdr_len);
170 pos += hdr_len;
171
172 i = CCMP_PN_LEN - 1;
173 while (i >= 0) {
174 key->tx_pn[i]++;
175 if (key->tx_pn[i] != 0)
176 break;
177 i--;
178 }
179
180 *pos++ = key->tx_pn[5];
181 *pos++ = key->tx_pn[4];
182 *pos++ = 0;
183 *pos++ = (key->key_idx << 6) | (1 << 5) ;
184 *pos++ = key->tx_pn[3];
185 *pos++ = key->tx_pn[2];
186 *pos++ = key->tx_pn[1];
187 *pos++ = key->tx_pn[0];
188
189 hdr = (struct rtllib_hdr_4addr *) skb->data;
190 if (!tcb_desc->bHwSec) {
191 struct aead_request *req;
192 struct scatterlist sg[2];
193 u8 *aad = key->tx_aad;
194 u8 iv[AES_BLOCK_LEN];
195 int aad_len, ret;
196 int data_len = skb->len - hdr_len - CCMP_HDR_LEN;
197
198 req = aead_request_alloc(key->tfm, GFP_ATOMIC);
199 if (!req)
200 return -ENOMEM;
201
202 aad_len = ccmp_init_iv_and_aad(hdr, key->tx_pn, iv, aad);
203
204 skb_put(skb, CCMP_MIC_LEN);
205 sg_init_table(sg, 2);
206 sg_set_buf(&sg[0], aad, aad_len);
207 sg_set_buf(&sg[1], skb->data + hdr_len + CCMP_HDR_LEN,
208 data_len + CCMP_MIC_LEN);
209
210 aead_request_set_callback(req, 0, NULL, NULL);
211 aead_request_set_ad(req, aad_len);
212 aead_request_set_crypt(req, sg, sg, data_len, iv);
213
214 ret = crypto_aead_encrypt(req);
215 aead_request_free(req);
216
217 return ret;
218 }
219
220 return 0;
221}
222
223
224static int rtllib_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
225{
226 struct rtllib_ccmp_data *key = priv;
227 u8 keyidx, *pos;
228 struct rtllib_hdr_4addr *hdr;
229 struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb +
230 MAX_DEV_ADDR_SIZE);
231 u8 pn[6];
232
233 if (skb->len < hdr_len + CCMP_HDR_LEN + CCMP_MIC_LEN) {
234 key->dot11RSNAStatsCCMPFormatErrors++;
235 return -1;
236 }
237
238 hdr = (struct rtllib_hdr_4addr *) skb->data;
239 pos = skb->data + hdr_len;
240 keyidx = pos[3];
241 if (!(keyidx & (1 << 5))) {
242 if (net_ratelimit()) {
243 pr_debug("CCMP: received packet without ExtIV flag from %pM\n",
244 hdr->addr2);
245 }
246 key->dot11RSNAStatsCCMPFormatErrors++;
247 return -2;
248 }
249 keyidx >>= 6;
250 if (key->key_idx != keyidx) {
251 pr_debug("CCMP: RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
252 key->key_idx, keyidx, priv);
253 return -6;
254 }
255 if (!key->key_set) {
256 if (net_ratelimit()) {
257 pr_debug("CCMP: received packet from %pM with keyid=%d that does not have a configured key\n",
258 hdr->addr2, keyidx);
259 }
260 return -3;
261 }
262
263 pn[0] = pos[7];
264 pn[1] = pos[6];
265 pn[2] = pos[5];
266 pn[3] = pos[4];
267 pn[4] = pos[1];
268 pn[5] = pos[0];
269 pos += 8;
270 if (memcmp(pn, key->rx_pn, CCMP_PN_LEN) <= 0) {
271 key->dot11RSNAStatsCCMPReplays++;
272 return -4;
273 }
274 if (!tcb_desc->bHwSec) {
275 size_t data_len = skb->len - hdr_len - CCMP_HDR_LEN;
276 struct aead_request *req;
277 struct scatterlist sg[2];
278 u8 *aad = key->rx_aad;
279 u8 iv[AES_BLOCK_LEN];
280 int aad_len, ret;
281
282 req = aead_request_alloc(key->tfm, GFP_ATOMIC);
283 if (!req)
284 return -ENOMEM;
285
286 aad_len = ccmp_init_iv_and_aad(hdr, pn, iv, aad);
287
288 sg_init_table(sg, 2);
289 sg_set_buf(&sg[0], aad, aad_len);
290 sg_set_buf(&sg[1], pos, data_len);
291
292 aead_request_set_callback(req, 0, NULL, NULL);
293 aead_request_set_ad(req, aad_len);
294 aead_request_set_crypt(req, sg, sg, data_len, iv);
295
296 ret = crypto_aead_decrypt(req);
297 aead_request_free(req);
298
299 if (ret) {
300 if (net_ratelimit()) {
301 pr_debug("CCMP: decrypt failed: STA= %pM\n",
302 hdr->addr2);
303 }
304 key->dot11RSNAStatsCCMPDecryptErrors++;
305 return -5;
306 }
307
308 memcpy(key->rx_pn, pn, CCMP_PN_LEN);
309 }
310
311 memmove(skb->data + CCMP_HDR_LEN, skb->data, hdr_len);
312 skb_pull(skb, CCMP_HDR_LEN);
313 skb_trim(skb, skb->len - CCMP_MIC_LEN);
314
315 return keyidx;
316}
317
318
319static int rtllib_ccmp_set_key(void *key, int len, u8 *seq, void *priv)
320{
321 struct rtllib_ccmp_data *data = priv;
322 int keyidx;
323 struct crypto_aead *tfm = data->tfm;
324
325 keyidx = data->key_idx;
326 memset(data, 0, sizeof(*data));
327 data->key_idx = keyidx;
328 data->tfm = tfm;
329 if (len == CCMP_TK_LEN) {
330 memcpy(data->key, key, CCMP_TK_LEN);
331 data->key_set = 1;
332 if (seq) {
333 data->rx_pn[0] = seq[5];
334 data->rx_pn[1] = seq[4];
335 data->rx_pn[2] = seq[3];
336 data->rx_pn[3] = seq[2];
337 data->rx_pn[4] = seq[1];
338 data->rx_pn[5] = seq[0];
339 }
340 if (crypto_aead_setauthsize(data->tfm, CCMP_MIC_LEN) ||
341 crypto_aead_setkey(data->tfm, data->key, CCMP_TK_LEN))
342 return -1;
343 } else if (len == 0) {
344 data->key_set = 0;
345 } else {
346 return -1;
347 }
348
349 return 0;
350}
351
352
353static int rtllib_ccmp_get_key(void *key, int len, u8 *seq, void *priv)
354{
355 struct rtllib_ccmp_data *data = priv;
356
357 if (len < CCMP_TK_LEN)
358 return -1;
359
360 if (!data->key_set)
361 return 0;
362 memcpy(key, data->key, CCMP_TK_LEN);
363
364 if (seq) {
365 seq[0] = data->tx_pn[5];
366 seq[1] = data->tx_pn[4];
367 seq[2] = data->tx_pn[3];
368 seq[3] = data->tx_pn[2];
369 seq[4] = data->tx_pn[1];
370 seq[5] = data->tx_pn[0];
371 }
372
373 return CCMP_TK_LEN;
374}
375
376
377static void rtllib_ccmp_print_stats(struct seq_file *m, void *priv)
378{
379 struct rtllib_ccmp_data *ccmp = priv;
380
381 seq_printf(m,
382 "key[%d] alg=CCMP key_set=%d tx_pn=%pM rx_pn=%pM format_errors=%d replays=%d decrypt_errors=%d\n",
383 ccmp->key_idx, ccmp->key_set,
384 ccmp->tx_pn, ccmp->rx_pn,
385 ccmp->dot11RSNAStatsCCMPFormatErrors,
386 ccmp->dot11RSNAStatsCCMPReplays,
387 ccmp->dot11RSNAStatsCCMPDecryptErrors);
388}
389
390static struct lib80211_crypto_ops rtllib_crypt_ccmp = {
391 .name = "R-CCMP",
392 .init = rtllib_ccmp_init,
393 .deinit = rtllib_ccmp_deinit,
394 .encrypt_mpdu = rtllib_ccmp_encrypt,
395 .decrypt_mpdu = rtllib_ccmp_decrypt,
396 .encrypt_msdu = NULL,
397 .decrypt_msdu = NULL,
398 .set_key = rtllib_ccmp_set_key,
399 .get_key = rtllib_ccmp_get_key,
400 .print_stats = rtllib_ccmp_print_stats,
401 .extra_mpdu_prefix_len = CCMP_HDR_LEN,
402 .extra_mpdu_postfix_len = CCMP_MIC_LEN,
403 .owner = THIS_MODULE,
404};
405
406
407static int __init rtllib_crypto_ccmp_init(void)
408{
409 return lib80211_register_crypto_ops(&rtllib_crypt_ccmp);
410}
411
412
413static void __exit rtllib_crypto_ccmp_exit(void)
414{
415 lib80211_unregister_crypto_ops(&rtllib_crypt_ccmp);
416}
417
418module_init(rtllib_crypto_ccmp_init);
419module_exit(rtllib_crypto_ccmp_exit);
420
421MODULE_LICENSE("GPL");
422
