From ea3b2a25abc093ba946068e62e8fe7b52a96ff2b Mon Sep 17 00:00:00 2001 From: kautism Date: Fri, 2 May 2025 00:57:23 +0800 Subject: [PATCH] Add Lgt8fx board support --- Makefile | 5 +- src/Kconfig | 5 +- src/lgt/Kconfig | 80 +++++++++++++++++ src/lgt/Makefile | 39 ++++++++ src/lgt/adc.c | 102 +++++++++++++++++++++ src/lgt/gpio.c | 99 +++++++++++++++++++++ src/lgt/gpio.h | 60 +++++++++++++ src/lgt/hard_pwm.c | 103 +++++++++++++++++++++ src/lgt/i2c.c | 118 ++++++++++++++++++++++++ src/lgt/internal.h | 18 ++++ src/lgt/irq.h | 38 ++++++++ src/lgt/main.c | 91 +++++++++++++++++++ src/lgt/pgm.h | 27 ++++++ src/lgt/pindefine.h | 7 ++ src/lgt/serial.c | 42 +++++++++ src/lgt/spi.c | 94 ++++++++++++++++++++ src/lgt/timer.c | 212 ++++++++++++++++++++++++++++++++++++++++++++ src/lgt/watchdog.c | 58 ++++++++++++ 18 files changed, 1196 insertions(+), 2 deletions(-) create mode 100644 src/lgt/Kconfig create mode 100644 src/lgt/Makefile create mode 100644 src/lgt/adc.c create mode 100644 src/lgt/gpio.c create mode 100644 src/lgt/gpio.h create mode 100644 src/lgt/hard_pwm.c create mode 100644 src/lgt/i2c.c create mode 100644 src/lgt/internal.h create mode 100644 src/lgt/irq.h create mode 100644 src/lgt/main.c create mode 100644 src/lgt/pgm.h create mode 100644 src/lgt/pindefine.h create mode 100644 src/lgt/serial.c create mode 100644 src/lgt/spi.c create mode 100644 src/lgt/timer.c create mode 100644 src/lgt/watchdog.c diff --git a/Makefile b/Makefile index 735d18633..532f0ccf6 100644 --- a/Makefile +++ b/Makefile @@ -39,6 +39,9 @@ OBJS_klipper.elf = $(patsubst %.c, $(OUT)src/%.o,$(src-y)) OBJS_klipper.elf += $(OUT)compile_time_request.o CFLAGS_klipper.elf = $(CFLAGS) -Wl,--gc-sections +# fix issue 4938 +LDFLAGS += -Wl,--defsym,__DATA_REGION_LENGTH__=0x1000 + CPPFLAGS = -I$(OUT) -P -MD -MT $@ # Default targets @@ -70,7 +73,7 @@ $(OUT)%.ld: %.lds.S $(OUT)autoconf.h $(OUT)klipper.elf: $(OBJS_klipper.elf) @echo " Linking $@" - $(Q)$(CC) $(OBJS_klipper.elf) $(CFLAGS_klipper.elf) -o $@ + $(Q)$(CC) $(LDFLAGS) $(OBJS_klipper.elf) $(CFLAGS_klipper.elf) -o $@ $(Q)scripts/check-gcc.sh $@ $(OUT)compile_time_request.o ################ Compile time requests diff --git a/src/Kconfig b/src/Kconfig index 05b9cb427..c700cd16c 100644 --- a/src/Kconfig +++ b/src/Kconfig @@ -12,6 +12,8 @@ choice prompt "Micro-controller Architecture" config MACH_AVR bool "Atmega AVR" + config MACH_LGT + bool "LogicGreen" config MACH_ATSAM bool "SAM3/SAM4/SAM E70" config MACH_ATSAMD @@ -35,6 +37,7 @@ choice endchoice source "src/avr/Kconfig" +source "src/lgt/Kconfig" source "src/atsam/Kconfig" source "src/atsamd/Kconfig" source "src/lpc176x/Kconfig" @@ -50,7 +53,7 @@ source "src/simulator/Kconfig" config SERIAL bool config SERIAL_BAUD - depends on SERIAL + depends on SERIAL && !MACH_LGT int "Baud rate for serial port" if LOW_LEVEL_OPTIONS default 250000 help diff --git a/src/lgt/Kconfig b/src/lgt/Kconfig new file mode 100644 index 000000000..2025ee5e8 --- /dev/null +++ b/src/lgt/Kconfig @@ -0,0 +1,80 @@ +# Kconfig settings for LogicGreen processors + +if MACH_LGT + +config LGT_SELECT + bool + default y + select HAVE_GPIO + select HAVE_GPIO_ADC + select HAVE_GPIO_SPI + select HAVE_GPIO_I2C + select HAVE_GPIO_HARD_PWM + select HAVE_STRICT_TIMING + select HAVE_LIMITED_CODE_SIZE + +config BOARD_DIRECTORY + string + default "lgt" + +choice + prompt "Processor model" + config MACH_lgtf328p + bool "lgtf328p" +endchoice + +config MCU + string + default "lgtf328p" if MACH_lgtf328p + +config AVRDUDE_PROTOCOL + string + default "arduino" + +choice + prompt "Processor speed" + config LGT_FREQ_32000000 + bool "32Mhz" + config LGT_FREQ_16000000 + bool "16Mhz" +endchoice + +config CLOCK_FREQ + int + default 32000000 if LGT_FREQ_32000000 + default 16000000 + +config LGT_STACK_SIZE + int + default 256 + +config LGT_WATCHDOG + bool + default y + +config SERIAL + bool + default y + +config SERIAL_BAUD_U2X + depends on SERIAL + bool + default y + +choice + prompt "Baud rate for serial port" + config LGT_SERIAL_BAUD_115200 + bool "115200" + config LGT_SERIAL_BAUD_250000 + bool "250000" +endchoice + +config SERIAL_BAUD + int + default 250000 if LGT_SERIAL_BAUD_250000 + default 115200 + +config SERIAL_PORT + int + default 0 +endif \ No newline at end of file diff --git a/src/lgt/Makefile b/src/lgt/Makefile new file mode 100644 index 000000000..bc66e8661 --- /dev/null +++ b/src/lgt/Makefile @@ -0,0 +1,39 @@ +# Additional lgt build rules + +# Use the lgt toolchain +CROSS_PREFIX=avr- + +# if you have a custom bootloader, do not forget to change this +#UPLOAD_BAUD=57600 +UPLOAD_BAUD=115200 + +dirs-y += src/lgt src/generic + +CFLAGS-$(CONFIG_HAVE_LIMITED_CODE_SIZE) += -Os +CFLAGS += $(CFLAGS-y) -mmcu=atmega328p + + +# Add lgt source files +src-y += lgt/main.c lgt/timer.c +src-$(CONFIG_HAVE_GPIO) += lgt/gpio.c +src-$(CONFIG_WANT_ADC) += lgt/adc.c +src-$(CONFIG_WANT_SPI) += lgt/spi.c +src-$(CONFIG_WANT_I2C) += lgt/i2c.c +src-$(CONFIG_WANT_HARD_PWM) += lgt/hard_pwm.c +src-$(CONFIG_LGT_WATCHDOG) += lgt/watchdog.c +src-$(CONFIG_SERIAL) += lgt/serial.c generic/serial_irq.c + +# Suppress broken "misspelled signal handler" warnings on gcc 4.8.1 +CFLAGS_klipper.elf := $(CFLAGS_klipper.elf) $(if $(filter 4.8.1, $(shell $(CC) -dumpversion)), -w) + +# Build the additional hex output file +target-y += $(OUT)klipper.elf.hex + +$(OUT)klipper.elf.hex: $(OUT)klipper.elf + @echo " Creating hex file $@" + $(Q)$(OBJCOPY) -j .text -j .data -O ihex $< $@ + +flash: $(OUT)klipper.elf.hex + @echo " Flashing $< to $(FLASH_DEVICE) via avrdude" + $(Q)if [ -z $(FLASH_DEVICE) ]; then echo "Please specify FLASH_DEVICE"; exit 1; fi + $(Q)avrdude -pm328p -b$(UPLOAD_BAUD) -c$(CONFIG_AVRDUDE_PROTOCOL) -P"$(FLASH_DEVICE)" -D -U"flash:w:$(<):i" diff --git a/src/lgt/adc.c b/src/lgt/adc.c new file mode 100644 index 000000000..b69280574 --- /dev/null +++ b/src/lgt/adc.c @@ -0,0 +1,102 @@ +// Analog to Digital Converter support +// +// Copyright (C) 2016-2018 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "autoconf.h" // CONFIG_MACH_atmega644p +#include "command.h" // shutdown +#include "gpio.h" // gpio_adc_read +#include "internal.h" // GPIO +#include "pgm.h" // PROGMEM +#include "sched.h" // sched_shutdown + +static const uint8_t adc_pins[] PROGMEM = { + GPIO('B', 5), GPIO('C', 0), GPIO('C', 1), GPIO('C', 2), + GPIO('C', 3), GPIO('C', 4), GPIO('C', 5), GPIO('D', 4), + GPIO('D', 6), GPIO('D', 7), GPIO('E', 0), GPIO('E', 1), + GPIO('E', 2), GPIO('E', 6), +}; + +enum { ADC_ENABLE = (1<= ARRAY_SIZE(adc_pins)) + shutdown("Not a valid ADC pin"); + if (READP(adc_pins[chan]) == pin) + break; + } + + // Enable ADC + ADCSRA = ADC_ENABLE; + + // Disable digital input for this pin +#ifdef DIDR2 + if (chan >= 8) + DIDR2 |= 1 << (chan & 0x07); + else +#endif + DIDR0 |= 1 << chan; + + return (struct gpio_adc){ chan }; +} + +enum { ADC_DUMMY=0xff }; +static uint8_t last_analog_read = ADC_DUMMY; +uint8_t analog_reference = 0; + +// Try to sample a value. Returns zero if sample ready, otherwise +// returns the number of clock ticks the caller should wait before +// retrying this function. +uint32_t +gpio_adc_sample(struct gpio_adc g) +{ + if (ADCSRA & (1<> 3) & 0x01) << MUX5; +#endif + ADMUX = (analog_reference << 6) | (g.chan & 0x1f); + + // Start the sample + ADCSRA = ADC_ENABLE | (1< +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "autoconf.h" // CONFIG_MACH_atmega644p +#include "command.h" // shutdown +#include "gpio.h" // gpio_out_write +#include "internal.h" // GPIO2REGS +#include "irq.h" // irq_save +#include "pgm.h" // PROGMEM +#include "sched.h" // sched_shutdown + + +DECL_ENUMERATION_RANGE("pin", "PB0", GPIO('B', 0), 6); +DECL_ENUMERATION_RANGE("pin", "PC0", GPIO('C', 0), 7); +DECL_ENUMERATION_RANGE("pin", "PD0", GPIO('D', 0), 8); +DECL_ENUMERATION_RANGE("pin", "PE0", GPIO('E', 0), 7); + +volatile uint8_t * const digital_regs[] PROGMEM = { + NULL, + &PINB, &PINC, &PIND, + &PINE, +}; + +struct gpio_out +gpio_out_setup(uint8_t pin, uint8_t val) +{ + if (GPIO2PORT(pin) >= ARRAY_SIZE(digital_regs)) + goto fail; + struct gpio_digital_regs *regs = GPIO2REGS(pin); + if (! regs) + goto fail; + struct gpio_out g = { .regs=regs, .bit=GPIO2BIT(pin) }; + gpio_out_reset(g, val); + return g; +fail: + shutdown("Not an output pin"); +} + +void +gpio_out_reset(struct gpio_out g, uint8_t val) +{ + irqstatus_t flag = irq_save(); + g.regs->out = val ? (g.regs->out | g.bit) : (g.regs->out & ~g.bit); + g.regs->mode |= g.bit; + irq_restore(flag); +} + +void +gpio_out_toggle_noirq(struct gpio_out g) +{ + g.regs->in = g.bit; +} + +void +gpio_out_toggle(struct gpio_out g) +{ + gpio_out_toggle_noirq(g); +} + +void +gpio_out_write(struct gpio_out g, uint8_t val) +{ + irqstatus_t flag = irq_save(); + g.regs->out = val ? (g.regs->out | g.bit) : (g.regs->out & ~g.bit); + irq_restore(flag); +} + +struct gpio_in +gpio_in_setup(uint8_t pin, int8_t pull_up) +{ + if (GPIO2PORT(pin) >= ARRAY_SIZE(digital_regs) || pull_up < 0) + goto fail; + struct gpio_digital_regs *regs = GPIO2REGS(pin); + if (! regs) + goto fail; + struct gpio_in g = { .regs=regs, .bit=GPIO2BIT(pin) }; + gpio_in_reset(g, pull_up); + return g; +fail: + shutdown("Not a valid input pin"); +} + +void +gpio_in_reset(struct gpio_in g, int8_t pull_up) +{ + irqstatus_t flag = irq_save(); + g.regs->out = pull_up > 0 ? (g.regs->out | g.bit) : (g.regs->out & ~g.bit); + g.regs->mode &= ~g.bit; + irq_restore(flag); +} + +uint8_t +gpio_in_read(struct gpio_in g) +{ + return !!(g.regs->in & g.bit); +} diff --git a/src/lgt/gpio.h b/src/lgt/gpio.h new file mode 100644 index 000000000..6caa7e4ba --- /dev/null +++ b/src/lgt/gpio.h @@ -0,0 +1,60 @@ +#ifndef __LGT_GPIO_H +#define __LGT_GPIO_H + + +#include +#include "pindefine.h" + + +struct gpio_out { + struct gpio_digital_regs *regs; + // gcc (pre v6) does better optimization when uint8_t are bitfields + uint8_t bit : 8; +}; +struct gpio_out gpio_out_setup(uint8_t pin, uint8_t val); +void gpio_out_reset(struct gpio_out g, uint8_t val); +void gpio_out_toggle_noirq(struct gpio_out g); +void gpio_out_toggle(struct gpio_out g); +void gpio_out_write(struct gpio_out g, uint8_t val); + +struct gpio_in { + struct gpio_digital_regs *regs; + uint8_t bit; +}; +struct gpio_in gpio_in_setup(uint8_t pin, int8_t pull_up); +void gpio_in_reset(struct gpio_in g, int8_t pull_up); +uint8_t gpio_in_read(struct gpio_in g); + +struct gpio_pwm { + void *reg; + uint8_t size8; +}; +struct gpio_pwm gpio_pwm_setup(uint8_t pin, uint32_t cycle_time, uint8_t val); +void gpio_pwm_write(struct gpio_pwm g, uint8_t val); + +struct gpio_adc { + uint8_t chan; +}; +struct gpio_adc gpio_adc_setup(uint8_t pin); +uint32_t gpio_adc_sample(struct gpio_adc g); +uint16_t gpio_adc_read(struct gpio_adc g); +void gpio_adc_cancel_sample(struct gpio_adc g); + +struct spi_config { + uint8_t spcr, spsr; +}; +struct spi_config spi_setup(uint32_t bus, uint8_t mode, uint32_t rate); +void spi_prepare(struct spi_config config); +void spi_transfer(struct spi_config config, uint8_t receive_data + , uint8_t len, uint8_t *data); + +struct i2c_config { + uint8_t addr; +}; + +struct i2c_config i2c_setup(uint32_t bus, uint32_t rate, uint8_t addr); +int i2c_write(struct i2c_config config, uint8_t write_len, uint8_t *write); +int i2c_read(struct i2c_config config, uint8_t reg_len, uint8_t *reg + , uint8_t read_len, uint8_t *read); + +#endif // gpio.h diff --git a/src/lgt/hard_pwm.c b/src/lgt/hard_pwm.c new file mode 100644 index 000000000..57ea51c2c --- /dev/null +++ b/src/lgt/hard_pwm.c @@ -0,0 +1,103 @@ +// Hardware PWM pin support +// +// Copyright (C) 2016-2018 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "autoconf.h" // CONFIG_MACH_atmega644p +#include "command.h" // shutdown +#include "gpio.h" // gpio_pwm_write +#include "internal.h" // GPIO2REGS +#include "irq.h" // irq_save +#include "pgm.h" // PROGMEM +#include "sched.h" // sched_shutdown + +struct gpio_pwm_info { + uint8_t pin; + volatile void *ocr; + volatile uint8_t *rega, *regb; + uint8_t en_bit, flags; +}; + +enum { GP_8BIT=1, GP_AFMT=2 }; + +static const struct gpio_pwm_info pwm_regs[] PROGMEM = { + { GPIO('D', 6), &OCR0A, &TCCR0A, &TCCR0B, 1<= &pwm_regs[ARRAY_SIZE(pwm_regs)]) + shutdown("Not a valid PWM pin"); + if (READP(p->pin) == pin) + break; + } + + // Map cycle_time to pwm clock divisor + uint8_t flags = READP(p->flags), cs; + if (flags & GP_AFMT) { + switch (cycle_time) { + case 0 ... (1+8) * 510L / 2 - 1: cs = 1; break; + case (1+8) * 510L / 2 ... (8+32) * 510L / 2 - 1: cs = 2; break; + case (8+32) * 510L / 2 ... (32+64) * 510L / 2 - 1: cs = 3; break; + case (32+64) * 510L / 2 ... (64+128) * 510L / 2 - 1: cs = 4; break; + case (64+128) * 510L / 2 ... (128+256) * 510L / 2 - 1: cs = 5; break; + case (128+256) * 510L / 2 ... (256+1024) * 510L / 2 - 1: cs = 6; break; + default: cs = 7; break; + } + } else { + switch (cycle_time) { + case 0 ... (1+8) * 510L / 2 - 1: cs = 1; break; + case (1+8) * 510L / 2 ... (8+64) * 510L / 2 - 1: cs = 2; break; + case (8+64) * 510L / 2 ... (64+256) * 510L / 2 - 1: cs = 3; break; + case (64+256) * 510L / 2 ... (256+1024) * 510L / 2 - 1: cs = 4; break; + default: cs = 5; break; + } + } + volatile uint8_t *rega = READP(p->rega), *regb = READP(p->regb); + uint8_t en_bit = READP(p->en_bit); + struct gpio_digital_regs *gpio_regs = GPIO2REGS(pin); + uint8_t gpio_bit = GPIO2BIT(pin); + struct gpio_pwm g = (struct gpio_pwm) { + (void*)READP(p->ocr), flags & GP_8BIT }; + if (rega == &TCCR1A) + shutdown("Can not use timer1 for PWM; timer1 is used for timers"); + + // Setup PWM timer + irqstatus_t flag = irq_save(); + uint8_t old_cs = *regb & 0x07; + if (old_cs && old_cs != cs) + shutdown("PWM already programmed at different speed"); + *regb = cs; + + // Set default value and enable output + gpio_pwm_write(g, val); + *rega |= (1<mode |= gpio_bit; + irq_restore(flag); + + return g; +} + +void +gpio_pwm_write(struct gpio_pwm g, uint8_t val) +{ + if (g.size8) { + *(volatile uint8_t*)g.reg = val; + } else { + irqstatus_t flag = irq_save(); + *(volatile uint16_t*)g.reg = val; + irq_restore(flag); + } +} diff --git a/src/lgt/i2c.c b/src/lgt/i2c.c new file mode 100644 index 000000000..b26049f37 --- /dev/null +++ b/src/lgt/i2c.c @@ -0,0 +1,118 @@ +// I2C functions on AVR +// +// Copyright (C) 2018 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // TWCR +#include "autoconf.h" // CONFIG_CLOCK_FREQ +#include "board/misc.h" // timer_is_before +#include "command.h" // shutdown +#include "gpio.h" // i2c_setup +#include "internal.h" // GPIO +#include "sched.h" // sched_shutdown +#include "i2ccmds.h" // I2C_BUS_SUCCESS + +DECL_ENUMERATION("i2c_bus", "twi", 0); + +static const uint8_t SCL = GPIO('C', 5), SDA = GPIO('C', 4); +DECL_CONSTANT_STR("BUS_PINS_twi", "PC5,PC4"); + +struct i2c_config +i2c_setup(uint32_t bus, uint32_t rate, uint8_t addr) +{ + if (bus) + shutdown("Unsupported i2c bus"); + + if (!(TWCR & (1<= 400000) + TWBR = ((CONFIG_CLOCK_FREQ / 400000) - 16) / 2; + else + TWBR = ((CONFIG_CLOCK_FREQ / 100000) - 16) / 2; + + // Enable interface + TWCR = (1< // cli +#include "compiler.h" // barrier + +static inline void irq_disable(void) { + cli(); + barrier(); +} + +static inline void irq_enable(void) { + barrier(); + sei(); +} + +typedef uint8_t irqstatus_t; + +static inline irqstatus_t irq_save(void) { + uint8_t flag = SREG; + irq_disable(); + return flag; +} + +static inline void irq_restore(irqstatus_t flag) { + barrier(); + SREG = flag; +} + +static inline void irq_wait(void) { + asm("sei\n nop\n cli" : : : "memory"); +} + +static inline void irq_poll(void) { +} + +#endif // irq.h diff --git a/src/lgt/main.c b/src/lgt/main.c new file mode 100644 index 000000000..c72985a36 --- /dev/null +++ b/src/lgt/main.c @@ -0,0 +1,91 @@ +// Main starting point for AVR boards. +// +// Copyright (C) 2016,2017 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // AVR_STACK_POINTER_REG +#include // _crc_ccitt_update + +#include "autoconf.h" // CONFIG_MCU +#include "board/misc.h" // dynmem_start +#include "command.h" // DECL_CONSTANT +#include "irq.h" // irq_enable +#include "pindefine.h" +#include "sched.h" // sched_main + +#if CONFIG_SERIAL_BAUD > 115200 +#warning "Lgt8f328p always using cheap usb to uart ic, some of lgt8f328p cannot support high baudrate" +#endif + +DECL_CONSTANT_STR("MCU", CONFIG_MCU); + +/**************************************************************** + * Dynamic memory + ****************************************************************/ + +// Return the start of memory available for dynamic allocations +void *dynmem_start(void) { + extern char _end; + return &_end; +} + +// Return the end of memory available for dynamic allocations +void *dynmem_end(void) { + return (void *)ALIGN(AVR_STACK_POINTER_REG, 256) - CONFIG_LGT_STACK_SIZE; +} + +// Optimized crc16_ccitt for the avr processor +uint16_t crc16_ccitt(uint8_t *buf, uint_fast8_t len) { + uint16_t crc = 0xFFFF; + while (len--) crc = _crc_ccitt_update(crc, *buf++); + return crc; +} + +void set_cpu_speed(void) { +// fix baudrate +#if CONFIG_SERIAL_BAUD == 115200 && CONFIG_CLOCK_FREQ == 16000000L + const uint8_t CAL_V = 6; + if ((uint8_t)RCMCAL >= CAL_V) { + RCMCAL -= CAL_V; + } else { + RCMCAL = 0; + } +#endif + + CLKPR = 0x80; + CLKPR = 0x01; + // switch to internal crystal + GPIOR0 = PMCR & 0x9f; + PMCR = 0x80; + PMCR = GPIOR0; + + // disable external crystal + GPIOR0 = PMCR & 0xfb; + PMCR = 0x80; + PMCR = GPIOR0; + + CLKPR = 0x80; +// Set the CPU speed +#if CONFIG_CLOCK_FREQ == 32000000L + CLKPR = 0x00; +#elif CONFIG_CLOCK_FREQ == 16000000L + CLKPR = 0x01; +#elif CONFIG_CLOCK_FREQ == 8000000L + CLKPR = 0x02; +#elif CONFIG_CLOCK_FREQ == 4000000L + CLKPR = 0x03; +#elif CONFIG_CLOCK_FREQ == 2000000L + CLKPR = 0x04; +#elif CONFIG_CLOCK_FREQ == 1000000L + CLKPR = 0x05; +#endif +} + +// Main entry point for avr code. +int main(void) { + set_cpu_speed(); + irq_enable(); + sched_main(); + return 0; +} diff --git a/src/lgt/pgm.h b/src/lgt/pgm.h new file mode 100644 index 000000000..1ff4c2baa --- /dev/null +++ b/src/lgt/pgm.h @@ -0,0 +1,27 @@ +#ifndef __LGT_PGM_H +#define __LGT_PGM_H +// This header provides the avr/pgmspace.h definitions for "PROGMEM" +// on AVR platforms. + +#include + +#define NEED_PROGMEM 1 + +#define READP(VAR) ({ \ + _Pragma("GCC diagnostic push"); \ + _Pragma("GCC diagnostic ignored \"-Wint-to-pointer-cast\""); \ + typeof(VAR) __val = \ + __builtin_choose_expr(sizeof(VAR) == 1, \ + (typeof(VAR))pgm_read_byte(&(VAR)), \ + __builtin_choose_expr(sizeof(VAR) == 2, \ + (typeof(VAR))pgm_read_word(&(VAR)), \ + __builtin_choose_expr(sizeof(VAR) == 4, \ + (typeof(VAR))pgm_read_dword(&(VAR)), \ + __force_link_error__unknown_type))); \ + _Pragma("GCC diagnostic pop"); \ + __val; \ + }) + +extern void __force_link_error__unknown_type(void); + +#endif // pgm.h diff --git a/src/lgt/pindefine.h b/src/lgt/pindefine.h new file mode 100644 index 000000000..02348771a --- /dev/null +++ b/src/lgt/pindefine.h @@ -0,0 +1,7 @@ +#ifndef __LGT_PINDEIFNE_H +#define __LGT_PINDEIFNE_H + +#define PMCR _SFR_MEM8(0xF2) +#define PINE _SFR_IO8(0x0C) +#define RCMCAL (*((volatile unsigned char *)0x66)) +#endif \ No newline at end of file diff --git a/src/lgt/serial.c b/src/lgt/serial.c new file mode 100644 index 000000000..d50fd6ab9 --- /dev/null +++ b/src/lgt/serial.c @@ -0,0 +1,42 @@ +// Example code for interacting with serial_irq.c +// +// Copyright (C) 2018 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // uint8_t +#include // USART_RX_vect +#include "board/serial_irq.h" // serial_get_tx_byte +#include "pindefine.h" +#include "sched.h" // DECL_INIT +#include "autoconf.h" // CONFIG_MCU +#include "command.h" // DECL_CONSTANT_STR + +DECL_CONSTANT_STR("RESERVE_PINS_serial", "PD0,PD1"); + +void serial_init(void) { + uint16_t baud_setting = ( CONFIG_CLOCK_FREQ / (CONFIG_SERIAL_BAUD * 8L) - 1 ); +#ifdef CONFIG_SERIAL_BAUD_U2X + UCSR0A = _BV(U2X0); // Double speed mode USART0 +#endif + UCSR0B = _BV(RXEN0) | _BV(TXEN0) | _BV(RXCIE0) | _BV(UDRIE0); + UCSR0C = _BV(UCSZ00) | _BV(UCSZ01); + UBRR0H = baud_setting >> 8; + UBRR0L = baud_setting; +} +DECL_INIT(serial_init); + +// Rx interrupt - data available to be read. +ISR(USART_RX_vect) { serial_rx_byte(UDR0); } + +// Tx interrupt - data can be written to serial. +ISR(USART_UDRE_vect) { + uint8_t data; + int ret = serial_get_tx_byte(&data); + if (ret) + UCSR0B &= ~(1 << UDRIE0); + else + UDR0 = data; +} + +void serial_enable_tx_irq(void) { UCSR0B |= _BV(UDRIE0); } diff --git a/src/lgt/spi.c b/src/lgt/spi.c new file mode 100644 index 000000000..9ec8b3f2e --- /dev/null +++ b/src/lgt/spi.c @@ -0,0 +1,94 @@ +// Serial Peripheral Interface (SPI) support +// +// Copyright (C) 2016-2018 Kevin O'Connor +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include "autoconf.h" // CONFIG_MACH_atmega644p +#include "command.h" // shutdown +#include "gpio.h" // spi_setup +#include "internal.h" // GPIO +#include "pgm.h" // READP +#include "sched.h" // sched_shutdown + +DECL_ENUMERATION("spi_bus", "spi", 0); + +static const uint8_t MISO = GPIO('B', 4), MOSI = GPIO('B', 3); +static const uint8_t SCK = GPIO('B', 5), SS = GPIO('B', 2); +DECL_CONSTANT_STR("BUS_PINS_spi", "PB4,PB3,PB5"); + +static void +spi_init(void) +{ + if (!(GPIO2REGS(SS)->mode & GPIO2BIT(SS))) + // The SS pin must be an output pin (but is otherwise unused) + gpio_out_setup(SS, 0); + gpio_out_setup(SCK, 0); + gpio_out_setup(MOSI, 0); + gpio_in_setup(MISO, 0); + + SPCR = (1<= (CONFIG_CLOCK_FREQ / 2)) { + config.spsr = (1<= (CONFIG_CLOCK_FREQ / 4)) { + config.spcr = 0; + } else if (rate >= (CONFIG_CLOCK_FREQ / 8)) { + config.spcr = 1; + config.spsr = (1<= (CONFIG_CLOCK_FREQ / 16)) { + config.spcr = 1; + } else if (rate >= (CONFIG_CLOCK_FREQ / 32)) { + config.spcr = 2; + config.spsr = (1<= (CONFIG_CLOCK_FREQ / 64)) { + config.spcr = 2; + } else { + config.spcr = 3; + } + + // Setup mode + config.spcr |= (1< +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // TCNT1 +#include "autoconf.h" // CONFIG_AVR_CLKPR +#include "board/misc.h" // timer_from_us +#include "command.h" // shutdown +#include "irq.h" // irq_save +#include "sched.h" // sched_timer_dispatch + + +/**************************************************************** + * Low level timer code + ****************************************************************/ + +DECL_CONSTANT("CLOCK_FREQ", CONFIG_CLOCK_FREQ); + +// Return the number of clock ticks for a given number of microseconds +uint32_t +timer_from_us(uint32_t us) +{ + return us * (CONFIG_CLOCK_FREQ / 1000000); +} + +union u32_u { + struct { uint8_t b0, b1, b2, b3; }; + struct { uint16_t lo, hi; }; + uint32_t val; +}; + +// Return true if time1 is before time2. Always use this function to +// compare times as regular C comparisons can fail if the counter +// rolls over. +uint8_t __always_inline +timer_is_before(uint32_t time1, uint32_t time2) +{ + // This asm is equivalent to: + // return (int32_t)(time1 - time2) < 0; + // But gcc doesn't do a good job with the above, so it's hand coded. + union u32_u utime1 = { .val = time1 }; + uint8_t f = utime1.b3; + asm(" cp %A1, %A2\n" + " cpc %B1, %B2\n" + " cpc %C1, %C2\n" + " sbc %0, %D2" + : "+r"(f) : "r"(time1), "r"(time2)); + return (int8_t)f < 0; +} + +static inline uint16_t +timer_get(void) +{ + return TCNT1; +} + +static inline void +timer_set(uint16_t next) +{ + OCR1A = next; +} + +static inline void +timer_repeat_set(uint16_t next) +{ + // Timer1B is used to limit the number of timers run from a timer1A irq + OCR1B = next; + // This is "TIFR1 = 1<= 0)) { + // Another timer is pending - briefly allow irqs and then run it + irq_enable(); + if (unlikely(TIFR1 & (1< +// +// This file may be distributed under the terms of the GNU GPLv3 license. + +#include // WDT_vect +#include // wdt_enable +#include "command.h" // shutdown +#include "irq.h" // irq_disable +#include "sched.h" // DECL_TASK + +static uint8_t watchdog_shutdown; + +ISR(WDT_vect) +{ + watchdog_shutdown = 1; + shutdown("Watchdog timer!"); +} + +void +watchdog_reset(void) +{ + wdt_reset(); + if (watchdog_shutdown) { + WDTCSR = 1<