libogc/lwp_watchdog.c

   1 #include <stdlib.h>
   2 #include <limits.h>
   3 #include "asm.h"
   4 #include "lwp_threads.h"
   5 #include "lwp_watchdog.h"
   6
   7 //#define _LWPWD_DEBUG
   8
   9 #ifdef _LWPWD_DEBUG
  10 #include <stdio.h>
  11 #endif
  12
  13 vu32 _wd_sync_level;
  14 vu32 _wd_sync_count;
  15 u32 _wd_ticks_since_boot;
  16
  17 lwp_queue _wd_ticks_queue;
  18
  19 static void __lwp_wd_settimer(wd_cntrl *wd)
  20 {
  21         u64 now;
  22         s64 diff;
  23         union uulc {
  24                 u64 ull;
  25                 u32 ul[2];
  26         } v;
  27
  28         now = gettime();
  29         v.ull = diff = diff_ticks(now,wd->fire);
  30 #ifdef _LWPWD_DEBUG
  31         printf("__lwp_wd_settimer(%p,%llu,%lld)\n",wd,wd->fire,diff);
  32 #endif
  33         if(diff<=0) {
  34 #ifdef _LWPWD_DEBUG
  35                 printf(" __lwp_wd_settimer(0): %lld<=0\n",diff);
  36 #endif
  37                 wd->fire = 0;
  38                 mtdec(0);
  39         } else if(diff<0x0000000080000000LL) {
  40 #ifdef _LWPWD_DEBUG
  41                 printf("__lwp_wd_settimer(%d): %lld<0x0000000080000000LL\n",v.ul[1],diff);
  42 #endif
  43                 mtdec(v.ul[1]);
  44         } else {
  45 #ifdef _LWPWD_DEBUG
  46                 printf("__lwp_wd_settimer(0x7fffffff)\n");
  47 #endif
  48                 mtdec(0x7fffffff);
  49         }
  50 }
  51
  52 void __lwp_watchdog_init()
  53 {
  54         _wd_sync_level = 0;
  55         _wd_sync_count = 0;
  56         _wd_ticks_since_boot = 0;
  57
  58         __lwp_queue_init_empty(&_wd_ticks_queue);
  59 }
  60
  61 void __lwp_wd_insert(lwp_queue *header,wd_cntrl *wd)
  62 {
  63         u32 level;
  64         u64 fire;
  65         u32 isr_nest_level;
  66         wd_cntrl *after;
  67 #ifdef _LWPWD_DEBUG
  68         printf("__lwp_wd_insert(%p,%llu,%llu)\n",wd,wd->start,wd->fire);
  69 #endif
  70         isr_nest_level = __lwp_isr_in_progress();
  71         wd->state = LWP_WD_INSERTED;
  72
  73         _wd_sync_count++;
  74 restart:
  75         _CPU_ISR_Disable(level);
  76         fire = wd->fire;
  77         for(after=__lwp_wd_first(header);;after=__lwp_wd_next(after)) {
  78                 if(fire==0 || !__lwp_wd_next(after)) break;
  79                 if(fire<after->fire) break;
  80
  81                 _CPU_ISR_Flash(level);
  82                 if(wd->state!=LWP_WD_INSERTED) goto exit_insert;
  83                 if(_wd_sync_level>isr_nest_level) {
  84                         _wd_sync_level = isr_nest_level;
  85                         _CPU_ISR_Restore(level);
  86                         goto restart;
  87                 }
  88         }
  89         __lwp_wd_activate(wd);
  90         wd->fire = fire;
  91         __lwp_queue_insertI(after->node.prev,&wd->node);
  92         if(__lwp_wd_first(header)==wd) __lwp_wd_settimer(wd);
  93
  94 exit_insert:
  95         _wd_sync_level = isr_nest_level;
  96         _wd_sync_count--;
  97         _CPU_ISR_Restore(level);
  98         return;
  99 }
 100
 101 u32 __lwp_wd_remove(lwp_queue *header,wd_cntrl *wd)
 102 {
 103         u32 level;
 104         u32 prev_state;
 105         wd_cntrl *next;
 106 #ifdef _LWPWD_DEBUG
 107         printf("__lwp_wd_remove(%p)\n",wd);
 108 #endif
 109         _CPU_ISR_Disable(level);
 110         prev_state = wd->state;
 111         switch(prev_state) {
 112                 case LWP_WD_INACTIVE:
 113                         break;
 114                 case  LWP_WD_INSERTED:
 115                         wd->state = LWP_WD_INACTIVE;
 116                         break;
 117                 case LWP_WD_ACTIVE:
 118                 case LWP_WD_REMOVE:
 119                         wd->state = LWP_WD_INACTIVE;
 120                         next = __lwp_wd_next(wd);
 121                         if(_wd_sync_count) _wd_sync_level = __lwp_isr_in_progress();
 122                         __lwp_queue_extractI(&wd->node);
 123                         if(!__lwp_queue_isempty(header) && __lwp_wd_first(header)==next) __lwp_wd_settimer(next);
 124                         break;
 125         }
 126         _CPU_ISR_Restore(level);
 127         return prev_state;
 128 }
 129
 130 void __lwp_wd_tickle(lwp_queue *queue)
 131 {
 132         wd_cntrl *wd;
 133         u64 now;
 134         s64 diff;
 135
 136         if(__lwp_queue_isempty(queue)) return;
 137
 138         wd = __lwp_wd_first(queue);
 139         now = gettime();
 140         diff = diff_ticks(now,wd->fire);
 141 #ifdef _LWPWD_DEBUG
 142         printf("__lwp_wd_tickle(%p,%08x%08x,%08x%08x,%08x%08x,%08x%08x)\n",wd,(u32)(now>>32),(u32)now,(u32)(wd->start>>32),(u32)wd->start,(u32)(wd->fire>>32),(u32)wd->fire,(u32)(diff>>32),(u32)diff);
 143 #endif
 144         if(diff<=0) {
 145                 do {
 146                         switch(__lwp_wd_remove(queue,wd)) {
 147                                 case LWP_WD_ACTIVE:
 148                                         wd->routine(wd->usr_data);
 149                                         break;
 150                                 case LWP_WD_INACTIVE:
 151                                         break;
 152                                 case LWP_WD_INSERTED:
 153                                         break;
 154                                 case LWP_WD_REMOVE:
 155                                         break;
 156                         }
 157                         wd = __lwp_wd_first(queue);
 158                 } while(!__lwp_queue_isempty(queue) && wd->fire==0);
 159         } else {
 160                 __lwp_wd_reset(wd);
 161         }
 162 }
 163
 164 void __lwp_wd_adjust(lwp_queue *queue,u32 dir,s64 interval)
 165 {
 166         u32 level;
 167         u64 abs_int;
 168
 169         _CPU_ISR_Disable(level);
 170         abs_int = gettime()+LWP_WD_ABS(interval);
 171         if(!__lwp_queue_isempty(queue)) {
 172                 switch(dir) {
 173                         case LWP_WD_BACKWARD:
 174                                 __lwp_wd_first(queue)->fire += LWP_WD_ABS(interval);
 175                                 break;
 176                         case LWP_WD_FORWARD:
 177                                 while(abs_int) {
 178                                         if(abs_int<__lwp_wd_first(queue)->fire) {
 179                                                 __lwp_wd_first(queue)->fire -= LWP_WD_ABS(interval);
 180                                                 break;
 181                                         } else {
 182                                                 abs_int -= __lwp_wd_first(queue)->fire;
 183                                                 __lwp_wd_first(queue)->fire = gettime();
 184                                                 __lwp_wd_tickle(queue);
 185                                                 if(__lwp_queue_isempty(queue)) break;
 186                                         }
 187                                 }
 188                                 break;
 189                 }
 190         }
 191         _CPU_ISR_Restore(level);
 192 }