#
# decode.py - Data stream decoders
#
# Copyright (C) 2008 by OpenMoko, Inc.
# Written by Werner Almesberger <werner@openmoko.org>
# All Rights Reserved
#
# 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.
#

#
# New-style decoders:
#
# - return a list of tuples
# - each tuple: (#bits, short, fmt, long)
#   - #bits: number of bits this tuple decodes
#   - short: short description
#   - fmt: printf-style format string for text-only display
#   - long: long description, e.g., for tooltips
#


import tmc.crc


# One CRC instance can be shared by all decoders

g_crc = tmc.crc.crc()


d_table = {}


def d_bits(bits):
    s = ""
    for b in bits:
	s += str(b)
    return s


def d_byte(bit_pos, bits):
    s = ""

    for i in range(0, len(bits)-7, 8):
	if len(s):
	    s += "."
        v = 0
	for j in range(0, 8):
	    if bits[i+j] == "X":
		s += "??"
		break
	    v |= bits[i+j] << bit_pos(j)
	else:
	    s += "%02X" % v

    if len(bits) & 7:
	s += "|"+d_bits(bits[len(bits) & ~7:len(bits)])
    return s


def d_byte_lsb(bits):
    return d_byte(lambda p: p, bits)


def d_byte_msb(bits):
    return d_byte(lambda p: 7-p, bits)


def d_counter(bits):
    return "%d" % len(bits)


def d_msb(bits):
    v = 0
    pos = len(bits)-1
    for b in bits:
	if b == "X":
	    return None
	v |= b << pos
	pos -= 1
    return v


def decode_address(addr):

    # CCCR
    if addr < 0x14:
	return (
	    "Rev", "SDR", "IOE", "IOR",
	    "IEN", "INT", "Abr", "Bus",
            "Cap", "CIS", "CIS", "CIS",
	    "Sus", "Sel", "EXF", "RDY",
	    "BSZ", "BSZ", "PWR", "HSp")[addr]

    # FBR
    if addr >= 0x100 and addr < 0x800 and (addr & 0xff) < 0x12:
	return ("F%d" % (addr >> 8))+(
	    "FIC", "EFC", "Pwr", "RFU",
	    "RFU", "RFU", "RFU", "RFU",
	    "RFU", "CIS", "CIS", "CIS",
	    "CSA", "CSA", "CSA", "DCA",
	    "BSZ", "BSZ")[addr & 0xff]
	  
    return None


def decode_arg(cmd, arg):
    if cmd == 52 or cmd == 53:
	write = (arg >> 31) & 1
	s = ("R", "W")[write]
	s += str((arg >> 28) & 7)
	addr = (arg >> 9) & 0x1ffff
	name = decode_address(addr)
	if name is None:
	    addr = "0x%X" % addr
	else:
	    addr = name+("(0x%X)" % addr)
    if cmd == 52:
	s += (" ", "W")[(arg >> 27) & 1]
	s += addr
	if write:
	    s += ",0x%X" % (arg & 0xff)
	return s
    if cmd == 53:
	s += ("b", "B")[(arg >> 27) & 1]
	s += ("=", "+")[(arg >> 26) & 1]
	s += addr
	return s+"*%d" % (arg & 0x1ff)
    return None


def d_crc7_msb(payload):
    crc = 0
    while len(payload):
	v = 0
	for b in range(0, 8):
	    if payload[0] == "X":
		return None
	    v = (v << 1) | payload.pop(0)
	crc = g_crc.crc7(crc, [v])
    return crc


def d_sdio_cmd(bits, stop = False):
    if bits[0]:
	s = "~"
    else:
	s = ""

    while len(bits) and bits[0]:
	del bits[0]

    # We access the payload as soon as the CRC has been read, but possibly
    # before we also have the stop bit.

    if len(bits) >= 47:
	payload = bits[0:40]

    if len(bits) == 0:
	return s
    s += "["
    del bits[0]

    if len(bits) == 0:
	return s
    if bits[0] and bits[0] != "X":
	s += ">"
    else:
	s += "?"
	if stop:
	    return s
    del bits[0]

    if len(bits) == 0:
	return s
    if len(bits) < 6:
	return s+"|"+d_bits(bits)
    cmd = d_msb(bits[0:6])
    if cmd is None:
	s += "CMD??"
	if stop:
	    return s
    else:
	s += "CMD%d" % cmd
    del bits[0:6]

    if len(bits) == 0:
	return s
    if len(bits) < 32:
	return s+"("+d_byte_msb(bits)
    arg = d_msb(bits[0:32])
    if cmd is None or arg is None:
	arg_pretty = None
    else:
	arg_pretty = decode_arg(cmd, arg)
    if arg_pretty is None:
	s += "("+d_byte_msb(bits[0:32])+")"
	if stop and arg is None:
	    return s
    else:
	s += "("+arg_pretty+")"
    del bits[0:32]

    if len(bits) == 0:
	return s
    if len(bits) < 7:
	return s+"|"+d_bits(bits)
    crc = d_msb(bits[0:7])
    if crc is None:
	s += "CRC??"
	if stop:
	    return s
    else:
	check = d_crc7_msb(payload)
	if check is None or check == crc:
	    s += "CRC%02X" % crc
	    if check is None:
		s += "?"
	    elif check != crc:
		s += "/%02X" % check
    del bits[0:7]

    if len(bits) == 0:
	return s
    if bits[0] and bits[0] != "X":
	s += "]"
    else:
	s += "?"
    del bits[0]

    if stop:
	return s

    if len(bits):
	s += "|"+d_bits(bits)

    return s


def d_sdio_resp_spi(bits):
    return "SDIO"


def d_sdio_resp_sd(bits):
    return "SDIO"


def d_sdio_r5_spi(bits, stop = False):
    if bits[0]:
	s = "~"
    else:
	s = ""

    while len(bits) and bits[0]:
	del bits[0]

    if len(bits) == 0:
	return s
    s += "["
    del bits[0]

    if len(bits) == 0:
	return s
    if bits[0]:
	s += "Prm"
    else:
	s += "-"
    del bits[0]

    if len(bits) == 0:
	return s
    if bits[0]:
	s += "???"
	if stop:
	    return s
    else:
	s += "-"
    del bits[0]

    if len(bits) == 0:
	return s
    if bits[0]:
	s += "Fn#"
    else:
	s += "-"
    del bits[0]

    if len(bits) == 0:
	return s
    if bits[0]:
	s += "Crc"
    else:
	s += "-"
    del bits[0]

    if len(bits) == 0:
	return s
    if bits[0]:
	s += "Cmd"
    else:
	s += "-"
    del bits[0]

    if len(bits) == 0:
	return s
    if bits[0]:
	s += "???"
	if stop:
	    return s
    else:
	s += "-"
    del bits[0]

    if len(bits) == 0:
	return s
    if bits[0]:
	s += "Idl"
    else:
	s += "-"
    del bits[0]

    s += d_byte_msb(bits[0:8])
    if len(bits) >= 8:
	s += "]"
    if len(bits) > 8 and not stop:
	s += "|"+d_bits(bits[8:])

    return s


def usb_field(raw, bits):
    bit = 0
    got = 0
    byte = 0
    while bit != len(raw):
	if raw[bit] == "S":
	    bit += 1
	    continue
	if raw[bit] == "X":
	    return None
	elif raw[bit]:
	    byte |= 1 << got
	got += 1
	bit += 1
	if got == bits:
	    del raw[0:bit]
	    return byte
    return None


def usb_token(raw):
    addr = usb_field(raw, 7)
    if addr is None:
	return ""
    s = "%d" % addr

    ep = usb_field(raw, 4)
    if ep is None:
	return s
    s += "/%d" % ep

    expect = g_crc.crc5(0x1f, ep << 7 | addr, 11) ^ 0x1f

    crc = usb_field(raw, 5)
    if crc is None:
	return s

    if crc == expect and len(raw) == 0:
	return s

    s += ",CRC%02X" % crc
    if crc != expect:
	s += "/%02X" % expect

    return s


def usb_sof(raw):
    frame = usb_field(raw, 11)
    if frame is None:
	return ""
    s = "%03X" % frame

    expect = g_crc.crc5(0x1f, frame, 11) ^ 0x1f

    crc = usb_field(raw, 5)
    if crc is None:
	return s

    if crc == expect and len(raw) == 0:
	return s

    s += ",CRC%02X" % crc
    if crc != expect:
	s += "/%02X" % expect

    return s


def usb_data(raw):
    bytes = []
    while True:
	byte = usb_field(raw, 8)
	if byte is None:
	    break
	bytes.append(byte)

    s = ""
    if len(bytes) < 2:
	crc = None
    else:
	crc = (bytes[-2], bytes[-1])
	del bytes[-1]
	del bytes[-1]
	expect = g_crc.crc16(0xffff, bytes) ^ 0xffff

    for b in bytes:
	if s:
	    s += "."
	else:
	    s += ":"
	s += "%02X" % b

    if crc is None:
	return s

    crc = crc[0] | crc[1] << 8
    if crc == expect and len(raw) == 0:
	return s

    if s:
	s += ","
    else:
	s += ":"
    s += "CRC%04X" % crc
    if crc != expect:
	s += "/%04X" % expect

    return s


def usb_handshake(raw):
    return ""


def usb_packet(raw):
    sync = usb_field(raw, 8)
    if sync is None:
	return ""
    if sync != 0x80:
	return "%02X?" % sync

    pid = usb_field(raw, 8)
    if pid is None:
	return "SYNC"

    if pid == 0xe1:
	return "OUT"+usb_token(raw)
    if pid == 0x69:
	return "IN"+usb_token(raw)
    if pid == 0xa5:
	return "SOF"+usb_sof(raw)
    if pid == 0x2d:
	return "SETUP"+usb_token(raw)

    if pid == 0xc3:
	return "DATA0"+usb_data(raw)
    if pid == 0x4b:
	return "DATA1"+usb_data(raw)
    if pid == 0x87:
	return "DATA2"+usb_data(raw)
    if pid == 0x0f:
	return "MDATA"+usb_data(raw)

    if pid == 0xd2:
	return "ACK"
    if pid == 0x5a:
	return "NAK"
    if pid == 0x1e:
	return "STALL"

    # we don't decode NYET and special packets

    return "SYNC"


def d_usb(bits):
    if bits[0]:
	s = "~"
    else:
	s = ""

    while len(bits) and bits[0]:
	del bits[0]

    raw = []
    last = 1
    ones = 0
    for b in bits:
	if b == "X":
	    raw.append("X")
	elif b == last:
	    raw.append(1)
	    ones += 1
	else:
	    if ones == 6:
		raw.append("S")
	    else:
		raw.append(0)
	    ones = 0
	last = b

    s += usb_packet(raw)

    if len(raw):
	s += "|"
	for b in raw:
	    s += str(b)

    return s


def d_usb_stream(dp, dm):
    s = ""

    packet = []
    while len(dp):
	if dp[0] == "X" or dm[0] == "X":
	    packet.append("X")
	elif not dp[0] and not dm[0]:
	    if len(packet):
		s += "["+d_usb(packet)+"]"
	    packet = []
	elif dp[0] and dm[0]:
	    if len(packet):
		s += "["+d_usb(packet)+"+"
	    packet = []
	    s += "SE1"
	else:
	    packet.append(dp[0])
	del dp[0]
	del dm[0]

    if len(packet):
	s += "["+d_usb(packet)+"+"

    return s


def d_usb_dp(bits):
    if d_usb_dm in d_table:
	return d_usb_stream(bits, d_table[d_usb_dm])
    else:
	return d_usb(bits)

def d_usb_dm(bits):
    if d_usb_dp in d_table:
	return ""
    inverted = []
    for b in bits:
	if b == "X":
	    inverted.append(b)
	else:
	    inverted.append(1-b)
    return d_usb(inverted)


def d_i2c_payload(bits):
    if len(bits) < 7:
	return d_byte_msb(bits)
    s = "%02X" % d_msb(bits[0:7])
    del bits[0:7]

    if not len(bits):
	return s
    if bits[0] == "X":
	s += "X"
    else:
	s += ("W", "R")[bits[0]]
    del bits[0]

    while len(bits):
	if bits[0] == "X":
	    s += "X"
	else:
	    s += ("", "N")[bits[0]]
	del bits[0]

	if len(bits) < 8:
	    return s+d_byte_msb(bits)
	s += "."+d_byte_msb(bits[0:8])
	del bits[0:8]

    return s


def d_i2c_sda(bits):
    if not len(bits):
	return ""
    if bits[0]:
	return "?"
    del bits[0]

    s = "S"
    p = []
    while len(bits) > 1:
	if bits[0] == 1 and bits[1] == 0:
	    s += d_i2c_payload(p)+".Sr"
	    r = []
	elif bits[0] == 0 and bits[1] == 1:
	    return s+d_i2c_payload(p)+".P"
	else:
	    p.append(bits[0])
	del bits[0:2]

    if len(bits):
	p.append(bits[0])
    if len(p):
	s += d_i2c_payload(p)
    return s
 

def d_i2c_scl(bits):
    n = len(bits)
    if n == 0:
	return ""
    else:
	return str(n/2)


def d_set_table(table):
    global d_table

    d_table = table


decoders = [
    #  0123456789abcdef -- maximum length is 16 characters
    ( "LSB->MSB",		d_byte_lsb,		False ),
    ( "MSB->LSB",		d_byte_msb,		False ),
    ( "SDIO CMD",		d_sdio_cmd,		False ),
    ( "SDIO RESP (SD)",		d_sdio_resp_sd,		False ),
    ( "SDIO RESP (SPI)",	d_sdio_resp_spi,	False ),
    ( "SDIO R5 (SPI)",		d_sdio_r5_spi,		False ),
    ( "USB D+",			d_usb_dp,		False ),
    ( "USB D-",			d_usb_dm,		False ),
    ( "I2C SDA",		d_i2c_sda,		False ),
    ( "I2C SCL",		d_i2c_scl,		True  ),
]