123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303 |
- extern unsigned stack;
- extern unsigned stack_end;
- extern unsigned TBLPTRU;
- extern unsigned TBLPTRH;
- extern unsigned TBLPTRL;
- extern unsigned FSR0L;
- extern unsigned FSR0H;
- extern unsigned TABLAT;
- extern unsigned POSTINC0;
- extern unsigned POSTDEC0;
- #if 1
- char __uflags = 0;
- #endif
- extern void main (void);
- void _entry (void) __naked __interrupt 0;
- void _startup (void) __naked;
- void _do_cinit (void) __naked;
- void _entry (void) __naked __interrupt 0
- {
- __asm goto __startup __endasm;
- }
- void _startup (void) __naked
- {
- __asm
-
- lfsr 1, _stack_end
- lfsr 2, _stack_end
- clrf _TBLPTRU, 0
-
-
-
- bsf 0xa6, 7, 0
- bcf 0xa6, 6, 0
- __endasm ;
-
-
- __asm
-
- ; movlw 0xff
- ; movwf _FSR0L, 0
- setf _FSR0L
- movlw 0x0e
- movwf _FSR0H, 0
-
-
- ; movlw 1
- ; movwf 0x00, 0
- setf 0x00
-
-
- movlw 0x00
-
- clear_loop:
- clrf _POSTDEC0
- movf 0x00, w
- bnz clear_loop
- __endasm ;
- _do_cinit();
-
- main();
- loop:
-
- goto loop;
- }
- extern __code struct
- {
- unsigned short num_init;
- struct _init_entry {
- unsigned long from;
- unsigned long to;
- unsigned long size;
- } entries[];
- } cinit;
- #define TBLRDPOSTINC tblrd*+
- #define prom 0x00
- #define curr_byte 0x03
- #define curr_entry 0x05
- #define data_ptr 0x07
- void _do_cinit (void) __naked
- {
-
- __asm
- ; TBLPTR = &cinit
- movlw low(_cinit)
- movwf _TBLPTRL
- movlw high(_cinit)
- movwf _TBLPTRH
- movlw upper(_cinit)
- movwf _TBLPTRU
-
- ; curr_entry = cinit.num_init
- ; movlb data_ptr
- TBLRDPOSTINC
- movf _TABLAT, w
- movwf curr_entry
- TBLRDPOSTINC
- movf _TABLAT, w
- movwf curr_entry+1
- ; while (curr_entry) {
- test:
- bnz cont1 ;;done1
- tstfsz curr_entry, 1
- bra cont1
- done1:
- goto done
- cont1:
- ; Count down so we only have to look up the data in _cinit once.
- ; At this point we know that TBLPTR points to the top of the current
- ; entry in _cinit, so we can just start reading the from, to, and
- ; size values.
- ; read the source address low
- TBLRDPOSTINC
- movf _TABLAT, w
- movwf prom
-
- ; source address high
- TBLRDPOSTINC
- movf _TABLAT, w
- movwf prom + 1
- ; source address upper
- TBLRDPOSTINC
- movf _TABLAT, w
- movwf prom + 2
- ; skip a byte since it is stored as a 32bit int
- TBLRDPOSTINC
- ; read the destination address directly into FSR0
- ; destination address low
- TBLRDPOSTINC
- movf _TABLAT, w
- movwf _FSR0L
- ; destination address high
- TBLRDPOSTINC
- movf _TABLAT, w
- movwf _FSR0H
- ; skip two bytes since it is stored as a 32bit int
- TBLRDPOSTINC
- TBLRDPOSTINC
- ; read the size of data to transfer to destination address
- TBLRDPOSTINC
- movf _TABLAT, w
- movwf curr_byte
-
- TBLRDPOSTINC
- movf _TABLAT, w
- movwf curr_byte+1
-
- ; skip two bytes since it is stored as a 32bit int
- TBLRDPOSTINC
- TBLRDPOSTINC
- ; prom = data_ptr->from;
- ; FSR0 = data_ptr->to;
- ; curr_byte = (unsigned short) data_ptr->size;
- ; the table pointer now points to the next entry. Save it
- ; off since we will be using the table pointer to do the copying
- ; for the entry
-
- ; data_ptr = TBLPTR
- movff _TBLPTRL, data_ptr
- movff _TBLPTRH, data_ptr + 1
- movff _TBLPTRU, data_ptr + 2
-
- ; now assign the source address to the table pointer
- ; TBLPTR = prom
-
- movff prom, _TBLPTRL
- movff prom + 1, _TBLPTRH
- movff prom + 2, _TBLPTRU
- ; do the copy loop
- ; determine if we have any more bytes to copy
- ; movlb curr_byte
- movf curr_byte, w
- copy_loop:
- bnz copy_one_byte ; copy_one_byte
- movf curr_byte + 1, w
- bz done_copying
- copy_one_byte:
- TBLRDPOSTINC
- movf _TABLAT, w
- movwf _POSTINC0
- ; decrement byte counter
- decf curr_byte, f
- bc copy_loop ; copy_loop
- decf curr_byte + 1, f
- bra copy_one_byte
- done_copying:
-
- ; restore the table pointer for the next entry
- ; TBLPTR = data_ptr
- movff data_ptr, _TBLPTRL
- movff data_ptr + 1, _TBLPTRH
- movff data_ptr + 2, _TBLPTRU
- decf curr_entry, f
- bc do_next
- decf curr_entry + 1, f
- do_next:
- ; next entry...
- ; _do_cinit_curr_entry--;
- goto test;
- ; emit done label
- done:
- return
- __endasm;
- }
|