1 /*
2 * Copyright (c) 2015 South Silicon Valley Microelectronics Inc.
3 * Copyright (c) 2015 iComm Corporation
4 *
5 * This program is free software: you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation, either version 3 of the License, or
8 * (at your option) any later version.
9 * This program is distributed in the hope that it will be useful, but
10 * WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
12 * See the GNU General Public License for more details.
13 * You should have received a copy of the GNU General Public License
14 * along with this program. If not, see <http://www.gnu.org/licenses/>.
15 */
16
17 #include <ssv6200.h>
18 #include <linux/types.h>
19 #include <linux/nl80211.h>
20 #include <net/mac80211.h>
21 #include <linux/nl80211.h>
22 #include <linux/etherdevice.h>
23 #include <linux/delay.h>
24 #include <linux/version.h>
25 #include <linux/time.h>
26 #include <linux/kthread.h>
27 #include <net/mac80211.h>
28 #include <ssv6200.h>
29 #include "p2p.h"
30 #include "dev.h"
31 #include "lib.h"
32 #ifdef CONFIG_P2P_NOA
33 #define P2P_IE_VENDOR_TYPE 0x506f9a09
34 #define P2P_NOA_DETECT_INTERVAL (5 * HZ)
35 #ifndef MAC2STR
36 #define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
37 #define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
38 #define COMPACT_MACSTR "%02x%02x%02x%02x%02x%02x"
39 #endif
40 void ssv6xxx_send_noa_cmd(struct ssv_softc *sc, struct ssv6xxx_p2p_noa_param *p2p_noa_param);
WPA_GET_BE32(const u8 * a)41 static inline u32 WPA_GET_BE32(const u8 *a)
42 {
43 return (a[0] << 24) | (a[1] << 16) | (a[2] << 8) | a[3];
44 }
WPA_GET_LE16(const u8 * a)45 static inline u16 WPA_GET_LE16(const u8 *a)
46 {
47 return (a[1] << 8) | a[0];
48 }
WPA_GET_LE32(const u8 * a)49 static inline u32 WPA_GET_LE32(const u8 *a)
50 {
51 return (a[3] << 24) |(a[2] << 16) |(a[1] << 8) | a[0];
52 }
53 #define IEEE80211_HDRLEN 24
54 enum p2p_attr_id {
55 P2P_ATTR_STATUS = 0,
56 P2P_ATTR_MINOR_REASON_CODE = 1,
57 P2P_ATTR_CAPABILITY = 2,
58 P2P_ATTR_DEVICE_ID = 3,
59 P2P_ATTR_GROUP_OWNER_INTENT = 4,
60 P2P_ATTR_CONFIGURATION_TIMEOUT = 5,
61 P2P_ATTR_LISTEN_CHANNEL = 6,
62 P2P_ATTR_GROUP_BSSID = 7,
63 P2P_ATTR_EXT_LISTEN_TIMING = 8,
64 P2P_ATTR_INTENDED_INTERFACE_ADDR = 9,
65 P2P_ATTR_MANAGEABILITY = 10,
66 P2P_ATTR_CHANNEL_LIST = 11,
67 P2P_ATTR_NOTICE_OF_ABSENCE = 12,
68 P2P_ATTR_DEVICE_INFO = 13,
69 P2P_ATTR_GROUP_INFO = 14,
70 P2P_ATTR_GROUP_ID = 15,
71 P2P_ATTR_INTERFACE = 16,
72 P2P_ATTR_OPERATING_CHANNEL = 17,
73 P2P_ATTR_INVITATION_FLAGS = 18,
74 P2P_ATTR_OOB_GO_NEG_CHANNEL = 19,
75 P2P_ATTR_VENDOR_SPECIFIC = 221
76 };
77 struct ssv6xxx_p2p_noa_attribute {
78 u8 index;
79 u16 ctwindows_oppps;
80 struct ssv6xxx_p2p_noa_param noa_param;
81 };
82 extern void _ssv6xxx_hexdump(const char *title, const u8 *buf,
83 size_t len);
p2p_find_noa(const u8 * ies,struct ssv6xxx_p2p_noa_attribute * noa_attr)84 bool p2p_find_noa(const u8 *ies, struct ssv6xxx_p2p_noa_attribute *noa_attr)
85 {
86 const u8 *end, *pos, *ie;
87 u32 len;
88 len = ie[1] - 4;
89 pos = ie + 6;
90 end = pos+len;
91 while (pos < end) {
92 u16 attr_len;
93 if (pos + 2 >= end) {
94 return false;
95 }
96 attr_len = WPA_GET_LE16(pos + 1);
97 if (pos + 3 + attr_len > end) {
98 return false;
99 }
100 if(pos[0] != P2P_ATTR_NOTICE_OF_ABSENCE){
101 pos += 3 + attr_len;
102 continue;
103 }
104 if(attr_len<15){
105 printk("*********************NOA descriptor does not exist len[%d]\n", attr_len);
106 break;
107 }
108 if(attr_len>15)
109 printk("More than one NOA descriptor\n");
110 noa_attr->index = pos[3];
111 noa_attr->ctwindows_oppps = pos[4];
112 noa_attr->noa_param.count = pos[5];
113 noa_attr->noa_param.duration = WPA_GET_LE32(&pos[6]);
114 noa_attr->noa_param.interval = WPA_GET_LE32(&pos[10]);
115 noa_attr->noa_param.start_time = WPA_GET_LE32(&pos[14]);
116 return true;
117 }
118 return false;
119 }
p2p_get_attribute_noa(const u8 * ies,u32 oui_type,struct ssv6xxx_p2p_noa_attribute * noa_attr)120 bool p2p_get_attribute_noa(const u8 *ies, u32 oui_type, struct ssv6xxx_p2p_noa_attribute *noa_attr)
121 {
122 const u8 *end, *pos, *ie;
123 u32 len;
124 pos = ies;
125 end = ies + ies_len;
126 ie = NULL;
127 while (pos + 1 < end) {
128 if (pos + 2 + pos[1] > end)
129 return false;
130 if (pos[0] == WLAN_EID_VENDOR_SPECIFIC && pos[1] >= 4 &&
131 WPA_GET_BE32(&pos[2]) == oui_type) {
132 ie = pos;
133 if(p2p_find_noa(ie, 0, noa_attr) == true)
134 return true;
135 }
136 pos += 2 + pos[1];
137 }
138 return false;
139 }
ssv6xxx_process_noa_event(struct ssv_softc * sc,struct sk_buff * skb)140 void ssv6xxx_process_noa_event(struct ssv_softc *sc, struct sk_buff *skb)
141 {
142 struct cfg_host_event *host_event;
143 struct ssv62xx_noa_evt *noa_evt;
144 host_event = (struct cfg_host_event *)skb->data;
145 noa_evt= (struct ssv62xx_noa_evt *)&host_event->dat[0];
146 switch(noa_evt->evt_id){
147 case SSV6XXX_NOA_START:
148 sc->p2p_noa.active_noa_vif |= (1<<noa_evt->vif);
149 printk("SSV6XXX_NOA_START===>[%08x]\n", sc->p2p_noa.active_noa_vif);
150 break;
151 case SSV6XXX_NOA_STOP:
152 sc->p2p_noa.active_noa_vif &= ~(1<<noa_evt->vif);
153 printk("SSV6XXX_NOA_STOP===>[%08x]\n", sc->p2p_noa.active_noa_vif);
154 break;
155 default:
156 printk("--------->NOA wrong command<---------\n");
157 break;
158 }
159 }
ssv6xxx_noa_reset(struct ssv_softc * sc)160 void ssv6xxx_noa_reset(struct ssv_softc *sc){
161 unsigned long flags;
162 printk("Reset NOA param...\n");
163 spin_lock_irqsave(&sc->p2p_noa.p2p_config_lock, flags);
164 memset(&sc->p2p_noa.noa_detect, 0, sizeof(struct ssv_p2p_noa_detect)*SSV_NUM_VIF);
165 sc->p2p_noa.active_noa_vif = 0;
166 sc->p2p_noa.monitor_noa_vif = 0;
167 spin_unlock_irqrestore(&sc->p2p_noa.p2p_config_lock, flags);
168 }
ssv6xxx_noa_host_stop_noa(struct ssv_softc * sc,u8 vif_id)169 void ssv6xxx_noa_host_stop_noa(struct ssv_softc *sc, u8 vif_id){
170 struct ssv6xxx_p2p_noa_attribute noa_attr;
171 if(sc->p2p_noa.noa_detect[vif_id].p2p_noa_index>=0){
172 sc->p2p_noa.noa_detect[vif_id].p2p_noa_index = -1;
173 sc->p2p_noa.active_noa_vif &= ~(1<<vif_id);
174 memset(&sc->p2p_noa.noa_detect[vif_id].noa_param_cmd, 0, sizeof(struct ssv6xxx_p2p_noa_param));
175 printk("->remove NOA operating vif[%d]\n", vif_id);
176 noa_attr.noa_param.enable = 0;
177 noa_attr.noa_param.vif_id = vif_id;
178 ssv6xxx_send_noa_cmd(sc, &noa_attr.noa_param);
179 }
180 }
ssv6xxx_noa_detect(struct ssv_softc * sc,struct ieee80211_hdr * hdr,u32 len)181 void ssv6xxx_noa_detect(struct ssv_softc *sc, struct ieee80211_hdr * hdr, u32 len){
182 int i;
183 unsigned long flags;
184 struct ieee80211_mgmt * mgmt =(struct ieee80211_mgmt *)hdr;
185 struct ssv6xxx_p2p_noa_attribute noa_attr;
186 spin_lock_irqsave(&sc->p2p_noa.p2p_config_lock, flags);
187 if(sc->p2p_noa.monitor_noa_vif == 0)
188 goto out;
189 for(i=0;i<SSV_NUM_VIF;i++){
190 if(sc->p2p_noa.noa_detect[i].noa_addr == NULL)
191 continue;
192 if(memcmp(mgmt->bssid, sc->p2p_noa.noa_detect[i].noa_addr, 6) != 0)
193 continue;
194 if(sc->p2p_noa.active_noa_vif &&
195 ((sc->p2p_noa.active_noa_vif & 1<<i) == 0))
196 continue;
197 sc->p2p_noa.noa_detect[i].last_rx = jiffies;
198 if(p2p_get_attribute_noa((const u8*)mgmt->u.beacon.variable,
199 len - (IEEE80211_HDRLEN + sizeof(mgmt->u.beacon)),
200 P2P_IE_VENDOR_TYPE,
201 &noa_attr)== false){
202 continue;
203 }
204 if(sc->p2p_noa.noa_detect[i].p2p_noa_index == noa_attr.index){
205 goto out;
206 }
207 printk(MACSTR"->set NOA element\n", MAC2STR(mgmt->bssid));
208 sc->p2p_noa.active_noa_vif |= (1<<i);
209 sc->p2p_noa.noa_detect[i].p2p_noa_index = noa_attr.index;
210 memcpy(&sc->p2p_noa.noa_detect[i].noa_param_cmd, &noa_attr.noa_param, sizeof(struct ssv6xxx_p2p_noa_param));
211 noa_attr.noa_param.enable = 1;
212 noa_attr.noa_param.vif_id = i;
213 memcpy(noa_attr.noa_param.addr, hdr->addr2, 6);
214 ssv6xxx_send_noa_cmd(sc, &noa_attr.noa_param);
215 }
216 out:
217 spin_unlock_irqrestore(&sc->p2p_noa.p2p_config_lock, flags);
218 }
ssv6xxx_noa_hdl_bss_change(struct ssv_softc * sc,enum ssv6xxx_noa_conf conf,u8 vif_idx)219 void ssv6xxx_noa_hdl_bss_change(struct ssv_softc *sc, enum ssv6xxx_noa_conf conf, u8 vif_idx){
220 unsigned long flags;
221 if(sc->vif_info[vif_idx].vif->type != NL80211_IFTYPE_STATION ||
222 sc->vif_info[vif_idx].vif->p2p != true)
223 return;
224 spin_lock_irqsave(&sc->p2p_noa.p2p_config_lock, flags);
225 printk("====>[NOA]ssv6xxx_noa_hdl_bss_change conf[%d] vif_idx[%d]\n", conf, vif_idx);
226 switch(conf)
227 {
228 case MONITOR_NOA_CONF_ADD:
229 memset(&sc->p2p_noa.noa_detect[vif_idx], 0, sizeof(struct ssv_p2p_noa_detect));
230 sc->p2p_noa.noa_detect[vif_idx].noa_addr = sc->vif_info[vif_idx].vif->bss_conf.bssid;
231 sc->p2p_noa.noa_detect[vif_idx].p2p_noa_index = -1;
232 sc->p2p_noa.noa_detect[vif_idx].last_rx = jiffies;
233 sc->p2p_noa.monitor_noa_vif |= 1<< vif_idx;
234 break;
235 case MONITOR_NOA_CONF_REMOVE:
236 sc->p2p_noa.monitor_noa_vif &= ~(1<< vif_idx);
237 sc->p2p_noa.noa_detect[vif_idx].noa_addr = NULL;
238 ssv6xxx_noa_host_stop_noa(sc, vif_idx);
239 break;
240 default:
241 break;
242 }
243 spin_unlock_irqrestore(&sc->p2p_noa.p2p_config_lock, flags);
244 }
ssv6xxx_send_noa_cmd(struct ssv_softc * sc,struct ssv6xxx_p2p_noa_param * p2p_noa_param)245 void ssv6xxx_send_noa_cmd(struct ssv_softc *sc, struct ssv6xxx_p2p_noa_param *p2p_noa_param)
246 {
247 struct sk_buff *skb;
248 struct cfg_host_cmd *host_cmd;
249 int retry_cnt = 5;
250 skb = ssv_skb_alloc(HOST_CMD_HDR_LEN + sizeof(struct ssv6xxx_p2p_noa_param));
251 skb->data_len = HOST_CMD_HDR_LEN + sizeof(struct ssv6xxx_p2p_noa_param);
252 skb->len = skb->data_len;
253 host_cmd = (struct cfg_host_cmd *)skb->data;
254 host_cmd->c_type = HOST_CMD;
255 host_cmd->h_cmd = (u8)SSV6XXX_HOST_CMD_SET_NOA;
256 host_cmd->len = skb->data_len;
257 memcpy(host_cmd->dat32, p2p_noa_param, sizeof(struct ssv6xxx_p2p_noa_param));
258 printk("Noa cmd NOA Parameter:\nEnable=%d\nInterval=%d\nDuration=%d\nStart_time=0x%08x\nCount=%d\nAddr=[%02x:%02x:%02x:%02x:%02x:%02x]vif[%d]\n\n",
259 p2p_noa_param->enable,
260 p2p_noa_param->interval,
261 p2p_noa_param->duration,
262 p2p_noa_param->start_time,
263 p2p_noa_param->count,
264 p2p_noa_param->addr[0],
265 p2p_noa_param->addr[1],
266 p2p_noa_param->addr[2],
267 p2p_noa_param->addr[3],
268 p2p_noa_param->addr[4],
269 p2p_noa_param->addr[5],
270 p2p_noa_param->vif_id);
271 while((HCI_SEND_CMD(sc->sh, skb)!=0)&&(retry_cnt)){
272 printk(KERN_INFO "NOA cmd retry=%d!!\n",retry_cnt);
273 retry_cnt--;
274 }
275 ssv_skb_free(skb);
276 }
277 #if 0
278 void ssv6200_cmd_work(struct work_struct *work)
279 {
280 struct ssv_softc *sc =
281 container_of(work, struct ssv_softc, cmd_work);
282 struct sk_buff *skb;
283 bool cmd_mode;
284 int retry_cnt = 5;
285 do{
286 if(sc->cmd_info.state == SSC_CMD_STATE_IDLE){
287 struct cfg_host_cmd *host_cmd;
288 skb = skb_peek(&sc->cmd_info.cmd_que);
289 if(skb == NULL)
290 break;
291 host_cmd = (struct cfg_host_cmd *)skb->data;
292 while((HCI_SEND_CMD(sc->sh, skb)!=0)&&(retry_cnt)){
293 printk(KERN_INFO "cmd retry=%d!!\n",retry_cnt);
294 retry_cnt--;
295 }
296 if(retry_cnt)
297 sc->cmd_info.state = SSC_CMD_STATE_WAIT_RSP;
298 }else{
299 skb = skb_dequeue(&sc->cmd_info.evt_que);
300 if(skb == NULL)
301 break;
302 }
303 }while(1);
304 }
305 #endif
306 #endif
307