c4m/nmigen/jtag/tap.py

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