# #############################################################################
#
# Copyright (c) 1999-2001 by Wayne C. Gramlich.
# All rights reserved.
#
# Permission to use, copy, modify, distribute, and sell this software
# for any purpose is hereby granted without fee provided that the above
# copyright notice and this permission are retained.  The author makes
# no representations about the suitability of this software for any purpose.
# It is provided "as is" without express or implied warranty.
#
# This is a test harness for testing the LED10 RoboBrick.  See:
#
#   http://web.gramlich.net/projects/robobricks/led10/index.html
#
# for more details.
#
# #############################################################################

processor pic16f628 cp=off cpd=off lvp=off bowden=off mclre=on pwrte=off wdte=off fosc=xt
constant clock_rate 10000000
constant clock_ticks_per_instruction 4
constant instructions_per_second clock_rate / clock_ticks_per_instruction
constant baud_rate 2400
constant instructions_per_bit clock_rate / (clock_ticks_per_instruction * baud_rate)
constant delays_per_bit 3
constant instructions_per_delay instructions_per_bit / delays_per_bit
constant extra_instructions_per_bit 12

constant extra_instructions_per_delay extra_instructions_per_bit / delays_per_bit
constant delay_instructions instructions_per_delay - extra_instructions_per_delay

# The null pulse that comes back from a clock pulse command is supposed to be
# exactly 9 bits long.  9 bits at 2400 baud is 9/2400 = 3.75mS.  The number
# iterations through the loop is 3.75mS / (number of instructions per iteration.)
constant nine_bits_instructions (clock_rate * 9) / (clock_ticks_per_instruction * baud_rate)
constant instructions_per_iteration 7
constant iterations_for_nine_bits nine_bits_instructions / instructions_per_iteration
constant iterations_high iterations_for_nine_bits / 256
constant iterations_low iterations_for_nine_bits - (iterations_high * 256)

# Some character constants:
constant sp 32
constant cr 13
constant lf 10

# Some bit definitions:
constant rx_slave_bit 5
constant tx_slave_bit 4
constant rx_master_bit 1
constant tx_master_bit 2
constant rx_slave_mask 1 << rx_slave_bit
constant tx_slave_mask 1 << tx_slave_bit
constant rx_master_mask 1 << rx_master_bit
constant tx_master_mask 1 << tx_master_bit

# Some register definitions:
register status 3
bind c status@0
bind z status@2

# Some port and pin definitions:
#port porta a unused none
port portb b bits_only read_write_static
pin tx_master_pin portb tx_master_bit write_only
pin rx_master_pin portb rx_master_bit read_only
pin tx_slave_pin portb tx_slave_bit write_only
pin rx_slave_pin portb rx_slave_bit read_only

# Miscellaneous definitions:
constant space 0xff
constant buffer_size 3

string_constants {
    common_string = 0s'Common'
    done_string = 0s'Done'
    fail_string = 0s'Fail'
    hello_string = 0s'BIROD2_Test'
    input_string = 0s'Input'
    interrupt_string = 0s'Interrupt'
    enable_string = 0s'Enable and Alternate'
    read_string = 0s'Read'
    register_string = 0s'Register'
    test_string = 0s'Test'
}

global temp byte

procedure main {
    arguments_none
    returns_nothing

    # Read a byte.

    variable buffer[buffer_size] byte
    variable char byte
    variable index byte
    variable number byte

    # Print out a welcome message:
    call master_crlf()
    call master_string(hello_string)
    call master_crlf()

    # Main loop
    number := 0
    loop_forever {
	# Get a character:
	tx_slave_pin := 1
	char := master_get()

	# Delay 2/3's of bit make sure that get_byte is done.
	call delay()
	call delay()

	if (0c'0' <= char && char < (0c'9' + 1)) {
	    # Do a multiply by 8 then add in digit:
	    call master_send(char)	    
	    number := (number << 3) + char - 0c'0'
	} else_if (char = 0c's') {
	    # Send byte to brick, no wait:

	    # Echo command and send CRLF:
	    call master_send(char)
	    call master_crlf()

	    # Ship the byte down to the brick:
	    call slave_send(number)
	    number := 0

	} else_if (char = 0c'w') {
	    # Send byte to brick, wait for results:

	    # Echo command and send CRLF:
	    call master_send(char)
	    call master_crlf()

	    # Ship the byte down to the brick ...
	    call slave_send(number)

	    number := 0
	    #call delay()

	    # ...and wait for a response:
	    index := 0
	    while (index < buffer_size) {
		buffer[index] := slave_get()
		index := index + 1
	    }

	    index := 0
	    while (index < buffer_size) {
		call master_octal(buffer[index])
		index := index + 1
	    }

	    # Terminate the output list
	    call master_crlf()
	} else_if (char = 0c'i') {
	    # Interrogate the slave RoboBrick:

	    call master_send(char)
	    call master_crlf()

	    # Initialize the id index:
	    call slave_send(0xfd)

	    # Get the first 8 bytes:
	    call common_test_id_bytes8()

	    # Get the next 8 bytes of random numbers:
	    call common_test_id_bytes8()

	    # Get the next 8 bytes of random numbers:
	    call common_test_id_bytes8()

	    # Get the slave brick name:
	    call common_test_id_string()

	    # Get the vendor name:
	    call common_test_id_string()
	} else_if (char = 0c't') {
	    # Do testing:
	    call master_send(char)
	    call master_crlf()
	    call test()
	} else_if (char = 0c'c') {
	    call master_send(char)
	    call master_crlf()
	    call clock_adjust(number)
	    number := 0
	} else {
	    # Just echo back the current number:

	    call master_send(0c'<')
	    call master_octal(char)
	    call master_send(0c'>')

	    # Send a carriage-return line-feed:
	    call master_crlf()
	
	    # Output the character in octal:
	    call master_octal(number)
	
	    # Send a carriage-return line-feed:
	    call master_crlf()

	    # Reset number
	    number := 0
	}
    }
}

procedure clock_adjust {
    argument adjust byte
    returns_nothing

    # This procedure will adjust the clock to the slave.

    variable command byte
    variable count byte
    variable error byte
    variable error_minimum byte
    variable high byte
    variable low byte
    variable target byte

    target := iterations_low - adjust
    count := 2
    error := 0xff
    error_minimum := 0xf0

    # Print out the target:
    call master_send(0c'T')
    call master_octal(iterations_high)
    call master_octal(target)
    call master_crlf()

    loop_forever {
	# Print out the clock value:
	call master_send(0c'C')
	call slave_send(0xfa)
	call master_octal(slave_get())

	# Ask for a timing byte:
	call slave_send(0xfb)

	low := 0
 	high := 0
	while (rx_slave_pin) {
	    # Do nothing:
	}
	while (!rx_slave_pin) {
	    low := low + 1
	    if (z) {
		high := high + 1
	    } 
	}

	# Print out high and low:
	call master_send(0c'H')
	call master_octal(high)
	call master_send(0c'L')
	call master_octal(low)

	# Now think about adjusting clock.
	if (high > iterations_high) {
	    # Clock pulse is too long; slave clock is too slow:
	    command := 0xf9
	    error := 0xff - target
        } else_if (high < iterations_high) {
	    # Clock pulse is too short; slave clock is too fast:
	    command := 0xf8
	    error := target
	} else {
	    # The high 8-bits are equal:
	    if (low > target) {
		# Clock pulse is too long; slave clock is too slow:
		command := 0xf9
		error := low - target
	    } else_if (low < target) {
		# Clock pulse is too short; slave clock is too fast:
		command := 0xf8
		error := target - low
	    } else {
		# Exact match; we are done:
		command := 0
		error := 0
		error_minimum := 0
	    }
	}

	# Print out the error and error minimum:
	call master_send(0c'E')
	call master_octal(error)
	call master_send(0c'M')
	call master_octal(error_minimum)

	if (error = error_minimum) {
	    call master_crlf()
	    return
	} else_if (error < error_minimum) {
	    error_minimum := error
	    error := error + 1
	} else {
	    count := count - 1
	    if (z) {
		error_minimum := error_minimum + 1
		count := 2
	    }
	}

	# Now adjust the clock:
	call slave_send(command)

	call master_crlf()
    }
}

procedure test {
    arguments_none
    returns_nothing

    # This procedure will test the Shaft2 RoboBrick:

    # Verify that the common commands are working:
    call common_test()

    # Verify that we can interrupt:
    call interrupt_test()

    # Let's go test some registers:
    call master_string(register_string)
    call master_string(test_string)
    call master_crlf()

    # Enable/Complement:
    call test_register(0x10, 3, 0x30)

    # High/Low:
    call test_register(0x20, 4, 0x31)

    # Raising/Falling:
    call test_register(0x30, 5, 0x32)

    # Now test that the BIROD's are actually working:
    call test_inputs(0)
    call test_inputs(1)
    call test_inputs(2)
    call test_inputs(3)

    # Test for interrupts:
    call master_string(interrupt_string)
    call master_string(test_string)
    call master_crlf()

    # High interrupt:
    call test_interrupt(0, 1, 0x20, 4)

    # Low interrupt:
    call test_interrupt(1, 0, 0x20, 1)

    # Raising interrupt:
    call test_interrupt(0, 1, 0x30, 4)

    # Falling interrupt:
    call test_interrupt(1, 0, 0x30, 1)

    # Announce we are done:
    call master_string(done_string)
    call master_crlf()
}

procedure test_inputs {
    argument pattern byte
    returns_nothing

    # This procedure will verify that the inputs can be set to pattern.

    variable sleep1 byte
    variable sleep2 byte
    variable counter byte
    variable test byte
    variable command byte
    variable mask byte
    variable masked_pattern byte
    variable alternate byte

    # Enable the IROD's:
    call test_prompt(pattern)

    count_down counter 8 {
	# Set the enable bits:
	test := counter - 1
	mask := test & 3
	command := 0x10 | (mask << 2)
	call slave_send(command)
	masked_pattern := mask & pattern

	# Set the alternate bit:
	alternate := test >> 2
	call slave_send(6 | alternate)

	# Announce test:
	call master_octal(test)
	call master_octal(masked_pattern)
	call master_string(enable_string)
	call master_string(test_string)
	call master_crlf()

	# Let the enables take hold:
	count_down sleep1 50 {
	    # 30 * 1/3 bit delays = 10 bit delays:
	    count_down sleep2 30 {
		call delay()
	    }
	}

	# Verify that the alternate bit is set:
	call slave_send(3)
	temp := slave_get()
	if (temp != command & 0xf) {
	    call master_octal(temp)
	    call master_fail(read_string, 0x40)
	}

	# Verify that the alternate bit is set:
	call slave_send(2)
	temp := slave_get()
	if (temp != alternate) {
	    #call master_octal(temp)
	    call master_fail(read_string, 0x41)
	}

	# Regular read:
	call slave_send(0)
	temp := slave_get()
	if (temp != masked_pattern) {
	    call master_octal(temp)
	    call master_fail(read_string, 0x42)
	}

	# Read raw:
	call slave_send(1)
	temp := slave_get()
	if (temp != masked_pattern) {
	    call master_octal(temp)
	    call master_fail(read_string, 0x43)
	}

	# Now test complement:
	call slave_send(command | 3)
	call slave_send(0)
	temp := slave_get()
	if (temp ^ 3 != masked_pattern) {
	    call master_octal(temp)
	    call master_fail(read_string, 0x44)
	}
  
	# Verify that raw did not change:
	call slave_send(1)
	temp := slave_get()
	if (temp != masked_pattern) {
	    call master_octal(temp)
	    call master_fail(read_string, 0x45)
	}

	# Reset complement bits:
	call slave_send(command)
    }
}

procedure test_prompt {
    argument pattern byte
    returns_nothing

    # This procedure will prompt the user for pattern.

    variable character byte

    # Enable both sensors:
    call slave_send(0x1c)

    # Wait for the inputs to match:
    call master_crlf()
    temp := pattern + 1
    while (temp != pattern) {
	# Display the pattern:
	call master_send(cr)
	call master_string(input_string)
	call test_show(pattern)
	call master_send(0c'<')
	call test_show(temp)
	call master_send(0c'>')
	call master_send(0c':')

	# Read the raw inputs:
	call slave_send(1)
	temp := slave_get()
    }

    # Print a carraige-return line-feed:
    call master_crlf()
}

procedure test_show {
    argument pattern byte
    returns_nothing

    # This procedure will print out pattern:

    variable character byte

    if (pattern@1) {
	character := 0c'1'
    } else {
	character := 0c'0'
    }
    call master_send(character)
    if (pattern@0) {
	character := 0c'1'
    } else {
	character := 0c'0'
    }
    call master_send(character)
}

procedure test_interrupt {
    argument start_pattern byte
    argument end_pattern byte
    argument set_command byte
    argument set_bits byte
    returns_nothing

    # This procedure will validate that an interrupt occurs.

    call test_prompt(start_pattern)

    # Enable only one:
    call slave_send(0x14)

    # Enable the mask:
    call slave_send(set_command | set_bits)

    # Let the tester know what we want:
    call test_show(end_pattern)
    call master_send(0c':')

    # Wait for an interrupt:
    call test_interrupt_wait()

    # Disable mask:
    call slave_send(set_command)
}

procedure test_interrupt_wait {
    arguments_none
    returns_nothing

    # This procedure will test an interrupt:

    # Now enable interrupts:
    call slave_send(0xf2)

    # Now wait for interrupt:
    while (rx_slave_pin) {
	call delay()
    }
    call master_crlf()
}

procedure test_register {
    argument set_command byte
    argument read_command byte
    argument error byte
    returns_nothing

    # This procedure will test the ability to set and read back a byte register:

    variable index byte

    index := 16
    while (index != 0) {
	index := index - 1
	call slave_send(set_command | index)

	call slave_send(read_command)
	temp := slave_get()
	if (temp != index) {
	    call master_octal(index)
	    call master_octal(temp)
	    call master_fail(register_string, error)
	}
    }
}

# The procedures below are used to test the common shared commands:

procedure common_test {
    arguments_none
    returns_nothing

    # This procedure will verify that the common shared commands work.

    variable actual byte

    # Print the ID information:
    # ID reset:
    call slave_send(0xfd)
    # Read the fixed bytes:
    call common_test_id_match(1, 0xc0)
    call common_test_id_match(0, 0xc1)
    call common_test_id_match(11, 0xc2)
    call common_test_id_match(1, 0xc3)
    call common_test_id_match(0, 0xc4)
    call common_test_id_match(0, 0xc5)
    call common_test_id_match(0, 0xc6)
    call common_test_id_match(0, 0xc7)
    call master_crlf()
    call common_test_id_bytes8()
    call common_test_id_bytes8()
    # Read the brick name:
    call common_test_id_string()
    # Read the vendor name:
    call common_test_id_string()
    # Verfify that we don't read off the end of the id and crash:
    call common_test_id_next()
    # Reset id:
    call slave_send(0xfd)
    # Verify that we are still alive:
    call common_test_id_match(1, 0xc8)

    # Read glitch:
    # Clear glitch register:
    call slave_send(0xfe)
    actual := slave_get()
    # Send a couple of glitches:
    call slave_send(0xff)
    call slave_send(0xff)
    # Read the glitch register:
    call slave_send(0xfe)
    if (slave_get() != 2) {
	call master_fail(common_string, 0xc9)
    }

    # Do a clock pulse:
    call slave_send(0xfb)
    if (slave_get() != 0) {
	call master_fail(common_string, 0xca)
    }
    # Read clock:
    call slave_send(0xfa)
    actual := slave_get()
    # Increment:
    call slave_send(0xf9)
    # Decrement:
    call slave_send(0xf8)
    # Read clock again:
    call slave_send(0xfa)
    if (actual != slave_get()) {
	call master_fail(common_string, 0xcb)
    }
}

procedure common_test_id_match {
    argument desired byte
    argument test_number byte
    returns_nothing

    # This procedure will verify that the next byte in the id is {desired}.

    if (desired != common_test_id_next()) {
	call master_fail(common_string, test_number)
    }
}

procedure common_test_id_bytes8 {
    arguments_none
    returns_nothing

    # This procedure will print out the next 8 bytes of the id in octal.

    variable count byte

    count_down count 8 {
	call master_octal(common_test_id_next())
    }
    call master_crlf()
}

procedure common_test_id_string {
    arguments_none
    returns_nothing

    # This procedure will print out the next id string.

    variable count byte

    count_down count (common_test_id_next()) {
	call master_send(common_test_id_next())
    }
    call master_crlf()
}

procedure common_test_id_next {
    arguments_none
    returns byte

    # This procedure returns the next byte from the identification string.

    call slave_send(0xfc)
    return slave_get()
}

# The procedures below are used to test the shared interrupt commands:

procedure interrupt_test {
    arguments_none
    returns_nothing

    # This procedure tests the common shared interrupt commands.

    variable counter byte

    # Clear interrupt bits:
    call slave_send(0xf0)
    call interrupt_read(0, 0xd0)

    # Set/clear interrupt pending bit only:
    call slave_send(0xf5)
    call interrupt_read(1, 0xd1)
    call slave_send(0xf4)
    call interrupt_read(0, 0xd2)
    call slave_send(0xf1)
    call interrupt_read(1, 0xd3)
    call slave_send(0xf0)
    call interrupt_read(0, 0xd4)

    # Set/clear interrupt enable bit only:
    call slave_send(0xf7)
    call interrupt_read(2, 0xd5)
    call slave_send(0xf6)
    call interrupt_read(0, 0xd6)
    call slave_send(0xf2)
    call interrupt_read(2, 0xd7)
    call slave_send(0xf0)
    call interrupt_read(0, 0xd8)

    # Now set both the interrupt enable and pending bit.
    # This must trigger an interrupt.

    call slave_send(0xf3)
    count_down counter 3 {
	call delay()
    }
    if (rx_slave_pin) {
	# No interrupt detected:
	call master_fail(interrupt_string, 0xd9)
    }
    # Read the interrupt enable bit is cleared as a side effect
    # of reading the bit.
    call interrupt_read(1, 0xda)

    # Clear the interrupt pending bit:
    call slave_send(0xf0)
    call interrupt_read(0, 0xdb)
}

procedure interrupt_read {
    argument desired byte
    argument test_number byte
    returns_nothing

    # This procedure will verify that the interrupt bits match {desired}.

    call slave_send(0xef)
    temp := slave_get()
    if (temp != desired) {
	call master_octal(temp)
	call master_fail(interrupt_string, test_number)
    }
}

# The following procedures are used to communicate with the master:

procedure master_crlf {
    arguments_none
    returns_nothing

    # This procedure will output a carriage-return line-feed
    # to the master.

    call master_send(cr)
    call master_send(lf)
}

procedure master_fail {
    argument test_name string
    argument test_number byte
    returns_nothing

    # This procedure will output `fail' followed by a carriage return
    # and line feed.

    call master_string(fail_string)
    call master_string(test_name)
    call master_octal(test_number)
    call master_crlf()
}

procedure master_get {
    arguments_none
    returns byte

    # This procedure will get the next character from the master.

    variable character byte

    character := 0xfc
    while (character = 0xfc) {
	character := get_byte(rx_master_mask)
    }
    return character
}

procedure master_octal {
    argument number byte
    returns_nothing

    # This procedure will output {number} in octal to the tx port.

    # Output the character in octal:
    call master_send((number>>6) + 0c'0')
    call master_send(((number>>3) & 7) + 0c'0')
    call master_send((number & 7) + 0c'0')
    call master_send(sp)
}

procedure slave_get {
    arguments_none
    returns byte

    # This procedure will get a byte from the slave or return 0xfc
    # if no byte is forthcoming.

    return get_byte(rx_slave_mask)
}

procedure slave_send {
    argument data byte
    returns_nothing

    # This procedure will send one byte of {data} to the slave.

    call send_byte(data, tx_slave_mask)
}

# The last procedures do character sending and receiving:

procedure get_byte {
    argument mask byte
    returns byte

    # Get an 8-bit byte from {mask} bit of {portb} and return it.
    # If no character shows up in a while 0xfc is returned.

    variable count byte
    variable char byte

    # Wait until a start bit arrives:
    count := 0
    while (portb & mask != 0) {
	count := count - 1
	if (count = 0) {
	    return 0xfc
	}
	call delay()
    }

    # Skip over the start bit:
    call delay()
    call delay()
    call delay()

    # Sample in the middle third of each data bit:
    # 1 cycle:
    char := 0
    # 2 cylces for loop setup:
    # 1+2 = 3
    nop extra_instructions_per_bit - 3
    count_down count 8 {
	call delay()
	# 2 cycles:
	char := char >> 1
	# 3 cycles:
	if (portb & mask != 0) {
	    char@7 := 1
	}
	call delay()
	call delay()
	# 3 cycles at end of loop:
	# 2+3+3 = 8
	nop extra_instructions_per_bit - 8
    }

    # Skip over 2/3 of the stop bit:
    call delay()
    call delay()

    return char
}

procedure send_byte {
    argument char byte
    argument mask byte
    returns_nothing

    # Send {char} to {mask} bit of {portb}.

    variable count byte
    variable mark byte

    # Send the start bit:
    # 3 cycles:
    mark := mask ^ space
    # 1 cycles (mark is already in W):
    portb := mark
    call delay()
    call delay()
    call delay()
    # 2 cylces for loop setup:
    # 3+1+2 = 6
    nop extra_instructions_per_bit - 6

    # Send the data:
    count_down count 8 {
	# 5 cylces:
	if (char@0) {
	    portb := space
	} else {
	    portb := mark
	}
	# 2 cycles:
	char := char >> 1
	call delay()
	call delay()
	call delay()
	# 3 cycles at end of loop:
	# 5+2+3 = 10
	nop extra_instructions_per_bit - 10
    }

    # Send the stop bit:
    # 1 cycle to finish off previous loop:
    nop 1
    # 2 cycles:
    portb := space
    call delay()
    call delay()
    call delay()
    # 4 cycles for call/return:
    # 4 cycles for to arguments:
    # 2+4+4 = 10
    nop extra_instructions_per_bit - 10
}

procedure master_send {
    argument character byte
    returns_nothing

    # This procedure will output {character} to the master.

    call send_byte(character, tx_master_mask)
}

procedure master_string {
    argument message string
    returns_nothing

    # This procedure will output {message} to the master.

    variable size byte
    variable index byte

    index := 0
    while (index < message.size) {
	call master_send(message[index])
	index := index + 1
    }
    call master_send(sp)
}

procedure delay {
    arguments_none
    returns_nothing
    uniform_delay delay_instructions

    # Delay 1 third of a bit:

    # Kick the dog:
    watch_dog_reset
}