diff --git a/gc/ogc/wd.h b/gc/ogc/wd.h index 6f9a2578..85f6d569 100644 --- a/gc/ogc/wd.h +++ b/gc/ogc/wd.h @@ -58,6 +58,14 @@ enum WDIOCTLV IOCTLV_WD_RECV_NOTIFICATION = 0x8001 // WD_ReceiveNotification }; +// Error Codes : + +#define WD_SUCCESS 0 +#define WD_UINITIALIZED -1 +#define WD_INVALIDBUFF -2 +#define WD_BUFFTOOSMALL -3 +#define WD_NOTFOUND -4 + // Capability flags : #define CAPAB_SECURED_FLAG 0x10 @@ -65,8 +73,13 @@ enum WDIOCTLV // Information Elements IDs : #define IEID_SSID 0x0 +#define IEID_COUNTRY 0x7 +#define IEID_SECURITY_RSN 0x30 #define IEID_VENDORSPECIFIC 0xDD -#define IEID_SECURITY 0x30 + +// OUI (Organization Unified ID) : + +#define OUI_WPA 0x0050F201 // Signal Strength : @@ -148,6 +161,44 @@ typedef struct IE_hdr u8 len; } IE_hdr; +// Security : + +enum WD_SECURITY +{ + WD_OPEN = 0x00, + WD_WEP = 0x01, + WD_WPA_TKIP = 0x02, + WD_WPA2_AES = 0x03, + WD_WPA_AES = 0x04, + WD_WPA2_TKIP = 0x05, +}; + +typedef struct IE_RSN +{ + u16 Version; + + u32 GDCS; // Group Data Cipher Suite + + u16 PCS_Count; // Pairwise Cipher Suite + + u16 AKMS_Count; // AKM Suite + + u16 RSN_Capab; + + u16 PMKID_Count; +} IE_RSN; + +typedef struct IE_WPA +{ + u16 Version; + + u32 GDCS; // Group Data Cipher Suite + + u16 PCS_Count; // Pairwise Cipher Suite + + u16 AKMS_Count; // AKM Suite +} IE_WPA; + // General Purpose : s32 NCD_LockWirelessDriver(); @@ -161,9 +212,23 @@ int WD_GetInfo(WDInfo* inf); u8 WD_GetRadioLevel(BSSDescriptor* Bss); int WD_Scan(ScanParameters *settings, u8* buff, u16 buffsize); int WD_ScanOnce(ScanParameters *settings, u8* buff, u16 buffsize); +void WD_SetDefaultScanParameters(ScanParameters* set); + +// IE related : + u8 WD_GetNumberOfIEs(BSSDescriptor* Bss); int WD_GetIELength(BSSDescriptor* Bss, u8 ID); int WD_GetIE(BSSDescriptor* Bss, u8 ID, u8* buff, u8 buffsize); -void WD_SetDefaultScanParameters(ScanParameters* set); +int WD_GetIEIDList(BSSDescriptor* Bss, u8* buff, u8 buffsize); +int WD_GetVendorSpecificIELength(BSSDescriptor* Bss, u32 OUI); +int WD_GetVendorSpecificIE(BSSDescriptor* Bss, u32 OUI, u8* buff, u8 buffsize); + +// AP Security related : + +int WD_GetRSNEssentials(BSSDescriptor *Bss, IE_RSN *IE); +int WD_GetRSNPCSList(BSSDescriptor *Bss, u8* buff, u8 buffsize); +int WD_GetWPAIEEssentials(BSSDescriptor *Bss, IE_WPA *IE); +int WD_GetWPA_PCSList(BSSDescriptor *Bss, u8* destbuff, u16 buffsize); +u8 WD_GetSecurity(BSSDescriptor *Bss); #endif \ No newline at end of file diff --git a/libogc/wd.c b/libogc/wd.c index 5d7a00b5..bb3a7a20 100644 --- a/libogc/wd.c +++ b/libogc/wd.c @@ -103,9 +103,9 @@ void WD_SetDefaultScanParameters(ScanParameters* set) { int WD_Init(u8 mode) { if(wd_fd < 0) { wd_fd = IOS_Open("/dev/net/wd/command", 0x10000 | mode); - if (wd_fd < 0) return -1; + if (wd_fd < 0) return WD_UINITIALIZED; } - return 0; + return WD_SUCCESS; } void WD_Deinit() { @@ -116,13 +116,13 @@ void WD_Deinit() { } u8 WD_GetRadioLevel(BSSDescriptor* Bss) { - if (Bss->RSSI >= 0xc4) + if ((u8)Bss->RSSI >= 0xc4) return WD_SIGNAL_STRONG; // Strong - if (Bss->RSSI >= 0xb5) + if ((u8)Bss->RSSI >= 0xb5) return WD_SIGNAL_NORMAL; // Normal - if (Bss->RSSI >= 0xab) + if ((u8)Bss->RSSI >= 0xab) return WD_SIGNAL_FAIR; // Fair return WD_SIGNAL_WEAK; // Weak @@ -131,7 +131,7 @@ u8 WD_GetRadioLevel(BSSDescriptor* Bss) { int WD_GetInfo(WDInfo* info) { s32 lockid = NCD_LockWirelessDriver(); - if(WD_Init(AOSSAPScan) < 0) return -1; + if(WD_Init(AOSSAPScan) < 0) return WD_UINITIALIZED; u8 inf[sizeof(WDInfo)] __attribute__((aligned(32))); @@ -145,11 +145,11 @@ int WD_GetInfo(WDInfo* info) { WD_Deinit(); NCD_UnlockWirelessDriver(lockid); - return 0; + return WD_SUCCESS; } int WD_Scan(ScanParameters *settings, u8* buff, u16 buffsize) { - if(wd_fd < 0) return -1; + if(wd_fd < 0) return WD_UINITIALIZED; u8 buf[buffsize + 2] __attribute__((aligned(32))); u8 settingsbuf[0x4e] __attribute__((aligned(32))); @@ -168,20 +168,20 @@ int WD_Scan(ScanParameters *settings, u8* buff, u16 buffsize) { usleep(100000); memcpy(buff, buf, buffsize); - return 0; + return WD_SUCCESS; } int WD_ScanOnce(ScanParameters *settings, u8* buff, u16 buffsize) { s32 lockid = NCD_LockWirelessDriver(); - if(WD_Init(AOSSAPScan) < 0) return -1; + if(WD_Init(AOSSAPScan) < 0) return WD_UINITIALIZED; WD_Scan(settings, buff, buffsize); WD_Deinit(); NCD_UnlockWirelessDriver(lockid); - return 0; + return WD_SUCCESS; } u8 WD_GetNumberOfIEs(BSSDescriptor* Bss) { @@ -214,32 +214,249 @@ int WD_GetIELength(BSSDescriptor* Bss, u8 ID) { offset += hdr->len + sizeof(IE_hdr); } - if(hdr->ID != ID) return -1; + if(hdr->ID != ID) return WD_NOTFOUND; return hdr->len; } int WD_GetIE(BSSDescriptor* Bss, u8 ID, u8* buff, u8 buffsize) { - if(!buff) return -2; + if(!buff) return WD_INVALIDBUFF; u16 IEslen = Bss->IEs_length; + u8* ptr = (u8*)Bss; + IE_hdr* hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor) + 1]; + u16 offset = 0; + + while((offset + hdr->len) < IEslen) + { + hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor) + offset]; + if(hdr->ID == ID) break; + offset += hdr->len + sizeof(IE_hdr); + } + + if(hdr->ID != ID) return WD_NOTFOUND; + if(buffsize < WD_GetIELength(Bss, ID)) return WD_BUFFTOOSMALL; + + memset(buff, 0, buffsize); + memcpy(buff, &ptr[offset + sizeof(BSSDescriptor) + sizeof(IE_hdr)], hdr->len); + + return WD_SUCCESS; +} + +int WD_GetIEIDList(BSSDescriptor* Bss, u8* buff, u8 buffsize) { + if(!buff) return WD_INVALIDBUFF; + if(buffsize < WD_GetNumberOfIEs(Bss)) return WD_BUFFTOOSMALL; + + u8 n = 0; + u8* ptr = (u8*)Bss; IE_hdr* hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor)]; u16 offset = 0; - while(hdr->ID != ID && (offset + hdr->len) < IEslen && hdr->len != 0) + while(offset < Bss->IEs_length && hdr->len != 0) { + buff[n] = hdr->ID; hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor) + offset]; offset += hdr->len + sizeof(IE_hdr); + n++; } - if(hdr->ID != ID) return -1; + return WD_SUCCESS; +} + +int WD_GetVendorSpecificIE(BSSDescriptor* Bss, u32 OUI, u8* buff, u8 buffsize) { + if(!buff) return WD_INVALIDBUFF; + u16 IEslen = Bss->IEs_length; + + u8* ptr = (u8*)Bss; + IE_hdr* hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor)]; + u16 offset = 0; + + u32 tgtOUI = 0; + + while((offset + hdr->len) < IEslen && hdr->len != 0) + { + hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor) + offset]; + tgtOUI = ptr[sizeof(BSSDescriptor) + offset + 2] << 24 | + ptr[sizeof(BSSDescriptor) + offset + 3] << 16 | + ptr[sizeof(BSSDescriptor) + offset + 4] << 8 | + ptr[sizeof(BSSDescriptor) + offset + 5]; + if (hdr->ID == IEID_VENDORSPECIFIC && tgtOUI == OUI) break; + offset += hdr->len + sizeof(IE_hdr); + } + + if(hdr->ID != IEID_VENDORSPECIFIC || + tgtOUI != OUI) return WD_NOTFOUND; + if(buffsize < hdr->len) return WD_BUFFTOOSMALL; memset(buff, 0, buffsize); memcpy(buff, &ptr[offset + sizeof(BSSDescriptor) + sizeof(IE_hdr)], hdr->len); - return 0; + return WD_SUCCESS; +} + +int WD_GetVendorSpecificIELength(BSSDescriptor* Bss, u32 OUI) { + u16 IEslen = Bss->IEs_length; + + u8* ptr = (u8*)Bss; + IE_hdr* hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor)]; + u16 offset = 0; + + u32 tgtOUI = 0; + + while((offset + hdr->len) < IEslen && hdr->len != 0) + { + hdr = (IE_hdr*)&ptr[sizeof(BSSDescriptor) + offset]; + tgtOUI = ptr[sizeof(BSSDescriptor) + offset + 2] << 24 | + ptr[sizeof(BSSDescriptor) + offset + 3] << 16 | + ptr[sizeof(BSSDescriptor) + offset + 4] << 8 | + ptr[sizeof(BSSDescriptor) + offset + 5]; + if (hdr->ID == IEID_VENDORSPECIFIC && tgtOUI == OUI) break; + offset += hdr->len + sizeof(IE_hdr); + } + + if(hdr->ID != IEID_VENDORSPECIFIC || + tgtOUI != OUI) return WD_NOTFOUND; + + return hdr->len; +} + +int WD_GetRSN_PCSList(BSSDescriptor *Bss, u8* destbuff, u16 buffsize) { + if(!Bss) return WD_INVALIDBUFF; + if(!destbuff) return WD_INVALIDBUFF; + + IE_RSN IE; + + int ret = WD_GetRSNEssentials(Bss, &IE); + + if(ret < 0) return WD_INVALIDBUFF; + if(IE.PCS_Count * 4 > buffsize) return WD_BUFFTOOSMALL; + + u8 IE_len = WD_GetIELength(Bss, IEID_SECURITY_RSN); + + u8 buff[IE_len]; + WD_GetIE(Bss, IEID_SECURITY_RSN, buff, IE_len); + + memset(destbuff, 0, buffsize); + memcpy(destbuff, &buff[8], IE.PCS_Count * 4); + + return WD_SUCCESS; +} + +int WD_GetWPA_PCSList(BSSDescriptor *Bss, u8* destbuff, u16 buffsize) { + if(!Bss) return WD_INVALIDBUFF; + if(!destbuff) return WD_INVALIDBUFF; + + IE_WPA IE; + + int ret = WD_GetWPAIEEssentials(Bss, &IE); + + if(ret < 0) return WD_INVALIDBUFF; + if(IE.PCS_Count * 4 > buffsize) return WD_BUFFTOOSMALL; + + u8 IE_len = WD_GetIELength(Bss, IEID_SECURITY_RSN); + u8 buff[IE_len]; + WD_GetIE(Bss, IEID_SECURITY_RSN, buff, IE_len); + + memset(destbuff, 0, buffsize); + memcpy(destbuff, &buff[8], IE.PCS_Count * 4); + + return WD_SUCCESS; +} + +int WD_GetRSNEssentials(BSSDescriptor *Bss, IE_RSN *IE) { + if(!Bss) return WD_INVALIDBUFF; + if(!IE) return WD_INVALIDBUFF; + + u8 IE_size = WD_GetIELength(Bss, IEID_SECURITY_RSN); + if(IE_size < 0) return WD_NOTFOUND; + + u8 buff[IE_size]; + + WD_GetIE(Bss, IEID_SECURITY_RSN, buff, IE_size); + + u8 offset = 0; + IE->Version = buff[0 + offset] | buff[1 + offset] << 8; + offset += 2; + IE->GDCS = buff[0 + offset] << 24 | buff[1 + offset] << 16 | buff[2 + offset] << 8 | buff[3 + offset]; + offset += 4; + IE->PCS_Count = buff[0 + offset] | buff[1 + offset] << 8; + offset += 2 + IE->PCS_Count * 4; + IE->AKMS_Count = buff[0 + offset] | buff[1 + offset] << 8; + offset += 2 + IE->AKMS_Count * 4; + + return WD_SUCCESS; +} + +int WD_GetWPAIEEssentials(BSSDescriptor *Bss, IE_WPA *IE) { + if(!Bss) return WD_INVALIDBUFF; + if(!IE) return WD_INVALIDBUFF; + + u8 IE_size = WD_GetVendorSpecificIELength(Bss, OUI_WPA); + if(IE_size < 0) return WD_NOTFOUND; + + u8 buff[IE_size]; + + WD_GetVendorSpecificIE(Bss, OUI_WPA, buff, IE_size); + + u8 offset = 4; + IE->Version = buff[0 + offset] | buff[1 + offset] << 8; + offset += 2; + IE->GDCS = buff[0 + offset] << 24 | buff[1 + offset] << 16 | buff[2 + offset] << 8 | buff[3 + offset]; + offset += 4; + IE->PCS_Count = buff[0 + offset] | buff[1 + offset] << 8; + offset += 2 + IE->PCS_Count * 4; + IE->AKMS_Count = buff[0 + offset] | buff[1 + offset] << 8; + offset += 2 + IE->AKMS_Count * 4; + + return WD_SUCCESS; +} + +u8 WD_GetSecurity(BSSDescriptor *Bss) { + if(!Bss) return WD_INVALIDBUFF; + if(!(Bss->Capabilities & CAPAB_SECURED_FLAG)) return WD_OPEN; + + int ie_len = WD_GetVendorSpecificIELength(Bss, OUI_WPA); + + if (ie_len != WD_NOTFOUND && ie_len > 0) { // WPA + IE_WPA IE; + WD_GetWPAIEEssentials(Bss, &IE); + + u8 buff[IE.PCS_Count * 4]; + WD_GetWPA_PCSList(Bss, buff, IE.PCS_Count * 4); + + u8 offset = 0; + + for (int i = 0; i < IE.PCS_Count; i++) { + if (buff[offset + 3] == 0x02) return WD_WPA_TKIP; + else if (buff[offset + 3] == 0x04) return WD_WPA_AES; + offset += 4; + } + } + + ie_len = WD_GetIELength(Bss, IEID_SECURITY_RSN); + + if(ie_len != WD_NOTFOUND && ie_len > 0) { // WPA2 + IE_RSN IE; + WD_GetRSNEssentials(Bss, &IE); + + u8 ret = 0; + + u8 buff[IE.PCS_Count * 4]; + WD_GetRSN_PCSList(Bss, buff, IE.PCS_Count * 4); + + u8 offset = 0; + + for (int i = 0; i < IE.PCS_Count; i++) { + if (buff[offset + 3] == 0x02) ret = WD_WPA2_TKIP; + if (buff[offset + 3] == 0x04) ret = WD_WPA2_AES; + offset += 4; + } + + return ret; + } + return WD_WEP; } #endif \ No newline at end of file