33
44mod algo;
55
6- use core:: mem;
76use core:: mem:: MaybeUninit ;
87
98use self :: algo:: * ;
109
11- fn find_func < T > ( tag : [ u8 ; 2 ] ) -> T {
12- let tag = u16:: from_le_bytes ( tag) ;
13-
10+ fn find_func < T > ( tag : [ u8 ; 2 ] ) -> Option < T > {
11+ let tag = u16:: from_le_bytes ( tag) as u32 ;
12+ type RomTableLookupFn = unsafe extern "C" fn ( table : * const u16 , code : u32 ) -> usize ;
13+ /// This location in flash holds a 16-bit truncated pointer for the ROM lookup function
14+ const ROM_TABLE_LOOKUP_PTR : * const u16 = 0x0000_0018 as _ ;
15+ /// This location in flash holds a 16-bit truncated pointer for the ROM function table
16+ /// (there's also a ROM data table which we don't need)
17+ const FUNC_TABLE : * const u16 = 0x0000_0014 as _ ;
1418 unsafe {
15- let mut entry = * ( 0x00000014 as * const u16 ) as * const u16 ;
16- loop {
17- let entry_tag = entry. read ( ) ;
18- if entry_tag == 0 {
19- panic ! ( "Func not found" ) ;
20- }
21- entry = entry. add ( 1 ) ;
22- let entry_addr = entry. read ( ) ;
23- entry = entry. add ( 1 ) ;
24- if entry_tag == tag {
25- return mem:: transmute_copy ( & ( entry_addr as u32 ) ) ;
26- }
19+ let lookup_func = ROM_TABLE_LOOKUP_PTR . read ( ) as usize ;
20+ let lookup_func: RomTableLookupFn = core:: mem:: transmute ( lookup_func) ;
21+ let table = FUNC_TABLE . read ( ) as usize ;
22+ let result = lookup_func ( table as * const u16 , tag) ;
23+ if result == 0 {
24+ return None ;
2725 }
26+ Some ( core:: mem:: transmute_copy ( & result) )
2827 }
2928}
3029
@@ -38,15 +37,15 @@ struct ROMFuncs {
3837}
3938
4039impl ROMFuncs {
41- fn load ( ) -> Self {
42- ROMFuncs {
43- connect_internal_flash : find_func ( * b"IF" ) ,
44- flash_exit_xip : find_func ( * b"EX" ) ,
45- flash_range_erase : find_func ( * b"RE" ) ,
46- flash_range_program : find_func ( * b"RP" ) ,
47- flash_flush_cache : find_func ( * b"FC" ) ,
48- flash_enter_cmd_xip : find_func ( * b"CX" ) ,
49- }
40+ fn load ( ) -> Option < Self > {
41+ Some ( ROMFuncs {
42+ connect_internal_flash : find_func ( * b"IF" ) ? ,
43+ flash_exit_xip : find_func ( * b"EX" ) ? ,
44+ flash_range_erase : find_func ( * b"RE" ) ? ,
45+ flash_range_program : find_func ( * b"RP" ) ? ,
46+ flash_flush_cache : find_func ( * b"FC" ) ? ,
47+ flash_enter_cmd_xip : find_func ( * b"CX" ) ? ,
48+ } )
5049 }
5150}
5251
@@ -58,14 +57,14 @@ algo!(RP2040Algo);
5857
5958const BLOCK_SIZE : u32 = 65536 ;
6059const SECTOR_SIZE : u32 = 4096 ;
61- const PAGE_SIZE : u32 = 256 ;
6260const BLOCK_ERASE_CMD : u8 = 0xd8 ;
6361const FLASH_BASE : u32 = 0x1000_0000 ;
6462
6563impl FlashAlgo for RP2040Algo {
6664 fn new ( _address : u32 , _clock : u32 , _function : u32 ) -> Result < Self , ErrorCode > {
67- let funcs = ROMFuncs :: load ( ) ;
68-
65+ let Some ( funcs) = ROMFuncs :: load ( ) else {
66+ return Err ( ErrorCode :: new ( 1 ) . unwrap ( ) ) ;
67+ } ;
6968 ( funcs. connect_internal_flash ) ( ) ;
7069 ( funcs. flash_exit_xip ) ( ) ;
7170 Ok ( Self { funcs } )
0 commit comments