; Comtf.inc - COMATOS Assembly Function Code ; Copyright 1999 Benjamin M. Greenblatt (11/3/99) OSInitF: ; Maximum Timeout Value sts (OSBase+OSMaxTimeoutH),OSTempH sts (OSBase+OSMaxTimeoutL),OSTempL ; Clear everything else clr OSTempL ldi OSTempH, 0xff ; -1 sts (OSBase+OSStatus),OSTempL sts (OSBase+OSNumTasks),OSTempH ; Initialize this to -1 sts (OSBase+OSCurrentTask),OSTempH sts (OSBase+OSReturnVal),OSTempL sts (OSBase+OSTimeoutH),OSTempL sts (OSBase+OSTimeoutL),OSTempL sts (OSBase+OSGlobalTime3),OSTempL sts (OSBase+OSGlobalTime2),OSTempL sts (OSBase+OSGlobalTime1),OSTempL sts (OSBase+OSGlobalTime0),OSTempL ; Start Timer0 ldi OSTempH, Timer0Clk out TCCR0, OSTempH ldi OSTempH, TimerMask out TIMSK, OSTempH ; If we are using the debugger, initialize the UART tst OSTempG ; If zero, we do not initialize the UART breq OSIN_1 ldi OSTempG, baud96 ; Set baud rate to 9600 out UBRR, OSTempG ldi OSTempG, 0b00001000 ; Enable transmit, but not interrupts out UCR, OSTempG OSIN_1: ret OSCreateTaskF: push OSTempG push ZH push ZL ; Increment Number of tasks lds OSTempG, OSBase+OSNumTasks inc OSTempG sts OSBase+OSNumTasks, OSTempG ; Put base address of PCBs in Z ldi ZH, high(PCBBase) ldi ZL, low(PCBBase) ; Loop sets Z to the offset for the new PCB OSCTF_1: tst OSTempG breq OSCTF_2 dec OSTempG adiw ZL,PCBSize rjmp OSCTF_1 OSCTF_2: ; Z Contains base address of the new process's PCB clr OSTempG std Z+PCBEntryPtrH, OSAddrRegH std Z+PCBEntryPtrL, OSAddrRegL std Z+PCBTimeoutVal, OSTempH std Z+PCBCurrentTime, OSTempG std Z+PCBMessMask, OSTempL std Z+PCBMessSrc, OSTempG std Z+PCBAck, OSTempG std Z+PCBState, OSTempG adiw ZL, PCBMessageBase ; Aim Z at the messages ldi OSTempH, 8 ; Number of mailboxes ; Clear the mailboxes OSCTF_3: st Z+, OSTempG dec OSTempH brne OSCTF_3 pop ZL pop ZH pop OSTempG ret ; Scheduler of tasks ; Runs through all available tasks and picks one that is eligible ; Note that we may be here for a while if no one's ready to run OSScheduler: ; Registers used: OSScdNum, OSScdC, OSTempG, OSTempH, Z, Y clr OSScdC lds OSScdNum, (OSBase+OSNumTasks) ; Get the number of tasks tst OSScdNum ; Check for lack of tasks brmi OSNoTasks ; If no tasks, die ; If we're here, there are tasks ldi YL, low(PCBBase) ; Load the first PCB location ldi YH, high(PCBBase) ; in the Z register OSScd_1:; Start of loop for each process ; Check if the task is using timeout ldd OSTempG, Y+PCBTimeoutVal ; Get the timeout value tst OSTempG breq OSScd_2 ; If zero, look at messages ldd OSTempG, Y+PCBCurrentTime ; Otherwise look at timeout tst OStempG ; If zero, elligible to run breq OSScd_Runnable ; Look at messages OSScd_2: ldd OSTempG, Y+PCBMessMask ldd OSTempH, Y+PCBMessSrc and OSTempH, OSTempG ; Extraneous messages are ok cp OSTempG, OSTempH ; Compare message mask and source brne OSScd_3 OSScd_Runnable: ; We've found a winner ; Run it ldi ZL, low(OSBase) ; Using Z and displacement addresssing ldi ZH, high(OSBase) ; is faster than using direct addressing std Z+OSCurrPCBL, YL ; Put the current PCB address std Z+OSCurrPCBH, YH ; into the OS block std Z+OSCurrentTask, OSScdC ; Indicate current task ldd OSTempH, Y+PCBTimeoutVal ; Place the maximum timeout std Y+PCBCurrentTime, OSTempH ; value in the current time ldd OSTempH, Z+OSMaxTimeoutH ; Also reset the OS Timeout std Z+OSTimeoutH, OSTempH ; counter ldd OSTempH, Z+OSMaxTimeoutL std Z+OSTimeoutL, OSTempH push YL ; Backup our own state push YH push OSScdC push OSScdNum ldd ZL, Y+PCBEntryPtrL ; Load the entry pointer ldd ZH, Y+PCBEntryPtrH ; into Z icall push OSTempG ; Set the current task back to 0xff to indicate that nothing is running ldi OSTempG, 0xff sts OSBase+OSCurrentTask, OSTempG ; Check the return code lds OSTempG, OSBase+OSReturnVal ; Load return code tst OSTempG ; Compare w. 0 brne OS_Bad_Ret_Code ; If not zero, bad return code pop OSTempG pop OSScdNum pop OSScdC pop YH pop YL OSScd_3: cp OSScdNum, OSScdC ; If we've gone through all processes breq OSScheduler ; Start looping again inc OSScdC ; Increment our loop counter adiw YL, PCBSize ; Increment Y register rjmp OSScd_1 ; Try next process OSNoTasks: ; Tell UART we only want to send one character ldi OSTempG, 1 sts OSBase+OSUARTCharCnt, OSTempG ; Send the no tasks flag ldi OSTempG, OSNoTasksFlag OSUARTWait OSTempG ; Send the message OSNoTasksHalt: rjmp OSNoTasksHalt OS_Bad_Ret_Code: ; Send three characters push ZH push ZL ldi OSTempG, 3 sts OSBase+OSUARTCharCnt, OSTempG ldi OSTempG, OSBadCode ldi ZH, high(OSBase+OSCurrentTask) ; Aim Z at the current task ldi ZL, low(OSBase+OSCurrentTask) OSUARTWait OSTempG ; Send the message pop ZL pop ZH pop OSTempG OSRegDump clr OSTempG out TIMSK, OSTempG ; Shut off the timers OS_Bad_Ret_Halt: rjmp OS_Bad_Ret_Halt OSHextoAscYF: push OSTempH clr OSTempH ; Counter which will become digit ; Do successive subtractions on 16s place OSHTA_1: cpi OSTempG, 0x10 brlo OSHTA_2 subi OSTempG, 0x10 ; Subtract 16 inc OSTempH ; Increment 16s place rjmp OSHTA_1 OSHTA_2: cpi OSTempH, 0x0a ; If a letter, we need to adjust for it brge OSHTA_5 subi OSTempH, -ascii0 OSHTA_3: st Y+, OSTempH cpi OSTempG, 0x0a brge OSHTA_6 subi OSTempG, -ascii0 OSHTA_4: st Y+, OSTempG pop OSTempH ret OSHTA_5: subi OSTempH, 0x0a subi OSTempH, -'A' ; Adjust by 'a' not '0' rjmp OSHTA_3 OSHTA_6: subi OSTempG, 0x0a subi OSTempG, -'A' rjmp OSHTA_4 OSUARTWaitF: out UDR, OSDumpReg ; Send the first character sbi UCR, UDRIE ; Enable tx empty interrupt OSUW_1: ; Keep looping until txdone bit is set lds OSDumpReg, OSBase+OSStatus sbrs OSDumpReg, txdone rjmp OSUW_1 ret OSGetMessageSrcF: push ZH push ZL push OSTempH OSPCBZ ; Put the current task's PCB address in Z ldd OSTempG, Z+PCBMessSrc ; Get the message src byte clr OSTempH std Z+PCBMessSrc, OSTempH ; Reset the message src pop OSTempH pop ZL pop ZH ret OSGetAckF: push ZH push ZL OSPCBZ ; Put the current task's PCB address in Z ldd OSTempG, Z+PCBAck ; Get the ack byte pop ZL pop ZH ret OSSendAckF: push ZH push ZL push OSTempH ldi ZH, high(PCBBase) ; Put the base address of the PCBs ldi ZL, low(PCBBase) ; into the Z register OSSAK_1: ; Find the correct PCB tst OSTempG ; We will be looping through all breq OSSAK_2 ; PCBs until we find the target's adiw ZL, PCBSize ; Go to next PCB dec OSTempG ; Decrement loop counter rjmp OSSAK_1 ; Next loop OSSAK_2: ; Set up the mask lds OSTempG, OSBase+OSCurrentTask ldi OSTempH, 0x01 ; Initialize the mask OSSAK_3: tst OSTempG ; Check to see if we're done looping breq OSSAK_4 lsl OSTempH ; Shift the mask over one dec OSTempG rjmp OSSAK_3 OSSAK_4: ldd OSTempG, Z+PCBAck or OSTempG, OSTempH ; Set the bit std Z+PCBAck, OSTempG pop OSTempH pop ZL pop ZH ret OSGetMessF: push ZH push ZL push OSTempL push OSTempH OSPCBZ ; Z starts out pointing to the current proces ldi OSTempL, 0xfe ; Start out with a mask clearing P0's bit sec OSGM_1: tst OSTempH breq OSGM_2 rol OSTempL dec OSTempH rjmp OSGM_1 OSGM_2: ldd OSTempH, Z+PCBMessSrc and OSTempH, OSTempL std Z+PCBMessSrc, OSTempH pop OSTempH push OSTempH OSGM_3: ; Find the correct message tst OSTempH ; We will be looping through all breq OSGM_4 ;messages until we find the right one adiw ZL, 1 ; Next message dec OSTempH rjmp OSGM_3 OSGM_4: ldd OSTempG, Z+PCBMessageBase ; Bring in the message ; Clear the message source bit for this message pop OSTempH ; Get the task number of the sender pop OSTempL pop ZL pop ZH ret OSSendMessF: push ZH push ZL push OSTempH ; We're going to need the scratch space ldi ZH, high(PCBBase) ; Put the base address of the PCBs ldi ZL, low(PCBBase) ; into the Z register OSSM_1: ; Find the correct PCB tst OSTempL ; We will be looping through all breq OSSM_2 ; PCBs until we find the target's adiw ZL, PCBSize ; Go to next PCB dec OSTempL ; Decrement loop counter rjmp OSSM_1 ; Next loop ; Z now points to the target PCB OSSM_2: ; Set the message source bit using a mask formed in OSTempL ldi OSTempL, 0x01 ; Initialize our mask OSSM_3: tst OSTempH ; Check to see if we're done looping breq OSSM_4 lsl OSTempL ; Shift the mask over one dec OSTempH rjmp OSSM_3 OSSM_4: ldd OSTempH, Z+PCBMessSrc ; Set the bit in messagesrc or OSTempH, OSTempL std Z+PCBMessSrc, OSTempH ; Z still points to the target PCB pop OSTempH ; In the last loop, we lost the current task adiw ZL, PCBMessageBase ; Aim Z at the messages add ZL, OSTempH ; Increment Z by OSTempH clr OSTempH ; So it points at the current task's adc ZH, OSTempH ; message slot st Z, OSTempG ; Write the message pop ZL pop ZH ret OSSetTimeoutF: push ZH push ZL OSPCBZ ; Put the current task's PCB address in Z std Z+PCBTimeoutVal, OSTempG ; Store the new timeout value std Z+PCBCurrentTime, OSTempG ; Reset the timer with the new value pop ZL pop ZH ret OSSetMessMaskF: push ZH push ZL OSPCBZ ; Put the current task's PCB address in Z std Z+PCBMessMask, OSTempG ; Store the new message mask pop ZL pop ZH ret OSRegDumpF: push YH push YL push ZH push ZL push OSDumpReg clr ZH ; Aim Z pointer at registers clr ZL ldi OSDumpReg, 28 ; Sending up to r28 sts OSBase+OSUARTCharCnt, OSDumpReg ; Indicate to UART how many characters to send ; Start sending beginning with DumpFlag ldi OSDumpReg, OSRegDumpFlag OSUARTWait OSDumpReg ; Send up to (not including) Z register ldi OSDumpReg, 3 sts OSBase+OSUARTCharCnt, OSDumpReg ; Indicate to UART how many characters to send ; Aim Z register at r28 ldi ZL, 28 ldi ZH, 00 pop OSDumpReg ; Get Dump Register for sending push OSDumpReg ; But keep copy on stack OSUARTWait OSDumpReg pop OSDumpReg pop YL ; Get everything back again pop YH ; but we are putting Z's value in Y push YH push YL ; We'll need them again push OSDumpReg ; Confused yet? ldi OSDumpReg, 2 ; Two more bytes to send (Z register) sts OSBase+OSUARTCharCnt, OSDumpReg ; Indicate to UART how many characters to send ldi ZL, 29 ; Aim Z at r29 ldi ZH, 0 OSUARTWait YL ; OK, we've dumped everything pop OSDumpReg pop ZL pop ZH pop YL pop YH ret OSSetEntryPtF: ; Load Z register with the pointer to the current PCB push ZL push ZH OSPCBZ ; Store the new entry pointer value into the PCB std Z+PCBEntryPtrH, OSTempH std Z+PCBEntryPtrL, OSTempL pop ZH pop ZL ret ; Timer 0 ISR ; Increments global system time ; Decrements process counters if they are using timeout ; Halts the OS if a process exceeds the expected runtime OST0ISR: push OSTempG in OSTempG, SREG push OSTempG ldi OSTempG, startval out tcnt0, OSTempG ; Check to see if this is the first half millisecond or a full ; millisecond lds OSTempG, (OSBase+OSStatus) sbrc OSTempG, t0ms rjmp OST0_1 ; If it is a 1, we do a timer tick ori OSTempG, exp2(t0ms) ; If not, we set the bit and wait ; for the next interrupt sts (OSBase+OSStatus), OSTempG pop OSTempG out SREG, OSTempG pop OSTempG reti OST0_1: ; If we're here, we have to do the full tick services push OST0Num push OSTempL push ZH push ZL ; Load Z with the OSBase address (faster than using lds/sts) ldi ZL, low(OSBase) ldi ZH, high(OSBase) ; First, we clear the t0ms bit andi OSTempG, 0xfe std Z+OSStatus, OSTempG ; Increment the global system time ldi OST0Num, 1 ldd OSTempG, Z+OSGlobalTime0 add OSTempG, OST0Num std Z+OSGlobalTime0, OSTempG clr OST0Num ldd OSTempG, Z+OSGlobalTime1 adc OSTempG, OST0Num std Z+OSGlobalTime1, OSTempG ldd OSTempG, Z+OSGlobalTime2 adc OSTempG, OST0Num std Z+OSGlobalTime2, OSTempG ldd OSTempG, Z+OSGlobalTime3 adc OSTempG, OST0Num std Z+OSGlobalTime3, OSTempG ; Make sure there are actual tasks running ldd OST0Num, Z+OSNumTasks tst OST0Num ; OST0Num has the number of tasks brmi OST0_3 inc OST0Num ; Makes our lives easier ; If we have tasks, we need to decrement their counters ldi ZL, low(PCBBase+PCBCurrentTime) ldi ZH, high(PCBBase+PCBCurrentTime) ; Loop through each task and decrement the counter OST0_2: ld OSTempG, Z ; Get the counter tst OSTempG ; Make sure it's not zero brbs 1, OST0_currnz ; Skip if it is zero dec OSTempG ; If it is zero, don't decrement OST0_currnz: st Z, OSTempG ; Put it back where you got it adiw ZL, PCBSize ; Go to the next counter dec OST0Num ; Decrement the counter for the number of ; processes brne OST0_2 ; If it's nonzero, go to next process ; Now, check the OS Task Timeout counter ; If it is zero and there is a task running, die ; If it is non-zero, decrement it ldi ZL, low(OSBase) ldi ZH, high(OSBase) ldd OSTempG, Z+OSCurrentTask ; If the current task is set to 0xff sbrc OSTempG, 7 ; it means that nothing is running rjmp OST0_3 ; therefore we do not need the timeout timer ldd OSTempG, Z+OSTimeoutH ldd OSTempL, Z+OSTimeoutL tst OSTempG brne OST0_2notimeout tst OSTempL breq OST0_crash OST0_2notimeout: ; Decrement the OSTimeout Timer clr OST0Num subi OSTempL, 1 sbc OSTempG, OST0Num std Z+OSTimeoutH, OSTempG std Z+OSTimeoutL, OSTempL OST0_3: ; We're done, pop everything and leave pop ZL pop ZH pop OSTempL pop OST0Num pop OSTempG out SREG, OSTempG pop OSTempG reti OST0_crash: ; Stop the OS Timer clr OSTempG out TIMSK, OSTempG ; Get task number ready to send lds OSTempG, OSBase+OSCurrentTask ; Get the current task number subi OSTempG, -ascii0 ; Make it ascii ldi ZL, low(USRBase) ; Place the next character to send ldi ZH, high(USRBase) ; in memory st Z, OSTempG ; Tell UART how many characters we want to send ldi OSTempG, 2 sts OSBase+OSUARTCharCnt, OSTempG ; send the timeout crash flag ldi OSTempG, OSTimeoutCrash sei ; Enable interrupts OSUARTWait OSTempG ; Pop everything so we most regs the way they were ; in order to do register dump pop ZL pop ZH pop OSTempL pop OST0Num pop OSTempG pop OSTempG ; Do register dump OSRegDump ; Now dump the PC ; Tell UART how many characters we want to send ldi OSTempG, 3 sts OSBase+OSUARTCharCnt, OSTempG pop OSTempH ; PC was on the stack pop OSTempL ; at this point ldi ZH, high(USRBase) ldi ZL, low(USRBase) sts USRBase, OSTempH ; Put address in memory sts USRBase+1, OSTempL ; where UART ISR can access it ldi OSTempG, OSPCDumpFlag ; Send the PC dump flag OSUARTWait OSTempG OST0_crash_halt: rjmp OST0_crash_halt ; UART TX Empty ISR ; Sets the txdone bit in the status register when done and shuts off UART OSTXISR: sts OSBase+OSUARTTemp, OSTempG ; Place OSTempG where we can get at it in OSTempG, SREG ; Backup SREG push OSTempG lds OSTempG, OSBase+OSUARTCharCnt ; Get number of characters to send dec OSTempG sts OSBase+OSUARTCharCnt, OSTempG breq OSTXI_Done ; If count is zero, stop sending stuff lds OSTempG, OSBase+OSUARTTemp ; What if Z is pointing to OSTempG? ld OSTempG, Z+ ; Get next character out UDR, OSTempG ; Send it rjmp OSTXI_Exit ; Leave ISR OSTXI_Done: lds OSTempG, OSBase+OSStatus ; Update the status flag ori OSTempG, exp2(txdone) sts OSBase+OSStatus, OSTempG cbi UCR, UDRIE ;clear the TXempty interrupt OSTXI_Exit: pop OSTempG out SREG, OSTempG lds OSTempG, OSBase+OSUARTTemp reti