From 07e4c7f2fa111eb03f9b0deb3e1d88bd7511079e Mon Sep 17 00:00:00 2001
From: Anjan Chanda <anjan.chanda@iopsys.eu>
Date: Fri, 15 Jan 2021 15:20:35 +0100
Subject: [PATCH] misc changes including WSC M1 and M2 verification
is_cmdu_type_response() returns true only for response to unicast type.
cmdu_alloc_simple() returns assigned 'mid' when passed as 0.
record receiving interface name in 'dev_ifname' of struct cmdu_buff.
Extend 'cmdu_ackq' to allow passing of user cookie so that it can be checked
in the response path, if needed.
Fix ap autoconfig search and response handling.
Fix WSC M2 message construction and processing.
---
src/cmdu.c | 11 ++--
src/cmdu.h | 3 +-
src/cmdu_ackq.c | 25 +++++---
src/cmdu_ackq.h | 5 +-
src/cmdu_input.c | 154 ++++++++++++++++++++++++++++++++++------------
src/cmdu_output.c | 94 ++++++++++++++++++++++++----
src/i1905.c | 23 ++++++-
src/i1905.h | 12 +++-
src/i1905_wsc.c | 29 +++++----
9 files changed, 271 insertions(+), 85 deletions(-)
diff --git a/src/cmdu.c b/src/cmdu.c
index 13e972bf..029141b5 100644
--- a/src/cmdu.c
+++ b/src/cmdu.c
@@ -101,11 +101,12 @@ int cmdu_should_relay(uint16_t type)
type == CMDU_TYPE_PUSH_BUTTON_JOIN_NOTIFICATION);
}
+/* for unicast request types only */
int is_cmdu_type_response(uint16_t type)
{
return (type == CMDU_TYPE_TOPOLOGY_RESPONSE ||
type == CMDU_TYPE_LINK_METRIC_RESPONSE ||
- type == CMDU_TYPE_AP_AUTOCONFIGURATION_RESPONSE ||
+ /* type == CMDU_TYPE_AP_AUTOCONFIGURATION_RESPONSE || */
type == CMDU_TYPE_HIGHER_LAYER_RESPONSE ||
type == CMDU_TYPE_INTERFACE_POWER_CHANGE_RESPONSE);
}
@@ -172,7 +173,7 @@ struct cmdu_buff *cmdu_alloc_default(void)
return cmdu_alloc(ETH_FRAME_SZ);
}
-struct cmdu_buff *cmdu_alloc_simple(uint16_t type, uint16_t mid)
+struct cmdu_buff *cmdu_alloc_simple(uint16_t type, uint16_t *mid)
{
struct cmdu_buff *f;
@@ -187,10 +188,10 @@ struct cmdu_buff *cmdu_alloc_simple(uint16_t type, uint16_t mid)
if (type <= CMDU_TYPE_MAX)
cmdu_set_type(f, type);
- if (mid == 0)
- mid = cmdu_get_next_mid();
+ if (*mid == 0)
+ *mid = cmdu_get_next_mid();
- cmdu_set_mid(f, mid);
+ cmdu_set_mid(f, *mid);
CMDU_SET_LAST_FRAGMENT(f->cdata);
f->data = (uint8_t *)(f->cdata + 1);
diff --git a/src/cmdu.h b/src/cmdu.h
index 9a78e8d5..e435cd1d 100644
--- a/src/cmdu.h
+++ b/src/cmdu.h
@@ -53,6 +53,7 @@ struct cmdu_buff {
uint8_t *tail;
uint8_t *end;
uint8_t dev_macaddr[6];
+ char dev_ifname[16];
uint8_t origin[6];
uint16_t datalen;
uint16_t len;
@@ -96,7 +97,7 @@ void tlv_free_linear(struct tlv *t);
struct cmdu_buff *cmdu_alloc(int size);
struct cmdu_buff *cmdu_alloc_default(void);
-struct cmdu_buff *cmdu_alloc_simple(uint16_t type, uint16_t mid);
+struct cmdu_buff *cmdu_alloc_simple(uint16_t type, uint16_t *mid);
void cmdu_free(struct cmdu_buff *c);
diff --git a/src/cmdu_ackq.c b/src/cmdu_ackq.c
index 9dd93d54..20cf4618 100644
--- a/src/cmdu_ackq.c
+++ b/src/cmdu_ackq.c
@@ -40,7 +40,8 @@ static int timeradd_msecs(struct timeval *a, unsigned long msecs,
}
struct cmdu_ackq_entry *cmdu_ackq_create_msg(uint16_t type, uint16_t mid,
- uint8_t *dest, uint32_t timeout)
+ uint8_t *dest, uint32_t timeout,
+ void *cookie)
{
struct cmdu_ackq_entry *msg;
struct timeval tsp = { 0 };
@@ -60,6 +61,7 @@ struct cmdu_ackq_entry *cmdu_ackq_create_msg(uint16_t type, uint16_t mid,
//msg->ageing_tmo.tv_usec = roundup(msg->ageing_tmo.tv_usec, 1000);
msg->ageing_tmo.tv_usec = (msg->ageing_tmo.tv_usec / 1000) * 1000;
memcpy(msg->origin, dest, 6);
+ msg->cookie = cookie;
fprintf(stderr,
" CREATE msg: type = 0x%04x mid = %hu origin = " MACFMT " timeout = { %u (%lu:%lu) }\n",
type, mid, MAC2STR(dest), msg->ageing_time, msg->ageing_tmo.tv_sec, msg->ageing_tmo.tv_usec / 1000);
@@ -69,8 +71,12 @@ struct cmdu_ackq_entry *cmdu_ackq_create_msg(uint16_t type, uint16_t mid,
static void cmdu_ackq_delete_msg(struct cmdu_ackq_entry *msg)
{
- if (msg)
+ if (msg) {
+ if (msg->cookie)
+ free(msg->cookie);
+
free(msg);
+ }
}
static void cmdu_ackq_ageout_entry(struct cmdu_ackq *st, struct hlist_head *head,
@@ -96,7 +102,7 @@ static void cmdu_ackq_ageout_entry(struct cmdu_ackq *st, struct hlist_head *head
hlist_del(&msg->hlist, head);
fprintf(stderr, "No response from " MACFMT " for CMDU 0x%x with mid = %hu\n",
MAC2STR(msg->origin), msg->mid, msg->type);
- cmdu_ackq_delete_msg(msg);
+ cmdu_ackq_delete_msg(msg); //TODO: user decides timeout action
} else {
struct timeval new_next_tmo = { 0 };
@@ -210,7 +216,7 @@ void cmdu_ackq_reset(void *cmdu_q)
/* In this function, type = cmdutype that is expected with 'mid' from 'dest' */
int cmdu_ackq_enqueue(void *cmdu_q, uint16_t type, uint16_t mid, uint8_t *dest,
- uint32_t timeout)
+ uint32_t timeout, void *cookie)
{
struct cmdu_ackq *q = (struct cmdu_ackq *)cmdu_q;
struct cmdu_ackq_entry *msg = NULL;
@@ -224,7 +230,7 @@ int cmdu_ackq_enqueue(void *cmdu_q, uint16_t type, uint16_t mid, uint8_t *dest,
return -1;
}
- msg = cmdu_ackq_create_msg(type, mid, dest, timeout);
+ msg = cmdu_ackq_create_msg(type, mid, dest, timeout, cookie);
if (msg) {
int idx = cmdu_ackq_hash(type, mid, dest);
@@ -233,7 +239,7 @@ int cmdu_ackq_enqueue(void *cmdu_q, uint16_t type, uint16_t mid, uint8_t *dest,
q->pending_cnt++;
fprintf(stderr,
- "ENQ: type = 0x%04x mid = %hu origin = " MACFMT "\n",
+ " ENQ: type = 0x%04x mid = %hu origin = " MACFMT "\n",
type, mid, MAC2STR(dest));
@@ -259,7 +265,7 @@ int cmdu_ackq_enqueue(void *cmdu_q, uint16_t type, uint16_t mid, uint8_t *dest,
return -1;
}
-int cmdu_ackq_dequeue(void *cmdu_q, uint16_t type, uint16_t mid, uint8_t *src)
+int cmdu_ackq_dequeue(void *cmdu_q, uint16_t type, uint16_t mid, uint8_t *src, void **cookie)
{
struct cmdu_ackq *q = (struct cmdu_ackq *)cmdu_q;
struct cmdu_ackq_entry *msg = NULL;
@@ -285,6 +291,11 @@ int cmdu_ackq_dequeue(void *cmdu_q, uint16_t type, uint16_t mid, uint8_t *src)
"DEQ: type = 0x%04x mid = %hu origin = " MACFMT "\n",
type, mid, MAC2STR(src));
+
+ /* After returning cookie back to user, we can safely delete the msg */
+ *cookie = msg->cookie;
+ msg->cookie = NULL;
+
cmdu_ackq_delete_msg(msg);
return 0;
diff --git a/src/cmdu_ackq.h b/src/cmdu_ackq.h
index 96a04323..24cc12e4 100644
--- a/src/cmdu_ackq.h
+++ b/src/cmdu_ackq.h
@@ -31,6 +31,7 @@ struct cmdu_ackq_entry {
uint16_t type;
uint16_t mid;
uint8_t origin[6];
+ void *cookie;
struct hlist_node hlist;
uint32_t ageing_time; /* in msecs */
@@ -51,9 +52,9 @@ struct cmdu_ackq {
extern int cmdu_ackq_init(void *q);
extern void cmdu_ackq_reset(void *q);
-extern int cmdu_ackq_dequeue(void *q, uint16_t type, uint16_t mid, uint8_t *src);
+extern int cmdu_ackq_dequeue(void *q, uint16_t type, uint16_t mid, uint8_t *src, void **cookie);
extern int cmdu_ackq_enqueue(void *q, uint16_t type, uint16_t mid, uint8_t *dest,
- uint32_t timeout);
+ uint32_t timeout, void *cookie);
#endif /* CMDU_ACKQ_H */
diff --git a/src/cmdu_input.c b/src/cmdu_input.c
index 33ae1987..0a574a07 100644
--- a/src/cmdu_input.c
+++ b/src/cmdu_input.c
@@ -52,7 +52,7 @@
int i1905_handle_topology_discovery(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
struct tlv_policy a_policy[] = {
[0] = { .type = TLV_TYPE_AL_MAC_ADDRESS_TYPE, .present = TLV_PRESENT_ONE },
@@ -95,7 +95,7 @@ int i1905_handle_topology_discovery(struct i1905_interface_private *pif,
}
int i1905_handle_topology_notification(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
struct tlv_policy a_policy[] = {
[0] = { .type = TLV_TYPE_AL_MAC_ADDRESS_TYPE, .present = TLV_PRESENT_ONE },
@@ -136,13 +136,13 @@ int i1905_handle_topology_notification(struct i1905_interface_private *pif,
}
int i1905_handle_topology_query(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
return i1905_send_topology_response(pif, rxf->origin, cmdu_get_mid(rxf));
}
int i1905_handle_topology_response(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
struct tlv_policy a_policy[] = {
[0] = { .type = TLV_TYPE_DEVICE_INFORMATION_TYPE, .present = TLV_PRESENT_ONE },
@@ -218,14 +218,14 @@ int i1905_handle_topology_response(struct i1905_interface_private *pif,
}
int i1905_handle_vendor_request(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
//TODO
return 0;
}
int i1905_handle_link_metric_query(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
struct tlv_policy a_policy[] = {
[0] = { .type = TLV_TYPE_LINK_METRIC_QUERY, .present = TLV_PRESENT_ONE },
@@ -270,7 +270,7 @@ int i1905_handle_link_metric_query(struct i1905_interface_private *pif,
}
int i1905_handle_link_metric_response(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
struct tlv_policy a_policy[] = {
[0] = { .type = TLV_TYPE_TRANSMITTER_LINK_METRIC, .present = TLV_PRESENT_MORE},
@@ -332,18 +332,18 @@ 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)
+ struct cmdu_buff *rxf, void *cookie)
{
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 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;
+ int ret = 0;
fprintf(stderr, "%s -------------->\n", __func__);
@@ -383,17 +383,19 @@ int i1905_handle_ap_autoconfig_search(struct i1905_interface_private *pif,
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");
+ fprintf(stderr, "%s: XXXXXXXXXXXX %d XXXXXXXXXXXX\n", __func__, __LINE__);
+ //TODO: TODO: TODO
+ //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)
+ struct cmdu_buff *rxf, void *cookie)
{
struct tlv_policy a_policy[] = {
[0] = { .type = TLV_TYPE_SUPPORTED_ROLE, .present = TLV_PRESENT_ONE },
@@ -405,6 +407,7 @@ int i1905_handle_ap_autoconfig_response(struct i1905_interface_private *pif,
struct i1905_interface *ifs;
struct tlv *tv[2][16];
int ret;
+ struct i1905_interface *outif;
fprintf(stderr, "%s -------------->\n", __func__);
@@ -435,12 +438,23 @@ int i1905_handle_ap_autoconfig_response(struct i1905_interface_private *pif,
//registrar_established.
//Else, send WSC-M1 for each unconfigured i1905 WiFi radios
+ outif = i1905_ifname_to_interface(pif->i1905private, rxf->dev_ifname);
+ if (!outif) {
+ fprintf(stderr, "Error handling AP-AUTOCONFIG RESPONSE!\n");
+ return -1;
+ }
+
list_for_each_entry(ifs, &self->iflist, list) {
+ if (IS_MEDIA_WIFI_2GHZ(ifs->media) && freq->band != IEEE80211_FREQUENCY_BAND_2_4_GHZ)
+ continue;
+
+ if (IS_MEDIA_WIFI_5GHZ(ifs->media) && freq->band != IEEE80211_FREQUENCY_BAND_5_GHZ)
+ continue;
+
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);
+ ret = i1905_send_ap_autoconfig_wsc_m1(outif->priv, ifs->priv, rxf->origin);
if (ret) {
fprintf(stderr, "Error sending AP_AUTOCONFIG_WSC_M1\n");
break;
@@ -452,7 +466,7 @@ int i1905_handle_ap_autoconfig_response(struct i1905_interface_private *pif,
}
int i1905_handle_ap_autoconfig_renew(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
struct tlv_policy a_policy[] = {
[0] = { .type = TLV_TYPE_AL_MAC_ADDRESS_TYPE, .present = TLV_PRESENT_ONE },
@@ -494,7 +508,7 @@ int i1905_handle_ap_autoconfig_renew(struct i1905_interface_private *pif,
list_for_each_entry(ifs, &self->iflist, list) {
if (IS_MEDIA_WIFI(ifs->media)) {
//TODO: check band in supported-bands
- ret = i1905_send_ap_autoconfig_wsc_m1(ifs->priv, rxf->origin);
+ ret = i1905_send_ap_autoconfig_wsc_m1(ifs->priv, ifs->priv, rxf->origin); //FIXME priv
if (ret) {
fprintf(stderr, "Error sending AP_AUTOCONFIG_WSC_M1\n");
break;
@@ -506,14 +520,15 @@ int i1905_handle_ap_autoconfig_renew(struct i1905_interface_private *pif,
}
static int i1905_wsc_process_m1(struct i1905_interface_private *pif,
- uint8_t *msg, uint16_t msglen)
+ uint8_t *msg, uint16_t msglen, uint16_t mid,
+ uint8_t *from)
{
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;
+ struct wps_credential cred = {0};
//ret = wsc_process_m1(iface->ifname, msg, msglen);
@@ -521,40 +536,80 @@ static int i1905_wsc_process_m1(struct i1905_interface_private *pif,
//TODO: get cred from config file
//i1905_interface_credential(&cred);
-
+#if 1 // TESTING TODO
+ strcpy((char *)cred.ssid, "TestSSID-123");
+ cred.ssidlen = strlen("TestSSID-123");
+ cred.auth_type = 0;
+ cred.enc_type = 0x4;
+ strcpy((char *)cred.key, "password");
+ cred.keylen = strlen("password");
+ cred.band = 2;
+#endif
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;
}
+ ret = i1905_send_ap_autoconfig_wsc_m2(pif, m2, m2_size, mid, from);
return 0;
}
static int i1905_wsc_process_m2(struct i1905_interface_private *pif,
- uint8_t *msg, uint16_t msglen)
+ uint8_t *msg, uint16_t msglen, uint16_t mid,
+ uint8_t *from, void *cookie)
{
struct i1905_interface *iface = i1905_interface_priv(pif);
- struct i1905_interface_private_wsc *wsc = pif->wsc;
+ struct i1905_interface_private *ifpriv;
+ struct i1905_interface_private_wsc *wsc;
struct wps_credential out = {0};
int ret;
+ char *ifname = cookie;
+ struct i1905_interface *ifwsc;
- ret = wsc_process_m2(iface->ifname, wsc->key, wsc->last_msg,
+ fprintf(stderr, "%s: Process M2. ingress ifname = %s (cookie = %s)\n", __func__, iface->ifname, ifname);
+
+ if (!cookie) {
+ fprintf(stderr,
+ "cookie = NULL! Valid cookie expected for WSC-M2 processing!\n");
+ return 0;
+ }
+
+
+ ifwsc = i1905_ifname_to_interface(pif->i1905private, ifname);
+ if (!ifwsc) {
+ fprintf(stderr, "Failed to match interface cookie '%s'\n", ifname);
+ return -1;
+ }
+
+ ifpriv = ifwsc->priv;
+ wsc = ifpriv->wsc;
+ if (!wsc) {
+ fprintf(stderr, "%s: wsc = NULL! Unexpected error!\n", __func__);
+ return -1;
+ }
+
+ ret = wsc_process_m2(ifwsc->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);
+ fprintf(stderr, "Error processing WSC M2 for '%s'\n", ifwsc->ifname);
return ret;
}
/* TODO: update wireless config */
-
+#if 1 // TESTING
+ fprintf(stderr, "####################################################\n");
+ fprintf(stderr, "Ssid: %s (len = %u)\n", out.ssid, out.ssidlen);
+ fprintf(stderr, "Key : %s (len = %u)\n", out.key, out.keylen);
+ fprintf(stderr, "####################################################\n");
+#endif
return 0;
}
int i1905_handle_ap_autoconfig_wsc(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
struct tlv_policy a_policy[] = {
[0] = { .type = TLV_TYPE_WSC, .present = TLV_PRESENT_ONE },
@@ -565,7 +620,9 @@ int i1905_handle_ap_autoconfig_wsc(struct i1905_interface_private *pif,
uint8_t wsc_msgtype;
uint16_t msglen;
uint8_t *msg;
+ uint16_t mid;
int ret = 0;
+ void *m1_cookie = NULL;
fprintf(stderr, "%s -------------->\n", __func__);
@@ -578,15 +635,29 @@ int i1905_handle_ap_autoconfig_wsc(struct i1905_interface_private *pif,
msg = tv[0][0]->data;
msglen = tlv_length(tv[0][0]);
+ mid = cmdu_get_mid(rxf);
+
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);
+ ret = i1905_wsc_process_m1(pif, msg, msglen, mid, rxf->origin);
break;
case WPS_M2:
fprintf(stderr, "Received WPS M2\n");
- ret = i1905_wsc_process_m2(pif, msg, msglen);
+
+ ret = cmdu_ackq_dequeue(&pif->txack_q,
+ CMDU_TYPE_AP_AUTOCONFIGURATION_WSC,
+ mid, rxf->origin, &m1_cookie);
+ if (!ret && m1_cookie) {
+ ret = i1905_wsc_process_m2(pif, msg, msglen, mid, rxf->origin, m1_cookie);
+ free(m1_cookie);
+ } else {
+ fprintf(stderr,
+ "%s: drop unexpected WSC-M2 CMDU (mid = %d)\n",
+ __func__, mid);
+ return -1;
+ }
break;
default:
fprintf(stderr, "Received WPS msgtype %u\n", wsc_msgtype);
@@ -597,27 +668,27 @@ int i1905_handle_ap_autoconfig_wsc(struct i1905_interface_private *pif,
}
int i1905_handle_pbc_notification(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
//TODO
return 0;
}
int i1905_handle_pbc_join_notification(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
//TODO
return 0;
}
int i1905_handle_higherlayer_query(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
return i1905_send_higherlayer_response(pif, rxf->origin, cmdu_get_mid(rxf));
}
int i1905_handle_higherlayer_response(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
struct tlv_policy a_policy[] = {
[0] = { .type = TLV_TYPE_AL_MAC_ADDRESS_TYPE, .present = TLV_PRESENT_ONE },
@@ -683,28 +754,28 @@ int i1905_handle_higherlayer_response(struct i1905_interface_private *pif,
}
int i1905_handle_interface_power_request(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
//TODO
return 0;
}
int i1905_handle_interface_power_response(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
//TODO
return 0;
}
int i1905_handle_generic_phy_query(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
//TODO
return 0;
}
int i1905_handle_generic_phy_response(struct i1905_interface_private *pif,
- struct cmdu_buff *rxf)
+ struct cmdu_buff *rxf, void *cookie)
{
//TODO
return 0;
@@ -712,7 +783,7 @@ int i1905_handle_generic_phy_response(struct i1905_interface_private *pif,
typedef int (*cmdu_handler_t)(struct i1905_interface_private *ifp,
- struct cmdu_buff *rxf);
+ struct cmdu_buff *rxf, void *cookie);
static const cmdu_handler_t i1905ftable[] = {
[0x00] = i1905_handle_topology_discovery,
@@ -744,6 +815,7 @@ int i1905_process_cmdu(struct i1905_interface_private *pif, struct cmdu_buff *rx
uint8_t *src;
cmdu_res_t res;
int ret;
+ void *cookie = NULL;
if (!rxf->cdata) {
@@ -781,7 +853,7 @@ int i1905_process_cmdu(struct i1905_interface_private *pif, struct cmdu_buff *rx
if (is_cmdu_type_response(type)) {
/* discard responses with no matching request awaiting */
- ret = cmdu_ackq_dequeue(&pif->txack_q, type, mid, src);
+ ret = cmdu_ackq_dequeue(&pif->txack_q, type, mid, src, &cookie);
if (ret) {
fprintf(stderr, "%s: drop unexpected CMDU (mid = %d)\n",
__func__, mid);
@@ -790,7 +862,7 @@ int i1905_process_cmdu(struct i1905_interface_private *pif, struct cmdu_buff *rx
}
if (i1905ftable[type])
- ret = i1905ftable[type](pif, rxf);
+ ret = i1905ftable[type](pif, rxf, cookie);
return ret;
diff --git a/src/cmdu_output.c b/src/cmdu_output.c
index 60a186f6..c94150ea 100644
--- a/src/cmdu_output.c
+++ b/src/cmdu_output.c
@@ -53,6 +53,7 @@ int i1905_send_topology_query(struct i1905_interface_private *pif, uint8_t *dest
struct i1905_interface *iface = i1905_interface_priv(pif);
struct cmdu_buff *frm = NULL;
int ret = 0;
+ uint16_t mid = 0;
if (!dest)
@@ -62,7 +63,7 @@ int i1905_send_topology_query(struct i1905_interface_private *pif, uint8_t *dest
fprintf(stderr, "%s: Send TOPOLOGY QUERY to " MACFMT "\n", __func__,
MAC2STR(dest));
- frm = cmdu_alloc_simple(CMDU_TYPE_TOPOLOGY_QUERY, 0);
+ frm = cmdu_alloc_simple(CMDU_TYPE_TOPOLOGY_QUERY, &mid);
if (!frm) {
fprintf(stderr, "%s: -ENOMEM\n", __func__);
return -1;
@@ -103,7 +104,7 @@ int i1905_send_topology_response(struct i1905_interface_private *pif,
- resp = cmdu_alloc_simple(CMDU_TYPE_TOPOLOGY_RESPONSE, mid);
+ resp = cmdu_alloc_simple(CMDU_TYPE_TOPOLOGY_RESPONSE, &mid);
if (!resp) {
fprintf(stderr, "%s: -ENOMEM\n", __func__);
return -1;
@@ -280,7 +281,7 @@ int i1905_send_link_metric_response(struct i1905_interface_private *pif,
- resp = cmdu_alloc_simple(CMDU_TYPE_LINK_METRIC_RESPONSE, mid);
+ resp = cmdu_alloc_simple(CMDU_TYPE_LINK_METRIC_RESPONSE, &mid);
if (!resp) {
fprintf(stderr, "%s: -ENOMEM\n", __func__);
return -1;
@@ -472,6 +473,7 @@ int i1905_send_ap_autoconfig_search(struct i1905_private *priv, uint8_t freqband
struct cmdu_buff *frm = NULL;
struct tlv *t;
int ret = 0;
+ uint16_t mid = 0;
/*
@@ -483,7 +485,7 @@ int i1905_send_ap_autoconfig_search(struct i1905_private *priv, uint8_t freqband
*/
- frm = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_SEARCH, 0);
+ frm = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_SEARCH, &mid);
if (!frm) {
fprintf(stderr, "%s: -ENOMEM\n", __func__);
return -1;
@@ -544,11 +546,23 @@ int i1905_send_ap_autoconfig_search(struct i1905_private *priv, uint8_t freqband
cmdu_put_eom(frm);
+ fprintf(stderr, "%s:%d: ifname = %s\n", __func__, __LINE__, ifs->ifname);
ret = i1905_send_cmdu(ifs->priv, MCAST_1905, ifs->aladdr,
ETHERTYPE_1905, frm);
if (ret) {
fprintf(stderr, "Error sending AP_AUTOCONFIG_SEARCH\n");
}
+
+#if 0
+ //TODO: move this to i1905_send_cmdu()
+ resp_type = cmdu_expect_response(CMDU_TYPE_AP_AUTOCONFIGURATION_SEARCH);
+ if (resp_type != CMDU_TYPE_NONE) {
+ struct i1905_interface_private *ifp = ifs->priv;
+
+ cmdu_ackq_enqueue(&ifp->txack_q, resp_type, *mid, dst,
+ CMDU_DEFAULT_TIMEOUT);
+ }
+#endif
}
return 0;
@@ -576,9 +590,10 @@ int i1905_send_ap_autoconfig_renew(struct i1905_private *priv, uint8_t freqband)
struct cmdu_buff *frm = NULL;
struct tlv *t;
int ret = 0;
+ uint16_t mid = 0;
- frm = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_RENEW, 0);
+ frm = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_RENEW, &mid);
if (!frm) {
fprintf(stderr, "%s: -ENOMEM\n", __func__);
return -1;
@@ -663,7 +678,7 @@ int i1905_send_ap_autoconfig_response(struct i1905_interface_private *pif,
- resp = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_RESPONSE, mid);
+ resp = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_RESPONSE, &mid);
if (!resp) {
fprintf(stderr, "%s: -ENOMEM\n", __func__);
return -1;
@@ -713,7 +728,8 @@ int i1905_send_ap_autoconfig_response(struct i1905_interface_private *pif,
return ret;
}
-int i1905_send_ap_autoconfig_wsc_m1(struct i1905_interface_private *pif,
+int i1905_send_ap_autoconfig_wsc_m1(struct i1905_interface_private *out_pif,
+ struct i1905_interface_private *pif,
uint8_t *dest)
{
struct i1905_interface *iface = i1905_interface_priv(pif);
@@ -730,9 +746,11 @@ int i1905_send_ap_autoconfig_wsc_m1(struct i1905_interface_private *pif,
//uint16_t auth = WPA2PSK;
//uint16_t enc = CCMP;
struct wps_data wps;
+ uint16_t resp_type;
+ uint16_t mid = 0;
- frm = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_WSC, 0);
+ frm = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_WSC, &mid);
if (!frm) {
fprintf(stderr, "%s: -ENOMEM\n", __func__);
return -1;
@@ -756,7 +774,7 @@ int i1905_send_ap_autoconfig_wsc_m1(struct i1905_interface_private *pif,
pif->wsc->key = key;
}
- t = cmdu_reserve_tlv(frm, FRAG_DATA_SIZE_TLV);
+ t = cmdu_reserve_tlv(frm, 1024);
if (!t) {
cmdu_free(frm);
return -1;
@@ -774,7 +792,61 @@ int i1905_send_ap_autoconfig_wsc_m1(struct i1905_interface_private *pif,
cmdu_put_eom(frm);
- ret = i1905_send_cmdu(pif, dest, iface->aladdr, ETHERTYPE_1905, frm);
+ fprintf(stderr, "%s:%d: out-ifname = %s dest = " MACFMT " mid = %hu\n",
+ __func__, __LINE__, iface->ifname, MAC2STR(dest), mid);
+ ret = i1905_send_cmdu(out_pif, dest, iface->aladdr, ETHERTYPE_1905, frm);
+ if (ret) {
+ fprintf(stderr, "Error sending AP_AUTOCONFIG_WSC_M1\n");
+ }
+
+ //TODO: move following to i1905_send_cmdu()
+ resp_type = cmdu_expect_response(CMDU_TYPE_AP_AUTOCONFIGURATION_WSC);
+ if (resp_type != CMDU_TYPE_NONE) {
+ cmdu_ackq_enqueue(&out_pif->txack_q, resp_type, mid, dest,
+ 2 * CMDU_DEFAULT_TIMEOUT, strdup(iface->ifname));
+ }
+
+ return ret;
+}
+
+int i1905_send_ap_autoconfig_wsc_m2(struct i1905_interface_private *out_pif,
+ uint8_t *msg, uint16_t msglen, uint16_t mid,
+ uint8_t *dest)
+{
+ struct i1905_interface *iface = i1905_interface_priv(out_pif);
+ struct cmdu_buff *frm;
+ struct tlv *t;
+ int ret;
+
+
+ frm = cmdu_alloc_simple(CMDU_TYPE_AP_AUTOCONFIGURATION_WSC, &mid);
+ if (!frm) {
+ fprintf(stderr, "%s: -ENOMEM\n", __func__);
+ return -1;
+ }
+
+ t = cmdu_reserve_tlv(frm, 1024);
+ if (!t) {
+ cmdu_free(frm);
+ return -1;
+ }
+
+ t->type = TLV_TYPE_WSC;
+ t->len = msglen;
+ memcpy(t->data, msg, msglen);
+ ret = cmdu_put_tlv(frm, t);
+ if (ret) {
+ fprintf(stderr, "%s: error: cmdu_put_tlv()\n", __func__);
+ cmdu_free(frm);
+ return -1;
+ }
+
+ cmdu_put_eom(frm);
+
+ fprintf(stderr, "%s:%d: out-ifname = %s dest = " MACFMT "\n",
+ __func__, __LINE__, iface->ifname, MAC2STR(dest));
+
+ ret = i1905_send_cmdu(out_pif, dest, iface->aladdr, ETHERTYPE_1905, frm);
if (ret) {
fprintf(stderr, "Error sending AP_AUTOCONFIG_WSC_M1\n");
}
@@ -799,7 +871,7 @@ int i1905_send_higherlayer_response(struct i1905_interface_private *pif,
- resp = cmdu_alloc_simple(CMDU_TYPE_HIGHER_LAYER_RESPONSE, mid);
+ resp = cmdu_alloc_simple(CMDU_TYPE_HIGHER_LAYER_RESPONSE, &mid);
if (!resp) {
fprintf(stderr, "%s: -ENOMEM\n", __func__);
return -1;
diff --git a/src/i1905.c b/src/i1905.c
index ec57f1ea..d3e50c6c 100644
--- a/src/i1905.c
+++ b/src/i1905.c
@@ -340,6 +340,23 @@ static void i1905_free_interface(struct i1905_interface *iface)
}
}
+struct i1905_interface *i1905_ifname_to_interface(struct i1905_private *priv,
+ const char *ifname)
+{
+ struct i1905_interface *ifs;
+ struct i1905_selfdevice *self;
+
+
+ self = &priv->dm.self;
+
+ list_for_each_entry(ifs, &self->iflist, list) {
+ if (!strncmp(ifs->ifname, ifname, 16))
+ return ifs;
+ }
+
+ return NULL;
+}
+
int i1905_send_cmdu(struct i1905_interface_private *ifp,
uint8_t *dst, uint8_t *src, uint16_t ethtype,
struct cmdu_buff *frm)
@@ -448,7 +465,7 @@ int i1905_cmdu_tx(struct i1905_interface_private *ifp,
resp_type = cmdu_expect_response(type);
if (resp_type != CMDU_TYPE_NONE) {
cmdu_ackq_enqueue(&ifp->txack_q, resp_type, *mid, dst,
- CMDU_DEFAULT_TIMEOUT);
+ CMDU_DEFAULT_TIMEOUT, NULL);
}
return 0;
@@ -481,6 +498,7 @@ static void i1905_recv_1905(struct uloop_fd *fd, unsigned int events)
break;
}
+ fprintf(stderr, "%s: Rx 1905 CMDU ifname = %s\n", __func__, iface->ifname);
bufprintf(rxf->head, res, "Received 1905 CMDU");
if (*(rxf->head + 12) == 0x81 && *(rxf->head + 13) == 0x00) {
@@ -493,6 +511,7 @@ static void i1905_recv_1905(struct uloop_fd *fd, unsigned int events)
rxf->datalen = res - eth_hdrsize - sizeof(struct cmdu_header);
rxf->tail = rxf->data + rxf->datalen;
memcpy(rxf->dev_macaddr, iface->macaddr, 6);
+ strncpy(rxf->dev_ifname, iface->ifname, 15);
memcpy(rxf->origin, rxf->head + 6, 6);
if (!IS_CMDU_LAST_FRAGMENT(rxf->cdata)) {
@@ -1207,7 +1226,7 @@ int i1905_apconfig_request(void *priv, uint8_t band)
//if (!i1905_has_registrar(priv, freqband))
// return -EINVAL;
- ret |= i1905_send_ap_autoconfig_search(priv, freqband);
+ return i1905_send_ap_autoconfig_search(priv, freqband);
}
/* for all wifi bands */
diff --git a/src/i1905.h b/src/i1905.h
index b64c897b..86729cba 100644
--- a/src/i1905.h
+++ b/src/i1905.h
@@ -101,6 +101,10 @@ bool i1905_has_registrar(void *priv, uint8_t freqband);
int i1905_apconfig_request(void *priv, uint8_t band);
int i1905_apconfig_renew(void *priv, uint8_t band);
+
+struct i1905_interface *i1905_ifname_to_interface(struct i1905_private *priv,
+ const char *ifname);
+
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);
@@ -131,9 +135,15 @@ int i1905_send_ap_autoconfig_renew(struct i1905_private *priv,
int i1905_send_ap_autoconfig_response(struct i1905_interface_private *pif,
uint8_t *dest, uint8_t band, uint16_t mid);
-int i1905_send_ap_autoconfig_wsc_m1(struct i1905_interface_private *pif,
+int i1905_send_ap_autoconfig_wsc_m1(struct i1905_interface_private *out_pif,
+ struct i1905_interface_private *pif,
uint8_t *dest);
+int i1905_send_ap_autoconfig_wsc_m2(struct i1905_interface_private *out_pif,
+ uint8_t *msg, uint16_t msglen,
+ uint16_t mid, uint8_t *dest);
+
+
int i1905_send_higherlayer_response(struct i1905_interface_private *pif,
uint8_t *dest, uint16_t mid);
diff --git a/src/i1905_wsc.c b/src/i1905_wsc.c
index bf8d0d13..256d7f39 100644
--- a/src/i1905_wsc.c
+++ b/src/i1905_wsc.c
@@ -799,13 +799,13 @@ int wsc_build_m2(uint8_t *m1, uint16_t m1_size, uint8_t **m2, uint16_t *m2_size,
*/
uint32_t setting_len = abs(r - plain);
- bufptr_put_be16(r, ATTR_ENCR_SETTINGS);
- bufptr_put_be16(r, AES_BLOCK_SIZE + setting_len);
+ bufptr_put_be16(p, ATTR_ENCR_SETTINGS);
+ bufptr_put_be16(p, 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);
+ bufptr_put(p, plain, setting_len);
/* Encrypt the data in-place.
* Note that the "ATTR_ENCR_SETTINGS" attribute containes both
@@ -857,16 +857,16 @@ int wsc_process_m2(const char *ifname, void *key, uint8_t *m1, uint16_t m1_size,
struct wsc_key *k;
uint8_t mapie = 0;
- uint8_t ssid[33];
+ uint8_t ssid[33] = {0};
uint8_t ssid_present;
int ssidlen = 0;
- uint8_t bssid[6];
+ uint8_t bssid[6] = {0};
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];
+ uint8_t network_key[64] = {0};
int network_keylen = 0;
uint8_t network_key_present;
//uint8_t band = 0;
@@ -1145,29 +1145,25 @@ int wsc_process_m2(const char *ifname, void *key, uint8_t *m1, uint16_t m1_size,
if (attr_len > 32)
break;
- bufptr_get(p, ssid, attr_len);
+ memcpy(ssid, p, 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);
+ memcpy(network_key, p, 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);
+ memcpy(bssid, p, attr_len);
bssid_present = 1;
break;
case ATTR_KEY_WRAP_AUTH:
@@ -1199,17 +1195,17 @@ int wsc_process_m2(const char *ifname, void *key, uint8_t *m1, uint16_t m1_size,
// May be one or more subelements (Section 12 of WSC spec)
end_of_mapie = p + attr_len;
- bufptr_get(p, id, sizeof(id));
+ memcpy(id, p, 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;
}
+#if 0 //TODO TODO
while (p < end_of_mapie) {
bufptr_get(p, &subelem, 1);
bufptr_get(p, &len, 1);
@@ -1220,6 +1216,7 @@ int wsc_process_m2(const char *ifname, void *key, uint8_t *m1, uint16_t m1_size,
p += len;
}
}
+#endif
}
break;
default:
@@ -1241,7 +1238,9 @@ int wsc_process_m2(const char *ifname, void *key, uint8_t *m1, uint16_t m1_size,
}
memcpy(out->ssid, ssid, ssidlen);
+ out->ssidlen = ssidlen;
memcpy(out->key, network_key, network_keylen);
+ out->keylen = network_keylen;
out->auth_type = auth_type;
out->enc_type = enc_type;
out->mapie = mapie;
--
GitLab