arch/arc/plat-eznps/mtm.c

   1 /*
   2  * Copyright(c) 2015 EZchip Technologies.
   3  *
   4  * This program is free software; you can redistribute it and/or modify it
   5  * under the terms and conditions of the GNU General Public License,
   6  * version 2, as published by the Free Software Foundation.
   7  *
   8  * This program is distributed in the hope it will be useful, but WITHOUT
   9  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  10  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  11  * more details.
  12  *
  13  * The full GNU General Public License is included in this distribution in
  14  * the file called "COPYING".
  15  */
  16
  17 #include <linux/smp.h>
  18 #include <linux/io.h>
  19 #include <linux/log2.h>
  20 #include <asm/arcregs.h>
  21 #include <plat/mtm.h>
  22 #include <plat/smp.h>
  23
  24 #define MT_CTRL_HS_CNT          0xFF
  25 #define MT_CTRL_ST_CNT          0xF
  26 #define NPS_NUM_HW_THREADS      0x10
  27
  28 static void mtm_init_nat(int cpu)
  29 {
  30         struct nps_host_reg_mtm_cfg mtm_cfg;
  31         struct nps_host_reg_aux_udmc udmc;
  32         int log_nat, nat = 0, i, t;
  33
  34         /* Iterate core threads and update nat */
  35         for (i = 0, t = cpu; i < NPS_NUM_HW_THREADS; i++, t++)
  36                 nat += test_bit(t, cpumask_bits(cpu_possible_mask));
  37
  38         log_nat = ilog2(nat);
  39
  40         udmc.value = read_aux_reg(CTOP_AUX_UDMC);
  41         udmc.nat = log_nat;
  42         write_aux_reg(CTOP_AUX_UDMC, udmc.value);
  43
  44         mtm_cfg.value = ioread32be(MTM_CFG(cpu));
  45         mtm_cfg.nat = log_nat;
  46         iowrite32be(mtm_cfg.value, MTM_CFG(cpu));
  47 }
  48
  49 static void mtm_init_thread(int cpu)
  50 {
  51         int i, tries = 5;
  52         struct nps_host_reg_thr_init thr_init;
  53         struct nps_host_reg_thr_init_sts thr_init_sts;
  54
  55         /* Set thread init register */
  56         thr_init.value = 0;
  57         iowrite32be(thr_init.value, MTM_THR_INIT(cpu));
  58         thr_init.thr_id = NPS_CPU_TO_THREAD_NUM(cpu);
  59         thr_init.str = 1;
  60         iowrite32be(thr_init.value, MTM_THR_INIT(cpu));
  61
  62         /* Poll till thread init is done */
  63         for (i = 0; i < tries; i++) {
  64                 thr_init_sts.value = ioread32be(MTM_THR_INIT_STS(cpu));
  65                 if (thr_init_sts.thr_id == thr_init.thr_id) {
  66                         if (thr_init_sts.bsy)
  67                                 continue;
  68                         else if (thr_init_sts.err)
  69                                 pr_warn("Failed to thread init cpu %u\n", cpu);
  70                         break;
  71                 }
  72
  73                 pr_warn("Wrong thread id in thread init for cpu %u\n", cpu);
  74                 break;
  75         }
  76
  77         if (i == tries)
  78                 pr_warn("Got thread init timeout for cpu %u\n", cpu);
  79 }
  80
  81 int mtm_enable_thread(int cpu)
  82 {
  83         struct nps_host_reg_mtm_cfg mtm_cfg;
  84
  85         if (NPS_CPU_TO_THREAD_NUM(cpu) == 0)
  86                 return 1;
  87
  88         /* Enable thread in mtm */
  89         mtm_cfg.value = ioread32be(MTM_CFG(cpu));
  90         mtm_cfg.ten |= (1 << (NPS_CPU_TO_THREAD_NUM(cpu)));
  91         iowrite32be(mtm_cfg.value, MTM_CFG(cpu));
  92
  93         return 0;
  94 }
  95
  96 void mtm_enable_core(unsigned int cpu)
  97 {
  98         int i;
  99         struct nps_host_reg_aux_mt_ctrl mt_ctrl;
 100         struct nps_host_reg_mtm_cfg mtm_cfg;
 101
 102         if (NPS_CPU_TO_THREAD_NUM(cpu) != 0)
 103                 return;
 104
 105         /* Initialize Number of Active Threads */
 106         mtm_init_nat(cpu);
 107
 108         /* Initialize mtm_cfg */
 109         mtm_cfg.value = ioread32be(MTM_CFG(cpu));
 110         mtm_cfg.ten = 1;
 111         iowrite32be(mtm_cfg.value, MTM_CFG(cpu));
 112
 113         /* Initialize all other threads in core */
 114         for (i = 1; i < NPS_NUM_HW_THREADS; i++)
 115                 mtm_init_thread(cpu + i);
 116
 117
 118         /* Enable HW schedule, stall counter, mtm */
 119         mt_ctrl.value = 0;
 120         mt_ctrl.hsen = 1;
 121         mt_ctrl.hs_cnt = MT_CTRL_HS_CNT;
 122         mt_ctrl.sten = 1;
 123         mt_ctrl.st_cnt = MT_CTRL_ST_CNT;
 124         mt_ctrl.mten = 1;
 125         write_aux_reg(CTOP_AUX_MT_CTRL, mt_ctrl.value);
 126
 127         /*
 128          * HW scheduling mechanism will start working
 129          * Only after call to instruction "schd.rw".
 130          * cpu_relax() calls "schd.rw" instruction.
 131          */
 132         cpu_relax();
 133 }