diff --git a/src/cmdu.c b/src/cmdu.c
index 13e972bf8a33dc434092d445bcc408eda4a9c6e8..029141b51864a959b2de977ff26b6ab7f0d91a31 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 9a78e8d57381c325c37d1d9cd7cf3935bf0af6f3..e435cd1dcd0fd40abdbd915d10bfb64d02ebf64a 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 9dd93d54ab8a4389b1eb7617c14a1866a1689703..20cf46180edef5904f2a9b151933dfd5b6ed4301 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 96a043231b503fd209cd43afa3c474949374f286..24cc12e4768e27f65b7c666e581a5814135341f6 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 33ae1987cc087514240c1536956d6718b6fa4652..0a574a074fd99dc13b525133b02a5c9dde637bd9 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 60a186f67e6d6fb17157045db80f6ce34e9ad208..c94150ea8a8955f57ffb5cd40ea73dd45f905b38 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 ec57f1eaf5b0398146702f91523c8b3494b9d65b..d3e50c6c72ebc2230c76ed4b6a1e7cd5f16d0afc 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 b64c897b10a8287ff50feb9166181591da46c290..86729cba65058f380eb3e685bce380a5effea558 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 bf8d0d139d807408b00fb4877bebcff3a42fc280..256d7f3939ac2112acf45f61c7f2a7581ae58dca 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;