ISACaller: add option mmu
[soc.git] / src / soc / decoder / isa / caller.py
index aeecc897a3ae4168187691e0f2cfc619dfc81ed2..4ee49443baf5ba64152af61481596bd3b96b1340 100644 (file)
@@ -21,7 +21,7 @@ from soc.decoder.selectable_int import (FieldSelectableInt, SelectableInt,
                                         selectconcat)
 from soc.decoder.power_enums import (spr_dict, spr_byname, XER_bits,
                                      insns, MicrOp, In1Sel, In2Sel, In3Sel,
-                                     OutSel)
+                                     OutSel, CROutSel)
 from soc.decoder.helpers import exts, gtu, ltu, undefined
 from soc.consts import PIb, MSRb  # big-endian (PowerISA versions)
 from soc.decoder.power_svp64 import SVP64RM, decode_extra
@@ -75,6 +75,282 @@ def create_args(reglist, extra=None):
     return retval
 
 
+"""
+    Get Root Page
+
+    //Accessing 2nd double word of partition table (pate1)
+    //Ref: Power ISA Manual v3.0B, Book-III, section 5.7.6.1
+    //           PTCR Layout
+    // ====================================================
+    // -----------------------------------------------
+    // | /// |     PATB                | /// | PATS  |
+    // -----------------------------------------------
+    // 0     4                       51 52 58 59    63
+    // PATB[4:51] holds the base address of the Partition Table,
+    // right shifted by 12 bits.
+    // This is because the address of the Partition base is
+    // 4k aligned. Hence, the lower 12bits, which are always
+    // 0 are ommitted from the PTCR.
+    //
+    // Thus, The Partition Table Base is obtained by (PATB << 12)
+    //
+    // PATS represents the partition table size right-shifted by 12 bits.
+    // The minimal size of the partition table is 4k.
+    // Thus partition table size = (1 << PATS + 12).
+    //
+    //        Partition Table
+    //  ====================================================
+    //  0    PATE0            63  PATE1             127
+    //  |----------------------|----------------------|
+    //  |                      |                      |
+    //  |----------------------|----------------------|
+    //  |                      |                      |
+    //  |----------------------|----------------------|
+    //  |                      |                      | <-- effLPID
+    //  |----------------------|----------------------|
+    //           .
+    //           .
+    //           .
+    //  |----------------------|----------------------|
+    //  |                      |                      |
+    //  |----------------------|----------------------|
+    //
+    // The effective LPID  forms the index into the Partition Table.
+    //
+    // Each entry in the partition table contains 2 double words, PATE0, PATE1,
+    // corresponding to that partition.
+    //
+    // In case of Radix, The structure of PATE0 and PATE1 is as follows.
+    //
+    //     PATE0 Layout
+    // -----------------------------------------------
+    // |1|RTS1|/|     RPDB          | RTS2 |  RPDS   |
+    // -----------------------------------------------
+    //  0 1  2 3 4                55 56  58 59      63
+    //
+    // HR[0] : For Radix Page table, first bit should be 1.
+    // RTS1[1:2] : Gives one fragment of the Radix treesize
+    // RTS2[56:58] : Gives the second fragment of the Radix Tree size.
+    // RTS = (RTS1 << 3 + RTS2) + 31.
+    //
+    // RPDB[4:55] = Root Page Directory Base.
+    // RPDS = Logarithm of Root Page Directory Size right shifted by 3.
+    //        Thus, Root page directory size = 1 << (RPDS + 3).
+    //        Note: RPDS >= 5.
+    //
+    //   PATE1 Layout
+    // -----------------------------------------------
+    // |///|       PRTB             |  //  |  PRTS   |
+    // -----------------------------------------------
+    // 0  3 4                     51 52  58 59     63
+    //
+    // PRTB[4:51]   = Process Table Base. This is aligned to size.
+    // PRTS[59: 63] = Process Table Size right shifted by 12.
+    //                Minimal size of the process table is 4k.
+    //                Process Table Size = (1 << PRTS + 12).
+    //                Note: PRTS <= 24.
+    //
+    //                Computing the size aligned Process Table Base:
+    //                   table_base = (PRTB  & ~((1 << PRTS) - 1)) << 12
+    //                Thus, the lower 12+PRTS bits of table_base will
+    //                be zero.
+
+
+    //Ref: Power ISA Manual v3.0B, Book-III, section 5.7.6.2
+    //
+    //        Process Table
+    // ==========================
+    //  0    PRTE0            63  PRTE1             127
+    //  |----------------------|----------------------|
+    //  |                      |                      |
+    //  |----------------------|----------------------|
+    //  |                      |                      |
+    //  |----------------------|----------------------|
+    //  |                      |                      | <-- effPID
+    //  |----------------------|----------------------|
+    //           .
+    //           .
+    //           .
+    //  |----------------------|----------------------|
+    //  |                      |                      |
+    //  |----------------------|----------------------|
+    //
+    // The effective Process id (PID) forms the index into the Process Table.
+    //
+    // Each entry in the partition table contains 2 double words, PRTE0, PRTE1,
+    // corresponding to that process
+    //
+    // In case of Radix, The structure of PRTE0 and PRTE1 is as follows.
+    //
+    //     PRTE0 Layout
+    // -----------------------------------------------
+    // |/|RTS1|/|     RPDB          | RTS2 |  RPDS   |
+    // -----------------------------------------------
+    //  0 1  2 3 4                55 56  58 59      63
+    //
+    // RTS1[1:2] : Gives one fragment of the Radix treesize
+    // RTS2[56:58] : Gives the second fragment of the Radix Tree size.
+    // RTS = (RTS1 << 3 + RTS2) << 31,
+    //        since minimal Radix Tree size is 4G.
+    //
+    // RPDB = Root Page Directory Base.
+    // RPDS = Root Page Directory Size right shifted by 3.
+    //        Thus, Root page directory size = RPDS << 3.
+    //        Note: RPDS >= 5.
+    //
+    //   PRTE1 Layout
+    // -----------------------------------------------
+    // |                      ///                    |
+    // -----------------------------------------------
+    // 0                                            63
+    // All bits are reserved.
+
+
+"""
+
+# see qemu/target/ppc/mmu-radix64.c for reference
+class RADIX:
+    def __init__(self, mem, caller):
+        self.mem = mem
+        self.caller = caller
+
+        # cached page table stuff
+        self.pgtbl0 = 0
+        self.pt0_valid = False
+        self.pgtbl3 = 0
+        self.pt3_valid = False
+
+    def ld(self, address, width=8, swap=True, check_in_mem=False):
+        print("RADIX: ld from addr 0x{:x} width {:d}".format(address, width))
+
+        pte = self._walk_tree()
+        # use pte to caclculate phys address
+        #mem.ld(address,width,swap,check_in_mem)
+
+    # TODO implement
+    # def st(self, addr, v, width=8, swap=True):
+    # def memassign(self, addr, sz, val):
+    def _next_level(self):
+        return True
+        ## DSISR_R_BADCONFIG
+        ## read_entry
+        ## DSISR_NOPTE
+        ## Prepare for next iteration
+
+    def _walk_tree(self):
+        """walk tree
+
+        // vaddr                    64 Bit
+        // vaddr |-----------------------------------------------------|
+        //       | Unused    |  Used                                   |
+        //       |-----------|-----------------------------------------|
+        //       | 0000000   | usefulBits = X bits (typically 52)      |
+        //       |-----------|-----------------------------------------|
+        //       |           |<--Cursize---->|                         |
+        //       |           |    Index      |                         |
+        //       |           |    into Page  |                         |
+        //       |           |    Directory  |                         |
+        //       |-----------------------------------------------------|
+        //                        |                       |
+        //                        V                       |
+        // PDE  |---------------------------|             |
+        //      |V|L|//|  NLB       |///|NLS|             |
+        //      |---------------------------|             |
+        // PDE = Page Directory Entry                     |
+        // [0] = V = Valid Bit                            |
+        // [1] = L = Leaf bit. If 0, then                 |
+        // [4:55] = NLB = Next Level Base                 |
+        //                right shifted by 8              |
+        // [59:63] = NLS = Next Level Size                |
+        //            |    NLS >= 5                       |
+        //            |                                   V
+        //            |                     |--------------------------|
+        //            |                     |   usfulBits = X-Cursize  |
+        //            |                     |--------------------------|
+        //            |---------------------><--NLS-->|                |
+        //                                  | Index   |                |
+        //                                  | into    |                |
+        //                                  | PDE     |                |
+        //                                  |--------------------------|
+        //                                                    |
+        // If the next PDE obtained by                        |
+        // (NLB << 8 + 8 * index) is a                        |
+        // nonleaf, then repeat the above.                    |
+        //                                                    |
+        // If the next PDE is a leaf,                         |
+        // then Leaf PDE structure is as                      |
+        // follows                                            |
+        //                                                    |
+        //                                                    |
+        // Leaf PDE                                           |
+        // |------------------------------|           |----------------|
+        // |V|L|sw|//|RPN|sw|R|C|/|ATT|EAA|           | usefulBits     |
+        // |------------------------------|           |----------------|
+        // [0] = V = Valid Bit                                 |
+        // [1] = L = Leaf Bit = 1 if leaf                      |
+        //                      PDE                            |
+        // [2] = Sw = Sw bit 0.                                |
+        // [7:51] = RPN = Real Page Number,                    V
+        //          real_page = RPN << 12 ------------->  Logical OR
+        // [52:54] = Sw Bits 1:3                               |
+        // [55] = R = Reference                                |
+        // [56] = C = Change                                   V
+        // [58:59] = Att =                                Physical Address
+        //           0b00 = Normal Memory
+        //           0b01 = SAO
+        //           0b10 = Non Idenmpotent
+        //           0b11 = Tolerant I/O
+        // [60:63] = Encoded Access
+        //           Authority
+        //
+        """
+        # walk tree starts on prtbl
+        while True:
+            ret = self._next_level()
+            if ret: return ret
+
+    def _segment_check(self):
+        """checks segment valid
+                    mbits := '0' & r.mask_size;
+            v.shift := r.shift + (31 - 12) - mbits;
+            nonzero := or(r.addr(61 downto 31) and not finalmask(30 downto 0));
+            if r.addr(63) /= r.addr(62) or nonzero = '1' then
+                v.state := RADIX_FINISH;
+                v.segerror := '1';
+            elsif mbits < 5 or mbits > 16 or mbits > (r.shift + (31 - 12)) then
+                v.state := RADIX_FINISH;
+                v.badtree := '1';
+            else
+                v.state := RADIX_LOOKUP;
+        """
+
+    def _check_perms(self):
+        """check page permissions
+                    -- test leaf bit
+                    if data(62) = '1' then
+                        -- check permissions and RC bits
+                        perm_ok := '0';
+                        if r.priv = '1' or data(3) = '0' then
+                            if r.iside = '0' then
+                                perm_ok := data(1) or (data(2) and not r.store);
+                            else
+                                -- no IAMR, so no KUEP support for now
+                                -- deny execute permission if cache inhibited
+                                perm_ok := data(0) and not data(5);
+                            end if;
+                        end if;
+                        rc_ok := data(8) and (data(7) or not r.store);
+                        if perm_ok = '1' and rc_ok = '1' then
+                            v.state := RADIX_LOAD_TLB;
+                        else
+                            v.state := RADIX_FINISH;
+                            v.perm_err := not perm_ok;
+                            -- permission error takes precedence over RC error
+                            v.rc_error := perm_ok;
+                        end if;
+        """
+
+
 class Mem:
 
     def __init__(self, row_bytes=8, initial_mem=None):
@@ -211,11 +487,17 @@ class GPR(dict):
 class PC:
     def __init__(self, pc_init=0):
         self.CIA = SelectableInt(pc_init, 64)
-        self.NIA = self.CIA + SelectableInt(4, 64)
+        self.NIA = self.CIA + SelectableInt(4, 64) # only true for v3.0B!
 
-    def update(self, namespace):
+    def update_nia(self, is_svp64):
+        increment = 8 if is_svp64 else 4
+        self.NIA = self.CIA + SelectableInt(increment, 64)
+
+    def update(self, namespace, is_svp64):
+        """updates the program counter (PC) by 4 if v3.0B mode or 8 if SVP64
+        """
         self.CIA = namespace['NIA'].narrow(64)
-        self.NIA = self.CIA + SelectableInt(4, 64)
+        self.update_nia(is_svp64)
         namespace['CIA'] = self.CIA
         namespace['NIA'] = self.NIA
 
@@ -245,6 +527,29 @@ class SVP64RMFields:
         self.subvl = FieldSelectableInt(self.spr, tuple(range(8,10)))
         self.extra = FieldSelectableInt(self.spr, tuple(range(10,19)))
         self.mode = FieldSelectableInt(self.spr, tuple(range(19,24)))
+        # these cover the same extra field, split into parts as EXTRA2
+        self.extra2 = list(range(4))
+        self.extra2[0] = FieldSelectableInt(self.spr, tuple(range(10,12)))
+        self.extra2[1] = FieldSelectableInt(self.spr, tuple(range(12,14)))
+        self.extra2[2] = FieldSelectableInt(self.spr, tuple(range(14,16)))
+        self.extra2[3] = FieldSelectableInt(self.spr, tuple(range(16,18)))
+        self.smask = FieldSelectableInt(self.spr, tuple(range(16,19)))
+        # and here as well, but EXTRA3
+        self.extra3 = list(range(3))
+        self.extra3[0] = FieldSelectableInt(self.spr, tuple(range(10,13)))
+        self.extra3[1] = FieldSelectableInt(self.spr, tuple(range(13,16)))
+        self.extra3[2] = FieldSelectableInt(self.spr, tuple(range(16,19)))
+
+
+SVP64RM_MMODE_SIZE = len(SVP64RMFields().mmode.br)
+SVP64RM_MASK_SIZE = len(SVP64RMFields().mask.br)
+SVP64RM_ELWIDTH_SIZE = len(SVP64RMFields().elwidth.br)
+SVP64RM_EWSRC_SIZE = len(SVP64RMFields().ewsrc.br)
+SVP64RM_SUBVL_SIZE = len(SVP64RMFields().subvl.br)
+SVP64RM_EXTRA2_SPEC_SIZE = len(SVP64RMFields().extra2[0].br)
+SVP64RM_EXTRA3_SPEC_SIZE = len(SVP64RMFields().extra3[0].br)
+SVP64RM_SMASK_SIZE = len(SVP64RMFields().smask.br)
+SVP64RM_MODE_SIZE = len(SVP64RMFields().mode.br)
 
 
 # SVP64 Prefix fields: see https://libre-soc.org/openpower/sv/svp64/
@@ -258,6 +563,11 @@ class SVP64PrefixFields:
         self.rm = FieldSelectableInt(self.insn, rmfields)
 
 
+SV64P_MAJOR_SIZE = len(SVP64PrefixFields().major.br)
+SV64P_PID_SIZE = len(SVP64PrefixFields().pid.br)
+SV64P_RM_SIZE = len(SVP64PrefixFields().rm.br)
+
+
 class SPR(dict):
     def __init__(self, dec2, initial_sprs={}):
         self.sd = dec2
@@ -354,6 +664,29 @@ def get_pdecode_idx_in(dec2, name):
     return None, False
 
 
+def get_pdecode_cr_out(dec2, name):
+    op = dec2.dec.op
+    out_sel = yield op.cr_out
+    out_bitfield = yield dec2.dec_cr_out.cr_bitfield.data
+    sv_cr_out = yield op.sv_cr_out
+    spec = yield dec2.crout_svdec.spec
+    sv_override = yield dec2.dec_cr_out.sv_override
+    # get the IN1/2/3 from the decoder (includes SVP64 remap and isvec)
+    out = yield dec2.e.write_cr.data
+    o_isvec = yield dec2.o_isvec
+    print ("get_pdecode_cr_out", out_sel, CROutSel.CR0.value, out, o_isvec)
+    print ("    sv_cr_out", sv_cr_out)
+    print ("    cr_bf", out_bitfield)
+    print ("    spec", spec)
+    print ("    override", sv_override)
+    # identify which regnames map to out / o2
+    if name == 'CR0':
+        if out_sel == CROutSel.CR0.value:
+            return out, o_isvec
+    print ("get_pdecode_idx_out not found", name)
+    return None, False
+
+
 def get_pdecode_idx_out(dec2, name):
     op = dec2.dec.op
     out_sel = yield op.out_sel
@@ -368,6 +701,7 @@ def get_pdecode_idx_out(dec2, name):
     elif name == 'RT':
         if out_sel == OutSel.RT.value:
             return out, o_isvec
+    print ("get_pdecode_idx_out not found", name)
     return None, False
 
 
@@ -389,7 +723,8 @@ class ISACaller:
                  initial_insns=None, respect_pc=False,
                  disassembly=None,
                  initial_pc=0,
-                 bigendian=False):
+                 bigendian=False,
+                 mmu=False):
 
         self.bigendian = bigendian
         self.halted = False
@@ -424,9 +759,13 @@ class ISACaller:
 
         # set up registers, instruction memory, data memory, PC, SPRs, MSR
         self.svp64rm = SVP64RM()
-        self.svstate = SVP64State(initial_svstate)
+        if isinstance(initial_svstate, int):
+            initial_svstate = SVP64State(initial_svstate)
+        self.svstate = initial_svstate
         self.gpr = GPR(decoder2, self, self.svstate, regfile)
         self.mem = Mem(row_bytes=8, initial_mem=initial_mem)
+        if mmu:
+            self.mem = RADIX(self.mem,self)
         self.imem = Mem(row_bytes=4, initial_mem=initial_insns)
         self.pc = PC()
         self.spr = SPR(decoder2, initial_sprs)
@@ -634,7 +973,7 @@ class ISACaller:
         so = so | ov
         self.spr['XER'][XER_bits['SO']] = so
 
-    def handle_comparison(self, outputs):
+    def handle_comparison(self, outputs, cr_idx=0):
         out = outputs[0]
         assert isinstance(out, SelectableInt), \
             "out zero not a SelectableInt %s" % repr(outputs)
@@ -652,11 +991,11 @@ class ISACaller:
         SO = self.spr['XER'][XER_bits['SO']]
         print("handle_comparison SO", SO)
         cr_field = selectconcat(negative, positive, zero, SO)
-        self.crl[0].eq(cr_field)
+        self.crl[cr_idx].eq(cr_field)
 
     def set_pc(self, pc_val):
         self.namespace['NIA'] = SelectableInt(pc_val, 64)
-        self.pc.update(self.namespace)
+        self.pc.update(self.namespace, self.is_svp64_mode)
 
     def setup_one(self):
         """set up one instruction
@@ -677,11 +1016,12 @@ class ISACaller:
         yield self.dec2.dec.bigendian.eq(self.bigendian)
         yield self.dec2.state.msr.eq(self.msr.value)
         yield self.dec2.state.pc.eq(pc)
+        yield self.dec2.state.svstate.eq(self.svstate.spr.value)
 
         # SVP64.  first, check if the opcode is EXT001, and SVP64 id bits set
         yield Settle()
         opcode = yield self.dec2.dec.opcode_in
-        pfx = SVP64PrefixFields()
+        pfx = SVP64PrefixFields() # TODO should probably use SVP64PrefixDecoder
         pfx.insn.value = opcode
         major = pfx.major.asint(msb0=True) # MSB0 inversion
         print ("prefix test: opcode:", major, bin(major),
@@ -689,12 +1029,15 @@ class ISACaller:
         self.is_svp64_mode = ((major == 0b000001) and
                               pfx.insn[7].value == 0b1 and
                               pfx.insn[9].value == 0b1)
+        self.pc.update_nia(self.is_svp64_mode)
         if not self.is_svp64_mode:
             return
 
         # in SVP64 mode.  decode/print out svp64 prefix, get v3.0B instruction
         print ("svp64.rm", bin(pfx.rm.asint(msb0=True)))
-        sv_rm = pfx.rm.asint()
+        print ("    svstate.vl", self.svstate.vl.asint(msb0=True))
+        print ("    svstate.mvl", self.svstate.maxvl.asint(msb0=True))
+        sv_rm = pfx.rm.asint(msb0=True)
         ins = self.imem.ld(pc+4, 4, False, True)
         print("     svsetup: 0x%x 0x%x %s" % (pc+4, ins & 0xffffffff, bin(ins)))
         yield self.dec2.dec.raw_opcode_in.eq(ins & 0xffffffff) # v3.0B suffix
@@ -714,8 +1057,10 @@ class ISACaller:
         opname = code.split(' ')[0]
         yield from self.call(opname)
 
+        # don't use this except in special circumstances
         if not self.respect_pc:
             self.fake_pc += 4
+
         print("execute one, CIA NIA", self.pc.CIA.value, self.pc.NIA.value)
 
     def get_assembly_name(self):
@@ -815,7 +1160,7 @@ class ISACaller:
         if instr_is_privileged and self.msr[MSRb.PR] == 1:
             self.TRAP(0x700, PIb.PRIV)
             self.namespace['NIA'] = self.trap_nia
-            self.pc.update(self.namespace)
+            self.pc.update(self.namespace, self.is_svp64_mode)
             return
 
         # check halted condition
@@ -832,7 +1177,7 @@ class ISACaller:
             print("illegal", name, asmop)
             self.TRAP(0x700, PIb.ILLEG)
             self.namespace['NIA'] = self.trap_nia
-            self.pc.update(self.namespace)
+            self.pc.update(self.namespace, self.is_svp64_mode)
             print("name %s != %s - calling ILLEGAL trap, PC: %x" %
                   (name, asmop, self.pc.CIA.value))
             return
@@ -853,6 +1198,17 @@ class ISACaller:
             dest_cr, src_cr, src_byname, dest_byname = False, False, {}, {}
         print ("sv rm", sv_rm, dest_cr, src_cr, src_byname, dest_byname)
 
+        # get SVSTATE srcstep.  TODO: dststep (twin predication)
+        srcstep = self.svstate.srcstep.asint(msb0=True)
+        vl = self.svstate.vl.asint(msb0=True)
+        mvl = self.svstate.maxvl.asint(msb0=True)
+
+        # VL=0 in SVP64 mode means "do nothing: skip instruction"
+        if self.is_svp64_mode and vl == 0:
+            self.pc.update(self.namespace, self.is_svp64_mode)
+            print("end of call", self.namespace['CIA'], self.namespace['NIA'])
+            return
+
         # main input registers (RT, RA ...)
         inputs = []
         for name in input_names:
@@ -860,8 +1216,15 @@ class ISACaller:
             # (mapping name RA RB RC RS to in1, in2, in3)
             regnum, is_vec = yield from get_pdecode_idx_in(self.dec2, name)
             if regnum is None:
+                # doing this is not part of svp64, it's because output
+                # registers, to be modified, need to be in the namespace.
                 regnum, is_vec = yield from get_pdecode_idx_out(self.dec2, name)
-            #regnum = yield getattr(self.decoder, name)
+            # here's where we go "vector".  TODO: zero-testing (RA_IS_ZERO)
+            # XXX already done by PowerDecoder2, now
+            #if is_vec:
+            #   regnum += srcstep # TODO, elwidth overrides
+
+            # in case getting the register number is needed, _RA, _RB
             regname = "_" + name
             self.namespace[regname] = regnum
             print('reading reg %s %d' % (name, regnum), is_vec)
@@ -878,9 +1241,9 @@ class ISACaller:
         # clear trap (trap) NIA
         self.trap_nia = None
 
-        print(inputs)
+        print("inputs", inputs)
         results = info.func(self, *inputs)
-        print(results)
+        print("results", results)
 
         # "inject" decorator takes namespace from function locals: we need to
         # overwrite NIA being overwritten (sigh)
@@ -929,7 +1292,8 @@ class ISACaller:
         else:
             rc_en = False
         if rc_en:
-            self.handle_comparison(results)
+            regnum, is_vec = yield from get_pdecode_cr_out(self.dec2, "CR0")
+            self.handle_comparison(results, regnum)
 
         # any modified return results?
         if info.write_regs:
@@ -953,15 +1317,46 @@ class ISACaller:
                     if name == 'MSR':
                         print('msr written', hex(self.msr.value))
                 else:
-                    regnum = yield getattr(self.decoder, name)
-                    print('writing reg %d %s' % (regnum, str(output)))
+                    regnum, is_vec = yield from get_pdecode_idx_out(self.dec2,
+                                                name)
+                    if regnum is None:
+                        # temporary hack for not having 2nd output
+                        regnum = yield getattr(self.decoder, name)
+                        is_vec = False
+                    print('writing reg %d %s' % (regnum, str(output)), is_vec)
                     if output.bits > 64:
                         output = SelectableInt(output.value, 64)
                     self.gpr[regnum] = output
 
-        print("end of call", self.namespace['CIA'], self.namespace['NIA'])
+        # check if it is the SVSTATE.src/dest step that needs incrementing
+        # this is our Sub-Program-Counter loop from 0 to VL-1
+        if self.is_svp64_mode:
+            # XXX twin predication TODO
+            vl = self.svstate.vl.asint(msb0=True)
+            mvl = self.svstate.maxvl.asint(msb0=True)
+            srcstep = self.svstate.srcstep.asint(msb0=True)
+            print ("    svstate.vl", vl)
+            print ("    svstate.mvl", mvl)
+            print ("    svstate.srcstep", srcstep)
+            # check if srcstep needs incrementing by one, stop PC advancing
+            # svp64 loop can end early if the dest is scalar
+            svp64_dest_vector = not (yield self.dec2.no_out_vec)
+            if svp64_dest_vector and srcstep != vl-1:
+                self.svstate.srcstep += SelectableInt(1, 7)
+                self.pc.NIA.value = self.pc.CIA.value
+                self.namespace['NIA'] = self.pc.NIA
+                print("end of sub-pc call", self.namespace['CIA'],
+                                     self.namespace['NIA'])
+                return # DO NOT allow PC to update whilst Sub-PC loop running
+            # reset to zero
+            self.svstate.srcstep[0:7] = 0
+            print ("    svstate.srcstep loop end (PC to update)")
+            self.pc.update_nia(self.is_svp64_mode)
+            self.namespace['NIA'] = self.pc.NIA
+
         # UPDATE program counter
-        self.pc.update(self.namespace)
+        self.pc.update(self.namespace, self.is_svp64_mode)
+        print("end of call", self.namespace['CIA'], self.namespace['NIA'])
 
 
 def inject():