├── .gitignore ├── examples ├── blink │ ├── Makefile │ └── blink.c ├── mutex │ ├── Makefile │ └── mutex.c ├── i2c │ ├── Makefile │ └── i2c.c ├── readline │ ├── Makefile │ └── readline.c ├── sirc │ ├── Makefile │ └── sirc.c ├── avrdude.sh └── Makefile.inc ├── readline.h ├── cond.h ├── uart ├── Makefile ├── uart.h.in └── uart.c.in ├── drivers ├── mma8452q.h ├── hd44780u.h ├── sirc.h ├── hd44780u.c ├── sirc.c ├── hmc5883l.h ├── hmc5883l.c └── mma8452q.c ├── i2c.h ├── uart.h ├── mutex.c ├── LICENSE ├── mutex.h ├── cond.c ├── README.md ├── task.h ├── readline.c ├── queue.h ├── uart.c ├── i2c.c └── task.c /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | *.lst 3 | *.map 4 | *.elf 5 | *.hex 6 | *.bin 7 | *.srec 8 | -------------------------------------------------------------------------------- /examples/blink/Makefile: -------------------------------------------------------------------------------- 1 | DIR = ../.. 2 | OBJS = $(DIR)/task.o 3 | 4 | default: blink.hex 5 | 6 | include ../Makefile.inc 7 | -------------------------------------------------------------------------------- /examples/mutex/Makefile: -------------------------------------------------------------------------------- 1 | DIR = ../.. 2 | OBJS = $(DIR)/task.o $(DIR)/mutex.o 3 | 4 | default: mutex.hex 5 | 6 | include ../Makefile.inc 7 | -------------------------------------------------------------------------------- /examples/i2c/Makefile: -------------------------------------------------------------------------------- 1 | DIR = ../.. 2 | OBJS = $(DIR)/task.o $(DIR)/i2c.o $(DIR)/uart.o 3 | 4 | default: i2c.hex 5 | 6 | include ../Makefile.inc 7 | -------------------------------------------------------------------------------- /examples/readline/Makefile: -------------------------------------------------------------------------------- 1 | DIR = ../.. 2 | OBJS = $(DIR)/task.o $(DIR)/uart.o $(DIR)/readline.o 3 | 4 | default: readline.hex 5 | 6 | include ../Makefile.inc 7 | -------------------------------------------------------------------------------- /examples/sirc/Makefile: -------------------------------------------------------------------------------- 1 | DIR = ../.. 2 | OBJS = $(DIR)/task.o $(DIR)/uart.o $(DIR)/drivers/sirc.c 3 | 4 | default: sirc.hex 5 | 6 | include ../Makefile.inc 7 | -------------------------------------------------------------------------------- /readline.h: -------------------------------------------------------------------------------- 1 | #ifndef _READLINE_H 2 | #define _READLINE_H 3 | 4 | #include 5 | 6 | int8_t readline(const char *prompt, char *buf, int8_t bufsz); 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /examples/avrdude.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | set -e 4 | 5 | if [ -z "$1" ] 6 | then 7 | echo "Usage: $0 DIR" 8 | exit 1 9 | fi 10 | 11 | pushd $1 12 | make 13 | popd 14 | 15 | hex=$(echo $1/*.hex) 16 | 17 | avrdude -q -V -D \ 18 | -p atmega328p \ 19 | -c arduino \ 20 | -P /dev/ttyUSB0 \ 21 | -U flash:w:${hex}:i 22 | -------------------------------------------------------------------------------- /cond.h: -------------------------------------------------------------------------------- 1 | #ifndef _COND_H 2 | #define _COND_H 3 | 4 | #include "mutex.h" 5 | 6 | typedef struct cond_s cond_t; 7 | 8 | struct cond_s { 9 | QUEUE waiting; 10 | }; 11 | 12 | void cond_init(cond_t *c); 13 | 14 | void cond_wait(cond_t *c, mutex_t *m); 15 | 16 | void cond_signal(cond_t *c); 17 | 18 | void cond_broadcast(cond_t *c); 19 | 20 | #endif 21 | -------------------------------------------------------------------------------- /uart/Makefile: -------------------------------------------------------------------------------- 1 | all: 2 | sed -e 's/@/0/g' -e 's/\(uart\)/\10/i' uart.h.in > uart0.h 3 | sed -e 's/@/1/g' -e 's/\(uart\)/\11/i' uart.h.in > uart1.h 4 | sed -e 's/@/2/g' -e 's/\(uart\)/\12/i' uart.h.in > uart2.h 5 | sed -e 's/@/3/g' -e 's/\(uart\)/\13/i' uart.h.in > uart3.h 6 | sed -e 's/@/0/g' -e 's/\(uart\)/\10/i' uart.c.in > uart0.c 7 | sed -e 's/@/1/g' -e 's/\(uart\)/\11/i' uart.c.in > uart1.c 8 | sed -e 's/@/2/g' -e 's/\(uart\)/\12/i' uart.c.in > uart2.c 9 | sed -e 's/@/3/g' -e 's/\(uart\)/\13/i' uart.c.in > uart3.c 10 | 11 | default: 12 | sed -e 's/@/0/g' -e 's/USART0/USART/i' uart.h.in > ../uart.h 13 | sed -e 's/@/0/g' -e 's/USART0/USART/i' uart.c.in > ../uart.c 14 | -------------------------------------------------------------------------------- /examples/blink/blink.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "task.h" 5 | 6 | volatile uint8_t delay_ms = 0; 7 | 8 | void blink_task(void *unused) { 9 | while (1) { 10 | task_sleep(delay_ms); 11 | PORTB ^= _BV(PB5); 12 | } 13 | } 14 | 15 | void delay_task(void *unused) { 16 | while (1) { 17 | delay_ms = 50; 18 | task_sleep(1000); 19 | delay_ms = 200; 20 | task_sleep(1000); 21 | } 22 | } 23 | 24 | int main() { 25 | // PB5 (pin 13) is an output pin 26 | DDRB |= _BV(PB5); 27 | 28 | task_init(); 29 | 30 | task_create(blink_task, NULL); 31 | task_create(delay_task, NULL); 32 | 33 | task_start(); 34 | 35 | return 0; // Never reached 36 | } 37 | -------------------------------------------------------------------------------- /drivers/mma8452q.h: -------------------------------------------------------------------------------- 1 | #ifndef _MMA8452_H 2 | #define _MMA8452_H 3 | 4 | #include 5 | 6 | /* Data rates. */ 7 | #define MMA8452Q_DR2 _BV(5) 8 | #define MMA8452Q_DR1 _BV(4) 9 | #define MMA8452Q_DR0 _BV(3) 10 | #define MMA8452Q_DR_800HZ (0) 11 | #define MMA8452Q_DR_400HZ (MMA8452Q_DR0) 12 | #define MMA8452Q_DR_200HZ (MMA8452Q_DR1) 13 | #define MMA8452Q_DR_100HZ (MMA8452Q_DR1 | MMA8452Q_DR0) 14 | #define MMA8452Q_DR_50HZ (MMA8452Q_DR2) 15 | #define MMA8452Q_DR_12HZ (MMA8452Q_DR2 | MMA8452Q_DR0) 16 | #define MMA8452Q_DR_6HZ (MMA8452Q_DR2 | MMA8452Q_DR1) 17 | #define MMA8452Q_DR_1HZ (MMA8452Q_DR2 | MMA8452Q_DR1 | MMA8452Q_DR0) 18 | 19 | int8_t mma8452q_configure(uint8_t dr); 20 | 21 | int8_t mma8452q_read(int16_t *axis); 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /examples/readline/readline.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "task.h" 4 | #include "uart.h" 5 | #include "readline.h" 6 | 7 | void echo_task(void *unused) { 8 | uint16_t i; 9 | char prompt[16]; 10 | char buf[16]; 11 | int8_t len; 12 | 13 | for (i = 1;; i++) { 14 | sprintf(prompt, "%u> ", i); 15 | 16 | len = readline(prompt, buf, sizeof(buf)); 17 | if (len > 0) { 18 | uart_putc('"', NULL); 19 | uart_write(buf, len); 20 | uart_putc('"', NULL); 21 | uart_write("\r\n", 2); 22 | } 23 | } 24 | } 25 | 26 | int main() { 27 | // 115200 for 16MHz clock 28 | uart_init(16, 1); 29 | 30 | task_init(); 31 | 32 | task_create(echo_task, NULL); 33 | 34 | task_start(); 35 | 36 | return 0; // Never reached 37 | } 38 | -------------------------------------------------------------------------------- /i2c.h: -------------------------------------------------------------------------------- 1 | #ifndef _I2C_H 2 | #define _I2C_H 3 | 4 | #ifndef I2C_FREQ 5 | #define I2C_FREQ 100000 6 | #endif 7 | 8 | struct i2c_iovec_s { 9 | uint8_t *base; 10 | uint8_t len; 11 | }; 12 | 13 | void i2c_init(void); 14 | 15 | void i2c_open(void); 16 | 17 | void i2c_close(void); 18 | 19 | int8_t i2c_readv(uint8_t address, struct i2c_iovec_s *iov, uint8_t iovcnt); 20 | 21 | int8_t i2c_writev(uint8_t address, struct i2c_iovec_s *iov, uint8_t iovcnt); 22 | 23 | int8_t i2c_read(uint8_t address, uint8_t *buf, uint8_t len); 24 | 25 | int8_t i2c_write(uint8_t address, uint8_t *buf, uint8_t len); 26 | 27 | int8_t i2c_read_from(uint8_t address, uint8_t reg, uint8_t *buf, uint8_t len); 28 | 29 | int8_t i2c_write_to(uint8_t address, uint8_t reg, uint8_t *buf, uint8_t len); 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /examples/sirc/sirc.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | void read_loop(void *unused) { 11 | FILE uart = FDEV_SETUP_STREAM(uart_putc, uart_getc, _FDEV_SETUP_RW); 12 | uint16_t code; 13 | uint8_t device; 14 | uint8_t command; 15 | 16 | while (1) { 17 | code = sirc_read(); 18 | device = (code >> 7) & 0x1f; 19 | command = code & 0x7f; 20 | fprintf(&uart, "Device: %d, command: %d\r\n", device, command); 21 | } 22 | } 23 | 24 | int main() { 25 | uart_init(); 26 | 27 | sirc_init(); 28 | 29 | task_init(); 30 | 31 | task_create(read_loop, NULL); 32 | 33 | task_start(); 34 | 35 | return 0; // Never reached 36 | } 37 | -------------------------------------------------------------------------------- /drivers/hd44780u.h: -------------------------------------------------------------------------------- 1 | #ifndef _HD44780U_H 2 | #define _HD44780U_H 3 | 4 | #include 5 | 6 | /* 7 | * Hitachi HD44780U LCD controller driver. 8 | * 9 | * This implementation expects the LCD data lines to be hooked up 10 | * through a shift register. This only requires 2 lines to hook up the 11 | * board to the shift register (data and latch), and 2 lines to hook 12 | * up the board to the LCD to clock data in (instruction and latch). 13 | */ 14 | 15 | // Initialize LCD. 16 | void lcd_init(void); 17 | 18 | // Clear LCD display. 19 | void lcd_clear_display(void); 20 | 21 | // Return cursor to origin of LCD. 22 | void lcd_return_home(void); 23 | 24 | // Write character to LCD. 25 | void lcd_write(char c); 26 | 27 | // Write buffer to LCD and advance cursor to the next line. 28 | void lcd_puts(const char *buf, int8_t len); 29 | 30 | #endif 31 | -------------------------------------------------------------------------------- /examples/mutex/mutex.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "mutex.h" 5 | #include "task.h" 6 | 7 | volatile uint8_t delay_ms = 0; 8 | 9 | void blink_task(void *unused) { 10 | while (1) { 11 | task_sleep(delay_ms); 12 | PORTB ^= _BV(PB5); 13 | } 14 | } 15 | 16 | // The mutex sequences multiple tasks trying to set a blink interval. 17 | mutex_t m; 18 | 19 | void delay_task(void *data) { 20 | while (1) { 21 | mutex_lock(&m); 22 | delay_ms = (uint16_t) data; 23 | task_sleep(1000); 24 | mutex_unlock(&m); 25 | } 26 | } 27 | 28 | int main() { 29 | // PB5 (pin 13) is an output pin 30 | DDRB |= _BV(PB5); 31 | 32 | mutex_init(&m); 33 | 34 | task_init(); 35 | 36 | task_create(blink_task, NULL); 37 | 38 | task_create(delay_task, (void *)20); 39 | task_create(delay_task, (void *)50); 40 | task_create(delay_task, (void *)100); 41 | 42 | task_start(); 43 | 44 | return 0; // Never reached 45 | } 46 | -------------------------------------------------------------------------------- /uart.h: -------------------------------------------------------------------------------- 1 | #ifndef _UART_H 2 | #define _UART_H 3 | 4 | #include 5 | #include 6 | 7 | #ifdef UART_COUNTERS 8 | #define UART_COUNT_TX_BYTES 9 | #define UART_COUNT_RX_BYTES 10 | #define UART_COUNT_RX_ERRORS 11 | #endif 12 | 13 | #ifdef UART_COUNT_TX_BYTES 14 | extern uint16_t uart_tx_bytes; 15 | #endif 16 | 17 | #ifdef UART_COUNT_RX_BYTES 18 | extern uint16_t uart_rx_bytes; 19 | #endif 20 | 21 | #ifdef UART_COUNT_RX_ERRORS 22 | // Frame Error 23 | extern uint8_t uart_rx_fe; 24 | // Data OverRun 25 | extern uint8_t uart_rx_dor; 26 | // Parity Error 27 | extern uint8_t uart_rx_pe; 28 | // Buffer Data OverRun (of software buffer) 29 | extern uint8_t uart_rx_bdor; 30 | #endif 31 | 32 | void uart_init(uint16_t ubrr, uint8_t x2); 33 | 34 | int uart_write(const void *buf, size_t count); 35 | 36 | int uart_read(void *buf, size_t count); 37 | 38 | int uart_read_nonblock(void *buf, size_t count); 39 | 40 | int uart_putc(char c, FILE *unused); 41 | 42 | int uart_getc(FILE *unused); 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /uart/uart.h.in: -------------------------------------------------------------------------------- 1 | #ifndef _UART_H 2 | #define _UART_H 3 | 4 | #include 5 | #include 6 | 7 | #ifdef UART_COUNTERS 8 | #define UART_COUNT_TX_BYTES 9 | #define UART_COUNT_RX_BYTES 10 | #define UART_COUNT_RX_ERRORS 11 | #endif 12 | 13 | #ifdef UART_COUNT_TX_BYTES 14 | extern uint16_t uart_tx_bytes; 15 | #endif 16 | 17 | #ifdef UART_COUNT_RX_BYTES 18 | extern uint16_t uart_rx_bytes; 19 | #endif 20 | 21 | #ifdef UART_COUNT_RX_ERRORS 22 | // Frame Error 23 | extern uint8_t uart_rx_fe; 24 | // Data OverRun 25 | extern uint8_t uart_rx_dor; 26 | // Parity Error 27 | extern uint8_t uart_rx_pe; 28 | // Buffer Data OverRun (of software buffer) 29 | extern uint8_t uart_rx_bdor; 30 | #endif 31 | 32 | void uart_init(uint16_t ubrr, uint8_t x2); 33 | 34 | int uart_write(const void *buf, size_t count); 35 | 36 | int uart_read(void *buf, size_t count); 37 | 38 | int uart_read_nonblock(void *buf, size_t count); 39 | 40 | int uart_putc(char c, FILE *unused); 41 | 42 | int uart_getc(FILE *unused); 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /mutex.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "mutex.h" 4 | 5 | void mutex_init(mutex_t *m) { 6 | m->status = MUTEX_UNLOCKED; 7 | QUEUE_INIT(&m->waiting); 8 | } 9 | 10 | void mutex_lock(mutex_t *m) { 11 | uint8_t sreg; 12 | 13 | sreg = SREG; 14 | cli(); 15 | 16 | if (m->status == MUTEX_LOCKED) { 17 | // Lock is transferred to this task when it is woken up. 18 | // m->status will still be set to MUTEX_LOCKED, to avoid any other tasks 19 | // being scheduled before this one and grabbing the lock. 20 | task_suspend(&m->waiting); 21 | } else { 22 | m->status = MUTEX_LOCKED; 23 | } 24 | 25 | SREG = sreg; 26 | } 27 | 28 | void mutex_unlock(mutex_t *m) { 29 | uint8_t sreg; 30 | QUEUE *q; 31 | task_t *t; 32 | 33 | sreg = SREG; 34 | cli(); 35 | 36 | if (QUEUE_EMPTY(&m->waiting)) { 37 | m->status = MUTEX_UNLOCKED; 38 | } else { 39 | // Wake up first waiting task to transfer lock 40 | q = QUEUE_HEAD(&m->waiting); 41 | t = QUEUE_DATA(q, task_t, member); 42 | QUEUE_REMOVE(q); 43 | task_wakeup(t); 44 | } 45 | 46 | SREG = sreg; 47 | } 48 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 Pieter Noordhuis 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /mutex.h: -------------------------------------------------------------------------------- 1 | #ifndef _MUTEX_H 2 | #define _MUTEX_H 3 | 4 | /* 5 | * When a task calls 'mutex_lock' and the mutex is not yet locked, it will 6 | * become locked and the calling task continues execution. This task 7 | * subsequently has to call 'mutex_unlock' to give up the lock and let other 8 | * tasks access the protected resource. 9 | * 10 | * When a task calls 'mutex_lock' and the mutex is locked, the task will be 11 | * suspended and pushed onto the list of waiting tasks. It is then suspended 12 | * until it both 1) becomes the first element of the list, and 2) another task 13 | * holding the mutex unlocks it. Then, it is woken up and the mutex is set to 14 | * be locked by this task. The latter is necessary to prevent the task that 15 | * unlocked the mutex from immediately locking it again and thereby starving 16 | * other tasks waiting for access to the same protected resource. 17 | */ 18 | 19 | #include "task.h" 20 | 21 | #define MUTEX_UNLOCKED 0 22 | #define MUTEX_LOCKED 1 23 | 24 | typedef struct mutex_s mutex_t; 25 | 26 | struct mutex_s { 27 | unsigned status:1; 28 | QUEUE waiting; 29 | }; 30 | 31 | void mutex_init(mutex_t *mutex); 32 | 33 | void mutex_lock(mutex_t *mutex); 34 | 35 | void mutex_unlock(mutex_t *mutex); 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /cond.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "cond.h" 4 | 5 | void cond_init(cond_t *c) { 6 | QUEUE_INIT(&c->waiting); 7 | } 8 | 9 | // Assume the mutex is held by the caller. 10 | // It is too expensive to do run-time integrity checks on this processor. 11 | void cond_wait(cond_t *c, mutex_t *m) { 12 | uint8_t sreg; 13 | 14 | sreg = SREG; 15 | cli(); 16 | 17 | // Unlocking and suspending must happen atomically. 18 | // If it doesn't, a race could cause a cond_{signal,broadcast} from another 19 | // task holding the lock before this task has been suspended. 20 | mutex_unlock(m); 21 | 22 | // Suspend task until woken up through cond_{signal,broadcast}. 23 | task_suspend(&c->waiting); 24 | 25 | // Task may be interrupted again. 26 | SREG = sreg; 27 | 28 | // Reacquire mutex. 29 | mutex_lock(m); 30 | } 31 | 32 | void cond_signal(cond_t *c) { 33 | uint8_t sreg; 34 | QUEUE *q; 35 | task_t *t; 36 | 37 | sreg = SREG; 38 | cli(); 39 | 40 | // Wake up first waiting task (FIFO order). 41 | if (!QUEUE_EMPTY(&c->waiting)) { 42 | q = QUEUE_HEAD(&c->waiting); 43 | t = QUEUE_DATA(q, task_t, member); 44 | QUEUE_REMOVE(q); 45 | task_wakeup(t); 46 | } 47 | 48 | SREG = sreg; 49 | } 50 | 51 | void cond_broadcast(cond_t *c) { 52 | uint8_t sreg; 53 | QUEUE *q; 54 | task_t *t; 55 | 56 | sreg = SREG; 57 | cli(); 58 | 59 | // Wake up all waiting tasks. 60 | while (!QUEUE_EMPTY(&c->waiting)) { 61 | q = QUEUE_HEAD(&c->waiting); 62 | t = QUEUE_DATA(q, task_t, member); 63 | QUEUE_REMOVE(q); 64 | task_wakeup(t); 65 | } 66 | 67 | SREG = sreg; 68 | } 69 | -------------------------------------------------------------------------------- /examples/Makefile.inc: -------------------------------------------------------------------------------- 1 | MCU_TARGET = atmega328 2 | OPTIMIZE = -O2 3 | 4 | DEFS = -DF_CPU=16000000 -DTASK_COUNT_SEC -DTASK_COUNT_MSEC -DTASK_COUNT_USEC 5 | LIBS = 6 | 7 | # You should not have to change anything below here. 8 | 9 | CC = avr-gcc 10 | 11 | # Override is only needed by avr-lib build system. 12 | 13 | override CFLAGS = -g -Wall $(OPTIMIZE) -mmcu=$(MCU_TARGET) $(DEFS) -I../../ 14 | override LDFLAGS = 15 | override ASFLAGS = -Wall -mmcu=$(MCU_TARGET) $(DEFS) 16 | 17 | OBJCOPY = avr-objcopy 18 | OBJDUMP = avr-objdump 19 | 20 | %.elf: %.o $(OBJS) 21 | $(CC) $(CFLAGS) $(LDFLAGS) -o $@ $^ $(LIBS) 22 | 23 | clean: 24 | rm -rf *.o *.elf *.eps *.png *.pdf *.bak 25 | rm -rf *.lst *.map $(EXTRA_CLEAN_FILES) 26 | 27 | %.lst: %.elf 28 | $(OBJDUMP) -h -S $< > $@ 29 | 30 | # Rules for building the .text rom images 31 | 32 | %.hex: %.elf 33 | $(OBJCOPY) -j .text -j .data -O ihex $< $@ 34 | 35 | %.srec: %.elf 36 | $(OBJCOPY) -j .text -j .data -O srec $< $@ 37 | 38 | %.bin: %.elf 39 | $(OBJCOPY) -j .text -j .data -O binary $< $@ 40 | 41 | # Rules for building the .eeprom rom images 42 | 43 | %_eeprom.hex: %.elf 44 | $(OBJCOPY) -j .eeprom --change-section-lma .eeprom=0 -O ihex $< $@ \ 45 | || { echo empty $@ not generated; exit 0; } 46 | 47 | %_eeprom.srec: %.elf 48 | $(OBJCOPY) -j .eeprom --change-section-lma .eeprom=0 -O srec $< $@ \ 49 | || { echo empty $@ not generated; exit 0; } 50 | 51 | %_eeprom.bin: %.elf 52 | $(OBJCOPY) -j .eeprom --change-section-lma .eeprom=0 -O binary $< $@ \ 53 | || { echo empty $@ not generated; exit 0; } 54 | 55 | # Every thing below here is used by avr-libc's build system and can be ignored 56 | # by the casual user. 57 | 58 | FIG2DEV = fig2dev 59 | EXTRA_CLEAN_FILES = *.hex *.bin *.srec 60 | 61 | %.eps: %.fig 62 | $(FIG2DEV) -L eps $< $@ 63 | 64 | %.pdf: %.fig 65 | $(FIG2DEV) -L pdf $< $@ 66 | 67 | %.png: %.fig 68 | $(FIG2DEV) -L png $< $@ 69 | -------------------------------------------------------------------------------- /drivers/sirc.h: -------------------------------------------------------------------------------- 1 | #ifndef _SIRC_H 2 | #define _SIRC_H 3 | 4 | /* 5 | * Sony IR code decoder. 6 | * 7 | * For a comprehensive introduction see: 8 | * http://www.righto.com/2010/03/understanding-sony-ir-remote-codes-lirc.html 9 | * 10 | * Also see: 11 | * http://www.hifi-remote.com/sony/ 12 | * 13 | * This decoder implements bit decoding where pulses are considered to be 14 | * preceded by delays rather than pulses preceding delays. Since the last pulse 15 | * is followed by a delay of arbitrary length, that delay cannot trigger an 16 | * interrupt. If instead the header is viewed to be preceded by a delay of 17 | * arbitrary length, that delay can be ignored upon seeing the header pulse. 18 | */ 19 | 20 | #include 21 | 22 | /* 23 | * Pin state marking the start of a pulse. 24 | * If the pin is high when idle, the pulse starts with low (0). 25 | * If the pin is low when idle, the pulse starts with high (1). 26 | */ 27 | #ifndef _SIRC_PULSE_START 28 | #define _SIRC_PULSE_START 0 29 | #endif 30 | 31 | // Length of header pulse. 32 | #ifndef _SIRC_HEADER_PULSE_US 33 | #define _SIRC_HEADER_PULSE_US 2400 34 | #endif 35 | 36 | // Header pulse error margin. 37 | #ifndef _SIRC_HEADER_ERROR_US 38 | #define _SIRC_HEADER_ERROR_US 200 39 | #endif 40 | 41 | // Length of one pulse. 42 | #ifndef _SIRC_ONE_PULSE_US 43 | #define _SIRC_ONE_PULSE_US 1200 44 | #endif 45 | 46 | // One pulse error margin. 47 | #ifndef _SIRC_ONE_ERROR_US 48 | #define _SIRC_ONE_ERROR_US 200 49 | #endif 50 | 51 | // Length of zero pulse. 52 | #ifndef _SIRC_ZERO_PULSE_US 53 | #define _SIRC_ZERO_PULSE_US 600 54 | #endif 55 | 56 | // Zero pulse error margin. 57 | #ifndef _SIRC_ZERO_ERROR_US 58 | #define _SIRC_ZERO_ERROR_US 200 59 | #endif 60 | 61 | // Length of delay between pulses. 62 | #ifndef _SIRC_DELAY_US 63 | #define _SIRC_DELAY_US 600 64 | #endif 65 | 66 | // Delay error margin. 67 | #ifndef _SIRC_DELAY_ERROR_US 68 | #define _SIRC_DELAY_ERROR_US 200 69 | #endif 70 | 71 | // Number of bits to capture. 72 | #define BITS 12 73 | 74 | void sirc_init(); 75 | 76 | uint16_t sirc_read(); 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # avr-tasks 2 | 3 | Multitasking library for the Atmel AVR processor. 4 | 5 | * Preemptive. 6 | * Uses TIMER0 for scheduler ticks (default: 2ms per tick). 7 | * Supports as many tasks as you can fit in RAM (default: 256 bytes per task). 8 | * Expects to be run on an ATmega328p. 9 | * For missing features, see _TODO_ below. 10 | 11 | ## Example 12 | 13 | An example can be found in [blink.c](examples/blink/blink.c). 14 | This program blinks the LED connected to pin 13, with the actual blinking and 15 | the blink rate controlled by two different tasks. 16 | 17 | Find more examples in the [examples](examples/) directory. 18 | 19 | ## Why? 20 | 21 | * To be able to execute arbitrary code while waiting for I/O but be 22 | interrupted when I/O _does_ happen. 23 | 24 | ## How? 25 | 26 | * Every task is offset at some point in the stack. 27 | * On task interruption, all relevant registers are pushed onto its stack. 28 | * A scheduler figures out which task to run next. 29 | * On task resume, all its context is popped off of its stack. 30 | 31 | ## Hacking 32 | 33 | The code was made to run on an ATmega328p, without portability in mind. 34 | The code can probably be ported to other variants without much effort. 35 | As I only have ATmega328p devices (Arduino's), I don't have an incentive to do 36 | this work. Pull requests are welcome. 37 | 38 | Links to a few resources I used: 39 | 40 | * [Multitasking on an AVR][1] -- outline for context save/restore routines 41 | * [avr-gcc][2] -- details about the compiler 42 | * [AVR instruction set][3] 43 | 44 | [1]: http://www.avrfreaks.net/modules/FreaksArticles/files/14/Multitasking%20on%20an%20AVR.pdf 45 | [2]: http://gcc.gnu.org/wiki/avr-gcc 46 | [3]: http://www.atmel.com/images/doc0856.pdf 47 | 48 | ## TODO 49 | 50 | * Communication / synchronization between tasks 51 | * Add task status field for run/sleep/wait/etc... 52 | * If waiting for I/O, how will a task be woken up? Likely through ISR not known 53 | to `task.c`. Maybe just call `task_yield()` from that handler. Maybe have 54 | some condition variable like apparatus to associate event X with task Y 55 | waiting for that event. 56 | 57 | ## License 58 | 59 | MIT (see ``LICENSE``). 60 | -------------------------------------------------------------------------------- /examples/i2c/i2c.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "i2c.h" 5 | #include "uart.h" 6 | #include "task.h" 7 | 8 | #define HMC5883L_ADDRESS (0x1e) 9 | 10 | void hmc5883l_init(void) { 11 | uint8_t b0[2] = { 0x0, _BV(4) }; 12 | uint8_t b1[2] = { 0x1, _BV(6) }; 13 | uint8_t b2[2] = { 0x2, 0 }; 14 | int8_t rv; 15 | 16 | // Initialize for continuous measurement at 15Hz 17 | rv = i2c_write(HMC5883L_ADDRESS, b0, sizeof(b0)); 18 | if (rv < 0) return; 19 | rv = i2c_write(HMC5883L_ADDRESS, b1, sizeof(b1)); 20 | if (rv < 0) return; 21 | rv = i2c_write(HMC5883L_ADDRESS, b2, sizeof(b2)); 22 | if (rv < 0) return; 23 | } 24 | 25 | void hmc5883l_measure(FILE *uart) { 26 | static char reinitialize = 1; 27 | uint8_t b3[1] = { 0x3 }; 28 | uint8_t data[6]; 29 | int16_t axis[3]; 30 | int8_t rv; 31 | 32 | if (reinitialize) { 33 | hmc5883l_init(); 34 | reinitialize = 0; 35 | } 36 | 37 | // Read measurement 38 | rv = i2c_write(HMC5883L_ADDRESS, b3, sizeof(b3)); 39 | if (rv < 0) { 40 | reinitialize = 1; 41 | return; 42 | } 43 | 44 | rv = i2c_read(HMC5883L_ADDRESS, data, sizeof(data)); 45 | if (rv < 0) { 46 | reinitialize = 1; 47 | return; 48 | } 49 | 50 | // Post-process measurement 51 | axis[0 /* X */] = (data[0] << 8) | data[1]; 52 | axis[2 /* Z */] = (data[2] << 8) | data[3]; 53 | axis[1 /* Y */] = (data[4] << 8) | data[5]; 54 | 55 | fprintf(uart, "X: %5d, Y: %5d, Z: %5d\r\n", axis[0], axis[1], axis[2]); 56 | } 57 | 58 | void hmc5883l_task(void *unused) { 59 | FILE uart = FDEV_SETUP_STREAM(uart_putc, uart_getc, _FDEV_SETUP_RW); 60 | 61 | while (1) { 62 | uint16_t t1, t2, msec; 63 | 64 | t1 = task_msec(); 65 | hmc5883l_measure(&uart); 66 | t2 = task_msec(); 67 | 68 | // This subtraction underflows if t1 > t2, but yields the right result. 69 | // Use measured time to space calls to "hmc5883l_measure" 100ms apart. 70 | msec = t2 - t1; 71 | task_sleep(100 - msec); 72 | } 73 | } 74 | 75 | int main() { 76 | i2c_init(); 77 | uart_init(); 78 | task_init(); 79 | 80 | task_create(hmc5883l_task, NULL); 81 | 82 | task_start(); 83 | 84 | return 0; // Never reached 85 | } 86 | -------------------------------------------------------------------------------- /task.h: -------------------------------------------------------------------------------- 1 | #ifndef _TASK_H 2 | #define _TASK_H 3 | 4 | #include 5 | 6 | #include "queue.h" 7 | 8 | #ifndef F_CPU 9 | #error "Define F_CPU" 10 | #endif 11 | 12 | #define MS_PER_TICK 2 13 | #define US_PER_TICK (1000 * MS_PER_TICK) 14 | #define US_PER_COUNT (US_PER_TICK / COUNTS_PER_TICK) 15 | 16 | #if F_CPU == 16000000L 17 | // Clock select: prescaler = 1/256 18 | #define _TCCR0B (_BV(CS02)) 19 | #define COUNTS_PER_TICK ((F_CPU / 256) / (1000 / MS_PER_TICK)) 20 | #elif F_CPU == 8000000L 21 | // Clock select: prescaler = 1/64 22 | #define _TCCR0B (_BV(CS01) | _BV(CS00)) 23 | #define COUNTS_PER_TICK ((F_CPU / 64) / (1000 / MS_PER_TICK)) 24 | #else 25 | #error "Unsupported F_CPU" 26 | #endif 27 | 28 | typedef void (*task_fn)(void *); 29 | 30 | typedef struct task_s task_t; 31 | 32 | struct task_s { 33 | void *sp; // Stack pointer this task can be resumed from. 34 | uint16_t delay; // Ticks until task can be scheduled again. 35 | 36 | QUEUE member; 37 | }; 38 | 39 | // Initialize internal structures, tick timer, etc. 40 | void task_init(void); 41 | 42 | // Creates a task for the specified function. 43 | task_t *task_create(task_fn fn, void *data); 44 | 45 | // Starts task execution. Never returns. 46 | void task_start(void); 47 | 48 | // Yield control from current task. 49 | void task_yield(void); 50 | 51 | // Return pointer to current task. 52 | task_t *task_current(void); 53 | 54 | // Suspend task until it is woken up explicitly. 55 | // The task is added to the tail of the queue pointed to by q. If q is NULL, 56 | // it is added to the system wide queue for suspended tasks. 57 | void task_suspend(QUEUE *h); 58 | 59 | // Wake up task. 60 | void task_wakeup(task_t *t); 61 | 62 | // Sleep current task for specified number of milliseconds. 63 | void task_sleep(uint16_t ms); 64 | 65 | // Only count seconds if specified 66 | #if TASK_COUNT_SEC 67 | #ifndef TASK_SEC_T 68 | #define TASK_SEC_T uint16_t 69 | #endif 70 | TASK_SEC_T task_sec(void); 71 | void task_set_sec(TASK_SEC_T); 72 | #endif 73 | 74 | // Only count milliseconds if specified 75 | #if TASK_COUNT_MSEC 76 | #ifndef TASK_MSEC_T 77 | #define TASK_MSEC_T uint16_t 78 | #endif 79 | TASK_MSEC_T task_msec(void); 80 | void task_set_msec(TASK_MSEC_T); 81 | #endif 82 | 83 | // Only count microseconds if specified 84 | #if TASK_COUNT_USEC 85 | #ifndef TASK_USEC_T 86 | #define TASK_USEC_T uint16_t 87 | #endif 88 | TASK_USEC_T task_usec(void); 89 | void task_set_usec(TASK_USEC_T); 90 | #endif 91 | 92 | #endif 93 | -------------------------------------------------------------------------------- /drivers/hd44780u.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include 5 | #include "hd44780u.h" 6 | 7 | typedef enum { 8 | INSTRUCTION = 0x0, 9 | DATA = 0x4, 10 | } mode_t; 11 | 12 | static void lcd_send(mode_t m, uint8_t b) { 13 | int8_t i; 14 | 15 | // Should be unrolled 16 | for (i = 0; i < 8; i++) { 17 | // Last bit first 18 | PORTB = (PORTB & 0b11111100) | (b & 0x80) >> 7; 19 | // Clock bit into shift register 20 | PORTB |= 0b00000010; 21 | b <<= 1; 22 | } 23 | 24 | // Last clock to move last bit to shift register output 25 | PORTB &= 0b11111100; 26 | PORTB |= 0b00000010; 27 | 28 | // Clock instruction into LCD 29 | PORTB |= m | 0b00001000; 30 | 31 | // Reset pins 32 | PORTB &= 0b11110000; 33 | } 34 | 35 | static void lcd_yield_usec(int16_t usec) { 36 | uint16_t t1; 37 | uint16_t t2; 38 | uint16_t dt; 39 | 40 | t1 = task_usec(); 41 | for (;;) { 42 | task_yield(); 43 | t2 = task_usec(); 44 | 45 | if (t2 > t1) { 46 | dt = t2 - t1; 47 | } else { 48 | dt = t2 + (UINT16_MAX - t1) + 1; 49 | } 50 | 51 | if (dt >= usec) { 52 | break; 53 | } 54 | 55 | usec -= dt; 56 | t1 = t2; 57 | } 58 | } 59 | 60 | void lcd_init(void) { 61 | // Function set: 8-bit data, 2 display lines, 5x8 font 62 | lcd_send(INSTRUCTION, 0b00111000); 63 | lcd_yield_usec(37); 64 | 65 | // Display control: on, no cursor, no blink 66 | lcd_send(INSTRUCTION, 0b00001100); 67 | lcd_yield_usec(37); 68 | 69 | // Entry mode set: increment cursor by 1 70 | lcd_send(INSTRUCTION, 0b00000110); 71 | lcd_yield_usec(37); 72 | 73 | // Clear and initialize cursor 74 | lcd_clear_display(); 75 | lcd_return_home(); 76 | } 77 | 78 | void lcd_clear_display(void) { 79 | lcd_send(INSTRUCTION, 0b00000001); 80 | lcd_yield_usec(1520); 81 | } 82 | 83 | void lcd_return_home(void) { 84 | lcd_send(INSTRUCTION, 0b00000010); 85 | lcd_yield_usec(1520); 86 | } 87 | 88 | void lcd_write(char c) { 89 | lcd_send(DATA, c); 90 | lcd_yield_usec(37); 91 | } 92 | 93 | void lcd_puts(const char *buf, int8_t len) { 94 | int8_t i; 95 | 96 | for (i = 0; i < len; i++) { 97 | lcd_write(buf[i]); 98 | } 99 | 100 | // The LCD advances to the next line after character 40 101 | for (; i < 40; i++) { 102 | lcd_send(INSTRUCTION, 0b00010100); 103 | lcd_yield_usec(37); 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /readline.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "readline.h" 4 | #include "uart.h" 5 | 6 | #define VT100_ERASE_EOL ("\x1b[K") 7 | #define VT100_CURSOR_FORWARD ("\x1b[C") 8 | #define VT100_CURSOR_BACKWARD ("\x1b[D") 9 | 10 | int8_t readline(const char *prompt, char *buf, int8_t bufsz) { 11 | char tbuf[6]; 12 | int8_t tlen; 13 | 14 | if (prompt != NULL) { 15 | uart_write(prompt, strlen(prompt)); 16 | } 17 | 18 | int8_t len = 0; 19 | int8_t pos = 0; 20 | 21 | for (;;) { 22 | char c = uart_getc(NULL); 23 | char c_; 24 | 25 | if (c >= 0x20 && c < 0x7f) { 26 | // Readable character 27 | if (len >= bufsz) { 28 | continue; 29 | } 30 | 31 | if (pos < len) { 32 | int8_t tail = len - pos; 33 | 34 | // Move tail one character to the end 35 | memmove(&buf[pos+1], &buf[pos], tail); 36 | buf[pos] = c; 37 | uart_write(VT100_ERASE_EOL, sizeof(VT100_ERASE_EOL)); 38 | uart_write(&buf[pos], tail+1); 39 | tlen = snprintf(tbuf, sizeof(tbuf), "\x1b[%dD", tail); 40 | uart_write(tbuf, tlen); 41 | } else { 42 | // Add to tail 43 | buf[pos] = c; 44 | uart_write(&c, 1); 45 | } 46 | pos++; 47 | len++; 48 | } else if (c == 0x0d) { 49 | // Write CRLF to confirm 50 | uart_write("\r\n", 2); 51 | break; 52 | } else if (c == 0x08) { 53 | // Backspace 54 | if (pos == 0) { 55 | continue; 56 | } 57 | 58 | if (pos < len) { 59 | int8_t tail = len - pos; 60 | 61 | // Move tail one character to the start 62 | memmove(&buf[pos-1], &buf[pos], tail); 63 | uart_write(VT100_CURSOR_BACKWARD, sizeof(VT100_CURSOR_BACKWARD)); 64 | uart_write(VT100_ERASE_EOL, sizeof(VT100_ERASE_EOL)); 65 | uart_write(&buf[pos-1], tail); 66 | tlen = snprintf(tbuf, sizeof(tbuf), "\x1b[%dD", tail); 67 | uart_write(tbuf, tlen); 68 | } else { 69 | // Remove from tail 70 | uart_write(VT100_CURSOR_BACKWARD, sizeof(VT100_CURSOR_BACKWARD)); 71 | uart_write(VT100_ERASE_EOL, sizeof(VT100_ERASE_EOL)); 72 | } 73 | pos--; 74 | len--; 75 | } else if (c == 0x1b) { 76 | // Escape sequence 77 | c_ = uart_getc(NULL); 78 | if (c_ != '[') { 79 | // Discard 80 | continue; 81 | } 82 | 83 | c_ = uart_getc(NULL); 84 | if (c_ == 'C') { 85 | // Move cursor forward one char 86 | if (pos < len) { 87 | pos++; 88 | 89 | // Confirm 90 | uart_write("\x1b[C", 3); 91 | } 92 | } else if (c_ == 'D') { 93 | // Move cursor backward one char 94 | if (pos > 0) { 95 | pos--; 96 | 97 | // Confirm 98 | uart_write("\x1b[D", 3); 99 | } 100 | } else { 101 | // Not handled 102 | } 103 | } else { 104 | // Not handled 105 | } 106 | } 107 | 108 | return len; 109 | } 110 | -------------------------------------------------------------------------------- /drivers/sirc.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | 8 | // Length of header pulse. 9 | #define HEADER_MIN_US (uint16_t)(_SIRC_HEADER_PULSE_US - _SIRC_HEADER_ERROR_US) 10 | #define HEADER_MAX_US (uint16_t)(_SIRC_HEADER_PULSE_US + _SIRC_HEADER_ERROR_US) 11 | 12 | // Length of one pulse. 13 | #define ONE_MIN_US (uint16_t)(_SIRC_ONE_PULSE_US - _SIRC_ONE_ERROR_US) 14 | #define ONE_MAX_US (uint16_t)(_SIRC_ONE_PULSE_US + _SIRC_ONE_ERROR_US) 15 | 16 | // Length of zero pulse. 17 | #define ZERO_MIN_US (uint16_t)(_SIRC_ZERO_PULSE_US - _SIRC_ZERO_ERROR_US) 18 | #define ZERO_MAX_US (uint16_t)(_SIRC_ZERO_PULSE_US + _SIRC_ZERO_ERROR_US) 19 | 20 | // Length of delay between pulses. 21 | #define DELAY_MIN_US (uint16_t)(_SIRC_DELAY_US - _SIRC_DELAY_ERROR_US) 22 | #define DELAY_MAX_US (uint16_t)(_SIRC_DELAY_US + _SIRC_DELAY_ERROR_US) 23 | 24 | // Initialize the decoder. 25 | void sirc_init() { 26 | // Configure DDB0 as input (port B, pin 0, Arduino pin 8). 27 | DDRB &= ~_BV(DDB0); 28 | PORTB |= _BV(PORTB0); // Pull-up 29 | 30 | // Enable pin change interrupts on bank 1 (port B) 31 | PCICR |= _BV(PCIE0); 32 | } 33 | 34 | // Enable pin change interrupt on bank 1, pin 0 (pin 8). 35 | static void sirc__enable() { 36 | PCMSK0 |= _BV(PCINT0); 37 | } 38 | 39 | // Disable pin change interrupt on bank 1, pin 0 (pin 8). 40 | static void sirc__disable() { 41 | PCMSK0 &= ~_BV(PCINT0); 42 | } 43 | 44 | // Task waiting for code. 45 | static task_t *task = NULL; 46 | 47 | // Bit index. 48 | static uint8_t bit = 0; 49 | 50 | // Code accumulator. 51 | static uint16_t code = 0; 52 | 53 | // Last interrupt trigger. 54 | static uint16_t prev_us = 0; 55 | 56 | // Most recent delay duration. 57 | static uint16_t delay_us = 0; 58 | 59 | ISR(PCINT0_vect) { 60 | uint16_t now_us, diff_us, pulse_us; 61 | 62 | now_us = task_us(); 63 | if (prev_us < now_us) { 64 | diff_us = now_us - prev_us; 65 | } else { 66 | diff_us = now_us + (UINT16_MAX - prev_us); 67 | } 68 | 69 | // Store current time for next edge. 70 | prev_us = now_us; 71 | 72 | // Pin flipped to state for pulse start; store diff as delay time. 73 | if ((PINB & _BV(PINB0)) == _SIRC_PULSE_START) { 74 | delay_us = diff_us; 75 | return; 76 | } 77 | 78 | // Pin flipped to state for pulse end. 79 | pulse_us = diff_us; 80 | 81 | // Check for header if needed. 82 | if (bit == 0) { 83 | if (pulse_us >= HEADER_MIN_US && pulse_us <= HEADER_MAX_US) { 84 | bit = 1; 85 | code = 0; 86 | } 87 | return; 88 | } 89 | 90 | // Expect delay. 91 | if (delay_us < DELAY_MIN_US || delay_us > DELAY_MAX_US) { 92 | // Reset. 93 | bit = 0; 94 | return; 95 | } 96 | 97 | // Expect one or zero. 98 | if (pulse_us >= ONE_MIN_US && pulse_us <= ONE_MAX_US) { 99 | code |= (1 << (bit - 1)); 100 | } else if (pulse_us >= ZERO_MIN_US && pulse_us <= ZERO_MAX_US) { 101 | // No-op. 102 | } else { 103 | // Reset. 104 | bit = 0; 105 | return; 106 | } 107 | 108 | // Wake up task after receiving enough bits. 109 | if (bit == BITS) { 110 | sirc__disable(); 111 | task_wakeup(task); 112 | 113 | // Reset. 114 | bit = 0; 115 | } else { 116 | bit++; 117 | } 118 | } 119 | 120 | // Block until code is read. 121 | uint16_t sirc_read() { 122 | sirc__enable(); 123 | task = task_current(); 124 | task_suspend(NULL); 125 | return code; 126 | } 127 | -------------------------------------------------------------------------------- /drivers/hmc5883l.h: -------------------------------------------------------------------------------- 1 | #ifndef _HMC5883L_H 2 | #define _HMC5883L_H 3 | 4 | /* 5 | * From the datasheet: 6 | * 7 | * The Honeywell HMC5883L is a surface-mount, multi-chip module designed for 8 | * low-field magnetic sensing with a digital interface for applications such as 9 | * low-cost compassing and magnetometry. The HMC5883L includes our 10 | * state-of-the-art, high-resolution HMC118X series magneto-resistive sensors 11 | * plus an ASIC containing amplification, automatic degaussing strap drivers, 12 | * offset cancellation, and a 12-bit ADC that enables 1 to 2 degree compass 13 | * heading accuracy. 14 | * 15 | * Notes: 16 | * 17 | * The module can be configured to use any of 8 gain modes to increase the 18 | * measurement resolution at the cost of a lower range. The digital 19 | * measurements can be converted to milli-gauss by applying a scaling factor. 20 | * This scaling factor is a combination of a static factor that is documented 21 | * and a dynamic factor that must be determined at run-time. The static factor 22 | * depends on the selected gain mode and can be found in the datasheet. The 23 | * dynamic factor is determined by executing a self-test built into the module. 24 | * The self-test takes the difference between two measurements: one for 25 | * reference and one where an ~1.1 Gauss field is added to the existing field. 26 | * Because the expected measurement value for this difference is known 27 | * (depending on the gain mode), this measurement allows scaling to compensate 28 | * for environment effects (e.g. temperature). 29 | */ 30 | 31 | #include 32 | 33 | // Configuration register A. 34 | #define MA1 _BV(6) 35 | #define MA0 _BV(5) 36 | #define DO2 _BV(4) 37 | #define DO1 _BV(3) 38 | #define DO0 _BV(2) 39 | #define MS1 _BV(1) 40 | #define MS0 _BV(0) 41 | 42 | // Number of samples averaged (1 to 8) per measurement output. 43 | #define SAMPLE_1 (0) 44 | #define SAMPLE_2 (MA0) 45 | #define SAMPLE_4 (MA1) 46 | #define SAMPLE_8 (MA1 | MA0) 47 | 48 | // Data Output Rate. 49 | #define RATE_0_75_HZ (0) 50 | #define RATE_1_5_HZ (DO0) 51 | #define RATE_3_HZ (DO1) 52 | #define RATE_7_5_HZ (DO1 | DO0) 53 | #define RATE_15_HZ (DO2) 54 | #define RATE_30_HZ (DO2 | DO0) 55 | #define RATE_75_HZ (DO2 | DO1) 56 | 57 | // Measurement Configuration. 58 | #define MEASURE_NORMAL (0) 59 | #define MEASURE_POS_BIAS (MS0) 60 | #define MEASURE_NEG_BIAS (MS1) 61 | 62 | // Configuration register B. 63 | #define GN2 _BV(7) 64 | #define GN1 _BV(6) 65 | #define GN0 _BV(5) 66 | 67 | // Gain Configuration. 68 | #define GAIN_1370 (0) 69 | #define GAIN_1090 (GN0) 70 | #define GAIN_820 (GN1) 71 | #define GAIN_660 (GN1 | GN0) 72 | #define GAIN_440 (GN2) 73 | #define GAIN_390 (GN2 | GN0) 74 | #define GAIN_330 (GN2 | GN1) 75 | #define GAIN_230 (GN2 | GN1 | GN0) 76 | 77 | // Mode register. 78 | #define HS _BV(7) 79 | #define MD1 _BV(1) 80 | #define MD0 _BV(0) 81 | 82 | // Mode Select. 83 | #define MODE_CONTINOUS (0) 84 | #define MODE_SINGLE (MD0) 85 | #define MODE_IDLE (MD1) 86 | 87 | typedef struct hmc5883l_s hmc5883l_t; 88 | 89 | struct hmc5883l_s { 90 | // Offset and scale determined by continuous self-test. 91 | // The result of these adjustments are values in milli-gauss. 92 | int16_t cali_offset[3]; 93 | float cali_scale[3]; 94 | 95 | // Offset and scale determined by one-time manual analysis. 96 | // Different scaling factors for positive and negative measurements are the 97 | // result of once observing a non-linearity where the negative range was 98 | // larger than the positive range while the axis was centered correctly. 99 | // This can be worked around by scaling these ranges independently. 100 | int16_t extra_offset[3]; 101 | float extra_scale_pos[3]; 102 | float extra_scale_neg[3]; 103 | }; 104 | 105 | int8_t hmc5883l_configure(uint8_t a, uint8_t b, uint8_t m); 106 | 107 | int8_t hmc5883l_read(int16_t axis[3]); 108 | 109 | void hmc5883l_init(hmc5883l_t *h); 110 | 111 | int8_t hmc5883l_calibrate(hmc5883l_t *h, uint8_t gain); 112 | 113 | int8_t hmc5883l_read_scaled(hmc5883l_t *h, int16_t axis[3]); 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /drivers/hmc5883l.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "hmc5883l.h" 4 | #include "i2c.h" 5 | #include "task.h" 6 | 7 | #define HMC5883L_ADDRESS 0x1E 8 | 9 | // Initialize the sensor by writing to its configuration and mode registers. 10 | int8_t hmc5883l_configure(uint8_t a, uint8_t b, uint8_t m) { 11 | uint8_t b0[2] = { 0x0, a }; 12 | uint8_t b1[2] = { 0x1, b }; 13 | uint8_t b2[2] = { 0x2, m }; 14 | int8_t rv; 15 | 16 | i2c_open(); 17 | 18 | rv = i2c_write(HMC5883L_ADDRESS, b0, sizeof(b0)); 19 | if (rv < 0) { 20 | goto done; 21 | } 22 | 23 | rv = i2c_write(HMC5883L_ADDRESS, b1, sizeof(b1)); 24 | if (rv < 0) { 25 | goto done; 26 | } 27 | 28 | rv = i2c_write(HMC5883L_ADDRESS, b2, sizeof(b2)); 29 | if (rv < 0) { 30 | goto done; 31 | } 32 | 33 | done: 34 | i2c_close(); 35 | return rv; 36 | } 37 | 38 | // Read raw values from sensor. 39 | // Prior to calling this function, the sensor either had to be configured to 40 | // take a single measurement (MODE_SINGLE), or to be in continuous measurement 41 | // mode (MODE_CONTINOUS). 42 | int8_t hmc5883l_read(int16_t axis[3]) { 43 | uint8_t b3[1] = { 0x03 }; 44 | uint8_t data[6]; 45 | int8_t rv; 46 | 47 | i2c_open(); 48 | 49 | rv = i2c_write(HMC5883L_ADDRESS, b3, sizeof(b3)); 50 | if (rv < 0) { 51 | goto done; 52 | } 53 | 54 | rv = i2c_read(HMC5883L_ADDRESS, data, sizeof(data)); 55 | if (rv < 0) { 56 | goto done; 57 | } 58 | 59 | axis[0 /* X */] = (data[0] << 8) | data[1]; 60 | axis[2 /* Z */] = (data[2] << 8) | data[3]; 61 | axis[1 /* Y */] = (data[4] << 8) | data[5]; 62 | 63 | done: 64 | i2c_close(); 65 | return rv; 66 | } 67 | 68 | // Initialize sensor struct. 69 | void hmc5883l_init(hmc5883l_t *h) { 70 | int8_t i; 71 | 72 | for (i = 0; i < 3; i++) { 73 | h->cali_offset[i] = 0; 74 | h->cali_scale[i] = 1.0; 75 | 76 | h->extra_offset[i] = 0; 77 | h->extra_scale_pos[i] = 1.0; 78 | h->extra_scale_neg[i] = 1.0; 79 | } 80 | } 81 | 82 | // Execute sensor self-test to fill in the offset and scale struct fields. 83 | int8_t hmc5883l_calibrate(hmc5883l_t *h, uint8_t gain) { 84 | int8_t rv; 85 | int16_t pos_axis[3], neg_axis[3]; 86 | uint16_t lsb_per_gauss = 0; 87 | 88 | rv = hmc5883l_configure(SAMPLE_8 | MEASURE_POS_BIAS, gain, MODE_SINGLE); 89 | if (rv < 0) { 90 | return rv; 91 | } 92 | 93 | task_sleep(8); 94 | rv = hmc5883l_read(pos_axis); 95 | if (rv < 0) { 96 | return rv; 97 | } 98 | 99 | rv = hmc5883l_configure(SAMPLE_8 | MEASURE_NEG_BIAS, gain, MODE_SINGLE); 100 | if (rv < 0) { 101 | return rv; 102 | } 103 | 104 | task_sleep(8); 105 | rv = hmc5883l_read(neg_axis); 106 | if (rv < 0) { 107 | return rv; 108 | } 109 | 110 | switch(gain) { 111 | case GAIN_1370: lsb_per_gauss = 1370; break; 112 | case GAIN_1090: lsb_per_gauss = 1090; break; 113 | case GAIN_820: lsb_per_gauss = 820; break; 114 | case GAIN_660: lsb_per_gauss = 660; break; 115 | case GAIN_440: lsb_per_gauss = 440; break; 116 | case GAIN_390: lsb_per_gauss = 390; break; 117 | case GAIN_330: lsb_per_gauss = 330; break; 118 | case GAIN_230: lsb_per_gauss = 230; break; 119 | } 120 | 121 | // Both the positively and negatively biased measurements should yield the 122 | // same values. This offset compensates for that measurement error. 123 | h->cali_offset[0] = -(pos_axis[0] + neg_axis[0]) / 2; 124 | h->cali_offset[1] = -(pos_axis[1] + neg_axis[1]) / 2; 125 | h->cali_offset[2] = -(pos_axis[2] + neg_axis[2]) / 2; 126 | 127 | // Static scaling factor for specified gain mode 128 | h->cali_scale[0] = 1000.0 / (float)lsb_per_gauss; 129 | h->cali_scale[1] = 1000.0 / (float)lsb_per_gauss; 130 | h->cali_scale[2] = 1000.0 / (float)lsb_per_gauss; 131 | 132 | // Dynamic scaling factor for specified gain mode 133 | h->cali_scale[0] *= 1.160 / ((pos_axis[0] + h->cali_offset[0]) / (float)lsb_per_gauss); 134 | h->cali_scale[1] *= 1.160 / ((pos_axis[1] + h->cali_offset[1]) / (float)lsb_per_gauss); 135 | h->cali_scale[2] *= 1.080 / ((pos_axis[2] + h->cali_offset[2]) / (float)lsb_per_gauss); 136 | 137 | return 0; 138 | } 139 | 140 | // Read scaled values from sensor. 141 | int8_t hmc5883l_read_scaled(hmc5883l_t *h, int16_t axis[3]) { 142 | int8_t rv; 143 | int8_t i; 144 | 145 | rv = hmc5883l_read(axis); 146 | if (rv < 0) { 147 | return rv; 148 | } 149 | 150 | for (i = 0; i < 3; i++) { 151 | axis[i] += h->cali_offset[i]; 152 | axis[i] *= h->cali_scale[i]; 153 | 154 | axis[i] += h->extra_offset[i]; 155 | if (axis[i] > 0) { 156 | axis[i] *= h->extra_scale_pos[i]; 157 | } else { 158 | axis[i] *= h->extra_scale_neg[i]; 159 | } 160 | } 161 | 162 | return 0; 163 | } 164 | -------------------------------------------------------------------------------- /drivers/mma8452q.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "i2c.h" 4 | #include "mma8452q.h" 5 | 6 | /* Slave address selection bit */ 7 | #define SA0 1 8 | #define MMA8452Q_ADDRESS (0x1C | (SA0)) 9 | 10 | /* Data Status Register */ 11 | #define STATUS 0x00 12 | 13 | /* Data Registers */ 14 | #define OUT_X_MSB 0x01 15 | #define OUT_X_LSB 0x02 16 | #define OUT_Y_MSB 0x03 17 | #define OUT_Y_LSB 0x04 18 | #define OUT_Z_MSB 0x05 19 | #define OUT_Z_LSB 0x06 20 | 21 | /* System Mode Register */ 22 | #define SYSMOD 0x0B 23 | 24 | /* System Interrupt Status Register */ 25 | #define INT_SOURCE 0x0C 26 | 27 | /* Device ID Register */ 28 | #define WHO_AM_I 0x0D 29 | 30 | #define XYZ_DATA_CFG 0x0E 31 | 32 | #define HPF_OUT _BV(4) 33 | #define FS1 _BV(1) 34 | #define FS0 _BV(0) 35 | 36 | #define RANGE_2G (0) 37 | #define RANGE_4G (FS0) 38 | #define RANGE_8G (FS1) 39 | 40 | #define HP_FILTER_CUTOFF 0x0F 41 | 42 | #define PULSE_HPF_BYP _BV(5) 43 | #define PULSE_LPF_EN _BV(4) 44 | #define SEL1 _BV(1) 45 | #define SEL0 _BV(0) 46 | 47 | /* Portrait/ Landscape Embedded Function Registers */ 48 | #define PL_STATUS 0x10 49 | #define PL_CFG 0x11 50 | #define PL_COUNT 0x12 51 | #define PL_BF_ZCOMP 0x13 52 | #define PL_THS_REG 0x14 53 | 54 | /* Motion and Freefall Embedded Function Registers */ 55 | #define FF_MT_CFG 0x15 56 | #define FF_MT_SRC 0x16 57 | #define FF_MT_THS 0x17 58 | #define FF_MT_COUNT 0x18 59 | 60 | /* Transient (HPF) Acceleration Detection */ 61 | #define TRANSIENT_CFG 0x1D 62 | #define TRANSIENT_SRC 0x1E 63 | #define TRANSIENT_THS 0x1F 64 | #define TRANSIENT_COUNT 0x20 65 | 66 | /* Single, Double, and Directional Tap Detection Registers */ 67 | #define PULSE_CFG 0x21 68 | #define PULSE_SRC 0x22 69 | #define PULSE_THSX 0x23 70 | #define PULSE_THSY 0x24 71 | #define PULSE_THSZ 0x25 72 | #define PULSE_TMLT 0x26 73 | #define PULSE_LTCY 0x27 74 | #define PULSE_WIND 0x28 75 | 76 | /* Auto-WAKE/SLEEP Detection */ 77 | #define ASLP_COUNT 0x29 78 | 79 | /* Control Registers */ 80 | #define CTRL_REG1 0x2A 81 | #define CTRL_REG2 0x2B 82 | #define CTRL_REG3 0x2C 83 | #define CTRL_REG4 0x2D 84 | #define CTRL_REG5 0x2E 85 | 86 | /* User Offset Correction Registers */ 87 | #define OFF_X 0x2F 88 | #define OFF_Y 0x30 89 | #define OFF_Z 0x31 90 | 91 | /* CTRL_REG1 */ 92 | #define ASLP_RATE1 _BV(7) 93 | #define ASLP_RATE0 _BV(6) 94 | #define DR2 _BV(5) 95 | #define DR1 _BV(4) 96 | #define DR0 _BV(3) 97 | #define LNOISE _BV(2) 98 | #define F_READ _BV(1) 99 | #define ACTIVE _BV(0) 100 | 101 | #define ASLP_RATE_50HZ (0) 102 | #define ASLP_RATE_12HZ (ASLP_RATE0) 103 | #define ASLP_RATE_6HZ (ASLP_RATE1) 104 | #define ASLP_RATE_1HZ (ASLP_RATE1 | ASLP_RATE0) 105 | 106 | #define DR_800HZ (0) 107 | #define DR_400HZ (DR0) 108 | #define DR_200HZ (DR1) 109 | #define DR_100HZ (DR1 | DR0) 110 | #define DR_50HZ (DR2) 111 | #define DR_12HZ (DR2 | DR0) 112 | #define DR_6HZ (DR2 | DR1) 113 | #define DR_1HZ (DR2 | DR1 | DR0) 114 | 115 | /* CTRL_REG2 */ 116 | #define ST _BV(7) 117 | #define RST _BV(6) 118 | #define SMODS1 _BV(4) 119 | #define SMODS0 _BV(3) 120 | #define SLPE _BV(2) 121 | #define MODS1 _BV(1) 122 | #define MODS0 _BV(0) 123 | 124 | /* CTRL_REG3 */ 125 | #define WAKE_TRANS _BV(6) 126 | #define WAKE_LNDPRT _BV(5) 127 | #define WAKE_PULSE _BV(4) 128 | #define WAKE_FF_MT _BV(3) 129 | #define IPOL _BV(1) 130 | #define PP_OD _BV(0) 131 | 132 | /* CTRL_REG4 */ 133 | #define INT_EN_ASLP _BV(7) 134 | #define INT_EN_TRANS _BV(5) 135 | #define INT_EN_LNDPRT _BV(4) 136 | #define INT_EN_PULSE _BV(3) 137 | #define INT_EN_FF_MT _BV(2) 138 | #define INT_EN_DRDY _BV(0) 139 | 140 | /* CTRL_REG5 */ 141 | #define INT_CFG_ASLP _BV(7) 142 | #define INT_CFG_TRANS _BV(5) 143 | #define INT_CFG_LNDPRT _BV(4) 144 | #define INT_CFG_PULSE _BV(3) 145 | #define INT_CFG_FF_MT _BV(2) 146 | #define INT_CFG_DRDY _BV(0) 147 | 148 | // Public initialization function (only sets data rate). 149 | int8_t mma8452q_configure(uint8_t d) { 150 | int8_t rv; 151 | 152 | i2c_open(); 153 | 154 | // Switch sensor to STANDBY mode. 155 | { 156 | uint8_t ctrl[6] = { CTRL_REG1, 0, 0, 0, 0, 0 }; 157 | rv = i2c_write(MMA8452Q_ADDRESS, ctrl, 6); 158 | if (rv < 0) { 159 | goto done; 160 | } 161 | } 162 | 163 | // Set resolution. 164 | // This only takes effect if the sensor is in STANDBY mode. 165 | { 166 | uint8_t hpf[] = { XYZ_DATA_CFG, RANGE_2G, 0 }; 167 | 168 | rv = i2c_write(MMA8452Q_ADDRESS, hpf, sizeof(hpf)); 169 | if (rv < 0) { 170 | goto done; 171 | } 172 | } 173 | 174 | // Switch sensor to ACTIVE mode. 175 | { 176 | uint8_t ctrl[6] = { CTRL_REG1, d | ACTIVE, 0, 0, 0, 0 }; 177 | rv = i2c_write(MMA8452Q_ADDRESS, ctrl, 6); 178 | if (rv < 0) { 179 | goto done; 180 | } 181 | } 182 | 183 | done: 184 | i2c_close(); 185 | return rv; 186 | } 187 | 188 | int8_t mma8452q_read(int16_t axis[3]) { 189 | uint8_t r = OUT_X_MSB; 190 | uint8_t data[6]; 191 | int8_t rv, i; 192 | 193 | i2c_open(); 194 | 195 | rv = i2c_write(MMA8452Q_ADDRESS, &r, 1); 196 | if (rv < 0) { 197 | goto error; 198 | } 199 | 200 | rv = i2c_read(MMA8452Q_ADDRESS, data, 6); 201 | if (rv < 0) { 202 | goto error; 203 | } 204 | 205 | // Every axis has two registers holding a measurement value MSB and LSB. 206 | // The compiler uses little endian representation, so we need to swap these 207 | // bytes before we can interpret the value as an 16-bit integer. 208 | for (i = 0; i < 6; i += 2) { 209 | r = data[i]; 210 | data[i] = data[i+1]; 211 | data[i+1] = r; 212 | } 213 | 214 | // Drop lowest 4 bits (measurements are 12-bit values). 215 | for (i = 0; i < 3; i++) { 216 | axis[i] = ((int16_t *)data)[i] >> 4; 217 | } 218 | 219 | error: 220 | i2c_close(); 221 | return rv; 222 | } 223 | -------------------------------------------------------------------------------- /queue.h: -------------------------------------------------------------------------------- 1 | /* Copyright (c) 2013, Ben Noordhuis 2 | * 3 | * Permission to use, copy, modify, and/or distribute this software for any 4 | * purpose with or without fee is hereby granted, provided that the above 5 | * copyright notice and this permission notice appear in all copies. 6 | * 7 | * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 8 | * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 9 | * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 10 | * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 11 | * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 12 | * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 13 | * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 14 | */ 15 | 16 | #ifndef QUEUE_H_ 17 | #define QUEUE_H_ 18 | 19 | #include 20 | 21 | typedef void *QUEUE[2]; 22 | 23 | /* Private macros. */ 24 | #define QUEUE_NEXT(q) ((*(q))[0]) 25 | #define QUEUE_PREV(q) ((*(q))[1]) 26 | #define QUEUE_PREV_NEXT(q) (QUEUE_NEXT((QUEUE *) QUEUE_PREV(q))) 27 | #define QUEUE_NEXT_PREV(q) (QUEUE_PREV((QUEUE *) QUEUE_NEXT(q))) 28 | 29 | /* Public macros. */ 30 | #define QUEUE_DATA(ptr, type, field) \ 31 | ((type *) ((char *) (ptr) - ((uintptr_t) &((type *) 0)->field))) 32 | 33 | #define QUEUE_FOREACH(q, h) \ 34 | for ((q) = (QUEUE *) (*(h))[0]; (q) != (h); (q) = (QUEUE *) (*(q))[0]) 35 | 36 | #define QUEUE_EMPTY(q) \ 37 | (QUEUE_NEXT(q) == (q)) 38 | 39 | #define QUEUE_HEAD(q) \ 40 | (QUEUE_NEXT(q)) 41 | 42 | #define QUEUE_INIT(q) \ 43 | do { \ 44 | QUEUE_NEXT(q) = (q); \ 45 | QUEUE_PREV(q) = (q); \ 46 | } \ 47 | while (0) 48 | 49 | #define QUEUE_ADD(h, n) \ 50 | do { \ 51 | QUEUE_PREV_NEXT(h) = QUEUE_NEXT(n); \ 52 | QUEUE_NEXT_PREV(n) = QUEUE_PREV(h); \ 53 | QUEUE_PREV(h) = QUEUE_PREV(n); \ 54 | QUEUE_PREV_NEXT(h) = (h); \ 55 | } \ 56 | while (0) 57 | 58 | #define QUEUE_SPLIT(h, q, n) \ 59 | do { \ 60 | QUEUE_PREV(n) = QUEUE_PREV(h); \ 61 | QUEUE_PREV_NEXT(n) = (n); \ 62 | QUEUE_NEXT(n) = (q); \ 63 | QUEUE_PREV(h) = QUEUE_PREV(q); \ 64 | QUEUE_PREV_NEXT(h) = (h); \ 65 | QUEUE_PREV(q) = (n); \ 66 | } \ 67 | while (0) 68 | 69 | #define QUEUE_INSERT_HEAD(h, q) \ 70 | do { \ 71 | QUEUE_NEXT(q) = QUEUE_NEXT(h); \ 72 | QUEUE_PREV(q) = (h); \ 73 | QUEUE_NEXT_PREV(q) = (q); \ 74 | QUEUE_NEXT(h) = (q); \ 75 | } \ 76 | while (0) 77 | 78 | #define QUEUE_INSERT_TAIL(h, q) \ 79 | do { \ 80 | QUEUE_NEXT(q) = (h); \ 81 | QUEUE_PREV(q) = QUEUE_PREV(h); \ 82 | QUEUE_PREV_NEXT(q) = (q); \ 83 | QUEUE_PREV(h) = (q); \ 84 | } \ 85 | while (0) 86 | 87 | #define QUEUE_REMOVE(q) \ 88 | do { \ 89 | QUEUE_PREV_NEXT(q) = QUEUE_NEXT(q); \ 90 | QUEUE_NEXT_PREV(q) = QUEUE_PREV(q); \ 91 | } \ 92 | while (0) 93 | 94 | // Make head through q the new tail, q->next through tail the new head. 95 | // Note that this is a no-op if QUEUE_NEXT(q) == (h). 96 | #define QUEUE_ROTATE(h, q) \ 97 | if (QUEUE_NEXT(q) != (h)) { \ 98 | QUEUE_NEXT_PREV(h) = QUEUE_PREV(h); \ 99 | QUEUE_PREV_NEXT(h) = QUEUE_NEXT(h); \ 100 | QUEUE_PREV(h) = (q); \ 101 | QUEUE_NEXT(h) = QUEUE_NEXT(q); \ 102 | QUEUE_PREV_NEXT(h) = (h); \ 103 | QUEUE_NEXT_PREV(h) = (h); \ 104 | } 105 | 106 | #endif /* QUEUE_H_ */ 107 | -------------------------------------------------------------------------------- /uart.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "uart.h" 6 | #include "task.h" 7 | 8 | #define _B(x, on) ((on) * _BV(x)) 9 | 10 | static task_t *tx_task = NULL; 11 | static const uint8_t *tx_buf; 12 | static uint8_t tx_count; 13 | 14 | static task_t *rx_task = NULL; 15 | static uint8_t *rx_buf; 16 | static uint8_t rx_count; 17 | 18 | #ifdef UART_COUNT_TX_BYTES 19 | uint16_t uart_tx_bytes = 0; 20 | #endif 21 | 22 | #ifdef UART_COUNT_RX_BYTES 23 | uint16_t uart_rx_bytes = 0; 24 | #endif 25 | 26 | #ifdef UART_COUNT_RX_ERRORS 27 | uint8_t uart_rx_fe = 0; 28 | uint8_t uart_rx_dor = 0; 29 | uint8_t uart_rx_pe = 0; 30 | uint8_t uart_rx_bdor = 0; 31 | #endif 32 | 33 | #define PRIV_BUF_SIZE (1<<4) 34 | #define PRIV_BUF_SIZE_MASK ((PRIV_BUF_SIZE)-1) 35 | #define PRIV_BUF_VAL(var) ((var) & PRIV_BUF_SIZE_MASK) 36 | 37 | // Toggle the MSB every time an index wraps. 38 | // This tells ppos and cpos apart if they are equal (empty vs full). 39 | #define PRIV_BUF_INCR(var) do { \ 40 | (var)++; \ 41 | if (PRIV_BUF_VAL(var) == 0) { \ 42 | (var) = (~((var) & 0x80) & 0x80); \ 43 | } \ 44 | } while (0); 45 | 46 | // Private receive buffer. 47 | static uint8_t rx_priv_buf[PRIV_BUF_SIZE]; 48 | static uint8_t rx_priv_ppos; // Producer 49 | static uint8_t rx_priv_cpos; // Consumer 50 | 51 | // Register descriptions as documented in the ATmega328p datasheet. 52 | // 53 | // Bits in UCSR0A, USART Control and Status Register 0A 54 | // RXC0: USART Receive Complete 55 | // TXC0: USART Transmit Complete 56 | // UDRE0: USART Data Register Empty 57 | // FE0: Frame Error 58 | // DOR0: Data Overrun 59 | // UPE0: USART Parity Error 60 | // U2X0: Double the USART Transmission Speed 61 | // MPCM0: Multi-processor Communication Mode 62 | // 63 | // Bits in UCSR0B, USART Control and Status Register 0B 64 | // RXCIE0: RX Complete Interrupt Enable 65 | // TXCIE0: TX Complete Interrupt Enable 66 | // UDRIE0: USART Data Register Empty Interrupt Enable 67 | // RXEN0: Receiver Enable 68 | // TXEN0: Transmitter Enable 69 | // UCSZ02: Character Size bit 2 70 | // RXB80: Receive Data Bit 8 71 | // TXB80: Transmit Data Bit 8 72 | // 73 | // Bits in UCSR0C, USART Control and Status Register 0C 74 | // UMSEL01: USART Mode Select bit 1 75 | // UMSEL00: USART Mode Select bit 0 76 | // UPM01: Parity Mode bit 1 77 | // UPM00: Parity Mode bit 0 78 | // USBS0: Stop Bit Select 79 | // UCSZ01: Character Size bit 1 80 | // UCSZ00: Character Size bit 0 81 | // UCPOL0: Clock Polarity 82 | // 83 | 84 | // Some of the assignments here evaluate to 0 making them a no-op. 85 | // They are included as documentation. 86 | void uart_init(uint16_t ubrr, uint8_t x2) { 87 | uint8_t sreg; 88 | 89 | sreg = SREG; 90 | cli(); 91 | 92 | UBRR0 = ubrr; 93 | 94 | UCSR0A = 0; 95 | UCSR0B = 0; 96 | UCSR0C = 0; 97 | 98 | // Double the transmission speed 99 | if (x2) { 100 | UCSR0A |= _B(U2X0, 1); 101 | } 102 | 103 | // Asynchronous USART 104 | UCSR0C |= _B(UMSEL01, 0) | _B(UMSEL00, 0); 105 | 106 | // 8 bit character size 107 | UCSR0B |= _B(UCSZ02, 0); 108 | UCSR0C |= _B(UCSZ01, 1) | _B(UCSZ00, 1); 109 | 110 | // No parity bit 111 | UCSR0C |= _B(UPM01, 0) | _B(UPM00, 0); 112 | 113 | // 1 stop bit 114 | UCSR0C |= _B(USBS0, 0); 115 | 116 | // Enable RX/TX 117 | UCSR0B |= _B(RXEN0, 1) | _B(TXEN0, 1); 118 | 119 | // Enable USART RX Complete Interrupt 120 | UCSR0B |= _B(RXCIE0, 1); 121 | 122 | SREG = sreg; 123 | } 124 | 125 | // Transmit interrupt handler. 126 | ISR(USART_UDRE_vect) { 127 | #ifdef UART_COUNT_TX_BYTES 128 | // The TX counter should be incremented from the TX complete interrupt 129 | // handler, but it is overkill to have a handler just for this. 130 | uart_tx_bytes++; 131 | #endif 132 | UDR0 = tx_buf[0]; 133 | if (--tx_count > 0) { 134 | tx_buf++; 135 | } else { 136 | // Disable USART Data Register Empty Interrupt 137 | UCSR0B &= ~_B(UDRIE0, 1); 138 | task_wakeup(tx_task); 139 | } 140 | } 141 | 142 | // Write data to UART. 143 | int uart_write(const void *buf, size_t count) { 144 | uint8_t sreg; 145 | 146 | sreg = SREG; 147 | cli(); 148 | 149 | tx_task = task_current(); 150 | tx_buf = buf; 151 | tx_count = count; 152 | 153 | // Enable USART Data Register Empty Interrupt 154 | // It is disabled by the interrupt handler when done. 155 | UCSR0B |= _B(UDRIE0, 1); 156 | 157 | // Task is woken up by the interrupt handler when done. 158 | task_suspend(NULL); 159 | 160 | SREG = sreg; 161 | 162 | return count; 163 | } 164 | 165 | // Receive interrupt handler. 166 | ISR(USART_RX_vect) { 167 | // Check for receive errors. 168 | if (UCSR0A & (_BV(FE0) | _BV(DOR0) | _BV(UPE0))) { 169 | #ifdef UART_COUNT_RX_ERRORS 170 | if (UCSR0A & _BV(DOR0)) { 171 | uart_rx_fe++; 172 | } 173 | if (UCSR0A & _BV(DOR0)) { 174 | uart_rx_dor++; 175 | } 176 | if (UCSR0A & _BV(UPE0)) { 177 | uart_rx_pe++; 178 | } 179 | #endif 180 | 181 | // Read data register to acknowledge interrupt. 182 | uint8_t tmp __attribute__((unused)) = UDR0; 183 | return; 184 | } 185 | 186 | #ifdef UART_COUNT_RX_BYTES 187 | // No errors; byte was successfully read. 188 | uart_rx_bytes++; 189 | #endif 190 | 191 | // Read into private buffer if external read buffer is not set. 192 | if (rx_buf == NULL) { 193 | rx_priv_buf[PRIV_BUF_VAL(rx_priv_ppos)] = UDR0; 194 | PRIV_BUF_INCR(rx_priv_ppos); 195 | 196 | // Advance consumer position if producer caught up. 197 | if (PRIV_BUF_VAL(rx_priv_ppos) == PRIV_BUF_VAL(rx_priv_cpos)) { 198 | PRIV_BUF_INCR(rx_priv_cpos); 199 | #ifdef UART_COUNT_RX_ERRORS 200 | uart_rx_bdor++; 201 | #endif 202 | } 203 | 204 | return; 205 | } 206 | 207 | rx_buf[0] = UDR0; 208 | if (--rx_count > 0) { 209 | rx_buf++; 210 | } else { 211 | rx_buf = NULL; 212 | task_wakeup(rx_task); 213 | } 214 | } 215 | 216 | // Read data from UART. 217 | int uart_read(void *buf, size_t count) { 218 | uint8_t *bbuf = buf; 219 | uint8_t sreg; 220 | int n = 0; 221 | 222 | sreg = SREG; 223 | cli(); 224 | 225 | // Read from private receive buffer if non-empty. 226 | while (count && rx_priv_cpos != rx_priv_ppos) { 227 | bbuf[0] = rx_priv_buf[PRIV_BUF_VAL(rx_priv_cpos)]; 228 | PRIV_BUF_INCR(rx_priv_cpos); 229 | 230 | bbuf++; 231 | count--; 232 | n++; 233 | } 234 | 235 | // Wait for interrupt handler to populate remaining bytes. 236 | if (count) { 237 | rx_task = task_current(); 238 | rx_buf = bbuf; 239 | rx_count = count; 240 | n += count; 241 | 242 | // Task is woken up by the interrupt handler when done. 243 | task_suspend(NULL); 244 | } 245 | 246 | SREG = sreg; 247 | return n; 248 | } 249 | 250 | // Read data from read buffer. 251 | int uart_read_nonblock(void *buf, size_t count) { 252 | uint8_t *bbuf = buf; 253 | uint8_t sreg; 254 | int n = 0; 255 | 256 | sreg = SREG; 257 | cli(); 258 | 259 | // Read from private receive buffer if non-empty. 260 | while (count && rx_priv_cpos != rx_priv_ppos) { 261 | bbuf[0] = rx_priv_buf[PRIV_BUF_VAL(rx_priv_cpos)]; 262 | PRIV_BUF_INCR(rx_priv_cpos); 263 | 264 | bbuf++; 265 | count--; 266 | n++; 267 | } 268 | 269 | SREG = sreg; 270 | return n; 271 | } 272 | 273 | // Write character to UART. 274 | int uart_putc(char c, FILE *unused) { 275 | uart_write(&c, 1); 276 | return 0; 277 | } 278 | 279 | // Read character from UART. 280 | int uart_getc(FILE *unused) { 281 | char c; 282 | uart_read(&c, 1); 283 | return c; 284 | } 285 | -------------------------------------------------------------------------------- /uart/uart.c.in: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "uart.h" 6 | #include "task.h" 7 | 8 | #define _B(x, on) ((on) * _BV(x)) 9 | 10 | static task_t *tx_task = NULL; 11 | static const uint8_t *tx_buf; 12 | static uint8_t tx_count; 13 | 14 | static task_t *rx_task = NULL; 15 | static uint8_t *rx_buf; 16 | static uint8_t rx_count; 17 | 18 | #ifdef UART_COUNT_TX_BYTES 19 | uint16_t uart_tx_bytes = 0; 20 | #endif 21 | 22 | #ifdef UART_COUNT_RX_BYTES 23 | uint16_t uart_rx_bytes = 0; 24 | #endif 25 | 26 | #ifdef UART_COUNT_RX_ERRORS 27 | uint8_t uart_rx_fe = 0; 28 | uint8_t uart_rx_dor = 0; 29 | uint8_t uart_rx_pe = 0; 30 | uint8_t uart_rx_bdor = 0; 31 | #endif 32 | 33 | #define PRIV_BUF_SIZE (1<<4) 34 | #define PRIV_BUF_SIZE_MASK ((PRIV_BUF_SIZE)-1) 35 | #define PRIV_BUF_VAL(var) ((var) & PRIV_BUF_SIZE_MASK) 36 | 37 | // Toggle the MSB every time an index wraps. 38 | // This tells ppos and cpos apart if they are equal (empty vs full). 39 | #define PRIV_BUF_INCR(var) do { \ 40 | (var)++; \ 41 | if (PRIV_BUF_VAL(var) == 0) { \ 42 | (var) = (~((var) & 0x80) & 0x80); \ 43 | } \ 44 | } while (0); 45 | 46 | // Private receive buffer. 47 | static uint8_t rx_priv_buf[PRIV_BUF_SIZE]; 48 | static uint8_t rx_priv_ppos; // Producer 49 | static uint8_t rx_priv_cpos; // Consumer 50 | 51 | // Register descriptions as documented in the ATmega328p datasheet. 52 | // 53 | // Bits in UCSR@A, USART Control and Status Register @A 54 | // RXC@: USART Receive Complete 55 | // TXC@: USART Transmit Complete 56 | // UDRE@: USART Data Register Empty 57 | // FE@: Frame Error 58 | // DOR@: Data Overrun 59 | // UPE@: USART Parity Error 60 | // U2X@: Double the USART Transmission Speed 61 | // MPCM@: Multi-processor Communication Mode 62 | // 63 | // Bits in UCSR@B, USART Control and Status Register @B 64 | // RXCIE@: RX Complete Interrupt Enable 65 | // TXCIE@: TX Complete Interrupt Enable 66 | // UDRIE@: USART Data Register Empty Interrupt Enable 67 | // RXEN@: Receiver Enable 68 | // TXEN@: Transmitter Enable 69 | // UCSZ@2: Character Size bit 2 70 | // RXB8@: Receive Data Bit 8 71 | // TXB8@: Transmit Data Bit 8 72 | // 73 | // Bits in UCSR@C, USART Control and Status Register @C 74 | // UMSEL@1: USART Mode Select bit 1 75 | // UMSEL@0: USART Mode Select bit 0 76 | // UPM@1: Parity Mode bit 1 77 | // UPM@0: Parity Mode bit 0 78 | // USBS@: Stop Bit Select 79 | // UCSZ@1: Character Size bit 1 80 | // UCSZ@0: Character Size bit 0 81 | // UCPOL@: Clock Polarity 82 | // 83 | 84 | // Some of the assignments here evaluate to 0 making them a no-op. 85 | // They are included as documentation. 86 | void uart_init(uint16_t ubrr, uint8_t x2) { 87 | uint8_t sreg; 88 | 89 | sreg = SREG; 90 | cli(); 91 | 92 | UBRR@ = ubrr; 93 | 94 | UCSR@A = 0; 95 | UCSR@B = 0; 96 | UCSR@C = 0; 97 | 98 | // Double the transmission speed 99 | if (x2) { 100 | UCSR@A |= _B(U2X@, 1); 101 | } 102 | 103 | // Asynchronous USART 104 | UCSR@C |= _B(UMSEL@1, 0) | _B(UMSEL@0, 0); 105 | 106 | // 8 bit character size 107 | UCSR@B |= _B(UCSZ@2, 0); 108 | UCSR@C |= _B(UCSZ@1, 1) | _B(UCSZ@0, 1); 109 | 110 | // No parity bit 111 | UCSR@C |= _B(UPM@1, 0) | _B(UPM@0, 0); 112 | 113 | // 1 stop bit 114 | UCSR@C |= _B(USBS@, 0); 115 | 116 | // Enable RX/TX 117 | UCSR@B |= _B(RXEN@, 1) | _B(TXEN@, 1); 118 | 119 | // Enable USART RX Complete Interrupt 120 | UCSR@B |= _B(RXCIE@, 1); 121 | 122 | SREG = sreg; 123 | } 124 | 125 | // Transmit interrupt handler. 126 | ISR(USART@_UDRE_vect) { 127 | #ifdef UART_COUNT_TX_BYTES 128 | // The TX counter should be incremented from the TX complete interrupt 129 | // handler, but it is overkill to have a handler just for this. 130 | uart_tx_bytes++; 131 | #endif 132 | UDR@ = tx_buf[0]; 133 | if (--tx_count > 0) { 134 | tx_buf++; 135 | } else { 136 | // Disable USART Data Register Empty Interrupt 137 | UCSR@B &= ~_B(UDRIE@, 1); 138 | task_wakeup(tx_task); 139 | } 140 | } 141 | 142 | // Write data to UART. 143 | int uart_write(const void *buf, size_t count) { 144 | uint8_t sreg; 145 | 146 | sreg = SREG; 147 | cli(); 148 | 149 | tx_task = task_current(); 150 | tx_buf = buf; 151 | tx_count = count; 152 | 153 | // Enable USART Data Register Empty Interrupt 154 | // It is disabled by the interrupt handler when done. 155 | UCSR@B |= _B(UDRIE@, 1); 156 | 157 | // Task is woken up by the interrupt handler when done. 158 | task_suspend(NULL); 159 | 160 | SREG = sreg; 161 | 162 | return count; 163 | } 164 | 165 | // Receive interrupt handler. 166 | ISR(USART@_RX_vect) { 167 | // Check for receive errors. 168 | if (UCSR@A & (_BV(FE@) | _BV(DOR@) | _BV(UPE@))) { 169 | #ifdef UART_COUNT_RX_ERRORS 170 | if (UCSR@A & _BV(DOR@)) { 171 | uart_rx_fe++; 172 | } 173 | if (UCSR@A & _BV(DOR@)) { 174 | uart_rx_dor++; 175 | } 176 | if (UCSR@A & _BV(UPE@)) { 177 | uart_rx_pe++; 178 | } 179 | #endif 180 | 181 | // Read data register to acknowledge interrupt. 182 | uint8_t tmp __attribute__((unused)) = UDR@; 183 | return; 184 | } 185 | 186 | #ifdef UART_COUNT_RX_BYTES 187 | // No errors; byte was successfully read. 188 | uart_rx_bytes++; 189 | #endif 190 | 191 | // Read into private buffer if external read buffer is not set. 192 | if (rx_buf == NULL) { 193 | rx_priv_buf[PRIV_BUF_VAL(rx_priv_ppos)] = UDR@; 194 | PRIV_BUF_INCR(rx_priv_ppos); 195 | 196 | // Advance consumer position if producer caught up. 197 | if (PRIV_BUF_VAL(rx_priv_ppos) == PRIV_BUF_VAL(rx_priv_cpos)) { 198 | PRIV_BUF_INCR(rx_priv_cpos); 199 | #ifdef UART_COUNT_RX_ERRORS 200 | uart_rx_bdor++; 201 | #endif 202 | } 203 | 204 | return; 205 | } 206 | 207 | rx_buf[0] = UDR@; 208 | if (--rx_count > 0) { 209 | rx_buf++; 210 | } else { 211 | rx_buf = NULL; 212 | task_wakeup(rx_task); 213 | } 214 | } 215 | 216 | // Read data from UART. 217 | int uart_read(void *buf, size_t count) { 218 | uint8_t *bbuf = buf; 219 | uint8_t sreg; 220 | int n = 0; 221 | 222 | sreg = SREG; 223 | cli(); 224 | 225 | // Read from private receive buffer if non-empty. 226 | while (count && rx_priv_cpos != rx_priv_ppos) { 227 | bbuf[0] = rx_priv_buf[PRIV_BUF_VAL(rx_priv_cpos)]; 228 | PRIV_BUF_INCR(rx_priv_cpos); 229 | 230 | bbuf++; 231 | count--; 232 | n++; 233 | } 234 | 235 | // Wait for interrupt handler to populate remaining bytes. 236 | if (count) { 237 | rx_task = task_current(); 238 | rx_buf = bbuf; 239 | rx_count = count; 240 | n += count; 241 | 242 | // Task is woken up by the interrupt handler when done. 243 | task_suspend(NULL); 244 | } 245 | 246 | SREG = sreg; 247 | return n; 248 | } 249 | 250 | // Read data from read buffer. 251 | int uart_read_nonblock(void *buf, size_t count) { 252 | uint8_t *bbuf = buf; 253 | uint8_t sreg; 254 | int n = 0; 255 | 256 | sreg = SREG; 257 | cli(); 258 | 259 | // Read from private receive buffer if non-empty. 260 | while (count && rx_priv_cpos != rx_priv_ppos) { 261 | bbuf[0] = rx_priv_buf[PRIV_BUF_VAL(rx_priv_cpos)]; 262 | PRIV_BUF_INCR(rx_priv_cpos); 263 | 264 | bbuf++; 265 | count--; 266 | n++; 267 | } 268 | 269 | SREG = sreg; 270 | return n; 271 | } 272 | 273 | // Write character to UART. 274 | int uart_putc(char c, FILE *unused) { 275 | uart_write(&c, 1); 276 | return 0; 277 | } 278 | 279 | // Read character from UART. 280 | int uart_getc(FILE *unused) { 281 | char c; 282 | uart_read(&c, 1); 283 | return c; 284 | } 285 | -------------------------------------------------------------------------------- /i2c.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "i2c.h" 7 | #include "task.h" 8 | 9 | struct i2c_op_s { 10 | uint8_t error; 11 | uint8_t address; 12 | struct i2c_iovec_s *iov; 13 | uint8_t iovcnt; 14 | }; 15 | 16 | // Task waiting for I2C operation completion. 17 | static task_t *i2c_task; 18 | 19 | // Current I2C operation. 20 | static struct i2c_op_s *i2c_op; 21 | 22 | // By default, the control register is set to: 23 | // - TWEA: Automatically send acknowledge bit in receive mode. 24 | // - TWEN: Enable the I2C system. 25 | // - TWIE: Enable interrupt requests when TWINT is set. 26 | #define TWCR_DEFAULT (_BV(TWEA) | _BV(TWEN) | _BV(TWIE)) 27 | 28 | #define TWCR_ACK (TWCR_DEFAULT | _BV(TWINT)) 29 | #define TWCR_NOT_ACK (TWCR_ACK & ~_BV(TWEA)) 30 | 31 | #define TWCR_START (TWCR_DEFAULT | _BV(TWINT) | _BV(TWSTA)) 32 | #define TWCR_STOP (TWCR_DEFAULT | _BV(TWINT) | _BV(TWSTO)) 33 | 34 | void i2c_init(void) { 35 | uint8_t sreg; 36 | 37 | sreg = SREG; 38 | cli(); 39 | 40 | // From ATmega328p datasheet: 41 | // SCL freq = (CPU Clock freq) / (16 + 2(TWBR) * (PrescalerValue)) 42 | // 43 | // Which means: 44 | // TWBR = ((CPU Clock freq) / (SCL freq) - 16) / (2 * (PrescalerValue)) 45 | // 46 | // Disable the prescaler and set TWBR according to CPU freq and SCL freq. 47 | // 48 | TWSR &= ~(_BV(TWPS1) | _BV(TWPS0)); 49 | TWBR = ((F_CPU / I2C_FREQ) - 16) / (2 * 1); 50 | 51 | // Active internal pull-up resistors for SCL and SDA. 52 | // Their ports are PC5 for SCL and PC4 for SDA on the ATmega328p. 53 | PORTC |= _BV(PC5) | _BV(PC4); 54 | 55 | // Enable the I2C system. 56 | TWCR = TWCR_DEFAULT; 57 | 58 | // Disable slave mode. 59 | TWAR = 0; 60 | 61 | SREG = sreg; 62 | } 63 | 64 | void i2c_open(void) { 65 | // No-op for now. 66 | } 67 | 68 | void i2c_close(void) { 69 | TWCR = TWCR_STOP; 70 | // Stop is unset when done.. wait for that 71 | while (TWCR & _BV(TWSTO)) { 72 | continue; 73 | } 74 | } 75 | 76 | // Prepares I2C operation and suspends task to wait for completion. 77 | static int8_t i2c__io(uint8_t address, struct i2c_iovec_s *iov, uint8_t iovcnt) { 78 | struct i2c_op_s op; 79 | uint8_t sreg; 80 | 81 | sreg = SREG; 82 | 83 | op.error = 0; 84 | op.address = address; 85 | op.iov = iov; 86 | op.iovcnt = iovcnt; 87 | 88 | cli(); 89 | 90 | i2c_task = task_current(); 91 | i2c_op = &op; 92 | 93 | // Transmit START to kickstart operation. 94 | TWCR = TWCR_START; 95 | 96 | task_suspend(NULL); 97 | 98 | SREG = sreg; 99 | 100 | if (op.error) { 101 | return -1; 102 | } 103 | 104 | return 0; 105 | } 106 | 107 | int8_t i2c_readv(uint8_t address, struct i2c_iovec_s *iov, uint8_t iovcnt) { 108 | return i2c__io((address << 1) | TW_READ, iov, iovcnt); 109 | } 110 | 111 | int8_t i2c_writev(uint8_t address, struct i2c_iovec_s *iov, uint8_t iovcnt) { 112 | return i2c__io((address << 1) | TW_WRITE, iov, iovcnt); 113 | } 114 | 115 | int8_t i2c_read(uint8_t address, uint8_t *buf, uint8_t len) { 116 | struct i2c_iovec_s iov = { buf, len }; 117 | return i2c_readv(address, &iov, 1); 118 | } 119 | 120 | int8_t i2c_write(uint8_t address, uint8_t *buf, uint8_t len) { 121 | struct i2c_iovec_s iov = { buf, len }; 122 | return i2c_writev(address, &iov, 1); 123 | } 124 | 125 | int8_t i2c_read_from(uint8_t address, uint8_t reg, uint8_t *buf, uint8_t len) { 126 | int8_t rv; 127 | 128 | rv = i2c_write(address, ®, sizeof(reg)); 129 | if (rv < 0) { 130 | return rv; 131 | } 132 | 133 | return i2c_read(address, buf, len); 134 | } 135 | 136 | int8_t i2c_write_to(uint8_t address, uint8_t reg, uint8_t *buf, uint8_t len) { 137 | struct i2c_iovec_s iov[2] = { { ®, 1 }, { buf, len } }; 138 | return i2c_writev(address, (struct i2c_iovec_s *) &iov, 2); 139 | } 140 | 141 | ISR(TWI_vect, ISR_BLOCK) { 142 | uint8_t status; 143 | 144 | status = TW_STATUS; 145 | 146 | switch (i2c_op->address & 0x1) { 147 | case TW_READ: 148 | // Master Receiver mode. 149 | switch (status) { 150 | 151 | // A START condition has been transmitted. 152 | case TW_START: 153 | // A repeated START condition has been transmitted. 154 | case TW_REP_START: 155 | TWDR = i2c_op->address; 156 | TWCR = TWCR_ACK; 157 | return; 158 | 159 | // Arbitration lost in SLA+R or NOT ACK bit. 160 | case TW_MR_ARB_LOST: 161 | // A START condition will be transmitted when the bus becomes free. 162 | TWCR = TWCR_START; 163 | return; 164 | 165 | // SLA+R has been transmitted; ACK has been received. 166 | case TW_MR_SLA_ACK: 167 | // Return NACK after next byte if it is the last one. 168 | if (i2c_op->iov[0].len == 1 && i2c_op->iovcnt == 1) { 169 | TWCR = TWCR_NOT_ACK; 170 | } else { 171 | TWCR = TWCR_ACK; 172 | } 173 | return; 174 | 175 | // SLA+R has been transmitted; NOT ACK has been received. 176 | case TW_MR_SLA_NACK: 177 | i2c_op->error = 1; 178 | goto done; 179 | 180 | // Data byte has been received; ACK has been returned. 181 | case TW_MR_DATA_ACK: 182 | i2c_op->iov[0].base[0] = TWDR; 183 | i2c_op->iov[0].base++; 184 | if (--i2c_op->iov[0].len == 0) { 185 | i2c_op->iov++; 186 | i2c_op->iovcnt--; 187 | // iovcnt is > 0, or we would have run TW_MR_DATA_NACK. 188 | } 189 | 190 | // Return NACK after next byte if it is the last one. 191 | if (i2c_op->iov[0].len == 1 && i2c_op->iovcnt == 1) { 192 | TWCR = TWCR_NOT_ACK; 193 | } else { 194 | TWCR = TWCR_ACK; 195 | } 196 | return; 197 | 198 | // Data byte has been received; NOT ACK has been returned. 199 | case TW_MR_DATA_NACK: 200 | i2c_op->iov[0].base[0] = TWDR; 201 | goto done; 202 | } 203 | 204 | // Never reached, but be sure... 205 | return; 206 | 207 | case TW_WRITE: 208 | // Master Transmitter mode. 209 | switch (status) { 210 | 211 | // A START condition has been transmitted. 212 | case TW_START: 213 | // A repeated START condition has been transmitted. 214 | case TW_REP_START: 215 | TWDR = i2c_op->address; 216 | TWCR = TWCR_DEFAULT | _BV(TWINT); 217 | return; 218 | 219 | // Arbitration lost in SLA+W or data bytes. 220 | case TW_MT_ARB_LOST: 221 | // A START condition will be transmitted when the bus becomes free. 222 | TWCR = TWCR_START; 223 | return; 224 | 225 | // SLA+W has been transmitted; ACK has been received. 226 | case TW_MT_SLA_ACK: 227 | TWDR = i2c_op->iov[0].base[0]; 228 | TWCR = TWCR_DEFAULT | _BV(TWINT); 229 | i2c_op->iov[0].base++; 230 | i2c_op->iov[0].len--; 231 | return; 232 | 233 | // SLA+W has been transmitted; NOT ACK has been received. 234 | case TW_MT_SLA_NACK: 235 | i2c_op->error = 1; 236 | goto done; 237 | 238 | // Data byte has been transmitted; ACK has been received. 239 | case TW_MT_DATA_ACK: 240 | if (i2c_op->iov[0].len == 0) { 241 | i2c_op->iov++; 242 | if (--i2c_op->iovcnt == 0) { 243 | // No more bytes left to transmit... 244 | goto done; 245 | } 246 | } 247 | 248 | TWDR = i2c_op->iov[0].base[0]; 249 | TWCR = TWCR_DEFAULT | _BV(TWINT); 250 | i2c_op->iov[0].base++; 251 | i2c_op->iov[0].len--; 252 | return; 253 | 254 | // Data byte has been transmitted; NOT ACK has been received. 255 | case TW_MT_DATA_NACK: 256 | if (i2c_op->iov[0].len == 0) { 257 | i2c_op->iov++; 258 | if (--i2c_op->iovcnt == 0) { 259 | // No more bytes left to transmit... 260 | goto done; 261 | } 262 | } 263 | 264 | // There were more bytes left to transmit! 265 | i2c_op->error = 1; 266 | goto done; 267 | } 268 | 269 | // Never reached, but be sure... 270 | return; 271 | } 272 | 273 | // Never reached, but be sure... 274 | return; 275 | 276 | done: 277 | // From the ATmega328p datasheet (section 21.9.2): 278 | // 279 | // The TWINT Flag must be cleared by software by writing a logic one to it. 280 | // Note that this flag is not automatically cleared by hardware when 281 | // executing the interrupt routine. Also note that clearing this flag 282 | // starts the operation of the TWI, so all accesses to the TWI Address 283 | // Register (TWAR), TWI Status Register (TWSR), and TWI Data Register 284 | // (TWDR) must be complete before clearing this flag. 285 | // 286 | // It is up to the code that uses I2C functions whether it wants to bundle 287 | // multiple operations in one atomic set or not (this is done using repeated 288 | // start). This means that TWINT cannot be cleared here. However, the 289 | // interrupt handler cannot be allowed to fire again, so we clear TWIE to 290 | // disable I2C interrupts altogether. 291 | // 292 | TWCR = TWCR_DEFAULT & ~_BV(TWIE); 293 | 294 | task_wakeup(i2c_task); 295 | return; 296 | } 297 | -------------------------------------------------------------------------------- /task.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "task.h" 5 | 6 | // Pointer to current task. 7 | // May only be changed by schedule routine. 8 | static task_t *_task__current = 0; 9 | 10 | // Queue with runnable tasks. 11 | // Holds tasks that may be scheduled immediately. 12 | static QUEUE _tasks__runnable; 13 | 14 | // Queue with suspended tasks. 15 | // Holds tasks that called "task_suspend". 16 | static QUEUE _tasks__suspended; 17 | 18 | // Queue with sleeping tasks. 19 | // Holds tasks that called "task_sleep". 20 | static QUEUE _tasks__sleeping; 21 | 22 | #if TASK_COUNT_SEC 23 | static TASK_SEC_T _task_sec = 0; 24 | 25 | // Counts down until a second has passed. 26 | static uint16_t _task_sec_countdown; 27 | 28 | TASK_SEC_T task_sec(void) { 29 | return _task_sec; 30 | } 31 | 32 | void task_set_sec(TASK_SEC_T t) { 33 | uint8_t sreg = SREG; 34 | 35 | cli(); 36 | _task_sec = t; 37 | _task_sec_countdown = 1000 / MS_PER_TICK; 38 | SREG = sreg; 39 | } 40 | #endif // TASK_COUNT_SEC 41 | 42 | #if TASK_COUNT_MSEC 43 | static TASK_MSEC_T _task_msec = 0; 44 | 45 | TASK_MSEC_T task_msec(void) { 46 | return _task_msec; 47 | } 48 | 49 | void task_set_msec(TASK_MSEC_T t) { 50 | uint8_t sreg = SREG; 51 | 52 | cli(); 53 | _task_msec = t; 54 | SREG = sreg; 55 | } 56 | #endif // TASK_COUNT_MSEC 57 | 58 | #if TASK_COUNT_USEC 59 | static TASK_USEC_T _task_usec = 0; 60 | 61 | TASK_USEC_T task_usec(void) { 62 | return (_task_usec + (TCNT0 * US_PER_COUNT)); 63 | } 64 | 65 | void task_set_usec(TASK_USEC_T t) { 66 | uint8_t sreg = SREG; 67 | 68 | cli(); 69 | _task_usec = t; 70 | SREG = sreg; 71 | } 72 | #endif // TASK_COUNT_USEC 73 | 74 | // Push a task's context onto its own stack. 75 | static inline void task__push(void) __attribute__ ((always_inline)); 76 | static inline void task__push(void) { 77 | asm volatile( 78 | "push r0\n" 79 | 80 | // Save status register 81 | "in r0, 0x3f\n" 82 | "cli\n" 83 | "push r0\n" 84 | 85 | // Save Z register pair (so it can be used below) 86 | "push r30\n" 87 | "push r31\n" 88 | 89 | // Load _task__current into Z register pair 90 | "lds r30, _task__current\n" // Low 91 | "lds r31, _task__current+1\n" // High 92 | 93 | // Z=1 if (r30 | r31) == 0 94 | "mov r0, r30\n" 95 | "or r0, r31\n" 96 | 97 | // Save if Z=0 (_task__current != NULL) 98 | "brne 2f\n" 99 | 100 | "1:\n" 101 | // Restore Z register pair 102 | "pop r31\n" 103 | "pop r30\n" 104 | 105 | // Restore status register and real r0 106 | "pop r0\n" 107 | "out 0x3f, r0\n" 108 | "pop r0\n" 109 | 110 | "rjmp 3f\n" 111 | 112 | "2:\n" 113 | // Save general registers 114 | "push r1\n" 115 | "push r2\n" 116 | "push r3\n" 117 | "push r4\n" 118 | "push r5\n" 119 | "push r6\n" 120 | "push r7\n" 121 | "push r8\n" 122 | "push r9\n" 123 | "push r10\n" 124 | "push r11\n" 125 | "push r12\n" 126 | "push r13\n" 127 | "push r14\n" 128 | "push r15\n" 129 | "push r16\n" 130 | "push r17\n" 131 | "push r18\n" 132 | "push r19\n" 133 | "push r20\n" 134 | "push r21\n" 135 | "push r22\n" 136 | "push r23\n" 137 | "push r24\n" 138 | "push r25\n" 139 | "push r26\n" 140 | "push r27\n" 141 | "push r28\n" 142 | "push r29\n" 143 | 144 | // Compiler expects r1 to be zero. As this code may interrupt anything, 145 | // the register may temporarily hold a non-zero value. 146 | "clr r1\n" 147 | 148 | // Save stack pointer in current task struct 149 | "in r0, 0x3d\n" // Low 150 | "st z+, r0\n" 151 | "in r0, 0x3e\n" // High 152 | "st z+, r0\n" 153 | 154 | "3:\n" 155 | ); 156 | } 157 | 158 | // Pop a task's context off of its own stack and resume it. 159 | static void task__pop(void) __attribute__ ((naked)); 160 | static void task__pop(void) { 161 | asm volatile( 162 | // Restore stack pointer from current task struct 163 | "lds r26, _task__current\n" // Low 164 | "lds r27, _task__current+1\n" // High 165 | "ld r0, x+\n" 166 | "out 0x3d, r0\n" // Low 167 | "ld r0, x+\n" 168 | "out 0x3e, r0\n" // High 169 | 170 | // Restore general registers 171 | "pop r29\n" 172 | "pop r28\n" 173 | "pop r27\n" 174 | "pop r26\n" 175 | "pop r25\n" 176 | "pop r24\n" 177 | "pop r23\n" 178 | "pop r22\n" 179 | "pop r21\n" 180 | "pop r20\n" 181 | "pop r19\n" 182 | "pop r18\n" 183 | "pop r17\n" 184 | "pop r16\n" 185 | "pop r15\n" 186 | "pop r14\n" 187 | "pop r13\n" 188 | "pop r12\n" 189 | "pop r11\n" 190 | "pop r10\n" 191 | "pop r9\n" 192 | "pop r8\n" 193 | "pop r7\n" 194 | "pop r6\n" 195 | "pop r5\n" 196 | "pop r4\n" 197 | "pop r3\n" 198 | "pop r2\n" 199 | "pop r1\n" 200 | 201 | // Restore Z register pair 202 | "pop r31\n" 203 | "pop r30\n" 204 | 205 | // Restore status register. 206 | // 207 | // In theory, if we set the status register directly, and it enabled 208 | // interrupts, it is possible for an interrupt to trigger when we're 209 | // executing the final instructions of 'task__pop'. If this happens, all 210 | // registers will be pushed onto the stack again, but this time the task 211 | // hasn't been fully restored yet since we still have the real r0 sitting 212 | // on the stack. If this happens a number of times successively, we risk a 213 | // stack overflow. To prevent this from happening, we have two paths: 214 | // 215 | // A) Interrupts should NOT be enabled: the status register is restored, we 216 | // pop the real r0, and execute 'ret' to return to the task. 217 | // 218 | // B) Interrupts should be enabled: the status register is restored without 219 | // the interrupt bit set, we pop the real r0, and execute 'reti' to return 220 | // to the task AND re-enable interrupts. 221 | // 222 | "pop r0\n" 223 | "sbrs r0, 7\n" // Skip if bit in register set 224 | "jmp task_pop_ret\n" 225 | "jmp task_pop_reti\n" 226 | 227 | "task_pop_ret:\n" 228 | "out 0x3f, r0\n" // Restore status register 229 | "pop r0\n" // Restore the real r0 230 | "ret\n" 231 | 232 | "task_pop_reti:\n" 233 | "clt\n" // Clear T in SREG 234 | "bld r0, 7\n" // Bit load from T to r0 bit 7 (interrupt bit) 235 | "out 0x3f, r0\n" // Restore status register (without interrupt bit set) 236 | "pop r0\n" // Restore the real r0 237 | "reti\n" 238 | 239 | ); 240 | } 241 | 242 | // Internal task initializer. 243 | // Written in assembly so that it can mangle the stack pointer to initialize 244 | // the task's stack. Note that interrupts are temporarily disabled. 245 | static void *task__internal_initialize(void *sp, task_fn fn, void *data) { 246 | void *result; 247 | 248 | asm volatile( 249 | // About to overwrite the stack pointer, disable interrupts 250 | "in r18, 0x3f\n" // r18 can be clobbered 251 | "cli\n" 252 | 253 | // Save current stack pointer 254 | "in r26, 0x3d\n" 255 | "in r27, 0x3e\n" 256 | 257 | // Set new task's stack pointer 258 | "out 0x3d, %A1\n" 259 | "out 0x3e, %B1\n" 260 | 261 | // Store location of task body as return address, such that 262 | // executing "ret" after "context_restore" will jump to it. 263 | "push %A2\n" 264 | "push %B2\n" 265 | #ifdef __AVR_3_BYTE_PC__ 266 | // Push extra zero, assuming the task function is not located 267 | // in high program memory (>128KiB). 268 | "clr __tmp_reg__\n" 269 | "push __tmp_reg__\n" 270 | #endif 271 | 272 | // Store r0 273 | "ldi r19, 0\n" // r19 can be clobbered 274 | "push r19\n" 275 | 276 | // Store status register 277 | "ldi r19, 0x80\n" // Start task with interrupts enabled 278 | "push r19\n" 279 | 280 | // Store general registers 281 | "ldi r19, 0\n" 282 | "push r19\n" // r30 283 | "push r19\n" // r31 284 | "push r19\n" // r1 285 | "push r19\n" // r2 286 | "push r19\n" // r3 287 | "push r19\n" // r4 288 | "push r19\n" // r5 289 | "push r19\n" // r6 290 | "push r19\n" // r7 291 | "push r19\n" // r8 292 | "push r19\n" // r9 293 | "push r19\n" // r10 294 | "push r19\n" // r11 295 | "push r19\n" // r12 296 | "push r19\n" // r13 297 | "push r19\n" // r14 298 | "push r19\n" // r15 299 | "push r19\n" // r16 300 | "push r19\n" // r17 301 | "push r19\n" // r18 302 | "push r19\n" // r19 303 | "push r19\n" // r20 304 | "push r19\n" // r21 305 | "push r19\n" // r22 306 | "push r19\n" // r23 307 | 308 | // Argument to task function 309 | "push %A3\n" // r24 310 | "push %B3\n" // r25 311 | 312 | "push r19\n" // r26 313 | "push r19\n" // r27 314 | "push r19\n" // r28 315 | "push r19\n" // r29 316 | 317 | // Store new task's stack pointer at return register 318 | "in %A0, 0x3d\n" 319 | "in %B0, 0x3e\n" 320 | 321 | // Restore stack pointer 322 | "out 0x3d, r26\n" 323 | "out 0x3e, r27\n" 324 | 325 | // Restore status register 326 | "out 0x3f, r18\n" 327 | 328 | : "=r" (result) 329 | : "r" (sp), "r" (fn), "r" (data) 330 | : "r18", "r19", "r26", "r27" 331 | ); 332 | 333 | return result; 334 | } 335 | 336 | // Creates a task for the specified function. 337 | task_t *task__internal_create(task_fn fn, void *data) { 338 | static void *start = (void *)RAMEND; 339 | void *sp; 340 | task_t *t; 341 | 342 | // Should protect this against underflow. 343 | start -= 0x100; 344 | 345 | // Stack grows down, don't overwrite first byte of task struct. 346 | t = start - sizeof(task_t); 347 | sp = (void *)t - 1; 348 | 349 | t->sp = task__internal_initialize(sp, fn, data); 350 | t->delay = 0; 351 | QUEUE_INIT(&t->member); 352 | 353 | return t; 354 | } 355 | 356 | // Creates a task for the specified function. 357 | // Adds it to the list of user tasks. 358 | task_t *task_create(task_fn fn, void *data) { 359 | task_t *t = task__internal_create(fn, data); 360 | 361 | QUEUE_INSERT_TAIL(&_tasks__runnable, &t->member); 362 | 363 | return t; 364 | } 365 | 366 | static void task__tick() { 367 | QUEUE *q, *r; 368 | task_t *t; 369 | 370 | #if TASK_COUNT_SEC 371 | if (--_task_sec_countdown == 0) { 372 | _task_sec++; 373 | _task_sec_countdown = 1000 / MS_PER_TICK; 374 | } 375 | #endif 376 | 377 | #if TASK_COUNT_MSEC 378 | _task_msec += MS_PER_TICK; 379 | #endif 380 | 381 | #if TASK_COUNT_USEC 382 | _task_usec += US_PER_TICK; 383 | #endif 384 | 385 | q = QUEUE_NEXT(&_tasks__sleeping); 386 | r = 0; 387 | for (; q != &_tasks__sleeping; q = r) { 388 | // Save pointer to next element so q can be 389 | // removed without breaking iteration. 390 | r = QUEUE_NEXT(q); 391 | t = QUEUE_DATA(q, task_t, member); 392 | 393 | if (t->delay) { 394 | t->delay--; 395 | } 396 | 397 | if (t->delay == 0) { 398 | task_wakeup(t); 399 | } 400 | } 401 | } 402 | 403 | static void task__scheduler(void) { 404 | // Overwrite stack pointer to RAMEND. 405 | // The task scheduler runs in its own piece of stack to prevent polluting (or 406 | // even overflowing) task stacks when interrupt handlers are executed. 407 | asm volatile( 408 | "out 0x3d, %A0\n" 409 | "out 0x3e, %B0\n" 410 | :: "x" (RAMEND) 411 | ); 412 | 413 | for (;;) { 414 | QUEUE *q; 415 | 416 | // No task is currently running. 417 | _task__current = 0; 418 | 419 | // Find task to schedule, if any. 420 | QUEUE_FOREACH(q, &_tasks__runnable) { 421 | // The first runnable task can be scheduled. 422 | _task__current = QUEUE_DATA(q, task_t, member); 423 | 424 | // Make [head..q] the new tail, so that q->next can be scheduled next. 425 | QUEUE_ROTATE(&_tasks__runnable, q); 426 | 427 | // This function doesn't continue execution beyond this point. 428 | // The task__pop function RETs back into the task. 429 | task__pop(); 430 | } 431 | 432 | // The processor wakes up from sleep to handle interrupts. 433 | // 434 | // 1. It is woken up by the task timer interrupt and the interrupt handler 435 | // jumps to this function (i.e. this function doesn't continue execution 436 | // beyond the sleep instruction). 437 | // 438 | // 2. It is woken up by another interrupt, the sleep instruction returns 439 | // after the handler has executed, and this function continues execution. 440 | // 441 | 442 | sei(); 443 | asm volatile ("sleep"); 444 | cli(); 445 | } 446 | } 447 | 448 | static void task__jmp_scheduler(void) { 449 | asm volatile ("ijmp" :: "z" (task__scheduler)); 450 | } 451 | 452 | // Must be naked to avoid mangling the stack. 453 | static void task__yield_from_timer(void) __attribute__((naked)); 454 | static void task__yield_from_timer(void) { 455 | task__push(); 456 | 457 | task__tick(); 458 | 459 | task__jmp_scheduler(); 460 | } 461 | 462 | // ISR can be naked because the first thing it does is push the current task. 463 | ISR(TIMER0_COMPA_vect, ISR_NAKED) { 464 | task__yield_from_timer(); 465 | 466 | // This interrupt handler was saved as part of the task's state. 467 | // We know the task was interrupted, or this handler wouldn't have triggered. 468 | // Because SREG was stored from within the ISR, we know the Global Interrupt 469 | // Enable bit is not set and we have to re-enable it upon returning from the 470 | // interrupt handler. Hence, the RETI. 471 | asm volatile ("reti"); 472 | } 473 | 474 | // Use TIMER0 for OS ticks. 475 | // Configure it to trigger a Output Compare Register interrupt every 2ms. 476 | static void task__setup_timer() { 477 | // Waveform generation mode: CTC 478 | // WGM02: 0 479 | // WGM01: 1 480 | // WGM00: 0 481 | TCCR0A = _BV(WGM01); 482 | 483 | // Clock select (see task.h) 484 | TCCR0B = _TCCR0B; 485 | 486 | // Output compare register 487 | OCR0A = COUNTS_PER_TICK - 1; 488 | } 489 | 490 | void task_init(void) { 491 | QUEUE_INIT(&_tasks__runnable); 492 | QUEUE_INIT(&_tasks__suspended); 493 | QUEUE_INIT(&_tasks__sleeping); 494 | 495 | task__setup_timer(); 496 | 497 | #if TASK_COUNT_SEC 498 | task_set_sec(0); 499 | #endif 500 | 501 | #if TASK_COUNT_MSEC 502 | task_set_msec(0); 503 | #endif 504 | 505 | #if TASK_COUNT_USEC 506 | task_set_usec(0); 507 | #endif 508 | } 509 | 510 | // Call this function to start task execution. 511 | // Never returns. 512 | void task_start(void) { 513 | // Global interrupt bit will be enabled when task is popped. 514 | cli(); 515 | 516 | // Enable interrupt on OCR0A match 517 | TIMSK0 |= _BV(OCIE0A); 518 | 519 | // Schedule next task to run. 520 | task__jmp_scheduler(); 521 | } 522 | 523 | // Yield execution to any other schedulable task. 524 | void task_yield(void) __attribute__((naked)); 525 | void task_yield(void) { 526 | task__push(); 527 | 528 | task__jmp_scheduler(); 529 | } 530 | 531 | // Return pointer to current task. 532 | task_t *task_current(void) { 533 | return _task__current; 534 | } 535 | 536 | void task__suspend(QUEUE *h) { 537 | uint8_t sreg = SREG; 538 | 539 | cli(); 540 | 541 | QUEUE *q = &_task__current->member; 542 | QUEUE_REMOVE(q); 543 | QUEUE_INSERT_TAIL(h, q); 544 | 545 | task_yield(); 546 | 547 | SREG = sreg; 548 | } 549 | 550 | // Suspend task until it is woken up explicitly. 551 | void task_suspend(QUEUE *h) { 552 | if (h == 0) { 553 | h = &_tasks__suspended; 554 | } 555 | 556 | task__suspend(h); 557 | } 558 | 559 | // Wake up task. 560 | void task_wakeup(task_t *t) { 561 | uint8_t sreg = SREG; 562 | 563 | cli(); 564 | 565 | QUEUE *q = &t->member; 566 | QUEUE_REMOVE(q); 567 | QUEUE_INSERT_TAIL(&_tasks__runnable, q); 568 | 569 | SREG = sreg; 570 | } 571 | 572 | // Make current task sleep for specified number of ticks. 573 | void task_sleep(uint16_t ms) { 574 | _task__current->delay = ms / MS_PER_TICK; 575 | task__suspend(&_tasks__sleeping); 576 | } 577 | --------------------------------------------------------------------------------