Skip to content

Commit 35989c0

Browse files
abdelali221abdelali221
authored
WD : Add WD_GetVendorSpecificIE, Add WD_GetSecurity & Fix WD_GetRadioLevel. (#249)
* Check description. - Add WD_GetVendorSpecificIE. - Use WD Errors instead of values. - Fix Wd_GetRadioLevel. * Add WD_GetSecurity and related functions. * Use one function for both WPA and RSN IEs. * Get the thing to compile correctly. * Finalize WD_GetPCSList & WD_GetRSN_WPAEssentials
1 parent bb055c7 commit 35989c0

File tree

2 files changed

+263
-18
lines changed

2 files changed

+263
-18
lines changed

‎gc/ogc/wd.h‎

Lines changed: 57 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -58,15 +58,28 @@ enum WDIOCTLV
5858
IOCTLV_WD_RECV_NOTIFICATION = 0x8001 // WD_ReceiveNotification
5959
};
6060

61+
// Error Codes :
62+
63+
#define WD_SUCCESS 0
64+
#define WD_UINITIALIZED -1
65+
#define WD_INVALIDBUFF -2
66+
#define WD_BUFFTOOSMALL -3
67+
#define WD_NOTFOUND -4
68+
6169
// Capability flags :
6270

6371
#define CAPAB_SECURED_FLAG 0x10
6472

6573
// Information Elements IDs :
6674

6775
#define IEID_SSID 0x0
76+
#define IEID_COUNTRY 0x7
77+
#define IEID_SECURITY_RSN 0x30
6878
#define IEID_VENDORSPECIFIC 0xDD
69-
#define IEID_SECURITY 0x30
79+
80+
// OUI (Organization Unified ID) :
81+
82+
#define OUI_WPA 0x0050F201
7083

7184
// Signal Strength :
7285

@@ -148,6 +161,36 @@ typedef struct IE_hdr
148161
u8 len;
149162
} IE_hdr;
150163

164+
// Security :
165+
166+
#define WPA_OFFSET 4
167+
#define RSN_OFFSET 0
168+
169+
enum WD_SECURITY
170+
{
171+
WD_OPEN = 0x00,
172+
WD_WEP = 0x01,
173+
WD_WPA_TKIP = 0x02,
174+
WD_WPA2_AES = 0x04,
175+
WD_WPA_AES = 0x08,
176+
WD_WPA2_TKIP = 0x10,
177+
};
178+
179+
typedef struct IE_RSN_WPA
180+
{
181+
u16 Version;
182+
183+
u32 GDCS; // Group Data Cipher Suite
184+
185+
u16 PCS_Count; // Pairwise Cipher Suite
186+
187+
u16 AKMS_Count; // AKM Suite
188+
189+
u16 RSN_Capab;
190+
191+
u16 PMKID_Count;
192+
} IE_RSN_WPA;
193+
151194
// General Purpose :
152195

153196
s32 NCD_LockWirelessDriver();
@@ -161,9 +204,21 @@ int WD_GetInfo(WDInfo* inf);
161204
u8 WD_GetRadioLevel(BSSDescriptor* Bss);
162205
int WD_Scan(ScanParameters *settings, u8* buff, u16 buffsize);
163206
int WD_ScanOnce(ScanParameters *settings, u8* buff, u16 buffsize);
207+
void WD_SetDefaultScanParameters(ScanParameters* set);
208+
209+
// IE related :
210+
164211
u8 WD_GetNumberOfIEs(BSSDescriptor* Bss);
165212
int WD_GetIELength(BSSDescriptor* Bss, u8 ID);
166213
int WD_GetIE(BSSDescriptor* Bss, u8 ID, u8* buff, u8 buffsize);
167-
void WD_SetDefaultScanParameters(ScanParameters* set);
214+
int WD_GetIEIDList(BSSDescriptor* Bss, u8* buff, u8 buffsize);
215+
int WD_GetVendorSpecificIELength(BSSDescriptor* Bss, u32 OUI);
216+
int WD_GetVendorSpecificIE(BSSDescriptor* Bss, u32 OUI, u8* buff, u8 buffsize);
217+
218+
// AP Security related :
219+
220+
int WD_GetPCSList(BSSDescriptor *Bss, u8* buff, u8 buffsize, u8 offset);
221+
int WD_GetRSN_WPAEssentials(BSSDescriptor *Bss, IE_RSN_WPA *IE, u8 offset);
222+
u8 WD_GetSecurity(BSSDescriptor *Bss);
168223

169224
#endif

‎libogc/wd.c‎

Lines changed: 206 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -103,9 +103,9 @@ void WD_SetDefaultScanParameters(ScanParameters* set) {
103103
int WD_Init(u8 mode) {
104104
if(wd_fd < 0) {
105105
wd_fd = IOS_Open("/dev/net/wd/command", 0x10000 | mode);
106-
if (wd_fd < 0) return -1;
106+
if (wd_fd < 0) return WD_UINITIALIZED;
107107
}
108-
return 0;
108+
return WD_SUCCESS;
109109
}
110110

111111
void WD_Deinit() {
@@ -116,13 +116,13 @@ void WD_Deinit() {
116116
}
117117

118118
u8 WD_GetRadioLevel(BSSDescriptor* Bss) {
119-
if (Bss->RSSI >= 0xc4)
119+
if ((u8)Bss->RSSI >= 0xc4)
120120
return WD_SIGNAL_STRONG; // Strong
121121

122-
if (Bss->RSSI >= 0xb5)
122+
if ((u8)Bss->RSSI >= 0xb5)
123123
return WD_SIGNAL_NORMAL; // Normal
124124

125-
if (Bss->RSSI >= 0xab)
125+
if ((u8)Bss->RSSI >= 0xab)
126126
return WD_SIGNAL_FAIR; // Fair
127127

128128
return WD_SIGNAL_WEAK; // Weak
@@ -131,7 +131,7 @@ u8 WD_GetRadioLevel(BSSDescriptor* Bss) {
131131
int WD_GetInfo(WDInfo* info) {
132132
s32 lockid = NCD_LockWirelessDriver();
133133

134-
if(WD_Init(AOSSAPScan) < 0) return -1;
134+
if(WD_Init(AOSSAPScan) < 0) return WD_UINITIALIZED;
135135

136136
u8 inf[sizeof(WDInfo)] __attribute__((aligned(32)));
137137

@@ -145,11 +145,11 @@ int WD_GetInfo(WDInfo* info) {
145145
WD_Deinit();
146146
NCD_UnlockWirelessDriver(lockid);
147147

148-
return 0;
148+
return WD_SUCCESS;
149149
}
150150

151151
int WD_Scan(ScanParameters *settings, u8* buff, u16 buffsize) {
152-
if(wd_fd < 0) return -1;
152+
if(wd_fd < 0) return WD_UINITIALIZED;
153153

154154
u8 buf[buffsize + 2] __attribute__((aligned(32)));
155155
u8 settingsbuf[0x4e] __attribute__((aligned(32)));
@@ -168,20 +168,20 @@ int WD_Scan(ScanParameters *settings, u8* buff, u16 buffsize) {
168168
usleep(100000);
169169
memcpy(buff, buf, buffsize);
170170

171-
return 0;
171+
return WD_SUCCESS;
172172
}
173173

174174
int WD_ScanOnce(ScanParameters *settings, u8* buff, u16 buffsize) {
175175
s32 lockid = NCD_LockWirelessDriver();
176176

177-
if(WD_Init(AOSSAPScan) < 0) return -1;
177+
if(WD_Init(AOSSAPScan) < 0) return WD_UINITIALIZED;
178178

179179
WD_Scan(settings, buff, buffsize);
180180

181181
WD_Deinit();
182182
NCD_UnlockWirelessDriver(lockid);
183183

184-
return 0;
184+
return WD_SUCCESS;
185185
}
186186

187187
u8 WD_GetNumberOfIEs(BSSDescriptor* Bss) {
@@ -214,32 +214,222 @@ int WD_GetIELength(BSSDescriptor* Bss, u8 ID) {
214214
offset += hdr->len + sizeof(IE_hdr);
215215
}
216216

217-
if(hdr->ID != ID) return -1;
217+
if(hdr->ID != ID) return WD_NOTFOUND;
218218

219219
return hdr->len;
220220
}
221221

222222
int WD_GetIE(BSSDescriptor* Bss, u8 ID, u8* buff, u8 buffsize) {
223-
if(!buff) return -2;
223+
if(!buff) return WD_INVALIDBUFF;
224224

225225
u16 IEslen = Bss->IEs_length;
226226

227+
u8* ptr = (u8*)Bss;
228+
IE_hdr* hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor) + 1];
229+
u16 offset = 0;
230+
231+
while((offset + hdr->len) < IEslen)
232+
{
233+
hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor) + offset];
234+
if(hdr->ID == ID) break;
235+
offset += hdr->len + sizeof(IE_hdr);
236+
}
237+
238+
if(hdr->ID != ID) return WD_NOTFOUND;
239+
if(buffsize < WD_GetIELength(Bss, ID)) return WD_BUFFTOOSMALL;
240+
241+
memset(buff, 0, buffsize);
242+
memcpy(buff, &ptr[offset + sizeof(BSSDescriptor) + sizeof(IE_hdr)], hdr->len);
243+
244+
return WD_SUCCESS;
245+
}
246+
247+
int WD_GetIEIDList(BSSDescriptor* Bss, u8* buff, u8 buffsize) {
248+
if(!buff) return WD_INVALIDBUFF;
249+
if(buffsize < WD_GetNumberOfIEs(Bss)) return WD_BUFFTOOSMALL;
250+
251+
u8 n = 0;
252+
227253
u8* ptr = (u8*)Bss;
228254
IE_hdr* hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor)];
229255
u16 offset = 0;
230256

231-
while(hdr->ID != ID && (offset + hdr->len) < IEslen && hdr->len != 0)
257+
while(offset < Bss->IEs_length && hdr->len != 0)
258+
{
259+
buff[n] = hdr->ID;
260+
hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor) + offset];
261+
offset += hdr->len + sizeof(IE_hdr);
262+
n++;
263+
}
264+
265+
return WD_SUCCESS;
266+
}
267+
268+
int WD_GetVendorSpecificIE(BSSDescriptor* Bss, u32 OUI, u8* buff, u8 buffsize) {
269+
if(!buff) return WD_INVALIDBUFF;
270+
u16 IEslen = Bss->IEs_length;
271+
272+
u8* ptr = (u8*)Bss;
273+
IE_hdr* hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor)];
274+
u16 offset = 0;
275+
276+
u32 tgtOUI = 0;
277+
278+
while((offset + hdr->len) < IEslen && hdr->len != 0)
232279
{
233280
hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor) + offset];
281+
tgtOUI = ptr[sizeof(BSSDescriptor) + offset + 2] << 24 |
282+
ptr[sizeof(BSSDescriptor) + offset + 3] << 16 |
283+
ptr[sizeof(BSSDescriptor) + offset + 4] << 8 |
284+
ptr[sizeof(BSSDescriptor) + offset + 5];
285+
if (hdr->ID == IEID_VENDORSPECIFIC && tgtOUI == OUI) break;
234286
offset += hdr->len + sizeof(IE_hdr);
235287
}
236288

237-
if(hdr->ID != ID) return -1;
289+
if(hdr->ID != IEID_VENDORSPECIFIC ||
290+
tgtOUI != OUI) return WD_NOTFOUND;
291+
if(buffsize < hdr->len) return WD_BUFFTOOSMALL;
238292

239293
memset(buff, 0, buffsize);
240294
memcpy(buff, &ptr[offset + sizeof(BSSDescriptor) + sizeof(IE_hdr)], hdr->len);
241295

242-
return 0;
296+
return WD_SUCCESS;
297+
}
298+
299+
int WD_GetVendorSpecificIELength(BSSDescriptor* Bss, u32 OUI) {
300+
u16 IEslen = Bss->IEs_length;
301+
302+
u8* ptr = (u8*)Bss;
303+
IE_hdr* hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor)];
304+
u16 offset = 0;
305+
306+
u32 tgtOUI = 0;
307+
308+
while((offset + hdr->len) < IEslen && hdr->len != 0)
309+
{
310+
hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor) + offset];
311+
tgtOUI = ptr[sizeof(BSSDescriptor) + offset + 2] << 24 |
312+
ptr[sizeof(BSSDescriptor) + offset + 3] << 16 |
313+
ptr[sizeof(BSSDescriptor) + offset + 4] << 8 |
314+
ptr[sizeof(BSSDescriptor) + offset + 5];
315+
if (hdr->ID == IEID_VENDORSPECIFIC && tgtOUI == OUI) break;
316+
offset += hdr->len + sizeof(IE_hdr);
317+
}
318+
319+
if(hdr->ID != IEID_VENDORSPECIFIC ||
320+
tgtOUI != OUI) return WD_NOTFOUND;
321+
322+
return hdr->len;
323+
}
324+
325+
int WD_GetPCSList(BSSDescriptor *Bss, u8* destbuff, u8 buffsize, u8 offset) {
326+
if(!Bss) return WD_INVALIDBUFF;
327+
if(!destbuff) return WD_INVALIDBUFF;
328+
329+
IE_RSN_WPA IE;
330+
331+
int ret = WD_GetRSN_WPAEssentials(Bss, &IE, offset);
332+
333+
if(ret < 0) return WD_INVALIDBUFF;
334+
if(IE.PCS_Count * 4 > buffsize) return WD_BUFFTOOSMALL;
335+
336+
u8 IE_len;
337+
if(offset == RSN_OFFSET) {
338+
IE_len = WD_GetIELength(Bss, IEID_SECURITY_RSN);
339+
} else {
340+
IE_len = WD_GetVendorSpecificIELength(Bss, OUI_WPA);
341+
}
342+
343+
344+
u8 buff[IE_len];
345+
if(offset == RSN_OFFSET) {
346+
WD_GetIE(Bss, IEID_SECURITY_RSN, buff, IE_len);
347+
} else {
348+
WD_GetVendorSpecificIE(Bss, OUI_WPA, buff, IE_len);
349+
}
350+
351+
352+
memset(destbuff, 0, buffsize);
353+
memcpy(destbuff, &buff[8 + offset], IE.PCS_Count * 4);
354+
355+
return WD_SUCCESS;
356+
}
357+
358+
int WD_GetRSN_WPAEssentials(BSSDescriptor *Bss, IE_RSN_WPA *IE, u8 offset) {
359+
if(!Bss) return WD_INVALIDBUFF;
360+
if(!IE) return WD_INVALIDBUFF;
361+
362+
u8 IE_len;
363+
if(offset == RSN_OFFSET) {
364+
IE_len = WD_GetIELength(Bss, IEID_SECURITY_RSN);
365+
} else {
366+
IE_len = WD_GetVendorSpecificIELength(Bss, OUI_WPA);
367+
}
368+
if(IE_len < 0) return WD_NOTFOUND;
369+
370+
u8 buff[IE_len];
371+
372+
if(offset == RSN_OFFSET) {
373+
WD_GetIE(Bss, IEID_SECURITY_RSN, buff, IE_len);
374+
} else {
375+
WD_GetVendorSpecificIE(Bss, OUI_WPA, buff, IE_len);
376+
}
377+
378+
IE->Version = buff[0 + offset] | buff[1 + offset] << 8;
379+
offset += 2;
380+
IE->GDCS = buff[0 + offset] << 24 | buff[1 + offset] << 16 | buff[2 + offset] << 8 | buff[3 + offset];
381+
offset += 4;
382+
IE->PCS_Count = buff[0 + offset] | buff[1 + offset] << 8;
383+
offset += 2 + IE->PCS_Count * 4;
384+
IE->AKMS_Count = buff[0 + offset] | buff[1 + offset] << 8;
385+
offset += 2 + IE->AKMS_Count * 4;
386+
387+
return WD_SUCCESS;
388+
}
389+
390+
u8 WD_GetSecurity(BSSDescriptor *Bss) {
391+
if(!Bss) return WD_INVALIDBUFF;
392+
if(!(Bss->Capabilities & CAPAB_SECURED_FLAG)) return WD_OPEN;
393+
394+
int ie_len = WD_GetVendorSpecificIELength(Bss, OUI_WPA);
395+
u8 ret = 0;
396+
397+
if (ie_len != WD_NOTFOUND && ie_len > 0) { // WPA
398+
IE_RSN_WPA IE;
399+
WD_GetRSN_WPAEssentials(Bss, &IE, WPA_OFFSET);
400+
401+
u8 buff[IE.PCS_Count * 4];
402+
WD_GetPCSList(Bss, buff, IE.PCS_Count * 4, WPA_OFFSET);
403+
404+
u8 offset = 0;
405+
406+
for (int i = 0; i < IE.PCS_Count; i++) {
407+
if (buff[offset + 3] == 0x02) ret |= WD_WPA_TKIP;
408+
else if (buff[offset + 3] == 0x04) ret |= WD_WPA_AES;
409+
offset += 4;
410+
}
411+
}
412+
413+
ie_len = WD_GetIELength(Bss, IEID_SECURITY_RSN);
414+
415+
if(ie_len != WD_NOTFOUND && ie_len > 0) { // WPA2
416+
IE_RSN_WPA IE;
417+
WD_GetRSN_WPAEssentials(Bss, &IE, RSN_OFFSET);
418+
419+
u8 buff[IE.PCS_Count * 4];
420+
WD_GetPCSList(Bss, buff, IE.PCS_Count * 4, RSN_OFFSET);
421+
u8 offset = 0;
422+
423+
for (int i = 0; i < IE.PCS_Count; i++) {
424+
if (buff[offset + 3] == 0x02) ret |= WD_WPA2_TKIP;
425+
if (buff[offset + 3] == 0x04) ret |= WD_WPA2_AES;
426+
offset += 4;
427+
}
428+
}
429+
430+
if(!ret) return WD_WEP;
431+
432+
return ret;
243433
}
244434

245435
#endif

0 commit comments

Comments
 (0)