| /** @file wifi_display.c |
| * |
| * |
| * Copyright (C) 2008-2017, Marvell International Ltd. |
| * |
| * This software file (the "File") is distributed by Marvell International |
| * Ltd. under the terms of the GNU General Public License Version 2, June 1991 |
| * (the "License"). You may use, redistribute and/or modify this File in |
| * accordance with the terms and conditions of the License, a copy of which |
| * is available along with the File in the gpl.txt file or by writing to |
| * the Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA |
| * 02111-1307 or on the worldwide web at http://www.gnu.org/licenses/gpl.txt. |
| * |
| * THE FILE IS DISTRIBUTED AS-IS, WITHOUT WARRANTY OF ANY KIND, AND THE |
| * IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE |
| * ARE EXPRESSLY DISCLAIMED. The License provides additional details about |
| * this warranty disclaimer. |
| * |
| */ |
| /**************************************************************************** |
| Change log: |
| 22/09/11: Initial creation |
| ****************************************************************************/ |
| |
| /**************************************************************************** |
| Header files |
| ****************************************************************************/ |
| |
| #include <stdarg.h> |
| #include <sys/types.h> |
| #include <unistd.h> |
| #include <sys/socket.h> |
| #include <sys/select.h> |
| #include <stdio.h> |
| #include <getopt.h> |
| #include <netinet/in.h> |
| #include <arpa/inet.h> |
| #include <errno.h> |
| #include <stdlib.h> |
| #include <string.h> |
| #include <ctype.h> |
| #include <linux/if.h> |
| #include <sys/ioctl.h> |
| #include <errno.h> |
| #include <linux/wireless.h> |
| |
| #include "wifi_display.h" |
| #include "wifidirectutl.h" |
| |
| /* |
| * @brief Show usage information for the wifidisplay_config command |
| * |
| * $return N/A |
| */ |
| static void |
| print_wifidisplay_config_usage(void) |
| { |
| printf("\nUsage : wifidisplay_config [CONFIG_FILE]\n"); |
| printf("\nIf CONFIG_FILE is provided, a 'set' is performed, else a 'get' is performed.\n"); |
| printf("CONFIG_FILE contains all WiFiDisplay parameters.\n"); |
| return; |
| } |
| |
| /* |
| ss and send ie config command |
| * @param ie_index A pointer to the IE buffer index |
| * @param data_len_wifidisplay Length of Wifidisplay data |
| * @param buf Pointer to buffer to set. |
| * @return SUCCESS--success, FAILURE--fail |
| */ |
| static int |
| wifiDisplay_ie_config(t_s16 *ie_index, t_u16 data_len_wifidisplay, t_u8 *buf) |
| { |
| struct iwreq iwr; |
| t_s32 sockfd; |
| int i, ret = SUCCESS; |
| tlvbuf_custom_ie *tlv = NULL; |
| custom_ie *ie_ptr = NULL; |
| |
| tlv = (tlvbuf_custom_ie *)buf; |
| tlv->tag = MRVL_MGMT_IE_LIST_TLV_ID; |
| /* Locate headers */ |
| ie_ptr = (custom_ie *)(tlv->ie_data); |
| |
| /* Set TLV fields : WFD IE parameters */ |
| if (data_len_wifidisplay) { |
| /* Set IE */ |
| #define DISPLAY_MASK 0xFFFF |
| ie_ptr->mgmt_subtype_mask = DISPLAY_MASK; |
| tlv->length = sizeof(custom_ie) + data_len_wifidisplay; |
| ie_ptr->ie_length = data_len_wifidisplay; |
| ie_ptr->ie_index = *ie_index; |
| } else { |
| /* Get WPS IE */ |
| tlv->length = 0; |
| } |
| |
| /* Locate headers */ |
| ie_ptr = (custom_ie *)((t_u8 *)(tlv->ie_data) + sizeof(custom_ie) + |
| data_len_wifidisplay); |
| |
| memset(&iwr, 0, sizeof(iwr)); |
| strncpy(iwr.ifr_name, (char *)dev_name, IFNAMSIZ); |
| |
| iwr.u.data.pointer = (void *)buf; |
| iwr.u.data.length = |
| ((2 * sizeof(custom_ie)) + sizeof(tlvbuf_custom_ie) + |
| data_len_wifidisplay); |
| iwr.u.data.flags = 0; |
| |
| /* |
| * * create a socket |
| * */ |
| if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) { |
| printf("Cannot open socket.\n"); |
| ret = FAILURE; |
| goto _exit_; |
| } |
| |
| if (ioctl(sockfd, CUSTOM_IE, &iwr)) { |
| perror("ioctl[CUSTOM_IE]"); |
| printf("Failed to set/get/clear the IE buffer\n"); |
| ret = FAILURE; |
| close(sockfd); |
| goto _exit_; |
| } |
| close(sockfd); |
| |
| if (!data_len_wifidisplay) { |
| /* Get the IE buffer index number for MGMT_IE_LIST_TLV */ |
| tlv = (tlvbuf_custom_ie *)buf; |
| *ie_index = -1; |
| if (tlv->tag == MRVL_MGMT_IE_LIST_TLV_ID) { |
| ie_ptr = (custom_ie *)(tlv->ie_data); |
| for (i = 0; i < MAX_MGMT_IE_INDEX; i++) { |
| // Index 0 and 1 are reserved for WFD in current implementation |
| if ((ie_ptr->mgmt_subtype_mask == DISPLAY_MASK) |
| && (ie_ptr->ie_length) && (i != 0) && |
| (i != 1)) { |
| *ie_index = ie_ptr->ie_index; |
| break; |
| } |
| if (i < (MAX_MGMT_IE_INDEX - 1)) |
| ie_ptr = (custom_ie *)((t_u8 *)ie_ptr + |
| sizeof(custom_ie) |
| + |
| ie_ptr-> |
| ie_length); |
| } |
| } |
| if (*ie_index == -1) { |
| printf("\nNo free IE buffer available\n"); |
| ret = FAILURE; |
| } |
| } |
| _exit_: |
| |
| return ret; |
| } |
| |
| /** |
| * @brief Creates a wifidisplay_config request and sends to the driver |
| * |
| * Usage: "Usage : wfd_config [CONFIG_FILE]" |
| * |
| * @param argc Number of arguments |
| * @param argv Pointer to the arguments |
| * @return None |
| */ |
| void |
| wifidisplaycmd_config(int argc, char *argv[]) |
| { |
| t_u8 *buf = NULL; |
| t_u16 ie_len_wifidisplay = 0, ie_len; |
| t_s16 ie_index = -1; |
| int opt, ret = SUCCESS; |
| |
| while ((opt = getopt_long(argc, argv, "+", cmd_options, NULL)) != -1) { |
| switch (opt) { |
| default: |
| print_wifidisplay_config_usage(); |
| return; |
| } |
| } |
| argc -= optind; |
| argv += optind; |
| |
| /* Check arguments */ |
| if (argc < 2) { |
| printf("ERR:wrong number of arguments.\n"); |
| print_wifidisplay_config_usage(); |
| return; |
| } |
| buf = (t_u8 *)malloc(MRVDRV_SIZE_OF_CMD_BUFFER); |
| if (!buf) { |
| printf("ERR:Cannot allocate memory!\n"); |
| return; |
| } |
| memset(buf, 0, MRVDRV_SIZE_OF_CMD_BUFFER); |
| |
| if (argc >= 3) { |
| /* Read parameters and send command to firmware */ |
| wifidisplay_file_params_config(argv[2], argv[1], buf |
| + sizeof(tlvbuf_custom_ie) + |
| sizeof(custom_ie), |
| &ie_len_wifidisplay); |
| if (argc == 4) { |
| ie_index = atoi(argv[3]); |
| if (ie_index >= 4) { |
| printf("ERR:wrong argument %s.\n", argv[3]); |
| return; |
| } |
| } |
| if (ie_len_wifidisplay > MAX_SIZE_IE_BUFFER) { |
| printf("ERR:IE parameter size exceeds limit in %s.\n", |
| argv[2]); |
| free(buf); |
| return; |
| } |
| ie_len = ie_len_wifidisplay + sizeof(tlvbuf_custom_ie) + |
| sizeof(custom_ie); |
| if (ie_len >= MRVDRV_SIZE_OF_CMD_BUFFER) { |
| printf("ERR:Too much data in configuration file %s.\n", |
| argv[2]); |
| free(buf); |
| return; |
| } |
| #ifdef DEBUG |
| hexdump(buf, ie_len, ' '); |
| #endif |
| ret = wifiDisplay_ie_config(&ie_index, ie_len_wifidisplay, buf); |
| if (ret != SUCCESS) { |
| printf("ERR:Could not set wfd parameters\n"); |
| } |
| } |
| } |
| |
| /* * @brief Read the wifidisplay parameters and sends to the driver |
| * |
| * @param file_name File to open for configuration parameters. |
| * @param cmd_name Command Name for which parameters are read. |
| * @param pbuf Pointer to output buffer |
| * @param ie_len_wfd Length of wifidisplay parameters to return |
| * @return SUCCESS or FAILURE |
| */ |
| #define DEVICE_DESCRIPTOR_LEN 24 |
| static const t_u8 wifidisplay_oui[] = { 0x50, 0x6F, 0x9A, 0x0A }; |
| |
| void |
| wifidisplay_file_params_config(char *file_name, char *cmd_name, |
| t_u8 *pbuf, t_u16 *ie_len_wifidisplay) |
| { |
| FILE *config_file = NULL; |
| char *line = NULL; |
| t_u8 *extra = NULL, *len_ptr = NULL; |
| t_u8 *buffer = pbuf; |
| char **args = NULL; |
| t_u16 cmd_len_wifidisplay = 0, tlv_len = 0; |
| tlvbuf_wifidisplay_ie_format *display_ie_buf = NULL; |
| int wifiDisplay_level = 0, ret = 0, coupled_sink_bitmap = 0; |
| t_u16 display_device_info, session_mgmt_control_port, |
| wfd_device_throuput; |
| t_u8 assoc_bssid[] = { 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE }; |
| t_u8 alternate_mac[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; |
| t_u8 peer_mac[] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }; |
| t_u8 default_mac[] = { 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE }; |
| char wfd_mac[20]; |
| int li = 0, arg_num = 0; |
| char *pos = NULL; |
| t_u8 cmd_found = 0; |
| t_u16 temp; |
| t_u16 wfd_session_len; |
| t_u8 total_num_device_info __attribute__ ((__unused__)); /* For future use */ |
| t_u8 curr_dev_info = 0; |
| t_u8 dev_info_dev_add[ETH_ALEN], dev_info_assoc_bssid[ETH_ALEN], |
| dev_info_coupled_add[ETH_ALEN]; |
| t_u16 descriptor_display_device_info, descriptor_wfd_device_throuput; |
| t_u8 descriptor_wfd_coupled_sink_status; |
| t_u8 wfd_dev_descriptor_arr[120], device_info_desc_len, ind = |
| DEVICE_DESCRIPTOR_LEN; |
| /* Check if file exists */ |
| config_file = fopen(file_name, "r"); |
| if (config_file == NULL) { |
| printf("\nERR:Config file can not open.\n"); |
| return; |
| } |
| |
| /* Memory allocations */ |
| line = (char *)malloc(MAX_CONFIG_LINE); |
| if (!line) { |
| printf("ERR:Cannot allocate memory for line\n"); |
| goto done; |
| } |
| memset(line, 0, MAX_CONFIG_LINE); |
| |
| extra = (t_u8 *)malloc(MAX_CONFIG_LINE); |
| if (!extra) { |
| printf("ERR:Cannot allocate memory for extra\n"); |
| goto done; |
| } |
| memset(extra, 0, MAX_CONFIG_LINE); |
| |
| args = (char **)malloc(sizeof(char *) * MAX_ARGS_NUM); |
| if (!args) { |
| printf("ERR:Cannot allocate memory for args\n"); |
| goto done; |
| } |
| memset(args, 0, (sizeof(char *) * MAX_ARGS_NUM)); |
| display_ie_buf = (tlvbuf_wifidisplay_ie_format *)buffer; |
| display_ie_buf->ElemId = VENDOR_SPECIFIC_IE_TAG; |
| len_ptr = buffer + 1; |
| memcpy(&display_ie_buf->Oui[0], wifidisplay_oui, |
| sizeof(wifidisplay_oui)); |
| cmd_len_wifidisplay += 2 + sizeof(wifidisplay_oui); |
| while (config_get_line(line, MAX_CONFIG_LINE, config_file, &li, &pos)) { |
| arg_num = parse_line(line, args); |
| if (!cmd_found && (cmd_name != NULL) |
| && strncmp(args[0], cmd_name, strlen(args[0]))) |
| cmd_found = 1; |
| if (strcmp(args[0], "display_dev_info") == 0) { |
| wifiDisplay_level = DISPLAY_DEVICE_INFO; |
| } else if (strcmp(args[0], "display_assoc_bssid") == 0) { |
| wifiDisplay_level = DISPLAY_ASSOCIATED_BSSID; |
| } else if (strcmp(args[0], "display_coupled_sink") == 0) { |
| wifiDisplay_level = DISPLAY_COUPLED_SINK; |
| } else if (strcmp(args[0], "display_session_info") == 0) { |
| wifiDisplay_level = DISPLAY_SESSION_INFO; |
| } else if (strcmp(args[0], "display_alternate_mac") == 0) { |
| wifiDisplay_level = DISPLAY_ALTERNATE_MAC_ADDR; |
| } else if (strcmp(args[0], "device_info") == 0) { |
| if (is_wifidisplay_input_valid |
| (WFD_DEVICE_INFO, arg_num - 1, args + 1) |
| != SUCCESS) { |
| goto done; |
| } |
| display_device_info = (t_u16)atoi(args[1]); |
| } else if (strcmp(args[0], "mgmt_control_port") == 0) { |
| if (is_wifidisplay_input_valid |
| (WFD_SESSION_MGMT_CONTROL_PORT, arg_num - 1, |
| args + 1) != SUCCESS) { |
| goto done; |
| } |
| session_mgmt_control_port = (t_u16)atoi(args[1]); |
| } else if (strcmp(args[0], "device_throuput") == 0) { |
| if (is_wifidisplay_input_valid |
| (WFD_DEVICE_THROUGHPUT, arg_num - 1, |
| args + 1) != SUCCESS) { |
| goto done; |
| } |
| wfd_device_throuput = (t_u16)atoi(args[1]); |
| } else if (strcmp(args[0], "assoc_bssid") == 0) { |
| strncpy(wfd_mac, args[1], 20); |
| if ((ret = mac2raw(wfd_mac, assoc_bssid)) != SUCCESS) { |
| printf("ERR: %s Address \n", |
| ret == FAILURE ? "Invalid MAC" : ret == |
| WIFIDIRECT_RET_MAC_BROADCAST ? |
| "Broadcast" : "Multicast"); |
| goto done; |
| } |
| } else if (strcmp(args[0], "alternate_mac") == 0) { |
| strncpy(wfd_mac, args[1], 20); |
| if ((ret = mac2raw(wfd_mac, alternate_mac)) != SUCCESS) { |
| printf("ERR: %s Address \n", |
| ret == FAILURE ? "Invalid MAC" : ret == |
| WIFIDIRECT_RET_MAC_BROADCAST ? |
| "Broadcast" : "Multicast"); |
| goto done; |
| } |
| } else if (strcmp(args[0], "coupled_sink_bitmap") == 0) { |
| if (is_wifidisplay_input_valid |
| (WFD_COUPLED_SINK, arg_num - 1, args + 1) |
| != SUCCESS) { |
| goto done; |
| } |
| coupled_sink_bitmap = (t_u8)atoi(args[1]); |
| } else if (strcmp(args[0], "session_info_len") == 0) { |
| wfd_session_len = (t_u16)atoi(args[1]); |
| total_num_device_info = |
| (wfd_session_len / DEVICE_DESCRIPTOR_LEN); |
| } else if (strncmp(args[0], "device_info_descriptor_len", 26) == |
| 0) { |
| device_info_desc_len = (t_u16)atoi(args[1]); |
| } else if (strncmp(args[0], "device_info_dev_id", 18) == 0) { |
| strncpy(wfd_mac, args[1], 20); |
| if ((ret = |
| mac2raw(wfd_mac, dev_info_dev_add)) != SUCCESS) { |
| printf("ERR: %s Address \n", |
| ret == FAILURE ? "Invalid MAC" : ret == |
| WIFIDIRECT_RET_MAC_BROADCAST ? |
| "Broadcast" : "Multicast"); |
| goto done; |
| } |
| } else if (strncmp(args[0], "device_info_assoc_bssid", 18) == 0) { |
| strncpy(wfd_mac, args[1], 20); |
| if ((ret = |
| mac2raw(wfd_mac, |
| dev_info_assoc_bssid)) != SUCCESS) { |
| printf("ERR: %s Address \n", |
| ret == FAILURE ? "Invalid MAC" : ret == |
| WIFIDIRECT_RET_MAC_BROADCAST ? |
| "Broadcast" : "Multicast"); |
| goto done; |
| } |
| } else if (strncmp(args[0], "descriptor_device_info", 22) == 0) { |
| descriptor_display_device_info = (t_u16)atoi(args[1]); |
| temp = htons(descriptor_display_device_info); |
| memcpy(&descriptor_display_device_info, &temp, 2); |
| } else if (strncmp(args[0], "descriptor_device_throuput", 24) == |
| 0) { |
| descriptor_wfd_device_throuput = (t_u16)atoi(args[1]); |
| temp = htons(descriptor_wfd_device_throuput); |
| memcpy(&descriptor_display_device_info, &temp, 2); |
| } else if (strncmp(args[0], "descriptor_cs_bitmap", 20) == 0) { |
| descriptor_wfd_coupled_sink_status = |
| (t_u8)atoi(args[1]); |
| } else if (strncmp(args[0], "device_info_coupled_address", 27) |
| == 0) { |
| strncpy(wfd_mac, args[1], 20); |
| if ((ret = |
| mac2raw(wfd_mac, |
| dev_info_coupled_add)) != SUCCESS) { |
| printf("ERR: %s Address \n", |
| ret == FAILURE ? "Invalid MAC" : ret == |
| WIFIDIRECT_RET_MAC_BROADCAST ? |
| "Broadcast" : "Multicast"); |
| goto done; |
| } |
| if (curr_dev_info > 5) { |
| printf("ERR in device descriptor"); |
| goto done; |
| } |
| memcpy(&wfd_dev_descriptor_arr[ind * curr_dev_info], |
| &device_info_desc_len, sizeof(t_u8)); |
| memcpy(&wfd_dev_descriptor_arr[ind * curr_dev_info + 1], |
| &dev_info_dev_add, ETH_ALEN); |
| memcpy(&wfd_dev_descriptor_arr[ind * curr_dev_info + 7], |
| &dev_info_assoc_bssid, ETH_ALEN); |
| memcpy(&wfd_dev_descriptor_arr |
| [ind * curr_dev_info + 13], |
| &descriptor_display_device_info, sizeof(t_u16)); |
| memcpy(&wfd_dev_descriptor_arr |
| [ind * curr_dev_info + 15], |
| &descriptor_wfd_device_throuput, sizeof(t_u16)); |
| memcpy(&wfd_dev_descriptor_arr |
| [ind * curr_dev_info + 17], |
| &descriptor_wfd_coupled_sink_status, |
| sizeof(t_u8)); |
| memcpy(&wfd_dev_descriptor_arr |
| [ind * curr_dev_info + 18], |
| &dev_info_coupled_add, ETH_ALEN); |
| curr_dev_info++; |
| } else if (strcmp(args[0], "}") == 0) { |
| switch (wifiDisplay_level) { |
| case DISPLAY_DEVICE_INFO: |
| { |
| tlvbuf_wfdisplay_device_info *tlv = |
| NULL; |
| /* Append a new TLV */ |
| tlv_len = |
| sizeof |
| (tlvbuf_wfdisplay_device_info); |
| tlv = (tlvbuf_wfdisplay_device_info |
| *)(buffer + cmd_len_wifidisplay); |
| cmd_len_wifidisplay += tlv_len; |
| /* Set TLV fields */ |
| tlv->tag = |
| TLV_TYPE_WIFIDISPLAY_DEVICE_INFO; |
| tlv->length = |
| htons(tlv_len - |
| (sizeof(t_u8) + |
| sizeof(t_u16))); |
| *ie_len_wifidisplay = |
| cmd_len_wifidisplay; |
| temp = htons(display_device_info); |
| memcpy(&tlv->display_device_info, &temp, |
| 2); |
| temp = htons(session_mgmt_control_port); |
| memcpy(&tlv->session_mgmt_control_port, |
| &temp, 2); |
| temp = htons(wfd_device_throuput); |
| memcpy(&tlv->wfd_device_throuput, &temp, |
| 2); |
| wifiDisplay_level = 0; |
| break; |
| } |
| case DISPLAY_ASSOCIATED_BSSID: |
| { |
| tlvbuf_wfdisplay_assoc_bssid *tlv = |
| NULL; |
| if (memcmp |
| (default_mac, assoc_bssid, |
| ETH_ALEN)) { |
| /* Append a new TLV */ |
| tlv_len = |
| sizeof |
| (tlvbuf_wfdisplay_assoc_bssid); |
| tlv = (tlvbuf_wfdisplay_assoc_bssid *)(buffer + cmd_len_wifidisplay); |
| cmd_len_wifidisplay += tlv_len; |
| *ie_len_wifidisplay = |
| cmd_len_wifidisplay; |
| /* Set TLV fields */ |
| tlv->tag = |
| TLV_TYPE_WIFIDISPLAY_ASSOC_BSSID; |
| tlv->length = |
| htons(tlv_len - |
| (sizeof(t_u8) + |
| sizeof(t_u16))); |
| memcpy(tlv->assoc_bssid, |
| assoc_bssid, ETH_ALEN); |
| |
| wifiDisplay_level = 0; |
| } |
| break; |
| } |
| case DISPLAY_COUPLED_SINK: |
| { |
| tlvbuf_wfdisplay_coupled_sink *tlv = |
| NULL; |
| tlv_len = |
| sizeof |
| (tlvbuf_wfdisplay_coupled_sink); |
| tlv = (tlvbuf_wfdisplay_coupled_sink |
| *)(buffer + cmd_len_wifidisplay); |
| cmd_len_wifidisplay += tlv_len; |
| *ie_len_wifidisplay = |
| cmd_len_wifidisplay; |
| /* Set TLV fields */ |
| tlv->tag = |
| TLV_TYPE_WIFIDISPLAY_COUPLED_SINK; |
| tlv->length = |
| htons(tlv_len - |
| (sizeof(t_u8) + |
| sizeof(t_u16))); |
| memcpy(tlv->peer_mac, peer_mac, |
| ETH_ALEN); |
| tlv->coupled_sink = coupled_sink_bitmap; |
| wifiDisplay_level = 0; |
| } |
| break; |
| case DISPLAY_SESSION_INFO: |
| { |
| tlvbuf_wifi_display_session_info *tlv = |
| NULL; |
| if (curr_dev_info > 0) { |
| tlv_len = |
| DEVICE_DESCRIPTOR_LEN * |
| curr_dev_info + 2; |
| tlv = (tlvbuf_wifi_display_session_info *)(buffer + cmd_len_wifidisplay); |
| cmd_len_wifidisplay += tlv_len; |
| *ie_len_wifidisplay = |
| cmd_len_wifidisplay; |
| /* Set TLV fields */ |
| tlv->tag = |
| TLV_TYPE_SESSION_INFO_SUBELEM; |
| tlv->length = |
| htons(tlv_len - |
| (sizeof(t_u8) + |
| sizeof(t_u16))); |
| memcpy((t_u8 *)&tlv-> |
| WFDDevInfoDesc, |
| (t_u8 *) |
| &wfd_dev_descriptor_arr, |
| (tlv_len - 2)); |
| wifiDisplay_level = 0; |
| } |
| } |
| break; |
| case DISPLAY_ALTERNATE_MAC_ADDR: |
| { |
| tlvbuf_wfdisplay_alternate_mac *tlv = |
| NULL; |
| if (memcmp |
| (default_mac, alternate_mac, |
| ETH_ALEN)) { |
| /* Append a new TLV */ |
| tlv_len = |
| sizeof |
| (tlvbuf_wfdisplay_alternate_mac); |
| tlv = (tlvbuf_wfdisplay_alternate_mac *)(buffer + cmd_len_wifidisplay); |
| cmd_len_wifidisplay += tlv_len; |
| *ie_len_wifidisplay = |
| cmd_len_wifidisplay; |
| /* Set TLV fields */ |
| tlv->tag = |
| TLV_TYPE_WIFIDISPLAY_ALTERNATE_MAC; |
| tlv->length = |
| htons(tlv_len - |
| (sizeof(t_u8) + |
| sizeof(t_u16))); |
| memcpy(tlv->alternate_mac, |
| alternate_mac, ETH_ALEN); |
| wifiDisplay_level = 0; |
| } |
| } |
| break; |
| default: |
| break; |
| } |
| } |
| } |
| *len_ptr = cmd_len_wifidisplay - 2; |
| done: |
| fclose(config_file); |
| if (line) |
| free(line); |
| if (extra) |
| free(extra); |
| if (args) |
| free(args); |
| return; |
| } |
| |
| /** |
| * @brief Show usage information for the wifidisplay_discovery commands |
| * |
| * $return N/A |
| */ |
| static void |
| print_wifidisplay_discovery_usage(void) |
| { |
| printf("\nUsage : wifidisplay_discovery_request/response [CONFIG_FILE]\n"); |
| printf("CONFIG_FILE contains WIFIDISPLAY service discovery payload.\n"); |
| return; |
| } |
| |
| /* @brief Creates a wifidirect_service_discovery request/response and |
| * sends to the driver |
| * |
| * Usage: "Usage : wifidirect_discovery_request/response [CONFIG_FILE]" |
| * |
| * @param argc Number of arguments |
| * @param argv Pointer to the arguments |
| * @return SUCCESS or FAILURE |
| **/ |
| void |
| wifidisplaycmd_service_discovery(int argc, char *argv[]) |
| { |
| wifidisplay_discovery_request *req_buf = NULL; |
| wifidisplay_discovery_response *resp_buf = NULL; |
| char *line = NULL; |
| FILE *config_file = NULL; |
| int i, opt, li = 0, arg_num = 0, ret = 0, wifidirect_level = 0; |
| char *args[30], *pos = NULL, wifidisplay_mac[20], wifidisplay_cmd[32]; |
| t_u8 dev_address[ETH_ALEN], cmd_found = 0; |
| t_u8 *buffer = NULL, *buf = NULL, *tmp_buffer = NULL; |
| t_u8 req_resp = 0; /* req = 0, resp = 1 */ |
| t_u16 cmd_len = 0, query_len = 0, vendor_len = 0, service_len = 0; |
| t_u16 ie_len_wifidisplay = 0; |
| |
| strncpy(wifidisplay_cmd, argv[2], sizeof(wifidisplay_cmd) - 1); |
| while ((opt = getopt_long(argc, argv, "+", cmd_options, NULL)) != -1) { |
| switch (opt) { |
| default: |
| print_wifidisplay_discovery_usage(); |
| return; |
| } |
| } |
| argc -= optind; |
| argv += optind; |
| |
| /* Check arguments */ |
| if (argc != 3) { |
| printf("ERR:Incorrect number of arguments.\n"); |
| print_wifidisplay_discovery_usage(); |
| return; |
| } |
| |
| /* Check if file exists */ |
| config_file = fopen(argv[2], "r"); |
| if (config_file == NULL) { |
| printf("\nERR:Config file can not open.\n"); |
| return; |
| } |
| line = (char *)malloc(MAX_CONFIG_LINE); |
| if (!line) { |
| printf("ERR:Cannot allocate memory for line\n"); |
| goto done; |
| } |
| memset(line, 0, MAX_CONFIG_LINE); |
| buf = (t_u8 *)malloc(MRVDRV_SIZE_OF_CMD_BUFFER); |
| if (!buf) { |
| printf("ERR:Cannot allocate memory!\n"); |
| goto done; |
| } |
| if (strcmp(args[0], "wifidisplay_discovery_response") == 0) { |
| t_u8 wfd_oui_header = 6; |
| wifidisplay_file_params_config(argv[2], NULL, buf, |
| &ie_len_wifidisplay); |
| ie_len_wifidisplay += wfd_oui_header; |
| buf += wfd_oui_header; |
| } else { |
| buf[0] = 0x03; |
| buf[1] = TLV_TYPE_WIFIDISPLAY_DEVICE_INFO; |
| buf[2] = TLV_TYPE_SESSION_INFO_SUBELEM; |
| buf[3] = TLV_TYPE_WIFIDISPLAY_COUPLED_SINK; |
| ie_len_wifidisplay = 4; |
| } |
| |
| /* Parse file and process */ |
| while (config_get_line(line, MAX_CONFIG_LINE, config_file, &li, &pos)) { |
| arg_num = parse_line(line, args); |
| if (!cmd_found && |
| strncmp(args[0], wifidisplay_cmd, strlen(args[0]))) |
| continue; |
| cmd_found = 1; |
| if (strcmp(args[0], "wifidisplay_discovery_request") == 0) { |
| wifidirect_level = |
| WIFIDIRECT_DISCOVERY_REQUEST_RESPONSE; |
| /* For wifidirect_service_discovery, basic initialization here */ |
| cmd_len = sizeof(wifidisplay_discovery_request); |
| buffer = (t_u8 *)malloc(cmd_len); |
| if (!buffer) { |
| printf("ERR:Cannot allocate memory!\n"); |
| goto done; |
| } |
| req_buf = (wifidisplay_discovery_request *)buffer; |
| req_buf->cmd_code = |
| HostCmd_CMD_WIFIDIRECT_SERVICE_DISCOVERY; |
| req_buf->size = cmd_len; |
| req_buf->seq_num = 0; |
| req_buf->result = 0; |
| } else if (strcmp(args[0], "wifidisplay_discovery_response") == |
| 0) { |
| wifidirect_level = |
| WIFIDIRECT_DISCOVERY_REQUEST_RESPONSE; |
| req_resp = 1; |
| /* For wifidirect_service_discovery, basic initialization here */ |
| cmd_len = sizeof(wifidisplay_discovery_response); |
| buffer = (t_u8 *)malloc(cmd_len); |
| if (!buffer) { |
| printf("ERR:Cannot allocate memory!\n"); |
| goto done; |
| } |
| resp_buf = (wifidisplay_discovery_response *)buffer; |
| resp_buf->cmd_code = |
| HostCmd_CMD_WIFIDIRECT_SERVICE_DISCOVERY; |
| resp_buf->size = cmd_len; |
| resp_buf->seq_num = 0; |
| resp_buf->result = 0; |
| |
| } else if (strcmp(args[0], "PeerAddr") == 0) { |
| strncpy(wifidisplay_mac, args[1], 20 - 1); |
| if ((ret = |
| mac2raw(wifidisplay_mac, |
| dev_address)) != SUCCESS) { |
| printf("ERR: %s Address \n", |
| ret == FAILURE ? "Invalid MAC" : ret == |
| WIFIDIRECT_RET_MAC_BROADCAST ? |
| "Broadcast" : "Multicast"); |
| goto done; |
| } |
| (!req_resp) ? memcpy(req_buf->peer_mac_addr, |
| dev_address, |
| ETH_ALEN) : memcpy(resp_buf-> |
| peer_mac_addr, |
| dev_address, |
| ETH_ALEN); |
| |
| } else if (strcmp(args[0], "Category") == 0) { |
| if (is_input_valid |
| (WIFIDIRECT_CATEGORY, arg_num - 1, args + 1) |
| != SUCCESS) { |
| goto done; |
| |
| } |
| (!req_resp) ? (req_buf->category = |
| (t_u8)atoi(args[1])) : (resp_buf-> |
| category = |
| (t_u8) |
| atoi(args[1])); |
| } else if (strcmp(args[0], "Action") == 0) { |
| if (is_input_valid |
| (WIFIDIRECT_ACTION, arg_num - 1, args + 1) |
| != SUCCESS) { |
| goto done; |
| } |
| (!req_resp) ? (req_buf->action = |
| (t_u8)A2HEXDECIMAL(args[1])) |
| : (resp_buf->action = |
| (t_u8)A2HEXDECIMAL(args[1])); |
| } else if (strcmp(args[0], "DialogToken") == 0) { |
| if (is_input_valid |
| (WIFIDIRECT_DIALOGTOKEN, arg_num - 1, args + 1) |
| != SUCCESS) { |
| goto done; |
| } |
| (!req_resp) ? (req_buf->dialog_taken = |
| (t_u8)atoi(args[1])) : (resp_buf-> |
| dialog_taken = |
| (t_u8) |
| atoi(args[1])); |
| } else if (strcmp(args[0], "StatusCode") == 0) { |
| resp_buf->status_code = (t_u8)atoi(args[1]); |
| } else if (strcmp(args[0], "GasComebackDelay") == 0) { |
| if (is_input_valid |
| (WIFIDIRECT_GAS_COMEBACK_DELAY, arg_num - 1, |
| args + 1) |
| != SUCCESS) { |
| goto done; |
| } |
| resp_buf->gas_reply = (t_u16)A2HEXDECIMAL(args[1]); |
| resp_buf->gas_reply = cpu_to_le16(resp_buf->gas_reply); |
| } else if (strcmp(args[0], "AdvertizementProtocolIE") == 0) { |
| if (is_input_valid |
| (WIFIDIRECT_DISC_ADPROTOIE, arg_num - 1, args + 1) |
| != SUCCESS) { |
| goto done; |
| } |
| if (!req_resp) { |
| for (i = 0; i < arg_num - 1; i++) |
| req_buf->advertize_protocol_ie[i] = |
| (t_u8)A2HEXDECIMAL(args[i + 1]); |
| } else { |
| for (i = 0; i < arg_num - 1; i++) |
| resp_buf->advertize_protocol_ie[i] = |
| (t_u8)A2HEXDECIMAL(args[i + 1]); |
| } |
| } else if (strcmp(args[0], "InfoId") == 0) { |
| if (is_input_valid |
| (WIFIDIRECT_DISC_INFOID, arg_num - 1, args + 1) |
| != SUCCESS) { |
| goto done; |
| } |
| if (!req_resp) { |
| for (i = 0; i < arg_num - 1; i++) |
| req_buf->info_id[i] = |
| (t_u8)A2HEXDECIMAL(args[i + 1]); |
| } else { |
| for (i = 0; i < arg_num - 1; i++) |
| resp_buf->info_id[i] = |
| (t_u8)A2HEXDECIMAL(args[i + 1]); |
| } |
| query_len += arg_num - 1; |
| } else if (strcmp(args[0], "OUI") == 0) { |
| if (is_input_valid |
| (WIFIDIRECT_OUI, arg_num - 1, args + 1) |
| != SUCCESS) { |
| goto done; |
| } |
| if (!req_resp) { |
| for (i = 0; i < arg_num - 1; i++) |
| req_buf->oui[i] = |
| (t_u8)A2HEXDECIMAL(args[i + 1]); |
| } else { |
| for (i = 0; i < arg_num - 1; i++) |
| resp_buf->oui[i] = |
| (t_u8)A2HEXDECIMAL(args[i + 1]); |
| } |
| service_len += arg_num - 1; |
| query_len += arg_num - 1; |
| } else if (strcmp(args[0], "OUISubType") == 0) { |
| if (is_input_valid |
| (WIFIDIRECT_OUISUBTYPE, arg_num - 1, args + 1) |
| != SUCCESS) { |
| goto done; |
| } |
| (!req_resp) ? (req_buf->oui_sub_type = |
| (t_u8)atoi(args[1])) : (resp_buf-> |
| oui_sub_type = |
| (t_u8) |
| atoi(args[1])); |
| service_len++; |
| query_len++; |
| } else if (strcmp(args[0], "QueryRequestLen") == 0 || |
| strcmp(args[0], "QueryResponseLen") == 0) { |
| wifidirect_level = WIFIDIRECT_DISCOVERY_QUERY; |
| } else if (strcmp(args[0], "RequestLen") == 0 || |
| strcmp(args[0], "ResponseLen") == 0) { |
| wifidirect_level = WIFIDIRECT_DISCOVERY_SERVICE; |
| query_len += 2; |
| } else if (strcmp(args[0], "VendorLen") == 0) { |
| wifidirect_level = WIFIDIRECT_DISCOVERY_VENDOR; |
| service_len += 2; |
| query_len += 2; |
| } else if (strcmp(args[0], "QueryData") == 0 || |
| strcmp(args[0], "ResponseData") == 0) { |
| wifidirect_level = |
| WIFIDIRECT_DISCOVERY_QUERY_RESPONSE_PER_PROTOCOL; |
| tmp_buffer = |
| realloc(buffer, cmd_len + ie_len_wifidisplay); |
| if (!tmp_buffer) { |
| printf("ERR:Cannot add DNS name to buffer!\n"); |
| goto done; |
| } else { |
| buffer = tmp_buffer; |
| tmp_buffer = NULL; |
| } |
| if (!req_resp) { |
| for (i = 0; i < ie_len_wifidisplay; i++) |
| req_buf->disc_query[i] = (t_u8)buf[i]; |
| } else { |
| resp_buf = |
| (wifidisplay_discovery_response *) |
| buffer; |
| for (i = 0; i < ie_len_wifidisplay; i++) |
| buffer[i + cmd_len] = (t_u8)buf[i]; |
| } |
| cmd_len += (ie_len_wifidisplay); |
| vendor_len += (ie_len_wifidisplay); |
| service_len += (ie_len_wifidisplay); |
| query_len += (ie_len_wifidisplay); |
| } else if (strcmp(args[0], "ServiceProtocol") == 0) { |
| if (!req_resp) { |
| req_buf->service_protocol = (t_u8)atoi(args[1]); |
| /* |
| * For uPnP, due to union allocation, a extra byte |
| * is allocated reduce it here for uPnP |
| */ |
| if (req_buf->service_protocol == 2) |
| cmd_len--; |
| } else { |
| resp_buf->service_protocol = |
| (t_u8)atoi(args[1]); |
| if (resp_buf->service_protocol == 2) |
| cmd_len--; |
| } |
| vendor_len++; |
| service_len++; |
| query_len++; |
| } else if (strcmp(args[0], "ServiceUpdateIndicator") == 0) { |
| if (is_input_valid |
| (WIFIDIRECT_SERVICEUPDATE_INDICATOR, arg_num - 1, |
| args + 1) |
| != SUCCESS) { |
| goto done; |
| } |
| (!req_resp) ? (req_buf->service_update_indicator = |
| cpu_to_le16((t_u16)atoi(args[1]))) : |
| (resp_buf->service_update_indicator = |
| cpu_to_le16((t_u16)atoi(args[1]))); |
| service_len += 2; |
| query_len += 2; |
| } else if (strcmp(args[0], "ServiceTransactionId") == 0) { |
| if (is_input_valid |
| (WIFIDIRECT_DISC_SERVICETRANSACID, arg_num - 1, |
| args + 1) |
| != SUCCESS) { |
| goto done; |
| } |
| (!req_resp) ? (req_buf->service_transaction_id = |
| (t_u8)atoi(args[1])) : (resp_buf-> |
| service_transaction_id |
| = |
| (t_u8) |
| atoi(args[1])); |
| vendor_len++; |
| service_len++; |
| query_len++; |
| } else if (strcmp(args[0], "}") == 0) { |
| switch (wifidirect_level) { |
| case WIFIDIRECT_DISCOVERY_QUERY: |
| (!req_resp) ? (req_buf->query_len = |
| cpu_to_le16(query_len)) |
| : (resp_buf->query_len = |
| cpu_to_le16(query_len)); |
| break; |
| case WIFIDIRECT_DISCOVERY_SERVICE: |
| (!req_resp) ? (req_buf->request_len = |
| cpu_to_le16(service_len)) |
| : (resp_buf->response_len = |
| cpu_to_le16(service_len)); |
| break; |
| case WIFIDIRECT_DISCOVERY_VENDOR: |
| (!req_resp) ? (req_buf->vendor_len = |
| cpu_to_le16(vendor_len)) |
| : (resp_buf->vendor_len = |
| cpu_to_le16(vendor_len)); |
| break; |
| default: |
| break; |
| } |
| if (wifidirect_level) { |
| if (wifidirect_level == |
| WIFIDIRECT_DISCOVERY_REQUEST_RESPONSE) |
| break; |
| wifidirect_level--; |
| } |
| } |
| } |
| /* Send collective command */ |
| wifidirect_ioctl((t_u8 *)buffer, &cmd_len, cmd_len); |
| done: |
| fclose(config_file); |
| if (buffer) |
| free(buffer); |
| if (line) |
| free(line); |
| } |
| |
| /** |
| * @brief Show usage information for the wfd display status command |
| * |
| * $return N/A |
| */ |
| static void |
| print_wifi_display_status_usage(void) |
| { |
| printf("\nUsage : wifi_display [STATUS]"); |
| printf("\nOptions: STATUS : 0 - stop wfd display"); |
| printf("\n 1 - start wfd display"); |
| printf("\n empty - get current wfd display status\n"); |
| return; |
| } |
| |
| /** |
| * @brief Creates wfd display start or stop request and send to driver |
| * |
| * Usage: "Usage : wifi_display [STATUS]" |
| * |
| * Options: STATUS : 0 - start wfd status |
| * 1 - stop wfd status |
| * |
| * @param argc Number of arguments |
| * @param argv Pointer to the arguments |
| * @return N/A |
| */ |
| void |
| wifidisplay_cmd_status(int argc, char *argv[]) |
| { |
| int opt, ret; |
| t_u16 data; |
| t_u16 cmd_len = 0; |
| t_u8 *buffer = NULL; |
| wifi_display_mode_config *cmd_buf = NULL; |
| while ((opt = getopt_long(argc, argv, "+", cmd_options, NULL)) != -1) { |
| switch (opt) { |
| default: |
| print_wifi_display_status_usage(); |
| return; |
| } |
| } |
| argc -= optind; |
| argv += optind; |
| /* Check arguments */ |
| if (argc < 2) { |
| printf("ERR:wrong arguments.\n"); |
| print_wifi_display_status_usage(); |
| return; |
| } |
| if (argc == 3) { |
| if ((ISDIGIT(argv[2]) == 0) || (atoi(argv[2]) < 0) || |
| (atoi(argv[2]) > 3)) { |
| printf("ERR:Illegal wfd mode %s. Must be in range from '0' to '3'.\n", argv[2]); |
| print_wifi_display_status_usage(); |
| return; |
| } |
| data = (t_u16)atoi(argv[2]); |
| } |
| |
| /* send hostcmd now */ |
| cmd_len = sizeof(wifi_display_mode_config); |
| buffer = (t_u8 *)malloc(cmd_len); |
| if (!buffer) { |
| printf("ERR:Cannot allocate memory!\n"); |
| return; |
| } |
| |
| cmd_buf = (wifi_display_mode_config *)buffer; |
| cmd_buf->cmd_code = HostCmd_CMD_WFD_DISPLAY_MODE_CONFIG; |
| cmd_buf->size = cmd_len - BUF_HEADER_SIZE; |
| cmd_buf->seq_num = 0; |
| cmd_buf->result = 0; |
| |
| if (argc == 2) { |
| cmd_buf->action = ACTION_GET; |
| } else { |
| cmd_buf->action = ACTION_SET; |
| cmd_buf->mode = cpu_to_le16(data); |
| } |
| cmd_buf->action = cpu_to_le16(cmd_buf->action); |
| ret = wifidirect_ioctl((t_u8 *)buffer, &cmd_len, cmd_len); |
| if (ret != SUCCESS) { |
| printf("Error executing wfd display_mode command\n"); |
| free(buffer); |
| return; |
| } |
| |
| data = le16_to_cpu(cmd_buf->mode); |
| switch (data) { |
| case 0: |
| printf("wfd display status = stopped\n"); |
| break; |
| case 1: |
| printf("wfd display status = started\n"); |
| break; |
| default: |
| printf("wfd display status = %d\n", data); |
| break; |
| } |
| free(buffer); |
| return; |
| } |
| |
| /** |
| * @brief Checkes a particular input for validatation for wifidisplay. |
| * |
| * @param cmd Type of input |
| * @param argc Number of arguments |
| * @param argv Pointer to the arguments |
| * @return SUCCESS or FAILURE |
| */ |
| |
| int |
| is_wifidisplay_input_valid(display_valid_inputs cmd, int argc, char *argv[]) |
| { |
| if (argc == 0) |
| return FAILURE; |
| if (argc != 1) { |
| printf("ERR:Incorrect number of arguments\n"); |
| return FAILURE; |
| } |
| switch (cmd) { |
| case WFD_DEVICE_INFO: |
| /*Bits 10-15 are reserved for device info */ |
| if ((ISDIGIT(argv[0]) == 0) || (atoi(argv[0]) < 0) |
| || (atoi(argv[0]) > ((1 << 10) - 1))) { |
| printf("ERR:Coupled sink paramater must be > 0 and < %d", ((1 << 10) - 1)); |
| return FAILURE; |
| } |
| /*bits 4 and 5 values 10 and 11 are reserved */ |
| if ((atoi(argv[0]) & 48) > 16) { |
| printf("ERR:Coupled sink paramater must not have bit 4 and 5 equal to 10 or 11 "); |
| return FAILURE; |
| } |
| break; |
| case WFD_SESSION_MGMT_CONTROL_PORT: |
| case WFD_DEVICE_THROUGHPUT: |
| if ((ISDIGIT(argv[0]) == 0) || (atoi(argv[0]) < 0) |
| || (atoi(argv[0]) > ((1 << 16) - 1))) { |
| printf("ERR:Paramater must be > 0 and < %d", |
| ((1 << 16) - 1)); |
| return FAILURE; |
| } |
| break; |
| case WFD_COUPLED_SINK: |
| /*Maximum value of coupled sink is 2 */ |
| if ((ISDIGIT(argv[0]) == 0) || (atoi(argv[0]) < 0) |
| || (atoi(argv[0]) > 2)) { |
| printf("ERR:Coupled sink paramater must be > 0 and < 3"); |
| return FAILURE; |
| } |
| break; |
| } |
| return SUCCESS; |
| } |
| |
| #define WFD_IE_HEADER_LEN 3 |
| /** |
| * @brief Wifi display update custom ie command |
| * |
| * $return N/A |
| */ |
| void |
| wifidisplay_update_custom_ie(int argc, char *argv[]) |
| { |
| t_s16 display_ie_index = -1; |
| t_u8 *buf; |
| int opt; |
| tlvbuf_custom_ie *tlv = NULL; |
| custom_ie *ie_ptr = NULL; |
| t_u16 len = 0, ie_len_wfd_org, ie_len_wfd, i = 0, new_wfd_dev_info = 0; |
| t_u8 *wfd_ptr; |
| while ((opt = getopt_long(argc, argv, "+", cmd_options, NULL)) != -1) { |
| switch (opt) { |
| default: |
| printf("ERR:Incorrect arguments.\n"); |
| return; |
| } |
| } |
| argc -= optind; |
| argv += optind; |
| /* Check arguments */ |
| if (argc < 2) { |
| printf("ERR:wrong arguments.\n"); |
| return; |
| } |
| if (argc == 3) { |
| if (ISDIGIT(argv[2]) == 0) { |
| printf("ERR:Illegal wfd mode %s. Must not be '0'.\n", |
| argv[2]); |
| return; |
| } |
| new_wfd_dev_info = (t_u16)atoi(argv[2]); |
| } |
| buf = (t_u8 *)malloc(MRVDRV_SIZE_OF_CMD_BUFFER); |
| if (!buf) { |
| printf("ERR:Cannot allocate memory!\n"); |
| return; |
| } |
| wifiDisplay_ie_config(&display_ie_index, 0, buf); |
| /* Clear WPS IE */ |
| if (display_ie_index < 0) { |
| free(buf); |
| return; |
| } |
| if (display_ie_index < (MAX_MGMT_IE_INDEX - 1)) { |
| tlv = (tlvbuf_custom_ie *)buf; |
| |
| if (tlv->tag == MRVL_MGMT_IE_LIST_TLV_ID) { |
| ie_ptr = (custom_ie *)(tlv->ie_data); |
| /* Goto appropriate Ie Index */ |
| for (i = 0; i < display_ie_index; i++) { |
| ie_ptr = (custom_ie *)((t_u8 *)ie_ptr + |
| sizeof(custom_ie) + |
| ie_ptr->ie_length); |
| } |
| ie_len_wfd_org = ie_ptr->ie_length; |
| wfd_ptr = ie_ptr->ie_buffer; |
| ie_len_wfd = *(wfd_ptr + 1); |
| wfd_ptr += sizeof(t_u8) + sizeof(t_u8); |
| wfd_ptr += (WIFIDISPLAY_OUI_LEN + WIFIDISPLAY_OUI_TYPE); |
| while (ie_len_wfd > WFD_IE_HEADER_LEN) { |
| memcpy(&len, wfd_ptr + 1, sizeof(t_u16)); |
| len = ntohs(len); |
| /* check capability type */ |
| if (*wfd_ptr == |
| TLV_TYPE_WIFIDISPLAY_DEVICE_INFO) { |
| tlvbuf_wfdisplay_device_info *wfd_tlv = |
| (tlvbuf_wfdisplay_device_info *) |
| wfd_ptr; |
| new_wfd_dev_info = |
| ntohs(new_wfd_dev_info); |
| memcpy((t_u8 *)&wfd_tlv-> |
| display_device_info, |
| (t_u8 *)&new_wfd_dev_info, |
| sizeof(t_u16)); |
| break; |
| } |
| wfd_ptr += len + WFD_IE_HEADER_LEN; |
| ie_len_wfd -= len + WFD_IE_HEADER_LEN; |
| } |
| /* Update New IE now */ |
| wifiDisplay_ie_config((t_s16 *)&display_ie_index, |
| ie_len_wfd_org, |
| (t_u8 *)ie_ptr - |
| sizeof(tlvbuf_custom_ie)); |
| } |
| } |
| return; |
| } |
| |
| /** |
| * @brief Wifi display update custom ie command |
| * |
| * $return N/A |
| */ |
| void |
| wifidisplay_update_coupledsink_bitmap(int argc, char *argv[]) |
| { |
| t_s16 display_ie_index = -1; |
| t_u8 *buf; |
| int opt; |
| tlvbuf_custom_ie *tlv = NULL; |
| custom_ie *ie_ptr = NULL; |
| t_u16 len = 0, ie_len_wfd_org, ie_len_wfd, i = 0; |
| t_u8 new_wfd_coupled_sink_bitmap = 0; |
| t_u8 *wfd_ptr; |
| while ((opt = getopt_long(argc, argv, "+", cmd_options, NULL)) != -1) { |
| switch (opt) { |
| default: |
| printf("ERR:Incorrect arguments.\n"); |
| return; |
| } |
| } |
| argc -= optind; |
| argv += optind; |
| /* Check arguments */ |
| if (argc < 2) { |
| printf("ERR:wrong arguments.\n"); |
| return; |
| } |
| if (argc == 3) { |
| if (ISDIGIT(argv[2]) == 0) { |
| printf("ERR:Illegal wfd mode %s. Must not be '0'.\n", |
| argv[2]); |
| return; |
| } |
| new_wfd_coupled_sink_bitmap = (t_u16)atoi(argv[2]); |
| } |
| buf = (t_u8 *)malloc(MRVDRV_SIZE_OF_CMD_BUFFER); |
| if (!buf) { |
| printf("ERR:Cannot allocate memory!\n"); |
| return; |
| } |
| wifiDisplay_ie_config(&display_ie_index, 0, buf); |
| /* Clear WPS IE */ |
| if (display_ie_index < 0) { |
| free(buf); |
| return; |
| } |
| if (display_ie_index < (MAX_MGMT_IE_INDEX - 1)) { |
| tlv = (tlvbuf_custom_ie *)buf; |
| |
| if (tlv->tag == MRVL_MGMT_IE_LIST_TLV_ID) { |
| ie_ptr = (custom_ie *)(tlv->ie_data); |
| /* Goto appropriate Ie Index */ |
| for (i = 0; i < display_ie_index; i++) { |
| ie_ptr = (custom_ie *)((t_u8 *)ie_ptr + |
| sizeof(custom_ie) + |
| ie_ptr->ie_length); |
| } |
| ie_len_wfd_org = ie_ptr->ie_length; |
| wfd_ptr = ie_ptr->ie_buffer; |
| ie_len_wfd = *(wfd_ptr + 1); |
| wfd_ptr += sizeof(t_u8) + sizeof(t_u8); |
| wfd_ptr += (WIFIDISPLAY_OUI_LEN + WIFIDISPLAY_OUI_TYPE); |
| while (ie_len_wfd > WFD_IE_HEADER_LEN) { |
| memcpy(&len, wfd_ptr + 1, sizeof(t_u16)); |
| len = ntohs(len); |
| /* check capability type */ |
| if (*wfd_ptr == |
| TLV_TYPE_WIFIDISPLAY_COUPLED_SINK) { |
| tlvbuf_wfdisplay_coupled_sink *wfd_tlv = |
| (tlvbuf_wfdisplay_coupled_sink |
| *)wfd_ptr; |
| wfd_tlv->coupled_sink = |
| new_wfd_coupled_sink_bitmap; |
| break; |
| } |
| wfd_ptr += len + WFD_IE_HEADER_LEN; |
| ie_len_wfd -= len + WFD_IE_HEADER_LEN; |
| } |
| /* Update New IE now */ |
| wifiDisplay_ie_config((t_s16 *)&display_ie_index, |
| ie_len_wfd_org, |
| (t_u8 *)ie_ptr - |
| sizeof(tlvbuf_custom_ie)); |
| } |
| } |
| free(buf); |
| return; |
| } |