sma2-explore

#! /usr/bin/python3
#
# sma2-explore - Interactive tool for analyzing SMAData2 protocol
# Copyright (C) 2014 David Gibson <david@gibson.dropbear.id.au>
#
# This program is free software; you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation; either version 2 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
#
# You should have received a copy of the GNU General Public License along
# with this program; if not, write to the Free Software Foundation, Inc.,
# 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

from __future__ import print_function
from __future__ import division
import sys
import os
import signal
import readline
import time

from smadata2.inverter.smabluetooth import Connection
from smadata2.inverter.smabluetooth import OTYPE_HELLO, OTYPE_ERROR, \
    OTYPE_VARVAL, OTYPE_GETVAR, OTYPE_PPP, OTYPE_PPP2, OVAR_SIGNAL
from smadata2.inverter.smabluetooth import bytes2int, int2bytes16


class Quit(Exception):
    def __init__(self):
        super(Quit, self).__init__("Quit at user request")


def hexdump(data, prefix):
    s = ''
    for i, b in enumerate(data):
        if (i % 16) == 0:
            s += '%s%04x: ' % (prefix, i)
        s += '%02X' % b
        if (i % 16) == 15:
            s += '\n'
        elif (i % 16) == 7:
            s += '-'
        else:
            s += ' '
    if s and (s[-1] == '\n'):
        s = s[:-1]
    return s


def dump_outer(prefix, from_, to_, type_, payload):
    print("%s%s -> %s TYPE %02X" % (prefix, from_, to_, type_))
    prefix = prefix + "    "
    if type_ == OTYPE_HELLO:
        print("%sHELLO!" % prefix)
    elif type_ == OTYPE_ERROR:
        print("%sERROR in previous packet:" % prefix)
        print(hexdump(payload[4:], prefix + "    "))


def dump_ppp_raw(prefix, payload):
    info = ""
    if payload[0] == 0x7e:
        info += " frame begins"
    if payload[-1] == 0x7e:
        info += " frame ends"
    print("%sPartial PPP data%s" % (prefix, info))


def dump_ppp(prefix, protocol, payload):
    print("%sPPP frame; protocol 0x%04x [%d bytes]"
          % (prefix, protocol, len(payload)))
    print(hexdump(payload, prefix + "    "))


def a65602str(addr):
    return "%02X.%02X.%02X.%02X.%02X.%02X" % tuple(addr)


def dump_6560(prefix, from2, to2, a2, b1, b2, c1, c2, tag,
              type_, subtype, arg1, arg2, extra,
              response, error, pktcount, first):
    print("%sSMA INNER PROTOCOL PACKET" % prefix)
    print("%s    %s => %s" % (prefix, a65602str(from2), a65602str(to2)))
    print("%s    control %02X %02X %02X %02X %02X"
          % (prefix, a2, b1, b2, c1, c2))
    if error:
        print("%s    ERROR!! code 0x%04x" % (prefix, error))
    s = []
    if first:
        s.append('first')
    if pktcount:
        s.append('%d packets left' % pktcount)
    else:
        s.append('last')
    s = ', '.join(s)
    print("%s    tag %04x (%s)" % (prefix, tag, s))
    if response:
        cr = "response"
    else:
        cr = "command"
    print("%s    %s 0x%04x subtype 0x%04x" % (prefix, cr, type_, subtype))
    print(hexdump(extra, prefix + "    "))


class SMAData2CLI(Connection):
    def __init__(self, addr):
        super(SMAData2CLI, self).__init__(addr)
        print("Connected %s -> %s"
              % (self.local_addr, self.remote_addr))
        self.rxpid = None

    def __del__(self):
        if self.rxpid:
            os.kill(self.rxpid, signal.SIGTERM)

    def rxloop(self):
        while True:
            self.rx()

    def start_rxthread(self):
        self.rxpid = os.fork()
        if self.rxpid == 0:
            while True:
                try:
                    self.rxloop()
                except Exception as e:
                    print(e)

    def rx_raw(self, pkt):
        print("\n" + hexdump(pkt, "Rx< "))
        super(SMAData2CLI, self).rx_raw(pkt)

    def rx_outer(self, from_, to_, type_, payload):
        dump_outer("Rx<     ", from_, to_, type_, payload)
        if type_ == OTYPE_VARVAL:
            varid = bytes2int(payload[:2])
            print("Rx<         VARVAL 0x%02x" % varid)
            if varid == OVAR_SIGNAL:
                print("Rx<             Signal level %0.1f%%"
                      % (payload[4] / 255 * 100))
        elif (type_ in [OTYPE_PPP, OTYPE_PPP2]):
            dump_ppp_raw("Rx<         ", payload)
        super(SMAData2CLI, self).rx_outer(from_, to_,
                                          type_, payload)

    def rx_ppp(self, from_, protocol, payload):
        dump_ppp("Rx<         ", protocol, payload)
        super(SMAData2CLI, self).rx_ppp(from_, protocol, payload)

    def rx_6560(self, from2, to2, a2, b1, b2, c1, c2, tag,
                type_, subtype, arg1, arg2, extra,
                response, error, pktcount, first):
        dump_6560("Rx<             ", from2, to2, a2, b1, b2, c1, c2, tag,
                  type_, subtype, arg1, arg2, extra,
                  response, error, pktcount, first)
        super(SMAData2CLI, self).rx_6560(from2, to2, a2, b1, b2, c1, c2,
                                         tag, type_, subtype,
                                         arg1, arg2,
                                         extra, response, error,
                                         pktcount, first)

    def tx_raw(self, pkt):
        super(SMAData2CLI, self).tx_raw(pkt)
        print("\n" + hexdump(pkt, "Tx> "))

    def tx_outer(self, from_, to_, type_, payload):
        super(SMAData2CLI, self).tx_outer(from_, to_,
                                          type_, payload)
        dump_outer("Tx>     ", from_, to_, type_, payload)
        if type_ == OTYPE_GETVAR:
            varid = bytes2int(payload[:2])
            print("Tx>         GETVAR 0x%02x" % varid)

    def tx_ppp(self, to_, protocol, payload):
        super(SMAData2CLI, self).tx_ppp(to_, protocol, payload)
        dump_ppp("Tx>         ", protocol, payload)

    def tx_6560(self, from2, to2, a2, b1, b2, c1, c2, tag,
                type_, subtype, arg1, arg2, extra=bytearray(),
                response=False, error=0, pktcount=0, first=True):
        super(SMAData2CLI, self).tx_6560(from2, to2, a2, b1, b2, c1, c2,
                                         tag, type_, subtype,
                                         arg1, arg2, extra, response,
                                         error, pktcount, first)
        dump_6560("Tx>             ", from2, to2, a2, b1, b2, c1, c2, tag,
                  type_, subtype, arg1, arg2, extra,
                  response, error, pktcount, first)

    def cli(self):
        while True:
            sys.stdout.write("SMA2 %s >> " % self.remote_addr)
            try:
                line = input().split()
            except EOFError:
                return

            if not line:
                continue

            cmd = 'cmd_' + line[0].lower()
            try:
                getattr(self, cmd)(*line[1:])
            except Quit:
                return
            except Exception as e:
                print("ERROR! %s" % e)

    def cmd_quit(self):
        raise Quit()

    def cmd_raw(self, *args):
        pkt = bytearray([int(x, 16) for x in args])
        self.tx_raw(pkt)

    def parse_addr(self, addr):
        if addr.lower() == "zero":
            return "00:00:00:00:00:00"
        elif addr.lower() == "local":
            return self.local_addr
        elif addr.lower() == "remote":
            return self.remote_addr
        elif addr.lower() == "bcast":
            return "ff:ff:ff:ff:ff:ff"
        else:
            return addr

    def cmd_send(self, from_, to_, type_, *args):
        from_ = self.parse_addr(from_)
        to_ = self.parse_addr(to_)
        type_ = int(type_, 16)
        payload = bytearray([int(x, 16) for x in args])
        self.tx_outer(from_, to_, type_, payload)

    def cmd_hello(self):
        self.tx_outer("00:00:00:00:00:00", self.remote_addr, OTYPE_HELLO,
                      bytearray('\x00\x04\x70\x00\x01\x00\x00\x00' +
                                '\x00\x01\x00\x00\x00'))

    def cmd_getvar(self, varid):
        varid = int(varid, 16)
        self.tx_outer("00:00:00:00:00:00", self.remote_addr, OTYPE_GETVAR,
                      int2bytes16(varid))

    def cmd_ppp(self, protocol, *args):
        protocol = int(protocol, 0)
        payload = bytearray([int(x, 16) for x in args])
        self.tx_ppp("ff:ff:ff:ff:ff:ff", protocol, payload)

    def cmd_send2(self, *args):
        bb = [int(x, 16) for x in args]
        a2 = bb[0]
        b1, b2 = bb[1], bb[2]
        c1, c2 = bb[3], bb[4]
        type_, subtype = bb[5], bb[6]
        arg1, arg2 = bb[7], bb[8]
        extra = bytearray(bb[9:])

        self.tx_6560(self.local_addr2, self.BROADCAST2,
                     a2, b1, b2, c1, c2, self.gettag(),
                     type_, subtype, arg1, arg2, extra)

    def cmd_logon(self, password='0000', timeout=900):
        timeout = int(timeout)
        self.tx_logon(password, timeout)

    def cmd_gdy(self):
        self.tx_gdy()

    def cmd_yield(self):
        self.tx_yield()

    def cmd_historic(self, fromtime=None, totime=None):
        if fromtime is None and totime is None:
            fromtime = 1356958800		# 1 Jan 2013
            totime = int(time.time())		# now
        else:
            fromtime = int(fromtime)
            totime = int(totime)
        self.tx_historic(fromtime, totime)


if __name__ == '__main__':
    if len(sys.argv) != 2:
        print("Usage: %s <BD addr>" % sys.argv[0], file=sys.stderr)
        sys.exit(1)

    cli = SMAData2CLI(sys.argv[1])

    cli.start_rxthread()
    cli.cli()