Initialze controller and log status
This commit is contained in:
parent
374c8322a0
commit
befe1eded5
GPG Key ID:
CA239FC6934E6F3A
360
Cargo.lock
generated
Normal file
360
Cargo.lock
generated
Normal file
@ -0,0 +1,360 @@
|
||||
# This file is automatically @generated by Cargo.
|
||||
# It is not intended for manual editing.
|
||||
version = 3
|
||||
|
||||
[[package]]
|
||||
name = "atomic-polyfill"
|
||||
version = "1.0.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "8cf2bce30dfe09ef0bfaef228b9d414faaf7e563035494d7fe092dba54b300f4"
|
||||
dependencies = [
|
||||
"critical-section",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "autocfg"
|
||||
version = "1.3.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0c4b4d0bd25bd0b74681c0ad21497610ce1b7c91b1022cd21c80c6fbdd9476b0"
|
||||
|
||||
[[package]]
|
||||
name = "bit_field"
|
||||
version = "0.10.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "dc827186963e592360843fb5ba4b973e145841266c1357f7180c43526f2e5b61"
|
||||
|
||||
[[package]]
|
||||
name = "bitflags"
|
||||
version = "2.6.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b048fb63fd8b5923fc5aa7b340d8e156aec7ec02f0c78fa8a6ddc2613f6f71de"
|
||||
|
||||
[[package]]
|
||||
name = "byteorder"
|
||||
version = "1.5.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1fd0f2584146f6f2ef48085050886acf353beff7305ebd1ae69500e27c67f64b"
|
||||
|
||||
[[package]]
|
||||
name = "cfg-if"
|
||||
version = "1.0.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "baf1de4339761588bc0619e3cbc0120ee582ebb74b53b4efbf79117bd2da40fd"
|
||||
|
||||
[[package]]
|
||||
name = "cobs"
|
||||
version = "0.2.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "67ba02a97a2bd10f4b59b25c7973101c79642302776489e030cd13cdab09ed15"
|
||||
|
||||
[[package]]
|
||||
name = "critical-section"
|
||||
version = "1.1.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f64009896348fc5af4222e9cf7d7d82a95a256c634ebcf61c53e4ea461422242"
|
||||
|
||||
[[package]]
|
||||
name = "embedded-io"
|
||||
version = "0.4.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "ef1a6892d9eef45c8fa6b9e0086428a2cca8491aca8f787c534a3d6d0bcb3ced"
|
||||
|
||||
[[package]]
|
||||
name = "embedded-io"
|
||||
version = "0.6.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d"
|
||||
|
||||
[[package]]
|
||||
name = "hash32"
|
||||
version = "0.2.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b0c35f58762feb77d74ebe43bdbc3210f09be9fe6742234d573bacc26ed92b67"
|
||||
dependencies = [
|
||||
"byteorder",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "heapless"
|
||||
version = "0.7.17"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "cdc6457c0eb62c71aac4bc17216026d8410337c4126773b9c5daba343f17964f"
|
||||
dependencies = [
|
||||
"atomic-polyfill",
|
||||
"hash32",
|
||||
"rustc_version",
|
||||
"serde",
|
||||
"spin",
|
||||
"stable_deref_trait",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "libc"
|
||||
version = "0.2.158"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "d8adc4bb1803a324070e64a98ae98f38934d91957a99cfb3a43dcbc01bc56439"
|
||||
|
||||
[[package]]
|
||||
name = "lock_api"
|
||||
version = "0.4.12"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "07af8b9cdd281b7915f413fa73f29ebd5d55d0d3f0155584dade1ff18cea1b17"
|
||||
dependencies = [
|
||||
"autocfg",
|
||||
"scopeguard",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "parking_lot"
|
||||
version = "0.12.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "f1bf18183cf54e8d6059647fc3063646a1801cf30896933ec2311622cc4b9a27"
|
||||
dependencies = [
|
||||
"lock_api",
|
||||
"parking_lot_core",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "parking_lot_core"
|
||||
version = "0.9.10"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "1e401f977ab385c9e4e3ab30627d6f26d00e2c73eef317493c4ec6d468726cf8"
|
||||
dependencies = [
|
||||
"cfg-if",
|
||||
"libc",
|
||||
"redox_syscall",
|
||||
"smallvec",
|
||||
"windows-targets",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "postcard"
|
||||
version = "1.0.10"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "5f7f0a8d620d71c457dd1d47df76bb18960378da56af4527aaa10f515eee732e"
|
||||
dependencies = [
|
||||
"cobs",
|
||||
"embedded-io 0.4.0",
|
||||
"embedded-io 0.6.1",
|
||||
"heapless",
|
||||
"serde",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "proc-macro2"
|
||||
version = "1.0.86"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "5e719e8df665df0d1c8fbfd238015744736151d4445ec0836b8e628aae103b77"
|
||||
dependencies = [
|
||||
"unicode-ident",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "ps2"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"bitflags",
|
||||
"syslog_rpc",
|
||||
"x86_64",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "quote"
|
||||
version = "1.0.37"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "b5b9d34b8991d19d98081b46eacdd8eb58c6f2b201139f7c5f643cc155a633af"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "redox_syscall"
|
||||
version = "0.5.3"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "2a908a6e00f1fdd0dfd9c0eb08ce85126f6d8bbda50017e74bc4a4b7d4a926a4"
|
||||
dependencies = [
|
||||
"bitflags",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rustc_version"
|
||||
version = "0.4.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "cfcb3a22ef46e85b45de6ee7e79d063319ebb6594faafcf1c225ea92ab6e9b92"
|
||||
dependencies = [
|
||||
"semver",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "rustversion"
|
||||
version = "1.0.17"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "955d28af4278de8121b7ebeb796b6a45735dc01436d898801014aced2773a3d6"
|
||||
|
||||
[[package]]
|
||||
name = "scopeguard"
|
||||
version = "1.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "94143f37725109f92c262ed2cf5e59bce7498c01bcc1502d7b9afe439a4e9f49"
|
||||
|
||||
[[package]]
|
||||
name = "semver"
|
||||
version = "1.0.23"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "61697e0a1c7e512e84a621326239844a24d8207b4669b41bc18b32ea5cbf988b"
|
||||
|
||||
[[package]]
|
||||
name = "serde"
|
||||
version = "1.0.210"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "c8e3592472072e6e22e0a54d5904d9febf8508f65fb8552499a1abc7d1078c3a"
|
||||
dependencies = [
|
||||
"serde_derive",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "serde_derive"
|
||||
version = "1.0.210"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "243902eda00fad750862fc144cea25caca5e20d615af0a81bee94ca738f1df1f"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"syn",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "smallvec"
|
||||
version = "1.13.2"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "3c5e1a9a646d36c3599cd173a41282daf47c44583ad367b8e6837255952e5c67"
|
||||
|
||||
[[package]]
|
||||
name = "spin"
|
||||
version = "0.9.8"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "6980e8d7511241f8acf4aebddbb1ff938df5eebe98691418c4468d0b72a96a67"
|
||||
dependencies = [
|
||||
"lock_api",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "stable_deref_trait"
|
||||
version = "1.2.0"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "a8f112729512f8e442d81f95a8a7ddf2b7c6b8a1a6f509a95864142b30cab2d3"
|
||||
|
||||
[[package]]
|
||||
name = "syn"
|
||||
version = "2.0.77"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "9f35bcdf61fd8e7be6caf75f429fdca8beb3ed76584befb503b1569faee373ed"
|
||||
dependencies = [
|
||||
"proc-macro2",
|
||||
"quote",
|
||||
"unicode-ident",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "syslog_rpc"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"parking_lot",
|
||||
"postcard",
|
||||
"syslog_structs",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "syslog_structs"
|
||||
version = "0.1.0"
|
||||
dependencies = [
|
||||
"serde",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "unicode-ident"
|
||||
version = "1.0.12"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "3354b9ac3fae1ff6755cb6db53683adb661634f67557942dea4facebec0fee4b"
|
||||
|
||||
[[package]]
|
||||
name = "volatile"
|
||||
version = "0.4.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "442887c63f2c839b346c192d047a7c87e73d0689c9157b00b53dcc27dd5ea793"
|
||||
|
||||
[[package]]
|
||||
name = "windows-targets"
|
||||
version = "0.52.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "9b724f72796e036ab90c1021d4780d4d3d648aca59e491e6b98e725b84e99973"
|
||||
dependencies = [
|
||||
"windows_aarch64_gnullvm",
|
||||
"windows_aarch64_msvc",
|
||||
"windows_i686_gnu",
|
||||
"windows_i686_gnullvm",
|
||||
"windows_i686_msvc",
|
||||
"windows_x86_64_gnu",
|
||||
"windows_x86_64_gnullvm",
|
||||
"windows_x86_64_msvc",
|
||||
]
|
||||
|
||||
[[package]]
|
||||
name = "windows_aarch64_gnullvm"
|
||||
version = "0.52.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "32a4622180e7a0ec044bb555404c800bc9fd9ec262ec147edd5989ccd0c02cd3"
|
||||
|
||||
[[package]]
|
||||
name = "windows_aarch64_msvc"
|
||||
version = "0.52.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "09ec2a7bb152e2252b53fa7803150007879548bc709c039df7627cabbd05d469"
|
||||
|
||||
[[package]]
|
||||
name = "windows_i686_gnu"
|
||||
version = "0.52.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "8e9b5ad5ab802e97eb8e295ac6720e509ee4c243f69d781394014ebfe8bbfa0b"
|
||||
|
||||
[[package]]
|
||||
name = "windows_i686_gnullvm"
|
||||
version = "0.52.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "0eee52d38c090b3caa76c563b86c3a4bd71ef1a819287c19d586d7334ae8ed66"
|
||||
|
||||
[[package]]
|
||||
name = "windows_i686_msvc"
|
||||
version = "0.52.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "240948bc05c5e7c6dabba28bf89d89ffce3e303022809e73deaefe4f6ec56c66"
|
||||
|
||||
[[package]]
|
||||
name = "windows_x86_64_gnu"
|
||||
version = "0.52.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "147a5c80aabfbf0c7d901cb5895d1de30ef2907eb21fbbab29ca94c5b08b1a78"
|
||||
|
||||
[[package]]
|
||||
name = "windows_x86_64_gnullvm"
|
||||
version = "0.52.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "24d5b23dc417412679681396f2b49f3de8c1473deb516bd34410872eff51ed0d"
|
||||
|
||||
[[package]]
|
||||
name = "windows_x86_64_msvc"
|
||||
version = "0.52.6"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "589f6da84c646204747d1270a2a5661ea66ed1cced2631d546fdfb155959f9ec"
|
||||
|
||||
[[package]]
|
||||
name = "x86_64"
|
||||
version = "0.15.1"
|
||||
source = "registry+https://github.com/rust-lang/crates.io-index"
|
||||
checksum = "4bc79523af8abf92fb1a970c3e086c5a343f6bcc1a0eb890f575cbb3b45743df"
|
||||
dependencies = [
|
||||
"bit_field",
|
||||
"bitflags",
|
||||
"rustversion",
|
||||
"volatile",
|
||||
]
|
||||
@ -4,3 +4,6 @@ version = "0.1.0"
|
||||
edition = "2021"
|
||||
|
||||
[dependencies]
|
||||
bitflags = "2.6.0"
|
||||
syslog_rpc = { version = "0.1.0", path = "../syslog/syslog_rpc" }
|
||||
x86_64 = "0.15.1"
|
||||
|
||||
282
src/main.rs
282
src/main.rs
@ -1,3 +1,281 @@
|
||||
fn main() {
|
||||
println!("Hello, world!");
|
||||
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(())
|
||||
}
|
||||
}
|
||||
|
||||
fn main() {
|
||||
let syslog_pid = loop {
|
||||
if let Some(pid) = syscalls::try_get_registered(2) {
|
||||
break pid;
|
||||
}
|
||||
};
|
||||
let syslog_client = syslog_rpc::Client::new(syslog_pid);
|
||||
syslog_client
|
||||
.send_text_message("ps2", "Initializing controller")
|
||||
.unwrap();
|
||||
let mut controller = unsafe { Ps2Controller::new(0x60) };
|
||||
if controller.initialize().is_err() {
|
||||
syslog_client
|
||||
.send_text_message("ps2", "Controller initialization failed, aborting")
|
||||
.unwrap();
|
||||
return;
|
||||
}
|
||||
if controller.two_ports {
|
||||
syslog_client
|
||||
.send_text_message("ps2", "Controller has two ports")
|
||||
.unwrap();
|
||||
} else {
|
||||
syslog_client
|
||||
.send_text_message("ps2", "Controller has one port")
|
||||
.unwrap();
|
||||
}
|
||||
|
||||
if controller.port1_ok {
|
||||
syslog_client.send_text_message("ps2", "Port 1 OK").unwrap();
|
||||
} else {
|
||||
syslog_client
|
||||
.send_text_message("ps2", "Port 1 failed self test")
|
||||
.unwrap();
|
||||
}
|
||||
|
||||
if controller.two_ports {
|
||||
if controller.port2_ok {
|
||||
syslog_client.send_text_message("ps2", "Port 1 OK").unwrap();
|
||||
} else {
|
||||
syslog_client
|
||||
.send_text_message("ps2", "Port 1 failed self test")
|
||||
.unwrap();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user