diff --git a/Makefile.am b/Makefile.am index 2b97979f..57c9edd2 100644 --- a/Makefile.am +++ b/Makefile.am @@ -48,6 +48,7 @@ src_iwd_SOURCES = src/main.c linux/nl80211.h linux/kdbus.h \ src/dbus.h src/dbus.c \ src/manager.h src/manager.c \ src/crypto.h src/crypto.c \ + src/mpdu.h src/mpdu.c \ iwd.h src_iwd_LDADD = ell/libell-internal.la diff --git a/src/mpdu.c b/src/mpdu.c new file mode 100644 index 00000000..df4da93d --- /dev/null +++ b/src/mpdu.c @@ -0,0 +1,166 @@ +/* + * + * Wireless daemon for Linux + * + * Copyright (C) 2014 Intel Corporation. All rights reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include +#endif + +#include + +#include "mpdu.h" + +static inline bool next_byte(const unsigned char *mpdu, int len, + int *offset, unsigned char *holder) +{ + if (len == *offset) + return false; + + *holder = mpdu[*offset]; + *offset = *offset + 1; + + return true; +} + +static inline bool next_2bytes(const unsigned char *mpdu, int len, + int *offset, uint16_t *holder) +{ + if (len == *offset || len < *offset + 2) + return false; + + *holder = L_LE16_TO_CPU(*((uint16_t *) mpdu+*offset)); + *offset = *offset + 2; + + return true; +} + +static inline bool next_data(const unsigned char *mpdu, int len, + int *offset, unsigned char *holder, int t_len) +{ + int i; + + if (len == *offset || len < *offset + t_len) + return false; + + for (i = 0; i < t_len; i++) + holder[i] = mpdu[*offset+i]; + + *offset += t_len; + + return true; +} + +static bool decode_mgmt_header(const unsigned char *mpdu, int len, + int *offset, struct mpdu *out) +{ + if (!next_2bytes(mpdu, len, offset, &out->mgmt_hdr.duration)) + return false; + + if (!next_data(mpdu, len, offset, out->mgmt_hdr.address_1, 6)) + return false; + + if (!next_data(mpdu, len, offset, out->mgmt_hdr.address_2, 6)) + return false; + + if (!next_data(mpdu, len, offset, out->mgmt_hdr.address_3, 6)) + return false; + + if (!next_2bytes(mpdu, len, offset, &out->mgmt_hdr.sequence_control)) + return false; + + if (out->fc.order) + *offset += sizeof(uint32_t); /* Skipping ht_control for now */ + + return true; +} + +static bool decode_authentication_mgmt_mpdu(const unsigned char *mpdu, + int len, int *offset, struct mpdu *out) +{ + if (!next_2bytes(mpdu, len, offset, &out->auth.algorithm)) + return false; + + if (!next_2bytes(mpdu, len, offset, &out->auth.transaction_sequence)) + return false; + + if (!next_2bytes(mpdu, len, offset, &out->auth.status)) + return false; + + if (out->auth.algorithm == MPDU_AUTH_ALGO_SK) { + if (out->auth.transaction_sequence < 2 && + out->auth.transaction_sequence > 3) + return true; + + if (!next_byte(mpdu, len, offset, + &out->auth.challenge_text_len)) + return false; + + if (!next_data(mpdu, len, offset, out->auth.challenge_text, + out->auth.challenge_text_len)) + return false; + } + + return true; +} + +static bool decode_deauthentication_mgmt_mpdu(const unsigned char *mpdu, + int len, int *offset, struct mpdu *out) +{ + return next_2bytes(mpdu, len, offset, &out->deauth.reason_code); +} + +static bool decode_mgmt_mpdu(const unsigned char *mpdu, int len, + int *offset, struct mpdu *out) +{ + if (!decode_mgmt_header(mpdu, len, offset, out)) + return false; + + switch (out->fc.subtype) { + case MPDU_MGMT_TYPE_AUTHENTICATION: + return decode_authentication_mgmt_mpdu(mpdu, len, offset, out); + case MPDU_MGMT_TYPE_DEAUTHENTICATION: + return decode_deauthentication_mgmt_mpdu(mpdu, len, offset, out); + default: + return false; + } + + return true; +} + +bool mpdu_decode(const unsigned char *mpdu, int len, struct mpdu *out) +{ + int offset = 0; + + if (!mpdu || !out) + return false; + + if (!next_2bytes(mpdu, len, &offset, &out->fc.content)) + return false; + + switch (out->fc.type) { + case MPDU_TYPE_MANAGEMENT: + return decode_mgmt_mpdu(mpdu, len, &offset, out); + default: + return false; + } + + return true; +} diff --git a/src/mpdu.h b/src/mpdu.h new file mode 100644 index 00000000..8a177a31 --- /dev/null +++ b/src/mpdu.h @@ -0,0 +1,97 @@ +/* + * + * Wireless daemon for Linux + * + * Copyright (C) 2014 Intel Corporation. All rights reserved. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2.1 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#include +#include + +enum mpdu_type { + MPDU_TYPE_MANAGEMENT = 0, +}; + +enum mpdu_mgmt_type { + MPDU_MGMT_TYPE_AUTHENTICATION = 0xB, + MPDU_MGMT_TYPE_DEAUTHENTICATION = 0xC, +}; + +enum mpdu_authentication_algorithm_number { + MPDU_AUTH_ALGO_OPEN = 0, + MPDU_AUTH_ALGO_SK, +}; + +struct mpdu_fc { + union { + struct { + uint16_t protocol_version:2; + uint16_t type:2; + uint16_t subtype:4; + uint16_t to_ds:1; + uint16_t from_ds:1; + uint16_t more_fragments:1; + uint16_t retry:1; + uint16_t power_mgmt:1; + uint16_t more_data:1; + uint16_t protected_frame:1; + uint16_t order:1; + }; + uint16_t content; + }; +}; + +struct mpdu_mgmt_header { + uint16_t duration; + unsigned char address_1[6]; + unsigned char address_2[6]; + unsigned char address_3[6]; + union { + struct { + uint16_t fragment_number:4; + uint16_t sequence_number:12; + }; + uint16_t sequence_control; + }; + uint32_t ht_control; /* ToDo? */ +}; + +struct mpdu_authentication { + uint16_t algorithm; + uint16_t transaction_sequence; + uint16_t status; + uint8_t challenge_text_len; + unsigned char challenge_text[253]; + /* ToDo: FT and SAE parts? */ +}; + +struct mpdu_deauthentication { + uint16_t reason_code; + /* ToDo: Vendor specific IE? MME? */ +}; + +struct mpdu { + struct mpdu_fc fc; + struct mpdu_mgmt_header mgmt_hdr; + union { + struct mpdu_authentication auth; + struct mpdu_deauthentication deauth; + }; +}; + +bool mpdu_decode(const unsigned char *mpdu, int len, struct mpdu *out);