1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <plat/inc/cmsis.h>
18 #include <cpu/inc/pendsv.h>
19 #include <stdio.h>
20
21
22 static PendsvCallbackF mSubscribers[MAX_PENDSV_SUBSCRIBERS] = {0,};
23
pendsvSubscribe(PendsvCallbackF cbk)24 bool pendsvSubscribe(PendsvCallbackF cbk)
25 {
26 int32_t i, free = -1;
27
28 //check for dupes and also look fro a free slot
29 for (i = 0; i < MAX_PENDSV_SUBSCRIBERS; i++) {
30 if (!mSubscribers[i])
31 free = i;
32 if (mSubscribers[i] == cbk)
33 return false;
34 }
35
36 //make sure we found a slot
37 if (free < 0)
38 return false;
39
40 mSubscribers[free] = cbk;
41 return true;
42 }
43
pendsvUnsubscribe(PendsvCallbackF cbk)44 bool pendsvUnsubscribe(PendsvCallbackF cbk)
45 {
46 uint32_t i;
47
48 for (i = 0; i < MAX_PENDSV_SUBSCRIBERS; i++) {
49 if (mSubscribers[i] == cbk) {
50 mSubscribers[i] = NULL;
51 return true;
52 }
53 }
54
55 return false;
56 }
57
pendsvTrigger(void)58 void pendsvTrigger(void)
59 {
60 SCB->ICSR = 1UL << 28;
61 }
62
pendsvClear(void)63 void pendsvClear(void)
64 {
65 SCB->ICSR = 1UL << 27;
66 }
67
pendsvIsPending(void)68 bool pendsvIsPending(void)
69 {
70 return !!(SCB->ICSR & (1UL << 28));
71 }
72
pendSvHandleC(struct PendsvRegsLow * loRegs,struct PendsvRegsHi * hiRegs)73 static void __attribute__((used)) pendSvHandleC(struct PendsvRegsLow *loRegs, struct PendsvRegsHi *hiRegs)
74 {
75 uint32_t i;
76
77 for (i = 0; i < MAX_PENDSV_SUBSCRIBERS; i++) {
78 if (mSubscribers[i])
79 mSubscribers[i](loRegs, hiRegs);
80 }
81 }
82
83 void PendSV_Handler(void);
PendSV_Handler(void)84 void PendSV_Handler(void)
85 {
86 asm volatile(
87 "tst lr, #4 \n"
88 "ite eq \n"
89 "mrseq r0, msp \n"
90 "mrsne r0, psp \n"
91 "push {r4-r11} \n"
92 "mov r1, sp \n"
93 "b pendSvHandleC\n"
94 );
95 }
96
97
98
99
100