static void debug_fiq(void *data, void *regs)
{
int c;
static int last_c;
while ((c = debug_getc()) != -1) {
if (!debug_enable) {
if ((c == 13) || (c == 10)) {
debug_enable = true;
debug_count = 0;
debug_prompt();
}
} else if ((c >= ' ') && (c < 127)) {
if (debug_count < (DEBUG_MAX - 1)) {
debug_buf[debug_count++] = c;
debug_putc(c);
}
} else if ((c == 8) || (c == 127)) {
if (debug_count > 0) {
debug_count--;
debug_putc(8);
debug_putc(' ');
debug_putc(8);
}
} else if ((c == 13) || (c == 10)) {
if (c == '\r' || (c == '\n' && last_c != '\r')) {
debug_putc('\r');
debug_putc('\n');
}
if (debug_count) {
debug_buf[debug_count] = 0;
debug_count = 0;
debug_exec(debug_buf, regs);
} else {
debug_prompt();
}
}
last_c = c;
}
debug_flush();
}
void main(void) {
BRD_init(); //Initalise NP2
Hardware_init();
HAL_Delay(3000);
struct PanTilt pantiltvars;
pantilt = &pantiltvars;
pantilt->write_angles = 0;
pantilt->read_angles = 0;
pantilt->set_angle_pan = 0;
pantilt->set_angle_tilt = 0;
struct Variables variables;
vars = &variables;
vars->count = 0;
vars->bit_half = 1;
vars->bit_count = 0;
vars->encoded_bit_count = -1;
vars->encoded_bit = 0;
vars->encoded_char = hamming_byte_encoder('<');
vars->transmit_frequency = 1000;
vars->period_multiplyer = ((1.000000/vars->transmit_frequency)*500)*0.998;
vars->recieve_element = 0;
vars->recieve_flag = 0;
Pb_init();
s4353096_radio_setchan(s4353096_chan);
s4353096_radio_settxaddress(s4353096_tx_addr);
s4353096_radio_setrxaddress(s4353096_rx_addr);
while (1) {
s4353096_radio_setfsmrx();
s4353096_radio_fsmprocessing();
if (vars->recieve_flag == 1) {
manchester_decode();
vars->recieve_flag = 0;
}
if (mode == S4353096_RADIO) {
/*Processes the current fsm state*/
RxChar = debug_getc();
if (RxChar != '\0') {
s4353096_keystroke = 1;
while(s4353096_keystroke == 1){
if (RxChar == '\r' || s4353096_payload_length == 7) {
for (int j = s4353096_payload_length; j < 7; j++) {
s4353096_payload_buffer[j] = '-';
}
s4353096_radio_fsmcurrentstate = S4353096_IDLE_STATE;
s4353096_radio_fsmprocessing();
s4353096_radio_fsmcurrentstate = S4353096_TX_STATE;
debug_printf("\n");
/*Compiles the transmit packet. Transmits packet if in TX state*/
s4353096_radio_sendpacket(s4353096_radio_getchan(), s4353096_addr_get, s4353096_payload_buffer);
s4353096_radio_fsmprocessing();
s4353096_radio_setfsmrx();
s4353096_radio_fsmprocessing();
s4353096_payload_length = 0;
s4353096_keystroke = 0;
} else if (RxChar != '\0') {
s4353096_payload_buffer[s4353096_payload_length] = RxChar;
s4353096_payload_length++;
debug_putc(RxChar); //reflect byte using putc - puts character into buffer
debug_flush(); //Must call flush, to send character //reflect byte using printf - must delay before calling printf again.
} else {
}
HAL_Delay(125);
RxChar = debug_getc();
}
} else {
}
s4353096_radio_fsmprocessing();
if (s4353096_radio_getrxstatus() == 1) { //Checks if packet has been recieved
/*Prints recieved packet to console*/
s4353096_radio_getpacket(s4353096_rx_buffer);
} else {
}
}
if (pantilt->write_angles == 1) {
s4353096_pantilt_angle_write(1, pantilt->set_angle_pan);
s4353096_pantilt_angle_write(0, pantilt->set_angle_tilt);
pantilt->write_angles = 0;
}
if (((HAL_GetTick()/10000) % 100) == 0) {
debug_printf("\nPan: %d Tlit: %d\n", pantilt->set_angle_pan, pantilt->set_angle_tilt);
}
if (pantilt->read_angles == 1) { /*Delay for 1 second or setup to delay for 0.2 seconds and set angle to += or -= 1 each time*/
if (mode == S4353096_JOYSTICK) {
y_value = s4353096_joystick_y_read();
if ((y_value > 2500) && (pantilt->set_angle_pan < 76)) {
pantilt->set_angle_pan += 1;
} else if ((y_value < 1550) && (pantilt->set_angle_pan > -76)) {
pantilt->set_angle_pan -= 1;
} else { //Joystick is stationary, no input
}
x_value = s4353096_joystick_x_read();
if ((x_value > 2500) && (pantilt->set_angle_tilt < 76)) {
pantilt->set_angle_tilt += 1;
} else if ((x_value < 1550) && (pantilt->set_angle_tilt > -76)) {
pantilt->set_angle_tilt -= 1;
} else {
}
} else if (mode == S4353096_TERMINAL) {
/* Receive characters using getc */
RxChar = debug_getc();
/* Check if character is not Null */
if (RxChar != '\0') {
switch (RxChar) {
case 'w':
pantilt->set_angle_tilt += 1;
break;
case 's':
pantilt->set_angle_tilt -= 1;
break;
case 'a':
pantilt->set_angle_pan += 1;
break;
case 'd':
pantilt->set_angle_pan -= 1;
break;
case 't':
mode = S4353096_LASER_TRANSMIT;
break;
default:
break;
}
debug_flush();
}
s4353096_terminal_angle_check();
}
pantilt->read_angles = 0;
}
}
}
void bsp_start( void )
{
rtems_status_code sc = RTEMS_SUCCESSFUL;
#if !defined(mvme2100)
unsigned l2cr;
#endif
uintptr_t intrStackStart;
uintptr_t intrStackSize;
ppc_cpu_id_t myCpu;
ppc_cpu_revision_t myCpuRevision;
prep_t boardManufacturer;
motorolaBoard myBoard;
Triv121PgTbl pt=0;
/*
* Get CPU identification dynamically. Note that the get_ppc_cpu_type()
* function store the result in global variables so that it can be used
* later...
*/
myCpu = get_ppc_cpu_type();
myCpuRevision = get_ppc_cpu_revision();
/*
* Init MMU block address translation to enable hardware access
*/
#if !defined(mvme2100)
/*
* PC legacy IO space used for inb/outb and all PC compatible hardware
*/
setdbat(1, _IO_BASE, _IO_BASE, 0x10000000, IO_PAGE);
#endif
/*
* PCI devices memory area. Needed to access OpenPIC features
* provided by the Raven
*
* T. Straumann: give more PCI address space
*/
setdbat(2, PCI_MEM_BASE+PCI_MEM_WIN0, PCI_MEM_BASE+PCI_MEM_WIN0, 0x10000000, IO_PAGE);
/*
* Must have acces to open pic PCI ACK registers provided by the RAVEN
*/
setdbat(3, 0xf0000000, 0xf0000000, 0x10000000, IO_PAGE);
#if defined(mvme2100)
/* Need 0xfec00000 mapped for this */
EUMBBAR = get_eumbbar();
#endif
/*
* enables L1 Cache. Note that the L1_caches_enables() codes checks for
* relevant CPU type so that the reason why there is no use of myCpu...
*/
L1_caches_enables();
#if !defined(mvme2100)
/*
* Enable L2 Cache. Note that the set_L2CR(L2CR) codes checks for
* relevant CPU type (mpc750)...
*/
l2cr = get_L2CR();
#ifdef SHOW_LCR2_REGISTER
printk("Initial L2CR value = %x\n", l2cr);
#endif
if ( (! (l2cr & 0x80000000)) && ((int) l2cr == -1))
set_L2CR(0xb9A14000);
#endif
/*
* Initialize the interrupt related settings.
*/
intrStackStart = (uintptr_t) __rtems_end;
intrStackSize = rtems_configuration_get_interrupt_stack_size();
/*
* Initialize default raw exception handlers.
*/
sc = ppc_exc_initialize(
PPC_INTERRUPT_DISABLE_MASK_DEFAULT,
intrStackStart,
intrStackSize
);
if (sc != RTEMS_SUCCESSFUL) {
BSP_panic("cannot initialize exceptions");
}
select_console(CONSOLE_LOG);
/*
* We check that the keyboard is present and immediately
* select the serial console if not.
*/
#if defined(BSP_KBD_IOBASE)
{ int err;
err = kbdreset();
if (err) select_console(CONSOLE_SERIAL);
}
#else
select_console(CONSOLE_SERIAL);
#endif
boardManufacturer = checkPrepBoardType(&residualCopy);
if (boardManufacturer != PREP_Motorola) {
printk("Unsupported hardware vendor\n");
while (1);
}
myBoard = getMotorolaBoard();
printk("-----------------------------------------\n");
printk("Welcome to %s on %s\n", _RTEMS_version,
motorolaBoardToString(myBoard));
printk("-----------------------------------------\n");
#ifdef SHOW_MORE_INIT_SETTINGS
printk("Residuals are located at %x\n", (unsigned) &residualCopy);
printk("Additionnal boot options are %s\n", loaderParam);
printk("Initial system stack at %x\n",stack);
printk("Software IRQ stack starts at %x with size %u\n", intrStackStart, intrStackSize);
printk("-----------------------------------------\n");
#endif
#ifdef TEST_RETURN_TO_PPCBUG
printk("Hit <Enter> to return to PPCBUG monitor\n");
printk("When Finished hit GO. It should print <Back from monitor>\n");
debug_getc();
_return_to_ppcbug();
printk("Back from monitor\n");
_return_to_ppcbug();
#endif /* TEST_RETURN_TO_PPCBUG */
#ifdef SHOW_MORE_INIT_SETTINGS
printk("Going to start PCI buses scanning and initialization\n");
#endif
pci_initialize();
{
const struct _int_map *bspmap = motorolaIntMap(currentBoard);
if( bspmap ) {
printk("pci : Configuring interrupt routing for '%s'\n",
motorolaBoardToString(currentBoard));
FixupPCI(bspmap, motorolaIntSwizzle(currentBoard));
}
else
printk("pci : Interrupt routing not available for this bsp\n");
}
#ifdef SHOW_MORE_INIT_SETTINGS
printk("Number of PCI buses found is : %d\n", pci_bus_count());
#endif
#ifdef TEST_RAW_EXCEPTION_CODE
printk("Testing exception handling Part 1\n");
/*
* Cause a software exception
*/
__asm__ __volatile ("sc");
/*
* Check we can still catch exceptions and return coorectly.
*/
printk("Testing exception handling Part 2\n");
__asm__ __volatile ("sc");
/*
* Somehow doing the above seems to clobber SPRG0 on the mvme2100. The
* interrupt disable mask is stored in SPRG0. Is this a problem?
*/
ppc_interrupt_set_disable_mask( PPC_INTERRUPT_DISABLE_MASK_DEFAULT);
#endif
/* See above */
BSP_mem_size = residualCopy.TotalMemory;
BSP_bus_frequency = residualCopy.VitalProductData.ProcessorBusHz;
BSP_processor_frequency = residualCopy.VitalProductData.ProcessorHz;
BSP_time_base_divisor = (residualCopy.VitalProductData.TimeBaseDivisor?
residualCopy.VitalProductData.TimeBaseDivisor : 4000);
/* clear hostbridge errors but leave MCP disabled -
* PCI config space scanning code will trip otherwise :-(
*/
_BSP_clear_hostbridge_errors(0 /* enableMCP */, 0/*quiet*/);
/* Allocate and set up the page table mappings
* This is only available on >604 CPUs.
*
* NOTE: This setup routine may modify the available memory
* size. It is essential to call it before
* calculating the workspace etc.
*/
pt = BSP_pgtbl_setup(&BSP_mem_size);
if (!pt || TRIV121_MAP_SUCCESS != triv121PgTblMap(
pt, TRIV121_121_VSID, 0xfeff0000, 1,
TRIV121_ATTR_IO_PAGE, TRIV121_PP_RW_PAGE)) {
printk("WARNING: unable to setup page tables VME "
"bridge must share PCI space\n");
}
/*
* initialize the device driver parameters
*/
bsp_clicks_per_usec = BSP_bus_frequency/(BSP_time_base_divisor * 1000);
/*
* Initalize RTEMS IRQ system
*/
BSP_rtems_irq_mng_init(0);
/* Activate the page table mappings only after
* initializing interrupts because the irq_mng_init()
* routine needs to modify the text
*/
if (pt) {
#ifdef SHOW_MORE_INIT_SETTINGS
printk("Page table setup finished; will activate it NOW...\n");
#endif
BSP_pgtbl_activate(pt);
/* finally, switch off DBAT3 */
setdbat(3, 0, 0, 0, 0);
}
#if defined(DEBUG_BATS)
ShowBATS();
#endif
#ifdef SHOW_MORE_INIT_SETTINGS
printk("Exit from bspstart\n");
#endif
}
