Add pcspkr driver

Cargo.lock

@@ -504,6 +504,13 @@ dependencies = [
  "toml 0.4.10 (registry+https://github.com/rust-lang/crates.io-index)",
 ]
 
+[[package]]
+name = "pcspkrd"
+version = "0.1.0"
+dependencies = [
+ "redox_syscall 0.1.51 (registry+https://github.com/rust-lang/crates.io-index)",
+]
+
 [[package]]
 name = "percent-encoding"
 version = "1.0.1"

Cargo.toml

@@ -7,6 +7,7 @@ members = [
     "ihdad",
     "nvmed",
     "pcid",
+    "pcspkrd",
     "ps2d",
     "rtl8168d",
     "vboxd",

pcspkrd/Cargo.toml

+[package]
+name = "pcspkrd"
+version = "0.1.0"
+authors = ["Tibor Nagy <xnagytibor@gmail.com>"]
+edition = "2018"
+
+[dependencies]
+redox_syscall = "0.1"

pcspkrd/src/main.rs

+mod pcspkr;
+mod scheme;
+
+use std::fs::File;
+use std::io::{Read, Write};
+use syscall::data::Packet;
+use syscall::iopl;
+use syscall::scheme::SchemeMut;
+
+use self::pcspkr::Pcspkr;
+use self::scheme::PcspkrScheme;
+
+fn main() {
+    // Daemonize
+    if unsafe { syscall::clone(0).unwrap() } == 0 {
+        unsafe { iopl(3).unwrap() };
+
+        let mut socket = File::create(":pcspkr").expect("pcspkrd: failed to create pcspkr scheme");
+
+        let pcspkr = Pcspkr::new();
+        println!(" + pcspkr");
+
+        let mut scheme = PcspkrScheme {
+            pcspkr: pcspkr,
+            handle: None,
+            next_id: 0,
+        };
+
+        syscall::setrens(0, 0).expect("pcspkrd: failed to enter null namespace");
+
+        loop {
+            let mut packet = Packet::default();
+            socket
+                .read(&mut packet)
+                .expect("pcspkrd: failed to read events from pcspkr scheme");
+            scheme.handle(&mut packet);
+            socket
+                .write(&packet)
+                .expect("pcspkrd: failed to write responses to pcspkr scheme");
+        }
+    }
+}

pcspkrd/src/pcspkr.rs

+use syscall::io::{Io, Pio};
+
+pub struct Pcspkr {
+    command: Pio<u8>,
+    channel: Pio<u8>,
+    gate: Pio<u8>,
+}
+
+const PIT_FREQUENCY: usize = 0x1234DC;
+
+impl Pcspkr {
+    pub fn new() -> Pcspkr {
+        Pcspkr {
+            command: Pio::new(0x43),
+            channel: Pio::new(0x42),
+            gate: Pio::new(0x61),
+        }
+    }
+
+    pub fn set_frequency(&mut self, frequency: usize) {
+        let div = PIT_FREQUENCY.checked_div(frequency).unwrap_or(0);
+        self.command.write(0xB6);
+        self.channel.write((div & 0xFF) as u8);
+        self.channel.write(((div >> 8) & 0xFF) as u8);
+    }
+
+    pub fn set_gate(&mut self, state: bool) {
+        let gate_value = self.gate.read();
+        if state {
+            self.gate.write(gate_value | 0x03);
+        } else {
+            self.gate.write(gate_value & 0xFC);
+        }
+    }
+}

pcspkrd/src/scheme.rs

+use syscall::data::Stat;
+use syscall::{
+    Error, Result, SchemeMut, EBUSY, EINVAL, EPERM, MODE_CHR, O_ACCMODE, O_STAT, O_WRONLY,
+};
+
+use crate::pcspkr::Pcspkr;
+
+pub struct PcspkrScheme {
+    pub pcspkr: Pcspkr,
+    pub handle: Option<usize>,
+    pub next_id: usize,
+}
+
+impl SchemeMut for PcspkrScheme {
+    fn open(&mut self, _path: &[u8], flags: usize, _uid: u32, _gid: u32) -> Result<usize> {
+        if (flags & O_ACCMODE == 0) && (flags & O_STAT == O_STAT) {
+            Ok(0)
+        } else if flags & O_ACCMODE == O_WRONLY {
+            if self.handle.is_none() {
+                self.next_id += 1;
+                self.handle = Some(self.next_id);
+                Ok(self.next_id)
+            } else {
+                Err(Error::new(EBUSY))
+            }
+        } else {
+            Err(Error::new(EINVAL))
+        }
+    }
+
+    fn dup(&mut self, _id: usize, _buf: &[u8]) -> Result<usize> {
+        Err(Error::new(EPERM))
+    }
+
+    fn read(&mut self, _id: usize, _buf: &mut [u8]) -> Result<usize> {
+        Err(Error::new(EPERM))
+    }
+
+    fn write(&mut self, id: usize, buf: &[u8]) -> Result<usize> {
+        if self.handle != Some(id) {
+            return Err(Error::new(EINVAL));
+        }
+
+        if buf.len() != 2 {
+            return Err(Error::new(EINVAL));
+        }
+
+        let frequency = buf[0] as usize + ((buf[1] as usize) << 8);
+
+        if frequency == 0 {
+            self.pcspkr.set_gate(false);
+        } else {
+            self.pcspkr.set_frequency(frequency);
+            self.pcspkr.set_gate(true);
+        }
+
+        Ok(buf.len())
+    }
+
+    fn fpath(&mut self, _id: usize, buf: &mut [u8]) -> Result<usize> {
+        let mut i = 0;
+        let scheme_path = b"pcspkr";
+        while i < buf.len() && i < scheme_path.len() {
+            buf[i] = scheme_path[i];
+            i += 1;
+        }
+        Ok(i)
+    }
+
+    fn fstat(&mut self, _id: usize, stat: &mut Stat) -> Result<usize> {
+        *stat = Stat {
+            st_mode: MODE_CHR | 0o222,
+            ..Default::default()
+        };
+
+        Ok(0)
+    }
+
+    fn fcntl(&mut self, _id: usize, _cmd: usize, _arg: usize) -> Result<usize> {
+        Ok(0)
+    }
+
+    fn close(&mut self, id: usize) -> Result<usize> {
+        if self.handle == Some(id) {
+            self.pcspkr.set_gate(false);
+            self.handle = None;
+        }
+
+        Ok(0)
+    }
+}