3 files changed, 64 insertions, 13 deletions
diff --git a/kernel/drivers.c b/kernel/drivers.c
index 3d6ec10..572ab98 100644
--- a/kernel/drivers.c
+++ b/kernel/drivers.c
@@ -10,7 +10,7 @@
 
 void drivers_init(void)
 {
-	pit_set_divider(1193); // 1ms
+	pit_set_freq(CHAN_TIMER, 1000); // 1ms
 	uart_init();
 	ps2_init();
 	pci_init();
diff --git a/kernel/drivers/pit.c b/kernel/drivers/pit.c
index 53a9263..0c7a207 100644
--- a/kernel/drivers/pit.c
+++ b/kernel/drivers/pit.c
@@ -1,29 +1,65 @@
+#include "lib/klib.h"
 #include <comus/asm.h>
 #include <comus/drivers/pit.h>
+#include <comus/drivers/spkr.h>
+#include <lib.h>
 
-#define CHAN_0 0x40
-#define CHAN_1 0x41
-#define CHAN_2 0x42
 #define CMD 0x43
+#define SPKR 0x61
+
+#define BASE 1193180
 
 volatile uint64_t ticks = 0;
 
-uint16_t pit_read_divider(void)
+uint32_t pit_read_freq(uint8_t chan)
 {
-	uint16_t count = 0;
+	uint16_t div = 0;
 	cli();
 	outb(CMD, 0); // clear bits
-	count = inb(CHAN_0); // low byte
-	count |= inb(CHAN_0) << 8; // highbyte
+	div = inb(chan); // low byte
+	div |= inb(chan) << 8; // highbyte
 	sti();
-	return count;
+	return div * BASE;
 }
 
-void pit_set_divider(uint16_t count)
+void pit_set_freq(uint8_t chan, uint32_t hz)
 {
-	(void)count;
+	uint16_t div = BASE / hz;
 	cli();
-	outb(CHAN_0, count & 0xFF); // low byte
-	outb(CHAN_0, (count & 0xFF00) >> 8); // high byte
+	outb(CMD, 0xb6);
+	outb(chan, div & 0xFF); // low byte
+	outb(chan, (div & 0xFF00) >> 8); // high byte
 	sti();
 }
+
+void spkr_play_tone(uint32_t hz)
+{
+	uint8_t reg;
+
+	if (hz == 0) {
+		spkr_quiet();
+		return;
+	}
+
+	// set spkr freq
+	pit_set_freq(CHAN_SPKR, hz);
+
+	// enable spkr gate
+	reg = inb(SPKR);
+	if (reg != (reg | 0x3))
+		outb(SPKR, reg | 0x3);
+}
+
+void spkr_quiet(void)
+{
+	uint8_t reg;
+	reg = inb(SPKR) & 0xFC;
+	outb(SPKR, reg);
+}
+
+void spkr_beep(void)
+{
+	spkr_play_tone(1000);
+	kspin_milliseconds(100);
+	spkr_quiet();
+}
diff --git a/kernel/drivers/ps2.c b/kernel/drivers/ps2.c
index e260f6b..f605c1d 100644
--- a/kernel/drivers/ps2.c
+++ b/kernel/drivers/ps2.c
@@ -266,3 +266,18 @@ int ps2_init(void)
 	sti();
 	return 0;
 }
+
+int ps2_set_leds(uint8_t state)
+{
+	uint8_t result;
+
+	if (!(has_mouse || has_kbd))
+			return 1;
+
+	ps2ctrl_out_cmd(0xED);
+	ps2ctrl_out_data(state);
+	if ((result = ps2ctrl_in()) != 0xFA)
+		return 1;
+
+	return 0;
+}