c4m/nmigen/jtag/tap.py

   1 #!/bin/env python3
   2 import os, textwrap
   3 from enum import Enum, auto
   4
   5 from nmigen import *
   6 from nmigen.build import *
   7 from nmigen.lib.io import *
   8 from nmigen.hdl.rec import Direction, Layout
   9 from nmigen.tracer import get_var_name
  10
  11 from nmigen_soc.wishbone import Interface as WishboneInterface
  12
  13 from .bus import Interface
  14
  15 __all__ = [
  16     "TAP", "ShiftReg", "IOType", "IOConn",
  17 ]
  18
  19
  20 class _FSM(Elaboratable):
  21     """TAP subblock for the FSM"""
  22     def __init__(self, *, bus):
  23         self.isir = Signal()
  24         self.isdr = Signal()
  25         self.capture = Signal()
  26         self.shift = Signal()
  27         self.update = Signal()
  28
  29         self.posjtag = ClockDomain("posjtag", local=True)
  30         self.negjtag = ClockDomain("negjtag", local=True, clk_edge="neg")
  31
  32         self._bus = bus
  33
  34     def elaborate(self, platform):
  35         m = Module()
  36
  37         rst = Signal()
  38         m.d.comb += [
  39             self.posjtag.clk.eq(self._bus.tck),
  40             self.posjtag.rst.eq(rst),
  41             self.negjtag.clk.eq(self._bus.tck),
  42             self.negjtag.rst.eq(rst),
  43         ]
  44
  45         # Make local clock domain optionally using trst of JTAG bus as reset
  46         if hasattr(self._bus, "trst"):
  47             m.domains.local = local = ClockDomain(local=True)
  48             m.d.comb += local.rst.eq(self._bus.trst)
  49         else:
  50             m.domains.local = local = ClockDomain(local=True, reset_less=True)
  51         m.d.comb += local.clk.eq(self._bus.tck)
  52
  53         with m.FSM(domain="local") as fsm:
  54             with m.State("TestLogicReset"):
  55                 # Be sure to reset isir, isdr
  56                 m.d.local += [
  57                     self.isir.eq(0),
  58                     self.isdr.eq(0),
  59                 ]
  60                 with m.If(self._bus.tms == 0):
  61                     m.next = "RunTestIdle"
  62             with m.State("RunTestIdle"):
  63                 # Be sure to reset isir, isdr
  64                 m.d.local += [
  65                     self.isir.eq(0),
  66                     self.isdr.eq(0),
  67                 ]
  68                 with m.If(self._bus.tms == 1):
  69                     m.next = "SelectDRScan"
  70             with m.State("SelectDRScan"):
  71                 with m.If(self._bus.tms == 0):
  72                     m.d.local += self.isdr.eq(1)
  73                     m.next = "CaptureState"
  74                 with m.Else():
  75                     m.next = "SelectIRScan"
  76             with m.State("SelectIRScan"):
  77                 with m.If(self._bus.tms == 0):
  78                     m.d.local += self.isir.eq(1)
  79                     m.next = "CaptureState"
  80                 with m.Else():
  81                     m.next = "TestLogicReset"
  82             with m.State("CaptureState"):
  83                 with m.If(self._bus.tms == 0):
  84                     m.next = "ShiftState"
  85                 with m.Else():
  86                     m.next = "Exit1"
  87             with m.State("ShiftState"):
  88                 with m.If(self._bus.tms == 1):
  89                     m.next = "Exit1"
  90             with m.State("Exit1"):
  91                 with m.If(self._bus.tms == 0):
  92                     m.next = "Pause"
  93                 with m.Else():
  94                     m.next = "UpdateState"
  95             with m.State("Pause"):
  96                 with m.If(self._bus.tms == 1):
  97                     m.next = "Exit2"
  98             with m.State("Exit2"):
  99                 with m.If(self._bus.tms == 0):
 100                     m.next = "ShiftState"
 101                 with m.Else():
 102                     m.next = "UpdateState"
 103             with m.State("UpdateState"):
 104                 m.d.local += [
 105                     self.isir.eq(0),
 106                     self.isdr.eq(0),
 107                 ]
 108                 with m.If(self._bus.tms == 0):
 109                     m.next = "RunTestIdle"
 110                 with m.Else():
 111                     m.next = "SelectDRScan"
 112
 113             m.d.comb += [
 114                 rst.eq(fsm.ongoing("TestLogicReset")),
 115                 self.capture.eq(fsm.ongoing("CaptureState")),
 116                 self.shift.eq(fsm.ongoing("ShiftState")),
 117                 self.update.eq(fsm.ongoing("UpdateState")),
 118             ]
 119
 120         return m
 121
 122 class _IRBlock(Elaboratable):
 123     """TAP subblock for handling the IR shift register"""
 124     def __init__(self, *, ir_width, cmd_idcode,
 125                  tdi, capture, shift, update,
 126                  name):
 127         self.name = name
 128         self.ir = Signal(ir_width, reset=cmd_idcode)
 129         self.tdo = Signal()
 130
 131         self._tdi = tdi
 132         self._capture = capture
 133         self._shift = shift
 134         self._update = update
 135
 136     def elaborate(self, platform):
 137         m = Module()
 138
 139         shift_ir = Signal(len(self.ir), reset_less=True)
 140
 141         m.d.comb += self.tdo.eq(self.ir[0])
 142         with m.If(self._capture):
 143             m.d.posjtag += shift_ir.eq(self.ir)
 144         with m.Elif(self._shift):
 145             m.d.posjtag += shift_ir.eq(Cat(shift_ir[1:], self._tdi))
 146         with m.Elif(self._update):
 147             # For ir we only update it on the rising edge of clock
 148             # to avoid that we already have the new ir value when still in
 149             # Update state
 150             m.d.posjtag += self.ir.eq(shift_ir)
 151
 152         return m
 153
 154 class IOType(Enum):
 155     In = auto()
 156     Out = auto()
 157     TriOut = auto()
 158     InTriOut = auto()
 159
 160 class IOConn(Record):
 161     """TAP subblock representing the interface for an JTAG IO cell.
 162     It contains signal to connect to the core and to the pad
 163
 164     This object is normally only allocated and returned from ``TAP.add_io``
 165     It is a Record subclass.
 166
 167     Attributes
 168     ----------
 169     core: subrecord with signals for the core
 170         i: Signal(1), present only for IOType.In and IOType.InTriOut.
 171             Signal input to core with pad input value.
 172         o: Signal(1), present only for IOType.Out, IOType.TriOut and IOType.InTriOut.
 173             Signal output from core with the pad output value.
 174         oe: Signal(1), present only for IOType.TriOut and IOType.InTriOut.
 175             Signal output from core with the pad output enable value.
 176     pad: subrecord with for the pad
 177         i: Signal(1), present only for IOType.In and IOType.InTriOut
 178             Output from pad with pad input value for core.
 179         o: Signal(1), present only for IOType.Out, IOType.TriOut and IOType.InTriOut.
 180             Input to pad with pad output value.
 181         oe: Signal(1), present only for IOType.TriOut and IOType.InTriOut.
 182             Input to pad with pad output enable value.
 183     """
 184     @staticmethod
 185     def layout(iotype):
 186         sigs = []
 187         if iotype in (IOType.In, IOType.InTriOut):
 188             sigs.append(("i", 1))
 189         if iotype in (IOType.Out, IOType.TriOut, IOType.InTriOut):
 190             sigs.append(("o", 1))
 191         if iotype in (IOType.TriOut, IOType.InTriOut):
 192             sigs.append(("oe", 1))
 193
 194         return Layout((("core", sigs), ("pad", sigs)))
 195
 196     def __init__(self, *, iotype, name=None, src_loc_at=0):
 197         super().__init__(self.__class__.layout(iotype), name=name, src_loc_at=src_loc_at+1)
 198
 199         self._iotype = iotype
 200
 201 class _IDBypassBlock(Elaboratable):
 202     """TAP subblock for the ID shift register"""
 203     def __init__(self, *, manufacturer_id, part_number, version,
 204                  tdi, capture, shift, update, bypass,
 205                  name):
 206         self.name = name
 207         if not isinstance(manufacturer_id, Const) and len(manufacturer_id) != 11:
 208             raise ValueError("manufacturer_id has to be Const of length 11")
 209         if not isinstance(part_number, Const) and len(manufacturer_id) != 16:
 210             raise ValueError("part_number has to be Const of length 16")
 211         if not isinstance(version, Const) and len(version) != 4:
 212             raise ValueError("version has to be Const of length 4")
 213         self._id = Cat(Const(1,1), manufacturer_id, part_number, version)
 214
 215         self.tdo = Signal(name=name+"_tdo")
 216
 217         self._tdi = tdi
 218         self._capture = capture
 219         self._shift = shift
 220         self._update = update
 221         self._bypass = bypass
 222
 223     def elaborate(self, platform):
 224         m = Module()
 225
 226         sr = Signal(32, reset_less=True, name=self.name+"_sr")
 227
 228         # Local signals for the module
 229         _tdi = Signal()
 230         _capture = Signal()
 231         _shift = Signal()
 232         _update = Signal()
 233         _bypass = Signal()
 234
 235         m.d.comb += [
 236             _tdi.eq(self._tdi),
 237             _capture.eq(self._capture),
 238             _shift.eq(self._shift),
 239             _update.eq(self._update),
 240             _bypass.eq(self._bypass),
 241             self.tdo.eq(sr[0]),
 242         ]
 243
 244         with m.If(_capture):
 245             m.d.posjtag += sr.eq(self._id)
 246         with m.Elif(_shift):
 247             with m.If(_bypass):
 248                 m.d.posjtag += sr[0].eq(_tdi)
 249             with m.Else():
 250                 m.d.posjtag += sr.eq(Cat(sr[1:], _tdi))
 251
 252         return m
 253
 254
 255 class ShiftReg(Record):
 256     """Object with interface for extra shift registers on a TAP.
 257
 258     Parameters
 259     ----------
 260     sr_length : int
 261     cmds : int, default=1
 262         The number of corresponding JTAG instructions
 263
 264     This object is normally only allocated and returned from ``TAP.add_shiftreg``
 265     It is a Record subclass.
 266
 267     Attributes
 268     ----------
 269     i: length=sr_length, FANIN
 270         The input data sampled during capture state of the TAP
 271     ie: length=cmds, FANOUT
 272         Indicates that data is to be sampled by the JTAG TAP and
 273         should be held stable. The bit indicates the corresponding
 274         instruction for which data is asked.
 275         This signal is kept high for a whole JTAG TAP clock cycle
 276         and may thus be kept higher for more than one clock cycle
 277         on the domain where ShiftReg is used.
 278         The JTAG protocol does not allow insertion of wait states
 279         so data need to be provided before ie goes down. The speed
 280         of the response will determine the max. frequency for the
 281         JTAG interface.
 282     o: length=sr_length, FANOUT
 283         The value of the shift register.
 284     oe: length=cmds, FANOUT
 285         Indicates that output is stable and can be sampled downstream because
 286         JTAG TAP is in the Update state. The bit indicates the corresponding
 287         instruction. The bit is only kept high for one clock cycle.
 288     """
 289     def __init__(self, *, sr_length, cmds=1, name=None, src_loc_at=0):
 290         layout = [
 291             ("i", sr_length, Direction.FANIN),
 292             ("ie", cmds, Direction.FANOUT),
 293             ("o", sr_length, Direction.FANOUT),
 294             ("oe", cmds, Direction.FANOUT),
 295         ]
 296         super().__init__(layout, name=name, src_loc_at=src_loc_at+1)
 297
 298
 299 class TAP(Elaboratable):
 300     #TODO: Document TAP
 301     def __init__(
 302         self, *, with_reset=False, ir_width=None,
 303         manufacturer_id=Const(0b10001111111, 11), part_number=Const(1, 16),
 304         version=Const(0, 4),
 305         name=None, src_loc_at=0
 306     ):
 307         assert((ir_width is None) or (isinstance(ir_width, int) and ir_width >= 2))
 308         assert(len(version) == 4)
 309
 310         if name is None:
 311             name = get_var_name(depth=src_loc_at+2, default="TAP")
 312         self.name = name
 313         self.bus = Interface(with_reset=with_reset, name=self.name+"_bus",
 314                              src_loc_at=src_loc_at+1)
 315
 316         ##
 317
 318         self._ir_width = ir_width
 319         self._manufacturer_id = manufacturer_id
 320         self._part_number = part_number
 321         self._version = version
 322
 323         self._ircodes = [0, 1, 2] # Already taken codes, all ones added at the end
 324
 325         self._ios = []
 326         self._srs = []
 327         self._wbs = []
 328
 329     def elaborate(self, platform):
 330         m = Module()
 331
 332         # Determine ir_width if not fixed.
 333         ir_max = max(self._ircodes) + 1 # One extra code needed with all ones
 334         ir_width = len("{:b}".format(ir_max))
 335         if self._ir_width is not None:
 336             assert self._ir_width >= ir_width, "Specified JTAG IR width not big enough for allocated shiift registers"
 337             ir_width = self._ir_width
 338
 339         # TODO: Make commands numbers configurable
 340         cmd_extest = 0
 341         cmd_intest = 0
 342         cmd_idcode = 1
 343         cmd_sample = 2
 344         cmd_preload = 2
 345         cmd_bypass = 2**ir_width - 1 # All ones
 346
 347         m.submodules._fsm = fsm = _FSM(bus=self.bus)
 348         m.domains.posjtag = fsm.posjtag
 349         m.domains.negjtag = fsm.negjtag
 350
 351         # IR block
 352         select_ir = fsm.isir
 353         m.submodules._irblock = irblock = _IRBlock(
 354             ir_width=ir_width, cmd_idcode=cmd_idcode, tdi=self.bus.tdi,
 355             capture=(fsm.isir & fsm.capture),
 356             shift=(fsm.isir & fsm.shift),
 357             update=(fsm.isir & fsm.update),
 358             name=self.name+"_ir",
 359         )
 360         ir = irblock.ir
 361
 362         # ID block
 363         select_id = fsm.isdr & ((ir == cmd_idcode) | (ir == cmd_bypass))
 364         m.submodules._idblock = idblock = _IDBypassBlock(
 365             manufacturer_id=self._manufacturer_id, part_number=self._part_number,
 366             version=self._version, tdi=self.bus.tdi,
 367             capture=(select_id & fsm.capture),
 368             shift=(select_id & fsm.shift),
 369             update=(select_id & fsm.update),
 370             bypass=(ir == cmd_bypass),
 371             name=self.name+"_id",
 372         )
 373
 374         # IO (Boundary scan) block
 375         io_capture = Signal()
 376         io_shift = Signal()
 377         io_update = Signal()
 378         io_bd2io = Signal()
 379         io_bd2core = Signal()
 380         sample = (ir == cmd_extest) | (ir == cmd_sample)
 381         preload = (ir == cmd_preload)
 382         select_io = fsm.isdr & (sample | preload)
 383         m.d.comb += [
 384             io_capture.eq(sample & fsm.capture), # Don't capture if not sample (like for PRELOAD)
 385             io_shift.eq(select_io & fsm.shift),
 386             io_update.eq(select_io & fsm.update),
 387             io_bd2io.eq(ir == cmd_extest),
 388             io_bd2core.eq(ir == cmd_intest),
 389         ]
 390         io_tdo = self._elaborate_ios(
 391             m=m,
 392             capture=io_capture, shift=io_shift, update=io_update,
 393             bd2io=io_bd2io, bd2core=io_bd2core,
 394         )
 395
 396         # chain tdo: select as appropriate, to go into into shiftregs
 397         tdo = Signal(name=self.name+"_tdo")
 398         with m.If(select_ir):
 399             m.d.comb += tdo.eq(irblock.tdo)
 400         with m.Elif(select_id):
 401             m.d.comb += tdo.eq(idblock.tdo)
 402         with m.Elif(select_io):
 403             m.d.comb += tdo.eq(io_tdo)
 404
 405         # shiftregs block
 406         self._elaborate_shiftregs(
 407             m, capture=fsm.capture, shift=fsm.shift, update=fsm.update,
 408             ir=irblock.ir, tdo_jtag=tdo
 409         )
 410
 411         # wishbone
 412         self._elaborate_wishbones(m)
 413
 414         return m
 415
 416
 417     def add_io(self, *, iotype, name=None, src_loc_at=0):
 418         """Add a io cell to the boundary scan chain
 419
 420         Parameters:
 421         - iotype: :class:`IOType` enum.
 422
 423         Returns:
 424         - :class:`IOConn`
 425         """
 426         if name is None:
 427             name = "ioconn" + str(len(self._ios))
 428
 429         ioconn = IOConn(iotype=iotype, name=name, src_loc_at=src_loc_at+1)
 430         self._ios.append(ioconn)
 431         return ioconn
 432
 433     def _elaborate_ios(self, *, m, capture, shift, update, bd2io, bd2core):
 434         connlength = {
 435             IOType.In: 1,
 436             IOType.Out: 1,
 437             IOType.TriOut: 2,
 438             IOType.InTriOut: 3,
 439         }
 440         length = sum(connlength[conn._iotype] for conn in self._ios)
 441
 442         io_sr = Signal(length)
 443         io_bd = Signal(length)
 444
 445         with m.If(capture):
 446             idx = 0
 447             for conn in self._ios:
 448                 if conn._iotype == IOType.In:
 449                     m.d.posjtag += io_sr[idx].eq(conn.pad.i)
 450                     idx += 1
 451                 elif conn._iotype == IOType.Out:
 452                     m.d.posjtag += io_sr[idx].eq(conn.core.o)
 453                     idx += 1
 454                 elif conn._iotype == IOType.TriOut:
 455                     m.d.posjtag += [
 456                         io_sr[idx].eq(conn.core.o),
 457                         io_sr[idx+1].eq(conn.core.oe),
 458                     ]
 459                     idx += 2
 460                 elif conn._iotype == IOType.InTriOut:
 461                     m.d.posjtag += [
 462                         io_sr[idx].eq(conn.pad.i),
 463                         io_sr[idx+1].eq(conn.core.o),
 464                         io_sr[idx+2].eq(conn.core.oe),
 465                     ]
 466                     idx += 3
 467                 else:
 468                     raise("Internal error")
 469             assert idx == length, "Internal error"
 470         with m.Elif(shift):
 471             m.d.posjtag += io_sr.eq(Cat(self.bus.tdi, io_sr[:-1]))
 472         with m.Elif(update):
 473             m.d.negjtag += io_bd.eq(io_sr)
 474
 475         idx = 0
 476         for conn in self._ios:
 477             if conn._iotype == IOType.In:
 478                 m.d.comb += conn.core.i.eq(Mux(bd2core, io_bd[idx], conn.pad.i))
 479                 idx += 1
 480             elif conn._iotype == IOType.Out:
 481                 m.d.comb += conn.pad.o.eq(Mux(bd2io, io_bd[idx], conn.core.o))
 482                 idx += 1
 483             elif conn._iotype == IOType.TriOut:
 484                 m.d.comb += [
 485                     conn.pad.o.eq(Mux(bd2io, io_bd[idx], conn.core.o)),
 486                     conn.pad.oe.eq(Mux(bd2io, io_bd[idx+1], conn.core.oe)),
 487                 ]
 488                 idx += 2
 489             elif conn._iotype == IOType.InTriOut:
 490                 m.d.comb += [
 491                     conn.core.i.eq(Mux(bd2core, io_bd[idx], conn.pad.i)),
 492                     conn.pad.o.eq(Mux(bd2io, io_bd[idx+1], conn.core.o)),
 493                     conn.pad.oe.eq(Mux(bd2io, io_bd[idx+2], conn.core.oe)),
 494                 ]
 495                 idx += 3
 496             else:
 497                 raise("Internal error")
 498         assert idx == length, "Internal error"
 499
 500         return io_sr[-1]
 501
 502
 503     def add_shiftreg(self, *, ircode, length, domain="sync", name=None, src_loc_at=0):
 504         """Add a shift register to the JTAG interface
 505
 506         Parameters:
 507         - ircode: code(s) for the IR; int or sequence of ints. In the latter case this
 508           shiftreg is shared between different IR codes.
 509         - length: the length of the shift register
 510         - domain: the domain on which the signal will be used"""
 511
 512         try:
 513             ir_it = iter(ircode)
 514             ircodes = ircode
 515         except TypeError:
 516             ir_it = ircodes = (ircode,)
 517         for _ircode in ir_it:
 518             if not isinstance(_ircode, int) or _ircode <= 0:
 519                 raise ValueError("IR code '{}' is not an int greater than 0".format(_ircode))
 520             if _ircode in self._ircodes:
 521                 raise ValueError("IR code '{}' already taken".format(_ircode))
 522
 523         self._ircodes.extend(ircodes)
 524
 525         if name is None:
 526             name = "sr{}".format(len(self._srs))
 527         sr = ShiftReg(sr_length=length, cmds=len(ircodes), name=name, src_loc_at=src_loc_at+1)
 528         self._srs.append((ircodes, domain, sr))
 529
 530         return sr
 531
 532     def _elaborate_shiftregs(self, m, capture, shift, update, ir, tdo_jtag):
 533         # tdos is tuple of (tdo, tdo_en) for each shiftreg
 534         tdos = []
 535         for ircodes, domain, sr in self._srs:
 536             reg = Signal(len(sr.o), name=sr.name+"_reg")
 537             m.d.comb += sr.o.eq(reg)
 538
 539             isir = Signal(len(ircodes), name=sr.name+"_isir")
 540             sr_capture = Signal(name=sr.name+"_capture")
 541             sr_shift = Signal(name=sr.name+"_shift")
 542             sr_update = Signal(name=sr.name+"_update")
 543             m.d.comb += [
 544                 isir.eq(Cat(ir == ircode for ircode in ircodes)),
 545                 sr_capture.eq((isir != 0) & capture),
 546                 sr_shift.eq((isir != 0) & shift),
 547                 sr_update.eq((isir != 0) & update),
 548             ]
 549
 550             # update signal is on the JTAG clockdomain, sr.oe is on `domain` clockdomain
 551             # latch update in `domain` clockdomain and see when it has falling edge.
 552             # At that edge put isir in sr.oe for one `domain` clockdomain
 553             update_core = Signal(name=sr.name+"_update_core")
 554             update_core_prev = Signal(name=sr.name+"_update_core_prev")
 555             m.d[domain] += [
 556                 update_core.eq(sr_update), # This is CDC from JTAG domain to given domain
 557                 update_core_prev.eq(update_core)
 558             ]
 559             with m.If(update_core_prev & ~update_core):
 560                 # Falling edge of update
 561                 m.d[domain] += sr.oe.eq(isir)
 562             with m.Else():
 563                 m.d[domain] += sr.oe.eq(0)
 564
 565             with m.If(sr_shift):
 566                 m.d.posjtag += reg.eq(Cat(reg[1:], self.bus.tdi))
 567             with m.If(sr_capture):
 568                 m.d.posjtag += reg.eq(sr.i)
 569
 570             # tdo = reg[0], tdo_en = shift
 571             tdos.append((reg[0], sr_shift))
 572
 573
 574         # Assign the right tdo to the bus tdo
 575         for i, (tdo, tdo_en) in enumerate(tdos):
 576             if i == 0:
 577                 with m.If(tdo_en):
 578                     m.d.comb += self.bus.tdo.eq(tdo)
 579             else:
 580                 with m.Elif(tdo_en):
 581                     m.d.comb += self.bus.tdo.eq(tdo)
 582
 583         if len(tdos) > 0:
 584             with m.Else():
 585                 m.d.comb += self.bus.tdo.eq(tdo_jtag)
 586         else:
 587             # Always connect tdo_jtag to
 588             m.d.comb += self.bus.tdo.eq(tdo_jtag)
 589
 590
 591     def add_wishbone(self, *, ircodes, address_width, data_width, granularity=None, domain="sync",
 592                      name=None, src_loc_at=0):
 593         """Add a wishbone interface
 594
 595         In order to allow high JTAG clock speed, data will be cached. This means that if data is
 596         output the value of the next address will be read automatically.
 597
 598         Parameters:
 599         -----------
 600         ircodes: sequence of three integer for the JTAG IR codes;
 601           they represent resp. WBADDR, WBREAD and WBREADWRITE. First code
 602           has a shift register of length 'address_width', the two other codes
 603           share a shift register of length data_width.
 604         address_width: width of the address
 605         data_width: width of the data
 606
 607         Returns:
 608         wb: nmigen_soc.wishbone.bus.Interface
 609             The Wishbone interface, is pipelined and has stall field.
 610         """
 611         if len(ircodes) != 3:
 612             raise ValueError("3 IR Codes have to be provided")
 613
 614         if name is None:
 615             name = "wb" + str(len(self._wbs))
 616         sr_addr = self.add_shiftreg(
 617             ircode=ircodes[0], length=address_width, domain=domain,
 618             name=name+"_addrsr"
 619         )
 620         sr_data = self.add_shiftreg(
 621             ircode=ircodes[1:], length=data_width, domain=domain,
 622             name=name+"_datasr"
 623         )
 624
 625         wb = WishboneInterface(data_width=data_width, addr_width=address_width,
 626                                granularity=granularity, features={"stall", "lock", "err", "rty"},
 627                                name=name, src_loc_at=src_loc_at+1)
 628
 629         self._wbs.append((sr_addr, sr_data, wb, domain))
 630
 631         return wb
 632
 633     def _elaborate_wishbones(self, m):
 634         for sr_addr, sr_data, wb, domain in self._wbs:
 635             m.d.comb += sr_addr.i.eq(wb.adr)
 636
 637             if hasattr(wb, "sel"):
 638                 # Always selected
 639                 m.d.comb += [s.eq(1) for s in wb.sel]
 640
 641             with m.FSM(domain=domain) as fsm:
 642                 with m.State("IDLE"):
 643                     with m.If(sr_addr.oe): # WBADDR code
 644                         m.d[domain] += wb.adr.eq(sr_addr.o)
 645                         m.next = "READ"
 646                     with m.Elif(sr_data.oe[0]): # WBREAD code
 647                         # If data is
 648                         m.d[domain] += wb.adr.eq(wb.adr + 1)
 649                         m.next = "READ"
 650                     with m.Elif(sr_data.oe[1]): # WBWRITE code
 651                         m.d[domain] += wb.dat_w.eq(sr_data.o)
 652                         m.next = "WRITEREAD"
 653                 with m.State("READ"):
 654                     with m.If(~wb.stall):
 655                         m.next = "READACK"
 656                 with m.State("READACK"):
 657                     with m.If(wb.ack):
 658                         # Store read data in sr_data.i and keep it there til next read
 659                         m.d[domain] += sr_data.i.eq(wb.dat_r)
 660                         m.next = "IDLE"
 661                 with m.State("WRITEREAD"):
 662                     with m.If(~wb.stall):
 663                         m.next = "WRITEREADACK"
 664                 with m.State("WRITEREADACK"):
 665                     with m.If(wb.ack):
 666                         m.d[domain] += wb.adr.eq(wb.adr + 1)
 667                         m.next = "READ"
 668
 669                 m.d.comb += [
 670                     wb.cyc.eq(~fsm.ongoing("IDLE")),
 671                     wb.stb.eq(fsm.ongoing("READ") | fsm.ongoing("WRITEREAD")),
 672                     wb.we.eq(fsm.ongoing("WRITEREAD")),
 673                 ]