stm32-ada/ravenscar.patch at master · telrob/stm32-ada · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
diff --git a/src/ada/s-bbthqu.adb b/src/ada/s-bbthqu.adb
index c5bc4e0..5cd4a59 100644
--- a/src/ada/s-bbthqu.adb
+++ b/src/ada/s-bbthqu.adb
@@ -91,9 +90,20 @@ package body System.BB.Threads.Queues is

          --  The running thread is no longer the highest priority thread

-         First_Thread_Table (CPU_Id) := Thread.Next;
+         if Thread = First_Thread_Table (CPU_Id) then
+            First_Thread_Table (CPU_Id) := Thread.Next;
+         else
+            Aux_Pointer := First_Thread_Table (CPU_Id);
+            while Aux_Pointer /= Null_Thread_Id
+              and then Aux_Pointer.Next /= Thread
+            loop
+               Aux_Pointer := Aux_Pointer.Next;
+            end loop;
+            pragma Assert (Aux_Pointer /= Null_Thread_Id);
+            Aux_Pointer.Next := Thread.Next;
+         end if;

-         Aux_Pointer := First_Thread_Table (CPU_Id);
+         Aux_Pointer := Thread.Next;

          --  FIFO_Within_Priorities dispatching policy. In ALRM D.2.2 it
          --  is said that when the active priority is lowered due to the
@@ -137,6 +147,7 @@ package body System.BB.Threads.Queues is
    -------------

    procedure Extract (Thread : Thread_Id) is
+      Aux_Pointer : Thread_Id;
       CPU_Id : constant CPU := Get_CPU (Thread);

    begin
@@ -147,14 +158,25 @@ package body System.BB.Threads.Queues is
       --  The only thread that can be extracted from the ready list is the one
       --  that is currently executing (as a result of a delay or a protected
       --  operation).
+      --  Because of PendSV, the above is not true.

       pragma Assert
         (Thread = Running_Thread_Table (CPU_Id)
-          and then Thread = First_Thread_Table (CPU_Id)
           and then Thread.State /= Runnable);

-      First_Thread_Table (CPU_Id) := Thread.Next;
-      Thread.Next := Null_Thread_Id;
+      if Thread = First_Thread_Table (CPU_Id) then
+         First_Thread_Table (CPU_Id) := Thread.Next;
+         Thread.Next := Null_Thread_Id;
+      else
+         Aux_Pointer := First_Thread_Table (CPU_Id);
+         while Aux_Pointer /= Null_Thread_Id
+           and then Aux_Pointer.Next /= Thread
+         loop
+            Aux_Pointer := Aux_Pointer.Next;
+         end loop;
+         pragma Assert (Aux_Pointer /= Null_Thread_Id);
+         Aux_Pointer.Next := Aux_Pointer.Next.Next;
+      end if;
    end Extract;

    -------------------------
@@ -387,11 +409,23 @@ package body System.BB.Threads.Queues is
       if Thread.Next /= Null_Thread_Id
         and then Thread.Next.Active_Priority = Prio
       then
-         First_Thread_Table (CPU_Id) := Thread.Next;
+         if Thread = First_Thread_Table (CPU_Id) then
+            First_Thread_Table (CPU_Id) := Thread.Next;
+         else
+            Aux_Pointer := First_Thread_Table (CPU_Id);
+            while Aux_Pointer /= Null_Thread_Id
+              and then Aux_Pointer.Next /= Thread
+            loop
+               Aux_Pointer := Aux_Pointer.Next;
+            end loop;
+            pragma Assert (Aux_Pointer /= Null_Thread_Id);
+            Aux_Pointer.Next := Thread.Next;
+         end if;
+
+         Aux_Pointer := Thread.Next;

          --  Look for the Aux_Pointer to insert the thread just after it

-         Aux_Pointer  := First_Thread_Table (CPU_Id);
          while Aux_Pointer.Next /= Null_Thread_Id
            and then Prio = Aux_Pointer.Next.Active_Priority
          loop