rpirena/rpigse.py

#! /usr/bin/ipython -i
# encoding: utf-8

from altera_ctrl import *

sys.ps1="RPiGSE> "
sys.ps2="RPiGSE. "
try:
    get_ipython().prompt_manager.templates = {
        'in':  'In [{color.number}{count}{color.prompt}] RPIGSE> ',
        'in2':                              '   .{dots}. RPIGSE. ',
        'out': 'Out[{color.number}{count}{color.prompt}] RPIGSE> ',
    }
except NameError:
    pass

def icucore_msg(ad=0, sz=0, da=0):
    h = struct.pack(">HH", 0x3c3d, ad + (sz<<14))
    if sz==1:
        h+=struct.pack(">H", da)
    if sz==2:
        h+=struct.pack(">L", da)
    if sz==3:
        h+=struct.pack(">LL", da>>32, da&0xffffffff)
    h += struct.pack(">H", crc1021(0xffff, [ord(b) for b in h]))
    spidev.sync_transfer(cmd='\x8a'+"\x8a".join(h))

sys.path[1:1] = ["../../python"]
import solomsgclass
solomsg = solomsgclass.solomsg(icucore_msg)
solomsg._export(globals())
    
from crc1021 import crc1021

def routing(table={}, arb=False, **k):
    switch = {
        "RxD": (0, { "TxD": 0, "tx":  1, "Rx2": 2, "Rx1": 3,}),
        "rx":  (2, { "TxD": 0, "tx":  1, "Rx2": 2, "Rx1": 3,}),
        "Tx1": (4, { "TxD": 0, "RxD": 1, "px":  2, "tx":  3,}),
        "Tx2": (6, { "TxD": 0, "RxD": 1, "tx":  2, "px":  3,}),
    }
    for kk in switch:
        if not kk in table:
            table[kk] = 3
    for kk in k:
        table[kk] = k[kk]
    for kk in table:
        if table[kk] in switch[kk][1]:
            table[kk] = switch[kk][1][table[kk]]
    m = 0
    for kk in switch:
        m |= table[kk] << switch[kk][0]
    if arb:
        m |= 0x8000
    reg(0x108, m)

def boot(rbf="rpigse.rbf"):
    altera_from_file(rbf)

    # serial bit clock from 96MHz mclk
    MCLK=96000000
    BAUD=115200
    BAUDMODULO = 240
    DIV = MCLK/16/BAUD
    FRAC = (MCLK/16*BAUDMODULO)/BAUD - DIV*BAUDMODULO
    reg(0x810, (DIV-1)*256 + FRAC)
    
    # routing:
    # Setup for sologse.py @/dev/ttyAMA0
    # w/ Rx2 for analog streaming
    # RxD = Rx1 (0x03/0x03)  TxD,  tx, Rx2, Rx1
    # rx  = Rx1 (0x0c/0x0c)  TxD,  tx, Rx2, Rx1
    # Tx1 = TxD (0x00/0x30)  TxD, RxD,  px,  tx
    # Tx2 = px  (0xc0/0xc0)  TxD, RxD,  tx,  px
    routing(RxD="Rx1", rx="Rx1", Tx1="TxD", Tx2="px")
    #reg(0x108, 0xcf)

def pps_en(on=True):
    if on:
        return reg(10, 1)
    else:
        return reg(9,  1)

def readrecordcmd(a, n, w=0):
    return struct.pack(">%dH"%(n+2+w), *( (a,)+(0,)*w+(0x8001,)*n+(0,) ))

def readrecord(a, n, w=0):
    c = readrecordcmd(a, n, w)
    return struct.unpack(">%dH"%n, spidev.sync_transfer(cmd=c, rsize=2*n))

def HK():
    return [i & 0xfff for i in readrecord(0x8030, 8)]

# master status register[15:12] bits tell if a packet is available
read_stat_cmd = readrecordcmd(0x8005, 1)
# match an ARB header and read two words
read_arb_header_cmd = struct.pack(">2H", 0xc101,0x5ead)+readrecordcmd(0x8016,2)
# read one word from fifo 2
read_header_iter  = struct.pack(">2H", 0x8001,0x8000)
# disable ARB header match
read_arb_body_prefix = struct.pack(">2H", 0xc101,0x0000)
# read two words from fifo 2
read_arb_body_iter  = struct.pack(">2H", 0x8016,0x8001)
# read a word from fifo 2
read_uart_cmd = struct.pack(">H", 0x8016) + read_stat_cmd 
# read 6/8 words from fifo 3
read_ev_header_cmd = readrecordcmd(0x8017,8)[:-4]
read_ev_body_iter = struct.pack(">HH", 0x8001,0x8001)
read_samp_cmd = readrecordcmd(0x8017,25)[:-4] + read_stat_cmd
# read 5 words from fifo 3
read_step_header_cmd = readrecordcmd(0x8017,7)[:-4]


def search_for_header(h, fmt, val, cmd=read_header_iter, mask=0xffffffff):
    r = ""
    rr = struct.unpack(fmt, h)
    while (rr[0] & mask) != val:
        r += h[:2]
        if len(r)>=2048:
            while len(r)>16 and r[-2:] == "\0\0": 
                r = r[:-2]
            e = "".join(["%02x" % ord(c) for c in r])
            raise IOError("invalid header (0x%04x): %s" % (val, e))
        h = h[2:] + spidev.sync_transfer(cmd=read_header_iter, rsize=2)
        rr = struct.unpack(fmt, h)
    return h, rr

def packetfifo2_config(size, header, mask=0xffff, idx=0, mmask=0x00000000, scale=0, offset=0, count=False):
    """Configure the packet fifo2 in the rpigse.
    header:      packet header
    size:        max size of the packet in words (16bit)
    mask:        valid bits in the header word.
    idx:         number of words to read before the match shall be made
    mmask:       32 bit mask of bits to use for the size match
    scale:       log₂ of the scale factor in words.
    offset:      number of extra words
    count:       False: count the bits, True: use the field as a number
"""
    if idx:
        idx = (size-idx+1) & 0x3ff
    count = 0x8000 if count else 0
    m = 0;
    for mm,bb in ((0xffff0000, 0x4000), (0xc000,0x2000), (0x3f00,0x1000), (0xfe,0x800), (1,0x400)):
        if mmask & mm:
            if (mmask & mm) != mm:
                raise ValueError("partial mask 0x%08x" % mm)
            m |= bb
    reg(0x103, size-1)
    reg(0x104, header)
    reg(0x105, mask)
    reg(0x106, count | m | idx)
    reg(0x107, (offset & 0x3ff) | (scale << 10))
    

def packetfifo1_config(size, header, mask=0xffff):
    """Configure the packet fifo1 in the rpigse.
    header:      packet header
    mask:        valid bits in the header word.
    size:        max size of the packet in words (16bit)
"""
    reg(0x100, size-1)
    reg(0x101, header)
    reg(0x102, mask)

def stream_to_binary(f, arb=True, samp=False, step=False, bug=1):
    from time import time
    if arb:
        routing(arb=True, Tx1="TxD")
        packetfifo1_config(2, 0x5ead)
    else:
        routing(arb=False, Tx1="TxD")
        packetfifo1_config(1, 0, 0)
    if step:
        packetfifo2_config(header=0x8def, size=38, idx=5, mmask=0xffffffff, scale=0, offset=1)
    elif samp:
        packetfifo2_config(size=25-bug, header=0x5a61)
    else:
        packetfifo2_config(header=0xbeef, size=66, idx=4, mmask=0xffff3fff, scale=1, offset=2)
    stat = reg(5)
    recv_buf = ""
    while True:
        ne = 0
        nr = 0
        while stat & 0x4000:
            nr += 1
            if arb:
                h = spidev.sync_transfer(cmd=read_arb_header_cmd, rsize=4)
                h, rr = search_for_header(h, ">HH", 0x5ead)
                n = 0
                for i in range(16):
                    if rr[1] & (1<<i):
                        n += (8,92,2,12,16,30,30,30,30,30,0,0,0,0,0,0)[i]
                c = read_arb_body_prefix + n/2*read_arb_body_iter + read_stat_cmd
                r = spidev.sync_transfer(cmd=c, rsize=2*n+2)
                stat = struct.unpack(">H", r[2*n:])[0]
                f.write(h+r[:2*n])
            else:
                r = spidev.sync_transfer(cmd=read_uart_cmd, rsize=4)
                rr = struct.unpack(">BBH", r)
                stat = rr[2]
                if rr[0] == 0x80:
                    f.write(r[1])
        while stat & 0x8000:
            ne += 1
            if step:
                h = spidev.sync_transfer(cmd=read_step_header_cmd, rsize=10)
                rr = struct.unpack(">HLL", h)
                if (rr[0] & 0xefff) != 0x8def:
                    h, rr = search_for_header(h, ">HLL", 0x8def, mask=0xefff, cmd=read_header_iter[:2])
                n = 1
                m = rr[2]
                while m:
                    n+=1
                    m &= m-1
                c = read_ev_body_iter[:2] * (n-1) + read_stat_cmd
                r = spidev.sync_transfer(cmd=c, rsize=2*n+2)
                stat = struct.unpack(">H", r[2*n:])[0]
                f.write(h+r[:2*n])
            elif samp:
                r = spidev.sync_transfer(cmd=read_samp_cmd, rsize=52)
                rr = struct.unpack(">H", r[:2])
                if rr[0] != 0x5a61:
                    r, rr = search_for_header(r, ">H12LH", 0x5a61)
                stat = struct.unpack(">H", r[50:])[0]
                f.write(r[0:50])
            else:
                h = spidev.sync_transfer(cmd=read_ev_header_cmd, rsize=12)
                rr = struct.unpack(">LLL", h)
                if rr[0] != 0xbeefa128L:
                    h, rr = search_for_header(h, ">LLL", 0xbeefa128L, read_header_iter[:2])
                if rr[1] & 0x00008000 or rr[1]==0xffff3fff:
                    n = 30
                else:
                    n = 0
                    m = rr[1]
                    while m:
                        n+=1
                        m &= m-1
                c = read_ev_body_iter * (n-1) + read_stat_cmd
                r = spidev.sync_transfer(cmd=c, rsize=4*n+2)
                stat = struct.unpack(">H", r[4*n:])[0]
                f.write(h+r[:4*n])
        if not stat & 0xc000:
            if nr or ne:
                sys.stderr.write("\r%10u R: %-5u E: %-5u " % (time(), nr, ne))
            stat = reg(5)
        if not stat & 0xc000:
            if ne:
                sleep(0.001)
            else:
                f.flush()
                sleep(0.1)
            stat = reg(5)

class sysclock(object):

    sync = "\xd7\x1c\x70\xcb"

    def __init__(self, filename, mode, sparse=10):
        self.sparse = sparse
        self.file = open(filename, mode)
        self.__old = self.__won = 0

    def timestamp(self, now):
        snow = struct.pack(">LH", int(now), int(now*0x10000)&0xffff)
        self.file.write(self.sync + snow)

    def write(self, s):
        import time

        if not self.file.closed:
            now = time.time()
            if self.sparse:
                self.__won = now - (now % self.sparse)
                if self.__won > self.__old:
                    self.timestamp(now)
                self.__old = self.__won
            else:
                self.timestamp(now)
            self.file.write(s)

    def close(self):
        self.file.close()

    def flush(self):
        self.file.flush()

    def __str__(self):
        return "SYSCLOCK "+str(self.file)
    
    def __repr__(self):
        return "SYSCLOCK "+repr(self.file)

def stream_file(fn, open=open, **args):
    aa={}
    if "sparse" in args:
        aa["sparse"]=args["sparse"]
        del args["sparse"]
    f = open(fn, "a", **aa)
    reg(7, 0x1f0)
    reg(10, 9)
    while True:
        try:
            stream_to_binary(f, **args)
        except IOError, e:
            reg(7, 0x1f0)
            print e
        except KeyboardInterrupt:
            f.close()
            break