diff --git a/orbtrace/trace/__init__.py b/orbtrace/trace/__init__.py index 17d30a8..02963d2 100644 --- a/orbtrace/trace/__init__.py +++ b/orbtrace/trace/__init__.py @@ -6,7 +6,7 @@ from .swo import ManchesterDecoder, PulseLengthCapture, BitsToBytes, NRZDecoder, UARTDecoder -from . import cobs +from . import cobs, tpiu class TracePHY(Module): def __init__(self, pads): @@ -308,15 +308,16 @@ def __init__(self, platform): phy := ClockDomainsRenamer('trace')(TracePHY(trace_pads)), ClockDomainsRenamer({'write': 'trace', 'read': 'sys'})(AsyncFIFO([('data', 128)], 4)), ByteSwap(16), - injector := Injector(), - pv := PipeValid([('data', 128)]), - Converter(128, 8), + #injector := Injector(), + #pv := PipeValid([('data', 128)]), + #Converter(128, 8), + tpiu.TPIUDemux(), cobs.COBSEncoder(), cobs.DelimiterAppender(), cobs.SuperFramer(7500000, 65536), ] - pv.comb += pv.source.last.eq(1) + #pv.comb += pv.source.last.eq(1) trace_stream = Endpoint([('data', 8)]) @@ -333,7 +334,7 @@ def __init__(self, platform): self.comb += [ keepalive.total_frames.eq(monitor.total), keepalive.lost_frames.eq(monitor.lost), - keepalive.source.connect(injector.sink_inject), + #keepalive.source.connect(injector.sink_inject), ] self.submodules += monitor, keepalive diff --git a/orbtrace/trace/tpiu.py b/orbtrace/trace/tpiu.py new file mode 100644 index 0000000..f255e58 --- /dev/null +++ b/orbtrace/trace/tpiu.py @@ -0,0 +1,172 @@ +from migen import * + +from litex.soc.interconnect.stream import Endpoint, Pipeline, CombinatorialActor, Converter + +class Rearrange(CombinatorialActor): + def __init__(self): + self.sink = sink = Endpoint([('data', 128)]) + self.source = source = Endpoint([('data', 135)]) + + self.comb += source.data.eq(Cat( + sink.data[120], sink.data[1:8], sink.data[0], + sink.data[8:16], C(0, 1), + sink.data[121], sink.data[17:24], sink.data[16], + sink.data[24:32], C(0, 1), + sink.data[122], sink.data[33:40], sink.data[32], + sink.data[40:48], C(0, 1), + sink.data[123], sink.data[49:56], sink.data[48], + sink.data[56:64], C(0, 1), + sink.data[124], sink.data[65:72], sink.data[64], + sink.data[72:80], C(0, 1), + sink.data[125], sink.data[81:88], sink.data[80], + sink.data[88:96], C(0, 1), + sink.data[126], sink.data[97:104], sink.data[96], + sink.data[104:112], C(0, 1), + sink.data[127], sink.data[113:120], sink.data[112], + )) + + super().__init__() + +class TrackStream(Module): + def __init__(self): + self.sink = sink = Endpoint([('data', 9)]) + self.source = source = Endpoint([('channel', 7), ('data', 8)]) + + channel = Signal(7) + next_channel = Signal(7) + next_channel_valid = Signal() + + self.sync += If(sink.valid & sink.ready & next_channel_valid, + channel.eq(next_channel), + next_channel_valid.eq(0), + ) + + self.sync += If(sink.valid & sink.data[8], + If(sink.data[0], + next_channel.eq(sink.data[1:8]), + next_channel_valid.eq(1), + ).Else( + channel.eq(sink.data[1:8]), + ), + ) + + self.comb += [ + source.channel.eq(channel), + source.data.eq(sink.data[0:8]), + If(sink.data[8], + sink.ready.eq(1), + ).Else( + sink.ready.eq(source.ready), + source.valid.eq(sink.valid), + ), + ] + +class Demux(Module): + def __init__(self): + self.sink = sink = Endpoint([('channel', 7), ('data', 8)]) + self.source_itm = source_itm = Endpoint([('data', 8)]) + self.source_etm = source_etm = Endpoint([('data', 8)]) + + self.comb += Case(sink.channel, { + 1: sink.connect(source_itm, omit = {'channel'}), + 2: sink.connect(source_etm, omit = {'channel'}), + 'default': sink.ready.eq(1), + }) + +class StripChannelZero(Module): + def __init__(self): + self.sink = sink = Endpoint([('channel', 7), ('data', 8)]) + self.source = source = Endpoint([('channel', 7), ('data', 8)]) + + self.comb += [ + sink.connect(source), + + If(sink.channel == 0, + sink.ready.eq(1), + source.valid.eq(0), + ), + ] + +class Packetizer(Module): + def __init__(self): + self.sink = sink = Endpoint([('channel', 7), ('data', 8)]) + self.source = source = Endpoint([('data', 8)]) + + channel = Signal(7) + + self.submodules.fsm = fsm = FSM() + + fsm.act('DATA', + source.data.eq(sink.data), + source.valid.eq(sink.valid & (sink.channel == channel)), + sink.ready.eq(source.ready & (sink.channel == channel)), + + If(sink.valid & (sink.channel != channel), + NextState('HEADER'), + NextValue(channel, sink.channel), + ), + ) + + fsm.act('HEADER', + source.data.eq(channel), + source.first.eq(1), + source.valid.eq(1), + + If(source.ready, + NextState('DATA'), + ), + ) + +class LastFromFirst(Module): + def __init__(self): + self.sink = sink = Endpoint([('data', 8)]) + self.source = source = Endpoint([('data', 8)]) + + data = Signal(8) + first = Signal() + valid = Signal() + + self.comb += [ + sink.ready.eq(~valid | (source.ready & source.valid)), + + source.data.eq(data), + source.first.eq(first), + source.last.eq(sink.first), + source.valid.eq(valid & sink.valid), + ] + + self.sync += [ + If(sink.ready & sink.valid, + data.eq(sink.data), + first.eq(sink.first), + valid.eq(1), + ), + ] + +class TPIUDemux(Module): + def __init__(self): + self.sink = sink = Endpoint([('data', 128)]) + self.source = source = Endpoint([('data', 8)]) + + self.submodules.rearrange = Rearrange() + self.submodules.converter = Converter(135, 9) + self.submodules.track_stream = TrackStream() + self.submodules.demux = Demux() + self.submodules.strip_channel_zero = StripChannelZero() + self.submodules.packetizer = Packetizer() + self.submodules.last_from_first = LastFromFirst() + + self.submodules.pipeline = Pipeline( + sink, + self.rearrange, + self.converter, + self.track_stream, + #self.demux, + self.strip_channel_zero, + self.packetizer, + self.last_from_first, + source, + ) + + #self.comb += self.demux.source_etm.connect(source) + #self.comb += self.demux.source_itm.ready.eq(1) \ No newline at end of file