+#ifdef CONFIG_CPU_MIPSR2_SRS
+/*
+ * MIPSR2 shadow register set allocation
+ * FIXME: SMP...
+ */
+
+static struct shadow_registers {
+ /*
+ * Number of shadow register sets supported
+ */
+ unsigned long sr_supported;
+ /*
+ * Bitmap of allocated shadow registers
+ */
+ unsigned long sr_allocated;
+} shadow_registers;
+
+static void mips_srs_init(void)
+{
+ shadow_registers.sr_supported = ((read_c0_srsctl() >> 26) & 0x0f) + 1;
+ printk(KERN_INFO "%ld MIPSR2 register sets available\n",
+ shadow_registers.sr_supported);
+ shadow_registers.sr_allocated = 1; /* Set 0 used by kernel */
+}
+
+int mips_srs_max(void)
+{
+ return shadow_registers.sr_supported;
+}
+
+int mips_srs_alloc(void)
+{
+ struct shadow_registers *sr = &shadow_registers;
+ int set;
+
+again:
+ set = find_first_zero_bit(&sr->sr_allocated, sr->sr_supported);
+ if (set >= sr->sr_supported)
+ return -1;
+
+ if (test_and_set_bit(set, &sr->sr_allocated))
+ goto again;
+
+ return set;
+}
+
+void mips_srs_free(int set)
+{
+ struct shadow_registers *sr = &shadow_registers;
+
+ clear_bit(set, &sr->sr_allocated);
+}
+
+static void *set_vi_srs_handler(int n, void *addr, int srs)
+{
+ unsigned long handler;
+ unsigned long old_handler = vi_handlers[n];
+ u32 *w;
+ unsigned char *b;
+
+ if (!cpu_has_veic && !cpu_has_vint)
+ BUG();
+
+ if (addr == NULL) {
+ handler = (unsigned long) do_default_vi;
+ srs = 0;
+ } else
+ handler = (unsigned long) addr;
+ vi_handlers[n] = (unsigned long) addr;
+
+ b = (unsigned char *)(ebase + 0x200 + n*VECTORSPACING);
+
+ if (srs >= mips_srs_max())
+ panic("Shadow register set %d not supported", srs);
+
+ if (cpu_has_veic) {
+ if (board_bind_eic_interrupt)
+ board_bind_eic_interrupt (n, srs);
+ } else if (cpu_has_vint) {
+ /* SRSMap is only defined if shadow sets are implemented */
+ if (mips_srs_max() > 1)
+ change_c0_srsmap (0xf << n*4, srs << n*4);
+ }
+
+ if (srs == 0) {
+ /*
+ * If no shadow set is selected then use the default handler
+ * that does normal register saving and a standard interrupt exit
+ */
+
+ extern char except_vec_vi, except_vec_vi_lui;
+ extern char except_vec_vi_ori, except_vec_vi_end;
+#ifdef CONFIG_MIPS_MT_SMTC
+ /*
+ * We need to provide the SMTC vectored interrupt handler
+ * not only with the address of the handler, but with the
+ * Status.IM bit to be masked before going there.
+ */
+ extern char except_vec_vi_mori;
+ const int mori_offset = &except_vec_vi_mori - &except_vec_vi;
+#endif /* CONFIG_MIPS_MT_SMTC */
+ const int handler_len = &except_vec_vi_end - &except_vec_vi;
+ const int lui_offset = &except_vec_vi_lui - &except_vec_vi;
+ const int ori_offset = &except_vec_vi_ori - &except_vec_vi;
+
+ if (handler_len > VECTORSPACING) {
+ /*
+ * Sigh... panicing won't help as the console
+ * is probably not configured :(
+ */
+ panic ("VECTORSPACING too small");
+ }
+
+ memcpy (b, &except_vec_vi, handler_len);
+#ifdef CONFIG_MIPS_MT_SMTC
+ if (n > 7)
+ printk("Vector index %d exceeds SMTC maximum\n", n);
+ w = (u32 *)(b + mori_offset);
+ *w = (*w & 0xffff0000) | (0x100 << n);
+#endif /* CONFIG_MIPS_MT_SMTC */
+ w = (u32 *)(b + lui_offset);
+ *w = (*w & 0xffff0000) | (((u32)handler >> 16) & 0xffff);
+ w = (u32 *)(b + ori_offset);
+ *w = (*w & 0xffff0000) | ((u32)handler & 0xffff);
+ flush_icache_range((unsigned long)b, (unsigned long)(b+handler_len));
+ }
+ else {
+ /*
+ * In other cases jump directly to the interrupt handler
+ *
+ * It is the handlers responsibility to save registers if required
+ * (eg hi/lo) and return from the exception using "eret"
+ */
+ w = (u32 *)b;
+ *w++ = 0x08000000 | (((u32)handler >> 2) & 0x03fffff); /* j handler */
+ *w = 0;
+ flush_icache_range((unsigned long)b, (unsigned long)(b+8));
+ }
+
+ return (void *)old_handler;
+}
+
+void *set_vi_handler(int n, void *addr)
+{
+ return set_vi_srs_handler(n, addr, 0);
+}
+
+#else
+
+static inline void mips_srs_init(void)
+{
+}
+
+#endif /* CONFIG_CPU_MIPSR2_SRS */
+