#include "pic.h"
#include <sys/control.h>
#include <sys/io.h>

#define PIC1 0x20
#define PIC2 0xa0
#define DATA 1

/* initialization */
#define ICW1_INIT 0x10
/* TODO */
#define ICW1_ICW4 0x01
/* 8086/88 mode */
#define ICW4_8086 0x01

void
pic_init()
{
  outb(ICW1_INIT | ICW1_ICW4, PIC1);
  outb(ICW1_INIT | ICW1_ICW4, PIC2);

  outb(0x20, PIC1 + DATA); /* offset 0x20 */
  outb(0x28, PIC2 + DATA); /* offset 0x28 */

  outb(0x04, PIC1 + DATA); /* tell master pic there is a slave pic */
  outb(0x02, PIC2 + DATA); /* tell slave pic its cascade identity */

  outb(ICW4_8086, PIC1 + DATA);
  outb(ICW4_8086, PIC2 + DATA);

  /* PIC masks */
  outb(0xff, PIC1 + DATA);
  outb(0xff, PIC2 + DATA);
}

static unsigned char
irq_mask(unsigned char irq)
{
  unsigned char mask = (unsigned char)~(1u << irq);
  return 0xff & mask;
}

void
pic_enable()
{
  unsigned char mask1 = 0xff;
  unsigned char mask2 = 0xff;

  mask1 &= irq_mask(0); /* irq0 timer */
  mask1 &= irq_mask(1); /* irq1 keyboard */
  mask1 &= irq_mask(2); /* irq2 cascade */
  outb(mask1, PIC1 + DATA);

  mask2 &= irq_mask(12 - 8); /* irq12 mouse */
  outb(mask2, PIC2 + DATA);

  enable_interrupts();
}

void
pic_clear(unsigned char irq)
{
  outb(0x20, PIC1);
  if (irq >= 8) outb(0x20, PIC2);
}