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
9import os
10import sys
11import time
12import random
13import threading
14import argparse
15
16import nfc
17import nfc.ndef
18import nfc.llcp
19import nfc.handover
20
21import logging
22
23import wpaspy
24
25wpas_ctrl = '/var/run/wpa_supplicant'
26ifname = None
27init_on_touch = False
28in_raw_mode = False
29prev_tcgetattr = 0
30include_wps_req = True
31include_p2p_req = True
32no_input = False
33srv = None
34continue_loop = True
35terminate_now = False
36summary_file = None
37success_file = None
38
39def summary(txt):
40    print txt
41    if summary_file:
42        with open(summary_file, 'a') as f:
43            f.write(txt + "\n")
44
45def success_report(txt):
46    summary(txt)
47    if success_file:
48        with open(success_file, 'a') as f:
49            f.write(txt + "\n")
50
51def 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
77def 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
90def 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
99def 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
109def 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
122def 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
132def 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
143def 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
153def 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
259class 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    # override to avoid parser error in request/response.pretty() in nfcpy
267    # due to new WSC handover format
268    def _process_request(self, request):
269        summary("received handover request {}".format(request.type))
270        response = nfc.ndef.Message("\xd1\x02\x01Hs\x12")
271        if not request.type == 'urn:nfc:wkt:Hr':
272            summary("not a handover request")
273        else:
274            try:
275                request = nfc.ndef.HandoverRequestMessage(request)
276            except nfc.ndef.DecodeError as e:
277                summary("error decoding 'Hr' message: {}".format(e))
278            else:
279                response = self.process_request(request)
280        summary("send handover response {}".format(response.type))
281        return response
282
283    def process_request(self, request):
284        self.ho_server_processing = True
285        clear_raw_mode()
286        print "HandoverServer - request received"
287        try:
288            print "Parsed handover request: " + request.pretty()
289        except Exception, e:
290            print e
291
292        sel = nfc.ndef.HandoverSelectMessage(version="1.2")
293
294        found = False
295
296        for carrier in request.carriers:
297            print "Remote carrier type: " + carrier.type
298            if carrier.type == "application/vnd.wfa.p2p":
299                print "P2P carrier type match - add P2P carrier record"
300                found = True
301                self.received_carrier = carrier.record
302                print "Carrier record:"
303                try:
304                    print carrier.record.pretty()
305                except Exception, e:
306                    print e
307                data = wpas_get_handover_sel()
308                if data is None:
309                    print "Could not get handover select carrier record from wpa_supplicant"
310                    continue
311                print "Handover select carrier record from wpa_supplicant:"
312                print data.encode("hex")
313                self.sent_carrier = data
314                if "OK" in wpas_report_handover(self.received_carrier, self.sent_carrier, "RESP"):
315                    success_report("P2P handover reported successfully (responder)")
316                else:
317                    summary("P2P handover report rejected")
318                    break
319
320                message = nfc.ndef.Message(data);
321                sel.add_carrier(message[0], "active", message[1:])
322                break
323
324        for carrier in request.carriers:
325            if found:
326                break
327            print "Remote carrier type: " + carrier.type
328            if carrier.type == "application/vnd.wfa.wsc":
329                print "WSC carrier type match - add WSC carrier record"
330                found = True
331                self.received_carrier = carrier.record
332                print "Carrier record:"
333                try:
334                    print carrier.record.pretty()
335                except Exception, e:
336                    print e
337                data = wpas_get_handover_sel_wps()
338                if data is None:
339                    print "Could not get handover select carrier record from wpa_supplicant"
340                    continue
341                print "Handover select carrier record from wpa_supplicant:"
342                print data.encode("hex")
343                self.sent_carrier = data
344                if "OK" in wpas_report_handover_wsc(self.received_carrier, self.sent_carrier, "RESP"):
345                    success_report("WSC handover reported successfully")
346                else:
347                    summary("WSC handover report rejected")
348                    break
349
350                message = nfc.ndef.Message(data);
351                sel.add_carrier(message[0], "active", message[1:])
352                found = True
353                break
354
355        print "Handover select:"
356        try:
357            print sel.pretty()
358        except Exception, e:
359            print e
360        print str(sel).encode("hex")
361
362        summary("Sending handover select")
363        self.success = True
364        return sel
365
366
367def clear_raw_mode():
368    import sys, tty, termios
369    global prev_tcgetattr, in_raw_mode
370    if not in_raw_mode:
371        return
372    fd = sys.stdin.fileno()
373    termios.tcsetattr(fd, termios.TCSADRAIN, prev_tcgetattr)
374    in_raw_mode = False
375
376
377def getch():
378    import sys, tty, termios, select
379    global prev_tcgetattr, in_raw_mode
380    fd = sys.stdin.fileno()
381    prev_tcgetattr = termios.tcgetattr(fd)
382    ch = None
383    try:
384        tty.setraw(fd)
385        in_raw_mode = True
386        [i, o, e] = select.select([fd], [], [], 0.05)
387        if i:
388            ch = sys.stdin.read(1)
389    finally:
390        termios.tcsetattr(fd, termios.TCSADRAIN, prev_tcgetattr)
391        in_raw_mode = False
392    return ch
393
394
395def p2p_tag_read(tag):
396    success = False
397    if len(tag.ndef.message):
398        for record in tag.ndef.message:
399            print "record type " + record.type
400            if record.type == "application/vnd.wfa.wsc":
401                summary("WPS tag - send to wpa_supplicant")
402                success = wpas_tag_read(tag.ndef.message)
403                break
404            if record.type == "application/vnd.wfa.p2p":
405                summary("P2P tag - send to wpa_supplicant")
406                success = wpas_tag_read(tag.ndef.message)
407                break
408    else:
409        summary("Empty tag")
410
411    if success:
412        success_report("Tag read succeeded")
413
414    return success
415
416
417def rdwr_connected_p2p_write(tag):
418    summary("Tag found - writing - " + str(tag))
419    global p2p_sel_data
420    tag.ndef.message = str(p2p_sel_data)
421    success_report("Tag write succeeded")
422    print "Done - remove tag"
423    global only_one
424    if only_one:
425        global continue_loop
426        continue_loop = False
427    global p2p_sel_wait_remove
428    return p2p_sel_wait_remove
429
430def wps_write_p2p_handover_sel(clf, wait_remove=True):
431    print "Write P2P handover select"
432    data = wpas_get_handover_sel(tag=True)
433    if (data == None):
434        summary("Could not get P2P handover select from wpa_supplicant")
435        return
436
437    global p2p_sel_wait_remove
438    p2p_sel_wait_remove = wait_remove
439    global p2p_sel_data
440    p2p_sel_data = nfc.ndef.HandoverSelectMessage(version="1.2")
441    message = nfc.ndef.Message(data);
442    p2p_sel_data.add_carrier(message[0], "active", message[1:])
443    print "Handover select:"
444    try:
445        print p2p_sel_data.pretty()
446    except Exception, e:
447        print e
448    print str(p2p_sel_data).encode("hex")
449
450    print "Touch an NFC tag"
451    clf.connect(rdwr={'on-connect': rdwr_connected_p2p_write})
452
453
454def rdwr_connected(tag):
455    global only_one, no_wait
456    summary("Tag connected: " + str(tag))
457
458    if tag.ndef:
459        print "NDEF tag: " + tag.type
460        try:
461            print tag.ndef.message.pretty()
462        except Exception, e:
463            print e
464        success = p2p_tag_read(tag)
465        if only_one and success:
466            global continue_loop
467            continue_loop = False
468    else:
469        summary("Not an NDEF tag - remove tag")
470        return True
471
472    return not no_wait
473
474
475def llcp_worker(llc):
476    global init_on_touch
477    if init_on_touch:
478            print "Starting handover client"
479            p2p_handover_client(llc)
480            return
481
482    global no_input
483    if no_input:
484        print "Wait for handover to complete"
485    else:
486        print "Wait for handover to complete - press 'i' to initiate ('w' for WPS only, 'p' for P2P only)"
487    global srv
488    global wait_connection
489    while not wait_connection and srv.sent_carrier is None:
490        if srv.ho_server_processing:
491            time.sleep(0.025)
492        elif no_input:
493            time.sleep(0.5)
494        else:
495            global include_wps_req, include_p2p_req
496            res = getch()
497            if res == 'i':
498                include_wps_req = True
499                include_p2p_req = True
500            elif res == 'p':
501                include_wps_req = False
502                include_p2p_req = True
503            elif res == 'w':
504                include_wps_req = True
505                include_p2p_req = False
506            else:
507                continue
508            clear_raw_mode()
509            print "Starting handover client"
510            p2p_handover_client(llc)
511            return
512
513    clear_raw_mode()
514    print "Exiting llcp_worker thread"
515
516def llcp_startup(clf, llc):
517    print "Start LLCP server"
518    global srv
519    srv = HandoverServer(llc)
520    return llc
521
522def llcp_connected(llc):
523    print "P2P LLCP connected"
524    global wait_connection
525    wait_connection = False
526    global init_on_touch
527    if not init_on_touch:
528        global srv
529        srv.start()
530    if init_on_touch or not no_input:
531        threading.Thread(target=llcp_worker, args=(llc,)).start()
532    return True
533
534def terminate_loop():
535    global terminate_now
536    return terminate_now
537
538def main():
539    clf = nfc.ContactlessFrontend()
540
541    parser = argparse.ArgumentParser(description='nfcpy to wpa_supplicant integration for P2P and WPS NFC operations')
542    parser.add_argument('-d', const=logging.DEBUG, default=logging.INFO,
543                        action='store_const', dest='loglevel',
544                        help='verbose debug output')
545    parser.add_argument('-q', const=logging.WARNING, action='store_const',
546                        dest='loglevel', help='be quiet')
547    parser.add_argument('--only-one', '-1', action='store_true',
548                        help='run only one operation and exit')
549    parser.add_argument('--init-on-touch', '-I', action='store_true',
550                        help='initiate handover on touch')
551    parser.add_argument('--no-wait', action='store_true',
552                        help='do not wait for tag to be removed before exiting')
553    parser.add_argument('--ifname', '-i',
554                        help='network interface name')
555    parser.add_argument('--no-wps-req', '-N', action='store_true',
556                        help='do not include WPS carrier record in request')
557    parser.add_argument('--no-input', '-a', action='store_true',
558                        help='do not use stdout input to initiate handover')
559    parser.add_argument('--tag-read-only', '-t', action='store_true',
560                        help='tag read only (do not allow connection handover)')
561    parser.add_argument('--handover-only', action='store_true',
562                        help='connection handover only (do not allow tag read)')
563    parser.add_argument('--freq', '-f',
564                        help='forced frequency of operating channel in MHz')
565    parser.add_argument('--summary',
566                        help='summary file for writing status updates')
567    parser.add_argument('--success',
568                        help='success file for writing success update')
569    parser.add_argument('command', choices=['write-p2p-sel'],
570                        nargs='?')
571    args = parser.parse_args()
572
573    global only_one
574    only_one = args.only_one
575
576    global no_wait
577    no_wait = args.no_wait
578
579    global force_freq
580    force_freq = args.freq
581
582    logging.basicConfig(level=args.loglevel)
583
584    global init_on_touch
585    init_on_touch = args.init_on_touch
586
587    if args.ifname:
588        global ifname
589        ifname = args.ifname
590        print "Selected ifname " + ifname
591
592    if args.no_wps_req:
593        global include_wps_req
594        include_wps_req = False
595
596    if args.summary:
597        global summary_file
598        summary_file = args.summary
599
600    if args.success:
601        global success_file
602        success_file = args.success
603
604    if args.no_input:
605        global no_input
606        no_input = True
607
608    clf = nfc.ContactlessFrontend()
609    global wait_connection
610
611    try:
612        if not clf.open("usb"):
613            print "Could not open connection with an NFC device"
614            raise SystemExit
615
616        if args.command == "write-p2p-sel":
617            wps_write_p2p_handover_sel(clf, wait_remove=not args.no_wait)
618            raise SystemExit
619
620        global continue_loop
621        while continue_loop:
622            print "Waiting for a tag or peer to be touched"
623            wait_connection = True
624            try:
625                if args.tag_read_only:
626                    if not clf.connect(rdwr={'on-connect': rdwr_connected}):
627                        break
628                elif args.handover_only:
629                    if not clf.connect(llcp={'on-startup': llcp_startup,
630                                             'on-connect': llcp_connected},
631                                       terminate=terminate_loop):
632                        break
633                else:
634                    if not clf.connect(rdwr={'on-connect': rdwr_connected},
635                                       llcp={'on-startup': llcp_startup,
636                                             'on-connect': llcp_connected},
637                                       terminate=terminate_loop):
638                        break
639            except Exception, e:
640                print "clf.connect failed"
641
642            global srv
643            if only_one and srv and srv.success:
644                raise SystemExit
645
646    except KeyboardInterrupt:
647        raise SystemExit
648    finally:
649        clf.close()
650
651    raise SystemExit
652
653if __name__ == '__main__':
654    main()
655