wpa_supplicant/examples/p2p-nfc.py

   1 #!/usr/bin/python
   2 #
   3 # Example nfcpy to wpa_supplicant wrapper for P2P NFC operations
   4 # Copyright (c) 2012-2013, Jouni Malinen <j@w1.fi>
   5 #
   6 # This software may be distributed under the terms of the BSD license.
   7 # See README for more details.
   8
   9 import os
  10 import sys
  11 import time
  12 import random
  13 import threading
  14 import argparse
  15
  16 import nfc
  17 import nfc.ndef
  18 import nfc.llcp
  19 import nfc.handover
  20
  21 import logging
  22
  23 import wpaspy
  24
  25 wpas_ctrl = '/var/run/wpa_supplicant'
  26 ifname = None
  27 init_on_touch = False
  28 in_raw_mode = False
  29 prev_tcgetattr = 0
  30 include_wps_req = True
  31 include_p2p_req = True
  32 no_input = False
  33 srv = None
  34 continue_loop = True
  35 terminate_now = False
  36 summary_file = None
  37 success_file = None
  38
  39 def summary(txt):
  40     print txt
  41     if summary_file:
  42         with open(summary_file, 'a') as f:
  43             f.write(txt + "\n")
  44
  45 def success_report(txt):
  46     summary(txt)
  47     if success_file:
  48         with open(success_file, 'a') as f:
  49             f.write(txt + "\n")
  50
  51 def wpas_connect():
  52     ifaces = []
  53     if os.path.isdir(wpas_ctrl):
  54         try:
  55             ifaces = [os.path.join(wpas_ctrl, i) for i in os.listdir(wpas_ctrl)]
  56         except OSError, error:
  57             print "Could not find wpa_supplicant: ", error
  58             return None
  59
  60     if len(ifaces) < 1:
  61         print "No wpa_supplicant control interface found"
  62         return None
  63
  64     for ctrl in ifaces:
  65         if ifname:
  66             if ifname not in ctrl:
  67                 continue
  68         try:
  69             print "Trying to use control interface " + ctrl
  70             wpas = wpaspy.Ctrl(ctrl)
  71             return wpas
  72         except Exception, e:
  73             pass
  74     return None
  75
  76
  77 def wpas_tag_read(message):
  78     wpas = wpas_connect()
  79     if (wpas == None):
  80         return False
  81     cmd = "WPS_NFC_TAG_READ " + str(message).encode("hex")
  82     global force_freq
  83     if force_freq:
  84         cmd = cmd + " freq=" + force_freq
  85     if "FAIL" in wpas.request(cmd):
  86         return False
  87     return True
  88
  89
  90 def wpas_get_handover_req():
  91     wpas = wpas_connect()
  92     if (wpas == None):
  93         return None
  94     res = wpas.request("NFC_GET_HANDOVER_REQ NDEF P2P-CR").rstrip()
  95     if "FAIL" in res:
  96         return None
  97     return res.decode("hex")
  98
  99 def wpas_get_handover_req_wps():
 100     wpas = wpas_connect()
 101     if (wpas == None):
 102         return None
 103     res = wpas.request("NFC_GET_HANDOVER_REQ NDEF WPS-CR").rstrip()
 104     if "FAIL" in res:
 105         return None
 106     return res.decode("hex")
 107
 108
 109 def wpas_get_handover_sel(tag=False):
 110     wpas = wpas_connect()
 111     if (wpas == None):
 112         return None
 113     if tag:
 114         res = wpas.request("NFC_GET_HANDOVER_SEL NDEF P2P-CR-TAG").rstrip()
 115     else:
 116         res = wpas.request("NFC_GET_HANDOVER_SEL NDEF P2P-CR").rstrip()
 117     if "FAIL" in res:
 118         return None
 119     return res.decode("hex")
 120
 121
 122 def wpas_get_handover_sel_wps():
 123     wpas = wpas_connect()
 124     if (wpas == None):
 125         return None
 126     res = wpas.request("NFC_GET_HANDOVER_SEL NDEF WPS-CR");
 127     if "FAIL" in res:
 128         return None
 129     return res.rstrip().decode("hex")
 130
 131
 132 def wpas_report_handover(req, sel, type):
 133     wpas = wpas_connect()
 134     if (wpas == None):
 135         return None
 136     cmd = "NFC_REPORT_HANDOVER " + type + " P2P " + str(req).encode("hex") + " " + str(sel).encode("hex")
 137     global force_freq
 138     if force_freq:
 139         cmd = cmd + " freq=" + force_freq
 140     return wpas.request(cmd)
 141
 142
 143 def wpas_report_handover_wsc(req, sel, type):
 144     wpas = wpas_connect()
 145     if (wpas == None):
 146         return None
 147     cmd = "NFC_REPORT_HANDOVER " + type + " WPS " + str(req).encode("hex") + " " + str(sel).encode("hex")
 148     if force_freq:
 149         cmd = cmd + " freq=" + force_freq
 150     return wpas.request(cmd)
 151
 152
 153 def p2p_handover_client(llc):
 154     message = nfc.ndef.HandoverRequestMessage(version="1.2")
 155     message.nonce = random.randint(0, 0xffff)
 156
 157     global include_p2p_req
 158     if include_p2p_req:
 159         data = wpas_get_handover_req()
 160         if (data == None):
 161             summary("Could not get handover request carrier record from wpa_supplicant")
 162             return
 163         print "Handover request carrier record from wpa_supplicant: " + data.encode("hex")
 164         datamsg = nfc.ndef.Message(data)
 165         message.add_carrier(datamsg[0], "active", datamsg[1:])
 166
 167     global include_wps_req
 168     if include_wps_req:
 169         print "Handover request (pre-WPS):"
 170         try:
 171             print message.pretty()
 172         except Exception, e:
 173             print e
 174
 175         data = wpas_get_handover_req_wps()
 176         if data:
 177             print "Add WPS request in addition to P2P"
 178             datamsg = nfc.ndef.Message(data)
 179             message.add_carrier(datamsg[0], "active", datamsg[1:])
 180
 181     print "Handover request:"
 182     try:
 183         print message.pretty()
 184     except Exception, e:
 185         print e
 186     print str(message).encode("hex")
 187
 188     client = nfc.handover.HandoverClient(llc)
 189     try:
 190         summary("Trying to initiate NFC connection handover")
 191         client.connect()
 192         summary("Connected for handover")
 193     except nfc.llcp.ConnectRefused:
 194         summary("Handover connection refused")
 195         client.close()
 196         return
 197     except Exception, e:
 198         summary("Other exception: " + str(e))
 199         client.close()
 200         return
 201
 202     summary("Sending handover request")
 203
 204     if not client.send(message):
 205         summary("Failed to send handover request")
 206         client.close()
 207         return
 208
 209     summary("Receiving handover response")
 210     message = client._recv()
 211     if message is None:
 212         summary("No response received")
 213         client.close()
 214         return
 215     if message.type != "urn:nfc:wkt:Hs":
 216         summary("Response was not Hs - received: " + message.type)
 217         client.close()
 218         return
 219
 220     print "Received message"
 221     try:
 222         print message.pretty()
 223     except Exception, e:
 224         print e
 225     print str(message).encode("hex")
 226     message = nfc.ndef.HandoverSelectMessage(message)
 227     summary("Handover select received")
 228     try:
 229         print message.pretty()
 230     except Exception, e:
 231         print e
 232
 233     for carrier in message.carriers:
 234         print "Remote carrier type: " + carrier.type
 235         if carrier.type == "application/vnd.wfa.p2p":
 236             print "P2P carrier type match - send to wpa_supplicant"
 237             if "OK" in wpas_report_handover(data, carrier.record, "INIT"):
 238                 success_report("P2P handover reported successfully (initiator)")
 239             else:
 240                 summary("P2P handover report rejected")
 241             break
 242
 243     print "Remove peer"
 244     client.close()
 245     print "Done with handover"
 246     global only_one
 247     if only_one:
 248         print "only_one -> stop loop"
 249         global continue_loop
 250         continue_loop = False
 251
 252     global no_wait
 253     if no_wait:
 254         print "Trying to exit.."
 255         global terminate_now
 256         terminate_now = True
 257
 258
 259 class HandoverServer(nfc.handover.HandoverServer):
 260     def __init__(self, llc):
 261         super(HandoverServer, self).__init__(llc)
 262         self.sent_carrier = None
 263         self.ho_server_processing = False
 264         self.success = False
 265
 266     def process_request(self, request):
 267         self.ho_server_processing = True
 268         clear_raw_mode()
 269         print "HandoverServer - request received"
 270         try:
 271             print "Parsed handover request: " + request.pretty()
 272         except Exception, e:
 273             print e
 274
 275         sel = nfc.ndef.HandoverSelectMessage(version="1.2")
 276
 277         found = False
 278
 279         for carrier in request.carriers:
 280             print "Remote carrier type: " + carrier.type
 281             if carrier.type == "application/vnd.wfa.p2p":
 282                 print "P2P carrier type match - add P2P carrier record"
 283                 found = True
 284                 self.received_carrier = carrier.record
 285                 print "Carrier record:"
 286                 try:
 287                     print carrier.record.pretty()
 288                 except Exception, e:
 289                     print e
 290                 data = wpas_get_handover_sel()
 291                 if data is None:
 292                     print "Could not get handover select carrier record from wpa_supplicant"
 293                     continue
 294                 print "Handover select carrier record from wpa_supplicant:"
 295                 print data.encode("hex")
 296                 self.sent_carrier = data
 297                 if "OK" in wpas_report_handover(self.received_carrier, self.sent_carrier, "RESP"):
 298                     success_report("P2P handover reported successfully (responder)")
 299                 else:
 300                     summary("P2P handover report rejected")
 301                     break
 302
 303                 message = nfc.ndef.Message(data);
 304                 sel.add_carrier(message[0], "active", message[1:])
 305                 break
 306
 307         for carrier in request.carriers:
 308             if found:
 309                 break
 310             print "Remote carrier type: " + carrier.type
 311             if carrier.type == "application/vnd.wfa.wsc":
 312                 print "WSC carrier type match - add WSC carrier record"
 313                 found = True
 314                 self.received_carrier = carrier.record
 315                 print "Carrier record:"
 316                 try:
 317                     print carrier.record.pretty()
 318                 except Exception, e:
 319                     print e
 320                 data = wpas_get_handover_sel_wps()
 321                 if data is None:
 322                     print "Could not get handover select carrier record from wpa_supplicant"
 323                     continue
 324                 print "Handover select carrier record from wpa_supplicant:"
 325                 print data.encode("hex")
 326                 self.sent_carrier = data
 327                 if "OK" in wpas_report_handover_wsc(self.received_carrier, self.sent_carrier, "RESP"):
 328                     success_report("WSC handover reported successfully")
 329                 else:
 330                     summary("WSC handover report rejected")
 331                     break
 332
 333                 message = nfc.ndef.Message(data);
 334                 sel.add_carrier(message[0], "active", message[1:])
 335                 found = True
 336                 break
 337
 338         print "Handover select:"
 339         try:
 340             print sel.pretty()
 341         except Exception, e:
 342             print e
 343         print str(sel).encode("hex")
 344
 345         summary("Sending handover select")
 346         self.success = True
 347         return sel
 348
 349
 350 def clear_raw_mode():
 351     import sys, tty, termios
 352     global prev_tcgetattr, in_raw_mode
 353     if not in_raw_mode:
 354         return
 355     fd = sys.stdin.fileno()
 356     termios.tcsetattr(fd, termios.TCSADRAIN, prev_tcgetattr)
 357     in_raw_mode = False
 358
 359
 360 def getch():
 361     import sys, tty, termios, select
 362     global prev_tcgetattr, in_raw_mode
 363     fd = sys.stdin.fileno()
 364     prev_tcgetattr = termios.tcgetattr(fd)
 365     ch = None
 366     try:
 367         tty.setraw(fd)
 368         in_raw_mode = True
 369         [i, o, e] = select.select([fd], [], [], 0.05)
 370         if i:
 371             ch = sys.stdin.read(1)
 372     finally:
 373         termios.tcsetattr(fd, termios.TCSADRAIN, prev_tcgetattr)
 374         in_raw_mode = False
 375     return ch
 376
 377
 378 def p2p_tag_read(tag):
 379     success = False
 380     if len(tag.ndef.message):
 381         for record in tag.ndef.message:
 382             print "record type " + record.type
 383             if record.type == "application/vnd.wfa.wsc":
 384                 summary("WPS tag - send to wpa_supplicant")
 385                 success = wpas_tag_read(tag.ndef.message)
 386                 break
 387             if record.type == "application/vnd.wfa.p2p":
 388                 summary("P2P tag - send to wpa_supplicant")
 389                 success = wpas_tag_read(tag.ndef.message)
 390                 break
 391     else:
 392         summary("Empty tag")
 393
 394     if success:
 395         success_report("Tag read succeeded")
 396
 397     return success
 398
 399
 400 def rdwr_connected_p2p_write(tag):
 401     summary("Tag found - writing - " + str(tag))
 402     global p2p_sel_data
 403     tag.ndef.message = str(p2p_sel_data)
 404     success_report("Tag write succeeded")
 405     print "Done - remove tag"
 406     global only_one
 407     if only_one:
 408         global continue_loop
 409         continue_loop = False
 410     global p2p_sel_wait_remove
 411     return p2p_sel_wait_remove
 412
 413 def wps_write_p2p_handover_sel(clf, wait_remove=True):
 414     print "Write P2P handover select"
 415     data = wpas_get_handover_sel(tag=True)
 416     if (data == None):
 417         summary("Could not get P2P handover select from wpa_supplicant")
 418         return
 419
 420     global p2p_sel_wait_remove
 421     p2p_sel_wait_remove = wait_remove
 422     global p2p_sel_data
 423     p2p_sel_data = nfc.ndef.HandoverSelectMessage(version="1.2")
 424     message = nfc.ndef.Message(data);
 425     p2p_sel_data.add_carrier(message[0], "active", message[1:])
 426     print "Handover select:"
 427     try:
 428         print p2p_sel_data.pretty()
 429     except Exception, e:
 430         print e
 431     print str(p2p_sel_data).encode("hex")
 432
 433     print "Touch an NFC tag"
 434     clf.connect(rdwr={'on-connect': rdwr_connected_p2p_write})
 435
 436
 437 def rdwr_connected(tag):
 438     global only_one, no_wait
 439     summary("Tag connected: " + str(tag))
 440
 441     if tag.ndef:
 442         print "NDEF tag: " + tag.type
 443         try:
 444             print tag.ndef.message.pretty()
 445         except Exception, e:
 446             print e
 447         success = p2p_tag_read(tag)
 448         if only_one and success:
 449             global continue_loop
 450             continue_loop = False
 451     else:
 452         summary("Not an NDEF tag - remove tag")
 453         return True
 454
 455     return not no_wait
 456
 457
 458 def llcp_worker(llc):
 459     global init_on_touch
 460     if init_on_touch:
 461             print "Starting handover client"
 462             p2p_handover_client(llc)
 463             return
 464
 465     global no_input
 466     if no_input:
 467         print "Wait for handover to complete"
 468     else:
 469         print "Wait for handover to complete - press 'i' to initiate ('w' for WPS only, 'p' for P2P only)"
 470     global srv
 471     global wait_connection
 472     while not wait_connection and srv.sent_carrier is None:
 473         if srv.ho_server_processing:
 474             time.sleep(0.025)
 475         elif no_input:
 476             time.sleep(0.5)
 477         else:
 478             global include_wps_req, include_p2p_req
 479             res = getch()
 480             if res == 'i':
 481                 include_wps_req = True
 482                 include_p2p_req = True
 483             elif res == 'p':
 484                 include_wps_req = False
 485                 include_p2p_req = True
 486             elif res == 'w':
 487                 include_wps_req = True
 488                 include_p2p_req = False
 489             else:
 490                 continue
 491             clear_raw_mode()
 492             print "Starting handover client"
 493             p2p_handover_client(llc)
 494             return
 495
 496     clear_raw_mode()
 497     print "Exiting llcp_worker thread"
 498
 499 def llcp_startup(clf, llc):
 500     print "Start LLCP server"
 501     global srv
 502     srv = HandoverServer(llc)
 503     return llc
 504
 505 def llcp_connected(llc):
 506     print "P2P LLCP connected"
 507     global wait_connection
 508     wait_connection = False
 509     global init_on_touch
 510     if not init_on_touch:
 511         global srv
 512         srv.start()
 513     if init_on_touch or not no_input:
 514         threading.Thread(target=llcp_worker, args=(llc,)).start()
 515     return True
 516
 517 def terminate_loop():
 518     global terminate_now
 519     return terminate_now
 520
 521 def main():
 522     clf = nfc.ContactlessFrontend()
 523
 524     parser = argparse.ArgumentParser(description='nfcpy to wpa_supplicant integration for P2P and WPS NFC operations')
 525     parser.add_argument('-d', const=logging.DEBUG, default=logging.INFO,
 526                         action='store_const', dest='loglevel',
 527                         help='verbose debug output')
 528     parser.add_argument('-q', const=logging.WARNING, action='store_const',
 529                         dest='loglevel', help='be quiet')
 530     parser.add_argument('--only-one', '-1', action='store_true',
 531                         help='run only one operation and exit')
 532     parser.add_argument('--init-on-touch', '-I', action='store_true',
 533                         help='initiate handover on touch')
 534     parser.add_argument('--no-wait', action='store_true',
 535                         help='do not wait for tag to be removed before exiting')
 536     parser.add_argument('--ifname', '-i',
 537                         help='network interface name')
 538     parser.add_argument('--no-wps-req', '-N', action='store_true',
 539                         help='do not include WPS carrier record in request')
 540     parser.add_argument('--no-input', '-a', action='store_true',
 541                         help='do not use stdout input to initiate handover')
 542     parser.add_argument('--tag-read-only', '-t', action='store_true',
 543                         help='tag read only (do not allow connection handover)')
 544     parser.add_argument('--handover-only', action='store_true',
 545                         help='connection handover only (do not allow tag read)')
 546     parser.add_argument('--freq', '-f',
 547                         help='forced frequency of operating channel in MHz')
 548     parser.add_argument('--summary',
 549                         help='summary file for writing status updates')
 550     parser.add_argument('--success',
 551                         help='success file for writing success update')
 552     parser.add_argument('command', choices=['write-p2p-sel'],
 553                         nargs='?')
 554     args = parser.parse_args()
 555
 556     global only_one
 557     only_one = args.only_one
 558
 559     global no_wait
 560     no_wait = args.no_wait
 561
 562     global force_freq
 563     force_freq = args.freq
 564
 565     logging.basicConfig(level=args.loglevel)
 566
 567     global init_on_touch
 568     init_on_touch = args.init_on_touch
 569
 570     if args.ifname:
 571         global ifname
 572         ifname = args.ifname
 573         print "Selected ifname " + ifname
 574
 575     if args.no_wps_req:
 576         global include_wps_req
 577         include_wps_req = False
 578
 579     if args.summary:
 580         global summary_file
 581         summary_file = args.summary
 582
 583     if args.success:
 584         global success_file
 585         success_file = args.success
 586
 587     if args.no_input:
 588         global no_input
 589         no_input = True
 590
 591     clf = nfc.ContactlessFrontend()
 592     global wait_connection
 593
 594     try:
 595         if not clf.open("usb"):
 596             print "Could not open connection with an NFC device"
 597             raise SystemExit
 598
 599         if args.command == "write-p2p-sel":
 600             wps_write_p2p_handover_sel(clf, wait_remove=not args.no_wait)
 601             raise SystemExit
 602
 603         global continue_loop
 604         while continue_loop:
 605             print "Waiting for a tag or peer to be touched"
 606             wait_connection = True
 607             try:
 608                 if args.tag_read_only:
 609                     if not clf.connect(rdwr={'on-connect': rdwr_connected}):
 610                         break
 611                 elif args.handover_only:
 612                     if not clf.connect(llcp={'on-startup': llcp_startup,
 613                                              'on-connect': llcp_connected},
 614                                        terminate=terminate_loop):
 615                         break
 616                 else:
 617                     if not clf.connect(rdwr={'on-connect': rdwr_connected},
 618                                        llcp={'on-startup': llcp_startup,
 619                                              'on-connect': llcp_connected},
 620                                        terminate=terminate_loop):
 621                         break
 622             except Exception, e:
 623                 print "clf.connect failed"
 624
 625             global srv
 626             if only_one and srv and srv.success:
 627                 raise SystemExit
 628
 629     except KeyboardInterrupt:
 630         raise SystemExit
 631     finally:
 632         clf.close()
 633
 634     raise SystemExit
 635
 636 if __name__ == '__main__':
 637     main()