a little bit of P2P parsing

diff --git a/scan.c b/scan.c
index e7be07105a6d86f96a124ab957b6aa518c170632..d083591d7a831da7d86bef76a7e26b1baf96e0b1 100644 (file)
--- a/scan.c
+++ b/scan.c
@@ -29,6 +29,7 @@
 
 static unsigned char ms_oui[3]         = { 0x00, 0x50, 0xf2 };
 static unsigned char ieee80211_oui[3]  = { 0x00, 0x0f, 0xac };
+static unsigned char wfa_oui[3]                = { 0x50, 0x6f, 0x9a };
 
 struct scan_params {
        bool unknown;
@@ -934,6 +935,90 @@ static const struct ie_print wifiprinters[] = {
        [4] = { "WPS", print_wifi_wps, 0, 255, BIT(PRINT_SCAN), },
 };
 
+static inline void print_p2p(const uint8_t type, uint8_t len, const uint8_t *data)
+{
+       bool first = true;
+       __u8 subtype;
+       __u16 sublen;
+
+       while (len >= 3) {
+               subtype = data[0];
+               sublen = (data[2] << 8) + data[1];
+
+               if (sublen > len - 3)
+                       break;
+
+               switch (subtype) {
+               case 0x02: /* capability */
+                       tab_on_first(&first);
+                       if (sublen < 2) {
+                               printf("\t * malformed capability\n");
+                               break;
+                       }
+                       printf("\t * Group capa: 0x%.2x, Device capa: 0x%.2x\n",
+                               data[3], data[4]);
+                       break;
+               case 0x0d: /* device info */
+                       if (sublen < 6 + 2 + 8 + 1) {
+                               printf("\t * malformed device info\n");
+                               break;
+                       }
+                       /* fall through for now */
+               case 0x00: /* status */
+               case 0x01: /* minor reason */
+               case 0x03: /* device ID */
+               case 0x04: /* GO intent */
+               case 0x05: /* configuration timeout */
+               case 0x06: /* listen channel */
+               case 0x07: /* group BSSID */
+               case 0x08: /* ext listen timing */
+               case 0x09: /* intended interface address */
+               case 0x0a: /* manageability */
+               case 0x0b: /* channel list */
+               case 0x0c: /* NoA */
+               case 0x0e: /* group info */
+               case 0x0f: /* group ID */
+               case 0x10: /* interface */
+               case 0x11: /* operating channel */
+               case 0x12: /* invitation flags */
+               case 0xdd: /* vendor specific */
+               default: {
+                       const __u8 *subdata = data + 4;
+                       __u16 tmplen = sublen;
+
+                       tab_on_first(&first);
+                       printf("\t * Unknown TLV (%#.2x, %d bytes):",
+                              subtype, tmplen);
+                       while (tmplen) {
+                               printf(" %.2x", *subdata);
+                               subdata++;
+                               tmplen--;
+                       }
+                       printf("\n");
+                       break;
+               }
+               }
+
+               data += sublen + 3;
+               len -= sublen + 3;
+       }
+
+       if (len != 0) {
+               tab_on_first(&first);
+               printf("\t * bogus tail data (%d):", len);
+               while (len) {
+                       printf(" %.2x", *data);
+                       data++;
+                       len--;
+               }
+               printf("\n");
+       }
+}
+
+static const struct ie_print wfa_printers[] = {
+       [9] = { "P2P", print_p2p, 2, 255, BIT(PRINT_SCAN), },
+};
+
 static void print_vendor(unsigned char len, unsigned char *data,
                         bool unknown, enum print_ie_type ptype)
 {
@@ -963,6 +1048,22 @@ static void print_vendor(unsigned char len, unsigned char *data,
                return;
        }
 
+       if (len >= 4 && memcmp(data, wfa_oui, 3) == 0) {
+               if (data[3] < ARRAY_SIZE(wfa_printers) &&
+                   wfa_printers[data[3]].name &&
+                   wfa_printers[data[3]].flags & BIT(ptype)) {
+                       print_ie(&wfa_printers[data[3]], data[3], len - 4, data + 4);
+                       return;
+               }
+               if (!unknown)
+                       return;
+               printf("\tWFA %#.2x, data:", data[3]);
+               for(i = 0; i < len - 4; i++)
+                       printf(" %.02x", data[i + 4]);
+               printf("\n");
+               return;
+       }
+
        if (!unknown)
                return;