Commit | Line | Data |
---|---|---|
4a66d3fe NC |
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 | } |