PageRenderTime 44ms CodeModel.GetById 1ms app.highlight 36ms RepoModel.GetById 1ms app.codeStats 0ms

/nfc/llcp/llc.py

https://bitbucket.org/jialvarez/nfcpy
Python | 597 lines | 493 code | 64 blank | 40 comment | 188 complexity | 9a307df232cd4f39510c0e6439d507a1 MD5 | raw file
  1# -*- coding: latin-1 -*-
  2# -----------------------------------------------------------------------------
  3# Copyright 2009-2011 Stephen Tiedemann <stephen.tiedemann@googlemail.com>
  4#
  5# Licensed under the EUPL, Version 1.1 or - as soon they 
  6# will be approved by the European Commission - subsequent
  7# versions of the EUPL (the "Licence");
  8# You may not use this work except in compliance with the
  9# Licence.
 10# You may obtain a copy of the Licence at:
 11#
 12# http://www.osor.eu/eupl
 13#
 14# Unless required by applicable law or agreed to in
 15# writing, software distributed under the Licence is
 16# distributed on an "AS IS" basis,
 17# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 18# express or implied.
 19# See the Licence for the specific language governing
 20# permissions and limitations under the Licence.
 21# -----------------------------------------------------------------------------
 22
 23import logging
 24log = logging.getLogger(__name__)
 25
 26import time
 27from types import *
 28import threading
 29import collections
 30import random
 31
 32# local imports
 33from tco import *
 34from pdu import *
 35from err import *
 36from opt import *
 37
 38RAW_ACCESS_POINT, LOGICAL_DATA_LINK, DATA_LINK_CONNECTION = range(3)
 39
 40wks_map = {
 41    "urn:nfc:sn:sdp" : 1,
 42    "urn:nfc:sn:ip"  : 2,
 43    "urn:nfc:sn:obex": 3,
 44    "urn:nfc:sn:snep": 4}
 45
 46class ServiceAccessPoint(object):
 47    def __init__(self, addr, llc):
 48        self.llc = llc
 49        self.addr = addr
 50        self.sock_list = collections.deque()
 51        self.send_list = collections.deque()
 52
 53    def __str__(self):
 54        return "SAP {0:>2}".format(self.addr)
 55
 56    @property
 57    def mode(self):
 58        with self.llc.lock:
 59            try:
 60                if isinstance(self.sock_list[0], RawAccessPoint):
 61                    return RAW_ACCESS_POINT
 62                if isinstance(self.sock_list[0], LogicalDataLink):
 63                    return LOGICAL_DATA_LINK
 64                if isinstance(self.sock_list[0], DataLinkConnection):
 65                    return DATA_LINK_CONNECTION
 66            except IndexError: return 0
 67
 68    def insert_socket(self, socket):
 69        with self.llc.lock:
 70            try: insertable = type(socket) == type(self.sock_list[0])
 71            except IndexError: insertable = True
 72            if insertable:
 73                socket.bind(self.addr)
 74                self.sock_list.appendleft(socket)
 75            else: log.error("can't insert socket of differing type")
 76            return insertable
 77
 78    def remove_socket(self, socket):
 79        assert socket.addr == self.addr
 80        socket.close()
 81        with self.llc.lock:
 82            try: self.sock_list.remove(socket)
 83            except ValueError: pass
 84            if len(self.sock_list) == 0:
 85                # completely remove this sap
 86                self.llc.sap[self.addr] = None
 87
 88    def send(self, pdu):
 89        self.send_list.append(pdu)
 90
 91    def shutdown(self):
 92        while True:
 93            try: socket = self.sock_list.pop()
 94            except IndexError: return
 95            log.debug("shutdown socket %s" % str(socket))
 96            socket.bind(None); socket.close()
 97
 98    #
 99    # enqueue() and dequeue() are called from llc run thread
100    #
101    def enqueue(self, pdu):
102        with self.llc.lock:
103            if isinstance(pdu, Connect):
104                for socket in self.sock_list:
105                    if socket.state.LISTEN:
106                        socket.enqueue(pdu)
107                        return
108            else:
109                for socket in self.sock_list:
110                    if pdu.ssap == socket.peer or socket.peer is None:
111                        socket.enqueue(pdu)
112                        return
113                    
114            if pdu.type in connection_mode_pdu_types:
115                self.send(DisconnectedMode(pdu.ssap, pdu.dsap, reason=1))
116
117    def dequeue(self, max_size):
118        with self.llc.lock:
119            for socket in self.sock_list:
120                #print "dequeue from", socket
121                pdu = socket.dequeue(max_size)
122                if pdu: return pdu
123            else:
124                try: return self.send_list.popleft()
125                except IndexError: pass
126
127    def sendack(self, max_size):
128        with self.llc.lock:
129            for socket in self.sock_list:
130                pdu = socket.sendack(max_size)
131                if pdu: return pdu
132
133class ServiceDiscovery(object):
134    def __init__(self, llc):
135        self.llc = llc
136        self.snl = dict()
137        self.tids = range(256)
138        self.resp = threading.Condition(self.llc.lock)
139        self.sent = dict()
140        self.sdreq = collections.deque()
141        self.sdres = collections.deque()
142        self.dmpdu = collections.deque()
143
144    def __str__(self):
145        return "SAP  1"
146
147    @property
148    def mode(self):
149        return LOGICAL_DATA_LINK
150
151    def resolve(self, name):
152        with self.resp:
153            if self.snl is None: return None
154            log.debug("resolve service name '{0}'".format(name))
155            try: return self.snl[name]
156            except KeyError: pass
157            tid = random.choice(self.tids)
158            self.tids.remove(tid)
159            self.sdreq.append((tid, name))
160            while not self.snl is None and not name in self.snl:
161                self.resp.wait()
162            return None if self.snl is None else self.snl[name]
163
164    #
165    # enqueue() and dequeue() are called from llc run thread
166    #
167    def enqueue(self, pdu):
168        with self.llc.lock:
169            if isinstance(pdu, ServiceNameLookup) and not self.snl is None:
170                for tid, sap in pdu.sdres:
171                    try: name = self.sent[tid]
172                    except KeyError: pass
173                    else:
174                        log.debug("resolved '{0}' to remote addr {1}"
175                                  .format(name, sap))
176                        self.snl[name] = sap
177                        self.tids.append(tid)
178                        self.resp.notify_all()
179                for tid, name in pdu.sdreq:
180                    try: sap = self.llc.snl[name]
181                    except KeyError: sap = 0
182                    self.sdres.append((tid, sap))
183
184    def dequeue(self, max_size):
185        if max_size < 2:
186            return None
187        with self.llc.lock:
188            if len(self.sdres) > 0 or len(self.sdreq) > 0:
189                pdu = ServiceNameLookup(dsap=1, ssap=1)
190                max_size -= len(pdu)
191                while max_size > 0:
192                    try: pdu.sdres.append(self.sdres.popleft())
193                    except IndexError: break
194                for i in range(len(self.sdreq)):
195                    tid, name = self.sdreq[0]
196                    if 1 + len(name) > max_size:
197                        self.sdreq.rotate(-1)
198                    else:
199                        pdu.sdreq.append(self.sdreq.popleft())
200                        self.sent[tid] = name
201                return pdu
202            if len(self.dmpdu) > 0 and max_size >= 2:
203                return self.dmpdu.popleft()
204
205    def shutdown(self):
206        with self.llc.lock:
207            self.snl = None
208            self.resp.notify_all()
209
210class LogicalLinkControl(threading.Thread):
211    def __init__(self, config):
212        super(LogicalLinkControl, self).__init__()
213        self.lock = threading.RLock()
214        self.cfg = dict()
215        self.cfg['recv-miu'] = config.get('recv-miu', 248)
216        self.cfg['send-lto'] = config.get('send-lto', 500)
217        self.cfg['send-agf'] = config.get('send-agf', True)
218        self.snl = dict({"urn:nfc:sn:sdp" : 1})
219        self.sap = 64 * [None]
220        self.sap[0] = ServiceAccessPoint(0, self)
221        self.sap[1] = ServiceDiscovery(self)
222
223    @property
224    def parameter_string(self):
225        miu = self.cfg['recv-miu']
226        lto = self.cfg['send-lto']
227        wks = 1+sum(sorted([1<<sap for sap in self.snl.values() if sap < 15]))
228        pax = ParameterExchange(version=(1,1), miu=miu, lto=lto, wks=wks)
229        return "Ffm" + pax.to_string().lstrip("\x00\x40")
230
231    def activate(self, mac):
232        info = ["LLCP Link established, I'm the DEP {0}".format(mac.role)]
233
234        pax = "\x00\x40" + self.parameter_string.lstrip("Ffm")
235        pax = ProtocolDataUnit.from_string(pax)
236        info.append("Local LLCP Settings")
237        info.append("  LLCP Version: {0[0]}.{0[1]}".format(pax.version))
238        info.append("  Link Timeout: {0} ms".format(pax.lto))
239        info.append("  Max Inf Unit: {0} octet".format(pax.miu))
240        info.append("  Service List: {0:016b}".format(pax.wks))
241
242        pax = "\x00\x40" + mac.general_bytes.lstrip("Ffm")
243        pax = ProtocolDataUnit.from_string(pax)
244        info.append("Remote LLCP Settings")
245        info.append("  LLCP Version: {0[0]}.{0[1]}".format(pax.version))
246        info.append("  Link Timeout: {0} ms".format(pax.lto))
247        info.append("  Max Inf Unit: {0} octet".format(pax.miu))
248        info.append("  Service List: {0:016b}".format(pax.wks))
249
250        self.mac = mac
251        self.cfg['rcvd-ver'] = pax.version
252        self.cfg['send-miu'] = pax.miu
253        self.cfg['recv-lto'] = pax.lto
254        self.cfg['send-wks'] = pax.wks
255        self.cfg['send-lsc'] = pax.lsc
256        log.debug("llc cfg {0}".format(self.cfg))
257        log.info('\n'.join(info))
258
259    def shutdown(self):
260        log.debug("shutdown requested")
261        if self.sap[0]:
262            self.sap[0].send(Disconnect(dsap=0, ssap=0))
263
264    def run(self):
265        def shutdown_clients(sap):
266            for i in range(63, -1, -1):
267                if not sap[i] is None:
268                    log.debug("closing service access point %d" % i)
269                    sap[i].shutdown()
270                    sap[i] = None
271
272        link_terminate_pdu = Disconnect(dsap=0, ssap=0)
273        link_terminate_str = link_terminate_pdu.to_string()
274        link_symmetry_pdu = Symmetry()
275
276        recv_timeout = self.cfg['recv-lto'] + 50
277        send_timeout = self.cfg['send-lto'] / 2
278
279        recv_symm_count = 0
280        recv_symm_level = 10
281
282        if self.mac.role == "Initiator":
283            pdu = self._collect()
284            while True:
285                if pdu is None:
286                    pdu = Symmetry()
287                if pdu == link_terminate_pdu:
288                    log.info("shutdown on local request")
289                    log.debug("SEND " + str(pdu))
290                    try: self.mac.exchange(pdu.to_string(), timeout=1)
291                    except IOError: pass
292                    shutdown_clients(self.sap)
293                    break
294                log.debug("SEND " + str(pdu))
295                try: data = self.mac.exchange(pdu.to_string(), recv_timeout)
296                except IOError as error:
297                    log.debug("in exchange => IOError {0}".format(error))
298                    data = None
299                if data is None or data == link_terminate_str:
300                    if data: log.info("shutdown on remote request")
301                    else: log.info("shutdown on link disruption")
302                    shutdown_clients(self.sap)
303                    break
304                pdu = ProtocolDataUnit.from_string(data)
305                log.debug("RECV " + str(pdu))
306                if pdu == link_symmetry_pdu:
307                    recv_symm_count += 1
308                else:
309                    recv_symm_count = 0
310                self._dispatch(pdu)
311                pdu = self._collect()
312                if pdu is None and recv_symm_count >= recv_symm_level:
313                    time.sleep(0.001 * send_timeout)
314                    pdu = self._collect()
315
316        if self.mac.role == "Target":
317            while True:
318                try: data = self.mac.wait_command(recv_timeout)
319                except IOError as error:
320                    log.debug("wait_command: IOError {0}".format(str(error)))
321                    data = None
322                if data:
323                    pdu = ProtocolDataUnit.from_string(data)
324                    log.debug("RECV " + str(pdu))
325                if data is None or data == link_terminate_str:
326                    if data: log.info("shutdown on remote request")
327                    else: log.info("shutdown on link disruption")
328                    shutdown_clients(self.sap)
329                    break
330                #FIXME: must be coordinated with DEP RWT
331                #if pdu == link_symmetry_pdu:
332                #    recv_symm_count += 1
333                #else:
334                #    recv_symm_count = 0
335                self._dispatch(pdu)
336                pdu = self._collect()
337                if pdu is None and recv_symm_count >= recv_symm_level:
338                    time.sleep(0.001 * send_timeout)
339                    pdu = self._collect()
340                if pdu is None:
341                    pdu = Symmetry()
342                log.debug("SEND " + str(pdu))
343                try: self.mac.send_response(pdu.to_string(), recv_timeout)
344                except IOError as err:
345                    if not pdu == link_terminate_pdu:
346                        log.debug("send_response: IOError {0}".format(err))
347                        log.info("shutdown on link disruption")
348                    shutdown_clients(self.sap)
349                    break
350
351        log.debug("llc run thread terminated")
352
353    def _collect(self):
354        pdu_list = list()
355        max_data = None
356        with self.lock:
357            active_sap_list = [sap for sap in self.sap if sap is not None]
358            for sap in active_sap_list:
359                #log.debug("query sap {0}, max_data={1}"
360                #          .format(sap, max_data))
361                pdu = sap.dequeue(max_data if max_data else 2179)
362                if not pdu is None:
363                    if self.cfg['send-agf'] == False:
364                        return pdu
365                    pdu_list.append(pdu)
366                    if max_data is None:
367                        max_data = self.cfg["send-miu"] + 2
368                    max_data -= len(pdu)
369                    if max_data < bool(len(pdu_list)==1) * 2 + 2 + 2:
370                        break
371            else: max_data = self.cfg["send-miu"] + 2
372
373            for sap in active_sap_list:
374                if sap.mode == DATA_LINK_CONNECTION:
375                    pdu = sap.sendack(max_data)
376                    if not pdu is None:
377                        if self.cfg['send-agf'] == False:
378                            return pdu
379                        pdu_list.append(pdu)
380                        max_data -= len(pdu)
381                        if max_data < bool(len(pdu_list)==1) * 2 + 2 + 3:
382                            break
383
384        if len(pdu_list) > 1:
385            return AggregatedFrame(aggregate=pdu_list)
386        if len(pdu_list) == 1:
387            return pdu_list[0]
388        return None
389
390    def _dispatch(self, pdu):
391        if isinstance(pdu, Symmetry):
392            return
393
394        if isinstance(pdu, AggregatedFrame):
395            if pdu.dsap == 0 and pdu.ssap == 0:
396                [log.debug("     " + str(p)) for p in pdu]
397                [self._dispatch(p) for p in pdu]
398            return
399
400        if isinstance(pdu, Connect) and pdu.dsap == 1:
401            # connect-by-name
402            addr = self.snl.get(pdu.sn)
403            if not addr or self.sap[addr] is None:
404                log.debug("no service named '{0}'".format(pdu.sn))
405                pdu = DisconnectedMode(pdu.ssap, 1, reason=2)
406                self.sap[1].dmpdu.append(pdu)
407                return
408            pdu = Connect(dsap=addr, ssap=pdu.ssap, rw=pdu.rw, miu=pdu.miu)
409
410        with self.lock:
411            sap = self.sap[pdu.dsap]
412            if sap:
413                sap.enqueue(pdu)
414                return
415
416        log.debug("discard PDU {0}".format(str(pdu)))
417        return
418
419    def resolve(self, name):
420        return self.sap[1].resolve(name)
421
422    def socket(self, socket_type):
423        if socket_type == RAW_ACCESS_POINT:
424            return RawAccessPoint(self.cfg["send-miu"], self.cfg["recv-miu"])
425        if socket_type == LOGICAL_DATA_LINK:
426            return LogicalDataLink(self.cfg["send-miu"], self.cfg["recv-miu"])
427        if socket_type == DATA_LINK_CONNECTION:
428            return DataLinkConnection()
429
430    def setsockopt(self, socket, option, value):
431        if not isinstance(socket, TransmissionControlObject):
432            raise Error(errno.ENOTSOCK)
433        return socket.setsockopt(option, value)
434
435    def getsockopt(self, socket, option):
436        if not isinstance(socket, TransmissionControlObject):
437            raise Error(errno.ENOTSOCK)
438        if isinstance(socket, LogicalDataLink):
439            if option == SO_SNDMIU:
440                return self.cfg["send-miu"]
441            if option == SO_RCVMIU:
442                return self.cfg["recv-miu"]
443        return socket.getsockopt(option)
444
445    def bind(self, socket, addr_or_name=None):
446        """Bind a socket to an address or service name. 
447        """
448        if not isinstance(socket, TransmissionControlObject):
449            raise Error(errno.ENOTSOCK)
450        if not socket.addr is None:
451            raise Error(errno.EINVAL)
452        if addr_or_name is None:
453            self._bind_by_none(socket)
454        elif type(addr_or_name) is IntType:
455            self._bind_by_addr(socket, addr_or_name)
456        elif type(addr_or_name) is StringType:
457            self._bind_by_name(socket, addr_or_name)
458        else: raise Error(errno.EFAULT)
459
460    def _bind_by_none(self, socket):
461        with self.lock:
462            try: addr = 32 + self.sap[32:64].index(None)
463            except ValueError: raise Error(errno.EAGAIN)
464            else:
465                socket.bind(addr)
466                self.sap[addr] = ServiceAccessPoint(addr, self)
467                self.sap[addr].insert_socket(socket)
468
469    def _bind_by_addr(self, socket, addr):
470        with self.lock:
471            if addr in range(32, 64):
472                if self.sap[addr] is None:
473                    socket.bind(addr)
474                    self.sap[addr] = ServiceAccessPoint(addr, self)
475                    self.sap[addr].insert_socket(socket)
476                else: raise Error(errno.EADDRINUSE)
477            else: raise Error(errno.EACCES)
478
479    def _bind_by_name(self, socket, name):
480        if not (name.startswith("urn:nfc:sn") or
481                name.startswith("urn:nfc:xsn") or
482                name == "com.android.npp"): # invalid name but legacy
483            raise Error(errno.EFAULT)
484        with self.lock:
485            if self.snl.get(name) != None:
486                raise Error(errno.EADDRINUSE)
487            addr = wks_map.get(name)
488            if addr is None:
489                try: addr = 16 + self.sap[16:32].index(None)
490                except ValueError: raise Error(errno.EADDRNOTAVAIL)
491            socket.bind(addr)
492            self.sap[addr] = ServiceAccessPoint(addr, self)
493            self.sap[addr].insert_socket(socket)
494            self.snl[name] = addr
495
496    def connect(self, socket, dest):
497        if not isinstance(socket, TransmissionControlObject):
498            raise Error(errno.ENOTSOCK)
499        if not socket.is_bound:
500            self.bind(socket)
501        socket.connect(dest)
502        log.debug("connected ({dlc.addr} ===> {dlc.peer})"
503                  .format(dlc=socket))
504
505    def listen(self, socket, backlog):
506        if not isinstance(socket, TransmissionControlObject):
507            raise Error(errno.ENOTSOCK)
508        if not isinstance(socket, DataLinkConnection):
509            raise Error(errno.EOPNOTSUPP)
510        if not type(backlog) == IntType:
511            raise TypeError("backlog must be integer")
512        if backlog < 0:
513            raise ValueError("backlog mmust not be negative")
514        backlog = min(backlog, 16)
515        if not socket.is_bound:
516            self.bind(socket)
517        socket.listen(backlog)
518
519    def accept(self, socket):
520        if not isinstance(socket, TransmissionControlObject):
521            raise Error(errno.ENOTSOCK)
522        if not isinstance(socket, DataLinkConnection):
523            raise Error(errno.EOPNOTSUPP)
524        while True:
525            client = socket.accept()
526            if not client.is_bound:
527                self.bind(client)
528            if self.sap[client.addr].insert_socket(client):
529                log.debug("new data link connection ({0} <=== {1})"
530                          .format(client.addr, client.peer))
531                return client
532            else:
533                pdu = DisconnectedMode(client.peer, socket.addr, reason=0x20)
534                super(DataLinkConnection, socket).send(pdu)
535
536    def send(self, socket, message):
537        return self.sendto(socket, message, socket.peer)
538
539    def sendto(self, socket, message, dest):
540        if not isinstance(socket, TransmissionControlObject):
541            raise Error(errno.ENOTSOCK)
542        if isinstance(socket, RawAccessPoint):
543            if not isinstance(message, ProtocolDataUnit):
544                raise TypeError("message must be a pdu on raw access point")
545            if not socket.is_bound:
546                self.bind(socket)
547            return socket.send(message)
548        if not type(message) == StringType:
549            raise TypeError("sendto() argument *message* must be a string")
550        if isinstance(socket, LogicalDataLink):
551            if dest is None:
552                raise Error(errno.EDESTADDRREQ)
553            if not socket.is_bound:
554                self.bind(socket)
555            return socket.sendto(message, dest)
556        if isinstance(socket, DataLinkConnection):
557            return socket.send(message)
558
559    def recv(self, socket):
560        message, sender = self.recvfrom(socket)
561        return message
562
563    def recvfrom(self, socket):
564        if not isinstance(socket, TransmissionControlObject):
565            raise Error(errno.ENOTSOCK)
566        if not (socket.addr and self.sap[socket.addr]):
567            raise Error(errno.EBADF)
568        if isinstance(socket, RawAccessPoint):
569            return (socket.recv(), None)
570        if isinstance(socket, LogicalDataLink):
571            return socket.recvfrom()
572        if isinstance(socket, DataLinkConnection):
573            return (socket.recv(), socket.peer)
574
575    def poll(self, socket, event, timeout=None):
576        if not isinstance(socket, TransmissionControlObject):
577            raise Error(errno.ENOTSOCK)
578        if not (socket.addr and self.sap[socket.addr]):
579            raise Error(errno.EBADF)
580        return socket.poll(event, timeout)
581
582    def close(self, socket):
583        if not isinstance(socket, TransmissionControlObject):
584            raise Error(errno.ENOTSOCK)
585        if socket.is_bound:
586            self.sap[socket.addr].remove_socket(socket)
587        else: socket.close()
588
589    def getsockname(self, socket):
590        if not isinstance(socket, TransmissionControlObject):
591            raise Error(errno.ENOTSOCK)
592        return socket.addr
593
594    def getpeername(self, socket):
595        if not isinstance(socket, TransmissionControlObject):
596            raise Error(errno.ENOTSOCK)
597        return socket.peer