Contiki-NG
cooja_mtarch.c
1 /*
2  * Copyright (c) 2005, Swedish Institute of Computer Science
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  * 1. Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * 3. Neither the name of the Institute nor the names of its contributors
14  * may be used to endorse or promote products derived from this software
15  * without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
18  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
21  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
22  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
23  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
24  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
25  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
26  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
27  * SUCH DAMAGE.
28  *
29  * This file is part of the Contiki operating system.
30  *
31  */
32 
33 #include <stddef.h>
34 
35 #include <limits.h>
36 #include <stdio.h>
37 #include <string.h>
38 #include "sys/cooja_mt.h"
39 
40 #ifndef __WORDSIZE
41 #define __WORDSIZE 32
42 #endif /* __WORDSIZE */
43 
44 #ifndef ON_64BIT_ARCH
45 #if __WORDSIZE == 64
46 #define ON_64BIT_ARCH 1
47 #else /* ON_64BIT_ARCH */
48 #define ON_64BIT_ARCH 0
49 #endif /* __WORDSIZE == 64 */
50 #endif /* ON_64BIT_ARCH */
51 
52 struct frame {
53  unsigned long flags;
54 #if ON_64BIT_ARCH
55  unsigned long rbp;
56  unsigned long rdi;
57  unsigned long rsi;
58  unsigned long rdx;
59  unsigned long rcx;
60  unsigned long rbx;
61  unsigned long rax;
62 #else /* ON_64BIT_ARCH */
63  unsigned long ebp;
64  unsigned long edi;
65  unsigned long esi;
66  unsigned long edx;
67  unsigned long ecx;
68  unsigned long ebx;
69  unsigned long eax;
70 #endif /* ON_64BIT_ARCH */
71  unsigned long retaddr;
72  unsigned long retaddr2;
73  unsigned long data;
74 };
75 
76 /*--------------------------------------------------------------------------*/
77 void
78 cooja_mtarch_start(struct cooja_mtarch_thread *t,
79  void (*function)(void *), void *data)
80 {
81  struct frame *f = (struct frame *)&t->stack[COOJA_MTARCH_STACKSIZE - sizeof(struct frame)/sizeof(unsigned long)];
82  int i;
83 
84  for(i = 0; i < COOJA_MTARCH_STACKSIZE; ++i) {
85  t->stack[i] = i;
86  }
87 
88  memset(f, 0, sizeof(struct frame));
89  f->retaddr = (unsigned long)function;
90  f->data = (unsigned long)data;
91  t->sp = (unsigned long)&f->flags;
92 #if ON_64BIT_ARCH
93  f->rbp = (unsigned long)&f->rax;
94 #else /* ON_64BIT_ARCH */
95  f->ebp = (unsigned long)&f->eax;
96 #endif /* ON_64BIT_ARCH */
97 }
98 /*--------------------------------------------------------------------------*/
99 static struct cooja_mtarch_thread *cooja_running_thread;
100 /*--------------------------------------------------------------------------*/
101 void cooja_sw(void)
102 {
103  /* Store registers */
104 #if ON_64BIT_ARCH
105  __asm__ (
106  "pushq %rax\n\t"
107  "pushq %rbx\n\t"
108  "pushq %rcx\n\t"
109  "pushq %rdx\n\t"
110  "pushq %rsi\n\t"
111  "pushq %rdi\n\t"
112  "pushq %rbp\n\t"
113  "pushq %rbp\n\t");
114 #else /* ON_64BIT_ARCH */
115  __asm__ (
116  "pushl %eax\n\t"
117  "pushl %ebx\n\t"
118  "pushl %ecx\n\t"
119  "pushl %edx\n\t"
120  "pushl %esi\n\t"
121  "pushl %edi\n\t"
122  "pushl %ebp\n\t"
123  "pushl %ebp\n\t");
124 #endif /* ON_64BIT_ARCH */
125 
126  /* Switch stack pointer */
127 #if ON_64BIT_ARCH
128  __asm__ ("movq %0, %%rax\n\t" : : "m" (cooja_running_thread));
129  __asm__ (
130  "movq (%rax), %rbx\n\t"
131  "movq %rsp, (%rax)\n\t"
132  "movq %rbx, %rsp\n\t"
133  );
134 #else /* ON_64BIT_ARCH */
135  __asm__ ("movl %0, %%eax\n\t" : : "m" (cooja_running_thread));
136  __asm__ (
137  "movl (%eax), %ebx\n\t"
138  "movl %esp, (%eax)\n\t"
139  "movl %ebx, %esp\n\t"
140  );
141 #endif /* ON_64BIT_ARCH */
142 
143  /* Restore previous registers */
144 #if ON_64BIT_ARCH
145  __asm__ (
146  "popq %rbp\n\t"
147  "popq %rbp\n\t"
148  "popq %rdi\n\t"
149  "popq %rsi\n\t"
150  "popq %rdx\n\t"
151  "popq %rcx\n\t"
152  "popq %rbx\n\t"
153  "popq %rax\n\t"
154 
155  "leave\n\t"
156  "ret\n\t"
157  );
158 #else /* ON_64BIT_ARCH */
159  __asm__ (
160  "popl %ebp\n\t"
161  "popl %ebp\n\t"
162  "popl %edi\n\t"
163  "popl %esi\n\t"
164  "popl %edx\n\t"
165  "popl %ecx\n\t"
166  "popl %ebx\n\t"
167  "popl %eax\n\t"
168 
169  "leave\n\t"
170  "ret\n\t"
171  );
172 #endif /* ON_64BIT_ARCH */
173 
174 }
175 
176 /*--------------------------------------------------------------------------*/
177 void
178 cooja_mtarch_exec(struct cooja_mtarch_thread *t)
179 {
180  cooja_running_thread = t;
181  cooja_sw();
182  cooja_running_thread = NULL;
183 }
184 /*--------------------------------------------------------------------------*/
185 void
186 cooja_mtarch_yield(void)
187 {
188  cooja_sw();
189 }
190 /*--------------------------------------------------------------------------*/