- if (avr_has_pending_interrupts(avr)) {
- if (avr->pending_wait) {
- avr->pending_wait--;
- if (avr->pending_wait == 0) {
- for (int bi = 0; bi < 2; bi++)
- if (avr->pending[bi]) {
- uint32_t map = avr->pending[bi];
- while (map) {
- int bit = ffs(map)-1;
- int v = (bi * 32) + bit; // vector
- avr_int_vector_t * vector = avr->vector[v];
- // if that single interupt is masked, ignore it and continue
- if (vector && !avr_regbit_get(avr, vector->enable)) {
- map &= ~(1 << bit);
- continue;
- }
- if (vector && vector->trace)
- printf("%s calling %d\n", __FUNCTION__, v);
- _avr_push16(avr, avr->pc >> 1);
- avr->sreg[S_I] = 0;
- avr->pc = v * avr->vector_size;
-
- avr_clear_interrupt(avr, v);
- break;
- }
- break;
- }
- }
- } else
- avr->pending_wait = 2; // for next one...
+ if (!avr_has_pending_interrupts(avr))
+ return;
+
+ avr_int_table_p table = &avr->interrupts;
+
+ if (!table->pending_wait) {
+ table->pending_wait = 2; // for next one...
+ return;
+ }
+ table->pending_wait--;
+ if (table->pending_wait)
+ return;
+
+ // how many are pending...
+ int cnt = table->pending_w > table->pending_r ?
+ table->pending_w - table->pending_r :
+ (table->pending_w + INT_FIFO_SIZE) - table->pending_r;
+ // locate the highest priority one
+ int min = 0xff;
+ int mini = 0;
+ for (int ii = 0; ii < cnt; ii++) {
+ int vi = INT_FIFO_MOD(table->pending_r + ii);
+ avr_int_vector_t * v = table->pending[vi];
+ if (v->vector < min) {
+ min = v->vector;
+ mini = vi;
+ }
+ }
+ avr_int_vector_t * vector = table->pending[mini];
+
+ // now move the one at the front of the fifo in the slot of
+ // the one we service
+ table->pending[mini] = table->pending[table->pending_r++];
+ table->pending_r = INT_FIFO_MOD(table->pending_r);
+
+ // if that single interrupt is masked, ignore it and continue
+ // could also have been disabled, or cleared
+ if (!avr_regbit_get(avr, vector->enable) || !vector->pending) {
+ vector->pending = 0;
+ } else {
+ if (vector && vector->trace)
+ printf("%s calling %d\n", __FUNCTION__, (int)vector->vector);
+ _avr_push16(avr, avr->pc >> 1);
+ avr->sreg[S_I] = 0;
+ avr->pc = vector->vector * avr->vector_size;
+
+ avr_clear_interrupt(avr, vector);