70 lines
1.4 KiB
C
70 lines
1.4 KiB
C
|
#include <stdint.h>
|
||
|
|
#include <drivers/ps2/ps2.h>
|
||
|
|
|
||
|
|
extern void ps2_wait_input_empty(void);
|
||
|
|
extern void ps2_wait_output_full(void);
|
||
|
|
extern void ps2_write_command(uint8_t);
|
||
|
|
extern uint8_t ps2_read_data(void);
|
||
|
|
extern void ps2_write_data(uint8_t);
|
||
|
|
extern void ps2_flush_output_buffer(void);
|
||
|
|
|
||
|
|
extern void io_wait(void);
|
||
|
|
|
||
|
|
static uint8_t read_ccb() {
|
||
|
|
ps2_write_command(CMD_READ_CCB);
|
||
|
|
io_wait();
|
||
|
|
return ps2_read_data();
|
||
|
|
}
|
||
|
|
|
||
|
|
static void write_ccb(uint8_t ccb) {
|
||
|
|
ps2_write_command(CMD_WRITE_CCB);
|
||
|
|
io_wait();
|
||
|
|
ps2_write_data(ccb);
|
||
|
|
io_wait();
|
||
|
|
}
|
||
|
|
|
||
|
|
static int perform_self_test() {
|
||
|
|
ps2_write_command(CMD_SELF_TEST);
|
||
|
|
io_wait();
|
||
|
|
return ps2_read_data();
|
||
|
|
}
|
||
|
|
|
||
|
|
static int test_port_1() {
|
||
|
|
ps2_write_command(CMD_TEST_P1);
|
||
|
|
io_wait();
|
||
|
|
return ps2_read_data();
|
||
|
|
}
|
||
|
|
|
||
|
|
static void enable_port_1() {
|
||
|
|
ps2_write_command(CMD_ENABLE_P1);
|
||
|
|
io_wait();
|
||
|
|
}
|
||
|
|
|
||
|
|
int init_ps2() {
|
||
|
|
ps2_write_command(CMD_DISABLE_P1);
|
||
|
|
ps2_write_command(CMD_DISABLE_P2);
|
||
|
|
|
||
|
|
ps2_flush_output_buffer();
|
||
|
|
|
||
|
|
uint8_t ccb = read_ccb();
|
||
|
|
ccb = ccb & 0xEC;
|
||
|
|
write_ccb(ccb);
|
||
|
|
|
||
|
|
int result = perform_self_test();
|
||
|
|
if (result != SELF_TEST_SUCCESS) {
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
result = test_port_1();
|
||
|
|
if (result != PORT_TEST_SUCCESS) {
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
enable_port_1();
|
||
|
|
|
||
|
|
ccb = read_ccb();
|
||
|
|
ccb |= 0x01; // this enables interrupts for port 1, which we want
|
||
|
|
write_ccb(ccb);
|
||
|
|
|
||
|
|
return 0;
|
||
|
|
}
|