arch/mips/netlogic/xlp/cop2-ex.c

   1 /*
   2  * This file is subject to the terms and conditions of the GNU General Public
   3  * License.  See the file "COPYING" in the main directory of this archive
   4  * for more details.
   5  *
   6  * Copyright (C) 2013 Broadcom Corporation.
   7  *
   8  * based on arch/mips/cavium-octeon/cpu.c
   9  * Copyright (C) 2009 Wind River Systems,
  10  *   written by Ralf Baechle <ralf@linux-mips.org>
  11  */
  12 #include <linux/init.h>
  13 #include <linux/irqflags.h>
  14 #include <linux/notifier.h>
  15 #include <linux/prefetch.h>
  16 #include <linux/sched.h>
  17
  18 #include <asm/cop2.h>
  19 #include <asm/current.h>
  20 #include <asm/mipsregs.h>
  21 #include <asm/page.h>
  22
  23 #include <asm/netlogic/mips-extns.h>
  24
  25 /*
  26  * 64 bit ops are done in inline assembly to support 32 bit
  27  * compilation
  28  */
  29 void nlm_cop2_save(struct nlm_cop2_state *r)
  30 {
  31         asm volatile(
  32                 ".set   push\n"
  33                 ".set   noat\n"
  34                 "dmfc2  $1, $0, 0\n"
  35                 "sd     $1, 0(%1)\n"
  36                 "dmfc2  $1, $0, 1\n"
  37                 "sd     $1, 8(%1)\n"
  38                 "dmfc2  $1, $0, 2\n"
  39                 "sd     $1, 16(%1)\n"
  40                 "dmfc2  $1, $0, 3\n"
  41                 "sd     $1, 24(%1)\n"
  42                 "dmfc2  $1, $1, 0\n"
  43                 "sd     $1, 0(%2)\n"
  44                 "dmfc2  $1, $1, 1\n"
  45                 "sd     $1, 8(%2)\n"
  46                 "dmfc2  $1, $1, 2\n"
  47                 "sd     $1, 16(%2)\n"
  48                 "dmfc2  $1, $1, 3\n"
  49                 "sd     $1, 24(%2)\n"
  50                 ".set   pop\n"
  51                 : "=m"(*r)
  52                 : "r"(r->tx), "r"(r->rx));
  53
  54         r->tx_msg_status = __read_32bit_c2_register($2, 0);
  55         r->rx_msg_status = __read_32bit_c2_register($3, 0) & 0x0fffffff;
  56 }
  57
  58 void nlm_cop2_restore(struct nlm_cop2_state *r)
  59 {
  60         u32 rstat;
  61
  62         asm volatile(
  63                 ".set   push\n"
  64                 ".set   noat\n"
  65                 "ld     $1, 0(%1)\n"
  66                 "dmtc2  $1, $0, 0\n"
  67                 "ld     $1, 8(%1)\n"
  68                 "dmtc2  $1, $0, 1\n"
  69                 "ld     $1, 16(%1)\n"
  70                 "dmtc2  $1, $0, 2\n"
  71                 "ld     $1, 24(%1)\n"
  72                 "dmtc2  $1, $0, 3\n"
  73                 "ld     $1, 0(%2)\n"
  74                 "dmtc2  $1, $1, 0\n"
  75                 "ld     $1, 8(%2)\n"
  76                 "dmtc2  $1, $1, 1\n"
  77                 "ld     $1, 16(%2)\n"
  78                 "dmtc2  $1, $1, 2\n"
  79                 "ld     $1, 24(%2)\n"
  80                 "dmtc2  $1, $1, 3\n"
  81                 ".set   pop\n"
  82                 : : "m"(*r), "r"(r->tx), "r"(r->rx));
  83
  84         __write_32bit_c2_register($2, 0, r->tx_msg_status);
  85         rstat = __read_32bit_c2_register($3, 0) & 0xf0000000u;
  86         __write_32bit_c2_register($3, 0, r->rx_msg_status | rstat);
  87 }
  88
  89 static int nlm_cu2_call(struct notifier_block *nfb, unsigned long action,
  90         void *data)
  91 {
  92         unsigned long flags;
  93         unsigned int status;
  94
  95         switch (action) {
  96         case CU2_EXCEPTION:
  97                 if (!capable(CAP_SYS_ADMIN) || !capable(CAP_SYS_RAWIO))
  98                         break;
  99                 local_irq_save(flags);
 100                 KSTK_STATUS(current) |= ST0_CU2;
 101                 status = read_c0_status();
 102                 write_c0_status(status | ST0_CU2);
 103                 nlm_cop2_restore(&(current->thread.cp2));
 104                 write_c0_status(status & ~ST0_CU2);
 105                 local_irq_restore(flags);
 106                 pr_info("COP2 access enabled for pid %d (%s)\n",
 107                                         current->pid, current->comm);
 108                 return NOTIFY_BAD;      /* Don't call default notifier */
 109         }
 110
 111         return NOTIFY_OK;               /* Let default notifier send signals */
 112 }
 113
 114 static int __init nlm_cu2_setup(void)
 115 {
 116         return cu2_notifier(nlm_cu2_call, 0);
 117 }
 118 early_initcall(nlm_cu2_setup);