From 13a8b4d301ae34c32433e5ff2b5a284a5750d6da Mon Sep 17 00:00:00 2001 From: Richard Cornwell Date: Tue, 19 Mar 2024 12:16:16 -0400 Subject: [PATCH] KA10: Updated PIDP10 panel support. --- PDP10/ka10_pipanel.c | 935 +++++++++++++++---------------------------- PDP10/kx10_cpu.c | 18 +- PDP10/kx10_defs.h | 2 +- 3 files changed, 344 insertions(+), 611 deletions(-) diff --git a/PDP10/ka10_pipanel.c b/PDP10/ka10_pipanel.c index 8b5f6cb..a0896c4 100644 --- a/PDP10/ka10_pipanel.c +++ b/PDP10/ka10_pipanel.c @@ -34,22 +34,11 @@ * To Build this: * make PIDP10=1 pdp10-ka */ -#include -#include +#include /* Needed for pthread */ +#include /* Needed for sleep/geteuid */ #include -#include -#include -#include // extra -#include //Needed for nanosleep -#include //Needed for pthread -#include -#include -#include //Needed for I2C port -#include //Needed for I2C port -#include //Needed for I2C port -#include -#include #include +#include "pinctrl/gpiolib.h" #include "kx10_defs.h" extern uint64 SW; /* Switch register */ @@ -90,115 +79,92 @@ int pwr_off; /* Power off system */ int rep_rate; /* Rate of repeat function */ int rep_count; /* Count down to repeat trigger */ - -/* PION_LAMP = pi_enable */ -/* RUN_LAMP = RUN */ -/* PWR_LAMP = 1 */ - -/* PI_LAMP = MI_flag, MI_LAMP = !MI_flag */ -/* PI_ACT = PIE */ -/* PI_REQ = PIR */ -/* PI_PRO = PIH */ -/* USER_LAMP = FLAGS & USER */ -/* PROG_STOP_LAMP = prog_stop */ -/* MEM_STOP_LAMP = watch_stop */ -/* MA = AB */ -/* MB = MI_flag ? MI : MB */ -/* IX_MASK = IX */ -/* AC_MASK = AC */ -/* IND_LAMP = IND */ - /* led row 0 */ -#define MB_MASK0 0xffff00000 /* 0-15 */ -#define MB_V_0 20 /* Right */ +#define MB_MASK0 RMASK /* 18-35 */ +#define MB_V_0 0 /* right */ /* led row 1 */ -#define MB_MASK1 0x0000ffff0 /* 16-31 */ -#define MB_V_1 4 /* Right */ +#define MB_MASK1 LMASK /* 0-17 */ +#define MB_V_1 18 /* right */ /* led row 2 */ -#define RUN_LAMP 0x0080 -#define PION_LAMP 0x0100 -#define PWR_LAMP 0x0200 -#define PI_ENB_MASK 0x007f -#define MB_MASK2 0x0000000f /* 32-35 */ -#define MB_V_2 12 /* Left */ -#define PI_LAMP 0x0400 -#define MI_LAMP 0x0800 +#define AB_MASK2 RMASK /* 18-35 */ +#define AB_V_2 0 /* right */ /* led row 3 */ -#define IR_MASK3 0x1ff /* 0-9 */ -#define IR_V_3 7 /* Left */ -#define AC_MASK3 0xf -#define AC_V_3 3 /* Left */ -#define IND_LAMP 0x4 -#define IX_MASK3 0xc -#define IX_V_3 2 /* Right */ +#define IX_MASK3 017 +#define IX_V_3 0 /* left */ +#define IND_LAMP 020 +#define AC_MASK3 017 +#define AC_V_3 5 /* left */ +#define IR_MASK3 0777 /* 0-9 */ +#define IR_V_3 9 /* left */ /* led row 4 */ -#define IX_MASK4 3 -#define IX_V_4 14 /* left */ -#define MA_MASK4 0x3fff0 -#define MA_V_4 4 /* Right */ +#define PC_MASK4 RMASK /* 18-35 */ +#define PC_V_4 0 /* right */ /* led row 5 */ -#define PROG_STOP_LAMP 0x0080 -#define USER_LAMP 0x0100 -#define MEM_STOP_LAMP 0x0200 -#define PI_REQ_MASK 0x007f -#define MA_MASK5 0xf -#define MA_V_5 12 /* Left */ +#define PI_IOB_MASK5 0177 +#define PI_IOB_V_5 7 /* left */ +#define PI_ENB_MASK5 0177 +#define PI_ENB_V_5 0 /* left */ +#define PROG_STOP_LAMP 0040000 +#define USER_LAMP 0100000 +#define MEM_STOP_LAMP 0200000 +#define PWR_LAMP 0400000 /* led row 6 */ -#define PC_MASK6 0x3fffc -#define PC_V_6 2 /* right */ - -/* led row 7 */ -#define PI_PRO_MASK7 0x007f -#define PI_IOB_MASK7 0x0f80 -#define PI_IOB_V_7 7 /* left */ -#define PC_MASK7 0x0003 -#define PC_V_7 14 /* Left */ +#define PI_REQ_MASK6 0177 +#define PI_REQ_V_6 0 /* left */ +#define PI_PRO_MASK6 0177 +#define PI_PRO_V_6 7 /* left */ +#define RUN_LAMP 0040000 +#define PION_LAMP 0100000 +#define PI_LAMP 0200000 +#define MI_LAMP 0400000 /* switch row 0 */ -#define SR_MASK_0 0xffff00000 -#define SR_V0 20 /* Left */ +#define SR_MASK0 RMASK +#define SR_V_0 0 /* Left */ /* switch row 1 */ -#define SR_MASK_1 0x0000ffff0 -#define SR_V1 4 /* Left */ +#define SR_MASK1 LMASK +#define SR_V_1 18 /* Left */ /* Switch row 2 */ -#define SR_MASK_2 0x00000000f -#define SR_V2 12 /* Right */ -#define DEP_THIS 0x0800 /* SW = 11 */ -#define DEP_NEXT 0x0400 /* SW = 10 */ -#define SING_INST 0x0200 /* set sing_inst */ -#define SING_CYCL 0x0100 /* Nop */ -#define PAR_STOP 0x0080 /* Nop */ -#define NXM_STOP 0x0040 /* set nxm_stop */ -#define REP_SW 0x0020 -#define INST_FETCH 0x0010 /* adr_cond */ -#define DATA_FETCH 0x0008 /* adr_cond */ -#define WRITE_SW 0x0004 /* adr_cond */ -#define ADR_STOP_SW 0x0002 /* adr_cond */ -#define ADR_BRK_SW 0x0001 /* adr_cond */ +#define MA_SW_MASK3 RMASK +#define MA_SW_V_3 0 /* Left */ /* Switch row 3 */ -#define MA_SW_MASK_3 0x3fffc -#define MA_SW_V3 2 /* Left */ +#define EXAM_NEXT 000001 /* SW=0 */ +#define EXAM_THIS 000002 /* SW=1 */ +#define XCT_SW 000004 /* SW=2 Set xct_inst */ +#define RESET_SW 000010 /* SW=3 Call reset */ +#define STOP_SW 000020 /* SW=4 Set RUN = 0 */ +#define CONT_SW 000040 /* SW=5 call sim_instr */ +#define START_SW 000100 /* SW=6 Call reset then sim_instr */ +#define READ_SW 000200 /* SW=7 Boot function */ +#define DEP_NEXT 000400 /* SW=8 */ +#define DEP_THIS 001000 /* SW=9 */ /* Switch row 4 */ -#define MA_SW_MASK_4 0x00003 -#define MA_SW_V4 14 /* Right */ -#define EXAM_NEXT 0x0001 /* SW=0 */ -#define EXAM_THIS 0x0002 /* SW=1 */ -#define XCT_SW 0x0004 /* SW=2 Set xct_inst */ -#define RESET_SW 0x0008 /* SW=3 Call reset */ -#define STOP_SW 0x0010 /* SW=4 Set RUN = 0 */ -#define CONT_SW 0x0020 /* SW=5 call sim_instr */ -#define START_SW 0x0040 /* SW=6 Call reset then sim_instr */ -#define READ_SW 0x0080 /* SW=7 Boot function */ +#define ADR_BRK_SW 000001 /* Address Break */ +#define ADR_STOP_SW 000002 /* Address stop */ +#define WRITE_SW 000004 /* Write stop */ +#define DATA_FETCH 000010 /* Data fetch stop */ +#define INST_FETCH 000020 /* Instruct fetch stop */ +#define REP_SW 000040 /* Repeat switch */ +#define NXM_STOP 000100 /* set nxm_stop */ +#define PAR_STOP 000200 /* Nop */ +#define SING_CYCL 000400 /* Nop */ +#define SING_INST 001000 /* set sing_inst */ + +int xrows[3] = { 4, 17, 27 }; +int xIO = 22; /* GPIO 22: */ +int cols[18] = { 21,20,16,12,7,8,25,24,23,18,10,9,11,5,6,13,19,26 }; + +long intervl = 50000; struct { int last_state; /* last state */ @@ -208,154 +174,25 @@ struct { } switch_state[16]; -int file_i2c; /* i2c channel */ -unsigned char buffer[60] = {0}; /* data exchange buffer for i2c use */ - - -#define BLOCK_SIZE (4*1024) - -struct bcm2835_peripheral { - unsigned long addr_p; - int mem_fd; - void *map; - volatile unsigned int *addr; -} gpio; - -static unsigned get_dt_ranges(const char *filename, unsigned offset) -{ - unsigned address = ~0; - FILE *fp = fopen(filename, "rb"); - if (fp) { - unsigned char buf[4]; - fseek(fp, offset, SEEK_SET); - if (fread(buf, 1, sizeof buf, fp) == sizeof buf) - address = buf[0] << 24 | buf[1] << 16 | buf[2] << 8 | buf[3] << 0; - fclose(fp); - } - return address; -} - - -static unsigned bcm_host_get_peripheral_address(void) // find Pi's gpio base address -{ -// Pi 4 fix: https://github.com/raspberrypi/userland/blob/master/host_applications/linux/libs/bcm_host/bcm_host.c - unsigned address = get_dt_ranges("/proc/device-tree/soc/ranges", 4); - if (address == 0) - address = get_dt_ranges("/proc/device-tree/soc/ranges", 8); - return address == ~0 ? 0x20000000 : address; -} - -void short_wait(void) // creates pause required in between clocked GPIO settings changes -{ - fflush(stdout); // - usleep(1); // suggested as alternative for asm which c99 does not accept -} - -// GPIO setup macros. -// In early versions INP_GPIO(x) was used always before OUT_GPIO(x), -// this is disabled now by INO_GPIO(g) -#define INO_GPIO(g) //INP_GPIO(g) // Use this before OUT_GPIO -#define INP_GPIO(g) *(gpio.addr + ((g)/10)) &= ~(7<<(((g)%10)*3)) -#define OUT_GPIO(g) *(gpio.addr + ((g)/10)) |= (1<<(((g)%10)*3)) -#define SET_GPIO_ALT(g,a) *(gpio.addr + (((g)/10))) |= (((a)<=3?(a) + 4:(a)==4?3:2)<<(((g)%10)*3)) -#define GPIO_SET *(gpio.addr + 7) // sets bits which are 1 ignores bits which are 0 -#define GPIO_CLR *(gpio.addr + 10) // clears bits which are 1 ignores bits which are 0 -#define GPIO_READ(g) *(gpio.addr + 13) &= (1<<(g)) -#define GPIO_PULL *(gpio.addr + 37) // pull up/pull down -#define GPIO_PULLCLK0 *(gpio.addr + 38) // pull up/pull down clock -// Pi 4 update, -/* https://github.com/RPi-Distro/raspi-gpio/blob/master/raspi-gpio.c */ -/* 2711 has a different mechanism for pin pull-up/down/enable */ -#define GPPUPPDN0 57 /* Pin pull-up/down for pins 15:0 */ -#define GPPUPPDN1 58 /* Pin pull-up/down for pins 31:16 */ -#define GPPUPPDN2 59 /* Pin pull-up/down for pins 47:32 */ -#define GPPUPPDN3 60 /* Pin pull-up/down for pins 57:48 */ - - -// GPIO pin definitions -// 8 ledrows each provide power to a block of 16 LEDs (when set to output high) -u_int8_t ledrows[8] = { 16,17,18,19, 20,21,22,23}; -// 5 rows each provide a current sink to a block of 16 switches (when set to output low) -u_int8_t rows[5] = { 4,5,6,7,8 }; -// 16 column pins in the MCP23017 provide a current sink to light up one of 16 LEDs (when set to output low) - - -// helper functions: 1 -- gpio -// -// map GPIO into virtual memory space ------------------------ -int map_peripheral(struct bcm2835_peripheral *p) -{ - if ((p->mem_fd = open("/dev/mem", O_RDWR | O_SYNC)) < 0) { - printf("Failed to open /dev/mem, try checking permissions.\n"); - return -1; - } - p->map = mmap(NULL, BLOCK_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, - p->mem_fd, // File descriptor to physical memory virtual file '/dev/mem' - p->addr_p); // Address in physical map that we want this memory block to expose - if (p->map == MAP_FAILED) { - perror("mmap"); - return -1; - } - p->addr = (volatile unsigned int *) p->map; - return 0; -} - -void unmap_peripheral(struct bcm2835_peripheral *p) -{ - munmap(p->map, BLOCK_SIZE); - close(p->mem_fd); -} - -// helper functions: 2 -- parallel threads for GPIO mux and LED brightness averaging -// -void *blink(void *ptr); // the real-time GPIO multiplexing process to start up -void *gpiopattern_update_leds(void *ptr); // the averaging thread -void gpio_mux_thread_start(void); +void *blink(void *ptr); /* the real-time GPIO multiplexing process to start up */ pthread_t blink_thread; int blink_thread_terminate = 0; -void gpio_mux_thread_start() +t_stat gpio_mux_thread_start() { int res; res = pthread_create(&blink_thread, NULL, blink, &blink_thread_terminate); if (res) { - fprintf(stderr, "Error creating gpio_mux thread, return code %d\n", res); - exit(EXIT_FAILURE); + return sim_messagef(SCPE_IERR, + "Error creating gpio_mux thread, return code %d\n", res); } - printf("Created blink_thread\n"); - sleep(2); // allow 2 sec for multiplex to start + sim_messagef(SCPE_OK, "Created blink_thread\n"); + sleep(2); /* allow 2 sec for multiplex to start */ + return SCPE_OK; } -// 3 - set MCP23017 'column' pins to input with pullup -static void mcp23017_to_input(void) -{ - // ----- set to 16 bits of input ----- - buffer[0] = 0x00; - buffer[1] = 0xff; - buffer[2] = 0xff; - if (write(file_i2c, buffer, 3) != 3) - printf("Failed to write to the i2c bus.\n"); - - // ----- enable pullups ----- - buffer[0] = 0x0c; - buffer[1] = 0xff; - buffer[2] = 0xff; - if (write(file_i2c, buffer, 3) != 3) - printf("Failed to write to the i2c bus.\n"); -} - -static void mcp23017_to_output(void) -{ - // ---- set to 16 bits of output ----- - buffer[0] = 0x00; - buffer[1] = 0x00; - buffer[2] = 0x00; - if (write(file_i2c, buffer, 3) != 3) - printf("Failed to write to the i2c bus.\n"); -} - /* * Debounce a momentary switch. */ @@ -377,217 +214,156 @@ static void debounce_sw(int state, int sw) } } +static t_addr +read_sw() +{ + int col, row, i; + t_uint64 sw; + t_addr new_as; + struct timespec spec; + + gpio_set_drive(xIO, DRIVE_HIGH); + for (i = 0; i < 18; i++) { + gpio_set_dir(cols[i], DIR_INPUT); + } + + spec.tv_sec = 0; + new_as = 0; + for (row=0; row<5; row++) { + /* Select row address */ + for (i = 0; i < 3; i++) { + if ((row & (1 << i)) == 0) { + gpio_set_drive(xrows[i], DRIVE_LOW); + } else { + gpio_set_drive(xrows[i], DRIVE_HIGH); + } + } + spec.tv_nsec = intervl/10; + nanosleep(&spec, NULL); + sw = 0; + for (i = 0; i < 18; i++) { + if (gpio_get_level(cols[i])) { + sw |= 1 << i; + } + } + /* Map row to values */ + switch (row) { + default: + case 0: + SW = (SW & SR_MASK1) | ((~sw << SR_V_0) & SR_MASK0); + break; + + case 1: + SW = (SW & SR_MASK0) | ((~sw << SR_V_1) & SR_MASK1); + break; + + case 2: + new_as = (t_addr)(~sw << MA_SW_V_3) & MA_SW_MASK3; + break; + + case 3: /* Momentary switches */ + for (col = 0; col < 10; col++) { + int state = (sw & (1 << col)) == 0; + debounce_sw(state, col); + } + break; + + case 4: +#if KA | KI + adr_cond = 0; + adr_cond |= ((sw & INST_FETCH) == 0) ? ADR_IFETCH : 0; + adr_cond |= ((sw & DATA_FETCH) == 0) ? ADR_DFETCH : 0; + adr_cond |= ((sw & WRITE_SW) == 0) ? ADR_WRITE : 0; + adr_cond |= ((sw & ADR_STOP_SW) == 0) ? ADR_STOP : 0; + adr_cond |= ((sw & ADR_BRK_SW) == 0) ? ADR_BREAK : 0; + nxm_stop = (sw & NXM_STOP) == 0; +#endif + sing_inst_sw = (sw & SING_INST) == 0; + /* PAR_STOP handle special features */ + par_stop = (sw & PAR_STOP) == 0; + /* SING_CYCL no function yet */ + repeat_sw = (sw & REP_SW) == 0; + break; + } + } + return new_as; +} + void *blink(void *ptr) { - int *terminate = (int *)ptr; - int ledrow, row, i, col; - uint16_t leds; - uint16_t sw; - uint64 new_sw; - t_addr new_as; - - // set thread to real time priority ----------------- + int *terminate = (int *)ptr; + int col, row, i; + int num_gpios, ret; + uint32 leds; + t_addr new_as; + struct timespec spec; struct sched_param sp; + + spec.tv_sec = 0; sp.sched_priority = 98; // maybe 99, 32, 31? if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &sp)) fprintf(stderr, "warning: failed to set RT priority\n"); - //----- OPEN THE I2C BUS ----- - char *filename = (char*)"/dev/i2c-1"; - if ((file_i2c = open(filename, O_RDWR)) < 0) { - printf("Failed to open the i2c bus"); - exit(1); - } - if (ioctl(file_i2c, I2C_SLAVE, 0x20) < 0) { - printf("Failed bus access and/or talk to slave.\n"); - exit(1); - } - printf("OK: access to MCP\r\n"); - - - // Find gpio address (different for Pi 1,2 and 4) ---------- - gpio.addr_p = bcm_host_get_peripheral_address() + +0x200000; - if (gpio.addr_p == 0x20200000) printf("*** RPi Plus detected\n"); - else if (gpio.addr_p == 0x3f000000) printf("*** RPi 2/3/Z detected\n"); - else if (gpio.addr_p == 0xfe200000) printf("*** RPi 4 detected\n"); - - // map GPIO into virtual memory space ------------------------ - if (map_peripheral(&gpio) == -1) { - printf("Failed to map the physical GPIO registers into the virtual memory space.\n"); - return (void *) -1; + ret = gpiolib_init(); + if (ret < 0) { + sim_messagef(SCPE_IERR, "Unable to initialize gpiolib- %d\n", ret); + return (void *)-1; } - // initialise GPIO (all pins used as inputs, with pull-ups enabled on cols) - for (ledrow = 0; ledrow < 8; ledrow++) { // Define ledrows as input - INP_GPIO(ledrows[ledrow]); - GPIO_CLR = 1 << ledrows[ledrow]; // so go to Low when switched to output + num_gpios = ret; + + if (num_gpios == 0) { + sim_messagef(SCPE_IERR, "No GPIO chips found\n"); + return (void *)-1; } - mcp23017_to_input(); // Define cols as input with pullups - // initialise GPIO pullups. Different for Pi 4 and older Pi's - if (gpio.addr_p==0xfe200000) - { - //printf("Configuring pullups for Pi 4\r\n"); - /* https://github.com/RPi-Distro/raspi-gpio/blob/master/raspi-gpio.c */ - /* 2711 has a different mechanism for pin pull-up/down/enable */ - int gpiox; - int pullreg; - int pullshift; - unsigned int pullbits; - unsigned int pull; - - // GPIO column pins - - // GPIO column pins - // -- already done during setup - - // GPIO row pins - for (i=0;i<5;i++) { - gpiox = rows[i]; - pullreg = GPPUPPDN0 + (gpiox>>4); - pullshift = (gpiox & 0xf) << 1; - pull = 0; // pullup - - pullbits = *(gpio.addr + pullreg); - pullbits &= ~(3 << pullshift); - pullbits |= (pull << pullshift); - *(gpio.addr + pullreg) = pullbits; - } - // GPIO ledrow pins - for (i=0;i<8;i++) { - gpiox = ledrows[i]; - pullreg = GPPUPPDN0 + (gpiox>>4); - pullshift = (gpiox & 0xf) << 1; - pull = 0; // pullup - - pullbits = *(gpio.addr + pullreg); - pullbits &= ~(3 << pullshift); - pullbits |= (pull << pullshift); - *(gpio.addr + pullreg) = pullbits; - } + ret = gpiolib_mmap(); + if (ret) { + if (ret == EACCES && geteuid()) { + sim_messagef(SCPE_IERR, "Must be root\n"); + } else { + sim_perror("Failed to mmap gpiolib"); } - else // configure pullups for older Pis - { -/* // BCM2835 ARM Peripherals PDF p 101 & elinux.org/RPi_Low-level_peripherals#Internal_Pull-Ups_.26_Pull-Downs - GPIO_PULL = 2; // pull-up - short_wait(); // must wait 150 cycles - GPIO_PULLCLK0 = 0x0c003ff0; // selects GPIO pins 4..13 and 26,27 - - short_wait(); - GPIO_PULL = 0; // reset GPPUD register - short_wait(); - GPIO_PULLCLK0 = 0; // remove clock - short_wait(); // probably unnecessary -*/ - // BCM2835 ARM Peripherals PDF p 101 & elinux.org/RPi_Low-level_peripherals#Internal_Pull-Ups_.26_Pull-Downs - GPIO_PULL = 0; // no pull-up no pull-down just float - short_wait(); // must wait 150 cycles - //GPIO_PULLCLK0 = 0x03f00000; // selects GPIO pins 20..25 - GPIO_PULLCLK0 = 0x0ff0000; // selects GPIO pins 16..23 (ledrow) - short_wait(); - GPIO_PULL = 0; // reset GPPUD register - short_wait(); - GPIO_PULLCLK0 = 0; // remove clock - short_wait(); // probably unnecessary - - // BCM2835 ARM Peripherals PDF p 101 & elinux.org/RPi_Low-level_peripherals#Internal_Pull-Ups_.26_Pull-Downs - GPIO_PULL = 0; // no pull-up no pull down just float - short_wait(); // must wait 150 cycles - //GPIO_PULLCLK0 = 0x070000; // selects GPIO pins 16..18 - GPIO_PULLCLK0 = 0x01f0; // selects GPIO pins 4..8 (row) - short_wait(); - GPIO_PULL = 0; // reset GPPUD register - short_wait(); - GPIO_PULLCLK0 = 0; // remove clock - short_wait(); // probably unnecessary + return (void *)-1; } - /* -------------------------------------------------- */ - /* Read in initial switch status */ - mcp23017_to_input(); + /* Initialize GPIO pins */ + gpio_set_fsel(xIO, GPIO_FSEL_OUTPUT); + gpio_set_dir(xIO, DIR_OUTPUT); + gpio_set_drive(xIO, DRIVE_HIGH); - new_sw = 0; - new_as = 0; - for (row=0; row<5; row++) { /* there are 5 rows of 16 switches each */ - /* ----- for this row, output 0V to overrule built-in pull-up */ - /* from column input pin ----- */ - INO_GPIO(rows[row]); - OUT_GPIO(rows[row]); - GPIO_CLR = 1 << rows[row]; - - /* ----- read switches (request MCP23017 gpio values) ----- */ - buffer[0] = 0x12; - if (write(file_i2c, buffer, 1) != 1) - printf("Failed i2c write (3)\n"); - if (read(file_i2c, buffer, 2) != 2) - printf("Failed i2c read (3)\n"); - else { - sw = (buffer[1] << 8) + buffer[0]; - switch (row) { - default: - case 0: - new_sw |= (((uint64)sw) << SR_V0) | SR_MASK_0; - break; - case 1: - new_sw |= (((uint64)sw) << SR_V1) | SR_MASK_1; - break; - case 2: - new_sw |= (((uint64)sw) << SR_V2) | SR_MASK_2; -#if KA | KI - adr_cond = sw & (INST_FETCH|DATA_FETCH|WRITE_SW|ADR_STOP_SW|ADR_BRK_SW); - nxm_stop = (sw & NXM_STOP) != 0; -#endif - sing_inst_sw = (sw & SING_INST) != 0; - /* PAR_STOP handle special features */ - /* SING_CYCL no function yet */ - for (col = 10; col < 12; col++) { - int state = (sw & (1 << col)) != 0; - switch_state[col].last_state = state; - switch_state[col].state = state; - switch_state[col].debounce = switch_state[col].changed = 0; - } - repeat_sw = (sw & REP_SW) != 0; - break; - case 3: - new_as |= (((t_addr)sw) << MA_SW_V3) | MA_SW_MASK_3; - break; - case 4: - new_as |= (((t_addr)sw) << MA_SW_V4) | MA_SW_MASK_4; - for (col = 0; col < 8; col++) { - int state = (sw & (1 << col)) != 0; - switch_state[col].last_state = state; - switch_state[col].state = state; - switch_state[col].debounce = switch_state[col].changed = 0; - } - break; - } - SW = new_sw; - AS = new_as; - // stop sinking current from this row of switches - INP_GPIO(rows[row]); - } - // stop sinking current from this row of switches - INP_GPIO(rows[row]); + /* Row address pins */ + for (row = 0; row < 3; row++) { + gpio_set_fsel(xrows[row], GPIO_FSEL_OUTPUT); + gpio_set_dir(xrows[row], DIR_OUTPUT); + gpio_set_drive(xrows[row], DRIVE_HIGH); } - SW = new_sw; - AS = new_as; - printf("\nPiDP-10 FP on\n"); + /* Columns */ + for (col = 0; col < 10; col++) { + gpio_set_fsel(cols[col], GPIO_FSEL_INPUT); + gpio_set_pull(cols[col], PULL_UP); + } + + /* Read initial value of switches */ + new_as = read_sw(); + sim_messagef(SCPE_OK, "PiDP-10 FP on\n"); - // start the actual multiplexing + /* start the actual multiplexing */ while (*terminate == 0) { - // ---- set to 16 bits of output ----- - buffer[0] = 0x00; - buffer[1] = 0x00; - buffer[2] = 0x00; - if (write(file_i2c, buffer, 3) != 3) - printf("Failed i2c write (1).\n"); + /* Set to point to switchs while updating digits */ + gpio_set_drive(xIO, DRIVE_HIGH); + /* Set direction of columns to output */ + for (i = 0; i < 18; i++) { + gpio_set_dir(cols[i], DIR_OUTPUT); + } - for (ledrow=0; ledrow<8; ledrow++) { /* 8 rows of LEDS get lit */ - switch (ledrow) { + /* Display each row */ + for (row=0; row<7; row++) { /* 7 rows of LEDS get lit */ + switch (row) { default: case 0: leds = (((MI_flag)? MI : MB) & MB_MASK0) >> MB_V_0; @@ -596,185 +372,116 @@ void *blink(void *ptr) leds = (((MI_flag)? MI : MB) & MB_MASK1) >> MB_V_1; break; case 2: - leds = PWR_LAMP; - leds |= (RUN) ? RUN_LAMP : 0; - leds |= (pi_enable) ? PION_LAMP : 0; - leds |= (PIE & PI_ENB_MASK); - leds |= (((MI_flag)? MI : MB) & MB_MASK2) << MB_V_2; - leds |= (MI_flag) ? PI_LAMP : MI_LAMP; + if (par_stop) { + leds = rdrin_dev & 0777; + leds |= rep_rate << 12; + leds |= MI_disable << 10; + } else { + leds = (AB & AB_MASK2) >> AB_V_2; + } break; case 3: leds = (IR & IR_MASK3) << IR_V_3; leds |= (AC & AC_MASK3) << AC_V_3; leds |= (IND) ? IND_LAMP : 0; - leds |= (IX & IX_MASK3) >> IX_V_3; + leds |= (IX & IX_MASK3) << IX_V_3; break; case 4: - leds = (IX & IX_MASK4) << IX_V_4; - if (par_stop) { - leds |= (rdrin_dev & MA_MASK4) >> MA_V_4; - leds |= rep_rate << 12; - leds |= MI_disable << 10; - } else { - leds |= (AB & MA_MASK4) >> MA_V_4; - } + leds = (PC & PC_MASK4) >> PC_V_4; break; case 5: - if (par_stop) { - leds = (rdrin_dev & MA_MASK5) << MA_V_5; - } else { - leds = (AB & MA_MASK5) << MA_V_5; - } + leds = PWR_LAMP; + leds |= (IOB_PI & PI_IOB_MASK5) << PI_IOB_V_5; + leds |= (PIE & PI_ENB_MASK5) << PI_ENB_V_5; leds |= (FLAGS & USER) ? USER_LAMP : 0; - leds |= (PIR & PI_REQ_MASK); leds |= (prog_stop) ? PROG_STOP_LAMP: 0; leds |= (watch_stop) ? MEM_STOP_LAMP: 0; break; case 6: - leds = (PC & PC_MASK6) >> PC_V_6; - break; - - case 7: - leds = (PC & PC_MASK7) << PC_V_7; - leds |= (PIH & PI_PRO_MASK7); - leds |= (IOB_PI & PI_IOB_MASK7) << PI_IOB_V_7; + leds = (RUN) ? RUN_LAMP : 0; + leds |= (pi_enable) ? PION_LAMP : 0; + leds |= (PIR & PI_REQ_MASK6) << PI_REQ_V_6; + leds |= (PIH & PI_PRO_MASK6) << PI_PRO_V_6; + leds |= (MI_flag) ? PI_LAMP : MI_LAMP; break; } - leds = ~leds; - - /* ----- set MCP23017 IO pin values */ - /* (determines which of the 16 LEDs will light up) ----- */ - buffer[0] = 0x14; - buffer[2] = leds >> 8; - buffer[1] = leds & 0x00ff; - if (write(file_i2c, buffer, 3) != 3) - printf("Failed i2c write (2)\n"); - - /* ----- set ledrow pin to high (switch on the power) ----- */ - INO_GPIO(ledrows[ledrow]); - GPIO_SET = 1 << ledrows[ledrow]; /* could be done once... TODO */ - OUT_GPIO(ledrows[ledrow]); - - /* ----- now wait a while with LEDs burning ----- */ - usleep(250); - - /* ----- Toggle ledrow off (cut the power) ----- */ - GPIO_CLR = 1 << ledrows[ledrow]; /* superfluous given next line */ - INP_GPIO(ledrows[ledrow]); - } - - /* ----- prepare for reading the switches */ - /* note: INP_GPIO has been done already for the ledrows, */ - /* so they are inputs already, nothing to be done */ - - /* ----- set MCP23017 IO pins to input with pullups enabled ----- */ - mcp23017_to_input(); - - new_sw = 0; - new_as = 0; - for (row=0; row<5; row++) { /* there are 5 rows of 16 switches each */ - /* ----- for this row, output 0V to overrule built-in pull-up */ - /* from column input pin ----- */ - INO_GPIO(rows[row]); - OUT_GPIO(rows[row]); - GPIO_CLR = 1 << rows[row]; - - /* ----- read switches (request MCP23017 gpio values) ----- */ - buffer[0] = 0x12; - if (write(file_i2c, buffer, 1) != 1) - printf("Failed i2c write (3)\n"); - if (read(file_i2c, buffer, 2) != 2) - printf("Failed i2c read (3)\n"); - else { - sw = (buffer[1] << 8) + buffer[0]; - switch (row) { - default: - case 0: - new_sw |= (((uint64)sw) << SR_V0) & SR_MASK_0; - break; - case 1: - new_sw |= (((uint64)sw) << SR_V1) & SR_MASK_1; - break; - case 2: - new_sw |= (((uint64)sw) << SR_V2) & SR_MASK_2; -#if KA | KI - adr_cond = sw & (INST_FETCH|DATA_FETCH|WRITE_SW| - ADR_STOP_SW|ADR_BRK_SW); - nxm_stop = (sw & NXM_STOP) != 0; -#endif - sing_inst_sw = (sw & SING_INST) != 0; - /* PAR_STOP handle special features */ - par_stop = (sw & PAR_STOP) != 0; - /* SING_CYCL no function yet */ - for (col = 10; col < 12; col++) { - int state = (sw & (1 << col)) != 0; - debounce_sw(state, col); - } - repeat_sw = (sw & REP_SW) != 0; - break; - case 3: - new_as |= (((t_addr)sw) << MA_SW_V3) & MA_SW_MASK_3; - break; - case 4: - new_as |= (((t_addr)sw) << MA_SW_V4) & MA_SW_MASK_4; - for (col = 0; col < 8; col++) { - int state = (sw & (1 << col)) != 0; - debounce_sw(state, col); - } - break; + /* Select correct row to display */ + for (i = 0; i < 3; i++) { + if ((row & (1 << i)) == 0) { + gpio_set_drive(xrows[i], DRIVE_LOW); + } else { + gpio_set_drive(xrows[i], DRIVE_HIGH); } } - /* stop sinking current from this row of switches */ - INP_GPIO(rows[row]); - } - SW = new_sw; - - /* If running, check for switch changes */ - if (par_stop) { - for (col = 0; col < 12; col++) { - if (switch_state[col].changed && switch_state[col].state) { - switch_state[col].changed = 0; - switch (col) { -#if KA | KI - case 7: /* ReadIN */ - rdrin_dev = 0774 & new_as; - break; -#endif - - case 5: /* Continue */ - MI_disable = !MI_disable; - if (MI_disable) - MI_flag = 0; - break; - - case 4: /* Stop function */ - stop_sw = 1; - pwr_off = 1; - break; - - case 1: /* Examine this */ - rep_rate = (new_as >> 14) & 0xf; - break; - - case 0: /* Examine next */ - case 2: /* Execute function */ - case 3: /* Reset function */ - case 6: /* Start */ - case 10: /* Deposit next */ - case 11: /* Deposit this */ - default: - break; - } + /* Update the leds for output */ + for (i = 0; i < 18; i++) { + if ((leds & (1 << i)) == 0) { + gpio_set_drive(cols[i], DRIVE_HIGH); + } else { + gpio_set_drive(cols[i], DRIVE_LOW); } } + + /* Select output */ + gpio_set_drive(xIO, DRIVE_LOW); + spec.tv_nsec = intervl; + nanosleep(&spec, NULL); + /* Deselect output */ + gpio_set_drive(xIO, DRIVE_HIGH); + spec.tv_nsec = intervl/10; + nanosleep(&spec, NULL); + } + + /* Read in switches */ + new_as = read_sw(); + + /* If running, check for switch changes */ + if (par_stop) { + /* Process all momentary switches */ + for (col = 0; col < 10; col++) { + if (switch_state[col].changed && switch_state[col].state) { + switch_state[col].changed = 0; + switch (col) { + case 1: /* Examine this */ + rep_rate = (new_as >> 14) & 0xf; + break; + + case 4: /* Stop function */ + stop_sw = 1; + pwr_off = 1; + break; + + case 5: /* Continue */ + MI_disable = !MI_disable; + if (MI_disable) + MI_flag = 0; + break; + +#if KA | KI + case 7: /* ReadIN */ + rdrin_dev = 0774 & new_as; + break; +#endif + + case 0: /* Examine next */ + case 2: /* Execute function */ + case 3: /* Reset function */ + case 6: /* Start */ + case 8: /* Deposit next */ + case 9: /* Deposit this */ + default: + break; + } + } + } } else { - AS = new_as; + AS = new_as; } /* Check repeat count */ if (rep_count > 0 && --rep_count == 0) { @@ -782,39 +489,45 @@ void *blink(void *ptr) switch_state[col].changed = switch_state[col].state; } } + /* Process switch changes if running */ if (RUN) { - for (col = 0; col < 12; col++) { + for (col = 0; col < 10; col++) { if (switch_state[col].changed && switch_state[col].state) { /* If repeat switch set, trigger timer */ if (repeat_sw) { rep_count = (rep_rate + 1) * 16; } switch (col) { - case 1: /* Examine this */ + case 1: /* Examine this */ examine_sw = 1; MI_flag = 0; switch_state[col].changed = 0; break; - case 0: /* Examine next */ - case 5: /* Continue */ - case 6: /* Start */ - case 7: /* ReadIN */ - case 10: /* Deposit next */ + + case 0: /* Examine next */ + case 5: /* Continue */ + case 6: /* Start */ + case 7: /* ReadIN */ + case 8: /* Deposit next */ default: switch_state[col].changed = 0; break; - case 2: /* Execute function */ + + case 2: /* Execute function */ xct_sw = 1; switch_state[col].changed = 0; break; - case 3: /* Reset function */ + + case 3: /* Reset function */ stop_sw = 1; break; - case 4: /* Stop function */ + + case 4: /* Stop function */ stop_sw = 1; switch_state[col].changed = 0; break; - case 11: /* Deposit this */ + + case 9: /* Deposit this */ deposit_sw = 1; MI_flag = 0; switch_state[col].changed = 0; @@ -823,10 +536,21 @@ void *blink(void *ptr) } } } - // done with reading the switches, so start the next cycle of lighting up LEDs + /* done with reading the switches, + * so start the next cycle of lighting up LEDs + */ } /* received terminate signal, close down */ + gpio_set_drive(xIO, DRIVE_HIGH); + for (i = 0; i < 18; i++) { + gpio_set_dir(cols[i], DIR_INPUT); + } + + for (row = 0; row < 3; row++) { + gpio_set_drive(xrows[row], DRIVE_HIGH); + } + } volatile int input_wait; @@ -835,11 +559,11 @@ static char *input_buffer; /* * Handler for EditLine package when line is complete. */ -static void read_line_handler(char *line) +static void +read_line_handler(char *line) { if (line != NULL) { input_buffer = line; - printf("Got: %s\n", line); input_wait = 0; } } @@ -877,7 +601,7 @@ vm_read(char *cptr, int32 sz, FILE *file) } /* Process switches */ - for (col = 0; col < 12; col++) { + for (col = 0; col < 10; col++) { if (switch_state[col].changed && switch_state[col].state) { /* If repeat switch set, trigger timer */ if (repeat_sw) { @@ -892,9 +616,9 @@ vm_read(char *cptr, int32 sz, FILE *file) case 1: /* Examine this */ AB = AS; - MB = (AB < 020) ? FM[AB] : M[AB]; - MB = M[AB]; + MB = (AS < 020) ? FM[AS] : M[AS]; MI_flag = 0; + printf("Examime %06o %012llo\r\n", AS, SW); break; case 2: /* Execute function */ @@ -962,7 +686,7 @@ vm_read(char *cptr, int32 sz, FILE *file) break; #endif - case 10: /* Deposit next */ + case 8: /* Deposit next */ AB++; if (AB < 020) { FM[AB] = SW; @@ -974,14 +698,15 @@ vm_read(char *cptr, int32 sz, FILE *file) MI_flag = 0; break; - case 11: /* Deposit this */ + case 9: /* Deposit this */ AB = AS; - if (AB < 020) { - FM[AB] = SW; - MB = FM[AB]; + printf("Deposit %06o %012llo\r\n", AS, SW); + if (AS < 020) { + FM[AS] = SW; + MB = FM[AS]; } else { - M[AB] = SW; - MB = M[AB]; + M[AS] = SW; + MB = M[AS]; } MI_flag = 0; break; @@ -1002,16 +727,18 @@ vm_post(t_bool from_scp) /* * Start panel thread, and install console read functions. */ -void pi_panel_start(void) +t_stat pi_panel_start(void) { - int terminate = 1; - int i,j; + int terminate = 1; + int i,j; + t_stat r; /* start up multiplexing thread */ - gpio_mux_thread_start(); + r = gpio_mux_thread_start(); sim_vm_read = &vm_read; sim_vm_post = &vm_post; rl_callback_handler_install("", (rl_vcpfunc_t*) &read_line_handler); + return r; } /* diff --git a/PDP10/kx10_cpu.c b/PDP10/kx10_cpu.c index 85e787b..e13b276 100644 --- a/PDP10/kx10_cpu.c +++ b/PDP10/kx10_cpu.c @@ -4484,13 +4484,13 @@ if ((reason = build_dev_tab ()) != SCPE_OK) /* build, chk dib_tab */ #if PIDP10 if (examine_sw) { /* Examine memory switch */ AB = AS; - (void)Mem_read(1, 0, 0, 0); + (void)Mem_read_nopage(); examine_sw = 0; } if (deposit_sw) { /* Deposit memory switch */ AB = AS; MB = SW; - (void)Mem_write(1, 0); + (void)Mem_write_nopage(); deposit_sw = 0; } if (xct_sw) { /* Handle Front panel xct switch */ @@ -4505,6 +4505,7 @@ if ((reason = build_dev_tab ()) != SCPE_OK) /* build, chk dib_tab */ if (stop_sw) { /* Stop switch set */ RUN = 0; stop_sw = 0; + reason = STOP_HALT; } if (sing_inst_sw) { /* Handle Front panel single instruction */ instr_count = 1; @@ -12261,6 +12262,7 @@ last: if (QITS) load_quantum(); #endif + RUN = 0; return SCPE_STEP; } } @@ -13573,13 +13575,17 @@ static const char *pdp10_clock_precalibrate_commands[] = { t_stat cpu_reset (DEVICE *dptr) { - int i; - static int initialized = 0; + int i; + t_stat r = SCPE_OK; + static int initialized = 0; if (!initialized) { initialized = 1; #if PIDP10 - pi_panel_start(); + r = pi_panel_start(); + if (r != SCPE_OK) { + return r; + } #endif } sim_debug(DEBUG_CONO, dptr, "CPU reset\n"); @@ -13654,7 +13660,7 @@ t_stat cpu_reset (DEVICE *dptr) #endif sim_vm_interval_units = "cycles"; sim_vm_step_unit = "instruction"; - return SCPE_OK; + return r; } /* Memory examine */ diff --git a/PDP10/kx10_defs.h b/PDP10/kx10_defs.h index 656cc78..0414303 100644 --- a/PDP10/kx10_defs.h +++ b/PDP10/kx10_defs.h @@ -857,7 +857,7 @@ extern uint32 dd_keyboard_line (void *); #endif #if PIDP10 -void pi_panel_start(); +t_stat pi_panel_start(); void pi_panel_stop(); #endif