@@ -34,6 +34,7 @@ use fast_trap::{FastContext, FastResult, FlowContext, FreeTrapStack, TrapStackBl
34
34
use hsm_cell:: HsmCell ;
35
35
use riscv:: register:: * ;
36
36
use rustsbi:: RustSBI ;
37
+ use sbi_spec:: binary:: SbiRet ;
37
38
use spin:: { Mutex , Once } ;
38
39
use uart_16550:: MmioSerialPort ;
39
40
@@ -173,7 +174,6 @@ extern "C" fn rust_main(_hartid: usize, opaque: usize) {
173
174
mie:: set_mext ( ) ;
174
175
mie:: set_msoft ( ) ;
175
176
mie:: set_mtimer ( ) ;
176
- mtvec:: write ( trap_vec:: trap_vec as _ , mtvec:: TrapMode :: Vectored ) ;
177
177
}
178
178
}
179
179
@@ -217,40 +217,54 @@ extern "C" fn fast_handler(
217
217
a6 : usize ,
218
218
a7 : usize ,
219
219
) -> FastResult {
220
- use mcause:: { Exception as E , Trap as T } ;
220
+ use mcause:: { Exception as E , Interrupt as I , Trap as T } ;
221
221
222
222
let cause = mcause:: read ( ) ;
223
- match cause. cause ( ) {
224
- // 启动
225
- T :: Exception ( E :: Unknown ) => match cause. bits ( ) {
226
- cause:: BOOT => {
227
- let hart_id = hart_id ( ) ;
228
- let hart_ctx = unsafe { ROOT_STACK [ hart_id] . hart_context ( ) } ;
229
- match unsafe { hart_ctx. hsm . local ( ) } . start ( ) {
230
- Ok ( supervisor) => {
231
- unsafe {
232
- mstatus:: set_mpie ( ) ;
233
- mstatus:: set_mpp ( mstatus:: MPP :: Supervisor ) ;
234
- }
235
- hart_ctx. trap . a [ 0 ] = hart_id;
236
- hart_ctx. trap . a [ 1 ] = supervisor. opaque ;
237
- hart_ctx. trap . pc = supervisor. start_addr ;
238
- }
239
- Err ( _state) => {
240
- hart_ctx. trap . pc = _stop as usize ;
241
- }
223
+ // 启动
224
+ if ( cause. cause ( ) == T :: Exception ( E :: Unknown ) && cause. bits ( ) == cause:: BOOT )
225
+ || cause. cause ( ) == T :: Interrupt ( I :: MachineSoft )
226
+ {
227
+ let hart_id = hart_id ( ) ;
228
+ let hart_ctx = unsafe { ROOT_STACK [ hart_id] . hart_context ( ) } ;
229
+ match unsafe { hart_ctx. hsm . local ( ) } . start ( ) {
230
+ Ok ( supervisor) => {
231
+ unsafe {
232
+ mtvec:: write ( trap_vec:: trap_vec as _ , mtvec:: TrapMode :: Vectored ) ;
233
+ mstatus:: set_mpie ( ) ;
234
+ mstatus:: set_mpp ( mstatus:: MPP :: Supervisor ) ;
242
235
}
243
- ctx. call ( 2 )
236
+ hart_ctx. trap . a [ 0 ] = hart_id;
237
+ hart_ctx. trap . a [ 1 ] = supervisor. opaque ;
238
+ hart_ctx. trap . pc = supervisor. start_addr ;
239
+ }
240
+ Err ( _state) => {
241
+ unsafe {
242
+ mtvec:: write ( trap_vec:: trap_vec as _ , mtvec:: TrapMode :: Direct ) ;
243
+ } ;
244
+ hart_ctx. trap . pc = _stop as usize ;
244
245
}
245
- _ => unreachable ! ( ) ,
246
- } ,
246
+ }
247
+ return ctx. call ( 2 ) ;
248
+ }
249
+ match cause. cause ( ) {
247
250
// SBI call
248
251
T :: Exception ( E :: SupervisorEnvCall ) => {
249
252
let ret = unsafe { SBI . assume_init_mut ( ) } . handle_ecall (
250
253
a7,
251
254
a6,
252
255
[ ctx. a0 ( ) , a1, a2, a3, a4, a5] ,
253
256
) ;
257
+ if ret. is_ok ( ) && a7 == sbi_spec:: hsm:: EID_HSM {
258
+ match a6 {
259
+ sbi_spec:: hsm:: HART_STOP => unsafe {
260
+ mtvec:: write ( trap_vec:: trap_vec as _ , mtvec:: TrapMode :: Direct ) ;
261
+ mstatus:: set_mpp ( mstatus:: MPP :: Machine ) ;
262
+ ROOT_STACK [ hart_id ( ) ] . hart_context ( ) . trap . pc = _stop as usize ;
263
+ return ctx. call ( 0 ) ;
264
+ } ,
265
+ _ => { }
266
+ }
267
+ }
254
268
mepc:: write ( mepc:: read ( ) + 4 ) ;
255
269
ctx. save_args ( ret. value , a2, a3, a4, a5, a6, a7) ;
256
270
ctx. write_a ( 0 , ret. error ) ;
@@ -389,3 +403,41 @@ type FixedRustSBI<'a> = RustSBI<
389
403
& ' a qemu_test:: QemuTest ,
390
404
Infallible ,
391
405
> ;
406
+
407
+ struct Hsm ;
408
+
409
+ impl rustsbi:: Hsm for Hsm {
410
+ fn hart_start ( & self , hartid : usize , start_addr : usize , opaque : usize ) -> SbiRet {
411
+ match unsafe { ROOT_STACK . get_mut ( hartid) . map ( |s| s. hart_context ( ) ) } {
412
+ Some ( hart) => {
413
+ if hart. hsm . remote ( ) . start ( Supervisor { start_addr, opaque } ) {
414
+ clint:: msip:: send ( hartid) ;
415
+ SbiRet :: success ( 0 )
416
+ } else {
417
+ SbiRet :: already_started ( )
418
+ }
419
+ }
420
+ None => SbiRet :: invalid_param ( ) ,
421
+ }
422
+ }
423
+
424
+ #[ inline]
425
+ fn hart_stop ( & self ) -> SbiRet {
426
+ unsafe { ROOT_STACK [ hart_id ( ) ] . hart_context ( ) . hsm . local ( ) . stop ( ) } ;
427
+ SbiRet :: success ( 0 )
428
+ }
429
+
430
+ #[ inline]
431
+ fn hart_get_status ( & self , hartid : usize ) -> SbiRet {
432
+ match unsafe { ROOT_STACK . get_mut ( hartid) . map ( |s| s. hart_context ( ) ) } {
433
+ Some ( hart) => SbiRet :: success ( hart. hsm . remote ( ) . sbi_get_status ( ) ) ,
434
+ None => SbiRet :: invalid_param ( ) ,
435
+ }
436
+ }
437
+
438
+ #[ inline]
439
+ fn hart_suspend ( & self , suspend_type : u32 , resume_addr : usize , opaque : usize ) -> SbiRet {
440
+ let _ = ( suspend_type, resume_addr, opaque) ;
441
+ SbiRet :: not_supported ( )
442
+ }
443
+ }
0 commit comments