diff --git a/src/cmdu.c b/src/cmdu.c
index 0bf9be037f7ec72f9ab272b7b9c0d21e26d17222..606d18848a7793aebfb29115c7b542692b6dc38e 100644
--- a/src/cmdu.c
+++ b/src/cmdu.c
@@ -261,9 +261,10 @@ int cmdu_put(struct cmdu_buff *c, uint8_t *bytes, int len)
return 0;
}
-int cmdu_parse_tlvs(struct cmdu_buff *c, struct tlv *tv[],
+int cmdu_parse_tlvs(struct cmdu_buff *c, struct tlv *tv[][16],
struct tlv_policy *policy, int policy_len)
{
+ int idx[policy_len];
struct tlv *t;
int len;
int i;
@@ -272,7 +273,10 @@ int cmdu_parse_tlvs(struct cmdu_buff *c, struct tlv *tv[],
if (!c)
return -1;
- memset(tv, 0, policy_len * sizeof(struct tlv *));
+ for (i = 0; i < policy_len; i++) {
+ memset(tv[i], 0, 16 * sizeof(struct tlv *));
+ idx[i] = 0;
+ }
len = c->datalen;
cmdu_for_each_tlv(t, c->data, len) {
@@ -288,10 +292,12 @@ int cmdu_parse_tlvs(struct cmdu_buff *c, struct tlv *tv[],
tlv_length(t) > policy[i].maxlen)
continue;
- if (tv[i])
- continue;
+ if (tv[i][0]) {
+ if (policy[i].present == TLV_PRESENT_ONE)
+ continue;
+ }
- tv[i] = t;
+ tv[i][idx[i]++] = t;
}
}
diff --git a/src/cmdu.h b/src/cmdu.h
index 67a9558b44101f113282c700ac93475ba69ddd04..5792134fa1bb48feb3d231d07c8686bff7812f52 100644
--- a/src/cmdu.h
+++ b/src/cmdu.h
@@ -45,11 +45,18 @@ struct cmdu_buff {
struct list_head list;
};
+enum tlv_presence {
+ TLV_PRESENT_UNDEFINED,
+ TLV_PRESENT_ONE,
+ TLV_PRESENT_MORE,
+ TLV_PRESENT_NUM,
+};
struct tlv_policy {
uint8_t type;
uint16_t minlen;
uint16_t maxlen;
+ enum tlv_presence present;
};
struct tlv {
@@ -80,7 +87,7 @@ uint16_t cmdu_get_next_mid(void);
int cmdu_validate(struct cmdu_buff *c, int max_tlvtype,
struct tlv_policy *policy);
-int cmdu_parse_tlvs(struct cmdu_buff *c, struct tlv *tv[],
+int cmdu_parse_tlvs(struct cmdu_buff *c, struct tlv *tv[][16],
struct tlv_policy *policy, int policy_len);
diff --git a/src/cmdu_input.c b/src/cmdu_input.c
index 035c9f3a44dcec6d7e255742c942eca97583c212..208d39553f07da1debf224c6d9da795948e0b4b9 100644
--- a/src/cmdu_input.c
+++ b/src/cmdu_input.c
@@ -45,12 +45,287 @@
#include "lldp_tlvs.h"
+#define CMDU_TYPE_1905_START 0x0001
+#define CMDU_TYPE_1905_END 0x0009
+
+
+int i1905_handle_topology_discovery(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ struct tlv_policy a_policy[] = {
+ [0] = { .type = TLV_TYPE_AL_MAC_ADDRESS_TYPE, .present = TLV_PRESENT_ONE },
+ [1] = { .type = TLV_TYPE_MAC_ADDRESS_TYPE, .present = TLV_PRESENT_ONE },
+ };
+ struct tlv *tv[2][16];
+ uint8_t aladdr_origin[6] = {0};
+ uint8_t macaddr_origin[6] = {0};
+ int ret;
+
+
+ cmdu_parse_tlvs(rxf, tv, a_policy, 2);
+
+ if (tv[0][0])
+ memcpy(aladdr_origin, tv[0][0]->data, tlv_length(tv[0][0]));
+
+ if (tv[1][0])
+ memcpy(macaddr_origin, tv[1][0]->data, tlv_length(tv[1][0]));
+
+
+ if (hwaddr_is_zero(macaddr_origin)) {
+ fprintf(stderr, "%s: Discard topo discovery from src = 0!\n",
+ __func__);
+
+ return -1;
+ }
+
+ ret = i1905_dm_neighour_update(i1905_interface_priv(pif),
+ aladdr_origin, macaddr_origin);
+ if (ret) {
+ fprintf(stderr, "%s: Error updating DM for neighbor " MACFMT"\n",
+ __func__, MAC2STR(macaddr_origin));
+
+ return -1;
+ }
+
+ return 0;
+}
+
+int i1905_handle_topology_notification(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ struct tlv_policy a_policy[] = {
+ [0] = { .type = TLV_TYPE_AL_MAC_ADDRESS_TYPE, .present = TLV_PRESENT_ONE },
+ };
+ struct tlv *tv[1][16];
+ uint8_t aladdr_origin[6] = {0};
+ int ret;
+
+
+ cmdu_parse_tlvs(rxf, tv, a_policy, 1);
+
+ if (tv[0][0])
+ memcpy(aladdr_origin, tv[0][0]->data, tlv_length(tv[0][0]));
+
+
+ if (hwaddr_is_zero(aladdr_origin)) {
+ fprintf(stderr, "%s: Discard topo notification from aladdr = 0!\n",
+ __func__);
+
+ return -1;
+ }
+
+ ret = i1905_dm_neighour_update(i1905_interface_priv(pif),
+ aladdr_origin, NULL);
+ if (ret) {
+ fprintf(stderr, "%s: Error updating DM for neighbor " MACFMT"\n",
+ __func__, MAC2STR(aladdr_origin));
+
+ return -1;
+ }
+
+ return 0;
+}
+
+int i1905_handle_topology_query(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_topology_response(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ struct tlv_policy a_policy[] = {
+ [0] = { .type = TLV_TYPE_DEVICE_INFORMATION_TYPE, .present = TLV_PRESENT_ONE },
+ [1] = { .type = TLV_TYPE_DEVICE_BRIDGING_CAPABILITIES, .present = TLV_PRESENT_MORE },
+ [2] = { .type = TLV_TYPE_NON_1905_NEIGHBOR_DEVICE_LIST, .present = TLV_PRESENT_MORE },
+ [3] = { .type = TLV_TYPE_NEIGHBOR_DEVICE_LIST, .present = TLV_PRESENT_MORE },
+ [4] = { .type = TLV_TYPE_POWER_OFF_INTERFACE, .present = TLV_PRESENT_MORE},
+ [5] = { .type = TLV_TYPE_L2_NEIGHBOR_DEVICE, .present = TLV_PRESENT_MORE },
+ };
+ struct tlv *tv[6][16];
+ uint8_t aladdr_origin[6] = {0};
+ uint8_t macaddr_origin[6] = {0};
+ int ret;
+
+
+ cmdu_parse_tlvs(rxf, tv, a_policy, 6);
+
+
+#if 0
+ if (hwaddr_is_zero(macaddr_origin)) {
+ fprintf(stderr, "%s: Discard topo discovery from src = 0!\n",
+ __func__);
+
+ return -1;
+ }
+
+ ret = i1905_dm_neighour_update(i1905_interface_priv(pif),
+ aladdr_origin, macaddr_origin);
+ if (ret) {
+ fprintf(stderr, "%s: Error updating DM for neighbor " MACFMT"\n",
+ __func__, MAC2STR(macaddr_origin));
+
+ return -1;
+ }
+#endif
+
+
+ return 0;
+}
+
+int i1905_handle_vendor_request(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_link_metric_query(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_link_metric_response(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_ap_autoconfig_search(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_ap_autoconfig_response(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_ap_autoconfig_renew(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_ap_autoconfig_wsc(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_pbc_notification(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_pbc_join_notification(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_higherlayer_query(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_higherlayer_response(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_interface_power_request(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_interface_power_response(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_generic_phy_query(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+int i1905_handle_generic_phy_response(struct i1905_interface_private *pif,
+ struct cmdu_buff *rxf)
+{
+ //TODO
+ return 0;
+}
+
+
+typedef int (*cmdu_handler_t)(struct i1905_interface_private *ifp,
+ struct cmdu_buff *rxf);
+
+static const cmdu_handler_t i1905ftable[] = {
+ [0x00] = i1905_handle_topology_discovery,
+ [0x01] = i1905_handle_topology_notification,
+ [0x02] = i1905_handle_topology_query,
+ [0x03] = i1905_handle_topology_response,
+ [0x04] = i1905_handle_vendor_request,
+ [0x05] = i1905_handle_link_metric_query,
+ [0x06] = i1905_handle_link_metric_response,
+ [0x07] = i1905_handle_ap_autoconfig_search,
+ [0x08] = i1905_handle_ap_autoconfig_response,
+ [0x09] = i1905_handle_ap_autoconfig_wsc,
+ [0x0a] = i1905_handle_ap_autoconfig_renew,
+ [0x0b] = i1905_handle_pbc_notification,
+ [0x0c] = i1905_handle_pbc_join_notification,
+ [0x0d] = i1905_handle_higherlayer_query,
+ [0x0e] = i1905_handle_higherlayer_response,
+ [0x0f] = i1905_handle_interface_power_request,
+ [0x10] = i1905_handle_interface_power_response,
+ [0x11] = i1905_handle_generic_phy_query,
+ [0x11] = i1905_handle_generic_phy_response,
+};
+
+
int i1905_process_cmdu(struct i1905_interface_private *pif, struct cmdu_buff *rxf)
{
+ uint16_t type;
+ int ret;
+
+ if (!rxf->cdata)
+ return -1;
+
+ // TODO: discard duplicates
+
+ type = buf_get_be16(rxf->cdata->hdr.type);
+
+ if (i1905ftable[type]) {
+ ret = i1905ftable[type](pif, rxf);
+ }
fprintf(stderr, "%s: ----->\n", __func__);
- return 0;
+ return ret;
}
diff --git a/src/i1905.h b/src/i1905.h
index 6fdab0da5a4245582e8412cc6acf4d27b9c37ace..d12e372fbf608a12053a7cc934c73a675ffb8eb0 100644
--- a/src/i1905.h
+++ b/src/i1905.h
@@ -71,6 +71,7 @@ extern struct i1905_dm *i1905_dm_get();
extern int i1905_dm_init(struct i1905_dm *dm);
extern int i1905_dm_free(struct i1905_dm *dm);
-
+extern int i1905_dm_neighour_update(struct i1905_interface *iface,
+ uint8_t *aladdr, uint8_t *macaddr);
#endif /* I1905_H */
diff --git a/src/i1905_dm.c b/src/i1905_dm.c
index 1b6e20c6fbb68b96f3da7de61fbd281ee631ae5b..b8951e7c06d6a6f52adcb9be60977ad17f039e51 100644
--- a/src/i1905_dm.c
+++ b/src/i1905_dm.c
@@ -30,6 +30,19 @@
+int i1905_dm_neighour_update(struct i1905_interface *iface, uint8_t *aladdr,
+ uint8_t *macaddr)
+{
+
+ //TODO: lookup aladdr/macaddr in DM
+ // if not available, add to DM
+ // if available, update tsp
+ //
+ // if tsp_lastupdated = 0 or > 10 secs, send topo query
+
+ return 0;
+}
+
int i1905_dm_init(struct i1905_dm *dm)
{