diff --git a/src/1905_tlvs.h b/src/1905_tlvs.h
index ae83679522cca5ab6d23b7d3e228beca85747fb8..3498cab7c622487e5e09689bc556217dac9c570b 100644
--- a/src/1905_tlvs.h
+++ b/src/1905_tlvs.h
@@ -11,6 +11,9 @@
#define ETHERTYPE_1905 0x893a
#define ETHERTYPE_LLDP 0x88cc
+extern uint8_t MCAST_1905[];
+extern uint8_t MCAST_LLDP[];
+
/* 1905 CMDU types */
#define CMDU_TYPE_TOPOLOGY_DISCOVERY 0x0000
@@ -98,6 +101,7 @@
#define IEEE80211_FREQUENCY_BAND_2_4_GHZ (0x00)
#define IEEE80211_FREQUENCY_BAND_5_GHZ (0x01)
#define IEEE80211_FREQUENCY_BAND_60_GHZ (0x02)
+#define IEEE80211_FREQUENCY_BAND_UNKNOWN (0xff)
/* IEEE80211 roles */
@@ -293,8 +297,7 @@ struct tlv_supported_band {
/* TLV: wsc */
struct tlv_wsc {
- uint16_t framelen;
- uint8_t frame[];
+ uint8_t frame[0];
} __attribute__((packed));
diff --git a/src/Makefile b/src/Makefile
index da69bbe3fbaedefcf2a9d2ed6efa09ab48a561e9..38b8fd66cda7af20b7d7897d02c28bf53142374b 100644
--- a/src/Makefile
+++ b/src/Makefile
@@ -6,9 +6,11 @@ OBJS = cmdu.o \
cmdu_input.o \
cmdu_output.o \
config.o \
+ cryptutil.o \
i1905.o \
i1905_dm.o \
i1905_al.o \
+ i1905_wsc.o \
i1905_extension.o \
i1905_ubus.o \
main.o \
@@ -24,6 +26,7 @@ LIBS += -leasy
LIBS += -lwifi-6
LIBS += -rdynamic -ldl
LIBS += -lnl-3 -lnl-genl-3 -lnl-route-3
+LIBS += -lssl -lcrypto
extmod_subdirs ?= $(wildcard extmodules/*)
diff --git a/src/cmdu_input.c b/src/cmdu_input.c
index b5585d37dfc1976731c57fd03be1023516228d6b..5922f103fafaf83cd18619e527b4f429fcad2b8d 100644
--- a/src/cmdu_input.c
+++ b/src/cmdu_input.c
@@ -474,6 +474,239 @@ out_send:
return ret;
}
+int i1905_send_ap_autoconfig_search(struct i1905_interface_private *pif,
+ uint8_t freqband)
+{
+ struct i1905_interface *iface = i1905_interface_priv(pif);
+ struct i1905_selfdevice *self = (struct i1905_selfdevice *)iface->device;
+ struct i1905_interface *ifs;
+ bool forall_bands = true;
+
+
+
+ if (!IS_MEDIA_WIFI(iface->media)) {
+ fprintf(stderr, "%s is not a WiFi interface\n", iface->ifname);
+ return -1;
+ }
+
+ if (freqband >= IEEE80211_FREQUENCY_BAND_2_4_GHZ
+ && freqband <= IEEE80211_FREQUENCY_BAND_60_GHZ
+ /* && wifi_is_band_supported(iface->ifname, freqband) */) {
+
+ forall_bands = false;
+ }
+
+
+ //TODO: check if reliable mcast for search needed ????
+ list_for_each_entry(ifs, &self->iflist, list) {
+ struct cmdu_buff *frm = NULL;
+ struct tlv *t;
+ int ret = 0;
+
+
+ if (!strncmp(iface->ifname, ifs->ifname, 16) &&
+ !memcmp(iface->macaddr, ifs->macaddr, 6)) {
+
+ continue;
+ }
+
+
+ frm = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_SEARCH, 0);
+ if (!frm) {
+ fprintf(stderr, "%s: -ENOMEM\n", __func__);
+ return -1;
+ }
+
+ t = tlv_alloc(256);
+ if (!t) {
+ cmdu_free(frm);
+ fprintf(stderr, "%s: -ENOMEM\n", __func__);
+ return -1;
+ }
+
+ t->type = TLV_TYPE_AL_MAC_ADDRESS_TYPE;
+ t->len = 6;
+ memcpy(t->data, iface->aladdr, 6);
+
+ ret = cmdu_put_tlv(frm, t);
+ if (ret) {
+ fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
+ cmdu_free(frm);
+ tlv_free_linear(t);
+ return -1;
+ }
+
+ tlv_zero(t); /* reuse the tlv buffer */
+
+ t->type = TLV_TYPE_SEARCHED_ROLE;
+ t->len = 1;
+ t->data[0] = 0x00;
+ ret = cmdu_put_tlv(frm, t);
+ if (ret) {
+ fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
+ cmdu_free(frm);
+ tlv_free_linear(t);
+ return -1;
+ }
+
+ tlv_zero(t);
+
+ t->type = TLV_TYPE_AUTOCONFIG_FREQ_BAND;
+ t->len = 1;
+ t->data[0] = freqband;
+
+ ret = cmdu_put_tlv(frm, t);
+ if (ret) {
+ fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
+ cmdu_free(frm);
+ tlv_free_linear(t);
+ return -1;
+ }
+
+ cmdu_put_eom(frm);
+
+ ret = i1905_send_cmdu(pif, MCAST_1905, iface->aladdr,
+ ETHERTYPE_1905, frm);
+ if (ret) {
+ fprintf(stderr, "Error sending AP_AUTOCONFIG_SEARCH\n");
+ }
+ }
+
+ return 0;
+}
+
+int i1905_send_ap_autoconfig_response(struct i1905_interface_private *pif,
+ uint8_t *dest, uint8_t band, uint16_t mid)
+{
+ struct i1905_interface *iface = i1905_interface_priv(pif);
+ //struct i1905_selfdevice *self = (struct i1905_selfdevice *)iface->device;
+ //struct i1905_device *rdev = NULL;
+ struct cmdu_buff *resp;
+ struct tlv *t;
+ int ret;
+
+
+
+ resp = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_RESPONSE, mid);
+ if (!resp) {
+ fprintf(stderr, "%s: -ENOMEM\n", __func__);
+ return -1;
+ }
+
+ t = tlv_alloc(256);
+ if (!t) {
+ cmdu_free(resp);
+ fprintf(stderr, "%s: -ENOMEM\n", __func__);
+ return -1;
+ }
+
+ t->type = TLV_TYPE_SUPPORTED_ROLE;
+ t->len = 1;
+ t->data[0] = 0x00;
+ ret = cmdu_put_tlv(resp, t);
+ if (ret) {
+ fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
+ cmdu_free(resp);
+ tlv_free_linear(t);
+ return -1;
+ }
+
+ tlv_zero(t);
+
+ t->type = TLV_TYPE_SUPPORTED_FREQ_BAND;
+ t->len = 1;
+ t->data[0] = band;
+
+ ret = cmdu_put_tlv(resp, t);
+ if (ret) {
+ fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
+ cmdu_free(resp);
+ tlv_free_linear(t);
+ return -1;
+ }
+
+ cmdu_put_eom(resp);
+
+ ret = i1905_send_cmdu(pif, dest, iface->aladdr, ETHERTYPE_1905, resp);
+ if (ret) {
+ fprintf(stderr, "Error sending AP_AUTOCONFIG_RESPONSE\n");
+ }
+
+ return ret;
+}
+
+int i1905_send_ap_autoconfig_wsc_m1(struct i1905_interface_private *pif,
+ uint8_t *dest)
+{
+ struct i1905_interface *iface = i1905_interface_priv(pif);
+ //struct i1905_selfdevice *self = (struct i1905_selfdevice *)iface->device;
+ //struct i1905_device *rdev = NULL;
+ struct cmdu_buff *frm;
+ struct tlv *t;
+ int ret;
+
+ uint8_t *m1;
+ uint16_t m1_size = 0;
+ void *key;
+ //uint8_t macaddr[6] = {0};
+ //uint16_t auth = WPA2PSK;
+ //uint16_t enc = CCMP;
+ struct wps_data wps;
+
+
+ frm = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_WSC, 0);
+ if (!frm) {
+ fprintf(stderr, "%s: -ENOMEM\n", __func__);
+ return -1;
+ }
+
+ wps.auth_types = pif->wsc->auth_types;
+ wps.enc_types = pif->wsc->enc_types;
+ wps.band = pif->wsc->band;
+
+ ret = wsc_build_m1(iface->ifname, &m1, &m1_size, &key, &wps);
+ if (ret) {
+ fprintf(stderr, "Failed to build WSC M1 frame!\n");
+ cmdu_free(frm);
+ return ret;
+ }
+
+ //TODO: improve wsc data and state -- maybe move to iface
+ if (pif->wsc) {
+ pif->wsc->last_msg = m1;
+ pif->wsc->last_msglen = m1_size;
+ pif->wsc->key = key;
+ }
+
+ //TODO: avoid following alloc and memcpy
+ t = tlv_alloc(FRAG_DATA_SIZE_TLV);
+ if (!t) {
+ cmdu_free(frm);
+ fprintf(stderr, "%s: -ENOMEM\n", __func__);
+ return -1;
+ }
+
+ t->type = TLV_TYPE_WSC;
+ t->len = m1_size;
+ memcpy(t->data, m1, m1_size);
+ ret = cmdu_put_tlv(frm, t);
+ if (ret) {
+ fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
+ cmdu_free(frm);
+ tlv_free_linear(t);
+ return -1;
+ }
+
+ cmdu_put_eom(frm);
+
+ ret = i1905_send_cmdu(pif, dest, iface->aladdr, ETHERTYPE_1905, frm);
+ if (ret) {
+ fprintf(stderr, "Error sending AP_AUTOCONFIG_WSC_M1\n");
+ }
+
+ return ret;
+}
+
int i1905_handle_topology_discovery(struct i1905_interface_private *pif,
struct cmdu_buff *rxf)
{
@@ -757,15 +990,121 @@ int i1905_handle_link_metric_response(struct i1905_interface_private *pif,
int i1905_handle_ap_autoconfig_search(struct i1905_interface_private *pif,
struct cmdu_buff *rxf)
{
- //TODO
- return 0;
+ struct tlv_policy a_policy[] = {
+ [0] = { .type = TLV_TYPE_AL_MAC_ADDRESS_TYPE, .present = TLV_PRESENT_ONE },
+ [1] = { .type = TLV_TYPE_SEARCHED_ROLE, .present = TLV_PRESENT_ONE },
+ [2] = { .type = TLV_TYPE_AUTOCONFIG_FREQ_BAND, .present = TLV_PRESENT_ONE },
+ };
+ struct i1905_interface *iface = i1905_interface_priv(pif);
+ struct tlv_autoconfig_band *freq;
+ uint8_t aladdr_origin[6] = {0};
+ struct tlv *tv[3][16];
+ int ret;
+
+
+ fprintf(stderr, "%s -------------->\n", __func__);
+
+ cmdu_parse_tlvs(rxf, tv, a_policy, 3);
+
+ if (!tv[0][0] || !tv[1][0] || !tv[2][0])
+ return -1;
+
+
+ memcpy(aladdr_origin, tv[0][0]->data, tlv_length(tv[0][0]));
+ if (hwaddr_is_zero(aladdr_origin)) {
+ fprintf(stderr,
+ "%s: Discard ap-autoconfig search from aladdr = 0!\n",
+ __func__);
+
+ return -1;
+ }
+
+ if (tv[1][0]->data[0] != IEEE80211_ROLE_REGISTRAR) {
+ fprintf(stderr,
+ "%s: Discard ap-autoconfig search for role != registrar\n",
+ __func__);
+ return -1;
+ }
+
+ freq = (struct tlv_autoconfig_band *)tv[2][0]->data;
+ if (freq->band > IEEE80211_FREQUENCY_BAND_60_GHZ) {
+ fprintf(stderr,
+ "%s: Discard ap-autoconfig search for invalid WiFi band\n",
+ __func__);
+ return -1;
+ }
+
+ if (i1905_has_registrar(pif, freq->band)) {
+ ret = i1905_send_ap_autoconfig_response(pif, aladdr_origin,
+ freq->band,
+ cmdu_get_mid(rxf));
+ } else {
+ ret = i1905_send_cmdu(pif, MCAST_1905, iface->aladdr,
+ ETHERTYPE_1905, rxf);
+ if (ret)
+ fprintf(stderr, "Error sending AP_AUTOCONFIG_SEARCH\n");
+ }
+
+ return ret;
}
int i1905_handle_ap_autoconfig_response(struct i1905_interface_private *pif,
struct cmdu_buff *rxf)
{
- //TODO
- return 0;
+ struct tlv_policy a_policy[] = {
+ [0] = { .type = TLV_TYPE_SUPPORTED_ROLE, .present = TLV_PRESENT_ONE },
+ [1] = { .type = TLV_TYPE_SUPPORTED_FREQ_BAND, .present = TLV_PRESENT_ONE },
+ };
+ struct i1905_interface *iface = i1905_interface_priv(pif);
+ struct i1905_selfdevice *self = (struct i1905_selfdevice *)iface->device;
+ struct tlv_supported_band *freq;
+ struct i1905_interface *ifs;
+ struct tlv *tv[2][16];
+ int ret;
+
+
+ fprintf(stderr, "%s -------------->\n", __func__);
+
+ cmdu_parse_tlvs(rxf, tv, a_policy, 2);
+
+ if (!tv[0][0] || !tv[1][0])
+ return -1;
+
+
+ if (tv[0][0]->data[0] != IEEE80211_ROLE_REGISTRAR) {
+ fprintf(stderr,
+ "%s: Discard ap-autoconfig response for role != registrar\n",
+ __func__);
+ return -1;
+ }
+
+ freq = (struct tlv_supported_band *)tv[1][0]->data;
+ if (freq->band > IEEE80211_FREQUENCY_BAND_60_GHZ) {
+ fprintf(stderr,
+ "%s: Discard ap-autoconfig response for invalid WiFi band\n",
+ __func__);
+ return -1;
+ }
+
+ //TODO: ubus notify discovered registrar
+ //If this is in response to discover_registrar(), then set
+ //registrar_established.
+ //Else, send WSC-M1 for each unconfigured i1905 WiFi radios
+
+ list_for_each_entry(ifs, &self->iflist, list) {
+ if (IS_MEDIA_WIFI(ifs->media) &&
+ !((struct i1905_interface_private *)ifs->priv)->configured) {
+
+ //TODO: check band in supported-bands
+ ret = i1905_send_ap_autoconfig_wsc_m1(ifs->priv, rxf->origin);
+ if (ret) {
+ fprintf(stderr, "Error sending AP_AUTOCONFIG_WSC_M1\n");
+ break;
+ }
+ }
+ }
+
+ return ret;
}
int i1905_handle_ap_autoconfig_renew(struct i1905_interface_private *pif,
@@ -775,11 +1114,95 @@ int i1905_handle_ap_autoconfig_renew(struct i1905_interface_private *pif,
return 0;
}
+static int i1905_wsc_process_m1(struct i1905_interface_private *pif,
+ uint8_t *msg, uint16_t msglen)
+{
+ struct i1905_interface *iface = i1905_interface_priv(pif);
+ //struct i1905_interface_private_wsc *wsc = pif->wsc;
+ int ret;
+ uint8_t *m2;
+ uint16_t m2_size;
+ struct wps_credential cred;
+
+
+ //ret = wsc_process_m1(iface->ifname, msg, msglen);
+
+ //TODO: get cred from config file
+ //i1905_interface_credential(&cred);
+
+
+ ret = wsc_build_m2(msg, msglen, &m2, &m2_size, &cred);
+ if (ret) {
+ fprintf(stderr, "Error sending WSC M2 for '%s'\n", iface->ifname);
+ return ret;
+ }
+
+ return 0;
+}
+
+static int i1905_wsc_process_m2(struct i1905_interface_private *pif,
+ uint8_t *msg, uint16_t msglen)
+{
+ struct i1905_interface *iface = i1905_interface_priv(pif);
+ struct i1905_interface_private_wsc *wsc = pif->wsc;
+ struct wps_credential out = {0};
+ int ret;
+
+
+ ret = wsc_process_m2(iface->ifname, wsc->key, wsc->last_msg,
+ wsc->last_msglen, msg, msglen, &out);
+ if (ret) {
+ fprintf(stderr, "Error processing WSC M2 for '%s'\n", iface->ifname);
+ return ret;
+ }
+
+ /* TODO: update wireless config */
+
+
+ return 0;
+}
+
int i1905_handle_ap_autoconfig_wsc(struct i1905_interface_private *pif,
struct cmdu_buff *rxf)
{
- //TODO
- return 0;
+ struct tlv_policy a_policy[] = {
+ [0] = { .type = TLV_TYPE_WSC, .present = TLV_PRESENT_ONE },
+ };
+ //struct i1905_interface *iface = i1905_interface_priv(pif);
+ //struct i1905_selfdevice *self = (struct i1905_selfdevice *)iface->device;
+ struct tlv *tv[1][16];
+ uint8_t wsc_msgtype;
+ uint16_t msglen;
+ uint8_t *msg;
+ int ret = 0;
+
+
+ fprintf(stderr, "%s -------------->\n", __func__);
+
+ cmdu_parse_tlvs(rxf, tv, a_policy, 1);
+
+ if (!tv[0][0])
+ return -1;
+
+ msg = tv[0][0]->data;
+ msglen = tlv_length(tv[0][0]);
+
+ wsc_msgtype = wsc_get_message_type(msg, msglen);
+ switch (wsc_msgtype) {
+ case WPS_M1:
+ fprintf(stderr, "Received WPS M1\n");
+ ret = i1905_wsc_process_m1(pif, msg, msglen);
+ break;
+ case WPS_M2:
+ fprintf(stderr, "Received WPS M2\n");
+ ret = i1905_wsc_process_m2(pif, msg, msglen);
+ break;
+ default:
+ fprintf(stderr, "Received WPS msgtype %u\n", wsc_msgtype);
+ return -1;
+ }
+
+ return ret;
}
int i1905_handle_pbc_notification(struct i1905_interface_private *pif,
diff --git a/src/cryptutil.c b/src/cryptutil.c
new file mode 100644
index 0000000000000000000000000000000000000000..7d470a6d5fc0de757d4fdea556373dd68205063e
--- /dev/null
+++ b/src/cryptutil.c
@@ -0,0 +1,809 @@
+/*
+ * cryptutil.c - crypto utility functions
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+
+#include <openssl/err.h>
+#include <openssl/bn.h>
+#include <openssl/evp.h>
+#include <openssl/dh.h>
+#include <openssl/hmac.h>
+
+#include "cryptutil.h"
+
+#ifndef AES_BLOCK_SIZE
+#define AES_BLOCK_SIZE 16
+#endif
+
+static unsigned char dh1536_p[] = {
+ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC9, 0x0F, 0xDA, 0xA2,
+ 0x21, 0x68, 0xC2, 0x34, 0xC4, 0xC6, 0x62, 0x8B, 0x80, 0xDC, 0x1C, 0xD1,
+ 0x29, 0x02, 0x4E, 0x08, 0x8A, 0x67, 0xCC, 0x74, 0x02, 0x0B, 0xBE, 0xA6,
+ 0x3B, 0x13, 0x9B, 0x22, 0x51, 0x4A, 0x08, 0x79, 0x8E, 0x34, 0x04, 0xDD,
+ 0xEF, 0x95, 0x19, 0xB3, 0xCD, 0x3A, 0x43, 0x1B, 0x30, 0x2B, 0x0A, 0x6D,
+ 0xF2, 0x5F, 0x14, 0x37, 0x4F, 0xE1, 0x35, 0x6D, 0x6D, 0x51, 0xC2, 0x45,
+ 0xE4, 0x85, 0xB5, 0x76, 0x62, 0x5E, 0x7E, 0xC6, 0xF4, 0x4C, 0x42, 0xE9,
+ 0xA6, 0x37, 0xED, 0x6B, 0x0B, 0xFF, 0x5C, 0xB6, 0xF4, 0x06, 0xB7, 0xED,
+ 0xEE, 0x38, 0x6B, 0xFB, 0x5A, 0x89, 0x9F, 0xA5, 0xAE, 0x9F, 0x24, 0x11,
+ 0x7C, 0x4B, 0x1F, 0xE6, 0x49, 0x28, 0x66, 0x51, 0xEC, 0xE4, 0x5B, 0x3D,
+ 0xC2, 0x00, 0x7C, 0xB8, 0xA1, 0x63, 0xBF, 0x05, 0x98, 0xDA, 0x48, 0x36,
+ 0x1C, 0x55, 0xD3, 0x9A, 0x69, 0x16, 0x3F, 0xA8, 0xFD, 0x24, 0xCF, 0x5F,
+ 0x83, 0x65, 0x5D, 0x23, 0xDC, 0xA3, 0xAD, 0x96, 0x1C, 0x62, 0xF3, 0x56,
+ 0x20, 0x85, 0x52, 0xBB, 0x9E, 0xD5, 0x29, 0x07, 0x70, 0x96, 0x96, 0x6D,
+ 0x67, 0x0C, 0x35, 0x4E, 0x4A, 0xBC, 0x98, 0x04, 0xF1, 0x74, 0x6C, 0x08,
+ 0xCA, 0x23, 0x73, 0x27, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
+};
+
+static unsigned char dh1536_g[] = { 0x02 };
+
+
+int PLATFORM_GENERATE_DH_KEY_PAIR(uint8_t **priv, uint16_t *priv_len,
+ uint8_t **pub, uint16_t *pub_len)
+{
+ DH *dh;
+
+ if (priv == NULL || priv_len == NULL || pub == NULL || pub_len == NULL) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+
+ dh = DH_new();
+ if (dh == NULL) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ // Convert binary to BIGNUM format
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+ BIGNUM * dhp_bn, *dhg_bn;
+
+ dhp_bn = BN_bin2bn(dh1536_p, sizeof(dh1536_p), NULL);
+ dhg_bn = BN_bin2bn(dh1536_g, sizeof(dh1536_g), NULL);
+
+ if (dhp_bn == NULL || dhg_bn == NULL || !DH_set0_pqg(dh, dhp_bn, NULL, dhg_bn)) {
+ DH_free(dh);
+ BN_free(dhp_bn);
+ BN_free(dhg_bn);
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+#else
+ dh->p = BN_bin2bn(dh1536_p, sizeof(dh1536_p), NULL);
+ if (dh->p == NULL) {
+ DH_free(dh);
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ dh->g = BN_bin2bn(dh1536_g, sizeof(dh1536_g), NULL);
+ if (dh->g == NULL) {
+ DH_free(dh);
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+#endif
+ // Obtain key pair
+ //
+ if (DH_generate_key(dh) == 0) {
+ DH_free(dh);
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+ const BIGNUM * pv = DH_get0_priv_key(dh);
+ *priv_len = BN_num_bytes(pv);
+ *priv = (uint8_t *) malloc(sizeof(uint8_t) * (*priv_len + 1));
+ if (*priv == NULL) {
+ fprintf(stderr, "Out of memory!");
+ exit(ENOMEM);
+ }
+ BN_bn2bin(pv, *priv);
+
+ const BIGNUM *pu = DH_get0_pub_key(dh);
+ *pub_len = BN_num_bytes(pu);
+ *pub = (uint8_t *) malloc(sizeof(uint8_t) * (*pub_len + 1));
+ if (*pub == NULL) {
+ fprintf(stderr, "Out of memory!");
+ exit(ENOMEM);
+ }
+
+ BN_bn2bin(pu, *pub);
+#else
+ *priv_len = BN_num_bytes(dh->priv_key);
+ *priv = (uint8_t *) malloc(*priv_len);
+ if (*priv == NULL) {
+ fprintf(stderr, "Out of memory!");
+ exit(ENOMEM);
+ }
+
+ BN_bn2bin(dh->priv_key, *priv);
+
+ *pub_len = BN_num_bytes(dh->pub_key);
+ *pub = (uint8_t *) malloc(*pub_len);
+ if (*pub == NULL) {
+ fprintf(stderr, "Out of memory!");
+ exit(ENOMEM);
+ }
+
+ BN_bn2bin(dh->pub_key, *pub);
+#endif
+ DH_free(dh);
+ // NOTE: This internally frees "dh->p" and "dh->q", thus no need for us
+ // to do anything else.
+
+ return 0;
+}
+
+int PLATFORM_COMPUTE_DH_SHARED_SECRET(uint8_t **shared_secret,
+ uint16_t *shared_secret_len,
+ uint8_t *remote_pub,
+ uint16_t remote_pub_len,
+ uint8_t *local_priv,
+ uint8_t local_priv_len)
+{
+ BIGNUM *pub_key;
+
+ size_t rlen;
+ int keylen;
+
+ DH *dh;
+
+ if (NULL == shared_secret || NULL == shared_secret_len || NULL == remote_pub || NULL == local_priv) {
+ fprintf(stderr, "#### problem detected");
+ return 0;
+ }
+
+ dh = DH_new();
+ if (dh == NULL) {
+ fprintf(stderr, "#### problem detected");
+ return 0;
+ }
+ // Convert binary to BIGNUM format
+ //
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+ BIGNUM *dhp_bn, *dhg_bn, *priv_key;
+
+ dhp_bn = BN_bin2bn(dh1536_p, sizeof(dh1536_p), NULL);
+ dhg_bn = BN_bin2bn(dh1536_g, sizeof(dh1536_g), NULL);
+
+ if (dhp_bn == NULL || dhg_bn == NULL || !DH_set0_pqg(dh, dhp_bn, NULL, dhg_bn)) {
+ DH_free(dh);
+ BN_free(dhp_bn);
+ BN_free(dhg_bn);
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ pub_key = BN_bin2bn(remote_pub, remote_pub_len, NULL);
+ if (pub_key == NULL) {
+ DH_free(dh);
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ priv_key = BN_bin2bn(local_priv, local_priv_len, NULL);
+ if (priv_key == NULL) {
+ BN_clear_free(priv_key);
+ DH_free(dh);
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ DH_set0_key(dh, pub_key, priv_key);
+#else
+ dh->p = BN_bin2bn(dh1536_p, sizeof(dh1536_p), NULL);
+ if (dh->p == NULL) {
+ DH_free(dh);
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ dh->g = BN_bin2bn(dh1536_g, sizeof(dh1536_g), NULL);
+ if (dh->g == NULL) {
+ DH_free(dh);
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ pub_key = BN_bin2bn(remote_pub, remote_pub_len, NULL);
+ if (pub_key == NULL) {
+ DH_free(dh);
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ dh->priv_key = BN_bin2bn(local_priv, local_priv_len, NULL);
+ if (dh->priv_key == NULL) {
+ DH_free(dh);
+ fprintf(stderr, "#### problem detected");
+ return 0;
+ }
+#endif
+ // Allocate output buffer
+ //
+ rlen = DH_size(dh);
+ *shared_secret = (uint8_t *) malloc(rlen);
+ if (*shared_secret == NULL) {
+ fprintf(stderr, "Out of memory!");
+ exit(ENOMEM);
+ }
+
+
+ // Compute the shared secret and save it in the output buffer
+ //
+ keylen = DH_compute_key(*shared_secret, pub_key, dh);
+ if (keylen < 0) {
+ *shared_secret_len = 0;
+ free(*shared_secret);
+ *shared_secret = NULL;
+ BN_clear_free(pub_key);
+ DH_free(dh);
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ } else {
+ *shared_secret_len = (uint16_t) keylen;
+ }
+
+#if OPENSSL_VERSION_NUMBER < 0x1010000fL
+ BN_clear_free(pub_key);
+#endif
+ DH_free(dh);
+
+ return 0;
+}
+
+int PLATFORM_SHA256(size_t num_elem, const uint8_t *addr[], const size_t *len, uint8_t *digest)
+{
+ int res;
+ unsigned int mac_len;
+ EVP_MD_CTX *ctx;
+
+#if OPENSSL_VERSION_NUMBER >= 0x10100000L
+ ctx = EVP_MD_CTX_new();
+ if (!ctx) {
+ fprintf(stderr, "#### problem detected");
+ return 0;
+ }
+#else
+ EVP_MD_CTX ctx_aux;
+
+ ctx = &ctx_aux;
+
+ EVP_MD_CTX_init(ctx);
+#endif
+
+ res = 1;
+
+ if (!EVP_DigestInit_ex(ctx, EVP_sha256(), NULL)) {
+ res = 0;
+ }
+
+ if (res == 1) {
+ size_t i;
+
+ for (i = 0; i < num_elem; i++) {
+ if (!EVP_DigestUpdate(ctx, addr[i], len[i])) {
+ res = 0;
+ break;
+ }
+ }
+ }
+
+ if (res == 1) {
+ if (!EVP_DigestFinal(ctx, digest, &mac_len)) {
+ res = 0;
+ }
+ }
+#if OPENSSL_VERSION_NUMBER >= 0x10100000L
+ EVP_MD_CTX_free(ctx);
+#endif
+
+ return !res;
+}
+
+
+int PLATFORM_HMAC_SHA256(const uint8_t *key, size_t keylen, size_t num_elem,
+ const uint8_t *addr[], const size_t *len, uint8_t *hmac)
+{
+ HMAC_CTX *ctx;
+ size_t i;
+ unsigned int mdlen = 32;
+
+#if OPENSSL_VERSION_NUMBER >= 0x10100000L
+ ctx = HMAC_CTX_new();
+ if (!ctx) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+#else
+ HMAC_CTX ctx_aux;
+
+ ctx = &ctx_aux;
+
+ HMAC_CTX_init(ctx);
+#endif
+
+ HMAC_Init_ex(ctx, key, keylen, EVP_sha256(), NULL);
+
+ for (i = 0; i < num_elem; i++) {
+ HMAC_Update(ctx, addr[i], len[i]);
+ }
+
+ HMAC_Final(ctx, hmac, &mdlen);
+
+#if OPENSSL_VERSION_NUMBER >= 0x10100000L
+ HMAC_CTX_free(ctx);
+#else
+ HMAC_CTX_cleanup(ctx);
+#endif
+
+ return 0;
+}
+
+int PLATFORM_AES_ENCRYPT(uint8_t *key, uint8_t *iv, uint8_t *data, uint32_t data_len)
+{
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+ EVP_CIPHER_CTX * ctx;
+
+ int clen, len;
+ uint8_t buf[AES_BLOCK_SIZE];
+
+ ctx = EVP_CIPHER_CTX_new();
+ if (!ctx) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+
+ if (EVP_EncryptInit_ex(ctx, EVP_aes_128_cbc(), NULL, key, iv) != 1) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ EVP_CIPHER_CTX_set_padding(ctx, 0);
+
+ clen = data_len;
+ if (EVP_EncryptUpdate(ctx, data, &clen, data, data_len) != 1 || clen != (int)data_len) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+
+ len = sizeof(buf);
+ if (EVP_EncryptFinal_ex(ctx, buf, &len) != 1 || len != 0) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ EVP_CIPHER_CTX_free(ctx);
+#else
+ EVP_CIPHER_CTX ctx;
+
+ int clen, len;
+ uint8_t buf[AES_BLOCK_SIZE];
+
+ EVP_CIPHER_CTX_init(&ctx);
+
+ if (EVP_EncryptInit_ex(&ctx, EVP_aes_128_cbc(), NULL, key, iv) != 1) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ EVP_CIPHER_CTX_set_padding(&ctx, 0);
+
+ clen = data_len;
+ if (EVP_EncryptUpdate(&ctx, data, &clen, data, data_len) != 1 || clen != (int)data_len) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+
+ len = sizeof(buf);
+ if (EVP_EncryptFinal_ex(&ctx, buf, &len) != 1 || len != 0) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ EVP_CIPHER_CTX_cleanup(&ctx);
+#endif
+
+ return 0;
+}
+
+int PLATFORM_AES_DECRYPT(uint8_t *key, uint8_t *iv, uint8_t *data, uint32_t data_len)
+{
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+ EVP_CIPHER_CTX * ctx;
+
+ int plen, len;
+ uint8_t buf[AES_BLOCK_SIZE];
+
+ ctx = EVP_CIPHER_CTX_new();
+ if (!ctx) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+
+ if (EVP_DecryptInit_ex(ctx, EVP_aes_128_cbc(), NULL, key, iv) != 1) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ EVP_CIPHER_CTX_set_padding(ctx, 0);
+
+ plen = data_len;
+ if (EVP_DecryptUpdate(ctx, data, &plen, data, data_len) != 1 || plen != (int)data_len) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+
+ len = sizeof(buf);
+ if (EVP_DecryptFinal_ex(ctx, buf, &len) != 1 || len != 0) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ EVP_CIPHER_CTX_free(ctx);
+#else
+ EVP_CIPHER_CTX ctx;
+
+ int plen, len;
+ uint8_t buf[AES_BLOCK_SIZE];
+
+ EVP_CIPHER_CTX_init(&ctx);
+
+ if (EVP_DecryptInit_ex(&ctx, EVP_aes_128_cbc(), NULL, key, iv) != 1) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ EVP_CIPHER_CTX_set_padding(&ctx, 0);
+
+ plen = data_len;
+ if (EVP_DecryptUpdate(&ctx, data, &plen, data, data_len) != 1 || plen != (int)data_len) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+
+ len = sizeof(buf);
+ if (EVP_DecryptFinal_ex(&ctx, buf, &len) != 1 || len != 0) {
+ fprintf(stderr, "#### problem detected");
+ return -1;
+ }
+ EVP_CIPHER_CTX_cleanup(&ctx);
+#endif
+ return 0;
+}
+
+
+
+
+#if 0
+uint8_t PLATFORM_GENERATE_DH_KEY_PAIR(uint8_t **priv, uint16_t *priv_len,
+ uint8_t **pub, uint16_t *pub_len)
+{
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+ BIGNUM *dhp_bn, *dhg_bn;
+ const BIGNUM *pv;
+ const BIGNUM *pu;
+#endif
+ DH *dh;
+
+
+ if (!priv || !priv_len || !pub || !pub_len) {
+ fprintf(stderr, stderr, "invalid args\n");
+ return -1;
+ }
+
+ dh = DH_new();
+ if (!dh) {
+ fprintf(stderr, stderr, "-ENOMEM\n");
+ return -1;
+ }
+
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+ dhp_bn = BN_bin2bn(dh1536_p, sizeof(dh1536_p), NULL);
+ dhg_bn = BN_bin2bn(dh1536_g, sizeof(dh1536_g), NULL);
+
+ if (!dhp_bn || !dhg_bn || !DH_set0_pqg(dh, dhp_bn, NULL, dhg_bn)) {
+ fprintf(stderr, stderr, "error!\n");
+ DH_free(dh);
+ BN_free(dhp_bn);
+ BN_free(dhg_bn);
+ return -1;
+ }
+#else
+ dh->p = BN_bin2bn(dh1536_p, sizeof(dh1536_p), NULL);
+ if (dh->p == NULL) {
+ DH_free(dh);
+ fprintf(stderr, stderr, "error!\n");
+ return -1;
+ }
+
+ dh->g = BN_bin2bn(dh1536_g, sizeof(dh1536_g), NULL);
+ if (dh->g == NULL) {
+ DH_free(dh);
+ fprintf(stderr, stderr, "error!\n");
+ return -1;
+ }
+#endif
+
+ if (DH_generate_key(dh) == 0) {
+ DH_free(dh);
+ fprintf(stderr, stderr, "error!\n");
+ return -1;
+ }
+
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+ pv = DH_get0_priv_key(dh);
+ *priv_len = BN_num_bytes(pv);
+ *priv = calloc(*priv_len + 1, sizeof(uint8_t));
+ if (!*priv) {
+ fprintf(stderr, stderr, "-ENOMEM\n");
+ return -1;
+ }
+
+ BN_bn2bin(pv, *priv);
+
+ pu = DH_get0_pub_key(dh);
+ *pub_len = BN_num_bytes(pu);
+ *pub = calloc(*pub_len + 1, sizeof(uint8_t));
+ if (!*pub) {
+ fprintf(stderr, stderr, "-ENOMEM\n");
+ return -1;
+ }
+
+ BN_bn2bin(pu, *pub);
+#else
+ *priv_len = BN_num_bytes(dh->priv_key);
+ *priv = malloc(*priv_len);
+ if (!*priv) {
+ fprintf(stderr, stderr, "-ENOMEM\n");
+ return -1;
+ }
+
+ BN_bn2bin(dh->priv_key, *priv);
+
+ *pub_len = BN_num_bytes(dh->pub_key);
+ *pub = malloc(*pub_len);
+ if (!*pub) {
+ fprintf(stderr, stderr, "-ENOMEM\n");
+ return -1;
+ }
+
+ BN_bn2bin(dh->pub_key, *pub);
+#endif
+
+ DH_free(dh);
+
+ return 0;
+}
+
+//void * dh5_init(struct wpabuf **priv, struct wpabuf **publ)
+int PLATFORM_COMPUTE_DH_SHARED_SECRET(uint8_t **shared_secret,
+ uint16_t *shared_secret_len,
+ uint8_t *remote_pub,
+ uint16_t remote_pub_len,
+ uint8_t *local_priv,
+ uint8_t local_priv_len)
+{
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+ BIGNUM *dhp_bn, *dhg_bn, *priv_key;
+#endif
+ BIGNUM *pub_key;
+ size_t rlen;
+ int keylen;
+ DH *dh;
+
+
+ if (!shared_secret || !shared_secret_len || !remote_pub || !local_priv) {
+ fprintf(stderr, stderr, "invalid args\n");
+ return -EINVAL;
+ }
+
+ dh = DH_new();
+ if (!dh) {
+ fprintf(stderr, stderr, "-ENOMEM\n");
+ return -1;
+ }
+
+#if OPENSSL_VERSION_NUMBER >= 0x1010000fL
+ dhp_bn = BN_bin2bn(dh1536_p, sizeof(dh1536_p), NULL);
+ dhg_bn = BN_bin2bn(dh1536_g, sizeof(dh1536_g), NULL);
+
+ if (!dhp_bn || !dhg_bn || !DH_set0_pqg(dh, dhp_bn, NULL, dhg_bn)) {
+ fprintf(stderr, stderr, "error!\n");
+ DH_free(dh);
+ BN_free(dhp_bn);
+ BN_free(dhg_bn);
+ return -1;
+ }
+
+ pub_key = BN_bin2bn(remote_pub, remote_pub_len, NULL);
+ if (pub_key == NULL) {
+ DH_free(dh);
+ fprintf(stderr, stderr, "error!\n");
+ return -1;
+ }
+
+ priv_key = BN_bin2bn(local_priv, local_priv_len, NULL);
+ if (priv_key == NULL) {
+ BN_clear_free(priv_key);
+ DH_free(dh);
+ fprintf(stderr, stderr, "error!\n");
+ return -1;
+ }
+
+ DH_set0_key(dh, pub_key, priv_key);
+#else
+ dh->p = BN_bin2bn(dh1536_p, sizeof(dh1536_p), NULL);
+ if (dh->p == NULL) {
+ DH_free(dh);
+ fprintf(stderr, stderr, "error!\n");
+ return -1;
+ }
+
+ dh->g = BN_bin2bn(dh1536_g, sizeof(dh1536_g), NULL);
+ if (dh->g == NULL) {
+ DH_free(dh);
+ fprintf(stderr, stderr, "error!\n");
+ return -1;
+ }
+ pub_key = BN_bin2bn(remote_pub, remote_pub_len, NULL);
+ if (pub_key == NULL) {
+ DH_free(dh);
+ fprintf(stderr, stderr, "error!\n");
+ return -1;
+ }
+ dh->priv_key = BN_bin2bn(local_priv, local_priv_len, NULL);
+ if (dh->priv_key == NULL) {
+ DH_free(dh);
+ fprintf(stderr, stderr, "error!\n");
+ return -1;
+ }
+#endif
+ rlen = DH_size(dh);
+
+ *shared_secret = calloc(rlen, sizeof(uint8_t));
+ if (!*shared_secret) {
+ fprintf(stderr, stderr, "-ENOMEM\n");
+ return -1;
+ }
+
+ keylen = DH_compute_key(*shared_secret, pub_key, dh);
+ if (keylen < 0) {
+ *shared_secret_len = 0;
+ free(*shared_secret);
+ *shared_secret = NULL;
+ BN_clear_free(pub_key);
+ DH_free(dh);
+ fprintf(stderr, stderr, "error!\n");
+ return -1;
+ } else {
+ *shared_secret_len = (uint16_t)keylen;
+ }
+
+#if OPENSSL_VERSION_NUMBER < 0x1010000fL
+ BN_clear_free(pub_key);
+#endif
+ DH_free(dh);
+
+ return 0;
+}
+
+static int openssl_digest_vector(const EVP_MD *type, size_t num_elem,
+ const uint8_t *addr[], const size_t *len,
+ uint8_t *mac)
+{
+ EVP_MD_CTX *ctx;
+ size_t i;
+ unsigned int mac_len;
+
+
+
+ ctx = EVP_MD_CTX_new();
+ if (!ctx)
+ return -1;
+ if (!EVP_DigestInit_ex(ctx, type, NULL)) {
+ fprintf(stderr, stderr, "OpenSSL: EVP_DigestInit_ex failed: %s",
+ ERR_error_string(ERR_get_error(), NULL));
+ EVP_MD_CTX_free(ctx);
+ return -1;
+ }
+ for (i = 0; i < num_elem; i++) {
+ if (!EVP_DigestUpdate(ctx, addr[i], len[i])) {
+ fprintf(stderr, stderr, "OpenSSL: EVP_DigestUpdate failed: %s",
+ ERR_error_string(ERR_get_error(), NULL));
+ EVP_MD_CTX_free(ctx);
+ return -1;
+ }
+ }
+ if (!EVP_DigestFinal(ctx, mac, &mac_len)) {
+ fprintf(stderr, stderr, "OpenSSL: EVP_DigestFinal failed: %s",
+ ERR_error_string(ERR_get_error(), NULL));
+ EVP_MD_CTX_free(ctx);
+ return -1;
+ }
+ EVP_MD_CTX_free(ctx);
+
+ return 0;
+}
+
+//int sha256_vector(size_t num_elem, const uint8_t *addr[], const size_t *len, uint8_t *mac)
+int PLATFORM_SHA256(size_t num_elem, const uint8_t *addr[], const size_t *len, uint8_t *mac)
+{
+
+ return openssl_digest_vector(EVP_sha256(), num_elem, addr, len, mac);
+}
+
+static int openssl_hmac_vector(const EVP_MD *type, const uint8_t *key,
+ size_t key_len, size_t num_elem,
+ const uint8_t *addr[], const size_t *len,
+ uint8_t *mac, unsigned int mdlen)
+{
+ HMAC_CTX *ctx;
+ size_t i;
+ int res;
+
+ ctx = HMAC_CTX_new();
+ if (!ctx)
+ return -1;
+ res = HMAC_Init_ex(ctx, key, key_len, type, NULL);
+ if (res != 1)
+ goto done;
+
+ for (i = 0; i < num_elem; i++)
+ HMAC_Update(ctx, addr[i], len[i]);
+
+ res = HMAC_Final(ctx, mac, &mdlen);
+done:
+ HMAC_CTX_free(ctx);
+
+ return res == 1 ? 0 : -1;
+}
+
+//int hmac_sha256_vector(const u8 *key, size_t key_len, size_t num_elem, const u8 *addr[], const size_t *len, u8 *mac)
+int PLATFORM_HMAC_SHA256(const uint8_t *key, size_t key_len, size_t num_elem,
+ const uint8_t *addr[], const size_t *len, uint8_t *mac)
+{
+
+ return openssl_hmac_vector(EVP_sha256(), key, key_len, num_elem, addr,
+ len, mac, 32);
+
+}
+
+//int aes_128_cbc_encrypt(const u8 *key, const u8 *iv, u8 *data, size_t data_len)
+int PLATFORM_AES_ENCRYPT(uint8_t *key, uint8_t *iv, uint8_t *data, uint32_t data_len)
+{
+ EVP_CIPHER_CTX *ctx;
+ uint8_t buf[16];
+ int clen, len;
+ int res = -1;
+
+
+ ctx = EVP_CIPHER_CTX_new();
+ if (!ctx)
+ return -1;
+ clen = data_len;
+ len = sizeof(buf);
+ if (EVP_EncryptInit_ex(ctx, EVP_aes_128_cbc(), NULL, key, iv) == 1 &&
+ EVP_CIPHER_CTX_set_padding(ctx, 0) == 1 &&
+ EVP_EncryptUpdate(ctx, data, &clen, data, data_len) == 1 &&
+ clen == (int) data_len &&
+ EVP_EncryptFinal_ex(ctx, buf, &len) == 1 && len == 0)
+ res = 0;
+ EVP_CIPHER_CTX_free(ctx);
+
+ return res;
+}
+
+//int aes_128_cbc_decrypt(const u8 *key, const u8 *iv, u8 *data, size_t data_len)
+int PLATFORM_AES_DECRYPT(uint8_t *key, uint8_t *iv, uint8_t *data, uint32_t data_len)
+{
+ EVP_CIPHER_CTX *ctx;
+ uint8_t buf[16];
+ int plen, len;
+ int res = -1;
+
+
+ ctx = EVP_CIPHER_CTX_new();
+ if (!ctx)
+ return -1;
+ plen = data_len;
+ len = sizeof(buf);
+ if (EVP_DecryptInit_ex(ctx, EVP_aes_128_cbc(), NULL, key, iv) == 1 &&
+ EVP_CIPHER_CTX_set_padding(ctx, 0) == 1 &&
+ EVP_DecryptUpdate(ctx, data, &plen, data, data_len) == 1 &&
+ plen == (int) data_len &&
+ EVP_DecryptFinal_ex(ctx, buf, &len) == 1 && len == 0)
+ res = 0;
+ EVP_CIPHER_CTX_free(ctx);
+
+ return res;
+}
+#endif // 0
diff --git a/src/cryptutil.h b/src/cryptutil.h
new file mode 100644
index 0000000000000000000000000000000000000000..c24449c2d4f3e5f92c00c54dc2fe9ebb0b770c8e
--- /dev/null
+++ b/src/cryptutil.h
@@ -0,0 +1,34 @@
+
+
+#ifndef CRYPT_UTIL_H
+#define CRYPT_UTIL_H
+
+#include <stdint.h>
+#include <sys/types.h>
+
+
+int PLATFORM_GENERATE_DH_KEY_PAIR(uint8_t **priv, uint16_t *priv_len,
+ uint8_t **pub, uint16_t *pub_len);
+
+int PLATFORM_COMPUTE_DH_SHARED_SECRET(uint8_t **shared_secret,
+ uint16_t *shared_secret_len,
+ uint8_t *remote_pub,
+ uint16_t remote_pub_len,
+ uint8_t *local_priv,
+ uint8_t local_priv_len);
+
+
+int PLATFORM_SHA256(size_t num_elem, const uint8_t *addr[], const size_t *len,
+ uint8_t *mac);
+
+
+
+int PLATFORM_HMAC_SHA256(const uint8_t *key, size_t key_len, size_t num_elem,
+ const uint8_t *addr[], const size_t *len, uint8_t *mac);
+
+
+int PLATFORM_AES_ENCRYPT(uint8_t *key, uint8_t *iv, uint8_t *data, uint32_t data_len);
+int PLATFORM_AES_DECRYPT(uint8_t *key, uint8_t *iv, uint8_t *data, uint32_t data_len);
+
+
+#endif /* CRYPT_UTIL_H */
diff --git a/src/i1905.c b/src/i1905.c
index 47a1d4c3f4f0901d6a8b3a836387fb1c56ad0913..f0feb2c801c7d2f1f686ed58fb510de3637e6fa3 100644
--- a/src/i1905.c
+++ b/src/i1905.c
@@ -712,6 +712,55 @@ static int i1905_destroy_al_interface(struct i1905_private *p)
return 0;
}
+static int i1905_init_interface_private_wsc(struct i1905_interface *n)
+{
+ struct i1905_interface_private *p =
+ (struct i1905_interface_private *)n->priv;
+ struct i1905_interface_private_wsc *wsc;
+ struct i1905_private *priv= (struct i1905_private *)p->i1905private;
+
+
+ wsc = calloc(1, sizeof(*wsc));
+ if (!wsc) {
+ fprintf(stderr, "%s: -ENOMEM\n", __func__);
+ return -ENOMEM;
+ }
+
+ p->wsc = wsc;
+
+ //TODO: get real data -
+ // auth_type, enc_type
+
+ //wifi_get_wps_device_info(n->ifname, struct wps_device *info)
+
+
+
+ snprintf(wsc->manufacturer, 64, "Unknown");
+ snprintf(wsc->device_name, 32, "Unknown");
+ snprintf(wsc->model_name, 32, "Unknown");
+ wsc->os_version = 0x00000001;
+
+ if (IS_MEDIA_WIFI_2GHZ(n->media)) {
+ p->wsc->band = WPS_RF_24GHZ;
+
+ if (priv->cfg.registrar_2g)
+ p->registrar = true;
+ }
+
+ if (IS_MEDIA_WIFI_5GHZ(n->media)) {
+ p->wsc->band |= WPS_RF_50GHZ;
+
+ if (priv->cfg.registrar_5g)
+ p->registrar = true;
+ }
+
+
+ // TODO: get 'enum wps_state' and decide
+ p->configured = false;
+
+ return 0;
+}
+
static int i1905_setup_interface_priv(struct i1905_interface *n)
{
struct i1905_interface_private *p =
@@ -732,6 +781,9 @@ static int i1905_setup_interface_priv(struct i1905_interface *n)
cmdu_ackq_init(&p->txack_q);
+ if (IS_MEDIA_WIFI(n->media))
+ i1905_init_interface_private_wsc(n);
+
i1905_init_interface_socket_1905(n, p);
i1905_init_interface_socket_lldp(n, p);
@@ -1073,6 +1125,25 @@ void topology_timer_cb(atimer_t *t)
timer_set(t, 60000);
}
+bool i1905_has_registrar(void *priv, uint8_t freqband)
+{
+ struct i1905_private *p = priv;
+
+ if (!p)
+ return false;
+
+ switch (freqband) {
+ case IEEE80211_FREQUENCY_BAND_2_4_GHZ:
+ return p->cfg.registrar_2g;
+ case IEEE80211_FREQUENCY_BAND_5_GHZ:
+ return p->cfg.registrar_5g;
+ case IEEE80211_FREQUENCY_BAND_60_GHZ:
+ default:
+ break;
+ }
+
+ return false;
+}
int i1905_init(void **priv, void *cfg)
{
diff --git a/src/i1905.h b/src/i1905.h
index 77a5229236e208640262b86438a123485374b6e1..90272fbbbe862c02e22d38746b8c4373373d0359 100644
--- a/src/i1905.h
+++ b/src/i1905.h
@@ -4,6 +4,7 @@
#define I1905_H
+#include "i1905_wsc.h"
#define OBJECT_INVALID ((uint32_t)-1)
@@ -26,6 +27,30 @@ struct i1905_private {
};
+struct i1905_interface_private_wsc {
+ union {
+ enum { SEND_M1, RECV_M2 } e;
+ enum { RECV_M1, SEND_M2 } r;
+ } state;
+
+ uint8_t uuid[16];
+ char manufacturer[65]; /* with terminating '\0' */
+ char model_name[33];
+ char device_name[33];
+ uint8_t model_number[32];
+ uint8_t serial_number[32];
+ uint32_t os_version;
+ uint8_t macaddr[6];
+ uint8_t band;
+ uint16_t auth_types;
+ uint16_t enc_types;
+ //uint8_t nonce[WPS_NONCE_LEN]; //TODO
+ uint8_t *last_msg;
+ uint16_t last_msglen;
+ void *key;
+ struct wps_credential cred;
+};
+
struct i1905_interface_private {
int sock_1905;
int sock_lldp;
@@ -38,6 +63,10 @@ struct i1905_interface_private {
struct cmdu_ackq txack_q;
void *i1905private;
+ bool registrar;
+ bool configured; // XXX
+ struct i1905_interface_private_wsc *wsc;
+
void *iface; /* points to i1905_interface */
struct ubus_object obj;
struct ubus_object_type obj_type;
@@ -67,6 +96,8 @@ int i1905_start();
int i1905_stop(struct i1905_private *p);
int i1905_reconfig(struct i1905_config *cfg, const char *conffile);
+bool i1905_has_registrar(void *priv, uint8_t freqband);
+
int i1905_cmdu_tx(struct i1905_interface_private *ifp,
uint8_t *dst, uint8_t *src, uint16_t type,
uint16_t *mid, uint8_t *data, int datalen);
diff --git a/src/i1905_dm.h b/src/i1905_dm.h
index 26d4c5a81a536ebeb98078a6082cf0fa35b28bc3..c8a9dbd4992bc1643dd33c1054fb0a2c95c51c72 100644
--- a/src/i1905_dm.h
+++ b/src/i1905_dm.h
@@ -137,6 +137,18 @@ enum i1905_mediatype {
#define IS_MEDIA_WIFI(m) \
((m) >= I1905_802_11B_2_4_GHZ && (m) <= I1905_802_11AX)
+
+#define IS_MEDIA_WIFI_5GHZ(m) \
+ ((m) == I1905_802_11A_5_GHZ || \
+ (m) == I1905_802_11N_5_GHZ || \
+ (m) == I1905_802_11AC_5_GHZ)
+
+#define IS_MEDIA_WIFI_2GHZ(m) \
+ ((m) == I1905_802_11B_2_4_GHZ || \
+ (m) == I1905_802_11G_2_4_GHZ || \
+ (m) == I1905_802_11N_2_4_GHZ)
+
+
struct i1905_fwdrule {
struct list_head iflist; /* list of ifnames/macaddres FIXME */
uint32_t mask; /* bitmap of I1905_FWDRULE_* */
diff --git a/src/i1905_wsc.c b/src/i1905_wsc.c
new file mode 100644
index 0000000000000000000000000000000000000000..bf8d0d139d807408b00fb4877bebcff3a42fc280
--- /dev/null
+++ b/src/i1905_wsc.c
@@ -0,0 +1,1252 @@
+
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+
+#include "util.h"
+#include "bufutil.h"
+
+#include "cryptutil.h"
+#include "i1905_wsc.h"
+
+struct wsc_key {
+ uint8_t *key;
+ uint32_t keylen;
+ uint8_t nonce[16];
+ uint8_t macaddr[6];
+};
+
+
+#if 0
+struct wsc_context {
+ union {
+ enum { SEND_M1, RECV_M2 } e;
+ enum { RECV_M1, SEND_M2 } r;
+ } state;
+
+ uint8_t uuid[16];
+ uint8_t macaddr[6];
+ uint8_t nonce[16];
+
+ uint8_t *last_msg;
+ uint8_t *last_msglen;
+};
+#endif
+
+#if 0
+struct mdata {
+ union {
+ struct input {
+ uint8_t ssid[64];
+ uint8_t network_key[64];
+ uint16_t auth_types;
+ uint16_t enc_types;
+ uint8_t macaddr[6];
+ uint8_t band;
+ uint8_t mapie;
+ } input;
+
+ struct output {
+ uint8_t ssid[64];
+ uint8_t network_key[64];
+ uint16_t auth_types;
+ uint16_t enc_types;
+ uint8_t mapie;
+ uint8_t bssid[6];
+ uint8_t band;
+ } output;
+ };
+};
+#endif
+
+
+#define bufptr_put_u8(b, v) \
+do { \
+ *b = (uint8_t)v; \
+ b += 1; \
+} while(0)
+
+
+#define bufptr_put_be16(_b, v) \
+do { \
+ buf_put_be16(_b, v); \
+ _b += 2; \
+} while(0)
+
+
+#define bufptr_put_be32(_b, v) \
+do { \
+ buf_put_be32(_b, v); \
+ _b += 4; \
+} while(0)
+
+
+#define bufptr_put(b, v, s) \
+do { \
+ memcpy(b, v, s); \
+ b += s; \
+} while(0)
+
+
+#define bufptr_get(b, v, s) \
+do { \
+ memcpy(v, b, s); \
+ b += s; \
+} while(0)
+
+
+
+#define AES_BLOCK_SIZE 16
+#define SHA256_MAC_LEN 32
+
+#define WPS_AUTHKEY_LEN 32
+#define WPS_KEYWRAPKEY_LEN 16
+#define WPS_EMSK_LEN 32
+
+void wps_kdf(const uint8_t *key, const uint8_t *label_prefix,
+ size_t label_prefix_len, const char *label, uint8_t *res,
+ size_t res_len)
+{
+
+ uint8_t i_buf[4], key_bits[4];
+ const uint8_t *addr[4];
+ size_t len[4];
+ int i, iter;
+ uint8_t hash[SHA256_MAC_LEN], *opos;
+ size_t left;
+
+ buf_put_be32(key_bits, res_len * 8);
+
+ addr[0] = i_buf;
+ len[0] = sizeof(i_buf);
+ addr[1] = label_prefix;
+ len[1] = label_prefix_len;
+ addr[2] = (const uint8_t *) label;
+ len[2] = strlen(label);
+ addr[3] = key_bits;
+ len[3] = sizeof(key_bits);
+
+ iter = (res_len + SHA256_MAC_LEN - 1) / SHA256_MAC_LEN;
+ opos = res;
+ left = res_len;
+
+ for (i = 1; i <= iter; i++) {
+ buf_put_be32(i_buf, i);
+ PLATFORM_HMAC_SHA256(key, SHA256_MAC_LEN, 4, addr, len, hash);
+ if (i < iter) {
+ memcpy(opos, hash, SHA256_MAC_LEN);
+ opos += SHA256_MAC_LEN;
+ left -= SHA256_MAC_LEN;
+ } else
+ memcpy(opos, hash, left);
+ }
+}
+
+uint8_t wsc_get_message_type(uint8_t *m, uint16_t m_size)
+{
+ uint8_t *p = m;
+
+ while (abs(p - m) < m_size) {
+ uint16_t attr_type;
+ uint16_t attr_len;
+ uint8_t msg_type;
+
+ attr_type = buf_get_be16(p);
+ p += 2;
+ attr_len = buf_get_be16(p);
+ p += 2;
+
+
+ switch (attr_type) {
+ case ATTR_MSG_TYPE:
+ if (attr_len != 1) {
+ fprintf(stderr,
+ "Incorrect length (%d) for ATTR_MSG_TYPE\n", attr_len);
+ return -EINVAL;
+ }
+ bufptr_get(p, &msg_type, 1);
+ return msg_type;
+ default:
+ break;
+ }
+
+ p += attr_len;
+ }
+
+ return 0xff;
+}
+
+int wsc_build_m1(const char *ifname, uint8_t **m1, uint16_t *m1_size, void **key,
+ struct wps_data *in)
+{
+ uint8_t oui[4] = { 0x00, 0x50, 0xf2, 0x00 };
+ const char *serial_number = "00000000";
+ const char *model_number = "00000000";
+ const char *uuid = "0000000000000000";
+ const char *manufacturer = "Unknown";
+ const char *device_name = "Unknown";
+ const char *model = "Unknown";
+ uint32_t os_version = 0x00000001;
+ struct wsc_key *private_key;
+ uint8_t nonce_e[16];
+ uint8_t *buf;
+ uint8_t *p;
+
+
+ //TODO: pass m1 and key buffer and use them.
+
+ if (!ifname || !m1 || !m1_size || !key || !in) {
+ fprintf(stderr, "%s(): invalid args!\n", __func__);
+ return -EINVAL;
+ }
+
+ buf = calloc(1000, sizeof(uint8_t));
+ if (!buf)
+ return -ENOMEM;
+
+ p = buf;
+
+ /* version */
+ bufptr_put_be16(p, ATTR_VERSION);
+ bufptr_put_be16(p, 1);
+ bufptr_put_u8(p, 0x10);
+
+
+ /* message type */
+ bufptr_put_be16(p, ATTR_MSG_TYPE);
+ bufptr_put_be16(p, 1);
+ bufptr_put_u8(p, WPS_M1);
+
+
+ /* uuid-e */
+ bufptr_put_be16(p, ATTR_UUID_E);
+ bufptr_put_be16(p, 16);
+ bufptr_put(p, uuid, 16);
+
+
+ /* macaddress */
+ bufptr_put_be16(p, ATTR_MAC_ADDR);
+ bufptr_put_be16(p, 6);
+ bufptr_put(p, in->macaddr, 6);
+
+
+ /* enrollee nonce */
+ get_random_bytes(16, nonce_e);
+ bufptr_put_be16(p, ATTR_ENROLLEE_NONCE);
+ bufptr_put_be16(p, 16);
+ bufptr_put(p, nonce_e, 16);
+
+
+ /* public key */
+ uint8_t *priv, *pub;
+ uint16_t priv_len = 0, pub_len = 0;
+
+ PLATFORM_GENERATE_DH_KEY_PAIR(&priv, &priv_len, &pub, &pub_len);
+ fprintf(stderr, "M1 plen|%d| publen|%d|\n", priv_len, pub_len);
+
+ bufptr_put_be16(p, ATTR_PUBLIC_KEY);
+ bufptr_put_be16(p, pub_len);
+ bufptr_put(p, pub, pub_len);
+
+ private_key = calloc(1, sizeof(*private_key));
+ if (!private_key) {
+ fprintf(stderr, "-ENOMEM\n");
+ return -ENOMEM;
+ }
+
+ private_key->key = calloc(priv_len, sizeof(uint8_t));
+ if (!private_key->key) {
+ free(private_key);
+ fprintf(stderr, "-ENOMEM\n");
+ return -ENOMEM;
+ }
+ private_key->keylen = priv_len;
+ if (priv_len > 0)
+ memcpy(private_key->key, priv, priv_len);
+
+ memcpy(private_key->macaddr, in->macaddr, 6);
+ memcpy(private_key->nonce, nonce_e, 16);
+ free(pub);
+ free(priv);
+
+ /* authentication type flags */
+ bufptr_put_be16(p, ATTR_AUTH_TYPE_FLAGS);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, in->auth_types);
+
+
+ /* encryption type flags */
+ bufptr_put_be16(p, ATTR_ENCR_TYPE_FLAGS);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, in->enc_types);
+
+
+ /* connection type flags */
+ bufptr_put_be16(p, ATTR_CONN_TYPE_FLAGS);
+ bufptr_put_be16(p, 1);
+ bufptr_put_u8(p, WPS_CONN_ESS);
+
+
+ /* configuration methods */
+ bufptr_put_be16(p, ATTR_CONFIG_METHODS);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, WPS_CONFIG_PHY_PUSHBUTTON | WPS_CONFIG_VIRT_PUSHBUTTON);
+
+
+ /* wps state */
+ bufptr_put_be16(p, ATTR_WPS_STATE);
+ bufptr_put_be16(p, 1);
+ bufptr_put_u8(p, WPS_STATE_NOT_CONFIGURED);
+
+
+ /* manufacturer */
+ bufptr_put_be16(p, ATTR_MANUFACTURER);
+ bufptr_put_be16(p, strlen(manufacturer));
+ bufptr_put(p, manufacturer, strlen(manufacturer));
+
+
+ /* model name */
+ bufptr_put_be16(p, ATTR_MODEL_NAME);
+ bufptr_put_be16(p, strlen(model));
+ bufptr_put(p, model, strlen(model));
+
+
+ /* model number */
+ bufptr_put_be16(p, ATTR_MODEL_NUMBER);
+ bufptr_put_be16(p, strlen(model_number));
+ bufptr_put(p, model_number, strlen(model_number));
+
+
+ /* serial number */
+ bufptr_put_be16(p, ATTR_SERIAL_NUMBER);
+ bufptr_put_be16(p, strlen(serial_number));
+ bufptr_put(p, serial_number, strlen(serial_number));
+
+
+ /* primary device type */
+ bufptr_put_be16(p, ATTR_PRIMARY_DEV_TYPE);
+ bufptr_put_be16(p, 8);
+ bufptr_put_be16(p, WPS_DEV_NETWORK_INFRA);
+ bufptr_put(p, oui, 4);
+ bufptr_put_be16(p, WPS_DEV_NETWORK_INFRA_ROUTER);
+
+
+ /* device name */
+ bufptr_put_be16(p, ATTR_DEV_NAME);
+ bufptr_put_be16(p, strlen(device_name));
+ bufptr_put(p, device_name, strlen(device_name));
+
+
+ /* rf bands */
+ bufptr_put_be16(p, ATTR_RF_BANDS);
+ bufptr_put_be16(p, 1);
+ bufptr_put_u8(p, in->band);
+
+
+ /* association state */
+ bufptr_put_be16(p, ATTR_ASSOC_STATE);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, WPS_ASSOC_NOT_ASSOC);
+
+
+ /* device password id */
+ bufptr_put_be16(p, ATTR_DEV_PASSWORD_ID);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, DEV_PW_PUSHBUTTON);
+
+
+ /* config error */
+ bufptr_put_be16(p, ATTR_CONFIG_ERROR);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, WPS_CFG_NO_ERROR);
+
+
+ /* os version */
+ bufptr_put_be16(p, ATTR_OS_VERSION);
+ bufptr_put_be16(p, 4);
+ bufptr_put_be32(p, 0x80000000 | os_version);
+
+
+ /* version2 - vendor extension */
+ bufptr_put_be16(p, ATTR_VENDOR_EXTENSION);
+ bufptr_put_be16(p, 6);
+ bufptr_put_be16(p, WPS_CFG_NO_ERROR);
+ bufptr_put_u8(p, WFA_VENDOR_ID_1);
+ bufptr_put_u8(p, WFA_VENDOR_ID_2);
+ bufptr_put_u8(p, WFA_VENDOR_ID_3);
+ bufptr_put_u8(p, WFA_ELEM_VERSION2);
+ bufptr_put_u8(p, 1);
+ bufptr_put_u8(p, WPS_VERSION);
+
+
+ *m1 = buf;
+ *m1_size = abs(p - buf);
+ *key = private_key;
+
+ return 0;
+}
+
+int wsc_build_m2(uint8_t *m1, uint16_t m1_size, uint8_t **m2, uint16_t *m2_size,
+ /* struct wps_data *in, */ struct wps_credential *cred)
+{
+ uint8_t *buffer;
+ uint8_t *p;
+
+ int i;
+
+ uint8_t *m1_macaddr = NULL;
+ uint8_t m1_macaddr_present = 0;
+ uint8_t *m1_nonce = NULL;
+ uint8_t m1_nonce_present = 0;
+ uint8_t *m1_pubkey = NULL;
+ uint8_t m1_pubkey_present = 0;
+ uint16_t m1_pubkey_len = 0;
+ uint8_t m1_rf_band = WPS_RF_24GHZ; // TODO: wsc_process_m1() at registrar
+
+ uint8_t *local_privkey;
+ uint16_t local_privkey_len;
+
+ uint8_t authkey[WPS_AUTHKEY_LEN];
+ uint8_t keywrapkey[WPS_KEYWRAPKEY_LEN];
+ uint8_t emsk[WPS_EMSK_LEN];
+
+ uint8_t uuid[16] = {0};
+ uint8_t nonce_r[16];
+ //uint8_t m2_mac[6] = {0};
+
+ uint8_t oui[4] = { 0x00, 0x50, 0xf2, 0x00 };
+ const char *manufacturer = "Unknown";
+ const char *serial_number = "00000000";
+ const char *model_number = "00000000";
+ const char *device_name = "Unknown";
+ uint32_t os_version = 0x00000001;
+ const char *model = "Unknown";
+ //uint16_t auth_types;
+
+
+
+ p = m1;
+
+ if (!cred) {
+ fprintf(stderr, "%s: cred = NULL\n", __func__);
+ return -EINVAL;
+ }
+
+ //if (cred && cred->macaddr)
+ //memcpy(m2_mac, cred->macaddr, 6);
+
+ while (abs(p - m1) < m1_size) {
+ uint16_t attr_type;
+ uint16_t attr_len;
+
+ attr_type = buf_get_be16(p);
+ p += 2;
+ attr_len = buf_get_be16(p);
+ p += 2;
+
+
+ switch (attr_type) {
+ case ATTR_MAC_ADDR:
+ if (attr_len != 6) {
+ fprintf(stderr, "Incorrect length (%d) for ATTR_MAC_ADDR\n", attr_len);
+ return -EINVAL;
+ }
+ m1_macaddr = p;
+ m1_macaddr_present = 1;
+ break;
+ case ATTR_ENROLLEE_NONCE:
+ if (attr_len != 16) {
+ fprintf(stderr, "Incorrect length (%d) for ATTR_ENROLLEE_NONCE\n", attr_len);
+ return -EINVAL;
+ }
+ m1_nonce = p;
+ m1_nonce_present = 1;
+ break;
+ case ATTR_PUBLIC_KEY:
+ m1_pubkey_len = attr_len;
+ m1_pubkey = p;
+ m1_pubkey_present = 1;
+ break;
+ case ATTR_RF_BANDS:
+ m1_rf_band = *p;
+ break;
+ default:
+ break;
+ }
+
+ p += attr_len;
+ }
+
+ if (!m1_pubkey_present || !m1_nonce_present || !m1_macaddr_present ||
+ !m1_rf_band) {
+
+ fprintf(stderr, "Required attr in M1 not present!\n");
+ return -1;
+ }
+
+ buffer = calloc(1000, sizeof(uint8_t));
+ if (!buffer) {
+ fprintf(stderr, "-ENOMEM\n");
+ return -ENOMEM;
+ }
+
+ p = buffer;
+
+ /* version */
+ bufptr_put_be16(p, ATTR_VERSION);
+ bufptr_put_be16(p, 1);
+ bufptr_put_u8(p, 0x10);
+
+ /* message type */
+ bufptr_put_be16(p, ATTR_MSG_TYPE);
+ bufptr_put_be16(p, 1);
+ bufptr_put_u8(p, WPS_M2);
+
+ /* enrollee nonce */
+ bufptr_put_be16(p, ATTR_ENROLLEE_NONCE);
+ bufptr_put_be16(p, 16);
+ bufptr_put(p, m1_nonce, 16);
+
+ /* registrar nonce */
+ get_random_bytes(16, nonce_r);
+ bufptr_put_be16(p, ATTR_REGISTRAR_NONCE);
+ bufptr_put_be16(p, 16);
+ bufptr_put(p, nonce_r, 16);
+
+ /* uuid-r */
+ bufptr_put_be16(p, ATTR_UUID_R);
+ bufptr_put_be16(p, 16);
+ bufptr_put(p, uuid, 16);
+
+
+ /* public key */
+ {
+ uint8_t *priv, *pub;
+ uint16_t priv_len = 0, pub_len = 0;
+
+ PLATFORM_GENERATE_DH_KEY_PAIR(&priv, &priv_len, &pub, &pub_len);
+ fprintf(stderr, "plen|%d|, publen|%d|\n", priv_len, pub_len);
+
+ bufptr_put_be16(p, ATTR_PUBLIC_KEY);
+ bufptr_put_be16(p, pub_len);
+ bufptr_put(p, pub, pub_len);
+
+ // We will use it later... save it.
+ local_privkey = priv;
+ local_privkey_len = priv_len;
+ free(pub);
+ }
+
+ /* derive keys - authkey, keywrapkey and emsk */
+ {
+ uint8_t *shared_secret;
+ uint16_t shared_secret_len;
+ const uint8_t *addr[3];
+ size_t len[3];
+ uint8_t dhkey[SHA256_MAC_LEN];
+ uint8_t kdk[SHA256_MAC_LEN];
+ uint8_t keys[WPS_AUTHKEY_LEN + WPS_KEYWRAPKEY_LEN + WPS_EMSK_LEN];
+
+ /* DH shared secret = enrollee's public key from M1 + private
+ * key (generated above).
+ *
+ * The enrollee after receiving M2, will obtain the same DH shared
+ * secret using its private key and our public key (sent in M2).
+ */
+ PLATFORM_COMPUTE_DH_SHARED_SECRET(&shared_secret,
+ &shared_secret_len,
+ m1_pubkey,
+ m1_pubkey_len,
+ local_privkey,
+ local_privkey_len);
+
+ /* dhkey = SHA-256 digest of the DH shared secret. */
+ addr[0] = shared_secret;
+ len[0] = shared_secret_len;
+
+ PLATFORM_SHA256(1, addr, len, dhkey);
+
+ /* Derive KDK -
+ *
+ * KDK = HMAC-SHA-256_DHKey (N1 || EnrolleeMAC || N2), where
+ * N1 is enrollee's nonce from M1,
+ * N2 is registrar's nonce generated above.
+ *
+ */
+
+ /* Compute HMAC of the following using 'dhkey' -
+ * the enrollee's nonce from M1,
+ * the enrolle MAC address from M1, and
+ * the our nonce generated above.
+ */
+ addr[0] = m1_nonce;
+ addr[1] = m1_macaddr;
+ addr[2] = nonce_r;
+ len[0] = 16;
+ len[1] = 6;
+ len[2] = 16;
+
+ PLATFORM_HMAC_SHA256(dhkey, SHA256_MAC_LEN, 3, addr, len, kdk);
+
+ wps_kdf(kdk, NULL, 0, "Wi-Fi Easy and Secure Key Derivation",
+ keys, sizeof(keys));
+
+ memcpy(authkey, keys, WPS_AUTHKEY_LEN);
+ memcpy(keywrapkey, keys + WPS_AUTHKEY_LEN, WPS_KEYWRAPKEY_LEN);
+ memcpy(emsk, keys + WPS_AUTHKEY_LEN + WPS_KEYWRAPKEY_LEN, WPS_EMSK_LEN);
+
+ fprintf(stderr, "WPS keys: \n");
+ bufprintf(m1_pubkey, m1_pubkey_len, "Enrollee public key");
+ /* bufprintf(local_privkey, local_privkey_len, "Registrar private key"); */
+ /* bufprintf(shared_secret, shared_secret_len, "DH Shared secret"); */
+ bufprintf(dhkey, 32, "DH Key");
+ bufprintf(m1_nonce, 16, "Nonce-E");
+ bufprintf(nonce_r, 16, "Nonce-R");
+ bufprintf(kdk, 32, "KDK");
+ bufprintf(authkey, WPS_AUTHKEY_LEN, "WPS authkey");
+ bufprintf(keywrapkey, WPS_KEYWRAPKEY_LEN, "WPS keywrapkey");
+ bufprintf(emsk, WPS_EMSK_LEN, "WPS emsk");
+
+ free(shared_secret);
+ }
+
+
+ /* authentication type flags */
+ bufptr_put_be16(p, ATTR_AUTH_TYPE_FLAGS);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, cred->auth_type);
+
+ /* encryption type flags */
+ bufptr_put_be16(p, ATTR_ENCR_TYPE_FLAGS);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, cred->enc_type);
+
+ /* connection types */
+ bufptr_put_be16(p, ATTR_CONN_TYPE_FLAGS);
+ bufptr_put_be16(p, 1);
+ bufptr_put_u8(p, WPS_CONN_ESS);
+
+ /* configuration methods */
+ bufptr_put_be16(p, ATTR_CONFIG_METHODS);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, WPS_CONFIG_PHY_PUSHBUTTON | WPS_CONFIG_VIRT_PUSHBUTTON);
+
+ /* manufacturer */
+ bufptr_put_be16(p, ATTR_MANUFACTURER);
+ bufptr_put_be16(p, strlen(manufacturer));
+ bufptr_put(p, manufacturer, strlen(manufacturer));
+
+ /* model name */
+ bufptr_put_be16(p, ATTR_MODEL_NAME);
+ bufptr_put_be16(p, strlen(model));
+ bufptr_put(p, model, strlen(model));
+
+ /* model number */
+ bufptr_put_be16(p, ATTR_MODEL_NUMBER);
+ bufptr_put_be16(p, strlen(model_number));
+ bufptr_put(p, model_number, strlen(model_number));
+
+ /* serial number */
+ bufptr_put_be16(p, ATTR_SERIAL_NUMBER);
+ bufptr_put_be16(p, strlen(serial_number));
+ bufptr_put(p, serial_number, strlen(serial_number));
+
+ /* primary device type */
+ bufptr_put_be16(p, ATTR_PRIMARY_DEV_TYPE);
+ bufptr_put_be16(p, 8);
+ bufptr_put_be16(p, WPS_DEV_NETWORK_INFRA);
+ bufptr_put(p, oui, 4);
+ bufptr_put_be16(p, WPS_DEV_NETWORK_INFRA_ROUTER);
+
+ /* device name */
+ bufptr_put_be16(p, ATTR_DEV_NAME);
+ bufptr_put_be16(p, strlen(device_name));
+ bufptr_put(p, device_name, strlen(device_name));
+
+ /* rf bands */
+ bufptr_put_be16(p, ATTR_RF_BANDS);
+ bufptr_put_be16(p, 1);
+ bufptr_put_u8(p, cred->band);
+
+ /* association state */
+ bufptr_put_be16(p, ATTR_ASSOC_STATE);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, WPS_ASSOC_CONN_SUCCESS);
+
+ /* configuration error */
+ bufptr_put_be16(p, ATTR_CONFIG_ERROR);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, WPS_CFG_NO_ERROR);
+
+ /* device password id */
+ bufptr_put_be16(p, ATTR_DEV_PASSWORD_ID);
+ bufptr_put_be16(p, 2);
+ bufptr_put_be16(p, DEV_PW_PUSHBUTTON);
+
+ /* os version */
+ bufptr_put_be16(p, ATTR_OS_VERSION);
+ bufptr_put_be16(p, 4);
+ bufptr_put_be32(p, 0x80000000 | os_version);
+
+ /* version2 - inside vendor extension */
+ bufptr_put_be16(p, ATTR_VENDOR_EXTENSION);
+ bufptr_put_be16(p, 6);
+ bufptr_put_u8(p, WFA_VENDOR_ID_1);
+ bufptr_put_u8(p, WFA_VENDOR_ID_2);
+ bufptr_put_u8(p, WFA_VENDOR_ID_3);
+ bufptr_put_u8(p, WFA_ELEM_VERSION2);
+ bufptr_put_u8(p, 1);
+ bufptr_put_u8(p, WPS_VERSION);
+
+
+ /* encrypted settings */
+ {
+ //char *ssid;
+ //char *key;
+ //int ssidlen, keylen;
+
+ uint8_t plain[200];
+ uint8_t hash[SHA256_MAC_LEN];
+ uint8_t *iv_start;
+ uint8_t *data_start;
+ uint8_t num_pad_bytes;
+ uint8_t *r;
+
+ const uint8_t *addr[1];
+ size_t len[1];
+
+ //ssid = (char *)cred->ssid;
+ //key = (char *)cred->key;
+ //ssidlen = strlen(ssid); // FIXME
+ //keylen = strlen(key);
+
+ r = plain;
+
+ /* ssid */
+ bufptr_put_be16(r, ATTR_SSID);
+ bufptr_put_be16(r, cred->ssidlen);
+ bufptr_put(r, cred->ssid, cred->ssidlen);
+
+ /* auth type */
+ bufptr_put_be16(r, ATTR_AUTH_TYPE);
+ bufptr_put_be16(r, 2);
+ bufptr_put_be16(r, cred->auth_type);
+
+ /* encryption type */
+ bufptr_put_be16(r, ATTR_ENCR_TYPE);
+ bufptr_put_be16(r, 2);
+ bufptr_put_be16(r, cred->enc_type);
+
+ /* key */
+ bufptr_put_be16(r, ATTR_NETWORK_KEY);
+ bufptr_put_be16(r, cred->keylen);
+ bufptr_put(r, cred->key, cred->keylen);
+
+ /* macaddr */
+ bufptr_put_be16(r, ATTR_MAC_ADDR);
+ bufptr_put_be16(r, 6);
+ bufptr_put(r, cred->macaddr, 6);
+
+
+ /* Version2 - vendor extension */ //TODO: revisit mapie
+ bufptr_put_be16(r, ATTR_VENDOR_EXTENSION);
+ bufptr_put_be16(r, 9);
+ bufptr_put_u8(r, WFA_VENDOR_ID_1);
+ bufptr_put_u8(r, WFA_VENDOR_ID_2);
+ bufptr_put_u8(r, WFA_VENDOR_ID_3);
+ bufptr_put_u8(r, WFA_ELEM_VERSION2);
+ bufptr_put_u8(r, 1);
+ bufptr_put_u8(r, WPS_VERSION);
+ bufptr_put_u8(r, WFA_MAP_EXT_ATTR); /* add multi-ap extension subelement */
+ bufptr_put_u8(r, 1);
+ bufptr_put_u8(r, cred->mapie);
+
+ fprintf(stderr, "AP configuration settings --->\n");
+ fprintf(stderr, "\tssid : %s\n", cred->ssid); // TODO: print_ssid()
+ fprintf(stderr, "\tbssid : " MACFMT "\n", MAC2STR(cred->macaddr));
+ fprintf(stderr, "\tauth_type : 0x%04x\n", cred->auth_type);
+ fprintf(stderr, "\tenc_type : 0x%04x\n", cred->enc_type);
+ fprintf(stderr, "\tkey : %s\n", cred->key); // TODO: print_key()
+ fprintf(stderr, "\tmap_extension : 0x%02x\n", cred->mapie);
+
+ /* compute HMAC of the settings buffer using "authkey" */
+ addr[0] = plain;
+ len[0] = abs(r - plain);
+ PLATFORM_HMAC_SHA256(authkey, WPS_AUTHKEY_LEN, 1, addr, len, hash);
+
+ /* append first 8 bytes of hash to settings buffer */
+ bufptr_put_be16(r, ATTR_KEY_WRAP_AUTH);
+ bufptr_put_be16(r, 8);
+ bufptr_put(r, hash, 8);
+
+
+ /* AES encrypt and add result to M2 as "ATTR_ENCR_SETTINGS" */
+
+ /* Pad length of the message to encrypt to a multiple of
+ * AES_BLOCK_SIZE. Each padded byte must have their value equal
+ * to the number of padded bytes (PKCS#5 v2.0 pad).
+ */
+ num_pad_bytes = AES_BLOCK_SIZE - (abs(r - plain) % AES_BLOCK_SIZE);
+ for (i = 0; i < num_pad_bytes; i++) {
+ bufptr_put_u8(r, num_pad_bytes);
+ }
+
+ /* Add "ATTR_ENCR_SETTINGS" attribute to the M2 buffer,
+ * followed by the IV and the settings data to encrypt.
+ */
+ uint32_t setting_len = abs(r - plain);
+
+ bufptr_put_be16(r, ATTR_ENCR_SETTINGS);
+ bufptr_put_be16(r, AES_BLOCK_SIZE + setting_len);
+ iv_start = p;
+ get_random_bytes(AES_BLOCK_SIZE, p);
+ p += AES_BLOCK_SIZE;
+ data_start = p;
+ memcpy(plain, p, setting_len);
+
+ /* Encrypt the data in-place.
+ * Note that the "ATTR_ENCR_SETTINGS" attribute containes both
+ * the IV and the encrypted data.
+ */
+ bufprintf(data_start, setting_len, "Cleartext AP settings");
+ bufprintf(iv_start, AES_BLOCK_SIZE, "IV");
+
+ PLATFORM_AES_ENCRYPT(keywrapkey, iv_start, data_start, setting_len);
+
+ bufprintf(data_start, setting_len, "Encrypted AP settings");
+ }
+
+ /* authenticator */
+ {
+ /* Concatenate M1 and M2 (everything in the M2 buffer up to
+ * this point) and calculate the HMAC.
+ * Finally, append it to M2 as a attribute.
+ */
+ uint8_t hash[SHA256_MAC_LEN];
+
+ const uint8_t *addr[2];
+ size_t len[2];
+
+ addr[0] = m1;
+ addr[1] = buffer;
+ len[0] = m1_size;
+ len[1] = abs(p - buffer);
+
+ PLATFORM_HMAC_SHA256(authkey, WPS_AUTHKEY_LEN, 2, addr, len, hash);
+
+ bufptr_put_be16(p, ATTR_AUTHENTICATOR);
+ bufptr_put_be16(p, 8);
+ bufptr_put(p, hash, 8);
+ }
+
+ *m2 = buffer;
+ *m2_size = abs(p - buffer);
+
+ free(local_privkey);
+
+ return 0;
+}
+
+int wsc_process_m2(const char *ifname, void *key, uint8_t *m1, uint16_t m1_size,
+ uint8_t *m2, uint16_t m2_size, struct wps_credential *out)
+{
+ uint8_t *p;
+ struct wsc_key *k;
+
+ uint8_t mapie = 0;
+ uint8_t ssid[33];
+ uint8_t ssid_present;
+ int ssidlen = 0;
+ uint8_t bssid[6];
+ uint8_t bssid_present;
+ uint16_t auth_type;
+ uint8_t auth_type_present;
+ uint16_t enc_type;
+ uint8_t enc_type_present;
+ uint8_t network_key[64];
+ int network_keylen = 0;
+ uint8_t network_key_present;
+ //uint8_t band = 0;
+
+ uint8_t authkey[WPS_AUTHKEY_LEN];
+ uint8_t keywrapkey[WPS_KEYWRAPKEY_LEN];
+ uint8_t emsk[WPS_EMSK_LEN];
+
+ uint8_t *m2_nonce = NULL;
+ uint8_t m2_nonce_present;
+ uint8_t *m2_pubkey = NULL;
+ uint8_t m2_pubkey_present;
+ uint16_t m2_pubkey_len = 0;
+ uint8_t *m2_encrypted_settings = NULL;
+ uint8_t m2_encrypted_settings_present;
+ uint16_t m2_encrypted_settings_len = 0;
+ uint8_t *m2_authenticator = NULL;
+ uint8_t m2_authenticator_present;
+
+ //uint8_t m1_nonce_present;
+ /*uint8_t *m1_pubkey; */
+ //uint8_t m1_pubkey_present;
+ //uint16_t m1_pubkey_len;
+
+ uint8_t *m1_privkey;
+ uint16_t m1_privkey_len;
+ uint8_t *m1_macaddr;
+ uint8_t *m1_nonce;
+
+
+
+ if (!key || !m1 || m1_size == 0) {
+ fprintf(stderr, "%p %p %hu\n", key, m1, m1_size);
+ fprintf(stderr, "Missing m1 or key\n");
+ return -1;
+ }
+
+ if (m2 == NULL || m2_size == 0) {
+ fprintf(stderr, "Missing m2\n");
+ return -1;
+ }
+
+
+ k = (struct wsc_key *)key;
+ m1_privkey = k->key;
+ m1_privkey_len = k->keylen;
+ m1_macaddr = k->macaddr;
+ m1_nonce = k->nonce;
+
+
+ m2_nonce_present = 0;
+ m2_pubkey_present = 0;
+ m2_encrypted_settings_present = 0;
+ m2_authenticator_present = 0;
+ p = m2;
+
+ while (abs(p - m2) < m2_size) {
+ uint16_t attr_type;
+ uint16_t attr_len;
+
+
+ attr_type = buf_get_be16(p);
+ p += 2;
+ attr_len = buf_get_be16(p);
+ p += 2;
+
+
+ switch (attr_type) {
+ case ATTR_REGISTRAR_NONCE:
+ if (attr_len != 16) {
+ fprintf(stderr,
+ "Incorrect length (%d) for ATTR_REGISTRAR_NONCE\n", attr_len);
+ return -EINVAL;
+ }
+ m2_nonce = p;
+ m2_nonce_present = 1;
+ break;
+ case ATTR_PUBLIC_KEY:
+ m2_pubkey_len = attr_len;
+ m2_pubkey = p;
+ m2_pubkey_present = 1;
+ break;
+ case ATTR_ENCR_SETTINGS:
+ m2_encrypted_settings_len = attr_len;
+ m2_encrypted_settings = p;
+ m2_encrypted_settings_present = 1;
+ break;
+ case ATTR_AUTHENTICATOR:
+ if (attr_len != 8) {
+ fprintf(stderr,
+ "Incorrect length (%d) for ATTR_AUTHENTICATOR\n", attr_len);
+ return 0;
+ }
+ m2_authenticator = p;
+ m2_authenticator_present = 1;
+ break;
+ default:
+ break;
+ }
+
+ p += attr_len;
+ }
+
+ if (0 == m2_nonce_present ||
+ 0 == m2_pubkey_present ||
+ 0 == m2_encrypted_settings_present ||
+ 0 == m2_authenticator_present) {
+
+ fprintf(stderr, "Missing attributes in the received M2 message\n");
+ return -EINVAL;
+ }
+
+#if 0
+ m1_nonce_present = 0;
+ m1_pubkey_present = 0;
+ p = m1;
+ while (abs(p - m1) < m1_size) {
+ uint16_t attr_type;
+ uint16_t attr_len;
+
+
+ attr_type = buf_get_be16(p);
+ p += 2;
+ attr_len = buf_get_be16(p);
+ p += 2;
+
+
+ switch (attr_type) {
+ case ATTR_ENROLLEE_NONCE:
+ if (attr_len != 16) {
+ fprintf(stderr,
+ "Incorrect length (%d) for ATTR_ENROLLEE_NONCE\n", attr_len);
+ return -EINVAL;
+ }
+ m1_nonce = p;
+ m1_nonce_present = 1;
+ break;
+ case ATTR_PUBLIC_KEY:
+ m1_pubkey_len = attr_len;
+ //m1_pubkey = p;
+ m1_pubkey_present = 1;
+ break;
+ case ATTR_RF_BANDS:
+ band = *p;
+ break;
+ default:
+ break;
+ }
+
+ p += attr_len;
+ }
+
+
+ if (0 == m1_nonce_present || 0 == m1_pubkey_present) {
+ fprintf(stderr, "Missing attributes in cached M1 message\n");
+ return -EINVAL;
+ }
+#endif
+
+ /* derive keys - authkey, keywrapkey and emsk */
+ {
+ uint8_t *shared_secret;
+ uint16_t shared_secret_len;
+ const uint8_t *addr[3];
+ size_t len[3];
+ uint8_t dhkey[SHA256_MAC_LEN];
+ uint8_t kdk[SHA256_MAC_LEN];
+ uint8_t keys[WPS_AUTHKEY_LEN + WPS_KEYWRAPKEY_LEN + WPS_EMSK_LEN];
+
+
+ PLATFORM_COMPUTE_DH_SHARED_SECRET(&shared_secret,
+ &shared_secret_len,
+ m2_pubkey,
+ m2_pubkey_len,
+ m1_privkey,
+ m1_privkey_len);
+
+ addr[0] = shared_secret;
+ len[0] = shared_secret_len;
+
+ PLATFORM_SHA256(1, addr, len, dhkey);
+
+ addr[0] = m1_nonce;
+ addr[1] = m1_macaddr;
+ addr[2] = m2_nonce;
+ len[0] = 16;
+ len[1] = 6;
+ len[2] = 16;
+
+ PLATFORM_HMAC_SHA256(dhkey, SHA256_MAC_LEN, 3, addr, len, kdk);
+
+ wps_kdf(kdk, NULL, 0, "Wi-Fi Easy and Secure Key Derivation",
+ keys, sizeof(keys));
+
+ memcpy(authkey, keys, WPS_AUTHKEY_LEN);
+ memcpy(keywrapkey, keys + WPS_AUTHKEY_LEN, WPS_KEYWRAPKEY_LEN);
+ memcpy(emsk, keys + WPS_AUTHKEY_LEN + WPS_KEYWRAPKEY_LEN, WPS_EMSK_LEN);
+
+ fprintf(stderr, "WPS keys: \n");
+ bufprintf(m2_pubkey, m2_pubkey_len, "Registrar public key");
+ /* bufprintf(m1_privkey, m1_privkey_len, "Enrollee private key"); */
+ /* bufprintf(shared_secret, shared_secret_len, "DH Shared secret"); */
+ bufprintf(dhkey, 32, "DH Key");
+ bufprintf(m1_nonce, 16, "Nonce-E");
+ bufprintf(m2_nonce, 16, "Nonce-R");
+ bufprintf(kdk, 32, "KDK");
+ bufprintf(authkey, WPS_AUTHKEY_LEN, "WPS authkey");
+ bufprintf(keywrapkey, WPS_KEYWRAPKEY_LEN, "WPS keywrapkey");
+ bufprintf(emsk, WPS_EMSK_LEN, "WPS emsk");
+
+ free(shared_secret);
+ }
+
+
+ /* Verify message authentication -
+ *
+ * Concatenate M1 and M2 (excluding the last 12 bytes, where the
+ * authenticator attribute is present) and calculate the HMAC.
+ * Check it against the actual authenticator attribute value.
+ */
+ {
+ uint8_t hash[SHA256_MAC_LEN];
+ const uint8_t *addr[2];
+ size_t len[2];
+
+ addr[0] = m1;
+ addr[1] = m2;
+ len[0] = m1_size;
+ len[1] = m2_size - 12;
+
+ PLATFORM_HMAC_SHA256(authkey, WPS_AUTHKEY_LEN, 2, addr, len, hash);
+
+ if (memcmp(m2_authenticator, hash, 8)) {
+ fprintf(stderr, "Message M2 authentication failed\n");
+ return -1;
+ }
+ }
+
+ /* Decrypt the message and check the keywrap */
+ {
+ uint8_t *plain;
+ uint32_t plain_len;
+ uint8_t m2_keywrap_present;
+
+ plain = m2_encrypted_settings + AES_BLOCK_SIZE;
+ plain_len = m2_encrypted_settings_len - AES_BLOCK_SIZE;
+
+ bufprintf(plain, plain_len, "Encrypted AP settings");
+ bufprintf(m2_encrypted_settings, AES_BLOCK_SIZE, "IV");
+
+ PLATFORM_AES_DECRYPT(keywrapkey, m2_encrypted_settings, plain, plain_len);
+
+ bufprintf(plain, plain_len, "Cleartext AP settings");
+ plain_len -= plain[plain_len - 1]; /* remove padding */
+
+ /* Parse contents of AP settings */
+ ssid_present = 0;
+ bssid_present = 0;
+ auth_type_present = 0;
+ enc_type_present = 0;
+ network_key_present = 0;
+ m2_keywrap_present = 0;
+ p = plain;
+
+ while ((uint32_t)(abs(p - plain)) < plain_len) {
+ uint16_t attr_type;
+ uint16_t attr_len;
+
+ attr_type = buf_get_be16(p);
+ p += 2;
+ attr_len = buf_get_be16(p);
+ p += 2;
+
+ switch (attr_type) {
+ case ATTR_SSID:
+ if (attr_len > 32)
+ break;
+
+ bufptr_get(p, ssid, attr_len);
+ ssidlen = attr_len;
+ //ssid[attr_len] = 0x00;
+ ssid_present = 1;
+ break;
+ case ATTR_AUTH_TYPE:
+ auth_type = buf_get_be16(p);
+ p += 2;
+ auth_type_present = 1;
+ break;
+ case ATTR_ENCR_TYPE:
+ enc_type = buf_get_be16(p);
+ p += 2;
+ enc_type_present = 1;
+ break;
+ case ATTR_NETWORK_KEY:
+ bufptr_get(p, network_key, attr_len);
+ network_keylen = attr_len;
+ //network_key[attr_len] = 0x00;
+ network_key_present = 1;
+ break;
+ case ATTR_MAC_ADDR:
+ bufptr_get(p, bssid, attr_len);
+ bssid_present = 1;
+ break;
+ case ATTR_KEY_WRAP_AUTH:
+ {
+ uint8_t *end_of_hmac;
+ uint8_t hash[SHA256_MAC_LEN];
+ const uint8_t *addr[1];
+ size_t len[1];
+
+ end_of_hmac = p - 4;
+ addr[0] = plain;
+ len[0] = abs(end_of_hmac - plain);
+
+ PLATFORM_HMAC_SHA256(authkey, WPS_AUTHKEY_LEN, 1, addr, len, hash);
+
+ if (memcmp(p, hash, 8)) {
+ fprintf(stderr, "M2 keywrap check failed\n");
+ return -1;
+ }
+ m2_keywrap_present = 1;
+ }
+ break;
+ case ATTR_VENDOR_EXTENSION:
+ {
+ uint8_t id[3];
+ uint8_t *end_of_mapie;
+ uint8_t subelem;
+ uint8_t len;
+
+ // May be one or more subelements (Section 12 of WSC spec)
+ end_of_mapie = p + attr_len;
+ bufptr_get(p, id, sizeof(id));
+
+ if (id[0] ^ WFA_VENDOR_ID_1
+ || id[1] ^ WFA_VENDOR_ID_2
+ || id[2] ^ WFA_VENDOR_ID_3) {
+
+ //p += (attr_len - sizeof(id));
+ p -= sizeof(id);
+ break;
+ }
+
+ while (p < end_of_mapie) {
+ bufptr_get(p, &subelem, 1);
+ bufptr_get(p, &len, 1);
+ if (!(subelem ^ WFA_MAP_EXT_ATTR)) {
+ /* MAP extension subelement is 1 byte */
+ bufptr_get(p, &mapie, 1);
+ } else {
+ p += len;
+ }
+ }
+ }
+ break;
+ default:
+ break;
+ }
+
+ p += attr_len;
+ }
+
+ if ( (ssid_present &
+ bssid_present &
+ auth_type_present &
+ enc_type_present &
+ network_key_present &
+ m2_keywrap_present) == 0) {
+ fprintf(stderr, "WSC M2 is missing settings attributes\n");
+ return -1;
+ }
+ }
+
+ memcpy(out->ssid, ssid, ssidlen);
+ memcpy(out->key, network_key, network_keylen);
+ out->auth_type = auth_type;
+ out->enc_type = enc_type;
+ out->mapie = mapie;
+ memcpy(out->macaddr, bssid, 6);
+ //out->output.band = band;
+
+ return 0;
+}
diff --git a/src/i1905_wsc.h b/src/i1905_wsc.h
new file mode 100644
index 0000000000000000000000000000000000000000..92f88dc286f873d5c9691506bd09cfdee2c51d73
--- /dev/null
+++ b/src/i1905_wsc.h
@@ -0,0 +1,189 @@
+
+
+#ifndef I1905_WSC_H
+#define I1905_WSC_H
+
+
+#include <stdint.h>
+
+#define ATTR_VERSION (0x104a)
+#define ATTR_MSG_TYPE (0x1022)
+#define WPS_M1 (0x04)
+#define WPS_M2 (0x05)
+#define ATTR_UUID_E (0x1047)
+#define ATTR_UUID_R (0x1048)
+#define ATTR_MAC_ADDR (0x1020)
+#define ATTR_ENROLLEE_NONCE (0x101a)
+#define ATTR_REGISTRAR_NONCE (0x1039)
+#define ATTR_PUBLIC_KEY (0x1032)
+#define ATTR_AUTH_TYPE_FLAGS (0x1004)
+#define WPS_AUTH_OPEN (0x0001)
+#define WPS_AUTH_WPAPSK (0x0002)
+#define WPS_AUTH_SHARED (0x0004) /* deprecated */
+#define WPS_AUTH_WPA (0x0008)
+#define WPS_AUTH_WPA2 (0x0010)
+#define WPS_AUTH_WPA2PSK (0x0020)
+#define ATTR_ENCR_TYPE_FLAGS (0x1010)
+#define WPS_ENCR_NONE (0x0001)
+#define WPS_ENCR_WEP (0x0002) /* deprecated */
+#define WPS_ENCR_TKIP (0x0004)
+#define WPS_ENCR_AES (0x0008)
+#define ATTR_CONN_TYPE_FLAGS (0x100d)
+#define WPS_CONN_ESS (0x01)
+#define WPS_CONN_IBSS (0x02)
+#define ATTR_CONFIG_METHODS (0x1008)
+#define WPS_CONFIG_VIRT_PUSHBUTTON (0x0280)
+#define WPS_CONFIG_PHY_PUSHBUTTON (0x0480)
+#define ATTR_WPS_STATE (0x1044)
+#define WPS_STATE_NOT_CONFIGURED (1)
+#define WPS_STATE_CONFIGURED (2)
+#define ATTR_MANUFACTURER (0x1021)
+#define ATTR_MODEL_NAME (0x1023)
+#define ATTR_MODEL_NUMBER (0x1024)
+#define ATTR_SERIAL_NUMBER (0x1042)
+#define ATTR_PRIMARY_DEV_TYPE (0x1054)
+#define WPS_DEV_COMPUTER (1)
+#define WPS_DEV_COMPUTER_PC (1)
+#define WPS_DEV_COMPUTER_SERVER (2)
+#define WPS_DEV_COMPUTER_MEDIA_CENTER (3)
+#define WPS_DEV_COMPUTER_ULTRA_MOBILE (4)
+#define WPS_DEV_COMPUTER_NOTEBOOK (5)
+#define WPS_DEV_COMPUTER_DESKTOP (6)
+#define WPS_DEV_COMPUTER_MID (7)
+#define WPS_DEV_COMPUTER_NETBOOK (8)
+#define WPS_DEV_COMPUTER_TABLET (9)
+#define WPS_DEV_INPUT (2)
+#define WPS_DEV_INPUT_KEYBOARD (1)
+#define WPS_DEV_INPUT_MOUSE (2)
+#define WPS_DEV_INPUT_JOYSTICK (3)
+#define WPS_DEV_INPUT_TRACKBALL (4)
+#define WPS_DEV_INPUT_GAMING (5)
+#define WPS_DEV_INPUT_REMOTE (6)
+#define WPS_DEV_INPUT_TOUCHSCREEN (7)
+#define WPS_DEV_INPUT_BIOMETRIC_READER (8)
+#define WPS_DEV_INPUT_BARCODE_READER (9)
+#define WPS_DEV_PRINTER (3)
+#define WPS_DEV_PRINTER_PRINTER (1)
+#define WPS_DEV_PRINTER_SCANNER (2)
+#define WPS_DEV_PRINTER_FAX (3)
+#define WPS_DEV_PRINTER_COPIER (4)
+#define WPS_DEV_PRINTER_ALL_IN_ONE (5)
+#define WPS_DEV_CAMERA (4)
+#define WPS_DEV_CAMERA_DIGITAL_STILL_CAMERA (1)
+#define WPS_DEV_CAMERA_VIDEO (2)
+#define WPS_DEV_CAMERA_WEB (3)
+#define WPS_DEV_CAMERA_SECURITY (4)
+#define WPS_DEV_STORAGE (5)
+#define WPS_DEV_STORAGE_NAS (1)
+#define WPS_DEV_NETWORK_INFRA (6)
+#define WPS_DEV_NETWORK_INFRA_AP (1)
+#define WPS_DEV_NETWORK_INFRA_ROUTER (2)
+#define WPS_DEV_NETWORK_INFRA_SWITCH (3)
+#define WPS_DEV_NETWORK_INFRA_GATEWAY (4)
+#define WPS_DEV_NETWORK_INFRA_BRIDGE (5)
+#define WPS_DEV_DISPLAY (7)
+#define WPS_DEV_DISPLAY_TV (1)
+#define WPS_DEV_DISPLAY_PICTURE_FRAME (2)
+#define WPS_DEV_DISPLAY_PROJECTOR (3)
+#define WPS_DEV_DISPLAY_MONITOR (4)
+#define WPS_DEV_MULTIMEDIA (8)
+#define WPS_DEV_MULTIMEDIA_DAR (1)
+#define WPS_DEV_MULTIMEDIA_PVR (2)
+#define WPS_DEV_MULTIMEDIA_MCX (3)
+#define WPS_DEV_MULTIMEDIA_SET_TOP_BOX (4)
+#define WPS_DEV_MULTIMEDIA_MEDIA_SERVER (5)
+#define WPS_DEV_MULTIMEDIA_PORTABLE_VIDEO_PLAYER (6)
+#define WPS_DEV_GAMING (9)
+#define WPS_DEV_GAMING_XBOX (1)
+#define WPS_DEV_GAMING_XBOX360 (2)
+#define WPS_DEV_GAMING_PLAYSTATION (3)
+#define WPS_DEV_GAMING_GAME_CONSOLE (4)
+#define WPS_DEV_GAMING_PORTABLE_DEVICE (5)
+#define WPS_DEV_PHONE (10)
+#define WPS_DEV_PHONE_WINDOWS_MOBILE (1)
+#define WPS_DEV_PHONE_SINGLE_MODE (2)
+#define WPS_DEV_PHONE_DUAL_MODE (3)
+#define WPS_DEV_PHONE_SP_SINGLE_MODE (4)
+#define WPS_DEV_PHONE_SP_DUAL_MODE (5)
+#define WPS_DEV_AUDIO (11)
+#define WPS_DEV_AUDIO_TUNER_RECV (1)
+#define WPS_DEV_AUDIO_SPEAKERS (2)
+#define WPS_DEV_AUDIO_PMP (3)
+#define WPS_DEV_AUDIO_HEADSET (4)
+#define WPS_DEV_AUDIO_HEADPHONES (5)
+#define WPS_DEV_AUDIO_MICROPHONE (6)
+#define WPS_DEV_AUDIO_HOME_THEATRE (7)
+#define ATTR_DEV_NAME (0x1011)
+#define ATTR_RF_BANDS (0x103c)
+#define WPS_RF_24GHZ (0x01)
+#define WPS_RF_50GHZ (0x02)
+#define WPS_RF_60GHZ (0x04)
+#define ATTR_ASSOC_STATE (0x1002)
+#define WPS_ASSOC_NOT_ASSOC (0)
+#define WPS_ASSOC_CONN_SUCCESS (1)
+#define ATTR_DEV_PASSWORD_ID (0x1012)
+#define DEV_PW_PUSHBUTTON (0x0004)
+#define ATTR_CONFIG_ERROR (0x1009)
+#define WPS_CFG_NO_ERROR (0)
+#define ATTR_OS_VERSION (0x102d)
+#define ATTR_VENDOR_EXTENSION (0x1049)
+#define WPS_VENDOR_ID_WFA_1 (0x00)
+#define WPS_VENDOR_ID_WFA_2 (0x37)
+#define WPS_VENDOR_ID_WFA_3 (0x2A)
+#define WFA_VENDOR_ID_1 WPS_VENDOR_ID_WFA_1
+#define WFA_VENDOR_ID_2 WPS_VENDOR_ID_WFA_2
+#define WFA_VENDOR_ID_3 WPS_VENDOR_ID_WFA_3
+
+#define WFA_ELEM_VERSION2 (0x00)
+#define WPS_VERSION (0x20)
+
+#define WFA_MAP_EXT_ATTR (0x06)
+
+
+#define ATTR_SSID (0x1045)
+#define ATTR_AUTH_TYPE (0x1003)
+#define ATTR_ENCR_TYPE (0x100f)
+#define ATTR_NETWORK_KEY (0x1027)
+#define ATTR_KEY_WRAP_AUTH (0x101e)
+#define ATTR_ENCR_SETTINGS (0x1018)
+#define ATTR_AUTHENTICATOR (0x1005)
+
+
+struct wps_credential {
+ uint8_t ssid[32];
+ size_t ssidlen;
+ uint16_t auth_type;
+ uint16_t enc_type;
+ uint8_t key[64];
+ size_t keylen;
+ uint8_t macaddr[6];
+ uint8_t band;
+ uint8_t mapie; // TODO: extra attr handling
+};
+
+struct wps_data {
+ uint8_t ssid[64];
+ uint8_t network_key[64];
+ uint16_t auth_types;
+ uint16_t enc_types;
+ uint8_t macaddr[6];
+ uint8_t band;
+ uint8_t mapie;
+};
+
+
+uint8_t wsc_get_message_type(uint8_t *m, uint16_t m_size);
+
+
+int wsc_build_m1(const char *ifname, uint8_t **m1, uint16_t *m1_size, void **key,
+ struct wps_data *in);
+
+int wsc_build_m2(uint8_t *m1, uint16_t m1_size, uint8_t **m2, uint16_t *m2_size,
+ struct wps_credential *in);
+
+int wsc_process_m2(const char *ifname, void *key, uint8_t *m1, uint16_t m1_size,
+ uint8_t *m2, uint16_t m2_size, struct wps_credential *out);
+
+
+
+#endif /* I1905_WSC_H */