#include "check_pin.h"

#define BIT(n)   (1u << (n))

static void write_port(volatile uint8_t *port, uint8_t mask, uint8_t on)
{
    if (on) *port |=  mask;
    else    *port &= (uint8_t)~mask;
}

static void set_output_pm(volatile uint8_t *pm, uint8_t mask)
{
    *pm &= (uint8_t)~mask;
}

void check_pin_init(void)
{
    // CHECK_1,2,4 : P15.4,5,6
    set_output_pm(&PM15, (uint8_t)(BIT(4)|BIT(5)|BIT(6)));

    // CHECK_3,7 : P0.0,3
    set_output_pm(&PM0, (uint8_t)(BIT(0)|BIT(3)));

    // CHECK_5,6 : P3.0,2
    set_output_pm(&PM3, (uint8_t)(BIT(0)|BIT(2)));

    // CHECK_8,9 : P7.0,1
    set_output_pm(&PM7, (uint8_t)(BIT(0)|BIT(1)));

    // CHECK_10~13 : P10.4~7
    set_output_pm(&PM10, (uint8_t)(BIT(4)|BIT(5)|BIT(6)|BIT(7)));

    // CHECK_14~17 : P5.7,6,5,4
    set_output_pm(&PM5, (uint8_t)(BIT(7)|BIT(6)|BIT(5)|BIT(4)));

    // CHECK_18~20 : P1.0,3,4
    set_output_pm(&PM1, (uint8_t)(BIT(0)|BIT(3)|BIT(4)));

    check_all_off();
}

void check_all_off(void)
{
    P15 &= (uint8_t)~(BIT(4)|BIT(5)|BIT(6));
    P0  &= (uint8_t)~(BIT(0)|BIT(3));
    P3  &= (uint8_t)~(BIT(0)|BIT(2));
    P7  &= (uint8_t)~(BIT(0)|BIT(1));
    P10 &= (uint8_t)~(BIT(4)|BIT(5)|BIT(6)|BIT(7));
    P5  &= (uint8_t)~(BIT(4)|BIT(5)|BIT(6)|BIT(7));
    P1  &= (uint8_t)~(BIT(0)|BIT(3)|BIT(4));
}

void check_pin(uint8_t ch, uint8_t on)
{
    switch (ch)
    {
        case 1:  write_port(&P15, BIT(4), on); break; // CHECK_1 : P154
        case 2:  write_port(&P15, BIT(5), on); break; // CHECK_2 : P155
        case 3:  write_port(&P0,  BIT(0), on); break; // CHECK_3 : P00
        case 4:  write_port(&P15, BIT(6), on); break; // CHECK_4 : P156
        case 5:  write_port(&P3,  BIT(0), on); break; // CHECK_5 : P30
        case 6:  write_port(&P3,  BIT(2), on); break; // CHECK_6 : P32
        case 7:  write_port(&P0,  BIT(3), on); break; // CHECK_7 : P03
        case 8:  write_port(&P7,  BIT(0), on); break; // CHECK_8 : P70
        case 9:  write_port(&P7,  BIT(1), on); break; // CHECK_9 : P71
        case 10: write_port(&P10, BIT(4), on); break; // CHECK_10: P104
        case 11: write_port(&P10, BIT(5), on); break; // CHECK_11: P105
        case 12: write_port(&P10, BIT(6), on); break; // CHECK_12: P106
        case 13: write_port(&P10, BIT(7), on); break; // CHECK_13: P107
        case 14: write_port(&P5,  BIT(7), on); break; // CHECK_14: P57
        case 15: write_port(&P5,  BIT(6), on); break; // CHECK_15: P56
        case 16: write_port(&P5,  BIT(5), on); break; // CHECK_16: P55
        case 17: write_port(&P5,  BIT(4), on); break; // CHECK_17: P54
        case 18: write_port(&P1,  BIT(0), on); break; // CHECK_18: P10
        case 19: write_port(&P1,  BIT(3), on); break; // CHECK_19: P13
        case 20: write_port(&P1,  BIT(4), on); break; // CHECK_20: P14
        default: /* ignore */ break;
    }
}