summaryrefslogtreecommitdiff
path: root/cleopatre
diff options
context:
space:
mode:
Diffstat (limited to 'cleopatre')
-rw-r--r--cleopatre/application/libalme/.gitignore2
-rw-r--r--cleopatre/application/libalme/Makefile68
-rw-r--r--cleopatre/application/libalme/inc/libalme.h125
-rw-r--r--cleopatre/application/libalme/src/libalme.c176
-rw-r--r--cleopatre/application/libalme/src/sync.c44
-rw-r--r--cleopatre/application/libspid/inc/defs.h5
-rw-r--r--cleopatre/application/libspid/inc/libspid.h1
-rw-r--r--cleopatre/application/libspid/src/network.c9
-rw-r--r--cleopatre/application/spidgoahead/LINUX/spidcom_asp_functions.c315
-rw-r--r--cleopatre/application/spidgoahead/Makefile13
-rw-r--r--cleopatre/application/spidgoahead/web/1905_stat.asp164
-rw-r--r--cleopatre/application/spidgoahead/web/close.gifbin0 -> 67 bytes
-rw-r--r--cleopatre/application/spidgoahead/web/intf.asp126
-rw-r--r--cleopatre/application/spidgoahead/web/metric.asp257
-rw-r--r--cleopatre/application/spidgoahead/web/nav.asp42
-rw-r--r--cleopatre/application/spidgoahead/web/open.gifbin0 -> 64 bytes
-rw-r--r--cleopatre/application/spidgoahead/web/tree_node0.gifbin0 -> 113 bytes
-rw-r--r--cleopatre/application/spidgoahead/web/tree_node1.gifbin0 -> 109 bytes
-rw-r--r--cleopatre/application/spidgoahead/web/wifi0.asp6
-rw-r--r--cleopatre/buildroot/package/Config.in6
-rw-r--r--cleopatre/buildroot/package/libalme/Config.in6
-rw-r--r--cleopatre/buildroot/package/libalme/libalme.mk73
-rw-r--r--cleopatre/buildroot/package/p1905_managerd/Config.in6
-rwxr-xr-xcleopatre/buildroot/package/p1905_managerd/p1905_managerd.mk77
-rw-r--r--cleopatre/buildroot/target/device/Spidcom/common/defconfig.base2
-rw-r--r--cleopatre/buildroot/target/device/Spidcom/common/linux26.config.base1
-rw-r--r--cleopatre/buildroot/target/device/Spidcom/targets/mpr520e/defconfig.part5
-rw-r--r--cleopatre/buildroot/target/device/Spidcom/targets/mpr520e/linux26.config.part1
-rw-r--r--cleopatre/devkit/mt7601udrv/RT2870AP.dat1
-rw-r--r--cleopatre/devkit/mt7601udrv/ap/ap_cfg.c61
-rw-r--r--cleopatre/devkit/mt7601udrv/common/cmm_info.c238
-rw-r--r--cleopatre/devkit/mt7601udrv/include/ap_cfg.h15
-rw-r--r--cleopatre/devkit/mt7601udrv/include/oid.h37
-rw-r--r--cleopatre/devkit/mt7601udrv/include/os/rt_os.h9
-rw-r--r--cleopatre/devkit/mt7601udrv/include/rtmp_cmd.h8
-rw-r--r--cleopatre/devkit/mt7601udrv/os/linux/ap_ioctl.c21
-rw-r--r--cleopatre/devkit/mt7601udrv/os/linux/config.mk10
-rw-r--r--cleopatre/devkit/p1905_managerd/.gitignore4
-rw-r--r--cleopatre/devkit/p1905_managerd/Makefile138
-rw-r--r--cleopatre/devkit/p1905_managerd/S98p190535
-rw-r--r--cleopatre/devkit/p1905_managerd/config.mk36
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/alme.h18
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/cmdu.h36
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/cmdu_fragment.h25
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/cmdu_message.h88
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/cmdu_message_parse.h16
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/cmdu_tlv.h108
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/cmdu_tlv_parse.h18
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/lldp.h21
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/lldp_message.h62
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/lldp_message_parse.h20
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/mt7601_io.h73
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/p1905_ap_autoconfig.h36
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/p1905_database.h144
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/p1905_managerd.h151
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/p1905_utils.h38
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/plc_utils.h34
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/rt5572_io.h36
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/wifi_utils.h96
-rw-r--r--cleopatre/devkit/p1905_managerd/inc/wsc_attr_tlv.h100
-rw-r--r--cleopatre/devkit/p1905_managerd/src/alme.c367
-rw-r--r--cleopatre/devkit/p1905_managerd/src/cmdu.c689
-rw-r--r--cleopatre/devkit/p1905_managerd/src/cmdu_fragment.c291
-rw-r--r--cleopatre/devkit/p1905_managerd/src/cmdu_message.c741
-rw-r--r--cleopatre/devkit/p1905_managerd/src/cmdu_message_parse.c1882
-rw-r--r--cleopatre/devkit/p1905_managerd/src/cmdu_tlv.c985
-rw-r--r--cleopatre/devkit/p1905_managerd/src/cmdu_tlv_parse.c957
-rw-r--r--cleopatre/devkit/p1905_managerd/src/lldp.c126
-rw-r--r--cleopatre/devkit/p1905_managerd/src/lldp_message.c147
-rw-r--r--cleopatre/devkit/p1905_managerd/src/lldp_message_parse.c408
-rw-r--r--cleopatre/devkit/p1905_managerd/src/mt7601_io.c1448
-rw-r--r--cleopatre/devkit/p1905_managerd/src/p1905_ap_autoconfig.c201
-rw-r--r--cleopatre/devkit/p1905_managerd/src/p1905_managerd.c608
-rw-r--r--cleopatre/devkit/p1905_managerd/src/p1905_push_button.c264
-rw-r--r--cleopatre/devkit/p1905_managerd/src/p1905_utils.c928
-rw-r--r--cleopatre/devkit/p1905_managerd/src/plc_utils.c328
-rw-r--r--cleopatre/devkit/p1905_managerd/src/rt5572_io.c769
-rw-r--r--cleopatre/devkit/p1905_managerd/src/wifi_utils.c540
-rw-r--r--cleopatre/devkit/p1905_managerd/src/wsc_attr_tlv.c1353
-rw-r--r--cleopatre/devkit/p1905_managerd/src/wsc_message.c597
-rw-r--r--cleopatre/linux-2.6.25.10-spc300/include/linux/if_bridge.h2
-rw-r--r--cleopatre/linux-2.6.25.10-spc300/include/linux/if_ether.h4
-rwxr-xr-x[-rw-r--r--]cleopatre/linux-2.6.25.10-spc300/net/Kconfig9
-rwxr-xr-x[-rw-r--r--]cleopatre/linux-2.6.25.10-spc300/net/bridge/br_device.c17
-rwxr-xr-x[-rw-r--r--]cleopatre/linux-2.6.25.10-spc300/net/bridge/br_input.c22
-rwxr-xr-x[-rw-r--r--]cleopatre/linux-2.6.25.10-spc300/net/bridge/br_ioctl.c62
-rwxr-xr-x[-rw-r--r--]cleopatre/linux-2.6.25.10-spc300/net/bridge/br_private.h3
87 files changed, 16984 insertions, 27 deletions
diff --git a/cleopatre/application/libalme/.gitignore b/cleopatre/application/libalme/.gitignore
new file mode 100644
index 0000000000..727969d2b0
--- /dev/null
+++ b/cleopatre/application/libalme/.gitignore
@@ -0,0 +1,2 @@
+/libalme.a
+/libalme.so
diff --git a/cleopatre/application/libalme/Makefile b/cleopatre/application/libalme/Makefile
new file mode 100644
index 0000000000..0673023d3b
--- /dev/null
+++ b/cleopatre/application/libalme/Makefile
@@ -0,0 +1,68 @@
+STAT_LIB=libalme.a
+DYN_LIB=libalme.so
+OBJPATH=obj
+SRCPATH=src
+INCPATH=inc
+
+CLEO_DIR = ../..
+LINUX_DIR = $(CLEO_DIR)/linux-2.6.25.10-spc300
+
+ifeq ($(CC_FOR_TARGET),) #direct compile
+CC=arm-linux-gcc
+AR=arm-linux-ar
+CC_WITH_CFLAGS=$(CC) -g -Os
+CC_WITHOUT_CFLAGS=$(CC)
+else #compile from buildroot
+CC_WITH_CFLAGS=$(CC)
+CC_WITHOUT_CFLAGS=$(CC_FOR_TARGET)
+endif
+
+RESPONSE_FILE = extra_flags
+INCLUDES = -I$(INCPATH) \
+ -I$(CLEO_DIR)/include \
+ -I$(LINUX_DIR)/include
+
+EXTRA_CFLAGS = $(INCLUDES) -MMD -Wall \
+ @$(CLEO_DIR)/$(RESPONSE_FILE) \
+
+SRCS=$(subst $(SRCPATH)/,,$(wildcard $(SRCPATH)/*.c))
+DYN_OBJS=$(addprefix $(OBJPATH)/,$(SRCS:.c=.dyn.o))
+STAT_OBJS=$(addprefix $(OBJPATH)/,$(SRCS:.c=.stat.o))
+DYN_DEPS=$(patsubst %o,%d,$(DYN_OBJS))
+STAT_DEPS=$(patsubst %o,%d,$(STAT_OBJS))
+#MMTYPES=$(INCPATH)/mmtypes.h
+#MMTCGEN=$(CLEO_DIR)/../common/lib/mmtcgen
+
+all: $(STAT_LIB) $(DYN_LIB)
+
+$(STAT_LIB): $(STAT_OBJS)
+ $(AR) cr $@ $(STAT_OBJS)
+
+$(DYN_LIB): $(DYN_OBJS)
+ $(CC_WITHOUT_CFLAGS) -shared -fPIC -o $@ $(DYN_OBJS)
+
+$(OBJPATH)/%.stat.o: $(SRCPATH)/%.c
+ $(CC_WITH_CFLAGS) $(EXTRA_CFLAGS) -o $@ -c $<
+
+$(OBJPATH)/%.dyn.o: $(SRCPATH)/%.c
+ $(CC_WITH_CFLAGS) $(EXTRA_CFLAGS) -fPIC -o $@ -c $<
+
+$(DYN_OBJS) $(STAT_OBJS): | $(OBJPATH)
+
+$(OBJPATH):
+ mkdir $(OBJPATH)
+
+-include $(STAT_DEPS) $(DYN_DEPS)
+
+.PHONY: all clean distclean
+
+#distclean: clean
+# if [ -e $(MMTCGEN) ]; then \
+# rm -f $(MMTYPES); \
+# fi;
+
+clean:
+ rm -f $(DYN_OBJS) $(STAT_OBJS) \
+ $(STAT_DEPS) $(DYN_DEPS) \
+ $(DYN_LIB) $(STAT_LIB)
+ rmdir $(OBJPATH)
diff --git a/cleopatre/application/libalme/inc/libalme.h b/cleopatre/application/libalme/inc/libalme.h
new file mode 100644
index 0000000000..3491a9ed33
--- /dev/null
+++ b/cleopatre/application/libalme/inc/libalme.h
@@ -0,0 +1,125 @@
+/*
+ * cleopatre/application/libalme/inc/libalme.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ *
+ */
+#ifndef LIBALME_H
+#define LIBALME_H
+#include <linux/if_ether.h>
+
+
+#define MAX_1905_NEIGHBOR_NUM 8
+#define MAX_1905_LINK_NUM 3 //PLC + ETH + WIFI
+#define MAX_ALME_MSG_BODY_LEN 1024
+
+typedef enum
+{
+ libalme_success = 0,
+ libalme_failure,
+
+} libalme_error_code;
+
+
+typedef enum
+{
+ alme_get_metric_req = 1,
+ alme_get_metric_rsp,
+ alme_get_intf_list_req,
+ alme_get_intf_list_rsp,
+
+
+ /*below is for MStar private*/
+ alme_get_1905_neighbor_req = 101,
+ alme_get_1905_neighbor_rsp,
+
+} ALME_TYPE;
+
+typedef enum
+{
+ alme_success = 0,
+ alme_unmatched_mac_addr,
+ alme_unsupported_pwr_state,
+ alme_unavailable_pwr_state,
+ alme_nbr_of_fwd_rule_exceeded,
+ alme_invalid_rule_id,
+ alme_duplicated_classification_set,
+ alme_unmatched_neighbor_mac_addr,
+ alme_failure,
+} ALME_REASON_CODE;
+
+typedef struct _ALME_MSG_HEADER
+{
+ ALME_TYPE type;
+ unsigned short length;
+} __attribute__ ((__packed__)) ALME_MSG_HEADER;
+
+typedef struct _ALME_MSG
+{
+ ALME_MSG_HEADER header;
+ unsigned char body[MAX_ALME_MSG_BODY_LEN];
+} __attribute__ ((__packed__)) ALME_MSG;
+
+/*get metric req/rsp message body struct*/
+typedef struct _ALME_GET_METRIC_REQ
+{
+ unsigned char mac_addr[6];
+} __attribute__ ((__packed__)) ALME_GET_METRIC_REQ;
+
+typedef struct _LINK_METRICS
+{
+ unsigned short itf_type;
+ unsigned int tx_packet_error;
+ unsigned int tx_total_packet;
+ unsigned short max_throughput_capacity;
+ unsigned short link_availability;
+ unsigned short phy_rate;
+ unsigned int rx_packet_error;
+ unsigned int rx_total_packet;
+ signed char rssi;
+} __attribute__ ((__packed__)) LINK_METRICS;
+
+typedef struct _METRIC_DESCRIPTOR
+{
+ unsigned char neighbor_al_mac[6];
+ unsigned char local_itf_mac[6];
+ unsigned char ieee802_1_bridge;
+ LINK_METRICS link_metrics;
+
+} __attribute__ ((__packed__)) METRIC_DESCRIPTOR;
+
+typedef struct _ALME_GET_METRIC_RSP
+{
+ METRIC_DESCRIPTOR descriptorlist[MAX_1905_LINK_NUM];
+ unsigned char descriptor_num;
+ ALME_REASON_CODE reason_code;
+} __attribute__ ((__packed__)) ALME_GET_METRIC_RSP;
+
+/*get interface list req/rsp message body struct*/
+typedef struct _INTFLIST
+{
+ unsigned char mac_addr[6];
+ unsigned short intf_type;
+ unsigned char ieee802_1_bridge;
+
+} __attribute__ ((__packed__)) INTFLIST;
+
+
+typedef struct _ALME_GET_INTF_LIST_RSP
+{
+ INTFLIST intflist[MAX_1905_LINK_NUM];
+ unsigned char intfnum;
+
+} __attribute__ ((__packed__)) ALME_GET_INTF_LIST_RSP;
+
+
+/*MStar private: get neighbor req/rsp message body struct*/
+typedef struct _ALME_GET_1905_NEIGHBOR_RSP
+{
+ unsigned char neighborlist[MAX_1905_NEIGHBOR_NUM * 6];
+ unsigned char neighbornum;
+
+} __attribute__ ((__packed__)) ALME_GET_1905_NEIGHBOR_RSP;
+
+
+#endif /* LIBALME_H */
diff --git a/cleopatre/application/libalme/src/libalme.c b/cleopatre/application/libalme/src/libalme.c
new file mode 100644
index 0000000000..9d8ee497d9
--- /dev/null
+++ b/cleopatre/application/libalme/src/libalme.c
@@ -0,0 +1,176 @@
+/*
+ * cleopatre/application/libalme/src/libalme.c
+ *
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <syslog.h>
+#include <errno.h>
+#include <assert.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <sys/socket.h>
+#include <sys/un.h>
+#include "libalme.h"
+
+#include <sys/types.h>
+#include <sys/ipc.h>
+#include <sys/sem.h>
+
+static libalme_error_code alme_send(ALME_MSG *msg)
+{
+ struct sockaddr_un sock_addr;
+ libalme_error_code alme_status = libalme_success;
+ int fd, status, len, result;
+ fd_set readfds;
+ ALME_TYPE alme_type;
+ int semid;
+
+ semid = get_binary_semaphore_id();
+ if(semid < 0)
+ {
+ syslog(LOG_WARNING,"alme semaphore get error(%s)\n",strerror(errno));
+ alme_status = libalme_failure;
+ goto end;
+ }
+
+ if(-1 == binary_semaphore_wait(semid))
+ {
+ syslog(LOG_WARNING,"alme semaphore wait error(%s)\n",strerror(errno));
+ alme_status = libalme_failure;
+ goto end;
+ }
+
+ fd = socket(AF_UNIX, SOCK_STREAM , 0);
+ if(fd < 0)
+ {
+ syslog(LOG_WARNING,"open ALME socket error(%s)\n",strerror(errno));
+ alme_status = libalme_failure;
+ goto end;
+ }
+
+ memset(&sock_addr, 0, sizeof(sock_addr));
+ sock_addr.sun_family = AF_UNIX;
+ strcpy(sock_addr.sun_path, "/tmp/alme_client");
+ unlink("/tmp/alme_client");
+
+ status = bind(fd, (struct sockaddr *)&sock_addr, sizeof(sock_addr));
+ if(status < 0)
+ {
+ syslog(LOG_WARNING,"bind to client ALME socket error(%s)", strerror(errno));
+ alme_status = libalme_failure;
+ goto error1;
+ }
+
+ memset(&sock_addr, 0, sizeof(sock_addr));
+ sock_addr.sun_family = AF_UNIX;
+ strcpy(sock_addr.sun_path, "/tmp/alme_server");
+ status = connect(fd, (struct sockaddr *)&sock_addr, sizeof(sock_addr));
+
+ if(status < 0)
+ {
+ syslog(LOG_WARNING,"connect ALME socket error(%s)", strerror(errno));
+ alme_status = libalme_failure;
+ goto error1;
+ }
+
+ alme_type = msg->header.type;
+
+ len = send(fd, msg, (msg->header.length + sizeof(msg->header)), 0);
+
+ if(len < 0)
+ {
+ syslog(LOG_WARNING,"send to ALME socket error(%s)", strerror(errno));
+ alme_status = libalme_failure;
+ goto error1;
+ }
+
+ while(1)
+ {
+ FD_ZERO(&readfds);
+ FD_SET(fd, &readfds);
+ result = select (fd + 1, &readfds, NULL, NULL, NULL);
+
+ if(result > 0)
+ {
+ len = recv(fd, msg, sizeof(ALME_MSG), 0);
+ if((alme_type + 1) == msg->header.type)
+ break;
+ }
+ }
+
+error1:
+ close(fd);
+ if(-1 == binary_semaphore_post(semid))
+ syslog(LOG_WARNING,"alme semaphore post error(%s)\n",strerror(errno));
+end:
+ return alme_status;
+}
+
+
+libalme_error_code libalme_get_metric_req_rsp(ALME_GET_METRIC_REQ *req,
+ ALME_GET_METRIC_RSP *rsp)
+{
+ ALME_MSG msg;
+ ALME_GET_METRIC_REQ *request;
+ ALME_GET_METRIC_RSP *response;
+
+ request = (ALME_GET_METRIC_REQ *)msg.body;
+ memcpy(request->mac_addr, req->mac_addr, 6);
+ msg.header.type = alme_get_metric_req;
+ msg.header.length = sizeof(ALME_GET_METRIC_REQ);
+
+ if(libalme_success != alme_send(&msg))
+ {
+ return libalme_failure;
+ }
+
+ response = (ALME_GET_METRIC_RSP *)msg.body;
+ memcpy(rsp, response, sizeof(ALME_GET_METRIC_RSP));
+
+ return libalme_success;
+}
+
+libalme_error_code
+libalme_get_intf_list_req_rsp(ALME_GET_INTF_LIST_RSP *rsp)
+{
+ ALME_MSG msg;
+ ALME_GET_INTF_LIST_RSP *response;
+
+ msg.header.type = alme_get_intf_list_req;
+ msg.header.length = 0;
+
+ if(libalme_success != alme_send(&msg))
+ {
+ return libalme_failure;
+ }
+
+ response = (ALME_GET_INTF_LIST_RSP *)msg.body;
+ memcpy(rsp, response, sizeof(ALME_GET_INTF_LIST_RSP));
+
+ return libalme_success;
+}
+
+
+/*below is MStar private ALME protocol*/
+libalme_error_code
+libalme_get_1905_neighbor_req_rsp(ALME_GET_1905_NEIGHBOR_RSP *rsp)
+{
+ ALME_MSG msg;
+ ALME_GET_1905_NEIGHBOR_RSP *response;
+
+ msg.header.type = alme_get_1905_neighbor_req;
+ msg.header.length = 0;
+
+ if(libalme_success != alme_send(&msg))
+ {
+ return libalme_failure;
+ }
+ response = (ALME_GET_1905_NEIGHBOR_RSP *)msg.body;
+ memcpy(rsp, response, sizeof(ALME_GET_1905_NEIGHBOR_RSP));
+
+ return libalme_success;
+}
+
diff --git a/cleopatre/application/libalme/src/sync.c b/cleopatre/application/libalme/src/sync.c
new file mode 100644
index 0000000000..67cdf0f879
--- /dev/null
+++ b/cleopatre/application/libalme/src/sync.c
@@ -0,0 +1,44 @@
+/*
+ * cleopatre/application/libalme/src/sync.c
+ *
+ */
+#include <sys/types.h>
+#include <sys/ipc.h>
+#include <sys/sem.h>
+
+int get_binary_semaphore_id(void)
+{
+ key_t key;
+
+ key = ftok("/tmp/alme_server",'a');
+
+ if(key == -1)
+ {
+ return -1;
+ }
+
+ return semget(key, 1, 0666);
+}
+
+int binary_semaphore_wait(int semid)
+{
+ struct sembuf op1;
+
+ op1.sem_num = 0;
+ op1.sem_op = -1;
+ op1.sem_flg = SEM_UNDO;
+
+ return semop(semid, &op1, 1);
+}
+
+int binary_semaphore_post(int semid)
+{
+ struct sembuf op1;
+
+ op1.sem_num = 0;
+ op1.sem_op = 1;
+ op1.sem_flg = SEM_UNDO;
+
+ return semop(semid, &op1, 1);
+}
+
diff --git a/cleopatre/application/libspid/inc/defs.h b/cleopatre/application/libspid/inc/defs.h
index 43a9ddd86a..ac49411768 100644
--- a/cleopatre/application/libspid/inc/defs.h
+++ b/cleopatre/application/libspid/inc/defs.h
@@ -28,7 +28,7 @@
/** Size of mac address */
#define LIBSPID_MAC_BIN_LEN 6
-#define LIBSPID_WIFI_DATA_ITEMS_COUNT 16
+#define LIBSPID_WIFI_DATA_ITEMS_COUNT 17
#define LIBSPID_WIFI_DATA_ITEMS_COMPARE_COUNT 1
#define LIBSPID_WIFI_DATA_LABEL_SSID "SSID"
@@ -47,6 +47,7 @@
#define LIBSPID_WIFI_DATA_LABEL_KEY1TYPE "Key1Type"
#define LIBSPID_WIFI_DATA_LABEL_KEY1STR "Key1Str"
#define LIBSPID_WIFI_DATA_LABEL_OWN_IP_ADDR "own_ip_addr"
+#define LIBSPID_WIFI_DATA_LABEL_WSC_CONF_STATUS "WscConfStatus"
@@ -66,7 +67,7 @@
#define LIBSPID_WIFI_DATA_KEY1TYPE_MAX_LEN 2 //0:HEX, 1::ASCII
#define LIBSPID_WIFI_DATA_KEY1STR_MAX_LEN 27
#define LIBSPID_WIFI_DATA_OWN_IP_ADDR_MAX_LEN 16
-
+#define LIBSPID_WIFI_DATA_WSC_CONF_STATUS_MAX_LEN 2
/* CONFIG ITEM/LINE */
#define LIBSPID_CONFIG_LINE_MAX_LEN 1024
diff --git a/cleopatre/application/libspid/inc/libspid.h b/cleopatre/application/libspid/inc/libspid.h
index da56e3cdaf..b47cb75354 100644
--- a/cleopatre/application/libspid/inc/libspid.h
+++ b/cleopatre/application/libspid/inc/libspid.h
@@ -103,6 +103,7 @@ typedef struct {
unsigned char key1_type[LIBSPID_WIFI_DATA_KEY1TYPE_MAX_LEN];
unsigned char key1_str[LIBSPID_WIFI_DATA_KEY1STR_MAX_LEN];
unsigned char own_ip_addr[LIBSPID_WIFI_DATA_OWN_IP_ADDR_MAX_LEN];
+ unsigned char wsc_conf_status[LIBSPID_WIFI_DATA_WSC_CONF_STATUS_MAX_LEN];
} libspid_wifi_t;
diff --git a/cleopatre/application/libspid/src/network.c b/cleopatre/application/libspid/src/network.c
index 13c23a9423..14ee445d96 100644
--- a/cleopatre/application/libspid/src/network.c
+++ b/cleopatre/application/libspid/src/network.c
@@ -390,7 +390,8 @@ libspid_error_t libspid_network_wifi_data(char *interface, libspid_wifi_t *wifi,
LIBSPID_WIFI_DATA_LABEL_DEFAULTKEYID,
LIBSPID_WIFI_DATA_LABEL_KEY1TYPE,
LIBSPID_WIFI_DATA_LABEL_KEY1STR,
- LIBSPID_WIFI_DATA_LABEL_OWN_IP_ADDR
+ LIBSPID_WIFI_DATA_LABEL_OWN_IP_ADDR,
+ LIBSPID_WIFI_DATA_LABEL_WSC_CONF_STATUS
};
char *values[LIBSPID_WIFI_DATA_ITEMS_COUNT] =
@@ -410,7 +411,8 @@ libspid_error_t libspid_network_wifi_data(char *interface, libspid_wifi_t *wifi,
wifi->default_key_id,
wifi->key1_type,
wifi->key1_str,
- wifi->own_ip_addr
+ wifi->own_ip_addr,
+ wifi->wsc_conf_status
};
@@ -432,7 +434,8 @@ libspid_error_t libspid_network_wifi_data(char *interface, libspid_wifi_t *wifi,
LIBSPID_WIFI_DATA_DEFAULTKEYID_MAX_LEN,
LIBSPID_WIFI_DATA_KEY1TYPE_MAX_LEN,
LIBSPID_WIFI_DATA_KEY1STR_MAX_LEN,
- LIBSPID_WIFI_DATA_OWN_IP_ADDR_MAX_LEN
+ LIBSPID_WIFI_DATA_OWN_IP_ADDR_MAX_LEN,
+ LIBSPID_WIFI_DATA_WSC_CONF_STATUS_MAX_LEN
};
//for the fields need compare.
diff --git a/cleopatre/application/spidgoahead/LINUX/spidcom_asp_functions.c b/cleopatre/application/spidgoahead/LINUX/spidcom_asp_functions.c
index 0d9a2be528..7977fd06d2 100644
--- a/cleopatre/application/spidgoahead/LINUX/spidcom_asp_functions.c
+++ b/cleopatre/application/spidgoahead/LINUX/spidcom_asp_functions.c
@@ -40,11 +40,13 @@ void formDefineUserMgmt(void);
#endif
#define DEBUG_ASP_FUNCTIONS
-
+#include "libalme.h"
#include "libspid.h"
-
+
+
typedef char mac_t[6];
+char query_mac[20] = "00:00:00:00:00:00";
char* human_readable(int return_value)
@@ -145,6 +147,296 @@ static int spidcom_asp_get_wifi_data_h(int eid, webs_t wp, int argc, char_t **ar
}
+static int spidcom_asp_get_1905_intf_list_h(int eid, webs_t wp, int argc, char_t **argv){
+
+ ALME_GET_INTF_LIST_RSP rsp;
+ int i = 0;
+ int string_len = 0;
+ char line_buffer[LIBSPID_CONFIG_LINE_MAX_LEN] = {0};
+
+ if(libalme_success == libalme_get_intf_list_req_rsp(&rsp))
+ {
+ if(0 != rsp.intfnum)
+ {
+
+ for(i = 0; i < rsp.intfnum; i++)
+ {
+ sprintf(&line_buffer[string_len], "%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%d:%d:",
+ rsp.intflist[i].mac_addr[0],
+ rsp.intflist[i].mac_addr[1],
+ rsp.intflist[i].mac_addr[2],
+ rsp.intflist[i].mac_addr[3],
+ rsp.intflist[i].mac_addr[4],
+ rsp.intflist[i].mac_addr[5],
+ rsp.intflist[i].intf_type,
+ rsp.intflist[i].ieee802_1_bridge);
+ string_len = strlen(line_buffer);
+ }
+
+ websWrite(wp, "\"%s\"", line_buffer);
+ }
+ else
+ {
+ websWrite(wp, "\"0:0\"");
+ }
+ }
+ else
+ {
+ websWrite(wp, "\"error: get 1905 intf list : %s\"", human_readable(LIBSPID_ERROR_PROCESSING));
+
+ }
+
+ return 0;
+}
+
+#if 0
+
+static int spidcom_asp_get_1905_neighbor_metric_h(int eid, webs_t wp, int argc, char_t **argv){
+
+ ALME_GET_1905_NEIGHBOR_RSP nei_rsp;
+ int i = 0;
+ int k = 0;
+ int string_len = 0;
+ char line_buffer[LIBSPID_CONFIG_LINE_MAX_LEN] = {0};
+
+ if(libalme_success == libalme_get_1905_neighbor_req_rsp(&nei_rsp))
+ {
+ if(0 != nei_rsp.neighbornum)
+ {
+ sprintf(&line_buffer[string_len], "%d:", nei_rsp.neighbornum);
+ string_len = strlen(line_buffer);
+#if 1
+ for(i = 0; i < nei_rsp.neighbornum; i++)
+ {
+ ALME_GET_METRIC_REQ metric_req;
+ ALME_GET_METRIC_RSP metric_rsp;
+
+ metric_req.mac_addr[0] = nei_rsp.neighborlist[i*6];
+ metric_req.mac_addr[1] = nei_rsp.neighborlist[i*6+1];
+ metric_req.mac_addr[2] = nei_rsp.neighborlist[i*6+2];
+ metric_req.mac_addr[3] = nei_rsp.neighborlist[i*6+3];
+ metric_req.mac_addr[4] = nei_rsp.neighborlist[i*6+4];
+ metric_req.mac_addr[5] = nei_rsp.neighborlist[i*6+5];
+
+ if(libalme_success == libalme_get_metric_req_rsp(&metric_req, &metric_rsp))
+ {
+ sprintf(&line_buffer[string_len], "%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%d:",
+ nei_rsp.neighborlist[i*6], nei_rsp.neighborlist[i*6+1],
+ nei_rsp.neighborlist[i*6+2], nei_rsp.neighborlist[i*6+3],
+ nei_rsp.neighborlist[i*6+4], nei_rsp.neighborlist[i*6+5],
+ metric_rsp.descriptor_num);
+
+ string_len = strlen(line_buffer);
+
+ if(0 != metric_rsp.descriptor_num)
+ {
+ for(k = 0; k < metric_rsp.descriptor_num; k++)
+ {
+ sprintf(&line_buffer[string_len],
+ "%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%d:%d:%d:%d:%d:%d:%d:%d:%d:%d:",
+ metric_rsp.descriptorlist[k].neighbor_al_mac[0],
+ metric_rsp.descriptorlist[k].neighbor_al_mac[1],
+ metric_rsp.descriptorlist[k].neighbor_al_mac[2],
+ metric_rsp.descriptorlist[k].neighbor_al_mac[3],
+ metric_rsp.descriptorlist[k].neighbor_al_mac[4],
+ metric_rsp.descriptorlist[k].neighbor_al_mac[5],
+ metric_rsp.descriptorlist[k].local_itf_mac[0],
+ metric_rsp.descriptorlist[k].local_itf_mac[1],
+ metric_rsp.descriptorlist[k].local_itf_mac[2],
+ metric_rsp.descriptorlist[k].local_itf_mac[3],
+ metric_rsp.descriptorlist[k].local_itf_mac[4],
+ metric_rsp.descriptorlist[k].local_itf_mac[5],
+ metric_rsp.descriptorlist[k].ieee802_1_bridge,
+ metric_rsp.descriptorlist[k].link_metrics.itf_type,
+ metric_rsp.descriptorlist[k].link_metrics.tx_packet_error,
+ metric_rsp.descriptorlist[k].link_metrics.tx_total_packet,
+ metric_rsp.descriptorlist[k].link_metrics.max_throughput_capacity,
+ metric_rsp.descriptorlist[k].link_metrics.link_availability,
+ metric_rsp.descriptorlist[k].link_metrics.phy_rate,
+ metric_rsp.descriptorlist[k].link_metrics.rx_packet_error,
+ metric_rsp.descriptorlist[k].link_metrics.rx_total_packet,
+ metric_rsp.descriptorlist[k].link_metrics.rssi);
+ string_len = strlen(line_buffer);
+ }
+ }
+
+ }
+ else
+ {
+ //query metric error....
+ websWrite(wp, "\"error: get metric req, neighbor mac %.2x:%.2x:%.2x:%.2x:%.2x:%.2x, reason code %d\"",
+ nei_rsp.neighborlist[i*6],
+ nei_rsp.neighborlist[i*6+1],
+ nei_rsp.neighborlist[i*6+2],
+ nei_rsp.neighborlist[i*6+3],
+ nei_rsp.neighborlist[i*6+4],
+ nei_rsp.neighborlist[i*6+5],
+ metric_rsp.reason_code);
+ return 0;
+ }
+
+ }
+#endif
+ websWrite(wp, "\"%s\"", line_buffer);
+
+ }
+ else
+ {
+ //no neighbor exists....
+ websWrite(wp, "\"0:0\"");
+ }
+ }
+ else
+ {
+ //query neighbor error....
+ websWrite(wp, "\"error: get 1905 neighbor : %s\"", human_readable(LIBSPID_ERROR_PROCESSING));
+
+ }
+
+ return 0;
+
+}
+#endif
+
+static int spidcom_asp_get_1905_metric_h(int eid, webs_t wp, int argc, char_t **argv){
+
+ ALME_GET_METRIC_REQ metric_req;
+ ALME_GET_METRIC_RSP metric_rsp;
+ int i = 0;
+ int k = 0;
+ int string_len = 0;
+ char line_buffer[LIBSPID_CONFIG_LINE_MAX_LEN] = {0};
+
+ if(strcmp(query_mac, "00:00:00:00:00:00"))
+ {
+ for(i = 0; i < 6; i++)
+ {
+ metric_req.mac_addr[i] = (unsigned char)strtol(&query_mac[i*3], NULL, 16);
+
+ }
+
+#if 1
+ if(libalme_success == libalme_get_metric_req_rsp(&metric_req, &metric_rsp))
+ {
+
+
+ if(0 != metric_rsp.descriptor_num)
+ {
+
+ sprintf(&line_buffer[string_len], "%d:", metric_rsp.descriptor_num);
+
+ string_len = strlen(line_buffer);
+
+ for(k = 0; k < metric_rsp.descriptor_num; k++)
+ {
+ sprintf(&line_buffer[string_len],
+ "%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:%d:%d:%d:%d:%d:%d:%d:%d:%d:%d:",
+ metric_rsp.descriptorlist[k].neighbor_al_mac[0],
+ metric_rsp.descriptorlist[k].neighbor_al_mac[1],
+ metric_rsp.descriptorlist[k].neighbor_al_mac[2],
+ metric_rsp.descriptorlist[k].neighbor_al_mac[3],
+ metric_rsp.descriptorlist[k].neighbor_al_mac[4],
+ metric_rsp.descriptorlist[k].neighbor_al_mac[5],
+ metric_rsp.descriptorlist[k].local_itf_mac[0],
+ metric_rsp.descriptorlist[k].local_itf_mac[1],
+ metric_rsp.descriptorlist[k].local_itf_mac[2],
+ metric_rsp.descriptorlist[k].local_itf_mac[3],
+ metric_rsp.descriptorlist[k].local_itf_mac[4],
+ metric_rsp.descriptorlist[k].local_itf_mac[5],
+ metric_rsp.descriptorlist[k].ieee802_1_bridge,
+ metric_rsp.descriptorlist[k].link_metrics.itf_type,
+ metric_rsp.descriptorlist[k].link_metrics.tx_packet_error,
+ metric_rsp.descriptorlist[k].link_metrics.tx_total_packet,
+ metric_rsp.descriptorlist[k].link_metrics.max_throughput_capacity,
+ metric_rsp.descriptorlist[k].link_metrics.link_availability,
+ metric_rsp.descriptorlist[k].link_metrics.phy_rate,
+ metric_rsp.descriptorlist[k].link_metrics.rx_packet_error,
+ metric_rsp.descriptorlist[k].link_metrics.rx_total_packet,
+ metric_rsp.descriptorlist[k].link_metrics.rssi);
+ string_len = strlen(line_buffer);
+ }
+
+ websWrite(wp, "\"%s\"", line_buffer);
+ }
+ else
+ {
+ //no local interface
+ websWrite(wp, "\"0:1\"");
+ }
+
+ }
+ else
+ {
+ //api return fail
+ websWrite(wp, "\"0:2\"");
+ }
+#endif
+ }
+ else
+ {
+ websWrite(wp, "\"0:3\"");
+ }
+
+ return 0;
+
+}
+
+
+static int spidcom_asp_get_1905_neighbor_h(int eid, webs_t wp, int argc, char_t **argv){
+
+ ALME_GET_1905_NEIGHBOR_RSP nei_rsp;
+ int i = 0;
+ int k = 0;
+ int string_len = 0;
+ char line_buffer[LIBSPID_CONFIG_LINE_MAX_LEN] = {0};
+
+ if(libalme_success == libalme_get_1905_neighbor_req_rsp(&nei_rsp))
+ {
+ if(0 != nei_rsp.neighbornum)
+ {
+ sprintf(&line_buffer[string_len], "%d:", nei_rsp.neighbornum);
+ string_len = strlen(line_buffer);
+
+ for(i = 0; i < nei_rsp.neighbornum; i++)
+ {
+ sprintf(&line_buffer[string_len], "%.2x:%.2x:%.2x:%.2x:%.2x:%.2x:",
+ nei_rsp.neighborlist[i*6], nei_rsp.neighborlist[i*6+1],
+ nei_rsp.neighborlist[i*6+2], nei_rsp.neighborlist[i*6+3],
+ nei_rsp.neighborlist[i*6+4], nei_rsp.neighborlist[i*6+5]);
+
+ string_len = strlen(line_buffer);
+
+ }
+
+ websWrite(wp, "\"%s\"", line_buffer);
+
+ }
+ else
+ {
+ //no neighbor exists....
+ websWrite(wp, "\"0:0\"");
+ }
+ }
+ else
+ {
+ //query neighbor error....
+ websWrite(wp, "\"error: get 1905 neighbor : %s\"", human_readable(LIBSPID_ERROR_PROCESSING));
+
+ }
+
+ return 0;
+
+}
+
+
+static int spidcom_asp_get_1905_query_mac_h(int eid, webs_t wp, int argc, char_t **argv){
+
+ websWrite(wp, "\"%s\"", query_mac);
+
+
+ return 0;
+
+}
/**
* Write the MAC address of a network interface.
@@ -562,6 +854,15 @@ static void spidcom_form_action_wifi(webs_t wp, char_t *path, char_t *query)
websDone(wp, 200);
}
+
+
+static void spidcom_form_action_metric(webs_t wp, char_t *path, char_t *query)
+{
+ strcpy(query_mac, websGetVar(wp, T("query_mac_string"), T("00:00:00:00:00:00")));
+ websRedirect(wp, "metric.asp");
+}
+
+
/**
*
* Bind actual asp name used in HTML to the implemented C function.
@@ -575,9 +876,15 @@ void spidcom_register_all()
websAspDefine(T("spidcomAspGetIpData"), spidcom_asp_get_ip_data_h);
websAspDefine(T("spidcomAspGetWifiData"), spidcom_asp_get_wifi_data_h);
websAspDefine(T("spidcomAspGetMac"), spidcom_asp_get_mac_h);
- websAspDefine(T("spidcomAspReboot"), spidcom_asp_reboot_b);
-
+ websAspDefine(T("spidcomAspReboot"), spidcom_asp_reboot_b);
+ websAspDefine(T("spidcomAspGet1905Metric"), spidcom_asp_get_1905_metric_h);
+ websAspDefine(T("spidcomAspGet1905Neighbor"), spidcom_asp_get_1905_neighbor_h);
+ websAspDefine(T("spidcomAspGet1905QueryMac"), spidcom_asp_get_1905_query_mac_h);
+
+ websAspDefine(T("spidcomAspGet1905intflist"), spidcom_asp_get_1905_intf_list_h);
+
websAspDefine(T("spidcomAction"), spidcom_asp_eoc_action);
websFormDefine(T("formActionWifi"), spidcom_form_action_wifi);
+ websFormDefine(T("formActionMetric"), spidcom_form_action_metric);
websFormDefine(T("formAction"), spidcom_form_action);
}
diff --git a/cleopatre/application/spidgoahead/Makefile b/cleopatre/application/spidgoahead/Makefile
index 23663400bc..36dd533d22 100644
--- a/cleopatre/application/spidgoahead/Makefile
+++ b/cleopatre/application/spidgoahead/Makefile
@@ -9,10 +9,11 @@ CLEO_DIR := ../..
LINUX_DIR := $(CLEO_DIR)/linux-2.6.25.10-spc300
LIBSPID_DIR := $(CLEO_DIR)/application/libspid
LIBMME_DIR := $(CLEO_DIR)/application/libmme
-
+LIBALME_DIR := $(CLEO_DIR)/application/libalme
LIBSPID_SO_BIN := $(LIBSPID_DIR)/libspid.so
LIBMME_SO_BIN := $(LIBMME_DIR)/libmme.so
+LIBALME_SO_BIN := $(LIBALME_DIR)/libalme.so
ifeq ($(SSL_SUPPORT),y)
ifeq ($(STAGING_DIR),)
@@ -41,6 +42,7 @@ RESPONSE_FILE := extra_flags
INCLUDES := -I$(INCPATH) \
-I$(LINUX_DIR)/include/asm-arm/arch-spc300 \
-I$(CLEO_DIR)/include \
+ -I$(LIBALME_DIR)/inc \
$(SSL_INC) \
$(shell $(LIBSPID_DIR)/libspid-config --I_opts)
@@ -52,8 +54,9 @@ EXTRA_CFLAGS := $(INCLUDES) -MMD -Wall \
LIBS := $(SSL_LIB) \
$(shell $(LIBSPID_DIR)/libspid-config --L_opts) \
- $(shell $(LIBSPID_DIR)/libspid-config --libs)
-
+ $(shell $(LIBSPID_DIR)/libspid-config --libs) \
+ -L$(LIBALME_DIR) -lalme
+
ALL_SRCS := $(wildcard $(SUBMOD)/*.c) $(wildcard *.c)
SRCS := $(filter-out $(SRC2AVOID),$(ALL_SRCS))
OBJS := $(addprefix $(OBJPATH)/,$(SRCS:.c=.o))
@@ -64,7 +67,7 @@ all: $(TARGET)
$(TARGET): $(TARGET_DEPS)
@touch $@
-httpd: $(OBJS) $(LIBSPID_SO_BIN) $(LIBMME_SO_BIN) $(SSL_LIB)
+httpd: $(OBJS) $(LIBSPID_SO_BIN) $(LIBMME_SO_BIN) $(LIBALME_SO_BIN) $(SSL_LIB)
$(CC_WITHOUT_CFLAGS) -o $@ $(OBJS) $(LIBS)
#TODO: 'httpd' dependency is a patch. Should be redo.
@@ -82,7 +85,7 @@ $(OBJS): | $(OBJPATH)
$(OBJPATH):
mkdir -p $(OBJPATH)/$(SUBMOD)
-$(LIBSPID_SO_BIN) $(LIBMME_SO_BIN):
+$(LIBSPID_SO_BIN) $(LIBMME_SO_BIN) $(LIBALME_SO_BIN):
$(error libspid or libmme output files are not found)
$(SSL_LIB):
diff --git a/cleopatre/application/spidgoahead/web/1905_stat.asp b/cleopatre/application/spidgoahead/web/1905_stat.asp
new file mode 100644
index 0000000000..3bad3fff84
--- /dev/null
+++ b/cleopatre/application/spidgoahead/web/1905_stat.asp
@@ -0,0 +1,164 @@
+<html>
+<head>
+<link rel="stylesheet" href="style/normal_ws.css" type="text/css">
+<script type = 'text/javascript'>
+
+//var RefreshID = setInterval("window.location.reload()",25000);
+
+//function StopRefresh(){
+//clearInterval(RefreshID);
+//}
+
+//function RestartRefresh(){
+//RefreshID = setInterval("window.location.reload()",25000);
+//}
+
+var neighbor_mac = <% spidcomAspGet1905Neighbor(); %>
+var neighbor_mac_array = neighbor_mac.split(':');
+
+var query_mac = <% spidcomAspGet1905QueryMac(); %>
+var query_mac_array = query_mac.split(':');
+
+var metric = <% spidcomAspGet1905Metric(); %>
+var metric_array = metric.split(':');
+
+
+function validate()
+{
+ parent.parent.footer2.document.getElementById('error_console').innerHTML = "neighbor_mac = "+neighbor_mac+"<br>"+"query_mac = "+query_mac+"<br>"+"metric = "+metric+"<br>";
+
+
+}
+
+function clear_metric()
+{
+ parent.document.getElementById('t_intf_type').innerHTML = 0;
+ parent.document.getElementById('t_packet_errors').innerHTML = 0;
+ parent.document.getElementById('t_packets').innerHTML = 0;
+ parent.document.getElementById('t_cap').innerHTML = 0;
+ parent.document.getElementById('t_link').innerHTML = 0;
+ parent.document.getElementById('t_phy_rate').innerHTML = 0;
+ parent.document.getElementById('r_packet_errors').innerHTML = 0;
+ parent.document.getElementById('r_packets').innerHTML = 0;
+ parent.document.getElementById('r_rssi').innerHTML = 0;
+
+}
+
+function fill_it(){
+
+ var lo_int = parent.document.mainForm.select_local_int;
+ var s = parent.document.mainForm.select_neighbor_mac;
+ var int_value = lo_int.options[lo_int.selectedIndex].value;
+ var int_text = lo_int.options[lo_int.selectedIndex].text;
+ var int_text_array = int_text.split(':');
+
+ var metric_num = parseInt(metric_array[0], 10);
+ var metric_shift = 1;
+
+ var neighbor_mac_num = parseInt(neighbor_mac_array[0], 10);
+
+ //pass metric to main page....
+ parent.document.getElementById('metric_string').value = metric;
+
+ s.options.length = 1; //clear select_neighbor_mac....
+
+ for(var i = 0; i < neighbor_mac_num; i++)
+ {
+
+ var newOption = new Option(neighbor_mac_array[i*6+1] + ":" + neighbor_mac_array[i*6+2] + ":" + neighbor_mac_array[i*6+3] + ":" + neighbor_mac_array[i*6+4] + ":" + neighbor_mac_array[i*6+5] + ":" + neighbor_mac_array[i*6+6], i+1);
+
+ if( query_mac_array[0] == neighbor_mac_array[i*6+1] &&
+ query_mac_array[1] == neighbor_mac_array[i*6+2] &&
+ query_mac_array[2] == neighbor_mac_array[i*6+3] &&
+ query_mac_array[3] == neighbor_mac_array[i*6+4] &&
+ query_mac_array[4] == neighbor_mac_array[i*6+5] &&
+ query_mac_array[5] == neighbor_mac_array[i*6+6])
+ {
+ newOption.selected = true;
+ }
+ s.options[i+1] = newOption;
+ }
+
+
+
+ //fill interface selection....
+ lo_int.options.length = 1;
+ for(var i = 0; i < metric_num; i++)
+ {
+ var interface_location = metric_shift + i*22 +6;
+ var newOption = new Option(metric_array[interface_location] + ":" + metric_array[interface_location+1] + ":" + metric_array[interface_location+2] + ":" + metric_array[interface_location+3] + ":" + metric_array[interface_location+4] + ":" + metric_array[interface_location+5], i+1);
+
+ if( int_text_array[0] == metric_array[interface_location] &&
+ int_text_array[1] == metric_array[interface_location+1] &&
+ int_text_array[2] == metric_array[interface_location+2] &&
+ int_text_array[3] == metric_array[interface_location+3] &&
+ int_text_array[4] == metric_array[interface_location+4] &&
+ int_text_array[5] == metric_array[interface_location+5])
+ {
+ newOption.selected = true;
+ }
+
+ lo_int.options[i+1] = newOption;
+
+ }
+
+
+ //if metric has value....
+ if(metric_array.length > 2)
+ {
+
+ for(var i = 0; i < metric_num; i++)
+ {
+ var interface_location = metric_shift + i*22 +6;
+
+ if( int_text_array[0] == metric_array[interface_location] &&
+ int_text_array[1] == metric_array[interface_location+1] &&
+ int_text_array[2] == metric_array[interface_location+2] &&
+ int_text_array[3] == metric_array[interface_location+3] &&
+ int_text_array[4] == metric_array[interface_location+4] &&
+ int_text_array[5] == metric_array[interface_location+5])
+ {
+ if(metric_array[interface_location+7] == '513')
+ parent.document.getElementById('t_intf_type').innerHTML = 'PLC';
+ else if(metric_array[interface_location+7] == '1')
+ parent.document.getElementById('t_intf_type').innerHTML = 'ETH';
+ else if(metric_array[interface_location+7] == '259')
+ parent.document.getElementById('t_intf_type').innerHTML = '802.11n(2.4G)';
+ else
+ parent.document.getElementById('t_intf_type').innerHTML = metric_array[interface_location+7];
+
+ parent.document.getElementById('t_packet_errors').innerHTML = metric_array[interface_location+8];
+ parent.document.getElementById('t_packets').innerHTML = metric_array[interface_location+9];
+ parent.document.getElementById('t_cap').innerHTML = metric_array[interface_location+10];
+ parent.document.getElementById('t_link').innerHTML = metric_array[interface_location+11];
+ parent.document.getElementById('t_phy_rate').innerHTML = metric_array[interface_location+12];
+ parent.document.getElementById('r_packet_errors').innerHTML = metric_array[interface_location+13];
+ parent.document.getElementById('r_packets').innerHTML = metric_array[interface_location+14];
+ parent.document.getElementById('r_rssi').innerHTML = metric_array[interface_location+15];
+ }
+ else
+ {
+ clear_metric();
+ }
+
+ }
+ }
+ else
+ {
+ clear_metric();
+ }
+
+
+
+ //parent.document.getElementById('t_intf_type').innerHTML = neighbor_mac;
+ //parent.document.getElementById('t_packet_errors').innerHTML = query_mac;
+ //parent.document.getElementById('r_packet_errors').innerHTML = metric;
+}
+
+</script>
+
+</head>
+<body onload='javascript:validate();fill_it();'>
+</body>
+
+</html> \ No newline at end of file
diff --git a/cleopatre/application/spidgoahead/web/close.gif b/cleopatre/application/spidgoahead/web/close.gif
new file mode 100644
index 0000000000..5bc7531bd8
--- /dev/null
+++ b/cleopatre/application/spidgoahead/web/close.gif
Binary files differ
diff --git a/cleopatre/application/spidgoahead/web/intf.asp b/cleopatre/application/spidgoahead/web/intf.asp
new file mode 100644
index 0000000000..b8451521a9
--- /dev/null
+++ b/cleopatre/application/spidgoahead/web/intf.asp
@@ -0,0 +1,126 @@
+<html>
+
+<head>
+ <title>SPC 300 - SPiDCOM</title>
+
+<link rel="stylesheet" href="style/normal_ws.css" type="text/css">
+
+<style type = 'text/css'>
+div.margina,table.margina{
+ margin-left:5%;
+ width:500px;
+ background-color:#F5F5F5;
+}
+td{
+ padding-left:5px;
+}
+
+input[type='text']{
+ margin-left:auto;
+ margin-right:auto;
+ text-align:center;
+}
+input[disabled='disabled'] {
+ background-color:BBBBBB;
+ color:white;
+ cursor:default;
+}
+body{
+ background-image: url("/spidcom.gif");
+ background-repeat: no-repeat;
+ background-attachment: fixed;
+ background-position: center center;
+}
+.style1 {
+ color: #0063C6;
+ font-weight: bold;
+}
+</style>
+
+<script type='text/javascript'>
+
+var intf_list = <% spidcomAspGet1905intflist(); %>
+var intf_list_array = intf_list.split(':');
+var myLabel = ["intfAddress","intfType","IEEE802.1BridgeFlag"];
+
+var totalrow = 0;
+
+function validate()
+{
+
+}
+
+function refresh(){
+ parent.content.location.reload();
+}
+
+</script>
+</head>
+
+
+<body onload='javascript:validate();'>
+<!-- Settings name -->
+<div class='margina' > <h2>INTF</h2>
+</div>
+
+<!-- Main form -->
+
+<form name='mainForm' action='/goform/formAction' target='_self' method='GET' onSubmit='return checkMainForm();'>
+
+<table id='background-image' class='d25' style='margin-left:5%;width:500px' border='0'>
+</table>
+
+<script>
+
+var t = document.getElementById("background-image");
+if(intf_list_array.length > 2)
+{
+ for(var i = 0; i < (intf_list_array.length-1); i += 8) {
+
+ t.insertRow(t.rows.length);
+ t.rows[totalrow].insertCell(0);
+ t.rows[totalrow].cells[0].innerHTML = "<span class='style1'>intfDescriptor" + (i/8) + "</span>";
+ totalrow++;
+
+ for(var j = 0; j < myLabel.length; j++) {
+ t.insertRow(t.rows.length);
+ t.rows[totalrow].insertCell(0);
+ t.rows[totalrow].cells[0].innerHTML = myLabel[j];
+ if(j == 0)
+ {
+ t.rows[totalrow].insertCell(1);
+ t.rows[totalrow].cells[1].innerHTML = intf_list_array[i+j] + ":" + intf_list_array[i+j+1] + ":" + intf_list_array[i+j+2] + ":" + intf_list_array[i+j+3] + ":" + intf_list_array[i+j+4] + ":" + intf_list_array[i+j+5];
+ }
+ else if(j == 1)
+ {
+ t.rows[totalrow].insertCell(1);
+ if(intf_list_array[i+j+5] == '513')
+ t.rows[totalrow].cells[1].innerHTML = 'PLC';
+ else if(intf_list_array[i+j+5] == '1')
+ t.rows[totalrow].cells[1].innerHTML = 'ETH';
+ else if(intf_list_array[i+j+5] == '259')
+ t.rows[totalrow].cells[1].innerHTML = '802.11n(2.4G)';
+ else
+ t.rows[totalrow].cells[1].innerHTML = intf_list_array[i+j+5];
+ }
+ else
+ {
+ t.rows[totalrow].insertCell(1);
+ t.rows[totalrow].cells[1].innerHTML = intf_list_array[i+j+5];
+ }
+ totalrow++;
+ }
+ }
+}
+</script>
+
+<div class='margina' style='margin-top:5px' align='right'>
+<pre ></pre>
+<input type='button' value='Refresh' onclick='javascript:refresh();'>
+<pre id ='tooltip'></pre>
+</div>
+
+</form>
+
+</body>
+</html> \ No newline at end of file
diff --git a/cleopatre/application/spidgoahead/web/metric.asp b/cleopatre/application/spidgoahead/web/metric.asp
new file mode 100644
index 0000000000..edde948179
--- /dev/null
+++ b/cleopatre/application/spidgoahead/web/metric.asp
@@ -0,0 +1,257 @@
+<html>
+
+<head>
+ <title>SPC 300 - SPiDCOM</title>
+
+<link rel="stylesheet" href="style/normal_ws.css" type="text/css">
+
+<style type = 'text/css'>
+div.margina,table.margina{
+ margin-left:5%;
+ width:500px;
+ background-color:#F5F5F5;
+}
+td{
+ padding-left:5px;
+}
+
+input[type='text']{
+ margin-left:auto;
+ margin-right:auto;
+ text-align:center;
+}
+input[disabled='disabled'] {
+ background-color:BBBBBB;
+ color:white;
+ cursor:default;
+}
+body{
+ background-image: url("/spidcom.gif");
+ background-repeat: no-repeat;
+ background-attachment: fixed;
+ background-position: center center;
+}
+.style1 {
+ color: #0063C6;
+ font-weight: bold;
+ text-align:center;
+}
+</style>
+
+<script type='text/javascript'>
+
+var neighbor_mac = <% spidcomAspGet1905Neighbor(); %>
+var neighbor_mac_array = neighbor_mac.split(':');
+
+var query_mac = <% spidcomAspGet1905QueryMac(); %>
+var query_mac_array = query_mac.split(':');
+
+function validate()
+{
+
+}
+
+function clear_metric()
+{
+ document.getElementById('t_intf_type').innerHTML = 0;
+ document.getElementById('t_packet_errors').innerHTML = 0;
+ document.getElementById('t_packets').innerHTML = 0;
+ document.getElementById('t_cap').innerHTML = 0;
+ document.getElementById('t_link').innerHTML = 0;
+ document.getElementById('t_phy_rate').innerHTML = 0;
+ document.getElementById('r_packet_errors').innerHTML = 0;
+ document.getElementById('r_packets').innerHTML = 0;
+ document.getElementById('r_rssi').innerHTML = 0;
+
+}
+
+function refresh()
+{
+ parent.content.location.reload();
+}
+
+function select_onchange_mac()
+{
+ if('====Select====' == document.getElementById('select_neighbor_mac').options[document.getElementById('select_neighbor_mac').selectedIndex].text)
+ {
+ document.getElementById('query_mac_string').value = '00:00:00:00:00:00';
+ }
+ else
+ {
+ document.getElementById('query_mac_string').value = document.getElementById('select_neighbor_mac').options[document.getElementById('select_neighbor_mac').selectedIndex].text;
+ }
+}
+
+
+
+function select_onchange_int()
+{
+ var passd_metric_string = document.getElementById('metric_string').value;
+ var passd_metric_string_array = passd_metric_string.split(':');
+ var lo_int = document.mainForm.select_local_int;
+ var int_text = lo_int.options[lo_int.selectedIndex].text;
+ var int_text_array = int_text.split(':');
+
+ var metric_num = parseInt(passd_metric_string_array[0], 10);
+ //if metric has value....
+ if(passd_metric_string_array.length > 2)
+ {
+
+ for(var i = 0; i < metric_num; i++)
+ {
+ var interface_location = 1 + i*22 +6;
+
+ if( int_text_array[0] == passd_metric_string_array[interface_location] &&
+ int_text_array[1] == passd_metric_string_array[interface_location+1] &&
+ int_text_array[2] == passd_metric_string_array[interface_location+2] &&
+ int_text_array[3] == passd_metric_string_array[interface_location+3] &&
+ int_text_array[4] == passd_metric_string_array[interface_location+4] &&
+ int_text_array[5] == passd_metric_string_array[interface_location+5])
+ {
+ if(passd_metric_string_array[interface_location+7] == '513')
+ document.getElementById('t_intf_type').innerHTML = 'PLC';
+ else if(passd_metric_string_array[interface_location+7] == '1')
+ document.getElementById('t_intf_type').innerHTML = 'ETH';
+ else if(passd_metric_string_array[interface_location+7] == '259')
+ document.getElementById('t_intf_type').innerHTML = '802.11n(2.4G)';
+ else
+ document.getElementById('t_intf_type').innerHTML = passd_metric_string_array[interface_location+7];
+
+ document.getElementById('t_packet_errors').innerHTML = passd_metric_string_array[interface_location+8];
+ document.getElementById('t_packets').innerHTML = passd_metric_string_array[interface_location+9];
+ document.getElementById('t_cap').innerHTML = passd_metric_string_array[interface_location+10];
+ document.getElementById('t_link').innerHTML = passd_metric_string_array[interface_location+11];
+ document.getElementById('t_phy_rate').innerHTML = passd_metric_string_array[interface_location+12];
+ document.getElementById('r_packet_errors').innerHTML = passd_metric_string_array[interface_location+13];
+ document.getElementById('r_packets').innerHTML = passd_metric_string_array[interface_location+14];
+ document.getElementById('r_rssi').innerHTML = passd_metric_string_array[interface_location+15];
+ }
+ else
+ {
+ clear_metric();
+ }
+
+ }
+ }
+ else
+ {
+ clear_metric();
+ }
+
+
+
+}
+
+</script>
+</head>
+
+
+<body onload='javascript:validate();'>
+<!-- Settings name -->
+<div class='margina' > <h2>METRIC</h2>
+</div>
+
+<!-- Main form -->
+
+<form name='mainForm' action='/goform/formActionMetric' target='_self' method='GET' onSubmit='return checkMainForm();'>
+
+<input type='hidden' name='query_mac_string' id='query_mac_string' value='00:00:00:00:00:00'/>
+<input type='hidden' name='metric_string' id='metric_string' value='0:0'/>
+<table id='background-image' class='d25' style='margin-left:5%;width:500px' border='0'>
+<tr>
+<td><span class='style1'>neighborDevAddress</span></td>
+</tr>
+<tr>
+<td><select name='select_neighbor_mac' id='select_neighbor_mac' style="width: 130px;" onchange='select_onchange_mac();'>
+ <option value="0" selected>====Select====</option></select>
+</td>
+<td>
+<input type='submit' value='Query'/>
+</td>
+</tr>
+<tr>
+<td><span class='style1'>localIntfAddress</span></td>
+</tr>
+<tr>
+<td><select name='select_local_int' id='select_local_int' style="width: 130px;" onchange='select_onchange_int();'>
+ <option value="0" selected>====Select====</option></select></td>
+</tr>
+<tr align='left'>
+<td width="180"><span class='style1'>transmitter link metric</span></td>
+<td width="70"></td>
+<td width="161"><span class='style1'>receiver link metric</span></td>
+<td width="71"></td>
+</tr>
+<tr>
+<td class='d25' >intfType</td>
+<td class='d25' id='t_intf_type' bgcolor='lavender' align='center'></td>
+<td>packetErrors</td>
+<td class='d25' id='r_packet_errors' bgcolor='lavender' align='center'>0</td>
+</tr>
+
+<tr>
+<td class='d25' >packetErrors</td>
+<td class='d25' id='t_packet_errors' bgcolor='lavender' align='center'>0</td>
+<td>packetsReceived</td>
+<td class='d25' id='r_packets' bgcolor='lavender' align='center'>0</td>
+</tr>
+
+<tr>
+<td class='d25' >transmittedPackets</td>
+<td class='d25' id='t_packets' bgcolor='lavender' align='center'>0</td>
+<td>rssi</td>
+<td class='d25' id='r_rssi' bgcolor='lavender' align='center'>0</td>
+</tr>
+<tr>
+<td class='d25' >macThroughputCapacity</td>
+<td class='d25' id='t_cap' bgcolor='lavender' align='center'>0</td>
+</tr>
+<tr>
+<td class='d25' >linkAvailability</td>
+<td class='d25' id='t_link' bgcolor='lavender' align='center'>0</td>
+</tr>
+<tr>
+<td class='d25' >phyRate</td>
+<td class='d25' id='t_phy_rate' bgcolor='lavender' align='center'>0</td>
+</tr>
+</table>
+<script>
+var s = document.getElementById("select_neighbor_mac");
+var neighbor_mac_num = parseInt(neighbor_mac_array[0], 10);
+
+s.options.length = 1; //clear select_neighbor_mac....
+
+for(var i = 0; i < neighbor_mac_num; i++)
+{
+
+ var newOption = new Option(neighbor_mac_array[i*6+1] + ":" + neighbor_mac_array[i*6+2] + ":" + neighbor_mac_array[i*6+3] + ":" + neighbor_mac_array[i*6+4] + ":" + neighbor_mac_array[i*6+5] + ":" + neighbor_mac_array[i*6+6], i+1);
+
+ if( query_mac_array[0] == neighbor_mac_array[i*6+1] &&
+ query_mac_array[1] == neighbor_mac_array[i*6+2] &&
+ query_mac_array[2] == neighbor_mac_array[i*6+3] &&
+ query_mac_array[3] == neighbor_mac_array[i*6+4] &&
+ query_mac_array[4] == neighbor_mac_array[i*6+5] &&
+ query_mac_array[5] == neighbor_mac_array[i*6+6])
+ {
+ newOption.selected = true;
+ }
+ s.options[i+1] = newOption;
+}
+//if just press query, pre set the query mac...
+document.getElementById('query_mac_string').value = query_mac;
+</script>
+
+
+
+<div class='margina' style='margin-top:5px' align='right'>
+<pre ></pre>
+
+<pre id ='tooltip'></pre>
+</div>
+
+</form>
+<iframe id='1905_stat_iframe' name='1905_stat_iframe' src ="1905_stat.asp" frameborder='0' height='5'>
+ <p>Your browser does not support iframes.</p>
+</iframe>
+</body>
+</html> \ No newline at end of file
diff --git a/cleopatre/application/spidgoahead/web/nav.asp b/cleopatre/application/spidgoahead/web/nav.asp
index f2b84a395f..9b7b435592 100644
--- a/cleopatre/application/spidgoahead/web/nav.asp
+++ b/cleopatre/application/spidgoahead/web/nav.asp
@@ -44,23 +44,53 @@ color:red;
}
</style>
+<script language="JavaScript" type="text/javascript">
+
+function $(_0)
+{
+ return document.getElementById(_0);
+}
+
+function popmenu(tableName,obj)
+{
+ if ($(tableName).style.display=="none")
+ {
+ $(tableName).style.display="block";
+ $(obj).getElementsByTagName("img")[0].src = "open.gif";
+ }
+ else
+ {
+ $(tableName).style.display= "none";
+ $(obj).getElementsByTagName("img")[0].src = "close.gif";
+ }
+}
+</script>
+
+
</head>
<body bgcolor="#F5F5F5">
-
<div style='position:relative;margin-top:15%'></div>
<div style='position:relative;margin-left:5%;margin-right:5%;background-color:#CCCCCC;text-align:center;color:black'><code>SPC 300</code>
<table id='one-column-emphasis'>
-<tr><td><a href = 'plc0.asp' target='content'>Plc </a></td></tr>
-<tr><td><a href = 'eth0.asp' target='content'>Ethernet </a></td></tr>
-<tr><td><a href = 'br0.asp' target='content'>Bridge </a></td></tr>
-<tr><td><a href = 'wifi0.asp' target='content'>Wifi </a></td></tr>
-<tr><td><a href = 'soft.asp' target='content'>Software </a></td></tr>
+<tr><td><div align="left"><a href = 'plc0.asp' target='content'>Plc </a></div></td></tr>
+<tr><td><div align="left"><a href = 'eth0.asp' target='content'>Ethernet </a></div></td></tr>
+<tr><td><div align="left"><a href = 'br0.asp' target='content'>Bridge </a></div></td></tr>
+<tr><td><div align="left"><a href = 'wifi0.asp' target='content'>Wifi </a></div></td></tr>
+<tr><td><div class="item">
+<div align="left"><a href="javascript:popmenu('DetailItem1','item1');" id="item1"><img src="close.gif" border="0"> 1905</a> </div>
+</div>
+<div class="detailItem" id="DetailItem1" style="display:none;">
+<div align="left"><a href="intf.asp" target='content'><img src="tree_node0.gif" border="0">INTF</a> </div>
+<div align="left"><a href="metric.asp" target='content'><img src="tree_node1.gif" border="0">METRIC</a> </div>
+</td></tr>
+<tr><td><div align="left"><a href = 'soft.asp' target='content'>Software </a></div></td></tr>
</table>
</div>
+
<div style='position:relative;margin-left:5%;margin-right:5%;background-color:#CCCCCC;text-align:center;color:black'><code>SPC 300</code></div>
diff --git a/cleopatre/application/spidgoahead/web/open.gif b/cleopatre/application/spidgoahead/web/open.gif
new file mode 100644
index 0000000000..5b1de3ab20
--- /dev/null
+++ b/cleopatre/application/spidgoahead/web/open.gif
Binary files differ
diff --git a/cleopatre/application/spidgoahead/web/tree_node0.gif b/cleopatre/application/spidgoahead/web/tree_node0.gif
new file mode 100644
index 0000000000..5dcc3edef8
--- /dev/null
+++ b/cleopatre/application/spidgoahead/web/tree_node0.gif
Binary files differ
diff --git a/cleopatre/application/spidgoahead/web/tree_node1.gif b/cleopatre/application/spidgoahead/web/tree_node1.gif
new file mode 100644
index 0000000000..733056d207
--- /dev/null
+++ b/cleopatre/application/spidgoahead/web/tree_node1.gif
Binary files differ
diff --git a/cleopatre/application/spidgoahead/web/wifi0.asp b/cleopatre/application/spidgoahead/web/wifi0.asp
index bd14498d4c..a954b8a774 100644
--- a/cleopatre/application/spidgoahead/web/wifi0.asp
+++ b/cleopatre/application/spidgoahead/web/wifi0.asp
@@ -185,7 +185,7 @@ if(wifi_data_array[4] == 'WEP'){
<table width="564" border='0' class='margina' style='background-color:#FFFFFF' name = 'WifiConfig'>
<tr>
-<td width="142"><h3>Network configuration</h3></td><td width="23"><input type='hidden' name='ip' id='ip' value='0'/> </td> <!-- This will be passed by form -->
+<td width="181"><h3>Network configuration</h3></td><td width="309"><input type='hidden' name='ip' id='ip' value='0'/> </td> <!-- This will be passed by form -->
</tr>
<tr>
<td>SSID</td>
@@ -229,13 +229,13 @@ if(wifi_data_array[4] == 'WEP'){
</select></td>
</tr>
<tr>
-<td width="142"><h3>Security configuration</h3></td>
+<td width="181"><h3>Security configuration</h3></td>
</tr>
<tr>
<td><label>
<input name="security_radiobutton" type="radio" value="NONE" checked='checked'>
No security</label></td>
-<td width="304">&nbsp;</td>
+<td width="309">&nbsp;</td>
</tr>
<tr>
<td><label>
diff --git a/cleopatre/buildroot/package/Config.in b/cleopatre/buildroot/package/Config.in
index 7eaca7887b..63fbb75994 100644
--- a/cleopatre/buildroot/package/Config.in
+++ b/cleopatre/buildroot/package/Config.in
@@ -106,6 +106,9 @@ source "package/module-init-tools/Config.in"
source "package/modutils/Config.in"
endif
source "package/ng-spice-rework/Config.in"
+if BR2_TARGET_SPIDCOM
+source "package/p1905_managerd/Config.in"
+endif
source "package/popt/Config.in"
source "package/portage/Config.in"
if !BR2_PACKAGE_BUSYBOX_HIDE_OTHERS
@@ -184,6 +187,9 @@ source "package/ipsec-tools/Config.in"
source "package/iptables/Config.in"
source "package/kismet/Config.in"
source "package/l2tp/Config.in"
+if BR2_TARGET_SPIDCOM
+source "package/libalme/Config.in"
+endif
source "package/libcgi/Config.in"
source "package/libcgicc/Config.in"
source "package/libeXosip2/Config.in"
diff --git a/cleopatre/buildroot/package/libalme/Config.in b/cleopatre/buildroot/package/libalme/Config.in
new file mode 100644
index 0000000000..16d8b2c702
--- /dev/null
+++ b/cleopatre/buildroot/package/libalme/Config.in
@@ -0,0 +1,6 @@
+config BR2_PACKAGE_LIBALME
+ bool "libalme"
+ help
+ ALME library for other application use.
+
+ http://www.mstarsemi.com
diff --git a/cleopatre/buildroot/package/libalme/libalme.mk b/cleopatre/buildroot/package/libalme/libalme.mk
new file mode 100644
index 0000000000..9d5c73cf83
--- /dev/null
+++ b/cleopatre/buildroot/package/libalme/libalme.mk
@@ -0,0 +1,73 @@
+#############################################################
+#
+# libalme (provides APIs for ALME communications needed by applications)
+#
+#############################################################
+# (C) Copyright 2013 MStar Semiconductor, Inc.
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU Library General Public License as
+# published by the Free Software Foundation; either version 2 of the
+# License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+# Library General Public License for more details.
+#
+# You should have received a copy of the GNU Library General Public
+# License along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
+# USA
+#
+
+LIBALME_VERSION_MSB:=0
+LIBALME_VERSION:=$(LIBALME_VERSION_MSB).0.1
+LIBALME_SITE:=$(BASE_DIR)/../application/libalme
+LIBALME_NAME:=libalme.so.$(LIBALME_VERSION)
+LIBALME_DIR:=$(BUILD_DIR)/libalme-$(LIBALME_VERSION)
+
+LIBALME_BIN_SO := $(LIBALME_SITE)/libalme.so $(LIBALME_DIR)/libalme.so
+
+libalme-source:
+
+$(LIBALME_DIR)/.unpacked:
+ ln -s $(LIBALME_SITE) $(LIBALME_DIR)
+ touch $(LIBALME_DIR)/.unpacked
+
+$(LIBALME_DIR)/.configured: $(LIBALME_DIR)/.unpacked
+ touch $(LIBALME_DIR)/.configured
+
+$(LIBALME_BIN_SO): $(LIBALME_DIR)/.configured FORCE_LIBALME_MAKE
+ @$(TARGET_CONFIGURE_OPTS) \
+ $(MAKE) -C $(LIBALME_DIR) libalme.so
+
+$(TARGET_DIR)/usr/lib/$(LIBALME_NAME): $(LIBALME_DIR)/libalme.so
+ mkdir -p $(TARGET_DIR)/usr/lib
+ cp $< $@
+ (cd $(TARGET_DIR)/usr/lib; \
+ ln -fs $(LIBALME_NAME) libalme.so; \
+ ln -fs $(LIBALME_NAME) libalme.so.$(LIBALME_VERSION_MSB))
+
+libalme: uclibc $(TARGET_DIR)/usr/lib/$(LIBALME_NAME)
+
+libalme-clean:
+ rm -f $(TARGET_DIR)/usr/lib/libalme.so
+ rm -f $(TARGET_DIR)/usr/lib/libalme.so.$(LIBALME_VERSION_MSB)
+ rm -f $(TARGET_DIR)/usr/lib/$(LIBALME_NAME)
+ -$(MAKE) -C $(LIBALME_DIR) clean
+
+libalme-dirclean:
+ rm -f $(LIBALME_DIR)
+
+# We declare FORCE_LIBALME_MAKE rule as PHONY to force compilation
+.PHONY: FORCE_LIBALME_MAKE
+
+#############################################################
+#
+# Toplevel Makefile options
+#
+#############################################################
+ifeq ($(strip $(BR2_PACKAGE_LIBALME)),y)
+TARGETS+=libalme
+endif
diff --git a/cleopatre/buildroot/package/p1905_managerd/Config.in b/cleopatre/buildroot/package/p1905_managerd/Config.in
new file mode 100644
index 0000000000..2b26c45a3f
--- /dev/null
+++ b/cleopatre/buildroot/package/p1905_managerd/Config.in
@@ -0,0 +1,6 @@
+config BR2_PACKAGE_P1905_MANAGERD
+ bool "p1905_managerd"
+ help
+ ieee1905.1 manager daemon.
+
+ http://www.spidcom.com
diff --git a/cleopatre/buildroot/package/p1905_managerd/p1905_managerd.mk b/cleopatre/buildroot/package/p1905_managerd/p1905_managerd.mk
new file mode 100755
index 0000000000..7e3e03c1a7
--- /dev/null
+++ b/cleopatre/buildroot/package/p1905_managerd/p1905_managerd.mk
@@ -0,0 +1,77 @@
+#############################################################
+#
+# p1905_managerd (provides IEEE P1905.1 functionality)
+#
+#
+#############################################################
+# Copyright (C) 2013 MStar Semiconductor, Inc.
+#
+# This program is free software; you can redistribute it and/or modify
+# it under the terms of the GNU Library General Public License as
+# published by the Free Software Foundation; either version 2 of the
+# License, or (at your option) any later version.
+#
+# This program is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+# Library General Public License for more details.
+#
+# You should have received a copy of the GNU Library General Public
+# License along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
+# USA
+#
+
+BUILDROOT_CONFIG_FILE:=$(BASE_DIR)/.config
+
+P1905_MANAGERD_VERSION:=0.0.1
+P1905_MANAGERD_SITE:=$(BASE_DIR)/../devkit/p1905_managerd
+P1905_MANAGERD_NAME:=p1905_managerd-$(P1905_MANAGERD_VERSION)
+P1905_MANAGERD_DIR:=$(BUILD_DIR)/p1905_managerd-$(P1905_MANAGERD_VERSION)
+
+p1905_managerd-source:
+
+$(P1905_MANAGERD_DIR)/.unpacked:
+ echo "$(P1905_MANAGERD_DIR)"
+ ln -s $(P1905_MANAGERD_SITE) $(P1905_MANAGERD_DIR)
+ touch $(P1905_MANAGERD_DIR)/.unpacked
+
+$(P1905_MANAGERD_DIR)/.configured: $(P1905_MANAGERD_DIR)/.unpacked $(BUILDROOT_CONFIG_FILE)
+ touch $(P1905_MANAGERD_DIR)/.configured
+
+$(P1905_MANAGERD_DIR)/p1905_managerd: $(P1905_MANAGERD_DIR)/.configured
+ (cd $(P1905_MANAGERD_DIR); \
+ $(TARGET_CONFIGURE_OPTS) $(MAKE) LINUX_DIR="$(BASE_DIR)/../linux-2.6.25.10-spc300" \
+ LIBMME_DIR="$(BASE_DIR)/../application/libmme" \
+ LIBSPID_DIR="$(BASE_DIR)/../application/libspid" \
+ LIBALME_DIR="$(BASE_DIR)/../application/libalme")
+
+$(TARGET_DIR)/usr/sbin/p1905_managerd: $(P1905_MANAGERD_DIR)/p1905_managerd
+ mkdir -p $(TARGET_DIR)/usr/sbin
+ cp $< $@
+
+$(TARGET_DIR)/etc/init.d/S98p1905: $(P1905_MANAGERD_DIR)/S98p1905
+ mkdir -p $(TARGET_DIR)/etc/init.d
+ cp $< $@
+ chmod a+x $@
+
+p1905_managerd: uclibc $(TARGET_DIR)/usr/sbin/p1905_managerd $(TARGET_DIR)/etc/init.d/S98p1905
+
+p1905_managerd-clean:
+ rm -f $(TARGET_DIR)/usr/sbin/p1905_managerd
+ -$(MAKE) -C $(P1905_MANAGERD_DIR) clean
+
+p1905_managerd-dirclean:
+ rm -f $(P1905_MANAGERD_DIR)
+
+# We declare $(P1905_MANAGERD_DIR)/p1905_managerd rule as PHONY to force compilation
+.PHONY: $(P1905_MANAGERD_DIR)/p1905_managerd
+
+#############################################################
+#
+# Toplevel Makefile options
+#
+#############################################################
+ifeq ($(strip $(BR2_PACKAGE_P1905_MANAGERD)),y)
+TARGETS+=p1905_managerd
+endif
diff --git a/cleopatre/buildroot/target/device/Spidcom/common/defconfig.base b/cleopatre/buildroot/target/device/Spidcom/common/defconfig.base
index 95dd8b8402..c47396d6b3 100644
--- a/cleopatre/buildroot/target/device/Spidcom/common/defconfig.base
+++ b/cleopatre/buildroot/target/device/Spidcom/common/defconfig.base
@@ -249,6 +249,7 @@ BR2_PACKAGE_FW_WD=y
# BR2_PACKAGE_IPKG is not set
# BR2_PACKAGE_KEXEC is not set
# BR2_PACKAGE_LESS is not set
+# BR2_PACKAGE_LIBALME is not set
# BR2_PACKAGE_LIBDAEMON is not set
# BR2_PACKAGE_LIBELF is not set
# BR2_PACKAGE_LIBEVENT is not set
@@ -267,6 +268,7 @@ BR2_PACKAGE_MANAGERD=y
# BR2_PACKAGE_MODULE_INIT_TOOLS is not set
# BR2_PACKAGE_MODUTILS is not set
# BR2_PACKAGE_NG_SPICE_REWORK is not set
+# BR2_PACKAGE_P1905_MANAGERD is not set
# BR2_PACKAGE_POPT is not set
# BR2_PACKAGE_PORTAGE is not set
# BR2_PACKAGE_PROCPS is not set
diff --git a/cleopatre/buildroot/target/device/Spidcom/common/linux26.config.base b/cleopatre/buildroot/target/device/Spidcom/common/linux26.config.base
index 62c578b2ee..7149448fc3 100644
--- a/cleopatre/buildroot/target/device/Spidcom/common/linux26.config.base
+++ b/cleopatre/buildroot/target/device/Spidcom/common/linux26.config.base
@@ -255,6 +255,7 @@ CONFIG_VLAN_8021Q=y
# CONFIG_ECONET is not set
# CONFIG_WAN_ROUTER is not set
# CONFIG_NET_SCHED is not set
+# CONFIG_IEEE1905_1_FORWARD_ENTITY is not set
#
# Network testing
diff --git a/cleopatre/buildroot/target/device/Spidcom/targets/mpr520e/defconfig.part b/cleopatre/buildroot/target/device/Spidcom/targets/mpr520e/defconfig.part
index 29c6cdccf9..2773e0133b 100644
--- a/cleopatre/buildroot/target/device/Spidcom/targets/mpr520e/defconfig.part
+++ b/cleopatre/buildroot/target/device/Spidcom/targets/mpr520e/defconfig.part
@@ -3,4 +3,7 @@ BR2_HOSTNAME="mpr520e"
BR2_TARGET_SPIDCOM_MPR520E=y
BR2_PACKAGE_PMD=y
BR2_PACKAGE_MT7601UDRV=y
-BR2_PACKAGE_SPIDGOAHEAD=y \ No newline at end of file
+BR2_PACKAGE_SPIDGOAHEAD=y
+BR2_PACKAGE_P1905_MANAGERD=y
+BR2_PACKAGE_LIBALME=y
+
diff --git a/cleopatre/buildroot/target/device/Spidcom/targets/mpr520e/linux26.config.part b/cleopatre/buildroot/target/device/Spidcom/targets/mpr520e/linux26.config.part
index 18bae9f1dc..3d9cf8eed6 100644
--- a/cleopatre/buildroot/target/device/Spidcom/targets/mpr520e/linux26.config.part
+++ b/cleopatre/buildroot/target/device/Spidcom/targets/mpr520e/linux26.config.part
@@ -17,6 +17,7 @@ CONFIG_SUSPEND_FREEZER=y
CONFIG_REALTEK_PHY=y
CONFIG_NETDEV_1000=y
+CONFIG_IEEE1905_1_FORWARD_ENTITY=y
#
# Kernel Features
diff --git a/cleopatre/devkit/mt7601udrv/RT2870AP.dat b/cleopatre/devkit/mt7601udrv/RT2870AP.dat
index c5f4951575..402eb47b45 100644
--- a/cleopatre/devkit/mt7601udrv/RT2870AP.dat
+++ b/cleopatre/devkit/mt7601udrv/RT2870AP.dat
@@ -108,6 +108,7 @@ MeshEncrypType=NONE
MeshWPAKEY=
MeshDefaultkey=1
MeshWEPKEY=
+WscConfStatus=1
WscManufacturer=
WscModelName=
WscDeviceName=
diff --git a/cleopatre/devkit/mt7601udrv/ap/ap_cfg.c b/cleopatre/devkit/mt7601udrv/ap/ap_cfg.c
index e90f7776d3..33f0c783fb 100644
--- a/cleopatre/devkit/mt7601udrv/ap/ap_cfg.c
+++ b/cleopatre/devkit/mt7601udrv/ap/ap_cfg.c
@@ -7509,6 +7509,11 @@ VOID RTMPIoctlStatistics(
#ifdef RTMP_EFUSE_SUPPORT
UINT efusefreenum=0;
#endif /* RTMP_EFUSE_SUPPORT */
+#ifdef P1905_LINK_METRICS
+ INT j;
+ ULONG DataRate;
+ UCHAR rf_bw, ext_ch;
+#endif
/* msg = (PSTRING)kmalloc(sizeof(CHAR)*(2048), MEM_ALLOC_FLAG); */
os_alloc_mem(pAd, (UCHAR **)&msg, sizeof(CHAR)*(2048));
@@ -7672,6 +7677,41 @@ VOID RTMPIoctlStatistics(
sprintf(msg+strlen(msg), "efuseFreeNumber = %d\n", efusefreenum);
}
#endif /* RTMP_EFUSE_SUPPORT */
+#ifdef P1905_LINK_METRICS
+ sprintf(msg+strlen(msg), "\n");
+
+ for (j=0; j<MAX_LEN_OF_MAC_TABLE; j++)
+ {
+ PMAC_TABLE_ENTRY pEntry = &pAd->MacTab.Content[j];
+ if ((IS_ENTRY_CLIENT(pEntry) || IS_ENTRY_APCLI(pEntry))
+ && (pEntry->Sst == SST_ASSOC))
+ {
+ sprintf(msg+strlen(msg), "sta mac = %.2x %.2x %.2x %.2x %.2x %.2x\n",
+ pEntry->Addr[0], pEntry->Addr[1], pEntry->Addr[2], pEntry->Addr[3],pEntry->Addr[4],pEntry->Addr[5]);
+ sprintf(msg+strlen(msg), "--TxPacket = %ld\n", (ULONG)pEntry->TxPackets.QuadPart);
+ sprintf(msg+strlen(msg), "--RxPacket = %ld\n", (ULONG)pEntry->RxPackets.QuadPart);
+
+ getRate(pEntry->HTPhyMode, &DataRate);
+ sprintf(msg+strlen(msg), "--Rate = %ld\n", DataRate);
+ }
+ }
+ sprintf(msg+strlen(msg), "\n");
+
+ if(get_ht_cent_ch(pAd, &rf_bw, &ext_ch) == FALSE)
+ {
+ rf_bw = BW_20;
+ pAd->CommonCfg.CentralChannel = pAd->CommonCfg.Channel;
+
+ sprintf(msg+strlen(msg), "CurrentBW = %d\n", rf_bw);
+ sprintf(msg+strlen(msg), "CenterFreq = %d\n", pAd->CommonCfg.CentralChannel);
+ }
+ else
+ {
+ sprintf(msg+strlen(msg), "CurrentBW = %d\n", rf_bw);
+ sprintf(msg+strlen(msg), "CenterFreq = %d\n", pAd->CommonCfg.CentralChannel);
+ }
+#endif
+
/* Copy the information into the user buffer */
wrq->u.data.length = strlen(msg);
Status = copy_to_user(wrq->u.data.pointer, msg, wrq->u.data.length);
@@ -11258,6 +11298,27 @@ INT RTMP_AP_IoctlHandle(
break;
#endif /* HOSTAPD_SUPPORT */
+#ifdef P1905_AP_AUTO_CONFIGURED
+ case CMD_RTPRIV_IOCTL_GET_DH_PUB_KEY:
+ Get_DH_Pub_Key(wrq);
+ break;
+ case CMD_RTPRIV_IOCTL_GET_DH_SECU_KEY:
+ Get_DH_Secu_Key(wrq);
+ break;
+ case CMD_RTPRIV_IOCTL_GET_AUTH_KEYWRAP_KEY:
+ Get_Auth_Keywrap_Key(wrq);
+ break;
+ case CMD_RTPRIV_IOCTL_GET_KWA_VALUE:
+ Get_KWA_Value(wrq);
+ break;
+ case CMD_RTPRIV_IOCTL_GET_AES_ENCRYPT_VALUE:
+ Get_AES_Encrypt_Value(wrq);
+ break;
+ case CMD_RTPRIV_IOCTL_GET_AES_DECRYPT_VALUE:
+ Get_AES_Decrypt_Value(wrq);
+ break;
+#endif
+
default:
Status = RTMP_COM_IoctlHandle(pAd, wrq, cmd, subcmd, pData, Data);
break;
diff --git a/cleopatre/devkit/mt7601udrv/common/cmm_info.c b/cleopatre/devkit/mt7601udrv/common/cmm_info.c
index 4d69674a66..439c178855 100644
--- a/cleopatre/devkit/mt7601udrv/common/cmm_info.c
+++ b/cleopatre/devkit/mt7601udrv/common/cmm_info.c
@@ -25,7 +25,14 @@
#include "rt_config.h"
-
+#ifdef P1905_AP_AUTO_CONFIGURED
+extern UINT8 WPS_DH_G_VALUE[1];
+extern UINT8 WPS_DH_P_VALUE[192];
+extern int WscDeriveKey (
+ unsigned char *kdk, unsigned int kdk_len,
+ unsigned char *prsnlString, unsigned int str_len,
+ unsigned char *key, unsigned int keyBits );
+#endif
/*
==========================================================================
@@ -6149,3 +6156,232 @@ INT RTMPShowCfgValue(
return Status;
}
+#ifdef P1905_AP_AUTO_CONFIGURED
+VOID Get_DH_Pub_Key(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq)
+{
+ DH_TABLE *pDHTab = NULL;
+ UINT pub_key_len = 0;
+ INT status;
+
+ os_alloc_mem(NULL, (UCHAR **)&pDHTab, sizeof(DH_TABLE));
+ if (pDHTab == NULL)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Allocate memory fail!!!\n", __FUNCTION__));
+ return;
+ }
+
+ status = copy_from_user(pDHTab, wrq->u.data.pointer, wrq->u.data.length);
+ if(status)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Copy from user error!!!\n", __FUNCTION__));
+ os_free_mem(NULL, pDHTab);
+ return;
+ }
+
+ pub_key_len = sizeof(pDHTab->pub_key);
+ RT_DH_PublicKey_Generate(
+ WPS_DH_G_VALUE, sizeof(WPS_DH_G_VALUE),
+ WPS_DH_P_VALUE, sizeof(WPS_DH_P_VALUE),
+ pDHTab->priv_key, sizeof(pDHTab->priv_key),
+ pDHTab->pub_key, &pub_key_len);
+
+ wrq->u.data.length = sizeof(DH_TABLE);
+ if (copy_to_user(wrq->u.data.pointer, pDHTab, wrq->u.data.length))
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: copy_to_user() fail\n", __FUNCTION__));
+ }
+
+ os_free_mem(NULL, pDHTab);
+}
+
+VOID Get_DH_Secu_Key(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq)
+{
+ DH_TABLE *pDHTab = NULL;
+ INT status;
+ UINT dh_len;
+
+ os_alloc_mem(NULL, (UCHAR **)&pDHTab, sizeof(DH_TABLE));
+ if (pDHTab == NULL)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Allocate memory fail!!!\n", __FUNCTION__));
+ return;
+ }
+
+ status = copy_from_user(pDHTab, wrq->u.data.pointer, wrq->u.data.length);
+ if(status)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Copy from user error!!!\n", __FUNCTION__));
+ os_free_mem(NULL, pDHTab);
+ return;
+ }
+
+ dh_len = sizeof(pDHTab->secu_key);
+ RT_DH_SecretKey_Generate(
+ pDHTab->pub_key, sizeof(pDHTab->pub_key),
+ WPS_DH_P_VALUE, sizeof(WPS_DH_P_VALUE),
+ pDHTab->priv_key, sizeof(pDHTab->priv_key),
+ pDHTab->secu_key, &dh_len);
+
+ wrq->u.data.length = sizeof(DH_TABLE);
+ if (copy_to_user(wrq->u.data.pointer, pDHTab, wrq->u.data.length))
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: copy_to_user() fail\n", __FUNCTION__));
+ }
+
+ os_free_mem(NULL, pDHTab);
+}
+
+VOID Get_Auth_Keywrap_Key(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq)
+{
+ KDK_KDF_TABLE *pTab = NULL;
+ INT status;
+ UCHAR DHKey[32] = {0};
+ UCHAR KdkInput[38] = {0};
+ UCHAR KDK[32] = {0};
+ UCHAR KdfKey[80] = {0};
+ unsigned char personlization_str[]=\
+ "Wi-Fi Easy and Secure Key Derivation";
+
+ os_alloc_mem(NULL, (UCHAR **)&pTab, sizeof(KDK_KDF_TABLE));
+ if (pTab == NULL)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Allocate memory fail!!!\n", __FUNCTION__));
+ return;
+ }
+ status = copy_from_user(pTab, wrq->u.data.pointer, wrq->u.data.length);
+ if(status)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Copy from user error!!!\n", __FUNCTION__));
+ os_free_mem(NULL, pTab);
+ return;
+ }
+
+ RT_SHA256(pTab->DH_Secu_Key, 192, DHKey);
+
+ NdisMoveMemory(&KdkInput[0], pTab->E_Nonce, 16);
+ NdisMoveMemory(&KdkInput[16], pTab->E_Mac_Addr, 6);
+ NdisMoveMemory(&KdkInput[22], pTab->R_Nonce, 16);
+
+ RT_HMAC_SHA256(DHKey, 32, KdkInput, 38, KDK, SHA256_DIGEST_SIZE);
+ WscDeriveKey(KDK, 32, personlization_str, (sizeof(personlization_str) - 1), KdfKey, 640);
+
+ NdisMoveMemory(pTab->AuthKey, &KdfKey[0], 32);
+ NdisMoveMemory(pTab->KeyWrapKey, &KdfKey[32], 16);
+ NdisMoveMemory(pTab->Emsk, &KdfKey[48], 32);
+
+ wrq->u.data.length = sizeof(KDK_KDF_TABLE);
+ if (copy_to_user(wrq->u.data.pointer, pTab, wrq->u.data.length))
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: copy_to_user() fail\n", __FUNCTION__));
+ }
+
+ os_free_mem(NULL, pTab);
+}
+
+VOID Get_KWA_Value(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq)
+{
+ KWA_TABLE *pKwaTab = NULL;
+ INT status;
+ UCHAR Buf[32];
+
+ os_alloc_mem(NULL, (UCHAR **)&pKwaTab, sizeof(KWA_TABLE));
+ if (pKwaTab == NULL)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Allocate memory fail!!!\n", __FUNCTION__));
+ return;
+ }
+
+ status = copy_from_user(pKwaTab, wrq->u.data.pointer, wrq->u.data.length);
+ if(status)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Copy from user error!!!\n", __FUNCTION__));
+ os_free_mem(NULL, pKwaTab);
+ return;
+ }
+
+ RT_HMAC_SHA256(pKwaTab->AuthKey, 32, pKwaTab->EncrptData, pKwaTab->EncrptDataLen,
+ Buf, SHA256_DIGEST_SIZE);
+
+ NdisMoveMemory(pKwaTab->KWA, Buf, 8);
+ wrq->u.data.length = sizeof(KWA_TABLE);
+
+ if (copy_to_user(wrq->u.data.pointer, pKwaTab, wrq->u.data.length))
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: copy_to_user() fail\n", __FUNCTION__));
+ }
+
+ os_free_mem(NULL, pKwaTab);
+}
+
+VOID Get_AES_Encrypt_Value(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq)
+{
+ AES_TABLE *pAesEnc = NULL;
+ INT status;
+
+ os_alloc_mem(NULL, (UCHAR **)&pAesEnc, sizeof(AES_TABLE));
+ if (pAesEnc == NULL)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Allocate memory fail!!!\n", __FUNCTION__));
+ return;
+ }
+
+ status = copy_from_user(pAesEnc, wrq->u.data.pointer, wrq->u.data.length);
+ if(status)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Copy from user error!!!\n", __FUNCTION__));
+ os_free_mem(NULL, pAesEnc);
+ return;
+ }
+
+ AES_CBC_Encrypt(pAesEnc->PlainText, pAesEnc->PlainTextLen, pAesEnc->KeyWrapKey,
+ sizeof(pAesEnc->KeyWrapKey), pAesEnc->IV, 16, (UINT8 *) pAesEnc->CipherText,
+ &(pAesEnc->CipherTextLen));
+
+ wrq->u.data.length = sizeof(AES_TABLE);
+ if (copy_to_user(wrq->u.data.pointer, pAesEnc, wrq->u.data.length))
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: copy_to_user() fail\n", __FUNCTION__));
+ }
+
+ os_free_mem(NULL, pAesEnc);
+}
+
+VOID Get_AES_Decrypt_Value(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq)
+{
+ AES_TABLE *pAesDec = NULL;
+ INT status;
+
+ os_alloc_mem(NULL, (UCHAR **)&pAesDec, sizeof(AES_TABLE));
+ if (pAesDec == NULL)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Allocate memory fail!!!\n", __FUNCTION__));
+ return;
+ }
+
+ status = copy_from_user(pAesDec, wrq->u.data.pointer, wrq->u.data.length);
+ if(status)
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: Copy from user error!!!\n", __FUNCTION__));
+ os_free_mem(NULL, pAesDec);
+ return;
+ }
+
+ AES_CBC_Decrypt(pAesDec->CipherText, pAesDec->CipherTextLen, pAesDec->KeyWrapKey,
+ sizeof(pAesDec->KeyWrapKey), pAesDec->IV, 16, (UINT8 *) pAesDec->PlainText,
+ (UINT *) &(pAesDec->PlainTextLen));
+
+ wrq->u.data.length = sizeof(AES_TABLE);
+ if (copy_to_user(wrq->u.data.pointer, pAesDec, wrq->u.data.length))
+ {
+ DBGPRINT(RT_DEBUG_ERROR, ("%s: copy_to_user() fail\n", __FUNCTION__));
+ }
+
+ os_free_mem(NULL, pAesDec);
+}
+#endif \ No newline at end of file
diff --git a/cleopatre/devkit/mt7601udrv/include/ap_cfg.h b/cleopatre/devkit/mt7601udrv/include/ap_cfg.h
index 81be09272b..7df65ceeee 100644
--- a/cleopatre/devkit/mt7601udrv/include/ap_cfg.h
+++ b/cleopatre/devkit/mt7601udrv/include/ap_cfg.h
@@ -68,6 +68,21 @@ VOID RTMPAPIoctlMAC(
#endif /* DBG */
+#ifdef P1905_AP_AUTO_CONFIGURED
+VOID Get_DH_Pub_Key(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq);
+VOID Get_DH_Secu_Key(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq);
+VOID Get_Auth_Keywrap_Key(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq);
+VOID Get_KWA_Value(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq);
+VOID Get_AES_Encrypt_Value(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq);
+VOID Get_AES_Decrypt_Value(
+ IN RTMP_IOCTL_INPUT_STRUCT *wrq);
+#endif
+
VOID RtmpDrvMaxRateGet(
IN VOID *pReserved,
/* IN PHTTRANSMIT_SETTING pHtPhyMode, */
diff --git a/cleopatre/devkit/mt7601udrv/include/oid.h b/cleopatre/devkit/mt7601udrv/include/oid.h
index 6152987c99..15793872b5 100644
--- a/cleopatre/devkit/mt7601udrv/include/oid.h
+++ b/cleopatre/devkit/mt7601udrv/include/oid.h
@@ -1229,7 +1229,42 @@ typedef struct _RT_LLTD_ASSOICATION_TABLE {
#define RT_WSC_UPNP_EVENT_FLAG 0x109
#endif /* WSC_INCLUDED */
-
+#ifdef P1905_AP_AUTO_CONFIGURED
+typedef struct _DH_TABLE {
+ UCHAR pub_key[192];
+ UCHAR priv_key[192];
+ UCHAR secu_key[192];
+} DH_TABLE, *PDH_TABLE;
+
+typedef struct _KDK_KDF_TABLE {
+ UCHAR E_Nonce[16];
+ UCHAR R_Nonce[16];
+ UCHAR E_Mac_Addr[6];
+
+ UCHAR AuthKey[32];
+ UCHAR KeyWrapKey[16];
+ UCHAR Emsk[32];
+
+ UCHAR DH_Secu_Key[192];
+} KDK_KDF_TABLE, *PKDK_KDF_TABLE;
+
+typedef struct _KWA_TABLE {
+ UCHAR AuthKey[32];
+ UCHAR *EncrptData;
+ UINT EncrptDataLen;
+ UCHAR KWA[8];
+} KWA_TABLE, *PKWA_TABLE;
+
+typedef struct _AES_TABLE {
+ UCHAR *PlainText;
+ UINT PlainTextLen;
+ UCHAR IV[16];
+ UCHAR *CipherText;
+ UINT CipherTextLen;
+
+ UCHAR KeyWrapKey[16];
+} AES_TABLE, *PAES_TABLE;
+#endif
/*#define MAX_CUSTOM_LEN 128 */
diff --git a/cleopatre/devkit/mt7601udrv/include/os/rt_os.h b/cleopatre/devkit/mt7601udrv/include/os/rt_os.h
index 3f3b8fc741..872b078dd1 100644
--- a/cleopatre/devkit/mt7601udrv/include/os/rt_os.h
+++ b/cleopatre/devkit/mt7601udrv/include/os/rt_os.h
@@ -78,6 +78,15 @@
#endif/* INF_AR9 */
#define RTPRIV_IOCTL_SET_WSCOOB (SIOCIWFIRSTPRIV + 0x19)
#define RTPRIV_IOCTL_WSC_CALLBACK (SIOCIWFIRSTPRIV + 0x1A)
+
+#ifdef P1905_AP_AUTO_CONFIGURED
+#define RTPRIV_IOCTL_GET_DH_PUB_KEY (SIOCIWFIRSTPRIV + 0x1B)
+#define RTPRIV_IOCTL_GET_DH_SECU_KEY (SIOCIWFIRSTPRIV + 0x1C)
+#define RTPRIV_IOCTL_GET_AUTH_KEYWRAP_KEY (SIOCIWFIRSTPRIV + 0x1D)
+#define RTPRIV_IOCTL_GET_KWA_VALUE (SIOCIWFIRSTPRIV + 0x1E)
+#define RTPRIV_IOCTL_GET_AES_ENCRYPT_VALUE (SIOCIWFIRSTPRIV + 0x14)
+#define RTPRIV_IOCTL_GET_AES_DECRYPT_VALUE (SIOCIWFIRSTPRIV + 0x15)
+#endif
#endif /* CONFIG_AP_SUPPORT */
#endif /* _RT_OS_H_ */
diff --git a/cleopatre/devkit/mt7601udrv/include/rtmp_cmd.h b/cleopatre/devkit/mt7601udrv/include/rtmp_cmd.h
index 98839563c2..7cae923051 100644
--- a/cleopatre/devkit/mt7601udrv/include/rtmp_cmd.h
+++ b/cleopatre/devkit/mt7601udrv/include/rtmp_cmd.h
@@ -126,6 +126,14 @@ typedef enum _CMD_RTPRIV_IOCTL_AP {
CMD_RTPRIV_IOCTL_AP_SIOCGIWRATEQ,
CMD_RTPRIV_IOCTL_AP_SIOCSIWGENIE,
+ /* p1905.1 ap auto configured*/
+ CMD_RTPRIV_IOCTL_GET_DH_PUB_KEY,
+ CMD_RTPRIV_IOCTL_GET_DH_SECU_KEY,
+ CMD_RTPRIV_IOCTL_GET_AUTH_KEYWRAP_KEY,
+ CMD_RTPRIV_IOCTL_GET_KWA_VALUE,
+ CMD_RTPRIV_IOCTL_GET_AES_ENCRYPT_VALUE,
+ CMD_RTPRIV_IOCTL_GET_AES_DECRYPT_VALUE,
+
/* can not exceed 0x5000 */
} CMD_RTPRIV_IOCTL_AP;
diff --git a/cleopatre/devkit/mt7601udrv/os/linux/ap_ioctl.c b/cleopatre/devkit/mt7601udrv/os/linux/ap_ioctl.c
index b2aad0d8bb..92bf80d41e 100644
--- a/cleopatre/devkit/mt7601udrv/os/linux/ap_ioctl.c
+++ b/cleopatre/devkit/mt7601udrv/os/linux/ap_ioctl.c
@@ -411,6 +411,27 @@ INT rt28xx_ap_ioctl(
break;
#endif /* DBG */
+#ifdef P1905_AP_AUTO_CONFIGURED
+ case RTPRIV_IOCTL_GET_DH_PUB_KEY:
+ RTMP_AP_IoctlHandle(pAd, wrq, CMD_RTPRIV_IOCTL_GET_DH_PUB_KEY, 0, NULL, 0);
+ break;
+ case RTPRIV_IOCTL_GET_DH_SECU_KEY:
+ RTMP_AP_IoctlHandle(pAd, wrq, CMD_RTPRIV_IOCTL_GET_DH_SECU_KEY, 0, NULL, 0);
+ break;
+ case RTPRIV_IOCTL_GET_AUTH_KEYWRAP_KEY:
+ RTMP_AP_IoctlHandle(pAd, wrq, CMD_RTPRIV_IOCTL_GET_AUTH_KEYWRAP_KEY, 0, NULL, 0);
+ break;
+ case RTPRIV_IOCTL_GET_KWA_VALUE:
+ RTMP_AP_IoctlHandle(pAd, wrq, CMD_RTPRIV_IOCTL_GET_KWA_VALUE, 0, NULL, 0);
+ break;
+ case RTPRIV_IOCTL_GET_AES_ENCRYPT_VALUE:
+ RTMP_AP_IoctlHandle(pAd, wrq, CMD_RTPRIV_IOCTL_GET_AES_ENCRYPT_VALUE, 0, NULL, 0);
+ break;
+ case RTPRIV_IOCTL_GET_AES_DECRYPT_VALUE:
+ RTMP_AP_IoctlHandle(pAd, wrq, CMD_RTPRIV_IOCTL_GET_AES_DECRYPT_VALUE, 0, NULL, 0);
+ break;
+#endif
+
default:
/* DBGPRINT(RT_DEBUG_ERROR, ("IOCTL::unknown IOCTL's cmd = 0x%08x\n", cmd)); */
Status = RTMP_IO_EOPNOTSUPP;
diff --git a/cleopatre/devkit/mt7601udrv/os/linux/config.mk b/cleopatre/devkit/mt7601udrv/os/linux/config.mk
index 87b00d5cf6..a95ef2fed2 100644
--- a/cleopatre/devkit/mt7601udrv/os/linux/config.mk
+++ b/cleopatre/devkit/mt7601udrv/os/linux/config.mk
@@ -199,6 +199,9 @@ HAS_MICROWAVE_OVEN_SUPPORT=n
HAS_WIFI_P2P_CONCURRENT_FAST_SCAN=n
+#support IEEE1905.1 for MStar PLC
+HAS_P1905_AP_AUTO_CONFIGURED=y
+HAS_P1905_LINK_METRICS=y
#################################################
CC := $(CROSS_COMPILE)gcc
@@ -408,6 +411,13 @@ ifeq ($(HAS_TXRX_SW_ANTDIV_SUPPORT),y)
WFLAGS += -DTXRX_SW_ANTDIV_SUPPORT
endif
+ifeq ($(HAS_P1905_AP_AUTO_CONFIGURED),y)
+WFLAGS += -DP1905_AP_AUTO_CONFIGURED
+endif
+
+ifeq ($(HAS_P1905_LINK_METRICS),y)
+WFLAGS += -DP1905_LINK_METRICS
+endif
endif #// endif of RT2860_MODE == AP //
diff --git a/cleopatre/devkit/p1905_managerd/.gitignore b/cleopatre/devkit/p1905_managerd/.gitignore
new file mode 100644
index 0000000000..ed818d7086
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/.gitignore
@@ -0,0 +1,4 @@
+obj/
+/p1905_managerd
+/.configured
+/.unpacked
diff --git a/cleopatre/devkit/p1905_managerd/Makefile b/cleopatre/devkit/p1905_managerd/Makefile
new file mode 100644
index 0000000000..878a12fcda
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/Makefile
@@ -0,0 +1,138 @@
+include ./config.mk
+BIN=p1905_managerd
+OBJPATH=obj
+SRCPATH=src
+INCPATH=inc
+OBJINCPATH=$(OBJPATH)/inc
+
+#ifeq ($(AP_AUTO_CONFIG_SUPPORT),y)
+#ifeq ($(AP_REGISTRAR),y)
+#BIN=p1905_managerd_ap_router
+#else
+#BIN=p1905_managerd_ap_extender
+#endif
+#endif
+
+ifeq ($(CC_FOR_TARGET),) #direct compile
+CC=arm-linux-gcc
+CC_WITH_CFLAGS=$(CC) -I/opt/spidcom/spc300/usr/include -g -Os
+CC_WITHOUT_CFLAGS=$(CC)
+LINUX_DIR=../../linux-2.6.25.10-spc300
+LIBSPID_DIR=../../application/libspid
+LIBMME_DIR=../../application/libmme
+LIBALME_DIR=../../application/libalme
+else #compile from buildroot
+CC_WITH_CFLAGS=$(CC)
+CC_WITHOUT_CFLAGS=$(CC_FOR_TARGET)
+endif
+CLEO_INC=../../include
+EXTRA_CFLAGS=-I$(INCPATH) -I$(OBJINCPATH) -I$(LINUX_DIR)/include -I$(LINUX_DIR)/include/asm-arm/arch-spc300 -I$(LIBMME_DIR)/inc -I$(LIBSPID_DIR)/inc -I$(LIBALME_DIR)/inc -I$(CLEO_INC) -MMD
+
+#################################################################################
+#support interface
+#################################################################################
+ifeq ($(PLC_SUPPORT),y)
+EXTRA_CFLAGS+= -DSUPPORT_PLC
+endif
+ifeq ($(WIFI_SUPPORT),y)
+EXTRA_CFLAGS+= -DSUPPORT_WIFI
+endif
+ifeq ($(ETH_SUPPORT),y)
+EXTRA_CFLAGS+= -DSUPPORT_ETH
+endif
+ifeq ($(WIFI_STATION_SUPPORT),y)
+EXTRA_CFLAGS+= -DSUPPORT_WIFI_STATION
+endif
+#################################################################################
+#support 1905.1 function
+#################################################################################
+ifeq ($(ALME_SUPPORT),y)
+EXTRA_CFLAGS+= -DSUPPORT_ALME
+endif
+ifeq ($(PBC_SUPPORT),y)
+EXTRA_CFLAGS+= -DSUPPORT_PBC
+endif
+ifeq ($(AP_AUTO_CONFIG_SUPPORT),y)
+EXTRA_CFLAGS+= -DSUPPORT_AP_AUTO_CONFIG
+ifeq ($(AP_REGISTRAR),y)
+EXTRA_CFLAGS+= -DSUPPORT_AP_REGISTRAR
+else
+EXTRA_CFLAGS+= -DSUPPORT_AP_ENROLLE
+endif
+endif
+#################################################################################
+#support wifi chips
+#################################################################################
+ifeq ($(WIFI_RT5572),y)
+EXTRA_CFLAGS+= -DRT5572_ENABLE
+else
+ifeq ($(WIFI_MT7601),y)
+EXTRA_CFLAGS+= -DMT7601_ENABLE
+endif
+endif
+
+LIBS=-L$(LIBMME_DIR) -L$(LIBSPID_DIR) -lmme -lspid -lpthread
+
+#################################################################################
+#below is for source code include
+#################################################################################
+SRCS=cmdu.c cmdu_fragment.c cmdu_message.c cmdu_message_parse.c cmdu_tlv.c cmdu_tlv_parse.c \
+ lldp.c lldp_message.c lldp_message_parse.c p1905_managerd.c p1905_push_button.c p1905_utils.c
+
+ifeq ($(WIFI_SUPPORT),y)
+SRCS+=wifi_utils.c
+ifeq ($(WIFI_RT5572),y)
+SRCS+=rt5572_io.c
+else
+ifeq ($(WIFI_MT7601),y)
+SRCS+=mt7601_io.c
+endif
+endif
+endif
+
+ifeq ($(AP_AUTO_CONFIG_SUPPORT),y)
+SRCS+=wsc_attr_tlv.c
+SRCS+=wsc_message.c
+SRCS+=p1905_ap_autoconfig.c
+endif
+
+ifeq ($(ALME_SUPPORT),y)
+SRCS+=alme.c
+endif
+
+ifeq ($(PLC_SUPPORT),y)
+SRCS+=plc_utils.c
+endif
+
+OBJS=$(addprefix $(OBJPATH)/,$(SRCS:.c=.o))
+DEPS=$(patsubst %o,%d,$(OBJS))
+
+all: $(BIN)
+
+$(BIN): libspid libmme $(OBJS)
+ $(CC_WITHOUT_CFLAGS) -o $@ $(OBJS) $(LIBS)
+
+$(OBJPATH)/%.o: $(SRCPATH)/%.c
+ $(CC_WITH_CFLAGS) $(EXTRA_CFLAGS) -o $@ -c $<
+
+$(OBJS): | $(OBJPATH)
+
+$(OBJPATH):
+ mkdir $(OBJPATH)
+
+$(OBJINCPATH): | $(OBJPATH)
+ mkdir $@
+libspid:
+ $(MAKE) -C $(LIBSPID_DIR) libspid.so
+
+libmme:
+ $(MAKE) -C $(LIBMME_DIR) libmme.so
+
+-include $(DEPS)
+
+.PHONY: all clean
+
+clean:
+ rm -f $(OBJS) $(DEPS) $(BIN)
+ rm -rf $(OBJINCPATH)
+ rmdir $(OBJPATH)
diff --git a/cleopatre/devkit/p1905_managerd/S98p1905 b/cleopatre/devkit/p1905_managerd/S98p1905
new file mode 100644
index 0000000000..a6d3e86ccc
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/S98p1905
@@ -0,0 +1,35 @@
+#!/bin/sh
+#
+#start the p1905.1 daemon....
+#
+
+start() {
+ echo "Starting p1905.1 daemon..."
+ /usr/sbin/p1905_managerd &
+}
+stop() {
+ echo -n "Stopping p1905.1 daemon..."
+ killall p1905_managerd
+}
+
+restart() {
+ stop
+ start
+}
+
+case "$1" in
+ start)
+ start
+ ;;
+ stop)
+ stop
+ ;;
+ restart|reload)
+ restart
+ ;;
+ *)
+ echo $"Usage: $0 {start|stop|restart}"
+ exit 1
+esac
+
+exit $?
diff --git a/cleopatre/devkit/p1905_managerd/config.mk b/cleopatre/devkit/p1905_managerd/config.mk
new file mode 100644
index 0000000000..166980648c
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/config.mk
@@ -0,0 +1,36 @@
+#PLC Support
+PLC_SUPPORT=y
+
+#WiFi Support
+WIFI_SUPPORT=y
+
+#ETH Support
+ETH_SUPPORT=y
+
+#ALME Support
+ALME_SUPPORT=y
+
+#PBC setup support
+PBC_SUPPORT=y
+
+#WiFi Chip Support
+ifeq ($(WIFI_SUPPORT),y)
+WIFI_RT5572=N
+WIFI_MT7601=y
+endif
+
+#WiFi station support. Define WIFI_STATION_SUPPORT=N for AP
+ifeq ($(WIFI_SUPPORT),y)
+WIFI_STATION_SUPPORT=N
+endif
+
+#AP Auto Configured
+ifeq ($(WIFI_SUPPORT),y)
+ifneq ($(WIFI_STATION_SUPPORT),y)
+AP_AUTO_CONFIG_SUPPORT=y
+endif
+endif
+
+ifeq ($(AP_AUTO_CONFIG_SUPPORT),y)
+AP_REGISTRAR=N
+endif \ No newline at end of file
diff --git a/cleopatre/devkit/p1905_managerd/inc/alme.h b/cleopatre/devkit/p1905_managerd/inc/alme.h
new file mode 100644
index 0000000000..5fb721fa78
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/alme.h
@@ -0,0 +1,18 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/alme.h
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ *
+ */
+#ifndef ALME_H
+#define ALME_H
+
+typedef enum
+{
+ alme_none = 0,
+ alme_wait_4_link_metrics_rsp,
+
+} ALME_STATE;
+
+
+#endif /* ALME_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/cmdu.h b/cleopatre/devkit/p1905_managerd/inc/cmdu.h
new file mode 100644
index 0000000000..4e3049895e
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/cmdu.h
@@ -0,0 +1,36 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/cmdu.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#ifndef CMDU_H
+#define CMDU_H
+
+#include "p1905_managerd.h"
+#include "cmdu_message.h"
+#include <sys/queue.h>
+
+/** Interfaces name */
+#define BR_IFNAME "br0"
+#define LO_IFNAME "lo"
+#define ETH0_IFNAME "eth0"
+#define PLC0_IFNAME "plc0"
+#define WIFI0_IFNAME "ra0"
+
+struct txq_list
+{
+ unsigned char dmac[6];
+ unsigned char smac[6];
+ msgtype mtype;
+ unsigned short mid;
+ TAILQ_ENTRY(txq_list) cmdu_txq_entry;
+};
+
+int cmdu_init(struct p1905_managerd_ctx *ctx);
+void cmdu_uninit(struct p1905_managerd_ctx *ctx);
+int process_cmdu_txq(struct p1905_managerd_ctx *ctx,unsigned char *buffer);
+void insert_cmdu_txq(unsigned char *dmac, unsigned char *smac,msgtype mtype,unsigned short mid);
+int cmdu_bridge_receive(struct p1905_managerd_ctx *ctx,unsigned char *buf,int len);
+int cmdu_parse(struct p1905_managerd_ctx *ctx,unsigned char *buf,int len);
+
+#endif /* CMDU_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/cmdu_fragment.h b/cleopatre/devkit/p1905_managerd/inc/cmdu_fragment.h
new file mode 100644
index 0000000000..ab065030f6
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/cmdu_fragment.h
@@ -0,0 +1,25 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/cmdu_fragment.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ *
+ */
+#ifndef CMDU_FRAGMENT_H
+#define CMDU_FRAGMENT_H
+
+#include <sys/queue.h>
+
+/*data structure of RX fragment queue*/
+struct fragment_list
+{
+ unsigned short mtype;
+ unsigned short mid;
+ unsigned char fid;
+ unsigned char lastfragment;
+ unsigned char *data;
+ unsigned short length;
+
+ LIST_ENTRY(fragment_list) fragment_entry;
+};
+
+#endif /* CMDU_FRAGMENT_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/cmdu_message.h b/cleopatre/devkit/p1905_managerd/inc/cmdu_message.h
new file mode 100644
index 0000000000..12e2494b97
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/cmdu_message.h
@@ -0,0 +1,88 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/cmdu_message.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ *
+ */
+#ifndef CMDU_MESSAGE_H
+#define CMDU_MESSAGE_H
+
+#include <linux/if_ether.h>
+#include "cmdu_tlv.h"
+
+#define MIN_TLVS_LENGTH 38 /*46 - 8(cmdu header size) */
+#define MAX_TLVS_LENGTH 1492 /*1500 -8(cmdu header size)*/
+#define CMDU_HLEN 8
+
+/*p1905.1 message version*/
+#define MESSAGE_VERSION 0
+
+
+/*p1905.1 message type*/
+#define TOPOLOGY_DISCOVERY 0x0000
+#define TOPOLOGY_NOTIFICATION 0x0001
+#define TOPOLOGY_QUERY 0x0002
+#define TOPOLOGY_RESPONSE 0x0003
+#define VENDOR_SPECIFIC 0x0004
+#define LINK_METRICS_QUERY 0x0005
+#define LINK_METRICS_RESPONSE 0x0006
+#define AP_AUTOCONFIG_SEARCH 0x0007
+#define AP_AUTOCONFIG_RESPONSE 0x0008
+#define AP_AUTOCONFIG_WSC 0x0009
+#define AP_AUTOCONFIG_RENEW 0x000A
+#define P1905_PB_EVENT_NOTIFY 0x000B
+#define P1905_PB_JOIN_NOTIFY 0x000C
+
+typedef enum
+{
+ e_topology_discovery = 1,
+ e_topology_notification,
+ e_topology_query,
+ e_topology_response,
+ e_vendor_specific,
+ e_link_metric_query,
+ e_link_metric_response,
+ e_push_button_event_notification,
+ e_push_button_join_notification,//for PLC
+ e_push_button_join_notification_wifi,//for wifi
+#ifdef SUPPORT_AP_AUTO_CONFIG
+#ifdef SUPPORT_AP_ENROLLE
+ e_ap_autoconfiguration_search,
+ e_ap_autoconfiguration_wsc_m1,
+#endif
+#ifdef SUPPORT_AP_REGISTRAR
+ e_ap_autoconfiguration_response,
+ e_ap_autoconfiguration_wsc_m2,
+ e_ap_autoconfiguration_renew,
+#endif
+#endif
+}msgtype;
+
+typedef struct
+{
+ unsigned char message_version;
+ unsigned char reserved_field_0;
+ __be16 message_type;
+ __be16 message_id;
+ unsigned char fragment_id;
+ unsigned char reserve_field_1:6;
+ unsigned char relay_indicator:1;
+ unsigned char last_fragment_indicator:1;
+} __attribute__ ((__packed__)) cmdu_message_header;
+
+unsigned char *get_tlv_buffer(void);
+
+unsigned short create_topology_discovery_message(unsigned char *buf,
+ unsigned char *al_mac, unsigned char *itf_mac);
+
+unsigned short create_topology_query_message(unsigned char *buf);
+
+unsigned short create_topology_response_message(unsigned char *buf,
+ unsigned char *al_mac, struct p1905_interface *itf_list,
+ struct bridge_capabiltiy *br_cap_list, struct p1905_neighbor *p1905_dev,
+ struct non_p1905_neighbor *non_p1905_dev, struct list_head_tprdb *tpgr_head);
+
+unsigned short create_topology_notification_message(unsigned char *buf,
+ unsigned char *al_mac);
+
+#endif /* CMDU_MESSAGE_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/cmdu_message_parse.h b/cleopatre/devkit/p1905_managerd/inc/cmdu_message_parse.h
new file mode 100644
index 0000000000..ad3d19aaba
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/cmdu_message_parse.h
@@ -0,0 +1,16 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/cmdu_message_parse.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#ifndef CMDU_MESSAGE_PARSE_H
+#define CMDU_MESSAGEPARSE_H
+
+#include <linux/if_ether.h>
+#include "p1905_managerd.h"
+#include "cmdu_tlv_parse.h"
+
+int parse_cmdu_message(struct p1905_managerd_ctx *ctx, unsigned char *buf,
+ unsigned char *dmac, unsigned char *smac, int len);
+
+#endif /* CMDU_MESSAGEPARSE_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/cmdu_tlv.h b/cleopatre/devkit/p1905_managerd/inc/cmdu_tlv.h
new file mode 100644
index 0000000000..a6039695c7
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/cmdu_tlv.h
@@ -0,0 +1,108 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/cmdu_tlv.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#ifndef CMDU_TLV_H
+#define CMDU_TLV_H
+
+#include "p1905_managerd.h"
+
+/*CMDU tlv type value*/
+#define END_OF_TLV_TYPE 0
+#define AL_MAC_ADDR_TLV_TYPE 1
+#define MAC_ADDR_TLV_TYPE 2
+#define DEVICE_INFO_TLV_TYPE 3
+#define BRIDGE_CAPABILITY_TLV_TYPE 4
+#define NON_P1905_NEIGHBOR_DEV_TLV_TYPE 6
+#define P1905_NEIGHBOR_DEV_TLV_TYPE 7
+#define LINK_METRICS_QUERY_TLV_TYPE 8
+#define TRANSMITTER_LINK_METRIC_TYPE 9
+#define RECEIVER_LINK_METRIC_TYPE 10
+#define VENDOR_SPECIFIC_TLV_TYPE 11
+#define RESULT_CODE_TLV_TYPE 12
+#define SEARCH_ROLE_TLV_TYPE 13
+#define AUTO_CONFIG_FREQ_BAND_TLV_TYPE 14
+#define SUPPORT_ROLE_TLV_TYPE 15
+#define SUPPORT_FREQ_BAND_TLV_TYPE 16
+#define WSC_TLV_TYPE 17
+#define PUSH_BUTTON_EVENT_NOTIFICATION_TYPE 18
+#define PUSH_BUTTON_JOIN_NOTIFICATION_TYPE 19
+
+
+/*CMDU tlv length(fixed part)*/
+#define END_OF_TLV_LENGTH 0
+#define AL_MAC_ADDR_TLV_LENGTH 6
+#define MAC_ADDR_TLV_LENGTH 6
+#define LINK_METRIC_RESULT_CODE_LENGTH 1
+#define PUSH_BUTTON_JOIN_NOTIFICATION_LENGTH 20
+#ifdef SUPPORT_AP_AUTO_CONFIG
+#define SEARCH_ROLE_LENGTH 1
+#define AUTOCONFIG_FREQ_BAND_LENGTH 1
+#define SUPPORTED_ROLE_LENGTH 1
+#define SUPPORTED_FREQ_BAND_LENGTH 1
+#endif
+
+/*link metrics query target*/
+#define QUERY_ALL_NEIGHBOR 0
+#define QUERY_SPECIFIC_NEIGHBOR 1
+
+/*link metrics query type*/
+#define TX_METRICS_ONLY 0
+#define RX_METRICS_ONLY 1
+#define BOTH_TX_AND_RX_METRICS 2
+
+
+/*Interface Media Type(spec 6.4.7)*/
+#define IEEE802_3_GROUP 0x0000
+#define IEEE802_11_GROUP 0x0100
+#define IEEE1901_GROUP 0x0200
+#define MOCA_GROUP 0x0300
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+/*for searched role tlv and support role tlv use*/
+#define ROLE_REGISTRAR 0x00
+
+/*for auto freq band tlv and supported freq band tlv use*/
+#define IEEE802_11_band_2P4G 0x00
+#define IEEE802_11_band_5G 0x01
+#define IEEE802_11_band_60G 0x02
+#endif
+
+typedef enum
+{
+ ieee_802_3_u = IEEE802_3_GROUP,
+ ieee_802_3_ab,
+
+ ieee_802_11_b = IEEE802_11_GROUP,
+ ieee_802_11_g,
+ ieee_802_11_a,
+ ieee_802_11_n_2_4G,
+ ieee_802_11_n_5G,
+ ieee_802_11_ac,
+ ieee_802_11_ad,
+ ieee_802_11_af,
+
+ ieee_1901_wavelet = IEEE1901_GROUP,
+ ieee_1901_FFT,
+
+ moca_v1_1 = MOCA_GROUP,
+}itftype;
+
+unsigned short append_1905_al_mac_addr_type_tlv(unsigned char *pkt,
+ unsigned char *al_mac);
+
+unsigned short append_mac_addr_type_tlv(unsigned char *pkt,
+ unsigned char *itf_mac);
+
+unsigned short append_device_info_type_tlv(unsigned char *pkt,
+ unsigned char *al_mac,struct p1905_interface *itf_list);
+
+unsigned short append_device_bridge_capability_type_tlv(unsigned char *pkt,
+ struct bridge_capabiltiy *br_cap_list);
+
+unsigned short append_p1905_neighbor_device_type_tlv(unsigned char *pkt,
+ struct p1905_neighbor *devlist,int num, unsigned char *seperate);
+
+unsigned short append_end_of_tlv(unsigned char *pkt);
+#endif /*CMDU_TLV_H*/
diff --git a/cleopatre/devkit/p1905_managerd/inc/cmdu_tlv_parse.h b/cleopatre/devkit/p1905_managerd/inc/cmdu_tlv_parse.h
new file mode 100644
index 0000000000..96ef951155
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/cmdu_tlv_parse.h
@@ -0,0 +1,18 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/cmdu_tlv_parse.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#ifndef CMDU_TLV_PARSE_H
+#define CMDU_TLV_PARSE_H
+
+#include "p1905_managerd.h"
+#include "cmdu_tlv.h"
+
+int get_cmdu_tlv_length(unsigned char *buf);
+int parse_al_mac_addr_type_tlv(unsigned char *buf, unsigned char *al_mac);
+int parse_mac_addr_type_tlv(unsigned char *buf,unsigned char *mac);
+int parse_device_info_type_tlv(unsigned char *buf,struct list_head_devinfo *devinfo_head);
+int parse_bridge_capability_type_tlv(unsigned char *buf,struct list_head_brcap *brcap_head);
+
+#endif /*CMDU_TLV_PARSE_H*/
diff --git a/cleopatre/devkit/p1905_managerd/inc/lldp.h b/cleopatre/devkit/p1905_managerd/inc/lldp.h
new file mode 100644
index 0000000000..efa9b11268
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/lldp.h
@@ -0,0 +1,21 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/lldp.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#ifndef LLDP_H
+#define LLDP_H
+
+#include <linux/if_ether.h>
+#include "p1905_managerd.h"
+#include "lldp_message.h"
+
+int send_802_1_bridge_discovery_msg(struct p1905_managerd_ctx *ctx,
+ unsigned char *dmac, unsigned char *smac,
+ unsigned char *buffer);
+
+int lldpdu_init(struct p1905_managerd_ctx *ctx);
+
+void lldpdu_uninit(struct p1905_managerd_ctx *ctx);
+
+#endif /* LLDP_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/lldp_message.h b/cleopatre/devkit/p1905_managerd/inc/lldp_message.h
new file mode 100644
index 0000000000..aa5d9efb07
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/lldp_message.h
@@ -0,0 +1,62 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/lldp_message.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#ifndef LLDP_MESSAGE_H
+#define LLDP_MESSAGE_H
+
+#include <linux/if_ether.h>
+
+#define LLDPDU_HLEN 2
+
+#define END_OF_LLDPDU_TLV_TYPE 0
+#define CHASSIS_ID_TLV_TYPE 1
+#define PORT_ID_TLV_TYPE 2
+#define TIME_TO_LIVE_TLV_TYPE 3
+
+/*chassis id tlv subtype*/
+#define C_CHASSIS_COMPONENT 1
+#define C_INTERFACE_ALIAS 2
+#define C_PORT_COMPONENT 3
+#define C_MAC_ADDRESS 4
+#define C_NETWORK_ADDRESS 5
+#define C_INTERFACE_NAME 6
+#define C_LOCAL_ASSIGN 7
+
+/*port id tlv subtype*/
+#define P_INTERFACE_ALIAS 1
+#define P_PORT_COMPONENT 2
+#define P_MAC_ADDRESS 3
+#define P_NETWORK_ADDRESS 4
+#define P_INTERFACE_NAME 5
+#define P_AGENT_CIRCUIT_ID 6
+#define P_LOCAL_ASSIGN 7
+
+typedef union
+{
+ __be16 lldpdu_hdr;
+
+ struct lldphdr
+ {
+ __be16 info_str_len:9;
+ __be16 tlv_type:7;
+
+ }lldpdu_msg;
+}lldpdu_message_header;
+
+
+unsigned short append_lldpdu_chassis_id_tlv(unsigned char *pkt,
+ unsigned char *al_mac);
+
+unsigned short append_lldpdu_port_id_tlv(unsigned char *pkt,
+ unsigned char *itf_mac);
+
+unsigned short append_lldpdu_time_to_live_tlv(unsigned char *pkt);
+
+unsigned short append_lldpdu_end_of_lldpdu_tlv(unsigned char *pkt);
+
+unsigned short create_802_1_bridge_discovery_message(unsigned char *buf,
+ unsigned char *al_mac, unsigned char *itf_mac);
+
+#endif /* LLDP_MESSAGE_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/lldp_message_parse.h b/cleopatre/devkit/p1905_managerd/inc/lldp_message_parse.h
new file mode 100644
index 0000000000..f68644e267
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/lldp_message_parse.h
@@ -0,0 +1,20 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/lldp_message_parse.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#ifndef lldp_MESSAGE_PARSE_H
+#define lldp_MESSAGEPARSE_H
+
+#include <linux/if_ether.h>
+
+
+struct lldpdu_rx
+{
+ unsigned char *receiving_local_itf_mac[ETH_ALEN];
+ unsigned char *port_id_mac[ETH_ALEN];
+ int ttl;
+ LIST_ENTRY(lldpdu_rx) lldpdu_entry;
+};
+
+#endif /* lldp_MESSAGEPARSE_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/mt7601_io.h b/cleopatre/devkit/p1905_managerd/inc/mt7601_io.h
new file mode 100644
index 0000000000..4b8979bae7
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/mt7601_io.h
@@ -0,0 +1,73 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/mt7601_io.h
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ *
+ */
+#ifndef MT7601_IO_H
+#define MT7601_IO_H
+
+/*define the ioctl index for mt7601*/
+#define RT_PRIV_IOCTL (SIOCIWFIRSTPRIV + 0x01)
+#define RTPRIV_IOCTL_SET (SIOCIWFIRSTPRIV + 0x02)
+#define RTPRIV_IOCTL_STATISTICS (SIOCIWFIRSTPRIV + 0x09)
+#define RTPRIV_IOCTL_GET_MAC_TABLE (SIOCIWFIRSTPRIV + 0x0F)
+#define RTPRIV_IOCTL_WSC_PROFILE (SIOCIWFIRSTPRIV + 0x12)
+#define RTPRIV_IOCTL_GET_DH_PUB_KEY (SIOCIWFIRSTPRIV + 0x1B)
+#define RTPRIV_IOCTL_GET_DH_SECU_KEY (SIOCIWFIRSTPRIV + 0x1C)
+#define RTPRIV_IOCTL_GET_AUTH_KEYWRAP_KEY (SIOCIWFIRSTPRIV + 0x1D)
+#define RTPRIV_IOCTL_GET_KWA_VALUE (SIOCIWFIRSTPRIV + 0x1E)
+#define RTPRIV_IOCTL_GET_AES_ENCRYPT_VALUE (SIOCIWFIRSTPRIV + 0x14)
+#define RTPRIV_IOCTL_GET_AES_DECRYPT_VALUE (SIOCIWFIRSTPRIV + 0x15)
+
+/*define ioctl flag value*/
+#define RT_OID_WSC_CONFIG_STATUS 0x074F
+#define RT_OID_WSC_UUID 0x0753
+
+typedef struct _mt7601_io_table
+{
+ unsigned char *data;
+ unsigned short length;
+ unsigned short flag;
+} mt7601_io_table;
+
+typedef struct __attribute__ ((packed)) _WSC_CONFIGURED_VALUE {
+ unsigned short WscConfigured;
+ unsigned char WscSsid[32 + 1];
+ unsigned short WscAuthMode;
+ unsigned short WscEncrypType;
+ unsigned char DefaultKeyIdx;
+ unsigned char WscWPAKey[64 + 1];
+} WSC_CONFIGURED_VALUE;
+
+typedef union _MACHTTRANSMIT_SETTING {
+ struct {
+ unsigned short MCS:7; /* MCS */
+ unsigned short BW:1; /*channel bandwidth 20MHz or 40 MHz */
+ unsigned short ShortGI:1;
+ unsigned short STBC:2; /*SPACE */
+ unsigned short rsv:3;
+ unsigned short MODE:2; /* Use definition MODE_xxx. */
+ } field;
+ unsigned short word;
+} MACHTTRANSMIT_SETTING, *PMACHTTRANSMIT_SETTING;
+
+typedef struct _RT_802_11_MAC_ENTRY {
+ unsigned char ApIdx;
+ unsigned char Addr[6];
+ unsigned char Aid;
+ unsigned char Psm; /* 0:PWR_ACTIVE, 1:PWR_SAVE */
+ unsigned char MimoPs; /* 0:MMPS_STATIC, 1:MMPS_DYNAMIC, 3:MMPS_Enabled */
+ signed char AvgRssi0;
+ signed char AvgRssi1;
+ signed char AvgRssi2;
+ unsigned int ConnectedTime;
+ MACHTTRANSMIT_SETTING TxRate;
+} RT_802_11_MAC_ENTRY, *PRT_802_11_MAC_ENTRY;
+
+typedef struct _RT_802_11_MAC_TABLE {
+ unsigned long Num;
+ RT_802_11_MAC_ENTRY Entry[32];
+} RT_802_11_MAC_TABLE, *PRT_802_11_MAC_TABLE;
+
+#endif /* MT7601_IO_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/p1905_ap_autoconfig.h b/cleopatre/devkit/p1905_managerd/inc/p1905_ap_autoconfig.h
new file mode 100644
index 0000000000..91a9c6e077
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/p1905_ap_autoconfig.h
@@ -0,0 +1,36 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/p1905_ap_autoconfig.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#ifndef P1905_AP_AUTOCONFIG_H
+#define P1905_AP_AUTOCONFIG_H
+
+#ifdef SUPPORT_AP_ENROLLE
+typedef enum
+{
+ no_ap_autoconfig = 1,
+ wait_4_send_ap_autoconfig_search,
+ wait_4_recv_ap_autoconfig_resp,
+ wait_4_send_m1,
+ wait_4_recv_m2,
+ wait_4_set_config,
+
+} enrolle_config_stage;
+#endif
+
+typedef struct _ap_config_para
+{
+ unsigned char e_nonce[16];
+ unsigned char r_nonce[16];
+ unsigned char private_key[192];
+ unsigned char public_key[192];
+ unsigned char security_key[192];
+ unsigned char peer_public_key[192];
+ unsigned char enrolle_mac[6];
+
+ unsigned char authKey[32];
+ unsigned char keyWrapKey[16];
+} ap_config_para;
+
+#endif /* P1905_AP_AUTOCONFIG_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/p1905_database.h b/cleopatre/devkit/p1905_managerd/inc/p1905_database.h
new file mode 100644
index 0000000000..0d4ac47adf
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/p1905_database.h
@@ -0,0 +1,144 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/p1905_database.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#ifndef P1905_DATABASE_H
+#define P1905_DATABASE_H
+
+#include <sys/queue.h>
+
+#if defined(SUPPORT_WIFI)//ETH+PLC+WIFI
+#define ITF_NUM 3
+#else//PLC+ETH
+#define ITF_NUM 2
+#endif
+
+#define BRIDGE_NUM 1
+
+/*define vendor specific data structure*/
+struct p1905_vs_info
+{
+ unsigned char al_mac[ETH_ALEN];
+};
+
+typedef struct
+{
+ unsigned char target[ETH_ALEN];
+ unsigned char type;
+
+}link_metrics_rep;
+
+typedef struct
+{
+ unsigned char target[ETH_ALEN];
+ unsigned char type;
+
+}link_metrics_query;
+
+/*below is the data structure of local information*/
+struct p1905_interface
+{
+ unsigned char mac_addr[ETH_ALEN];
+ unsigned short media_type;
+ unsigned char *vs_info;
+ unsigned char vs_info_length;
+};
+
+struct bridge_capabiltiy
+{
+ unsigned char interface_num;
+ unsigned char *itf_mac_list;
+};
+
+struct non_p1905_neighbor_info
+{
+ unsigned char itf_mac_addr[ETH_ALEN];
+ LIST_ENTRY(non_p1905_neighbor_info) non_p1905nbr_entry;
+};
+
+struct non_p1905_neighbor
+{
+ unsigned char local_mac_addr[ETH_ALEN];
+ unsigned short br_port_no;
+ struct non_p1905_neighbor_info info;
+ LIST_HEAD(list_head_nonp1905nbr, non_p1905_neighbor_info) non_p1905nbr_head;
+};
+
+struct p1905_neighbor_info
+{
+ unsigned char al_mac_addr[ETH_ALEN];
+ unsigned char ieee802_1_bridge;
+ LIST_ENTRY(p1905_neighbor_info) p1905nbr_entry;
+};
+
+struct p1905_neighbor
+{
+ unsigned char local_mac_addr[ETH_ALEN];
+ struct p1905_neighbor_info info;
+ LIST_HEAD(list_head_p1905nbr, p1905_neighbor_info) p1905nbr_head;
+};
+
+/*below is data structure for received topology message*/
+struct topology_discovery_db
+{
+ unsigned char al_mac[6];
+ unsigned char itf_mac[6];
+ unsigned char receive_itf_mac[6];
+ int time_to_live;
+ LIST_ENTRY(topology_discovery_db) tpddb_entry;
+};
+
+struct non_p1905_neighbor_device_list_db
+{
+ unsigned char non_p1905_device_interface_mac[6];
+ SLIST_ENTRY(non_p1905_neighbor_device_list_db) non_p1905_nbrdb_entry;
+};
+
+struct p1905_neighbor_device_db
+{
+ unsigned char p1905_neighbor_al_mac[6];
+ unsigned char ieee_802_1_bridge_exist;
+ SLIST_ENTRY(p1905_neighbor_device_db) p1905_nbrdb_entry;
+};
+
+struct device_info_db
+{
+ unsigned char mac_addr[6];
+ unsigned short media_type;
+ unsigned char vs_info_len;
+ unsigned char *vs_info;
+ struct p1905_neighbor_device_db p1905_dev;
+ struct non_p1905_neighbor_device_list_db non_p1905_dev;
+ SLIST_ENTRY(device_info_db) devinfo_entry;
+ SLIST_HEAD(list_head_p1905nbrdb, p1905_neighbor_device_db) p1905_nbrdb_head;
+ SLIST_HEAD(list_head_nonp1905nbrdb, non_p1905_neighbor_device_list_db) non_p1905_nbrdb_head;
+};
+
+struct device_bridge_capability_db
+{
+ unsigned char interface_amount;
+ unsigned char *interface_mac_tuple;
+ LIST_ENTRY(device_bridge_capability_db) brcap_entry;
+};
+
+struct topology_response_db
+{
+ unsigned char al_mac_addr[6];
+ struct device_info_db dev_info_entry;
+ struct device_bridge_capability_db bridge_capability_entry;
+ SLIST_ENTRY(topology_response_db) tprdb_entry;
+ SLIST_HEAD(list_head_devinfo, device_info_db) devinfo_head;
+ LIST_HEAD(list_head_brcap, device_bridge_capability_db) brcap_head;
+};
+
+/*data structure of topology database*/
+struct p1905_topology_db
+{
+ struct topology_discovery_db dcv_entry;
+ struct topology_response_db drp_entry;
+ SLIST_HEAD(list_head_tprdb, topology_response_db) tprdb_head;
+ LIST_HEAD(list_head_tpddb, topology_discovery_db)tpddb_head;
+};
+
+#endif /*P1905_DATABASE_H*/
diff --git a/cleopatre/devkit/p1905_managerd/inc/p1905_managerd.h b/cleopatre/devkit/p1905_managerd/inc/p1905_managerd.h
new file mode 100644
index 0000000000..4cc243b329
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/p1905_managerd.h
@@ -0,0 +1,151 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/p1905_managerd.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#ifndef P1905_MANAGERD_H
+#define P1905_MANAGERD_H
+
+#include <linux/if_packet.h>
+#include <netinet/in.h>
+#include <net/if.h>
+#include <linux/if_ether.h>
+#include "p1905_database.h"
+#include "../../linux/gpio.h"
+#ifdef SUPPORT_WIFI
+#include "wifi_utils.h"
+#ifdef SUPPORT_AP_AUTO_CONFIG
+#include "wsc_attr_tlv.h"
+#include "p1905_ap_autoconfig.h"
+#endif
+#endif
+
+#ifdef SUPPORT_ALME
+#include "libalme.h"
+#include "alme.h"
+#endif
+
+/** Define packet max length */
+#define MAX_PKT_LEN 1524
+
+/*define time_to_live of topology discovery database*/
+#define TOPOLOGY_DISCOVERY_TTL 70 //70 seconds
+
+/*for vendor specific*/
+#define OUI_SPIDCOM "\x00\x13\xd7"
+#define OUI_LEN 3
+
+/* for push button join notify, we must record the current stations
+ * information to compare. although in PLC there can be 256 stations, but
+ * we use 32 to reduce memory usage.
+ */
+#define MAX_STATIONS 32
+
+/* HPAV station information, we use this structure to maintain
+ * the information gotton from MME(CC_DISCOVER_LIST_CNF)
+ */
+typedef struct
+{
+ unsigned char station_num;
+ unsigned char station_mac_list[MAX_STATIONS][ETH_ALEN];
+ unsigned char new_station[ETH_ALEN];
+} hpav_info;
+
+#ifdef SUPPORT_WIFI
+typedef struct
+{
+ unsigned char no_need_start_pbc;
+ unsigned char station_num;
+ unsigned char station_mac_list[WIFI_MAX_STATION_NUM][ETH_ALEN];
+ unsigned char new_station[ETH_ALEN];
+
+} iee802_11_info;
+#endif
+
+/* for 1905.1 push button join notification use. */
+typedef struct
+{
+ /*store the al id from push button event notification*/
+ unsigned char al_id[ETH_ALEN];
+ /*strore the message id from push button event notification*/
+ unsigned short mid;
+ /*is_sc: 1 ==> simple connect on-going*/
+ unsigned char is_sc;
+ /*store the information of PLC*/
+ hpav_info info;
+
+#ifdef SUPPORT_WIFI
+ /*store the information of WIFI*/
+ iee802_11_info wifi_info;
+#endif
+} push_button_param;
+
+/** Context structure */
+struct p1905_managerd_ctx {
+ int sock_br0; /* br0 socket */
+ int sock_lldp; /* lldp socket*/
+ /*push button use*/
+ int gpio_fd;
+
+ struct sockaddr_ll br0_sll;
+ struct sockaddr_ll lldp_sll;
+
+ uint8_t p1905_al_mac_addr[ETH_ALEN];
+ uint8_t br0_mac_addr[ETH_ALEN];
+ uint8_t plc0_mac_addr[ETH_ALEN];
+ uint8_t eth0_mac_addr[ETH_ALEN];
+#ifdef SUPPORT_WIFI
+ uint8_t wifi0_mac_addr[ETH_ALEN];
+#ifdef SUPPORT_AP_AUTO_CONFIG
+ ap_config_para *ap_config;
+ unsigned char uuid[16];
+#ifdef SUPPORT_AP_REGISTRAR
+ unsigned char *last_rx_data;
+ unsigned short last_rx_length;
+#endif
+#ifdef SUPPORT_AP_ENROLLE
+ unsigned char *last_tx_data;
+ unsigned short last_tx_length;
+ unsigned char *current_rx_data;
+ unsigned short current_rx_length;
+ unsigned char is_authenticator_exist_in_M2;
+ unsigned char is_in_encrypt_settings;
+ unsigned char get_config_attr_kind;
+ enrolle_config_stage enrolle_state;
+ unsigned short autoconfig_search_mid;
+ WSC_CONFIG *ap_config_data;
+ unsigned char ap_config_timeout_cnt;
+#endif
+ unsigned char need_ap_config;
+#endif
+#endif
+
+#ifdef SUPPORT_ALME
+ int sock_alme;
+ int almefd;
+ unsigned short link_metric_query_mid;
+ ALME_STATE alme_state;
+ unsigned char alme_wait_4_link_metrics_cnt;
+ int semid;
+#endif
+
+ /*push button use*/
+ union gpio_info sc_it;
+ push_button_param pbc_param;
+
+ unsigned short mid;
+ unsigned char need_relay;
+
+ /*for link metric query/response use*/
+ link_metrics_query link_metric_query_para;
+ link_metrics_rep link_metric_response_para;
+
+ struct p1905_interface itf[ITF_NUM];
+ struct bridge_capabiltiy br_cap[BRIDGE_NUM];
+ struct non_p1905_neighbor non_p1905_neighbor_dev[ITF_NUM];
+ struct p1905_neighbor p1905_neighbor_dev[ITF_NUM];
+
+ struct p1905_topology_db topology_entry;
+};
+
+#endif /* P1905_MANAGERD_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/p1905_utils.h b/cleopatre/devkit/p1905_managerd/inc/p1905_utils.h
new file mode 100644
index 0000000000..fc80d4da47
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/p1905_utils.h
@@ -0,0 +1,38 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/p1905_utils.h
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#ifndef P1905_UTILS_H
+#define P1905_UTILS_H
+
+#include "p1905_managerd.h"
+
+/** Interfaces name */
+#define BR0_IFNAME "br0"
+#define ETH0 "eth0"
+#define PLC0 "plc0"
+
+/*tx/rx link statistics*/
+typedef struct
+{
+ unsigned int total_tx_packets;
+ unsigned int error_tx_packets;
+ unsigned int total_rx_packets;
+ unsigned int error_rx_packets;
+} link_stat;
+
+/*thread ctx*/
+typedef struct
+{
+ unsigned short mmtype;
+ unsigned char *local_itf;
+ unsigned char *recv_buf;
+} thread_ctx;
+
+int get_receive_port_addr(unsigned char *inputmac,unsigned char *outputmac);
+int set_bridge_not_forward_dest(unsigned char *mac);
+void dump_topology_info(struct p1905_managerd_ctx *ctx);
+
+
+#endif /* P1905_UTILS_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/plc_utils.h b/cleopatre/devkit/p1905_managerd/inc/plc_utils.h
new file mode 100644
index 0000000000..9e50997446
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/plc_utils.h
@@ -0,0 +1,34 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/plc_utils.h
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ */
+#ifndef PLC_UTILS_H
+#define PLC_UTILS_H
+
+#include "libmme.h"
+
+
+typedef enum
+{
+ plc_utils_success,
+ plc_utils_fail,
+ plc_utils_no_new_station,
+
+} PLC_UTILS_STATUS;
+
+typedef struct _PLC_MME_PARAM
+{
+ unsigned short mmtype;
+ mme_send_type_t sndtype;
+ unsigned char dst[6];
+ unsigned int sndlen;
+ unsigned int rcvlen;
+
+ unsigned char sndbuf[2048];
+ unsigned char rcvbuf[2048];
+
+} PLC_MME_PARAM;
+
+
+#endif /* PLC_UTILS_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/rt5572_io.h b/cleopatre/devkit/p1905_managerd/inc/rt5572_io.h
new file mode 100644
index 0000000000..1ee62e2920
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/rt5572_io.h
@@ -0,0 +1,36 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/rt5572_io.h
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ *
+ */
+#ifndef RT5572_IO_H
+#define RT5572_IO_H
+
+/*define the ioctl index for rt5572*/
+#define RTPRIV_IOCTL_SET (SIOCIWFIRSTPRIV + 0x02)
+#define RTPRIV_IOCTL_WSC_PROFILE (SIOCIWFIRSTPRIV + 0x12)
+#define RTPRIV_IOCTL_GET_DH_PUB_KEY (SIOCIWFIRSTPRIV + 0x1B)
+#define RTPRIV_IOCTL_GET_DH_SECU_KEY (SIOCIWFIRSTPRIV + 0x1C)
+#define RTPRIV_IOCTL_GET_AUTH_KEYWRAP_KEY (SIOCIWFIRSTPRIV + 0x1D)
+#define RTPRIV_IOCTL_GET_KWA_VALUE (SIOCIWFIRSTPRIV + 0x1E)
+#define RTPRIV_IOCTL_GET_AES_ENCRYPT_VALUE (SIOCIWFIRSTPRIV + 0x14)
+#define RTPRIV_IOCTL_GET_AES_DECRYPT_VALUE (SIOCIWFIRSTPRIV + 0x15)
+
+typedef struct _rt5572_io_table
+{
+ unsigned char *data;
+ unsigned short length;
+ unsigned short flag;
+} rt5572_io_table;
+
+typedef struct __attribute__ ((packed)) _WSC_CONFIGURED_VALUE {
+ unsigned short WscConfigured;
+ unsigned char WscSsid[32 + 1];
+ unsigned short WscAuthMode;
+ unsigned short WscEncrypType;
+ unsigned char DefaultKeyIdx;
+ unsigned char WscWPAKey[64 + 1];
+} WSC_CONFIGURED_VALUE;
+
+#endif /* RT5572_IO_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/wifi_utils.h b/cleopatre/devkit/p1905_managerd/inc/wifi_utils.h
new file mode 100644
index 0000000000..45afc429db
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/wifi_utils.h
@@ -0,0 +1,96 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/wifi_utils.h
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ */
+#ifndef WIFI_UTILS_H
+#define WIFI_UTILS_H
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+/*WSC Encryption type, defined in wsc 2.0.2 p.114*/
+#define ENCRYP_NONE 0x0001
+#define ENCRYP_WEP 0x0002
+#define ENCRYP_TKIP 0x0004
+#define ENCRYP_AES 0x0008
+
+/*WSC Authentication type, defined in wsc 2.0.2 p.105*/
+#define AUTH_OPEN 0x0001
+#define AUTH_WPA_PERSONAL 0x0002
+#define AUTH_SHARED 0x0004
+#define AUTH_WPA_ENTERPRISE 0x0008
+#define AUTH_WPA2_ENTERPRISE 0x0010
+#define AUTH_WPA2_PERSONAL 0x0020
+
+/*WSC connection type*/
+#define ESS 0x1
+#define IBSS 0x2
+
+/*WSC configured status*/
+#define AP_NOT_CONFIGURED 0x1
+#define AP_ALREADY_CONFIGURED 0x2
+#endif
+
+#define WIFI_MAX_STATION_NUM 16
+
+typedef enum
+{
+ wifi_utils_success = 0,
+ wifi_utils_error,
+
+
+
+} WIFI_UTILS_STATUS;
+
+typedef enum
+{
+ rf_band_2p4G = 1,
+ rf_band_5G,
+} RF_BAND;
+
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+typedef struct _DH_TABLE
+{
+ unsigned char pub_key[192];
+ unsigned char priv_key[192];
+ unsigned char secu_key[192];
+} DH_TABLE, *PDH_TABLE;
+
+typedef struct _KDK_KDF_TABLE {
+ unsigned char E_Nonce[16];
+ unsigned char R_Nonce[16];
+ unsigned char E_Mac_Addr[6];
+
+ unsigned char AuthKey[32];
+ unsigned char KeyWrapKey[16];
+ unsigned char Emsk[32];
+
+ unsigned char DH_Secu_Key[192];
+} KDK_KDF_TABLE, *PKDK_KDF_TABLE;
+
+typedef struct _KWA_TABLE {
+ unsigned char AuthKey[32];
+ unsigned char *EncrptData;
+ unsigned int EncrptDataLen;
+ unsigned char KWA[8];
+} KWA_TABLE, *PKWA_TABLE;
+
+typedef struct _AES_TABLE {
+ unsigned char *PlainText;
+ unsigned int PlainTextLen;
+ unsigned char IV[16];
+ unsigned char *CipherText;
+ unsigned int CipherTextLen;
+
+ unsigned char KeyWrapKey[16];
+} AES_TABLE, *PAES_TABLE;
+
+typedef struct _WSC_CONFIG{
+ unsigned char Ssid[32 + 1];
+ unsigned short AuthMode;
+ unsigned short EncrypType;
+ unsigned char WPAKey[64 + 1];
+} WSC_CONFIG, *PWSC_CONFIG;
+#endif
+
+#endif /* WIFI_UTILS_H */
diff --git a/cleopatre/devkit/p1905_managerd/inc/wsc_attr_tlv.h b/cleopatre/devkit/p1905_managerd/inc/wsc_attr_tlv.h
new file mode 100644
index 0000000000..d9435df901
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/inc/wsc_attr_tlv.h
@@ -0,0 +1,100 @@
+/*
+ * cleopatre/application/p1905_managerd/inc/wsc_attr_tlv.h
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ *
+ */
+#ifndef WSC_ATTR_TLV_H
+#define WSC_ATTR_TLV_H
+
+#include "linux/types.h"
+#include "p1905_managerd.h"
+
+#define ATTR_TYPE_FIELD_LENGTH 2
+#define ATTR_LENGTH_FIELD_LENGTH 2
+#define ATTR_HEADR_LENGTH (ATTR_TYPE_FIELD_LENGTH + ATTR_LENGTH_FIELD_LENGTH)
+
+/*define wsc attribute ID*/
+#define ATTR_ASSOCIATION_STATE_ID 0x1002
+#define ATTR_AUTHENTICATION_TYPE_ID 0x1003
+#define ATTR_AUTH_TYPE_FLAG_ID 0x1004
+#define ATTR_AUTHENTICATOR_ID 0x1005
+#define ATTR_CONFIG_METHOD_ID 0x1008
+#define ATTR_CONFIG_ERROR_ID 0x1009
+#define ATTR_CONNECTION_TYPE_FLAG_ID 0x100D
+#define ATTR_ENCRYPTION_TYPE_ID 0x100F
+#define ATTR_ENCRYPTION_TYPE_FLAG_ID 0x1010
+#define ATTR_DEVICE_NAME_ID 0x1011
+#define ATTR_DEVICE_PWD_ID 0x1012
+#define ATTR_ENCRYPTED_SETTINGS_ID 0x1018
+#define ATTR_ENROLLE_NONCE_ID 0x101A
+#define ATTR_KEY_WRAP_AUTHENTICATOR_ID 0x101E
+#define ATTR_MAC_ADDRESS_ID 0x1020
+#define ATTR_MANUFACTURER_ID 0x1021
+#define ATTR_MESSAGE_TYPE_ID 0x1022
+#define ATTR_MODEL_NAME_ID 0x1023
+#define ATTR_MODEL_NUMBER_ID 0x1024
+#define ATTR_NETWORK_KEY_ID 0x1027
+#define ATTR_OS_VERSION_ID 0x102D
+#define ATTR_PUBLIC_KEY_ID 0x1032
+#define ATTR_REGISTRAR_NONCE_ID 0x1039
+#define ATTR_RF_BAND_ID 0x103C
+#define ATTR_SERIAL_NUMBER_ID 0x1042
+#define ATTR_WSC_STATE_ID 0x1044
+#define ATTR_SSID_ID 0x1045
+#define ATTR_UUID_E_ID 0x1047
+#define ATTR_UUID_R_ID 0x1048
+#define ATTR_VENDOR_EXTENSION_ID 0x1049
+#define ATTR_VERSION_ID 0x104A
+#define ATTR_PRIM_DEV_TYPE_ID 0x1054
+
+/*define wsc attribute length*/
+#define ATTR_VERSION_DATA_LENGTH 1
+#define ATTR_MESSAGE_TYPE_DATA_LENGTH 1
+#define ATTR_MAC_ADDRESS_DATA_LENGTH 6
+#define ATTR_ENROLLE_NONCE_DATA_LENGTH 16
+#define ATTR_REGISTRAR_NONCE_DATA_LENGTH 16
+#define ATTR_PUBLIC_KEY_DATA_LENGTH 192
+#define ATTR_KWA_LENGTH 8
+#define ATTR_AUTHENTICATION_TYPE_DATA_LENGTH 2
+#define ATTR_ENCRYPTION_TYPE_DATA_LENGTH 2
+#define ATTR_UUID_E_DATA_LENGTH 16
+#define ATTR_UUID_R_DATA_LENGTH 16
+#define ATTR_AUTHENTICATOR_DATA_LENGTH 8
+
+/*define message type*/
+#define MESSAGE_TYPE_M1 0x04
+#define MESSAGE_TYPE_M2 0x05
+
+/*define version*/
+#define WSC_VERSION 0x10
+
+typedef enum
+{
+ wsc_attr_success = 0,
+ wsc_attr_error,
+ wsc_attr_not_find,
+ wsc_attr_not_expect,
+ wsc_attr_out_of_order,
+ wsc_attr_not_get_all_tlv,
+
+} WSC_ATTR_STATUS;
+
+typedef struct
+{
+ __be16 type;
+ __be16 length;
+}__attribute__ ((__packed__)) WSC_TLV_HDR;
+
+typedef struct _WSC_ATTR_FUNC
+{
+ unsigned short id;
+ unsigned char M1_order;
+ unsigned char M2_order;
+
+ WSC_ATTR_STATUS (*create) (unsigned char *out, unsigned char *in, unsigned short *length);
+ WSC_ATTR_STATUS (*parse) (unsigned char *in, void *ctx, unsigned short *length);
+
+} WSC_ATTR_FUNC;
+
+#endif /* WSC_ATTR_TLV_H */
diff --git a/cleopatre/devkit/p1905_managerd/src/alme.c b/cleopatre/devkit/p1905_managerd/src/alme.c
new file mode 100644
index 0000000000..1c446c1fa4
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/alme.c
@@ -0,0 +1,367 @@
+/*
+ * cleopatre/application/p1905_managerd/src/alme.c
+ *
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <syslog.h>
+#include <errno.h>
+#include <assert.h>
+#include <fcntl.h>
+
+#include "p1905_managerd.h"
+#include "cmdu_tlv.h"
+#include "cmdu_message.h"
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#include <sys/ipc.h>
+#include <sys/types.h>
+#include <sys/sem.h>
+
+//#define ALME_DEBUG
+#ifdef ALME_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
+
+typedef union semun
+{
+ int val;
+ struct semid_ds *buf;
+ unsigned short *array;
+}semun_t;
+
+static void parse_alme_get_metric_req(struct p1905_managerd_ctx *ctx, ALME_MSG *msg)
+{
+ ALME_GET_METRIC_REQ *req;
+#ifdef SUPPORT_WIFI
+ unsigned char sta_itf[6];
+#endif
+ req = (ALME_GET_METRIC_REQ *)msg->body;
+ //memcpy(ctx->link_metric_query_para.target, req->mac_addr, 6);
+ memcpy(ctx->link_metric_query_para.target, ctx->p1905_al_mac_addr, 6);
+
+ /*ALME does not define which type we should use, so I profile both RX/TX*/
+ ctx->link_metric_query_para.type = BOTH_TX_AND_RX_METRICS;
+
+ ctx->mid++;//message id increment
+ /*record message id, need to compare with the mid value from link metric response*/
+ ctx->link_metric_query_mid = ctx->mid;
+ /*query the link metrics of neighbor, send link metric query*/
+
+#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, req->mac_addr))
+ {
+ debug_syslog("3. 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, ctx->mid);
+ }
+ else
+#endif
+ insert_cmdu_txq(req->mac_addr, ctx->p1905_al_mac_addr,\
+ e_link_metric_query, ctx->mid);
+
+ /*set alme state to alme_wait_4_link_metrics_rsp*/
+ ctx->alme_state = alme_wait_4_link_metrics_rsp;
+}
+
+int send_alme_get_metric_rsp(int fd, ALME_MSG *msg, ALME_REASON_CODE code)
+{
+ int len;
+
+ ALME_GET_METRIC_RSP *rsp;
+
+ msg->header.type = alme_get_metric_rsp;
+ msg->header.length = sizeof(ALME_GET_METRIC_RSP);
+
+ rsp = (ALME_GET_METRIC_RSP *)msg->body;
+ rsp->reason_code = code;
+
+ len = send(fd, msg, sizeof(ALME_MSG), 0);
+ if(len < 0)
+ {
+ debug_syslog("send ALME_GET_METRIC_RSP error(%s) reason %d\n",strerror(errno), code);
+ }
+ return len;
+}
+
+int wait_4_link_metric_rsp_timeout(int fd)
+{
+ ALME_MSG msg;
+ int len;
+
+ len = send_alme_get_metric_rsp(fd, &msg, alme_failure);
+ return len;
+}
+
+int send_alme_get_intf_list_rsp(struct p1905_managerd_ctx *ctx)
+{
+ int len, i;
+ ALME_MSG msg;
+ ALME_GET_INTF_LIST_RSP *rsp;
+ unsigned intf_num = 0;
+ struct p1905_neighbor_info *dev_info;
+
+
+ msg.header.type = alme_get_intf_list_rsp;
+ msg.header.length = sizeof(ALME_GET_INTF_LIST_RSP);
+
+ rsp = (ALME_GET_INTF_LIST_RSP *)msg.body;
+ rsp->intfnum = 0;
+
+ intf_num = ITF_NUM;
+
+ if(ITF_NUM > MAX_1905_LINK_NUM)
+ {
+ debug_syslog("ALME_GET_INTF_LIST_RSP too mush interface, %d\n", ITF_NUM);
+ intf_num = MAX_1905_LINK_NUM;
+ }
+
+ for(i=0;i<intf_num;i++)
+ {
+ memcpy(rsp->intflist[i].mac_addr, ctx->itf[i].mac_addr, 6);
+ rsp->intflist[i].intf_type = ctx->itf[i].media_type;
+ if(!LIST_EMPTY(&(ctx->p1905_neighbor_dev[i].p1905nbr_head)))
+ {
+ LIST_FOREACH(dev_info, &(ctx->p1905_neighbor_dev[i].p1905nbr_head),
+ p1905nbr_entry)
+ {
+ if(dev_info->ieee802_1_bridge == 1)
+ {
+ rsp->intflist[i].ieee802_1_bridge = 1;
+ break;
+ }
+ }
+ }
+ else
+ rsp->intflist[i].ieee802_1_bridge = 0;
+
+ rsp->intfnum++;
+ }
+
+ len = send(ctx->almefd, &msg, sizeof(ALME_MSG), 0);
+ if(len < 0)
+ {
+ debug_syslog("send ALME_GET_INTF_LIST_RSP error(%s)\n",strerror(errno));
+ }
+ return len;
+}
+
+int send_alme_get_1905_neighbor_rsp(struct p1905_managerd_ctx *ctx)
+{
+ int len;
+ ALME_MSG msg;
+ ALME_GET_1905_NEIGHBOR_RSP *rsp;
+ struct topology_discovery_db *tpg_db;
+ int i = 0;
+
+ msg.header.type = alme_get_1905_neighbor_rsp;
+ msg.header.length = sizeof(ALME_GET_1905_NEIGHBOR_RSP);
+
+ rsp = (ALME_GET_1905_NEIGHBOR_RSP *)msg.body;
+ rsp->neighbornum = 0;
+
+ /*query the topology discovery database to get 1905.1 neighbor*/
+ if(!LIST_EMPTY(&(ctx->topology_entry.tpddb_head)))
+ {
+ i = 0;
+ LIST_FOREACH(tpg_db, &(ctx->topology_entry.tpddb_head), tpddb_entry)
+ {
+ if(rsp->neighbornum == MAX_1905_NEIGHBOR_NUM)
+ break;
+
+ memcpy(&(rsp->neighborlist[i*6]), tpg_db->al_mac, 6);
+ rsp->neighbornum += 1;
+ i++;
+ }
+ }
+
+ len = send(ctx->almefd, &msg, sizeof(ALME_MSG), 0);
+ if(len < 0)
+ {
+ debug_syslog("send ALME_GET_1905_NEIGHBOR_RSP error(%s)\n",strerror(errno));
+ }
+ return len;
+}
+
+int ALME_init(struct p1905_managerd_ctx *ctx)
+{
+ struct sockaddr_un sock_addr;
+ int status = 0;
+
+ ctx->sock_alme = socket(AF_UNIX, SOCK_STREAM , 0);
+ if(ctx->sock_alme < 0)
+ {
+ debug_syslog("open ALME socket error\n");
+ return -1;
+ }
+
+ memset(&sock_addr, 0, sizeof(sock_addr));
+ sock_addr.sun_family = AF_UNIX;
+ strcpy(sock_addr.sun_path, "/tmp/alme_server");
+ status = bind(ctx->sock_alme, (struct sockaddr *)&sock_addr, sizeof(sock_addr));
+ if(status < 0)
+ {
+ close(ctx->sock_alme);
+ debug_syslog("bind ALME fail(%s)\n",strerror(errno));
+ return -1;
+ }
+
+ if(listen(ctx->sock_alme, 1) < 0)
+ {
+ close(ctx->sock_alme);
+ debug_syslog("listen ALME fail(%s)\n",strerror(errno));
+ return -1;
+ }
+
+ ctx->alme_state = alme_none;
+ ctx->almefd = -1;
+ ctx->alme_wait_4_link_metrics_cnt = 0;
+
+ return 0;
+}
+
+int ALME_sync_init(struct p1905_managerd_ctx *ctx)
+{
+ semun_t arg;
+ key_t key;
+
+ key = ftok("/tmp/alme_server",'a');
+ if(key == -1)
+ {
+ debug_syslog("create key for semaphore fail(%s)\n",strerror(errno));
+ close(ctx->sock_alme);
+ return -1;
+ }
+
+ /*create synchroniztion mechanism for libalme use*/
+ ctx->semid = semget(key, 1, 0666|IPC_CREAT);
+ if(ctx->semid < 0)
+ {
+ debug_syslog("create semaphore fail(%s)\n",strerror(errno));
+ close(ctx->sock_alme);
+ return -1;
+ }
+
+ arg.val = 1;
+ if (semctl(ctx->semid, 0, SETVAL, arg) == -1)
+ {
+ debug_syslog("init semaphore fail(%s)\n",strerror(errno));
+ close(ctx->sock_alme);
+
+ if(semctl(ctx->semid, 0, IPC_RMID, arg) == -1)
+ {
+ debug_syslog("de-allocate semaphore fail(%s)\n",strerror(errno));
+ return -1;
+ }
+ return -1;
+ }
+
+ return 0;
+}
+
+int ALME_uninit(struct p1905_managerd_ctx *ctx)
+{
+ close(ctx->sock_alme);
+
+ if(ctx->almefd > 0)
+ {
+ close(ctx->almefd);
+ ctx->almefd = -1;
+ }
+}
+
+int ALME_sync_uninit(struct p1905_managerd_ctx *ctx)
+{
+ semun_t arg;
+
+ if(semctl(ctx->semid, 0, IPC_RMID, arg) == -1)
+ {
+ debug_syslog("de-allocate semaphore fail(%s)\n",strerror(errno));
+ return -1;
+ }
+ return 0;
+}
+
+int alme_receive(struct p1905_managerd_ctx *ctx, unsigned char *buf, int len)
+{
+ struct sockaddr_un from;
+ int lenfrom = sizeof(from);
+
+ ctx->almefd = accept(ctx->sock_alme, (struct sockaddr *)&from, &lenfrom);
+ if(ctx->almefd < 0)
+ {
+ debug_syslog("accept alme fail\n");
+ return -1;
+ }
+
+ len = recv(ctx->almefd, buf, len, 0);
+ if(0 > len)
+ {
+ debug_syslog("recv alme fail\n");
+
+ if(ctx->almefd > 0)
+ {
+ close(ctx->almefd);
+ ctx->almefd = -1;
+ }
+ return -1;
+ }
+
+ /* we don't close ctx->almefd here because we may need this file descriptor
+ * to feedback alme message to HLE
+ */
+ //close(ctx->almefd);
+ return len;
+}
+
+int alme_process(struct p1905_managerd_ctx *ctx, unsigned char *buf, int len)
+{
+ ALME_MSG *msg;
+
+ msg = (ALME_MSG *)buf;
+
+ switch(msg->header.type)
+ {
+ case alme_get_metric_req:
+ parse_alme_get_metric_req(ctx, msg);
+ break;
+ case alme_get_intf_list_req:
+ send_alme_get_intf_list_rsp(ctx);
+ break;
+
+ /*below is MStar private*/
+ case alme_get_1905_neighbor_req:
+ send_alme_get_1905_neighbor_rsp(ctx);
+ break;
+ default:
+ break;
+ }
+
+
+ /* for some alme message, we can free the server and client connection.
+ * for alme_get_metric_req, we need to wait for receiving the link metrics response and then free connection.
+ * for .........
+ */
+ if(msg->header.type != alme_get_metric_req)
+ {
+ if(ctx->almefd > 0)
+ {
+ close(ctx->almefd);
+ ctx->almefd = -1;
+ }
+ }
+
+ return 0;
+}
+
+
diff --git a/cleopatre/devkit/p1905_managerd/src/cmdu.c b/cleopatre/devkit/p1905_managerd/src/cmdu.c
new file mode 100644
index 0000000000..e923aa9c70
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/cmdu.c
@@ -0,0 +1,689 @@
+/*
+ * cleopatre/application/p1905_managerd/src/cmdu.c
+ */
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <sys/socket.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.h"
+
+//#define CMDU_DEBUG
+#ifdef CMDU_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
+
+
+TAILQ_HEAD(list_head_txq, txq_list) cmdu_txq_head;
+
+static void delete_cmdu_txq_all(void);
+static int cmdu_tx_fragment(struct p1905_managerd_ctx *ctx, unsigned char *tlv,
+ cmdu_message_header* msg_hdr, unsigned char *buffer);
+static int cmdu_bridge_send(struct p1905_managerd_ctx *ctx, unsigned char *dmac,
+ unsigned char *smac, msgtype mtype, unsigned short mid,
+ unsigned char *buffer);
+static int cmdu_bridge_relay(struct p1905_managerd_ctx *ctx,
+ unsigned char *buf, int len);
+static int cmdu_bridge_relay_send(struct p1905_managerd_ctx *ctx,
+ unsigned char *smac, unsigned char *buf, int len);
+
+/**
+ * Set ieee1905.1 abstration layer mac address.
+ *
+ * \param ctx p1905_managerd_ctx context.
+ *
+ */
+static void set_al_mac_addr(struct p1905_managerd_ctx *ctx)
+{
+ unsigned char al_mac[ETH_ALEN] = {0};
+
+ /* we use PLC MAC ADDR as a reference for 1905.1 abstraction layer mac
+ * addr. i.e. if PLC MAC is 00:13:d7:00:01:33, the abstraction layer mac
+ * is 00:13:d7:01:f0:33
+ */
+ memcpy(al_mac, ctx->plc0_mac_addr, ETH_ALEN);
+ al_mac[4] = al_mac[4] | 0xf0;
+
+ memcpy(ctx->p1905_al_mac_addr, al_mac, ETH_ALEN);
+}
+
+/**
+ * create ethernet socket to transmit/receive 1905.1 message.
+ *
+ * \param ctx p1905_managerd_ctx context.
+ * \return error code.
+ */
+static int ether_init(struct p1905_managerd_ctx *ctx)
+{
+ int sock_eth0;
+ struct ifreq ifr;
+
+ if(0 > (sock_eth0 = socket(AF_PACKET, SOCK_RAW, ETH_P_1905)))
+ {
+ syslog(LOG_WARNING, "cannot open socket on %s (%s)", ETH0_IFNAME,
+ strerror(errno));
+ return -1;
+ }
+
+ strncpy(ifr.ifr_name, (char*)ETH0_IFNAME, IFNAMSIZ);
+
+ if(-1 == (ioctl(sock_eth0, SIOCGIFINDEX, &ifr)))
+ {
+ syslog(LOG_WARNING, "cannot get interface %s index (%s)", ETH0_IFNAME,
+ strerror(errno));
+ close(sock_eth0);
+ return -1;
+ }
+
+ /*Find eth0 mac address*/
+ if(-1 == (ioctl(sock_eth0, SIOCGIFHWADDR, &ifr)))
+ {
+ syslog(LOG_WARNING, "cannot get interface %s mac address (%s)",
+ ETH0_IFNAME, strerror(errno));
+ close(sock_eth0);
+ return -1;
+ }
+
+ memcpy(ctx->eth0_mac_addr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
+ close(sock_eth0);
+
+ debug("eth0 mac = 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",ctx->eth0_mac_addr[0],
+ ctx->eth0_mac_addr[1], ctx->eth0_mac_addr[2], ctx->eth0_mac_addr[3],
+ ctx->eth0_mac_addr[4],ctx->eth0_mac_addr[5]);
+
+ return 0;
+}
+
+/**
+ * init cmdu tx queue.
+ *
+ */
+static void cmdu_txq_init(void)
+{
+ TAILQ_INIT(&cmdu_txq_head);
+}
+
+/**
+ * Initialize all parameters for CMDU.
+ *
+ * \param ctx p1905_managerd_ctx context.
+ * \return error code.
+ */
+int cmdu_init(struct p1905_managerd_ctx *ctx)
+{
+ struct sockaddr_ll sll;
+ struct ifreq ifr;
+
+ ctx->mid = 0;
+ ctx->need_relay = 0;
+
+ /*get eth0 mac address*/
+ if(0 > ether_init(ctx))
+ {
+ syslog(LOG_WARNING, "init on %s fail", ETH0_IFNAME);
+ return -1;
+ }
+
+ /*Create a receive connection of CMDU on bridge interface*/
+ if(0 > (ctx->sock_br0 = socket(AF_PACKET, SOCK_RAW, ETH_P_1905)))
+ {
+ syslog(LOG_WARNING, "cannot open socket on %s (%s)", BR_IFNAME,
+ strerror(errno));
+ return -1;
+ }
+
+ /*Prepare br0 socket address*/
+ strncpy(ifr.ifr_name, (char*)BR_IFNAME, IFNAMSIZ);
+ if(-1 == (ioctl(ctx->sock_br0, SIOCGIFINDEX, &ifr)))
+ {
+ syslog(LOG_WARNING, "cannot get interface %s index (%s)", BR_IFNAME,
+ strerror(errno));
+ close(ctx->sock_br0);
+ return -1;
+ }
+
+ ctx->br0_sll.sll_family = AF_PACKET;
+ ctx->br0_sll.sll_ifindex = ifr.ifr_ifindex;
+ ctx->br0_sll.sll_protocol = htons(ETH_P_1905);
+
+ /*Get br0 mac address*/
+ if(-1 == (ioctl(ctx->sock_br0, SIOCGIFHWADDR, &ifr)))
+ {
+ syslog(LOG_WARNING, "cannot get interface %s mac address (%s)",
+ BR_IFNAME, strerror(errno));
+ close(ctx->sock_br0);
+ return -1;
+ }
+ memcpy(ctx->br0_mac_addr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
+
+ /*br0 has same mac addr as plc0, it is implementation defined*/
+ memcpy(ctx->plc0_mac_addr, ifr.ifr_hwaddr.sa_data, ETH_ALEN);
+
+ /*Bind br0 socket to this interface*/
+ if(-1 == (bind(ctx->sock_br0, (struct sockaddr *)&ctx->br0_sll,
+ sizeof(struct sockaddr_ll))))
+ {
+ syslog(LOG_WARNING, "cannot bind raw socket to interface %s (%s)",
+ BR_IFNAME, strerror(errno));
+ close(ctx->sock_br0);
+ return -1;
+ }
+
+ /* set p1905.1 abstract layer mac address*/
+ set_al_mac_addr(ctx);
+
+ /* Let bridge do not forward if dest address is local AL MAC address.
+ * It is for 1905.1 unicast message usage.
+ */
+ if(0>set_bridge_not_forward_dest(ctx->p1905_al_mac_addr))
+ {
+ return -1;
+ }
+
+ /*initialize the cmdu transmit queue */
+ cmdu_txq_init();
+
+ /* initialize the cmdu fragment queue. It will queue fragment cmdu until
+ * all fragmenets received
+ */
+ init_fragment_queue();
+
+ return 0;
+}
+
+/**
+ * uninit cmdu, it will be called when exit this process.
+ *
+ * \param ctx p1905_managerd_ctx context.
+ */
+void cmdu_uninit(struct p1905_managerd_ctx *ctx)
+{
+ /*Close br0 connections*/
+ close(ctx->sock_br0);
+
+ /*delete all message queue in TX queue*/
+ delete_cmdu_txq_all();
+
+ /*delete all fragment message queue in fragment queue*/
+ uninit_fragment_queue();
+}
+/**
+ * get the cmdu message in tx queue and send to bridge.
+ *
+ * \param ctx p1905_managerd_ctx context.
+ * \param buffer tx buffer.
+ * \return error code.
+ */
+int process_cmdu_txq(struct p1905_managerd_ctx *ctx, unsigned char *buffer)
+{
+ struct txq_list *tlist;
+ int result = 0;
+
+ if(TAILQ_EMPTY(&cmdu_txq_head))
+ {
+ debug("txq empty\n");
+ return 0;
+ }
+
+ TAILQ_FOREACH(tlist, &cmdu_txq_head, cmdu_txq_entry)
+ {
+ debug("txq send:\n");
+ debug("dst %.2x %.2x %.2x %.2x %.2x %.2x\n",tlist->dmac[0],
+ tlist->dmac[1], tlist->dmac[2], tlist->dmac[3], tlist->dmac[4],
+ tlist->dmac[5]);
+ debug("src %.2x %.2x %.2x %.2x %.2x %.2x\n",tlist->smac[0],
+ tlist->smac[1], tlist->smac[2], tlist->smac[3], tlist->smac[4],
+ tlist->smac[5]);
+ debug("mtype %d\n",tlist->mtype);
+ debug("mid %d\n",tlist->mid);
+
+ result = cmdu_bridge_send(ctx, tlist->dmac, tlist->smac,
+ tlist->mtype, tlist->mid, buffer);
+ if(0 > result)
+ {
+ debug("txq send fail\n");
+ return -1;
+ }
+ }
+ delete_cmdu_txq_all();
+ return 0;
+}
+
+/**
+ * insert cmdu message into tx queue.
+ *
+ * \param dmac destination mac address.
+ * \param smac source mac addr, it awii affect the forwarding of bridge.
+ * \param mtype cmdu message type, defined in 1905.1 spec.
+ * \param mid message id, defined in 1905.1 spec
+ */
+void insert_cmdu_txq(unsigned char *dmac, unsigned char *smac, msgtype mtype,
+ unsigned short mid)
+{
+ struct txq_list *tlist;
+
+ tlist = (struct txq_list*)malloc(sizeof(struct txq_list));
+ memcpy(tlist->dmac, dmac, 6);
+ memcpy(tlist->smac, smac, 6);
+ tlist->mtype = mtype;
+ tlist->mid = mid;
+
+ TAILQ_INSERT_TAIL(&cmdu_txq_head, tlist, cmdu_txq_entry);
+}
+
+/**
+ * delete all cmdu message in tx queue.
+ *
+ */
+static void delete_cmdu_txq_all(void)
+{
+ struct txq_list *tlist, *tlist_temp;
+
+ tlist = TAILQ_FIRST(&cmdu_txq_head);
+ while(tlist)
+ {
+ tlist_temp = TAILQ_NEXT(tlist, cmdu_txq_entry);
+ TAILQ_REMOVE(&cmdu_txq_head, tlist, cmdu_txq_entry);
+ free(tlist);
+ tlist = tlist_temp;
+ }
+}
+
+/**
+ * Fragment tx. Use this funciton when CMDU message size exceed max size.
+ *
+ * \param ctx p1905_managerd_ctx context.
+ * \param tlv pointer of tlv start position.
+ * \param msg_hdr pointer of message header start position.
+ * \param buffer pointer of start position of whole message.
+ * \return error code
+ */
+static int cmdu_tx_fragment(struct p1905_managerd_ctx *ctx, unsigned char *tlv,
+ cmdu_message_header* msg_hdr, unsigned char *buffer)
+{
+ unsigned short offset = 0;
+ unsigned char *tlv_buf;
+ unsigned char *temp_buf;
+ unsigned short tlv_len = 0;
+ int len = 0;
+ int i = 0;
+
+ tlv_buf = get_tlv_buffer();
+ temp_buf = tlv_buf;
+
+ for(;;)
+ {
+ if((*temp_buf) == END_OF_TLV_TYPE)
+ {
+ /* if last fragment is larger than MAX_TLVS_LENGTH,
+ * we need to seperate this fragment into two fragments
+ * the total length of end of tlv is 3
+ */
+ if((offset + 3) > MAX_TLVS_LENGTH)
+ {
+ memcpy(tlv, tlv_buf, offset);
+ msg_hdr->fragment_id = i;
+ msg_hdr->last_fragment_indicator = 0;
+ len = offset + CMDU_HLEN + ETH_HLEN;
+ len = send(ctx->sock_br0, buffer, len, 0);
+ if(0 > len)
+ return -1;
+
+ /*add for fragment id*/
+ i++;
+ /* write end of tlv in to send buffer, no need to padding
+ * padding will be done automatically by driver
+ */
+ memset(tlv, 0, 3);
+ msg_hdr->fragment_id = i;
+ msg_hdr->last_fragment_indicator = 1;
+ len = 3 + CMDU_HLEN + ETH_HLEN;
+ len = send(ctx->sock_br0, buffer, len, 0);
+ if(0 > len)
+ return -1;
+ }
+ else
+ {
+ /* It is the last fragment so we set last fragment ind to 1.
+ * And send this buffer
+ */
+ msg_hdr->fragment_id = i;
+ msg_hdr->last_fragment_indicator = 1;
+
+ len = (offset + 3) + CMDU_HLEN + ETH_HLEN;
+ memcpy(tlv, tlv_buf, (offset + 3));
+ len = send(ctx->sock_br0, buffer, len, 0);
+ if(0 > len)
+ return -1;
+ }
+ return 0;
+ }
+
+ /* below is non-last fragment case.
+ * We should go here first when fragment tx happen.
+ */
+ tlv_len = get_cmdu_tlv_length(temp_buf);
+ if((offset + tlv_len) > MAX_TLVS_LENGTH)
+ {
+
+ memcpy(tlv, tlv_buf, offset);
+
+ /* set fragment id & last fragment ind in cmdu message header.
+ * In this case, last fragment ind must be 0 because no end_of_tlv.
+ */
+ msg_hdr->fragment_id = i;
+ msg_hdr->last_fragment_indicator = 0;
+
+ len = offset + CMDU_HLEN + ETH_HLEN;
+ len = send(ctx->sock_br0, buffer, len, 0);
+ if(0 > len)
+ return -1;
+
+ /*shift to start position of not sent cmdu payload */
+ tlv_buf += offset;
+
+ /* record the tlv_len, which was gotten by get_cmdu_tlv_length
+ * Don't worry about buffer address, because we will shift buffer
+ * to correct position in the end of this for loop.
+ */
+ offset = tlv_len;
+
+ /*for fragment id use*/
+ i++;
+ }
+ else
+ offset += tlv_len;
+
+ /*shift buffer to correct position*/
+ temp_buf += tlv_len;
+ }
+}
+
+/**
+ * create & send cmdu message to bridge.
+ *
+ * \param ctx p1905_managerd_ctx context.
+ * \param dmac destination mac address.
+ * \param smac source mac address.
+ * \param mtype cmdu message type.
+ * \param mid cmdu message id.
+ * \param buffer cmdu buffer for sending.
+ * \return error code
+ */
+static int cmdu_bridge_send(struct p1905_managerd_ctx *ctx, unsigned char *dmac,
+ unsigned char *smac, msgtype mtype, unsigned short mid,
+ unsigned char *buffer)
+{
+ struct ethhdr *eth_hdr;
+ cmdu_message_header *msg_hdr;
+ unsigned char *msg;
+ unsigned short msg_leng = 0;
+ int len;
+ unsigned char *tlv;
+
+ eth_hdr = (struct ethhdr*)buffer;
+ memcpy(eth_hdr->h_dest, dmac, ETH_ALEN);
+ memcpy(eth_hdr->h_source, smac, ETH_ALEN);
+ eth_hdr->h_proto = htons(ETH_P_1905);
+
+ /*start position of message header*/
+ msg = buffer + ETH_HLEN;
+ msg_hdr = (cmdu_message_header*)(msg);
+
+ /*start position of tlvs */
+ tlv = msg + CMDU_HLEN;
+
+ switch(mtype)
+ {
+ case e_topology_discovery:
+ msg_leng = create_topology_discovery_message(msg,\
+ ctx->p1905_al_mac_addr,smac);
+ break;
+ case e_topology_notification:
+ msg_leng = create_topology_notification_message(msg,\
+ ctx->p1905_al_mac_addr);
+ break;
+ case e_topology_query:
+ msg_leng = create_topology_query_message(msg);
+ break;
+ case e_topology_response:
+ msg_leng = create_topology_response_message(msg,\
+ ctx->p1905_al_mac_addr, ctx->itf, ctx->br_cap,\
+ ctx->p1905_neighbor_dev, ctx->non_p1905_neighbor_dev,
+ &(ctx->topology_entry.tprdb_head));
+ break;
+ case e_vendor_specific:
+ {
+ struct p1905_vs_info *vs_info;
+ vs_info =\
+ (struct p1905_vs_info *)malloc(sizeof(struct p1905_vs_info));
+ memcpy(vs_info->al_mac,ctx->p1905_al_mac_addr, ETH_ALEN);
+ msg_leng = create_vendor_specific_message(msg, vs_info);
+ free(vs_info);
+ break;
+ }
+ case e_link_metric_query:
+ msg_leng = create_link_metrics_query_message(msg,\
+ ctx->link_metric_query_para.target,\
+ ctx->link_metric_query_para.type);
+ break;
+
+ case e_link_metric_response:
+ msg_leng = create_link_metrics_response_message(msg,\
+ ctx->link_metric_response_para.target,\
+ ctx->link_metric_response_para.type,\
+ ctx->p1905_al_mac_addr,\
+ ctx->itf, ctx->p1905_neighbor_dev,\
+ &(ctx->topology_entry.tpddb_head));
+ break;
+ case e_push_button_event_notification:
+ msg_leng = push_button_event_notification_message(msg,\
+ ctx->p1905_al_mac_addr, ctx->itf);
+ break;
+ case e_push_button_join_notification://for PLC joined
+ msg_leng = push_button_join_notification_message(msg,\
+ ctx->plc0_mac_addr, ctx->p1905_al_mac_addr,\
+ &(ctx->pbc_param), 1);
+ break;
+#ifdef SUPPORT_AP_REGISTRAR
+ case e_push_button_join_notification_wifi://for WIFI station joined
+ msg_leng = push_button_join_notification_message(msg,\
+ ctx->wifi0_mac_addr, ctx->p1905_al_mac_addr,\
+ &(ctx->pbc_param), 0);
+ break;
+#endif
+#ifdef SUPPORT_AP_AUTO_CONFIG
+#ifdef SUPPORT_AP_ENROLLE
+ case e_ap_autoconfiguration_search:
+ msg_leng = ap_autoconfiguration_search_message(msg, ctx);
+ break;
+ case e_ap_autoconfiguration_wsc_m1:
+ msg_leng = ap_autoconfiguration_wsc_message(msg, ctx, MESSAGE_TYPE_M1);
+ break;
+#endif
+#ifdef SUPPORT_AP_REGISTRAR
+ case e_ap_autoconfiguration_response:
+ msg_leng = ap_autoconfiguration_response_message(msg, ctx);
+ break;
+ case e_ap_autoconfiguration_wsc_m2:
+ msg_leng = ap_autoconfiguration_wsc_message(msg, ctx, MESSAGE_TYPE_M2);
+ break;
+ case e_ap_autoconfiguration_renew:
+ msg_leng = ap_autoconfiguration_renew_message(msg, ctx);
+ break;
+#endif
+#endif
+ default:
+ break;
+ }
+
+ msg_hdr->message_id = htons(mid);
+
+ /* temporarily set the fragment id & last fragment ind
+ * these two variables will be updated.
+ */
+ msg_hdr->fragment_id = 0;
+ msg_hdr->last_fragment_indicator = 1;
+
+
+ /*exceed the max frame size, do need to fragment TX*/
+ if(msg_leng > MAX_TLVS_LENGTH)
+ {
+ if(0 > cmdu_tx_fragment(ctx,tlv,msg_hdr,buffer))
+ {
+ syslog(LOG_WARNING, "fragment failed on %s (%s)", BR_IFNAME,
+ strerror(errno));
+ return -1;
+ }
+ }
+ else
+ {
+ memcpy(tlv, get_tlv_buffer(), msg_leng);
+ len = msg_leng + CMDU_HLEN + ETH_HLEN;
+ len = send(ctx->sock_br0, buffer, len, 0);
+ if(0 > len)
+ {
+ syslog(LOG_WARNING, "send failed on %s (%s)", BR_IFNAME,
+ strerror(errno));
+ return -1;
+ }
+ }
+ return 0;
+}
+
+/**
+ * for cmdu message relay.
+ *
+ * \param ctx p1905_managerd_ctx context.
+ * \param buf pointer of buffer for tx.
+ * \param len transmit length.
+ * \return error code
+ */
+static int cmdu_bridge_relay(struct p1905_managerd_ctx *ctx,
+ unsigned char *buf, int len)
+{
+ int length;
+
+ length = send(ctx->sock_br0, buf, len, 0);
+
+ if(0 > length)
+ {
+ syslog(LOG_WARNING, "relay failed on %s (%s)", BR_IFNAME,
+ strerror(errno));
+ return -1;
+ }
+
+ return 0;
+}
+
+/**
+ * for cmdu message relay.
+ *
+ * \param ctx p1905_managerd_ctx context.
+ * \param buf pointer of buffer for tx.
+ * \param len transmit length.
+ * \return error code
+ */
+static int cmdu_bridge_relay_send(struct p1905_managerd_ctx *ctx,
+ unsigned char *smac, unsigned char *buf, int len)
+{
+ int i = 0;
+ unsigned char recv_itf_mac_addr[ETH_ALEN];
+
+ /* Need to know which port the message comes from, and then relay to
+ * the other port. so use get_receive_port_addr
+ */
+ if(0 > get_receive_port_addr(smac, recv_itf_mac_addr))
+ {
+ debug_syslog("relay get recv port fail\n");
+ return -1;
+ }
+
+ for(i=0;i<ITF_NUM;i++)
+ {
+ /*do not relay to original receive interface*/
+ if(memcmp(ctx->itf[i].mac_addr, recv_itf_mac_addr, ETH_ALEN))
+ {
+ memcpy(smac, ctx->itf[i].mac_addr, ETH_ALEN);
+ if(0 > cmdu_bridge_relay(ctx, buf, len))
+ {
+ return -1;
+ }
+ }
+ }
+ return 0;
+}
+
+/**
+ * receive cmdu message from socket.
+ *
+ * \param ctx p1905_managerd_ctx context.
+ * \param buf pointer of buffer for rx.
+ * \param len max receive length.
+ * \return error code
+ */
+int cmdu_bridge_receive(struct p1905_managerd_ctx *ctx, unsigned char *buf, int len)
+{
+ len = recv(ctx->sock_br0, buf, len, 0);
+ debug("cmdu_bridge_receive -- len =%d\n",len);
+
+ if(0 >= len)
+ {
+ syslog(LOG_WARNING, "receive failed on %s (%s)", BR_IFNAME,
+ strerror(errno));
+ return -1;
+ }
+
+ return len;
+}
+
+/**
+ * parse cmdu.
+ *
+ * \param ctx p1905_managerd_ctx context.
+ * \param buf pointer of buffer for rx.
+ * \param len receive length.
+ * \return error code
+ */
+int cmdu_parse(struct p1905_managerd_ctx *ctx,unsigned char *buf,int len)
+{
+ struct ethhdr *eth_hdr;
+ unsigned char *temp_buf;
+
+ eth_hdr = (struct ethhdr*)buf;
+
+ /*shift to cmdu message start position*/
+ temp_buf = buf;
+ temp_buf += ETH_HLEN;
+
+ if(0 > parse_cmdu_message(ctx,temp_buf,eth_hdr->h_dest,eth_hdr->h_source,len))
+ {
+ debug_syslog("error cmdu parse\n");
+ return -1;
+ }
+
+ if(ctx->need_relay)
+ {
+ ctx->need_relay = 0;
+ if(0 > cmdu_bridge_relay_send(ctx, eth_hdr->h_source, buf, len))
+ {
+ debug_syslog("relay cmdu fail\n");
+ return -1;
+ }
+ }
+
+ return 0;
+}
+
diff --git a/cleopatre/devkit/p1905_managerd/src/cmdu_fragment.c b/cleopatre/devkit/p1905_managerd/src/cmdu_fragment.c
new file mode 100644
index 0000000000..a7d2cff701
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/cmdu_fragment.c
@@ -0,0 +1,291 @@
+/*
+ * cleopatre/application/p1905_managerd/src/cmdu_fragment.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_fragment.h"
+#include "cmdu_tlv.h"
+
+//#define CMDU_FRAGMENT_DEBUG
+#ifdef CMDU_FRAGMENT_DEBUG
+#define debug(...) printf(__VA_ARGS__)
+#else
+#define debug(...)
+#endif
+
+LIST_HEAD(list_head_fragment, fragment_list) fragment_head;
+
+static unsigned char clear_fragment_cnt = 0;
+
+static void reset_fragment_cnt(void)
+{
+ clear_fragment_cnt = 0;
+}
+
+void add_fragment_cnt(void)
+{
+ clear_fragment_cnt++;
+}
+
+unsigned char get_fragment_cnt(void)
+{
+ return clear_fragment_cnt;
+}
+
+int calculate_fragment_tlvs_len(unsigned char *buf, int ref_length)
+{
+ unsigned char *temp_buf = buf;
+ int length = 0, total_length = 0;
+
+ while(((*temp_buf) > END_OF_TLV_TYPE) &&\
+ ((*temp_buf) <= PUSH_BUTTON_JOIN_NOTIFICATION_TYPE) &&\
+ (total_length < ref_length))
+ {
+ length = get_cmdu_tlv_length(temp_buf);
+ temp_buf += length;
+ total_length += length;
+ }
+
+ debug("fragment ref length = %d\n",ref_length);
+ debug("fragment total_length = %d\n",total_length);
+ return total_length;
+}
+
+/**
+ * initialize the list head of rx fragment queue.
+ *
+ */
+void init_fragment_queue(void)
+{
+ LIST_INIT(&fragment_head);
+}
+
+/**
+ * delete fragment queue by message type and message id.
+ *
+ * \param mtype cmdu message type.
+ * \param mid cmdu message id.
+ */
+static void delete_fragment_queue(unsigned short mtype, unsigned short mid)
+{
+ struct fragment_list *flist, *flist_temp;
+
+ flist = LIST_FIRST(&fragment_head);
+ while(flist)
+ {
+ flist_temp = LIST_NEXT(flist, fragment_entry);
+
+ if(flist->mtype == mtype && flist->mid == mid)
+ {
+ debug("delete fragment queue: mtype = %d, mid = %d\n",mtype, mid);
+ LIST_REMOVE(flist, fragment_entry);
+
+ if(flist->length)
+ free(flist->data);
+ free(flist);
+ }
+ flist = flist_temp;
+ }
+}
+
+/**
+ * delete all rxfragment queue.
+ *
+ */
+void delete_fragment_queue_all(void)
+{
+ struct fragment_list *flist, *flist_temp;
+
+ flist = LIST_FIRST(&fragment_head);
+ while(flist)
+ {
+ flist_temp = LIST_NEXT(flist, fragment_entry);
+
+ debug("delete fragment queue all: mtype = %d, mid = %d\n",
+ flist->mtype, flist->mid);
+ LIST_REMOVE(flist, fragment_entry);
+
+ if(flist->length)
+ free(flist->data);
+ free(flist);
+ flist = flist_temp;
+ }
+}
+
+/**
+ * insert rx fragment into queue.
+ *
+ * \param mtype cmdu message type.
+ * \param mid cmdu message id.
+ * \param fid cmdu fragment id.
+ * \param lastfragment cmdu last fragment indication.
+ * \param length length of this fragment.
+ * \param buffer tlv start address of rx cmdu message.
+ */
+void insert_fragment_queue(unsigned short mtype, unsigned short mid,
+ unsigned char fid, unsigned char lastfragment,
+ unsigned short length, unsigned char *buffer)
+{
+ struct fragment_list *flist;
+ unsigned char new_db = 1;
+
+#ifdef CMDU_FRAGMENT_DEBUG
+ int i = 0;
+#endif
+
+ /* serach fragment queue to know this fragment is existed or not*/
+ if(!LIST_EMPTY(&fragment_head))
+ {
+ LIST_FOREACH(flist, &fragment_head, fragment_entry)
+ {
+ if(flist->mtype == mtype && flist->mid == mid)
+ {
+ /*if already in queue, do not store it*/
+ if(fid == flist->fid)
+ {
+ new_db = 0;
+ break;
+ }
+ }
+ }
+ }
+ if(new_db)
+ {
+ flist = (struct fragment_list *)malloc(sizeof(struct fragment_list));
+ flist->fid = fid;
+ flist->lastfragment = lastfragment;
+ flist->mid = mid;
+ flist->mtype = mtype;
+ flist->length = length;
+
+ if(length)
+ {
+ flist->data = (unsigned char*)malloc(length);
+ memcpy(flist->data, buffer, length);
+ }
+
+ debug("insert fragment queue:\n");
+ debug("fid = %d last = %d mid = %d mtype = %d, len = %d\n",fid,
+ lastfragment, mid, mtype, length);
+#ifdef CMDU_FRAGMENT_DEBUG
+ for(i=0;i<length;i++)
+ {
+ debug("%.2x ",*(flist->data + i));
+ if((i % 16) == 0)
+ printf("\n");
+ }
+#endif
+
+ LIST_INSERT_HEAD(&fragment_head, flist, fragment_entry);
+
+ /*if new fragment inserted into queue, reset fragment timeout counter*/
+ reset_fragment_cnt();
+ }
+}
+
+/**
+ * reassembly the fragment in rx fragment queue.
+ *
+ * \param rxbuf buffer for store reassembly fragments.
+ * \param mtype cmdu message type.
+ * \param mid cmdu message id.
+ * \return total length of reassembly fragments
+ */
+unsigned short reassembly_fragment_queue(unsigned char *rxbuf,
+ unsigned short mtype, unsigned short mid)
+{
+ unsigned char find = 0;
+ unsigned char fcnt = 0;
+ struct fragment_list *flist, *temp_flist;
+ int i = 0;
+ unsigned char *temp_rxbuf;
+ unsigned short total_length = 0;
+
+ temp_rxbuf = rxbuf;
+
+ LIST_FOREACH(flist, &fragment_head, fragment_entry)
+ {
+ /*check if the last fragment is existed in fragment queue*/
+ if((flist->mtype == mtype) && (flist->mid == mid) &&\
+ (flist->lastfragment == 1))
+ {
+ find =1;
+ break;
+ }
+ }
+
+ /* find == 1 ==> has last fragment in queue*/
+ if(find)
+ {
+ /* Calculate total amount of this kind fragment. After calculation,
+ * we store the result to fcnt
+ * Don't worry about the duplicated fragment, becasue it has already
+ * been filterd in Insert stage.
+ */
+ LIST_FOREACH(temp_flist, &fragment_head, fragment_entry)
+ {
+ if(temp_flist->mid == mid && temp_flist->mtype == mtype)
+ fcnt++;
+ }
+
+ /* fcnt ==> total fragment amount
+ * flist->fid ==> fragment id of last fragment
+ * fcnt == (flist->fid + 1) ==> represents we got all fragments,
+ * so start to assembly.
+ */
+ if(fcnt == (flist->fid + 1))
+ {
+ for(i=0;i<fcnt;i++)
+ {
+ LIST_FOREACH(temp_flist, &fragment_head, fragment_entry)
+ {
+ /* ok , we need to get fragments from queue in sequence.
+ * so use fragment id to get.
+ * then fill these fragments into buffer in sequence.
+ */
+ if(temp_flist->fid == i && temp_flist->mid == mid\
+ && temp_flist->mtype == mtype)
+ {
+ memcpy(temp_rxbuf, temp_flist->data, temp_flist->length);
+ temp_rxbuf += temp_flist->length;
+ total_length += temp_flist->length;
+ }
+ }
+ }
+ delete_fragment_queue(mtype, mid);
+ }
+ else
+ return 0;
+ }
+ else
+ return 0;
+
+ return total_length;
+}
+
+/**
+ * un-init rx fragment queue.
+ *
+ */
+void uninit_fragment_queue(void)
+{
+ delete_fragment_queue_all();
+}
+
diff --git a/cleopatre/devkit/p1905_managerd/src/cmdu_message.c b/cleopatre/devkit/p1905_managerd/src/cmdu_message.c
new file mode 100644
index 0000000000..71a8c81c71
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/cmdu_message.c
@@ -0,0 +1,741 @@
+/*
+ * cleopatre/application/p1905_managerd/src/cmdu_message.c
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ *
+ */
+#include <stdio.h>
+#include <string.h>
+#include <net/if.h>
+#include <syslog.h>
+#include <assert.h>
+#include <linux/if_ether.h>
+#include <asm/byteorder.h>
+#include "cmdu_message.h"
+
+static unsigned char tlv_temp[3072]={0};
+
+unsigned char *get_tlv_buffer(void)
+{
+ return tlv_temp;
+}
+
+
+/**
+ * delete all non-1905.1 neighbor device.
+ *
+ * \param non_p1905_dev pointer of non_1905.1 database
+ * \param retun error code
+ */
+void delete_non_p1905_neighbor_dev_info(struct non_p1905_neighbor *non_p1905_dev)
+{
+ int i = 0;
+ struct non_p1905_neighbor_info *dev_info, *dev_info_temp;
+
+ for(i=0;i<ITF_NUM;i++)
+ {
+ if(!LIST_EMPTY(&(non_p1905_dev[i].non_p1905nbr_head)))
+ {
+ dev_info = LIST_FIRST(&(non_p1905_dev[i].non_p1905nbr_head));
+ while(dev_info)
+ {
+ dev_info_temp = LIST_NEXT(dev_info, non_p1905nbr_entry);
+ LIST_REMOVE(dev_info, non_p1905nbr_entry);
+ free(dev_info);
+ dev_info = dev_info_temp;
+ }
+ }
+ }
+}
+
+/**
+ * create topology discovery message.
+ *
+ * \param buf pointer to cmdu message header start position.
+ * \param al_mac pointer of local abstraction layer mac address.
+ * \param itf_mac pointer of transmitting ibterface mac address.
+ * \return length of total tlvs included in this message
+ */
+unsigned short create_topology_discovery_message(unsigned char *buf,
+ unsigned char *al_mac, unsigned char *itf_mac)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ /*fill into tlvs*/
+ length = append_1905_al_mac_addr_type_tlv(tlv_temp_buf,al_mac);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_mac_addr_type_tlv(tlv_temp_buf,itf_mac);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(TOPOLOGY_DISCOVERY);
+ msg_hdr->relay_indicator = 0x0;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+
+/**
+ * create topology query message.
+ *
+ * \param buf pointer to cmdu message header start position.
+ * \return length of total tlvs included in this message
+ */
+unsigned short create_topology_query_message(unsigned char *buf)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(TOPOLOGY_QUERY);
+ msg_hdr->relay_indicator = 0x0;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+
+/**
+ * create topology response message.
+ *
+ * \param buf pointer to cmdu message header start position.
+ * \param al_mac pointer of local abstraction layer mac address.
+ * \param itf_list pointer of local interfaces information.
+ * \param br_cap_list pointer of local internal bridge information.
+ * \param p1905_dev pointer of 1905.1 neighbor devices information.
+ * \param non_p1905_dev poniter of non-1905.1 device information
+ * \param tpgr_list list head of received topology response database
+ * \return length of total tlvs included in this message
+ */
+unsigned short create_topology_response_message(unsigned char *buf,
+ unsigned char *al_mac, struct p1905_interface *itf_list,
+ struct bridge_capabiltiy *br_cap_list, struct p1905_neighbor *p1905_dev,
+ struct non_p1905_neighbor *non_p1905_dev, struct list_head_tprdb *tpgr_head)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+ int i = 0;
+ unsigned char seperate = 0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ length = append_device_info_type_tlv(tlv_temp_buf, al_mac, itf_list);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_device_bridge_capability_type_tlv(tlv_temp_buf,\
+ br_cap_list);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ for(i=0;i<ITF_NUM;i++)
+ {
+ do{
+
+ length = append_p1905_neighbor_device_type_tlv(tlv_temp_buf, p1905_dev,
+ i, &seperate);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+ }while(seperate);
+ }
+
+ /*append non-p1905.1 device tlv*/
+ for(i=0;i<ITF_NUM;i++)
+ {
+ /* we don't need to restrict the number of non-1905.1 neighbor.
+ * This number is restricted by MAX_FDB_ENTRIES = 48
+ */
+ length = append_non_p1905_neighbor_device_type_tlv(tlv_temp_buf,\
+ non_p1905_dev,i);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+ }
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(TOPOLOGY_RESPONSE);
+ msg_hdr->relay_indicator = 0x0;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+
+/**
+ * create topology notification message.
+ *
+ * \param buf pointer to cmdu message header start position.
+ * \param al_mac pointer of local abstraction layer mac address.
+ * \return length of total tlvs included in this message
+ */
+unsigned short create_topology_notification_message(unsigned char *buf,
+ unsigned char *al_mac)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ length = append_1905_al_mac_addr_type_tlv(tlv_temp_buf,al_mac);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(TOPOLOGY_NOTIFICATION);
+ /* topology notification is a relay multicast message,
+ * so set relay_indicator
+ */
+ msg_hdr->relay_indicator = 0x1;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+
+/**
+ * create vendor specific message.
+ *
+ * \param buf pointer to cmdu message header start position.
+ * \param vs_info vendor specific information.
+ * \return length of total tlvs included in this message
+ */
+unsigned short create_vendor_specific_message(unsigned char *buf,
+ struct p1905_vs_info *vs_info)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ //fill into tlvs
+ length = append_vendor_specific_type_tlv(tlv_temp_buf,vs_info);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(VENDOR_SPECIFIC);
+ msg_hdr->relay_indicator = 0x0;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+
+/**
+ * create link metric query message.
+ *
+ * \param buf pointer to cmdu message header start position.
+ * \param target_al_mac 0: all neighbors, otherwise, mac address of target.
+ * \param type TX_METRICS_ONLY/RX_METRICS_ONLY/BOTH_TX_AND_RX_METRICS.
+ * \return length of total tlvs included in this message
+ */
+unsigned short create_link_metrics_query_message(unsigned char *buf,
+ unsigned char *target_al_mac, unsigned char type)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ //fill into tlvs
+ length = append_link_metrics_query_type_tlv(tlv_temp_buf, target_al_mac, type);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(LINK_METRICS_QUERY);
+ msg_hdr->relay_indicator = 0x0;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+
+/**
+ * create link metric response message.
+ *
+ * \param buf pointer to cmdu message header start position.
+ * \param target_al_mac 0: all neighbors, otherwise, mac address of target.
+ * \param type TX_METRICS_ONLY/RX_METRICS_ONLY/BOTH_TX_AND_RX_METRICS.
+ * \param local_al_mac local AL mac.
+ * \param itf_list pointer of local interfaces information.
+ * \param tpd_head list head of topology discovery database .
+ * \return length of total tlvs included in this message
+ */
+unsigned short create_link_metrics_response_message(unsigned char *buf,
+ unsigned char *target_al_mac, unsigned char type,
+ unsigned char *local_al_mac, struct p1905_interface *itf_list,
+ struct p1905_neighbor *devlist, struct list_head_tpddb *tpd_head)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+ unsigned char query_all_neighbor[ETH_ALEN] = {0,0,0,0,0,0};
+ struct topology_discovery_db *dcv_db;
+ unsigned char valid_neighbor = 0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ //fill into tlvs
+ if(!memcmp(target_al_mac, query_all_neighbor, ETH_ALEN))
+ {
+ /*query all neighbors, response all neighbor's link metric*/
+ LIST_FOREACH(dcv_db, tpd_head, tpddb_entry)
+ {
+ if(type == TX_METRICS_ONLY)
+ {
+ length = append_transmitter_link_metrics_tlv(tlv_temp_buf,\
+ local_al_mac, dcv_db->al_mac, dcv_db->receive_itf_mac,\
+ dcv_db->itf_mac, itf_list, devlist);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+ }
+ else if(type == RX_METRICS_ONLY)
+ {
+ length = append_receiver_link_metrics_tlv(tlv_temp_buf,\
+ local_al_mac, dcv_db->al_mac, dcv_db->receive_itf_mac,\
+ dcv_db->itf_mac, itf_list);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+ }
+ else if(type == BOTH_TX_AND_RX_METRICS)
+ {
+ length = append_transmitter_link_metrics_tlv(tlv_temp_buf,\
+ local_al_mac, dcv_db->al_mac, dcv_db->receive_itf_mac,\
+ dcv_db->itf_mac, itf_list, devlist);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_receiver_link_metrics_tlv(tlv_temp_buf,\
+ local_al_mac, dcv_db->al_mac, dcv_db->receive_itf_mac,\
+ dcv_db->itf_mac, itf_list);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+ }
+ }
+ }
+ else
+ {
+ LIST_FOREACH(dcv_db, tpd_head, tpddb_entry)
+ {
+ if(!memcmp(target_al_mac, dcv_db->al_mac, ETH_ALEN))
+ {
+ valid_neighbor = 1;
+
+ if(type == TX_METRICS_ONLY)
+ {
+ length = append_transmitter_link_metrics_tlv(tlv_temp_buf,\
+ local_al_mac, dcv_db->al_mac, dcv_db->receive_itf_mac,\
+ dcv_db->itf_mac, itf_list, devlist);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+ }
+ else if(type == RX_METRICS_ONLY)
+ {
+ length = append_receiver_link_metrics_tlv(tlv_temp_buf,\
+ local_al_mac, dcv_db->al_mac, dcv_db->receive_itf_mac,\
+ dcv_db->itf_mac, itf_list);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+ }
+ else if(type == BOTH_TX_AND_RX_METRICS)
+ {
+ length = append_transmitter_link_metrics_tlv(tlv_temp_buf,\
+ local_al_mac, dcv_db->al_mac, dcv_db->receive_itf_mac,\
+ dcv_db->itf_mac, itf_list, devlist);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_receiver_link_metrics_tlv(tlv_temp_buf,\
+ local_al_mac, dcv_db->al_mac, dcv_db->receive_itf_mac,\
+ dcv_db->itf_mac, itf_list);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+ }
+ break;
+ }
+ }
+
+ if(valid_neighbor != 1)
+ {
+ length = append_link_metrics_result_code_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+ }
+ }
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(LINK_METRICS_RESPONSE);
+ msg_hdr->relay_indicator = 0x0;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+
+/**
+ * create push button event notification message.
+ *
+ * \param buf pointer to cmdu message header start position.
+ * \param plc_mac mac address of PLC interface.
+ * \param al_mac local AL mac address.
+ * \return length of total tlvs included in this message
+ */
+unsigned short push_button_event_notification_message(unsigned char *buf,
+ unsigned char *al_mac, struct p1905_interface *itf_list)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ //fill into tlvs
+ length = append_1905_al_mac_addr_type_tlv(tlv_temp_buf, al_mac);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_push_button_event_notification_tlv(tlv_temp_buf, itf_list);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(P1905_PB_EVENT_NOTIFY);
+ /*push button event notification is a multicast relay message*/
+ msg_hdr->relay_indicator = 0x1;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+
+/**
+ * create push button join notification message.
+ *
+ * \param buf pointer to cmdu message header start position.
+ * \param plc_mac mac address of PLC interface.
+ * \param al_mac local AL mac address.
+ * \param pbc_p instance of push_button_param
+ * \return length of total tlvs included in this message
+ */
+unsigned short push_button_join_notification_message(unsigned char *buf,
+ unsigned char *local_mac, unsigned char *al_mac,
+ push_button_param *pbc_p, unsigned char is_plc)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ //fill into tlvs
+ length = append_1905_al_mac_addr_type_tlv(tlv_temp_buf, al_mac);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ if(is_plc)
+ {
+ length = append_push_button_join_notification_tlv(tlv_temp_buf,
+ pbc_p->al_id, pbc_p->mid, local_mac, pbc_p->info.new_station);
+ }
+#ifdef SUPPORT_AP_REGISTRAR
+ else
+ {
+ length = append_push_button_join_notification_tlv(tlv_temp_buf,
+ pbc_p->al_id, pbc_p->mid, local_mac, pbc_p->wifi_info.new_station);
+ }
+#endif
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(P1905_PB_JOIN_NOTIFY);
+ /*push button join notification is a multicast relay message*/
+ msg_hdr->relay_indicator = 0x1;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+#ifdef SUPPORT_AP_ENROLLE
+unsigned short ap_autoconfiguration_search_message(
+ unsigned char *buf, struct p1905_managerd_ctx *ctx)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ length = append_1905_al_mac_addr_type_tlv(tlv_temp_buf, ctx->p1905_al_mac_addr);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_searched_role_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_autoconfig_freq_band_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(AP_AUTOCONFIG_SEARCH);
+ /*AP autoconfiguration search message is a multicast relay message*/
+ msg_hdr->relay_indicator = 0x1;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+#endif
+#ifdef SUPPORT_AP_REGISTRAR
+unsigned short ap_autoconfiguration_response_message(
+ unsigned char *buf, struct p1905_managerd_ctx *ctx)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ length = append_supported_role_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_supported_freq_band_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(AP_AUTOCONFIG_RESPONSE);
+ msg_hdr->relay_indicator = 0x0;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+
+unsigned short ap_autoconfiguration_renew_message(
+ unsigned char *buf, struct p1905_managerd_ctx *ctx)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ length = append_1905_al_mac_addr_type_tlv(tlv_temp_buf, ctx->p1905_al_mac_addr);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_supported_role_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_supported_freq_band_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(AP_AUTOCONFIG_RENEW);
+ msg_hdr->relay_indicator = 0x1;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+#endif
+unsigned short ap_autoconfiguration_wsc_message(
+ unsigned char *buf, struct p1905_managerd_ctx *ctx, unsigned char wsc_type)
+{
+ unsigned char *tlv;
+ unsigned char *tlv_temp_buf = tlv_temp;
+ cmdu_message_header *msg_hdr;
+ unsigned short length = 0;
+ unsigned short total_tlvs_length =0;
+
+ msg_hdr = (cmdu_message_header*)buf;
+ tlv = buf + CMDU_HLEN;
+
+ length = append_WSC_tlv(tlv_temp_buf, ctx, wsc_type);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ length = append_end_of_tlv(tlv_temp_buf);
+ total_tlvs_length += length;
+ tlv_temp_buf += length;
+
+ //0x00: for this version of the specification
+ //0x01~0xFF: Reserved Values
+ msg_hdr->message_version = 0x0;
+
+ //set reserve field to 0
+ msg_hdr->reserved_field_0 = 0x0;
+ msg_hdr->message_type = htons(AP_AUTOCONFIG_WSC);
+ msg_hdr->relay_indicator = 0x0;
+ //set reserve field to 0
+ msg_hdr->reserve_field_1 = 0x0;
+
+ return total_tlvs_length;
+}
+#endif
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;
+}
+
diff --git a/cleopatre/devkit/p1905_managerd/src/cmdu_tlv.c b/cleopatre/devkit/p1905_managerd/src/cmdu_tlv.c
new file mode 100644
index 0000000000..8a22a1e39c
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/cmdu_tlv.c
@@ -0,0 +1,985 @@
+/*
+ * cleopatre/application/p1905_managerd/src/cmdu_tlv.c
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <syslog.h>
+#include <assert.h>
+#include <linux/if_ether.h>
+#include <asm/byteorder.h>
+
+#include "cmdu_tlv.h"
+#include "p1905_utils.h"
+
+//#define CMDU_TLV_DEBUG
+#ifdef CMDU_TLV_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
+
+
+#define MAX_NEIGHBORS_IN_ONE_TLV 100
+
+unsigned short append_1905_al_mac_addr_type_tlv(unsigned char *pkt,
+ unsigned char *al_mac)
+{
+ unsigned short total_length = 0;
+
+ *pkt = AL_MAC_ADDR_TLV_TYPE;
+ total_length += 1;
+
+ *(pkt+1) = (unsigned char)((AL_MAC_ADDR_TLV_LENGTH & 0xFF00) >> 8);
+ *(pkt+2) = (unsigned char)(AL_MAC_ADDR_TLV_LENGTH & 0x00FF);
+ total_length += 2;
+
+ memcpy((pkt+3),al_mac,ETH_ALEN);
+
+ total_length += AL_MAC_ADDR_TLV_LENGTH;
+ return total_length;
+}
+
+unsigned short append_mac_addr_type_tlv(unsigned char *pkt,
+ unsigned char *itf_mac)
+{
+ unsigned short total_length = 0;
+
+ *pkt = MAC_ADDR_TLV_TYPE;
+ total_length += 1;
+
+ *(pkt+1) = (unsigned char)((MAC_ADDR_TLV_LENGTH & 0xFF00) >> 8);
+ *(pkt+2) = (unsigned char)(MAC_ADDR_TLV_LENGTH & 0x00FF);
+
+ total_length += 2;
+
+ memcpy((pkt+3),itf_mac,ETH_ALEN);
+
+ total_length += MAC_ADDR_TLV_LENGTH;
+ return total_length;
+}
+
+unsigned short append_device_info_type_tlv(unsigned char *pkt,
+ unsigned char *al_mac,struct p1905_interface *itf_list)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+ int i =0;
+
+ temp_buf = pkt;
+
+ *temp_buf = DEVICE_INFO_TLV_TYPE;
+ temp_buf +=1;
+ total_length += 1;
+
+ /* shift to tlv value ,we will calculate the tlv value length in the end
+ * of this function
+ */
+ temp_buf +=2;
+ total_length += 2;
+
+ /*al mac address*/
+ memcpy(temp_buf,al_mac,ETH_ALEN);
+ temp_buf +=6;
+ total_length += 6;
+
+ /*amount of local p1905.1 interface */
+ *temp_buf = ITF_NUM;
+ temp_buf +=1;
+ total_length += 1;
+
+ /*info of every interface*/
+ for(i=0;i<ITF_NUM;i++)
+ {
+ memcpy(temp_buf,itf_list[i].mac_addr,ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ *temp_buf = (unsigned char)(itf_list[i].media_type >> 8) & 0xFF;
+ *(temp_buf + 1) = (unsigned char)(itf_list[i].media_type) & 0xFF;
+ temp_buf += 2;
+ total_length += 2;
+
+ *temp_buf = itf_list[i].vs_info_length;
+ temp_buf += 1;
+ total_length += 1;
+
+ if(itf_list[i].vs_info_length > 0)
+ {
+
+#ifdef SUPPORT_PLC
+ if(itf_list[i].media_type >= IEEE1901_GROUP &&\
+ itf_list[i].media_type < MOCA_GROUP)
+ {
+ /*PLC interface info is network id, so get NID*/
+ if(!get_network_id(itf_list[i].mac_addr, itf_list[i].vs_info))
+ {
+ debug_syslog("get network id fail\n");
+ }
+ }
+#endif
+#ifdef SUPPORT_WIFI
+ if(itf_list[i].media_type >= IEEE802_11_GROUP &&\
+ itf_list[i].media_type < IEEE1901_GROUP)
+ {
+ /*dot11CurrentChannelWidth*/
+ if(wifi_utils_success != get_channel_width((itf_list[i].vs_info) + 7))
+ {
+ debug_syslog("get wifi channel width fail\n");
+ }
+
+ /*dot11CurrentChannelCenterFrequencyIndex0*/
+ if(wifi_utils_success != get_center_freq_index((itf_list[i].vs_info) + 8))
+ {
+ debug_syslog("get wifi center freq fail fail\n");
+ }
+
+ /* we use 802.11n device, dot11CurrentChannelCenterFrequencyIndex1 = 0*/
+ *((itf_list[i].vs_info) + 9) = 0;
+ }
+#endif
+ memcpy(temp_buf,itf_list[i].vs_info,itf_list[i].vs_info_length);
+ temp_buf += itf_list[i].vs_info_length;
+ total_length += itf_list[i].vs_info_length;
+ }
+ }
+ /*finally, fill the total tlv value length into tlv length field*/
+ *(pkt + 1) = (unsigned char)(((total_length - 3) >> 8) & 0xff);
+ *(pkt + 2) = (unsigned char)((total_length - 3) & 0xff);
+
+ return total_length;
+}
+
+unsigned short append_device_bridge_capability_type_tlv(unsigned char *pkt,
+ struct bridge_capabiltiy *br_cap_list)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+ int i =0;
+
+ temp_buf = pkt;
+
+ *temp_buf = BRIDGE_CAPABILITY_TLV_TYPE;
+ temp_buf +=1;
+ total_length += 1;
+
+ /* shift to tlv value ,we will calculate the tlv value length in the end
+ * ofthis function
+ */
+ temp_buf +=2;
+ total_length += 2;
+
+ /*amount of internal bridge*/
+ *temp_buf = BRIDGE_NUM;
+ temp_buf +=1;
+ total_length += 1;
+
+ for(i=0;i<BRIDGE_NUM;i++)
+ {
+ *temp_buf = br_cap_list[i].interface_num;
+ temp_buf +=1;
+ total_length += 1;
+
+ memcpy(temp_buf, br_cap_list[i].itf_mac_list,\
+ (ETH_ALEN*br_cap_list[i].interface_num));
+
+ temp_buf += ETH_ALEN * br_cap_list[i].interface_num;
+ total_length += ETH_ALEN * br_cap_list[i].interface_num;
+ }
+
+ /*finally, fill the total tlv value length into tlv length field*/
+ *(pkt + 1) = (unsigned char)(((total_length - 3) >> 8) & 0xff);
+ *(pkt + 2) = (unsigned char)((total_length - 3) & 0xff);
+
+ return total_length;
+}
+
+unsigned short append_p1905_neighbor_device_type_tlv(unsigned char *pkt,
+ struct p1905_neighbor *devlist, int num, unsigned char *seperate)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+ int i = 0;
+ struct p1905_neighbor_info *info;
+ unsigned char exist = 0;
+ unsigned char neighbor_num = 0;
+ static unsigned char record_mac[ETH_ALEN] = {0};
+
+ temp_buf = pkt;
+ i = num;
+
+ /*firstly shift to tlv value*/
+ temp_buf += 3;
+ total_length += 3;
+
+ if(!LIST_EMPTY(&(devlist[i].p1905nbr_head)))
+ {
+ exist = 1;
+ memcpy(temp_buf, devlist[i].local_mac_addr, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ LIST_FOREACH(info, &(devlist[i].p1905nbr_head), p1905nbr_entry)
+ {
+ if(*seperate)
+ {
+ if(memcmp(record_mac, info->al_mac_addr, ETH_ALEN))
+ {
+ continue;
+ }
+
+ /*shift to next p1905.1 neighbor device database*/
+ *seperate = 0;
+ continue;
+ }
+
+ memcpy(temp_buf, info->al_mac_addr, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ if(info->ieee802_1_bridge)
+ (*temp_buf) = 0x80;
+ else
+ (*temp_buf) = 0x0;
+ temp_buf += 1;
+ total_length += 1;
+
+ /* use neighbor_num to record how many neighbors in one tlv
+ * we don't want the tlv length larger then max message length
+ * so we restrict the neighbor number to MAX_NEIGHBORS_IN_ONE_TLV
+ * Notice: although we can fragmet the cmdu, but cmdu is protocol
+ * tlv boundary. So one tlv cannot exceed max message length.
+ */
+ neighbor_num += 1;
+ if(neighbor_num == MAX_NEIGHBORS_IN_ONE_TLV)
+ {
+ *seperate = 1;
+ memcpy(record_mac, info->al_mac_addr, ETH_ALEN);
+ break;
+ }
+ }
+ }
+
+ /*if really has the p1905.1 neighbor device, append type & length */
+ if(exist)
+ {
+ (*pkt) = P1905_NEIGHBOR_DEV_TLV_TYPE;
+ *(pkt + 1) = (unsigned char)(((total_length - 3) >> 8) & 0xff);
+ *(pkt + 2) = (unsigned char)((total_length - 3) & 0xff);
+ return total_length;
+ }
+ else
+ return 0;
+}
+
+unsigned short append_non_p1905_neighbor_device_type_tlv(unsigned char *pkt,
+ struct non_p1905_neighbor *devlist, int num)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+ unsigned char exist = 0;
+ struct non_p1905_neighbor_info *info;
+
+ temp_buf = pkt;
+
+ /*shift to value field*/
+ temp_buf += 3;
+ total_length += 3;
+
+ /*local interface mac field*/
+ memcpy(temp_buf, devlist[num].local_mac_addr, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ /*mac address of non-1905.1 neighbor device*/
+ if(!LIST_EMPTY(&(devlist[num].non_p1905nbr_head)))
+ {
+ exist =1;
+ LIST_FOREACH(info, &(devlist[num].non_p1905nbr_head), non_p1905nbr_entry)
+ {
+ memcpy(temp_buf, info->itf_mac_addr, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+ }
+ }
+
+ /*if really has the non-1905.1 neighbor device, append type & length */
+ if(exist)
+ {
+ (*pkt) = NON_P1905_NEIGHBOR_DEV_TLV_TYPE;
+ *(pkt + 1) = (unsigned char)(((total_length - 3) >> 8) & 0xff);
+ *(pkt + 2) = (unsigned char)((total_length - 3) & 0xff);
+ return total_length;
+ }
+ else
+ return 0;
+}
+
+unsigned short append_end_of_tlv(unsigned char *pkt)
+{
+ unsigned short total_length = 0;
+
+ *pkt = END_OF_TLV_TYPE;
+ total_length += 1;
+
+ *(pkt+1) = (unsigned char)((END_OF_TLV_LENGTH & 0xFF00) >> 8);
+ *(pkt+2) = (unsigned char)(END_OF_TLV_LENGTH & 0x00FF);
+ total_length += 2;
+
+ return total_length;
+}
+
+unsigned short append_vendor_specific_type_tlv(unsigned char *pkt,
+ struct p1905_vs_info *vs_info)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+
+ temp_buf = pkt;
+ *pkt = VENDOR_SPECIFIC_TLV_TYPE;
+ total_length += 1;
+ temp_buf += 1;
+
+ /*shift to OUI field*/
+ temp_buf += 2;
+ total_length += 2;
+
+ memcpy(temp_buf, OUI_SPIDCOM, OUI_LEN);
+ temp_buf += OUI_LEN;
+ total_length += OUI_LEN;
+
+ /*append AL MAC*/
+ memcpy(temp_buf, vs_info->al_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ /*add more vendor specific information below*/
+
+ /*.........................................*/
+ /*add more vendor specific information above*/
+ *(pkt + 1) = (unsigned char)(((total_length - 3) >> 8) & 0xff);
+ *(pkt + 2) = (unsigned char)((total_length - 3) & 0xff);
+
+ return total_length;
+}
+
+unsigned short append_link_metrics_query_type_tlv(unsigned char *pkt,
+ unsigned char *target_al_mac,
+ unsigned char type)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+ unsigned char query_all_neighbor[ETH_ALEN] = {0,0,0,0,0,0};
+
+
+ temp_buf = pkt;
+
+ *temp_buf = LINK_METRICS_QUERY_TLV_TYPE;
+ temp_buf +=1;
+ total_length += 1;
+
+ /* The length of link metrics query is a variable.
+ * It depends on target & type, so shift to payload first
+ */
+ temp_buf +=2;
+ total_length += 2;
+
+ /*query link metrics of ALL neighbor or specific neighbor */
+ if(!memcmp(target_al_mac, query_all_neighbor, ETH_ALEN))
+ {
+ *temp_buf = QUERY_ALL_NEIGHBOR;
+ temp_buf += 1;
+ total_length += 1;
+ }
+ else
+ {
+ *temp_buf = QUERY_SPECIFIC_NEIGHBOR;
+ /*specific neighbor, append the AL MAC of neighbor in following field*/
+ temp_buf += 1;
+ total_length += 1;
+
+ memcpy(temp_buf, target_al_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+ }
+
+ if(type == TX_METRICS_ONLY)
+ *temp_buf = TX_METRICS_ONLY;
+ else if(type == RX_METRICS_ONLY)
+ *temp_buf = RX_METRICS_ONLY;
+ else if(type == BOTH_TX_AND_RX_METRICS)
+ *temp_buf = BOTH_TX_AND_RX_METRICS;
+
+ temp_buf += 1;
+ total_length += 1;
+
+ /*calculate totoal length & fill into the length field*/
+ *(pkt + 1) = (unsigned char)(((total_length - 3) >> 8) & 0xff);
+ *(pkt + 2) = (unsigned char)((total_length - 3) & 0xff);
+
+ return total_length;
+}
+
+unsigned short append_transmitter_link_metrics_tlv(unsigned char *pkt,
+ unsigned char *local_al_mac, unsigned char *neighbor_mac,
+ unsigned char *local_itf_mac, unsigned char *neighbor_itf_mac,
+ struct p1905_interface *itf_list, struct p1905_neighbor *devlist)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+ int i, j;
+ link_stat link_statistics;
+ unsigned short phy_rate, max_throughput;
+ struct p1905_neighbor_info *info;
+
+
+ temp_buf = pkt;
+
+ *temp_buf = TRANSMITTER_LINK_METRIC_TYPE;
+ temp_buf +=1;
+ total_length += 1;
+
+ /* The length of tx link metric tlv is a variable.
+ * It depends on connetced interface number, shift to payload first
+ */
+ temp_buf +=2;
+ total_length += 2;
+
+ /*fill into local abstration layer mac addr*/
+ memcpy(temp_buf, local_al_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ /*fill into neighbor abstration layer mac addr*/
+ memcpy(temp_buf, neighbor_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ /* In spec, below fields need to be repeated. But our product is PLC bridge
+ * and impossible to connect two devices with more than one interface.
+ * So I just write once.
+ */
+ /*fill into local interface mac addr*/
+ memcpy(temp_buf, local_itf_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ /*fill into neighbor interface mac addr*/
+ memcpy(temp_buf, neighbor_itf_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ /*fill into interface media type*/
+ for(i=0;i<ITF_NUM;i++)
+ {
+ if(!memcmp(itf_list[i].mac_addr, local_itf_mac, ETH_ALEN))
+ {
+ *temp_buf = (unsigned char)(itf_list[i].media_type >> 8) & 0xFF;
+ *(temp_buf + 1) = (unsigned char)(itf_list[i].media_type) & 0xFF;
+ temp_buf += 2;
+ total_length += 2;
+ break;
+ }
+ }
+
+ LIST_FOREACH(info, &(devlist[i].p1905nbr_head), p1905nbr_entry)
+ {
+ if(!memcmp(neighbor_mac, info->al_mac_addr, ETH_ALEN))
+ {
+ if(info->ieee802_1_bridge)
+ *temp_buf = 0x1;
+ else
+ *temp_buf = 0x0;
+
+ temp_buf += 1;
+ total_length += 1;
+ break;
+ }
+ }
+
+ if(0 == get_tx_link_statistics(&link_statistics, itf_list[i].media_type,
+ local_itf_mac, neighbor_itf_mac))
+ {
+ return 0;
+ }
+
+ if(0 == get_link_phy_rate(itf_list[i].media_type, neighbor_itf_mac,
+ local_itf_mac, &phy_rate))
+ {
+ return 0;
+ }
+
+ /*fill into tx packets error*/
+ *temp_buf = (unsigned char)(link_statistics.error_tx_packets >> 24) & 0xFF;
+ *(temp_buf + 1) = (unsigned char)(link_statistics.error_tx_packets >> 16) & 0xFF;
+ *(temp_buf + 2) = (unsigned char)(link_statistics.error_tx_packets >> 8) & 0xFF;
+ *(temp_buf + 3) = (unsigned char)(link_statistics.error_tx_packets) & 0xFF;
+ temp_buf += 4;
+ total_length += 4;
+
+ /*fill into total transmitted packets*/
+ *temp_buf = (unsigned char)(link_statistics.total_tx_packets >> 24) & 0xFF;
+ *(temp_buf + 1) = (unsigned char)(link_statistics.total_tx_packets >> 16) & 0xFF;
+ *(temp_buf + 2) = (unsigned char)(link_statistics.total_tx_packets >> 8) & 0xFF;
+ *(temp_buf + 3) = (unsigned char)(link_statistics.total_tx_packets) & 0xFF;
+ temp_buf += 4;
+ total_length += 4;
+
+ /*fill into max throughput capability*/
+ if(phy_rate == 0xFFFF)
+ max_throughput = 0xFFFF;
+ else
+ max_throughput = get_link_max_throughput_capacity(phy_rate, itf_list[i].media_type);
+
+ *temp_buf = (unsigned char)((max_throughput >> 8) & 0xff);
+ *(temp_buf + 1) = (unsigned char)(max_throughput & 0xff);
+ temp_buf += 2;
+ total_length += 2;
+
+ /*temporarily fill 100 into link availability filed */
+ *temp_buf = 0;
+ *(temp_buf + 1) = 100;
+ temp_buf += 2;
+ total_length += 2;
+
+ /* fill into phy rate*/
+ *temp_buf = (unsigned char)((phy_rate >> 8) & 0xff);
+ *(temp_buf + 1) = (unsigned char)(phy_rate & 0xff);
+ temp_buf += 2;
+ total_length += 2;
+
+ /*calculate totoal length & fill into the length field*/
+ *(pkt + 1) = (unsigned char)(((total_length - 3) >> 8) & 0xff);
+ *(pkt + 2) = (unsigned char)((total_length - 3) & 0xff);
+
+ return total_length;
+}
+
+unsigned short append_receiver_link_metrics_tlv(unsigned char *pkt,
+ unsigned char *local_al_mac, unsigned char *neighbor_mac,
+ unsigned char *local_itf_mac, unsigned char *neighbor_itf_mac,
+ struct p1905_interface *itf_list)
+
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+ int i = 0;
+ link_stat link_statistics;
+ char rssi;
+
+ temp_buf = pkt;
+
+ *temp_buf = RECEIVER_LINK_METRIC_TYPE;
+ temp_buf +=1;
+ total_length += 1;
+
+ /* The length of rx link metric tlv is a variable.
+ * It depends on connetced interface number, shift to payload first
+ */
+ temp_buf +=2;
+ total_length += 2;
+
+ /*fill into local abstration layer mac addr*/
+ memcpy(temp_buf, local_al_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ /*fill into neighbor abstration layer mac addr*/
+ memcpy(temp_buf, neighbor_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ /*fill into local interface mac addr*/
+ memcpy(temp_buf, local_itf_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ /*fill into neighbor interface mac addr*/
+ memcpy(temp_buf, neighbor_itf_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+ total_length += ETH_ALEN;
+
+ /*fill into interface media type*/
+ for(i=0;i<ITF_NUM;i++)
+ {
+ if(!memcmp(itf_list[i].mac_addr, local_itf_mac, ETH_ALEN))
+ {
+ *temp_buf = (unsigned char)(itf_list[i].media_type >> 8) & 0xFF;
+ *(temp_buf + 1) = (unsigned char)(itf_list[i].media_type) & 0xFF;
+ temp_buf += 2;
+ total_length += 2;
+ break;
+ }
+ }
+
+ if(0 == get_rx_link_statistics(&link_statistics, itf_list[i].media_type,
+ local_itf_mac, neighbor_itf_mac))
+ {
+ return 0;//return length = 0
+ }
+
+ /*fill into rx packets error*/
+ *temp_buf = (unsigned char)(link_statistics.error_rx_packets >> 24) & 0xFF;
+ *(temp_buf + 1) = (unsigned char)(link_statistics.error_rx_packets >> 16) & 0xFF;
+ *(temp_buf + 2) = (unsigned char)(link_statistics.error_rx_packets >> 8) & 0xFF;
+ *(temp_buf + 3) = (unsigned char)(link_statistics.error_rx_packets) & 0xFF;
+ temp_buf += 4;
+ total_length += 4;
+
+ /*fill into total received packets*/
+ *temp_buf = (unsigned char)(link_statistics.total_rx_packets >> 24) & 0xFF;
+ *(temp_buf + 1) = (unsigned char)(link_statistics.total_rx_packets >> 16) & 0xFF;
+ *(temp_buf + 2) = (unsigned char)(link_statistics.total_rx_packets >> 8) & 0xFF;
+ *(temp_buf + 3) = (unsigned char)(link_statistics.total_rx_packets) & 0xFF;
+ temp_buf += 4;
+ total_length += 4;
+
+ /*fill int RSSI value*/
+ if(0 == get_link_rssi(&rssi, neighbor_itf_mac, itf_list[i].media_type))
+ {
+ return 0;
+ }
+
+ *temp_buf = rssi;
+ temp_buf += 1;
+ total_length += 1;
+
+ /*calculate totoal length & fill into the length field*/
+ *(pkt + 1) = (unsigned char)(((total_length - 3) >> 8) & 0xff);
+ *(pkt + 2) = (unsigned char)((total_length - 3) & 0xff);
+
+ return total_length;
+}
+
+unsigned short append_link_metrics_result_code_tlv(unsigned char *pkt)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+
+ temp_buf = pkt;
+
+ *temp_buf = RESULT_CODE_TLV_TYPE;
+ temp_buf +=1;
+ total_length += 1;
+
+ /*calculate totoal length & fill into the length field*/
+ *temp_buf = (unsigned char)((LINK_METRIC_RESULT_CODE_LENGTH >> 8) & 0xff);
+ *(temp_buf + 1) = (unsigned char)(LINK_METRIC_RESULT_CODE_LENGTH & 0xff);
+
+ temp_buf +=2;
+ total_length += 2;
+
+ /*0x00: Invalid neighbor, other value: reserved*/
+ *temp_buf = 0x0;
+ temp_buf +=1;
+ total_length += 1;
+ return total_length;
+}
+
+unsigned short append_push_button_event_notification_tlv(unsigned char *pkt,
+ struct p1905_interface *itf_list)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+ int i = 0, launch_itf_amount = 0;
+
+ temp_buf = pkt;
+
+ *temp_buf = PUSH_BUTTON_EVENT_NOTIFICATION_TYPE;
+ temp_buf +=1;
+ total_length += 1;
+
+ /*shift the tlvLength field firstly*/
+ temp_buf +=2;
+ total_length += 2;
+
+ for(i=0;i<ITF_NUM;i++)
+ {
+ if(((itf_list[i].media_type >= IEEE1901_GROUP) &&
+ (itf_list[i].media_type < MOCA_GROUP)))
+ launch_itf_amount++;
+#ifdef SUPPORT_WIFI
+ else if(((itf_list[i].media_type >= IEEE802_11_GROUP) &&
+ (itf_list[i].media_type < IEEE1901_GROUP)))
+ {
+ /*if wifi interface is AP, need to launch PBC configuration*/
+ if((*((itf_list[i].vs_info) + 6)) == 0)
+ launch_itf_amount++;
+ #ifdef SUPPORT_WIFI_STATION
+ /* if wifi interface is station and no link, need to launch
+ * PBC configuration, implement in future
+ */
+ #endif
+ }
+#endif
+ }
+ /*fill into number of media type*/
+ *temp_buf = launch_itf_amount;
+ temp_buf +=1;
+ total_length += 1;
+
+ for(i=0;i<ITF_NUM;i++)
+ {
+ if(((itf_list[i].media_type >= IEEE1901_GROUP) &&
+ (itf_list[i].media_type < MOCA_GROUP)))
+ {
+ /*media type field, 2 octets*/
+ *temp_buf = (itf_list[i].media_type >> 8) & 0xFF;
+ *(temp_buf + 1) = itf_list[i].media_type & 0xFF;
+ temp_buf += 2;
+ total_length += 2;
+
+ /*media specific info length*/
+ *temp_buf = 7;
+ temp_buf += 1;
+ total_length += 1;
+
+ /*media specific information*/
+ if(!get_network_id(itf_list[i].mac_addr, itf_list[i].vs_info))
+ {
+ debug_syslog("get network id fail in PB notify tlv\n");
+ }
+ memcpy(temp_buf, itf_list[i].vs_info, itf_list[i].vs_info_length);
+ temp_buf += itf_list[i].vs_info_length;
+ total_length += itf_list[i].vs_info_length;
+ }
+#ifdef SUPPORT_WIFI
+ else if(((itf_list[i].media_type >= IEEE802_11_GROUP) &&
+ (itf_list[i].media_type < IEEE1901_GROUP)))
+ {
+ /*media type field, 2 octets*/
+ *temp_buf = (itf_list[i].media_type >> 8) & 0xFF;
+ *(temp_buf + 1) = itf_list[i].media_type & 0xFF;
+ temp_buf += 2;
+ total_length += 2;
+
+ /*media specific info length*/
+ *temp_buf = 10;
+ temp_buf += 1;
+ total_length += 1;
+
+ /*media specific infomation*/
+ /*dot11CurrentChannelWidth*/
+ if(wifi_utils_success != get_channel_width((itf_list[i].vs_info) + 7))
+ {
+ debug_syslog("get wifi channel width fail in PBC notify tlv\n");
+ }
+
+ /*dot11CurrentChannelCenterFrequencyIndex0*/
+ if(wifi_utils_success != get_center_freq_index((itf_list[i].vs_info) + 8))
+ {
+ debug_syslog("get wifi center freq fail in PBC notify tlv\n");
+ }
+
+ /* we use 802.11n device, dot11CurrentChannelCenterFrequencyIndex1 = 0*/
+ *((itf_list[i].vs_info) + 9) = 0;
+
+ memcpy(temp_buf,itf_list[i].vs_info,itf_list[i].vs_info_length);
+ temp_buf += itf_list[i].vs_info_length;
+ total_length += itf_list[i].vs_info_length;
+ }
+#endif
+ }
+
+ /*calculate totoal length & fill into the length field*/
+ *(pkt + 1) = (unsigned char)(((total_length - 3) >> 8) & 0xff);
+ *(pkt + 2) = (unsigned char)((total_length - 3) & 0xff);
+
+ return total_length;
+}
+
+unsigned short append_push_button_join_notification_tlv(unsigned char *pkt,
+ unsigned char *al_id, unsigned short mid,
+ unsigned char *local_mac, unsigned char *new_device_mac)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+
+ temp_buf = pkt;
+
+ *temp_buf = PUSH_BUTTON_JOIN_NOTIFICATION_TYPE;
+ temp_buf +=1;
+
+ *(temp_buf) = (unsigned char)((PUSH_BUTTON_JOIN_NOTIFICATION_LENGTH & 0xFF00) >> 8);
+ *(temp_buf + 1) = (unsigned char)(PUSH_BUTTON_JOIN_NOTIFICATION_LENGTH & 0x00FF);
+ temp_buf += 2;
+ total_length = PUSH_BUTTON_JOIN_NOTIFICATION_LENGTH + 3;
+
+ /*fill into al id field*/
+ memcpy(temp_buf, al_id, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+
+ /*fill into mid field*/
+ *(temp_buf) = (unsigned char)((mid & 0xFF00) >> 8);
+ *(temp_buf + 1) = (unsigned char)(mid & 0x00FF);
+ temp_buf += 2;
+
+ /*fill into local plc mac*/
+ memcpy(temp_buf, local_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+
+ /*fill into new joined plc mac*/
+ memcpy(temp_buf, new_device_mac, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+
+ return total_length;
+}
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+#ifdef SUPPORT_AP_ENROLLE
+unsigned short append_searched_role_tlv(
+ unsigned char *pkt)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+
+ temp_buf = pkt;
+
+ *temp_buf = SEARCH_ROLE_TLV_TYPE;
+ temp_buf +=1;
+
+ (*temp_buf) = (unsigned char)((SEARCH_ROLE_LENGTH & 0xFF00) >> 8);
+ (*(temp_buf+1)) = (unsigned char)(SEARCH_ROLE_LENGTH & 0x00FF);
+ temp_buf += 2;
+ total_length = SEARCH_ROLE_LENGTH + 3;
+
+ *temp_buf = ROLE_REGISTRAR;
+ return total_length;
+}
+
+unsigned short append_autoconfig_freq_band_tlv(
+ unsigned char *pkt)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+ RF_BAND band = rf_band_2p4G;
+
+ temp_buf = pkt;
+
+ *temp_buf = AUTO_CONFIG_FREQ_BAND_TLV_TYPE;
+ temp_buf +=1;
+
+ (*temp_buf) = (unsigned char)((AUTOCONFIG_FREQ_BAND_LENGTH & 0xFF00) >> 8);
+ (*(temp_buf+1)) = (unsigned char)(AUTOCONFIG_FREQ_BAND_LENGTH & 0x00FF);
+ temp_buf += 2;
+ total_length = AUTOCONFIG_FREQ_BAND_LENGTH + 3;
+
+ if(wifi_utils_success != get_rf_band(&band))
+ {
+ debug_syslog("append auto config freq band error\n");
+ band = rf_band_2p4G;//if we got error, use default
+ }
+
+ /*0x00: 2.4G, 0x01:5G, 0x02:60G, no 60G case now*/
+ if(band == rf_band_5G)
+ *temp_buf = 0x1;
+ else
+ *temp_buf = 0x0;
+
+ return total_length;
+}
+#endif
+
+#ifdef SUPPORT_AP_REGISTRAR
+unsigned short append_supported_role_tlv(
+ unsigned char *pkt)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+
+ temp_buf = pkt;
+
+ *temp_buf = SUPPORT_ROLE_TLV_TYPE;
+ temp_buf +=1;
+
+ (*temp_buf) = (unsigned char)((SUPPORTED_ROLE_LENGTH & 0xFF00) >> 8);
+ (*(temp_buf+1)) = (unsigned char)(SUPPORTED_ROLE_LENGTH & 0x00FF);
+ temp_buf += 2;
+ total_length = SUPPORTED_ROLE_LENGTH + 3;
+
+ *temp_buf = ROLE_REGISTRAR;
+ return total_length;
+}
+
+unsigned short append_supported_freq_band_tlv(
+ unsigned char *pkt)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+ RF_BAND band = rf_band_2p4G;
+
+ temp_buf = pkt;
+
+ *temp_buf = SUPPORT_FREQ_BAND_TLV_TYPE;
+ temp_buf +=1;
+
+ (*temp_buf) = (unsigned char)((SUPPORTED_FREQ_BAND_LENGTH & 0xFF00) >> 8);
+ (*(temp_buf+1)) = (unsigned char)(SUPPORTED_FREQ_BAND_LENGTH & 0x00FF);
+ temp_buf += 2;
+ total_length = SUPPORTED_FREQ_BAND_LENGTH + 3;
+
+ if(wifi_utils_success != get_rf_band(&band))
+ {
+ debug_syslog("append auto config freq band error\n");
+ band = rf_band_2p4G;//if we got error, use default
+ }
+
+ /*0x00: 2.4G, 0x01:5G, 0x02:60G, no 60G case now*/
+ if(band == rf_band_5G)
+ *temp_buf = 0x1;
+ else
+ *temp_buf = 0x0;
+
+ return total_length;
+}
+#endif
+
+unsigned short append_WSC_tlv(
+ unsigned char *pkt, struct p1905_managerd_ctx *ctx, unsigned char wsc_type)
+{
+ unsigned short total_length = 0;
+ unsigned char *temp_buf;
+ WSC_ATTR_STATUS status = wsc_attr_success;
+ unsigned short wsc_length = 0;
+
+ temp_buf = pkt;
+
+ *temp_buf = WSC_TLV_TYPE;
+ temp_buf +=1;
+ total_length += 1;
+
+ /*shift the tlvLength field firstly*/
+ temp_buf +=2;
+ total_length += 2;
+
+#if defined(SUPPORT_AP_ENROLLE)
+ if(wsc_type == MESSAGE_TYPE_M1)
+ status = create_wsc_msg_M1(ctx, temp_buf, &wsc_length);
+#elif defined(SUPPORT_AP_REGISTRAR)
+ if(wsc_type == MESSAGE_TYPE_M2)
+ status = create_wsc_msg_M2(ctx, temp_buf, &wsc_length);
+#else
+ #error "not define AP Enrolle or AP Registrar"
+#endif
+
+ if(status != wsc_attr_success)
+ {
+ debug_syslog("create WSC TLV type %d fail\n",wsc_type);
+ total_length = 0;
+ goto end;
+ }
+
+ total_length += wsc_length;
+
+ /*calculate totoal length & fill into the length field*/
+ *(pkt + 1) = (unsigned char)(((total_length - 3) >> 8) & 0xff);
+ *(pkt + 2) = (unsigned char)((total_length - 3) & 0xff);
+
+end:
+ return total_length;
+}
+#endif
diff --git a/cleopatre/devkit/p1905_managerd/src/cmdu_tlv_parse.c b/cleopatre/devkit/p1905_managerd/src/cmdu_tlv_parse.c
new file mode 100644
index 0000000000..7766d6aef0
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/cmdu_tlv_parse.c
@@ -0,0 +1,957 @@
+/*
+ * cleopatre/application/p1905_managerd/src/cmdu_tlv_parse.c
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <syslog.h>
+#include <assert.h>
+#include <linux/if_ether.h>
+#include <asm/byteorder.h>
+
+#include "cmdu_tlv_parse.h"
+
+//#define CMDU_TLV_PARSE_DEBUG
+#ifdef CMDU_TLV_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
+
+#define TOTAL_AL_MAC_ADDR_TLV_LENGTH 9
+#define TOTAL_MAC_ADDR_TLV_LENGTH 9
+
+
+int get_cmdu_tlv_length(unsigned char *buf)
+{
+ unsigned char *temp_buf = buf;
+ int length;
+
+ temp_buf +=1;//shift to length field
+
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+
+ return (length+3);
+}
+
+int parse_al_mac_addr_type_tlv(unsigned char *buf,unsigned char *al_mac)
+{
+ unsigned char *temp_buf;
+ temp_buf = buf;
+
+ if((*temp_buf) == AL_MAC_ADDR_TLV_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+
+ if(*temp_buf == 0x0 && *(temp_buf+1) == 0x6)
+ temp_buf +=2;
+ else
+ {
+ return -1;
+ }
+
+ memcpy(al_mac,temp_buf,ETH_ALEN);
+
+ return TOTAL_AL_MAC_ADDR_TLV_LENGTH;
+}
+
+
+int parse_mac_addr_type_tlv(unsigned char *buf,unsigned char *mac)
+{
+ unsigned char *temp_buf;
+ temp_buf = buf;
+
+ if((*temp_buf) == MAC_ADDR_TLV_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+
+ if(*temp_buf == 0x0 && *(temp_buf+1) == 0x6)
+ temp_buf +=2;
+ else
+ {
+ return -1;
+ }
+
+ memcpy(mac,temp_buf,ETH_ALEN);
+
+ return TOTAL_MAC_ADDR_TLV_LENGTH;
+}
+
+int parse_device_info_type_tlv(unsigned char *buf,
+ struct list_head_devinfo *devinfo_head)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+ unsigned char itfs_num=0;
+ unsigned char vs_info_len =0;
+ int i =0;
+ struct device_info_db *dev;
+ unsigned char mac[ETH_ALEN];
+
+ temp_buf = buf;
+
+ if((*temp_buf) == DEVICE_INFO_TLV_TYPE)
+ {
+ temp_buf++;
+ }
+ else
+ {
+ debug("should not go here\n");
+ return -1;
+ }
+
+ //calculate tlv length
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+
+ //shift to tlv value field
+ temp_buf+=2;
+
+ //skip AL MAC address field
+ temp_buf+=ETH_ALEN;
+
+ //get the amount of local interface
+ itfs_num = *temp_buf;
+ temp_buf +=1;
+
+ for(i=0;i<itfs_num;i++)
+ {
+ memcpy(mac,temp_buf,ETH_ALEN);
+ temp_buf += ETH_ALEN;
+
+ dev = (struct device_info_db *)malloc(sizeof(struct device_info_db));
+ memcpy(dev->mac_addr,mac,ETH_ALEN);
+ /*init the list head of p1905.1 neighbor device list*/
+ SLIST_INIT(&(dev->p1905_nbrdb_head));
+ /*init the list head of non-p1905.1 neighbor device list*/
+ SLIST_INIT(&(dev->non_p1905_nbrdb_head));
+ SLIST_INSERT_HEAD(devinfo_head, dev, devinfo_entry);
+
+ debug("device mac address = 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+ dev->mac_addr[0], dev->mac_addr[1], dev->mac_addr[2],
+ dev->mac_addr[3],dev->mac_addr[4],dev->mac_addr[5]);
+
+ dev->media_type = (*temp_buf);
+ dev->media_type = (dev->media_type << 8) & 0xFF00;
+ dev->media_type |= (*(temp_buf+1));
+ temp_buf +=2;
+ debug("dev->media_type = %d",dev->media_type);
+
+ vs_info_len = (*temp_buf);
+ dev->vs_info_len = vs_info_len;
+ temp_buf +=1;
+
+ debug("vs_info_len = %d\n",vs_info_len);
+ if(vs_info_len > 0)
+ {
+ dev->vs_info = (unsigned char*)malloc(vs_info_len);
+ memcpy(dev->vs_info, temp_buf, vs_info_len);
+ temp_buf += vs_info_len;
+ }
+ }
+
+ return (length+3);
+}
+
+int parse_bridge_capability_type_tlv(unsigned char *buf,
+ struct list_head_brcap *brcap_head)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+ unsigned char br_num=0;
+ int i = 0;
+ unsigned char br_itfs_num=0;
+ struct device_bridge_capability_db *br;
+
+ temp_buf = buf;
+
+ if((*temp_buf) == BRIDGE_CAPABILITY_TLV_TYPE)
+ {
+ temp_buf++;
+ }
+ else
+ {
+ debug("should not go here--bridge capability\n");
+ return -1;
+ }
+
+ //calculate tlv length
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ //shift to tlv value field
+ temp_buf+=2;
+
+ //the field of total number of bridge tuple
+ br_num = *temp_buf;
+ temp_buf++;
+
+ debug("br_num = %d\n",br_num);
+ //loop br_num times to store the bridge tuple information
+ for(i=0;i<br_num;i++)
+ {
+ br_itfs_num = *temp_buf;
+ temp_buf++;
+
+ debug("br_itfs_num = %d\n",br_itfs_num);
+
+ br= (struct device_bridge_capability_db *)malloc(sizeof(struct device_bridge_capability_db));
+ br->interface_amount = br_itfs_num;
+
+ if(br_itfs_num > 0)
+ br->interface_mac_tuple = (unsigned char *)malloc(br_itfs_num*ETH_ALEN);
+
+ memcpy(br->interface_mac_tuple,temp_buf,(br_itfs_num*ETH_ALEN));
+ LIST_INSERT_HEAD(brcap_head, br, brcap_entry);
+
+ temp_buf += br_itfs_num * ETH_ALEN;
+
+ debug("br->interface_amount = %d\n",br->interface_amount);
+#ifdef CMDU_TLV_PARSE_DEBUG
+ for(i=0;i<(br_itfs_num*ETH_ALEN);i++)
+ debug("0x%x ",*(br->interface_mac_tuple + i));
+ debug("\n");
+#endif
+ }
+
+ return (length+3);
+}
+
+int parse_p1905_neighbor_device_type_tlv(unsigned char *buf,
+ struct list_head_devinfo *devinfo_head)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+ struct p1905_neighbor_device_db *dev;
+ struct device_info_db *dev_info;
+ unsigned char local_mac[ETH_ALEN];
+ unsigned char al_mac[ETH_ALEN];
+ unsigned char exist = 0;
+ unsigned char new_db = 1;
+ int tuple_num = 0;
+ int i = 0;
+
+ temp_buf = buf;
+
+ if((*temp_buf) == P1905_NEIGHBOR_DEV_TLV_TYPE)
+ {
+ temp_buf++;
+ }
+ else
+ {
+ debug("should not go here--p1905.1 neighbor device\n");
+ return -1;
+ }
+
+ /*calculate tlv length*/
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ /*shift to tlv value field*/
+ temp_buf+=2;
+
+ memcpy(local_mac,temp_buf,ETH_ALEN);
+ temp_buf+=ETH_ALEN;
+
+ /* tlv value has one local mac address field(6 bytes) and several
+ * (al mac address field(6 bytes) + 802.1 exist field(1 byte)),
+ * so tlv (value length - 6 ) % 7 must be zero
+ */
+ if(((length - 6) % 7) == 0)
+ {
+ tuple_num = (length - 6) /7;
+ debug("tuple_num = %d\n",tuple_num);
+ }
+ else
+ {
+ debug("ignore p1905.1 neighbor tlv\n");
+ return (length+3);
+ }
+
+ SLIST_FOREACH(dev_info, devinfo_head, devinfo_entry)
+ {
+ if(!memcmp(local_mac, dev_info->mac_addr, ETH_ALEN))
+ {
+ exist = 1;
+ break;
+ }
+ }
+
+ if(exist)
+ {
+ for(i=0;i<tuple_num;i++)
+ {
+ memcpy(al_mac, temp_buf, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+
+ new_db = 1;
+
+ /* maybe we don't needt to do this check because the old topology
+ * response db will be deleted when we get a new topology response
+ * message.
+ */
+ SLIST_FOREACH(dev, &(dev_info->p1905_nbrdb_head), p1905_nbrdb_entry)
+ {
+ if(!memcmp(al_mac, dev->p1905_neighbor_al_mac, ETH_ALEN))
+ {
+ new_db = 0;
+ break;
+ }
+ }
+
+ if(new_db)
+ {
+ dev = (struct p1905_neighbor_device_db *)malloc(sizeof(struct p1905_neighbor_device_db));
+ memcpy(dev->p1905_neighbor_al_mac, al_mac, ETH_ALEN);
+ SLIST_INSERT_HEAD(&(dev_info->p1905_nbrdb_head), dev, p1905_nbrdb_entry);
+ }
+
+ if(((*temp_buf) & 0x80) == 0x80)
+ dev->ieee_802_1_bridge_exist = 1;
+ else
+ dev->ieee_802_1_bridge_exist = 0;
+
+ temp_buf += 1;
+ }
+ }
+
+ return (length+3);
+}
+
+int parse_non_p1905_neighbor_device_type_tlv(unsigned char *buf,
+ struct list_head_devinfo *devinfo_head)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+ int tuple_num = 0;
+ unsigned char local_mac[ETH_ALEN];
+ unsigned char neighbor_mac[ETH_ALEN];
+ struct device_info_db *dev_info;
+ struct non_p1905_neighbor_device_list_db *dev;
+
+ unsigned char exist = 0;
+ unsigned char new_db = 1;
+ int i = 0;
+
+ temp_buf = buf;
+
+ if((*temp_buf) == NON_P1905_NEIGHBOR_DEV_TLV_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+
+ /*calculate tlv length*/
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ /*shift to tlv value field*/
+ temp_buf+=2;
+
+ /*get local interface field*/
+ memcpy(local_mac,temp_buf,ETH_ALEN);
+ temp_buf+=ETH_ALEN;
+
+ /*calculate how many non-1905.1 device*/
+ if(((length - 6) % 6) == 0)
+ {
+ tuple_num = (length - 6) /6;
+ debug("tuple_num = %d\n",tuple_num);
+ }
+ else
+ {
+ debug("ignore non p1905.1 neighbor tlv\n");
+ return (length+3);
+ }
+
+ SLIST_FOREACH(dev_info, devinfo_head, devinfo_entry)
+ {
+ if(!memcmp(local_mac,dev_info->mac_addr,ETH_ALEN))
+ {
+ exist = 1;
+ break;
+ }
+ }
+
+ if(exist)
+ {
+ for(i=0;i<tuple_num;i++)
+ {
+ memcpy(neighbor_mac, temp_buf, ETH_ALEN);
+ temp_buf += ETH_ALEN;
+
+ new_db = 1;
+ /* maybe we don't need to do this check because the old topology
+ * response db will be deleted when we get a new topology response
+ * message.
+ */
+ SLIST_FOREACH(dev, &(dev_info->non_p1905_nbrdb_head), non_p1905_nbrdb_entry)
+ {
+ if(!memcmp(neighbor_mac, dev->non_p1905_device_interface_mac,ETH_ALEN))
+ {
+ new_db = 0;
+ break;
+ }
+ }
+ if(new_db)
+ {
+ dev = (struct non_p1905_neighbor_device_list_db *)\
+ malloc(sizeof(struct non_p1905_neighbor_device_list_db));
+ memcpy(dev->non_p1905_device_interface_mac, neighbor_mac, ETH_ALEN);
+ SLIST_INSERT_HEAD(&(dev_info->non_p1905_nbrdb_head), dev, non_p1905_nbrdb_entry);
+ }
+ }
+ }
+ return (length+3);
+}
+
+int parse_vendor_specific_type_tlv(unsigned char *buf,struct p1905_vs_info *vs_info)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+
+ temp_buf = buf;
+
+ if((*temp_buf) == VENDOR_SPECIFIC_TLV_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+
+ /*calculate tlv length*/
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ /*shift to tlv value field*/
+ temp_buf+=2;
+
+ /*check OUI*/
+ if(memcmp(OUI_SPIDCOM, temp_buf, OUI_LEN))
+ {
+ debug("not local OUI\n");
+ //return (length+3);
+
+ /*it is not our OUI, so do not parse this tlv*/
+ return -1;
+ }
+
+ temp_buf += OUI_LEN;
+ memcpy(vs_info->al_mac,temp_buf,ETH_ALEN);
+
+ return (length+3);
+}
+
+int parse_link_metric_query_type_tlv(unsigned char *buf,
+ unsigned char *target, unsigned char *type)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+
+ temp_buf = buf;
+
+ if((*temp_buf) == LINK_METRICS_QUERY_TLV_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+
+ /*calculate tlv length*/
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ /*shift to tlv value field*/
+ temp_buf+=2;
+
+ /*set target mac = 0 if query all neighbors*/
+ if(*temp_buf == QUERY_ALL_NEIGHBOR)
+ {
+ memset(target, 0, ETH_ALEN);
+ temp_buf+=1;
+ }
+ else if(*temp_buf == QUERY_SPECIFIC_NEIGHBOR)
+ {
+ temp_buf+=1;
+ memcpy(target, temp_buf, ETH_ALEN);
+ temp_buf+=ETH_ALEN;
+ }
+
+ if(*temp_buf == TX_METRICS_ONLY)
+ *type = TX_METRICS_ONLY;
+ else if(*temp_buf == RX_METRICS_ONLY)
+ *type = RX_METRICS_ONLY;
+ else if(*temp_buf == BOTH_TX_AND_RX_METRICS)
+ *type = BOTH_TX_AND_RX_METRICS;
+
+ return (length+3);
+}
+
+#ifdef SUPPORT_ALME
+int parse_transmitter_link_metrics_type_tlv(unsigned char *buf,
+ unsigned char *local_al_mac, ALME_GET_METRIC_RSP *metric_rsp)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+ unsigned char response_al_mac[6];
+ unsigned short link_tuple_num = 0;
+ int i = 0;
+ unsigned int value;
+
+
+ temp_buf = buf;
+
+ if((*temp_buf) == TRANSMITTER_LINK_METRIC_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+
+ /*calculate tlv length*/
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ /*shift to tlv value field*/
+ temp_buf+=2;
+
+ /*transmit device's al mac field*/
+ memcpy(response_al_mac, temp_buf, 6);
+ temp_buf += 6;
+
+ /*shift to neighbor al mac field*/
+ /*if the neighbor al mac is not local, return fail*/
+ if(memcmp(local_al_mac, temp_buf, 6))
+ {
+ debug_syslog("got a wrong target tx link metrics tlv\n");
+ return -1;
+ }
+ temp_buf += 6;
+
+ if(((length - 12) % 29) != 0)
+ {
+ debug_syslog("got a wrong length tx link metrics tlv\n");
+ return -1;
+ }
+
+ link_tuple_num = (length - 12) /29;
+
+ if(link_tuple_num > MAX_1905_LINK_NUM)
+ {
+ debug_syslog("got a wrong link num tx link metrics tlv\n");
+ return -1;
+ }
+
+ metric_rsp->descriptor_num = link_tuple_num;
+ for(i=0;i<link_tuple_num;i++)
+ {
+ /*fill into neighbor al mac field first*/
+ memcpy(metric_rsp->descriptorlist[i].neighbor_al_mac, response_al_mac, 6);
+
+ /*shift to interface mac addr of neighbor field*/
+ temp_buf += 6;
+ /*fill into local interface field in alme*/
+ memcpy(metric_rsp->descriptorlist[i].local_itf_mac, temp_buf, 6);
+ temp_buf += 6;
+
+ /*intf type*/
+ value = *(temp_buf);
+ value = ((value << 8) & 0x0000FF00)| (*(temp_buf + 1));
+ metric_rsp->descriptorlist[i].link_metrics.itf_type = (unsigned short)value;
+ temp_buf += 2;
+
+ /*ieee802.1 bridge*/
+ metric_rsp->descriptorlist[i].ieee802_1_bridge = *(temp_buf);
+ temp_buf += 1;
+
+ /*packet error*/
+ value = *(temp_buf);
+ value = ((value << 8) & 0x0000FF00)| (*(temp_buf + 1));
+ value = ((value << 8) & 0x00FFFF00)| (*(temp_buf + 2));
+ value = ((value << 8) & 0xFFFFFF00)| (*(temp_buf + 3));
+ metric_rsp->descriptorlist[i].link_metrics.tx_packet_error = value;
+ temp_buf += 4;
+
+ /*transmitted packets*/
+ value = *(temp_buf);
+ value = ((value << 8) & 0x0000FF00)| (*(temp_buf + 1));
+ value = ((value << 8) & 0x00FFFF00)| (*(temp_buf + 2));
+ value = ((value << 8) & 0xFFFFFF00)| (*(temp_buf + 3));
+ metric_rsp->descriptorlist[i].link_metrics.tx_total_packet = value;
+ temp_buf += 4;
+
+ /*mac throghput capacity*/
+ value = *(temp_buf);
+ value = ((value << 8) & 0x0000FF00)| (*(temp_buf + 1));
+ metric_rsp->descriptorlist[i].link_metrics.max_throughput_capacity = (unsigned short)value;
+ temp_buf += 2;
+
+ /*link availability*/
+ value = *(temp_buf);
+ value = ((value << 8) & 0x0000FF00)| (*(temp_buf + 1));
+ metric_rsp->descriptorlist[i].link_metrics.link_availability = (unsigned short)value;
+ temp_buf += 2;
+
+ /*phy rate*/
+ value = *(temp_buf);
+ value = ((value << 8) & 0x0000FF00)| (*(temp_buf + 1));
+ metric_rsp->descriptorlist[i].link_metrics.phy_rate = (unsigned short)value;
+ temp_buf += 2;
+ }
+
+ return (length+3);
+}
+
+int parse_receiver_link_metrics_type_tlv(unsigned char *buf,
+ unsigned char *local_al_mac, ALME_GET_METRIC_RSP *metric_rsp)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+ unsigned char response_al_mac[6];
+ unsigned short link_tuple_num = 0;
+ int i = 0, j = 0;
+ unsigned int value;
+
+ temp_buf = buf;
+
+ if((*temp_buf) == RECEIVER_LINK_METRIC_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+
+ /*calculate tlv length*/
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ /*shift to tlv value field*/
+ temp_buf+=2;
+
+ /*transmit device's al mac field*/
+ memcpy(response_al_mac, temp_buf, 6);
+ temp_buf += 6;
+
+ /*shift to neighbor al mac field*/
+ /*if the neighbor al mac is not local, return fail*/
+ if(memcmp(local_al_mac, temp_buf, 6))
+ {
+ debug_syslog("got a wrong target rx link metrics tlv\n");
+ return -1;
+ }
+ temp_buf += 6;
+
+ if(((length - 12) % 23) != 0)
+ {
+ debug_syslog("got a wrong length rx link metrics tlv\n");
+ return -1;
+ }
+
+ link_tuple_num = (length - 12) /23;
+
+ /*compare link tuple num with tx and max link number*/
+ if((link_tuple_num > MAX_1905_LINK_NUM) || (link_tuple_num != metric_rsp->descriptor_num))
+ {
+ debug_syslog("got a wrong link num rx link metrics tlv %d\n",link_tuple_num);
+ return -1;
+ }
+
+ for(i=0;i<link_tuple_num;i++)
+ {
+ /*shift to interface mac addr of neighbor field*/
+ temp_buf += 6;
+ /*get correct position in metric_rsp*/
+ for(j=0;j<link_tuple_num;j++)
+ {
+ if(!memcmp(temp_buf, metric_rsp->descriptorlist[j].local_itf_mac, 6))
+ break;
+ }
+ if(j == link_tuple_num)
+ {
+ debug_syslog("got un-matched neighbor interface in rx link metric tlv\n");
+ return -1;
+ }
+ temp_buf += 6;
+ /*ignore iftype field*/
+ temp_buf += 2;
+ /*packet error*/
+ value = *(temp_buf);
+ value = ((value << 8) & 0x0000FF00)| (*(temp_buf + 1));
+ value = ((value << 8) & 0x00FFFF00)| (*(temp_buf + 2));
+ value = ((value << 8) & 0xFFFFFF00)| (*(temp_buf + 3));
+ metric_rsp->descriptorlist[j].link_metrics.rx_packet_error = value;
+ temp_buf += 4;
+ /*total packet*/
+ value = *(temp_buf);
+ value = ((value << 8) & 0x0000FF00)| (*(temp_buf + 1));
+ value = ((value << 8) & 0x00FFFF00)| (*(temp_buf + 2));
+ value = ((value << 8) & 0xFFFFFF00)| (*(temp_buf + 3));
+ metric_rsp->descriptorlist[j].link_metrics.rx_total_packet = value;
+ temp_buf += 4;
+ /*RSSI*/
+ metric_rsp->descriptorlist[i].link_metrics.rssi = *(temp_buf);
+ temp_buf += 1;
+ }
+
+ return (length+3);
+}
+
+int parse_link_metrics_result_code_type_tlv(unsigned char *buf)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+
+ temp_buf = buf;
+ /*shift to length field*/
+ temp_buf++;
+
+ /*calculate tlv length*/
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ /*shift to tlv value field*/
+ temp_buf+=2;
+
+ /*get result of invalid neighbor*/
+ if((*temp_buf) == 0)
+ return -1;
+
+ return (length + 3);
+}
+#endif
+
+#ifdef SUPPORT_AP_REGISTRAR
+int parse_push_button_event_notification_tlv(unsigned char *buf,
+ iee802_11_info *wifi_info)
+{
+ unsigned char *temp_buf;
+ int length = 0, i = 0;
+ unsigned char num_media = 0;
+ unsigned short media_type;
+ unsigned char media_info_len = 0;
+
+ temp_buf = buf;
+ /*shift to length field*/
+ temp_buf++;
+
+ /*calculate tlv length*/
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ /*shift to tlv value field*/
+ temp_buf+=2;
+
+ num_media = *temp_buf;
+ temp_buf++;
+
+ if(num_media > 0)
+ {
+ for(i=0;i<num_media;i++)
+ {
+ media_type = *temp_buf;
+ media_type = ((media_type << 8) & 0xFF00)| (*(temp_buf + 1));
+ temp_buf += 2;
+
+ debug_syslog("media_type in pb event notify = 0x%x\n",media_type);
+ media_info_len = *temp_buf;
+ temp_buf += 1;
+ /* AP is a registrar, and the received push button event notification
+ * carried the wifi media information, do not launch PBC sequece
+ * on wifi AP
+ */
+ if((media_type >= IEEE802_11_GROUP) && (media_type < IEEE1901_GROUP))
+ {
+ wifi_info->no_need_start_pbc = 1;
+ }
+ temp_buf += media_info_len;
+ }
+ }
+
+ return (length + 3);
+}
+#endif
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+#ifdef SUPPORT_AP_REGISTRAR
+int parse_search_role_tlv(
+ unsigned char *buf)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+
+ temp_buf = buf;
+
+ if((*temp_buf) == SEARCH_ROLE_TLV_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ temp_buf+=2;
+
+ if(length != SEARCH_ROLE_LENGTH)
+ return -1;
+
+ if((*temp_buf) != ROLE_REGISTRAR)
+ return -1;
+
+ return (length+3);
+}
+
+int parse_auto_config_freq_band_tlv(
+ unsigned char *buf)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+ RF_BAND band = rf_band_2p4G;
+
+ temp_buf = buf;
+ if((*temp_buf) == AUTO_CONFIG_FREQ_BAND_TLV_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ temp_buf+=2;
+
+ if(length != AUTOCONFIG_FREQ_BAND_LENGTH)
+ return -1;
+
+ if(wifi_utils_success != get_rf_band(&band))
+ {
+ debug_syslog("parse auto config freq band error\n");
+ band = rf_band_2p4G;
+ }
+
+ if(((band == rf_band_2p4G) && ((*temp_buf) != IEEE802_11_band_2P4G)) ||
+ ((band == rf_band_5G) && ((*temp_buf) != IEEE802_11_band_5G)))
+ {
+ debug_syslog("got a non-matched freq band\n");
+ return -1;
+ }
+
+ return (length+3);
+}
+
+#endif
+
+#ifdef SUPPORT_AP_ENROLLE
+int parse_supported_role_tlv(
+ unsigned char *buf)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+
+ temp_buf = buf;
+
+ if((*temp_buf) == SUPPORT_ROLE_TLV_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ temp_buf+=2;
+
+ if(length != SUPPORTED_ROLE_LENGTH)
+ return -1;
+ if((*temp_buf) != ROLE_REGISTRAR)
+ return -1;
+
+ return (length+3);
+}
+
+int parse_supported_freq_band_tlv(
+ unsigned char *buf)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+ RF_BAND band = rf_band_2p4G;
+
+ temp_buf = buf;
+
+ if((*temp_buf) == SUPPORT_FREQ_BAND_TLV_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ temp_buf+=2;
+
+ if(length != SUPPORTED_FREQ_BAND_LENGTH)
+ return -1;
+
+ if(wifi_utils_success != get_rf_band(&band))
+ {
+ debug_syslog("parse supported freq band error\n");
+ band = rf_band_2p4G;
+ }
+
+ if(((band == rf_band_2p4G) && ((*temp_buf) != IEEE802_11_band_2P4G)) ||
+ ((band == rf_band_5G) && ((*temp_buf) != IEEE802_11_band_5G)))
+ {
+ debug_syslog("non-matched freq band, band = %d, (*temp_buf) = %d\n",band, (*temp_buf));
+ return -1;
+ }
+
+ return (length+3);
+}
+#endif
+
+int parse_wsc_tlv(
+ unsigned char *buf, struct p1905_managerd_ctx *ctx)
+{
+ unsigned char *temp_buf;
+ int length = 0;
+
+ temp_buf = buf;
+
+ if((*temp_buf) == WSC_TLV_TYPE)
+ temp_buf++;
+ else
+ {
+ return -1;
+ }
+
+ /*calculate tlv length*/
+ length = (*temp_buf);
+ length = (length << 8) & 0xFF00;
+ length = length |(*(temp_buf+1));
+ /*shift to tlv value field*/
+ temp_buf+=2;
+
+ if(wsc_attr_success != parse_wsc_msg(ctx, temp_buf, length))
+ return -1;
+
+ return (length+3);
+}
+#endif
diff --git a/cleopatre/devkit/p1905_managerd/src/lldp.c b/cleopatre/devkit/p1905_managerd/src/lldp.c
new file mode 100644
index 0000000000..cdc0f14007
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/lldp.c
@@ -0,0 +1,126 @@
+/*
+ * cleopatre/application/p1905_managerd/src/lldp.c
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#include <stdio.h>
+#include <string.h>
+#include <net/if.h>
+#include <syslog.h>
+#include <assert.h>
+#include <linux/if_ether.h>
+#include <asm/byteorder.h>
+#include <sys/socket.h>
+#include <errno.h>
+#include <linux/if_packet.h>
+#include <sys/ioctl.h>
+#include "lldp.h"
+
+//#define LLDP_DEBUG
+#ifdef LLDP_DEBUG
+#define debug(...) printf(__VA_ARGS__)
+#else
+#define debug(...)
+#endif
+
+
+#define LLDP_IFNAME "br0"
+
+int lldpdu_init(struct p1905_managerd_ctx *ctx)
+{
+ struct ifreq ifr;
+
+ /*Create a receive connection of lldpdu on bridge interface*/
+ if(0 > (ctx->sock_lldp = socket(AF_PACKET, SOCK_RAW, ETH_P_LLDP)))
+ {
+ syslog(LOG_WARNING, "cannot open lldpdu socket on %s (%s)", LLDP_IFNAME,\
+ strerror(errno));
+ return -1;
+ }
+
+ /*Prepare lldpdu socket interface index*/
+ strncpy(ifr.ifr_name, (char*)LLDP_IFNAME, IFNAMSIZ);
+ if(-1 == (ioctl(ctx->sock_lldp, SIOCGIFINDEX, &ifr)))
+ {
+ syslog(LOG_WARNING, "cannot get interface %s index (%s) for lldpdu",\
+ LLDP_IFNAME, strerror(errno));
+ close(ctx->sock_lldp);
+ return -1;
+ }
+
+ ctx->lldp_sll.sll_family = AF_PACKET;
+ ctx->lldp_sll.sll_ifindex = ifr.ifr_ifindex;
+ ctx->lldp_sll.sll_protocol = htons(ETH_P_LLDP);
+
+ /*Bind lldpdu socket to this interface*/
+ if(-1 == (bind(ctx->sock_lldp, (struct sockaddr *)&ctx->lldp_sll,\
+ sizeof(struct sockaddr_ll))))
+ {
+ syslog(LOG_WARNING, "cannot bind raw socket to interface %s (%s)",\
+ LLDP_IFNAME, strerror(errno));
+ close(ctx->sock_lldp);
+ return -1;
+ }
+
+ /*init lldp rx queue*/
+ init_lldp_rx_queue();
+
+ return 0;
+}
+
+void lldpdu_uninit(struct p1905_managerd_ctx *ctx)
+{
+ delete_lldp_queue_all();
+ close(ctx->sock_lldp);
+}
+
+int send_802_1_bridge_discovery_msg(struct p1905_managerd_ctx *ctx,
+ unsigned char *dmac, unsigned char *smac,
+ unsigned char *buffer)
+{
+ struct ethhdr *eth_hdr;
+ unsigned char *temp_buf;
+ int total_len;
+
+ temp_buf = buffer;
+
+ eth_hdr = (struct ethhdr*)buffer;
+ memcpy(eth_hdr->h_dest, dmac, ETH_ALEN);
+ memcpy(eth_hdr->h_source, smac, ETH_ALEN);
+ eth_hdr->h_proto = htons(ETH_P_LLDP);
+
+ /*shift to start position of lldpdu */
+ temp_buf += ETH_HLEN;
+
+ total_len =\
+ create_802_1_bridge_discovery_message(temp_buf,ctx->p1905_al_mac_addr,smac);
+
+ /*total length = lldpdu length + ether header length*/
+ total_len += ETH_HLEN;
+
+ total_len = send(ctx->sock_lldp, buffer, total_len, 0);
+ if(0 > total_len)
+ {
+ debug("lldpdu send fail\n");
+ syslog(LOG_WARNING, "LLDP send failed on %s (%s)", LLDP_IFNAME,
+ strerror(errno));
+ return -1;
+ }
+
+ return total_len;
+}
+
+int receive_802_1_bridge_discovery_msg(struct p1905_managerd_ctx *ctx,
+ unsigned char *buf, int len)
+{
+ len = recv(ctx->sock_lldp, buf, len, 0);
+ debug(" receive_802_1_bridge_discovery_msg-- len =%d\n",len);
+
+ if(0 >= len)
+ {
+ syslog(LOG_WARNING, "lldp receive failed on %s (%s)", LLDP_IFNAME,
+ strerror(errno));
+ return -1;
+ }
+ return len;
+}
diff --git a/cleopatre/devkit/p1905_managerd/src/lldp_message.c b/cleopatre/devkit/p1905_managerd/src/lldp_message.c
new file mode 100644
index 0000000000..96c7d45ee2
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/lldp_message.c
@@ -0,0 +1,147 @@
+/*
+ * cleopatre/application/p1905_managerd/src/lldp_message.c
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#include <stdio.h>
+#include <string.h>
+#include <net/if.h>
+#include <syslog.h>
+#include <assert.h>
+#include <linux/if_ether.h>
+#include <asm/byteorder.h>
+#include <lldp_message.h>
+
+#define LLDP_TIME_TO_LIVE 180 //180 seconds
+
+unsigned short append_lldpdu_chassis_id_tlv(unsigned char *pkt,
+ unsigned char *al_mac)
+{
+ lldpdu_message_header msg_hdr;
+ unsigned char *temp_buf;
+
+ temp_buf = pkt;
+
+ msg_hdr.lldpdu_msg.tlv_type = CHASSIS_ID_TLV_TYPE;
+ /*sting length = cahssis id subtype(1 octect) + mac addr(6 otects)*/
+ msg_hdr.lldpdu_msg.info_str_len = 7;
+
+ *temp_buf = (unsigned char)(msg_hdr.lldpdu_hdr >> 8);
+ *(temp_buf + 1) = (unsigned char)(msg_hdr.lldpdu_hdr & 0xff);
+
+
+ /*shift to chassis id subtype field*/
+ temp_buf += LLDPDU_HLEN;
+ /*set subtype to 4 ==> MAC ADDRESS*/
+ (*temp_buf) = 4;
+
+ /*shift to chassis id field*/
+ temp_buf++;
+
+ /*set chassis id to p1905.1 AL MAC ADDR*/
+ memcpy(temp_buf,al_mac,ETH_ALEN);
+
+ /*return total length*/
+ return (LLDPDU_HLEN + 7);
+}
+
+unsigned short append_lldpdu_port_id_tlv(unsigned char *pkt,
+ unsigned char *itf_mac)
+{
+ lldpdu_message_header msg_hdr;
+ unsigned char *temp_buf;
+
+ temp_buf = pkt;
+
+ msg_hdr.lldpdu_msg.tlv_type = PORT_ID_TLV_TYPE;
+ /*sting length = port ID subtype(1 octect) + mac addr(6 otects)*/
+ msg_hdr.lldpdu_msg.info_str_len = 7;
+
+ *temp_buf = (unsigned char)(msg_hdr.lldpdu_hdr >> 8);
+ *(temp_buf + 1) = (unsigned char)(msg_hdr.lldpdu_hdr & 0xff);
+
+ /*shift to port id subtype field*/
+ temp_buf += LLDPDU_HLEN;
+ /*set port id subtype to 3 ==> MAC ADDRESS*/
+ (*temp_buf) = 3;
+
+ /*shift to port id field*/
+ temp_buf++;
+
+ /*set port id to MAC ADDR of interface*/
+ memcpy(temp_buf,itf_mac,ETH_ALEN);
+
+ /*return total length*/
+ return (LLDPDU_HLEN + 7);
+}
+
+unsigned short append_lldpdu_time_to_live_tlv(unsigned char *pkt)
+{
+ lldpdu_message_header msg_hdr;
+ unsigned char *temp_buf;
+
+ temp_buf = pkt;
+
+ msg_hdr.lldpdu_msg.tlv_type = TIME_TO_LIVE_TLV_TYPE;
+ /*sting length = 2*/
+ msg_hdr.lldpdu_msg.info_str_len = 2;
+
+ *temp_buf = (unsigned char)(msg_hdr.lldpdu_hdr >> 8);
+ *(temp_buf + 1) = (unsigned char)(msg_hdr.lldpdu_hdr & 0xff);
+
+ /*shift to tlv information field*/
+ temp_buf += LLDPDU_HLEN;
+
+ /*set TTL to 180 seconds*/
+ *temp_buf = 0;
+ *(temp_buf + 1) = LLDP_TIME_TO_LIVE;
+
+ /*return total length*/
+ return (LLDPDU_HLEN + 2);
+}
+
+unsigned short append_lldpdu_end_of_lldpdu_tlv(unsigned char *pkt)
+{
+ lldpdu_message_header msg_hdr;
+ unsigned char *temp_buf;
+
+ temp_buf = pkt;
+
+ msg_hdr.lldpdu_msg.tlv_type = END_OF_LLDPDU_TLV_TYPE;
+ /*sting length = 0*/
+ msg_hdr.lldpdu_msg.info_str_len = 0;
+
+ *temp_buf = (unsigned char)(msg_hdr.lldpdu_hdr >> 8);
+ *(temp_buf + 1) = (unsigned char)(msg_hdr.lldpdu_hdr & 0xff);
+
+ return LLDPDU_HLEN;
+}
+
+unsigned short create_802_1_bridge_discovery_message(unsigned char *buf,
+ unsigned char *al_mac,
+ unsigned char *itf_mac)
+{
+ unsigned short total_len = 0;
+ unsigned char *temp_buf;
+ unsigned short len = 0;
+
+ temp_buf = buf;
+
+ len = append_lldpdu_chassis_id_tlv(temp_buf,al_mac);
+ temp_buf += len;
+ total_len += len;
+
+ len = append_lldpdu_port_id_tlv(temp_buf,itf_mac);
+ temp_buf += len;
+ total_len += len;
+
+ len = append_lldpdu_time_to_live_tlv(temp_buf);
+ temp_buf += len;
+ total_len += len;
+
+ len = append_lldpdu_end_of_lldpdu_tlv(temp_buf);
+ temp_buf += len;
+ total_len += len;
+
+ return total_len;
+}
diff --git a/cleopatre/devkit/p1905_managerd/src/lldp_message_parse.c b/cleopatre/devkit/p1905_managerd/src/lldp_message_parse.c
new file mode 100644
index 0000000000..dfc8374fb6
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/lldp_message_parse.c
@@ -0,0 +1,408 @@
+/*
+ * cleopatre/application/p1905_managerd/src/lldp_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 "lldp_message.h"
+#include "p1905_managerd.h"
+#include "cmdu_message.h"
+#include "lldp_message_parse.h"
+
+//#define lldp_MESSAGE_PARSE_DEBUG
+#ifdef lldp_MESSAGE_PARSE_DEBUG
+#define debug(...) printf(__VA_ARGS__)
+#else
+#define debug(...)
+#endif
+
+extern const unsigned char p1905_multicast_address[6];
+
+LIST_HEAD(list_head_lldprx, lldpdu_rx) lldpdu_head;
+
+int init_lldp_rx_queue(void)
+{
+ LIST_INIT(&lldpdu_head);
+ return 0;
+}
+
+int insert_lldp_rx_queue(unsigned char *receive_itf_mac,
+ unsigned char *port_id_mac, int ttl)
+{
+ unsigned char new_db = 1;
+ struct lldpdu_rx *lldpdu;
+
+ if(!LIST_EMPTY(&lldpdu_head))
+ {
+ LIST_FOREACH(lldpdu, &lldpdu_head, lldpdu_entry)
+ {
+ if(!memcmp(lldpdu->port_id_mac, port_id_mac, ETH_ALEN) &&\
+ !memcmp(lldpdu->receiving_local_itf_mac, receive_itf_mac, ETH_ALEN))
+ {
+ debug("receive lldpdu is already existed\n");
+ lldpdu->ttl = (ttl + 10);
+ new_db = 0;
+ break;
+ }
+ }
+ }
+
+ if(new_db)
+ {
+ lldpdu = (struct lldpdu_rx*)malloc(sizeof(struct lldpdu_rx));
+ memcpy(lldpdu->port_id_mac, port_id_mac, ETH_ALEN);
+ memcpy(lldpdu->receiving_local_itf_mac, receive_itf_mac, ETH_ALEN);
+ lldpdu->ttl = (ttl + 10);
+ LIST_INSERT_HEAD(&lldpdu_head, lldpdu, lldpdu_entry);
+ }
+ return 0;
+}
+
+void update_lldp_queue_ttl(int sec)
+{
+ struct lldpdu_rx *lldpdu, *lldpdu_temp;
+
+ if(!LIST_EMPTY(&lldpdu_head))
+ {
+ lldpdu = LIST_FIRST(&lldpdu_head);
+ while(lldpdu)
+ {
+ lldpdu_temp = LIST_NEXT(lldpdu, lldpdu_entry);
+ lldpdu->ttl -= sec;
+ if(lldpdu->ttl <= 0)
+ {
+ debug("delete lldp database\n");
+ LIST_REMOVE(lldpdu, lldpdu_entry);
+ free(lldpdu);
+ }
+ lldpdu = lldpdu_temp;
+ }
+ }
+}
+
+void delete_lldp_queue_all(void)
+{
+ struct lldpdu_rx *lldpdu, *lldpdu_temp;
+
+ if(!LIST_EMPTY(&lldpdu_head))
+ {
+ lldpdu = LIST_FIRST(&lldpdu_head);
+ while(lldpdu)
+ {
+ lldpdu_temp = LIST_NEXT(lldpdu, lldpdu_entry);
+ LIST_REMOVE(lldpdu, lldpdu_entry);
+ free(lldpdu);
+ lldpdu = lldpdu_temp;
+ }
+ }
+}
+
+int find_lldp_by_port_id_mac(unsigned char *receiving_mac,
+ unsigned char *port_id_mac)
+{
+ struct lldpdu_rx *lldpdu;
+
+ if(!LIST_EMPTY(&lldpdu_head))
+ {
+ LIST_FOREACH(lldpdu, &lldpdu_head, lldpdu_entry)
+ {
+ if((!memcmp(lldpdu->port_id_mac, port_id_mac, ETH_ALEN)) &&\
+ (!memcmp(lldpdu->receiving_local_itf_mac, receiving_mac, ETH_ALEN)))
+ return 1;//find same mac in LLDP storage, return 1
+ }
+ }
+
+ /*not match in LLDP storage, return 0*/
+ return 0;
+}
+
+int lldp_update_802_1_bridge_exist(struct p1905_managerd_ctx *ctx,
+ unsigned char *smac, unsigned char *chassis_mac)
+{
+ unsigned char local_itf_mac[ETH_ALEN];
+ int i = 0;
+ struct list_head *pos;
+ struct p1905_neighbor_info *dev_info;
+
+ if(0 > get_receive_port_addr(smac,local_itf_mac))
+ return -1;
+ debug("lldp receive in %.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
+ local_itf_mac[0], local_itf_mac[1], local_itf_mac[2],
+ local_itf_mac[3], local_itf_mac[4], local_itf_mac[5]);
+ for(i=0;i<ITF_NUM;i++)
+ {
+ debug("%.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
+ ctx->p1905_neighbor_dev[i].local_mac_addr[0],
+ ctx->p1905_neighbor_dev[i].local_mac_addr[1],
+ ctx->p1905_neighbor_dev[i].local_mac_addr[2],
+ ctx->p1905_neighbor_dev[i].local_mac_addr[3],
+ ctx->p1905_neighbor_dev[i].local_mac_addr[4],
+ ctx->p1905_neighbor_dev[i].local_mac_addr[5]
+ );
+
+ if(!memcmp(ctx->p1905_neighbor_dev[i].local_mac_addr, local_itf_mac, ETH_ALEN))
+ break;
+
+ if(i == (ITF_NUM - 1))
+ {
+ debug("no this local interface in lldp_update_802_1_bridge_exist\n");
+ return -1;
+ }
+ }
+
+ if(LIST_EMPTY(&(ctx->p1905_neighbor_dev[i].p1905nbr_head)))
+ {
+ debug("list empty in lldp_update_802_1_bridge_exist\n");
+ return -1;
+ }
+
+ LIST_FOREACH(dev_info, &(ctx->p1905_neighbor_dev[i].p1905nbr_head), p1905nbr_entry)
+ {
+ if(!memcmp(dev_info->al_mac_addr, chassis_mac, ETH_ALEN))
+ {
+ /* receive LLDP & topology discover message in same interface
+ * and the mac addr in chassis id tlv is same as the AL mac addr
+ * in local p1905.1 neighbor device information
+ */
+ if(dev_info->ieee802_1_bridge)
+ {
+ debug("get same mac in lldp_update_802_1_bridge_exist\n");
+ dev_info->ieee802_1_bridge = 0;
+ }
+ return 0;
+ }
+ }
+ return -1;
+}
+
+unsigned short get_lldpdu_hdr(unsigned char *buf)
+{
+ unsigned short hdr;
+
+ hdr = *(buf);
+ hdr = (hdr << 8) & 0xFF00;
+ hdr = hdr |(*(buf+1));
+
+ return hdr;
+}
+
+int parse_chassis_id_tlv(unsigned char *buf,unsigned char *al_mac)
+{
+ unsigned short msg_hdr = 0;
+ unsigned char *temp_buf;
+ unsigned short length = 0;
+ unsigned char subtype = 0;
+
+ temp_buf = buf;
+ msg_hdr = get_lldpdu_hdr(buf);
+
+ if(!(((msg_hdr >> 9) & 0x7f) == CHASSIS_ID_TLV_TYPE))
+ {
+ debug("wrong chassis id tlv type\n");
+ return -1;
+ }
+ else
+ {
+ debug("receive lldp chassis id tlv\n");
+ }
+
+ length = (msg_hdr & 0x01ff);
+ debug("chassis id info string len = %d\n",length);
+ temp_buf += LLDPDU_HLEN;
+
+ /*chassis id tlv subtype field*/
+ subtype = *temp_buf;
+ temp_buf++;
+
+ if(subtype != C_MAC_ADDRESS)
+ {
+ debug("chassis id tlv subtype = %d\n",subtype);
+ return 0;
+ }
+ memcpy(al_mac, temp_buf, ETH_ALEN);
+ return (length + LLDPDU_HLEN);
+}
+
+int parse_port_id_tlv(unsigned char *buf,unsigned char *itf_mac)
+{
+ unsigned short msg_hdr = 0;
+ unsigned char *temp_buf;
+ unsigned short length = 0;
+ unsigned char subtype = 0;
+
+ temp_buf = buf;
+ msg_hdr = get_lldpdu_hdr(buf);
+
+ if(!(((msg_hdr >> 9) & 0x7f) == PORT_ID_TLV_TYPE))
+ {
+ debug("wrong port id tlv type\n");
+ return -1;
+ }
+ else
+ {
+ debug("receive lldp port id tlv\n");
+ }
+
+ length = (msg_hdr & 0x01ff);
+ debug("port id info string len = %d\n",length);
+ temp_buf += LLDPDU_HLEN;
+
+ /*port id tlv subtype field*/
+ subtype = *temp_buf;
+ temp_buf++;
+
+ if(subtype != P_MAC_ADDRESS)
+ {
+ debug("port id tlv subtype = %d\n",subtype);
+ return 0;
+ }
+
+ memcpy(itf_mac, temp_buf, ETH_ALEN);
+ return (length + LLDPDU_HLEN);
+}
+
+int parse_time_to_live_tlv(unsigned char *buf,unsigned short *ttl)
+{
+ unsigned short msg_hdr = 0;
+ unsigned char *temp_buf;
+ unsigned short length = 0;
+ unsigned short time = 0;
+
+ temp_buf = buf;
+ msg_hdr = get_lldpdu_hdr(buf);
+
+ if(!(((msg_hdr >> 9) & 0x7f) == TIME_TO_LIVE_TLV_TYPE))
+ {
+ debug("wrong time to live tlv type\n");
+ return -1;
+ }
+ else
+ {
+ debug("receive lldp time to live tlv\n");
+ }
+
+ length = (msg_hdr & 0x01ff);
+ if(length != 2)
+ {
+ debug("time to live tlv info string len = %d\n",length);
+ return -1;
+ }
+
+ temp_buf += LLDPDU_HLEN;
+
+ time = *(temp_buf);
+ time = (time << 8) & 0xFF00;
+ time = time |(*(temp_buf+1));
+
+ *ttl = time;
+ return (length + LLDPDU_HLEN);
+}
+
+int parse_802_1_bridge_discovery_msg(struct p1905_managerd_ctx *ctx,
+ unsigned char *buf)
+{
+ unsigned char *temp_buf;
+ struct ethhdr *eth_hdr;
+ unsigned char chassis_mac[ETH_HLEN];
+ unsigned char port_mac[ETH_HLEN];
+ unsigned char local_itf_mac[ETH_HLEN];
+ unsigned short ttl = 0;
+ int len;
+ unsigned char exist_in_topology = 0;
+ unsigned char need_record = 1;
+
+ temp_buf = buf;
+ eth_hdr = (struct ethhdr *)temp_buf;
+
+ if(eth_hdr->h_proto != htons(ETH_P_LLDP))
+ {
+ debug("wrong LLDP ether type\n");
+ return -1;
+ }
+
+ /*shift to LLDPDU start position*/
+ temp_buf += ETH_HLEN;
+ len = parse_chassis_id_tlv(temp_buf,chassis_mac);
+ if(0 > len)
+ {
+ debug("parse chassis id tlv fail\n");
+ return -1;
+ }
+ else if(0 == len)
+ {
+ /*if we get a chassis id tlv which we don't need(subtype != mac addr),
+ *ignore this tlv
+ */
+ debug("do not need this chassis id tlv\n");
+ }
+ else
+ {
+ if(0 == lldp_update_802_1_bridge_exist(ctx, eth_hdr->h_source, chassis_mac))
+ {
+ /*chassic mac already exist in p1905.1 neighbor info,
+ *so we can reagrd this local interface without ieee802.1 bridge.
+ *update the 802.1 bridge exist field in p1905.1 neighbor info
+ */
+ debug("this chassis mac is produced by p1905.1 device");
+ exist_in_topology = 1;
+ //return 0;
+ }
+ }
+ temp_buf += len;
+
+ len = parse_port_id_tlv(temp_buf,port_mac);
+ if(0 > len)
+ {
+ debug("parse port id tlv fail\n");
+ return -1;
+ }
+ else if(0 == len)
+ {
+ /*if we get a port id tlv which we don't need(subtype != MAC_ADDR),
+ *ignore this tlv
+ */
+ need_record = 0;
+ debug("do not need this port id tlv\n");
+ }
+
+ temp_buf += len;
+
+ len = parse_time_to_live_tlv(temp_buf, &ttl);
+ if(0 > len)
+ {
+ debug("parse time to live tlv fail\n");
+ return -1;
+ }
+
+ if(need_record)
+ {
+ if(0 > get_receive_port_addr(eth_hdr->h_source,local_itf_mac))
+ {
+ return -1;
+ }
+
+ debug("insert lldp queue, port id mac =\n");
+ debug("%.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",port_mac[0],port_mac[1],
+ port_mac[2],port_mac[3],port_mac[4],port_mac[5]);
+ insert_lldp_rx_queue(local_itf_mac, port_mac, ttl);
+ }
+
+ return 0;
+}
+
diff --git a/cleopatre/devkit/p1905_managerd/src/mt7601_io.c b/cleopatre/devkit/p1905_managerd/src/mt7601_io.c
new file mode 100644
index 0000000000..273b081b99
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/mt7601_io.c
@@ -0,0 +1,1448 @@
+/*
+ * cleopatre/application/p1905_managerd/src/mt7601_io.c
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ *
+ */
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <net/if.h>
+#include <syslog.h>
+#include <asm/byteorder.h>
+#include <sys/socket.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+#include <linux/wireless.h>
+#include "wifi_utils.h"
+#include <time.h>
+#include "mt7601_io.h"
+#include "libspid.h"
+
+//#define MT7601_IO_DEBUG
+#ifdef MT7601_IO_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
+
+/*define wifi interface name*/
+#define RA0 "ra0"
+
+static WIFI_UTILS_STATUS mt7601_ioctl(int cmd, mt7601_io_table *tab)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+
+ wrq.u.data.length = tab->length;
+ wrq.u.data.pointer =(unsigned char *) tab->data;
+ wrq.u.data.flags = tab->flag;
+
+ if(ioctl(socket_id, cmd, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ tab->length = wrq.u.data.length;
+
+ close(socket_id);
+ return status;
+}
+
+void mt7601_init(void)
+{
+ /*use srand to set seed of rand*/
+ srand(time(NULL));
+}
+
+WIFI_UTILS_STATUS mt7601_get_rf_band(RF_BAND *band)
+{
+ libspid_wifi_t config_file;
+
+ if(LIBSPID_SUCCESS != libspid_network_wifi_data(RA0, &config_file, 0))
+ return wifi_utils_error;
+
+ if(!strcmp(config_file.wireless_mode, "2") ||
+ !strcmp(config_file.wireless_mode, "8") ||
+ !strcmp(config_file.wireless_mode, "10"))
+ {
+ *band = rf_band_5G;
+ }
+ else
+ *band = rf_band_2p4G;
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS mt7601_get_uuid(unsigned char uu[])
+{
+ mt7601_io_table tab;
+ unsigned char data[50] = {0};
+ int i = 0;
+ unsigned char h_nibble = 0, l_nibble = 0;
+
+ tab.data = data;
+ tab.length = sizeof(sizeof(unsigned char));
+ tab.flag = RT_OID_WSC_UUID;
+
+ if(wifi_utils_error == mt7601_ioctl(RT_PRIV_IOCTL, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ debug("uuid length %d\n",tab.length);
+ debug("uuid %s\n",tab.data);
+
+ while(*(tab.data) != '\0')
+ {
+ if(*(tab.data) != '-')
+ {
+ h_nibble = *(tab.data);
+ l_nibble = *(tab.data + 1);
+
+ if(h_nibble >= 'a' && h_nibble <= 'f')
+ h_nibble -= ('a' - 10);
+ else if(h_nibble >= 'A' && h_nibble <= 'F')
+ h_nibble -= ('A' - 10);
+ else
+ h_nibble -= '0';
+
+ if(l_nibble >= 'a' && l_nibble <= 'f')
+ l_nibble -= ('a' - 10);
+ else if(l_nibble >= 'A' && l_nibble <= 'F')
+ l_nibble -= ('A' - 10);
+ else
+ l_nibble -= '0';
+
+ uu[i] = (h_nibble << 4) | l_nibble;
+ tab.data += 2;
+ i++;
+ }
+ else
+ {
+ tab.data += 1;
+ }
+ }
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS
+mt7601_get_statistics(unsigned char peer_mac[], unsigned int *packet_total,
+ unsigned int *packet_error, unsigned char is_tx)
+{
+#ifndef SUPPORT_WIFI_STATION
+ mt7601_io_table tab;
+ unsigned char data[2048] = {0};
+ unsigned char *parse_str;
+ unsigned char *ptr;
+ char *pEnd;
+ char sta_mac[6] = {0};
+
+ strcpy(data, "stat");
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_STATISTICS, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ parse_str = data;
+
+ while((parse_str = strstr(parse_str, "mac")) != NULL)
+ {
+ parse_str = strstr(parse_str, "=");
+ parse_str = parse_str + 2;
+
+ ptr = strstr(parse_str, "\n");
+ *ptr = '\0';
+ ptr++;
+
+ sta_mac[0] = strtol(parse_str, &pEnd, 16);
+ sta_mac[1] = strtol(pEnd, &pEnd, 16);
+ sta_mac[2] = strtol(pEnd, &pEnd, 16);
+ sta_mac[3] = strtol(pEnd, &pEnd, 16);
+ sta_mac[4] = strtol(pEnd, &pEnd, 16);
+ sta_mac[5] = strtol(pEnd, &pEnd, 16);
+
+ debug("%.2x %.2x %.2x %.2x %.2x %.2x",sta_mac[0],sta_mac[1],
+ sta_mac[2],sta_mac[3],sta_mac[4],sta_mac[5]);
+
+ if(!memcmp(peer_mac, sta_mac, 6))
+ {
+ if(is_tx)
+ {
+ if((ptr = strstr(ptr, "TxPacket")) != NULL)
+ {
+ ptr = strstr(ptr, "=");
+ ptr = ptr + 2;
+
+ *packet_total = (unsigned int)strtol(ptr, NULL, 10);
+ /*temp let packet error = 0, need to implement*/
+ *packet_error = 0;
+
+ debug("packet tx = %d",*packet_total);
+ break;
+ }
+ else
+ {
+ debug_syslog("get TxPacket error\n");
+ return wifi_utils_error;
+ }
+ }
+ else
+ {
+ if((ptr = strstr(ptr, "RxPacket")) != NULL)
+ {
+ ptr = strstr(ptr, "=");
+ ptr = ptr + 2;
+
+ *packet_total = (unsigned int)strtol(ptr, NULL, 10);
+ /*temp let packet error = 0, need to implement*/
+ *packet_error = 0;
+ debug("packet rx = %d",*packet_total);
+ break;
+ }
+ else
+ {
+ debug_syslog("get RxPacket error\n");
+ return wifi_utils_error;
+ }
+ }
+ }
+
+ parse_str = ptr;
+ }
+
+ return wifi_utils_success;
+#else
+
+ *packet_total = 0;
+ *packet_error = 0;
+ return wifi_utils_success;
+#endif
+}
+
+WIFI_UTILS_STATUS
+mt7601_get_phyrate(unsigned char peer_mac[], unsigned short *phyrate)
+{
+#ifndef SUPPORT_WIFI_STATION
+ mt7601_io_table tab;
+ unsigned char data[2048] = {0};
+ unsigned char *parse_str;
+ unsigned char *ptr;
+ char *pEnd;
+ char sta_mac[6] = {0};
+
+ strcpy(data, "stat");
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_STATISTICS, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ parse_str = data;
+
+ while((parse_str = strstr(parse_str, "mac")) != NULL)
+ {
+ parse_str = strstr(parse_str, "=");
+ parse_str = parse_str + 2;
+
+ ptr = strstr(parse_str, "\n");
+ *ptr = '\0';
+ ptr++;
+
+ sta_mac[0] = strtol(parse_str, &pEnd, 16);
+ sta_mac[1] = strtol(pEnd, &pEnd, 16);
+ sta_mac[2] = strtol(pEnd, &pEnd, 16);
+ sta_mac[3] = strtol(pEnd, &pEnd, 16);
+ sta_mac[4] = strtol(pEnd, &pEnd, 16);
+ sta_mac[5] = strtol(pEnd, &pEnd, 16);
+
+ debug("%.2x %.2x %.2x %.2x %.2x %.2x",sta_mac[0],sta_mac[1],
+ sta_mac[2],sta_mac[3],sta_mac[4],sta_mac[5]);
+
+ if(!memcmp(peer_mac, sta_mac, 6))
+ {
+ if((ptr = strstr(ptr, "Rate")) != NULL)
+ {
+ ptr = strstr(ptr, "=");
+ ptr = ptr + 2;
+
+ *phyrate = (unsigned short)strtol(ptr, NULL, 10);
+ debug("phyrate tx = %d",*phyrate);
+ break;
+ }
+ else
+ {
+ debug_syslog("get phyrate error\n");
+ return wifi_utils_error;
+ }
+ }
+ parse_str = ptr;
+ }
+ return wifi_utils_success;
+#else
+ *phyrate = 0;
+ return wifi_utils_success;
+#endif
+}
+
+WIFI_UTILS_STATUS mt7601_get_rssi(char *rssi, unsigned char peer_mac[])
+{
+#ifndef SUPPORT_WIFI_STATION
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+ RT_802_11_MAC_TABLE MacTable;
+ int i = 0;
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(RT_802_11_MAC_TABLE);
+ wrq.u.data.pointer =(unsigned char *) &MacTable;
+ wrq.u.data.flags = 0;
+
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_MAC_TABLE, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ for(i=0;i<MacTable.Num;i++)
+ {
+ if(!memcmp(MacTable.Entry[i].Addr, peer_mac, 6))
+ {
+ *rssi = MacTable.Entry[i].AvgRssi0;
+ break;
+ }
+ }
+
+ close(socket_id);
+ return status;
+#else
+ *rssi = 0xFF;
+ return wifi_utils_success;
+#endif
+}
+
+WIFI_UTILS_STATUS mt7601_get_current_channelwidth(unsigned char *width)
+{
+#ifndef SUPPORT_WIFI_STATION
+ mt7601_io_table tab;
+ unsigned char data[2048] = {0};
+ unsigned char *parse_str;
+ unsigned char BW;
+
+ strcpy(data, "stat");
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_STATISTICS, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ parse_str = data;
+
+ if((parse_str = strstr(parse_str, "CurrentBW")) != NULL)
+ {
+ parse_str = strstr(parse_str, "=");
+ parse_str = parse_str + 2;
+
+ BW = (unsigned char)strtol(parse_str, NULL, 10);
+
+ if(BW == 0)
+ *width = 0;
+ else if(BW == 1)
+ *width = 1;
+ else
+ {
+ debug_syslog("mt7601_get_current_channelwidth wrong\n");
+ return wifi_utils_error;
+ }
+ }
+ else
+ return wifi_utils_error;
+
+ return wifi_utils_success;
+#else
+ *width = 0;
+ return wifi_utils_success;
+#endif
+}
+
+WIFI_UTILS_STATUS mt7601_get_center_freq_index(unsigned char *index)
+{
+#ifndef SUPPORT_WIFI_STATION
+ mt7601_io_table tab;
+ unsigned char data[2048] = {0};
+ unsigned char *parse_str;
+
+ strcpy(data, "stat");
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_STATISTICS, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ parse_str = data;
+
+ if((parse_str = strstr(parse_str, "CenterFreq")) != NULL)
+ {
+ parse_str = strstr(parse_str, "=");
+ parse_str = parse_str + 2;
+
+ *index = (unsigned char)strtol(parse_str, NULL, 10);
+ }
+ else
+ return wifi_utils_error;
+
+ return wifi_utils_success;
+#else
+ *index = 0;
+ return wifi_utils_success;
+#endif
+}
+
+WIFI_UTILS_STATUS
+mt7601_get_station_mac(unsigned char station_list[WIFI_MAX_STATION_NUM][6],
+ unsigned char *sta_num)
+{
+#ifndef SUPPORT_WIFI_STATION
+ mt7601_io_table tab;
+ unsigned char data[2048] = {0};
+ unsigned char *parse_str;
+ unsigned char *ptr;
+ char *pEnd;
+ unsigned char num = 0;
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_STATISTICS, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ parse_str = data;
+
+ while((parse_str = strstr(parse_str, "mac")) != NULL)
+ {
+ parse_str = strstr(parse_str, "=");
+ parse_str = parse_str + 2;
+
+ ptr = strstr(parse_str, "\n");
+ *ptr = '\0';
+ ptr++;
+
+ station_list[num][0] = strtol(parse_str, &pEnd, 16);
+ station_list[num][1] = strtol(pEnd, &pEnd, 16);
+ station_list[num][2] = strtol(pEnd, &pEnd, 16);
+ station_list[num][3] = strtol(pEnd, &pEnd, 16);
+ station_list[num][4] = strtol(pEnd, &pEnd, 16);
+ station_list[num][5] = strtol(pEnd, &pEnd, 16);
+
+ num++;
+ parse_str = ptr;
+ }
+
+ *sta_num = num;
+#else
+ /*return fail if MT7601 used as a station*/
+ return wifi_utils_error;
+#endif
+ return wifi_utils_success;
+}
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+WIFI_UTILS_STATUS mt7601_get_wsc_conf_status(unsigned char *status)
+{
+ mt7601_io_table tab;
+ unsigned char data[1] = {0};
+
+ tab.data = data;
+ tab.length = sizeof(sizeof(unsigned char));
+ tab.flag = RT_OID_WSC_CONFIG_STATUS;
+
+ if(wifi_utils_error == mt7601_ioctl(RT_PRIV_IOCTL, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ *status = data[0];
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS mt7601_generate_DH_pub_priv_key(DH_TABLE *pdh)
+{
+ struct iwreq wrq;
+ int socket_id, i = 0, len = 0;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+ len = sizeof(pdh->priv_key);
+
+ /*ramdon generate the Diffie-Hellman private key*/
+ for(i=0;i<len;i++)
+ pdh->priv_key[i] = ((rand() % 255) + 1);
+
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer =(unsigned char *) pdh;
+ wrq.u.data.flags = 0;
+
+ /* use this ioctl to generate Diffie-Helman public key*/
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_DH_PUB_KEY, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+
+WIFI_UTILS_STATUS mt7601_generate_DH_secu_key(DH_TABLE *pdh)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer =(unsigned char *) pdh;
+ wrq.u.data.flags = 0;
+
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_DH_SECU_KEY, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+
+WIFI_UTILS_STATUS mt7601_generate_auth_keywrap_key(KDK_KDF_TABLE *pkdk_kdf)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(KDK_KDF_TABLE);
+ wrq.u.data.pointer =(unsigned char *) pkdk_kdf;
+ wrq.u.data.flags = 0;
+
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_AUTH_KEYWRAP_KEY, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+
+WIFI_UTILS_STATUS mt7601_generate_kwa(KWA_TABLE *pkwa)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(KWA_TABLE);
+ wrq.u.data.pointer =(unsigned char *) pkwa;
+ wrq.u.data.flags = 0;
+
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_KWA_VALUE, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+
+WIFI_UTILS_STATUS mt7601_generate_AES_encrypt_value(AES_TABLE *paes)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(AES_TABLE);
+ wrq.u.data.pointer =(unsigned char *) paes;
+ wrq.u.data.flags = 0;
+
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_AES_ENCRYPT_VALUE, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+
+WIFI_UTILS_STATUS mt7601_generate_AES_decrypt_value(AES_TABLE *paes)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(AES_TABLE);
+ wrq.u.data.pointer =(unsigned char *) paes;
+ wrq.u.data.flags = 0;
+
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_AES_DECRYPT_VALUE, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+#endif
+
+WIFI_UTILS_STATUS mt7601_get_mac_addr(unsigned char *mac_addr)
+{
+ int socket_id;
+ struct ifreq ifr;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+ strncpy(ifr.ifr_name, (char*)RA0, IFNAMSIZ);
+
+ if(-1 == (ioctl(socket_id, SIOCGIFINDEX, &ifr)))
+ {
+ /*need to do error handle*/
+ }
+
+ if(-1 == (ioctl(socket_id, SIOCGIFHWADDR, &ifr)))
+ {
+ /*need to do error handle*/
+ }
+
+ memcpy(mac_addr, ifr.ifr_hwaddr.sa_data, 6);
+
+ close(socket_id);
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS mt7601_get_ssid(unsigned char *ssid, unsigned short *len)
+{
+ mt7601_io_table tab;
+
+ tab.data = ssid;
+ tab.length = (*len);
+ tab.flag = 0;
+ if(wifi_utils_error == mt7601_ioctl(SIOCGIWESSID, &tab))
+ {
+ return wifi_utils_error;
+ }
+ *len = tab.length;
+
+ return wifi_utils_success;
+}
+
+/*the parameter len = strlen(ssid)*/
+WIFI_UTILS_STATUS mt7601_set_ssid(unsigned char *ssid, unsigned short len)
+{
+ mt7601_io_table tab;
+ /*5 otects =>SSID= ; 32 otects ==> max SSID length ; 1 otects==> '\0'*/
+ unsigned char data[37 + 1] = {0};
+
+ data[0] = 'S';
+ data[1] = 'S';
+ data[2] = 'I';
+ data[3] = 'D';
+ data[4] = '=';
+
+ memcpy(&data[5], ssid, (len+1));
+ tab.data = data;
+ tab.length = 5 + len + 1;
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+WIFI_UTILS_STATUS mt7601_set_authmode(unsigned short authmode)
+{
+ mt7601_io_table tab;
+ unsigned char data[25] = {0};
+
+ strcpy(data, "AuthMode=");
+
+ switch(authmode)
+ {
+ case AUTH_OPEN:
+ strcat(data, "OPEN");
+ break;
+ case AUTH_WPA_PERSONAL:
+ strcat(data, "WPAPSK");
+ break;
+ case AUTH_SHARED:
+ strcat(data, "SHARED");
+ break;
+ case AUTH_WPA_ENTERPRISE:
+ strcat(data, "WPA");
+ break;
+ case AUTH_WPA2_ENTERPRISE:
+ strcat(data, "WPA2");
+ break;
+ case (AUTH_WPA2_ENTERPRISE | AUTH_WPA_ENTERPRISE):
+ strcat(data, "WPA1WPA2");
+ break;
+ case AUTH_WPA2_PERSONAL:
+ strcat(data, "WPA2PSK");
+ break;
+ case (AUTH_WPA2_PERSONAL | AUTH_WPA_PERSONAL):
+ strcat(data, "WPAPSKWPA2PSK");
+ break;
+ default:
+ strcat(data, "OPEN");
+ break;
+ }
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS mt7601_set_encryptmode(unsigned short encryptmode)
+{
+ mt7601_io_table tab;
+ unsigned char data[20] = {0};
+
+ strcpy(data, "EncrypType=");
+
+ switch(encryptmode)
+ {
+ case ENCRYP_NONE:
+ strcat(data, "NONE");
+ break;
+ case ENCRYP_WEP:
+ strcat(data, "WEP");
+ break;
+ case ENCRYP_TKIP:
+ strcat(data, "TKIP");
+ break;
+ case ENCRYP_AES:
+ strcat(data, "AES");
+ break;
+ case (ENCRYP_TKIP|ENCRYP_AES):
+ strcat(data, "TKIPAES");
+ break;
+ default:
+ strcat(data, "NONE");
+ break;
+ }
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS mt7601_set_wsc_conf_status(unsigned char confstatus)
+{
+ mt7601_io_table tab;
+ unsigned char data[20] = {0};
+
+ strcpy(data, "WscConfStatus=");
+
+ if(confstatus == 2)//ap configured
+ strcat(data,"2");
+ else
+ strcat(data,"1");
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS mt7601_set_ieee802_1x(unsigned char ieee8021x)
+{
+ mt7601_io_table tab;
+ unsigned char data[20] = {0};
+
+ strcpy(data, "IEEE8021X=");
+
+ if(ieee8021x)
+ strcat(data, "1");
+ else
+ strcat(data, "0");
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+/*the parameter len = strlen(wpapsk)*/
+WIFI_UTILS_STATUS mt7601_set_wpapsk(unsigned char *wpapsk, unsigned short len)
+{
+ mt7601_io_table tab;
+ unsigned char data[7+64+1] = {0};
+
+ strcpy(data, "WPAPSK=");
+ memcpy(&data[7], wpapsk, len+1);
+ tab.data = data;
+ tab.length = 7 + len + 1;
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS mt7601_set_default_keyid(unsigned char keyid)
+{
+ mt7601_io_table tab;
+ unsigned char data[20] = {0};
+
+ strcpy(data, "DefaultKeyID=");
+
+ if(keyid >= 1 && keyid <= 4)
+ {
+ keyid += 0x30;//changne Hex to ASCII
+ }
+ else
+ return wifi_utils_error;
+
+ data[13] = keyid;
+ data[14] = '\0';
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS mt7601_set_key1(unsigned char *key, unsigned short len)
+{
+ mt7601_io_table tab;
+ unsigned char data[5+65] = {0};
+
+ strcpy(data, "Key1=");
+ memcpy(&data[5], key, len+1);
+ tab.data = data;
+ tab.length = 5 + len + 1;
+
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS mt7601_get_wsc_config(WSC_CONFIG *wsc_config)
+{
+ WSC_CONFIGURED_VALUE *wsc;
+ mt7601_io_table tab;
+
+ wsc = (WSC_CONFIGURED_VALUE *)malloc(sizeof(WSC_CONFIGURED_VALUE));
+
+ tab.data = (unsigned char *)wsc;
+ tab.length = sizeof(WSC_CONFIGURED_VALUE);
+ tab.flag = 0;
+
+ if(wifi_utils_error == mt7601_ioctl(RTPRIV_IOCTL_WSC_PROFILE, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ memcpy(wsc_config->Ssid, wsc->WscSsid, sizeof(wsc_config->Ssid));
+ wsc_config->AuthMode = wsc->WscAuthMode;
+ wsc_config->EncrypType = wsc->WscEncrypType;
+ memcpy(wsc_config->WPAKey, wsc->WscWPAKey, sizeof(wsc_config->WPAKey));
+
+ free(wsc);
+}
+
+WIFI_UTILS_STATUS mt7601_set_wsc_config(WSC_CONFIG *wsc_config)
+{
+
+ if(wsc_config->AuthMode & (AUTH_WPA_PERSONAL | AUTH_WPA2_PERSONAL))
+ {
+ if(wsc_config->EncrypType & (ENCRYP_TKIP | ENCRYP_AES))
+ {
+ /*need to check more aout mixed mode*/
+ mt7601_set_authmode(wsc_config->AuthMode);
+ mt7601_set_encryptmode(wsc_config->EncrypType);
+ mt7601_set_ssid(wsc_config->Ssid, strlen(wsc_config->Ssid));
+ mt7601_set_wpapsk(wsc_config->WPAKey, strlen(wsc_config->WPAKey));
+ mt7601_set_ssid(wsc_config->Ssid, strlen(wsc_config->Ssid));
+ }
+ else
+ {
+ return wifi_utils_error;
+ }
+ }
+ else if(wsc_config->EncrypType == ENCRYP_WEP)
+ {
+ mt7601_set_authmode(wsc_config->AuthMode);
+ mt7601_set_encryptmode(wsc_config->EncrypType);
+ mt7601_set_default_keyid(1);
+ mt7601_set_key1(wsc_config->WPAKey, strlen(wsc_config->WPAKey));
+ mt7601_set_ssid(wsc_config->Ssid, strlen(wsc_config->Ssid));
+ }
+ else if(wsc_config->AuthMode == AUTH_OPEN && wsc_config->EncrypType == ENCRYP_NONE)
+ {
+ mt7601_set_authmode(wsc_config->AuthMode);
+ mt7601_set_encryptmode(wsc_config->EncrypType);
+ mt7601_set_ssid(wsc_config->Ssid, strlen(wsc_config->Ssid));
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS mt7601_update_config_file(WSC_CONFIG *wsc_config)
+{
+ libspid_wifi_t config_file;
+
+ if(LIBSPID_SUCCESS != libspid_network_wifi_data(RA0, &config_file, 0))
+ return wifi_utils_error;
+
+ strcpy(config_file.ssid, wsc_config->Ssid);
+ if(wsc_config->AuthMode & (AUTH_WPA_PERSONAL | AUTH_WPA2_PERSONAL))
+ {
+ strcpy(config_file.wpapsk, wsc_config->WPAKey);
+ }
+ else
+ {
+ if(wsc_config->EncrypType == ENCRYP_WEP)
+ {
+ strcpy(config_file.default_key_id,"1");
+
+ if(strlen(wsc_config->WPAKey) == 5 || strlen(wsc_config->WPAKey) == 13)
+ strcpy(config_file.key1_type,"1");//ASCII
+ else if(strlen(wsc_config->WPAKey) == 10 || strlen(wsc_config->WPAKey) == 26)
+ strcpy(config_file.key1_type,"0");//HEX
+ else
+ {
+ debug_syslog("wep key length = %d\n",strlen(wsc_config->WPAKey));
+ return wifi_utils_error;
+ }
+ strcpy(config_file.key1_str, wsc_config->WPAKey);
+ }
+ }
+
+ switch(wsc_config->AuthMode)
+ {
+ case AUTH_OPEN:
+ strcpy(config_file.auth_mode, "OPEN");
+ break;
+ case AUTH_WPA_PERSONAL:
+ strcpy(config_file.auth_mode, "WPAPSK");
+ break;
+ case AUTH_SHARED:
+ strcpy(config_file.auth_mode, "SHARED");
+ break;
+ case AUTH_WPA_ENTERPRISE:
+ strcpy(config_file.auth_mode, "WPA");
+ break;
+ case AUTH_WPA2_ENTERPRISE:
+ strcpy(config_file.auth_mode, "WPA2");
+ break;
+ case (AUTH_WPA2_ENTERPRISE | AUTH_WPA_ENTERPRISE):
+ strcpy(config_file.auth_mode, "WPA1WPA2");
+ break;
+ case AUTH_WPA2_PERSONAL:
+ strcpy(config_file.auth_mode, "WPA2PSK");
+ break;
+ case (AUTH_WPA2_PERSONAL | AUTH_WPA_PERSONAL):
+ strcpy(config_file.auth_mode, "WPAPSKWPA2PSK");
+ break;
+ default:
+ strcpy(config_file.auth_mode, "OPEN");
+ break;
+ }
+
+ switch(wsc_config->EncrypType)
+ {
+ case ENCRYP_NONE:
+ strcpy(config_file.encryp_type, "NONE");
+ break;
+ case ENCRYP_WEP:
+ strcpy(config_file.encryp_type, "WEP");
+ break;
+ case ENCRYP_TKIP:
+ strcpy(config_file.encryp_type, "TKIP");
+ break;
+ case ENCRYP_AES:
+ strcpy(config_file.encryp_type, "AES");
+ break;
+ case (ENCRYP_TKIP|ENCRYP_AES):
+ strcpy(config_file.encryp_type, "TKIPAES");
+ break;
+ default:
+ strcpy(config_file.encryp_type, "NONE");
+ break;
+ }
+
+ /*set confstatus to configured*/
+ strcpy(config_file.wsc_conf_status, "2");
+
+ if(LIBSPID_SUCCESS != libspid_network_wifi_data(RA0, &config_file, 1))
+ return wifi_utils_error;
+
+ if(LIBSPID_SUCCESS != libspid_system_save())
+ return wifi_utils_error;
+
+ return wifi_utils_success;
+}
+
+void mt7601_set_wsc_config_test(void)
+{
+#if 1
+ unsigned char ssid[32+1] = "testwpa2aes";
+ unsigned char wpapsk[64+1] = "iwanttotestwpa2aes";
+ WSC_CONFIG wsc;
+
+ mt7601_set_authmode(AUTH_WPA2_PERSONAL);
+ mt7601_set_encryptmode(ENCRYP_AES);
+ mt7601_set_ssid(ssid, strlen(ssid));
+ mt7601_set_wpapsk(wpapsk, strlen(wpapsk));
+ mt7601_set_ssid(ssid, strlen(ssid));
+#endif
+
+#if 0
+ unsigned char ssid[32+1] = "testwpa2tkip";
+ unsigned char wpapsk[64+1] = "iwanttotestwpa2tkip";
+ WSC_CONFIG wsc;
+
+ mt7601_set_authmode(AUTH_WPA2_PERSONAL);
+ mt7601_set_encryptmode(ENCRYP_TKIP);
+ mt7601_set_ssid(ssid, strlen(ssid));
+ mt7601_set_wpapsk(wpapsk, strlen(wpapsk));
+ mt7601_set_ssid(ssid, strlen(ssid));
+#endif
+
+#if 0
+ unsigned char ssid[32+1] = "testwepshareascii13";
+ unsigned char wpapsk[64+1] = "itestascii13y";
+ WSC_CONFIG wsc;
+ mt7601_set_authmode(AUTH_SHARED);
+ mt7601_set_encryptmode(ENCRYP_WEP);
+ mt7601_set_default_keyid(1);
+ mt7601_set_key1(wpapsk, strlen(wpapsk));
+ mt7601_set_ssid(ssid, strlen(ssid));
+#endif
+
+#if 0
+ unsigned char ssid[32+1] = "testwpaaes";
+ unsigned char wpapsk[64+1] = "iwanttotestwpaaes";
+ WSC_CONFIG wsc;
+
+ mt7601_set_authmode(AUTH_WPA_PERSONAL);
+ mt7601_set_encryptmode(ENCRYP_AES);
+ mt7601_set_ssid(ssid, strlen(ssid));
+ mt7601_set_wpapsk(wpapsk, strlen(wpapsk));
+ mt7601_set_ssid(ssid, strlen(ssid));
+#endif
+
+#if 0
+ unsigned char ssid[32+1] = "testnone";
+ WSC_CONFIG wsc;
+
+ mt7601_set_authmode(AUTH_OPEN);
+ mt7601_set_encryptmode(ENCRYP_NONE);
+ mt7601_set_ssid(ssid, strlen(ssid));
+#endif
+
+
+ mt7601_get_wsc_config(&wsc);
+ printf("ssid %s\n",wsc.Ssid);
+ printf("authmode 0x%.4x\n",wsc.AuthMode);
+ printf("encrypt mode 0x%.4x\n",wsc.EncrypType);
+ printf("wpapsk %s\n",wsc.WPAKey);
+}
+
+void mt7601_AES_test(void)
+{
+#ifdef MT7601_IO_DEBUG
+ int i = 0;
+ AES_TABLE Enc;
+ AES_TABLE Dec;
+
+ srand(time(NULL));
+
+ for(i=0;i<16;i++)
+ Enc.KeyWrapKey[i] = ((rand() % 255) + 1);
+ memcpy(Dec.KeyWrapKey, Enc.KeyWrapKey, 16);
+
+ for(i=0;i<16;i++)
+ Enc.IV[i] = ((rand() % 255) + 1);
+ memcpy(Dec.IV, Enc.IV, 16);
+
+ Enc.PlainTextLen = ((rand() % (512-16)) + 1);
+ printf("plain text len = %d\n",Enc.PlainTextLen);
+
+ Enc.PlainText = malloc(Enc.PlainTextLen);
+ for(i=0;i<Enc.PlainTextLen;i++)
+ *(Enc.PlainText + i) = ((rand() % 255) + 1);
+
+ Enc.CipherText = malloc(512-16);
+ Enc.CipherTextLen = 512 -16;
+
+ mt7601_generate_AES_encrypt_value(&Enc);
+
+ printf("encrypt data len = %d\n",Enc.CipherTextLen);
+
+ Dec.CipherText = malloc(Enc.CipherTextLen);
+ Dec.CipherTextLen = Enc.CipherTextLen;
+ memcpy(Dec.CipherText, Enc.CipherText, Enc.CipherTextLen);
+
+ Dec.PlainText = malloc(1024);
+ Dec.PlainTextLen = 1024;
+
+ mt7601_generate_AES_decrypt_value(&Dec);
+
+ if(Dec.PlainTextLen != Enc.PlainTextLen)
+ {
+ printf("Dec.PlainTextLen = %d\n",Dec.PlainTextLen);
+ printf("Enc.PlainTextLen = %d\n",Enc.PlainTextLen);
+ }
+ else if(memcmp(Enc.PlainText, Dec.PlainText, Dec.PlainTextLen))
+ {
+ printf("Dec/Encrypt error\n");
+ }
+ else
+ printf("Dec/Encrypt success\n");
+
+
+ free(Enc.PlainText);
+ free(Enc.CipherText);
+ free(Dec.CipherText);
+ free(Dec.PlainText);
+#endif
+}
+
+void mt7601_keywrap_test(void)
+{
+#ifdef MT7601_IO_DEBUG
+ DH_TABLE dh1;
+ DH_TABLE dh2;
+ KDK_KDF_TABLE k1;
+ KDK_KDF_TABLE k2;
+ unsigned char temp_public[192] = {0};
+ int i = 0;
+
+ srand(time(NULL));
+
+ mt7601_generate_DH_pub_priv_key(&dh2);
+ mt7601_generate_DH_pub_priv_key(&dh1);
+
+ printf("DH public 1:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh1.pub_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+ printf("DH public 2:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh2.pub_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+ memcpy(temp_public, dh2.pub_key, 192);
+ memcpy(dh2.pub_key, dh1.pub_key, 192);
+ memcpy(dh1.pub_key, temp_public, 192);
+
+ printf("after DH public 1:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh1.pub_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+ printf("after DH public 2:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh2.pub_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+ mt7601_generate_DH_secu_key(&dh2);
+ mt7601_generate_DH_secu_key(&dh1);
+
+ printf("DH security 1:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh1.secu_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+ printf("DH security 2:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh2.secu_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+
+ if(!memcmp(dh1.secu_key, dh2.secu_key, 192))
+ printf("DH key ok\n");
+ else
+ printf("DH key fail\n");
+
+ memcpy(k1.DH_Secu_Key, dh1.secu_key, 192);
+ memcpy(k2.DH_Secu_Key, dh2.secu_key, 192);
+
+ for(i=0;i<16;i++)
+ k1.E_Nonce[i] = ((rand() % 255) + 1);
+ memcpy(k2.E_Nonce, k1.E_Nonce, 16);
+
+ for(i=0;i<16;i++)
+ k1.R_Nonce[i] = ((rand() % 255) + 1);
+ memcpy(k2.R_Nonce, k1.R_Nonce, 16);
+
+ for(i=0;i<6;i++)
+ k1.E_Mac_Addr[i] = ((rand() % 255) + 1);
+ memcpy(k2.E_Mac_Addr, k1.E_Mac_Addr, 6);
+
+ mt7601_generate_auth_keywrap_key(&k1);
+ mt7601_generate_auth_keywrap_key(&k2);
+
+ printf("auth key1 =\n");
+ for(i=0;i<sizeof(k1.AuthKey);i++)
+ {
+ printf("%.2x ",k1.AuthKey[i]);
+ }
+ printf("\n");
+
+ printf("auth key2 =\n");
+ for(i=0;i<sizeof(k2.AuthKey);i++)
+ {
+ printf("%.2x ",k2.AuthKey[i]);
+ }
+ printf("\n");
+ if(!memcmp(k1.AuthKey, k2.AuthKey, 32))
+ printf("auth key OK\n");
+
+ printf("keywrap key1 =\n");
+ for(i=0;i<sizeof(k1.KeyWrapKey);i++)
+ {
+ printf("%.2x ",k1.KeyWrapKey[i]);
+ }
+ printf("\n");
+
+ printf("keywrap key2 =\n");
+ for(i=0;i<sizeof(k2.KeyWrapKey);i++)
+ {
+ printf("%.2x ",k2.KeyWrapKey[i]);
+ }
+ printf("\n");
+ if(!memcmp(k1.KeyWrapKey, k2.KeyWrapKey, 32))
+ printf("auth key OK\n");
+#endif
+}
+
+void mt7601_DH_test(void)
+{
+#ifdef MT7601_IO_DEBUG
+ DH_TABLE dh1;
+ DH_TABLE dh2;
+
+ struct iwreq wrq;
+ int socket_id;
+ unsigned char name[6] = {0};
+ unsigned char public[192] = {0};
+
+ int i = 0;
+ int len = 0;
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+
+ srand(time(NULL));
+
+ for(i=0;i<192;i++)
+ {
+ dh1.priv_key[i] = ((rand() % 255) + 1);
+ }
+
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer =(unsigned char *) &dh1;
+ wrq.u.data.flags = 0;
+
+ if(0 == ioctl(socket_id, RTPRIV_IOCTL_GET_DH_PUB_KEY, &wrq))
+ {
+ printf("1 ==>wsc_dh_generate_key OK, len = %d\n",wrq.u.data.length);
+
+ len = sizeof(dh1.pub_key);
+ for(i=0;i<len;i++)
+ {
+ printf("%.2x ",dh1.pub_key[i]);
+ if(i%10 == 0)
+ printf("\n");
+ }
+ }
+ /*=================================================================*/
+ for(i=0;i<192;i++)
+ {
+ dh2.priv_key[i] = ((rand() % 255) + 1);
+ }
+
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer = (unsigned char *) &dh2;
+ wrq.u.data.flags = 0;
+
+ if(0 == ioctl(socket_id, RTPRIV_IOCTL_GET_DH_PUB_KEY, &wrq))
+ {
+ printf("2 ==>wsc_dh_generate_key OK, len = %d\n",wrq.u.data.length);
+
+ len = sizeof(dh2.pub_key);
+ for(i=0;i<len;i++)
+ {
+ printf("%.2x ",dh2.pub_key[i]);
+ if(i%10 == 0)
+ printf("\n");
+ }
+ }
+ /*=================================================================*/
+ memcpy(public, dh1.pub_key, 192);
+ memcpy(dh1.pub_key, dh2.pub_key, 192);
+ memset(dh1.secu_key,0,192);
+ memcpy(dh2.pub_key, public, 192);
+ memset(dh2.secu_key,0,192);
+
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer = (unsigned char *)(&dh1);
+ wrq.u.data.flags = 0;
+
+ if(0 == ioctl(socket_id, RTPRIV_IOCTL_GET_DH_SECU_KEY, &wrq))
+ {
+ printf("1 ==>compute key OK, len = %d\n",wrq.u.data.length);
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",(dh1.secu_key[i]));
+ if(i%10 == 0)
+ printf("\n");
+ }
+ }
+
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer = (unsigned char *)(&dh2);
+ wrq.u.data.flags = 0;
+
+ if(0 == ioctl(socket_id, RTPRIV_IOCTL_GET_DH_SECU_KEY, &wrq))
+ {
+ printf("2 ==>compute key OK, len = %d\n",wrq.u.data.length);
+
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",(dh2.secu_key[i]));
+ if(i%10 == 0)
+ printf("\n");
+ }
+ }
+
+ if(!memcmp(dh1.secu_key, dh2.secu_key, sizeof(dh1.secu_key)))
+ printf("\nkey is same\n");
+
+ close(socket_id);
+#endif
+}
+#endif
diff --git a/cleopatre/devkit/p1905_managerd/src/p1905_ap_autoconfig.c b/cleopatre/devkit/p1905_managerd/src/p1905_ap_autoconfig.c
new file mode 100644
index 0000000000..884131df5f
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/p1905_ap_autoconfig.c
@@ -0,0 +1,201 @@
+/*
+ * cleopatre/application/p1905_managerd/src/p1905_ap_autoconfig.c
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <syslog.h>
+#include <errno.h>
+#include "p1905_managerd.h"
+#include "p1905_ap_autoconfig.h"
+#include "cmdu_message.h"
+#include "libspid.h"
+
+//#define P1905_AP_AUTOCONFIG_DEBUG
+#ifdef P1905_AP_AUTOCONFIG_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
+
+#define AUTO_CONFIG_TIMEOUT_CNT 3//15 seconds(5 seconds per count)
+
+extern const unsigned char p1905_multicast_address[6];
+
+void ap_autoconfig_init(struct p1905_managerd_ctx *ctx)
+{
+#if defined(SUPPORT_AP_ENROLLE)
+ unsigned char conf_status;
+ debug_syslog("AP is an enrollee\n");
+
+ ctx->enrolle_state = no_ap_autoconfig;
+ ctx->ap_config_data = NULL;
+ ctx->last_tx_data = NULL;
+ ctx->current_rx_data = NULL;
+ ctx->is_authenticator_exist_in_M2 = 0;
+ ctx->is_in_encrypt_settings = 0;
+ ctx->get_config_attr_kind = 0;
+
+ /* check wsc configured status.
+ * if this enrollee is not configured, send ap auto config search
+ */
+ ctx->need_ap_config = 0;
+ if(wifi_utils_success == get_wsc_conf_status(&conf_status))
+ {
+ if(conf_status == AP_NOT_CONFIGURED)
+ ctx->need_ap_config = 1;
+ }
+#elif defined(SUPPORT_AP_REGISTRAR)
+ /* if it is ap registrar, check the WIFI_CFG_CHANGE in /etc/web_reset.info
+ * if get value=yes, need to send ap auto config renew
+ */
+ unsigned char status[5] = {0};
+ libspid_error_t ret;
+ debug_syslog("AP is an registrar\n");
+
+ if(LIBSPID_SUCCESS == libspid_config_read_item(LIBSPID_WEB_RESET_INFO_PATH,
+ LIBSPID_WEB_RESET_INFO_LABEL_WIFI_CFG_CHANGE, status, 5))
+ {
+ if(!strcmp(status, LIBSPID_WEB_RESET_INFO_RESET_BY_WEB_YES))
+ {
+ ctx->need_ap_config = 1;
+ ret = libspid_config_write_item (LIBSPID_WEB_RESET_INFO_PATH,
+ LIBSPID_WEB_RESET_INFO_LABEL_WIFI_CFG_CHANGE,
+ LIBSPID_WEB_RESET_INFO_RESET_BY_WEB_NO);
+ if (LIBSPID_SUCCESS == ret)
+ libspid_system_save_file (LIBSPID_WEB_RESET_INFO_PATH);
+ }
+ }
+
+ ctx->last_rx_data = NULL;
+#endif
+ ctx->ap_config = NULL;
+ get_uuid(ctx->uuid);
+}
+
+void ap_autoconfig_free_memory(struct p1905_managerd_ctx *ctx)
+{
+
+
+}
+
+#ifdef SUPPORT_AP_ENROLLE
+void ap_autoconfig_enrolle_sm(struct p1905_managerd_ctx *ctx)
+{
+ WSC_CONFIG config_data;
+
+ switch(ctx->enrolle_state)
+ {
+ case wait_4_send_ap_autoconfig_search:
+ /* if PLC is not authenticated, break!
+ * this function will be called periodically by timer handler
+ * so system will check PLC status periodically
+ */
+ if(!is_connection_status_authenticated())
+ {
+ debug("cannot send auto config search message\n");
+ break;
+ }
+ //debug_syslog("enrollee send ap config search\n");
+ ctx->mid++;
+ ctx->autoconfig_search_mid = ctx->mid;
+ insert_cmdu_txq(p1905_multicast_address,ctx->plc0_mac_addr,\
+ e_ap_autoconfiguration_search, ctx->mid);
+ insert_cmdu_txq(p1905_multicast_address,ctx->eth0_mac_addr,\
+ e_ap_autoconfiguration_search, ctx->mid);
+ ctx->enrolle_state = wait_4_recv_ap_autoconfig_resp;
+ ctx->ap_config_timeout_cnt = 0;
+ break;
+
+ case wait_4_recv_ap_autoconfig_resp:
+ /*check timeout*/
+ if(ctx->ap_config_timeout_cnt > AUTO_CONFIG_TIMEOUT_CNT)
+ {
+ /* ap auto config timeout. Set state to no_ap_autoconfig
+ re-trigger ap autoconfig search process.
+ */
+ ctx->enrolle_state = no_ap_autoconfig;
+ debug_syslog("enrollee wait_4_recv_ap_autoconfig_resp timeout\n");
+ }
+ break;
+
+ case wait_4_recv_m2:
+ /*allocate memory for wifi config use*/
+ if(!ctx->ap_config_data)
+ ctx->ap_config_data = (WSC_CONFIG *)malloc(sizeof(WSC_CONFIG));
+ /*check timeout*/
+ /*if timeout happen, need to free ctx->ap_config_data*/
+ if(ctx->ap_config_timeout_cnt > AUTO_CONFIG_TIMEOUT_CNT)
+ {
+ debug_syslog("enrollee wait_4_recv_m2 timeout\n");
+
+ /* ap auto config timeout. Set state to no_ap_autoconfig
+ re-trigger ap autoconfig search process.
+ */
+ ctx->enrolle_state = no_ap_autoconfig;
+ if(ctx->ap_config_data != NULL)
+ {
+ free(ctx->ap_config_data);
+ ctx->ap_config_data = NULL;
+ }
+ if(ctx->ap_config != NULL)
+ {
+ free(ctx->ap_config);
+ ctx->ap_config = NULL;
+ }
+ if(ctx->last_tx_data != NULL)
+ {
+ free(ctx->last_tx_data);
+ ctx->last_tx_data = NULL;
+ }
+ }
+ break;
+
+ case wait_4_set_config:
+ if(ctx->ap_config_data != NULL)
+ {
+ if(wifi_utils_success != set_wsc_config(ctx->ap_config_data))
+ {
+ debug_syslog("set wsc config error\n");
+ }
+
+ /*test setting*/
+ get_wsc_config(&config_data);
+ debug_syslog("after key = %s\n",config_data.WPAKey);
+ debug_syslog("after SSID = %s\n",config_data.Ssid);
+ debug_syslog("after Auth = 0x%.4x\n",config_data.AuthMode);
+ debug_syslog("after Encry = 0x%.4x\n",config_data.EncrypType);
+
+ if(wifi_utils_success != update_wifi_config_file(ctx->ap_config_data))
+ {
+ debug_syslog("ap auto-config write config file error\n");
+ }
+ /*no need to do ap autoconfig anymore*/
+ ctx->need_ap_config = 0;
+ /*set the configured statue of local wifi device*/
+ set_wsc_conf_status(AP_ALREADY_CONFIGURED);
+
+ free(ctx->ap_config_data);
+ ctx->ap_config_data = NULL;
+ }
+ ctx->enrolle_state = no_ap_autoconfig;
+ break;
+ }
+}
+#endif
+
+#ifdef SUPPORT_AP_REGISTRAR
+void ap_auto_config_renew_start(struct p1905_managerd_ctx *ctx)
+{
+ ctx->mid++;
+ insert_cmdu_txq(p1905_multicast_address,ctx->plc0_mac_addr,\
+ e_ap_autoconfiguration_renew, ctx->mid);
+ insert_cmdu_txq(p1905_multicast_address,ctx->eth0_mac_addr,\
+ e_ap_autoconfiguration_renew, ctx->mid);
+}
+#endif
+
diff --git a/cleopatre/devkit/p1905_managerd/src/p1905_managerd.c b/cleopatre/devkit/p1905_managerd/src/p1905_managerd.c
new file mode 100644
index 0000000000..b95348d1cb
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/p1905_managerd.c
@@ -0,0 +1,608 @@
+/*
+ * cleopatre/application/p1905_managerd/src/p1905_managerd.c
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <syslog.h>
+#include <errno.h>
+#include <assert.h>
+#include <sys/time.h>
+#include <signal.h>
+
+
+#include "p1905_managerd.h"
+#include "cmdu.h"
+
+#ifdef SUPPORT_WIFI
+#include "wifi_utils.h"
+#endif
+
+//#define P1905_MANAGERD_DEBUG
+#ifdef P1905_MANAGERD_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
+
+#ifdef CHANGE_BR_AGEING_TIME
+#define BRIDGE_AGEING_TIME 150//seconds
+#endif
+
+#define P1905_TIMER_INTERVAL_SEC 5
+#define TOPOLOGY_DISCOVERY_COUNT 55
+#define DUMP_INFO_COUNT 70
+
+#ifdef SUPPORT_ALME
+#define ALME_WAIT_LINK_METRIC_RSP_CNT 2
+#endif
+
+unsigned char p1905_multicast_address[6]
+ = { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x13 };
+
+unsigned char nearest_bridge_group_address[6]
+ = { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x0e };
+
+static unsigned char lldpdu_timer_expired = 0;
+volatile sig_atomic_t timer_expired = 0;
+
+static void p1905_timer_handler(int signum)
+{
+ timer_expired = 1;
+}
+
+static void p1905_signal_handler(struct p1905_managerd_ctx *ctx)
+{
+ static unsigned int cnt = 0;
+ int is_tpg_changed = 0;
+
+ if(timer_expired)
+ {
+ timer_expired =0;
+
+ is_tpg_changed = is_topology_changed(&(ctx->topology_entry.tpddb_head),\
+ ctx->non_p1905_neighbor_dev);
+
+ if(is_tpg_changed)
+ {
+ delete_non_p1905_neighbor_dev_info(ctx->non_p1905_neighbor_dev);
+ update_non_1905_device_from_fdb(&(ctx->topology_entry.tpddb_head),
+ ctx->non_p1905_neighbor_dev);
+
+ /* use is_topology_changed() to detect any non-1905.1 neighbor
+ * device joined/removed. if yes, need to send notify.
+ */
+ ctx->mid ++;
+ 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
+ }
+
+ /* now this function be triggerd per second, use cnt to seperate
+ * different timer
+ */
+ cnt++;
+
+ if((cnt % P1905_TIMER_INTERVAL_SEC) == 0 )
+ {
+ /*1. update LLDP database, if exceed time to live , delete*/
+ update_lldp_queue_ttl(P1905_TIMER_INTERVAL_SEC);
+
+ /*2. delete 1905.1 neighbor if not receiving new topology discovery*/
+ if(delete_not_exist_p1905_neighbor_device(ctx, P1905_TIMER_INTERVAL_SEC) &&\
+ (is_tpg_changed == 0))
+ {
+ ctx->mid ++;
+ 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
+ }
+
+ /*3. if push button is pressed, need to check any new device join*/
+ if(ctx->pbc_param.is_sc)
+ detect_new_device_join(ctx);
+
+ /*4. if support AP auto config(enrollee or registrar)
+ * use this timer to do re-send mechanism
+ */
+#if defined(SUPPORT_AP_ENROLLE)
+ if(ctx->need_ap_config && (ctx->enrolle_state == no_ap_autoconfig))
+ {
+ ctx->enrolle_state = wait_4_send_ap_autoconfig_search;
+ }
+
+ if(ctx->enrolle_state > wait_4_send_ap_autoconfig_search &&
+ ctx->enrolle_state < wait_4_set_config)
+ {
+ ctx->ap_config_timeout_cnt++;
+ }
+#elif defined(SUPPORT_AP_REGISTRAR)
+ if(ctx->need_ap_config && is_connection_status_authenticated())
+ {
+ ap_auto_config_renew_start(ctx);
+ ctx->need_ap_config = 0;
+ }
+#endif
+
+ /*5. if support ALME link metrics query, use this timer to timeout*/
+#ifdef SUPPORT_ALME
+ if(ctx->alme_state == alme_wait_4_link_metrics_rsp)
+ {
+ ctx->alme_wait_4_link_metrics_cnt += 1;
+
+ if(ctx->alme_wait_4_link_metrics_cnt >= ALME_WAIT_LINK_METRIC_RSP_CNT)
+ {
+ ctx->alme_wait_4_link_metrics_cnt = 0;
+ ctx->alme_state = alme_none;
+
+ if(ctx->almefd > 0)
+ {
+ wait_4_link_metric_rsp_timeout(ctx->almefd);
+ close(ctx->almefd);
+ ctx->almefd = -1;
+ }
+ else
+ {
+ debug_syslog("error!! alme connection already closed\n");
+ }
+ }
+ }
+#endif
+ }
+
+ if((cnt % TOPOLOGY_DISCOVERY_COUNT) == 0 )
+ {
+ ctx->mid ++;
+ insert_cmdu_txq(p1905_multicast_address,ctx->plc0_mac_addr,\
+ e_topology_discovery,ctx->mid);
+ insert_cmdu_txq(p1905_multicast_address,ctx->eth0_mac_addr,\
+ e_topology_discovery,ctx->mid);
+#ifdef SUPPORT_WIFI
+ insert_cmdu_txq(p1905_multicast_address, ctx->wifi0_mac_addr,\
+ e_topology_discovery, ctx->mid);
+#endif
+ lldpdu_timer_expired = 1;
+
+ /*increment fragment timeout counter*/
+ add_fragment_cnt();
+ if(get_fragment_cnt() > 2)
+ delete_fragment_queue_all();
+ }
+
+#ifdef P1905_MANAGERD_DEBUG
+ if((cnt % DUMP_INFO_COUNT) == 0)
+ {
+ dump_topology_info(ctx);
+ }
+#endif
+
+ }
+}
+
+static int p1905_database_init(struct p1905_managerd_ctx *ctx)
+{
+ LIST_INIT(&(ctx->topology_entry.tpddb_head));
+ SLIST_INIT(&(ctx->topology_entry.tprdb_head));
+ return 0;
+}
+
+static int p1905_interface_init(struct p1905_managerd_ctx *ctx)
+{
+#ifdef SUPPORT_WIFI
+ RF_BAND rf_band;
+#endif
+
+ /*init plc0 interface information*/
+ memcpy(ctx->itf[0].mac_addr,ctx->plc0_mac_addr,ETH_ALEN);
+ ctx->itf[0].media_type = 0x0201;
+ ctx->itf[0].vs_info_length = 7;
+ ctx->itf[0].vs_info= malloc(ctx->itf[0].vs_info_length);
+ memset(ctx->itf[0].vs_info,0,ctx->itf[0].vs_info_length);
+
+ /*init eth0 interface information*/
+ memcpy(ctx->itf[1].mac_addr,ctx->eth0_mac_addr,ETH_ALEN);
+ ctx->itf[1].media_type = 0x0001;
+ ctx->itf[1].vs_info_length = 0;
+#ifdef SUPPORT_WIFI
+ /*init wifi0 interface information*/
+ memcpy(ctx->itf[2].mac_addr, ctx->wifi0_mac_addr, ETH_ALEN);
+
+ if(wifi_utils_success != get_rf_band(&rf_band))
+ {
+ debug_syslog("interface init get wifi band fail\n");
+ rf_band = rf_band_2p4G;
+ //return -1;
+ }
+ if(rf_band == rf_band_2p4G)
+ ctx->itf[2].media_type = 0x0103;//802.11n 2.4G
+ else
+ ctx->itf[2].media_type = 0x0104;//802.11n 5G
+
+ ctx->itf[2].vs_info_length = 10;
+ ctx->itf[2].vs_info = malloc(ctx->itf[2].vs_info_length);
+ memset(ctx->itf[2].vs_info, 0, ctx->itf[2].vs_info_length);
+ /*WIFI device is AP, so directly copy mac address to BSSID field*/
+ memcpy(ctx->itf[2].vs_info, ctx->wifi0_mac_addr, 6);
+#ifdef SUPPORT_WIFI_STATION
+ *((ctx->itf[2].vs_info) + 6) = 0x40;//non-ap station
+#endif
+
+#endif
+
+ /*init local bridge capability */
+#ifdef SUPPORT_WIFI
+ ctx->br_cap[0].interface_num = 3;
+#else
+ ctx->br_cap[0].interface_num = 2;
+#endif
+ ctx->br_cap[0].itf_mac_list=malloc(ETH_ALEN * ctx->br_cap[0].interface_num);
+ memcpy(ctx->br_cap[0].itf_mac_list,ctx->plc0_mac_addr,ETH_ALEN);
+ memcpy((ctx->br_cap[0].itf_mac_list + ETH_ALEN),ctx->eth0_mac_addr,ETH_ALEN);
+#ifdef SUPPORT_WIFI
+ memcpy((ctx->br_cap[0].itf_mac_list + (ETH_ALEN * 2)),ctx->wifi0_mac_addr,ETH_ALEN);
+#endif
+
+ /*init local non-p1905.1 neighbor device information*/
+ memcpy(ctx->non_p1905_neighbor_dev[0].local_mac_addr,ctx->plc0_mac_addr,ETH_ALEN);
+ ctx->non_p1905_neighbor_dev[0].br_port_no = get_bridge_port_no(PLC0_IFNAME);
+ if(ctx->non_p1905_neighbor_dev[0].br_port_no == 0)
+ {
+ syslog(LOG_WARNING, "get plc0 br port no fail");
+ return -1;
+ }
+ debug("plc0 br port no = %d\n",ctx->non_p1905_neighbor_dev[0].br_port_no);
+
+ LIST_INIT(&(ctx->non_p1905_neighbor_dev[0].non_p1905nbr_head));
+
+ memcpy(ctx->non_p1905_neighbor_dev[1].local_mac_addr,ctx->eth0_mac_addr,ETH_ALEN);
+ ctx->non_p1905_neighbor_dev[1].br_port_no = get_bridge_port_no(ETH0_IFNAME);
+ if(ctx->non_p1905_neighbor_dev[1].br_port_no == 0)
+ {
+ syslog(LOG_WARNING, "get eth0 br port no fail");
+ return -1;
+ }
+ debug("eth0 br port no = %d\n",ctx->non_p1905_neighbor_dev[1].br_port_no);
+
+ LIST_INIT(&(ctx->non_p1905_neighbor_dev[1].non_p1905nbr_head));
+
+#ifdef SUPPORT_WIFI
+ memcpy(ctx->non_p1905_neighbor_dev[2].local_mac_addr, ctx->wifi0_mac_addr, ETH_ALEN);
+ ctx->non_p1905_neighbor_dev[2].br_port_no = get_bridge_port_no(WIFI0_IFNAME);
+ if(ctx->non_p1905_neighbor_dev[2].br_port_no == 0)
+ {
+ syslog(LOG_WARNING, "get wifi0 br port no fail");
+ return -1;
+ }
+ LIST_INIT(&(ctx->non_p1905_neighbor_dev[2].non_p1905nbr_head));
+#endif
+
+ /*init local p1905.1 neighbor device information*/
+ memcpy(ctx->p1905_neighbor_dev[0].local_mac_addr,ctx->plc0_mac_addr,ETH_ALEN);
+ LIST_INIT(&(ctx->p1905_neighbor_dev[0].p1905nbr_head));
+ memcpy(ctx->p1905_neighbor_dev[1].local_mac_addr,ctx->eth0_mac_addr,ETH_ALEN);
+ LIST_INIT(&(ctx->p1905_neighbor_dev[1].p1905nbr_head));
+#ifdef SUPPORT_WIFI
+ memcpy(ctx->p1905_neighbor_dev[2].local_mac_addr, ctx->wifi0_mac_addr, ETH_ALEN);
+ LIST_INIT(&(ctx->p1905_neighbor_dev[2].p1905nbr_head));
+#endif
+ return 0;
+}
+
+static int p1905_interface_uninit(struct p1905_managerd_ctx *ctx)
+{
+ int i = 0;
+
+ for(i=0;i<ITF_NUM;i++)
+ {
+ if(ctx->itf[i].vs_info_length > 0)
+ free(ctx->itf[i].vs_info);
+ }
+
+ for(i=0;i<BRIDGE_NUM;i++)
+ {
+ free(ctx->br_cap[i].itf_mac_list);
+ }
+}
+
+static int p1905_managerd_init(struct p1905_managerd_ctx *ctx)
+{
+
+ struct itimerval t;
+ sigset_t sigmask;
+
+ //Check arguments
+ assert(ctx != NULL);
+
+ if(-1 == cmdu_init(ctx))
+ return -1;
+#ifdef SUPPORT_WIFI
+ if(wifi_utils_success != wifi_utils_init(ctx->wifi0_mac_addr))
+ return -1;
+#endif
+
+ if(-1 == p1905_database_init(ctx))
+ return -1;
+
+ if(-1 == p1905_interface_init(ctx))//must do this function after cmdu_init
+ return -1;
+
+ if(-1 == lldpdu_init(ctx))
+ return -1;
+
+#ifdef SUPPORT_ALME
+ if(-1 == ALME_init(ctx))
+ return -1;
+#endif
+ if(-1 == p1905_push_button_init(ctx))
+ return -1;
+
+#ifdef SUPPORT_ALME
+ if(-1 == ALME_sync_init(ctx))
+ return -1;
+#endif
+
+#ifdef CHANGE_BR_AGEING_TIME
+ /*set bridge ageing time*/
+ if(-1 == set_br_fdb_ageing_time(BRIDGE_AGEING_TIME))
+ return -1;
+#endif
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+ ap_autoconfig_init(ctx);
+#endif
+ /* Block the SIGALRM signal */
+ sigemptyset (&sigmask);
+ sigaddset (&sigmask, SIGALRM);
+
+ if (sigprocmask (SIG_BLOCK, &sigmask, NULL) == -1)
+ {
+ syslog(LOG_WARNING, "sigprocmask (%s)", strerror(errno));
+ return -1;
+ }
+
+ t.it_interval.tv_usec = 0;
+ t.it_interval.tv_sec = 1;//P1905_TIMER_INTERVAL_SEC;
+ t.it_value.tv_usec = 0;
+ t.it_value.tv_sec = 1;//P1905_TIMER_INTERVAL_SEC;
+
+ if( setitimer( ITIMER_REAL, &t, NULL) < 0 ){
+ debug("set p1905 timer fail\n");
+ return -1;
+ }
+ signal( SIGALRM, p1905_timer_handler);
+
+ return 0;
+}
+
+static int p1905_managerd_process(struct p1905_managerd_ctx *ctx)
+{
+ int result = 0;
+ int length = 0;
+
+ unsigned char *buffer;
+ sigset_t empty_sigmask;
+
+ buffer = (unsigned char*)malloc(MAX_PKT_LEN);
+ if(NULL == buffer)
+ {
+ debug("allocate fail\n");
+ syslog(LOG_WARNING, "cannot allocate memory (%s)", strerror(errno));
+ return -1;
+ }
+
+ //Check arguments
+ assert(ctx != NULL);
+ sigemptyset (&empty_sigmask);
+
+ //debug_syslog("sock_br0 %d, sock_lldp %d, sock_alme %d, gpio_fd %d\n",
+ // ctx->sock_br0, ctx->sock_lldp, ctx->sock_alme, ctx->gpio_fd);
+
+ while(1)
+ {
+ fd_set readfds, exceptfds;
+
+ FD_ZERO(&readfds);
+ FD_ZERO(&exceptfds);
+ /*listen to cmdu message, by ether type*/
+ FD_SET(ctx->sock_br0, &readfds);
+ /*listen to lldp message, by ether type*/
+ FD_SET(ctx->sock_lldp, &readfds);
+#ifdef SUPPORT_ALME
+ /*listen to socket for ALME*/
+ FD_SET(ctx->sock_alme, &readfds);
+#endif
+ /*listen to push button interrupt happen*/
+ FD_SET(ctx->gpio_fd, &exceptfds);
+
+ /* for listening GPIO interrupt, we must use exceptfds because of POLLPRI
+ * is the return value from spc300_gpio_poll.
+ * please check the function do_select() in select.c ==>
+ * if ((mask & POLLEX_SET) && (ex & bit))
+ * and the definition #define POLLEX_SET (POLLPRI)
+ */
+
+ /*do not block signal in pselect, if any signal arrive , return errno = EINTR */
+ result = pselect (ctx->gpio_fd + 1, &readfds, NULL, &exceptfds, NULL,&empty_sigmask);
+ /*block signal from here*/
+ if (result > 0)
+ {
+ if(FD_ISSET(ctx->sock_br0, &readfds))
+ {
+ /*receive cmdu , process here*/
+ debug("receive 1905.1 cmdu\n");
+
+ length = cmdu_bridge_receive(ctx,buffer,MAX_PKT_LEN);
+ if(0 > length)
+ {
+ debug_syslog("cmdu_bridge_receive fail \n");
+ return -1;
+ }
+
+ if(0 > cmdu_parse(ctx,buffer,length))
+ {
+ debug_syslog("cmdu parsing fail \n");
+ return -1;
+ }
+ }
+ else if(FD_ISSET(ctx->sock_lldp, &readfds))
+ {
+ /*reveive LLDPDU, process here*/
+ debug("receive lldp message\n");
+ length = receive_802_1_bridge_discovery_msg(ctx, buffer, MAX_PKT_LEN);
+ if(0 > length)
+ {
+ debug("802.1 bridge discovery receive fail \n");
+ return -1;
+ }
+
+ if(0 > parse_802_1_bridge_discovery_msg(ctx, buffer))
+ {
+ debug("802.1 bridge discovery parse fail \n");
+ return -1;
+ }
+ }
+#ifdef SUPPORT_ALME
+ else if(FD_ISSET(ctx->sock_alme, &readfds))
+ {
+ debug("receive ALME\n");
+ length = alme_receive(ctx, buffer, sizeof(ALME_MSG));
+ debug("receive ALME len = %d\n",length);
+ alme_process(ctx, buffer, length);
+ }
+#endif
+ else if(FD_ISSET(ctx->gpio_fd, &exceptfds))
+ {
+ if(0 > p1905_push_button_process(ctx))
+ {
+ debug_syslog("p1905_push_button_process fail \n");
+ return -1;
+ }
+ }
+ }
+ else
+ {
+ /*by signal*/
+ if(errno == EINTR)
+ {
+ /*pselect was interrupted and return errno == EINTR,
+ deal with the signal here*/
+ p1905_signal_handler(ctx);
+ }
+ else
+ {
+ syslog (LOG_WARNING, "select failed (%s)", strerror (errno));
+ return -1;
+ }
+ }
+#ifdef SUPPORT_AP_ENROLLE
+ if(ctx->enrolle_state != no_ap_autoconfig)
+ ap_autoconfig_enrolle_sm(ctx);
+#endif
+ /*if any cmdu need to be transmited , it will be sent here*/
+ if(0 > process_cmdu_txq(ctx,buffer))
+ {
+ debug_syslog("process cmdu txq fail \n");
+ return -1;
+ }
+
+ if(lldpdu_timer_expired)
+ {
+ /*according to IEEE P1905.1 spec, we need to send 802.1 bridge
+ *detection message after sending topology discovery message
+ *so we implement this functionality here
+ */
+ lldpdu_timer_expired = 0;
+ if(0 > send_802_1_bridge_discovery_msg(ctx,\
+ nearest_bridge_group_address, ctx->eth0_mac_addr,buffer))
+ {
+ debug_syslog("ETH send lldpdu fail \n");
+ return -1;
+ }
+
+ if(0 > send_802_1_bridge_discovery_msg(ctx,\
+ nearest_bridge_group_address, ctx->plc0_mac_addr,buffer))
+ {
+ debug_syslog("PLC send lldpdu fail \n");
+ return -1;
+ }
+#ifdef SUPPORT_WIFI
+ if(0 > send_802_1_bridge_discovery_msg(ctx,\
+ nearest_bridge_group_address, ctx->wifi0_mac_addr, buffer))
+ {
+ debug_syslog("WIFI send lldpdu fail \n");
+ return -1;
+ }
+#endif
+ }
+ }
+
+ free(buffer);
+ return result;
+}
+
+static void p1905_managerd_uninit(struct p1905_managerd_ctx *ctx)
+{
+ assert(ctx != NULL);
+
+ p1905_interface_uninit(ctx);
+ cmdu_uninit(ctx);
+ lldpdu_uninit(ctx);
+ p1905_push_button_uninit(ctx);
+
+#ifdef SUPPORT_ALME
+ ALME_uninit(ctx);
+ ALME_sync_uninit(ctx);
+#endif
+}
+
+/**
+ * Main function
+ *
+ * \param argc number of arguments.
+ * \param argv arguments pointer.
+ * \return error code.
+ */
+int main(int argc, char **argv)
+{
+ struct p1905_managerd_ctx ctx;
+ int result;
+
+ //Open syslog file
+ openlog("p1905_managerd", LOG_PID, LOG_DAEMON);
+
+ //Initialize manager daemon
+ if(0 != p1905_managerd_init(&ctx))
+ {
+ closelog();
+ return -1;
+ }
+
+ syslog (LOG_NOTICE, "p1905_Manager Daemon Running");
+
+ //Start manager daemon core
+ result = p1905_managerd_process(&ctx);
+ syslog (LOG_NOTICE, "p1905_Manager Daemon exiting");
+
+ //Uninitialize p1905_manager daemon
+ p1905_managerd_uninit(&ctx);
+
+ //Open syslog file
+ closelog();
+
+ return result;
+}
diff --git a/cleopatre/devkit/p1905_managerd/src/p1905_push_button.c b/cleopatre/devkit/p1905_managerd/src/p1905_push_button.c
new file mode 100644
index 0000000000..cc87b28147
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/p1905_push_button.c
@@ -0,0 +1,264 @@
+/*
+ * cleopatre/application/p1905_managerd/src/p1905_push_button.c
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <syslog.h>
+#include <errno.h>
+#include <assert.h>
+#include <fcntl.h>
+
+#include "p1905_managerd.h"
+#include "libspid.h"
+#include "label.h"
+#include "defs.h"
+#include "path.h"
+#include "cmdu.h"
+
+#define GPIO_DEVICE "/dev/gpio"
+#define SC_BUT_GPIO_NUM 1
+
+//#define P1905_PUSH_BUTTON_DEBUG
+#ifdef P1905_PUSH_BUTTON_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
+
+extern unsigned char p1905_multicast_address[6];
+
+int p1905_push_button_init(struct p1905_managerd_ctx *ctx)
+{
+ /* Set GPIO info for SC button interruption */
+ ctx->sc_it.gpioit.enable = 1;
+ ctx->sc_it.gpioit.mask = (1 << SC_BUT_GPIO_NUM);
+
+ /* Open GPIO device */
+ if (0 > (ctx->gpio_fd = open (GPIO_DEVICE, O_RDWR)))
+ {
+ syslog (LOG_WARNING, "cannot open gpio device (%s)", strerror (errno));
+ return -1;
+ }
+
+ /*register listener for gpio interrupt*/
+ /* Set GPIO interrupt for SC button */
+ if (0 > ioctl (ctx->gpio_fd, GPIOIOC_SETIT,
+ (unsigned long *) &ctx->sc_it))
+ {
+ syslog (LOG_WARNING, "cannot call ioctl for SC interruption (%s)",
+ strerror (errno));
+ }
+
+ ctx->pbc_param.is_sc = 0;
+
+#ifdef SUPPORT_AP_REGISTRAR
+ ctx->pbc_param.wifi_info.no_need_start_pbc = 0;
+#endif
+
+ return 0;
+}
+
+int p1905_push_button_uninit(struct p1905_managerd_ctx *ctx)
+{
+ ctx->sc_it.gpioit.enable = 0;
+ ctx->sc_it.gpioit.mask = 0;
+
+ /* Reset GPIO interrupt for SC button */
+ if (0 > ioctl (ctx->gpio_fd, GPIOIOC_SETIT,
+ (unsigned long *) &ctx->sc_it))
+ {
+ syslog (LOG_WARNING, "cannot call ioctl for SC interruption (%s)",
+ strerror (errno));
+ }
+
+ /* Close GPIO device */
+ close (ctx->gpio_fd);
+}
+
+int p1905_push_button_process(struct p1905_managerd_ctx *ctx)
+{
+ unsigned char status[LIBSPID_HPAV_INFO_STATUS_MAX_LEN] = {0};
+#ifdef SUPPORT_WIFI
+ unsigned char station_mac_list[WIFI_MAX_STATION_NUM][ETH_ALEN];
+ unsigned char sta_num = 0;
+#endif
+
+ /*send 1905.1 push button event notification to eth port*/
+ ctx->mid ++;
+ insert_cmdu_txq(p1905_multicast_address, ctx->eth0_mac_addr,\
+ e_push_button_event_notification, ctx->mid);
+
+ /* detect connection status. if authenticated, need to send
+ * 1905.1 push button event notification message. Otherwise, it just start
+ * the simple connection process.
+ */
+ if(LIBSPID_SUCCESS != libspid_config_read_item(LIBSPID_HPAV_INFO_PATH,
+ LIBSPID_HPAV_INFO_LABEL_STATUS, status, LIBSPID_HPAV_INFO_STATUS_MAX_LEN))
+ {
+ syslog (LOG_WARNING, "libspid config read item failed in p1905.1");
+ return -1;
+ }
+
+ if(!strcmp(status, LIBSPID_HPAV_INFO_VALUE_STATUS_AUTHENTICATED))
+ {
+ /*status is authenticated, need to send push button event notification*/
+ insert_cmdu_txq(p1905_multicast_address, ctx->plc0_mac_addr,
+ e_push_button_event_notification, ctx->mid);
+ }
+
+#ifdef SUPPORT_WIFI
+ if(wifi_utils_success == get_station_mac(station_mac_list, &sta_num))
+ {
+ if(sta_num > 0)
+ {
+ /* has one or more stations, so need to send PB event notification
+ * in wifi interface
+ */
+ insert_cmdu_txq(p1905_multicast_address, ctx->wifi0_mac_addr,
+ e_push_button_event_notification, ctx->mid);
+ }
+ }
+#endif
+
+ return 0;
+}
+
+int is_connection_status_authenticated(void)
+{
+ unsigned char status[LIBSPID_HPAV_INFO_STATUS_MAX_LEN] = {0};
+
+ if(LIBSPID_SUCCESS != libspid_config_read_item(LIBSPID_HPAV_INFO_PATH,
+ LIBSPID_HPAV_INFO_LABEL_STATUS, status, LIBSPID_HPAV_INFO_STATUS_MAX_LEN))
+ {
+ syslog (LOG_WARNING, "libspid config read item failed in p1905.1");
+ return -1;
+ }
+
+ if(!strcmp(status, LIBSPID_HPAV_INFO_VALUE_STATUS_AUTHENTICATED))
+ return 1;
+ else
+ return 0;
+}
+
+int trigger_push_button_config_start(struct p1905_managerd_ctx *ctx)
+{
+ /* Write SC button GPIO status */
+ libspid_config_write_item (LIBSPID_HPAV_INFO_PATH,
+ LIBSPID_HPAV_INFO_LABEL_SC_BUTTON,
+ "yes");
+
+ /* plc daemon needs to be informed */
+ if (LIBSPID_SUCCESS != libspid_system_file_update_warn (getpid(),
+ LIBSPID_HPAV_INFO_PATH))
+ {
+ syslog (LOG_WARNING, "libspid system file update warn failed");
+ return -1;
+ }
+ return 0;
+}
+
+int is_doing_sc(void)
+{
+ unsigned char status[10] = {0};
+
+ if(LIBSPID_SUCCESS != libspid_config_read_item(LIBSPID_HPAV_INFO_PATH,
+ LIBSPID_HPAV_INFO_LABEL_SC, status, 10))
+ {
+ syslog (LOG_WARNING, "libspid config read item failed in p1905.1");
+ return -1;
+ }
+
+ if(!strcmp(status, LIBSPID_VALUE_BOOLEAN_TRUE))
+ return 1;
+ else
+ return 0;
+}
+
+int detect_new_device_join(struct p1905_managerd_ctx *ctx)
+{
+ unsigned char compare[6] = {0};
+#ifdef SUPPORT_AP_REGISTRAR
+ unsigned char station_mac_list[WIFI_MAX_STATION_NUM][ETH_ALEN];
+ unsigned char sta_num = 0;
+ int i = 0, j = 0;
+#endif
+
+ /*need to implement WIFI push button status check*/
+ if(is_doing_sc())
+ {
+ debug("in simple connect state\n");
+ return 0;
+ }
+
+ if(get_station_info(ctx, 2))
+ {
+ if(memcmp(ctx->pbc_param.info.new_station, compare, 6))
+ {
+ /*has a new station joined, so send push button join notification*/
+ ctx->mid ++;
+ insert_cmdu_txq(p1905_multicast_address, ctx->eth0_mac_addr,\
+ e_push_button_join_notification, ctx->mid);
+ insert_cmdu_txq(p1905_multicast_address, ctx->plc0_mac_addr,\
+ e_push_button_join_notification, ctx->mid);
+#ifdef SUPPORT_WIFI
+ insert_cmdu_txq(p1905_multicast_address, ctx->wifi0_mac_addr,\
+ e_push_button_join_notification, ctx->mid);
+#endif
+ }
+ }
+
+#ifdef SUPPORT_AP_REGISTRAR
+ if(wifi_utils_success == get_station_mac(station_mac_list, &sta_num))
+ {
+ if(sta_num > ctx->pbc_param.wifi_info.station_num)
+ {
+ if(ctx->pbc_param.wifi_info.station_num == 0)
+ {
+ memcpy(ctx->pbc_param.wifi_info.new_station,
+ &(station_mac_list[0][0]), ETH_ALEN);
+ }
+ else
+ {
+ for(i=0;i<sta_num;i++)
+ {
+ for(j=0;j<ctx->pbc_param.wifi_info.station_num;j++)
+ {
+ if(memcmp(&(station_mac_list[i][0]),
+ &(ctx->pbc_param.wifi_info.station_mac_list[j][0]), 6))
+ {
+ memcpy(ctx->pbc_param.wifi_info.new_station,\
+ &(station_mac_list[i][0]), ETH_ALEN);
+ }
+ }
+ }
+ }
+
+ /*send push button event joined notification CMDU*/
+ ctx->mid ++;
+ insert_cmdu_txq(p1905_multicast_address, ctx->eth0_mac_addr,\
+ e_push_button_join_notification_wifi, ctx->mid);
+ insert_cmdu_txq(p1905_multicast_address, ctx->plc0_mac_addr,\
+ e_push_button_join_notification_wifi, ctx->mid);
+ insert_cmdu_txq(p1905_multicast_address, ctx->wifi0_mac_addr,\
+ e_push_button_join_notification_wifi, ctx->mid);
+ }
+ else
+ {
+ /*no new wifi station joined, set all 0*/
+ memset(ctx->pbc_param.wifi_info.new_station, 0, ETH_ALEN);
+ }
+ }
+ else
+ {
+ debug_syslog("get wifi station fail in device joined detection\n");
+ }
+#endif
+
+ ctx->pbc_param.is_sc = 0;
+ return 0;
+}
diff --git a/cleopatre/devkit/p1905_managerd/src/p1905_utils.c b/cleopatre/devkit/p1905_managerd/src/p1905_utils.c
new file mode 100644
index 0000000000..0da5cdac0d
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/p1905_utils.c
@@ -0,0 +1,928 @@
+/*
+ * cleopatre/application/p1905_managerd/src/p1905_utils.c
+ *
+ * (C) Copyright 2013 MSsar Semiconductor, Inc.
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <sys/socket.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 <linux/sockios.h>
+#include <linux/if_bridge.h>
+#include "p1905_utils.h"
+#include "cmdu_tlv.h"
+#include "libmme.h"
+#include <sys/queue.h>
+#ifdef SUPPORT_PLC
+#include "plc_utils.h"
+#endif
+
+//#define P1905_UTILS_DEBUG
+#ifdef P1905_UTILS_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
+
+static unsigned int has_confirm = 0;
+
+#define MAX_FDB_ENTRIES 48
+
+#define PATH_PLC0_PORT_NO "/sys/class/net/plc0/brport/port_no"
+#define PATH_ETH0_PORT_NO "/sys/class/net/eth0/brport/port_no"
+#ifdef SUPPORT_WIFI
+#define PATH_WIFI0_PORT_NO "/sys/class/net/ra0/brport/port_no"
+#endif
+
+int get_receive_port_addr(unsigned char *inputmac,unsigned char *outputmac)
+{
+ int fd;
+ struct ifreq ifr;
+ unsigned long args[4];
+
+ ifr.ifr_data = (char *) &args;
+
+ fd = socket(AF_INET, SOCK_STREAM, 0);
+ strncpy(ifr.ifr_name, (char*)BR0_IFNAME, IFNAMSIZ);
+ args[0] = BRCTL_GET_RECV_PORT_ADDR;
+ args[1] = (unsigned long)outputmac;
+ args[2] = (unsigned long)inputmac;
+ args[3] = 0;
+
+ if(0 > (ioctl(fd, SIOCDEVPRIVATE, &ifr)))
+ {
+ debug("bridge_get_port_addr error");
+ close(fd);
+ return -1;
+ }
+
+ close(fd);
+ return 0;
+}
+
+int set_bridge_not_forward_dest(unsigned char *mac)
+{
+ int fd;
+ struct ifreq ifr;
+ unsigned long args[4];
+
+ ifr.ifr_data = (char *) &args;
+
+ fd = socket(AF_INET, SOCK_STREAM, 0);
+ strncpy(ifr.ifr_name, (char*)BR0_IFNAME, IFNAMSIZ);
+ args[0] = BRCTL_SET_BRIDGE_NOT_FORWARD_DEST;
+ args[1] = (unsigned long)mac;
+ args[2] = 0;
+ args[3] = 0;
+
+ if(0 > (ioctl(fd, SIOCDEVPRIVATE, &ifr)))
+ {
+ debug("set bridge not forward dest fail\n");
+ close(fd);
+ return -1;
+ }
+
+ close(fd);
+ return 0;
+}
+
+int get_br_fdb_entries(struct __fdb_entry *fdb_buf)
+{
+ int fd;
+ struct ifreq ifr;
+ unsigned long args[4];
+ int num = 0;
+
+ ifr.ifr_data = (char *) &args;
+ fd = socket(AF_INET, SOCK_STREAM, 0);
+ strncpy(ifr.ifr_name, (char*)BR0_IFNAME, IFNAMSIZ);
+
+ args[0] = BRCTL_GET_FDB_ENTRIES;
+ args[1] = (unsigned long)fdb_buf;
+ //we temporarily define max entry number to 48, it can be modified
+ args[2] = MAX_FDB_ENTRIES;
+ args[3] = 0;
+
+ num = ioctl(fd, SIOCDEVPRIVATE, &ifr);
+ if(0 > num)
+ {
+ debug("get_br_fdb_entries fail\n");
+ close(fd);
+ return -1;
+ }
+ close(fd);
+ return num;
+}
+
+int set_br_fdb_ageing_time(unsigned long ageing_time)
+{
+ int fd;
+ struct ifreq ifr;
+ unsigned long args[4];
+ int result = 0;
+
+ ifr.ifr_data = (char *) &args;
+ fd = socket(AF_INET, SOCK_STREAM, 0);
+ strncpy(ifr.ifr_name, (char*)BR0_IFNAME, IFNAMSIZ);
+
+ args[0] = BRCTL_SET_AGEING_TIME;
+ args[1] = (unsigned long)(ageing_time * 100);
+ args[2] = 0;
+ args[3] = 0;
+
+ result = ioctl(fd, SIOCDEVPRIVATE, &ifr);
+ if(0 > result)
+ {
+ debug("set br ageing time fail\n");
+ close(fd);
+ return -1;
+ }
+ close(fd);
+ return 0;
+}
+
+static int is_p1905_device(struct list_head_tpddb *tpd_head, unsigned char *mac)
+{
+ struct list_head *pos, *pos1;
+ struct topology_discovery_db *dcv_db;
+
+ if(!LIST_EMPTY(tpd_head))
+ {
+ LIST_FOREACH(dcv_db, tpd_head, tpddb_entry)
+ {
+ if(!memcmp(mac, dcv_db->al_mac, ETH_ALEN))
+ {
+ /* this mac existed in topology discovery database.
+ * so it is a p1905 neighbor , return 1.
+ */
+ debug("AL ==>exist in topology discovery\n");
+ return 1;
+ }
+
+ if(!memcmp(mac, dcv_db->itf_mac, ETH_ALEN))
+ {
+ /* this mac existed in topology discovery database.
+ * so it is a p1905 neighbor , return 1.
+ */
+ debug("itf ==>exist in topology discovery\n");
+ return 1;
+ }
+ }
+ }
+
+#if 0
+ /*compare with topology response database*/
+ if(!SLIST_EMPTY(tpgr_head))
+ {
+ SLIST_FOREACH(tpr_db, tpgr_head, tprdb_entry)
+ {
+ if(!memcmp(mac, tpr_db->al_mac_addr, ETH_ALEN))
+ {
+ /* mac address exist in AL mac field in topology response
+ * database, so this device is a 1905.1 device
+ */
+ debug("AL ==>exist in topology response\n");
+ return 1;
+ }
+
+ /* topology response db is not empty , so we already init devinfo
+ * head before. We can use this list head directly
+ */
+ if(!SLIST_EMPTY(&(tpr_db->devinfo_head)))
+ {
+ SLIST_FOREACH(dev_info, &(tpr_db->devinfo_head), devinfo_entry)
+ {
+ if(!memcmp(mac, dev_info->mac_addr, ETH_ALEN))
+ {
+ /* mac address exist in local mac field in topology response
+ * database, so this device is a 1905.1 device
+ */
+ debug("interface ==> exist in topology response\n");
+ return 1;
+ }
+ }
+ }
+ }
+ }
+#endif
+
+
+ return 0;
+}
+
+void update_non_1905_device_from_fdb(struct list_head_tpddb *tpd_head,
+ struct non_p1905_neighbor *dev)
+{
+ struct __fdb_entry *fdb, *fdb1;
+ int num = 0, i = 0, j=0;
+
+ struct non_p1905_neighbor_info *dev_info;
+
+ fdb = (struct __fdb_entry*)malloc(MAX_FDB_ENTRIES * sizeof(struct __fdb_entry));
+ num = get_br_fdb_entries(fdb);
+
+ if(num > 0)
+ {
+ fdb1 = fdb;
+ for(i = 0;i < num; i++)
+ {
+ debug("fdb%d mac:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x portno%d: local:%d\n",
+ i,fdb1->mac_addr[0],fdb1->mac_addr[1],fdb1->mac_addr[2],
+ fdb1->mac_addr[3],fdb1->mac_addr[4],fdb1->mac_addr[5],
+ fdb1->port_no, fdb1->is_local);
+ if(!fdb1->is_local)
+ {
+ if(!is_p1905_device(tpd_head, fdb1->mac_addr))
+ {
+ /* update to non-1905.1 neighbor device database
+ * use fdb1->port_no to distinguish differnet interface
+ */
+ for(j=0;j<ITF_NUM;j++)
+ {
+ if(fdb1->port_no == dev[j].br_port_no)
+ {
+ debug("get br port no = %d\n",dev[j].br_port_no);
+ break;
+ }
+ }
+ /* no need to check existence of non-1905.1 device
+ * because we will delete all before updating this database
+ */
+ dev_info = (struct non_p1905_neighbor_info *)
+ malloc(sizeof(struct non_p1905_neighbor_info));
+ memcpy(dev_info->itf_mac_addr, fdb1->mac_addr, ETH_ALEN);
+ LIST_INSERT_HEAD(&(dev[j].non_p1905nbr_head), dev_info,
+ non_p1905nbr_entry);
+ debug("new non-1905.1 device added\n");
+ }
+ }
+ fdb1++;
+ }
+ }
+ free(fdb);
+}
+
+int is_topology_changed(struct list_head_tpddb *tpd_head,
+ struct non_p1905_neighbor *dev)
+{
+ struct __fdb_entry *fdb, *fdb1;
+ int num = 0, i = 0, j=0;
+
+ struct non_p1905_neighbor_info *dev_info;
+ unsigned char exist = 0;
+ unsigned short non_1905_neighbor_num = 0;
+ unsigned short cnt = 0;
+
+ /*detect any new or removed non-1905.1 neighbor device*/
+ fdb = (struct __fdb_entry*)malloc(MAX_FDB_ENTRIES * sizeof(struct __fdb_entry));
+ num = get_br_fdb_entries(fdb);
+
+ if(num > 0)
+ {
+ fdb1 = fdb;
+ for(i = 0;i < num; i++)
+ {
+ debug("fdb1%d mac:%.2x:%.2x:%.2x:%.2x:%.2x:%.2x portno%d: local:%d\n",
+ i,fdb1->mac_addr[0],fdb1->mac_addr[1],fdb1->mac_addr[2],
+ fdb1->mac_addr[3],fdb1->mac_addr[4],fdb1->mac_addr[5],
+ fdb1->port_no, fdb1->is_local);
+
+ if(!fdb1->is_local)
+ {
+ if(!is_p1905_device(tpd_head, fdb1->mac_addr))
+ {
+ /*calculate the number of total non-1901.1 neighbors*/
+ non_1905_neighbor_num++;
+
+ for(j=0;j<ITF_NUM;j++)
+ {
+ if(fdb1->port_no == dev[j].br_port_no)
+ {
+ debug("get br port no = %d\n",dev[j].br_port_no);
+ break;
+ }
+ }
+
+
+ /* if list is empty, it means that no non-1905.1 device
+ * originally and a new non-1905.1 neighbor device joined.
+ * So return true
+ */
+ if(LIST_EMPTY(&(dev[j].non_p1905nbr_head)))
+ {
+ debug("topology change, new device joined\n");
+ free(fdb);
+ return 1;
+ }
+ else
+ {
+ exist = 0;
+ LIST_FOREACH(dev_info, &(dev[j].non_p1905nbr_head),
+ non_p1905nbr_entry)
+ {
+ if(!memcmp(dev_info->itf_mac_addr, fdb1->mac_addr,\
+ ETH_ALEN))
+ {
+ exist = 1;
+ break;
+ }
+ }
+ /* compare with all the non-1905.1 neighbor device
+ * and cannot find a matched device. So this is a new
+ * joined non-1905.1 neighbor , return true
+ */
+ if(!exist)
+ {
+ debug("topology change, another new device joined\n");
+ free(fdb);
+ return 1;
+ }
+ }
+ }
+ }
+ fdb1++;
+ }
+ }
+ free(fdb);
+
+ /* if goto here, it means no new non-1905.1 neighbor joined. So check if any
+ * non-1905.1 neighbor removed. Calculate total num of local non-1905.1
+ * neighbor and compare with non_1905_neighbor_num.
+ */
+ for(j=0;j<ITF_NUM;j++)
+ LIST_FOREACH(dev_info, &(dev[j].non_p1905nbr_head), non_p1905nbr_entry)
+ {
+ cnt++;
+ }
+ debug("non_1905_neighbor_num=%d, cnt = %d\n",non_1905_neighbor_num,cnt);
+ if(non_1905_neighbor_num < cnt)
+ {
+ debug("topology change, device removed\n");
+ return 1;
+ }
+ else
+ {
+ debug("topology is the same\n");
+ return 0;
+ }
+}
+
+int get_tx_link_statistics(link_stat *stat, itftype media_type,
+ unsigned char *local_itf, unsigned char *neighbor_itf)
+{
+ FILE *fp;
+ char tmp_buffer[256];
+ char name[64], *ptr;
+
+ if((media_type >= IEEE802_3_GROUP) && (media_type < IEEE802_11_GROUP))
+ {
+ if((fp = fopen ("/proc/net/dev", "r")) == NULL)
+ return 0;
+
+ fgets(tmp_buffer, sizeof(tmp_buffer), fp);
+ fgets(tmp_buffer, sizeof(tmp_buffer), fp);
+
+ while(fgets (tmp_buffer, sizeof(tmp_buffer), fp))
+ {
+ if((ptr = strchr (tmp_buffer, ':')) == NULL)
+ continue;
+ *ptr = '\0';
+ ptr++;
+ if((sscanf (tmp_buffer, "%s", name) == 1)
+ && !strncmp (name, ETH0, strlen(ETH0))
+ && (sscanf (ptr, "%*s %lu %lu %*s %*s %*s %*s %*s %*s %lu %lu %*s %*s %*s %*s %*s",
+ &stat->total_rx_packets, &stat->error_rx_packets,
+ &stat->total_tx_packets, &stat->error_tx_packets)) == 4)
+ {
+ debug("ETH tx statics %lu,%lu,%lu,%lu\n",stat->total_rx_packets,
+ stat->error_rx_packets,stat->total_tx_packets,
+ stat->error_tx_packets);
+
+ fclose(fp);
+ return 1;
+ }
+ }
+
+ fclose(fp);
+ return 0;
+ }
+#ifdef SUPPORT_WIFI
+ else if(media_type >= IEEE802_11_GROUP && media_type < IEEE1901_GROUP)
+ {
+ if(wifi_utils_success != get_tx_statistics(neighbor_itf, &stat->total_tx_packets,
+ &stat->error_tx_packets))
+ return 0;
+
+ return 1;
+ }
+#endif
+#ifdef SUPPORT_PLC
+ else if(media_type >= IEEE1901_GROUP && media_type < MOCA_GROUP)
+ {
+ if(plc_utils_success != plc_get_tx_link_statistics(local_itf,
+ neighbor_itf, &stat->total_tx_packets, &stat->error_tx_packets, 1))
+ return 0;
+ return 1;
+ }
+#endif
+ else
+ {
+ debug_syslog("get_tx_link_statistics unsupport media type\n");
+ return 0;
+ }
+}
+
+int get_rx_link_statistics(link_stat *stat, itftype media_type,
+ unsigned char *local_itf, unsigned char *neighbor_itf)
+{
+ FILE *fp;
+ char tmp_buffer[256];
+ char name[64], *ptr;
+
+ if(media_type >= IEEE802_3_GROUP && media_type < IEEE802_11_GROUP)
+ {
+ if((fp = fopen ("/proc/net/dev", "r")) == NULL)
+ return 0;
+
+ fgets(tmp_buffer, sizeof(tmp_buffer), fp);
+ fgets(tmp_buffer, sizeof(tmp_buffer), fp);
+
+ while(fgets (tmp_buffer, sizeof(tmp_buffer), fp))
+ {
+ if((ptr = strchr (tmp_buffer, ':')) == NULL)
+ continue;
+ *ptr = '\0';
+ ptr++;
+ if((sscanf (tmp_buffer, "%s", name) == 1)
+ && !strncmp (name, ETH0, strlen(ETH0))
+ && (sscanf (ptr, "%*s %lu %lu %*s %*s %*s %*s %*s %*s %lu %lu %*s %*s %*s %*s %*s",
+ &stat->total_rx_packets, &stat->error_rx_packets,
+ &stat->total_tx_packets, &stat->error_tx_packets)) == 4)
+ {
+ debug("ETH tx statics %lu,%lu,%lu,%lu\n",stat->total_rx_packets,
+ stat->error_rx_packets,stat->total_tx_packets,
+ stat->error_tx_packets);
+
+ fclose(fp);
+ return 1;
+ }
+ }
+
+ fclose(fp);
+ return 0;
+ }
+#ifdef SUPPORT_WIFI
+ else if(media_type >= IEEE802_11_GROUP && media_type < IEEE1901_GROUP)
+ {
+ if(wifi_utils_success != get_rx_statistics(neighbor_itf, &stat->total_rx_packets,
+ &stat->error_rx_packets))
+ return 0;
+
+ return 1;
+ }
+#endif
+#ifdef SUPPORT_PLC
+ else if(media_type >= IEEE1901_GROUP && media_type < MOCA_GROUP)
+ {
+ if(plc_utils_success != plc_get_tx_link_statistics(neighbor_itf,
+ local_itf, &stat->total_rx_packets, &stat->error_rx_packets, 0))
+ return 0;
+ return 1;
+ }
+#endif
+ else
+ {
+ debug_syslog("get_rx_link_statistics unsupport media type\n");
+ return 0;
+ }
+}
+
+unsigned short get_bridge_port_no(char *itf_name)
+{
+ FILE *fp;
+ char tmp_buffer[10];
+ unsigned short port_no = 0;
+
+ if(!strncmp(itf_name,"plc0",5))
+ {
+ if((fp = fopen (PATH_PLC0_PORT_NO, "r")) == NULL)
+ return 0;
+ }
+ else if(!strncmp(itf_name,"eth0",5))
+ {
+ if((fp = fopen (PATH_ETH0_PORT_NO, "r")) == NULL)
+ return 0;
+ }
+#ifdef SUPPORT_WIFI
+ else if(!strncmp(itf_name, "ra0", 5))
+ {
+ if((fp = fopen (PATH_WIFI0_PORT_NO, "r")) == NULL)
+ return 0;
+ }
+#endif
+ else
+ return 0;
+
+ fgets(tmp_buffer, sizeof(tmp_buffer), fp);
+ sscanf(tmp_buffer, "0x%hx",&port_no);
+ fclose(fp);
+ return port_no;
+}
+
+static void* mme_thread(void *arg)
+{
+ mme_error_t ret = MME_SUCCESS;
+ mme_ctx_t s_rcv_ctx, s_snd_ctx;
+ mme_ctx_t *rcv_ctx = &s_rcv_ctx;
+ mme_ctx_t *snd_ctx = &s_snd_ctx;
+ thread_ctx *td_ctx;
+
+ unsigned char snd_buff[2048] = {0};
+ unsigned char rcv_buff[2048] = {0};
+ unsigned char dst_addr[6] = {0};
+
+ td_ctx = (thread_ctx *)arg;
+ memcpy(dst_addr,td_ctx->local_itf, ETH_ALEN);
+
+ /* form the send message context */
+ snd_ctx->buffer = snd_buff;
+ snd_ctx->mmtype = td_ctx->mmtype;
+ snd_ctx->length = sizeof(snd_buff);
+ snd_ctx->head = 256;
+ snd_ctx->tail = 256;
+ snd_ctx->status = MME_STATUS_OK;
+
+ /* form the receive message context */
+ rcv_ctx->buffer = rcv_buff;
+ rcv_ctx->mmtype = td_ctx->mmtype + 1;
+ rcv_ctx->length = sizeof(rcv_buff);
+ rcv_ctx->head = 256;
+ rcv_ctx->tail = 256;
+ rcv_ctx->status = MME_STATUS_OK;
+
+ ret = mme_send (snd_ctx, MME_SEND_REQ_CNF, NULL, dst_addr, rcv_ctx);
+
+ if(ret == MME_SUCCESS)
+ {
+ has_confirm = 1;
+ memcpy(td_ctx->recv_buf, (rcv_ctx->buffer + rcv_ctx->head),
+ (rcv_ctx->tail - rcv_ctx->head));
+ }
+ else
+ {
+ debug_syslog("mme fail, ret=%d\n",ret);
+ has_confirm = 0;
+ }
+}
+
+int get_link_phy_rate(itftype media_type, unsigned char *target,
+ unsigned char *local_itf, unsigned short *phyrate)
+{
+ if(media_type >= IEEE802_3_GROUP && media_type < IEEE802_11_GROUP)
+ {
+ *phyrate = 0;//need to implement ETH phyrate
+ }
+#ifdef SUPPORT_WIFI
+ /* If the media type of the link is IEEE 802.3, then IEEE 1901 or MoCA 1.1
+ * (8 MSB bits value of media type as defined in Table 6-12 is 0, 2, or 3).
+ * This value is the PHY rate estimated at the transmitter of the link expressed in Mb/s;
+ * otherwise, it is set to 0xFFFF.
+ * Samuel: based on description above in spec p40, we should set phy rate to 0xffff
+ * but for good user experience, I still get real PHY rate from wifi
+ */
+ else if(media_type >= IEEE802_11_GROUP && media_type < IEEE1901_GROUP)
+ {
+ if(wifi_utils_success != get_phyrate(target, phyrate))
+ *phyrate = 0;
+ }
+#endif
+#ifdef SUPPORT_PLC
+ else if(media_type >= IEEE1901_GROUP && media_type < MOCA_GROUP)
+ {
+ if(plc_utils_success != plc_get_tx_link_phyrate(local_itf, target,
+ phyrate))
+ *phyrate = 0xFFFF;
+ }
+#endif
+ else
+ *phyrate = 0xFFFF;
+
+ return 1;
+}
+
+#ifdef SUPPORT_PLC
+int get_network_id(unsigned char *local_itf, unsigned char *network_id)
+{
+ if(plc_utils_success != PLC_get_network_id(local_itf, network_id))
+ return 0;
+
+ return 1;
+}
+#endif
+
+unsigned short
+get_link_max_throughput_capacity(unsigned short phyrate, itftype media_type)
+{
+ /*max throughput capacity = phyrate & mac overhead*/
+ unsigned short cap;
+
+ if(media_type >= IEEE802_3_GROUP && media_type < IEEE802_11_GROUP)
+ {
+ cap = 0;//need to implement
+ }
+#ifdef SUPPORT_WIFI
+ else if(media_type >= IEEE802_11_GROUP && media_type < IEEE1901_GROUP)
+ {
+ cap = 0;
+ }
+#endif
+#ifdef SUPPORT_PLC
+ else if(media_type >= IEEE1901_GROUP && media_type < MOCA_GROUP)
+ {
+ plc_get_mac_throughput_capacity(phyrate, &cap);
+ }
+#endif
+ else
+ cap = 0;
+
+ return cap;
+}
+
+int get_link_rssi(char *rssi, unsigned char peer_mac[], itftype media_type)
+{
+#ifdef SUPPORT_WIFI
+ if(media_type >= IEEE802_11_GROUP && media_type < IEEE1901_GROUP)
+ {
+ if(wifi_utils_success != get_rssi(rssi, peer_mac))
+ {
+ debug_syslog("get rssi fail\n");
+ return 0;
+ }
+ }
+ else
+#endif
+ *rssi = 0xFF;
+
+ return 1;
+}
+
+int get_station_info(struct p1905_managerd_ctx *ctx, unsigned char step)
+{
+
+ if(step == 1)
+ {
+ if(plc_utils_success != plc_get_station_info(ctx->plc0_mac_addr, &(ctx->pbc_param.info), 1))
+ return 0;
+ }
+ else if(step == 2)
+ {
+ if(plc_utils_success != plc_get_station_info(ctx->plc0_mac_addr, &(ctx->pbc_param.info), 2))
+ return 0;
+ }
+
+ return 1;
+}
+
+int is_neighbor_wifi_sta(struct p1905_managerd_ctx *ctx, unsigned char sta_itf[],
+ unsigned char al_mac[])
+{
+ struct topology_discovery_db *tpg_db;
+ int i = 0;
+
+ if(!LIST_EMPTY(&(ctx->topology_entry.tpddb_head)))
+ {
+ LIST_FOREACH(tpg_db, &(ctx->topology_entry.tpddb_head), tpddb_entry)
+ {
+ if(memcmp(tpg_db->al_mac, al_mac, 6))
+ continue;
+ for(i=0;i<ITF_NUM;i++)
+ {
+ /*check 802.11 specific information length*/
+ if(ctx->itf[i].vs_info_length == 10)
+ {
+ /*check the receiving interface is AP*/
+ if(*((ctx->itf[i].vs_info)+6) == 0x00)
+ {
+ if(!memcmp(tpg_db->receive_itf_mac, ctx->itf[i].mac_addr, 6))
+ {
+ memcpy(sta_itf, tpg_db->itf_mac, 6);
+ return 1;
+ }
+ }
+ }
+ }
+ }
+ }
+
+ return 0;
+}
+
+void dump_topology_info(struct p1905_managerd_ctx *ctx)
+{
+ struct topology_discovery_db *tpg_db;
+ struct topology_response_db *tpr_db;
+ struct device_info_db *dev_info;
+ struct p1905_neighbor_device_db *p1905_dev;
+ struct non_p1905_neighbor_device_list_db *non_p1905_dev;
+ struct p1905_neighbor_info *neighbor_dev_info;
+ struct non_p1905_neighbor_info *non_1905neighbor_info;
+
+ struct device_bridge_capability_db *br_cap;
+ int i = 0;
+ int j = 0;
+ int k = 0;
+ int z = 0;
+ int a = 0, b = 0;
+ int f = 0;
+
+ printf("====================topology discovery====================\n");
+ if(!LIST_EMPTY(&(ctx->topology_entry.tpddb_head)))
+ {
+ LIST_FOREACH(tpg_db, &(ctx->topology_entry.tpddb_head), tpddb_entry)
+ {
+ i++;
+ printf("neighbor%d AL MAC:\n",i);
+ printf("0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",tpg_db->al_mac[0],
+ tpg_db->al_mac[1],tpg_db->al_mac[2],tpg_db->al_mac[3],
+ tpg_db->al_mac[4],tpg_db->al_mac[5]);
+ printf("neighbor%d connect interface MAC:\n",i);
+ printf("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]);
+ }
+ }
+ else
+ printf("!!!no topology discovery\n");
+
+ printf("====================topology response====================\n");
+ if(!SLIST_EMPTY(&(ctx->topology_entry.tprdb_head)))
+ {
+ i = 0;
+ SLIST_FOREACH(tpr_db, &(ctx->topology_entry.tprdb_head), tprdb_entry)
+ {
+
+ i++;
+ printf("device%d AL MAC:\n",i);
+ printf("0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",tpr_db->al_mac_addr[0],
+ tpr_db->al_mac_addr[1],tpr_db->al_mac_addr[2],
+ tpr_db->al_mac_addr[3],tpr_db->al_mac_addr[4],
+ tpr_db->al_mac_addr[5]);
+ if(!SLIST_EMPTY(&(tpr_db->devinfo_head)))
+ {
+ SLIST_FOREACH(dev_info, &(tpr_db->devinfo_head), devinfo_entry)
+ {
+ j++;
+ printf("device%d interface%d :\n",i,j);
+ printf("mac = 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+ dev_info->mac_addr[0],dev_info->mac_addr[1],
+ dev_info->mac_addr[2],dev_info->mac_addr[3],
+ dev_info->mac_addr[4],dev_info->mac_addr[5]);
+ printf("media type = %d\n",dev_info->media_type);
+ if(dev_info->vs_info_len > 0)
+ {
+ printf("media specific info: \n");
+ for(k=0;k<7;k++)
+ printf("0x%x ",*(dev_info->vs_info + k));
+ printf("\n");
+ }
+
+ if(!SLIST_EMPTY(&(dev_info->p1905_nbrdb_head)))
+ {
+ SLIST_FOREACH(p1905_dev, &(dev_info->p1905_nbrdb_head), p1905_nbrdb_entry)
+ {
+ z++;
+ printf("p1905 neighbor device%d :\n",z);
+ printf("AL MAC = 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+ p1905_dev->p1905_neighbor_al_mac[0],
+ p1905_dev->p1905_neighbor_al_mac[1],
+ p1905_dev->p1905_neighbor_al_mac[2],
+ p1905_dev->p1905_neighbor_al_mac[3],
+ p1905_dev->p1905_neighbor_al_mac[4],
+ p1905_dev->p1905_neighbor_al_mac[5]);
+ if(p1905_dev->ieee_802_1_bridge_exist)
+ printf("802.1 bridge exist\n");
+ else
+ printf("802.1 bridge not exist\n");
+
+ }
+ z = 0;
+
+ }
+ else
+ printf("!!!no p1905.1 neighbor device\n");
+
+ if(!SLIST_EMPTY(&(dev_info->non_p1905_nbrdb_head)))
+ {
+ SLIST_FOREACH(non_p1905_dev, &(dev_info->non_p1905_nbrdb_head), non_p1905_nbrdb_entry)
+ {
+ z++;
+
+ printf("non p1905 neighbor device%d :\n",z);
+ printf("MAC = 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+ non_p1905_dev->non_p1905_device_interface_mac[0],
+ non_p1905_dev->non_p1905_device_interface_mac[1],
+ non_p1905_dev->non_p1905_device_interface_mac[2],
+ non_p1905_dev->non_p1905_device_interface_mac[3],
+ non_p1905_dev->non_p1905_device_interface_mac[4],
+ non_p1905_dev->non_p1905_device_interface_mac[5]);
+
+ }
+ z = 0;
+ }
+ else
+ printf("!!!no non-p1905.1 neighbor device\n");
+ }
+ j = 0;
+
+ }
+
+ if(!LIST_EMPTY(&(tpr_db->brcap_head)))
+ {
+ LIST_FOREACH(br_cap, &(tpr_db->brcap_head), brcap_entry)
+ {
+ a++;
+ printf("device%d bridge%d:\n",i,a);
+ printf("interface amount = %d\n",br_cap->interface_amount);
+ if(br_cap->interface_amount > 0)
+ {
+ printf("interface_mac_tuple:\n");
+ for(b=0;b<(br_cap->interface_amount)*6;b++)
+ printf("0x%x ",*((br_cap->interface_mac_tuple)+b));
+ printf("\n");
+ }
+ }
+ }
+ }
+ }
+
+ printf("====================local info====================\n");
+ j=0;
+ f=0;
+ for(i=0;i<ITF_NUM;i++)
+ {
+ printf("local MAC %d = 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n", i,
+ ctx->p1905_neighbor_dev[i].local_mac_addr[0],
+ ctx->p1905_neighbor_dev[i].local_mac_addr[1],
+ ctx->p1905_neighbor_dev[i].local_mac_addr[2],
+ ctx->p1905_neighbor_dev[i].local_mac_addr[3],
+ ctx->p1905_neighbor_dev[i].local_mac_addr[4],
+ ctx->p1905_neighbor_dev[i].local_mac_addr[5]);
+
+ if(!LIST_EMPTY(&(ctx->p1905_neighbor_dev[i].p1905nbr_head)))
+ {
+ LIST_FOREACH(neighbor_dev_info, &(ctx->p1905_neighbor_dev[i].p1905nbr_head),
+ p1905nbr_entry)
+ {
+ j++;
+ printf("neighbor device %d\n AL MAC =\n",j);
+ printf("0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+ neighbor_dev_info->al_mac_addr[0],
+ neighbor_dev_info->al_mac_addr[1],
+ neighbor_dev_info->al_mac_addr[2],
+ neighbor_dev_info->al_mac_addr[3],
+ neighbor_dev_info->al_mac_addr[4],
+ neighbor_dev_info->al_mac_addr[5]);
+
+ printf("802.1 bridge: ");
+ if(neighbor_dev_info->ieee802_1_bridge)
+ printf("yes\n");
+ else
+ printf("NO\n");
+ }
+ }
+ else
+ printf("no 1905.1 neighbors!!!\n");
+
+
+ if(!LIST_EMPTY(&(ctx->non_p1905_neighbor_dev[i].non_p1905nbr_head)))
+ {
+ LIST_FOREACH(non_1905neighbor_info, &(ctx->non_p1905_neighbor_dev[i].non_p1905nbr_head),
+ non_p1905nbr_entry)
+ {
+ f++;
+ printf("non 1905 neighbor device %d\n MAC =\n",f);
+ printf("0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+ non_1905neighbor_info->itf_mac_addr[0],
+ non_1905neighbor_info->itf_mac_addr[1],
+ non_1905neighbor_info->itf_mac_addr[2],
+ non_1905neighbor_info->itf_mac_addr[3],
+ non_1905neighbor_info->itf_mac_addr[4],
+ non_1905neighbor_info->itf_mac_addr[5]);
+ }
+ }
+ else
+ printf("no non-1905.1 neighbors!!!\n");
+ }
+}
+
diff --git a/cleopatre/devkit/p1905_managerd/src/plc_utils.c b/cleopatre/devkit/p1905_managerd/src/plc_utils.c
new file mode 100644
index 0000000000..8ab81b1bb1
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/plc_utils.c
@@ -0,0 +1,328 @@
+/*
+ * cleopatre/application/p1905_managerd/src/plc_utils.c
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ *
+ */
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <syslog.h>
+#include <errno.h>
+#include "plc_utils.h"
+#include "p1905_managerd.h"
+
+//#define PLC_UTILS_DEBUG
+#ifdef PLC_UTILS_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
+
+#define MAX_MSDU_SIZE 1518
+#define MAC_HEADER_SIZE 6
+#define PHY_BLOCK_HEADER_SIZE 8
+
+typedef struct _CM_LINK_STAT_REQ
+{
+ unsigned char reqtype;
+ unsigned char reqid;
+ unsigned char NID[7];
+ unsigned char LID;
+ unsigned char tlflag;
+ unsigned char mgmt_flag;
+ unsigned char DstSrc[6];
+
+} __attribute__ ((__packed__)) CM_LINK_STAT_REQ;
+
+
+static PLC_UTILS_STATUS
+plc_send_recv_mme(PLC_MME_PARAM *param)
+{
+ mme_error_t ret = MME_SUCCESS;
+ mme_ctx_t s_rcv_ctx, s_snd_ctx;
+ mme_ctx_t *rcv_ctx = &s_rcv_ctx;
+ mme_ctx_t *snd_ctx = &s_snd_ctx;
+ unsigned char recv_buffer[2048];
+
+ snd_ctx->buffer = param->sndbuf;
+ snd_ctx->mmtype = param->mmtype;
+ snd_ctx->length = sizeof(param->sndbuf);
+ snd_ctx->head = 0;
+ snd_ctx->tail = 0 + param->sndlen;
+ snd_ctx->status = MME_STATUS_OK;
+
+ rcv_ctx->buffer = recv_buffer;
+ rcv_ctx->mmtype = param->mmtype + 1;
+ rcv_ctx->length = sizeof(recv_buffer);
+ rcv_ctx->head = 256;
+ rcv_ctx->tail = 256;
+ rcv_ctx->status = MME_STATUS_OK;
+
+ ret = mme_send (snd_ctx, param->sndtype, NULL, param->dst, rcv_ctx);
+
+ if(ret != MME_SUCCESS)
+ {
+ return plc_utils_fail;
+ }
+
+ param->rcvlen = (rcv_ctx->tail - rcv_ctx->head);
+ memcpy(param->rcvbuf, (rcv_ctx->buffer + rcv_ctx->head),
+ (rcv_ctx->tail - rcv_ctx->head));
+
+ return plc_utils_success;
+}
+
+PLC_UTILS_STATUS
+PLC_get_network_id(unsigned char *local_itf, unsigned char *nid)
+{
+ PLC_MME_PARAM param;
+ unsigned char *temp_buf;
+
+ param.mmtype = CM_NW_INFO_REQ;
+ param.sndtype = MME_SEND_REQ_CNF;
+ memcpy(param.dst, local_itf, 6);
+ param.sndlen = 0;
+
+ if(plc_utils_success != plc_send_recv_mme(&param))
+ return plc_utils_fail;
+
+ temp_buf = param.rcvbuf;
+
+ /*shift to nid field, ref HPAV spec 11.5.27*/
+ temp_buf += 1;
+ memcpy(nid, temp_buf, 7);
+
+ return plc_utils_success;
+}
+
+PLC_UTILS_STATUS
+plc_get_tx_link_statistics(unsigned char *local_itf, unsigned char *neighbor_itf,
+ unsigned int *tx_total, unsigned int *tx_error, unsigned char is_tx)
+{
+ PLC_MME_PARAM param;
+ CM_LINK_STAT_REQ *req;
+ static unsigned char reqid = 0x5;
+ unsigned char *temp_buf;
+ unsigned int value;
+
+ param.mmtype = CM_LINK_STATS_REQ;
+ param.sndtype = MME_SEND_REQ_CNF;
+ memcpy(param.dst, local_itf, 6);
+ param.sndlen = sizeof(CM_LINK_STAT_REQ);
+
+ /*fill MME payload*/
+ req = (CM_LINK_STAT_REQ *)param.sndbuf;
+ req->reqtype = 0x01;//get corresponding link statistics
+ if(reqid == 0xff) reqid = 0;
+ reqid++;
+ req->reqid = reqid;
+
+ if(is_tx)
+ {
+ if(plc_utils_success != PLC_get_network_id(local_itf, req->NID))
+ {
+ debug_syslog("plc_get_tx_link_statistics get network id fail\n");
+ return plc_utils_fail;
+ }
+ }
+ else
+ {
+ if(plc_utils_success != PLC_get_network_id(neighbor_itf, req->NID))
+ {
+ debug_syslog("plc_get_tx_link_statistics get network id fail\n");
+ return plc_utils_fail;
+ }
+ }
+
+ req->LID = 0x01;
+ req->tlflag = 0x00; //transmit link
+ req->mgmt_flag = 0x00;//not mme
+ memcpy(req->DstSrc, neighbor_itf, 6);
+
+ if(plc_utils_success != plc_send_recv_mme(&param))
+ {
+ debug_syslog("plc_get_tx_link_statistics send CM_LINK_STATS_REQ fail\n");
+ return plc_utils_fail;
+ }
+ temp_buf = param.rcvbuf;
+ /*parse payload of CM_LINK_STATS_CNF, refer to HPAV 1.1 11.5.36*/
+ if(reqid != *(temp_buf))
+ {
+ debug_syslog("req id different %d %d\n",reqid, *(temp_buf));
+ return plc_utils_fail;
+ }
+ temp_buf += 1;
+
+ if(0 != (*temp_buf))//failure
+ {
+ debug_syslog("CM_LINK_STATS_CNF fail\n");
+ return plc_utils_fail;
+ }
+ temp_buf += 1;
+
+ /*shift 26 otects to TX_NumMPDUs field*/
+ temp_buf += 26;
+ value = *(temp_buf + 3);
+ value = ((value << 8) & 0x0000FF00)| (*(temp_buf + 2));
+ value = ((value << 8) & 0x00FFFF00)| (*(temp_buf + 1));
+ value = ((value << 8) & 0xFFFFFF00)| (*temp_buf);
+ *tx_total = value;
+
+ /*shift 8 otects to TX_NumSACKs field*/
+ temp_buf += 8;
+ value = *(temp_buf + 3);
+ value = ((value << 8) & 0x0000FF00)| (*(temp_buf + 2));
+ value = ((value << 8) & 0x00FFFF00)| (*(temp_buf + 1));
+ value = ((value << 8) & 0xFFFFFF00)| (*temp_buf);
+ *tx_error = (*tx_total) - value;
+
+ debug_syslog("plc_get_tx_link_statistics total %d success %d error %d\n",
+ *tx_total, value, *tx_error);
+
+ return plc_utils_success;
+}
+
+PLC_UTILS_STATUS
+plc_get_tx_link_phyrate(unsigned char *local_itf, unsigned char *neighbor_itf,
+ unsigned short *phyrate)
+{
+ PLC_MME_PARAM param;
+ unsigned char *temp_buf;
+ unsigned char num_station = 0;
+ int i = 0;
+
+ param.mmtype = CM_NW_STATS_REQ;
+ param.sndtype = MME_SEND_REQ_CNF;
+ memcpy(param.dst, local_itf, 6);
+ param.sndlen = 0;
+
+ if(plc_utils_success != plc_send_recv_mme(&param))
+ return plc_utils_fail;
+
+ temp_buf = param.rcvbuf;
+ /*parse payload of CM_NW_STATS_CNF, refer to HPAV 1.1 11.5.33*/
+ num_station = *temp_buf;
+ temp_buf += 1;
+
+ for(i=0;i<num_station;i++)
+ {
+ if(!memcmp(neighbor_itf, temp_buf, 6))
+ {
+ /*shift to TX phy rate field*/
+ temp_buf += 6;
+ *phyrate = *(temp_buf);
+ break;
+ }
+ temp_buf += 8;
+ }
+
+ debug_syslog("plc_get_tx_link_phyrate num_station = %d, neighbor itf %.2x %.2x %.2x %.2x %.2x %.2x ,phyrate %d\n",
+ num_station, neighbor_itf[0], neighbor_itf[1], neighbor_itf[2], neighbor_itf[3],
+ neighbor_itf[4], neighbor_itf[5], *phyrate);
+ return plc_utils_success;
+}
+
+PLC_UTILS_STATUS
+plc_get_mac_throughput_capacity(unsigned short phyrate, unsigned short *capacity)
+{
+ *capacity = (MAX_MSDU_SIZE * phyrate)/
+ (MAX_MSDU_SIZE + (MAC_HEADER_SIZE + (4 * PHY_BLOCK_HEADER_SIZE)));
+
+ return plc_utils_success;
+}
+
+PLC_UTILS_STATUS
+plc_get_station_info(unsigned char *local_itf, hpav_info *info,
+ unsigned char step)
+{
+ PLC_MME_PARAM param;
+ unsigned char *temp_buf;
+ unsigned char station_num = 0;
+ int i = 0;
+
+ param.mmtype = CM_NW_STATS_REQ;
+ param.sndtype = MME_SEND_REQ_CNF;
+ memcpy(param.dst, local_itf, 6);
+ param.sndlen = 0;
+
+ if(plc_utils_success != plc_send_recv_mme(&param))
+ return plc_utils_fail;
+
+ temp_buf = param.rcvbuf;
+ /*parse payload of CM_NW_STATS_CNF, refer to HPAV 1.1 11.5.33*/
+ station_num = *temp_buf;
+ temp_buf += 1;
+
+ if(station_num > MAX_STATIONS)
+ {
+ debug_syslog("station amount exceeds MAX station number\n");
+ station_num = MAX_STATIONS;
+ }
+
+ if(step == 1)
+ {
+ /*in step 1, we just need to record the stations mac/amount into database*/
+
+ info->station_num = station_num;
+
+ for(i=0;i<info->station_num;i++)
+ {
+ /*store station MAC address*/
+ memcpy(&(info->station_mac_list[i][0]), temp_buf, 6);
+ temp_buf += 8;
+ }
+ }
+ else if(step == 2)
+ {
+ int j = 0, z = 0;
+ /* in step 2, get new stations and compare with old stations
+ * if any new device join, there will be a different station
+ * between old station and new station
+ */
+ if(station_num > info->station_num)
+ {
+ debug_syslog("step 2 station number = %d\n",station_num);
+ if(info->station_num == 0)
+ {
+ memcpy(info->new_station, temp_buf, 6);
+ debug_syslog("1. new add mac %.2x %.2x %.2x %.2x %.2x %.2x\n",
+ info->new_station[0], info->new_station[1],
+ info->new_station[2], info->new_station[3],
+ info->new_station[4], info->new_station[5]);
+
+ return plc_utils_success;
+ }
+ else
+ {
+ for(j=0;j<station_num;j++)
+ {
+ for(z=0;z<info->station_num;z++)
+ {
+ if(memcmp(&(info->station_mac_list[z][0]), temp_buf, 6))
+ {
+ memcpy(info->new_station, temp_buf, 6);
+ debug_syslog("2. new add mac %.2x %.2x %.2x %.2x %.2x %.2x\n",
+ info->new_station[0], info->new_station[1],
+ info->new_station[2], info->new_station[3],
+ info->new_station[4], info->new_station[5]);
+ return plc_utils_success;
+ }
+ }
+
+ temp_buf += 8;
+ }
+
+ /*if no new joined device , set new station mac all 0*/
+ memset(info->new_station, 0, 6);
+ return plc_utils_no_new_station;
+ }
+ }
+ else
+ return plc_utils_no_new_station;
+ }
+
+ return plc_utils_success;
+}
diff --git a/cleopatre/devkit/p1905_managerd/src/rt5572_io.c b/cleopatre/devkit/p1905_managerd/src/rt5572_io.c
new file mode 100644
index 0000000000..04d3cd8abf
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/rt5572_io.c
@@ -0,0 +1,769 @@
+/*
+ * cleopatre/application/p1905_managerd/src/rt5572_io.c
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ *
+ */
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <net/if.h>
+#include <syslog.h>
+#include <asm/byteorder.h>
+#include <sys/socket.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+#include <linux/wireless.h>
+#include "wifi_utils.h"
+#include <time.h>
+#include "rt5572_io.h"
+
+//#define RT5572_IO_DEBUG
+#ifdef RT5572_IO_DEBUG
+#define debug(...) printf(__VA_ARGS__)
+#else
+#define debug(...)
+#endif
+
+/*define wifi interface name*/
+#define RA0 "ra0"
+
+static WIFI_UTILS_STATUS rt5572_ioctl(int cmd, rt5572_io_table *tab)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+
+ wrq.u.data.length = tab->length;
+ wrq.u.data.pointer =(unsigned char *) tab->data;
+ wrq.u.data.flags = tab->flag;
+
+ if(ioctl(socket_id, cmd, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ tab->length = wrq.u.data.length;
+
+ close(socket_id);
+ return status;
+}
+
+void rt5572_init(void)
+{
+ /*use srand to set seed of rand*/
+ srand(time(NULL));
+}
+
+WIFI_UTILS_STATUS rt5572_generate_DH_pub_priv_key(DH_TABLE *pdh)
+{
+ struct iwreq wrq;
+ int socket_id, i = 0, len = 0;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+ len = sizeof(pdh->priv_key);
+
+ /*ramdon generate the Diffie-Hellman private key*/
+ for(i=0;i<len;i++)
+ pdh->priv_key[i] = ((rand() % 255) + 1);
+
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer =(unsigned char *) pdh;
+ wrq.u.data.flags = 0;
+
+ /* use this ioctl to generate Diffie-Helman public key*/
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_DH_PUB_KEY, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+
+WIFI_UTILS_STATUS rt5572_generate_DH_secu_key(DH_TABLE *pdh)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer =(unsigned char *) pdh;
+ wrq.u.data.flags = 0;
+
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_DH_SECU_KEY, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+
+WIFI_UTILS_STATUS rt5572_generate_auth_keywrap_key(KDK_KDF_TABLE *pkdk_kdf)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(KDK_KDF_TABLE);
+ wrq.u.data.pointer =(unsigned char *) pkdk_kdf;
+ wrq.u.data.flags = 0;
+
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_AUTH_KEYWRAP_KEY, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+
+WIFI_UTILS_STATUS rt5572_generate_kwa(KWA_TABLE *pkwa)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(KWA_TABLE);
+ wrq.u.data.pointer =(unsigned char *) pkwa;
+ wrq.u.data.flags = 0;
+
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_KWA_VALUE, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+
+WIFI_UTILS_STATUS rt5572_generate_AES_encrypt_value(AES_TABLE *paes)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(AES_TABLE);
+ wrq.u.data.pointer =(unsigned char *) paes;
+ wrq.u.data.flags = 0;
+
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_AES_ENCRYPT_VALUE, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+
+WIFI_UTILS_STATUS rt5572_generate_AES_decrypt_value(AES_TABLE *paes)
+{
+ struct iwreq wrq;
+ int socket_id;
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+ unsigned char name[6] = {0};
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(AES_TABLE);
+ wrq.u.data.pointer =(unsigned char *) paes;
+ wrq.u.data.flags = 0;
+
+ if(ioctl(socket_id, RTPRIV_IOCTL_GET_AES_DECRYPT_VALUE, &wrq))
+ {
+ status = wifi_utils_error;
+ }
+
+ close(socket_id);
+ return status;
+}
+
+WIFI_UTILS_STATUS rt5572_get_ssid(unsigned char *ssid, unsigned short *len)
+{
+ rt5572_io_table tab;
+
+ tab.data = ssid;
+ tab.length = (*len);
+ tab.flag = 0;
+ if(wifi_utils_error == rt5572_ioctl(SIOCGIWESSID, &tab))
+ {
+ return wifi_utils_error;
+ }
+ *len = tab.length;
+
+ return wifi_utils_success;
+}
+
+/*the parameter len = strlen(ssid)*/
+WIFI_UTILS_STATUS rt5572_set_ssid(unsigned char *ssid, unsigned short len)
+{
+ rt5572_io_table tab;
+ /*5 otects =>SSID= ; 32 otects ==> max SSID length ; 1 otects==> '\0'*/
+ unsigned char data[37 + 1] = {0};
+
+ data[0] = 'S';
+ data[1] = 'S';
+ data[2] = 'I';
+ data[3] = 'D';
+ data[4] = '=';
+
+ memcpy(&data[5], ssid, (len+1));
+ tab.data = data;
+ tab.length = 5 + len + 1;
+ tab.flag = 0;
+
+ if(wifi_utils_error == rt5572_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS rt5572_set_authmode(unsigned short authmode)
+{
+ rt5572_io_table tab;
+ unsigned char data[25] = {0};
+
+ strcpy(data, "AuthMode=");
+
+ switch(authmode)
+ {
+ case AUTH_OPEN:
+ strcat(data, "OPEN");
+ break;
+ case AUTH_WPA_PERSONAL:
+ strcat(data, "WPAPSK");
+ break;
+ case AUTH_SHARED:
+ strcat(data, "SHARED");
+ break;
+ case AUTH_WPA_ENTERPRISE:
+ strcat(data, "WPA");
+ break;
+ case AUTH_WPA2_ENTERPRISE:
+ strcat(data, "WPA2");
+ break;
+ case (AUTH_WPA2_ENTERPRISE | AUTH_WPA_ENTERPRISE):
+ strcat(data, "WPA1WPA2");
+ break;
+ case AUTH_WPA2_PERSONAL:
+ strcat(data, "WPA2PSK");
+ break;
+ case (AUTH_WPA2_PERSONAL | AUTH_WPA_PERSONAL):
+ strcat(data, "WPAPSKWPA2PSK");
+ break;
+ default:
+ strcat(data, "OPEN");
+ break;
+ }
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == rt5572_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS rt5572_set_encryptmode(unsigned short encryptmode)
+{
+ rt5572_io_table tab;
+ unsigned char data[20] = {0};
+
+ strcpy(data, "EncrypType=");
+
+ switch(encryptmode)
+ {
+ case ENCRYP_NONE:
+ strcat(data, "NONE");
+ break;
+ case ENCRYP_WEP:
+ strcat(data, "WEP");
+ break;
+ case ENCRYP_TKIP:
+ strcat(data, "TKIP");
+ break;
+ case ENCRYP_AES:
+ strcat(data, "AES");
+ break;
+ case (ENCRYP_TKIP|ENCRYP_AES):
+ strcat(data, "TKIPAES");
+ break;
+ default:
+ strcat(data, "NONE");
+ break;
+ }
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == rt5572_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS rt5572_set_ieee802_1x(unsigned char ieee8021x)
+{
+ rt5572_io_table tab;
+ unsigned char data[20] = {0};
+
+ strcpy(data, "IEEE8021X=");
+
+ if(ieee8021x)
+ strcat(data, "1");
+ else
+ strcat(data, "0");
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == rt5572_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+/*the parameter len = strlen(wpapsk)*/
+WIFI_UTILS_STATUS rt5572_set_wpapsk(unsigned char *wpapsk, unsigned short len)
+{
+ rt5572_io_table tab;
+ unsigned char data[7+64+1] = {0};
+
+ strcpy(data, "WPAPSK=");
+ memcpy(&data[7], wpapsk, len+1);
+ tab.data = data;
+ tab.length = 7 + len + 1;
+ tab.flag = 0;
+
+ if(wifi_utils_error == rt5572_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS rt5572_set_default_keyid(unsigned char keyid)
+{
+ rt5572_io_table tab;
+ unsigned char data[20] = {0};
+
+ strcpy(data, "DefaultKeyID=");
+
+ if(keyid >= 1 && keyid <= 4)
+ {
+ keyid += 0x30;//changne Hex to ASCII
+ }
+ else
+ return wifi_utils_error;
+
+ data[13] = keyid;
+ data[14] = '\0';
+
+ tab.data = data;
+ tab.length = strlen(data);
+ tab.flag = 0;
+
+ if(wifi_utils_error == rt5572_ioctl(RTPRIV_IOCTL_SET, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ return wifi_utils_success;
+}
+
+WIFI_UTILS_STATUS rt5572_get_wsc_config(WSC_CONFIG *wsc_config)
+{
+ WSC_CONFIGURED_VALUE *wsc;
+ rt5572_io_table tab;
+
+ wsc = (WSC_CONFIGURED_VALUE *)malloc(sizeof(WSC_CONFIGURED_VALUE));
+
+ tab.data = (unsigned char *)wsc;
+ tab.length = sizeof(WSC_CONFIGURED_VALUE);
+ tab.flag = 0;
+
+ if(wifi_utils_error == rt5572_ioctl(RTPRIV_IOCTL_WSC_PROFILE, &tab))
+ {
+ return wifi_utils_error;
+ }
+
+ memcpy(wsc_config->Ssid, wsc->WscSsid, sizeof(wsc_config->Ssid));
+ wsc_config->AuthMode = wsc->WscAuthMode;
+ wsc_config->EncrypType = wsc->WscEncrypType;
+ memcpy(wsc_config->WPAKey, wsc->WscWPAKey, sizeof(wsc_config->WPAKey));
+
+ free(wsc);
+}
+
+WIFI_UTILS_STATUS rt5572_set_wsc_config(WSC_CONFIG *wsc_config)
+{
+
+}
+
+void rt5572_set_wsc_config_test(void)
+{
+ /*only test WPA2PSK & AES*/
+ unsigned char ssid[32+1] = "wscconfigtestssid00001111111122";
+ unsigned char wpapsk[64+1] = "testwsc12345678";
+ WSC_CONFIG wsc;
+
+ rt5572_set_authmode(AUTH_WPA2_PERSONAL);
+ rt5572_set_encryptmode(ENCRYP_AES);
+ rt5572_set_ieee802_1x(0);
+ rt5572_set_ssid(ssid, strlen(ssid));
+ rt5572_set_wpapsk(wpapsk, strlen(wpapsk));
+ rt5572_set_default_keyid(2);
+ rt5572_set_ssid(ssid, strlen(ssid));
+
+ rt5572_get_wsc_config(&wsc);
+ printf("ssid %s\n",wsc.Ssid);
+ printf("authmode 0x%.4x\n",wsc.AuthMode);
+ printf("encrypt mode 0x%.4x\n",wsc.EncrypType);
+ printf("wpapsk %s\n",wsc.WPAKey);
+}
+
+void rt5572_AES_test(void)
+{
+#ifdef RT5572_IO_DEBUG
+ int i = 0;
+ AES_TABLE Enc;
+ AES_TABLE Dec;
+
+ srand(time(NULL));
+
+ for(i=0;i<16;i++)
+ Enc.KeyWrapKey[i] = ((rand() % 255) + 1);
+ memcpy(Dec.KeyWrapKey, Enc.KeyWrapKey, 16);
+
+ for(i=0;i<16;i++)
+ Enc.IV[i] = ((rand() % 255) + 1);
+ memcpy(Dec.IV, Enc.IV, 16);
+
+ Enc.PlainTextLen = ((rand() % (512-16)) + 1);
+ printf("plain text len = %d\n",Enc.PlainTextLen);
+
+ Enc.PlainText = malloc(Enc.PlainTextLen);
+ for(i=0;i<Enc.PlainTextLen;i++)
+ *(Enc.PlainText + i) = ((rand() % 255) + 1);
+
+ Enc.CipherText = malloc(512-16);
+ Enc.CipherTextLen = 512 -16;
+
+ rt5572_generate_AES_encrypt_value(&Enc);
+
+ printf("encrypt data len = %d\n",Enc.CipherTextLen);
+
+ Dec.CipherText = malloc(Enc.CipherTextLen);
+ Dec.CipherTextLen = Enc.CipherTextLen;
+ memcpy(Dec.CipherText, Enc.CipherText, Enc.CipherTextLen);
+
+ Dec.PlainText = malloc(1024);
+ Dec.PlainTextLen = 1024;
+
+ rt5572_generate_AES_decrypt_value(&Dec);
+
+ if(Dec.PlainTextLen != Enc.PlainTextLen)
+ {
+ printf("Dec.PlainTextLen = %d\n",Dec.PlainTextLen);
+ printf("Enc.PlainTextLen = %d\n",Enc.PlainTextLen);
+ }
+ else if(memcmp(Enc.PlainText, Dec.PlainText, Dec.PlainTextLen))
+ {
+ printf("Dec/Encrypt error\n");
+ }
+ else
+ printf("Dec/Encrypt success\n");
+
+
+ free(Enc.PlainText);
+ free(Enc.CipherText);
+ free(Dec.CipherText);
+ free(Dec.PlainText);
+#endif
+}
+
+void rt5572_keywrap_test(void)
+{
+#ifdef RT5572_IO_DEBUG
+ DH_TABLE dh1;
+ DH_TABLE dh2;
+ KDK_KDF_TABLE k1;
+ KDK_KDF_TABLE k2;
+ unsigned char temp_public[192] = {0};
+ int i = 0;
+
+ srand(time(NULL));
+
+ rt5572_generate_DH_pub_priv_key(&dh2);
+ rt5572_generate_DH_pub_priv_key(&dh1);
+
+ printf("DH public 1:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh1.pub_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+ printf("DH public 2:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh2.pub_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+ memcpy(temp_public, dh2.pub_key, 192);
+ memcpy(dh2.pub_key, dh1.pub_key, 192);
+ memcpy(dh1.pub_key, temp_public, 192);
+
+ printf("after DH public 1:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh1.pub_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+ printf("after DH public 2:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh2.pub_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+ rt5572_generate_DH_secu_key(&dh2);
+ rt5572_generate_DH_secu_key(&dh1);
+
+ printf("DH security 1:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh1.secu_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+ printf("DH security 2:\n");
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",dh2.secu_key[i]);
+ if((i % 10) == 0)
+ printf("\n");
+ }
+ printf("\n");
+
+
+ if(!memcmp(dh1.secu_key, dh2.secu_key, 192))
+ printf("DH key ok\n");
+ else
+ printf("DH key fail\n");
+
+ memcpy(k1.DH_Secu_Key, dh1.secu_key, 192);
+ memcpy(k2.DH_Secu_Key, dh2.secu_key, 192);
+
+ for(i=0;i<16;i++)
+ k1.E_Nonce[i] = ((rand() % 255) + 1);
+ memcpy(k2.E_Nonce, k1.E_Nonce, 16);
+
+ for(i=0;i<16;i++)
+ k1.R_Nonce[i] = ((rand() % 255) + 1);
+ memcpy(k2.R_Nonce, k1.R_Nonce, 16);
+
+ for(i=0;i<6;i++)
+ k1.E_Mac_Addr[i] = ((rand() % 255) + 1);
+ memcpy(k2.E_Mac_Addr, k1.E_Mac_Addr, 6);
+
+ rt5572_generate_auth_keywrap_key(&k1);
+ rt5572_generate_auth_keywrap_key(&k2);
+
+ printf("auth key1 =\n");
+ for(i=0;i<sizeof(k1.AuthKey);i++)
+ {
+ printf("%.2x ",k1.AuthKey[i]);
+ }
+ printf("\n");
+
+ printf("auth key2 =\n");
+ for(i=0;i<sizeof(k2.AuthKey);i++)
+ {
+ printf("%.2x ",k2.AuthKey[i]);
+ }
+ printf("\n");
+ if(!memcmp(k1.AuthKey, k2.AuthKey, 32))
+ printf("auth key OK\n");
+
+ printf("keywrap key1 =\n");
+ for(i=0;i<sizeof(k1.KeyWrapKey);i++)
+ {
+ printf("%.2x ",k1.KeyWrapKey[i]);
+ }
+ printf("\n");
+
+ printf("keywrap key2 =\n");
+ for(i=0;i<sizeof(k2.KeyWrapKey);i++)
+ {
+ printf("%.2x ",k2.KeyWrapKey[i]);
+ }
+ printf("\n");
+ if(!memcmp(k1.KeyWrapKey, k2.KeyWrapKey, 32))
+ printf("auth key OK\n");
+#endif
+}
+
+void rt5572_DH_test(void)
+{
+#ifdef RT5572_IO_DEBUG
+ DH_TABLE dh1;
+ DH_TABLE dh2;
+
+ struct iwreq wrq;
+ int socket_id;
+ unsigned char name[6] = {0};
+ unsigned char public[192] = {0};
+
+ int i = 0;
+ int len = 0;
+
+ socket_id = socket(AF_INET, SOCK_STREAM, 0);
+
+ srand(time(NULL));
+
+ for(i=0;i<192;i++)
+ {
+ dh1.priv_key[i] = ((rand() % 255) + 1);
+ }
+
+ sprintf(name, RA0);
+ strcpy(wrq.ifr_name, name);
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer =(unsigned char *) &dh1;
+ wrq.u.data.flags = 0;
+
+ if(0 == ioctl(socket_id, RTPRIV_IOCTL_GET_DH_PUB_KEY, &wrq))
+ {
+ printf("1 ==>wsc_dh_generate_key OK, len = %d\n",wrq.u.data.length);
+
+ len = sizeof(dh1.pub_key);
+ for(i=0;i<len;i++)
+ {
+ printf("%.2x ",dh1.pub_key[i]);
+ if(i%10 == 0)
+ printf("\n");
+ }
+ }
+ /*=================================================================*/
+ for(i=0;i<192;i++)
+ {
+ dh2.priv_key[i] = ((rand() % 255) + 1);
+ }
+
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer = (unsigned char *) &dh2;
+ wrq.u.data.flags = 0;
+
+ if(0 == ioctl(socket_id, RTPRIV_IOCTL_GET_DH_PUB_KEY, &wrq))
+ {
+ printf("2 ==>wsc_dh_generate_key OK, len = %d\n",wrq.u.data.length);
+
+ len = sizeof(dh2.pub_key);
+ for(i=0;i<len;i++)
+ {
+ printf("%.2x ",dh2.pub_key[i]);
+ if(i%10 == 0)
+ printf("\n");
+ }
+ }
+ /*=================================================================*/
+ memcpy(public, dh1.pub_key, 192);
+ memcpy(dh1.pub_key, dh2.pub_key, 192);
+ memset(dh1.secu_key,0,192);
+ memcpy(dh2.pub_key, public, 192);
+ memset(dh2.secu_key,0,192);
+
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer = (unsigned char *)(&dh1);
+ wrq.u.data.flags = 0;
+
+ if(0 == ioctl(socket_id, RTPRIV_IOCTL_GET_DH_SECU_KEY, &wrq))
+ {
+ printf("1 ==>compute key OK, len = %d\n",wrq.u.data.length);
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",(dh1.secu_key[i]));
+ if(i%10 == 0)
+ printf("\n");
+ }
+ }
+
+ wrq.u.data.length = sizeof(DH_TABLE);
+ wrq.u.data.pointer = (unsigned char *)(&dh2);
+ wrq.u.data.flags = 0;
+
+ if(0 == ioctl(socket_id, RTPRIV_IOCTL_GET_DH_SECU_KEY, &wrq))
+ {
+ printf("2 ==>compute key OK, len = %d\n",wrq.u.data.length);
+
+ for(i=0;i<192;i++)
+ {
+ printf("%.2x ",(dh2.secu_key[i]));
+ if(i%10 == 0)
+ printf("\n");
+ }
+ }
+
+ if(!memcmp(dh1.secu_key, dh2.secu_key, sizeof(dh1.secu_key)))
+ printf("\nkey is same\n");
+
+ close(socket_id);
+#endif
+}
+
diff --git a/cleopatre/devkit/p1905_managerd/src/wifi_utils.c b/cleopatre/devkit/p1905_managerd/src/wifi_utils.c
new file mode 100644
index 0000000000..5bc8d45c45
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/wifi_utils.c
@@ -0,0 +1,540 @@
+/*
+ * cleopatre/application/p1905_managerd/src/wifi_utils.c
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ *
+ */
+#include <stdio.h>
+#include <syslog.h>
+#include <errno.h>
+#include "wifi_utils.h"
+
+//#define WIFI_UTILS_DEBUG
+#ifdef WIFI_UTILS_DEBUG
+#define debug(...) printf(__VA_ARGS__)
+#else
+#define debug(...)
+#endif
+
+WIFI_UTILS_STATUS get_mac_addr(unsigned char *mac_addr);
+
+
+WIFI_UTILS_STATUS wifi_utils_init(unsigned char *mac_addr)
+{
+ WIFI_UTILS_STATUS status = wifi_utils_success;
+
+ status = get_mac_addr(mac_addr);
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+#if defined(MT7601_ENABLE)
+ mt7601_init();
+#elif defined(RT5572_ENABLE)
+ rt5572_init();
+#endif
+#endif
+
+ return status;
+}
+
+/**
+ * get rf band(2.4G or 5G)
+ *
+ * \param band pointer output band
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS get_rf_band(RF_BAND *band)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_rf_band(band);
+#elif defined(RT5572_ENABLE)
+ return rt5572_get_rf_band(band);
+#endif
+}
+
+/**
+ * get uuid for ap-auto configure use.
+ *
+ * \param uuid output uuid
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS get_uuid(unsigned char uuid[])
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_uuid(uuid);
+#elif defined(RT5572_ENABLE)
+ return rt5572_get_uuid(uuid);
+#endif
+}
+
+/**
+ * get rx link statistic for link metrics use.
+ *
+ * \param peer_mac peer mac address
+ * \param rx_total rx total packets
+ * \param rx_error rx error packets
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS
+get_rx_statistics(unsigned char peer_mac[], unsigned int *rx_total,
+ unsigned int *rx_error)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_statistics(peer_mac, rx_total, rx_error, 0);
+#else
+ #error "not define WiFi chip in get_rx_statistics"
+#endif
+}
+
+/**
+ * get tx link statistic for link metrics use.
+ *
+ * \param peer_mac peer mac address
+ * \param tx_total tx total packets
+ * \param tx_error tx error packets
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS
+get_tx_statistics(unsigned char peer_mac[], unsigned int *tx_total,
+ unsigned int *tx_error)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_statistics(peer_mac, tx_total, tx_error, 1);
+#else
+ #error "not define WiFi chip in get_tx_statistics"
+#endif
+}
+
+/**
+ * get phyrate for link metrics use.
+ *
+ * \param peer_mac peer mac address
+ * \param phyrate phyrate value
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS
+get_phyrate(unsigned char peer_mac[], unsigned short *phyrate)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_phyrate(peer_mac, phyrate);
+#else
+ #error "not define WiFi chip in get_phyrate"
+#endif
+}
+
+/**
+ * get rssi for link metrics use.
+ *
+ * \param rssi output rssi
+ * \param peer_mac mac of neighbor device interface
+ * \return error code.
+ */
+WIFI_UTILS_STATUS get_rssi(char *rssi, unsigned char peer_mac[])
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_rssi(rssi, peer_mac);
+#else
+ #error "not define WiFi chip in get_rssi"
+#endif
+}
+
+/**
+ * get channel width for media specific information use.
+ *
+ * \param width output channel width
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS get_channel_width(unsigned char *width)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_current_channelwidth(width);
+#else
+ #error "not define WiFi chip in get_channel_width"
+#endif
+}
+
+/**
+ * get center frequnecy index for media specific information use.
+ *
+ * \param index output center frequnecy index
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS get_center_freq_index(unsigned char *index)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_center_freq_index(index);
+#else
+ #error "not define WiFi chip in get_center_freq_index"
+#endif
+}
+
+/**
+ * get all stations mac address.
+ *
+ * \param station_list storage of station mac address
+ * \param sta_num output amount of station
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS
+get_station_mac(unsigned char station_list[WIFI_MAX_STATION_NUM][6],
+ unsigned char *sta_num)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_station_mac(station_list, sta_num);
+#else
+ #error "not define WiFi chip in get_station_mac"
+#endif
+}
+
+/**
+ * trigger wifi PBC configuration.
+ *
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS trigger_wifi_PBC_config(void)
+{
+ /*need to implement*/
+ return wifi_utils_success;
+}
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+/**
+ * get WSC configuration statis for ap-auto configure use.
+ *
+ * \param status output configuration status
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS get_wsc_conf_status(unsigned char *status)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_wsc_conf_status(status);
+#elif defined(RT5572_ENABLE)
+ return rt5572_get_wsc_conf_status(status);
+#endif
+}
+
+/**
+ * set WSC configuration statis for ap-auto configure use.
+ *
+ * \param status omput configuration status
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS set_wsc_conf_status(unsigned char status)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_set_wsc_conf_status(status);
+#elif defined(RT5572_ENABLE)
+ return rt5572_set_wsc_conf_status(status);
+#endif
+}
+
+/**
+ * create Diffie-Hellman public key & private key for ap-auto configure use.
+ *
+ * \param pdh pointer of DH_TABLE.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS generate_DH_pub_priv_key(DH_TABLE *pdh)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_generate_DH_pub_priv_key(pdh);
+#elif defined(RT5572_ENABLE)
+ return rt5572_generate_DH_pub_priv_key(pdh);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+/**
+ * create Diffie-Hellman security key for ap-auto configure use.
+ *
+ * \param pdh pointer of DH_TABLE.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS generate_DH_secu_key(DH_TABLE *pdh)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_generate_DH_secu_key(pdh);
+#elif defined(RT5572_ENABLE)
+ return rt5572_generate_DH_secu_key(pdh);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+/**
+ * create auth & keywrap keys for ap-auto configure use.
+ *
+ * \param pkdk_kdf pointer of KDK_KDF_TABLE.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS generate_auth_keywrap_key(KDK_KDF_TABLE *pkdk_kdf)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_generate_auth_keywrap_key(pkdk_kdf);
+#elif defined(RT5572_ENABLE)
+ return rt5572_generate_auth_keywrap_key(pkdk_kdf);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+/**
+ * create KWA for ap-auto configure use.
+ *
+ * \param pkwa pointer of KDK_KDF_TABLE.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS generate_kwa(KWA_TABLE *pkwa)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_generate_kwa(pkwa);
+#elif defined(RT5572_ENABLE)
+ return rt5572_generate_kwa(pkwa);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+/**
+ * AES encrypt for ap-auto configure use.
+ *
+ * \param enc pointer of AES_TABLE.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS generate_AES_CBC_encrypt_value(AES_TABLE *enc)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_generate_AES_encrypt_value(enc);
+#elif defined(RT5572_ENABLE)
+ return rt5572_generate_AES_encrypt_value(enc);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+/**
+ * AES decrypt for ap-auto configure use.
+ *
+ * \param dec pointer of AES_TABLE.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS generate_AES_CBC_decrypt_value(AES_TABLE *dec)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_generate_AES_decrypt_value(dec);
+#elif defined(RT5572_ENABLE)
+ return rt5572_generate_AES_decrypt_value(dec);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+#endif
+
+/**
+ * get mac address for ap-auto configure use.
+ *
+ * \param mac_addr pointer of mac address.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS get_mac_addr(unsigned char *mac_addr)
+{
+ #if defined(MT7601_ENABLE)
+ return mt7601_get_mac_addr(mac_addr);
+ #elif defined(RT5572_ENABLE)
+
+ #endif
+}
+
+/**
+ * get SSID for ap-auto configure use.
+ *
+ * \param ssid pointer of ssid.
+ * \param length input/output ssid length.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS get_ssid(unsigned char *ssid, unsigned short *length)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_ssid(ssid, length);
+#elif defined(RT5572_ENABLE)
+ return rt5572_get_ssid(ssid, length);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+/**
+ * set SSID for ap-auto configure use.
+ *
+ * \param ssid pointer of setting ssid.
+ * \param length input ssid length.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS set_ssid(unsigned char *ssid, unsigned short length)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_set_ssid(ssid, length);
+#elif defined(RT5572_ENABLE)
+ return rt5572_set_ssid(ssid, length);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+#ifdef SUPPORT_AP_AUTO_CONFIG
+/**
+ * set authetication mode for ap-auto configure use.
+ *
+ * \param authmode authentication mode value.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS set_auth_mode(unsigned short authmode)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_set_authmode(authmode);
+#elif defined(RT5572_ENABLE)
+ return rt5572_set_authmode(authmode);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+/**
+ * set encrytion mode for ap-auto configure use.
+ *
+ * \param encmode encryption mode value.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS set_encrypt_mode(unsigned short encryptmode)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_set_encryptmode(encryptmode);
+#elif defined(RT5572_ENABLE)
+ return rt5572_set_encryptmode(encryptmode);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+/**
+ * get WSC config for ap-auto configure use.
+ *
+ * \param wsc pointer of wsc_config.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS get_wsc_config(WSC_CONFIG *wsc)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_get_wsc_config(wsc);
+#elif defined(RT5572_ENABLE)
+ return rt5572_get_wsc_config(wsc);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+/**
+ * set WSC config for ap-auto configure use.
+ *
+ * \param wsc pointer of wsc_config.
+ * \return error code.
+ */
+WIFI_UTILS_STATUS set_wsc_config(WSC_CONFIG *wsc)
+{
+#if defined(MT7601_ENABLE)
+ return mt7601_set_wsc_config(wsc);
+#elif defined(RT5572_ENABLE)
+ return rt5572_set_wsc_config(wsc);
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+/**
+ * update wifi config file.
+ *
+ * \param wsc pointer of wsc_config.
+ * \return error code.
+ */
+
+WIFI_UTILS_STATUS update_wifi_config_file(WSC_CONFIG *wsc)
+{
+ #if defined(MT7601_ENABLE)
+ return mt7601_update_config_file(wsc);
+ #elif defined(RT5572_ENABLE)
+ return rt5572_update_config_file(wsc);
+ #else
+ #error "not define WiFi chip"
+ #endif
+}
+
+/* =========================================================================*/
+/* below is test code, and can be disable if you want to reduce code size */
+/* =========================================================================*/
+void set_wsc_config_test(void)
+{
+#if defined(MT7601_ENABLE)
+ mt7601_set_wsc_config_test();
+#elif defined(RT5572_ENABLE)
+ rt5572_set_wsc_config_test();
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+void AES_test(void)
+{
+#if defined(MT7601_ENABLE)
+ mt7601_AES_test();
+#elif defined(RT5572_ENABLE)
+ rt5572_AES_test();
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+void keywrap_test(void)
+{
+#if defined(MT7601_ENABLE)
+ mt7601_keywrap_test();
+#elif defined(RT5572_ENABLE)
+ rt5572_keywrap_test();
+#else
+ #error "not define WiFi chip"
+#endif
+}
+
+void DH_test(void)
+{
+#if defined(MT7601_ENABLE)
+ mt7601_DH_test();
+#elif defined(RT5572_ENABLE)
+ rt5572_DH_test();
+#else
+ #error "not define WiFi chip"
+#endif
+}
+#endif
diff --git a/cleopatre/devkit/p1905_managerd/src/wsc_attr_tlv.c b/cleopatre/devkit/p1905_managerd/src/wsc_attr_tlv.c
new file mode 100644
index 0000000000..015dffe830
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/wsc_attr_tlv.c
@@ -0,0 +1,1353 @@
+/*
+ * cleopatre/application/p1905_managerd/src/wsc_attr_tlv.c
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ *
+ */
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <syslog.h>
+#include <errno.h>
+#include "wifi_utils.h"
+#include "wsc_attr_tlv.h"
+#include <asm/byteorder.h>
+
+//#define WSC_ATTR_TLV_DEBUG
+#ifdef WSC_ATTR_TLV_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
+
+#if defined(SUPPORT_AP_REGISTRAR)
+#define DEVICE_NAME "REGISTRAR"
+#define TOTAL_REQUIRED_TLV 22
+
+#elif defined(SUPPORT_AP_ENROLLE)
+#define DEVICE_NAME "ENROLEE"
+#define TOTAL_REQUIRED_TLV 21
+#endif
+
+enum
+{
+ HAS_SSID_ATTR = 0,
+ HAS_AUTHENTICATION_TYPE_ATTR,
+ HAS_ENCRYPTION_TYPE_ATTR,
+ HAS_NETWORK_KEY_ATTR,
+ HAS_MAC_ADDR_ATTR,
+//max is 7, because I use a char to record
+};
+
+static WSC_ATTR_STATUS create_encrypt_settings_filed(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_enrolle_nonce_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_mac_address_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_message_type_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_public_key_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_registrar_nonce_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_uuid_e_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_uuid_r_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_version_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_ssid_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_kwa_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_authentication_type_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_encryption_type_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_network_key_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_auth_type_flag_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_encrypt_type_flag_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_conn_type_flag_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_config_method_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_wsc_state_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_manufacturer_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_model_name_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_model_number_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_serial_number_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_prim_dev_type_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_device_name_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_rf_band_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_association_state_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_device_pwd_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_config_error_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_os_version_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_authenticator_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+static WSC_ATTR_STATUS create_vendor_extension_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length);
+
+
+static WSC_ATTR_STATUS parse_version_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_message_type_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_encrypt_settings_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_enrolle_nonce_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_mac_address_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_public_key_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_registrar_nonce_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_uuid_e_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_uuid_r_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_kwa_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_ssid_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_authentication_type_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_encryption_type_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_network_key_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_auth_type_flag_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_encrypt_type_flag_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_conn_type_flag_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_config_method_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_wsc_state_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_manufacturer_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_model_name_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_model_number_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_serial_number_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_prim_dev_type_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_device_name_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_rf_band_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_association_state_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_device_pwd_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_config_error_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_os_version_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_authenticator_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+static WSC_ATTR_STATUS parse_vendor_extension_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length);
+
+
+/* Notice!! if you want to add new function into wsc_func, please insert new
+ * function in sequence. The id value is from small to big for binary search
+ * use.
+ */
+WSC_ATTR_FUNC wsc_func[] =
+{
+ {ATTR_ASSOCIATION_STATE_ID, 19, 18, create_association_state_field, parse_association_state_attr},
+ {ATTR_AUTHENTICATION_TYPE_ID, 255, 255, create_authentication_type_field, parse_authentication_type_attr},
+ {ATTR_AUTH_TYPE_FLAG_ID, 7, 7, create_auth_type_flag_field, parse_auth_type_flag_attr},
+ {ATTR_AUTHENTICATOR_ID, 255, 255, create_authenticator_field, parse_authenticator_attr},
+ {ATTR_CONFIG_METHOD_ID, 10, 10, create_config_method_field, parse_config_method_attr},
+ {ATTR_CONFIG_ERROR_ID, 21, 19, create_config_error_field, parse_config_error_attr},
+ {ATTR_CONNECTION_TYPE_FLAG_ID, 9, 9, create_conn_type_flag_field, parse_conn_type_flag_attr},
+ {ATTR_ENCRYPTION_TYPE_ID, 255, 255, create_encryption_type_field, parse_encryption_type_attr},
+ {ATTR_ENCRYPTION_TYPE_FLAG_ID, 8, 8, create_encrypt_type_flag_field, parse_encrypt_type_flag_attr},
+ {ATTR_DEVICE_NAME_ID, 17, 16, create_device_name_field, parse_device_name_attr},
+ {ATTR_DEVICE_PWD_ID, 20, 20, create_device_pwd_field, parse_device_pwd_attr},
+ {ATTR_ENCRYPTED_SETTINGS_ID, 255, 255, create_encrypt_settings_filed, parse_encrypt_settings_attr},
+ {ATTR_ENROLLE_NONCE_ID, 5, 3, create_enrolle_nonce_field, parse_enrolle_nonce_attr},
+ {ATTR_KEY_WRAP_AUTHENTICATOR_ID, 255, 255, create_kwa_field, parse_kwa_attr},
+ {ATTR_MAC_ADDRESS_ID, 4, 255, create_mac_address_field, parse_mac_address_attr},
+ {ATTR_MANUFACTURER_ID, 12, 11, create_manufacturer_field, parse_manufacturer_attr},
+ {ATTR_MESSAGE_TYPE_ID, 2, 2, create_message_type_field, parse_message_type_attr},
+ {ATTR_MODEL_NAME_ID, 13, 12, create_model_name_field, parse_model_name_attr},
+ {ATTR_MODEL_NUMBER_ID, 14, 13, create_model_number_field, parse_model_number_attr},
+ {ATTR_NETWORK_KEY_ID, 255, 255, create_network_key_field, parse_network_key_attr},
+ {ATTR_OS_VERSION_ID, 22, 21, create_os_version_field, parse_os_version_attr},
+ {ATTR_PUBLIC_KEY_ID, 6, 6, create_public_key_field, parse_public_key_attr},
+ {ATTR_REGISTRAR_NONCE_ID, 255, 4, create_registrar_nonce_field, parse_registrar_nonce_attr},
+ {ATTR_RF_BAND_ID, 18, 17, create_rf_band_field, parse_rf_band_attr},
+ {ATTR_SERIAL_NUMBER_ID, 15, 14, create_serial_number_field, parse_serial_number_attr},
+ {ATTR_WSC_STATE_ID, 11, 255, create_wsc_state_field, parse_wsc_state_attr},
+ {ATTR_SSID_ID, 255, 255, create_ssid_field, parse_ssid_attr},
+ {ATTR_UUID_E_ID, 3, 255, create_uuid_e_field, parse_uuid_e_attr},
+ {ATTR_UUID_R_ID, 255, 5, create_uuid_r_field, parse_uuid_r_attr},
+ {ATTR_VENDOR_EXTENSION_ID, 255, 255, create_vendor_extension_field, parse_vendor_extension_attr},
+ {ATTR_VERSION_ID, 1, 1, create_version_field, parse_version_attr},
+ {ATTR_PRIM_DEV_TYPE_ID, 16, 15, create_prim_dev_type_field, parse_prim_dev_type_attr},
+};
+
+/* Return index of wsc_func.
+ * If return value is -1 ==> cannot find any matched item
+ */
+static int search_match_function_by_id(unsigned short tlv_type)
+{
+ /*binary search*/
+ unsigned short first = 0;
+ unsigned short last = (unsigned short)(sizeof(wsc_func)/sizeof(WSC_ATTR_FUNC)) - 1;
+ unsigned int index = 0;
+
+ while(first <= last)
+ {
+ index = (first + last)/2;
+ if(wsc_func[index].id < tlv_type)
+ first = index + 1;
+ else if(wsc_func[index].id > tlv_type)
+ last = index -1;
+ else
+ return index;
+ }
+ return -1;
+}
+
+/* =========================================================================*/
+/* WSC attribute TLVs create functions */
+/* =========================================================================*/
+
+static WSC_ATTR_STATUS create_encrypt_settings_filed(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ AES_TABLE *enc;
+ unsigned char *temp;
+
+ enc = (AES_TABLE *)inbuf;
+ temp = outbuf;
+
+ memcpy(temp, enc->IV, 16);
+ temp += 16;
+
+ if(wifi_utils_success != generate_AES_CBC_encrypt_value(enc))
+ return wsc_attr_error;
+
+ memcpy(temp, enc->CipherText, enc->CipherTextLen);
+ (*length) = enc->CipherTextLen + 16;//cipher text length + IV length
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_enrolle_nonce_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ if((*length) == ATTR_ENROLLE_NONCE_DATA_LENGTH)
+ memcpy(outbuf, inbuf, ATTR_ENROLLE_NONCE_DATA_LENGTH);
+ else
+ return wsc_attr_error;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_mac_address_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ if((*length) == ATTR_MAC_ADDRESS_DATA_LENGTH)
+ memcpy(outbuf, inbuf, ATTR_MAC_ADDRESS_DATA_LENGTH);
+ else
+ return wsc_attr_error;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_public_key_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ if((*length) == ATTR_PUBLIC_KEY_DATA_LENGTH)
+ memcpy(outbuf, inbuf, (*length));
+ else
+ return wsc_attr_error;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_registrar_nonce_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ if((*length) == ATTR_REGISTRAR_NONCE_DATA_LENGTH)
+ memcpy(outbuf, inbuf, ATTR_REGISTRAR_NONCE_DATA_LENGTH);
+ else
+ return wsc_attr_error;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_ssid_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ memcpy(outbuf, inbuf, (*length));
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_uuid_e_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ memcpy(outbuf, inbuf, (*length));
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_uuid_r_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ memcpy(outbuf, inbuf, (*length));
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_version_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ *outbuf = *inbuf;
+ *length = ATTR_VERSION_DATA_LENGTH;
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_message_type_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ *outbuf = *inbuf;
+ *length = ATTR_MESSAGE_TYPE_DATA_LENGTH;
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_kwa_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ KWA_TABLE *kwa;
+
+ kwa = (KWA_TABLE *)inbuf;
+ if(wifi_utils_success != generate_kwa(kwa))
+ return wsc_attr_error;
+
+ memcpy(outbuf, kwa->KWA, (*length));
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_authentication_type_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+#ifdef SUPPORT_AP_REGISTRAR
+ unsigned short authmode;
+
+ authmode = (unsigned short)(*inbuf);
+ *outbuf = (unsigned char)((authmode) & 0xFF00) >> 8;
+ *(outbuf + 1) = (unsigned char)((authmode) & 0xFF);
+
+ debug("create authmode %.2x%.2x\n",*outbuf, *(outbuf + 1));
+#endif
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_encryption_type_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+#ifdef SUPPORT_AP_REGISTRAR
+ unsigned short encryptype;
+
+ encryptype = (unsigned short)(*inbuf);
+ *outbuf = (unsigned char)((encryptype) & 0xFF00) >> 8;
+ *(outbuf + 1) = (unsigned char)((encryptype) & 0xFF);
+
+ debug("create encryptype %.2x%.2x\n",*outbuf, *(outbuf + 1));
+#endif
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_network_key_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+#ifdef SUPPORT_AP_REGISTRAR
+ if((*length) == 0)
+ return wsc_attr_success;
+ if((*length) > 64)
+ return wsc_attr_error;
+
+ memcpy(outbuf, inbuf, (*length));
+#endif
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_auth_type_flag_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ unsigned short flag = 0;
+
+// flag = (AUTH_OPEN | AUTH_WPA_PERSONAL | AUTH_SHARED | AUTH_WPA2_PERSONAL);
+ //modify for only support wsc 2.0 above
+ flag = (AUTH_OPEN | AUTH_WPA_PERSONAL | AUTH_WPA2_PERSONAL);
+ *outbuf = (unsigned char)((flag & 0xff00) >> 8);
+ *(outbuf + 1) = (unsigned char)(flag & 0x00ff);
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_encrypt_type_flag_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ unsigned short flag = 0;
+
+ flag = (ENCRYP_NONE | ENCRYP_WEP | ENCRYP_TKIP | ENCRYP_AES);
+ *outbuf = (unsigned char)((flag & 0xff00) >> 8);
+ *(outbuf + 1) = (unsigned char)(flag & 0x00ff);
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_conn_type_flag_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ *outbuf = ESS;
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_config_method_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ //unsigned short method = 0x238C;//temp refer to WSC 2.0.2
+ /*modify for support push button/virtual push button/physical push button*/
+ unsigned short method = 0x0680;//temp refer to WSC 2.0.2
+
+ *outbuf = (unsigned char)((method & 0xff00) >> 8);
+ *(outbuf + 1) = (unsigned char)(method & 0x00ff);
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_wsc_state_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+#if 0
+ /*configured state: 0x2, not configured state: 0x1*/
+#if defined(SUPPORT_AP_REGISTRAR)
+ /*registrar set to configured state*/
+ *outbuf = 0x02;
+#elif defined(SUPPORT_AP_ENROLLE)
+ /*enrollee set to not configured state*/
+ *outbuf = 0x01;
+#endif
+#endif
+ /*set reserve for enrollee*/
+ *outbuf = 0x00;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_manufacturer_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ /* Need to get manufacturer attribute.
+ * I left this item for futher implementation. set all 0
+ */
+ memset(outbuf, 0, (*length));
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_model_name_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ /* Need to get model name attribute.
+ * I left this item for futher implementation. set all 0
+ */
+ memset(outbuf, 0, (*length));
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_model_number_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ /* Need to get model number attribute.
+ * I left this item for futher implementation. set all 0
+ */
+ memset(outbuf, 0, (*length));
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_serial_number_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ /* Need to get serial number attribute.
+ * I left this item for futher implementation. set all 0
+ */
+ memset(outbuf, 0, (*length));
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_prim_dev_type_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ unsigned char *temp;
+
+ temp = outbuf;
+
+ /*category ID, 2 bytes*/
+ /*set category ID to 0x6(network infrastructure), refert to WSC 2.0.2*/
+ *temp = 0x0;
+ *(temp + 1) = 0x6;
+ temp += 2;
+
+ /*OUI, use WIFI Alliance OUI 00 50 F2 04, 4 bytes*/
+ *temp = 0x0;
+ *(temp + 1) = 0x50;
+ *(temp + 2) = 0xF2;
+ *(temp + 3) = 0x04;
+ temp += 4;
+
+ /*set sub category ID to 0x1(AP), 2 bytes*/
+ *temp = 0x0;
+
+ //*(temp + 1) = 0x5;
+ *(temp + 1) = 0x1;
+ temp += 2;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_device_name_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ memcpy(outbuf, DEVICE_NAME, strlen(DEVICE_NAME));
+ *length = strlen(DEVICE_NAME);
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_rf_band_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ RF_BAND band = rf_band_2p4G;
+
+ if(wifi_utils_success != get_rf_band(&band))
+ return wsc_attr_error;
+
+ if(band == rf_band_2p4G)
+ *outbuf = 0x1;
+ else if(band == rf_band_5G)
+ *outbuf = 0x2;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_association_state_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ memset(outbuf, 0, (*length));
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_device_pwd_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+#if 0
+ /*set device password ID to 0x0005(registrar-specified), WSC 2.0.2*/
+ *outbuf = 0x00;
+ *(outbuf + 1) = 0x05;
+#else
+ /*set device password ID to 0x0004(push button), WSC 2.0.2*/
+ *outbuf = 0x00;
+ *(outbuf + 1) = 0x04;
+#endif
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_config_error_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ /*set configure error to 0x0000(no error), WSC 2.0.2*/
+ *outbuf = 0x00;
+ *(outbuf + 1) = 0x00;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_os_version_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ /*the most significant bit must be set to 1, WSC 2.0.2*/
+ *outbuf = 0x80;
+ *(outbuf + 1) = 0x00;
+ *(outbuf + 2) = 0x00;
+ *(outbuf + 3) = 0x00;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_authenticator_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ KWA_TABLE *kwa;
+
+ kwa = (KWA_TABLE *)inbuf;
+ if(wifi_utils_success != generate_kwa(kwa))
+ return wsc_attr_error;
+
+ memcpy(outbuf, kwa->KWA, (*length));
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS create_vendor_extension_field(
+ unsigned char *outbuf, unsigned char *inbuf, unsigned short *length)
+{
+ unsigned char WFA_vendor_ext_attr_id[3] = {0x00, 0x37, 0x2A};
+ unsigned char *tmp = outbuf;
+ unsigned short total_len = 0;
+
+ /*WFA Vendor Extension attribute use vendor id 0x00372A*/
+ memcpy(tmp, WFA_vendor_ext_attr_id, 3);
+ tmp += 3;
+ total_len += 3;
+
+ /*include subelement version2*/
+ *tmp = 0x00;//version2 ID is 0x00
+ tmp += 1;
+ total_len += 1;
+
+ *tmp = 0x01;//version2 length is 1
+ tmp += 1;
+ total_len += 1;
+
+ *tmp = 0x20;//support version 2.0
+ tmp += 1;
+ total_len += 1;
+
+ *length = total_len;
+ return wsc_attr_success;
+}
+
+/* =========================================================================*/
+/* WSC attribute TLVs parse functions */
+/* =========================================================================*/
+
+static WSC_ATTR_STATUS parse_kwa_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ /*need to kwa check*/
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_version_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ unsigned char *temp;
+
+ temp = pkt;
+ if(((*length) != ATTR_VERSION_DATA_LENGTH) || ((*temp) != WSC_VERSION))
+ return wsc_attr_error;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_message_type_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ unsigned char *temp;
+ //struct p1905_managerd_ctx *ctx_temp = (struct p1905_managerd_ctx *)ctx;
+
+ temp = pkt;
+#if defined(SUPPORT_AP_REGISTRAR)
+ if(((*length) != ATTR_MESSAGE_TYPE_DATA_LENGTH) || ((*temp) != MESSAGE_TYPE_M1))
+#elif defined(SUPPORT_AP_ENROLLE)
+ if(((*length) != ATTR_MESSAGE_TYPE_DATA_LENGTH) || ((*temp) != MESSAGE_TYPE_M2))
+#endif
+ return wsc_attr_error;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_enrolle_nonce_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ unsigned char *temp;
+ struct p1905_managerd_ctx *ctx_temp = (struct p1905_managerd_ctx *)ctx;
+ temp = pkt;
+
+ if((*length) != ATTR_ENROLLE_NONCE_DATA_LENGTH)
+ return wsc_attr_error;
+
+#if defined(SUPPORT_AP_REGISTRAR)
+ memcpy(ctx_temp->ap_config->e_nonce, temp, ATTR_ENROLLE_NONCE_DATA_LENGTH);
+#elif defined(SUPPORT_AP_ENROLLE)
+ if(memcmp(ctx_temp->ap_config->e_nonce, temp, ATTR_ENROLLE_NONCE_DATA_LENGTH))
+ return wsc_attr_error;
+#endif
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_registrar_nonce_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+#ifdef SUPPORT_AP_ENROLLE
+ unsigned char *temp;
+ struct p1905_managerd_ctx *ctx_temp = (struct p1905_managerd_ctx *)ctx;
+
+ if((*length) != ATTR_REGISTRAR_NONCE_DATA_LENGTH)
+ return wsc_attr_error;
+
+ temp = pkt;
+ memcpy(ctx_temp->ap_config->r_nonce, temp, ATTR_REGISTRAR_NONCE_DATA_LENGTH);
+#endif
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_public_key_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ unsigned char *temp;
+ struct p1905_managerd_ctx *ctx_temp = (struct p1905_managerd_ctx *)ctx;
+#ifdef SUPPORT_AP_ENROLLE
+ DH_TABLE *dh_key;
+ KDK_KDF_TABLE *kdk;
+#endif
+
+ if((*length) != ATTR_PUBLIC_KEY_DATA_LENGTH)
+ return wsc_attr_error;
+
+ temp = pkt;
+ memcpy(ctx_temp->ap_config->peer_public_key, temp, ATTR_PUBLIC_KEY_DATA_LENGTH);
+
+#ifdef SUPPORT_AP_ENROLLE
+ /*if we are enrolle, need to calculate Diffie-Helman security key here*/
+ dh_key = (DH_TABLE *)malloc(sizeof(DH_TABLE));
+ memcpy(dh_key->priv_key, ctx_temp->ap_config->private_key, 192);
+ memcpy(dh_key->pub_key, ctx_temp->ap_config->peer_public_key, 192);
+
+ if(wifi_utils_success != generate_DH_secu_key(dh_key))
+ {
+ free(dh_key);
+ return wsc_attr_error;
+ }
+ memcpy(ctx_temp->ap_config->security_key, dh_key->secu_key, 192);
+ free(dh_key);
+
+ /*if we are enrolle, need to calculate keywrapkey & authkey here*/
+ kdk = (KDK_KDF_TABLE *)malloc(sizeof(KDK_KDF_TABLE));
+ memcpy(kdk->DH_Secu_Key, ctx_temp->ap_config->security_key, 192);
+ memcpy(kdk->E_Nonce, ctx_temp->ap_config->e_nonce, 16);
+ memcpy(kdk->R_Nonce, ctx_temp->ap_config->r_nonce, 16);
+ memcpy(kdk->E_Mac_Addr, ctx_temp->ap_config->enrolle_mac, 6);
+ if(wifi_utils_success != generate_auth_keywrap_key(kdk))
+ {
+ free(kdk);
+ return wsc_attr_error;
+ }
+ memcpy(ctx_temp->ap_config->keyWrapKey, kdk->KeyWrapKey, 16);
+ memcpy(ctx_temp->ap_config->authKey, kdk->AuthKey, 32);
+ free(kdk);
+#endif
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_ssid_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+#ifdef SUPPORT_AP_ENROLLE
+ unsigned char *temp;
+ struct p1905_managerd_ctx *ctx_temp = (struct p1905_managerd_ctx *)ctx;
+
+if(ctx_temp->is_in_encrypt_settings)
+{
+ temp = pkt;
+
+ if(ctx_temp->ap_config_data != NULL)
+ {
+ memset(ctx_temp->ap_config_data->Ssid, 0, 32+1);
+ memcpy(ctx_temp->ap_config_data->Ssid, temp, (*length));
+ ctx_temp->get_config_attr_kind |= (1 << HAS_SSID_ATTR);
+ }
+ else
+ return wsc_attr_error;
+}
+#endif
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_uuid_e_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_uuid_r_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+#ifdef SUPPORT_AP_ENROLLE
+ /*need to recore the registrar uuid*/
+#endif
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_mac_address_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+
+ unsigned char *temp;
+ struct p1905_managerd_ctx *ctx_temp = (struct p1905_managerd_ctx *)ctx;
+ temp = pkt;
+
+ if((*length) != ATTR_MAC_ADDRESS_DATA_LENGTH)
+ return wsc_attr_error;
+#if defined(SUPPORT_AP_REGISTRAR)
+ memcpy(ctx_temp->ap_config->enrolle_mac, temp, ATTR_MAC_ADDRESS_DATA_LENGTH);
+#elif defined(SUPPORT_AP_ENROLLE)
+if(ctx_temp->is_in_encrypt_settings)
+{
+ if(memcmp(ctx_temp->wifi0_mac_addr, temp, ATTR_MAC_ADDRESS_DATA_LENGTH))
+ return wsc_attr_error;
+
+ ctx_temp->get_config_attr_kind |= (1 << HAS_MAC_ADDR_ATTR);
+}
+#endif
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_encrypt_settings_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ WSC_ATTR_STATUS status = wsc_attr_success;
+#ifdef SUPPORT_AP_ENROLLE
+ unsigned char *temp;
+ struct p1905_managerd_ctx *ctx_temp = (struct p1905_managerd_ctx *)ctx;
+ AES_TABLE *dec;
+ WSC_TLV_HDR *hdr;
+ KWA_TABLE kwa;
+ unsigned char *temp_buf, *temp1_buf;
+ unsigned short type = 0, tlv_len = 0, total_len = 0;
+ int index;
+
+ ctx_temp->is_in_encrypt_settings = 1;
+ ctx_temp->get_config_attr_kind = 0;
+
+ temp = pkt;
+ kwa.EncrptData = malloc(512);
+
+ dec = (AES_TABLE *)malloc(sizeof(AES_TABLE));
+ dec->PlainText = malloc(512);
+ dec->PlainTextLen = 512;
+
+ memcpy(dec->KeyWrapKey, ctx_temp->ap_config->keyWrapKey, 16);
+
+ /*get IV and shift to encrypt data start position*/
+ memcpy(dec->IV, temp, 16);
+ temp += 16;
+
+ /*get cipher text*/
+ dec->CipherText = malloc(512);
+ dec->CipherTextLen = (*length) - 16;
+ memcpy(dec->CipherText, temp, dec->CipherTextLen);
+
+ if(wifi_utils_success != generate_AES_CBC_decrypt_value(dec))
+ {
+ status = wsc_attr_error;
+ goto end;
+ }
+
+ /*start to parse plain text, check KWA first*/
+ /*1. calculate KWA for compare*/
+ temp_buf = dec->PlainText;
+ temp1_buf = dec->PlainText;
+ memcpy(kwa.EncrptData, temp1_buf,
+ (dec->PlainTextLen - ATTR_KWA_LENGTH - ATTR_HEADR_LENGTH));
+ kwa.EncrptDataLen = (dec->PlainTextLen - ATTR_KWA_LENGTH - ATTR_HEADR_LENGTH);
+ memcpy(kwa.AuthKey, ctx_temp->ap_config->authKey, 32);
+ if(wifi_utils_success != generate_kwa(&kwa))
+ {
+ status = wsc_attr_error;
+ goto end;
+ }
+
+ /*2. shift to start position of received KWA tlv*/
+ temp1_buf += (dec->PlainTextLen - ATTR_KWA_LENGTH - ATTR_HEADR_LENGTH);
+ hdr = (WSC_TLV_HDR *)temp1_buf;
+ if((htons(hdr->type) != ATTR_KEY_WRAP_AUTHENTICATOR_ID) ||
+ (htons(hdr->length) != ATTR_KWA_LENGTH))
+ {
+ status = wsc_attr_error;
+ goto end;
+ }
+ temp1_buf += ATTR_HEADR_LENGTH;
+ if(memcmp(temp1_buf, kwa.KWA, ATTR_KWA_LENGTH))
+ {
+ status = wsc_attr_error;
+ goto end;
+ }
+
+ /*3. parse encrypt settings tlv*/
+ while((dec->PlainTextLen - ATTR_KWA_LENGTH - ATTR_HEADR_LENGTH) > total_len)
+ {
+ hdr = (WSC_TLV_HDR *)temp_buf;
+
+ type = htons(hdr->type);
+ tlv_len = htons(hdr->length);
+
+ debug_syslog("get encrypt settings tlv type 0x%.4x\n",type);
+ debug_syslog("get encrypt settings length 0x%.4x\n",tlv_len);
+
+ index = search_match_function_by_id(type);
+ if(index >= 0)
+ {
+ temp_buf += ATTR_HEADR_LENGTH;//shift to data field
+ status = wsc_func[index].parse(temp_buf, ctx, &tlv_len);
+ if(status != wsc_attr_success)
+ {
+ debug_syslog("encrypt settings error, type = 0x%.4x\n",type);
+ goto end;
+ }
+ }
+ else
+ {
+ debug_syslog("not matched item in encrypt settings, type = 0x%.4x\n",type);
+ status = wsc_attr_not_find;
+ goto end;
+ }
+ temp_buf += tlv_len;
+ total_len += (tlv_len + ATTR_HEADR_LENGTH);
+ }
+
+end:
+ free(kwa.EncrptData);
+ free(dec->CipherText);
+ free(dec->PlainText);
+ free(dec);
+ ctx_temp->is_in_encrypt_settings = 0;
+#endif
+ return status;
+}
+
+static WSC_ATTR_STATUS parse_authentication_type_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+#ifdef SUPPORT_AP_ENROLLE
+ unsigned char *temp;
+ struct p1905_managerd_ctx *ctx_temp = (struct p1905_managerd_ctx *)ctx;
+ unsigned short authmode;
+
+if(ctx_temp->is_in_encrypt_settings)
+{
+ temp = pkt;
+
+ if((*length) != ATTR_AUTHENTICATION_TYPE_DATA_LENGTH)
+ return wsc_attr_error;
+
+ authmode = (*temp);
+ authmode = (authmode << 8) & 0xFF00;
+ authmode = authmode |(*(temp+1));
+
+ debug("parse authmode = 0x%.4x\n",authmode);
+
+ if(ctx_temp->ap_config_data != NULL)
+ ctx_temp->ap_config_data->AuthMode = authmode;
+
+ ctx_temp->get_config_attr_kind |= (1 << HAS_AUTHENTICATION_TYPE_ATTR);
+}
+
+#endif
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_encryption_type_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+#ifdef SUPPORT_AP_ENROLLE
+ unsigned char *temp;
+ struct p1905_managerd_ctx *ctx_temp = (struct p1905_managerd_ctx *)ctx;
+ unsigned short encryptype;
+
+if(ctx_temp->is_in_encrypt_settings)
+{
+ if((*length) != ATTR_ENCRYPTION_TYPE_DATA_LENGTH)
+ return wsc_attr_error;
+
+ temp = pkt;
+ encryptype = (*temp);
+ encryptype = (encryptype << 8) & 0xFF00;
+ encryptype = encryptype |(*(temp+1));
+
+ debug("parse encryptype = 0x%.4x\n",encryptype);
+
+ if(ctx_temp->ap_config_data != NULL)
+ ctx_temp->ap_config_data->EncrypType = encryptype;
+
+ ctx_temp->get_config_attr_kind |= (1 << HAS_ENCRYPTION_TYPE_ATTR);
+}
+#endif
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_network_key_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+#ifdef SUPPORT_AP_ENROLLE
+ unsigned char *temp;
+ struct p1905_managerd_ctx *ctx_temp = (struct p1905_managerd_ctx *)ctx;
+
+if(ctx_temp->is_in_encrypt_settings)
+{
+ if((*length) == 0)
+ {
+ ctx_temp->get_config_attr_kind |= (1 << HAS_NETWORK_KEY_ATTR);
+ return wsc_attr_success;
+ }
+
+ temp = pkt;
+
+ if(ctx_temp->ap_config_data != NULL)
+ {
+ memset(ctx_temp->ap_config_data->WPAKey, 0, 64+1);
+ memcpy(ctx_temp->ap_config_data->WPAKey, temp, (*length));
+ }
+ else
+ return wsc_attr_error;
+
+ ctx_temp->get_config_attr_kind |= (1 << HAS_NETWORK_KEY_ATTR);
+}
+#endif
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_auth_type_flag_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_encrypt_type_flag_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_conn_type_flag_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_config_method_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_wsc_state_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ unsigned char *temp;
+ temp = pkt;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_manufacturer_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_model_name_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_model_number_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_serial_number_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_prim_dev_type_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_device_name_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_rf_band_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ RF_BAND band = rf_band_2p4G;
+
+ if(wifi_utils_success != get_rf_band(&band))
+ return wsc_attr_error;
+ if((*pkt) != band)
+ return wsc_attr_error;
+
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_association_state_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_device_pwd_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_config_error_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_os_version_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+static WSC_ATTR_STATUS parse_authenticator_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ WSC_ATTR_STATUS status = wsc_attr_success;
+
+#ifdef SUPPORT_AP_ENROLLE
+ unsigned char *temp;
+ struct p1905_managerd_ctx *ctx_temp = (struct p1905_managerd_ctx *)ctx;
+ KWA_TABLE *authenticator;
+
+ temp = pkt;
+
+ /*1. get (M1 || M2*) and calculated hash value by HMAC-SHA-256*/
+ authenticator = (KWA_TABLE *)malloc(sizeof(KWA_TABLE));
+ authenticator->EncrptData = malloc(ctx_temp->last_tx_length + ctx_temp->current_rx_length);
+ authenticator->EncrptDataLen = (ctx_temp->last_tx_length + ctx_temp->current_rx_length);
+ memcpy(authenticator->EncrptData, ctx_temp->last_tx_data, ctx_temp->last_tx_length);
+ memcpy((authenticator->EncrptData + ctx_temp->last_tx_length),
+ ctx_temp->current_rx_data, ctx_temp->current_rx_length);
+ memcpy(authenticator->AuthKey, ctx_temp->ap_config->authKey, 32);
+ if(wifi_utils_success != generate_kwa(authenticator))
+ {
+ status = wsc_attr_error;
+ goto end;
+ }
+ /* 2. compare the calculated value and authenticator data value from M2*/
+ if(memcmp(authenticator->KWA, temp, 8))
+ {
+ debug_syslog("authenticaor error\n");
+ /*different value, we got a error packet*/
+ status = wsc_attr_error;
+ goto end;
+ }
+
+end:
+ free(authenticator->EncrptData);
+ free(authenticator);
+ if(ctx_temp->last_tx_data != NULL)
+ {
+ free(ctx_temp->last_tx_data);
+ ctx_temp->last_tx_data = NULL;
+ }
+ if(status == wsc_attr_success)
+ ctx_temp->is_authenticator_exist_in_M2 = 1;
+#endif
+ return status;
+}
+
+static WSC_ATTR_STATUS parse_vendor_extension_attr(
+ unsigned char *pkt, void *ctx, unsigned short *length)
+{
+ return wsc_attr_success;
+}
+
+/* =========================================================================*/
+/* common WSC attribute TLVs append function */
+/* =========================================================================*/
+
+WSC_ATTR_STATUS append_wsc_attr_tlv(
+ unsigned char *pkt, unsigned char *inbuf, unsigned short *length,
+ unsigned short tlv_type)
+{
+ unsigned char *temp_buf;
+ WSC_TLV_HDR *hdr;
+ unsigned short data_length = (*length);
+ int index = 0;
+ WSC_ATTR_STATUS status = wsc_attr_success;
+
+ temp_buf = pkt;
+ hdr = (WSC_TLV_HDR *)temp_buf;
+
+ /*fill into tlv type*/
+ hdr->type = htons(tlv_type);
+ /*shift to data field*/
+ temp_buf += ATTR_HEADR_LENGTH;
+
+ /*use tlv_type to find matched fucntion, then create data field*/
+ index = search_match_function_by_id(tlv_type);
+ if(index >= 0)
+ {
+ status = wsc_func[index].create(temp_buf, inbuf, &data_length);
+ if(status != wsc_attr_success)
+ {
+ debug_syslog("wsc_function error, type = 0x%.4x\n",tlv_type);
+ goto end;
+ }
+ }
+ else
+ {
+ debug_syslog("not matched item in wsc_func, type = 0x%.4x\n",tlv_type);
+ status = wsc_attr_not_find;
+ goto end;
+ }
+
+ /*fill into tlv length*/
+ hdr->length = htons(data_length);
+
+ *length = data_length + ATTR_HEADR_LENGTH;
+
+end:
+ return status;
+}
+
+/* =========================================================================*/
+/* common WSC attribute TLVs parse function */
+/* =========================================================================*/
+
+WSC_ATTR_STATUS parse_wsc_attr_tlv(
+ unsigned char *pkt, struct p1905_managerd_ctx *ctx, unsigned short length)
+{
+ unsigned char *temp_buf;
+ WSC_TLV_HDR *hdr;
+ WSC_ATTR_STATUS status = wsc_attr_success;
+ int index = 0;
+ unsigned short total_len = 0, tlv_len = 0;
+ unsigned short type = 0;
+ unsigned char tlv_sequence_num = 0;
+ unsigned char total_receive_tlv_num = 0;
+
+ temp_buf = pkt;
+
+#ifdef SUPPORT_AP_ENROLLE
+ /* if enrollee, need to parse authenticator to check integrity of
+ * (M1 || M2*). so need to store M2*.(WSC 2.0.2 ch7.2)
+ */
+ /* 1. calculate M2* length*/
+ ctx->current_rx_length = length - ATTR_AUTHENTICATOR_DATA_LENGTH - ATTR_HEADR_LENGTH;
+ /* 2. store M2* data to ctx->current_rx_data*/
+ ctx->current_rx_data = pkt;
+#endif
+
+ /* the input parameter length will introduce the tlv length gotten from
+ * cmdu tlv length field
+ */
+ while(length > total_len)
+ {
+ hdr = (WSC_TLV_HDR *)temp_buf;
+
+ type = htons(hdr->type);
+ tlv_len = htons(hdr->length);
+
+ debug_syslog("get wsc tlv type 0x%.4x\n",type);
+ debug_syslog("get wsc tlv length 0x%.4x\n",tlv_len);
+ tlv_sequence_num++;
+
+ index = search_match_function_by_id(type);
+ if(index >= 0)
+ {
+#if defined(SUPPORT_AP_REGISTRAR)
+ if((wsc_func[index].M1_order != tlv_sequence_num) &&
+ (total_receive_tlv_num < TOTAL_REQUIRED_TLV))
+ {
+ debug_syslog("stop parse wsc because out of order\n");
+ status = wsc_attr_out_of_order;
+ goto end;
+ }
+#elif defined(SUPPORT_AP_ENROLLE)
+ if((wsc_func[index].M2_order != tlv_sequence_num) &&
+ (total_receive_tlv_num < TOTAL_REQUIRED_TLV))
+ {
+ debug_syslog("stop parse wsc because out of order\n");
+ status = wsc_attr_out_of_order;
+ goto end;
+ }
+#endif
+ total_receive_tlv_num++;
+
+ temp_buf += ATTR_HEADR_LENGTH;//shift to data field
+ status = wsc_func[index].parse(temp_buf, ctx, &tlv_len);
+ if(status != wsc_attr_success)
+ {
+ debug_syslog("wsc_function error, type = 0x%.4x\n",type);
+ goto end;
+ }
+ }
+ else
+ {
+ debug_syslog("not matched item in parse wsc, type = 0x%.4x\n",type);
+ temp_buf += ATTR_HEADR_LENGTH;
+ if( total_receive_tlv_num < TOTAL_REQUIRED_TLV)
+ {
+ debug_syslog("stop parse wsc because out of order\n");
+ status = wsc_attr_out_of_order;
+ goto end;
+ }
+ }
+ temp_buf += tlv_len;
+ total_len += (tlv_len + ATTR_HEADR_LENGTH);
+ }
+#ifdef SUPPORT_AP_ENROLLE
+ /*enrollee need to check existence of authenticator tlv*/
+ if((!ctx->is_authenticator_exist_in_M2) ||
+ (ctx->get_config_attr_kind !=((1<<HAS_SSID_ATTR)|(1<<HAS_AUTHENTICATION_TYPE_ATTR)|
+ (1<<HAS_ENCRYPTION_TYPE_ATTR)|(1<<HAS_NETWORK_KEY_ATTR)|(1<<HAS_MAC_ADDR_ATTR))))
+ {
+ debug_syslog("is_authenticator_exist_in_M2 %d\n",ctx->is_authenticator_exist_in_M2);
+ debug_syslog("get_config_attr_kind 0x%.2x\n",ctx->get_config_attr_kind);
+ status = wsc_attr_not_get_all_tlv;
+ goto end;
+ }
+ else
+ ctx->is_authenticator_exist_in_M2 = 0;
+#endif
+
+end:
+ return status;
+}
+
+
diff --git a/cleopatre/devkit/p1905_managerd/src/wsc_message.c b/cleopatre/devkit/p1905_managerd/src/wsc_message.c
new file mode 100644
index 0000000000..ec4c79e204
--- /dev/null
+++ b/cleopatre/devkit/p1905_managerd/src/wsc_message.c
@@ -0,0 +1,597 @@
+/*
+ * cleopatre/application/p1905_managerd/src/wsc_message.c
+ *
+ * (C) Copyright 2013 MStar Semiconductor, Inc.
+ *
+ */
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <syslog.h>
+#include <errno.h>
+#include "wifi_utils.h"
+#include "wsc_attr_tlv.h"
+#include "p1905_managerd.h"
+
+//#define WSC_MESSAGE_DEBUG
+#ifdef WSC_MESSAGE_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
+
+
+#define ENCRYPT_LENGTH 512
+
+#ifdef SUPPORT_AP_ENROLLE
+WSC_ATTR_STATUS create_wsc_msg_M1(
+ struct p1905_managerd_ctx *ctx, unsigned char *pkt, unsigned short *length)
+{
+ unsigned char *temp_buf;
+ unsigned char *inbuf;
+ unsigned short temp_length = 0;
+ int i = 0;
+ WSC_ATTR_STATUS status = wsc_attr_success;
+ DH_TABLE *dh_key;
+
+ temp_buf = pkt;
+ inbuf = malloc(256);
+
+ /*enrolle need to malloc ap_config data structure when create M1*/
+ if(!ctx->ap_config)
+ ctx->ap_config = (ap_config_para *)malloc(sizeof(ap_config_para));
+
+ *inbuf = WSC_VERSION;
+ temp_length = ATTR_VERSION_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_VERSION_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ *inbuf = MESSAGE_TYPE_M1;
+ temp_length = ATTR_MESSAGE_TYPE_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_MESSAGE_TYPE_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ memcpy(inbuf, ctx->uuid, ATTR_UUID_E_DATA_LENGTH);
+ temp_length = ATTR_UUID_E_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_UUID_E_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ memcpy(ctx->ap_config->enrolle_mac, ctx->wifi0_mac_addr, 6);
+ memcpy(inbuf, ctx->wifi0_mac_addr, ATTR_MAC_ADDRESS_DATA_LENGTH);
+ temp_length = ATTR_MAC_ADDRESS_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_MAC_ADDRESS_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ for(i=0;i<16;i++)
+ ctx->ap_config->e_nonce[i] = ((rand() % 255) + 1);
+ memcpy(inbuf, ctx->ap_config->e_nonce, ATTR_ENROLLE_NONCE_DATA_LENGTH);
+ temp_length = ATTR_ENROLLE_NONCE_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_ENROLLE_NONCE_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*calculate Diffie-Hellman key begin*/
+ dh_key = (DH_TABLE *)malloc(sizeof(DH_TABLE));
+ if(wifi_utils_success != generate_DH_pub_priv_key(dh_key))
+ {
+ status = wsc_attr_error;
+ goto end2;
+ }
+ memcpy(ctx->ap_config->private_key, dh_key->priv_key, 192);
+ memcpy(ctx->ap_config->public_key , dh_key->pub_key, 192);
+
+ memcpy(inbuf, dh_key->pub_key, ATTR_PUBLIC_KEY_DATA_LENGTH);
+ temp_length = ATTR_PUBLIC_KEY_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_PUBLIC_KEY_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+ /*calculate Diffie-Hellman key end*/
+
+ /*. create authentication type flag*/
+ temp_length = 2;//authentication type flag tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_AUTH_TYPE_FLAG_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create encryption type flag*/
+ temp_length = 2;//encryption type flag tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_ENCRYPTION_TYPE_FLAG_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create connection type flag*/
+ temp_length = 1;//connection type flag tlv has fixed length 1 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_CONNECTION_TYPE_FLAG_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create configuration method*/
+ temp_length = 2;//configuration method tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_CONFIG_METHOD_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create WiFi simple configuration state*/
+ temp_length = 1;//WiFi simple configuration state tlv has fixed length 1 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_WSC_STATE_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create manufacturer*/
+ temp_length = 64;//manufacturer has max length 64 Bytes, implementation specified
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_MANUFACTURER_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create model name*/
+ temp_length = 32;//model name has max length 64 Bytes, implementation specified
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_MODEL_NAME_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create model number*/
+ temp_length = 32;//model number has max length 64 Bytes, implementation specified
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_MODEL_NUMBER_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create serial number*/
+ temp_length = 32;//serial number has max length 64 Bytes, implementation specified
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_SERIAL_NUMBER_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create primary device type*/
+ temp_length = 8;//primary device type has fixed length 8 bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_PRIM_DEV_TYPE_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create device name*/
+ temp_length = 32;//device name has max length 64 Bytes, implementation specified
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_DEVICE_NAME_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create rf band*/
+ temp_length = 1;//rf band tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_RF_BAND_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create association state*/
+ temp_length = 2;//association state tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_ASSOCIATION_STATE_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create device password id*/
+ temp_length = 2;//device password id tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_DEVICE_PWD_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create configuration error*/
+ temp_length = 2;//configuration error tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_CONFIG_ERROR_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create OS version*/
+ temp_length = 4;//OS version tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_OS_VERSION_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create version2 (WFA vendor extension)*/
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_VENDOR_EXTENSION_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*store M1 content to ctx->last_tx_data for authenticator tlv parsing*/
+ if(!(ctx->last_tx_data))
+ {
+ ctx->last_tx_data = malloc((*length));
+ ctx->last_tx_length = (*length);
+ memcpy(ctx->last_tx_data, pkt, (*length));
+ }
+ else
+ {
+ debug_syslog("error!! M1 re-entry\n");
+ }
+
+ goto end1;
+
+end2:
+ /*because something error when create the M1, so we free ctx->ap_config*/
+ if(ctx->ap_config != NULL)
+ {
+ free(ctx->ap_config);
+ ctx->ap_config = NULL;
+ }
+end1:
+ free(dh_key);
+ free(inbuf);
+ return status;
+}
+#endif
+
+#ifdef SUPPORT_AP_REGISTRAR
+WSC_ATTR_STATUS create_wsc_msg_M2(
+ struct p1905_managerd_ctx *ctx, unsigned char *pkt, unsigned short *length)
+{
+ unsigned char *temp_buf;
+ unsigned char *inbuf;
+ unsigned short temp_length = 0, encrypt_length = 0;
+ int i = 0;
+ WSC_ATTR_STATUS status = wsc_attr_success;
+ DH_TABLE *dh_key;
+ KDK_KDF_TABLE *kdk;
+ WSC_CONFIG *config_data;
+ KWA_TABLE *kwa, *authenticator;
+ AES_TABLE *enc;
+
+ temp_buf = pkt;
+ inbuf = malloc(ENCRYPT_LENGTH);
+
+ *inbuf = WSC_VERSION;
+ temp_length = ATTR_VERSION_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_VERSION_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ *inbuf = MESSAGE_TYPE_M2;
+ temp_length = ATTR_MESSAGE_TYPE_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_MESSAGE_TYPE_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /* because registrar must send M2 after receive M1, so the ap_config has
+ * already been allocated.
+ */
+ memcpy(inbuf, ctx->ap_config->e_nonce, ATTR_ENROLLE_NONCE_DATA_LENGTH);
+ temp_length = ATTR_ENROLLE_NONCE_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_ENROLLE_NONCE_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ for(i=0;i<16;i++)
+ ctx->ap_config->r_nonce[i] = ((rand() % 255) + 1);
+ memcpy(inbuf, ctx->ap_config->r_nonce, ATTR_REGISTRAR_NONCE_DATA_LENGTH);
+ temp_length = ATTR_REGISTRAR_NONCE_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_REGISTRAR_NONCE_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ memcpy(inbuf, ctx->uuid, ATTR_UUID_R_DATA_LENGTH);
+ temp_length = ATTR_UUID_R_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_UUID_R_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*calculate Diffie-Hellman key begin*/
+ dh_key = (DH_TABLE *)malloc(sizeof(DH_TABLE));
+
+ if(wifi_utils_success != generate_DH_pub_priv_key(dh_key))
+ {
+ status = wsc_attr_error;
+ goto end2;
+ }
+ memcpy(ctx->ap_config->private_key, dh_key->priv_key, 192);
+ memcpy(ctx->ap_config->public_key , dh_key->pub_key, 192);
+
+ memcpy(inbuf, dh_key->pub_key, ATTR_PUBLIC_KEY_DATA_LENGTH);
+ temp_length = ATTR_PUBLIC_KEY_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_PUBLIC_KEY_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+ /*calculate security key*/
+ memcpy(dh_key->pub_key, ctx->ap_config->peer_public_key, 192);
+ if(wifi_utils_success != generate_DH_secu_key(dh_key))
+ {
+ status = wsc_attr_error;
+ goto end2;
+ }
+ memcpy(ctx->ap_config->security_key, dh_key->secu_key, 192);
+ /*calculate Diffie-Hellman key end*/
+
+ /*calculate keywrap key & auth key by KDK begin*/
+ kdk = (KDK_KDF_TABLE *)malloc(sizeof(KDK_KDF_TABLE));
+ memcpy(kdk->DH_Secu_Key, ctx->ap_config->security_key, 192);
+ memcpy(kdk->E_Nonce, ctx->ap_config->e_nonce, 16);
+ memcpy(kdk->R_Nonce, ctx->ap_config->r_nonce, 16);
+ memcpy(kdk->E_Mac_Addr, ctx->ap_config->enrolle_mac, 6);
+ if(wifi_utils_success != generate_auth_keywrap_key(kdk))
+ {
+ status = wsc_attr_error;
+ goto end3;
+ }
+ memcpy(ctx->ap_config->keyWrapKey, kdk->KeyWrapKey, 16);
+ memcpy(ctx->ap_config->authKey, kdk->AuthKey, 32);
+ /*calculate keywrap key & auth key by KDK end*/
+
+ /*. create authentication type flag*/
+ temp_length = 2;//authentication type flag tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_AUTH_TYPE_FLAG_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create encryption type flag*/
+ temp_length = 2;//encryption type flag tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_ENCRYPTION_TYPE_FLAG_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create connection type flag*/
+ temp_length = 1;//connection type flag tlv has fixed length 1 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_CONNECTION_TYPE_FLAG_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create configuration method*/
+ temp_length = 2;//configuration method tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_CONFIG_METHOD_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create manufacturer*/
+ temp_length = 64;//manufacturer has max length 64 Bytes, implementation specified
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_MANUFACTURER_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create model name*/
+ temp_length = 32;//model name has max length 64 Bytes, implementation specified
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_MODEL_NAME_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create model number*/
+ temp_length = 32;//model number has max length 64 Bytes, implementation specified
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_MODEL_NUMBER_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create serial number*/
+ temp_length = 32;//serial number has max length 64 Bytes, implementation specified
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_SERIAL_NUMBER_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create primary device type*/
+ temp_length = 8;//primary device type has fixed length 8 bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_PRIM_DEV_TYPE_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create device name*/
+ temp_length = 32;//device name has max length 64 Bytes, implementation specified
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_DEVICE_NAME_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create rf band*/
+ temp_length = 1;//rf band tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_RF_BAND_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create association state*/
+ temp_length = 2;//association state tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_ASSOCIATION_STATE_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create configuration error*/
+ temp_length = 2;//configuration error tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_CONFIG_ERROR_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create device password id*/
+ temp_length = 2;//device password id tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_DEVICE_PWD_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create OS version*/
+ temp_length = 4;//OS version tlv has fixed length 2 Bytes
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_OS_VERSION_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*. create version2 (WFA vendor extension)*/
+ append_wsc_attr_tlv(temp_buf, inbuf, &temp_length, ATTR_VENDOR_EXTENSION_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+ /*prepare content of encrypt settings tlv begin*/
+ /*1. get config data*/
+ config_data = (WSC_CONFIG *)malloc(sizeof(WSC_CONFIG));
+ if(wifi_utils_success != get_wsc_config(config_data))
+ {
+ status = wsc_attr_error;
+ goto end4;
+ }
+ /*2. create ssid tlv*/
+ /*need to check more, ASCII case/Hex case ......*/
+ temp_length = strlen(config_data->Ssid);
+ append_wsc_attr_tlv(inbuf, config_data->Ssid, &temp_length, ATTR_SSID_ID);
+ encrypt_length += temp_length;
+ inbuf += temp_length;
+
+ /*3. create authentication type tlv*/
+ temp_length = ATTR_AUTHENTICATION_TYPE_DATA_LENGTH;
+ append_wsc_attr_tlv(inbuf,(unsigned char *) &(config_data->AuthMode), &temp_length, ATTR_AUTHENTICATION_TYPE_ID);
+ encrypt_length += temp_length;
+ inbuf += temp_length;
+
+ /*4. create encryption type tlv*/
+ temp_length = ATTR_ENCRYPTION_TYPE_DATA_LENGTH;
+ append_wsc_attr_tlv(inbuf,(unsigned char *) &(config_data->EncrypType), &temp_length, ATTR_ENCRYPTION_TYPE_ID);
+ encrypt_length += temp_length;
+ inbuf += temp_length;
+
+ /*5. create network key tlv*/
+ if(config_data->AuthMode == AUTH_OPEN && config_data->EncrypType == ENCRYP_NONE)
+ temp_length = 0;
+ else //we need to check more about Hex network key
+ temp_length = strlen(config_data->WPAKey);
+
+ append_wsc_attr_tlv(inbuf, config_data->WPAKey, &temp_length, ATTR_NETWORK_KEY_ID);
+ encrypt_length += temp_length;
+ inbuf += temp_length;
+
+ /*6. create mac address tlv*/
+ temp_length = ATTR_MAC_ADDRESS_DATA_LENGTH;
+ append_wsc_attr_tlv(inbuf, ctx->ap_config->enrolle_mac, &temp_length, ATTR_MAC_ADDRESS_ID);
+ encrypt_length += temp_length;
+ inbuf += temp_length;
+
+ /*7. create key wrap authenticator tlv*/
+ /*back to inbuf begining to calculate KWA*/
+ inbuf -= encrypt_length;
+ kwa = (KWA_TABLE *)malloc(sizeof(KWA_TABLE));
+ kwa->EncrptData = malloc(encrypt_length);
+ memcpy(kwa->EncrptData, inbuf, encrypt_length);
+ kwa->EncrptDataLen = encrypt_length;
+ memcpy(kwa->AuthKey, ctx->ap_config->authKey, 32);
+ temp_length = ATTR_KWA_LENGTH;
+ /*return to tail of inbuf. KWA tlv needs to be appended*/
+ inbuf += encrypt_length;
+ append_wsc_attr_tlv(inbuf, (unsigned char *)kwa, &temp_length,
+ ATTR_KEY_WRAP_AUTHENTICATOR_ID);
+ inbuf += temp_length;
+ encrypt_length += temp_length;
+
+ /*8. create encrypt settings tlv*/
+ inbuf -= encrypt_length; //back to inbuf begining to encrypt
+ enc = (AES_TABLE *)malloc(sizeof(AES_TABLE));
+ enc->PlainText = inbuf;
+ enc->PlainTextLen = encrypt_length;
+ for(i=0;i<16;i++)
+ enc->IV[i] = ((rand() % 255) + 1);
+ enc->CipherText = malloc(ENCRYPT_LENGTH -16);
+ enc->CipherTextLen = ENCRYPT_LENGTH -16;
+ memcpy(enc->KeyWrapKey, ctx->ap_config->keyWrapKey, 16);
+ temp_length = 0;
+ append_wsc_attr_tlv(temp_buf, (unsigned char *)enc, &temp_length,
+ ATTR_ENCRYPTED_SETTINGS_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+ /*prepare content of encrypt settings tlv end*/
+
+ /*create authenticator tlv*/
+ /* 1. create (M1 || M2*) */
+ authenticator = (KWA_TABLE *)malloc(sizeof(KWA_TABLE));
+ authenticator->EncrptData = malloc(ctx->last_rx_length + (*length));
+ memcpy(authenticator->EncrptData, ctx->last_rx_data, ctx->last_rx_length);
+ authenticator->EncrptData += ctx->last_rx_length;
+ memcpy(authenticator->EncrptData, pkt, (*length));
+ authenticator->EncrptData -= ctx->last_rx_length;
+ authenticator->EncrptDataLen = ctx->last_rx_length + (*length);
+ /* 2. default key is auth key */
+ memcpy(authenticator->AuthKey, ctx->ap_config->authKey, 32);
+ /* 3. HMAC-SHA-256(M1 || M2*), key is auth key, then append*/
+ temp_length = ATTR_AUTHENTICATOR_DATA_LENGTH;
+ append_wsc_attr_tlv(temp_buf, (unsigned char *)authenticator, &temp_length,
+ ATTR_AUTHENTICATOR_ID);
+ temp_buf += temp_length;
+ (*length) += temp_length;
+
+end7:
+ free(authenticator->EncrptData);
+ free(authenticator);
+end6:
+ free(enc->CipherText);
+ free(enc);
+end5:
+ free(kwa->EncrptData);
+ free(kwa);
+end4:
+ free(config_data);
+end3:
+ free(kdk);
+end2:
+ free(dh_key);
+end1:
+ free(inbuf);
+ /* Because M2 message create flow is done(whatever success of fail).
+ * don't need this dynamic memory anymore
+ */
+ if(ctx->ap_config != NULL)
+ {
+ free(ctx->ap_config);
+ ctx->ap_config = NULL;
+ }
+ if(ctx->last_rx_data != NULL)
+ {
+ free(ctx->last_rx_data);
+ ctx->last_rx_data = NULL;
+ }
+ return status;
+}
+#endif
+
+WSC_ATTR_STATUS parse_wsc_msg(
+ struct p1905_managerd_ctx *ctx, unsigned char *pkt, unsigned short length)
+{
+#ifdef SUPPORT_AP_REGISTRAR
+ /* if AP is registrar, ctx->ap_config will be allocated here.
+ * we need to prepare ctx->ap_config to store exchaged data from M1.
+ */
+ if(!ctx->ap_config)
+ ctx->ap_config = (ap_config_para *)malloc(sizeof(ap_config_para));
+ /* for authenticator tlv use, we need to store M1 content for future use.
+ * so allocate a buffer to store M1.
+ */
+ if(!ctx->last_rx_data)
+ {
+ ctx->last_rx_data = malloc(length);
+ ctx->last_rx_length = length;
+ memcpy(ctx->last_rx_data, pkt, length);
+ }
+#endif
+
+ if(wsc_attr_success != parse_wsc_attr_tlv(pkt, ctx, length))
+ {
+ debug("parse_wsc_msg not success\n");
+#ifdef SUPPORT_AP_REGISTRAR
+ /* does not get correct M1, so registrar does not need to send M2.
+ * Free some dynamic memory prepared for exchanged data.
+ */
+ if(ctx->ap_config != NULL)
+ {
+ free(ctx->ap_config);
+ ctx->ap_config = NULL;
+ }
+
+ if(ctx->last_rx_data != NULL)
+ {
+ free(ctx->last_rx_data);
+ ctx->last_rx_data = NULL;
+ }
+#endif
+ return wsc_attr_error;
+ }
+
+#ifdef SUPPORT_AP_ENROLLE
+ /* if goto here, it means Enrolle got correct M2 and we don't need
+ * the exchanged data anymore, so free the ctx->ap_config
+ */
+ if(ctx->ap_config != NULL)
+ {
+ free(ctx->ap_config);
+ ctx->ap_config = NULL;
+ }
+#endif
+ return wsc_attr_success;
+}
+
+
diff --git a/cleopatre/linux-2.6.25.10-spc300/include/linux/if_bridge.h b/cleopatre/linux-2.6.25.10-spc300/include/linux/if_bridge.h
index 58e43e5664..c70efd044d 100644
--- a/cleopatre/linux-2.6.25.10-spc300/include/linux/if_bridge.h
+++ b/cleopatre/linux-2.6.25.10-spc300/include/linux/if_bridge.h
@@ -44,6 +44,8 @@
#define BRCTL_SET_PORT_PRIORITY 16
#define BRCTL_SET_PATH_COST 17
#define BRCTL_GET_FDB_ENTRIES 18
+#define BRCTL_GET_RECV_PORT_ADDR 19
+#define BRCTL_SET_BRIDGE_NOT_FORWARD_DEST 20
#define BR_STATE_DISABLED 0
#define BR_STATE_LISTENING 1
diff --git a/cleopatre/linux-2.6.25.10-spc300/include/linux/if_ether.h b/cleopatre/linux-2.6.25.10-spc300/include/linux/if_ether.h
index 983ebf914c..117cd9e2dd 100644
--- a/cleopatre/linux-2.6.25.10-spc300/include/linux/if_ether.h
+++ b/cleopatre/linux-2.6.25.10-spc300/include/linux/if_ether.h
@@ -80,6 +80,10 @@
#define ETH_P_HPAV 0x88E1 /* HomePlug AV packet */
#define ETH_P_8021QINQ 0x9100 /* Cisco QinQ */
+#define ETH_P_1905 0x893A /*IEEE P1905.1 packet*/
+#define ETH_P_LLDP 0x88CC /*IEEE 802.1ab LLDP packet*/
+
+
/*
* Non DIX types. Won't clash for 1500 types.
*/
diff --git a/cleopatre/linux-2.6.25.10-spc300/net/Kconfig b/cleopatre/linux-2.6.25.10-spc300/net/Kconfig
index 29d688f59e..e86fdf1c25 100644..100755
--- a/cleopatre/linux-2.6.25.10-spc300/net/Kconfig
+++ b/cleopatre/linux-2.6.25.10-spc300/net/Kconfig
@@ -193,6 +193,15 @@ source "net/econet/Kconfig"
source "net/wanrouter/Kconfig"
source "net/sched/Kconfig"
+config IEEE1905_1_FORWARD_ENTITY
+ bool "Enable ieee1905.1 forward entity"
+ depends on BRIDGE
+ default N
+ ---help---
+ Use ethernet bridge as ieee1905.1 forward entity.
+
+ If unsure, say N.
+
menu "Network testing"
config NET_PKTGEN
diff --git a/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_device.c b/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_device.c
index bf7787395f..05cbc2ecef 100644..100755
--- a/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_device.c
+++ b/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_device.c
@@ -40,6 +40,23 @@ int br_dev_xmit(struct sk_buff *skb, struct net_device *dev)
skb_reset_mac_header(skb);
skb_pull(skb, ETH_HLEN);
+#ifdef CONFIG_IEEE1905_1_FORWARD_ENTITY
+ /*
+ * if protocol is ieee1905.1 or LLDP, let bridge select transmitting port
+ * by source mac field in mac header.
+ * This modification is for topology discover message because topology
+ * discovery message should be transmitted in specific interface defined
+ * in mac address type tlv.
+ */
+ if (skb->protocol == htons(ETH_P_1905) ||
+ skb->protocol == htons(ETH_P_LLDP)) {
+ if ((dst = __br_fdb_get(br, eth_hdr(skb)->h_source)) != NULL) {
+ br_deliver(dst->dst, skb);
+ return 0;
+ }
+ }
+#endif
+
if (dest[0] & 1)
br_flood_deliver(br, skb);
else if ((dst = __br_fdb_get(br, dest)) != NULL)
diff --git a/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_input.c b/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_input.c
index 255c00f60c..de03bc482f 100644..100755
--- a/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_input.c
+++ b/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_input.c
@@ -22,6 +22,10 @@
/* Bridge group multicast address 802.1d (pg 51). */
const u8 br_group_address[ETH_ALEN] = { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x00 };
+#ifdef CONFIG_IEEE1905_1_FORWARD_ENTITY
+const u8 br_p1905_multicast_address[ETH_ALEN] = { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x13 };
+#endif
+
static void br_pass_frame_up(struct net_bridge *br, struct sk_buff *skb)
{
struct net_device *indev;
@@ -63,7 +67,23 @@ int br_handle_frame_finish(struct sk_buff *skb)
dst = NULL;
- if (is_multicast_ether_addr(dest)) {
+#ifdef CONFIG_IEEE1905_1_FORWARD_ENTITY
+ if (skb->protocol == htons(ETH_P_1905)){
+ /* only send to upper layer(don't forward) if 1905.1 multicast address
+ * or local abstraction layer mac address.
+ * According to ieee1905.1 spec, neighbor multicast CMDU and unicast
+ * CMDU should not be forwarded.
+ */
+ if((!compare_ether_addr(br_p1905_multicast_address,dest)) ||
+ (!compare_ether_addr(br_get_not_forward_dest(),dest)) ||
+ ((dst = __br_fdb_get(br, dest)) && dst->is_local)){
+ skb2 = skb;
+ skb = NULL;
+ }
+ }
+ else
+#endif
+ if (is_multicast_ether_addr(dest)) {
br->statistics.multicast++;
skb2 = skb;
} else if ((dst = __br_fdb_get(br, dest)) && dst->is_local) {
diff --git a/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_ioctl.c b/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_ioctl.c
index 0655a5f07f..326af8668a 100644..100755
--- a/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_ioctl.c
+++ b/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_ioctl.c
@@ -22,6 +22,9 @@
#include <asm/uaccess.h>
#include "br_private.h"
+#ifdef CONFIG_IEEE1905_1_FORWARD_ENTITY
+unsigned char not_forward_dest[6]={0x0,0x0,0x0,0x0,0x0,0x0};
+#endif
/* called with RTNL */
static int get_bridge_ifindices(int *indices, int num)
{
@@ -83,6 +86,46 @@ static int get_fdb_entries(struct net_bridge *br, void __user *userbuf,
return num;
}
+#ifdef CONFIG_IEEE1905_1_FORWARD_ENTITY
+/*
+ * This function is for 1905.1 daemon use.
+ * for 1905.1 relay multicast message, daemon use this function to select
+ * correct interface to re-transmit message.
+ * for topology message, daemon use this function to record the
+ * receiving interface for topology used.
+ */
+static int get_received_port_addr(struct net_bridge *br, void __user *ruserbuf,
+ void __user *wuserbuf)
+{
+ unsigned char mac_addr[6];
+ struct net_bridge_fdb_entry *dst;
+
+ if(copy_from_user(mac_addr, wuserbuf, 6))
+ return -EFAULT;
+
+ if((dst = __br_fdb_get(br,mac_addr)) == NULL)
+ return -EFAULT;
+
+ if (copy_to_user(ruserbuf, dst->dst->dev->dev_addr,6))
+ return -EFAULT;
+
+ return 0;
+}
+
+/*
+ * This function is for 1905.1 daemon use.
+ * let bridge not forward 1905.1 multicast address and local abstraction layer
+ * mac address.
+ */
+static int set_not_forward_dest(struct net_bridge *br, void __user *userbuf)
+{
+ if(copy_from_user(not_forward_dest, userbuf, 6))
+ return -EFAULT;
+
+ return 0;
+}
+#endif
+
static int add_del_if(struct net_bridge *br, int ifindex, int isadd)
{
struct net_device *dev;
@@ -306,6 +349,13 @@ static int old_dev_ioctl(struct net_device *dev, struct ifreq *rq, int cmd)
case BRCTL_GET_FDB_ENTRIES:
return get_fdb_entries(br, (void __user *)args[1],
args[2], args[3]);
+#ifdef CONFIG_IEEE1905_1_FORWARD_ENTITY
+ case BRCTL_GET_RECV_PORT_ADDR:
+ return get_received_port_addr(br, (void __user *)args[1],\
+ (void __user *)args[2]);
+ case BRCTL_SET_BRIDGE_NOT_FORWARD_DEST:
+ return set_not_forward_dest(br, (void __user *)args[1]);
+#endif
}
return -EOPNOTSUPP;
@@ -365,6 +415,18 @@ static int old_deviceless(void __user *uarg)
return -EOPNOTSUPP;
}
+#ifdef CONFIG_IEEE1905_1_FORWARD_ENTITY
+/*
+ * used in function br_handle_frame_finish.
+ * 1905.1 daemon set not forward mac address in user space, then kernel use
+ * it to handle forwarding rule.
+ */
+unsigned char *br_get_not_forward_dest(void)
+{
+ return &not_forward_dest[0];
+}
+#endif
+
int br_ioctl_deviceless_stub(struct net *net, unsigned int cmd, void __user *uarg)
{
switch (cmd) {
diff --git a/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_private.h b/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_private.h
index c11b554fd1..1050fd7f6e 100644..100755
--- a/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_private.h
+++ b/cleopatre/linux-2.6.25.10-spc300/net/bridge/br_private.h
@@ -193,6 +193,9 @@ extern struct sk_buff *br_handle_frame(struct net_bridge_port *p,
/* br_ioctl.c */
extern int br_dev_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
extern int br_ioctl_deviceless_stub(struct net *net, unsigned int cmd, void __user *arg);
+#ifdef CONFIG_IEEE1905_1_FORWARD_ENTITY
+extern unsigned char *br_get_not_forward_dest(void);
+#endif
/* br_netfilter.c */
#ifdef CONFIG_BRIDGE_NETFILTER