summaryrefslogtreecommitdiff
path: root/cleopatre/devkit/p1905_managerd/src/cmdu_message_parse.c
diff options
context:
space:
mode:
Diffstat (limited to 'cleopatre/devkit/p1905_managerd/src/cmdu_message_parse.c')
-rw-r--r--cleopatre/devkit/p1905_managerd/src/cmdu_message_parse.c1882
1 files changed, 1882 insertions, 0 deletions
diff --git a/cleopatre/devkit/p1905_managerd/src/cmdu_message_parse.c b/cleopatre/devkit/p1905_managerd/src/cmdu_message_parse.c
new file mode 100644
index 0000000000..ef65404f06
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/cmdu_message_parse.c
@@ -0,0 +1,1882 @@
+/*
+ * cleopatre/application/p1905_managerd/src/cmdu_message_parse.c
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <errno.h>
+#include <linux/if_packet.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+#include <syslog.h>
+#include <assert.h>
+#include <linux/if_ether.h>
+#include <asm/byteorder.h>
+
+#include "cmdu_message_parse.h"
+#include "cmdu_message.h"
+#include "cmdu_fragment.h"
+
+//#define CMDU_MESSAGE_PARSE_DEBUG
+#ifdef CMDU_MESSAGE_PARSE_DEBUG
+#define debug(...) printf(__VA_ARGS__)
+#define debug_syslog(...) printf(__VA_ARGS__)
+#else
+#define debug(...)
+#define debug_syslog(format, ...) syslog(LOG_WARNING, format, ##__VA_ARGS__)
+#endif
+
+/*use flags below to check integrity of received message*/
+const static unsigned int check_integrity[]=
+{
+ (1<<AL_MAC_ADDR_TLV_TYPE)|(1<<MAC_ADDR_TLV_TYPE),//topology discovery
+ (1<<AL_MAC_ADDR_TLV_TYPE), //topology notification
+ 0, //topology query
+ (1<<DEVICE_INFO_TLV_TYPE), //topology response
+ (1<<VENDOR_SPECIFIC_TLV_TYPE), //vendor specific
+ (1<<LINK_METRICS_QUERY_TLV_TYPE), //link metrics query
+ (1<<TRANSMITTER_LINK_METRIC_TYPE)|(1<<RECEIVER_LINK_METRIC_TYPE),//link mtrics response
+ (1<<AL_MAC_ADDR_TLV_TYPE)|(1<<SEARCH_ROLE_TLV_TYPE)|(1<<AUTO_CONFIG_FREQ_BAND_TLV_TYPE),//ap autoconfig search
+ (1<<SUPPORT_ROLE_TLV_TYPE)|(1<<SUPPORT_FREQ_BAND_TLV_TYPE),//ap autoconfig response
+ (1<<WSC_TLV_TYPE), //ap autoconfig WSC
+ (1<<AL_MAC_ADDR_TLV_TYPE)|(1<<SUPPORT_ROLE_TLV_TYPE)|(1<<SUPPORT_FREQ_BAND_TLV_TYPE),//ap autoconfig renew
+ (1<<AL_MAC_ADDR_TLV_TYPE)|(1<<PUSH_BUTTON_EVENT_NOTIFICATION_TYPE),//PB event notification
+ (1<<AL_MAC_ADDR_TLV_TYPE)|(1<<PUSH_BUTTON_JOIN_NOTIFICATION_TYPE),//PB join notification
+};
+
+unsigned char rx_buffer[3072]={0};
+
+extern const unsigned char p1905_multicast_address[6];
+
+/**
+ * add/update 1905.1 neighbor device information.
+ *
+ * \param ctx 1905.1 managerd context
+ * \param al_mac neighbor al mac addr in topology discovery message.
+ * \param neighbor_itf_mac neighbor device interface mac in topology discover.
+ * \param itf_mac_addr the interface topology discovery received from.
+ * \param notify if need to send topology notify, set this flag
+ * \param retun error code
+ */
+static int update_p1905_neighbor_dev_info(struct p1905_managerd_ctx *ctx,
+ unsigned char *al_mac, unsigned char *neighbor_itf_mac,
+ unsigned char *itf_mac_addr, unsigned char *notify)
+{
+ int i = 0;
+ struct p1905_neighbor_info *dev_info;
+
+ for(i=0;i<ITF_NUM;i++)
+ {
+ if(!memcmp(ctx->p1905_neighbor_dev[i].local_mac_addr,itf_mac_addr,ETH_ALEN))
+ break;
+ if(i == (ITF_NUM - 1))
+ {
+ debug_syslog("no this local interface --error\n");
+ return -1;
+ }
+ }
+
+ if(!LIST_EMPTY(&(ctx->p1905_neighbor_dev[i].p1905nbr_head)))
+ {
+ LIST_FOREACH(dev_info, &(ctx->p1905_neighbor_dev[i].p1905nbr_head),
+ p1905nbr_entry)
+ {
+ if(!memcmp(dev_info->al_mac_addr, al_mac, ETH_ALEN))
+ {
+ if(dev_info->ieee802_1_bridge == 0)
+ {
+ /* can not find any matched mac address in LLDP storage
+ * it means that local device just receive the topology
+ * discovery but no 802.1 bridge detection message.
+ * so a 802.1 bridge exist
+ */
+ if(0 == find_lldp_by_port_id_mac(itf_mac_addr,neighbor_itf_mac))
+ {
+ dev_info->ieee802_1_bridge = 1;
+ *notify = 1;
+ }
+ }
+ return 0;
+ }
+ }
+ }
+
+ dev_info = (struct p1905_neighbor_info *)malloc(sizeof(struct p1905_neighbor_info));
+ memcpy(dev_info->al_mac_addr, al_mac, ETH_ALEN);
+ dev_info->ieee802_1_bridge = 1;
+ LIST_INSERT_HEAD(&(ctx->p1905_neighbor_dev[i].p1905nbr_head), dev_info,
+ p1905nbr_entry);
+ /*need to send topology notification message*/
+ *notify = 1;
+ return 0;
+}
+
+/**
+ * delete 1905.1 topology response database.
+ *
+ * \param ctx 1905.1 managerd context
+ * \param al_mac input the target device al mac to delete.
+ */
+void delete_not_exist_topology_response_database(struct p1905_managerd_ctx *ctx,
+ unsigned char *al_mac)
+{
+ struct topology_response_db *tpgr_db;
+ struct device_info_db *dev_info, *dev_info_temp;
+ struct p1905_neighbor_device_db *p1905_neighbor, *p1905_neighbor_temp;
+ struct non_p1905_neighbor_device_list_db *non_p1905_neighbor, *non_p1905_neighbor_temp;
+ struct device_bridge_capability_db *br_cap, *br_cap_temp;
+
+ if(!SLIST_EMPTY(&(ctx->topology_entry.tprdb_head)))
+ {
+ SLIST_FOREACH(tpgr_db, &(ctx->topology_entry.tprdb_head), tprdb_entry)
+ {
+ if(!memcmp(tpgr_db->al_mac_addr, al_mac, ETH_ALEN))
+ {
+ if(!SLIST_EMPTY(&(tpgr_db->devinfo_head)))
+ {
+ dev_info = SLIST_FIRST(&(tpgr_db->devinfo_head));
+ while(dev_info)
+ {
+ dev_info_temp = SLIST_NEXT(dev_info, devinfo_entry);
+ if(!SLIST_EMPTY(&(dev_info->p1905_nbrdb_head)))
+ {
+ /*delete all p1905.1 device in this interface*/
+ p1905_neighbor =\
+ SLIST_FIRST(&(dev_info->p1905_nbrdb_head));
+ while(p1905_neighbor)
+ {
+ p1905_neighbor_temp =\
+ SLIST_NEXT(p1905_neighbor, p1905_nbrdb_entry);
+
+ SLIST_REMOVE(&(dev_info->p1905_nbrdb_head),\
+ p1905_neighbor, p1905_neighbor_device_db,\
+ p1905_nbrdb_entry);
+ free(p1905_neighbor);
+ p1905_neighbor = p1905_neighbor_temp;
+ }
+ }
+
+ /*delete all non-p1905.1 device in this interface*/
+ if(!SLIST_EMPTY(&(dev_info->non_p1905_nbrdb_head)))
+ {
+ non_p1905_neighbor =\
+ SLIST_FIRST(&(dev_info->non_p1905_nbrdb_head));
+ while(non_p1905_neighbor)
+ {
+ non_p1905_neighbor_temp =\
+ SLIST_NEXT(non_p1905_neighbor, non_p1905_nbrdb_entry);
+
+ SLIST_REMOVE(&(dev_info->non_p1905_nbrdb_head),\
+ non_p1905_neighbor, non_p1905_neighbor_device_list_db,\
+ non_p1905_nbrdb_entry);
+ free(non_p1905_neighbor);
+ non_p1905_neighbor = non_p1905_neighbor_temp;
+ }
+ }
+ /*remove this device info db from list*/
+ SLIST_REMOVE(&(tpgr_db->devinfo_head), dev_info,
+ device_info_db, devinfo_entry);
+ /*delete this device info db*/
+ if(dev_info->vs_info_len)
+ free(dev_info->vs_info);
+
+ free(dev_info);
+ dev_info = dev_info_temp;
+ }
+ }
+
+ /*delete bridge capability info*/
+ if(!LIST_EMPTY(&(tpgr_db->brcap_head)))
+ {
+ br_cap = LIST_FIRST(&(tpgr_db->brcap_head));
+ while(br_cap)
+ {
+ br_cap_temp = LIST_NEXT(br_cap, brcap_entry);
+ LIST_REMOVE(br_cap, brcap_entry);
+
+ if(br_cap->interface_amount)
+ free(br_cap->interface_mac_tuple);
+
+ free(br_cap);
+ br_cap = br_cap_temp;
+ }
+ }
+ /*finall, delete whole topology response with this AL MAC*/
+ SLIST_REMOVE(&(ctx->topology_entry.tprdb_head), tpgr_db,\
+ topology_response_db, tprdb_entry);
+ free(tpgr_db);
+ break;
+ }
+ }
+ }
+}
+
+/**
+ * delete one 1905.1 neighbor device.
+ *
+ * \param ctx 1905.1 managerd context
+ * \param al_mac input the neighbor device al mac to delete.
+ * \param retun error code
+ */
+int delete_p1905_neighbor_dev_info(struct p1905_managerd_ctx *ctx,
+ unsigned char *al_mac)
+{
+ int i = 0;
+ struct p1905_neighbor_info *dev_info;
+ int result = 0;
+
+ for(i=0;i<ITF_NUM;i++)
+ {
+ if(!LIST_EMPTY(&(ctx->p1905_neighbor_dev[i].p1905nbr_head)))
+ {
+ LIST_FOREACH(dev_info, &(ctx->p1905_neighbor_dev[i].p1905nbr_head),
+ p1905nbr_entry)
+ {
+ if(!memcmp(dev_info->al_mac_addr, al_mac, ETH_ALEN))
+ {
+ debug("delete p1905 neighbor\n");
+ LIST_REMOVE(dev_info, p1905nbr_entry);
+ free(dev_info);
+ result = 1;
+ /* add a break here because we don't need to search same
+ * list anymore if we find the neighbor which we want
+ */
+ break;
+ }
+ }
+ }
+ }
+
+ return result;
+}
+
+/**
+ * update the time to live for topology dicovery message.
+ * if value of time to live is less than 0, need to delete this record
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param sec for 1905.1 managerd use.
+ * \return error code
+ */
+int delete_not_exist_p1905_neighbor_device(struct p1905_managerd_ctx *ctx,
+ int sec)
+{
+ struct topology_discovery_db *tpg_db, *tpg_db_temp;
+ int result = 0;
+
+ if(!LIST_EMPTY(&(ctx->topology_entry.tpddb_head)))
+ {
+ tpg_db = LIST_FIRST(&(ctx->topology_entry.tpddb_head));
+ while(tpg_db)
+ {
+ tpg_db_temp = LIST_NEXT(tpg_db, tpddb_entry);
+ tpg_db->time_to_live -= sec;
+ if(tpg_db->time_to_live <= 0)
+ {
+ debug("delete topology discovery\n");
+ /*delete the local p1905.1 neighbor device info*/
+ if(delete_p1905_neighbor_dev_info(ctx, tpg_db->al_mac))
+ result = 1;
+#if 0 //to store the topology response database forever
+ /*delete the topology response database*/
+ delete_not_exist_topology_response_database(ctx, tpg_db->al_mac);
+#endif
+ /*delete the topology discovery database*/
+ LIST_REMOVE(tpg_db, tpddb_entry);
+ free(tpg_db);
+ }
+ tpg_db = tpg_db_temp;
+ }
+ }
+
+ return result;
+}
+
+/**
+ * find remote device's al mac address
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param mac_addr input, use the sa mac value we got from other unicast message .
+ * \param al_mac_addr output, provide the al mac
+ * \return error code
+ */
+static int find_al_mac_address(struct p1905_managerd_ctx *ctx,
+ unsigned char *mac_addr, unsigned char *al_mac_addr)
+{
+ struct topology_discovery_db *tpg_db;
+ struct topology_response_db *tpgr_db;
+ struct device_info_db *dev_db;
+ int result = -1;
+
+ /*search in topology discovery database*/
+ if(!LIST_EMPTY(&(ctx->topology_entry.tpddb_head)))
+ {
+ LIST_FOREACH(tpg_db, &(ctx->topology_entry.tpddb_head), tpddb_entry)
+ {
+ if(!memcmp(mac_addr,tpg_db->al_mac,ETH_ALEN))
+ {
+ memcpy(al_mac_addr,mac_addr,ETH_ALEN);
+ result = 0;
+ goto find_finish;
+ }
+ if(!memcmp(mac_addr,tpg_db->itf_mac,ETH_ALEN))
+ {
+ memcpy(al_mac_addr,tpg_db->al_mac,ETH_ALEN);
+ result = 0;
+ goto find_finish;
+ }
+ }
+ }
+ /* search in topology reponse database
+ * if no matched al mac in topology discovery databse, it means this
+ * device is not a neighbor. So we check the topology response database
+ * to find any matched deivce
+ */
+ if(!SLIST_EMPTY(&(ctx->topology_entry.tprdb_head)))
+ {
+ SLIST_FOREACH(tpgr_db, &(ctx->topology_entry.tprdb_head), tprdb_entry)
+ {
+ if(!memcmp(mac_addr, tpgr_db->al_mac_addr, ETH_ALEN))
+ {
+ memcpy(al_mac_addr, mac_addr, ETH_ALEN);
+ result = 0;
+ goto find_finish;
+ }
+ if(!SLIST_EMPTY(&(tpgr_db->devinfo_head)))
+ {
+ SLIST_FOREACH(dev_db, &(tpgr_db->devinfo_head), devinfo_entry)
+ {
+ if(!memcmp(mac_addr, dev_db->mac_addr, ETH_ALEN))
+ {
+ memcpy(al_mac_addr, tpgr_db->al_mac_addr, ETH_ALEN);
+ result = 0;
+ goto find_finish;
+ }
+ }
+ }
+ }
+ }
+
+find_finish:
+ return result;
+}
+
+/**
+ * get cmdu message version from header.
+ *
+ * \param buf input, cmdu message header start address
+ * \retun cmdu message version
+ */
+unsigned char get_mversion_from_msg_hdr(unsigned char *buf)
+{
+ return (*buf);
+}
+
+/**
+ * get cmdu message type from header.
+ *
+ * \param buf input, cmdu message header start address
+ * \retun cmdu message type
+ */
+unsigned short get_mtype_from_msg_hdr(unsigned char *buf)
+{
+ unsigned short mtype = 0;
+
+ mtype = *(buf+2);
+ mtype = (mtype << 8) & 0xFF00;
+ mtype = mtype |(*(buf+3));
+
+ return mtype;
+}
+
+/**
+ * get cmdu message id from header.
+ *
+ * \param buf input, cmdu message header start address
+ * \retun cmdu message id
+ */
+unsigned short get_mid_from_msg_hdr(unsigned char *buf)
+{
+ unsigned short mid = 0;
+
+ mid = *(buf+4);
+ mid = (mid << 8) & 0xFF00;
+ mid = mid |(*(buf+5));
+
+ return mid;
+}
+
+/**
+ * get cmdu fragment id from header.
+ *
+ * \param buf input, cmdu message header start address
+ * \retun cmdu fragment id
+ */
+unsigned char get_fid_from_msg_hdr(unsigned char *buf)
+{
+ return *(buf+6);
+}
+
+/**
+ * get cmdu last fragment ind from header.
+ *
+ * \param buf input, cmdu message header start address
+ * \retun cmdu last fragment ind
+ */
+unsigned char get_last_fragment_ind_from_msg_hdr(unsigned char *buf)
+{
+ if(((*(buf+7)) & 0x80) == 0x80)
+ return 1;
+ else
+ return 0;
+}
+
+/**
+ * get cmdu relay ind from header.
+ *
+ * \param buf input, cmdu message header start address
+ * \retun cmdu relay ind
+ */
+unsigned char get_relay_ind_from_msg_hdr(unsigned char *buf)
+{
+ if(((*(buf+7)) & 0x40) == 0x40)
+ return 1;
+ else
+ return 0;
+}
+
+/**
+ * check whether receive a relay multicast message
+ *
+ *
+ * \param relay_ind input, get from relay ind field of message header
+ * \param mtype input, get from message type field og message header
+ * \return 0: error 1: relay message
+ */
+int check_relay_message(unsigned char relay_ind,unsigned short mtype)
+{
+ if(relay_ind)
+ {
+ if((mtype != TOPOLOGY_NOTIFICATION) && (mtype != VENDOR_SPECIFIC) &&\
+ (mtype != AP_AUTOCONFIG_SEARCH) && (mtype != AP_AUTOCONFIG_RENEW) &&\
+ (mtype != P1905_PB_EVENT_NOTIFY) && (mtype != P1905_PB_JOIN_NOTIFY))
+ {
+ return 0;
+ }
+ }
+ return 1;
+}
+
+/**
+ * parse topology discovery message
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param buf input, tlvs start position of receive cmdu packet
+ * \param query flag for sending topology query
+ * \param notify flag for sending topology notification
+ * \param al_mac output al mac address from this message
+ * \return error code
+ */
+static int parse_topology_discovery_message(struct p1905_managerd_ctx *ctx,
+ unsigned char *buf, unsigned char *query ,unsigned char *notify,
+ unsigned char *al_mac, unsigned char *smac)
+{
+ int length =0;
+ unsigned char *temp_buf;
+ unsigned char al_mac_addr[ETH_ALEN];
+ unsigned char mac_addr[ETH_ALEN];
+ unsigned char itf_mac_addr[ETH_ALEN];
+ struct topology_discovery_db *tpg_db;
+ unsigned char new_db = 1;
+ unsigned int integrity = 0;
+
+ temp_buf = buf;
+ while(1)
+ {
+ if(*temp_buf == AL_MAC_ADDR_TLV_TYPE)
+ {
+ integrity|= (1<<AL_MAC_ADDR_TLV_TYPE);
+ length = parse_al_mac_addr_type_tlv(temp_buf,al_mac_addr);
+
+ if(length < 0)
+ {
+ debug("error al mac tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == MAC_ADDR_TLV_TYPE)
+ {
+ integrity|= (1<<MAC_ADDR_TLV_TYPE);
+ length = parse_mac_addr_type_tlv(temp_buf,mac_addr);
+
+ if(length < 0)
+ {
+ debug("error mac tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ debug("wrong TLV in topology discovery message\n");
+ /*ignore extra tlv*/
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+ /*check integrity*/
+ if(integrity != check_integrity[TOPOLOGY_DISCOVERY])
+ {
+ debug_syslog("incomplete topology discovery 0x%x 0x%x\n",
+ integrity,check_integrity[TOPOLOGY_DISCOVERY]);
+ return -1;
+ }
+
+ /*search the AL MAC ADDRESS exist or not*/
+ if(!LIST_EMPTY(&(ctx->topology_entry.tpddb_head)))
+ {
+ LIST_FOREACH(tpg_db, &(ctx->topology_entry.tpddb_head), tpddb_entry)
+ {
+ /*compare the AL MAC ADDR with database*/
+ if(!memcmp(tpg_db->al_mac, al_mac_addr, ETH_ALEN))
+ {
+ debug("one al mac is matched\n");
+ /*this AL MAC address exist in database*/
+ new_db = 0;
+ break;
+ }
+ }
+ }
+
+ if(new_db)
+ {
+ (*query) = 1;
+ debug("need add new topology_discovery_db database\n");
+ /*no this AL MAC ADDR , add a new topology_discovery_db component in database*/
+ tpg_db = (struct topology_discovery_db *)malloc(sizeof(struct topology_discovery_db));
+ memcpy(tpg_db->al_mac, al_mac_addr, ETH_ALEN);
+ memset(tpg_db->itf_mac, 0, ETH_ALEN);
+
+ memcpy(al_mac, al_mac_addr, ETH_ALEN);
+ LIST_INSERT_HEAD(&(ctx->topology_entry.tpddb_head), tpg_db, tpddb_entry);
+ }
+
+ /*copy the interface mac into this component */
+ if(memcmp(tpg_db->itf_mac, mac_addr, ETH_ALEN))
+ {
+ /*because get differect mac address from topology discovery.
+ * it will affect our local p1905.1 neighbor device info, so notify
+ */
+ *notify = 1;
+ memcpy(tpg_db->itf_mac, mac_addr,ETH_ALEN);
+ }
+
+ /*use smac to check the receiving interface*/
+ if(-1 == get_receive_port_addr(smac, itf_mac_addr))
+ {
+ debug_syslog("no fdb in bridge\n");
+ return -1;
+ }
+
+ /* becasue timeout of topology discovery message is 60s
+ * add this code to delete non-exist 1905.1 neighbor immediately
+ * it is OK for us because our product is PLC bridge and cannot connect
+ * to another device with 2 or more interfaces.
+ * but if our product connects to other manufactuer's devices, it may
+ * cause some problems. need to check.
+ */
+ if(memcmp(tpg_db->receive_itf_mac, itf_mac_addr, ETH_ALEN))
+ {
+ /*deifferent receive interface, need to erase original neighbor device*/
+ if(delete_p1905_neighbor_dev_info(ctx, al_mac_addr))
+ {
+ debug("delete neighbor device because recv interface change\n");
+ }
+ memcpy(tpg_db->receive_itf_mac, itf_mac_addr, ETH_ALEN);
+ }
+
+ /*update the value of time_to_live of topology discovery*/
+ tpg_db->time_to_live = TOPOLOGY_DISCOVERY_TTL;
+
+ debug("interface mac addr = 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",tpg_db->itf_mac[0],
+ tpg_db->itf_mac[1],tpg_db->itf_mac[2],tpg_db->itf_mac[3],tpg_db->itf_mac[4],
+ tpg_db->itf_mac[5]);
+
+ /*update the local p1905.1 neighbor device information */
+ if(0 > update_p1905_neighbor_dev_info(ctx, al_mac_addr, mac_addr, itf_mac_addr, notify))
+ return -1;
+
+ return 0;
+}
+
+/**
+ * parse topology notification message
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param buf input, tlvs start position of receive cmdu packet
+ * \param al_mac output al mac address from this message
+ * \return error code
+ */
+static int parse_topology_notification_message(struct p1905_managerd_ctx *ctx,
+ unsigned char *buf,unsigned char *al_mac)
+{
+ int length =0;
+ unsigned char *temp_buf;
+ unsigned char al_mac_addr[ETH_ALEN];
+ unsigned int integrity = 0;
+
+ temp_buf = buf;
+ while(1)
+ {
+ if(*temp_buf == AL_MAC_ADDR_TLV_TYPE)
+ {
+ integrity |= (1<<AL_MAC_ADDR_TLV_TYPE);
+ length=parse_al_mac_addr_type_tlv(temp_buf,al_mac_addr);
+
+ if(length < 0)
+ {
+ debug("error al mac tlv \n");
+ return -1;
+ }
+
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ debug("wrong TLV in topology notification message\n");
+ /*ignore extra tlv*/
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+
+ /*check integrity*/
+ if(integrity != check_integrity[TOPOLOGY_NOTIFICATION])
+ {
+ debug_syslog("incomplete topology notification 0x%x 0x%x\n",
+ integrity,check_integrity[TOPOLOGY_NOTIFICATION]);
+ return -1;
+ }
+
+ memcpy(al_mac,al_mac_addr,ETH_ALEN);
+ return 0;
+}
+
+/**
+ * parse topology notification message
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param buf input, tlvs start position of receive cmdu packet
+ * \return error code
+ */
+static int parse_topology_response_message(struct p1905_managerd_ctx *ctx,
+ unsigned char *buf)
+{
+ int length =0;
+ unsigned char *temp_buf;
+ unsigned char al_mac_addr[ETH_ALEN];
+ struct list_head *pos;
+ struct topology_response_db *tpgr_db;
+ unsigned int integrity = 0;
+
+ temp_buf = buf;
+
+ /*we must get the AL MAC addr to get correct database pointer
+ , so need to search device info tlv firstly*/
+ while(1)
+ {
+ if((*temp_buf) == DEVICE_INFO_TLV_TYPE)
+ {
+ integrity |= (1 << DEVICE_INFO_TLV_TYPE);
+ break;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+
+ //need to implement error handling
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+
+ /*check integrity*/
+ if(integrity != check_integrity[TOPOLOGY_RESPONSE])
+ {
+ debug_syslog("incomplete topology response 0x%x 0x%x\n",
+ integrity,check_integrity[TOPOLOGY_RESPONSE]);
+ return -1;
+ }
+
+ temp_buf +=3;//shift to AL MAC addr field in device information tlv
+ memcpy(al_mac_addr,temp_buf,ETH_ALEN);
+
+ /*delete the database if already exist*/
+ delete_not_exist_topology_response_database(ctx, al_mac_addr);
+
+ /*add a new topology_response_db component in database*/
+ tpgr_db= (struct topology_response_db *)malloc(sizeof(struct topology_response_db));
+ memcpy(tpgr_db->al_mac_addr, al_mac_addr, ETH_ALEN);
+ SLIST_INSERT_HEAD(&(ctx->topology_entry.tprdb_head), tpgr_db, tprdb_entry);
+
+ /*init its own device information list and bridge capacity list*/
+ SLIST_INIT(&(tpgr_db->devinfo_head));
+ LIST_INIT(&(tpgr_db->brcap_head));
+
+ /*then shift back to start of device info tlv, parsing it first*/
+ temp_buf -= 3;
+ length = parse_device_info_type_tlv(temp_buf, &(tpgr_db->devinfo_head));
+ if(length < 0)
+ {
+ debug("error device info tlv \n");
+ return -1;
+ }
+
+ /* until now, we get the correct pointer
+ * Move to the start of buf, then do tlvs parsing
+ */
+ temp_buf = buf;
+ while(1)
+ {
+ if(*temp_buf == DEVICE_INFO_TLV_TYPE)
+ {
+ /*we have parsed this tlv, so just get length & shift*/
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ else if(*temp_buf == BRIDGE_CAPABILITY_TLV_TYPE)
+ {
+ length=parse_bridge_capability_type_tlv(temp_buf,\
+ &(tpgr_db->brcap_head));
+ if(length < 0)
+ {
+ debug("error bridge capability tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == P1905_NEIGHBOR_DEV_TLV_TYPE)
+ {
+ length=parse_p1905_neighbor_device_type_tlv(temp_buf,\
+ &(tpgr_db->devinfo_head));
+ if(length < 0)
+ {
+ debug("error p1905.1 neighbor device tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == NON_P1905_NEIGHBOR_DEV_TLV_TYPE)
+ {
+ length=parse_non_p1905_neighbor_device_type_tlv(temp_buf,\
+ &(tpgr_db->devinfo_head));
+ if(length < 0)
+ {
+ debug("error non-p1905.1 neighbor device tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ debug("wrong TLV in topology response message\n");
+ /*ignore extra tlv*/
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+
+ return 0;
+}
+
+/**
+ * parse vendor specific message. It for PC tool use.
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param vs_info input
+ * \return error code
+ */
+static int parse_vendor_specific_message(unsigned char *buf,
+ struct p1905_vs_info *vs_info)
+{
+ int length =0;
+ unsigned char *temp_buf;
+ unsigned int integrity = 0;
+
+ temp_buf = buf;
+ while(1)
+ {
+ if(*temp_buf == VENDOR_SPECIFIC_TLV_TYPE)
+ {
+ integrity |= (1 << VENDOR_SPECIFIC_TLV_TYPE);
+ length=parse_vendor_specific_type_tlv(temp_buf,vs_info);
+
+ if(length < 0)
+ {
+ debug("error vs tlv \n");
+ return -1;
+ }
+
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ /* ignore extra tlv, although vendor specific cmdu can include any
+ * type tlvs, but we have no definition about it.
+ */
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+
+ /*check integrity*/
+ if(integrity != check_integrity[VENDOR_SPECIFIC])
+ {
+ debug_syslog("incomplete vendor specific 0x%x 0x%x\n",
+ integrity,check_integrity[VENDOR_SPECIFIC]);
+ return -1;
+ }
+
+
+ return 0;
+}
+
+/**
+ * parse link metric query message.
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param buf input, tlvs start position of receive cmdu packet
+ * \return error code
+ */
+static int parse_link_metric_query_message(struct p1905_managerd_ctx *ctx,
+ unsigned char *buf)
+{
+ int length =0;
+ unsigned char *temp_buf;
+ unsigned int integrity = 0;
+
+ temp_buf = buf;
+ while(1)
+ {
+ if(*temp_buf == LINK_METRICS_QUERY_TLV_TYPE)
+ {
+ integrity |= (1 << LINK_METRICS_QUERY_TLV_TYPE);
+
+ length=parse_link_metric_query_type_tlv(temp_buf,\
+ ctx->link_metric_response_para.target,
+ &(ctx->link_metric_response_para.type));
+
+ if(length < 0)
+ {
+ debug("error link metric query tlv \n");
+ return -1;
+ }
+
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ debug("wrong TLV in link metric query message\n");
+ /*ignore extra tlv*/
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+
+ /*check integrity*/
+ if(integrity != check_integrity[LINK_METRICS_QUERY])
+ {
+ debug_syslog("incomplete link metrics query 0x%x 0x%x\n",
+ integrity,check_integrity[LINK_METRICS_QUERY]);
+ return -1;
+ }
+
+ return 0;
+}
+
+#ifdef SUPPORT_ALME
+/**
+ * parse link metric response message.
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param buf input, tlvs start position of receive cmdu packet
+ * \param metric_rsp pointer of alme response buffer
+ * \return error code
+ */
+
+static int parse_link_metric_response_message(struct p1905_managerd_ctx *ctx,
+ unsigned char *buf, ALME_GET_METRIC_RSP *metric_rsp)
+{
+ int length =0;
+ unsigned char *temp_buf;
+ unsigned char got_tx_link_tlv = 0, got_rx_link_tlv = 0;
+
+ temp_buf = buf;
+
+ /* in implementation, I use BOTH_TX_AND_RX_METRICS to query link metrics.
+ * so I must get both RX & TX metrics tlv in this message.
+ * Based on this concept, I parse tx link metrics tlv first to get the number
+ * of connecting interfaces and then parse rx link metrics tlv.
+ */
+ while(1)
+ {
+ if(*temp_buf == TRANSMITTER_LINK_METRIC_TYPE)
+ {
+ length = parse_transmitter_link_metrics_type_tlv(temp_buf,
+ ctx->p1905_al_mac_addr, metric_rsp);
+
+ if(length < 0)
+ {
+ debug_syslog("error parse transmitted link metrics tlv type\n");
+ return -1;
+ }
+
+ got_tx_link_tlv = 1;
+ temp_buf += length;
+ }
+ else if(*temp_buf == RESULT_CODE_TLV_TYPE)
+ {
+ length = parse_link_metrics_result_code_type_tlv(temp_buf);
+
+ if(length < 0)
+ {
+ debug_syslog("link metrics response with invalid neighbor\n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ /*ignore other tlv*/
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+ if(!got_tx_link_tlv)
+ {
+ debug_syslog("no tx link metrics tlv\n");
+ return -1;
+ }
+
+ /*return to start position again to parse rx link metrics tlv*/
+ temp_buf = buf;
+ while(1)
+ {
+ if(*temp_buf == RECEIVER_LINK_METRIC_TYPE)
+ {
+ length = parse_receiver_link_metrics_type_tlv(temp_buf,
+ ctx->p1905_al_mac_addr, metric_rsp);
+
+ if(length < 0)
+ {
+ debug_syslog("error parse receiver link metrics tlv type\n");
+ return -1;
+ }
+
+ got_rx_link_tlv = 1;
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ /*ignore other tlv*/
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+ if(!got_rx_link_tlv)
+ {
+ debug_syslog("no rx link metrics tlv\n");
+ return -1;
+ }
+
+ /* no integrity check because we already used got_tx_link_tlv and
+ * got_rx_link_tlv to check
+ */
+
+ return 0;
+}
+#endif
+
+/**
+ * parse push button event notification message.
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param buf input, tlvs start position of receive cmdu packet
+ * \return error code
+ */
+static int parse_push_button_event_notification_message
+ (struct p1905_managerd_ctx *ctx, unsigned char *buf)
+{
+ int length =0;
+ unsigned char *temp_buf;
+ unsigned char al_id[ETH_ALEN] = {0};
+ unsigned int integrity = 0;
+#if 0
+ int i = 0;
+#endif
+
+ temp_buf = buf;
+
+ while(1)
+ {
+ if(*temp_buf == AL_MAC_ADDR_TLV_TYPE)
+ {
+ integrity |= (1 << AL_MAC_ADDR_TLV_TYPE);
+
+ length=parse_al_mac_addr_type_tlv(temp_buf,al_id);
+
+ if(length < 0)
+ {
+ debug("error al mac tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == PUSH_BUTTON_EVENT_NOTIFICATION_TYPE)
+ {
+ integrity |= (1 << PUSH_BUTTON_EVENT_NOTIFICATION_TYPE);
+#ifdef SUPPORT_AP_REGISTRAR
+ length = parse_push_button_event_notification_tlv(temp_buf,
+ &(ctx->pbc_param.wifi_info));
+#else
+ /*if not AP registrar, no need to parse this tlv*/
+ length = get_cmdu_tlv_length(temp_buf);
+#endif
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+
+ /*check integrity*/
+ if(integrity != check_integrity[P1905_PB_EVENT_NOTIFY])
+ {
+ debug_syslog("incomplete push button event notification 0x%x 0x%x\n",
+ integrity,check_integrity[P1905_PB_EVENT_NOTIFY]);
+ return -1;
+ }
+
+ /* if a new device join because of push button event notification
+ * need to send push button join notification to inform all network
+ * this message needs to include the AL id from push button event
+ * notification, so we store this al id
+ */
+ memcpy(ctx->pbc_param.al_id, al_id, ETH_ALEN);
+
+ debug("AL_ID : %.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
+ ctx->pbc_param.al_id[0],ctx->pbc_param.al_id[1],ctx->pbc_param.al_id[2],
+ ctx->pbc_param.al_id[3],ctx->pbc_param.al_id[4],ctx->pbc_param.al_id[5]);
+
+ if(is_connection_status_authenticated())
+ {
+ if(!get_station_info(ctx, 1))
+ {
+ debug_syslog("PLC get station info error\n");
+ ctx->pbc_param.info.station_num = 0;
+ //return 0;
+ }
+
+#if 0
+ debug_syslog("station num = %d\n",ctx->pbc_param.info.station_num);
+ for(i=0;i<ctx->pbc_param.info.station_num;i++)
+ debug_syslog("station%d: %.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",i,
+ ctx->pbc_param.info.station_mac_list[i][0],
+ ctx->pbc_param.info.station_mac_list[i][1],
+ ctx->pbc_param.info.station_mac_list[i][2],
+ ctx->pbc_param.info.station_mac_list[i][3],
+ ctx->pbc_param.info.station_mac_list[i][4],
+ ctx->pbc_param.info.station_mac_list[i][5]);
+#endif
+ }
+ else
+ ctx->pbc_param.info.station_num = 0;
+
+#ifdef SUPPORT_AP_REGISTRAR
+ if(!ctx->pbc_param.wifi_info.no_need_start_pbc)
+ {
+ /* if received push button notification has no ieee802.1 media
+ * need to do wifi PBC, so store current station database for
+ * futher comparison
+ */
+ if(wifi_utils_success !=\
+ get_station_mac(ctx->pbc_param.wifi_info.station_mac_list,
+ &(ctx->pbc_param.wifi_info.station_num)))
+ {
+ debug_syslog("get wifi station info fail in PB notify parse\n");
+ }
+#if 0
+ debug_syslog("station amount %d\n",ctx->pbc_param.wifi_info.station_num);
+
+ if(ctx->pbc_param.wifi_info.station_num > 0)
+ {
+ for(i=0;i<ctx->pbc_param.wifi_info.station_num;i++)
+ {
+ debug_syslog("%.2x %.2x %.2x %.2x %.2x %.2x\n",
+ ctx->pbc_param.wifi_info.station_mac_list[i][0],
+ ctx->pbc_param.wifi_info.station_mac_list[i][1],
+ ctx->pbc_param.wifi_info.station_mac_list[i][2],
+ ctx->pbc_param.wifi_info.station_mac_list[i][3],
+ ctx->pbc_param.wifi_info.station_mac_list[i][4],
+ ctx->pbc_param.wifi_info.station_mac_list[i][5]);
+ }
+ }
+#endif
+ }
+#endif
+ return 0;
+}
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+#ifdef SUPPORT_AP_REGISTRAR
+/**
+ * parse auto configuration search message.
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param buf input, tlvs start position of receive cmdu packet
+ * \param al_mac output, gotten from the al mac type tlv
+ * \return error code
+ */
+static int parse_ap_autoconfig_search_message
+ (struct p1905_managerd_ctx *ctx, unsigned char *buf, unsigned char *al_mac)
+{
+ int length =0;
+ unsigned char *temp_buf;
+ unsigned int integrity = 0;
+
+ temp_buf = buf;
+
+ while(1)
+ {
+ if(*temp_buf == AL_MAC_ADDR_TLV_TYPE)
+ {
+ integrity |= (1 << AL_MAC_ADDR_TLV_TYPE);
+
+ length = parse_al_mac_addr_type_tlv(temp_buf, al_mac);
+
+ if(length < 0)
+ {
+ debug_syslog("ap-search error al mac tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == SEARCH_ROLE_TLV_TYPE)
+ {
+ integrity |= (1 << SEARCH_ROLE_TLV_TYPE);
+
+ length = parse_search_role_tlv(temp_buf);
+ if(length < 0)
+ {
+ debug_syslog("ap-search error search role tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == AUTO_CONFIG_FREQ_BAND_TLV_TYPE)
+ {
+ integrity |= (1 << AUTO_CONFIG_FREQ_BAND_TLV_TYPE);
+
+ length = parse_auto_config_freq_band_tlv(temp_buf);
+ if(length < 0)
+ {
+ debug_syslog("ap-search error freq tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+
+ /*check integrity*/
+ if(integrity != check_integrity[AP_AUTOCONFIG_SEARCH])
+ {
+ debug_syslog("incomplete ap auto config search 0x%x 0x%x\n",
+ integrity,check_integrity[AP_AUTOCONFIG_SEARCH]);
+ return -1;
+ }
+
+ return 0;
+}
+#endif
+
+#ifdef SUPPORT_AP_ENROLLE
+/**
+ * parse auto configuration response message.
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param buf input, tlvs start position of receive cmdu packet
+ * \return error code
+ */
+
+static int parse_ap_autoconfig_response_message
+ (struct p1905_managerd_ctx *ctx, unsigned char *buf)
+{
+ int length =0;
+ unsigned char *temp_buf;
+ unsigned int integrity = 0;
+
+ temp_buf = buf;
+
+ while(1)
+ {
+ if(*temp_buf == SUPPORT_ROLE_TLV_TYPE)
+ {
+ integrity |= (1 << SUPPORT_ROLE_TLV_TYPE);
+
+ length = parse_supported_role_tlv(temp_buf);
+ if(length < 0)
+ {
+ debug_syslog("ap-resp error support role tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == SUPPORT_FREQ_BAND_TLV_TYPE)
+ {
+ integrity |= (1 << SUPPORT_FREQ_BAND_TLV_TYPE);
+
+ length = parse_supported_freq_band_tlv(temp_buf);
+ if(length < 0)
+ {
+ debug_syslog("ap-resp error support freq tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+
+ /*check integrity*/
+ if(integrity != check_integrity[AP_AUTOCONFIG_RESPONSE])
+ {
+ debug_syslog("incomplete ap auto config response 0x%x 0x%x\n",
+ integrity,check_integrity[AP_AUTOCONFIG_RESPONSE]);
+ return -1;
+ }
+
+ return 0;
+}
+
+static int parse_ap_autoconfig_renew_message
+ (struct p1905_managerd_ctx *ctx, unsigned char *buf, unsigned char *al_mac)
+{
+ int length =0;
+ unsigned char *temp_buf;
+ unsigned int integrity = 0;
+
+ temp_buf = buf;
+
+ while(1)
+ {
+ if(*temp_buf == AL_MAC_ADDR_TLV_TYPE)
+ {
+ integrity |= (1 << AL_MAC_ADDR_TLV_TYPE);
+
+ length = parse_al_mac_addr_type_tlv(temp_buf, al_mac);
+
+ if(length < 0)
+ {
+ debug_syslog("ap-renew error al mac tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == SUPPORT_ROLE_TLV_TYPE)
+ {
+ integrity |= (1 << SUPPORT_ROLE_TLV_TYPE);
+
+ length = parse_supported_role_tlv(temp_buf);
+ if(length < 0)
+ {
+ debug_syslog("ap-renew error support role tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == SUPPORT_FREQ_BAND_TLV_TYPE)
+ {
+ integrity |= (1 << SUPPORT_FREQ_BAND_TLV_TYPE);
+
+ length = parse_supported_freq_band_tlv(temp_buf);
+ if(length < 0)
+ {
+ debug_syslog("ap-renew error support freq tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+
+ /*check integrity*/
+ if(integrity != check_integrity[AP_AUTOCONFIG_RENEW])
+ {
+ debug_syslog("incomplete ap auto config renew 0x%x 0x%x\n",
+ integrity,check_integrity[AP_AUTOCONFIG_RENEW]);
+ return -1;
+ }
+
+ return 0;
+}
+#endif
+/**
+ * parse wsc M1/M2 message.
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param buf input, tlvs start position of receive cmdu packet
+ * \return error code
+ */
+
+static int parse_ap_autoconfig_wsc_message
+ (struct p1905_managerd_ctx *ctx, unsigned char *buf)
+{
+ int length =0;
+ unsigned char *temp_buf;
+ unsigned int integrity = 0;
+
+ temp_buf = buf;
+
+ while(1)
+ {
+ if(*temp_buf == WSC_TLV_TYPE)
+ {
+ integrity |= (1 << WSC_TLV_TYPE);
+
+ length = parse_wsc_tlv(temp_buf, ctx);
+
+ if(length < 0)
+ {
+ debug_syslog("error wsc tlv \n");
+ return -1;
+ }
+ temp_buf += length;
+ }
+ else if(*temp_buf == END_OF_TLV_TYPE)
+ break;
+ else
+ {
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ }
+ }
+
+ /*check integrity*/
+ if(integrity != check_integrity[AP_AUTOCONFIG_WSC])
+ {
+ debug_syslog("incomplete ap auto config WSC 0x%x 0x%x\n",
+ integrity,check_integrity[AP_AUTOCONFIG_WSC]);
+ return -1;
+ }
+
+ return 0;
+}
+#endif
+
+/**
+ * parse cmdu message.
+ *
+ *
+ * \param ctx 1905.1 managerd context
+ * \param buf input, cmdu message start address(header + payload)
+ * \param dmac input, dmac of receive packet
+ * \param smac input, smac of receive packet
+ * \param len input, length of received packet
+ * \return error code
+ */
+int parse_cmdu_message(struct p1905_managerd_ctx *ctx, unsigned char *buf,
+ unsigned char *dmac, unsigned char *smac,
+ int len)
+{
+ unsigned char *temp_buf;
+ unsigned char mversion;
+ unsigned short mtype;
+ unsigned short mid;
+ unsigned char fid;
+ unsigned char last_fragment_ind;
+ unsigned char relay_ind;
+ unsigned char al_mac[ETH_ALEN];
+ unsigned char need_query = 0;
+ unsigned char need_notify = 0;
+ int tlv_len = 0, temp_tlv_len = 0;
+ unsigned short queue_len = 0;
+ unsigned char *tlv_start;
+#ifdef CMDU_MESSAGE_PARSE_DEBUG
+ int i = 0;
+#endif
+#ifdef SUPPORT_AP_AUTO_CONFIG
+ //unsigned char recv_port_mac[ETH_ALEN];
+#endif
+#ifdef SUPPORT_ALME
+ ALME_MSG *msg;
+ ALME_GET_METRIC_RSP *metric_rsp;
+ int metric_status = 1;
+#endif
+#ifdef SUPPORT_WIFI
+ unsigned char sta_itf[6];
+#endif
+ struct p1905_vs_info vs_info;
+
+ /*get message info from message header*/
+ temp_buf = buf;
+ tlv_start = buf + 8;
+ mversion = get_mversion_from_msg_hdr(temp_buf);
+ mtype = get_mtype_from_msg_hdr(temp_buf);
+ mid = get_mid_from_msg_hdr(temp_buf);
+ fid = get_fid_from_msg_hdr(temp_buf);
+ last_fragment_ind = get_last_fragment_ind_from_msg_hdr(temp_buf);
+ relay_ind = get_relay_ind_from_msg_hdr(temp_buf);
+
+ debug("cmdu message type = 0x%x\n",mtype);
+ debug("cmdu message id = %d\n",mid);
+ debug("cmdu message fid = %d\n",fid);
+ debug("cmdu message last fragment ind = %d\n",last_fragment_ind);
+
+ /*deal with message version, it must be 0x00*/
+ if(mversion != MESSAGE_VERSION)
+ return 0;
+
+ /*if relay indicator =1 but this is not relay multicast message, ignore*/
+ if(!check_relay_message(relay_ind, mtype))
+ return 0;
+ if(relay_ind)
+ ctx->need_relay = 1;
+
+ /*check the fragment relevant field
+ *if fragment, insert to fragment queue and try to reassembly
+ */
+ if(last_fragment_ind == 0 || fid != 0)
+ {
+ /*because of receiving fragment cmdu, we need to record the total
+ *tlvs length of message.
+ *we use the received length reported by socket and to minus the
+ *ether header length & cmdu message header length
+ */
+ temp_tlv_len = len - ETH_HLEN - sizeof(cmdu_message_header);
+#ifdef CMDU_MESSAGE_PARSE_DEBUG
+ debug("receive a fragment CMDU\n");
+ for(i=0;i<temp_tlv_len;i++)
+ {
+ debug("%.2x ",*(tlv_start + i));
+ if((i % 16) == 0)
+ debug("\n");
+ }
+#endif
+ if(last_fragment_ind == 0)
+ tlv_len = calculate_fragment_tlvs_len(tlv_start, temp_tlv_len);
+ else
+ tlv_len = temp_tlv_len;
+
+ insert_fragment_queue(mtype, mid, fid, last_fragment_ind, tlv_len,\
+ tlv_start);
+ queue_len = reassembly_fragment_queue(rx_buffer, mtype, mid);
+
+ /*do not parse this message unless each fragment was received*/
+ if(queue_len == 0)
+ {
+ if(relay_ind)
+ ctx->need_relay = 1;
+
+ return 0;
+ }
+ }
+
+ /*shift to payload, if queue_len > 0 ==> get message tlv from fragment
+ *queue. otherwise, get payload from original allocated buffer
+ */
+ if(queue_len > 0)
+ temp_buf = rx_buffer;
+ else
+ temp_buf = tlv_start;
+
+ switch(mtype)
+ {
+ case TOPOLOGY_DISCOVERY:
+ /* need to do ==> check al mac addr tlv
+ * & MID to know whether this message is duplicated
+ */
+ if(0 > parse_topology_discovery_message(ctx, temp_buf,\
+ &need_query, &need_notify, al_mac, smac))
+ {
+ debug("receive error topology discovery message\n");
+ //return -1;
+ break;
+ }
+ /*if receive topology discovery message in new device, query it*/
+ if(need_query)
+ {
+ ctx->mid = ctx->mid + 1;
+#ifdef SUPPORT_WIFI
+ /* if a AP want to send unicast cmdu to STA, it needs to use interface mac
+ * instead of AL mac address
+ */
+ if(is_neighbor_wifi_sta(ctx, sta_itf, al_mac))
+ {
+ debug_syslog("1. STA neighbor: %.2x %.2x %.2x %.2x %.2x %.2x\n",
+ sta_itf[0], sta_itf[1], sta_itf[2], sta_itf[3], sta_itf[4], sta_itf[5]);
+
+ insert_cmdu_txq(sta_itf, ctx->p1905_al_mac_addr,
+ e_topology_query, ctx->mid);
+ }
+ else
+#endif
+ insert_cmdu_txq(al_mac, ctx->p1905_al_mac_addr,
+ e_topology_query, ctx->mid);
+ }
+ /*if local p1905.1 neighbor device info updated, notify*/
+ if(need_notify)
+ {
+ ctx->mid = ctx->mid + 1;
+ insert_cmdu_txq(p1905_multicast_address, ctx->plc0_mac_addr,
+ e_topology_notification,ctx->mid);
+ insert_cmdu_txq(p1905_multicast_address, ctx->eth0_mac_addr,
+ e_topology_notification,ctx->mid);
+#ifdef SUPPORT_WIFI
+ insert_cmdu_txq(p1905_multicast_address, ctx->wifi0_mac_addr,
+ e_topology_notification, ctx->mid);
+#endif
+ }
+ break;
+ case TOPOLOGY_NOTIFICATION:
+ if(0 > parse_topology_notification_message(ctx,temp_buf,al_mac))
+ {
+ debug("receive error topology notification message\n");
+ //return -1;
+ break;
+ }
+
+ /*send a topology query after receiving the topology notification message*/
+ ctx->mid = ctx->mid + 1;
+
+#ifdef SUPPORT_WIFI
+ /* if a AP want to send unicast cmdu to STA, it needs to use interface mac
+ * instead of AL mac address
+ */
+ if(is_neighbor_wifi_sta(ctx, sta_itf, al_mac))
+ {
+ debug_syslog("8. STA neighbor: %.2x %.2x %.2x %.2x %.2x %.2x\n",
+ sta_itf[0], sta_itf[1], sta_itf[2], sta_itf[3], sta_itf[4], sta_itf[5]);
+
+ insert_cmdu_txq(sta_itf, ctx->p1905_al_mac_addr,
+ e_topology_query, ctx->mid);
+ }
+ else
+#endif
+ insert_cmdu_txq(al_mac, ctx->p1905_al_mac_addr, e_topology_query, ctx->mid);
+ ctx->need_relay = 1;
+ break;
+ case TOPOLOGY_QUERY:
+ /*response topology response message with the same mid in received
+ *topology query message do not need to parse tlvs becasue no
+ *p1905.1 tlvs in topology query message
+ */
+ if(0> find_al_mac_address(ctx,smac,al_mac))//cannot find AL mac addr by source mac , use source mac as destination
+ {
+ debug("cannot find al mac in database\n");
+ memcpy(al_mac,smac,ETH_ALEN);
+ }
+
+#ifdef SUPPORT_WIFI
+ /* if a AP want to send unicast cmdu to STA, it needs to use interface mac
+ * instead of AL mac address
+ */
+ if(is_neighbor_wifi_sta(ctx, sta_itf, al_mac))
+ {
+ debug_syslog("2. STA neighbor: %.2x %.2x %.2x %.2x %.2x %.2x\n",
+ sta_itf[0], sta_itf[1], sta_itf[2], sta_itf[3], sta_itf[4], sta_itf[5]);
+ insert_cmdu_txq(sta_itf, ctx->p1905_al_mac_addr,
+ e_topology_response, mid);
+ }
+ else
+#endif
+ insert_cmdu_txq(al_mac,ctx->p1905_al_mac_addr,e_topology_response,mid);
+ break;
+ case TOPOLOGY_RESPONSE:
+ if(0 > parse_topology_response_message(ctx,temp_buf))
+ {
+ debug_syslog("receive error topology response message\n");
+ //return -1;
+ }
+ break;
+ case VENDOR_SPECIFIC:
+ if(0 > parse_vendor_specific_message(temp_buf, &vs_info))
+ {
+ debug_syslog("receive other vendor's vendor specific message\n");
+ break;
+ }
+ ctx->mid++;
+ insert_cmdu_txq(smac, ctx->eth0_mac_addr,
+ e_vendor_specific,ctx->mid);
+ break;
+
+ case LINK_METRICS_QUERY:
+ if(0 > parse_link_metric_query_message(ctx, temp_buf))
+ {
+ debug("receive error link metric query response message\n");
+ //return -1;
+ break;
+ }
+
+ if(0> find_al_mac_address(ctx,smac,al_mac))
+ {
+ debug("cannot find al mac in database\n");
+ memcpy(al_mac,smac,ETH_ALEN);
+ }
+#ifdef SUPPORT_WIFI
+ /* if a AP want to send unicast cmdu to STA, it needs to use interface mac
+ * instead of AL mac address
+ */
+ if(is_neighbor_wifi_sta(ctx, sta_itf, al_mac))
+ {
+ debug_syslog("4. STA neighbor: %.2x %.2x %.2x %.2x %.2x %.2x\n",
+ sta_itf[0], sta_itf[1], sta_itf[2], sta_itf[3], sta_itf[4], sta_itf[5]);
+ insert_cmdu_txq(sta_itf, ctx->p1905_al_mac_addr,
+ e_link_metric_response, mid);
+ }
+ else
+#endif
+ insert_cmdu_txq(al_mac,ctx->p1905_al_mac_addr,e_link_metric_response,mid);
+ break;
+
+#ifdef SUPPORT_ALME
+ case LINK_METRICS_RESPONSE:
+ if(ctx->alme_state == alme_wait_4_link_metrics_rsp &&
+ ctx->link_metric_query_mid == mid)
+ {
+ /*allocate a buffer for alme message feedback*/
+ msg = (ALME_MSG *)malloc(sizeof(ALME_MSG));
+ metric_rsp = (ALME_GET_METRIC_RSP *)msg->body;
+
+ if(0 > parse_link_metric_response_message(ctx, temp_buf, metric_rsp))
+ {
+ ///*free alme message buffer*/
+ //free(msg);
+ //break;
+ metric_status = -1;
+ }
+
+ /*send alme get metric response*/
+ if(metric_status > 0)
+ send_alme_get_metric_rsp(ctx->almefd, msg, alme_success);
+ else
+ send_alme_get_metric_rsp(ctx->almefd, msg, alme_failure);
+
+ /*reset ALME state and close connnection between alme server and client*/
+ ctx->alme_state = alme_none;
+ ctx->alme_wait_4_link_metrics_cnt = 0;
+ if(ctx->almefd > 0)
+ {
+ close(ctx->almefd);
+ ctx->almefd = -1;
+ }
+ /*free alme message buffer*/
+ free(msg);
+ }
+ break;
+#endif
+ case P1905_PB_EVENT_NOTIFY:
+ if(ctx->pbc_param.is_sc)
+ {
+ ctx->need_relay = 1;
+ return 0;
+ }
+
+ if(0 > parse_push_button_event_notification_message(ctx, temp_buf))
+ {
+ debug("receive error pbc event notification message\n");
+#ifdef SUPPORT_AP_REGISTRAR
+ /*reset no_need_start_pbc*/
+ ctx->pbc_param.wifi_info.no_need_start_pbc = 0;
+#endif
+ break;
+ }
+ /*strore the message id for pbc join notification use*/
+ ctx->pbc_param.mid = mid;
+ /* set ctx->pbc_param.is_sc to 1 ==>
+ * 1. start polling the sc state, if new device joined, need to
+ * send push button join notification
+ * 2. not allowed to deal with anthoner push button event notification
+ * until ctx->pbc_param.is_sc is set to 0 again
+ */
+ ctx->pbc_param.is_sc = 1;
+
+ if(0 > trigger_push_button_config_start(ctx))
+ {
+ debug("P1905_PB_EVENT_NOTIFY trigger fail\n");
+ }
+
+#ifdef SUPPORT_AP_REGISTRAR
+ if(!ctx->pbc_param.wifi_info.no_need_start_pbc)
+ {
+ /*launch wifi PBC configuration*/
+ trigger_wifi_PBC_config();
+ /*reset no_need_start_pbc*/
+ ctx->pbc_param.wifi_info.no_need_start_pbc = 0;
+ }
+#endif
+ ctx->need_relay = 1;
+ break;
+#ifdef SUPPORT_AP_AUTO_CONFIG
+#ifdef SUPPORT_AP_REGISTRAR
+ case AP_AUTOCONFIG_SEARCH:
+ debug("got AP_AUTOCONFIG_SEARCH\n");
+ //ctx->need_relay = 1;
+ if(0 > parse_ap_autoconfig_search_message(ctx, temp_buf, al_mac))
+ {
+ debug("no need to response this autoconfig search message\n");
+ break;
+ }
+ /*send ap auto config response message, unicast*/
+ insert_cmdu_txq(al_mac, ctx->p1905_al_mac_addr,
+ e_ap_autoconfiguration_response, mid);
+ break;
+#endif
+#ifdef SUPPORT_AP_ENROLLE
+ case AP_AUTOCONFIG_RESPONSE:
+ debug("got AP_AUTOCONFIG_RESPONSE\n");
+ if((ctx->autoconfig_search_mid != mid) ||
+ (ctx->enrolle_state != wait_4_recv_ap_autoconfig_resp))
+ break;
+
+ if(0 > parse_ap_autoconfig_response_message(ctx, temp_buf))
+ {
+ debug("no need to response this autoconfig response message\n");
+ break;
+ }
+ /*send ap auto config WSC M1 message, unicast*/
+ ctx->mid++;
+
+ if(0> find_al_mac_address(ctx,smac,al_mac))//cannot find AL mac addr by source mac , use source mac as destination
+ {
+ debug("cannot find al mac in database\n");
+ memcpy(al_mac,smac,ETH_ALEN);
+ }
+ insert_cmdu_txq(al_mac, ctx->p1905_al_mac_addr,
+ e_ap_autoconfiguration_wsc_m1, ctx->mid);
+
+ ctx->enrolle_state = wait_4_recv_m2;
+
+ debug_syslog("send WSC M1 to %.2x %.2x %.2x %.2x %.2x %.2x\n",
+ al_mac[0], al_mac[1], al_mac[2], al_mac[3], al_mac[4], al_mac[5]);
+
+ break;
+
+ case AP_AUTOCONFIG_RENEW:
+ debug("got AP_AUTOCONFIG_RENEW\n");
+ if(ctx->enrolle_state != no_ap_autoconfig)
+ break;
+
+ if(0 > parse_ap_autoconfig_renew_message(ctx, temp_buf, al_mac))
+ {
+ debug("no need to response this autoconfig renew message\n");
+ break;
+ }
+
+ /* need to implement check mechanism to make sure this message
+ * comes from original registrar...............
+ */
+
+ /*send ap auto config WSC M1 message, unicast*/
+ ctx->mid++;
+ insert_cmdu_txq(al_mac, ctx->p1905_al_mac_addr,
+ e_ap_autoconfiguration_wsc_m1, ctx->mid);
+ ctx->enrolle_state = wait_4_recv_m2;
+ debug_syslog("got AP_AUTOCONFIG_RENEW ctx->ap_config_timeout_cnt = %d\n",ctx->ap_config_timeout_cnt);
+ ctx->ap_config_timeout_cnt = 0;
+
+ break;
+#endif
+ case AP_AUTOCONFIG_WSC:
+ debug("got AP_AUTOCONFIG_WSC\n");
+#ifdef SUPPORT_AP_ENROLLE
+ if(ctx->enrolle_state != wait_4_recv_m2)
+ break;
+#endif
+ if(0 > parse_ap_autoconfig_wsc_message(ctx, temp_buf))
+ {
+ debug_syslog("receive error ap autoconfig wsc message\n");
+ break;
+ }
+#ifdef SUPPORT_AP_ENROLLE
+ /*we got correct M2, so enter the set config state*/
+ ctx->enrolle_state = wait_4_set_config;
+#endif
+#ifdef SUPPORT_AP_REGISTRAR
+ /* Registrar got correct M1. Need to response M2.(Unicast)
+ * It has a strange behavior about mid. Spec does not specify we
+ * need to use same mid for M1 and M2. So I create a new mid for M2
+ */
+ ctx->mid++;
+
+ if(0> find_al_mac_address(ctx,smac,al_mac))//cannot find AL mac addr by source mac , use source mac as destination
+ {
+ debug("cannot find al mac in database\n");
+ memcpy(al_mac,smac,ETH_ALEN);
+ }
+ insert_cmdu_txq(al_mac, ctx->p1905_al_mac_addr,
+ e_ap_autoconfiguration_wsc_m2, ctx->mid);
+
+ debug_syslog("send WSC M2 to %.2x %.2x %.2x %.2x %.2x %.2x\n",
+ al_mac[0], al_mac[1], al_mac[2], al_mac[3], al_mac[4], al_mac[5]);
+#endif
+ break;
+#endif
+ }
+ return 0;
+}
+