diff --git a/src/wsc.c b/src/wsc.c index 9bcd7558..aeb3bba1 100644 --- a/src/wsc.c +++ b/src/wsc.c @@ -30,6 +30,42 @@ #include "wsc.h" +void wsc_wfa_ext_iter_init(struct wsc_wfa_ext_iter *iter, + const unsigned char *pdu, unsigned short len) +{ + iter->pdu = pdu; + iter->max = len; + iter->pos = 0; +} + +bool wsc_wfa_ext_iter_next(struct wsc_wfa_ext_iter *iter) +{ + const unsigned char *start = iter->pdu + iter->pos; + const unsigned char *end = iter->pdu + iter->max; + unsigned char type; + unsigned char len; + + if (iter->pos + 2 >= iter->max) + return false; + + type = *start; + start += 1; + + len = *start; + start += 1; + + if (start + len > end) + return false; + + iter->type = type; + iter->len = len; + iter->data = start; + + iter->pos = start + len - iter->pdu; + + return true; +} + void wsc_attr_iter_init(struct wsc_attr_iter *iter, const unsigned char *pdu, unsigned int len) { @@ -66,3 +102,22 @@ bool wsc_attr_iter_next(struct wsc_attr_iter *iter) return true; } + +bool wsc_attr_iter_recurse_wfa_ext(struct wsc_attr_iter *iter, + struct wsc_wfa_ext_iter *wfa_iter) +{ + static const unsigned char wfa_ext[3] = { 0x00, 0x37, 0x2a }; + + if (iter->type != WSC_ATTR_VENDOR_EXTENSION) + return false; + + if (iter->len < 3) + return false; + + if (memcmp(iter->data, wfa_ext, sizeof(wfa_ext))) + return false; + + wsc_wfa_ext_iter_init(wfa_iter, iter->data + 3, iter->len - 3); + + return true; +} diff --git a/src/wsc.h b/src/wsc.h index ddc170ea..27cdfdf2 100644 --- a/src/wsc.h +++ b/src/wsc.h @@ -257,6 +257,37 @@ enum wsc_config_state { WSC_CONFIG_STATE_CONFIGURED = 0x02, }; +struct wsc_wfa_ext_iter { + unsigned short max; + unsigned short pos; + const unsigned char *pdu; + unsigned char type; + unsigned char len; + const unsigned char *data; +}; + +void wsc_wfa_ext_iter_init(struct wsc_wfa_ext_iter *iter, + const unsigned char *pdu, unsigned short len); +bool wsc_wfa_ext_iter_next(struct wsc_wfa_ext_iter *iter); + +static inline unsigned char wsc_wfa_ext_iter_get_type( + struct wsc_wfa_ext_iter *iter) +{ + return iter->type; +} + +static inline unsigned char wsc_wfa_ext_iter_get_length( + struct wsc_wfa_ext_iter *iter) +{ + return iter->len; +} + +static inline const unsigned char *wsc_wfa_ext_iter_get_data( + struct wsc_wfa_ext_iter *iter) +{ + return iter->data; +} + struct wsc_attr_iter { unsigned int max; unsigned int pos; @@ -269,6 +300,8 @@ struct wsc_attr_iter { void wsc_attr_iter_init(struct wsc_attr_iter *iter, const unsigned char *pdu, unsigned int len); bool wsc_attr_iter_next(struct wsc_attr_iter *iter); +bool wsc_attr_iter_recurse_wfa_ext(struct wsc_attr_iter *iter, + struct wsc_wfa_ext_iter *wfa_iter); static inline unsigned int wsc_attr_iter_get_type(struct wsc_attr_iter *iter) {