initial import
[dosrtxon] / libs / mikmod / drivers / dos / dosirq.c
1 /*
2     Implementation of IRQ routines on DOS
3     Copyright (C) 1999 by Andrew Zabolotny, <bit@eltech.ru>
4
5     This library is free software; you can redistribute it and/or
6     modify it under the terms of the GNU Library General Public
7     License as published by the Free Software Foundation; either
8     version 2 of the License, or (at your option) any later version.
9
10     This library is distributed in the hope that it will be useful,
11     but WITHOUT ANY WARRANTY; without even the implied warranty of
12     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13     Library General Public License for more details.
14
15     You should have received a copy of the GNU Library General Public
16     License along with this library; if not, write to the Free
17     Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
18 */
19
20 #include "dosirq.h"
21
22 #include <dpmi.h>
23 #include <go32.h>
24 #include <dos.h>
25 #include <sys/nearptr.h>
26 #include <malloc.h>
27 #include <string.h>
28 #include "mikmod.h" /* for MikMod_malloc() & co */
29
30 unsigned int __irq_stack_size = 0x4000;
31 unsigned int __irq_stack_count = 1;
32
33 static void __int_stub_template (void)
34 {
35 /* *INDENT-OFF* */
36         asm("   pushal\n"
37                 "       pushl   %ds\n"
38                 "       pushl   %es\n"
39                 "       pushl   %fs\n"
40                 "       pushl   %gs\n"
41                 "       movw    $0x1234,%ax\n"          /* Get DPMI data selector */
42                 "       movw    %ax,%ds\n"                      /* Set DS and ES to data selector */
43                 "       movw    %ax,%es\n"
44                 "       movl    $0x12345678,%ebx\n"     /* Interrupt stack top */
45                 "       movl    (%ebx),%ecx\n"
46                 "       movl    %ecx,%edx\n"
47                 "       subl    $0x12345678,%ecx\n"     /* Subtract irq_stack_count */
48                 "       movl    %ecx,(%ebx)\n"
49                 "       movw    %ss,%si\n"                      /* Save old SS:ESP */
50                 "       movl    %esp,%edi\n"
51                 "       movl    %edx,%esp\n"            /* Set SS:ESP to interrupt stack */
52                 "       movw    %ax,%ss\n"
53                 "       pushl   %esi\n"
54                 "       pushl   %edi\n"
55                 "       pushl   %ebx\n"
56                 "       pushl   %edx\n"
57                 "       call    1f\n"                           /* Call user interrupt handler */
58                 "1:     popl    %edx\n"
59                 "       popl    %ebx\n"
60                 "       movl    %edx,(%ebx)\n"
61                 "       popl    %edi\n"
62                 "       popl    %esi\n"
63                 "       movl    %edi,%esp\n"            /* Restore old SS:ESP */
64                 "       movw    %si,%ss\n"
65                 "       popl    %gs\n"
66                 "       popl    %fs\n"
67                 "       popl    %es\n"
68                 "       popl    %ds\n"
69                 "       popal\n"
70                 "       iret\n");
71 /* *INDENT-ON* */
72 }
73
74 #include <stdio.h>
75
76 static int _allocate_iret_wrapper(_go32_dpmi_seginfo * info)
77 {
78         unsigned char *irqtpl = (unsigned char *)__int_stub_template;
79         unsigned char *irqend, *irqwrapper, *tmp;
80         __dpmi_meminfo handler_info;
81         unsigned int wrappersize;
82
83         /* First, skip until pushal */
84         while (*irqtpl != 0x60)
85                 irqtpl++;
86         /* Now find the iret */
87         irqend = irqtpl;
88         while (*irqend++ != 0xcf);
89
90         wrappersize = 4 + __irq_stack_size * __irq_stack_count + 4 +
91           ((long)irqend - (long)irqtpl);
92         irqwrapper = (unsigned char *) MikMod_malloc(wrappersize);
93         /* Lock the wrapper */
94         handler_info.address = __djgpp_base_address + (unsigned long)irqwrapper;
95         handler_info.size = wrappersize;
96         if (__dpmi_lock_linear_region(&handler_info)) {
97                 MikMod_free(irqwrapper);
98                 return -1;
99         }
100
101         /* First comes the interrupt wrapper size */
102         *(unsigned long *)irqwrapper = wrappersize;
103
104         /* Next comes the interrupt stack */
105         tmp = irqwrapper + 4 + __irq_stack_size * __irq_stack_count;
106
107         /* The following dword is interrupt stack pointer */
108         *((void **)tmp) = tmp;
109         tmp += 4;
110
111         /* Now comes the interrupt wrapper itself */
112         memcpy(tmp, irqtpl, irqend - irqtpl);
113         *(unsigned short *)(tmp + 9) = _my_ds();
114         *(unsigned long *)(tmp + 16) = (unsigned long)tmp - 4;
115         *(unsigned long *)(tmp + 26) = __irq_stack_size;
116         *(unsigned long *)(tmp + 46) =
117           info->pm_offset - (unsigned long)(tmp + 50);
118
119         info->pm_offset = (unsigned long)tmp;
120         info->pm_selector = _my_cs();
121
122         return 0;
123 }
124
125 static void _free_iret_wrapper(_go32_dpmi_seginfo * info)
126 {
127         __dpmi_meminfo handler_info;
128
129         info->pm_offset -= 4 + __irq_stack_size * __irq_stack_count + 4;
130
131         handler_info.address = __djgpp_base_address + info->pm_offset;
132         handler_info.size = *(unsigned long *)info->pm_offset;
133         __dpmi_unlock_linear_region(&handler_info);
134
135         MikMod_free((void *)info->pm_offset);
136 }
137
138 struct irq_handle *irq_hook(int irqno, void (*handler)(), unsigned long size)
139 {
140         int interrupt;
141         struct irq_handle *irq;
142         __dpmi_version_ret version;
143         __dpmi_meminfo handler_info, struct_info;
144         _go32_dpmi_seginfo info;
145         unsigned long old_sel, old_ofs;
146
147         __dpmi_get_version(&version);
148         if (irqno < 8)
149                 interrupt = version.master_pic + irqno;
150         else
151                 interrupt = version.slave_pic + (irqno - 8);
152
153         if (_go32_dpmi_get_protected_mode_interrupt_vector(interrupt, &info))
154                 return NULL;
155
156         old_sel = info.pm_selector;
157         old_ofs = info.pm_offset;
158
159         info.pm_offset = (unsigned long)handler;
160         if (_allocate_iret_wrapper(&info))
161                 return NULL;
162
163         /* Lock the interrupt handler in memory */
164         handler_info.address = __djgpp_base_address + (unsigned long)handler;
165         handler_info.size = size;
166         if (__dpmi_lock_linear_region(&handler_info)) {
167                 _free_iret_wrapper(&info);
168                 return NULL;
169         }
170
171         irq = (struct irq_handle *) MikMod_malloc(sizeof(struct irq_handle));
172         irq->c_handler = handler;
173         irq->handler_size = size;
174         irq->handler = info.pm_offset;
175         irq->prev_selector = old_sel;
176         irq->prev_offset = old_ofs;
177         irq->int_num = interrupt;
178         irq->irq_num = irqno;
179         irq->pic_base = irqno < 8 ? PIC1_BASE : PIC2_BASE;
180
181         struct_info.address = __djgpp_base_address + (unsigned long)irq;
182         struct_info.size = sizeof(struct irq_handle);
183         if (__dpmi_lock_linear_region(&struct_info)) {
184                 MikMod_free(irq);
185                 __dpmi_unlock_linear_region(&handler_info);
186                 _free_iret_wrapper(&info);
187                 return NULL;
188         }
189
190         _go32_dpmi_set_protected_mode_interrupt_vector(interrupt, &info);
191
192         irq->pic_mask = irq_state(irq);
193         return irq;
194 }
195
196 void irq_unhook(struct irq_handle *irq)
197 {
198         _go32_dpmi_seginfo info;
199         __dpmi_meminfo mem_info;
200
201         if (!irq)
202                 return;
203
204         /* Restore the interrupt vector */
205         irq_disable(irq);
206         info.pm_offset = irq->prev_offset;
207         info.pm_selector = irq->prev_selector;
208         _go32_dpmi_set_protected_mode_interrupt_vector(irq->int_num, &info);
209
210         /* Unlock the interrupt handler */
211         mem_info.address = __djgpp_base_address + (unsigned long)irq->c_handler;
212         mem_info.size = irq->handler_size;
213         __dpmi_unlock_linear_region(&mem_info);
214
215         /* Unlock the irq_handle structure */
216         mem_info.address = __djgpp_base_address + (unsigned long)irq;
217         mem_info.size = sizeof(struct irq_handle);
218         __dpmi_unlock_linear_region(&mem_info);
219
220         info.pm_offset = irq->handler;
221         _free_iret_wrapper(&info);
222
223         /* If IRQ was enabled before we hooked, restore enabled state */
224         if (irq->pic_mask)
225                 irq_enable(irq);
226         else
227                 irq_disable(irq);
228
229         MikMod_free(irq);
230 }
231
232 /*---------------------------------------------- IRQ detection mechanism -----*/
233 static struct irq_handle *__irqs[16];
234 static int (*__irq_confirm) (int irqno);
235 static volatile unsigned int __irq_mask;
236 static volatile unsigned int __irq_count[16];
237
238 #define DECLARE_IRQ_HANDLER(irqno)                                                      \
239 static void __irq##irqno##_handler ()                                           \
240 {                                                                                                                       \
241   if (irq_check (__irqs [irqno]) && __irq_confirm (irqno))      \
242   {                                                                                                                     \
243     __irq_count [irqno]++;                                                                      \
244     __irq_mask |= (1 << irqno);                                                         \
245   }                                                                                                                     \
246   irq_ack (__irqs [irqno]);                                                                     \
247 }
248
249 /* *INDENT-OFF* */
250 DECLARE_IRQ_HANDLER(0)
251 DECLARE_IRQ_HANDLER(1)
252 DECLARE_IRQ_HANDLER(2)
253 DECLARE_IRQ_HANDLER(3)
254 DECLARE_IRQ_HANDLER(4)
255 DECLARE_IRQ_HANDLER(5)
256 DECLARE_IRQ_HANDLER(6)
257 DECLARE_IRQ_HANDLER(7)
258 DECLARE_IRQ_HANDLER(8)
259 DECLARE_IRQ_HANDLER(9)
260 DECLARE_IRQ_HANDLER(10)
261 DECLARE_IRQ_HANDLER(11)
262 DECLARE_IRQ_HANDLER(12)
263 DECLARE_IRQ_HANDLER(13)
264 DECLARE_IRQ_HANDLER(14)
265 DECLARE_IRQ_HANDLER(15)
266 /* *INDENT-ON* */
267
268 static void (*__irq_handlers[16]) () = {
269         __irq0_handler, __irq1_handler, __irq2_handler, __irq3_handler,
270           __irq4_handler, __irq5_handler, __irq6_handler, __irq7_handler,
271           __irq8_handler, __irq9_handler, __irq10_handler, __irq11_handler,
272           __irq12_handler, __irq13_handler, __irq14_handler, __irq15_handler};
273
274 void irq_detect_start(unsigned int irqs, int (*irq_confirm) (int irqno))
275 {
276         int i;
277
278         __irq_mask = 0;
279         __irq_confirm = irq_confirm;
280         memset(&__irqs, 0, sizeof(__irqs));
281         memset((void *) &__irq_count, 0, sizeof(__irq_count));
282
283         /* Hook all specified IRQs */
284         for (i = 1; i <= 15; i++)
285                 if (irqs & (1 << i)) {
286                         __irqs[i] = irq_hook(i, __irq_handlers[i], 200);
287                         /* Enable the interrupt */
288                         irq_enable(__irqs[i]);
289                 }
290         /* Enable IRQ2 if we need at least one IRQ above 7 */
291         if (irqs & 0xff00)
292                 _irq_enable(2);
293 }
294
295 void irq_detect_end()
296 {
297         int i;
298         for (i = 15; i >= 1; i--)
299                 if (__irqs[i])
300                         irq_unhook(__irqs[i]);
301 }
302
303 int irq_detect_get(int irqno, unsigned int *irqmask)
304 {
305         int oldirq = disable();
306         int count = __irq_count[irqno];
307         *irqmask = __irq_mask;
308         __irq_mask = 0;
309         if (oldirq)
310                 enable();
311         return count;
312 }
313
314 void irq_detect_clear()
315 {
316         int oldirq = disable();
317         memset((void *) &__irq_count, 0, sizeof(__irq_count));
318         __irq_mask = 0;
319         if (oldirq)
320                 enable();
321 }
322
323 /* ex:set ts=4: */