Extract controller code into it's own file
This commit is contained in:
parent
befe1eded5
commit
32b70bc075
GPG Key ID:
CA239FC6934E6F3A
245
src/controller.rs
Normal file
245
src/controller.rs
Normal file
@ -0,0 +1,245 @@
|
||||
use bitflags::bitflags;
|
||||
use x86_64::instructions::port::{Port, PortReadOnly, PortWriteOnly};
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
enum Ps2ControllerCommand {
|
||||
ReadRAM(u8),
|
||||
WriteRAM(u8, u8),
|
||||
DisablePort2,
|
||||
EnablePort2,
|
||||
TestPort2,
|
||||
TestController,
|
||||
TestPort1,
|
||||
DiagDump,
|
||||
DisablePort1,
|
||||
EnablePort1,
|
||||
ReadControllerInput,
|
||||
CopyInputLowStatus,
|
||||
CopyInputHighStatus,
|
||||
ReadControllerOutput,
|
||||
WriteControllerOutput(u8),
|
||||
WritePort1Output(u8),
|
||||
WritePort2Output(u8),
|
||||
WritePort2Input(u8),
|
||||
PulseOutput(u8),
|
||||
WritePort1Input(u8),
|
||||
}
|
||||
|
||||
impl Ps2ControllerCommand {
|
||||
fn get_command_byte(self) -> Option<u8> {
|
||||
match self {
|
||||
Self::ReadRAM(addr) => Some(0x20 + (addr & 0x1F)),
|
||||
Self::WriteRAM(addr, _) => Some(0x60 + (addr & 0x1F)),
|
||||
Self::DisablePort2 => Some(0xA7),
|
||||
Self::EnablePort2 => Some(0xA8),
|
||||
Self::TestPort2 => Some(0xA9),
|
||||
Self::TestController => Some(0xAA),
|
||||
Self::TestPort1 => Some(0xAB),
|
||||
Self::DiagDump => Some(0xAC),
|
||||
Self::DisablePort1 => Some(0xAD),
|
||||
Self::EnablePort1 => Some(0xAE),
|
||||
Self::ReadControllerInput => Some(0xC0),
|
||||
Self::CopyInputLowStatus => Some(0xC1),
|
||||
Self::CopyInputHighStatus => Some(0xC2),
|
||||
Self::ReadControllerOutput => Some(0xD0),
|
||||
Self::WriteControllerOutput(_) => Some(0xD1),
|
||||
Self::WritePort1Output(_) => Some(0xD2),
|
||||
Self::WritePort2Output(_) => Some(0xD3),
|
||||
Self::WritePort2Input(_) => Some(0xD4),
|
||||
Self::PulseOutput(mask) => Some(0xF0 + (mask & 0xF)),
|
||||
Self::WritePort1Input(_) => None,
|
||||
}
|
||||
}
|
||||
|
||||
fn get_data_byte(self) -> Option<u8> {
|
||||
match self {
|
||||
Self::WriteRAM(_, data) => Some(data),
|
||||
Self::WriteControllerOutput(data) => Some(data),
|
||||
Self::WritePort1Output(data) => Some(data),
|
||||
Self::WritePort2Output(data) => Some(data),
|
||||
Self::WritePort2Input(data) => Some(data),
|
||||
Self::WritePort1Input(data) => Some(data),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
fn has_data_response(self) -> bool {
|
||||
matches!(
|
||||
self,
|
||||
Self::ReadRAM(_)
|
||||
| Self::TestPort2
|
||||
| Self::TestController
|
||||
| Self::TestPort1
|
||||
| Self::ReadControllerInput
|
||||
| Self::ReadControllerOutput
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
bitflags! {
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub struct Ps2Status: u8 {
|
||||
const OutputBufFull = 0b0000_0001;
|
||||
const InputBufFull = 0b0000_0010;
|
||||
const SystemFlag = 0b0000_0100;
|
||||
const CommandData = 0b0000_1000;
|
||||
const Bit4 = 0b0001_0000;
|
||||
const Bit5 = 0b0010_0000;
|
||||
const TimeoutError = 0b0100_0000;
|
||||
const ParityError = 0b1000_0000;
|
||||
}
|
||||
}
|
||||
|
||||
bitflags! {
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub struct Ps2OutputPort: u8 {
|
||||
const SystemReset = 0b0000_0001;
|
||||
const A20Gate = 0b0000_0010;
|
||||
const Port2Clock = 0b0000_0100;
|
||||
const Port2Data = 0b0000_1000;
|
||||
const OutputBufFullPort1 = 0b0001_0000;
|
||||
const OutputBufFullPort2 = 0b0010_0000;
|
||||
const Port1Clock = 0b0100_0000;
|
||||
const Port12Data = 0b1000_0000;
|
||||
}
|
||||
}
|
||||
|
||||
bitflags! {
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub struct Ps2ConfigByte: u8 {
|
||||
const Port1Interrupt = 0b0000_0001;
|
||||
const Port2Interrupt = 0b0000_0010;
|
||||
const SystemFlag = 0b0000_0100;
|
||||
const ShouldZero = 0b0000_1000;
|
||||
const Port1ClockDisable = 0b0001_0000;
|
||||
const Port2ClockDisable = 0b0010_0000;
|
||||
const Port1Translation = 0b0100_0000;
|
||||
const MustZero = 0b1000_0000;
|
||||
}
|
||||
}
|
||||
|
||||
pub struct Ps2Controller {
|
||||
data_port: Port<u8>,
|
||||
status_register: PortReadOnly<u8>,
|
||||
command_register: PortWriteOnly<u8>,
|
||||
two_ports: bool,
|
||||
port1_ok: bool,
|
||||
port2_ok: bool,
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub enum Ps2InitFailure {
|
||||
SelfTestFailed,
|
||||
}
|
||||
|
||||
impl Ps2Controller {
|
||||
pub unsafe fn new(base: u16) -> Self {
|
||||
Self {
|
||||
data_port: Port::new(base),
|
||||
status_register: PortReadOnly::new(base + 0x4),
|
||||
command_register: PortWriteOnly::new(base + 0x4),
|
||||
two_ports: false, // This is determined during initialiation
|
||||
port1_ok: false, // This is determined during initialiation
|
||||
port2_ok: false, // This is determined during initialiation
|
||||
}
|
||||
}
|
||||
|
||||
fn get_status(&mut self) -> Ps2Status {
|
||||
Ps2Status::from_bits(unsafe { self.status_register.read() }).unwrap()
|
||||
}
|
||||
|
||||
fn send_command(&mut self, command: Ps2ControllerCommand) -> Option<u8> {
|
||||
if let Some(command_byte) = command.get_command_byte() {
|
||||
while self.get_status().contains(Ps2Status::InputBufFull) {}
|
||||
unsafe { self.command_register.write(command_byte) }
|
||||
}
|
||||
if let Some(data_byte) = command.get_data_byte() {
|
||||
while self.get_status().contains(Ps2Status::InputBufFull) {}
|
||||
unsafe { self.data_port.write(data_byte) }
|
||||
}
|
||||
if command.has_data_response() {
|
||||
while !self.get_status().contains(Ps2Status::OutputBufFull) {}
|
||||
Some(unsafe { self.data_port.read() })
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
fn get_config_byte(&mut self) -> Ps2ConfigByte {
|
||||
let config_byte = self.send_command(Ps2ControllerCommand::ReadRAM(0)).unwrap();
|
||||
Ps2ConfigByte::from_bits(config_byte).unwrap()
|
||||
}
|
||||
|
||||
fn set_config_byte(&mut self, config_byte: Ps2ConfigByte) {
|
||||
self.send_command(Ps2ControllerCommand::WriteRAM(0, config_byte.bits()));
|
||||
}
|
||||
|
||||
pub fn initialize(&mut self) -> Result<(), Ps2InitFailure> {
|
||||
self.send_command(Ps2ControllerCommand::DisablePort1);
|
||||
self.send_command(Ps2ControllerCommand::DisablePort2);
|
||||
|
||||
// Flush the output buffer
|
||||
unsafe { self.data_port.read() };
|
||||
|
||||
let mut config_byte = self.get_config_byte();
|
||||
config_byte.set(Ps2ConfigByte::Port1Interrupt, false);
|
||||
config_byte.set(Ps2ConfigByte::Port1ClockDisable, false);
|
||||
config_byte.set(Ps2ConfigByte::Port1Translation, false);
|
||||
self.set_config_byte(config_byte);
|
||||
|
||||
let self_test_result = self.send_command(Ps2ControllerCommand::TestController).unwrap();
|
||||
if self_test_result != 0x55 {
|
||||
return Err(Ps2InitFailure::SelfTestFailed);
|
||||
}
|
||||
|
||||
// Testing the controller can reset it, so redo early initialization to be safe.
|
||||
self.send_command(Ps2ControllerCommand::DisablePort1);
|
||||
self.send_command(Ps2ControllerCommand::DisablePort2);
|
||||
|
||||
// Flush the output buffer
|
||||
unsafe { self.data_port.read() };
|
||||
|
||||
self.set_config_byte(config_byte);
|
||||
|
||||
self.send_command(Ps2ControllerCommand::EnablePort2);
|
||||
if !self
|
||||
.get_config_byte()
|
||||
.contains(Ps2ConfigByte::Port2ClockDisable)
|
||||
{
|
||||
self.two_ports = true;
|
||||
self.send_command(Ps2ControllerCommand::DisablePort2);
|
||||
let mut config_byte = self.get_config_byte();
|
||||
config_byte.set(Ps2ConfigByte::Port2Interrupt, false);
|
||||
config_byte.set(Ps2ConfigByte::Port2ClockDisable, false);
|
||||
self.set_config_byte(config_byte);
|
||||
}
|
||||
|
||||
let port1_test_result = self.send_command(Ps2ControllerCommand::TestPort1).unwrap();
|
||||
if port1_test_result == 0x0 {
|
||||
self.port1_ok = true;
|
||||
self.send_command(Ps2ControllerCommand::EnablePort1);
|
||||
}
|
||||
|
||||
if self.two_ports {
|
||||
let port2_test_result = self.send_command(Ps2ControllerCommand::TestPort2).unwrap();
|
||||
if port2_test_result == 0x0 {
|
||||
self.port2_ok = true;
|
||||
self.send_command(Ps2ControllerCommand::EnablePort2);
|
||||
}
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub fn two_ports(&self) -> bool {
|
||||
self.two_ports
|
||||
}
|
||||
|
||||
pub fn port1_ok(&self) -> bool {
|
||||
self.port1_ok
|
||||
}
|
||||
|
||||
pub fn port2_ok(&self) -> bool {
|
||||
self.port2_ok
|
||||
}
|
||||
}
|
||||
248
src/main.rs
248
src/main.rs
@ -1,238 +1,8 @@
|
||||
mod controller;
|
||||
|
||||
use std::os::mikros::syscalls;
|
||||
|
||||
use bitflags::{bitflags, Flags};
|
||||
use x86_64::instructions::port::{Port, PortReadOnly, PortWriteOnly};
|
||||
|
||||
#[derive(Copy, Clone, Debug)]
|
||||
enum Ps2Command {
|
||||
ReadRAM(u8),
|
||||
WriteRAM(u8, u8),
|
||||
DisablePort2,
|
||||
EnablePort2,
|
||||
TestPort2,
|
||||
TestController,
|
||||
TestPort1,
|
||||
DiagDump,
|
||||
DisablePort1,
|
||||
EnablePort1,
|
||||
ReadControllerInput,
|
||||
CopyInputLowStatus,
|
||||
CopyInputHighStatus,
|
||||
ReadControllerOutput,
|
||||
WriteControllerOutput(u8),
|
||||
WritePort1Output(u8),
|
||||
WritePort2Output(u8),
|
||||
WritePort2Input(u8),
|
||||
PulseOutput(u8),
|
||||
WritePort1Input(u8),
|
||||
}
|
||||
|
||||
impl Ps2Command {
|
||||
fn get_command_byte(self) -> Option<u8> {
|
||||
match self {
|
||||
Self::ReadRAM(addr) => Some(0x20 + (addr & 0x1F)),
|
||||
Self::WriteRAM(addr, _) => Some(0x60 + (addr & 0x1F)),
|
||||
Self::DisablePort2 => Some(0xA7),
|
||||
Self::EnablePort2 => Some(0xA8),
|
||||
Self::TestPort2 => Some(0xA9),
|
||||
Self::TestController => Some(0xAA),
|
||||
Self::TestPort1 => Some(0xAB),
|
||||
Self::DiagDump => Some(0xAC),
|
||||
Self::DisablePort1 => Some(0xAD),
|
||||
Self::EnablePort1 => Some(0xAE),
|
||||
Self::ReadControllerInput => Some(0xC0),
|
||||
Self::CopyInputLowStatus => Some(0xC1),
|
||||
Self::CopyInputHighStatus => Some(0xC2),
|
||||
Self::ReadControllerOutput => Some(0xD0),
|
||||
Self::WriteControllerOutput(_) => Some(0xD1),
|
||||
Self::WritePort1Output(_) => Some(0xD2),
|
||||
Self::WritePort2Output(_) => Some(0xD3),
|
||||
Self::WritePort2Input(_) => Some(0xD4),
|
||||
Self::PulseOutput(mask) => Some(0xF0 + (mask & 0xF)),
|
||||
Self::WritePort1Input(_) => None,
|
||||
}
|
||||
}
|
||||
|
||||
fn get_data_byte(self) -> Option<u8> {
|
||||
match self {
|
||||
Self::WriteRAM(_, data) => Some(data),
|
||||
Self::WriteControllerOutput(data) => Some(data),
|
||||
Self::WritePort1Output(data) => Some(data),
|
||||
Self::WritePort2Output(data) => Some(data),
|
||||
Self::WritePort2Input(data) => Some(data),
|
||||
Self::WritePort1Input(data) => Some(data),
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
fn has_data_response(self) -> bool {
|
||||
matches!(
|
||||
self,
|
||||
Self::ReadRAM(_)
|
||||
| Self::TestPort2
|
||||
| Self::TestController
|
||||
| Self::TestPort1
|
||||
| Self::ReadControllerInput
|
||||
| Self::ReadControllerOutput
|
||||
)
|
||||
}
|
||||
}
|
||||
|
||||
bitflags! {
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub struct Ps2Status: u8 {
|
||||
const OutputBufFull = 0b0000_0001;
|
||||
const InputBufFull = 0b0000_0010;
|
||||
const SystemFlag = 0b0000_0100;
|
||||
const CommandData = 0b0000_1000;
|
||||
const Bit4 = 0b0001_0000;
|
||||
const Bit5 = 0b0010_0000;
|
||||
const TimeoutError = 0b0100_0000;
|
||||
const ParityError = 0b1000_0000;
|
||||
}
|
||||
}
|
||||
|
||||
bitflags! {
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub struct Ps2OutputPort: u8 {
|
||||
const SystemReset = 0b0000_0001;
|
||||
const A20Gate = 0b0000_0010;
|
||||
const Port2Clock = 0b0000_0100;
|
||||
const Port2Data = 0b0000_1000;
|
||||
const OutputBufFullPort1 = 0b0001_0000;
|
||||
const OutputBufFullPort2 = 0b0010_0000;
|
||||
const Port1Clock = 0b0100_0000;
|
||||
const Port12Data = 0b1000_0000;
|
||||
}
|
||||
}
|
||||
|
||||
bitflags! {
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
pub struct Ps2ConfigByte: u8 {
|
||||
const Port1Interrupt = 0b0000_0001;
|
||||
const Port2Interrupt = 0b0000_0010;
|
||||
const SystemFlag = 0b0000_0100;
|
||||
const ShouldZero = 0b0000_1000;
|
||||
const Port1ClockDisable = 0b0001_0000;
|
||||
const Port2ClockDisable = 0b0010_0000;
|
||||
const Port1Translation = 0b0100_0000;
|
||||
const MustZero = 0b1000_0000;
|
||||
}
|
||||
}
|
||||
|
||||
struct Ps2Controller {
|
||||
data_port: Port<u8>,
|
||||
status_register: PortReadOnly<u8>,
|
||||
command_register: PortWriteOnly<u8>,
|
||||
two_ports: bool,
|
||||
port1_ok: bool,
|
||||
port2_ok: bool,
|
||||
}
|
||||
|
||||
#[derive(Clone, Copy, Debug)]
|
||||
enum Ps2InitFailure {
|
||||
SelfTestFailed,
|
||||
}
|
||||
|
||||
impl Ps2Controller {
|
||||
pub unsafe fn new(base: u16) -> Self {
|
||||
Self {
|
||||
data_port: Port::new(base),
|
||||
status_register: PortReadOnly::new(base + 0x4),
|
||||
command_register: PortWriteOnly::new(base + 0x4),
|
||||
two_ports: false, // This is determined during initialiation
|
||||
port1_ok: false, // This is determined during initialiation
|
||||
port2_ok: false, // This is determined during initialiation
|
||||
}
|
||||
}
|
||||
|
||||
fn get_status(&mut self) -> Ps2Status {
|
||||
Ps2Status::from_bits(unsafe { self.status_register.read() }).unwrap()
|
||||
}
|
||||
|
||||
fn send_command(&mut self, command: Ps2Command) -> Option<u8> {
|
||||
if let Some(command_byte) = command.get_command_byte() {
|
||||
while self.get_status().contains(Ps2Status::InputBufFull) {}
|
||||
unsafe { self.command_register.write(command_byte) }
|
||||
}
|
||||
if let Some(data_byte) = command.get_data_byte() {
|
||||
while self.get_status().contains(Ps2Status::InputBufFull) {}
|
||||
unsafe { self.data_port.write(data_byte) }
|
||||
}
|
||||
if command.has_data_response() {
|
||||
while !self.get_status().contains(Ps2Status::OutputBufFull) {}
|
||||
Some(unsafe { self.data_port.read() })
|
||||
} else {
|
||||
None
|
||||
}
|
||||
}
|
||||
|
||||
fn get_config_byte(&mut self) -> Ps2ConfigByte {
|
||||
let config_byte = self.send_command(Ps2Command::ReadRAM(0)).unwrap();
|
||||
Ps2ConfigByte::from_bits(config_byte).unwrap()
|
||||
}
|
||||
|
||||
fn set_config_byte(&mut self, config_byte: Ps2ConfigByte) {
|
||||
self.send_command(Ps2Command::WriteRAM(0, config_byte.bits()));
|
||||
}
|
||||
|
||||
fn initialize(&mut self) -> Result<(), Ps2InitFailure> {
|
||||
self.send_command(Ps2Command::DisablePort1);
|
||||
self.send_command(Ps2Command::DisablePort2);
|
||||
|
||||
// Flush the output buffer
|
||||
unsafe { self.data_port.read() };
|
||||
|
||||
let mut config_byte = self.get_config_byte();
|
||||
config_byte.set(Ps2ConfigByte::Port1Interrupt, false);
|
||||
config_byte.set(Ps2ConfigByte::Port1ClockDisable, false);
|
||||
config_byte.set(Ps2ConfigByte::Port1Translation, false);
|
||||
self.set_config_byte(config_byte);
|
||||
|
||||
let self_test_result = self.send_command(Ps2Command::TestController).unwrap();
|
||||
if self_test_result != 0x55 {
|
||||
return Err(Ps2InitFailure::SelfTestFailed);
|
||||
}
|
||||
|
||||
// Testing the controller can reset it, so redo early initialization to be safe.
|
||||
self.send_command(Ps2Command::DisablePort1);
|
||||
self.send_command(Ps2Command::DisablePort2);
|
||||
|
||||
// Flush the output buffer
|
||||
unsafe { self.data_port.read() };
|
||||
|
||||
self.set_config_byte(config_byte);
|
||||
|
||||
self.send_command(Ps2Command::EnablePort2);
|
||||
if !self
|
||||
.get_config_byte()
|
||||
.contains(Ps2ConfigByte::Port2ClockDisable)
|
||||
{
|
||||
self.two_ports = true;
|
||||
self.send_command(Ps2Command::DisablePort2);
|
||||
let mut config_byte = self.get_config_byte();
|
||||
config_byte.set(Ps2ConfigByte::Port2Interrupt, false);
|
||||
config_byte.set(Ps2ConfigByte::Port2ClockDisable, false);
|
||||
self.set_config_byte(config_byte);
|
||||
}
|
||||
|
||||
let port1_test_result = self.send_command(Ps2Command::TestPort1).unwrap();
|
||||
if port1_test_result == 0x0 {
|
||||
self.port1_ok = true;
|
||||
self.send_command(Ps2Command::EnablePort1);
|
||||
}
|
||||
|
||||
if self.two_ports {
|
||||
let port2_test_result = self.send_command(Ps2Command::TestPort2).unwrap();
|
||||
if port2_test_result == 0x0 {
|
||||
self.port2_ok = true;
|
||||
self.send_command(Ps2Command::EnablePort2);
|
||||
}
|
||||
}
|
||||
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
use controller::Ps2Controller;
|
||||
|
||||
fn main() {
|
||||
let syslog_pid = loop {
|
||||
@ -251,7 +21,7 @@ fn main() {
|
||||
.unwrap();
|
||||
return;
|
||||
}
|
||||
if controller.two_ports {
|
||||
if controller.two_ports() {
|
||||
syslog_client
|
||||
.send_text_message("ps2", "Controller has two ports")
|
||||
.unwrap();
|
||||
@ -261,7 +31,7 @@ fn main() {
|
||||
.unwrap();
|
||||
}
|
||||
|
||||
if controller.port1_ok {
|
||||
if controller.port1_ok() {
|
||||
syslog_client.send_text_message("ps2", "Port 1 OK").unwrap();
|
||||
} else {
|
||||
syslog_client
|
||||
@ -269,12 +39,12 @@ fn main() {
|
||||
.unwrap();
|
||||
}
|
||||
|
||||
if controller.two_ports {
|
||||
if controller.port2_ok {
|
||||
syslog_client.send_text_message("ps2", "Port 1 OK").unwrap();
|
||||
if controller.two_ports() {
|
||||
if controller.port2_ok() {
|
||||
syslog_client.send_text_message("ps2", "Port 2 OK").unwrap();
|
||||
} else {
|
||||
syslog_client
|
||||
.send_text_message("ps2", "Port 1 failed self test")
|
||||
.send_text_message("ps2", "Port 2 failed self test")
|
||||
.unwrap();
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user