ccmp.c 8.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349
  1. /*
  2. * CTR with CBC-MAC Protocol (CCMP)
  3. * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
  4. *
  5. * This software may be distributed under the terms of the BSD license.
  6. * See README for more details.
  7. */
  8. #include "utils/includes.h"
  9. #include "utils/common.h"
  10. #include "common/ieee802_11_defs.h"
  11. #include "crypto/aes.h"
  12. #include "wlantest.h"
  13. static void ccmp_aad_nonce(const struct ieee80211_hdr *hdr, const u8 *data,
  14. u8 *aad, size_t *aad_len, u8 *nonce)
  15. {
  16. u16 fc, stype, seq;
  17. int qos = 0, addr4 = 0;
  18. u8 *pos;
  19. nonce[0] = 0;
  20. fc = le_to_host16(hdr->frame_control);
  21. stype = WLAN_FC_GET_STYPE(fc);
  22. if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
  23. (WLAN_FC_TODS | WLAN_FC_FROMDS))
  24. addr4 = 1;
  25. if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA) {
  26. fc &= ~0x0070; /* Mask subtype bits */
  27. if (stype & 0x08) {
  28. const u8 *qc;
  29. qos = 1;
  30. fc &= ~WLAN_FC_ORDER;
  31. qc = (const u8 *) (hdr + 1);
  32. if (addr4)
  33. qc += ETH_ALEN;
  34. nonce[0] = qc[0] & 0x0f;
  35. }
  36. } else if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT)
  37. nonce[0] |= 0x10; /* Management */
  38. fc &= ~(WLAN_FC_RETRY | WLAN_FC_PWRMGT | WLAN_FC_MOREDATA);
  39. fc |= WLAN_FC_ISWEP;
  40. WPA_PUT_LE16(aad, fc);
  41. pos = aad + 2;
  42. os_memcpy(pos, hdr->addr1, 3 * ETH_ALEN);
  43. pos += 3 * ETH_ALEN;
  44. seq = le_to_host16(hdr->seq_ctrl);
  45. seq &= ~0xfff0; /* Mask Seq#; do not modify Frag# */
  46. WPA_PUT_LE16(pos, seq);
  47. pos += 2;
  48. os_memcpy(pos, hdr + 1, addr4 * ETH_ALEN + qos * 2);
  49. pos += addr4 * ETH_ALEN;
  50. if (qos) {
  51. pos[0] &= ~0x70;
  52. if (1 /* FIX: either device has SPP A-MSDU Capab = 0 */)
  53. pos[0] &= ~0x80;
  54. pos++;
  55. *pos++ = 0x00;
  56. }
  57. *aad_len = pos - aad;
  58. os_memcpy(nonce + 1, hdr->addr2, ETH_ALEN);
  59. nonce[7] = data[7]; /* PN5 */
  60. nonce[8] = data[6]; /* PN4 */
  61. nonce[9] = data[5]; /* PN3 */
  62. nonce[10] = data[4]; /* PN2 */
  63. nonce[11] = data[1]; /* PN1 */
  64. nonce[12] = data[0]; /* PN0 */
  65. }
  66. static void xor_aes_block(u8 *dst, const u8 *src)
  67. {
  68. u32 *d = (u32 *) dst;
  69. u32 *s = (u32 *) src;
  70. *d++ ^= *s++;
  71. *d++ ^= *s++;
  72. *d++ ^= *s++;
  73. *d++ ^= *s++;
  74. }
  75. u8 * ccmp_decrypt(const u8 *tk, const struct ieee80211_hdr *hdr,
  76. const u8 *data, size_t data_len, size_t *decrypted_len)
  77. {
  78. u8 aad[2 + 30], nonce[13];
  79. size_t aad_len;
  80. u8 b[AES_BLOCK_SIZE], x[AES_BLOCK_SIZE], a[AES_BLOCK_SIZE];
  81. void *aes;
  82. const u8 *m, *mpos, *mic;
  83. size_t mlen, last;
  84. int i;
  85. u8 *plain, *ppos;
  86. u8 t[8];
  87. if (data_len < 8 + 8)
  88. return NULL;
  89. plain = os_malloc(data_len + AES_BLOCK_SIZE);
  90. if (plain == NULL)
  91. return NULL;
  92. aes = aes_encrypt_init(tk, 16);
  93. if (aes == NULL) {
  94. os_free(plain);
  95. return NULL;
  96. }
  97. m = data + 8;
  98. mlen = data_len - 8 - 8;
  99. last = mlen % AES_BLOCK_SIZE;
  100. os_memset(aad, 0, sizeof(aad));
  101. ccmp_aad_nonce(hdr, data, &aad[2], &aad_len, nonce);
  102. WPA_PUT_BE16(aad, aad_len);
  103. wpa_hexdump(MSG_EXCESSIVE, "CCMP AAD", &aad[2], aad_len);
  104. wpa_hexdump(MSG_EXCESSIVE, "CCMP nonce", nonce, 13);
  105. /* CCM: M=8 L=2, Adata=1, M' = (M-2)/2 = 3, L' = L-1 = 1 */
  106. /* A_i = Flags | Nonce N | Counter i */
  107. a[0] = 0x01; /* Flags = L' */
  108. os_memcpy(&a[1], nonce, 13);
  109. /* Decryption */
  110. mic = data + data_len - 8;
  111. wpa_hexdump(MSG_EXCESSIVE, "CCMP U", mic, 8);
  112. /* U = T XOR S_0; S_0 = E(K, A_0) */
  113. WPA_PUT_BE16(&a[14], 0);
  114. aes_encrypt(aes, a, x);
  115. for (i = 0; i < 8; i++)
  116. t[i] = mic[i] ^ x[i];
  117. wpa_hexdump(MSG_EXCESSIVE, "CCMP T", t, 8);
  118. /* plaintext = msg XOR (S_1 | S_2 | ... | S_n) */
  119. ppos = plain;
  120. mpos = m;
  121. for (i = 1; i <= mlen / AES_BLOCK_SIZE; i++) {
  122. WPA_PUT_BE16(&a[14], i);
  123. /* S_i = E(K, A_i) */
  124. aes_encrypt(aes, a, ppos);
  125. xor_aes_block(ppos, mpos);
  126. ppos += AES_BLOCK_SIZE;
  127. mpos += AES_BLOCK_SIZE;
  128. }
  129. if (last) {
  130. WPA_PUT_BE16(&a[14], i);
  131. aes_encrypt(aes, a, ppos);
  132. /* XOR zero-padded last block */
  133. for (i = 0; i < last; i++)
  134. *ppos++ ^= *mpos++;
  135. }
  136. wpa_hexdump(MSG_EXCESSIVE, "CCMP decrypted", plain, mlen);
  137. /* Authentication */
  138. /* B_0: Flags | Nonce N | l(m) */
  139. b[0] = 0x40 /* Adata */ | (3 /* M' */ << 3) | 1 /* L' */;
  140. os_memcpy(&b[1], nonce, 13);
  141. WPA_PUT_BE16(&b[14], mlen);
  142. wpa_hexdump(MSG_EXCESSIVE, "CCMP B_0", b, AES_BLOCK_SIZE);
  143. aes_encrypt(aes, b, x); /* X_1 = E(K, B_0) */
  144. wpa_hexdump(MSG_EXCESSIVE, "CCMP B_1", aad, AES_BLOCK_SIZE);
  145. xor_aes_block(aad, x);
  146. aes_encrypt(aes, aad, x); /* X_2 = E(K, X_1 XOR B_1) */
  147. wpa_hexdump(MSG_EXCESSIVE, "CCMP B_2", &aad[AES_BLOCK_SIZE],
  148. AES_BLOCK_SIZE);
  149. xor_aes_block(&aad[AES_BLOCK_SIZE], x);
  150. aes_encrypt(aes, &aad[AES_BLOCK_SIZE], x); /* X_3 = E(K, X_2 XOR B_2)
  151. */
  152. ppos = plain;
  153. for (i = 0; i < mlen / AES_BLOCK_SIZE; i++) {
  154. /* X_i+1 = E(K, X_i XOR B_i) */
  155. xor_aes_block(x, ppos);
  156. ppos += AES_BLOCK_SIZE;
  157. aes_encrypt(aes, x, x);
  158. }
  159. if (last) {
  160. /* XOR zero-padded last block */
  161. for (i = 0; i < last; i++)
  162. x[i] ^= *ppos++;
  163. aes_encrypt(aes, x, x);
  164. }
  165. aes_encrypt_deinit(aes);
  166. if (os_memcmp(x, t, 8) != 0) {
  167. u16 seq_ctrl = le_to_host16(hdr->seq_ctrl);
  168. wpa_printf(MSG_INFO, "Invalid CCMP MIC in frame: A1=" MACSTR
  169. " A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u",
  170. MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
  171. MAC2STR(hdr->addr3),
  172. WLAN_GET_SEQ_SEQ(seq_ctrl),
  173. WLAN_GET_SEQ_FRAG(seq_ctrl));
  174. wpa_hexdump(MSG_DEBUG, "CCMP decrypted", plain, mlen);
  175. os_free(plain);
  176. return NULL;
  177. }
  178. *decrypted_len = mlen;
  179. return plain;
  180. }
  181. void ccmp_get_pn(u8 *pn, const u8 *data)
  182. {
  183. pn[0] = data[7]; /* PN5 */
  184. pn[1] = data[6]; /* PN4 */
  185. pn[2] = data[5]; /* PN3 */
  186. pn[3] = data[4]; /* PN2 */
  187. pn[4] = data[1]; /* PN1 */
  188. pn[5] = data[0]; /* PN0 */
  189. }
  190. u8 * ccmp_encrypt(const u8 *tk, u8 *frame, size_t len, size_t hdrlen, u8 *qos,
  191. u8 *pn, int keyid, size_t *encrypted_len)
  192. {
  193. u8 aad[2 + 30], nonce[13];
  194. size_t aad_len;
  195. u8 b[AES_BLOCK_SIZE], x[AES_BLOCK_SIZE], a[AES_BLOCK_SIZE];
  196. void *aes;
  197. u8 *crypt, *pos, *ppos, *mpos;
  198. size_t plen, last;
  199. struct ieee80211_hdr *hdr;
  200. int i;
  201. if (len < hdrlen || hdrlen < 24)
  202. return NULL;
  203. plen = len - hdrlen;
  204. last = plen % AES_BLOCK_SIZE;
  205. crypt = os_malloc(hdrlen + 8 + plen + 8 + AES_BLOCK_SIZE);
  206. if (crypt == NULL)
  207. return NULL;
  208. os_memcpy(crypt, frame, hdrlen);
  209. hdr = (struct ieee80211_hdr *) crypt;
  210. hdr->frame_control |= host_to_le16(WLAN_FC_ISWEP);
  211. pos = crypt + hdrlen;
  212. *pos++ = pn[5]; /* PN0 */
  213. *pos++ = pn[4]; /* PN1 */
  214. *pos++ = 0x00; /* Rsvd */
  215. *pos++ = 0x20 | (keyid << 6);
  216. *pos++ = pn[3]; /* PN2 */
  217. *pos++ = pn[2]; /* PN3 */
  218. *pos++ = pn[1]; /* PN4 */
  219. *pos++ = pn[0]; /* PN5 */
  220. aes = aes_encrypt_init(tk, 16);
  221. if (aes == NULL) {
  222. os_free(crypt);
  223. return NULL;
  224. }
  225. os_memset(aad, 0, sizeof(aad));
  226. ccmp_aad_nonce(hdr, crypt + hdrlen, &aad[2], &aad_len, nonce);
  227. WPA_PUT_BE16(aad, aad_len);
  228. wpa_hexdump(MSG_EXCESSIVE, "CCMP AAD", &aad[2], aad_len);
  229. wpa_hexdump(MSG_EXCESSIVE, "CCMP nonce", nonce, 13);
  230. /* Authentication */
  231. /* B_0: Flags | Nonce N | l(m) */
  232. b[0] = 0x40 /* Adata */ | (3 /* M' */ << 3) | 1 /* L' */;
  233. os_memcpy(&b[1], nonce, 13);
  234. WPA_PUT_BE16(&b[14], plen);
  235. wpa_hexdump(MSG_EXCESSIVE, "CCMP B_0", b, AES_BLOCK_SIZE);
  236. aes_encrypt(aes, b, x); /* X_1 = E(K, B_0) */
  237. wpa_hexdump(MSG_EXCESSIVE, "CCMP B_1", aad, AES_BLOCK_SIZE);
  238. xor_aes_block(aad, x);
  239. aes_encrypt(aes, aad, x); /* X_2 = E(K, X_1 XOR B_1) */
  240. wpa_hexdump(MSG_EXCESSIVE, "CCMP B_2", &aad[AES_BLOCK_SIZE],
  241. AES_BLOCK_SIZE);
  242. xor_aes_block(&aad[AES_BLOCK_SIZE], x);
  243. aes_encrypt(aes, &aad[AES_BLOCK_SIZE], x); /* X_3 = E(K, X_2 XOR B_2)
  244. */
  245. ppos = frame + hdrlen;
  246. for (i = 0; i < plen / AES_BLOCK_SIZE; i++) {
  247. /* X_i+1 = E(K, X_i XOR B_i) */
  248. xor_aes_block(x, ppos);
  249. ppos += AES_BLOCK_SIZE;
  250. aes_encrypt(aes, x, x);
  251. }
  252. if (last) {
  253. /* XOR zero-padded last block */
  254. for (i = 0; i < last; i++)
  255. x[i] ^= *ppos++;
  256. aes_encrypt(aes, x, x);
  257. }
  258. /* Encryption */
  259. /* CCM: M=8 L=2, Adata=1, M' = (M-2)/2 = 3, L' = L-1 = 1 */
  260. /* A_i = Flags | Nonce N | Counter i */
  261. a[0] = 0x01; /* Flags = L' */
  262. os_memcpy(&a[1], nonce, 13);
  263. ppos = crypt + hdrlen + 8;
  264. /* crypt = msg XOR (S_1 | S_2 | ... | S_n) */
  265. mpos = frame + hdrlen;
  266. for (i = 1; i <= plen / AES_BLOCK_SIZE; i++) {
  267. WPA_PUT_BE16(&a[14], i);
  268. /* S_i = E(K, A_i) */
  269. aes_encrypt(aes, a, ppos);
  270. xor_aes_block(ppos, mpos);
  271. ppos += AES_BLOCK_SIZE;
  272. mpos += AES_BLOCK_SIZE;
  273. }
  274. if (last) {
  275. WPA_PUT_BE16(&a[14], i);
  276. aes_encrypt(aes, a, ppos);
  277. /* XOR zero-padded last block */
  278. for (i = 0; i < last; i++)
  279. *ppos++ ^= *mpos++;
  280. }
  281. wpa_hexdump(MSG_EXCESSIVE, "CCMP T", x, 8);
  282. /* U = T XOR S_0; S_0 = E(K, A_0) */
  283. WPA_PUT_BE16(&a[14], 0);
  284. aes_encrypt(aes, a, b);
  285. for (i = 0; i < 8; i++)
  286. ppos[i] = x[i] ^ b[i];
  287. wpa_hexdump(MSG_EXCESSIVE, "CCMP U", ppos, 8);
  288. wpa_hexdump(MSG_EXCESSIVE, "CCMP encrypted", crypt + hdrlen + 8, plen);
  289. aes_encrypt_deinit(aes);
  290. *encrypted_len = hdrlen + 8 + plen + 8;
  291. return crypt;
  292. }